龔建銘，范 陽，周建輝

（1.北京科技大學順德研究生院，廣東 佛山 528399；2.北京科技大學 機械工程學院，北京 100083；3.華北理工大學遷安學院 機械工程系，河北 遷安 064400）

1 引言

全局定位是指：移動機器人丟失位姿估計時，如何根據(jù)傳感器信息來重新估計自身位姿的問題[1]，是機器人能夠承擔地圖構建、導航、目標檢測及跟蹤等任務的基礎[2]-[4]。針對該問題的研究由來已久，其中基于貝葉斯濾波（Bayes filter）的定位方法可以提供很好的全局定位解決方案[5]。在貝葉斯濾波框架下，擴展卡爾曼濾波（Extended Kalman Filter）定位算法[6]-[7]可以精確實現(xiàn)機器人定位，但由于高斯分布的單峰特性，該類算法無法有效解決全局定位問題。多假設跟蹤定位算法[8]將保留多個位姿的假設并使這些假設進行狀態(tài)的更新，以此來實現(xiàn)全局定位。蒙特卡洛定位（MCL）算法[9]-[10]作為一種基于粒子濾波的實現(xiàn)方式，采用帶有權重值的粒子集來近似表示任意的后驗概率分布。然而，由于該算法進行全局定位時采用了次優(yōu)先級的重要性函數(shù)，存在有粒子貧乏和計算效率較低的問題。[11]

為了避免MCL算法需要大量粒子來確定初始狀態(tài)而導致計算效率低的問題，Zhang等人[12]基于雷達放置在AGV正中心的假設，提出了相似能量區(qū)域SER（Similar Energy Region）的SAMCL算法采樣方法，即預先判斷每個粒子周圍的環(huán)境狀態(tài)來提高全局定位的效率。Dihua等人[13]提出了種自適應的KLD（Kullback-Leibler Divergence）采樣方法，該方法通過K維樹來調(diào)整箱樣本集的大小和樣本分布的變化。Zhang等人[14]將全局定位問題劃分為初始位姿估計與位姿兩個階段，提出將粒子群優(yōu)化算法與粒子濾波器方法相結合，來提高定位算法的性能。章洋等人[15]增加了線段特征匹配的過程用于縮小定位范圍，從而改善定位的精度和實時性。

本文針對移動機器人全局定位時機器人可能發(fā)生的碰撞、打滑和被人為搬運等情況而導致的位姿估計失效問題，當認為位姿估計偏差較大時通過引入粒子群優(yōu)化算法來對粒子集的分布進行優(yōu)化，并將這一方法與MCL算法相結合，使得定位算法在該種情況下能夠準確地恢復位姿估計。

2 基于粒子群優(yōu)化的移動機器人MCL全局定位算法

本文計劃在機器人位姿丟失的情況下引入PSO來恢復位姿估計。PSO和原有MCL算法框架下的重要性采樣都具有隨機性，同時優(yōu)化機器人位置與姿態(tài)將導致定位算法的結果不穩(wěn)定。所以，PSO只應用于重新估計位置，對于姿態(tài)的估計仍然依靠重要性采樣來實現(xiàn)當前預測粒子集Sˉt對粒子集St的近似。

2.1 粒子群優(yōu)化算法

PSO是Kennedy和Eberhart等人[10]于1995年提出的一種受鳥群覓食行為而啟發(fā)的智能優(yōu)化算法，與MCL算法一樣，每一個粒子都代表實際問題的可行解，二者存在相融合的應用價值。

所有粒子按公式（1）、（2）更新其速度與位置，以尋找狀態(tài)空間中的最優(yōu)適應度值：

其中，w(t)為慣性權值；r1,r2為服從0～1之間均勻分布隨機數(shù)；c1,c2為學習因子，一般取c1=c2=2。

2.2 基于粒子群優(yōu)化的移動機器人MCL全局定位算法（PSO-GL）

為了將粒子的平移分量和旋轉分量相分離以便恢復對機器人位置的估計。從條件輸入入手：將柵格地圖模型M與當前觀測A轉換為與方向無關的描述。

（1）觀測數(shù)據(jù)轉換為與方向無關的描述

假設使用能夠360度掃描的激光雷達進行定位與建圖。則對于雷達的觀測數(shù)據(jù)來說，可根據(jù)ρi的大小將ρi進行分類。記需要劃分的間隔大小為gap，雷達的最大觀測距離為max_range。條件輸入為觀測數(shù)據(jù)輸出為表征與旋轉無關的描述向量vA。具體步驟如下：

步驟1定義一個零向量vA，其大小為max_range/gap+1；

步驟2對于每一個ρi∈A，計算的值并將中的值計數(shù)+1，對于激光雷達可能存在返回NaN（Not a Number）的情況，向量vA存儲存在NaN情況的個數(shù)；

步驟3對向量vA進行歸一化處理。

（2）預緩存的地圖位置描述

受文獻的啟發(fā)，使用預緩存的地圖表達技術將地圖M表達為相應柵格分辨率的二維柵格地圖G2D。在G2D的每一個柵格處可采用Ray casting算法產(chǎn)生模擬的觀測數(shù)據(jù)并進行對觀測數(shù)據(jù)的轉換。最后將所有轉換的數(shù)據(jù)存儲在G2D相對應的柵格中。對于使用該策略的改進算法而言，以后的每次迭代中只需要查找G2D中對應柵格點的描述值就能得到該點處的與方向無關描述向量v。其實，不論模擬觀測的姿態(tài)角設置為多大，由于對觀測數(shù)據(jù)的轉換只記錄落在相應間隔點的個數(shù)，其對G2D不造成影響，所以只需將姿態(tài)角設置為0。最后得到的結果是G2D的各個元素對應其地圖M的與旋轉無關描述的表達。

得到觀測數(shù)據(jù)和地圖模型的與旋轉無關描述就可以利用PSO搜索初始位置。具體地，在初始位姿估計時對于當前觀測A與具有位置的某一粒子Xi來說，其相應的適應度值定義為它們觀測的與方向無關描述vA,vX，之間的余弦距離：

確定了在位置搜索下的適應度值就能將基于該改進策略的PSO進行粒子集位置的重新計算。機器人在運行中獲得運動數(shù)據(jù)ut、觀測數(shù)據(jù)vt的同時，PSO-GL算法會不斷進行在MCL算法框架下進行位姿遞推，直到PSO-GL算法認為發(fā)生位姿估計失效的情況，即對于有效粒子數(shù)

步驟1由當前的預測粒子集Sˉt為輸入，在G2D中查找對應的與方向無關的描述向量v；對于當前觀測數(shù)據(jù)A進行同樣的轉化得到vA。

步驟2由公式（3）計算各個粒子的適應度值，并進行個體極值pbest、全局極值gbest的初始化；

步驟3由公式（1）、（2）更新每個粒子的速度、位置，若存在更新后粒子超出地圖邊界的情況，則將其隨機分布于地圖中；

步驟4重新計算粒子集的適應度值，將個體極值和全局極值進行比較，尋找最大的極值并將其替換gbest；

步驟5判斷是否達到終止條件，若達到終止條件，則移動至步驟6；若未達到終止條件，則回到步驟3；

步驟6 PSO算法輸出更加符合當前預測的粒子集-S′t，將-S′t進行測量更新與重要性采樣得到粒子集Sˉt。

PSO-GL算法的總體流程圖如圖1所示。

圖1 PSO-GL算法

3 全局定位算法算法仿真實驗

3.1 仿真地圖環(huán)境

在Matlab下搭建與文獻[16]相類似的仿真地圖環(huán)境MRL Arena，如圖2（a）所示，其中地圖大小為5.55m*5.02m，分辨率為0.01。

設定激光雷達最大量程max_range、角度噪聲和徑向噪聲σθ,σr，則可通過光線投影算法模擬在此位姿下機器人所獲得的環(huán)境觀測信息，如圖2（b）所示。

圖2 MRL Arena仿真環(huán)境下的地圖和觀測信息

3.2 仿真實驗

在t=4時，添加擾動量F=（0.2，0.4，0.3）。實驗參數(shù)的設置為：PSO-GL算法與MCL算法的粒子集規(guī)模nPSO-GL=nMCL=300；PSO-GL算法位姿失效恢復部分中，迭代次數(shù)k=5，學習因子c1=c2=2，定義收縮因子

參數(shù)c=c1+c2，粒子按照按公式（1）、（2）、（6）進行更新：

在運動過程中，機器人運動的真實軌跡、PSO-GL的估計軌跡以及MCL算法估計的軌跡如圖3所示。

圖3 仿真實驗中真實軌跡和PSO-GL算法、MCL算法估計的軌跡

t=4時，由于添加了添加擾動量F，MCL算法中運動更新后的粒子集分布于真實位姿存在較大偏差，故此時MCL算法估計這類問題的能力只能取決于少數(shù)隨機分布，并且被認為是在上一時刻估計“不準確”的粒子，并由這些粒子根據(jù)重要性采樣階段來產(chǎn)生更多的粒子，以此來估計機器人的位姿。PSO-GL算法和相比這一方式，額外存在有基于PSO的位置調(diào)整策略，使得所改進算法對這類位姿估計失效問題不敏感。提取兩種算法各自估計的機器人位姿點，分析其各自的估計誤差，如圖4所示。

圖4 仿真實驗中PSO-GL算法、MCL算法的估計誤差

MCL算法的位置估計誤差與角度估計誤差分別增大了412.5%，700.1%；而PSO-GL算法的位置估計誤差略微減小41.9%。角度估計度誤差增大了361.2%。可見，PSO-GL算法對擾動量F魯棒，其具體結果是：在發(fā)生擾動后，PSO-GL算法的位姿估計恢復部分使得PSO-GL算法的位置估計精度得到了提高，相應地，擾動量F對PSO-GL算法的角度估計誤差影響也明顯小于MCL算法。

4 移動機器人地圖構建與全局定位實驗

4.1 地圖構建實驗

本文所進行的物理實驗是基于HuanYuBot移動機器人實現(xiàn)的，其總體結構如圖5所示。

圖5 HuanYubot機器人的總體結構

HuanYuBot機器人采用思嵐A1雷達作為外部傳感器并以機器人操作系統(tǒng)（ROS）為平臺來實現(xiàn)實驗場景地圖的構建。本文的實驗地點為某研究生實驗室，該場景主要由實驗室工位以及過道組成，如圖6所示。

圖6 進行建圖實驗的場景

采用Gmapping開源算法來構建實驗室的地圖。在地圖構建的過程中，為了使得地圖避免出現(xiàn)地圖障礙物點或出現(xiàn)類似于“褶皺”的情況，應該盡量將機器人的直行和旋轉運動分開。最終，通過rosrun命令啟動map_saver節(jié)點保存地圖文件，得到地圖map.pgm與地圖的描述文件map.yaml。構建的實驗室柵格地圖如圖7所示。其中，藍色的小車部分為機器人模型，紅色的點云部分為當前位姿下的激光雷達數(shù)據(jù)。地圖的大小為20.8m×14.55m，地圖分辨率為0.05。

圖7 構建的柵格地圖

4.2 全局定位實驗

控制機器人移動到實驗室的任意位姿，記錄在這些位姿下激光雷達觀測數(shù)據(jù)以及位姿之間的里程計變化量，通過rosbag record命令保存得到bag文件，并將地圖文件與這些數(shù)據(jù)導入到Matlab中，PSO-GL算法和MCL算法的估計精度。實驗參數(shù)的設置與3.2一致。

如圖8所示，a點到j點為機器人運動的真實軌跡，為了驗證所提出算法對機器人位姿估計失效恢復的性能，e點到f點之間存在較大的位移且只記錄這一段間隔的部分里程計數(shù)據(jù)。

圖8 真實軌跡和PSO-GL算法、MCL算法估計的軌跡

可見，除了f點之外三條不同顏色的軌跡線大體上是相接近的，兩種算法大致都能較好地跟蹤機器人的位置，由于e點到f點之間只記錄了部分的運動數(shù)據(jù)使得MCL算法估計的位置產(chǎn)生了較大偏差，而基于PSO的位置估計改進策略使得PSO-GL算法能夠?qū)⒘Ｗ蛹臃嫌^測信息的高似然區(qū)域移動，從而在f點具有更好的位置估計精度。提取兩種算法各自估計的機器人位姿點，分析其各自的估計誤差，如圖9所示。

t≠6時，PSO-GL算法的位置估計誤差均值、角度估計誤差均值分別為0.0563m、-0.0275rad，而MCL算法的位置估計誤差均值、角度估計誤差均值分別為0.0688m、-0.0328rad，可見兩種算法在沒有位姿估計失效的情況下估計精度相差不大。

t=6時，由圖9可見，MCL算法的位置估計誤差與角度估計誤差分別增大了267%，375%；而PSO-GL算法的位置估計誤差略微減小40%。角度估計度誤差增大了220%。故PSO-GL算法能夠?qū)αＷ蛹念A測分布與真實分布之間存在較大偏差的情況進行響應，粒子集能夠朝向真實分布的區(qū)域移動，其最終結果使得PSOGL算法對粒子集的預測階段產(chǎn)生的偏差魯棒，有效減少位置估計誤差并使得角度估計偏差也隨之減小。

圖9 PSO-GL算法和MCL算法的估計誤差對比

5 結論

本文將PSO引入到MCL算法中來，通過判斷當前預測粒子集Sˉt中的有效粒子數(shù)目來識別定位算法是否存在位姿估計丟失的情況，通過PSO對粒子的采樣進行優(yōu)化，使得采樣分布向觀測概率較高的位置移動并依靠重要性采樣完成粒子集位置的更新。在仿真和物理實驗下的結果表明，PSO-GL算法對位姿估計失效的情況魯棒、位置估計誤差較MCL算法顯著減少，角度估計誤差的增幅也小于MCL算法。