黃帥 付光遠 伍明 岳敏

摘 要：在未知環(huán)境下基于單目視覺的機器人同時定位、地圖構建和目標追蹤的耦合問題（SLAMOT）中，需要足夠的視差才能滿足目標跟蹤的可觀性條件。同時，針對目標運動的不確定性以及系統(tǒng)對于目標運動方式的未知性，提出一種基于次優(yōu)視差的多模濾波目標跟蹤算法。首先，采用目標不確定性橢球投影面積變化最大的方向為次優(yōu)視差方向，并將其作為機器人視差控制方向;然后，采用多模濾波算法計算目標各種運動方式的概率;其次，對各運動方式的目標狀態(tài)進行估計，最后根據各運動方式的概率加權估計出目標狀態(tài)。另外，考慮到工程應用中應減小能耗，因此，在滿足目標跟蹤要求的條件下，降低視差速度。仿真實驗表明：視差速度為0.3m/s時，次優(yōu)視差算法的殘差均值為0.16m，而啟發(fā)式算法、多模濾波算法、傳統(tǒng)擴展卡爾曼濾波（EKF）算法的殘差均值為0.25m、0.06m和0.16m。在視差速度較低時，所提算法也能滿足目標跟蹤的可觀性條件，具有較強的工程應用價值。

關鍵詞：目標跟蹤;同時定位與地圖構建;多模濾波;次優(yōu)視差;控制策略

中圖分類號： 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 引言

隨著計算機和感知技術的不斷發(fā)展，機器人領域逐漸成為學者們研究的熱點。未知環(huán)境下機器人目標跟蹤是指移動機器人在陌生環(huán)境中對運動目標的空間狀態(tài)進行估計的過程。而目標狀態(tài)估計是以移動機器人自身狀態(tài)估計為基礎的，要在未知環(huán)境中實現(xiàn)移動機器人自身狀態(tài)估計，實際就是解決同時定位與地圖構建（Simultaneous Localization and Mapping， SLAM）問題。實際就是解決機器人SLAM同時定位與地圖構建（Simultaneous Localization and Mapping， SLAM）問題。因此，未知環(huán)境下機器人目標跟蹤所研究的是機器人同時定位、地圖構建與目標跟蹤（Simultaneous Localization， Mapping and Object Tracking， SLAMOT）問題，它能夠實現(xiàn)機器人對自身狀態(tài)、環(huán)境狀態(tài)和目標狀態(tài)的同時估計。

2002年，卡內基梅隆大學Wang等[1]在中首次提出將SLAM和動態(tài)目標偵測和跟蹤（Detection and Tracking of Moving Objects， DTMO）作為一個耦合問題來處理的思想，該方法以柵格地圖差異性比較為基礎，并利用智能導航車Naclab11平臺成功地進行了城市環(huán)境地圖構建，結果驗證了其有效性。此后，Wang在[2-4]文獻[2-4]中系統(tǒng)地給出了該問題基于貝葉斯理論的解決框架，方法基于兩種假設條件：首先，假設系統(tǒng)能夠成功區(qū)分環(huán)境特征和目標的觀測值。其次，假設目標觀測值只影響目標的狀態(tài)更新而對機器人和環(huán)境特征狀態(tài)沒有影響，該假設實際上是將SLAM和DTMO問題作為兩個獨立過程來考慮，并沒有考慮機器人和目標間的相關性對系統(tǒng)狀態(tài)估計的影響。Vu等[5]運用全局鄰域法（Global Nearest Neighborhood， GNN）進行動態(tài)物體檢測并用擴展卡爾曼濾波（Extended Kalman Filter， EKF ）對運動物體進行跟蹤，但該方法假設物體運動模態(tài)唯一，因此不能解決機動目標跟蹤問題。Wang等[6]介紹了一種基于交互模態(tài)的機動目標跟蹤方法，使系統(tǒng)能夠對動靜（Move-Stop）模態(tài)運動的目標進行跟蹤，但該方法依然假設機器人和目標的狀態(tài)估計相互獨立。以上研究成果都是基于主動式傳感器系統(tǒng)，而這類傳感器存在體積大、質量重、消耗能量多、設備造價高、觀測距離有限等缺陷，這些缺陷限制了SLAMOT的實際應用范圍。單目視覺傳感器，憑借其體積小、耗能少、造價低廉、細節(jié)呈現(xiàn)度高及直觀性好等特點，正逐漸受到機器人學界的重視。目前，盡管存在利用單目傳感器解決SLAM問題的方法，但這些方法并未同時考慮目標跟蹤（Object Tracking， OT）問題。

對于基于單目視覺的SLAMOT問題來說，首先，由于目標的動態(tài)性和運動模式未知性，已有的基于視覺的SLAM估計方法（例如，反轉深度法[7-9]、批優(yōu)化方法[10-12]）將無法有效和實時地完成目標狀態(tài)估計，因此無法滿足目標跟蹤的實際需求;其次，單目視覺目標跟蹤等同于純方位角目標跟蹤問題，學界稱之為：方位角目標運動分析問題（Bearing Only Target Motion Analysis， BOTMA）。為了滿足BOTMA可觀性條件，通常要求觀測平臺相對于目標機動運動，而傳統(tǒng)BOTMA方法假設在機動過程中觀測平臺狀態(tài)已知，但在實際應用中，則需要首先解決觀測平臺自身的狀態(tài)估計問題。同時，為了保證觀測平臺運行的安全性，系統(tǒng)還需對外界環(huán)境狀態(tài)進行估計。最后，已有的未知環(huán)境下移動平臺運動目標跟蹤方法，一般采用多傳感器信息融合來實現(xiàn)移動平臺和目標狀態(tài)的獨立估計，多傳感器的使用限制了研究成果的應用范圍并且降低了問題的難度，而獨立估計方法沒有考慮不同對象狀態(tài)的耦合性問題，因此影響了系統(tǒng)狀態(tài)估計的準確性。

經過學者們長期的研究，目前，SFM（Structure From Motion）和濾波的方法成為了解決單目SLAM問題的主流[13-14] 。濾波的方法運算較快，能較好滿足SLAM實時性需求。而SFM的方法[15]，通過大量計算達到優(yōu)化的目的，整體估計精度較高，但實時性較差。為了滿足后續(xù)目標跟蹤實時性要求，并解決目標的動態(tài)性和運動模式未知性問題，本文選用多模濾波的方法對單目SLAMOT問題進行研究。在對目標狀態(tài)估計時，會在三維空間產生不確定性橢球，如果不對機器人進行適當?shù)目刂疲淮_定性橢球就會累積得越來越大，最終導致跟蹤過程失敗。為了避免這種情況，一方面，需要控制機器人產生足夠的視差，使得目標保持可觀性;另一方面，需要控制機器人接近目標，從而不跟丟目標。

1 模型介紹

在利用EKF框架估計和更新目標的狀態(tài)的過程中，根據協(xié)方差矩陣和估計目標的坐標可知，目標的實際位置在以某一點為中心的三維橢球中。為了更好地跟蹤目標，需要制定相應的控制策略，使機器人既能跟蹤上目標，又能更準確地估計出目標的狀態(tài)。為此，本文將控制速度分解為兩個方向的控制速度：1）跟隨速度;2）視差速度。不妨設機器人能達到的最大速度為v0，跟隨速度最大為v1，視差速度最大為v2。則有：

1.1 跟隨速度選取

選取機器人位置與目標位置連線方向作為跟隨速度方向。制定合適的跟隨速度大小，使機器人既能跟蹤上目標，又能減少能耗（降低機器人速度）。直觀來看，當機器人與目標距離相對較大的時候，跟隨速度應該相應地提高，這樣才不至于跟丟目標;當機器人與目標距離較小的時候，跟隨速度應當適當降低，以減少機器人能量的損失。為此，設計相應的跟隨速度大小與機器人到目標距離的函數(shù)：

1.2 視差速度選取

根據協(xié)方差矩陣和目標的估計位置，可以求出目標在三維空間中的不確定性橢球。為了量化視差大小，可以將視差問題轉化為三維橢球投影面積變化大小問題，最后，將其抽象為一個三維空間求梯度問題。推導求解出機器人視差速度方向過程如圖1。

1.3 多模濾波模型

2 仿真結果

本文先在傳統(tǒng)EKF_SLAM（Extended Kalman Filter—Simultaneous Localization and Mapping）框架內，分別研究次優(yōu)視差算法和啟發(fā)式算法的跟蹤效果。在此基礎上，再研究多模濾波算法的跟蹤效果。

在人們的先驗認知中，平行橢球最短軸會產生較大的視差（啟發(fā)式算法）。為了驗證次優(yōu)視差算法的可行性，將啟發(fā)式算法與次優(yōu)視差算法進行對照實驗。通過對比目標的實際位置與估計位置的殘差均值大小，評定更優(yōu)的方法。

實驗所用電腦為DELL INTEL I5 雙核，Matlab仿真實驗軟件，以及Toolbox工具箱。

不難想象，視差速度將影響視差大小，從而影響對目標狀態(tài)的估計效果。為此，分別設置視差速度從0.1m/s到0.5m/s，研究其對跟蹤效果的影響，得到表1。

為了更加直觀地反映表1中數(shù)據，將其轉換為柱狀圖的形式如圖3和4所示。

由于視差速度為0.1m/s時，啟發(fā)式方法的殘差均值過大，影響對整體效果的觀測，故剔除視差速度為0.1m/s的點，得到圖4。

當視差速度分別為0.1m/s（較?。?、0.3m/s（中等）和0.5m/s（較大）時，通過仿真，得到啟發(fā)式算法和次優(yōu)視差算法的結果如圖5所示。

當視差速度大小為0.1m/s時，由圖5（a） 和圖5 （b）可以看出，次優(yōu)視差算法殘差均值為0.46m遠遠小于啟發(fā)式算法殘差均值18.2m;當視差速度為0.3m/s（中等）時，由圖5（c）和圖5 （d）可以看出，啟發(fā)式算法殘差均值為0.25m，次優(yōu)視差算法殘差均值為0.16m;當視差速度為0.5m/s（較大）時，由圖5（e）和圖5（f）可以看出，啟發(fā)式算法和次優(yōu)視差算法殘差均值分別為0.13m、0.1m。由此可見，次優(yōu)視差算法跟蹤效果比啟發(fā)式算法好，但隨著視差速度提高，兩種算法差距越來越小。

當視差速度為0.5m/s時，得到次優(yōu)視差算法的目標跟蹤軌跡如圖6所示。

而在實際情況中，機動目標大多數(shù)是以多模態(tài)運動的。因此，用多模濾波的方法對機動目標進行跟蹤顯得十分必要。分別用傳統(tǒng)EKF_SLAM算法和多模濾波算法對機動目標進行跟蹤。在仿真實驗中，設定目標1～100幀及451～520幀為定速模型;101～450幀及 521～550為轉彎模型（轉彎時速度大小保持不變）;551～600幀為加速模型。得到軌跡如圖7所示。

在此基礎上，分別計算兩種算法的殘差均值，得到圖8。

由圖8（a）和圖8（b）可以看出，傳統(tǒng)EKF_SLAM目標跟蹤殘差均值為0.15m，而多模濾波目標跟蹤殘差均值僅為0.06m。另外，在目標模態(tài)變化時，多模濾波目標跟蹤能更準確地預測目標狀態(tài)。

3 結語

本文通過數(shù)學推導證明本文方法的理論性，通過仿真實驗驗證了本文方法的可行性，同時根據仿真實驗結果可以得出以下結論：

1）在視差速度大小相同的情況下，本文算法要優(yōu)于啟發(fā)式算法。

2）當視差速度較小時，啟發(fā)式算法誤差很大，導致跟蹤失敗，而本文算法誤差較小，能保持跟蹤正常進行，這說明本文方法能夠節(jié)省能耗，具有工程應用價值。

3）隨著控制速度增大，所獲得的視差越大，兩種方法對目標狀態(tài)的估計誤差越小，同時兩種方法之間估計的差距也越小。

4）對于機動目標，多模濾波的目標跟蹤算法估計效果更準確，其殘差均值不到傳統(tǒng)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] 李慧平，徐德民，張福斌，等.基于量測噪聲和觀測次數(shù)的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] 梁明杰，閔華清，羅榮華.基于圖優(yōu)化的同時定位與地圖創(chuàng)建綜述[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.）