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

一種未知環(huán)境下機器人多目標跟蹤算法

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

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

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

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

DOI:10.3969/j.issn.1673-4785.201405051

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

收稿日期：2014-05-23. 網(wǎng)絡出版日期：2015-05-08.

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

作者簡介：

中文引用格式：伍明，李琳琳，魏振華，等. 一種未知環(huán)境下機器人多目標跟蹤算法[J]. 智能系統(tǒng)學報, 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問題作為耦合問題來處理，例如未知環(huán)境下目標跟蹤任務[1]、未知環(huán)境下機器人圍捕任務[2-3]。此類任務要求機器人在未知環(huán)境條件下對目標進行定位和追蹤，也就是說，算法需要對機器人狀態(tài)、環(huán)境特征狀態(tài)以及目標狀態(tài)進行同步估計。Wang等[4]首先對該問題進行了研究并提出一種基于掃描點匹配的解決方法，由于該方法采用的是ICP(iterative closest point)匹配算法，因此無法描述機器人和目標的不確定性和相關性，并且存在累積誤差[5-7]。Vu等[1]運用全局鄰域法(global nearest neighborhood, GNN)進行動態(tài)物體檢測，并用EKF對運動物體進行跟蹤。伍明等[8]提出了基于卡爾曼濾波框架的單目標跟蹤算法，實現(xiàn)了機器人同時定位、地圖構建與多目標跟蹤(simultaneous localization, mapping and multi-objects tracking, SLOMMOT)的對象狀態(tài)耦合估計?，F(xiàn)有上述方法并未考慮機器人未知環(huán)境下多目標跟蹤問題，然而對于該問題的研究能夠有效擴展已有方法的應用前景，使相關算法更具實用價值。

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

1問題描述及聯(lián)合概率數(shù)據(jù)關聯(lián)

多目標數(shù)據(jù)關聯(lián)目的是確定目標觀測值和每個目標的對應關系，假設在k時刻，系統(tǒng)得到nk個目標觀測值，記為

(1)

而k時刻n個目標的狀態(tài)集合為

(2)

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

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

(3)

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

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

(4)

(5)

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

(6)

(7)

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

(8)

2 聯(lián)合概率數(shù)據(jù)關聯(lián)粒子濾波的多目標SLAMOT實現(xiàn)

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

(9)

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

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

(10)

(11)

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

(12)

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

3實驗結果及分析

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

3.1 算法跟蹤結果

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

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

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

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

(a)機器人軌跡估計

(b)目標1軌跡估計

(c)目標2軌跡估計

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

3.2粒子數(shù)量影響

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

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

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

(a)目標1位置誤差

(b)目標2位置誤差

(c)目標3位置誤差

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

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

4結束語

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

參考文獻:

[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]伍明, 孫繼銀. 基于擴展式卡爾曼濾波的移動機器人未知環(huán)境下動態(tài)目標跟蹤[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年生，講師，博士，主要研究方向為自主機器人控制、多機器人協(xié)作、機器人環(huán)境構建。發(fā)表學術論文10余篇，其中被EI檢索8篇。

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