錢臻，齊英杰

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

智能車的同時定位與地圖創(chuàng)建(simultaneous localization and mapping，SLAM)問題[1]主要解決智能車輛在沒有先驗地圖的未知環(huán)境中的環(huán)境探索問題，不僅要根據(jù)帶有噪聲的傳感器觀測建立工作環(huán)境的地圖，同時要根據(jù)部分已建地圖估計智能車輛在環(huán)境中的位姿.基于EKF的SLAM方法其復(fù)雜度是環(huán)境特征個數(shù)的平方級，對錯誤數(shù)據(jù)的關(guān)聯(lián)非常敏感，難以應(yīng)用在智能車輛的自主導(dǎo)航問題中.針對這些不足，有人提出了基于粒子濾波器(PF)[2-3]的SLAM 方法(如 FastSLAM1.0，F(xiàn)astSLAM2.0 等)和基于無跡卡爾曼濾波(UKF)[4-5]的 SLAM 算法.UKF是一種通過選擇一組確定性采樣點(diǎn)(Sigma點(diǎn))的采樣策略逼近非線性分布的方法，其計算復(fù)雜度與EKF算法相同，但性能優(yōu)于EKF，具有計算量小、不需要求導(dǎo)計算雅克比矩陣、能夠避免采樣粒子衰退等問題的優(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)也是一種通過選擇采樣點(diǎn)(Sigma點(diǎn))的采樣策略逼近非線性分布的方法[11].但其具有比UKF較高的理論精度，而且更加容易實現(xiàn).祝繼華等[12]提出了基于CDKF的SLAM方法，在利用中心差分濾波方法產(chǎn)生改進(jìn)的提議分布函數(shù)基礎(chǔ)上，利用中心差分濾波方法初始化環(huán)境特征和更新地圖中的特征，提高移動機(jī)器人的同時定位與地圖創(chuàng)建的精度.田翔等[13]使用CDKF處理預(yù)測方程和測量方程，得到當(dāng)前時刻狀態(tài)向量相對于各個路標(biāo)的權(quán)重，利用此權(quán)重進(jìn)行狀態(tài)向量和狀態(tài)向量協(xié)方差的動態(tài)調(diào)整，從而提高了SLAM的速度.

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

1 FastSLAM算法

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

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

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

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

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

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

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

ICDKF算法如下:

第1步初始化:

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

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

2)時間更新方程:

3)測量值更新方程:

①迭代

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

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

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

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

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

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

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

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

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

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

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

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

3.3 權(quán)重更新

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

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

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

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

4 迭代中心差分粒子濾波SLAM算法的仿真實驗

4.1 實驗?zāi)Ｐ?/h3>

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

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

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

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

4.2 實驗結(jié)果分析

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

圖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)噪聲與觀測噪聲分都為高斯白噪聲條件下，4種算法在進(jìn)行路徑估計時X、Y和φ方向上算法估計誤差的對比.橫坐標(biāo)t表示實驗走過的步長，每個實驗都經(jīng)過了17 383步.從圖中可以看出，F(xiàn)astslam2.0的估計精度要低于其他3種SLAM算法，本文所提出的ICDPFFastSLAM算法采用了迭代測量更新，使其估計精度明顯高于其他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ù)據(jù)統(tǒng)計對比Table.1 Experiment statistics in an environment with Gauss noise

6 結(jié)束語

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

致 謝

實驗采用了澳大利亞野外機(jī)器人研究所(ACFR)提供的部分Matlab源碼，特此致謝，同時感謝在本文寫作過程中哈爾濱工業(yè)大學(xué)計算學(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].自動化學(xué)報，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é)報，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.