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

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

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

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

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

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

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

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

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

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

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

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時刻檢測范圍內特征集合的轉移密度；fB(Mk-w|xk)為k時刻進入檢測范圍內新特征Bk(xk)的密度。

同時,可以采用標準馬爾科夫模型對自主車動力學系統進行建模，轉移密度表示為fx(xk|xk-1,uk)，uk為控制輸入。則k時刻自主車位姿和地圖的聯合轉移密度為

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產生的測量RFS;Sk(xk)為k時刻的誤檢，與當前自主車的位姿xk有關。

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

g(yk,xk)=

(8)

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

(9)

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

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

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

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，δ表示集合積分。

聯合后驗密度涵蓋了所有地圖和自主車位姿的統計信息，可以根據截止到k時刻的測量和控制歷史進行推理。RFS-SLAM算法將數據關聯和特征數量的不確定性集成到一個單一的貝葉斯濾波器中，不需要獨立的數據關聯操作和相應的特征管理。RFS-SLAM的聯合后驗估計等價于軌跡的后驗密度估計和基于軌跡的地圖后驗估計，即

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個獨立的密度函數，其中np為地圖中特征的個數，首先完成數據相關和特征數量估計，然后采用獨立的估計器對地圖進行傳遞和更新。相比較而言，RFS-SLAM采用RFS的形式描述，地圖信息傳遞采用RFS表示，不依賴數據關聯和特征數量估計的結果。

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

2.1 RFS-SLAM算法描述

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

(15)

1) 預測

(1) 采樣

(16)

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

(2) 計算預測粒子權重

(17)

2) 更新

(18)

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

(19)

3) 重采樣

4) 狀態估計

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

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

(20)

(21)

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

ω(θk) =e。

(22)

(23)

(24)

(25)

式中：Np為采樣粒子數。

3 算法驗證

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

圖1 測試環境地圖和車輛軌跡

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

圖2 東向定位誤差

圖3 北向定位誤差

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

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

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

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

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

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

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

4 結論

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