迭代中心差分粒子濾波的SLAM算法

錢臻，齊英杰

(1.東北林業(yè)大學(xué)交通學(xué)院，黑龍江 哈爾濱150040;2.哈爾濱市職工大學(xué)，黑龍江 哈爾濱150020)

智能車的同時(shí)定位與地圖創(chuàng)建(simultaneous localization and mapping，SLAM)問(wèn)題[1]主要解決智能車輛在沒(méi)有先驗(yàn)地圖的未知環(huán)境中的環(huán)境探索問(wèn)題，不僅要根據(jù)帶有噪聲的傳感器觀測(cè)建立工作環(huán)境的地圖，同時(shí)要根據(jù)部分已建地圖估計(jì)智能車輛在環(huán)境中的位姿.基于EKF的SLAM方法其復(fù)雜度是環(huán)境特征個(gè)數(shù)的平方級(jí)，對(duì)錯(cuò)誤數(shù)據(jù)的關(guān)聯(lián)非常敏感，難以應(yīng)用在智能車輛的自主導(dǎo)航問(wèn)題中.針對(duì)這些不足，有人提出了基于粒子濾波器(PF)[2-3]的SLAM 方法(如 FastSLAM1.0，F(xiàn)astSLAM2.0 等)和基于無(wú)跡卡爾曼濾波(UKF)[4-5]的 SLAM 算法.UKF是一種通過(guò)選擇一組確定性采樣點(diǎn)(Sigma點(diǎn))的采樣策略逼近非線性分布的方法，其計(jì)算復(fù)雜度與EKF算法相同，但性能優(yōu)于EKF，具有計(jì)算量小、不需要求導(dǎo)計(jì)算雅克比矩陣、能夠避免采樣粒子衰退等問(wèn)題的優(yōu)點(diǎn).但是，UKF濾波的精度容易受到系統(tǒng)噪聲的影響，缺乏自適應(yīng)能力，容易產(chǎn)生采樣的非局部效應(yīng).因此，一些學(xué)者提出了很多改進(jìn)方法，以期提高UKF的估算精度，如將UKF與粒子濾波器相結(jié)合的 Unscented FastSLAM 算法[6-7]、將 UKF與RBPF相結(jié)合的URBPF算法[8-9]、調(diào)節(jié)UKF參數(shù)的改進(jìn)算法[10]等.

和UKF一樣，中心差分卡爾曼濾波(CDKF)也是一種通過(guò)選擇采樣點(diǎn)(Sigma點(diǎn))的采樣策略逼近非線性分布的方法[11].但其具有比UKF較高的理論精度，而且更加容易實(shí)現(xiàn).祝繼華等[12]提出了基于CDKF的SLAM方法，在利用中心差分濾波方法產(chǎn)生改進(jìn)的提議分布函數(shù)基礎(chǔ)上，利用中心差分濾波方法初始化環(huán)境特征和更新地圖中的特征，提高移動(dòng)機(jī)器人的同時(shí)定位與地圖創(chuàng)建的精度.田翔等[13]使用CDKF處理預(yù)測(cè)方程和測(cè)量方程，得到當(dāng)前時(shí)刻狀態(tài)向量相對(duì)于各個(gè)路標(biāo)的權(quán)重，利用此權(quán)重進(jìn)行狀態(tài)向量和狀態(tài)向量協(xié)方差的動(dòng)態(tài)調(diào)整，從而提高了SLAM的速度.

為了進(jìn)一步提高SLAM算法中的位姿估計(jì)精度，本文將迭代濾波理論引入到中心差分卡爾曼濾波器中，重復(fù)利用觀測(cè)信息，得到更高的位姿估計(jì)精度，提高迭代的穩(wěn)定性.該方法利用迭代中心差分濾波方法融合新的觀測(cè)數(shù)據(jù)使提議分布更加接近后驗(yàn)概率分布，從而提高了智能車輛定位與建圖的精度.

1 FastSLAM算法

當(dāng)智能車輛穿過(guò)一個(gè)未知環(huán)境時(shí)，設(shè)t時(shí)刻車輛位姿 s=[xt，yt，θt]T，已經(jīng)觀測(cè)到地圖為 M，其中mk表示第k個(gè)路標(biāo)，N表示已經(jīng)觀測(cè)到的路標(biāo)數(shù)，kt∈{1，2，…，N}表示 t時(shí)刻感知到的路標(biāo)索引號(hào).系統(tǒng)的完整狀態(tài)可以表示為 x=[s1∶t，M]T，其中s1∶t=s1，s2，…，st表示智能車輛從 1 到 t時(shí)刻的運(yùn)動(dòng)路徑，同樣，ut-1表示t-1到t時(shí)刻的運(yùn)動(dòng)控制信息，zt表示智能車輛的當(dāng)前感知信息.車輛從位姿s0開(kāi)始通過(guò)控制命令序列u0，u1，…，ut-1移動(dòng)，隨著車輛的移動(dòng)，附近的路標(biāo)被接收到，時(shí)刻t=1，接收到路標(biāo)m1，并獲得車輛位姿的測(cè)量數(shù)據(jù)z1(包括位置和方向)，時(shí)刻t=2，接收到路標(biāo)m2，并在時(shí)刻t=3，接收到路標(biāo)m1，現(xiàn)在已經(jīng)形成的地圖為M={m1，m2，…，mn}.

智能車輛的SLAM的概率描述形式可以表示為

式中:η是標(biāo)準(zhǔn)化常數(shù).上式根據(jù)給定的分布p(zt|st，mkt，kt)和 p(st|st-1，ut-1)可以遞歸的估計(jì)地圖和路徑的后驗(yàn)概率分布，分別表示車輛的感知模型和運(yùn)動(dòng)模型:

式(1)給出了SLAM問(wèn)題的迭代形式，是SLAM問(wèn)題的核心.要實(shí)現(xiàn)智能車輛的SLAM，也就是要對(duì)該式進(jìn)行求解.

粒子濾波器(PF)用n個(gè)加權(quán)的粒子近似Bel(xt)={x[m]t，w[m]t}m=1，…，n，但由于系統(tǒng)狀態(tài)包括的地圖中通常有大量的路標(biāo)，不能采用PF直接進(jìn)行求解.因此，首先需要對(duì)SLAM問(wèn)題進(jìn)行分解.由于路標(biāo)之間的相關(guān)性是由智能車輛的位姿不確定而引起的，如果智能車輛的位置完全確定，那么路標(biāo)之間是不相關(guān)的，因此，RBPF將式(1)分解為

式中:K表示地圖中的路標(biāo)數(shù)，即智能車輛的SLAM問(wèn)題可以分解為K+1個(gè)估計(jì)問(wèn)題，其中一個(gè)是估計(jì)車輛的路徑s1∶t，其他K個(gè)估計(jì)是對(duì)環(huán)境中的路標(biāo)位置進(jìn)行估計(jì).

2 迭代中心差分卡爾曼濾波算法

CDKF采用中心差分代替Talor展開(kāi)中的一階和二階導(dǎo)數(shù)，對(duì)于L維的狀態(tài)向量，CDKF的Sigma點(diǎn)的個(gè)數(shù)為2L+1，Sigma點(diǎn)和權(quán)值為

式中:h為中心差分半步長(zhǎng)度，它決定了Sigma點(diǎn)圍繞均值的分布，其最佳取值應(yīng)與上述隨機(jī)變量峰值的均方根相對(duì)應(yīng)，對(duì)于高斯分布，h的最佳取值為在觀測(cè)更新后，利用狀態(tài)濾波值代替預(yù)測(cè)值重新對(duì)原來(lái)的非線性方程線性化，來(lái)提高濾波器的精度.這一過(guò)程進(jìn)行多次，就構(gòu)成了迭代中心差分卡爾曼濾波(ICDKF)，進(jìn)一步利用Levenberg-Marquardt(LM)方法調(diào)整預(yù)測(cè)協(xié)方差矩陣，以保證算法具有全局收斂性.該方法的核心是在每次迭代過(guò)程中，使用參數(shù)μi對(duì)預(yù)測(cè)協(xié)方差矩陣進(jìn)行修正，即調(diào)整協(xié)方差矩陣為

ICDKF算法如下:

第1步初始化:

第2 步 For t=1，2，…，∞;

1)計(jì)算時(shí)間更新階段協(xié)方差矢量:

2)時(shí)間更新方程:

3)測(cè)量值更新方程:

①迭代

式中:Lx、Lv、Ln分別是狀態(tài)、過(guò)程噪聲和觀測(cè)噪聲的維數(shù)，Rv和Rn分別是過(guò)程噪聲和觀測(cè)噪聲的協(xié)方差值，(·)2表示矢量外積的縮寫，如)i表示對(duì)稱方陣P的第i列的平方根.表示向量范數(shù)，th為設(shè)定的終止迭代的閾值，LM參數(shù) μj設(shè)定為 0.1.Pv、Pn分別為系統(tǒng)過(guò)程噪聲方差和觀測(cè)噪聲方差;為更新后得到的t時(shí)刻的狀態(tài)量和觀測(cè)量.

3 迭代中心差分卡爾曼粒子濾波快速SLAM算法(ICDPFFastSLAM)

對(duì)于解決智能車輛的FastSLAM而言，采用ICDKF產(chǎn)生提議分布，不僅更符合SLAM問(wèn)題中的狀態(tài)變量的實(shí)際后驗(yàn)概率分布，而且可以較好解決粒子耗散問(wèn)題和粒子早熟問(wèn)題.

3.1 改進(jìn)的提議分布函數(shù)

第m個(gè)粒子(t-1)時(shí)刻的智能車狀態(tài)估計(jì)值為(xa[m]t-1，Pa[m]t-1)，則在t時(shí)刻系統(tǒng)的狀態(tài)隨機(jī)變量及對(duì)應(yīng)的協(xié)方差可擴(kuò)展為

將變換后的點(diǎn)集利用式(10)、(11)可以獲得車體狀態(tài)的一步預(yù)測(cè)值和相應(yīng)的方差:

利用傳感器所獲取的環(huán)境觀測(cè)信息對(duì)車體狀態(tài)進(jìn)行更新，獲取預(yù)測(cè)值z(mì)^[i][m]t，t時(shí)刻第m個(gè)粒子的狀態(tài)迭代值可以由式(14)給出.

通過(guò)上述計(jì)算可以獲得改進(jìn)的提議分布函數(shù)，并從提議分布函數(shù)中抽取新的粒子:

通過(guò)這種方式計(jì)算FastSLAM的提議分布函數(shù)，可以避免傳統(tǒng)算法中復(fù)雜的Jacobian矩陣計(jì)算，減少了非線性系統(tǒng)的泰勒展開(kāi)誤差，獲得了較好的提議分布函數(shù).

3.2 地圖特征路標(biāo)更新

特征更新過(guò)程取決于t時(shí)刻這個(gè)特征是否被車輛看到.如果一個(gè)特征沒(méi)有被車輛看到，那么它的位置均值和方差保持不變.如果在t時(shí)刻某個(gè)特征}被車輛觀測(cè)到，那么

在進(jìn)行路標(biāo)更新時(shí)，即實(shí)現(xiàn)對(duì)第i個(gè)粒子第kt個(gè)路標(biāo)的后驗(yàn)估計(jì)，更新后的值}與新的車輛采樣位姿一起加入到臨時(shí)粒子集中，路標(biāo)kt更新取決于它在時(shí)刻t是否被感知到，即t時(shí)刻獲得的GPS數(shù)據(jù)是否與地圖庫(kù)中該路標(biāo)kt相關(guān)的數(shù)據(jù)匹配成功，如果匹配成功，則應(yīng)用上述算法進(jìn)行更新，否則沒(méi)有被感知，則位姿保持不變.

3.3 權(quán)重更新

從提議分布抽樣的粒子可能并不很好的近似后驗(yàn)分布，它們之間的差別需要通過(guò)一種評(píng)價(jià)來(lái)衡量，因此權(quán)重系數(shù)是目標(biāo)分布與提議分布的比值.其中的目標(biāo)分布就是抽樣的粒子要近似的車輛運(yùn)行路徑后驗(yàn)分布，假設(shè)前一時(shí)刻路徑s[m]1∶t-1按照后驗(yàn)分布p(s[m]1∶t-1|z1∶t-1，u1∶t-2，n1∶t-1)產(chǎn)生，則粒子的權(quán)重可以通過(guò)對(duì)非線性感知函數(shù)h在感知zt的線性近似獲得，而本文獲得均值和方差，則第 i個(gè)粒子的權(quán)重近似為

3.4 自適應(yīng)重采樣

重新采樣可以對(duì)粒子濾波器的性能產(chǎn)生很大影響:不僅低權(quán)重粒子被高權(quán)重粒子所代替，同時(shí)只允許有限必要的粒子近似后驗(yàn).因此當(dāng)提議分布與后驗(yàn)分布相差較大時(shí)，重新采樣非常重要，但是重新采樣時(shí)也可能會(huì)忽略粒子集中某些權(quán)重較高的粒子，最壞時(shí)能導(dǎo)致濾波器發(fā)散.為此定義一個(gè)有效值Neff，根據(jù)該有效值自適應(yīng)重新采樣:

表示當(dāng)前粒子集近似后驗(yàn)的好壞，以決定是否進(jìn)行重新采樣:如果Neff≤n/2(n是粒子總數(shù))，就進(jìn)行重新采樣，否則不進(jìn)行.

4 迭代中心差分粒子濾波SLAM算法的仿真實(shí)驗(yàn)

4.1 實(shí)驗(yàn)?zāi)Ｐ?/h3>

智能車輛的運(yùn)動(dòng)模型采用Ackerman模型:

式中:xv(k)為k時(shí)刻機(jī)器人的位姿，包括[xvx(k)xvy(k)xvφ(k)]T，Δt為傳感器采樣時(shí)間，v(k)為智能車輛的行駛速度，α(k)為車輛在k時(shí)刻的方向角，B為車輛兩輪軸間的間距.xv(k+1)為k+1時(shí)刻車輛的新位姿.

智能車輛中配置的距離傳感器和方向傳感器的觀測(cè)模型如下式所示:

式中:(xL，yL)為智能車輛所攜帶的傳感器探測(cè)到的第L個(gè)特征的位置坐標(biāo)，xv(k)為車輛位姿，zr、zρ分別為傳感器感知到的路標(biāo)特征距離車輛的距離和與機(jī)器人前進(jìn)方向的夾角φ.

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

表1是系統(tǒng)噪聲和觀測(cè)噪聲皆為高斯白噪聲條件下的4種算法的實(shí)驗(yàn)結(jié)果對(duì)比.從復(fù)雜度上來(lái)說(shuō)，EKFSLAM采用一階線性化，其復(fù)雜度為O(n2)，其他3種算法都采用了粒子濾波，復(fù)雜度相對(duì)降低，皆為 O(Nn).從運(yùn)行時(shí)間上來(lái)說(shuō)，F(xiàn)astSLAM2.0 、UnscentedSLAM由于要產(chǎn)生大量的隨機(jī)粒子，并且算法在特征更新和重取樣階段都要采用Cholesky分解來(lái)完成KF更新，需要耗費(fèi)更多的時(shí)間.盡管ICDPFFastSLAM算法也采用了粒子濾波，但在重取樣和特征更新階段采用采用中心差分代替Talor展開(kāi)中的一階和二階導(dǎo)數(shù)，構(gòu)造Sigma點(diǎn)，因此不僅有效提高了路徑估計(jì)和地圖創(chuàng)建的精度，而且減少了運(yùn)行時(shí)間.

圖1 高斯噪聲下不同算法X軸誤差比較Fig.1 X errors of some algorithms under Gauss noises

圖2 高斯噪聲下不同算法Y軸誤差比較Fig.2 Y errors of some algorithms under Gauss noises

圖1和圖2是系統(tǒng)噪聲與觀測(cè)噪聲分都為高斯白噪聲條件下，4種算法在進(jìn)行路徑估計(jì)時(shí)X、Y和φ方向上算法估計(jì)誤差的對(duì)比.橫坐標(biāo)t表示實(shí)驗(yàn)走過(guò)的步長(zhǎng)，每個(gè)實(shí)驗(yàn)都經(jīng)過(guò)了17 383步.從圖中可以看出，F(xiàn)astslam2.0的估計(jì)精度要低于其他3種SLAM算法，本文所提出的ICDPFFastSLAM算法采用了迭代測(cè)量更新，使其估計(jì)精度明顯高于其他3種算法，其誤差范圍分別在(-0.5 m，0.5 m)、(-0.5 m，0.5 m)和(-0.5°，0.5°)內(nèi)，遠(yuǎn)遠(yuǎn)低于其他3 種算法.

表1 高斯噪聲環(huán)境下實(shí)驗(yàn)數(shù)據(jù)統(tǒng)計(jì)對(duì)比Table.1 Experiment statistics in an environment with Gauss noise

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

本文提出了一種基于迭代測(cè)量更新的中心差分粒子濾波器算法，來(lái)代替其中的擴(kuò)展卡爾曼濾波器(EKF)并迭代融合新的觀測(cè)數(shù)據(jù)使提議分布更加接近后驗(yàn)概率分布，并且能夠精確估計(jì)智能車輛的位姿，進(jìn)而更新特征地圖的位置.該算法在保證精度的同時(shí)減少了計(jì)算的復(fù)雜度，提高系統(tǒng)的估計(jì)性能，增加了迭代算法的穩(wěn)定性，仿真實(shí)驗(yàn)結(jié)果驗(yàn)證了方法的有效性.本文提出的方法為智能車輛在室外未知環(huán)境下的定位與地圖創(chuàng)建提供了一種新思路.

致 謝

實(shí)驗(yàn)采用了澳大利亞野外機(jī)器人研究所(ACFR)提供的部分Matlab源碼，特此致謝，同時(shí)感謝在本文寫作過(guò)程中哈爾濱工業(yè)大學(xué)計(jì)算學(xué)院蔡則蘇老師的支持.

[1]SMITH R，CHEESEMAN P.On the representation and estimation of spatial uncertainly[J].International Journal of Robotics Research，1987，5(4):56-68.

[2]MONTERMERLO M，THRUN S，KOLLER D.FastSLAM:a factored solution to the simultaneous localization and mapping problem[C]//Proc of the Eighteenth National Conference on Artificial Intelligence.Edmonton，2002:593-598.

[3]MONTERMERLO M，THRUN S，KOLLER D，et al.FastSLAM 2.0:an improved particle filtering algorithm for simultaneous localization and mapping that provably converges[C]//Proc of the Int Joint Conference on Artificial Intelligence.Acapulco，Mexico，2003:1151-1156.

[4]JULIER S，UHLMANN J，DURRANT-WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Transactions on Automatic Control，2000，45(3):477-482.

[5]LEE S S，LEE S H，KIM D S.Recursive unscented Kalman filtering based SLAM using a large number of noisy observations[J].International Journal of Control，Automation，and Systems，2006，4(6):736-747.

[6]HOLMES S A，KLEIN G，MURRAY D W.An O(N^2)square root unscented Kalman filter for visual simultaneous localization and mapping[J].IEEE Trans Pattern Anal Mach Intell，2009，31(7):1251-1263.

[7]ATSUSHI S，TEPPEI S，YOJI K.Robust landmark estimation for SLAM in dynamic outdoor environment[J].Journal of Robotics and Mechatronics，2010，22(2):140-145.

[8]LI M H，HONG B R，CAI ZS，et al.Novel Rao-blackwellized particle filter for mobile robot SLAM using monocular vision[J].International Journal of Information and Communication Engineering，2007，3(1):39-45.

[9]LI M H，HONG B R，CAI Z S，et al.Novel indoor mobile robot navigation using monocular vision[J].Engineering Applications of Artificial Intelligence，2008，21(3):485-497.

[10]SHOJAIE K，MOHAMMAD S.Iterated unscented SLAM algorithm for navigation of an autonomous mobile robot[C]//IEEE International Conference on Intelligent Robots and Systems.Nice，F(xiàn)rance，2008:22-26.

[11]VANDERMERWE R，WAN E.Sigma-point Kalman filters for probabilistic inference in dynamic state-space models[D].Oregon:Oregon Health＆ Science University，2004.

[12]祝繼華，鄭南寧，袁澤劍，等.基于中心差分粒子濾波的SLAM算法[J].自動(dòng)化學(xué)報(bào)，2010，36(2):249-257.ZHU Jihua，ZHENG Nanning，YUAN Zejian，et al.A SLAM algorithm based on central difference particle filter[J].Acta Automatica Sinica，2010，36(2):249-257.

[13]田翔，張亮，陳耀武.基于中心差分卡爾曼濾波器的快速SLAM算法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2010，42(9):1454-1461.TIAN Xiang，ZHANG Liang，CHEN Yaowu.Fast SLAM algorithm based on central difference Kalman filter[J].Journal of Harbin Institute of Technology，2010，42(9):1454-1461.