基于自適應(yīng)重采樣的同步定位與地圖構(gòu)建

曲麗萍，王宏健，邊信黔

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

0 引言

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

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

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

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

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

1 移動(dòng)機(jī)器人SLAM算法

1.1 移動(dòng)機(jī)器人SLAM問(wèn)題數(shù)學(xué)描述

移動(dòng)機(jī)器人SLA M問(wèn)題描述如圖1所示。

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

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

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

1.2 基于擴(kuò)展卡爾曼濾波 （EKF）的移動(dòng)機(jī)器人SLAM算法

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

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

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

1.2.2 基于擴(kuò)展卡爾曼濾波的移動(dòng)機(jī)器人SLA M算法

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

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

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

1.3 Rao-Blackwellise粒子濾波器

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

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

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

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

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

1.4 Fast SLAM算法的重采樣

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

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

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

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

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

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

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

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

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

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

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

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

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

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

1）移動(dòng)機(jī)器人位姿預(yù)測(cè)

根據(jù)控制向量uk、機(jī)器人運(yùn)動(dòng)模型，預(yù)測(cè)機(jī)器人k＋1時(shí)刻的位姿分布，即計(jì)算各個(gè)粒子的位姿向量

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

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

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

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

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

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

5）機(jī)器人路徑估計(jì)

采用Rao-Black wellise粒子濾波器估計(jì)機(jī)器人的路徑，獲取k＋1時(shí)刻機(jī)器人路徑后驗(yàn)概率分布

6）更新地圖估計(jì)

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

3 仿真實(shí)驗(yàn)及結(jié)果

3.1 仿真設(shè)計(jì)

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

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

3.2 仿真實(shí)驗(yàn)

3.2.1 本文運(yùn)動(dòng)模型

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

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

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

3.2.2 本文觀測(cè)模型

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

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

3.3 仿真結(jié)果

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

圖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)誤差對(duì)比圖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)用算法的機(jī)器人路徑X向，Y向及方向角誤差對(duì)比圖Fig.5 Vehicle error result f or si mulators based on EKF and Based on Adaptive Resample Fast SLA M

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

4 結(jié)論

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

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

［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)化的移動(dòng)機(jī)器人SLAM 方法［J］.機(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］鄒智榮，蔡自興，陳白帆，等.移動(dòng)機(jī)器人SLA M中一種混合數(shù)據(jù)關(guān)聯(lián)方法［J］.小型微型計(jì)算機(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］張海強(qiáng)，竇麗華，陳杰，等.典型場(chǎng)景下EKF-SLA M 估計(jì)一致性分析［J］.北京理工大學(xué)學(xué)報(bào)，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］周武，趙春霞，沈亞強(qiáng)，等.基于全局觀測(cè)地圖模型的SLA M 研究［J］.機(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］厲茂海，洪炳熔，羅榮華.用改進(jìn)的 Rao-Black wellized粒子濾波器實(shí)現(xiàn)移動(dòng)機(jī)器人同時(shí)定位和地圖［J］.吉林大學(xué)學(xué)報(bào)（工學(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］夏益民，楊宜民，一種利用模糊邏輯改進(jìn)Fast SLA M 2.0的方法［J］.計(jì)算機(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.