未知环境基于单目次优视差的多模滤波目标跟踪算法

黄帅 付光远 伍明 岳敏

摘 要：在未知环境下基于单目视觉的机器人同时定位、地图构建和目标追踪的耦合问题（SLAMOT）中，需要足够的视差才能满足目标跟踪的可观性条件。同时，针对目标运动的不确定性以及系统对于目标运动方式的未知性，提出一种基于次优视差的多模滤波目标跟踪算法。首先，采用目标不确定性椭球投影面积变化最大的方向为次优视差方向，并将其作为机器人视差控制方向;然后，采用多模滤波算法计算目标各种运动方式的概率;其次，对各运动方式的目标状态进行估计，最后根据各运动方式的概率加权估计出目标状态。另外，考虑到工程应用中应减小能耗，因此，在满足目标跟踪要求的条件下，降低视差速度。仿真实验表明：视差速度为0.3m/s时，次优视差算法的残差均值为0.16m，而启发式算法、多模滤波算法、传统扩展卡尔曼滤波（EKF）算法的残差均值为0.25m、0.06m和0.16m。在视差速度较低时，所提算法也能满足目标跟踪的可观性条件，具有较强的工程应用价值。

关键词：目标跟踪;同时定位与地图构建;多模滤波;次优视差;控制策略

中图分类号： TP249

文献标志码：A

文章编号：1001-9081（2019）03-0864-05

Abstract： Under unknown environment， Simultaneous Localization， Mapping and Object Tracking （SLAMOT） based on monocular vision needs sufficient parallax to meet the observability condition of object tracking. Focused on the uncertainty of target motion and the unknown of the system on target motion mode， a multi-mode filtering target tracking algorithm based on monocular suboptimal parallax was proposed. Firstly， the direction in which the target uncertain ellipsoid projection area changed the most was selected as the suboptimal parallax direction and was used as the robot parallax control direction. Then， multi-mode filtering algorithm was used to calculate the probability of different motion modes of the target， estimating the target state of different motion modes. Finally， the target state was estimated according to the probabilistic weighting of each motion mode. The simulation results show that the residual error of suboptimal disparity algorithm is 0.16m when the parallax velocity is 0.3m/s， meanwhile the residual means of heuristic algorithm， multimode filtering algorithm， traditional Extended Kalman Filter （EKF） algorithm are 0.25m， 0.06m and 0.16m respectively. Besides， when the parallax speed is small， the proposed algorithm also can satisfy the observability condition of target tracking， having important engineering application value.

Key words： object tracking; Simultaneous Localization And Mapping （SLAM）; multi-mode filtering; suboptimal parallax; control strategy

0 引言

隨着计算机和感知技术的不断发展，机器人领域逐渐成为学者们研究的热点。未知环境下机器人目标跟踪是指移动机器人在陌生环境中对运动目标的空间状态进行估计的过程。而目标状态估计是以移动机器人自身状态估计为基础的，要在未知环境中实现移动机器人自身状态估计，实际就是解决同时定位与地图构建（Simultaneous Localization and Mapping， SLAM）问题。实际就是解决机器人SLAM同时定位与地图构建（Simultaneous Localization and Mapping， SLAM）问题。因此，未知环境下机器人目标跟踪所研究的是机器人同时定位、地图构建与目标跟踪（Simultaneous Localization， Mapping and Object Tracking， SLAMOT）问题，它能够实现机器人对自身状态、环境状态和目标状态的同时估计。

2002年，卡内基梅隆大学Wang等[1]在中首次提出将SLAM和动态目标侦测和跟踪（Detection and Tracking of Moving Objects， DTMO）作为一个耦合问题来处理的思想，该方法以栅格地图差异性比较为基础，并利用智能导航车Naclab11平台成功地进行了城市环境地图构建，结果验证了其有效性。此后，Wang在[2-4]文献[2-4]中系统地给出了该问题基于贝叶斯理论的解决框架，方法基于两种假设条件：首先，假设系统能够成功区分环境特征和目标的观测值。其次，假设目标观测值只影响目标的状态更新而对机器人和环境特征状态没有影响，该假设实际上是将SLAM和DTMO问题作为两个独立过程来考虑，并没有考虑机器人和目标间的相关性对系统状态估计的影响。Vu等[5]运用全局邻域法（Global Nearest Neighborhood， GNN）进行动态物体检测并用扩展卡尔曼滤波（Extended Kalman Filter， EKF ）对运动物体进行跟踪，但该方法假设物体运动模态唯一，因此不能解决机动目标跟踪问题。Wang等[6]介绍了一种基于交互模态的机动目标跟踪方法，使系统能够对动静（Move-Stop）模态运动的目标进行跟踪，但该方法依然假设机器人和目标的状态估计相互独立。以上研究成果都是基于主动式传感器系统，而这类传感器存在体积大、质量重、消耗能量多、设备造价高、观测距离有限等缺陷，这些缺陷限制了SLAMOT的实际应用范围。单目视觉传感器，凭借其体积小、耗能少、造价低廉、细节呈现度高及直观性好等特点，正逐渐受到机器人学界的重视。目前，尽管存在利用单目传感器解决SLAM问题的方法，但这些方法并未同时考虑目标跟踪（Object Tracking， OT）问题。

对于基于单目视觉的SLAMOT问题来说，首先，由于目标的动态性和运动模式未知性，已有的基于视觉的SLAM估计方法（例如，反转深度法[7-9]、批优化方法[10-12]）将无法有效和实时地完成目标状态估计，因此无法满足目标跟踪的实际需求;其次，单目视觉目标跟踪等同于纯方位角目标跟踪问题，学界称之为：方位角目标运动分析问题（Bearing Only Target Motion Analysis， BOTMA）。为了满足BOTMA可观性条件，通常要求观测平台相对于目标机动运动，而传统BOTMA方法假设在机动过程中观测平台状态已知，但在实际应用中，则需要首先解决观测平台自身的状态估计问题。同时，为了保证观测平台运行的安全性，系统还需对外界环境状态进行估计。最后，已有的未知环境下移动平台运动目标跟踪方法，一般采用多传感器信息融合来实现移动平台和目标状态的独立估计，多传感器的使用限制了研究成果的应用范围并且降低了问题的难度，而独立估计方法没有考虑不同对象状态的耦合性问题，因此影响了系统状态估计的准确性。

经过学者们长期的研究，目前，SFM（Structure From Motion）和滤波的方法成为了解决单目SLAM问题的主流[13-14] 。滤波的方法运算较快，能较好满足SLAM实时性需求。而SFM的方法[15]，通过大量计算达到优化的目的，整体估计精度较高，但实时性较差。为了满足后续目标跟踪实时性要求，并解决目标的动态性和运动模式未知性问题，本文选用多模滤波的方法对单目SLAMOT问题进行研究。在对目标状态估计时，会在三维空间产生不确定性椭球，如果不对机器人进行适当的控制，不确定性椭球就会累积得越来越大，最终导致跟踪过程失败。为了避免这种情况，一方面，需要控制机器人产生足够的视差，使得目标保持可观性;另一方面，需要控制机器人接近目标，从而不跟丢目标。

1 模型介绍

在利用EKF框架估计和更新目标的状态的过程中，根据协方差矩阵和估计目标的坐标可知，目标的实际位置在以某一点为中心的三维椭球中。为了更好地跟踪目标，需要制定相应的控制策略，使机器人既能跟踪上目标，又能更准确地估计出目标的状态。为此，本文将控制速度分解为两个方向的控制速度：1）跟随速度;2）视差速度。不妨设机器人能达到的最大速度为v0，跟随速度最大为v1，视差速度最大为v2。则有：

1.1 跟随速度选取

選取机器人位置与目标位置连线方向作为跟随速度方向。制定合适的跟随速度大小，使机器人既能跟踪上目标，又能减少能耗（降低机器人速度）。直观来看，当机器人与目标距离相对较大的时候，跟随速度应该相应地提高，这样才不至于跟丢目标;当机器人与目标距离较小的时候，跟随速度应当适当降低，以减少机器人能量的损失。为此，设计相应的跟随速度大小与机器人到目标距离的函数：

1.2 视差速度选取

根据协方差矩阵和目标的估计位置，可以求出目标在三维空间中的不确定性椭球。为了量化视差大小，可以将视差问题转化为三维椭球投影面积变化大小问题，最后，将其抽象为一个三维空间求梯度问题。推导求解出机器人视差速度方向过程如图1。

1.3 多模滤波模型

2 仿真结果

本文先在传统EKF_SLAM（Extended Kalman Filter—Simultaneous Localization and Mapping）框架内，分别研究次优视差算法和启发式算法的跟踪效果。在此基础上，再研究多模滤波算法的跟踪效果。

在人们的先验认知中，平行椭球最短轴会产生较大的视差（启发式算法）。为了验证次优视差算法的可行性，将启发式算法与次优视差算法进行对照实验。通过对比目标的实际位置与估计位置的残差均值大小，评定更优的方法。

实验所用电脑为DELL INTEL I5 双核，Matlab仿真实验软件，以及Toolbox工具箱。

不难想象，视差速度将影响视差大小，从而影响对目标状态的估计效果。为此，分别设置视差速度从0.1m/s到0.5m/s，研究其对跟踪效果的影响，得到表1。

为了更加直观地反映表1中数据，将其转换为柱状图的形式如图3和4所示。

由于视差速度为0.1m/s时，启发式方法的残差均值过大，影响对整体效果的观测，故剔除视差速度为0.1m/s的点，得到图4。

当视差速度分别为0.1m/s（较小）、0.3m/s（中等）和0.5m/s（较大）时，通过仿真，得到启发式算法和次优视差算法的结果如图5所示。

当视差速度大小为0.1m/s时，由图5（a） 和图5 （b）可以看出，次优视差算法残差均值为0.46m远远小于启发式算法残差均值18.2m;当视差速度为0.3m/s（中等）时，由图5（c）和图5 （d）可以看出，启发式算法残差均值为0.25m，次优视差算法残差均值为0.16m;当视差速度为0.5m/s（较大）时，由图5（e）和图5（f）可以看出，启发式算法和次优视差算法残差均值分别为0.13m、0.1m。由此可见，次优视差算法跟踪效果比启发式算法好，但随着视差速度提高，两种算法差距越来越小。

当视差速度为0.5m/s时，得到次优视差算法的目标跟踪轨迹如图6所示。

而在实际情况中，机动目标大多数是以多模态运动的。因此，用多模滤波的方法对机动目标进行跟踪显得十分必要。分别用传统EKF_SLAM算法和多模滤波算法对机动目标进行跟踪。在仿真实验中，设定目标1～100帧及451～520帧为定速模型;101～450帧及 521～550为转弯模型（转弯时速度大小保持不变）;551～600帧为加速模型。得到轨迹如图7所示。

在此基础上，分别计算两种算法的残差均值，得到图8。

由图8（a）和图8（b）可以看出，传统EKF_SLAM目标跟踪残差均值为0.15m，而多模滤波目标跟踪残差均值仅为0.06m。另外，在目标模态变化时，多模滤波目标跟踪能更准确地预测目标状态。

3 结语

本文通过数学推导证明本文方法的理论性，通过仿真实验验证了本文方法的可行性，同时根据仿真实验结果可以得出以下结论：

1）在视差速度大小相同的情况下，本文算法要優于启发式算法。

2）当视差速度较小时，启发式算法误差很大，导致跟踪失败，而本文算法误差较小，能保持跟踪正常进行，这说明本文方法能够节省能耗，具有工程应用价值。

3）随着控制速度增大，所获得的视差越大，两种方法对目标状态的估计误差越小，同时两种方法之间估计的差距也越小。

4）对于机动目标，多模滤波的目标跟踪算法估计效果更准确，其残差均值不到传统EKF_SLAM目标跟踪算法的1/2。

5）视差方向的选取和速度大小的控制对目标跟踪影响大， 说明对于单目SLAMOT问题，控制策略的研究十分必要。

参考文献 （References）

[1] WANG C-C， THORPE C. Simultaneous localization and mapping with detection and tracking of moving objects [C]// Proceedings of the 2002 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2002： 2918-2924.

[2] WANG C-C， THORPE C， THRUN S， et al. Simultaneous localization， mapping and moving object tracking [J]. International Journal of Robotics Research， 2007， 26（9）： 889-916.

[3] WAN K W， WANG C-C， TON T T. Weakly interacting object tracking in indoor environments [C]// Proceedings of the 2008 IEEE Workshop on Advanced Robotics and Its Social Impacts. Piscataway， NJ： IEEE， 2008：1-6.

https：//www.csie.ntu.edu.tw/～bobwang/Papers/wan_arso08.pdf

[4] WANG C-C， THORPE C， THRUN S. Online simultaneous localization and mapping with detection and tracking of moving objects： theory and results from a ground vehicle in crowded urban areas [C]// Proceedings of the 2003 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2003，1： 842-849.

[5] BAIG Q， VU T-D， AYCARD O. Online localization and mapping with moving objects detection in dynamic outdoor environments [C]// Proceedings of the 2009 IEEE 5th International Conference on Intelligent Computer Communication and Processing. Piscataway， NJ： IEEE， 2009： 190-195.

[6] WANG C-C， LO T-C， YANG S-W. Interacting object tracking in crowded urban areas [C]// Proceedings of the 2007 IEEE International Conference on Robotics and Automation. Piscataway， NJ： IEEE， 2007： 4626-4632.

[7] CIVERA J， DAVISON A J， MONTIEL J M M. Inverse depth parametrization for monocular SLAM [J]. IEEE Transactions on Robotics， 2008， 24（5）： 932-945.

[8] DAVISON A J. Real-time simultaneous localisation and mapping with a single camera [C]// ICCV '03： Proceedings of the 9th IEEE International Conference on Computer Vision. Washington， DC： IEEE Computer Society， 2003， 2： 1403.

[9] DAVISON A J， REID I D， MOLTON N D， et al. MonoSLAM： real-time single camera SLAM [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2007， 29（6）： 1052-1067.

[10] ESS A， LEIBE B， SCHINDLER K， et al. Robust multiperson tracking from a mobile platform [J]. IEEE Transactions on Pattern Analysis and Machine Intelligence， 2009， 31（10）： 1831-1846.

[11] ESS A， LEIBE B， SCHINDLER K， et al. Moving obstacle detection in highly dynamic scenes [C]// Proceedings of the 2009 IEEE International Conference on Robotics and Automation. Piscataway， NJ：  IEEE， 2009：56-63.

[12] ESS A， LEIBE B， SCHINDLER K， et al. A mobile vision system for robust multi-person tracking [C]// Proceedings of the 2008 IEEE Conference on Computer Vision and Pattern Recognition. Piscataway， NJ： IEEE， 2008： 1-8.

[13] CIVERA J， GRASA O G， DAVISON A J， et al. 1-point RANSAC for extended Kalman filtering： application to real-time structure from motion and visual odometry [J]. Journal of Field Robotics， 2010， 27（5）： 609-631.

[14] 李慧平，徐德民，張福斌，等.基于量测噪声和观测次数的EKF-SLAM一致性分析[J].自动化学报，2009，35（9）：1177-1184.（LI H P， XU D M， ZHANG F B， et al. Consistency analysis of EKF-based SLAM by measurement noise and observation times [J]. Journal of Automation， 2009， 35（9）： 1177-1184.）

LI H-P， XU D-M， ZHANG F-B， et al. Consistency analysis of EKF-based SLAM by measurement noise and observation times [J]. Acta Automatica Sinica， 2009， 35（9）： 1177-1184.

[15] 梁明杰，闵华清，罗荣华.基于图优化的同时定位与地图创建综述[J].机器人，2013，35（4）：500-512.（LIANG M J， MIN H Q， LUO R H. Graph-based SLAM： a survey [J]. Robot， 2013， 35（4）： 500-512.）