一種未知環境下機器人多目標跟蹤算法

網絡出版地址：http://www.cnki.net/kcms/detail/23.1538.tp.20150508.1546.002.html

一種未知環境下機器人多目標跟蹤算法

伍明，李琳琳，魏振華，汪洪橋

(第二炮兵工程大學 指揮信息工程系，陜西 西安 710025)

摘要：針對未知環境下移動機器人多目標跟蹤問題，設計了一種基于聯合概率數據關聯的粒子濾波算法。該算法利用聯合概率數據關聯方法對Rao-Blackwellized粒子濾波算法進行改進，使機器人能夠完成未知環境條件下對自身狀態、環境特征狀態和多目標狀態的在線聯合估計。算法將系統狀態變量分為代表多目標、環境特征狀態的線性變量和代表機器人狀態的非線性變量，并利用聯合概率數據關聯Kalman濾波和粒子濾波對系統狀態進行更新。通過仿真實驗證明了該算法對機器人狀態、環境特征狀態以及多目標狀態的估計準確性，驗證了算法對未知環境下多目標的跟蹤能力。

關鍵詞：機器人；同時定位與地圖構建；多目標跟蹤；粒子濾波；聯合概率數據關聯；Rao-Blackwellised粒子濾波；Kalman濾波

DOI:10.3969/j.issn.1673-4785.201405051

中圖分類號：TP242.6 文獻標志碼：A

收稿日期：2014-05-23. 網絡出版日期：2015-05-08.

基金項目：國家自然科學基金資助項目(61202332)；陜西省自然科學基礎研究計劃項目(2013JQ8030).

作者簡介：

中文引用格式：伍明，李琳琳，魏振華，等. 一種未知環境下機器人多目標跟蹤算法[J]. 智能系統學報, 2015, 10(3): 448-453.

英文引用格式：WU Ming, LI Linlin, WEI Zhenhua, et al. A robot multi-object tracking algorithm in unknown environments[J]. CAAI Transactions on Intelligent Systems, 2015, 10(3): 448-453.

A robot multi-object tracking algorithm in unknown environments

WU Ming, LI Linlin, WEI Zhenhua, WANG Hongqiao

(Command Information Engineering Department, The Second Artillery Engineering College, Xian 710025, China)

Abstract：In this paper, a particle filtering algorithm based on the joint integrated probabilistic data association (JIPDA) is proposed in order to solve the problem of motile robot multi-object tracking in unknown environments. The Rao-Blackwellized particle filtering is reconstructed based on the JIPDA in the new algorithm. It allows the robot to estimate joint states of itself, environment features and multi-object states simultaneously. The algorithm divides the system variables into two parts: the lineal variable representing multi-object and environment feature states, and the non-linear variable representing robot states. The system state is updated by JIPDA Kalman filtering and particle filtering. Estimation precision of robot states, environment feature states and multi-object states is verified by simulation results, verifying the ability of multi-object tracking in unknown environments.

Keywords：robot; simultaneous localization and mapping (SLAM); multi-object tracking; particle filtering; joint integrated probabilistic data association (JIPDA); Rao-Blackwellized particle filtering; Kalman filtering

通信作者：伍明. E-mail: hyacinth531@163.com.

機器人同時定位與地圖構建問題(simultaneous localization and mapping, SLAM)和目標跟蹤問題(object tracking, OT)在機器人學界通常被作為2個獨立問題加以研究，而對于某些實際任務，需要將SLAM和OT問題作為耦合問題來處理，例如未知環境下目標跟蹤任務[1]、未知環境下機器人圍捕任務[2-3]。此類任務要求機器人在未知環境條件下對目標進行定位和追蹤，也就是說，算法需要對機器人狀態、環境特征狀態以及目標狀態進行同步估計。Wang等[4]首先對該問題進行了研究并提出一種基于掃描點匹配的解決方法，由于該方法采用的是ICP(iterative closest point)匹配算法，因此無法描述機器人和目標的不確定性和相關性，并且存在累積誤差[5-7]。Vu等[1]運用全局鄰域法(global nearest neighborhood, GNN)進行動態物體檢測，并用EKF對運動物體進行跟蹤。伍明等[8]提出了基于卡爾曼濾波框架的單目標跟蹤算法，實現了機器人同時定位、地圖構建與多目標跟蹤(simultaneous localization, mapping and multi-objects tracking, SLOMMOT)的對象狀態耦合估計。現有上述方法并未考慮機器人未知環境下多目標跟蹤問題，然而對于該問題的研究能夠有效擴展已有方法的應用前景，使相關算法更具實用價值。

針對SLOMMOT問題提出了一種基于聯合概率數據關聯(joint integrated probabilistic data association, JIPDA)的粒子濾波算法，算法采用Rao-Blackwellized粒子濾波框架，并利用IPDA解決多目標跟蹤問題。首先對未知環境下機器人多目標跟蹤問題的相關模型進行描述，然后給出基于聯合概率數據關聯粒子濾波算法的具體處理過程，最后通過仿真實驗驗證算法對機器人狀態、多目標狀態和環境特征狀態估計的有效性，并分析粒子數對機器人和目標定位準確性的影響。

1問題描述及聯合概率數據關聯

多目標數據關聯目的是確定目標觀測值和每個目標的對應關系，假設在k時刻，系統得到nk個目標觀測值，記為

(1)

而k時刻n個目標的狀態集合為

(2)

未知環境下多目標跟蹤算法目的是機器人在SLAM的同時追蹤定位這n個移動目標。

聯合概率數據關聯濾波由Musicki等[9]首先提出，JIPDA能夠解決多目標跟蹤問題，該方法對每個目標產生一個跟蹤濾波器，每次迭代時考慮所有目標觀測值-跟蹤濾波器組合情況，并用全部目標觀測值對每個跟蹤濾波器進行更新。

(3)

式中：上標k代表截止k時刻為止的所有目標觀測值序列，稱該值βj,i為關聯系數。

假設該估計問題為馬爾可夫過程，對式(3)進行如下推導：

(4)

(5)

由貝葉斯定理可得目標ti在k時刻狀態估計為

(6)

(7)

式(7)表示目標狀態的預測過程。由于不能確定目標觀測值和目標的對應關系，此處利用加權和的形式來表示式(7)，即

(8)

2 聯合概率數據關聯粒子濾波的多目標SLAMOT實現

為了解決未知環境下機器人多目標追蹤問題，在基于Rao-Blackwellised粒子濾波的單目標SLAMOT算法基礎上，修改粒子群結構如下：

(9)

機器人單目標Rao-Blackwellised粒子濾波運用傳統粒子濾波完成機器人狀態估計，并對每一個粒子包含的標志柱、目標狀態均值和協方差運用EKF進行估計。當涉及多個目標的追蹤時，需要運用聯合概率數據關聯方法對原算法進行改進。

未知環境下多目標跟蹤算法粒子權值目標部分推導如下：

(10)

(11)

最后介紹目標更新過程，利用式(8)對粒子i對應的tm目標狀態進行更新：

(12)

整個方法的時間復雜度為O(N·lb(M+1))，其中N為粒子個數，M+1為環境特征和機器人個數之和。該計算復雜度為M+1的線性復雜度，因此能夠滿足計算效率要求。

3實驗結果及分析

下面通過仿真實驗驗證多目標追蹤PFJPDA_SLAMOT算法的有效性，并分析粒子數對于機器人和目標定位準確性的影響，實驗在MATLAB 7.5環境下進行。

3.1 算法跟蹤結果

假設運行環境范圍為100 m×100 m，機器人同時追蹤3個自左向右運動的目標，機器人控制輸入噪聲為diag(0.3m,0.0872rad)，目標運動符合定速模型，目標運動不確定系數為0.4。機器人將根據目標位置計算控制量并向目標構成的三角形中心運動。實驗共進行20次仿真，其中一次仿真總體結果如圖1所示。

圖1　PFJPDA_SLAMOT算法多目標跟蹤仿真結果 Fig. 1　 The results of simulation about multi-object tracking using PFJPDA_SLAMOT

仿真中粒子數為50，圖1顯示了此次運行的總體情況，圖2顯示了相應區域的局部放大圖，其中實線軌跡代表對機器人和目標的估計軌跡，虛線軌跡代表機器人和目標的實際運動軌跡。線段代表機器人當前對標志柱的觀測值。星號代表標志柱的真實分布，標志柱周圍的粒子群代表對標志柱位置的粒子群估計。

從圖2可見，算法能夠較好地對3個目標和機器人進行定位。本次仿真中對機器人位置估計的誤差均值和方差分別為0.452和0.117。對目標1位置估計的誤差均值和方差分別為0.923和0.353。對目標2位置估計的誤差均值和方差分別為1.544和1.235。對目標3位置估計的誤差均值和方差分別為1.952和0.554 3。可見，機器人定位精度要高于目標定位精度。仿真發現，算法對于目標跟蹤的靈敏度小于對機器人定位的靈敏度，當機器人估計位置和實際位置出現差別時算法能夠較快進行調節；但是當目標估計位置和實際位置出現差別時算法調節較慢。該特點從圖1中得以體現，當代表目標實際位置的實線和代表估計位置的虛線出現偏差時往往需要較長的時間，虛線才能重新跟上實線，而代表機器人實際位置的實線和代表估計位置的虛線出現偏差時，只需要很少的時間虛線就能夠跟上實線。其原因在于(如式(11)所示)粒子權值目標部分是根據多個目標觀測相似度的乘積來確定的，當對一個目標的觀測相似度降低時對其他目標的觀測相似度不一定下降，那么它們的乘積值不一定變小，這樣對于單目標位置估計的粒子群更新就不一定及時，因此算法對目標跟蹤靈敏度相比對機器人定位靈敏度來說較低。另外，從圖中可見，目標2和目標3在坐標(39, 63)處交匯，由于算法采用了聯合概率數據關聯方法使得對于兩目標的跟蹤仍能保持準確。

(a)機器人軌跡估計

(b)目標1軌跡估計

(c)目標2軌跡估計

(d)目標3軌跡估計 圖2　各對象軌跡估計局部區域放大 Fig. 2　Object trajectory in some local areas

3.2粒子數量影響

本節分析粒子數對機器人和多目標定位的影響，仿真分別采用粒子數為20、50、100、150、200、250、300、350、400、450、500、550和600個，對于每一種粒子數分別進行20次仿真得到的機器人位置誤差平均值隨粒子數變化如圖3所示。

圖3　機器人定位精度隨粒子數變化情況 Fig. 3　 Localization accuracy of robot as a function of the number of particles

從圖3可知，在粒子數大于50的條件下，機器人定位精度并沒有隨粒子數的增加而顯著提高，其呈現出波動狀態。這種現象反映出機器人定位精確度隨粒子數量的增加存在一個飽和值，從仿真可見，并非粒子數越多，定位越準確，適當選擇粒子數既能夠保持高精度又能減少計算量。同樣，對應不同粒子數目，算法對3目標定位誤差的變化如圖4所示。

(a)目標1位置誤差

(b)目標2位置誤差

(c)目標3位置誤差

(d)3個目標總體平均位置誤差 圖4　目標定位精度隨粒子數變化情況 Fig. 4　 Localization accuracy of objects as a function of the number of particles

圖4(a)~(c)圖分別代表對于目標1、2、3的定位誤差的變化情況，圖4(d)為3目標平均定位誤差的變化情況。從圖中可見，每一個目標的定位精度并不都是隨粒子數的增加而提高，目標1在粒子數為500時定位精度最差，造成這種現象的原因同樣是因為粒子權值是根據多個目標觀測相似度乘積共同產生，當對目標1的定位誤差提高時，算法對于目標2、3的定位精度仍然較高，因此粒子對于目標1的調節并不明顯。從圖4(d)可見，總體來看對于多目標的綜合定位性能，當粒子數目達到一定數量后系統定位精度并未得到進一步提高。

4結束語

本文針對未知環境下機器人多目標跟蹤問題，提出了一種基于聯合概率數據關聯粒子濾波的算法。該算法基于Rao-Blackwellised粒子濾波框架，粒子由機器人狀態、多目標狀態和環境特征狀態聯合構成，粒子權值則根據所有目標觀測值和跟蹤器組合相似度加權和計算得出，該算法能夠有效克服多目標觀測值對單一目標狀態估計的影響，保證較高的目標狀態平均估計準確度，仿真實驗證明了該算法的有效性。這主要解決了機器人、目標以及環境特征狀態估計問題，下一步將在狀態估計基礎上進行機器人未知環境下導航控制方法研究，另外，利用相關優化方法對粒子群個數進行在線優化選擇也將是后續研究重點。

參考文獻:

[1]VU T D, AYCARD O, APPENRODT N. Online localization and mapping with moving object tracking in dynamic outdoor environments[C]//Proceedings of the IEEE Intelligent Vehicles Symposium. Istanbul, Turkey, 2007: 190-196.

[2]VIDAL R, RASHID S, SHARP C, et al. Pursuit-evasion games with unmanned ground and aerial vehicles[C]//Proceedings of the IEEE International Conference on Robotics and Automation. Seoul, Korea, 2001: 2948-2955.

[3]HUANG Feifei, WANG Long, WANG Qining, et al. Coordinated control of multiple mobile robots in pursuit-evasion games[C]//Proceedings of American Control Conference. St. Louis, USA, 2008: 2861-2866.

[4]WANG C C, THORPE C. Simultaneous localization and mapping with detection and tracking of moving objects[C]//Proceedings IEEE International Conference on Robotics and Automation. Washington, DC, USA, 2002: 2918-2924.

[5]BORRMANN D, ELSEBERG J, LINGEMANN K, et al. Globally consistent 3D mapping with scan matching[J]. Robotics and Autonomous Systems, 2008, 56(2): 130-142.

[6]WANG C C, THORPE C, THRUN S, et al. Simultaneous localization, mapping and moving object tracking[J]. The International Journal of Robotics Research, 2007, 26(9): 889-916.

[7]WAN Kaowei, WANG C C, TON T T. Weakly interacting object tracking in indoor environments[C]//Proceedings of the IEEE International Conference on Advanced Robotics and Its Social Impacts. Taipei, China, 2008: 1-6.

[8]伍明, 孫繼銀. 基于擴展式卡爾曼濾波的移動機器人未知環境下動態目標跟蹤[J]. 機器人, 2010, 32(3): 334-343.

WU Ming, SUN Jigen. Extended Kalman filter based moving object tracking by mobile robot in unknown environment[J]. Robot, 2010, 32(3): 334-343.

伍明，男，1981年生，講師，博士，主要研究方向為自主機器人控制、多機器人協作、機器人環境構建。發表學術論文10余篇，其中被EI檢索8篇。

李琳琳，女，1974年生，副教授，博士，主要研究方向為信息柵格技術、多傳感器網絡、物聯網。主持國家高技術研究項目1項，發表學術論文20余篇，其中被EI檢索12篇。