智能空间下基于异质传感信息融合的目标跟踪

桑森，田国会，段朋，吴皓

山东大学控制科学与工程学院，济南 250061

智能空间下基于异质传感信息融合的目标跟踪

桑森，田国会，段朋，吴皓

山东大学控制科学与工程学院，济南 250061

针对大范围未知环境下的机器人目标跟踪问题，在智能空间下分布式智能网络设备与机器人本体二维激光的基础上，提出了一种基于异质信息融合结构以实现机器人对目标实时检测和跟踪的方法。系统通过颜色信息进行移动目标匹配，根据三角测量原理基于最小二乘法对目标进行三维重建；检测出运动目标后，通过激光传感器扫描目标人腿进行近邻点聚类获得目标的准确深度信息；利用一种优化的迭代扩展卡尔曼滤波算法对异质传感器进行信息融合，以实现基于智能空间的机器人定位与目标跟踪。实验结果验证了方法的有效性。

目标跟踪；分布式智能网络设备；扩展卡尔曼滤波；异质传感器信息融合

1 引言

随着计算机、通讯技术的发展，具有人机交互能力的服务机器人已经逐渐走入家庭、办公室等场所，在助老助残等领域发挥着重要的作用；而为实现机器人与人的积极友好互动[1]，服务机器人的运动目标检测与跟踪成为机器人的一项基本和重要功能。在过去的二十年中，很多学者对运动目标的检测与跟踪进行了大量深入的研究，提出了很多行之有效的方法；如基于肤色识别、基于运动模型和机器学习[2-3]等方法，而这些方法大多数使用单一的视觉传感器，在进行目标跟踪时缺少准确的距离信息；为获得机器人与目标人的距离信息，Kazuyuki和Shintaro等人[4]用激光传感器进行人体轮廓检测以得到高精度的距离测量，但此方法识别目标的能力很差，当有多移动物体或障碍的情况下，容易造成跟踪失误；为此Wen Dai和A.Scheidig等人[5]用协方差交叉算法融合多个具有互补特性的传感器信息，如机器人本体携带的视觉传感器，激光传感器，超声波传感器等，以增强对人体检测和跟踪的准确性，弥补传感器彼此的薄弱点，但人的垂直运动在研究中并没有得到显示，而且当跟踪丢失时，难以搜寻大范围未知环境下目标人的当前位置，导致实时性降低。

受机器人感知环境信息的约束，传统意义下，机器人本体传感器的感知范围相对狭窄，所能获得的服务对象的信息十分有限，难以高质量地完成对目标的检测与跟踪；近年来，智能空间技术的应用使服务机器人的研究进入一个新的水平并取得了很多研究成果[6-7]。在智能空间技术的支持下，借助于机器人与智能空间的信息交互，为机器人服务的智能性和服务质量的提高提供了解决途径。

为解决上述问题，以山东大学服务机器人实验室为背景，设计了基于四台顶棚摄像机的全局视觉系统，建立了面向服务机器人的大小为7.35 m×15.3 m智能空间；通过多摄像机目标交接，实现各视角跟踪任务的分配，尽量得到目标清晰的无遮挡图像以确定全局环境下目标人的位置，弥补了机器人感知范围相对狭窄的缺陷，并与机器人进行通信，使机器人准确地搜寻到目标人所在的位置，然后通过机器人本体激光传感器扫描目标人腿进行近邻点聚类以获得准确的深度信息，最后将两种传感器的测量信息在智能空间服务器上用优化的迭代扩展卡尔曼滤波算法进行异质传感器的信息融合，以得到目标人准确位姿估计，实现了更准确的人机同步定位和目标跟踪。实验利用自行研制的家庭服务机器人验证了所提方法的有效性。

2 智能空间下基于双目立体视觉的目标跟踪定位

当对一个大的场景进行监控时，必须用到多个摄像机同时进行监控，并选择最合适的两台摄像机对目标进行跟踪定位，为尽可能得到清晰无遮挡图形，先判断是否被遮挡，选择目标未被遮挡的两台摄像机；若满足条件的摄像机不止两个，选择离目标近的两台摄像机组成双目立体视觉，从两个不同的位置和角度同时获取同一个目标的两幅图像，然后通过计算目标在两幅图像中的视差，利用三角测量原理来得到目标的三维信息[8]。基于摄像机的摆放位置不同，双目立体视觉分为平视和任意放置的立体模型，如果要使用平视的双目立体视觉，则要经过准确的校准，在目前的实验室环境下难以达到此要求，因此使用任意放置的立体模型。图1是通过三维重建方法来获取目标三维坐标信息的流程图。

图1 双目立体视觉框图

2.1 双目立体视觉的目标匹配

在智能空间下可能会存在多个移动目标，需要对两个摄像机检测到的移动目标区域进行匹配，或者因为视角的关系，移动目标只出现在一个摄像机的视野范围内，而另一个摄像机观察不到，由此需要在两个视野范围内搜寻同一个移动目标区域，在此不需要进行精确的点的匹配，只要确定两个区域是同一个移动目标即可。

为了能够增加匹配结果的准确性以及加快匹配算法的速度，减少噪声匹配的干扰，本研究在双目立体视觉中加入极线约束关系，如图2所示I1和I2为左右两个成像平面，OL和OR分别为左右两个摄像机的光心，射线OLPn上各空间点在I1面上的投影都为PL，而在右图像上的投影点组成一条直线d2，由此可以确定I1成像面上的PL点在I2成像面上的对应点肯定在d2上，d2就被称为PL在右图像上的极线，与之对应的d1被称为Pr在左图像上的极线，因此对于任意一空间点P，如果已知其在一个成像平面上的投影点，就可以在它的极线上搜索它所对应的点。

图2 双目立体视觉的极线几何约束

对两个摄像机的投影方程进行整理：

其中，X表示世界坐标系下的齐次坐标值，u1和u2表示图像坐标系下的齐次坐标值。将式（1）和（2）展开：

x是空间点的世界坐标，将式（3）、（4）消去x展开得：

将式（5）展开得到三个等式，消去z1、z2就可以得到u1和u2的关系式：

其中[M]x是反对称矩阵，根据式（6），如果已知u1，就可得到关于u2的线性方程，反知u2，可得u1线性方程。

综上所述，基于颜色相似性度量[9]的匹配方法如下：

（1）使用标定好的两个摄像机对同一场景进行拍摄，得到两幅图像I1和I2。

（2）对图像I1(I2)进行目标检测，根据要求选定参考目标，建立目标区域颜色模型，同时选定几个特征点，建立其对应的极线约束方程。

（3）对图像I2(I1)进行目标检测，如果某个运动区域与极线约束方程相交，则将该区域作为候选目标区域，建立该区域的颜色模型。

（4）将参考目标的颜色模型与候选目标的颜色模型进行相似性度量，如果相似性大于一定阈值则认为是同一目标，否则为不同目标。

2.2 目标三维重建

左右摄像机目标匹配完成之后，利用最小二乘法就可进行目标空间位置的计算，假设空间一点p(x，y，z)在两个成像平面上的图像坐标分别为p1(ul,vl)和p2(ur,vr)，两个摄像机的投影矩阵分别为Ml和Mr，根据成像模型可得：

式中，Ml和Mr表示投影矩阵，将公式（1）、（2）分别消去Zl、Zr，得：

并将其化成矩阵的形式可得到：

利用最小二乘法，解得p(x，y，z)的值为：

即P(x,y,z)为目标所求的三维坐标信息。

3 机器人自定位

机器人在对目标人搜寻之前，首先要获得本身在世界坐标系下的位置，机器人的定位精度对目标人的寻找有很大的影响，而机器人本体的里程计定位精度很低，难以满足机器人定位要求，因此，采用智能空间下StarGazer红外定位系统并融合里程计来实现机器人的实时精确定位。图3为StarGazer系统红外发射/接收器与被动式红外标签。

图3 StarGazer红外定位系统

本文将4×4矩阵式标签布置在智能空间的天花板上，将发射/接收器安装在机器人上方的平台上，当发射/接收器检测到标签时就通过串口将其相对于标签的位姿（包括X、Y轴分量和相对角度信息）传递给机器人，由于标签的ID号唯一，且标签在世界坐标系中的位置己知，通过坐标变换机器人就可获取自身在世界坐标系中的位姿，实现自定位，然后机器人通过A*算法[10]进行路径规划找到目标人当前的位置；StarGazer系统的定位精度很高，误差在2 cm以内，但存在数据跳变，导致定位不稳定，将里程计与StarGazer的数据进行融合可解决数据跳变的问题，提高机器人定位的稳定性。

4 基于激光的人腿轮廓聚类分析

首先智能空间服务器将分布式视觉得到的目标的世界坐标转化到机器人本身地图的坐标系上，然后经无线网络与机器人本体通信，把目标坐标发送给机器人，机器人依坐标信息可搜寻当前目标人所在的位置；然后机器人采用激光传感器扫描人腿[11]通过近邻点聚类分析来确定目标人。

通过统计实验提取腿部的基本特征，如人腿模型近似一对圆弧形状，人腿的直径约为90～150 mm，人在站立时两腿之间的距离一般为10～40 mm左右，人在正常行走时左腿和右腿前后之间的距离在0～400 mm之间变动，因此通过腿部的这些属性可以很容易地从干扰物中辨识出人的左右腿，例如室内桌椅腿的宽度大概为10 mm左右，垃圾桶轮廓与人腿轮廓相似，都为圆弧形，但垃圾桶圆弧长远远大于人腿。检测到的人被视为目标进行跟踪，图4是基于近邻点聚类通过激光扫描得到腿部边缘信息，红线表示通过点的聚类得到的腿部轮廓，白色区域为人腿所在的位置，中间空心原点为两腿的中心点。

图4 基于近邻点聚类得到的腿部轮廓

当机器人与人进行人机交互时，两者之间距离太近（0～0.4 m），会使人产生不安和厌恶感，距离太远（大于2 m），反而会丢失目标，因此使机器人与目标人之间的距离保持在1.3 m左右。图5是激光检测到人腿的坐标图，以两腿的中心点作为人的位置。

图5 激光对人腿的检测和位置计算

设人的中心点Xφ与机器人前进方向夹角为θ，激光与中心点的距离为d，左腿X1和右腿X2与激光Ylaser的夹角分别为α和β，且与激光的距离分别为Lα和Lβ，由图中三角关系可得：X1laser=lαsinα；Y1laser=lαcosα；X2laser=lβsinβ；Y2laser=lβcosβ，所以目标人的中心点坐标：

5 基于优化迭代扩展卡尔曼滤波算法的异质传感器信息融合

为保证机器人对目标人实时的准确定位与跟踪，采用优化的迭代扩展卡尔曼滤波[12-13]将激光传感器与分布式视觉得到的目标人的位姿信息进行融合；激光可以直接探测到机器人与目标人的距离信息，所以对激光信息的预处理只需要进行相应的坐标转换；而视觉系统仅能提供图像的灰度或彩色信息，这种信息不是距离信息，因而不能够直接和激光的探测信息进行融合，所以，在视觉信息的预处理中还需要通过特征匹配和三维重建的方法来获取立体视觉中的深度信息，以更好地实现对异质传感器信息[14]的融合。图6为传感器信息处理与融合过程。

图6 传感器信息处理与融合过程

本文研究采用的系统运动及观测模型是非线性的，为了减少线性化误差，提高非线性滤波性能，将迭代滤波理论引入到扩展卡尔曼滤波方法中，重复利用观测信息，得到更高的估计精度。并利用Levenberg-M arquardt方法优化迭代[15]过程，增加迭代算法的稳定性，针对本文研究的非线性传感器系统，建立离散时间系统模型f和非线性观测模型h：

首先预测目标人的坐标，计算预测系统状态及协方差：

计算预测系统状态、观测预测及协方差：

其中修正的协方差阵：

计算卡尔曼滤波增益矩阵：

更新协方差矩阵、系统状态：

其中，I为单位矩阵，Levenberg-Marquardt参数ui设定为0.1，使用参数ui对预测协方差阵进行修正，保证算法具有全局收敛性，准确地估计目标人在每一时刻的状态。

6 实验结果与分析

6.1 实验环境设计

本文实验所用的分布式视觉由四台TOSHIBA GiantDragon CSGV 90CC3组成，最高分辨率可达640× 480像素，最大传输速率为90 f/s；机器人以山东大学服务机器人实验室自主研发的病房巡视机器人作为研究平台，上位机操作系统为W indow s XP系统，软件开发平台应用Microsoft Visual Studio 2008，本体携带几种传感器设备，如StarGazer、以及激光传感器等。实验用的激光传感器为HOKUYO公司产品UTM-30LX 2D型激光扫描测距仪，安装在机器人距离地面30 cm处，角度分辨率为0.25°，具有高精度、高分辨率、宽视场等特点。

6.2 智能空间下的目标检测和定位

在智能空间下可能会存在多个移动目标，需要对两个摄像机检测到的移动目标区域进行匹配，如果视野内两个候选目标都与选定目标的颜色相似，通过颜色信息就不能进行区分，这时引入极线约束进行判断，如图7所示在左图像中选定目标的头顶点，在右图像中做该点的极线约束方程，则该方程一定通过目标点在右图像中的投影点。提取右图像中所有轮廓的最高点，离该极线方程最近的点所对应的轮廓区域，就是所要确定的匹配区域。

图7 存在干扰目标时的目标检测

考虑到人在房间内运动时，身体容易受到桌子、椅子等物体的遮挡，但头部一般不会被遮挡，因此，在计算目标的位置时，选择头顶作为特征点；目标在空间中的运动是连续的，头顶的位置不会发生突然大距离的变化，如果两个轮廓相交，此时目标的头顶位置可能会发生很大的变化，对此也进行判断，同时用跟踪框的中心点代替头顶点进行计算。如图8所示上方为左右摄像机拍摄的图像，此时用蓝色的跟踪框来跟踪目标。左下方显示室内的实验环境地图，在地图上用红色线来描述目标的移动轨迹，头顶位置用红色圆圈表示，右下方为获得人的定位坐标，此刻定位的坐标为（537 cm，156 cm）。

图8 分布视觉的定位界面

其次，为了证明优化迭代的扩展卡尔曼滤波融合算法在目标人定位中的准确性，设计了一组实验，以坐标点（50 cm，460 cm）为起始点，（450 cm，460 cm）为终止点画一条直线，作为目标人的行走轨迹；行人的速度范围控制在0.3 m/s到1.0 m/s，首先用分布式视觉单独对目标人进行全局定位，获得目标人在行走过程中的全局世界坐标。然后用全局视觉与激光结合的方法进行目标人的定位与跟踪，用EKF估计得到目标人的全局世界坐标。图9为设计实验图。

图9 目标人定位实验图

实验数据如图10所示，采用分布式视觉进行机器人的全局定位虽然精度不低，但曲线振荡幅度比较大，误差在20 cm以内。而采用优化迭代的扩展卡尔曼滤波融合算法将两种传感器信息融合的定位方式，全局定位曲线更接近期望的路径，而且保持了较好的连续性，变化幅度也较小，误差基本保持在10 cm以内。

6.3 同时机器人定位与目标跟踪

为了证明在智能空间下本文所提出方法的可行性，实验中人沿着所设定的4.5 m×8 m的方框（黄线）路径行走，起点即为终点，实验之前首先将视觉与激光的观测数据时间经时间配准[16]，考虑到网络传输时间和数据处理时间的延迟，采样时间为160 ms，为了显示融合算法的精确性，在智能空间服务器地图中将视觉得到的人行走路径用红线画出，而经优化迭代的EKF数据融合得到的人行走轨迹用黑线画出，机器人跟踪人得到的轨迹用绿线标明。由图11可以看出融合之后人的轨迹更接近所设定的轨迹，机器人相对于行人的运行速度较慢，跟随在行人后面，轨迹显示该方法能精确地实现对目标人的跟踪，并计算目标人在不同时间段采用不同方式定位的平均误差，结果如表1显示，采用EKF融合算法得到的定位精度明显优于单一传感器的定位精度。

图11 机器人跟踪目标人的轨迹

表1 目标人在不同时间段的定位平均误差cm

7 结束语

在智能空间下提出了用家庭陪护机器人去实时和准确地跟踪目标人的方法，并且用实验证实了该方法的可行性。全局环境下借助智能空间去识别和跟踪机器人视野之外的目标人，扩大了机器人跟踪目标的范围，同时机器人本体激光能准确地计算机器人与目标的距离，用优化迭代的EKF融合异质传感器的信息以弥补每个传感器的测量弱点，得到目标人的准确位姿和速度信息，提高机器人跟踪目标的可靠性。在接下来的工作中，将着重研究机器人与人怎么保持一个灵活的距离，如机器人的任务是搬运行李，应保持与人一定距离。如果需要机器人和人类之间的密切互动，机器人应接近人类，让人类可以触摸到机器人，以更好地实现人性化导航。

[1]谭民，王硕.机器人技术研究进展[J].自动化学报，2013，39（7）：963-972.

[2]Ercan A O，Gamal A E，Guibas L J.Object tracking in the presence of occlusions using multiple cameras：a sensor network approach[J].ACM Transactions on Sensor Networks（TOSN），2013，9（2）.

[3]Vadakkepat，Prahlad.Multimodal approach to human-face detection and tracking[C]//IEEE Transactions on Industrial Electronics，2008：1385-1393.

[4]Morioka，Kazuyuki.Human tracking based on integration of laser range scanners and cameras in intelligent space[C]// 2011 8th Asian on Control Conference（ASCC）.[S.l.]：IEEE，2011.

[5]Dai Wen，Cuhadar A，Liu P X.Robot tracking using vision and laser sensors[C]//IEEE International Conference on Automation Science and Engineering.[S.l.]：IEEE，2008.

[6]Rampinelli，Mariana.Use of computer vision for localization of a robotic wheelchair in an intelligent space[C]// Biosignals and Biorobotics Conference（BRC）.[S.l.]：IEEE，2013.

[7]李国栋，田国会，周凤余.服务机器人智能空间中顶棚投影器建模与外参数标定研究[J].控制与决策，2014.

[8]曹焱.基于双目立体视觉信息的三维重建方法研究[D].长春：吉林大学，2013.

[9]Andreas J，Koch R.Direct model-based tracking of 3d object deformations in depth and color video[J].International Journal of Computer Vision，2013，102（1/3）：239-255.

[10]张紫辉，熊岳山.未知环境下基于A*的机器人路径规划算法[J].计算机工程与科学，2013，34（11）：141-147.

[11]Hoyeon K，Chung Woojin，Yoo Yoonkyu.Detection and tracking of human legs for a mobile service robot[C]// 2010 IEEE/ASME International Conference on Advanced Intelligent Mechatronics（AIM）.[S.l.]：IEEE，2010.

[12]Mohammad A，Park Sang-Young.Simultaneous spacecraft attitude and orbit estimation using magnetic field vector measurements[J].Aerospace Science and Technology，2011，15（8）：653-669.

[13]杨宏，李亚安，李国辉.一种改进扩展卡尔曼滤波新方法[J].计算机工程与应用，2010，46（19）：18-20.

[14]Bruno M，Delahoche L.An evidential approach for detection of abnormal behaviour in the presence of unreliable sensors[J].Information Fusion，2012，13（2）：146-160.

[15]Fang Huazhen，Wang Yebin.State of charge estimation for lithium-ion batteries：An adaptive approach[J].Control Engineering Practice，2014，25：45-54.

[16]彭焱，徐敏毓，金宏斌.多传感器数据融合系统中时间配准算法分析[J].雷达与对抗，2005，2（2）：16-19.

SANG Sen,TIAN Guohui,DUAN Peng,WU Hao

School of Control Science and Engineering,Shandong University,Jinan 250061,China

For a wide range of detection and tracking of target for a mobile service robot problem under the unknown environment,this article puts forward a method based on the heterogeneous information fusion structure to realize the target detection and tracking in real-time of robots,whose basis is distributed intelligent network devices and the robot’s 2D laser range finder in the intelligence space.The system matches the moving target by the information on color,and carries out the 3D reconstruction of the target depending on the triangulation technique and least square method.The moment it detects the moving target,in order to get the exact distance information,it w ill scan the target human’s leg by the laser range finder to cluster the nearest neighbor.Meanwhile,this article raises a better Iterative Extend Kalman Filter for the heterogeneous sensor information fusion to realize the simultaneous robot localization and target tracking based on the intelligence space.The experimental results verify the effectiveness of the proposed method.

target tracking;distributed intelligent network devices;extend Kalman filter;heterogeneous sensor information fusion

A

TP242.3

10.3778/j.issn.1002-8331.1402-0261

SANG Sen,TIAN Guohui,DUAN Peng,et al.Target track ing based on heterogeneous sensor in formation fusion in intelligent space.Computer Engineering and Applications,2014,50（16）：48-53.

国家高技术研究发展计划（863）（No.2009AA 04Z220）；山东省自然科学基金资助课题（No.ZR2011FM 011）。

桑森（1987—），男，硕士研究生，研究领域为机器人，多传感器信息融合；田国会（1969—），男，工学博士，博士后，教授、博士生导师，研究领域为：服务机器人，智能空间，多机器人系统的协调与协作。E-mail：g.h.tian@sdu.edu.cn

2014-02-24

2014-03-21

1002-8331（2014）16-0048-06