曲麗萍，王宏健，邊信黔

（1.哈爾濱工程大學(xué)自動化學(xué)院，黑龍江 哈爾濱 150001；2.北華大學(xué)電氣信息工程學(xué)院，吉林 吉林 132021）

0 引言

移動機器人同步定位與地圖構(gòu)建（Si multaneous Localization And Mapping，簡稱SLA M），是指機器人在自身位置不確定條件下，在部分已知或完全未知環(huán)境中運動時，根據(jù)位置估計和傳感器數(shù)據(jù)進行自身定位、同時建造增量式地圖［1－2］。

SLAM問題研究中，應(yīng)用較多的是擴展卡爾曼濾波（EKF）SLAM和粒子濾波SLAM解決方案。EKF利用非線性函數(shù)泰勒展開式的一階偏導(dǎo)部分（忽略高階項），常常導(dǎo)致在狀態(tài)的后驗分布的估計上產(chǎn)生較大的誤差，影響濾波算法的性能。另外，EKF算法要求狀態(tài)的概率密度函數(shù)滿足高斯分布，但實際系統(tǒng)往往是非高斯、非線性的（即系統(tǒng)狀態(tài)／量測模型是非線性的，模型／量測噪聲不服從高斯分布）。

對于非線性、非高斯系統(tǒng)的狀態(tài)估計問題，比較有發(fā)展前途的是基于序貫蒙特卡羅方法和SIS的粒子濾波方法。該方法對系統(tǒng)噪聲沒有任何限制，它通過預(yù)測和更新來自于系統(tǒng)概率密度函數(shù)的采樣集來近似非線性系統(tǒng)的隨機貝葉斯估計。粒子濾波方法的優(yōu)點是采樣集中在高概率的區(qū)域，采樣計算的過程也很簡單，是一種非線性、非高斯系統(tǒng)的貝葉斯估計［2］。

但是，粒子濾波SLAM在進行機器人路徑估計和路標(biāo)位置估計過程中，經(jīng)過幾次迭代，粒子的權(quán)值會集中到少數(shù)粒子上，使得大量的計算工作都被浪費在用來更新那些對估計幾乎不起作用的粒子上，這就是粒子退化問題，粒子濾波SLA M中的最經(jīng)典的算法Fast SLA M也不例外。解決粒子退化問題的主要方法就是重采樣。過頻地重采樣將增加計算負(fù)擔(dān)，過少地重采樣又會導(dǎo)致效率降低。

針對粒子退化和重采樣問題，本文對最具代表性的Fast SLA M算法進行了改進，提出了基于自適應(yīng)重采樣的Fast SLA M方法。

1 移動機器人SLAM算法

1.1 移動機器人SLAM問題數(shù)學(xué)描述

移動機器人SLA M問題描述如圖1所示。

圖1 移動機器人SLAM系統(tǒng)Fig.1 State of mobile robot SLA M system

假設(shè)機器人在未知環(huán)境中移動，同時使用自身攜帶的傳感器探測外部未知的路標(biāo)信息和獲取自身的位姿信息（即機器人的位置和方向）。Xt表示t時刻移動機器人的位姿狀態(tài)向量，mk表示第k個路標(biāo)的位置狀態(tài)向量，ut為機器人從t－1時刻到t時刻的輸入控制向量，zt為t時刻傳感器觀測向量，t時刻機器人的系統(tǒng)狀態(tài)（包含t時刻機器人位姿和路標(biāo)位置）記為st，即st＝ ｛Xt，mt｝。從概率學(xué)來看，假定移動機器人運動到當(dāng)前位置并進行觀測，則系統(tǒng)當(dāng)前狀態(tài)與之前的系統(tǒng)狀態(tài)、觀測信息以及輸入有關(guān)，即 p （st｜s0：t－1，z0：t，u1：t）［3］。假設(shè)系統(tǒng)當(dāng)前的狀態(tài)僅與前一時刻的系統(tǒng)狀態(tài)和當(dāng)前的輸入有關(guān)，即前一時刻的系統(tǒng)狀態(tài)已經(jīng)包含了之前的系統(tǒng)狀態(tài)、觀測信息和輸入，則當(dāng)前系統(tǒng)狀態(tài)的分布概率為：

在此系統(tǒng)狀態(tài)估計上獲得的觀測信息的估計為

1.2 基于擴展卡爾曼濾波 （EKF）的移動機器人SLAM算法

1.2.1 聯(lián)合狀態(tài)向量動態(tài)模型

SLA M問題中的狀態(tài)向量Xk，包括機器人的位姿Xrk和環(huán)境特征的位置Xmk，由于特征地圖是靜止?fàn)顟B(tài)，k－1和k時刻特征地圖的狀態(tài)相等，故基于擴展卡爾曼濾波的移動機器人SLA M算法的聯(lián)合狀態(tài)向量的動態(tài)模型為

式（3）中，F(xiàn)（·）為聯(lián)合狀態(tài)轉(zhuǎn)移函數(shù)，f（·）為移動機器人運動模型［4］。

1.2.2 基于擴展卡爾曼濾波的移動機器人SLA M算法

EKF是非線性系統(tǒng)均方差估計方法，假設(shè)狀態(tài)向量Xk，觀測量序列z1：k＝ ［z1，z2，…，zk］T，則狀態(tài)向量的最小均方差估計為：

式（4）、式（5）中，所處理的變量假設(shè)服從高斯分布，用均值和方差表示為：

過程噪聲wk和外部傳感器的觀測噪聲nk認(rèn)為是均值為零、方差分別為Qk及Rk的高斯白噪聲。

1.3 Rao-Blackwellise粒子濾波器

Mur phy等人對動態(tài)貝葉斯網(wǎng)絡(luò)（Dynamic Bayesian Net wor ks）進行分析得出結(jié)論：若機器人的路徑估計己知，則路標(biāo)估計 （mi，mj，i≠j）之間相互獨立。由貝葉斯公式及特征估計之間的獨立性假設(shè)，對后驗概率分布p（x1：k＋1，m｜z1：k＋1，u0：k，x0）分解：

式（10）描述了Rao-Black wellise粒子濾波器思想，即首先將機器人路徑和地圖的聯(lián)合估計問題分解為機器人路徑估計和地圖估計兩部分，然后再將地圖估計再分解為M（特征的數(shù)目）個相互獨立的特征估計。

在粒子濾波器中，變量x的概率分布p（x）用帶權(quán)重的粒子集表示，即

式（11）中，xi為第i個粒子，wi為第i個粒子的歸一

式（12）中，δ（·）是迪拉克函數(shù)，xi1：k＋1表示第i個粒子的機器人路徑，wik＋1是第i個粒子的權(quán)值。如果可以直接從p（x1：k＋1｜z1：k＋1，u0：k，x0）中產(chǎn)生粒子，則所有粒子皆賦予相同權(quán)值1／N。但一般來講，p（x1：k＋1｜z1：k＋1，u0：k，x0）是未知的，所以假定一個近似后驗概率分布的提議分布q（x1：k＋1｜z1：k＋1，u0：k，x0），然后從這個提議分布產(chǎn)生粒子，并按照下式賦權(quán)值：

1.4 Fast SLAM算法的重采樣

2 基于自適應(yīng)重采樣的Fast SLAM算法

目前廣泛使用的Fast SLA M算法，是采用Rao－Black wellise粒子濾波器估計機器人路徑，采用擴展卡爾曼濾波器估計地圖。Fast SLA M在系統(tǒng)狀態(tài)估計過程中，不可避免地會出現(xiàn)粒子退化問題。解決粒子退化問題主要方法是重采樣。過頻地重采樣將增加計算負(fù)擔(dān)，過少地重采樣又會導(dǎo)致效率降低。因此，尋求一種適當(dāng)?shù)闹夭蓸臃椒?，對于Fast SLA M的應(yīng)用研究至關(guān)重要。

2.1 自適應(yīng)重采樣方法

Fast SLA M算法的基礎(chǔ)是粒子濾波，粒子濾波的基本思想是利用序列重要性采樣（SIS）的概念近似，用一系列離散的帶權(quán)重的隨機樣本近似相應(yīng)的概率密度函數(shù)［6］。當(dāng)用重要性函數(shù)替代后驗概率分布作為采樣函數(shù)時，理想情況是重要性函數(shù)非常接近后驗概率分布，也就是希望重要性函數(shù)的方差基本為零。即

但是由于粒子濾波選擇先驗概率密度作為重要密度函數(shù)，沒有考慮當(dāng)前的量測值，從重要性概率密度中取樣得到的樣本與從真實后驗概率密度采樣得到的樣本有很大偏差，尤其當(dāng)似然函數(shù)位于系統(tǒng)狀態(tài)轉(zhuǎn)移概率密度的尾部或似然函數(shù)呈尖峰狀態(tài)時，這種偏差更明顯。因此，重要性權(quán)重的方差隨著時間而隨機遞增，使得粒子的權(quán)重集中到少數(shù)粒子上，出現(xiàn)了粒子退化問題［7］。

增加粒子數(shù)目NS可以解決退化問題，但會使得計算量上升，影響算法的實時性。普遍采用的方法是重要性樣本的重采樣。重采樣是一個減小無效樣本，增加有效樣本的方法，即舍棄“壞”的樣本（具有小的重要性權(quán)值）、復(fù)制“好”的樣本（具有較大的重要性權(quán)值）以適應(yīng)系統(tǒng)的動態(tài)變化。但過頻地重采樣將增加計算負(fù)擔(dān)，過少地重采樣又會導(dǎo)致效率降低［8］。

本文采用的是自適應(yīng)重采樣方法：

1）計算有效抽樣尺度Neff，確定粒子退化程度。

Neff定義為 Neff＝ NS／（1＋varq（·｜z1：k）（））≤NS，但實際中無法按照上式確切計算Neff數(shù)值，所以通過式（15）近似估計：

Neff越小，意味著退化現(xiàn)象越嚴(yán)重。

2）設(shè)定有效樣本數(shù)閾值，實施自適應(yīng)重采樣

設(shè)定有效樣本數(shù)Nthreshold＝0.75 Nparticle作為閾值（其中Nparticle為粒子個數(shù)），當(dāng)Neff＜Nthreshold時，進行重采樣，這樣就實現(xiàn)了自適應(yīng)地根據(jù)樣本情況決定是否進行重采樣，在一定程度上降低了算法復(fù)雜度。

自適應(yīng)重采樣方法，保證了只在必要時才進行重采樣，有效減少了重采樣次數(shù)，提高了Fast SLA M改進算法的魯棒性。

2.2 基于自適應(yīng)重采樣的Fast SLAM算法

1）移動機器人位姿預(yù)測

根據(jù)控制向量uk、機器人運動模型，預(yù)測機器人k＋1時刻的位姿分布，即計算各個粒子的位姿向量

2）特征地圖路標(biāo)數(shù)據(jù)關(guān)聯(lián)

采用極大化觀測概率函數(shù)的數(shù)據(jù)關(guān)聯(lián)方法，將可觀測到的觀測信息（激光測距傳感器可達到的范圍內(nèi)的路標(biāo)）和各個粒子k時刻的估計地圖依次進行數(shù)據(jù)關(guān)聯(lián)。

3）采用自適應(yīng)重采樣，提高計算精度和效率

按照式（15）計算有效粒子數(shù)Neff，當(dāng)Neff＜Nmin（Nmin取0.75 Nparticle，Nparticle為粒子個數(shù)），說明粒子退化嚴(yán)重，則轉(zhuǎn)入第4）步進行重采樣；否則直接轉(zhuǎn)至第5）步。

4）需要重采樣時，獲取提議分布并采樣

根據(jù)各個粒子的關(guān)聯(lián)觀測信息，采用EKF對粒子的先驗分布p（xk＋1｜，uk）進行觀測更新，計算各個粒子的位姿向量在k＋1時刻的濾波值和方差，得到各個粒子的提議分布q（x1：k＋1｜，zk＋1，uk）。

5）機器人路徑估計

采用Rao-Black wellise粒子濾波器估計機器人的路徑，獲取k＋1時刻機器人路徑后驗概率分布

6）更新地圖估計

根據(jù)各個粒子的關(guān)聯(lián)觀測信息，采用EKF更新各個粒子k＋1時刻的關(guān)聯(lián)特征估計，計算各個特征的估計均值和方差，對于沒有和地圖中的已有特征關(guān)聯(lián)上的觀測信息，則將對應(yīng)的觀測特征作為新特征加入地圖。

3 仿真實驗及結(jié)果

3.1 仿真設(shè)計

仿真環(huán)境：在200～250 m的仿真環(huán)境中設(shè)置機器人運動路徑和135個路標(biāo)，機器人的運動路徑通過設(shè)定關(guān)鍵點人為設(shè)定，機器人從坐標(biāo)（0，0）開始逆時針運行，機器人運動速度為3 m／s，最大轉(zhuǎn)角30°，控制信號時間間隔為0.025 s，速度噪聲為0.3 m／s，觀測最遠(yuǎn)距離30 m，觀測的間隔時間為0.2 s，觀測距離噪聲為0.1 m，觀測角度噪聲為1。

仿真任務(wù)：通過EKF-SLAM 及Fast SLA M2.0編程運行，估計機器人運動軌跡路徑及周圍路標(biāo)位置。

3.2 仿真實驗

3.2.1 本文運動模型

考慮到移動機器人是通過控制其左、右輪的正反轉(zhuǎn)實現(xiàn)前進、轉(zhuǎn)向等動作，所以當(dāng)將一個控制量ut（前向速度和角速度）施加到機器人時，機器人運動的預(yù)測狀態(tài)模型為［9］：

式（16）中，（xr（t），yr（t））和φr（t）為t時刻機器人的位置與方向角，v為機器人的運動速度，γ為角速度，wxyφ∈N（0，σxyφ）為均值為零的加性高斯噪聲項，用于描述各種未知因素對機器人運動產(chǎn)生的影響。

假設(shè)環(huán)境路標(biāo)是靜止的，故環(huán)境路標(biāo)的狀態(tài)各時刻保持不變，即

3.2.2 本文觀測模型

考慮到機器人利用自身攜帶傳感器探測路標(biāo)，得到路標(biāo)的距離和方向角，故觀測模型可表示為［5］：

式（18）中，（xr，yr）為機器人的位置坐標(biāo)，（xθi，yθr）為路標(biāo)i的位置坐標(biāo)，wR和wθ是測量距離和角度的噪聲序列，且符合N（0，σR）和N（0，σθ）的高斯分布。

3.3 仿真結(jié)果

針對上述仿真實驗，選取Matlab7.0運行環(huán)境，分別采用本文提出的兩種算法，對機器人運動路徑和環(huán)境路標(biāo)進行了仿真估計，仿真結(jié)果如圖2—圖5所示。圖2和圖3為兩種算法仿真結(jié)果，細(xì)實線為移動機器人設(shè)定路徑，粗實線為仿真估計路徑。圖4為兩種算法路標(biāo)位置估計誤差曲線。圖5為兩種算法機器人路徑估計誤差曲線。

圖2 基于EKF的SLAM應(yīng)用算法的仿真結(jié)果Fig.2 Si mulation result f or si mulators based on EKF

圖3 基于自適應(yīng)重采樣Fast SLAM的仿真結(jié)果Fig.3 Si mulation result for si mulators based on adaptive resample fast SLAM

圖4 基于EKF和基于Fast SLA M的SLA M應(yīng)用算法的路標(biāo)誤差對比圖Fig.4 Land mar k Error Result for Si mulators Based on EKF and Based on Adaptive Resample Fast SLA M

圖5 基于EKF和基于自適應(yīng)重采樣Fast SLA M的SLAM應(yīng)用算法的機器人路徑X向，Y向及方向角誤差對比圖Fig.5 Vehicle error result f or si mulators based on EKF and Based on Adaptive Resample Fast SLA M

由圖2可以看出，在路標(biāo)位置估計方面，基于自適應(yīng)重采樣Fast SLAM算法較基于EKF的SLA M應(yīng)用算法，估計的路標(biāo)位置與設(shè)定的路標(biāo)點重合的程度更好一些，即路標(biāo)估計精度更高；在機器人運行路徑估計方面，前者較后者誤差更小一些，具有更高的精度。

4 結(jié)論

本文提出了基于自適應(yīng)重采樣Fast SLA M算法，該算法首先計算粒子集的有效樣本數(shù)，確定粒子退化程度。然后設(shè)定有效樣本閾值，當(dāng)有效樣本數(shù)小于閾值時則進行重采樣。

仿真表明，EKF-SLA M和基于自適應(yīng)重采樣Fast SLA M兩種算法都能完成機器人運動路徑和路標(biāo)地圖的估計，但由于后者能夠根據(jù)系統(tǒng)觀測量信息獲取更加符合的提議分布，且重采樣的效率更高，算法魯棒性更好。不但具有非線性、非高斯的普遍適用性，在機器人路徑和路標(biāo)位置的估計上，也具有更高的精度。

［1］Durrant-Whyte H，Bailey T.Simultaneous localization and mapping（SLA M）：part I essential algorith ms［J］.IEEE Robotics and Automation Magazine，2006，13（2）：99-108.

［2］Durrant-Whyte H，Bailey T.Si multaneous localization and mapping（SLA M）：part II state of the art［J］.IEEE Robotics and Auto mation Magazine，2006，13（3）：108-117.

［3］陳白帆，蔡自興，袁成.基于粒子群優(yōu)化的移動機器人SLAM 方法［J］.機器人，2009，31（6）：513-517.CHEN Baifan，CAI Zixing，YUAN Cheng.Mobile robot slam method based on particle swar m opti mization［J］.Robot，2009，31（6）：513-517.

［4］Wang Y M，Yang JB，Xu DL.A preference aggregation method through the esti mation of utility intervals［J］.Computers and operations research，2005（32）：2 027-2 049.

［5］鄒智榮，蔡自興，陳白帆，等.移動機器人SLA M中一種混合數(shù)據(jù)關(guān)聯(lián)方法［J］.小型微型計算機系統(tǒng)，2011，32，（7）：1 341-1 343.ZOU Zhirong，CAI Zixing，CHEN Baifan.Hybrid appr oach of data association in mobile robot SLA M［J］.Journal of Chinese Computer Systems，2011，32，（7）：1 341-1 343.

［6］張海強，竇麗華，陳杰，等.典型場景下EKF-SLA M 估計一致性分析［J］.北京理工大學(xué)學(xué)報，2011，31（10）：1 194-1 202.ZHANG Haiqiang，DOU Lihua，CHEN Jie，et al.Consistency analysis of EKF-SLA M for a basic scenario［J］.Journal of Beijing Institute of Technology，2011，31（10）：1 194-1 202.

［7］周武，趙春霞，沈亞強，等.基于全局觀測地圖模型的SLA M 研究［J］.機器人，2010，32（5）：627-654.ZHOU Wu，ZHAO Chunxia，SHEN Yaqiang，et al.SLA M research based on global observation map model［J］.Robot，2010，32（5）：627-654.

［8］厲茂海，洪炳熔，羅榮華.用改進的 Rao-Black wellized粒子濾波器實現(xiàn)移動機器人同時定位和地圖［J］.吉林大學(xué)學(xué)報（工學(xué)版），2007，37（2）：401-406.LI Maohai，HONG Bingrong，LUO Ronghua.Impr oved rao-black wellized particle filters f or mobile robot si multaneous localization and mapping［J］.Jour nal of Jilin University（Engineering and Technology Edition），2007，37（2）：401-406.

［9］夏益民，楊宜民，一種利用模糊邏輯改進Fast SLA M 2.0的方法［J］.計算機工程與應(yīng)用，2010，46（33）：233-238.XIA Yi min，YANG Yi min.Improved Fast SLA M 2.0 algorit h m based on f uzzy logic［J］.Co mputer Engineering and Applications，2010，46（33）：233-238.