未知環(huán)境基于單目次優(yōu)視差的多模濾波目標(biāo)跟蹤算法

黃帥 付光遠(yuǎn) 伍明 岳敏

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

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

中圖分類號： TP249

文獻標(biāo)志碼：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 引言

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

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

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

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

1 模型介紹

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

1.1 跟隨速度選取

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

1.2 視差速度選取

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

1.3 多模濾波模型

2 仿真結(jié)果

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

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

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

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

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

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

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

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

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

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

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

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

3 結(jié)語

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

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

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

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

4）對于機動目標(biāo)，多模濾波的目標(biāo)跟蹤算法估計效果更準(zhǔn)確，其殘差均值不到傳統(tǒng)EKF_SLAM目標(biāo)跟蹤算法的1/2。

5）視差方向的選取和速度大小的控制對目標(biāo)跟蹤影響大， 說明對于單目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].自動化學(xué)報，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.）