一種改進(jìn)的移動(dòng)機(jī)器人自定位算法

張 琪，蒙禾青，周 嶸，包琨超，王宗玉

（1.武漢理工大學(xué) 信息工程學(xué)院，湖北 武漢 430070；2.西安交通大學(xué)軟件學(xué)院，陜西 西安 710049）

自定位是移動(dòng)機(jī)器人的核心問(wèn)題。通過(guò)自定位，移動(dòng)機(jī)器人可以在特定環(huán)境中完成人類(lèi)指定的各種任務(wù)，例如傳遞指令、取送物品、危險(xiǎn)物排除、管理辦公室或家居和機(jī)器人足球等［1－2］，因此這項(xiàng)研究具有重要價(jià)值。移動(dòng)機(jī)器人的定位，需要兩個(gè)條件:一是感知內(nèi)、外環(huán)境的傳感器，例如筆者采用經(jīng)典的測(cè)距傳感器來(lái)獲取環(huán)境信息；二是關(guān)于行走環(huán)境的地圖。地圖的獲取有兩種方式，一是機(jī)器人在行走過(guò)程中根據(jù)實(shí)際量測(cè)來(lái)自動(dòng)繪制環(huán)境地圖；二是把已經(jīng)標(biāo)定好的環(huán)境地圖存放在移動(dòng)機(jī)器人的芯片中。為了簡(jiǎn)化問(wèn)題，筆者采用第二種方式，即地圖在自定位之前已經(jīng)獲得。地圖采用離散柵格形式，如圖1所示。其中黑色柵格表示障礙物，用數(shù)字0表示，而白色部分表示機(jī)器人可以通過(guò)的柵格，用數(shù)字1表示。

圖1 離散柵格地圖示例

地圖建立以后，機(jī)器人通過(guò)移動(dòng)，不斷獲取與墻壁或周邊物體的距離信息，經(jīng)過(guò)與地圖的匹配，逐步判斷機(jī)器人在當(dāng)前時(shí)刻的位置和方向。由于機(jī)器人定位是一個(gè)非線性、含有較多不確定性因素的問(wèn)題，因此當(dāng)前主要采用基于貝葉斯濾波的概率推導(dǎo)方法，如卡爾曼濾波器、多峰值估計(jì)法、馬爾科夫定位和蒙特卡洛法等［3－4］。在實(shí)例方面，卡內(nèi)基梅隆大學(xué)與德國(guó)伯恩大學(xué)基于蒙特卡洛方法，合作研發(fā)出博物館導(dǎo)游機(jī)器人Rhino及Minerva，可以從博物館中任意一個(gè)位置出發(fā)，較快地實(shí)現(xiàn)自定位［5］。

國(guó)內(nèi)對(duì)于自定位問(wèn)題的研究起步相對(duì)較晚，主要集中在近10年內(nèi)。如利用馬爾科夫算法對(duì)機(jī)器人進(jìn)行定位；利用改進(jìn)的自適應(yīng)粒子濾波算法，降低運(yùn)算復(fù)雜度，提升算法穩(wěn)健性；利用特征粒子提取思想，降低粒子數(shù)目；采用改進(jìn)的Hough密度譜和Fouder－Mellin變換方法來(lái)估計(jì)機(jī)器人的運(yùn)動(dòng)參數(shù)等［6－9］。

筆者將基于粒子濾波器原理，對(duì)已知地圖的輪式移動(dòng)機(jī)器人的自定位展開(kāi)研究。在經(jīng)典粒子濾波算法的基礎(chǔ)上，提出了兩個(gè)改進(jìn)策略，可在保證跟蹤精度的同時(shí)，顯著減少計(jì)算量:

（1）粒子初始化時(shí)，每個(gè)粒子的角度經(jīng)過(guò)一次匹配得到，可減少計(jì)算量。具體來(lái)說(shuō)，首先估計(jì)每個(gè)粒子在所有可能方向上與周?chē)h(huán)境的距離，然后將這些距離估計(jì)值與移動(dòng)機(jī)器人的測(cè)距傳感器測(cè)量值進(jìn)行匹配，找出最接近該測(cè)量值的角度，作為該粒子的初始角度。這樣不僅能減少自定位所需要的粒子數(shù)，而且能減少在計(jì)算移動(dòng)機(jī)器人角度方面的計(jì)算量。

（2）采用查表式方法估計(jì)距離，可提高定位效率。首先對(duì)環(huán)境地圖進(jìn)行離散化，其后針對(duì)地圖中每個(gè)小格，進(jìn)行360度等間隔離散，并計(jì)算各角度上該小格與周?chē)h(huán)境的距離，將其存為表格形式。當(dāng)機(jī)器人在環(huán)境中行走，并獲得量測(cè)距離時(shí)，可以直接調(diào)用上述表格文件，通過(guò)查表方法，判斷各個(gè)粒子在各方向上的概率，從而減少自定位的在線計(jì)算時(shí)間，增強(qiáng)機(jī)器人運(yùn)行及定位的實(shí)時(shí)性。

1 機(jī)器人運(yùn)動(dòng)模型

在對(duì)輪式機(jī)器人分析的過(guò)程中，常把機(jī)器人建模為輪子上的一個(gè)剛體，在水平面上運(yùn)動(dòng)。參見(jiàn)圖2，建立平面坐標(biāo)系XOY，則機(jī)器人的位置和姿態(tài)可以用如下?tīng)顟B(tài)向量表示:

式中:x，y為機(jī)器人在二維直角坐標(biāo)系中的位置坐標(biāo)；θ為機(jī)器人的朝向，即機(jī)器人運(yùn)動(dòng)正方向與X軸的夾角。

圖2 機(jī)器人狀態(tài)示意圖

假設(shè)在 t1時(shí)刻機(jī)器人處于狀態(tài) S1（x1，y1，θ1），若機(jī)器人左右輪都進(jìn)行勻速運(yùn)動(dòng)，經(jīng)過(guò)短暫時(shí)間 Δt之后，機(jī)器人處于狀態(tài) S2（x2，y2，θ2），那么這段時(shí)間內(nèi)機(jī)器人的運(yùn)動(dòng)可近似看作圓周運(yùn)動(dòng)，其運(yùn)動(dòng)模型如圖3所示。

圖3 運(yùn)動(dòng)模型

則當(dāng)前狀態(tài) S2（x2，y2，θ2）與上一狀態(tài) S1（x1，y1，θ1）之間的關(guān)系為:

式中:Δsr和Δsl分別為右輪和左輪一次行走的距離；b為機(jī)器人同軸兩個(gè)輪子間的距離；Δs和Δθ分別為機(jī)器人一次運(yùn)動(dòng)行走的距離和方向的變化，這兩個(gè)量即為機(jī)器人在自身局部坐標(biāo)系中的移動(dòng)和轉(zhuǎn)動(dòng)，式（2）即為局部坐標(biāo)系與全局坐標(biāo)系之間的轉(zhuǎn)換公式。

2 移動(dòng)機(jī)器人自定位設(shè)計(jì)

2.1 移動(dòng)機(jī)器人定位貝葉斯建模

移動(dòng)機(jī)器人定位原理圖如圖4所示?？梢圆捎酶怕蕡D來(lái)描述移動(dòng)機(jī)器人的運(yùn)動(dòng)過(guò)程，其中ut為作用于機(jī)器人的控制信息，pt為t時(shí)刻移動(dòng)機(jī)器人的狀態(tài)，ct為機(jī)器人在t時(shí)刻得到的傳感器距離信息。

圖4 移動(dòng)機(jī)器人定位原理圖

可以將機(jī)器人定位理解為貝葉斯濾波問(wèn)題。機(jī)器人依據(jù)獲得的量測(cè)信息，以一定的置信度（Belief）確定它的位置:

式（5）表示在已知0～t時(shí)刻的所有控制數(shù)據(jù)及觀測(cè)數(shù)據(jù) d0…t={ct，ut－1，ct－1，ut－2，…，c0}的情況下，機(jī)器人在t時(shí)刻位于狀態(tài)pt的概率。

結(jié)合貝葉斯公式變換可得到:

由于系統(tǒng)具有馬爾科夫性，且 P（ct|ut－1，ct－1，ut－2，…，c0）為一常數(shù)，因此式（6）可進(jìn)一步簡(jiǎn)化為:

通過(guò)對(duì)狀態(tài)空間離散化，則式（7）可以寫(xiě)為:

式中，E為離散后的狀態(tài)空間。

2.2 基于粒子濾波器的數(shù)值化實(shí)現(xiàn)

式（8）給出了離散狀態(tài)空間中的定位模型，由于各種先驗(yàn)概率、似然度函數(shù)往往不具備解析形式，可能為非線性、非高斯的概率，直接由式（8）來(lái)解析求解狀態(tài)是不可能的。

因此筆者采用經(jīng)典的粒子濾波器來(lái)數(shù)值化求解式（8）。粒子濾波器也稱(chēng)為序貫蒙特卡洛方法（sequential Monte Carlo，SMC），是一種在時(shí)間序列上的離散、非參數(shù)式的貝葉斯濾波方法，能夠處理各種非線性、非高斯、多模態(tài)問(wèn)題。

2.2.1 狀態(tài)方程及量測(cè)方程

機(jī)器人的定位問(wèn)題可以描述為:給定一組觀測(cè)向量Ct，估計(jì)機(jī)器人當(dāng)前所在狀態(tài)pt。其中:Ct={c0，c1，…，ct}為觀測(cè)序列；pt=［xt，yt，θt］T為機(jī)器人在t時(shí)刻的位置和朝向。

設(shè)移動(dòng)機(jī)器人狀態(tài)方程為:

式中:ut為機(jī)器人的運(yùn)動(dòng)控制輸入變量；wt為過(guò)程噪聲。函數(shù)f（x）由機(jī)器人的運(yùn)動(dòng)模型給定，即在式（2）的基礎(chǔ)上加上噪聲wt，則可得:

式中，ws和wθ分別為機(jī)器人的運(yùn)動(dòng)距離和偏轉(zhuǎn)角度的噪聲。

系統(tǒng)狀態(tài)量測(cè)方程為:

式中:M為環(huán)境地圖；vt為量測(cè)噪聲；pt的估計(jì)由求解Bel（pt）得到。

2.2.2 初始化粒子

移動(dòng)機(jī)器人自定位程序首先是粒子的初始化。初始的粒子服從均勻分布，即每個(gè)粒子出現(xiàn)在圖像中任意一個(gè)點(diǎn)的概率都是1/N，其中N為移動(dòng)機(jī)器人在地圖上可能出現(xiàn)的位置總數(shù)。

由式（1）可知，粒子的初始化除了要初始化粒子的位置之外，還要初始化粒子的朝向角度，即θ。因此需要大量的粒子來(lái)表示移動(dòng)機(jī)器人所有可能的位置和角度，計(jì)算量非常大。

假定θ值按均勻分布生成粒子，即θ～U（0，2π），那么當(dāng)某個(gè)粒子的坐標(biāo)與機(jī)器人的真實(shí)2D位置吻合時(shí)，該粒子隨機(jī)生成的θ與機(jī)器人真實(shí)的方向吻合的概率并不大，從而造成計(jì)算浪費(fèi)。

實(shí)際上，在已獲得距離量測(cè)的情況下，狀態(tài)向量中角度分量θ是不獨(dú)立的，其與其他兩個(gè)分量，即機(jī)器人在二維空間的坐標(biāo)x、y是相關(guān)的。因此可以把狀態(tài)變量分為兩個(gè)部分，則有式（12）:

根據(jù)式（12），首先基于均勻分布p（xt，yt）～U（M）對(duì)二維坐標(biāo)x，y進(jìn)行采樣（M為環(huán)境地圖），然后對(duì)于每一個(gè)粒子，求出該粒子在自己位置上匹配權(quán)重最大的角度，將該值作為θ的初始值，這與文獻(xiàn)［10］提出的方法有些類(lèi)似，都是把狀態(tài)向量分為幾個(gè)部分，將聯(lián)合概率分解為條件概率的乘積形式。而筆者的創(chuàng)新之處在于，結(jié)合坐標(biāo)分量和量測(cè)來(lái)直接估計(jì)每個(gè)粒子的角度分量。其優(yōu)點(diǎn)在于既能有效提高粒子的采樣質(zhì)量，又能極大地減少用于確定機(jī)器人朝向的粒子數(shù)量，并且在以后的定位計(jì)算中也不需要再考慮移動(dòng)機(jī)器人的θ值問(wèn)題，只需根據(jù)式（10）將機(jī)器人運(yùn)動(dòng)產(chǎn)生的θ值的變化量Δθ加到每個(gè)粒子的θ值即可，不用再計(jì)算移動(dòng)機(jī)器人的θ值。

2.2.3 粒子重采樣

在完成粒子的初始化之后，每個(gè)粒子的權(quán)重也在初始化θ值時(shí)賦給了每個(gè)粒子。此時(shí)就可以找出權(quán)重最大的粒子作為本次機(jī)器人位置的最佳估計(jì)。下一步的工作就是對(duì)粒子進(jìn)行重采樣，為下一次估計(jì)機(jī)器人位置做準(zhǔn)備。

重采樣的目的是去掉權(quán)重小的粒子，同時(shí)將權(quán)重大的粒子分解成若干個(gè)權(quán)重較小的新粒子。重采樣的思想可以用圖5來(lái)表示。

圖5 重采樣示意圖

圖5（a）為粒子權(quán)重的累計(jì)概率分布函數(shù)，橫坐標(biāo)為粒子的序號(hào)；圖5（b）為0～1間均勻分布的隨機(jī)變量，u為該隨機(jī)變量的一次采樣值重采樣過(guò)程中，首先計(jì)算出粒子概率的累積概率，然后在0～1間按均勻分布隨機(jī)采樣，投影到圖5（a）中，投影的位置向下指示出累積概率相應(yīng)定義域的位置，這就是新粒子的索引。這樣共產(chǎn)生L個(gè)粒子，其權(quán)重都重置為1/L。顯然，權(quán)重大的粒子就可能會(huì)被多次重復(fù)采樣。

2.2.4 粒子狀態(tài)預(yù)測(cè)

完成了粒子的重采樣之后，機(jī)器人運(yùn)動(dòng)一次。根據(jù)里程計(jì)的返回值由式（3）和式（4）計(jì)算出這一次運(yùn)動(dòng)后，機(jī)器人相對(duì)運(yùn)動(dòng)前的位置和角度的變化，并由該變化量按照式（10）系統(tǒng)狀態(tài)方程，更新每個(gè)粒子的位置和角度，使每個(gè)粒子都能相對(duì)于自己上一時(shí)刻的位置和角度，作與移動(dòng)機(jī)器人相同的運(yùn)動(dòng)。

如機(jī)器人向前走了10 cm，每個(gè)粒子也按自己的方向向前走10 cm。實(shí)際環(huán)境中由于受車(chē)輪打滑、里程計(jì)測(cè)量有誤差等因素的影響，雖然里程計(jì)返回值是10 cm，但機(jī)器人很有可能不是走了剛好10 cm。因此式（10）引入了過(guò)程噪聲wt。

2.2.5 粒子權(quán)值更新

在所有粒子都隨移動(dòng)機(jī)器人運(yùn)動(dòng)后，下一步的工作就是為所有粒子重新計(jì)算權(quán)值。

基于環(huán)境柵格地圖，對(duì)于每一個(gè)粒子，根據(jù)該粒子的狀態(tài)（包含位置及方向），計(jì)算出該粒子在該方向上與環(huán)境的距離d，并將距離值與移動(dòng)機(jī)器人測(cè)距傳感器的真實(shí)返回值d^相比較，從而求出式（8）中的P（ct|pt）:

式中:C為歸一化系數(shù)；∑為由傳感器量測(cè)噪聲vt的方差。

由于粒子的數(shù)目非常多，如果用實(shí)時(shí)計(jì)算的方式來(lái)求解每個(gè)粒子的測(cè)量值會(huì)需要很長(zhǎng)的時(shí)間。因此，設(shè)計(jì)了一種基于查表方式的權(quán)值更新方法。首先在離線形式下，把機(jī)器人在地圖中所有可能的位置和姿態(tài)列出來(lái)，并計(jì)算出相應(yīng)各傳感器的距離信息，生成一個(gè)二進(jìn)制文件。

通過(guò)查表方法，可以實(shí)時(shí)地將需要的數(shù)據(jù)d^從文件中找出來(lái)，由式（13）計(jì)算量測(cè)概率，從而減少了機(jī)器人自定位的計(jì)算量，縮短了機(jī)器人兩次運(yùn)動(dòng)之間的時(shí)間，增加了自定位程序的實(shí)時(shí)性，但也一定程度地增加了機(jī)器人內(nèi)存的消耗。

基于粒子濾波理論的移動(dòng)機(jī)器人自定位程序的流程圖如圖6所示。

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

圖6 自定位流程圖

筆者使用VC2005對(duì)算法進(jìn)行仿真。圖7中使用的粒子數(shù)為3500，灰色區(qū)域?yàn)檎系K物和墻壁，灰色的點(diǎn)為粒子。實(shí)線為仿真程序生成的機(jī)器人實(shí)際位置，機(jī)器人前方兩條線的長(zhǎng)度表示兩個(gè)測(cè)距傳感器的實(shí)際測(cè)量值；虛線為機(jī)器人最優(yōu)估計(jì)粒子的位置和方向，兩條射線的長(zhǎng)度表示在該位置和方向上，兩個(gè)測(cè)距傳感器對(duì)應(yīng)的測(cè)量估計(jì)值，可以通過(guò)查表獲得。

仿真使用的地圖是由4個(gè)房間和1條走廊以及每個(gè)房間內(nèi)的障礙物組成簡(jiǎn)單結(jié)構(gòu)的環(huán)境。由于仿真環(huán)境地圖比較簡(jiǎn)單，環(huán)境中存在對(duì)稱(chēng)的點(diǎn)，則機(jī)器人在自定位時(shí)，粒子會(huì)集中到這些與機(jī)器人所在真實(shí)環(huán)境對(duì)稱(chēng)的點(diǎn)附近，只有當(dāng)機(jī)器人運(yùn)動(dòng)到獨(dú)特的位置時(shí)，所有的粒子才會(huì)聚集到機(jī)器人的真實(shí)位置附近。

圖7（b）為機(jī)器人仿真定位程序的第1次定位結(jié)果。與圖7（a）的初始狀態(tài)相比，粒子已經(jīng)開(kāi)始聚集，不過(guò)由于信息還比較少，因此最優(yōu)的粒子與實(shí)際的機(jī)器人位于不同房間。

圖7（c）為仿真程序的第2次定位結(jié)果，與第1次定位的結(jié)果相比，粒子又進(jìn)一步集中，且最優(yōu)估計(jì)狀態(tài)也正確估計(jì)出機(jī)器人所在房間。

隨著機(jī)器人的運(yùn)動(dòng)，距離量測(cè)信息越來(lái)越多，第4次定位后，效果比較明顯，參見(jiàn)圖7（e），到第6次定位完成后，估計(jì)就非常準(zhǔn)確了，參見(jiàn)圖7（f）。

將筆者提出的算法與經(jīng)典蒙特卡洛算法進(jìn)行比較可知，筆者采用了角度計(jì)算策略，而經(jīng)典算法直接對(duì)式（1）狀態(tài)進(jìn)行采樣；筆者采用了查表計(jì)算權(quán)值，而經(jīng)典算法是采用在線計(jì)算粒子權(quán)重的方法。

采用的仿真實(shí)驗(yàn)粒子數(shù)為3500，運(yùn)行環(huán)境為Windows Xp下 VS2005，奔騰雙核 2.6 G，1 G 內(nèi)存。比較結(jié)果如表1所示，可以發(fā)現(xiàn)筆者算法在耗時(shí)及收斂速度上具有優(yōu)越性。

圖7 筆者算法定位仿真

表1 筆者算法與經(jīng)典蒙特卡洛算法比較

4 結(jié)論

筆者針對(duì)經(jīng)典蒙特卡洛定位算法，提出了兩種改進(jìn)策略。首先粒子初始化時(shí)，每個(gè)粒子的角度經(jīng)過(guò)一次匹配得到，從而減小了計(jì)算量。其次，采用離線方式計(jì)算場(chǎng)景的權(quán)重表，在定位過(guò)程中，經(jīng)過(guò)直接查表，可極大地減少定位的時(shí)間消耗。

通過(guò)與經(jīng)典蒙特卡洛定位算法的比較，筆者的算法具有計(jì)算量小，收斂速度快的優(yōu)點(diǎn)。

［1］蔡自興.機(jī)器人學(xué)［M］.北京:清華大學(xué)出版社，2000:16－25.

［2］IEGWART R，NOURBAKHSH I R.自主移動(dòng)機(jī)器人導(dǎo)論［M］.李仁厚，譯.西安:西安交通大學(xué)出版社，2006:21－120.

［3］DURRANT－WHYTE H，BAILEY T.Simultaneous localization and mapping:part I［J］.Robotics ＆ Automation Magazine，IEEE，2006，13（2）:99 －110.

［4］BAILEY T，DURRANT －WHYTE H.Simultaneous localization and mapping（SLAM）:part II［J］.Robotics＆ Automation Magazine，IEEE，2006，13（3）:108－117.

［5］THURN S，BEETZ M，BENNEWITZ M，et al.Probabilistic algorithms and the interactive museum tourguide robot minerva［J］.International Journal of Robotics Research，2000，19（11）:972 －999.

［6］方正，佟國(guó)峰，徐心和.基于貝葉斯濾波理論的自助機(jī)器人自定位方法研究［J］.控制與決策，2006，21（8）:841－862.

［7］曾慧，吳福朝，胡占義.基于平面激光測(cè)量的移動(dòng)機(jī)器人自定位方法［J］.自動(dòng)化學(xué)報(bào)，2007，33（2）:138 －144.

［8］劉承俊，原魁，鄒偉，等.基于特征粒子的Monte Carlo自定位方法［J］.機(jī)器人，2006，28（1）:30 －35.

［9］陳衛(wèi)東，張飛.移動(dòng)機(jī)器人的同步自定位與地圖創(chuàng)建研究進(jìn)展［J］.控制理論與應(yīng)用，2005，22（3）:455 －460.

［10］OLIVIER C，SIMON J G，ERIC M.An overview of existing methods and recent advances in sequential Monte Carlo［J］.IEEE，2007，95（5）:654 －662.