孫 昊，周 陽(yáng)，李麗娜

(遼寧大學(xué) 物理學(xué)院，遼寧 沈陽(yáng) 110036)

隨著機(jī)器人技術(shù)成熟，移動(dòng)機(jī)器人在家政服務(wù)，無(wú)人駕駛，監(jiān)控跟蹤等方面發(fā)揮出越來(lái)越重要的作用。移動(dòng)機(jī)器在固定區(qū)域內(nèi)的自我定位和環(huán)境構(gòu)建成為機(jī)器人領(lǐng)域的關(guān)鍵技術(shù)?；诹Ｗ訛V波〔1-2〕的Slam〔3〕算法證明移動(dòng)機(jī)器根據(jù)周圍環(huán)境自我定位和映射周圍環(huán)境信息在理論上可行，但存在著兩個(gè)致命缺陷，一是建圖對(duì)機(jī)器人位姿有較高的要求〔4〕，二是頻繁的重采樣會(huì)導(dǎo)致粒子耗散。GMapping正是針對(duì)這兩處缺陷，提出了具有針對(duì)性的解決方案。改進(jìn)一：改善粒子采樣選擇；改進(jìn)二：選擇性重采樣。本文通過(guò)改進(jìn)的GMapping構(gòu)建了室內(nèi)全局地圖。

1 粒子濾波理論描述

1.1 粒子濾波原理

在把粒子濾波原理應(yīng)用到slam問(wèn)題中時(shí)，一個(gè)粒子需要同時(shí)保存所有歷史時(shí)刻的機(jī)器人位姿和整個(gè)地圖信息。粒子濾波是貝葉斯濾波〔5〕在特殊條件下一種特殊方式。

根據(jù)條件貝葉斯法則，有：

p(x1：t,m|z1：t，u1：t-1) =p(m|x1：t，z1：t)·

p(x1：t|z1：t，u1：t-1)

(1)

這一分解可看成把SLAM分解為定位與建圖兩部分，該方法稱為粒子濾波方法(RBPF)。

1.2 粒子濾波缺陷

粒子濾波(RBPF)為slam提供了理論的支撐，在實(shí)踐上遇到兩個(gè)主要的問(wèn)題：

(1)通過(guò)機(jī)器人運(yùn)動(dòng)模型和特征點(diǎn)法采樣局部關(guān)鍵幀，可以得到非常多的粒子。而這些粒子分布又很分散，與真實(shí)分布相差過(guò)大。要想提高真實(shí)分布附近的粒子數(shù)，就一定要整體增加粒子數(shù)，而運(yùn)算過(guò)程中涉及到求矩陣逆，粒子數(shù)越多計(jì)算量指數(shù)倍增大。(2)在機(jī)器人的行走過(guò)程屢次采樣會(huì)引起粒子耗散。各個(gè)粒子信息含有機(jī)器人走過(guò)路徑上大部分的位姿和整個(gè)地圖，因重采樣依賴的權(quán)重更多取決于當(dāng)前觀測(cè)，而不是某次觀測(cè)在幾分鐘前。本文改進(jìn)的GMapping算法正是能夠有效解決這兩種問(wèn)題，為粒子濾波在slam問(wèn)題上的應(yīng)用提供了有效的改進(jìn)辦法。

2 粒子濾波優(yōu)化

2.1 改進(jìn)一：改善粒子采樣選擇

大部分粒子不能真實(shí)反映位姿信息是因?yàn)榱Ｗ訛V波采用加速度計(jì)運(yùn)動(dòng)模型作為提議分布。和加速度計(jì)運(yùn)動(dòng)模型不同，激光雷達(dá)觀測(cè)模型使用濾波器搭配直接法選擇局部關(guān)鍵幀。關(guān)鍵幀上采樣得到的粒子分布在一個(gè)相對(duì)集中的范圍，如圖1所示。

圖1 粒子采樣分布位置圖

具體的方法是先讓加速度計(jì)給出位姿估計(jì)，激光雷達(dá)觀測(cè)模型給出觀測(cè)，再以該觀測(cè)為初值進(jìn)行特征匹配。在特征點(diǎn)匹配過(guò)程中，文章在點(diǎn)云匹配過(guò)程中同時(shí)計(jì)算各維護(hù)點(diǎn)到點(diǎn)之間的距離和點(diǎn)到線之間的距離〔6〕，只有當(dāng)兩種距離都符合設(shè)定的條件，才判定為激光點(diǎn)云匹配成功。當(dāng)機(jī)器人運(yùn)動(dòng)回之前經(jīng)過(guò)的位置并且方向和角度和之前很像，這種方法增強(qiáng)了回環(huán)的可靠性。由于運(yùn)用兩種方法進(jìn)行匹配，如果按照傳統(tǒng)的最小二乘法計(jì)算歐式距離最小值，計(jì)算開(kāi)支非常大,建圖和定位實(shí)時(shí)性會(huì)下降。所以特征點(diǎn)之間距離計(jì)算本文采用累加曼哈頓距離和切比雪夫距離的線性相加來(lái)取代歐式距離，減小了cpu的計(jì)算量，提高了運(yùn)行速度和實(shí)時(shí)性。掃描匹配之后，本文就找到了L位置的突起區(qū)域，接下來(lái)需要在L中隨機(jī)抽樣采集N個(gè)點(diǎn)。依據(jù)這N個(gè)點(diǎn)的加速度計(jì)估計(jì)模型和激光雷達(dá)觀測(cè)模型確定該突起區(qū)域?qū)?yīng)的高斯分布的均值和方差，如下式所示：

(2)

(3)

(4)

這樣就獲得了最佳估計(jì)，采樣得到新的粒子繼續(xù)從該突起的高斯分布中抽樣采集得到。

接著需要對(duì)于每個(gè)粒子計(jì)算一個(gè)權(quán)重，來(lái)確定粒子的可信度大小，供接下來(lái)的重采樣使用，如公式(5)所示：

(5)

2.2 改進(jìn)二：選擇性重新采樣

如果頻繁進(jìn)行采樣，不但粒子的豐富性會(huì)消失，而且存儲(chǔ)器中粒子和機(jī)器人位姿也會(huì)變得更加單調(diào)，所以應(yīng)限制重采樣次數(shù)來(lái)避免粒子耗散。

一個(gè)重要的問(wèn)題是何時(shí)進(jìn)行重新采樣。應(yīng)該清楚的知道，只有檢測(cè)到機(jī)器人回到之前的位置后并且方向角度和之前相似才會(huì)進(jìn)行回環(huán)，回環(huán)時(shí)計(jì)算誤差大小，進(jìn)而為不同的粒子傳遞不同的權(quán)值。準(zhǔn)確粒子傳遞大的權(quán)值而誤差大粒子傳遞較小的權(quán)值，只有它們的差有質(zhì)的變化，這時(shí)再重新采樣才能夠起到好的效果。

GMapping不存在回環(huán)檢測(cè)〔7〕，要想知道回環(huán)是不是發(fā)生，需要引入新的計(jì)算方法，本文給出了有效地實(shí)現(xiàn)檢測(cè)的方法。下面通過(guò)引入公式計(jì)算粒子權(quán)重的差距大小。

(6)

Neff值越大，粒子被傳遞權(quán)重之間相減相應(yīng)就越小。當(dāng)Neff很小，就說(shuō)明粒子準(zhǔn)確性差距很大，有較少的粒子能夠真實(shí)反映現(xiàn)實(shí)空間，較多的粒子不能反映真實(shí)值。當(dāng)準(zhǔn)確表示真實(shí)值的粒子較少，這時(shí)就需要引入新的粒子。在這個(gè)時(shí)間點(diǎn)，正需要做重新采樣。所謂重新采樣，即以wi的概率來(lái)接受這個(gè)粒子，生成一個(gè)隨機(jī)數(shù)，依據(jù)所在區(qū)間決定wi及重復(fù)數(shù)次。為減少重采樣次數(shù)，用一個(gè)量Neff來(lái)表示當(dāng)前估計(jì)和真實(shí)分布的差異性: 如果Neff較小，表明差異性較大，需要進(jìn)行重采樣；如果Neff較大，表明差異性較小，暫停重新采樣。如此可以極大的減少重新采樣的次數(shù)，緩解粒子耗散問(wèn)題。

圖2 隨機(jī)重采樣圖

3 GMapping從理論到實(shí)踐

改進(jìn)的GMapping流程圖如圖3所示，采樣之后計(jì)算高斯分布，再?gòu)母咚狗植贾兄夭蓸硬⒂?jì)算權(quán)重，得到位姿之后進(jìn)行地圖更新，之后不斷重復(fù)這個(gè)過(guò)程直到得到全局的地圖。

圖3 改進(jìn)的GMapping程序流程圖

圖4是在實(shí)驗(yàn)室中基于改進(jìn)的GMapping算法構(gòu)建的地圖，對(duì)比圖5一般粒子濾波器構(gòu)建的地圖，圖中標(biāo)記2的位置可以看出改進(jìn)的GMapping算法在相同條件下建圖連續(xù)性更好，圖中標(biāo)記1的位置可以看出改進(jìn)之后算法的誤差更小，更加準(zhǔn)確的構(gòu)建了實(shí)驗(yàn)室的環(huán)境。對(duì)比圖6實(shí)驗(yàn)室實(shí)景可以看出，地圖可以準(zhǔn)確的表達(dá)實(shí)驗(yàn)室的真實(shí)結(jié)構(gòu)。

圖5 一般粒子濾波器構(gòu)建地圖

圖6 實(shí)驗(yàn)室實(shí)景圖

4 結(jié)語(yǔ)

針對(duì)辦公室、實(shí)驗(yàn)室或工廠等小型靜態(tài)環(huán)境，改進(jìn)的GMapping算法改善了粒子采樣的選擇，通過(guò)設(shè)定嚴(yán)格的激光點(diǎn)云匹配策略，更精確地估計(jì)了機(jī)器人軌跡和室內(nèi)環(huán)境。為了降低計(jì)算量，本文特征點(diǎn)之間距離計(jì)算采用曼哈頓距離和切比雪夫距離的線性相加來(lái)取代歐式距離。為了限制重采樣次數(shù)通過(guò)設(shè)定閾值選擇性重新采樣，改進(jìn)的GMapping 雖然好用但只限于實(shí)驗(yàn)室或辦公環(huán)境等小場(chǎng)景，在大場(chǎng)景下進(jìn)行全局地圖構(gòu)建〔8〕還存在誤差累積等問(wèn)題，未來(lái)還需要在大場(chǎng)景下繼續(xù)優(yōu)化。