IEEE2018|An Accurate and Real-time 3D Tracking System for Robots

↑ 点击蓝字关注极市平台 识别先机 创造未来

来源：https://zhuanlan.zhihu.com/p/34920240

作者：MM Wang(著有知乎专栏：目标跟踪算法)

论文题目：Accurate and Real-time 3D Tracking for the Following Robots by Fusing Vision and Ultra-sonar Information

论文链接：已经被IEEE/ASME Transactions on Mechatronics, 2018收录，发表时公开文章。

论文作者：Mengmeng Wang (@MM Wang), Yong Liu, Daobilige Su, Yufan Liao, Lei Shi and Jinhong Xu

1、Motivations

出发点是想在机器人上实现一套能够稳定跟踪目标的系统，实时获得目标的三维位置信息。对于熟悉的视觉模块，能够得到图像坐标系中的位置信息，但无法获得目标与机器人的距离信息，并且一旦目标运动出图像范围，机器人很难进行运动决策。

实验室刚好有同学研究的是超声波跟踪，超声波跟踪恰好能够获得目标与机器人之间的距离信息，但无法获得其他信息，且超声波容易受反射影响。因此，我们考虑将这两种传感器进行融合，实现移动机器人平台的三维目标跟踪系统。

为了验证提出的三维跟踪系统的有效性，我们在仿真平台和实际的移动机器人平台都做了大量的实验。实验结果表明，本章提出的跟踪系统在仿真平台以及实际移动机器人上跟踪目标人物的精度和实时性都满足要求，且鲁棒性高。

综上，本章的提出的算法的贡献与创新可以总结为以下三点：

• 提出了一个三维人体跟踪系统，并在仿真平台和移动机器人平台上都实现了该系统。

• 异步地结合了视觉传感器和超声传感器信息，通过扩展卡尔曼滤波（EKF）进行融合得到三维位置信息。

• 利用目标的三维位置信息，提出了一种新的尺度估计方法，进一步提升跟踪算法的精度。

2、Method

文章提出了一个机器人实时三维人体跟踪系统，系统包括三个子模块︰ 单目相机传感器跟踪模块、 超声波传感器跟踪模块和多传感器信息融合模块。

将目标的状态定义为目标人物穿戴超声波发射器的位置，其中下标k表示第k个时序段。图1是在机器人局部坐标系示意图。目标的位置信息会由视觉传感器和超声波传感器共同感知。

由于主要进行人体的跟踪，需要选择的跟踪算法对人体的形变、遮挡及丢失高度鲁棒，并且算法必须高度实时、可靠。为此，在视觉传感器跟踪模块，由于这个工作在CVPR之前完成的，所以我们当时用到了LMCF的雏形来提供部分的二维位置估计，旨在跟踪的过程中能够进行遮挡和目标丢失的判断、 尺度估计这两方面。

超声波传感器跟踪模块则用来计算从目标到机器人的范围信息，提供视觉无法得到的另一方向的二维位置估计。最后，多传感器信息融合模块采用扩展卡尔曼滤波异步融合视觉和超声波两个不同频率的数据流。

图1 机器人局部坐标系

2.1 单目视觉传感器跟踪模块

单目相机安装在超声波接收器顶部，两者都安装在移动机器人平台的最前端。视觉传感器的观测模型是一个式(1)所示的简单的相机映射模型。

(1)

其中是目标在图像坐标系中的位置，将由第三章提出的视觉目标跟踪算法进行计算，和分别是相机的外参和内参。

对于传统的视觉目标跟踪问题而言，需要跟踪的目标是在第一帧中由人工标注或者某种检测子给出的。而在本章的三维目标跟踪系统中，初始的目标框是由初始的三维位置和目标人物的身高h通过三维到二维的投影计算得到的。

在所有的实验中，假设人的宽度是0.4米，并将超声波发射器穿戴在目标的50%的身高处。那么初始的三维位置信息，包括目标的左边框，右边框，头部，和脚可以如式（2）这样计算：

（2）

图像坐标系中，目标初始的宽，高，和目标的中心可以这样计算：

（3）

其中，是式（1）中的和图像坐标系中u轴的值。和分别对应和在图像坐标系中v轴的值。

由于移动机器人平台的特殊性，需要选择的跟踪算法对人体的形变、遮挡及丢失高度鲁棒，并且算法必须高度实时、可靠。本章的视觉跟踪算法用的是LMCF的雏形，也是从这里产生了LMCF中的APCE的灵感。

对移动机器人实现稳定视觉跟踪很重要的一点是对遮挡和目标丢失这两种情况的处理，为此，我们当时用到了APCE的前身PCE（式（4））来作为模型更新策略，利用高置信度的前向跟踪结果的反馈信息来制定模型更新策略，从而避免模型腐蚀问题，使得机器人能够有效的判断目标是否被遮挡或丢失。

除此之外，针对移动机器人平台，本系统利用得到的目标的三维位置信息，提出了一种新的尺度估计方法，进一步提升跟踪算法的精度。

当机器人跟踪一个目标时，机器人和目标相对的运动速度一直在变化，因此单目相机拍到的目标的大小也在不断变化，跟踪系统需要对目标的尺度变化有足够的鲁棒性，才能准确地跟住目标。

为了在二维视觉跟踪过程中能够适应目标的尺度变化，这里采用DSST的尺度估计方法。首先，由平移模型用细致特征找到当前帧中目标的中心位置，在这个位置产生一组不同尺度的框，用粗糙特征与尺度相关滤波器进行相关运算，寻找匹配度最高的一个尺度。与此同时，的标准差由的不确定性计算得到。

为了进一步提升尺度计算的精度，本系统同时利用三维位置估计来计算尺度。在第k帧中，计算得到的三维位置为，由于目标的身高h在跟踪过程中是固定的，可以由式(2)可以计算得到目标的头部的位置和脚的位置。

将和通过式(1)映射到图像坐标系可以得到和，分别对应目标的头部和脚在图像坐标系中v轴的值。假定目标的宽度和高度的尺度变化是同步的。那么三维位置信息得到的尺度为：

其中，是由式(3)得到的目标的初始高度。最后，第k帧的尺度由下式计算得到：

其中是的不确定性，是第k个估计状态的协方差矩阵。

2.2 超声波传感器跟踪模块

本系统用传统的超声波传感器来获取目标与机器人之间的距离信息。传统的超声波传感器使用传播时间和三角测量来计算发射源与接收器之间的距离。

我们将超声波发射器设计为一个可穿戴装置，穿戴在被目标人体上。对应的超声波接收器由两个超声单元组成，放置在机器人的最前方。为了匹配发射器和接收器，引入无线射频模块RF（Radio Frequency Wireless Module）在发射器和接收器上来发射和接收射频信号。

当机器人前方的超声波接收器上安装的射频模块接收到了发射器上的射频模块发射的射频信号时，接收器上的射频模块会开启计时器并对发射器上的射频模块发送响应信号。当发射器接收到这个响应信号后，便开始发射超声信号。

于是，从计时器开始计时到超声波接收器接收到超声波信号的这段时间就是超声波的传播时间。这个时间实际上包括了射频模块的响应信号的接收时间。经过大量实验，我们多次测量发现这个响应时间是一个固定的值，表示为。

图2 超声波传感器跟踪模块示意图。图中黑色的坐标系表示机器人坐标系，黑色的点表示超声波发射器在这个坐标系中的三维位置，红色的点表示超声波发射器在超声平面中的二维位置信息，d1,d2分别表示超声波发射器到接收器的距离。

如图2所示，超声波发射器在超声平面中的位置表示为可从下式计算得到：

（7）

其中v表示声速，一般认为是340 m/s，D表示超声波接收器上两个超声接收单元之间的距离，，是它们各自的传播时间。

转换到机器人坐标系的系统状态，超声波传感器模块期望的观测值表示为：

其中，

2.3 多传感器信息融合模块

由于两个传感器的更新频率不同，它们中任意一种可用时，本系统的融合算法就会运行。我们采用标准扩展卡尔曼滤波算法来按顺序处理异构的视觉传感器模块和超声波传感器模块的异步信息。

2.3.1 预测

由于没有目标运动的先验知识，本系统用一个速度为常量的随机游走模型来预测机器人坐标系中目标的位置，表示为：

(9)

其中是协方差矩阵，是式(9)第一个式子的雅可比矩阵（对于随机游走模型来说是一个3×3的单位矩阵）。是时间步长之间的运动噪声，成正比于。比例系数是一个常数噪声等级。

2.3.2 更新

当任何一个观测值可用的情况下，系统状态将会进行更新：

(10)

其中下标中的*表示超声或者视觉的观测值，我们将其都称为下一步观测。是提供预测值的传感器模型。视觉传感器模型定义为式(1)，超声波传感器模型定义为式(8)。

是的雅可比矩阵。是实际的传感器观测值。是由视觉跟踪算法估计得到的，是由超声传感器式（7）预测的平均距离值。

是观测噪声。对于超声波模块，是其对应的协方差矩阵。对于视觉模块，观测噪声高置信度指标式（4）定义的PCE成反比。

当PCE越大，说明观测结果的置信度越高，则噪声越小。因此本系统用PCE判据的倒数（为了数量级一致，再除以100）来作为噪声，定义为

因此，相机传感器模块的输入为和都来自与视觉跟踪算法。超声传感器模块的输入为和。

3、实验

为了验证本论文提出的多传感器融合的机器人实时三维目标跟踪系统，我们在仿真平台和真实场景中都做了大量的实验。仿真平台在一个机器人仿真器V-REP（Virtual Robot Experimentation Platform）上实现的，用来验证在理想情况下本论文提出的系统的跟踪能力。另外，我们还将系统实现在一个实际的机器人平台Rock105上，如图1所示。

3.1 实现细节

我们所有的实验的实现平台为MATLAB R2015a，3.2GHz的Intel内核i7 CPU，16GB内存的电脑。用ROS（Robot Operating System）作为Rock105的软件框架，通过MATLAB-ROS包与MATLAB进行通信。

单目相机传感器和超声波传感器安装在Rock105前端的同一垂直平面。视觉跟踪算法由于硬件平台与第三章不同，在机器人平台的跟踪速度为25帧每秒，超声波跟踪的速度为20Hz。目标的身高h可认为是一个参数，由每次跟踪的目标的具体身高进行设定，目标的宽度设为一个统一的值0.4米。

超声波传感器的设置包括两个参数，第一个是两个超声接收单元之间的距离D，Rock105的实际安装距离为0.6米。另外一个参数是RF射频模块的响应时长，经测量得到为0.152毫秒。

视觉跟踪算法的参数设置如下。首先，选择多通道梯度朝向直方图HOG，单元长度为4个像素，以及颜色特征CN的前两个维度。PCE 的判据阈值在所有实验中都设置为0.2，当PCE的值大于0.2时，跟踪模型才会进行更新，反之认为当前帧的跟踪结果不可靠，不进行更新。

仿真平台和真实平台的实验都包括室内室外场景，充分验证本论文提出的三维跟踪系统能在各种场景下工作。本系统采用最简单的微分控制器来控制机器人的运动，使其能够实现自动跟踪。

3.2 仿真平台实验

在仿真平台上不会有真实环境中的噪声干扰或意外情况，因此本论文在仿真环境中验证提出的三维目标跟踪算法的理论框架。仿真机器人设计如图3所示，由两个超声波接收单元组成的超声波接收器安装在机器人前端，单目相机安装在超声波接收器正上方。

图3 仿真机器人示意图

3.2.1 多传感器验证

安全起见，我们只在仿真环境中使用单个传感器进行实验，验证用两个传感器进行融合的必要性。仿真环境中，目标的运动轨迹设置有z方向的波动，类似于正弦函数曲线。

当只用超声波传感器时，得到的三维位置信息中的z轴的精度较低，如图4所示。当只用视觉传感器时，由于没有x轴方向的位置信息，x轴方向的位置估计是不准确的，如图5所示。

当同时用两种传感器进行跟踪时，x和z轴两个方向的位置估计精度会明显比单个传感器高，同时y轴的估计精度也会得到提升。

实验说明融合视觉和超声波两种传感器得到的信息对于估计目标的三维位置信息是十分有必要的，三个方向的跟踪结果都会比单个传感器更加准确和稳定，如图6所示。

图4 只用超声波传感器误差分析图

图5 只用视觉传感器误差分析图



图6 仿真室外场景，双传感器误差分析图

3.2.2 室内室外场景实验

室内场景设计为一个如图7所示的室内办公室，里面有很多人，以及一些办公室常见用品。室外场景则是一个如图8所示的有很多高楼，行人，树木，交通工具等的城市场所。

量化的跟踪结果如图9和图10所示，图中黑色的曲线表示目标运动轨迹的真值，绿色的曲线则表示本论文提出的跟踪系统估计的位置信息。

从图中可以看出，跟踪算法估计的位置误差在三个方向上都很小，两条曲线离得很近，说明本论文提出的跟踪算法在三个方向上的位置估计精度都较高。

图7 仿真室内场景

图8 仿真室外场景

图9 仿真室内误差分析图

图10 仿真室外误差分析图

3.3 真实机器人平台实验

我们将提室外误差分析图出的三维目标跟踪系统实现在图1所示的机器人平台Rock105上。目标的真值用Xtion PRO LIVE asus.com/3D-Sensor/Xti)的骨架跟踪框架中的腰部的骨架点的位置来获取。另外，Xtion的RGB相机作为单目视觉传感器，来拍摄目标的运动过程。如图11与图12所示，室内部分的实验在实验室外研究所走廊进行，室外部分在研究所外进

图12 真实机器人室外场景

真实场景中存在各种各样的影响因素和挑战，如光照变化、遮挡、背景杂乱等。目标在这样的环境中行走给移动机器人的跟踪带来了很大的挑战。

室内场景的跟踪结果如图13所示，室外场景的跟踪结果如图14所示，图中红色的曲线是真实的目标位置变化，绿色的曲线是本系统的室内跟踪结果，蓝色的曲线是室外跟踪结果。实验结果表明本论文提出的跟踪系统用在真实机器人在实际场景下的目标跟踪误差也很小。

图13 真实室内场景跟踪结果误差分析

图14 真实场景室外跟踪结果误差分析

更多实验结果可参考文章。

4、Conclusion

文章提出了一个应用在移动机器人平台上的机器人实时三维目标跟踪系统。结合了单目相机传感器与超声波传感器，并用EKF进行融合。

在视觉模块，用到了LMCF的雏形进行遮挡和目标丢失这两种情况的处理。除此之外，针对移动机器人平台，利用三维的目标位置信息结合二维图像信息来计算目标的尺度变化。

使得本系统的视觉跟踪算法能够在目标遮挡、丢失以及尺度变化等极具挑战的情况下稳定工作。超声模块主要用来提供视觉无法提供的范围信息，即目标与机器人之间的距离信息。

两个模块的工作频率不同，且信息异构，本章用EKF按序处理到达的异步信息，进行两个传感器的信息融合。本系统在仿真平台和实际的移动机器人平台都进行了大量的实验，实验结果表明，本章中提出的系统可以稳定地、高精度并且实时地实现三维目标跟踪。

*推荐文章*

---

PS.极市平台正寻求与开发者视觉算法的合作，欢迎联系小助手（微信：Extreme-Vision）沟通合作~   填写表单http://form.mikecrm.com/wcotd9，即可加入极市CV开发者群，与CV大牛切磋交流~