蘇奎峰， 常天慶， 張 雷， 楊國振

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

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

蘇奎峰， 常天慶， 張 雷， 楊國振

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

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

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

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

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

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

1 基于RFS的SLAM模型

(1)

測量信息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時刻之前每個時刻觀測特征空間的并集，則遍歷過程地圖或特征更新的方程為

(4)

(5)

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

同時,可以采用標準馬爾科夫模型對自主車動力學系統(tǒng)進行建模，轉(zhuǎn)移密度表示為fx(xk|xk-1,uk)，uk為控制輸入。則k時刻自主車位姿和地圖的聯(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時刻的量測信息描述為

(7)

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

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

g(yk,xk)=

(8)

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

(9)

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

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

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

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)

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

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

2.1 RFS-SLAM算法描述

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

(15)

1) 預測

(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)移和傳感器測量特征，采用混合高斯模型給出先驗信息。

(2) 計算預測粒子權(quán)重

(17)

2) 更新

(18)

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

(19)

3) 重采樣

4) 狀態(tài)估計

2.2 基于PMCMC的RFS-SLAM問題求解

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

(20)

(21)

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

ω(θk) =e。

(22)

(23)

(24)

(25)

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

3 算法驗證

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

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

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

圖2 東向定位誤差

圖3 北向定位誤差

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

圖4 當測量噪聲增加時的濾波性能比較

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

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

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

圖5 采用RFS計算得到的位置和路標估計

圖6 采用RFS計算得到的路標高斯混合后驗強度

4 結(jié)論

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

(責任編輯:尚彩娟)

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

軍隊科研計劃項目

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

TP24

A

10.3969/j.issn.1672-1497.2015.02.014