蘇奎峰， 常天慶， 張 雷， 楊國(guó)振

(裝甲兵工程學(xué)院控制工程系， 北京 100072)

基于PMCMC-RFS的自主車SLAM算法

蘇奎峰， 常天慶， 張 雷， 楊國(guó)振

(裝甲兵工程學(xué)院控制工程系， 北京 100072)

針對(duì)無(wú)人自主車同時(shí)定位與地圖構(gòu)建(Simultaneous Localization And Mapping，SLAM)問(wèn)題，采用隨機(jī)有限集(Random Finite Set, RFS)方法對(duì)環(huán)境特征和車輛的位姿進(jìn)行描述，將SLAM算法涉及到的多路標(biāo)特征檢測(cè)、跟蹤、識(shí)別及相關(guān)等問(wèn)題在一個(gè)統(tǒng)一的貝葉斯?fàn)顟B(tài)估計(jì)框架內(nèi)表述，從而可以有效地解決后驗(yàn)估計(jì)、信息融合等算法嚴(yán)重依賴數(shù)據(jù)關(guān)聯(lián)結(jié)果的問(wèn)題。同時(shí)，為了計(jì)算復(fù)雜的聯(lián)合后驗(yàn)分布，解決粒子濾波算法中提議分布選擇困難問(wèn)題，采用序貫蒙特卡羅(Sequential Monte Carlo，SMC)算法為馬爾科夫鏈蒙特卡羅(Markov Chain Monte Carlo，MCMC)采樣構(gòu)建高維提議分布策略，提出了基于PMCMC-RFS(Particle MCMC based RFS)的SLAM問(wèn)題求解方法。試驗(yàn)結(jié)果表明：PMCMC-RFS算法能動(dòng)態(tài)估計(jì)感知范圍內(nèi)的特征數(shù)量，有效地避免了數(shù)據(jù)關(guān)聯(lián)問(wèn)題，從而提高了狀態(tài)估計(jì)性能。

同時(shí)定位與地圖構(gòu)建；隨機(jī)有限集；馬爾科夫鏈蒙特卡羅；粒子濾波

同時(shí)定位與地圖構(gòu)建(Simultaneous Localization And Mapping，SLAM)最早由Smith等[1]作為統(tǒng)計(jì)狀態(tài)估計(jì)問(wèn)題而提出，是指無(wú)人平臺(tái)或機(jī)器人在未知環(huán)境行駛過(guò)程中逐步構(gòu)建環(huán)境地圖的同時(shí)估計(jì)自身的位姿狀態(tài)，是無(wú)人自主車輛的關(guān)鍵技術(shù)，是環(huán)境感知、路徑規(guī)劃、避障以及運(yùn)動(dòng)控制的基礎(chǔ)。因SLAM具有重要的理論和應(yīng)用價(jià)值，被眾多學(xué)者認(rèn)為是移動(dòng)機(jī)器人實(shí)現(xiàn)完全自主行駛的關(guān)鍵技術(shù)，已成為機(jī)器人領(lǐng)域研究的熱點(diǎn)之一。

經(jīng)典SLAM問(wèn)題采用空間矢量方法描述自主車和路標(biāo)狀態(tài)，需要獨(dú)立估計(jì)路標(biāo)特征、數(shù)量以及自主車位姿狀態(tài)，狀態(tài)估計(jì)結(jié)果嚴(yán)重依賴數(shù)據(jù)關(guān)聯(lián)結(jié)果，且對(duì)其不確定性非常敏感。此類SLAM算法的主要問(wèn)題在于：沒(méi)有將數(shù)據(jù)關(guān)聯(lián)、狀態(tài)估計(jì)的不確定性集成在統(tǒng)一的貝葉斯框架內(nèi)考慮，而且在狀態(tài)估計(jì)階段又假設(shè)數(shù)據(jù)關(guān)聯(lián)結(jié)果已知，從而導(dǎo)致算法的環(huán)境適應(yīng)能力和魯棒性較差。在SLAM 問(wèn)題中，地圖特征實(shí)質(zhì)上是有限集合，采用隨機(jī)有限集(Random Finite Set，RFS)理論描述和處理此類問(wèn)題更為合理。將地圖特征對(duì)應(yīng)的多路標(biāo)狀態(tài)和傳感器觀測(cè)信息表示成RFS，對(duì)自主車位姿和地圖特征聯(lián)合后驗(yàn)概率密度分布進(jìn)行遞推貝葉斯估計(jì)，進(jìn)而能夠有效描述多種不確定因素的同時(shí)，避免了復(fù)雜的多目標(biāo)數(shù)據(jù)關(guān)聯(lián)問(wèn)題，解決了復(fù)雜環(huán)境下路標(biāo)特征數(shù)量不確定的多特征跟蹤問(wèn)題[2-3]。

采用RFS理論可將多傳感器信息融合的多路標(biāo)檢測(cè)、跟蹤、識(shí)別以及相關(guān)等問(wèn)題統(tǒng)一起來(lái)，能夠有效處理后驗(yàn)估計(jì)、信息融合算法評(píng)估等問(wèn)題。本文針對(duì)路標(biāo)特征密集、動(dòng)態(tài)特征較多的城鄉(xiāng)道路環(huán)境，采用RFS理論構(gòu)建了RFS-SLAM模型，并將SLAM路標(biāo)估計(jì)問(wèn)題轉(zhuǎn)換為多路標(biāo)跟蹤問(wèn)題，給出了基于粒子濾波概率密度假設(shè)的RFS-SLAM求解方法。為計(jì)算復(fù)雜的聯(lián)合后驗(yàn)分布，解決粒子濾波算法中提議分布選擇困難問(wèn)題，采用序貫蒙特卡羅(Sequential Monte Carlo，SMC)算法為馬爾科夫鏈蒙特卡羅(Markov Chain Monte Carlo，MCMC)采樣設(shè)計(jì)高維提議分布，使得可以從SMC或MCMC不能單獨(dú)采樣的高維概率分布密度函數(shù)中采樣，進(jìn)而提出了基于PMCMC-RFS(Particle MCMC based RFS) 的SLAM問(wèn)題求解方法。最后，通過(guò)無(wú)人自主車現(xiàn)場(chǎng)試驗(yàn)對(duì)相應(yīng)的算法性能進(jìn)行了分析驗(yàn)證。

1 基于RFS的SLAM模型

(1)

測(cè)量信息yk的集合表示為

(2)

Mk=Mk-1∩C(x0:k-1),

(3)

式中：C(x0:k-1)=C(x0)∪C(x1)∪…∪C(xk-1)。因此，Mk-1就表示k-1時(shí)刻之前每個(gè)時(shí)刻觀測(cè)特征空間的并集，則遍歷過(guò)程地圖或特征更新的方程為

(4)

(5)

式中：fM(w|Mk-1)為k-1到k時(shí)刻檢測(cè)范圍內(nèi)特征集合的轉(zhuǎn)移密度；fB(Mk-w|xk)為k時(shí)刻進(jìn)入檢測(cè)范圍內(nèi)新特征Bk(xk)的密度。

同時(shí),可以采用標(biāo)準(zhǔn)馬爾科夫模型對(duì)自主車動(dòng)力學(xué)系統(tǒng)進(jìn)行建模，轉(zhuǎn)移密度表示為fx(xk|xk-1,uk)，uk為控制輸入。則k時(shí)刻自主車位姿和地圖的聯(lián)合轉(zhuǎn)移密度為

fk|k-1(Mk,xk|Mk-1,xk-1,uk)=

fM(Mk|Mk-1,xk)fx(xk|xk-1,uk)，

(6)

k時(shí)刻的量測(cè)信息描述為

(7)

式中：Mk(m,xk)為特征m產(chǎn)生的測(cè)量RFS;Sk(xk)為k時(shí)刻的誤檢，與當(dāng)前自主車的位姿xk有關(guān)。

yk由ny個(gè)隨機(jī)測(cè)量組成，ny也是隨機(jī)的，其出現(xiàn)的次序相對(duì)估計(jì)的特征地圖沒(méi)有任何物理意義。特征m產(chǎn)生的測(cè)量RFS可以采用Bernoulli分布對(duì)RFS進(jìn)行模型化:

g(yk,xk)=

(8)

對(duì)于給定自主車位姿xk，pM(m,xk)為檢測(cè)到特征m的概率;一旦檢測(cè)到特征，gk(yk|xk,Mk)就是在m處產(chǎn)生測(cè)量yk相對(duì)應(yīng)的似然。根據(jù)隨機(jī)集合統(tǒng)計(jì)理論，測(cè)量似然

(9)

式中:fM(w|Mk,xk)為觀測(cè)有限集的密度;fS(yk-w|xk)為誤檢RFS的密度。

由此可見(jiàn):似然中直接包含了固有的測(cè)量不確定性，fM(w|Mk,xk)也綜合了檢測(cè)不確定量和測(cè)量噪聲。

令地圖Mk和車輛軌跡x1:k的聯(lián)合后驗(yàn)密度為pk(Mk,x1:k|y1:k,u1:k,x0)，靜態(tài)地圖的遞歸更新為

pk|k-1(Mk,x1:k)=fx(xk|xk-1,uk)×

∫fM(Mk|Mk-1,xk)×

pk-1(Mk-1,x1:k-1)δMk-1,

(10)

(11)

式中:gk(yk|y0:k-1,x0)=?gk(yk|Mk,xk)pk|k-1(Mk,xk|y1:k-1,u1:k,x0)δxkδMk，δ表示集合積分。

聯(lián)合后驗(yàn)密度涵蓋了所有地圖和自主車位姿的統(tǒng)計(jì)信息，可以根據(jù)截止到k時(shí)刻的測(cè)量和控制歷史進(jìn)行推理。RFS-SLAM算法將數(shù)據(jù)關(guān)聯(lián)和特征數(shù)量的不確定性集成到一個(gè)單一的貝葉斯濾波器中，不需要獨(dú)立的數(shù)據(jù)關(guān)聯(lián)操作和相應(yīng)的特征管理。RFS-SLAM的聯(lián)合后驗(yàn)估計(jì)等價(jià)于軌跡的后驗(yàn)密度估計(jì)和基于軌跡的地圖后驗(yàn)估計(jì)，即

pk|k-1(Mk|x0:k)= ∫fM(Mk|Mk-1,xk)×

pk-1(Mk-1,x1:k-1)δMk-1,

(12)

(13)

pk(x1:k)=

(14)

除了采用測(cè)量和地圖的隨機(jī)有限集似然外，由式(12)-(14)確定的RFS-SLAM遞歸貝葉斯估計(jì)模型算法與EKF-FastSLAM[4]算法類似。但EKF-FastSLAM算法將式(14)進(jìn)一步分解成np個(gè)獨(dú)立的密度函數(shù)，其中np為地圖中特征的個(gè)數(shù)，首先完成數(shù)據(jù)相關(guān)和特征數(shù)量估計(jì)，然后采用獨(dú)立的估計(jì)器對(duì)地圖進(jìn)行傳遞和更新。相比較而言，RFS-SLAM采用RFS的形式描述，地圖信息傳遞采用RFS表示，不依賴數(shù)據(jù)關(guān)聯(lián)和特征數(shù)量估計(jì)的結(jié)果。

2 基于粒子濾波的RFS-SLAM算法

2.1 RFS-SLAM算法描述

式 (12)給出的RFS-SLAM后驗(yàn)密度計(jì)算涉及到多個(gè)集合積分的計(jì)算問(wèn)題，相對(duì)單一狀態(tài)估計(jì)而言，計(jì)算復(fù)雜度非常高。SMC采用加權(quán)的隨機(jī)采樣近似后驗(yàn)分布[5-6]，能夠?qū)崿F(xiàn)后驗(yàn)密度的遞歸估計(jì)。RFS的置信度在某種意義上就是RFS的概率密度[7]，其采樣和近似方法與概率密度函數(shù)基本相同。單路標(biāo)特征粒子濾波算法可以直接應(yīng)用于多路標(biāo)特征情況，只是多路標(biāo)特征的粒子是有限集合，本身維數(shù)也可能是變化的。

(15)

1) 預(yù)測(cè)

(1) 采樣

(16)

式中：Jk為新產(chǎn)生的粒子數(shù)，可以選擇狀態(tài)轉(zhuǎn)移密度函數(shù)fk|k-1(ζk|ζk-1,uk,yk)作為重要性采樣密度函數(shù)；pk(·|yk)表示新特征在傳感器感知范圍內(nèi)出現(xiàn)的概率密度函數(shù)，根據(jù)自主車位姿狀態(tài)轉(zhuǎn)移和傳感器測(cè)量特征，采用混合高斯模型給出先驗(yàn)信息。

(2) 計(jì)算預(yù)測(cè)粒子權(quán)重

(17)

2) 更新

(18)

(2) 對(duì)i=1,…,Nk-1+Jk更新權(quán)重

(19)

3) 重采樣

4) 狀態(tài)估計(jì)

2.2 基于PMCMC的RFS-SLAM問(wèn)題求解

當(dāng)特征測(cè)量集合或目標(biāo)狀態(tài)數(shù)較大時(shí)， RFS遞歸貝葉斯后驗(yàn)公式中的似然很難計(jì)算，從而限制了MCMC算法對(duì)后驗(yàn)分布的直接近似。為此，通過(guò)引入中間變量的方法采用給出的似然描述方式進(jìn)行計(jì)算，即

(20)

(21)

式中:κk為k時(shí)刻的噪聲干擾強(qiáng)度；u,v=∫u(t)v(t)dt。

ω(θk) =e。

(22)

(23)

(24)

(25)

式中：Np為采樣粒子數(shù)。

3 算法驗(yàn)證

首先選用如圖1所示的仿真測(cè)試環(huán)境，其中車輛的參考軌跡利用自主車DGPS-RTK模式測(cè)量得到，行駛距離、航向角變化等數(shù)據(jù)根據(jù)實(shí)車測(cè)量得到并加注時(shí)間標(biāo)簽，以便在仿真試驗(yàn)時(shí)方便對(duì)準(zhǔn)時(shí)間。路標(biāo)點(diǎn)則采用人工設(shè)置，并假定傳感器的探測(cè)距離為15 m，感知角度為360°。FastSLAM算法采用獨(dú)立的特征管理操作處理數(shù)據(jù)關(guān)聯(lián)問(wèn)題，RFS-SLAM算法則將特征管理、數(shù)據(jù)關(guān)聯(lián)和狀態(tài)估計(jì)集成在一個(gè)統(tǒng)一的貝葉斯更新框架內(nèi)。

圖1 測(cè)試環(huán)境地圖和車輛軌跡

采用EKF-FastSLAM和PMCMC-RFS-SLAM算法估計(jì)的東向和北向誤差分別如圖2、3所示，其中：EKF-FastSLAM算法采用最近鄰算法處理相關(guān)問(wèn)題，且根據(jù)傳感器的測(cè)量范圍，僅考慮檢測(cè)范圍內(nèi)的路標(biāo)，進(jìn)而實(shí)現(xiàn)路標(biāo)的動(dòng)態(tài)管理。從圖2、3中可以看出：PMCMC-RFS-SLAM算法相對(duì)EKF-FastSLAM算法對(duì)車輛位姿的估計(jì)性能平均提高了約1.5倍，且定位不存在跳變現(xiàn)象，相對(duì)比較平滑。其主要原因是PMCMC-RFS-SLAM算法不依賴特征相關(guān)結(jié)果，對(duì)特征的數(shù)量也不需要進(jìn)行預(yù)定假設(shè)。

圖2 東向定位誤差

圖3 北向定位誤差

圖4給出了當(dāng)測(cè)量噪聲增加時(shí)的濾波性能比較，可以看出：隨著噪聲水平的增加，PMCMC-RFS-SLAM算法性能基本保持穩(wěn)定，而EKF-FastSLAM算法明顯變差。

圖4 當(dāng)測(cè)量噪聲增加時(shí)的濾波性能比較

通過(guò)分析圖2-4給出的試驗(yàn)結(jié)果可知：與經(jīng)典EKF-FastSLAM算法相比，在具有數(shù)據(jù)關(guān)聯(lián)不確定和特征誤檢、漏檢的情況下，RFS-SLAM算法能夠準(zhǔn)確地估計(jì)地圖中的特征數(shù)量以及相應(yīng)的位置。EKF-FastSLAM算法利用隨機(jī)向量對(duì)自主車的狀態(tài)轉(zhuǎn)移和傳感器的觀測(cè)信息建模，只能描述觀測(cè)信息的不確定性，而忽略了傳感器漏檢、噪聲干擾誤檢以及特征數(shù)量不確定等問(wèn)題;但這些問(wèn)題又會(huì)對(duì)SLAM的過(guò)程估計(jì)產(chǎn)生很大的影響，尤其是影響數(shù)據(jù)關(guān)聯(lián)的結(jié)果，從而導(dǎo)致定位和地圖估計(jì)精度下降，甚至產(chǎn)生狀態(tài)估計(jì)跳變。而對(duì)于RFS-SLAM算法，由于其采用RFS描述系統(tǒng)狀態(tài)、環(huán)境地圖和觀測(cè)信息，在建模的過(guò)程中就已經(jīng)包含了上述特征，能夠更為真實(shí)地反映多特征、多觀測(cè)的特點(diǎn)，同時(shí)避免了復(fù)雜的數(shù)據(jù)關(guān)聯(lián)問(wèn)題。

為了進(jìn)一步驗(yàn)證算法的有效性，選擇無(wú)人自主車平臺(tái)為測(cè)試對(duì)象，采用64線三維激光雷達(dá)和慣性導(dǎo)航系統(tǒng)為感知器，以構(gòu)建SLAM系統(tǒng)。鑒于RFS-SLAM算法通過(guò)PMCMC粒子濾波算法實(shí)現(xiàn)，因此選擇經(jīng)典的EKF-FastSLAM算法進(jìn)行比較分析，EKF-FastSLAM算法選擇最大似然數(shù)據(jù)關(guān)聯(lián)。理論上，環(huán)境中所有特征都可以用來(lái)修正車輛的位姿估計(jì)，為了提高系統(tǒng)的可靠性和降低算法的計(jì)算復(fù)雜度，仍然選擇樹(shù)干和路燈桿作為路標(biāo)特征。

PMCMC-RFS算法采用RFS構(gòu)建的SLAM模型將多路標(biāo)檢測(cè)、跟蹤及數(shù)據(jù)關(guān)聯(lián)等問(wèn)題集成到一個(gè)統(tǒng)一的貝葉斯框架內(nèi)，根據(jù)計(jì)算的概率強(qiáng)度，圖5、6分別給出了采用RFS計(jì)算得到的位置、路標(biāo)估計(jì)和路標(biāo)高斯混合后驗(yàn)強(qiáng)度。求解概率強(qiáng)度的局部極大值即可得到路標(biāo)的位置，極大值的數(shù)量就是在傳感器感知范圍內(nèi)的路標(biāo)數(shù)量，如圖6所示，可見(jiàn)：采用PMCMC-RFS算法解決了路標(biāo)特征密集、動(dòng)態(tài)特征多的復(fù)雜城鄉(xiāng)環(huán)境中SLAM 求解魯棒性差的問(wèn)題，同時(shí)有效地解決了高維粒子濾波算法的提議分布選擇和高維積分計(jì)算困難等問(wèn)題。

圖5 采用RFS計(jì)算得到的位置和路標(biāo)估計(jì)

圖6 采用RFS計(jì)算得到的路標(biāo)高斯混合后驗(yàn)強(qiáng)度

4 結(jié)論

本文采用RFS理論構(gòu)建了RFS-SLAM模型，并給出了求解集合上的高維積分的PMCMC計(jì)算方法。所構(gòu)建的PMCMC-RFS-SLAM 算法與經(jīng)典的EKF-FastSLAM算法相比，估計(jì)精度提高了大約50%，且解決了經(jīng)典SLAM 算法對(duì)數(shù)據(jù)關(guān)聯(lián)結(jié)果的嚴(yán)重依賴和地圖動(dòng)態(tài)管理問(wèn)題，同時(shí)還能夠估計(jì)視野范圍內(nèi)的特征數(shù)量，為準(zhǔn)確的位姿估計(jì)提供了有效的途徑，提高了復(fù)雜多路標(biāo)特征環(huán)境SLAM算法的魯棒性。

[1] Smith R C, Self M,Cheeseman P. A Stochastic Map for Uncertain Spatial Relationships[EB/OL].(2012-05-30)[2014-10-08].http://wenku.baidu.com/view/8ae34d87e53a580217fcfe00.html

[2] Dissanayake M G, Newman P, Clark S, et al. A Solution to the Simultaneous Localization and Map Building (SLAM) Problem[J]. IEEE Transactions on Robotics and Automation,2001,17(3):229-241.

[3] Martinez-Cantin R, de Freitas N, Castellanos J A. Analysis of Particle Methods for Simultaneous Robot Localization and Mapping and a New Algorithm: Marginal-SLAM[C]∥Proceedings of 2007 IEEE International Conference on Robotics and Automation.Roma,Italy:IEEE, 2007: 2415-2420.

[4] Martinez-Cantin R, Castellanos J A. Unscented SLAM for Large-scale Outdoor Environments[C]∥Proceedings of 2005 IEEE International Conference on Intelligent Robots and Systems.Roma,Italy:IEEE,2005:3427-3432.

[5] Havangi R, Nekoui M A, Teshnehlab M, et al. A SLAM Based on Auxiliary Marginalised Particle Filter and Differential Evolution[J]. International Journal of Systems Science, 2014, 45(9): 1913-1926.

[6] Nemeth C, Fearnhead P, Mihaylova L. Sequential Monte Carlo Methods for State and Parameter Estimation in Abruptly Changing Environments[J]. IEEE Transactions on Signal Processing, 2014, 62(5): 1245-1255.

[7] Yoon J H, Kim D Y, Yoon K J. Efficient Importance Sampling Function Design for Sequential Monte Carlo PHD Filter[J]. Signal Processing, 2012, 92(9): 2315-2321.

[8] Mullane J, Vo B N, Adams M D, et al. A Random-Finite-Set Approach to Bayesian SLAM[J]. IEEE Transactions on Robotics,2011, 27(2): 268-282.

[9] Du H Y,Zhao Y X,Yang Y P, et al. SLAM Algorithm Based on Random Finite Set[J]. Systems Engineering and Electronics, 2012, 7:29037.

[10] Vo B N, Singh S, Doucet A. Sequential Monte Carlo Methods for Multitarget Filtering with Random Finite Sets[J]. IEEE Transactions on Aerospace and Electronic Systems, 2005, 41(4):1224-1245.

(責(zé)任編輯:尚彩娟)

Autonomous Vehicle SLAM Algorithm Based on PMCMC-RFS

SU Kui-feng, CHANG Tian-qing, ZHANG Lei, YANG Guo-zhen

(Department of Control Engineering, Academy of Armored Force Engineering, Beijing 100072, China)

Aiming at the problem of Simultaneous Localization And mapping (SLAM) for autonomous vehicles, the environment feature as well as vehicle position and attitude are described by using Random Finite Set (RFS), which integrates the aspects such as multi-feature detection, tracking, recognition and correlation related to SLAM algorithm into a single unified Bayesian framework so that the thorny problem that the algorithm of posterior estimation and information fusion relies on the result of association greatly can be effectively solved. Meanwhile, to calculate the complex joint posterior estimation distribution and resolve the proposal distribution selection difficulties in particle filtering algorithm, SLAM resolving method based on Particle Markov Chain Monte Carlo (PMCMC) is proposed using Sequential Monte Carlo (SMC) and Markov Chain Monte Carlo (MCMC). The autonomous ground vehicle experimental results shows that the proposed method can estimate in dynamic manner the feature numbers of perspective scope and has better performance in states estimation without consideration of data association.

Simultaneous Localization And Mapping (SLAM); Random Finite Set (RFS); Markov Chain Monte Carlo (MCMC); particle filter

1672-1497(2015)02-0070-06

2014-12-08

軍隊(duì)科研計(jì)劃項(xiàng)目

蘇奎峰(1976-)，男，講師，博士。

TP24

A

10.3969/j.issn.1672-1497.2015.02.014