基于改進(jìn)粒子濾波的移動(dòng)機(jī)器人行人跟蹤*

夏克付，李鵬飛，陳小平+

1.安徽電子信息職業(yè)技術(shù)學(xué)院，安徽 蚌埠 233030

2.中國科學(xué)技術(shù)大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，合肥 230027

基于改進(jìn)粒子濾波的移動(dòng)機(jī)器人行人跟蹤*

夏克付1,2，李鵬飛2，陳小平2+

1.安徽電子信息職業(yè)技術(shù)學(xué)院，安徽 蚌埠 233030

2.中國科學(xué)技術(shù)大學(xué) 計(jì)算機(jī)科學(xué)與技術(shù)學(xué)院，合肥 230027

移動(dòng)機(jī)器人對(duì)行人進(jìn)行跟蹤，是體現(xiàn)機(jī)器人智能的一個(gè)重要方面，具有廣闊的發(fā)展前景和應(yīng)用價(jià)值。然而環(huán)境的復(fù)雜性和行人運(yùn)動(dòng)的不確定性給行人的跟蹤帶來了極大的挑戰(zhàn)。為此，在分析粒子濾波框架的基礎(chǔ)上，對(duì)基本粒子濾波算法進(jìn)行了兩方面改進(jìn)，提出了適用于移動(dòng)機(jī)器人的行人跟蹤方法。一方面在相似性估計(jì)階段結(jié)合顏色信息、深度信息和社交力概念，提高了跟蹤的精度；另一方面提出了二級(jí)粒子的概念，解決了粒子多樣性缺失問題，提高了跟蹤的準(zhǔn)確度。在移動(dòng)機(jī)器人turtlebot和公開數(shù)據(jù)集IAS-Lab上對(duì)改進(jìn)的粒子濾波、序貫重要性重采樣（sequential importance resampling，SIR）粒子濾波和擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）算法進(jìn)行對(duì)比，實(shí)驗(yàn)結(jié)果表明，改進(jìn)的粒子濾波算法明顯優(yōu)于其他兩種算法。

移動(dòng)機(jī)器人；行人跟蹤；粒子濾波；深度信息；社交力；二級(jí)粒子

1 引言

隨著計(jì)算機(jī)和自動(dòng)化技術(shù)的不斷進(jìn)步，移動(dòng)機(jī)器人的應(yīng)用領(lǐng)域越來越廣泛。在各種各樣的場景中，移動(dòng)機(jī)器人經(jīng)常需要進(jìn)行人機(jī)交互[1]，對(duì)行人進(jìn)行跟蹤，進(jìn)而為行人提供服務(wù)逐漸發(fā)展成為移動(dòng)機(jī)器人的一項(xiàng)基本功能[2]。然而，移動(dòng)機(jī)器人工作的環(huán)境中除了行人往往還存在其他物體，復(fù)雜的環(huán)境給行人跟蹤帶來了一定的困難。不僅如此，在傳統(tǒng)基于視頻圖像的行人跟蹤方法中，光照變化、背景與行人顏色相近、多個(gè)行人互相遮擋等問題也給視覺跟蹤增加了難度。因此研究如何在復(fù)雜環(huán)境以及各種困難條件下對(duì)行人進(jìn)行有效跟蹤，對(duì)移動(dòng)機(jī)器人完成任務(wù)服務(wù)行人具有重要意義[3]。

針對(duì)行人跟蹤，傳統(tǒng)的方法大致有兩類：一類是基于概率密度估計(jì)的方法，主要有Meanshift算法[4-5]和Camshift算法[6-7]；另一類是基于貝葉斯估計(jì)的方法，主要有卡爾曼濾波和粒子濾波[8]。Meanshift算法因其無需參數(shù)以及快速匹配的特征應(yīng)用廣泛。然而，Meanshift算法要求相鄰幀圖像之間必須有重疊區(qū)域，如果跟蹤目標(biāo)運(yùn)動(dòng)過快容易跟蹤失敗。同時(shí)，Meanshift算法的跟蹤窗口大小固定不變影響跟蹤的準(zhǔn)確性。Camshift算法又稱連續(xù)自適應(yīng)Meanshift算法，將前一幀的跟蹤結(jié)果作為當(dāng)前幀的初始值，每一幀都利用Meanshift算法進(jìn)行跟蹤，可以自動(dòng)調(diào)節(jié)跟蹤窗口的大小。無論是Meanshift算法還是Camshift算法，當(dāng)跟蹤目標(biāo)運(yùn)動(dòng)過快，或者背景和目標(biāo)顏色相近時(shí)，它們的跟蹤效果都不佳。相比于基于概率密度估計(jì)的方法，基于貝葉斯估計(jì)的跟蹤方法更加適用于移動(dòng)機(jī)器人，因?yàn)樗鼈兛紤]跟蹤目標(biāo)的運(yùn)動(dòng)信息?？柭鼮V波對(duì)線性高斯系統(tǒng)具有良好的跟蹤效果，例如在場景中跟蹤單個(gè)勻速運(yùn)動(dòng)的行人。但是針對(duì)復(fù)雜非線性非高斯系統(tǒng)，粒子濾波更具優(yōu)勢(shì)。Okuma等人在文獻(xiàn)[9]中將Adaboost算法與粒子濾波算法相結(jié)合，實(shí)現(xiàn)了對(duì)多個(gè)曲棍球運(yùn)動(dòng)的檢測(cè)跟蹤。Wang等人在文獻(xiàn)[10]中通過融合強(qiáng)度信息和邊緣信息對(duì)粒子濾波算法進(jìn)行了改進(jìn)，提高了粒子濾波算法在紅外圖像中對(duì)行人跟蹤的魯棒性和準(zhǔn)確性。不過，上述兩種方法都是假設(shè)攝像頭固定的情況，這種條件下背景是固定的，不太適用于移動(dòng)機(jī)器人。Aguirre等人在文獻(xiàn)[11]中將粒子濾波算法應(yīng)用于移動(dòng)機(jī)器人的跟蹤，通過將激光檢測(cè)的人腿融合粒子濾波的觀測(cè)模型，可以在行人靜止或運(yùn)動(dòng)情況下進(jìn)行有效跟蹤。同時(shí)，Aguirre等人在文獻(xiàn)[12]中對(duì)原有方法進(jìn)行了改進(jìn)，他們?cè)谝苿?dòng)機(jī)器人上不僅安裝激光檢測(cè)人腿，還安裝Kinect檢測(cè)人臉，然后將兩種檢測(cè)結(jié)果融入粒子濾波跟蹤框架，提高了跟蹤的準(zhǔn)確率。

雖然在移動(dòng)機(jī)器人上利用粒子濾波算法進(jìn)行行人跟蹤的研究很多，但它們往往只是在特定的場景中側(cè)重某些情況進(jìn)行改進(jìn)與分析。本文通過對(duì)粒子濾波框架的分析，在原始粒子濾波的基礎(chǔ)上進(jìn)行了兩點(diǎn)改進(jìn)。為了提高跟蹤的精確度，提出了融合顏色信息、深度信息、社交力概念的相似性計(jì)算方法。為了克服粒子濾波、粒子多樣性缺失問題，提出了二級(jí)粒子的概念。通過引入二級(jí)粒子的個(gè)數(shù)隨機(jī)性和狀態(tài)隨機(jī)性，提高了跟蹤的魯棒性。

2 粒子濾波跟蹤框架

粒子濾波是一種基于貝葉斯理論的狀態(tài)估計(jì)算法。行人跟蹤問題可以轉(zhuǎn)化為在跟蹤空間對(duì)行人位置進(jìn)行最優(yōu)估計(jì)的過程，在貝葉斯框架下利用先驗(yàn)信息和觀測(cè)信息，可以預(yù)測(cè)得到行人的最佳位置。在貝葉斯理論中使用后驗(yàn)概率密度的方式對(duì)所有時(shí)刻的狀態(tài)進(jìn)行了估計(jì)，其進(jìn)行狀態(tài)估計(jì)的推導(dǎo)包含了預(yù)測(cè)和更新兩個(gè)過程。預(yù)測(cè)階段：通過系統(tǒng)的狀態(tài)方程，在還沒有獲取當(dāng)前時(shí)刻的觀測(cè)量時(shí)，就可以事先推導(dǎo)出當(dāng)前時(shí)刻的先驗(yàn)概率。更新階段：通過系統(tǒng)的觀測(cè)方程，可以獲取當(dāng)前時(shí)刻的觀測(cè)值。然后基于貝葉斯公式，利用預(yù)測(cè)階段得到的當(dāng)前時(shí)刻的先驗(yàn)概率。

貝葉斯估計(jì)在計(jì)算后驗(yàn)概率密度時(shí)采用推導(dǎo)的方式，這種方法在實(shí)際環(huán)境中由于環(huán)境的復(fù)雜性和系統(tǒng)的動(dòng)態(tài)性往往不能精確計(jì)算得到。粒子濾波算法借鑒了概率算法的思想，使用了蒙特卡洛算法近似估計(jì)系統(tǒng)的狀態(tài)。粒子濾波算法的一般過程如圖1所示，主要涉及5個(gè)過程。粒子初始化：在算法的初始階段，將所有粒子集中分布在待估計(jì)的目標(biāo)上，并且設(shè)置每個(gè)粒子的權(quán)重都相等。狀態(tài)轉(zhuǎn)移：根據(jù)狀態(tài)轉(zhuǎn)移函數(shù)移動(dòng)每個(gè)粒子，并附加一個(gè)隨機(jī)誤差。相似性計(jì)算：計(jì)算每個(gè)粒子狀態(tài)轉(zhuǎn)移之后的觀測(cè)值與狀態(tài)估計(jì)值之間的相似性。權(quán)重更新：由相似性大小更新每個(gè)粒子的權(quán)重，并對(duì)權(quán)重進(jìn)行歸一化處理。狀態(tài)估計(jì)：在當(dāng)前時(shí)刻系統(tǒng)的狀態(tài)可以由所有粒子的狀態(tài)及其權(quán)重進(jìn)行加權(quán)求和得到，在當(dāng)前時(shí)刻的狀態(tài)估計(jì)完成之后，系統(tǒng)將進(jìn)入下一時(shí)刻的狀態(tài)轉(zhuǎn)移階段。如此循環(huán)往復(fù)，粒子濾波算法就可以不斷估計(jì)出動(dòng)態(tài)系統(tǒng)各個(gè)時(shí)刻的狀態(tài)。

Fig.1 Flow diagram of particle filter圖1 粒子濾波算法流程圖

3 改進(jìn)的相似性計(jì)算

相似性計(jì)算在粒子濾波跟蹤框架中十分重要，該階段通過比較候選目標(biāo)與跟蹤目標(biāo)的相似程度，為之后的權(quán)重更新提供了依據(jù)，進(jìn)而直接影響了跟蹤的精度。在對(duì)行人進(jìn)行跟蹤時(shí)，傳統(tǒng)的粒子濾波算法簡單地使用了顏色信息進(jìn)行相似性計(jì)算。顏色信息在一定程度上能夠區(qū)分跟蹤的行人與環(huán)境，然而當(dāng)背景與行人顏色相近，或者光照變化時(shí)，只使用顏色信息不能夠有效地匹配跟蹤行人。本文將深度信息約束、社交力概念與顏色信息相結(jié)合，能夠極大提高相似性計(jì)算的準(zhǔn)確率。

3.1 顏色相似性

顏色作為一種最為明顯且十分穩(wěn)定的特征，對(duì)于旋轉(zhuǎn)、平移和尺度變化等都不敏感，而且計(jì)算簡單，在行人檢測(cè)和行人匹配方面使用廣泛[13-14]。因此，在使用粒子濾波算法對(duì)行人進(jìn)行跟蹤時(shí)，使用顏色信息計(jì)算相似性比較合適。常用的顏色模型有RGB模型、HSV模型和CMY模型。攝像機(jī)采集的數(shù)字圖像一般使用RGB模型，每個(gè)像素由紅、綠、藍(lán)三原色混合組成。然而，RGB模型并不適合人們對(duì)顏色的直觀感受。HSV模型以人們更加熟悉的方式封裝了關(guān)于顏色的信息，是一種刻畫顏色的有效辦法，其中H分量代表色相，S分量代表飽和度，V分量代表色亮度。因此，本文以HSV模型為基礎(chǔ)，采用了顏色直方圖來計(jì)算相似性。顏色直方圖描述了不同色彩信息在圖像中所占的比例，其提取過程如下。

（1）顏色空間轉(zhuǎn)換：將圖像從原始的RGB空間轉(zhuǎn)換到HSV空間。

（2）顏色量化：根據(jù)每個(gè)像素的HSV分量，將其劃分到不同的bin。

（3）生成直方圖：統(tǒng)計(jì)每個(gè)bin內(nèi)像素的個(gè)數(shù)，就可以得到顏色直方圖。

在提取到圖像的HSV顏色直方圖之后，就需要計(jì)算模板和待跟蹤圖像的相似度。本文采用巴式距離計(jì)算其相似性，假設(shè)待跟蹤圖像的直方圖為H1={h11,h12,…,h1N}，模板的直方圖為H0={h01,h02,…,h0N}，則相似性其定義如下：

其中dis越小代表其相似性越大，dis越大代表其相似性越小。

3.2 深度信息約束

在對(duì)行人進(jìn)行跟蹤時(shí)，行人距離機(jī)器人的遠(yuǎn)近可能會(huì)發(fā)生變化。當(dāng)行人距離機(jī)器人過遠(yuǎn)時(shí)，待匹配窗口除了包含行人，還包含很多背景信息。這些背景信息會(huì)對(duì)相似性計(jì)算造成干擾，影響匹配的準(zhǔn)確度。同理，當(dāng)行人距離機(jī)器人過近時(shí)，待匹配窗口可能只包含行人的局部信息。如果行人局部顏色信息差異很大，也會(huì)造成相似性計(jì)算失效。遠(yuǎn)近變化對(duì)匹配造成的影響如圖2所示。其中圖（a）代表模板，圖（b）是遠(yuǎn)距離匹配的情況，圖（c）是近距離匹配的情況。圖中實(shí)線矩形框代表進(jìn)行匹配的模板，虛線矩形框代表進(jìn)行匹配的待檢測(cè)圖像。

Fig.2 Template match in different distances圖2 遠(yuǎn)近距離的模板匹配

為了解決行人距離機(jī)器人遠(yuǎn)近不同影響相似性計(jì)算的問題，本文提出了基于深度圖像的漫水算法[15]。隨著傳感器技術(shù)的發(fā)展，傳感器獲取的信息越來越多，其中微軟公司的Kinect就是其中的代表。Kinect傳感器除了能夠獲取RGB顏色信息外，還能夠獲取深度信息，進(jìn)而形成點(diǎn)云圖像。深度圖像能夠準(zhǔn)確反映環(huán)境中物體到Kinect的距離，其中每個(gè)像素的值代表了距離大小。漫水算法以粒子所在的位置為中心，比較粒子與其周圍像素值的差值，這個(gè)差值代表了粒子與周圍像素之間的距離。如果差值小于設(shè)定的閾值，就將周圍像素加入隊(duì)列。然后將隊(duì)列頭元素出隊(duì)列，并以其為中心位置，循環(huán)尋找其周圍像素差值小的像素，直到隊(duì)列為空。圖3展示了漫水算法的大致流程。在這個(gè)不斷循環(huán)的過程中，漫水算法實(shí)際上將粒子所在位置，同時(shí)隸屬于一個(gè)物體的所有像素進(jìn)行了聚類。完成聚類后，可以用一個(gè)最小的矩形框?qū)⑿腥撕蜻x圖像標(biāo)識(shí)出來。這個(gè)矩形框既不會(huì)太大也不會(huì)太小，這樣就避免了行人距離遠(yuǎn)近對(duì)模板匹配造成的不良影響，提高了相似性計(jì)算的準(zhǔn)確性。

Fig.3 Procedure of flood filled algorithm圖3 漫水算法的流程

3.3 社交力

在使用粒子濾波算法進(jìn)行行人跟蹤時(shí)，對(duì)行人的運(yùn)動(dòng)采用了二階自回歸模型。這個(gè)模型假設(shè)行人的速度是恒定的，并不能很好地反映真實(shí)環(huán)境中行人的運(yùn)動(dòng)。在復(fù)雜的實(shí)際場景中，行人并不是單獨(dú)存在的，行人的運(yùn)動(dòng)受到其他行人或障礙物的影響。同時(shí)，如果單純使用顏色和深度信息進(jìn)行相似性估計(jì)，在出現(xiàn)多個(gè)行人部分遮擋甚至完全遮擋時(shí)，相似估計(jì)的計(jì)算就會(huì)失效。其實(shí)相似性估計(jì)并不是最終目的，最終目的是通過相似性估計(jì)的結(jié)果調(diào)整每個(gè)粒子的權(quán)重。粒子濾波算法中，k時(shí)刻的狀態(tài)由k-1時(shí)刻的粒子狀態(tài)轉(zhuǎn)移得來，并且用N個(gè)粒子來構(gòu)建k時(shí)刻的狀態(tài)分布。本文使用社交力方法[16]能夠更加真實(shí)地刻畫行人運(yùn)動(dòng)所受到的約束，通過社交力改變每個(gè)粒子權(quán)重的分配。

社交力分為吸引力和排斥力，吸引力主要考慮行人的運(yùn)動(dòng)意圖以及目標(biāo)位置，大多數(shù)情況下無從推測(cè)。排斥力主要涉及環(huán)境和其他行人，本文主要考慮行人之間的排斥力。行人之間排斥力的大小，與它們之間的距離有關(guān)。假設(shè)k時(shí)刻粒子s代表的行人i的位置為，與它相近的行人j的位置為，則行人i與行人j之間的距離可以定義為：

行人i與行人j之間形成的社交力分配給粒子s的權(quán)重為：

其中，αd是為了避免行人相互碰撞而設(shè)置的距離參數(shù)。在環(huán)境中有多個(gè)行人時(shí)，行人i與其他所有行人之間形成的社交力分配給粒子s的總權(quán)重為：

其中，參數(shù)eij代表其他行人對(duì)行人i的影響力，其定義為：

參數(shù)εw代表行人的影響力半徑。針對(duì)環(huán)境中的每一個(gè)行人，都可以用類似的方法計(jì)算其社交力對(duì)粒子權(quán)重的影響。

采用改進(jìn)的相似性估計(jì)方法，首先利用深度信息獲取待檢測(cè)的最佳窗口，然后計(jì)算HSV顏色直方圖之間的相似性，并且引入社交力對(duì)粒子的權(quán)重影響，最終重新分配粒子權(quán)重。

4 粒子分布的優(yōu)化

4.1 粒子退化及多樣性缺失

粒子濾波算法本身存在一個(gè)很嚴(yán)重的問題，那就是粒子退化[17]。粒子退化是指算法在對(duì)目標(biāo)進(jìn)行估計(jì)時(shí)，由于經(jīng)過多次迭代，粒子權(quán)重各不相同。只有少部分權(quán)重較大的粒子對(duì)估計(jì)結(jié)果影響很大，大部分權(quán)重較小的粒子對(duì)估計(jì)結(jié)果只起到了微乎其微的效果。這個(gè)問題一方面導(dǎo)致估計(jì)的偏差增大，另一方面導(dǎo)致大量的計(jì)算耗費(fèi)在無用粒子上。

為了盡量減少粒子退化，常見的做法是將先驗(yàn)概率當(dāng)作采樣函數(shù)。然而選擇合適的采樣函數(shù)并不能真正解決問題，當(dāng)先驗(yàn)概率和觀測(cè)概率重合時(shí)，還是會(huì)使得權(quán)重過于集中在少數(shù)粒子上，最終仍然導(dǎo)致粒子退化。粒子重采樣可以很好地解決粒子退化問題，其主要思想是通過粒子權(quán)重進(jìn)行粒子采樣。通過這種方法，權(quán)重太小的粒子在重采樣過程中被淘汰，權(quán)重較大的粒子在其周圍采樣多個(gè)粒子。主要過程如圖4所示。

Fig.4 Particle resampling圖4 粒子重采樣

重采樣過程根據(jù)權(quán)重機(jī)制很好地解決了粒子退化問題，但同時(shí)它導(dǎo)致了粒子多樣性的缺失。每個(gè)粒子的權(quán)重在重采樣過后都會(huì)相等，但粒子會(huì)出現(xiàn)集中現(xiàn)象，多個(gè)粒子的狀態(tài)很相近，造成粒子的多樣性缺失。在對(duì)單個(gè)行人進(jìn)行跟蹤時(shí)，通過重采樣就可以取得良好的跟蹤效果。但是當(dāng)跟蹤多個(gè)行人目標(biāo)時(shí)，粒子多樣性的缺失會(huì)導(dǎo)致部分行人跟蹤失敗，其中一個(gè)極端的例子就是在多個(gè)行人跟蹤問題過程中只能跟蹤到一個(gè)行人。

4.2 二級(jí)粒子

在粒子濾波算法中引入重采樣過程可以有效解決粒子退化問題，但其引起的粒子多樣性缺失問題依然影響算法的跟蹤性能。為此，本文提出二級(jí)粒子的概念，即對(duì)于新產(chǎn)生的粒子，不是僅僅根據(jù)粒子的權(quán)重進(jìn)行重采樣，而是結(jié)合權(quán)重利用高斯分布在原始粒子周圍隨機(jī)產(chǎn)生。

假設(shè)重采樣之前有N個(gè)粒子，這些粒子的權(quán)重之和等于1，根據(jù)粒子權(quán)重將[0,1]區(qū)間劃分成N份：

通過在均勻分布U[0,1]中隨機(jī)產(chǎn)生一個(gè)數(shù)ui來決定在重采樣階段哪些粒子保留哪些粒子丟棄。假設(shè)ui落在區(qū)間i上，則權(quán)重為wi的粒子保留，并根據(jù)產(chǎn)生二級(jí)粒子。如果在N次生成中，隨機(jī)數(shù)ui落在第i個(gè)區(qū)間的次數(shù)為ni,則生成ni個(gè)二級(jí)粒子。粒子產(chǎn)生的二級(jí)粒子滿足如下正態(tài)分布：

分布的均值為，方差為α2，其中方差由粒子退化程度Deff和狀態(tài)變量的方差ε2決定。

當(dāng)Deff減小時(shí)，二級(jí)粒子之間的方差變大，二級(jí)粒子的采樣空間變大。ε2決定了跟蹤的精度，ε2增大時(shí)跟蹤精度減小同時(shí)二級(jí)粒子的采樣空間變大。參數(shù)r決定了二級(jí)粒子分布的擴(kuò)展程度。引入二級(jí)粒子后，權(quán)值大的粒子在重采樣階段，在其周圍將大概率事件生成更多的粒子。這樣和基于權(quán)值重要性重采樣比較類似，對(duì)于相似度較高的物體能夠進(jìn)行有效的跟蹤。但與其不同是，其粒子多樣性更加明顯?；跈?quán)值重要性的重采樣，在多個(gè)目標(biāo)同時(shí)跟蹤時(shí)，當(dāng)某個(gè)目標(biāo)丟失又重新出現(xiàn)后，很難在短時(shí)間內(nèi)恢復(fù)到多目標(biāo)的跟蹤。而二級(jí)粒子，由于其重采樣粒子分布具有部分隨機(jī)性，能夠更加快速應(yīng)對(duì)目標(biāo)丟失又出現(xiàn)的情況。

二級(jí)粒子重采樣方法與權(quán)值重要性重采樣在重采樣粒子個(gè)數(shù)方面都具有隨機(jī)性，但在重采樣粒子的分布方面，權(quán)值重要性重采樣在重采樣階段完全依賴上一階段粒子的權(quán)值，二級(jí)粒子重采樣則在依賴上一階段粒子權(quán)值的基礎(chǔ)上引入了隨機(jī)性。二級(jí)粒子重采樣有效避免了粒子退化，在出現(xiàn)行人部分甚至完全遮擋后，仍然能對(duì)恢復(fù)的行人進(jìn)行有效的跟蹤。本文采用Matlab模擬了改進(jìn)粒子濾波和SIR[15,18]粒子濾波兩種采樣方式在跟蹤行人時(shí)的場景，如圖5所示。在初始階段，采樣粒子隨機(jī)分布在需要跟蹤的3個(gè)目標(biāo)A、B、C上。在粒子跟蹤一段時(shí)間后，目標(biāo)B突然消失（模擬完全遮擋等情況）。此時(shí)B消失處的粒子與目標(biāo)相似度極小，導(dǎo)致該處的粒子權(quán)值很小。SIR粒子濾波的重采樣機(jī)制十分依賴權(quán)值，因而在重采樣后，B消失處沒有產(chǎn)生新粒子。但改進(jìn)粒子濾波由于引入了二級(jí)粒子增加了隨機(jī)性，相似度雖然小，但還是產(chǎn)生了少量粒子。繼續(xù)跟蹤一段時(shí)間后，消失的目標(biāo)B重新出現(xiàn)。改進(jìn)的粒子濾波相比SIR粒子濾波能很快捕捉到重新出現(xiàn)的目標(biāo)B。一段時(shí)間后，改進(jìn)粒子濾波對(duì)目標(biāo)B的跟蹤趨于穩(wěn)定，與一直跟蹤正常的A、C類似。但是SIR粒子濾波由于多樣性的不足，并沒有很快地跟蹤到B。在對(duì)消失后又重新出現(xiàn)的目標(biāo)B進(jìn)行跟蹤時(shí)，明顯改進(jìn)的粒子濾波具有更好的魯棒性。

5 實(shí)驗(yàn)結(jié)果與分析

5.1 實(shí)驗(yàn)設(shè)置

Fig.5 Comparison of two sampling methods圖5 兩種采樣方法的對(duì)比

為了驗(yàn)證改進(jìn)粒子濾波算法的實(shí)際效果，本文在兩組數(shù)據(jù)上進(jìn)行了實(shí)驗(yàn)。第一組數(shù)據(jù)是使用turtle-bot機(jī)器人在大小為4.9 m×4.5 m的實(shí)驗(yàn)室環(huán)境中采集的，其中對(duì)turtlebot機(jī)器人進(jìn)行了改造，使Kinect位置更加適合識(shí)別跟蹤行人，如圖6所示。第二組數(shù)據(jù)采用了網(wǎng)上公開數(shù)據(jù)集IAS-Lab[19-21]。該數(shù)據(jù)集使用Pioneer P3AT機(jī)器人在室內(nèi)環(huán)境中采集，其中RGB圖像的分辨率為640×480，深度圖像的分辨率為160×120。數(shù)據(jù)集分為機(jī)器人移動(dòng)和不移動(dòng)兩大類，每大類又分為如下3個(gè)小類：沒有障礙物，行人做簡單的線性運(yùn)動(dòng)；沒有障礙物，行人做復(fù)雜的運(yùn)動(dòng)，相互之間有穿插；有障礙物，行人做復(fù)雜的運(yùn)動(dòng)，相互之間有穿插。上述兩組數(shù)據(jù)集，一組是實(shí)際環(huán)境中采集的，一組是網(wǎng)上公開的，可以比較充分地驗(yàn)證算法的性能。每組數(shù)據(jù)都有可供參考的ground truth，用來判斷跟蹤結(jié)果與真實(shí)結(jié)果之間的差異。系統(tǒng)運(yùn)行的環(huán)境是Intel?Core?i5-4200M CPU@2.50 GHz×4的筆記本電腦。

Fig.6 Remoulded turtlebot圖6 改造的turtlebot機(jī)器人

5.2 評(píng)估標(biāo)準(zhǔn)

良好的跟蹤算法必須確保在跟蹤多個(gè)目標(biāo)時(shí)沒有遺漏，并且準(zhǔn)確估計(jì)出每個(gè)目標(biāo)的位置、方向和軌跡等。同時(shí)跟蹤算法應(yīng)該具有可持續(xù)性，為每一個(gè)目標(biāo)確定一個(gè)跟蹤ID。因此主要的評(píng)價(jià)指標(biāo)有：準(zhǔn)確估計(jì)出每個(gè)跟蹤目標(biāo)的確切位置；可持續(xù)性地跟蹤目標(biāo)，能夠產(chǎn)生每個(gè)目標(biāo)的跟蹤軌跡。此外，良好的評(píng)價(jià)標(biāo)準(zhǔn)還應(yīng)該擁有以下特征：盡量少的參數(shù)變量，可自動(dòng)調(diào)節(jié)的閾值；清晰易于理解，符合人的直觀感受；具有足夠的通用性，容易對(duì)跟蹤算法進(jìn)行比較；指標(biāo)數(shù)量少，但是每個(gè)指標(biāo)都能反映重要性能的優(yōu)劣。

本文借鑒了文獻(xiàn)[22]的兩個(gè)跟蹤指標(biāo)：多目標(biāo)跟蹤精度（multiple object tracking precision，MOTP），多目標(biāo)跟蹤準(zhǔn)確度（multiple object tracking accuracy，MOTA）。MOTP表示在所有幀中對(duì)已經(jīng)跟蹤到的目標(biāo)的位置估計(jì)的總偏差，它反映了跟蹤算法對(duì)目標(biāo)位置估計(jì)的精度，計(jì)算方法如下：

其中，ct代表在第t幀中跟蹤目標(biāo)的個(gè)數(shù)；代表在第t幀中第i個(gè)跟蹤目標(biāo)的估計(jì)位置與實(shí)際位置之間的距離。MOTP表示跟蹤目標(biāo)個(gè)數(shù)與實(shí)際目標(biāo)個(gè)數(shù)之間的差異，計(jì)算方法如下：

其中，mt、fpt、mmet分別代表丟失跟蹤的目標(biāo)個(gè)數(shù)、跟蹤非目標(biāo)的個(gè)數(shù)和跟蹤匹配錯(cuò)誤的個(gè)數(shù)。

5.3 結(jié)果分析

在準(zhǔn)備好數(shù)據(jù)集，定義好評(píng)價(jià)標(biāo)準(zhǔn)后，本文對(duì)比了改進(jìn)的粒子濾波算法、SIR粒子濾波算法和EKF[15,23]算法對(duì)行人跟蹤的效果。SIR粒子濾波和EKF濾波是兩種經(jīng)典的狀態(tài)估計(jì)方法，其中EKF濾波通過線性化方法將非線性問題轉(zhuǎn)換成線性問題，然后使用KF濾波進(jìn)行估計(jì)，是一種觀測(cè)推導(dǎo)的過程，SIR是一種根據(jù)權(quán)值重采樣的蒙特卡洛估計(jì)方法。在非線性估計(jì)領(lǐng)域，SIR粒子濾波和EKF濾波是研究的兩大主流方向。很多的研究都是基于這兩種方法中的一種進(jìn)行改進(jìn)，本文也是在SIR粒子濾波的基礎(chǔ)上提出了二級(jí)粒子的概念。因此，本文采用了SIR粒子濾波和EKF濾波進(jìn)行了對(duì)比分析。圖7顯示了在實(shí)驗(yàn)室采集的數(shù)據(jù)集上，行人跟蹤的效果圖，每一幀圖像由上方的RGB圖像和下方的深度圖像組成。本文截取的4幀圖像中，藍(lán)色方框是改進(jìn)粒子濾波的跟蹤效果，紅色方框是SIR粒子濾波的跟蹤效果，綠色方框是EKF的跟蹤效果。圖8顯示在跟蹤過程中，3種跟蹤算法精度誤差的不斷變化。在單個(gè)行人跟蹤的初始階段，3種算法都能較好地跟蹤行人位置。隨著跟蹤繼續(xù)，3種跟蹤算法的誤差都有所增大。SIR粒子濾波算法在進(jìn)行匹配時(shí)只考慮了顏色信息，匹配的精確度受環(huán)境背景的影響，出現(xiàn)了一定程度的漂移。EKF算法在跟蹤時(shí)，對(duì)環(huán)境中非高斯噪聲不敏感，在跟蹤過程中其跟蹤矩形框截取了行人的部分軀體，并沒有完全圈出整個(gè)行人。改進(jìn)粒子濾波算法不但利用了顏色信息，還使用了深度信息。利用漫水算法在深度圖像上漫延，可以覆蓋行人的整部分，作為顏色信息的有效補(bǔ)充，提高跟蹤的精度。由于采用不同的檢測(cè)信息對(duì)跟蹤精度有不同的影響，本文做了4組對(duì)比實(shí)驗(yàn)進(jìn)行分析，如圖9所示。顏色是最主要的特征，因此4組實(shí)驗(yàn)中的檢測(cè)信息都使用了顏色。從圖中可以看出跟蹤的精度整體趨于不斷增大、逐漸穩(wěn)定的態(tài)勢(shì)，其中對(duì)跟蹤精度影響最大的是顏色信息，其次是深度信息，再次是社交力。

Fig.7 Tracking results on lab's data圖7 在實(shí)驗(yàn)室數(shù)據(jù)集上的跟蹤結(jié)果

Fig.8 Precision error of different algorithms圖8 3種算法的跟蹤精度誤差

公開數(shù)據(jù)集IAS-Lab上的實(shí)驗(yàn)如圖10所示，主要測(cè)試了3種情況下的跟蹤效果，每種情況由左邊的RGB圖像和右邊與其對(duì)應(yīng)的點(diǎn)云圖像構(gòu)成。每種情況中，藍(lán)色方框是改進(jìn)粒子濾波的跟蹤效果，紅色方框是SIR粒子濾波的跟蹤效果，綠色方框是EKF的跟蹤效果。第一種情況是沒有障礙物，兩個(gè)行人做簡單的線性運(yùn)動(dòng)。第二種情況是沒有障礙物，3個(gè)行人做復(fù)雜的運(yùn)動(dòng)，相互之間有穿插。第三種情況是有障礙物，行人做復(fù)雜的運(yùn)動(dòng)，相互之間有穿插。在第一種情況下，3種算法跟蹤基本穩(wěn)定，只是跟蹤的精度有所差別。第二種情況下，SIR粒子濾波在行人互相遮擋時(shí)會(huì)丟失跟蹤目標(biāo)，EKF算法在行人從跟蹤窗口消失然后又出現(xiàn)時(shí)也會(huì)丟失目標(biāo)，改進(jìn)粒子濾波跟蹤相對(duì)良好。這是因?yàn)镾IR粒子濾波的粒子多樣性比較缺失，行人相互遮擋后粒子會(huì)變得過度集中，進(jìn)而丟失跟蹤目標(biāo)。EKF算法的本質(zhì)是將非線性系統(tǒng)轉(zhuǎn)換為線性系統(tǒng)，當(dāng)行人在窗口消失后又出現(xiàn)時(shí)，算法并不能處理新出現(xiàn)的行人。第三種情況下，3種算法都有出現(xiàn)目標(biāo)跟蹤丟失的情況。與第二種情況不同的是環(huán)境中增加了障礙物，當(dāng)障礙物與行人互相遮擋時(shí)，對(duì)行人跟蹤的精度產(chǎn)生了影響。當(dāng)完全遮擋時(shí)，SIR算法和EKF算法甚至跟蹤失敗了，表1顯示了3種算法在IAS-Lab數(shù)據(jù)集上的平均MOTP值和平均MOTA值。

Fig.10 Tracking results on public data圖10 公開數(shù)據(jù)集的跟蹤結(jié)果

Table 1 Performance of 3 algorithms表1 3種算法的跟蹤性能 %

因?yàn)樗惴ú渴鹪趯?shí)體機(jī)器人上，所以本文還對(duì)比了3種算法的平均運(yùn)行時(shí)間，如表2所示。從表中可以看出，EKF算法的運(yùn)行時(shí)間最短，改進(jìn)粒子濾波其次，SIR粒子濾波的運(yùn)行時(shí)間最長。EKF算法通過線性化處理非線性濾波估計(jì)，采用線性推導(dǎo)的方式，因此處理速度最快。改進(jìn)粒子濾波算法與SIR粒子濾波都是使用采樣估計(jì)的方式，但改進(jìn)粒子濾波在計(jì)算粒子相似性方面極大提高了速率。改進(jìn)粒子濾波使用了深度信息與社交力概念，其中深度信息相比于RGB信息計(jì)算量小，對(duì)一部分深度信息跳躍性大的粒子做了摒棄，社交力概念對(duì)不符合避免碰撞推理的粒子也做了摒棄，減少了很大的計(jì)算量。因此，改進(jìn)粒子濾波在運(yùn)行時(shí)間上較SIR粒子濾波縮短很大。

Table 2 Running time of 3 algorithms表2 3種算法的運(yùn)行時(shí)間

改進(jìn)粒子濾波相比于SIR粒子濾波，在粒子個(gè)數(shù)和粒子分布上有更明顯的隨機(jī)化。隨機(jī)化的程度直接影響著跟蹤的準(zhǔn)確性和精確性，因此對(duì)于隨機(jī)化程度的控制十分重要。在改進(jìn)粒子濾波中，隨機(jī)化程度是使用二級(jí)粒子在所有重采樣粒子中的占比和二級(jí)粒子的采樣半徑來決定的。本文為了確定最佳的隨機(jī)化程度，做了多組實(shí)驗(yàn)，如圖11所示。通過對(duì)比發(fā)現(xiàn)，當(dāng)二級(jí)粒子所占比例為27%，二級(jí)粒子的采樣半徑為1.1 m時(shí)，行人跟蹤的精確率和準(zhǔn)確率是最高的。通過分析發(fā)現(xiàn)，二級(jí)粒子的采樣半徑與行人的步速呈正相關(guān)。同時(shí)，二級(jí)粒子所占比例的最佳值也不是固定的，與環(huán)境的復(fù)雜度以及行人遮擋消失也相關(guān)。

Fig.11 Precision error affected by second particles'proportion and sampling radius圖11 精度誤差受二級(jí)粒子占比和采樣半徑的影響

6 結(jié)論

針對(duì)移動(dòng)機(jī)器人的行人跟蹤，本文選取適用于非線性非高斯系統(tǒng)估計(jì)的粒子濾波算法。在分析基本粒子濾波算法的基礎(chǔ)上，從兩方面進(jìn)行了改進(jìn)。一方面為了提高跟蹤的精度，不但使用了顏色直方圖進(jìn)行相似性估計(jì)，還在深度圖像上利用漫水算法解決了遠(yuǎn)近匹配估計(jì)不準(zhǔn)的問題。同時(shí)，引入社交力概念在對(duì)粒子分配權(quán)重時(shí)，考慮了行人之間互相避免碰撞情形。另一方面提出了二級(jí)粒子的概念，解決了原始粒子濾波算法多樣性缺失的問題。二級(jí)粒子通過引入粒子個(gè)數(shù)隨機(jī)化和粒子狀態(tài)隨機(jī)化，提高了跟蹤多個(gè)行人的準(zhǔn)確度。在實(shí)驗(yàn)室采集的數(shù)據(jù)集和公開的IAS-Lab數(shù)據(jù)集上，通過實(shí)驗(yàn)證明了改進(jìn)粒子濾波算法的有效性和優(yōu)越性。

[1]Dondrup C,Bellotto N,Jovan F,et al.Real-time multisensor people tracking for human-robot spatial interaction[C]//International Conference on Robotics and Automation Workshop on Machine Learning for Social Robotics,Seattle,USA,May 26-31,2015.Piscataway,USA:IEEE,2015:26-31.

[2]Linder T,Kai O A.People detection,tracking and visualization using ROS on a mobile service robot[M].New York:Springer International Publishing,2016:187-213.

[3]Smeulders AW,Chu D M,Cucchiara R,et al.Visual tracking:an experimental survey[J].IEEE Transactions on PatternAnalysis and Machine Intelligence,2014,36(7):1442-1468.

[4]Comaniciu D,Ramesh V,Meer P.Kernel-based object tracking[J].IEEE Transactions on Pattern Analysis and Machine Intelligence,2003,25(5):564-575.

[5]Du Kai,JuYongfeng,Jin Yinli,et al.MeanShift tracking algorithm with adaptive block color histogram[J].Journal of Wuhan University of Technology,2012,34(6):140-144.

[6]Park K I,Park C I,Lee J.Vision based people tracking using stereo camera and Camshift[C]//LNCS 8102:Proceedings of the 6th International Conference on Intelligent Robotics and Applications,Busan,South Korea,Sep 25-28,2013.Berlin,Heidelberg:Springer,2013:145-153.

[7]Lee S J,Won M.Avision based people tracking and following for mobile robots using CAMSHIFT and KLT feature tracker[J].Journal of Korea Multimedia Societ,2014,17(7):787-796.

[8]Arulampalam M S,Maskell S,Gordon N,et al.A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J].IEEE Transactions on Signal Processing,2002,50(2):174-188.

[9]Okuma K,Taleghani A,Freitas N D,et al.A boosted particle filter:multitarget detection and tracking[C]//LNCS 3021:Proceedings of the 2004 European Conference on Computer Vision,Prague,Czech Republic,May 11-14,2004.Berlin,Heidelberg:Springer,2004:28-39.

[10]Wang Xin,Tang Zhenmin.Modified particle filter-based infrared pedestrian tracking[J].Infrared Physics&Technology,2010,53(4):280-287.

[11]Aguirre E,García-Silvente M,Plata J.Leg detection and tracking for a mobile robot and based on a laser device,supervised learning and particle filtering[C]//Proceedings of the 1st Iberian Robotics Conference-Advances in Robotics,Madrid,Spain,Nov 28-29,2013.Switzerland:Springer International Publishing,2014:433-440.

[12]Aguirre E,García-Silvente M,Pascual D.A multisensor based approach using supervised learning and particle filtering for people detection and tracking[C]//Proceedings of the 2nd Iberian Robotics Conference-Advances in Robotics,Lisbon,Portugal,Nov 19-21,2015.Switzerland:Springer International Publishing,2016:645-657.

[13]Danelljan M,Khan F S,Felsberg M,et al.Adaptive color attributes for real-time visual tracking[C]//Proceedings of the 2014 Conference on Computer Vision and Pattern Recognition,Columbus,USA,Jun 23-28,2014.Washington:IEEE Computer Society,2014:1090-1097.

[14]Singh P,Deepak B B V L,Sethi T,et al.Real-time object detection and tracking using color feature and motion[C]//Proceedings of the 2015 International Conference on Communications and Signal Processing,Melmaruvathur,India,Apr 2-4,2015.Piscataway,USA:IEEE,2015:1236-1241.

[15]Gordon N J,Salmond D J,Smith A F M,et al.Novel approach to nonlinear/non-Gaussian Bayesian state estimation[J].IEE Proceedings F-Radar and Signal Processing,1993,140(2):107-113.

[16]Luber M,Stork J A,Tipaldi G D,et al.People tracking with human motion predictions from social forces[C]//Proceedings of the 2010 International Conference on Robotics and Automation,Anchorage,USA,May 3-7,2010.Piscataway,USA:IEEE,2010:464-469.

[17]Daum F,Huang J.Particle degeneracy:root cause and solution[J].Proceedings of SPIE,2011,8050(4):1-11.

[18]Ruslan F A,Adnan R,Samad A M,et al.Parameters effect in sampling importance resampling(SIR)particle filter prediction and tracking of flood water level performance[C]//Proceedings of the 12th International Conference on Control,Automation and Systems,Jeju Island,South Korea,Oct17-21,2012.Piscataway,USA:IEEE,2012:868-872.

[19]Basso F,Munaro M,Michieletto S,et al.Fast and robust multi-people tracking from RGB-D data for a mobile robot[C]//Proceedings of the 12th International Conference on Intelligent Autonomous Systems 12,Jeju Island,Korea,Jun 26-29,2012.Berlin,Heidelberg:Springer,2013:265-276.

[20]Munaro M,Basso F,Menegatti E.Tracking people within groups with RGB-D data[C]//Proceedings of the 2012 International Conference on Intelligent Robots and Systems,Vilamoura,Portugal,Oct 7-12,2012.Piscataway,USA:IEEE,2012:2101-2107.

[21]Munaro M,Menegatti E.Fast RGB-D people tracking for service robots[J].Autonomous Robots,2014,37(3):227-242.

[22]Keni B,Rainer S.Evaluating multiple object tracking performance:the CLEAR MOT metrics[J].Eurasip Journal on Image&Video Processing,2008(1):1-10.

[23]Toloei A,Niazi S.State estimation for target tracking problems with nonlinear Kalman filter algorithms[J].International Journal of ComputerApplications,2014,98(17):30-36.

附中文參考文獻(xiàn):

[5]杜凱,巨永鋒,靳引利,等.自適應(yīng)分塊顏色直方圖的MeanShift跟蹤算法[J].武漢理工大學(xué)學(xué)報(bào),2012,34(6):140-144.

2017-03,Accepted 2017-05.

People Tracking of Mobile Robot Using Improved Particle Filter*

XIAKefu1,2,LI Pengfei2,CHEN Xiaoping2+
1.Anhui Vocational College of Electronics&Information Technology,Bengbu,Anhui 233030,China
2.School of Computer Science and Technology,University of Science and Technology of China,Hefei 230027,China
+Corresponding author:E-mail:xpchen@ustc.edu.cn

People tracking for mobile robot is an important application which reflects the intelligence of robot,and it has wide prospect and practical value.However,there exists a big challenge because of the complexity of environment and the uncertainty of people moving.This paper improves the particle filter for mobile robot people tracking based on the analysis of particle filter framework.On the hand,the color information,depth information and social force are combined to estimate the similarity between the template and candidate,which increases the tracking precision.On the other hand,a concept of secondary particle is proposed to overcome the loss of particle's diversity,which increases the tracking accuracy.In the last,the improved particle filter,sequential importance resampling(SIR)particle filter and extended Kalman filter(EKF)algorithm are compared on the turtlebot robot and the public data IAS-Lab.Results prove the superiority of improved particle filter.

mobile robot;people tracking;particle filter;depth information;social force;secondary particle

10.3778/j.issn.1673-9418.1703052

*The Key Natural Science Research Project of Anhui Province University under Grant No.KJ2016A050(安徽省高校自然科學(xué)研究重點(diǎn)項(xiàng)目);the Fundamental Research Funds for the Central Universities of China under Grant No.WK0110000038(中央高校基本科研業(yè)務(wù)費(fèi)專項(xiàng)資金).

CNKI網(wǎng)絡(luò)優(yōu)先出版:2017-05-22,http://kns.cnki.net/kcms/detail/11.5602.TP.20170522.1046.002.html

XIA Kefu,LI Pengfei,CHEN Xiaoping.People tracking of mobile robot using improved particle filter.Journal of Frontiers of Computer Science and Technology,2017,11(11)：1849-1859.

A

TP311

XIA Kefu was born in 1978.He received the M.S.degree in computer technology from Hefei University of Technology in 2008.Now he is an associate professor at Anhui Vocational College of Electronics&Information Technology.His research interests include image processing and machine vision,etc.

夏克付（1978—），男，安徽宿松人，2008年于合肥工業(yè)大學(xué)獲得碩士學(xué)位，現(xiàn)為安徽電子信息職業(yè)技術(shù)學(xué)院副教授，主要研究領(lǐng)域?yàn)閳D像處理與識(shí)別，機(jī)器視覺等。

LI Pengfei was born in 1989.He is an M.S.candidate at University of Science and Technology of China.His research interest is service robot.

李鵬飛（1989—），男，湖北麻城人，中國科學(xué)技術(shù)大學(xué)碩士研究生，主要研究領(lǐng)域?yàn)榉?wù)機(jī)器人。

CHEN Xiaoping was born in 1955.He received the Ph.D.degree in pattern recognition and intelligent system from University of Science and Technology of China in 1996.Now he is a professor and Ph.D.supervisor at University of Science and Technology of China.His research interests include artificial intelligence and autonomous robot,etc.陳小平（1955—），男，北京人，1996年于中國科學(xué)技術(shù)大學(xué)獲得博士學(xué)位，現(xiàn)為中國科學(xué)技術(shù)大學(xué)教授、博士生導(dǎo)師，主要研究領(lǐng)域?yàn)槿斯ぶ悄埽灾鳈C(jī)器人等。