基于自適應漸消F K F的FastSL A M算法

劉 丹，段建民，于宏嘯（北京工業大學城市交通學院，北京100124）

基于自適應漸消F K F的FastSL A M算法

劉 丹，段建民，于宏嘯
（北京工業大學城市交通學院，北京100124）

快速同時定位與建圖（fast sim ultaneous localization and mapping，FastSL A M）算法的采樣過程會帶來粒子退化問題，為了改進算法的性能，提高估計精度，從研究粒子濾波的建議分布函數出發，提出基于自適應漸消擴展卡爾曼濾波（adaptive fading extended Kalman filter，A FE K F）的FastSL A M算法。該算法基于FastSL A M的基本框架，利用A FE K F產生一種參數可自適應調節的建議分布函數，使其更接近移動機器人的后驗位姿概率分布，減緩粒子集的退化。因此在同等粒子數的情況下，該算法有效提高了SL A M精度，以此減少所使用的粒子數，降低算法的復雜度。基于模擬器和標準數據集的實驗仿真結果驗證了該算法的有效性。

快速同時定位與建圖；粒子退化；自適應漸消擴展卡爾曼濾波；建議分布函數

網址：w w w.sys-ele.co m

0 引 言

同時定位與建圖（simultaneous local ization and mapping，SL A M）是指移動機器人在未知環境中探索時，根據自身攜帶的傳感器建造環境地圖，并在建造的地圖中確定自身的位置。SL A M問題被稱為自主移動機器人界的“圣杯”［1 2］。

SLAM問題針對的是未知且不確定的環境，目前常用的兩種解決方法［3］：一種是基于擴展卡爾曼濾波的SL A M （extended Kalman filter-SL A M，E K F-SL A M）算法；另一種是快速同時定位與地圖構建（fast simultaneous localization and mapping，FastSL A M）算法。然而隨著SL A M問題研究的深入，發現E K F方法在實際應用中存在很多問題，包括算法復雜度高，線性化誤差大以及數據關聯復雜等問題［4-5］，這些都嚴重限制其在SL A M研究領域的應用范圍。M ontemerlo于2003年提出了基于Rao-Blackwellized particle filter的FastSL A M算法［6］。FastSL A M算法有兩個版本：FastSL A M 1.0和FastSL A M 2.0［7］。FastSL A M 1.0算法通過粒子濾波器來估計移動機器人的位姿，用E K F來完成環境特征位置的估計［8］。雖然FastSL A M 1.0算法在一定程度上很好地解決了E K F-SL A M計算復雜度過高等問題，但其將SL A M問題的過程模型直接作為建議分布，沒有融合最新的測量信息，這使得算法存在較大缺陷，容易導致算法運行過程中粒子嚴重退化［9］，進而導致SL A M發散。為了解決這個問題，FastSL A M 2.0采用E K F算法對移動機器人位姿進行遞歸估計，得到估計均值和方差，因而產生的建議分布函數中融入了移動機器人的環境觀測信息，使得粒子集中分布于移動機器人真實位姿附近［7］。因此，FastSL A M 2.0算法的性能比FastSL A M 1.0算法整體上有所提高。針對FastSL A M算法中如何獲取更接近粒子后驗概率分布的建議分布函數問題，文獻［10］在FastSL A M 2.0算法基礎上提出了基于無跡卡爾曼濾波的FastSL A M算法，用無跡卡爾曼濾波（unscented Kalman filter，U K F）來估計粒子的后驗位姿建議分布，提高粒子采樣精度，進而提高系統狀態估計的精度。文獻［11-12］提出了基于迭代擴展Kalman濾波（iterated extended Kalman fi lter，IE K F）建議分布的FastSL A M算法，運用IE K F使得粒子分布于高觀測似然區域。因此，在FastSL A M2.0的基礎上，探討如何獲得一個可自適應調節的建議分布函數，來降低粒子退化程度，提高SL A M的精度，是本文工作的研究重點。

引入文獻［13-16］中所用的漸消濾波，將其與E K F融合，產生一種參數可自適應調節的建議分布函數，使系統具有更好的自適應性和魯棒性［16］；并且該函數使粒子可以更好地近似狀態變量的后驗概率密度函數，降低粒子的退化程度［3］，從而有效提高對移動機器人位姿的估計精度。因此本文提出了基于自適應漸消E K F的FastSL A M算法，可簡稱為：自適應漸消FastSL A M（adaptive fading sim ultaneous localization and mapping，A FFastSL A M），即在該算法遞歸估計過程中，由A FE K F來產生粒子重要性采樣函數，用粒子濾波估計移動機器人的位姿，E K F來完成地圖的預測和更新。

1 傳統的FastSL A M算法

1.1 SL A M問題的描述

從概率角度來說，在線SL A M問題是一個關于如何根據移動機器人前一時刻的位姿、觀測信息以及控制輸入信息來求得下一時刻位姿和環境地圖的聯合后驗概率問題［8］，聯合后驗概率表示為

式中，xt表示t時刻移動機器人的位姿；u1∶t和z1∶t分別表示控制信息和觀測信息c1∶t=［c1，c2，…，ct］表示從1時刻到t時刻的數據關聯；m表示環境地圖，每一個環境特征表示為mj。利用貝葉斯公式，將后驗概率表示為

式中，ct=｛η1，η2，…，ηM｝表示t時刻各個路標元素之間的數據關聯；ηj=k表示觀測信息中第j個觀測點與地圖中第k個路標為同一路標；ηj=表示觀測測信息中第j個觀測點與地圖中路標關聯失敗；η表示歸一化常數。

1.2 FastSL A M算法的基本思想

FastSL A M算法的基本思想是把SL A M系統的狀態估計分解為對移動機器人運動路徑的遞歸估計和基于估計路徑的環境特征位置估計［8 9］，表示為

式中，N表示地圖特征的個數。在FastSL A M算法中采用粒子濾波器進行路徑估計，每一個粒子都保存一份地圖，再將地圖估計分解為N個獨立的環境特征估計，每個環境特征采用一個擴展卡爾曼濾波器進行估計，因此在FastSL A M算法中，若粒子數為M個，則總共有M×N個擴展卡爾曼濾波器。t時刻第i個粒子的數據結構表達式為［8］

2 基于自適應漸消F K F的FastSL A M算法

FastSL A M算法是利用粒子濾波器進行位姿估計，而粒子濾波的退化問題卻是無法避免的，退化問題的根源在于粒子濾波方法中使用的粒子點并不是從后驗分布中抽樣出來的，而是從建議分布函數中抽樣得到的［17］；由于建議分布和后驗分布之間的差異，隨著算法的迭代，必然出現退化問題，所以建議分布函數選取的好壞，對權重方差的大小、退化問題的嚴重程度影響很大［17］，進而直接影響SL A M的估計精度。針對這個問題提出了基于自適應漸消E K F的FastSL A M算法，該算法對單個粒子的建議分布采用自適應漸消E K F獲得。

2.1 自適應漸消F K F

以非線性離散隨機系統模型為例［13］，即

式中，xt、zt分別表示t時刻系統的狀態向量和觀測向量；f為關于狀態的非線性函數；隨機系統噪聲wt-1～N（0，Q）；隨機觀測噪聲vt～N（0，R）。則基本的E K F方程為

（1）狀態預測

（2）卡爾曼增益

（3）狀態更新

傳統的E K F成為A FE K F的充分條件是在線選擇t時刻的時變濾波增益矩陣Kt使得式（12）和式（13）成立［18］。

式中，γt是新息，式（12）是E K F狀態估計新息最小方差性能指標；式（13）表示在不同時刻的輸出新息序列要互不相關，建立在式（12）和式（13）基礎上的A FE K F通過引入漸消因子λt，使得式（14）成立。

即輸出的新息序列是互不相關的，從物理意義上理解，表示這一濾波器與通常的E K F相比可更大限度地將有效信息從新息中提取出來，因此，A FE K F能高精度地跟蹤狀態量，在應對過程參數變化的魯棒性方面也有所改善［13］。令

式（16）用來衡量濾波器的性能，由于S（t）=｛Sij（t）｝，可知當f（t）取值越小，濾波性能就越好，則應選擇λt使f（t）最小。由式（15）可知最優時應滿足

此時的E K F就變成了A FE K F，即式（8）變為

式中，λt為自適應漸消因子，限定其最小值為1。即

λt的求解過程就是將式（9）和式（18）代入式（17）中，可得到

其中

則式（20）化簡為：Nt=Mt·λ，兩邊求跡，近似得到：

上述過程與一般的E K F相比，預測協方差矩陣前面多乘了一個自適應漸消因子λt，其物理意義是：過程參數發生變化或在不精確的噪聲統計情況下，新息的增大會引起Vt的增大，進而使得Nt增大，由λt的算式可以看出，λt相應增大，濾波器的估計能力增強，使系統狀態估計迅速收斂到真實值附近，此時濾波性能達到最佳。這就意味著采用漸消E K F時，能夠減小陳舊觀測值對估計的影響，使系統具有更好的自適應性和魯棒性，進而提高了濾波器的估計精度［19］。

2.2 AFFastSL A M算法

2.2.1 位姿估計（建議分布計算）

改進的算法中，利用A FE K F產生粒子的建議分布函數，采用粒子濾波器來進行位姿估計。位姿估計過程包含位姿預測和位姿更新兩個階段。

（1）位姿預測

首先預測t時刻移動機器人位姿的均值和方差，即

式中，ut表示控制輸入信息；f表示運動模型方程；表示t-1時刻移動機器人的位姿表示t時刻的位姿預測均值。

式中，λt為漸消因子為位姿預測協方差矩陣表示函數f對位姿x求得的雅克比矩陣表示上一時刻位姿估計方差；Qt-1表示控制噪聲的協方差矩陣。

（2）位姿更新

當觀測的路標信息與已經建立的地圖中的路標信息匹配時，即索引為j的環境路標被移動機器人重新觀測到，則執行更新步驟

此時根據漸消自適應E K F思想，利用式（20）～式（27）計算得到λt，進而通過式（35）～式（37）計算更新后位姿的均值和方差

t時刻，若移動機器人同時觀測到多個環境路標，且其都存在于已經建立的地圖中，則需要依次根據每一個路標的觀測值對移動機器人的位姿均值x及其協方差進行更新，每次更新均以前次更新結果作為初始值。更新完畢后，得到建議分布函數N由于漸消因子λt對權值方差進行調節，可以更準確地提取有效信息，抑制突變狀況對控制模型和觀測模型的影響，使得到的建議分布函數更接近真實的分布［16，19］。

對每個粒子計算權值：

并歸一化權值

2.2.2 環境地圖估計

傳統FastSL A M算法中，將地圖估計分解為N個特征估計問題，并通過E K F來估計地圖中路標的條件概率分布。由于地圖中的每個環境特征均服從獨立的二維高斯分布，所以雅可比矩陣是一個2×2的矩陣，計算量相對較小［3，20］，計算效率比較高，此時，E K F相比U K F［10］，IE K F［11 12］以及粒子濾波等算法都有明顯的計算優勢，因而基于自適應漸消擴展Kalman濾波的FastSL A M算法中的每一個粒子，在更新完移動機器人位姿的部分后，利用E K F算法對環境路標進行更新。在此階段需要對當前時刻的重復觀測路標和首次觀測路標采用不同的策略進行處理。

（1）當t時刻觀測的索引為j的路標信息與已建立地圖中的某些路標經過數據關聯能夠匹配時，更新的過程如下：

首先，在移動機器人位姿已知的前提下，對索引為j的路標的預測觀測值計算公式為

式中，Kj，t表示路標更新卡爾曼增益；表示t-1時刻的路標j的估計方差。

式中，zj，t表示t時刻第j個環境路標的真實測量值；和分別表示更新后第j個路標的均值和方差。

（2）t時刻，第一次觀測到索引為n的環境路標時，需計算路標的全局坐標并將其加入所建的地圖中，具體過程為

式中，zn，t為觀測值；分別表示首次觀測到路標的均值和方差。

2.2.3 重采樣

除了通過獲取最優的建議分布函數，來解決粒子退化問題外，重采樣方法在一定程度上也會緩解粒子的退化現象。重采樣的基本思想是拋棄那些重要性權重很小的粒子點，而復制權重較大的粒子點來替代它們。但重采樣也會帶來一些問題，它使得小權重粒子大量消失，大權重粒子被反復復制，致使粒子多樣性變差而發生粒子貧化現象，造成濾波精度下降。文獻［21］提出了以一種改進的重采樣算法，按照局部重采樣算法對粒子進行分類，中等權重的粒子保持不變，大權重和小權重的粒子采用Tho m pson-Taylor算法進行隨機線性組合產生新粒子。文獻［11］將復制的粒子與偽拋棄組中的粒子進行線性組合，提出了線性優化重采樣方法，這些改進算法在一定程度上都降低了粒子的貧化程度，保證了粒子的多樣性，但算法的運算復雜度增加。本文提出算法中應用的重采樣方法是由文獻［22］提出的自適應重采樣。首先設定一個閾值3N/4，N為粒子數，然后計算粒子集的有效粒子數Neff。若Neff小于設定的閾值，則對粒子集進行系統重采樣。Neff可以由式（48）近似計算得到。

此重采樣方法相對簡單，運算效率高，但是在確保粒子多樣性問題上還存在不足，因此在未來的工作中會研究如何獲得一個衡量粒子退化程度的度量函數，來解決何時進行重采樣，多少粒子需要重采樣等問題，來減少重采樣計算量，保證粒子多樣性，提高濾波精度，進而提高SL A M精度。

2.2.4 算法流程

綜上，改進的t時刻的SL A M算法具體過程如下：

設粒子數N，控制噪聲Q，觀測噪聲R。

步驟1 初始化粒子集S0=；從先驗分布中隨機抽取采樣點，令其權值為其中N為粒子數。

步驟2 重要性采樣：首先利用A FE K F計算粒子的后驗位姿建議分布，然后從建議分布中進行采樣得到新一代粒子（式（38）），并計算其重要性權重（式（39））。

步驟3 對于已經存儲在粒子中的重復性觀測路標，根據式（41）～式（44）更新其狀態以及協方差矩陣。

步驟4 對于第一次被觀測到的路標，根據式（45）～式（47）初始化環境路標的狀態均值和協方差矩陣，并加入所建立的環境地圖中。

步驟5 按照式（48）計算粒子集的Neff，如果Neff≤3N/4，則對粒子集進行系統重采樣。

3 仿真結果

為了驗證提出算法的性能，分別基于模擬器和“Car Park Dataset”數據集的實驗仿真對A FFastSL A M、U FastSL A M、FastSL A M 2.0進行比較，程序的運行環境為M atlab2012b，使用的計算機CP U型號為Intel（R）Core （T M）i5-3470，主頻為3.2 G Hz。

3.1 基于模擬器的實驗仿真

利用文獻［23］開發的SL A M算法模擬器，對提出的A FFastSL A M算法與U FastSL A M，FastSL A M 2.0算法的性能進行對比。

仿真環境如圖1所示，在這個環境中包含了移動機器人行走的路徑和環境路標（星號“*”），其中，這些路標都是靜態路標。在仿真中用到的運動學模型和環境模型如式（49）和式（50）所示，其中運動學模型為后輪驅動模型。仿真實驗中，機器人的移動速度為3 m/s，最大轉向角為30°，軸距為2 m，激光雷達最大的觀測范圍為20 m，觀測頻率為50 Hz，控制時間間隔為0.025 s。

圖1 基于模擬器的仿真環境

輸入：xv（t-1）為移動機器人在t-1時刻的位姿，dt為傳感器采樣時間，v為速度，G為機器人在t時刻的方向角，W B為軸距。輸出：xv（t）為機器人的新位姿。

輸入：（xj，yj）為激光雷達探測到的第j個特征的位置坐標，xv（t）為移動機器人的位姿。輸出：路標特征點與移動機器人的距離r以及與機器人前進方向的夾角θ。

3.1.1 算法性能的比較

（1）不同測量噪聲條件下的算法性能比較

仿真中設定粒子數為10個，控制噪聲設為（0.3 m/s，3°）；觀測噪聲分別設置5組：（0.04 m，0.6°）、（0.07 m，0.8°）、（0.1 m，1°）、（0.2 m，3°）、（0.4 m，4°），對于每組觀測噪聲，均對A FFastSL A M算法、U FastSL A M算法、FastSL A M 2.0算法進行50次蒙特卡羅仿真實驗［7，24］，并將50次仿真的移動機器人位姿的均方根誤差（root meansquare error，R M SE）均值（評估SL A M估計精度）及標準差（評估SL A M執行穩定性）作為評價標準［7，24］，R M SE的計算公式如式（51）所示，實驗結果如圖2所示，其中線棒代表R M SE標準差，柱狀代表R M SE均值。可見隨著噪聲增加，3種SL A M算法對移動機器人位姿估計的R M SE均值和標準差都逐漸增加，但是A FFastSL A M算法對移動機器人位姿估計的R M SE均值要低于U FastSL A M和FastSL A M 2.0兩種算法，而且較之另外兩種算法，A FFastSL A M算法對應的RS M E標準差增長速度也相對緩慢，因此提出的算法具有較高的估計精度、較強的抑制噪聲能力和自適應能力。

式中，N為每次SL A M仿真中離散采樣點的數量；sk為移動機器人的實際位姿；為移動機器人的估計位姿。

圖2 不同測量噪聲條件下的3種算法R M SE

（2）粒子數目不同條件下的算法性能比較

研究提出的算法在增長粒子數條件下的性能［7，24 25］。其中粒子數分別設定為5，10，30，50，70，100，控制噪聲設為（0.2 m/s，1°），觀測噪聲為（0.1 m，0.8°），分別對3種SL A M算法進行30次仿真，實驗結果如圖3所示，可以看出，隨著粒子數的增多，3種算法的位姿估計的R M SE均值都逐漸降低，即位姿估計精度越來越高，但是要達到相同的位姿估計精度，提出的SL A M算法所需要的粒子數要少于其他兩種算法使用的粒子數，所以提出的算法可以使用更少的粒子達到更高的精度，而且能夠保證計算效率。

圖3 不同粒子數下的3種算法R M SE

3.1.2 算法復雜度的對比

提出算法和U FastSL A M、FastSL A M 2.0算法的復雜度都主要取決于路徑估計、地圖估計、權重計算、以及粒子重采樣這4個步驟的計算復雜度［25 26］。假設粒子數M個，環境特征點N個，傳感器一次觀測到n個特征，其中，k個特征已經存在于地圖中，則計算建議分布的復雜度為O（M*k）；粒子權重的計算復雜度與粒子的數目成正比，即為O（M），更新地圖的復雜度為O（M*n）；對于重采樣階段，首先需要計算有效粒子數Neff，其復雜度為O（M），同時每個粒子中都存有各自的特征地圖，因此重采樣的復雜度為O（M*N）。以表1為例，提出的算法和其他兩種算法的復雜度基本相同，但是要達到同樣的估計精度，提出的算法要比其他兩種算法中所用的粒子數少，即O（M）＜O（M1）＜O（M2）。如表2所示，當位姿估計的R M SE均值大致相同時，A FFastSL A M所需要的粒子數要少于U FastSL A M和FastSL A M 2.0；而且如表3所示，在采用相同粒子數進行算法仿真實驗時，提出算法所得到的位姿估計精度是最高的，其運行時間也遠小于U FastSL A M，但由于提出算法中存在計算自適應漸消因子的環節，所以其執行時間相比FastSL A M 2.0有所增加。因此要獲得更準確的SL A M結果，應用提出的算法會降低計算的復雜程度，提高SL A M過程的計算效率，同時因重采樣導致的粒子貧化程度也會相對降低。

表1 3種算法復雜度對比表

表2 均方根誤差近似相等時的粒子數比較

表3 粒子數為100時均方根誤差和運行時間對比表

3.2 基于標準數據集的實驗仿真

采用悉尼大學地面移動機器人中心（A CF R）提供的“Car Park Dataset”數據集對SL A M算法進行仿真研究，該數據集是SL A M研究領域的一個標準數據集，其數據包和相關文檔均可以在文獻［27］中獲得。該數據集的實驗地點位于一個衛星數量較多，能夠獲取高質量GPS信息的30× 45 m2的校園停車場內，實驗平臺為一個配備有里程計、前輪偏角傳感器，激光雷達、全球定位系統（global position systerm，G PS）的四輪汽車，在實驗中使用60 m m的鋼管作為人造路標；激光雷達作為環境感知傳感器；G PS用于獲取車輛和路標的定位信息，其數據僅用來評價SL A M算法的性能，并不參與SL A M過程的運算［7］。如圖4所示，僅僅根據里程計和前輪偏角傳感器測得的車輛速度和轉向角，通過航跡推算得到的車輛路徑，會偏離G PS測得的實際路徑［7，25］。

圖4 “Car Park Dataset”數據集的G PS路徑和航跡推算路徑

利用“Car Park Dataset”數據集，對A FFastSL A M、U FastSL A M、FastSL A M 2.0算法進行對比研究。仿真實驗中，控制噪聲設為（0.7 m/s，7°）；觀測噪聲設為（0.1 m，1°）；粒子數設為50個。圖5表示提出的算法和其他兩種算法的仿真結果，可以看出，A FFastSL A M使得估計路徑與G PS產生的真實路徑吻合程度更高，相對應的路標估計也更準確，因此A FFastSL A M性能要優于U FastSL A M和FastSL A M 2.0。

圖5 “Car Park Dataset”數據集的SL A M實驗結果

為了進一步進行量化分析，得到了圖6所示的位姿估計誤差曲線和圖7所示的路標估計誤差曲線。圖6（a）和圖6（b）分別表示車輛在x軸和y軸方向的位姿估計誤差，不難看出，所提算法的位姿估計誤差要小于其他兩種算法，其中提出算法位姿估計的誤差最大為0.919 1m，而U FastSL A M和FastSL A M 2.0算法的最大位姿誤差分別為1.155 0 m和1.208 2 m。如圖7所示，提出算法的路標估計誤差要小于FastSL A M 2.0算法，而在第5和第6個路標點處的估計誤差，提出算法要大于U FastSL A M算法中產生的誤差，但提出算法對整個地圖估計的精度要大于U FastSL A M算法。由圖6和圖7量化分析得出，所提算法相比其他兩種算法具有更好的定位精度和地圖構建精度。

圖6 位姿估計誤差曲線

圖7 路標估計誤差曲線

為了更準確的評估提出算法的性能，基于該數據集，在粒子數均選取30個的前提下，對3種SL A M算法分別進行10次蒙特卡羅仿真實驗，并以10次實驗的位姿估計和路標估計的R M SE的均值和標準差作為評價標準，結果如圖8所示，圖中曲線表示R M SE均值，線棒表示R M SE標準差。可以看出，提出的算法得到的位姿估計和路標估計的R M SE均值均低于U FastSL A M算法和FastSL A M2.0算法，這意味著提出的算法對移動機器人位姿的估計精度和地圖構建精度都要優于其他兩種算法，而且從位姿估計和路標估計的RS M E標準差變化趨勢中可以看出，提出算法的運行穩定性較強。

圖8 位姿估計和路標估計的均方根誤差

4 結 論

本文將漸消濾波的思想引入到FastSL A M2.0算法中，提出了一種基于自適應漸消E K F的FastSL A M算法，該算法在產生建議分布函數時，融入了最新環境觀測值，利用漸消因子自適應的調節權值，充分提取有效信息，使之得到的建議分布函數更加貼近后驗分布函數，從而提高了SL A M預測過程的粒子采樣效率，保證了粒子的收斂性。實驗結果表明，與U FastSL A M，FastSL A M2.0算法相比，提出的算法降低了粒子的退化程度，提高了SL A M精度，能夠在粒子數較少的情況下較精確地完成移動機器人的同時定位與建圖。

［1］Durrant-W hyte H，Bailey T.Sim ultaneous localization and mapping：part I［J］.IE E E Trans.on Robotics and Autom ation M agazine，2006，13（2）：99-110.

［2］Bailey T，Durrant-W hyte H.Sim ultaneous localization and mapping：part II［J］.IE E E Trans.on Robotics andA utomation M agazine，2006，13（3）：108-117.

［3］Zhou W，Zhao C X.A FastSL A M 2.0 algorithm based on genetic algorithm［J］.Robot，2009，31（1）：25-32.（周武，趙春霞.一種基于遺傳算法的FastSL A M 2.0算法［J］.機器人，2009，31（1）：25-32.）

［4］H olmes S，Klein G，M urray D W.A n O（N2）square root unscented Kalman filter for visual sim ultaneous localization and mapping［J］.IE E E Trans.on Pattern Analysis and M achine Intelligence，2009，31（7）：1251-1263.

［5］H wang S Y，Song J B.M onocular vision-based SL A M in indoor environ ment using corner，lam p，and door features fro m upward-looking ca mera［J］.IE E E Trans.on Industrial Electro-nics，2011，58（10）：4804-4812.

［6］M ontemerlo M.FastSL A M：a factored solution to the sim ultaneous localization and mapping problem with unknow n data association［D］.Pennsylvania：Carnegie M ellon U niversity，2003.

［7］Song Y，Li Q L，Kang Y F，et al.SL A M with square-root cubature Rao-Black willised particle filter［J］.Acta A utom atica Sinica，2014，40（2）：357-367.（宋宇，李慶玲，康軼非，等.平方根容積Rao-Black willised粒子濾波SL A M算法［J］.自動化學報，2014，40（2）：357-367.）

［8］T hrun S，M ontemerlo M，K oller D，et al.FastSL A M：an efficient solution to the sim ultaneous localization and mapping problemwith unknow n data association［J］.M achine Learning，2004，4（3）：380-407.

［9］Tang W J，Zhang G L，Jing B.Co m parative study of FastSL A M algorith m for m obile robot［J］.Com puter Engineering and Design，2012，33（3）：1165-1169，1180.（湯文俊，張國良，敬斌.移動機器人FastSL A M算法的對比研究［J］.計算機工程與設計，2012，33（3）：1165-1169，1180.）

［10］Kim C，Sakthivel R，Chung W K.U nscented FastSL A M：a robust and efficient solution to the SL A Mproblem［J］.IE E E Trans.on Robotics，2008，24（4）：808-820.

［11］W ang H J，W ang J，Liu Z Y.Fast sim ultaneous localization and mapping based on iterative extended Kalman filter proposal distribution and linear optimization resa m pling［J］.Journal of Electronics&Inform ation Technology，2014，36（2）：318-324.（王宏健，王晶，劉振業.基于迭代擴展Kalman濾波建議分布和線性優化重采樣的快速同步定位與構圖［J］.電子與信息學報，2014，36（2）：318-324.）

［12］Zhan L，Zhao C X.A new FastSL A M algorith m based on iterated E K F［J］.Journal of Shandong University （Engineering Science），2012，42（4）：41-47.（張麗，趙春霞.一種基于迭代E K F的FastSL A M算法［J］.山東大學學報（工學版），2012，42 （4）：41-47.）

［13］Zhou DH，Xi Y G，Zhang Z J.Suboptimal fading extended Kalman filtering for nonlinear systems［J］.Control and Decision，1990，3（5）：1-6.（周東華，席裕庚，張鐘俊.非線性系統帶次優漸消因子的擴展卡爾曼濾波［J］.控制與決策，1990，3 （5）：1-6.）

［14］Yang L Q，Xiao Q G，Niu Y，et al.Design oflocalization system based on reducing Kalman filter［J］.Journal of N anjing University of Aeronautics&Astronautics，2012，44（1）：134-138.（楊柳慶，肖前貴，牛妍，等.基于漸消卡爾曼濾波器的定位系統設計［J］.南京航空航天大學學報，2012，44（1）：134-138.）

［15］Xu D J，H e R，Shen F，et al.A daptive fading Kalman filter based on innovation covariance［J］.Systems Engineering and Electronics，2011，33（12）：2696-2699.（徐定杰，賀瑞，沈鋒，等.基于新息協方差的自適應漸消卡爾曼濾波器［J］.系統工程與電子技術，2011，33（12）：2696-2699.）

［16］Gong Y S，Gui QM，Li B L，et al.A daptive fading extended Kalman particle filtering applied to integrated navigation［J］. Geodesy and Geodyna mics，2010，30（1）：99-103.（宮軼松，歸慶明，李保利，等.自適應漸消擴展Kalman粒子濾波方法在組合導航中的應用［J］.大地測量與地球動力學，2010，30 （1）：99-103.）

［17］Du Z C.Particle fi lter and the research of its appl ication in MIMO wireless com munication［D］.Chengdu：University of Electronic Science and Technology，2008.（杜正聰.粒子濾波及其在MIMO無線通信中的應用研究［D］.成都：電子科技大學，2008.）

［18］W ang H J，Fu G X，Li J，et al.Strong tracking C K F based SL A Mmethod for un manned underwater vehicle［J］.Chinese Journal of Scientific Instru ment，2013，34（1）：2543-2550.（王宏健，傅桂霞，李娟，等.基于強跟蹤C K F的無人水下航行器SL A M［J］.儀器儀表學報，2013，34（1）：2543-2550.）

［19］Gao S S，Xue L，W ei WH.Fading adaptive U PF algorith m and its application to integrated navigation［J］.Journal of N orthwestern Polytechnical University，2012，30（1）：27-31.（高社生，薛麗，魏文輝.漸消自適應Unscented粒子濾波及其在組合導航中的應用［J］.西北工業大學學報，2012，30（1）：27-31.）

［20］W ang H，Ding J D，Fang B F，et al.Particle filter algorith m based on genetic optimization method［J］.Frontiers of Computer Science and Technology，2012，6（10）：927-934.（王浩，丁家棟，方寶富，等.融合遺傳優化的粒子濾波器算法［J］.計算機科學與探索，2012，6（10）：927-934.）

［21］Chang T Q，LI Y，Liu Z R，et al.Particle filter algorith m based on im proved resa m pling［J］.A pplication Research of Com puters，2013，30（3）：748-750.（常天慶，李勇，劉忠仁，等.一種改進重采樣的粒子濾波算法［J］.計算機應用研究，2013，30（3）：748-750.）

［22］Doucet A，Godsill S，A ndrieu C.O n sequential M onte Carlo sam plingmethods for Bayesian filtering［J］.Statistics and Com puting，2000，10（3）：197-208.

［23］Bai ley T，Nieto J，Nebot E.Consistency of the FastSL A M algorithm［C］∥Proc.of the Robotics and Automation，2006：424-429.

［24］Song Y，Li QL，Kang YF，et al.Effective cubature FastSL A M：SL A M with Rao-Black wellized particle filter and cubature rule for Gaussian weighted integral［J］.A dvanced Robotics，2013，27（17）：1301-1312.

［25］H avangi R，Taghirad H D，Nekoui M A，et al.A square root unscented FastSL A M with im proved proposal distribution and resam pling［J］.IE E E Trans.on Industrial Electronics，2014，61（5）：2334-2345.

［26］Zhu J H，Zhen NN，Yuan Z J，et al.A SL A M algorith m based on central difference particle filter［J］.Acta A utom atica Sinica，2010，36（2）：249-257.（祝繼華，鄭南寧，袁澤劍，等.基于中心差分粒子濾波的SL A M算法［J］.自動化學報，2010，36 （2）：249-257.）

［27］Nieto J，G uivant J，Nebot E，et al.Real time data association for FastSL A M［C］∥Proc.of the Robotics and A utomation，2003：412-418.

FastSL A Malgorith m based on adaptive fading extended Kalman filter

LIU Dan，D U A N Jian-min，Y U H ong-xiao
（College of M etropolitan Transportation，Beijing University of Technology，Beijing 100124，China）

Sampling process often causes particle degradation in fast simultaneous localization and mapping （FastSL A M）.Fro m the point view of the proposal distribution function，a method named the FastSL A M based on adaptive fading extended Kalman filter is proposed to improve the performance of the algorith m and increase estimation accuracy.It uses the adaptive fading extended Kalman filter（A F E K F）to co m pute proposal distribution based on the basic framework of FastSL A M，then this proposal distribution is m ore close to the posterior position of the m obile robot and the degree of particle degradation is reduced.In the case of the same number of particles，the algorith m can effectively improve the accuracy of SL A M.Henceit can reduce the number of particles used in the algorith m and the complexity of the algorithm.The validity of the proposed algorith m is verified by the experimental simulation results based on the simulator and the standard data set.

fast sim ultaneous localization and mapping（FastSL A M）；particle degradation；adaptive fading extended Kalman filter（A F E K F）；proposal distribution

T P 242.6

A

10.3969/j.issn.1001-506 X.2016.03.26

1001-506 X（2016）03-0644-08

2014-12-15；

2015-07-21；網絡優先出版日期：2015-09-17。

網絡優先出版地址：http：∥w w w.cnki.net/kcms/detail/11.2422.T N.20150917.1659.004.html

北京市教委基金項目（JJ002790200802）資助課題

劉 丹（1990-），女，博士研究生，主要研究方向為智能車輛的同時定位與建圖。

E-mail：danaliu@yeah.net

段建民（1959-），男，教授，博士研究生導師，主要研究方向為車輛環境識別與自動駕駛技術、網絡化測控系統與現場總線技術、嵌入式汽車電子控制技術。

E-mail：jm duan@bjut.edu.cn

于宏嘯（1986-），男，博士研究生，主要研究方向為智能車輛橫縱向控制。

E-mail：yhxiao321@gmail.com