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

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

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

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

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

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

DOI:10.3969/j.issn.1673-4785.201405051

中圖分類(lèi)號(hào)：TP242.6 文獻(xiàn)標(biāo)志碼：A

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

基金項(xiàng)目：國(guó)家自然科學(xué)基金資助項(xiàng)目(61202332)；陜西省自然科學(xué)基礎(chǔ)研究計(jì)劃項(xiàng)目(2013JQ8030).

作者簡(jiǎn)介：

中文引用格式：伍明，李琳琳，魏振華，等. 一種未知環(huán)境下機(jī)器人多目標(biāo)跟蹤算法[J]. 智能系統(tǒng)學(xué)報(bào), 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.

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

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

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

多目標(biāo)數(shù)據(jù)關(guān)聯(lián)目的是確定目標(biāo)觀測(cè)值和每個(gè)目標(biāo)的對(duì)應(yīng)關(guān)系，假設(shè)在k時(shí)刻，系統(tǒng)得到nk個(gè)目標(biāo)觀測(cè)值，記為

(1)

而k時(shí)刻n個(gè)目標(biāo)的狀態(tài)集合為

(2)

未知環(huán)境下多目標(biāo)跟蹤算法目的是機(jī)器人在SLAM的同時(shí)追蹤定位這n個(gè)移動(dòng)目標(biāo)。

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

(3)

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

假設(shè)該估計(jì)問(wèn)題為馬爾可夫過(guò)程，對(duì)式(3)進(jìn)行如下推導(dǎo)：

(4)

(5)

由貝葉斯定理可得目標(biāo)ti在k時(shí)刻狀態(tài)估計(jì)為

(6)

(7)

式(7)表示目標(biāo)狀態(tài)的預(yù)測(cè)過(guò)程。由于不能確定目標(biāo)觀測(cè)值和目標(biāo)的對(duì)應(yīng)關(guān)系，此處利用加權(quán)和的形式來(lái)表示式(7)，即

(8)

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

為了解決未知環(huán)境下機(jī)器人多目標(biāo)追蹤問(wèn)題，在基于Rao-Blackwellised粒子濾波的單目標(biāo)SLAMOT算法基礎(chǔ)上，修改粒子群結(jié)構(gòu)如下：

(9)

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

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

(10)

(11)

最后介紹目標(biāo)更新過(guò)程，利用式(8)對(duì)粒子i對(duì)應(yīng)的tm目標(biāo)狀態(tài)進(jìn)行更新：

(12)

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

3實(shí)驗(yàn)結(jié)果及分析

下面通過(guò)仿真實(shí)驗(yàn)驗(yàn)證多目標(biāo)追蹤PFJPDA_SLAMOT算法的有效性，并分析粒子數(shù)對(duì)于機(jī)器人和目標(biāo)定位準(zhǔn)確性的影響，實(shí)驗(yàn)在MATLAB 7.5環(huán)境下進(jìn)行。

3.1 算法跟蹤結(jié)果

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

圖1　PFJPDA_SLAMOT算法多目標(biāo)跟蹤仿真結(jié)果 Fig. 1　 The results of simulation about multi-object tracking using PFJPDA_SLAMOT

仿真中粒子數(shù)為50，圖1顯示了此次運(yùn)行的總體情況，圖2顯示了相應(yīng)區(qū)域的局部放大圖，其中實(shí)線軌跡代表對(duì)機(jī)器人和目標(biāo)的估計(jì)軌跡，虛線軌跡代表機(jī)器人和目標(biāo)的實(shí)際運(yùn)動(dòng)軌跡。線段代表機(jī)器人當(dāng)前對(duì)標(biāo)志柱的觀測(cè)值。星號(hào)代表標(biāo)志柱的真實(shí)分布，標(biāo)志柱周?chē)牧Ｗ尤捍韺?duì)標(biāo)志柱位置的粒子群估計(jì)。

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

(a)機(jī)器人軌跡估計(jì)

(b)目標(biāo)1軌跡估計(jì)

(c)目標(biāo)2軌跡估計(jì)

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

3.2粒子數(shù)量影響

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

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

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

(a)目標(biāo)1位置誤差

(b)目標(biāo)2位置誤差

(c)目標(biāo)3位置誤差

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

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

4結(jié)束語(yǔ)

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

參考文獻(xiàn):

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

李琳琳，女，1974年生，副教授，博士，主要研究方向?yàn)樾畔鸥窦夹g(shù)、多傳感器網(wǎng)絡(luò)、物聯(lián)網(wǎng)。主持國(guó)家高技術(shù)研究項(xiàng)目1項(xiàng)，發(fā)表學(xué)術(shù)論文20余篇，其中被EI檢索12篇。