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

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

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

圖1 離散柵格地圖示例

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

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

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

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

（2）采用查表式方法估計距離，可提高定位效率。首先對環(huán)境地圖進行離散化，其后針對地圖中每個小格，進行360度等間隔離散，并計算各角度上該小格與周圍環(huán)境的距離，將其存為表格形式。當機器人在環(huán)境中行走，并獲得量測距離時，可以直接調用上述表格文件，通過查表方法，判斷各個粒子在各方向上的概率，從而減少自定位的在線計算時間，增強機器人運行及定位的實時性。

1 機器人運動模型

在對輪式機器人分析的過程中，常把機器人建模為輪子上的一個剛體，在水平面上運動。參見圖2，建立平面坐標系XOY，則機器人的位置和姿態(tài)可以用如下狀態(tài)向量表示:

式中:x，y為機器人在二維直角坐標系中的位置坐標；θ為機器人的朝向，即機器人運動正方向與X軸的夾角。

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

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

圖3 運動模型

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

式中:Δsr和Δsl分別為右輪和左輪一次行走的距離；b為機器人同軸兩個輪子間的距離；Δs和Δθ分別為機器人一次運動行走的距離和方向的變化，這兩個量即為機器人在自身局部坐標系中的移動和轉動，式（2）即為局部坐標系與全局坐標系之間的轉換公式。

2 移動機器人自定位設計

2.1 移動機器人定位貝葉斯建模

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

圖4 移動機器人定位原理圖

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

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

結合貝葉斯公式變換可得到:

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

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

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

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

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

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

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

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

設移動機器人狀態(tài)方程為:

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

式中，ws和wθ分別為機器人的運動距離和偏轉角度的噪聲。

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

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

2.2.2 初始化粒子

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

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

假定θ值按均勻分布生成粒子，即θ～U（0，2π），那么當某個粒子的坐標與機器人的真實2D位置吻合時，該粒子隨機生成的θ與機器人真實的方向吻合的概率并不大，從而造成計算浪費。

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

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

2.2.3 粒子重采樣

在完成粒子的初始化之后，每個粒子的權重也在初始化θ值時賦給了每個粒子。此時就可以找出權重最大的粒子作為本次機器人位置的最佳估計。下一步的工作就是對粒子進行重采樣，為下一次估計機器人位置做準備。

重采樣的目的是去掉權重小的粒子，同時將權重大的粒子分解成若干個權重較小的新粒子。重采樣的思想可以用圖5來表示。

圖5 重采樣示意圖

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

2.2.4 粒子狀態(tài)預測

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

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

2.2.5 粒子權值更新

在所有粒子都隨移動機器人運動后，下一步的工作就是為所有粒子重新計算權值。

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

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

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

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

基于粒子濾波理論的移動機器人自定位程序的流程圖如圖6所示。

3 仿真實驗及結果分析

圖6 自定位流程圖

筆者使用VC2005對算法進行仿真。圖7中使用的粒子數(shù)為3500，灰色區(qū)域為障礙物和墻壁，灰色的點為粒子。實線為仿真程序生成的機器人實際位置，機器人前方兩條線的長度表示兩個測距傳感器的實際測量值；虛線為機器人最優(yōu)估計粒子的位置和方向，兩條射線的長度表示在該位置和方向上，兩個測距傳感器對應的測量估計值，可以通過查表獲得。

仿真使用的地圖是由4個房間和1條走廊以及每個房間內(nèi)的障礙物組成簡單結構的環(huán)境。由于仿真環(huán)境地圖比較簡單，環(huán)境中存在對稱的點，則機器人在自定位時，粒子會集中到這些與機器人所在真實環(huán)境對稱的點附近，只有當機器人運動到獨特的位置時，所有的粒子才會聚集到機器人的真實位置附近。

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

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

隨著機器人的運動，距離量測信息越來越多，第4次定位后，效果比較明顯，參見圖7（e），到第6次定位完成后，估計就非常準確了，參見圖7（f）。

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

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

圖7 筆者算法定位仿真

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

4 結論

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

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

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

［2］IEGWART R，NOURBAKHSH I R.自主移動機器人導論［M］.李仁厚，譯.西安:西安交通大學出版社，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］方正，佟國峰，徐心和.基于貝葉斯濾波理論的自助機器人自定位方法研究［J］.控制與決策，2006，21（8）:841－862.

［7］曾慧，吳福朝，胡占義.基于平面激光測量的移動機器人自定位方法［J］.自動化學報，2007，33（2）:138 －144.

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

［9］陳衛(wèi)東，張飛.移動機器人的同步自定位與地圖創(chuàng)建研究進展［J］.控制理論與應用，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.