基于自適應(yīng)漸消F K F的FastSL A M算法

劉 丹，段建民，于宏嘯（北京工業(yè)大學(xué)城市交通學(xué)院，北京100124）

基于自適應(yīng)漸消F K F的FastSL A M算法

劉 丹，段建民，于宏嘯
（北京工業(yè)大學(xué)城市交通學(xué)院，北京100124）

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

快速同時(shí)定位與建圖；粒子退化；自適應(yīng)漸消擴(kuò)展卡爾曼濾波；建議分布函數(shù)

網(wǎng)址：w w w.sys-ele.co m

0 引 言

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

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

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

1 傳統(tǒng)的FastSL A M算法

1.1 SL A M問(wèn)題的描述

從概率角度來(lái)說(shuō)，在線SL A M問(wèn)題是一個(gè)關(guān)于如何根據(jù)移動(dòng)機(jī)器人前一時(shí)刻的位姿、觀測(cè)信息以及控制輸入信息來(lái)求得下一時(shí)刻位姿和環(huán)境地圖的聯(lián)合后驗(yàn)概率問(wèn)題［8］，聯(lián)合后驗(yàn)概率表示為

式中，xt表示t時(shí)刻移動(dòng)機(jī)器人的位姿；u1∶t和z1∶t分別表示控制信息和觀測(cè)信息c1∶t=［c1，c2，…，ct］表示從1時(shí)刻到t時(shí)刻的數(shù)據(jù)關(guān)聯(lián)；m表示環(huán)境地圖，每一個(gè)環(huán)境特征表示為mj。利用貝葉斯公式，將后驗(yàn)概率表示為

式中，ct=｛η1，η2，…，ηM｝表示t時(shí)刻各個(gè)路標(biāo)元素之間的數(shù)據(jù)關(guān)聯(lián)；ηj=k表示觀測(cè)信息中第j個(gè)觀測(cè)點(diǎn)與地圖中第k個(gè)路標(biāo)為同一路標(biāo)；ηj=表示觀測(cè)測(cè)信息中第j個(gè)觀測(cè)點(diǎn)與地圖中路標(biāo)關(guān)聯(lián)失??；η表示歸一化常數(shù)。

1.2 FastSL A M算法的基本思想

FastSL A M算法的基本思想是把SL A M系統(tǒng)的狀態(tài)估計(jì)分解為對(duì)移動(dòng)機(jī)器人運(yùn)動(dòng)路徑的遞歸估計(jì)和基于估計(jì)路徑的環(huán)境特征位置估計(jì)［8 9］，表示為

式中，N表示地圖特征的個(gè)數(shù)。在FastSL A M算法中采用粒子濾波器進(jìn)行路徑估計(jì)，每一個(gè)粒子都保存一份地圖，再將地圖估計(jì)分解為N個(gè)獨(dú)立的環(huán)境特征估計(jì)，每個(gè)環(huán)境特征采用一個(gè)擴(kuò)展卡爾曼濾波器進(jìn)行估計(jì)，因此在FastSL A M算法中，若粒子數(shù)為M個(gè)，則總共有M×N個(gè)擴(kuò)展卡爾曼濾波器。t時(shí)刻第i個(gè)粒子的數(shù)據(jù)結(jié)構(gòu)表達(dá)式為［8］

2 基于自適應(yīng)漸消F K F的FastSL A M算法

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

2.1 自適應(yīng)漸消F K F

以非線性離散隨機(jī)系統(tǒng)模型為例［13］，即

式中，xt、zt分別表示t時(shí)刻系統(tǒng)的狀態(tài)向量和觀測(cè)向量；f為關(guān)于狀態(tài)的非線性函數(shù)；隨機(jī)系統(tǒng)噪聲wt-1～N（0，Q）；隨機(jī)觀測(cè)噪聲vt～N（0，R）。則基本的E K F方程為

（1）狀態(tài)預(yù)測(cè)

（2）卡爾曼增益

（3）狀態(tài)更新

傳統(tǒng)的E K F成為A FE K F的充分條件是在線選擇t時(shí)刻的時(shí)變?yōu)V波增益矩陣Kt使得式（12）和式（13）成立［18］。

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

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

式（16）用來(lái)衡量濾波器的性能，由于S（t）=｛Sij（t）｝，可知當(dāng)f（t）取值越小，濾波性能就越好，則應(yīng)選擇λt使f（t）最小。由式（15）可知最優(yōu)時(shí)應(yīng)滿足

此時(shí)的E K F就變成了A FE K F，即式（8）變?yōu)?/p>

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

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

其中

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

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

2.2 AFFastSL A M算法

2.2.1 位姿估計(jì)（建議分布計(jì)算）

改進(jìn)的算法中，利用A FE K F產(chǎn)生粒子的建議分布函數(shù)，采用粒子濾波器來(lái)進(jìn)行位姿估計(jì)。位姿估計(jì)過(guò)程包含位姿預(yù)測(cè)和位姿更新兩個(gè)階段。

（1）位姿預(yù)測(cè)

首先預(yù)測(cè)t時(shí)刻移動(dòng)機(jī)器人位姿的均值和方差，即

式中，ut表示控制輸入信息；f表示運(yùn)動(dòng)模型方程；表示t-1時(shí)刻移動(dòng)機(jī)器人的位姿表示t時(shí)刻的位姿預(yù)測(cè)均值。

式中，λt為漸消因子為位姿預(yù)測(cè)協(xié)方差矩陣表示函數(shù)f對(duì)位姿x求得的雅克比矩陣表示上一時(shí)刻位姿估計(jì)方差；Qt-1表示控制噪聲的協(xié)方差矩陣。

（2）位姿更新

當(dāng)觀測(cè)的路標(biāo)信息與已經(jīng)建立的地圖中的路標(biāo)信息匹配時(shí)，即索引為j的環(huán)境路標(biāo)被移動(dòng)機(jī)器人重新觀測(cè)到，則執(zhí)行更新步驟

此時(shí)根據(jù)漸消自適應(yīng)E K F思想，利用式（20）～式（27）計(jì)算得到λt，進(jìn)而通過(guò)式（35）～式（37）計(jì)算更新后位姿的均值和方差

t時(shí)刻，若移動(dòng)機(jī)器人同時(shí)觀測(cè)到多個(gè)環(huán)境路標(biāo)，且其都存在于已經(jīng)建立的地圖中，則需要依次根據(jù)每一個(gè)路標(biāo)的觀測(cè)值對(duì)移動(dòng)機(jī)器人的位姿均值x及其協(xié)方差進(jìn)行更新，每次更新均以前次更新結(jié)果作為初始值。更新完畢后，得到建議分布函數(shù)N由于漸消因子λt對(duì)權(quán)值方差進(jìn)行調(diào)節(jié)，可以更準(zhǔn)確地提取有效信息，抑制突變狀況對(duì)控制模型和觀測(cè)模型的影響，使得到的建議分布函數(shù)更接近真實(shí)的分布［16，19］。

對(duì)每個(gè)粒子計(jì)算權(quán)值：

并歸一化權(quán)值

2.2.2 環(huán)境地圖估計(jì)

傳統(tǒng)FastSL A M算法中，將地圖估計(jì)分解為N個(gè)特征估計(jì)問(wèn)題，并通過(guò)E K F來(lái)估計(jì)地圖中路標(biāo)的條件概率分布。由于地圖中的每個(gè)環(huán)境特征均服從獨(dú)立的二維高斯分布，所以雅可比矩陣是一個(gè)2×2的矩陣，計(jì)算量相對(duì)較?。?，20］，計(jì)算效率比較高，此時(shí)，E K F相比U K F［10］，IE K F［11 12］以及粒子濾波等算法都有明顯的計(jì)算優(yōu)勢(shì)，因而基于自適應(yīng)漸消擴(kuò)展Kalman濾波的FastSL A M算法中的每一個(gè)粒子，在更新完移動(dòng)機(jī)器人位姿的部分后，利用E K F算法對(duì)環(huán)境路標(biāo)進(jìn)行更新。在此階段需要對(duì)當(dāng)前時(shí)刻的重復(fù)觀測(cè)路標(biāo)和首次觀測(cè)路標(biāo)采用不同的策略進(jìn)行處理。

（1）當(dāng)t時(shí)刻觀測(cè)的索引為j的路標(biāo)信息與已建立地圖中的某些路標(biāo)經(jīng)過(guò)數(shù)據(jù)關(guān)聯(lián)能夠匹配時(shí)，更新的過(guò)程如下：

首先，在移動(dòng)機(jī)器人位姿已知的前提下，對(duì)索引為j的路標(biāo)的預(yù)測(cè)觀測(cè)值計(jì)算公式為

式中，Kj，t表示路標(biāo)更新卡爾曼增益；表示t-1時(shí)刻的路標(biāo)j的估計(jì)方差。

式中，zj，t表示t時(shí)刻第j個(gè)環(huán)境路標(biāo)的真實(shí)測(cè)量值；和分別表示更新后第j個(gè)路標(biāo)的均值和方差。

（2）t時(shí)刻，第一次觀測(cè)到索引為n的環(huán)境路標(biāo)時(shí)，需計(jì)算路標(biāo)的全局坐標(biāo)并將其加入所建的地圖中，具體過(guò)程為

式中，zn，t為觀測(cè)值；分別表示首次觀測(cè)到路標(biāo)的均值和方差。

2.2.3 重采樣

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

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

2.2.4 算法流程

綜上，改進(jìn)的t時(shí)刻的SL A M算法具體過(guò)程如下：

設(shè)粒子數(shù)N，控制噪聲Q，觀測(cè)噪聲R。

步驟1 初始化粒子集S0=；從先驗(yàn)分布中隨機(jī)抽取采樣點(diǎn)，令其權(quán)值為其中N為粒子數(shù)。

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

步驟3 對(duì)于已經(jīng)存儲(chǔ)在粒子中的重復(fù)性觀測(cè)路標(biāo)，根據(jù)式（41）～式（44）更新其狀態(tài)以及協(xié)方差矩陣。

步驟4 對(duì)于第一次被觀測(cè)到的路標(biāo)，根據(jù)式（45）～式（47）初始化環(huán)境路標(biāo)的狀態(tài)均值和協(xié)方差矩陣，并加入所建立的環(huán)境地圖中。

步驟5 按照式（48）計(jì)算粒子集的Neff，如果Neff≤3N/4，則對(duì)粒子集進(jìn)行系統(tǒng)重采樣。

3 仿真結(jié)果

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

3.1 基于模擬器的實(shí)驗(yàn)仿真

利用文獻(xiàn)［23］開(kāi)發(fā)的SL A M算法模擬器，對(duì)提出的A FFastSL A M算法與U FastSL A M，F(xiàn)astSL A M 2.0算法的性能進(jìn)行對(duì)比。

仿真環(huán)境如圖1所示，在這個(gè)環(huán)境中包含了移動(dòng)機(jī)器人行走的路徑和環(huán)境路標(biāo)（星號(hào)“*”），其中，這些路標(biāo)都是靜態(tài)路標(biāo)。在仿真中用到的運(yùn)動(dòng)學(xué)模型和環(huán)境模型如式（49）和式（50）所示，其中運(yùn)動(dòng)學(xué)模型為后輪驅(qū)動(dòng)模型。仿真實(shí)驗(yàn)中，機(jī)器人的移動(dòng)速度為3 m/s，最大轉(zhuǎn)向角為30°，軸距為2 m，激光雷達(dá)最大的觀測(cè)范圍為20 m，觀測(cè)頻率為50 Hz，控制時(shí)間間隔為0.025 s。

圖1 基于模擬器的仿真環(huán)境

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

輸入：（xj，yj）為激光雷達(dá)探測(cè)到的第j個(gè)特征的位置坐標(biāo)，xv（t）為移動(dòng)機(jī)器人的位姿。輸出：路標(biāo)特征點(diǎn)與移動(dòng)機(jī)器人的距離r以及與機(jī)器人前進(jìn)方向的夾角θ。

3.1.1 算法性能的比較

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

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

式中，N為每次SL A M仿真中離散采樣點(diǎn)的數(shù)量；sk為移動(dòng)機(jī)器人的實(shí)際位姿；為移動(dòng)機(jī)器人的估計(jì)位姿。

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

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

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

圖3 不同粒子數(shù)下的3種算法R M SE

3.1.2 算法復(fù)雜度的對(duì)比

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

表1 3種算法復(fù)雜度對(duì)比表

表2 均方根誤差近似相等時(shí)的粒子數(shù)比較

表3 粒子數(shù)為100時(shí)均方根誤差和運(yùn)行時(shí)間對(duì)比表

3.2 基于標(biāo)準(zhǔn)數(shù)據(jù)集的實(shí)驗(yàn)仿真

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

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

利用“Car Park Dataset”數(shù)據(jù)集，對(duì)A FFastSL A M、U FastSL A M、FastSL A M 2.0算法進(jìn)行對(duì)比研究。仿真實(shí)驗(yàn)中，控制噪聲設(shè)為（0.7 m/s，7°）；觀測(cè)噪聲設(shè)為（0.1 m，1°）；粒子數(shù)設(shè)為50個(gè)。圖5表示提出的算法和其他兩種算法的仿真結(jié)果，可以看出，A FFastSL A M使得估計(jì)路徑與G PS產(chǎn)生的真實(shí)路徑吻合程度更高，相對(duì)應(yīng)的路標(biāo)估計(jì)也更準(zhǔn)確，因此A FFastSL A M性能要優(yōu)于U FastSL A M和FastSL A M 2.0。

圖5 “Car Park Dataset”數(shù)據(jù)集的SL A M實(shí)驗(yàn)結(jié)果

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

圖6 位姿估計(jì)誤差曲線

圖7 路標(biāo)估計(jì)誤差曲線

為了更準(zhǔn)確的評(píng)估提出算法的性能，基于該數(shù)據(jù)集，在粒子數(shù)均選取30個(gè)的前提下，對(duì)3種SL A M算法分別進(jìn)行10次蒙特卡羅仿真實(shí)驗(yàn)，并以10次實(shí)驗(yàn)的位姿估計(jì)和路標(biāo)估計(jì)的R M SE的均值和標(biāo)準(zhǔn)差作為評(píng)價(jià)標(biāo)準(zhǔn)，結(jié)果如圖8所示，圖中曲線表示R M SE均值，線棒表示R M SE標(biāo)準(zhǔn)差?？梢钥闯?，提出的算法得到的位姿估計(jì)和路標(biāo)估計(jì)的R M SE均值均低于U FastSL A M算法和FastSL A M2.0算法，這意味著提出的算法對(duì)移動(dòng)機(jī)器人位姿的估計(jì)精度和地圖構(gòu)建精度都要優(yōu)于其他兩種算法，而且從位姿估計(jì)和路標(biāo)估計(jì)的RS M E標(biāo)準(zhǔn)差變化趨勢(shì)中可以看出，提出算法的運(yùn)行穩(wěn)定性較強(qiáng)。

圖8 位姿估計(jì)和路標(biāo)估計(jì)的均方根誤差

4 結(jié) 論

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

［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］.機(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］.自動(dòng)化學(xué)報(bào)，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.（湯文俊，張國(guó)良，敬斌.移動(dòng)機(jī)器人FastSL A M算法的對(duì)比研究［J］.計(jì)算機(jī)工程與設(shè)計(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.（王宏健，王晶，劉振業(yè).基于迭代擴(kuò)展Kalman濾波建議分布和線性優(yōu)化重采樣的快速同步定位與構(gòu)圖［J］.電子與信息學(xué)報(bào)，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］.山東大學(xué)學(xué)報(bào)（工學(xué)版），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.（周東華，席裕庚，張鐘俊.非線性系統(tǒng)帶次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波［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.（楊柳慶，肖前貴，牛妍，等.基于漸消卡爾曼濾波器的定位系統(tǒng)設(shè)計(jì)［J］.南京航空航天大學(xué)學(xué)報(bào)，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.（徐定杰，賀瑞，沈鋒，等.基于新息協(xié)方差的自適應(yīng)漸消卡爾曼濾波器［J］.系統(tǒng)工程與電子技術(shù)，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.（宮軼松，歸慶明，李保利，等.自適應(yīng)漸消擴(kuò)展Kalman粒子濾波方法在組合導(dǎo)航中的應(yīng)用［J］.大地測(cè)量與地球動(dòng)力學(xué)，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無(wú)線通信中的應(yīng)用研究［D］.成都：電子科技大學(xué)，2008.）

［18］W ang H J，F(xiàn)u 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.（王宏健，傅桂霞，李娟，等.基于強(qiáng)跟蹤C(jī) K F的無(wú)人水下航行器SL A M［J］.儀器儀表學(xué)報(bào)，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.（高社生，薛麗，魏文輝.漸消自適應(yīng)Unscented粒子濾波及其在組合導(dǎo)航中的應(yīng)用［J］.西北工業(yè)大學(xué)學(xué)報(bào)，2012，30（1）：27-31.）

［20］W ang H，Ding J D，F(xiàn)ang 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.（王浩，丁家棟，方寶富，等.融合遺傳優(yōu)化的粒子濾波器算法［J］.計(jì)算機(jī)科學(xué)與探索，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ìn)重采樣的粒子濾波算法［J］.計(jì)算機(jī)應(yīng)用研究，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］.自動(dòng)化學(xué)報(bào)，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；網(wǎng)絡(luò)優(yōu)先出版日期：2015-09-17。

網(wǎng)絡(luò)優(yōu)先出版地址：http：∥w w w.cnki.net/kcms/detail/11.2422.T N.20150917.1659.004.html

北京市教委基金項(xiàng)目（JJ002790200802）資助課題

劉 丹（1990-），女，博士研究生，主要研究方向?yàn)橹悄苘囕v的同時(shí)定位與建圖。

E-mail：danaliu@yeah.net

段建民（1959-），男，教授，博士研究生導(dǎo)師，主要研究方向?yàn)檐囕v環(huán)境識(shí)別與自動(dòng)駕駛技術(shù)、網(wǎng)絡(luò)化測(cè)控系統(tǒng)與現(xiàn)場(chǎng)總線技術(shù)、嵌入式汽車電子控制技術(shù)。

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

于宏嘯（1986-），男，博士研究生，主要研究方向?yàn)橹悄苘囕v橫縱向控制。

E-mail：yhxiao321@gmail.com