王志紅，王 奔

（1.武漢理工大學 現(xiàn)代汽車零部件技術(shù)湖北省重點實驗室，武漢430070；2.汽車零部件技術(shù)湖北省協(xié)同創(chuàng)新中心，武漢430070）

定位是機器人要解決的核心問題。機器人的定位問題主要涉及到三點，即位置跟蹤、全局定位以及綁架問題。常用的算法是MCL 的各種衍生算法，主要分為兩大類：（1）對地圖進行離線處理，如文獻[1]為了加強初始時匹配的成功率，對地圖進行了膨脹處理；（2）提高采樣效率，如文獻[2]使用自適應KLD 采樣提高重采樣效率。

自適應蒙特卡洛定位算法（SAMCL）[3]結(jié)合了以上兩種思路，使用了一種預處理（Pre-caching）的離線處理方法，降低實時計算的負擔。此外，SAMCL 限定粒子重采樣的范圍以提升采樣效率，并將粒子分配為2 組來處理定位失效的問題。

SAMCL 的缺點是要求距離傳感器必須按照中心對稱排布，影響了該算法的實用性。文獻[4-5]提出了基于橢圓能量柵格的SAMCL，解決了多個非對稱傳感器應用SAMCL 的問題，并基于ROS 進行了測試。

本文結(jié)合二維激光雷達提出一種SAMCL 改進算法（下稱iSAMCL），使其能夠應用在非對稱分布的單個傳感器，以視角為270°的激光雷達為例進行說明，并基于ROS 進行仿真與實驗。

1 自適應蒙特卡洛定位算法（SAMCL）

算法總體架構(gòu)如圖1所示。SAMCL 算法包含預處理、計算SER、定位3 個過程。其中預處理離線進行，另外兩個步驟實時進行。SAMCL 針對傳統(tǒng)的MCL 算法主要提出了3 點改進：

（1）采用了離線的地圖預處理方法，減少實時計算量。預處理的主要思想是：預測地圖上每個柵格處的一組距離測量值｛d1，…，dN｝，根據(jù)式（1）計算出預測能量值E，保存在柵格中。保存了能量信息的地圖稱為能量地圖，記作GE。在需要獲取粒子的權(quán)重時，每個粒子從它所在的柵格中獲取能量值，避免了實時計算能量。

（2）提出等能量區(qū)域（similar energy region，SER）的概念，限定重采樣區(qū)域，提高采樣效率。運行時機器人上安裝的激光雷達得到測量值，按照式（1）計算得到，稱為真實能量值。SER 是GE的子集，表示E 與接近的柵格的集合，因此在SER中進行重采樣能夠提升效率。

圖1 自適應蒙特卡洛定位算法架構(gòu)Fig.1 Architecture of SAMCL algorithm

圖2 自適應蒙特卡洛定位算法Fig.2 Flow chart of SAMCL algorithm

2 改良的自適應蒙特卡洛定位算法（iSAMCL）

算法流程上iSAMCL 與SAMCL 相同，包含在線與離線兩個階段。離線部分對地圖進行預處理，在線部分通過粒子濾波實時計算更新機器人位姿，兩個算法的主要區(qū)別在于預處理及SER 的計算方法。

2.1 預處理

SAMCL 規(guī)定了只能使用中心對稱的傳感器，假設(shè)地圖如圖3（a）所示，使用視角360°的激光雷達。當機器人在圖中黑色圓點所示的位置時，預測激光雷達的測量數(shù)據(jù)如圖3（b）所示。因此在預處理時，每個柵格根據(jù)式（1）計算得到一個能量值E。

若使用視角270°的激光雷達，激光雷達可能測量到的數(shù)據(jù)如圖3（c）所示。此時如果機器人原地轉(zhuǎn)向，雷達數(shù)據(jù)改變，計算得到的能量值E 也會變化，因此同一個柵格對應了多個E 值。

圖3 地圖與預處理Fig.3 Pre-caching of map

iSAMCL 在預處理過程中，將方向維度劃分為離散的空間，算法將同一位置不同方向的能量值計算出來，則生成三維能量柵格地圖，記作G3D。這一步離線進行。

地圖預處理算法如算法1所示，算法的輸入是二維地圖中的某個柵格的坐標（x，y）。第2 行中I 表示將方向維度劃分為I 等份，激光雷達的光束數(shù)量用N 表示，可視角度為β。例如激光雷達的視角為270°，且每幀包含540 個距離數(shù)據(jù)，則N=540，I=540×360°/270°＝720，將方向維度720 等分。第3～4行表示，如果當前單元格未知或者被占用，則能量為1。第5～6 行表示該柵格與障礙物的距離小于機器人半徑R，能量為1。第8 行根據(jù)式（1）計算能量，得到的結(jié)果是區(qū)間（0，1］之內(nèi)的數(shù)。

算法1：地圖預處理算法

2.2 等能量區(qū)域的計算

SER 是G3D的一個子集。SER 的計算方法如算法2所示，前4 行根據(jù)真實光束的數(shù)值計算機器人當前的真實能量值E?，此后，與每個柵格（x，y，θ）中能量對比，若E?與G3D（x，y，θ）相差在δ 范圍內(nèi)，則將柵格標記為SER。

算法2：SER 計算算法

借助ROS 使用圖3（a）所示的地圖進行測試，SER 的計算結(jié)果如圖4所示。由于SER 的柵格含有x，y，θ 三個維度的坐標，在二維地圖中，使用箭頭來表示，箭頭末端表示x，y，箭頭指向表示θ。圖4（a）是能量值為0.95 時的SER，可以看到其覆蓋區(qū)域略小于地圖。圖4（b）是能量為1.0 時的SER，可以看到與圖4（a）相比，面積大幅縮小了，若在該區(qū)域中進行采樣，效率將會大大提升。對應地，圖4（c）與圖4（d）分別為能量為0.95 與1.0 時，在SER 中進行采樣得到的粒子。

圖4 等能量區(qū)域以及等能量區(qū)域中的采樣Fig.4 SER & sampling in SER

2.3 定位

iSAMCL 實時定位算法如算法3所示，主要分為5 個部分。

第二部分（第6～10 行）：確定用于全局定位與用于局部定位的粒子數(shù)量。這一步根據(jù)判定是否需要劃分粒子。α 為分配系數(shù)，可以根據(jù)需要進行調(diào)整。根據(jù)α 計算出用于全局定位的粒子數(shù)量NG與用于局部定位的粒子數(shù)量NL。

第三部分（第11～14 行）：局部定位的粒子進行重采樣。這一步是對局部粒子的操作，與傳統(tǒng)的MCL 算法相同。開始時，對權(quán)重wt進行歸一化，重采樣基于wt進行。

第四部分（第15～18 行）：計算SER 與全局粒子采樣。從SER 中按照均勻分布采樣，獲得全局定位的粒子。

算法3：iSAMCL 算法

3 實驗

3.1 實驗平臺

實驗基于EAIBOT Dashgo D1 機器人搭建平臺，傳感器采用了Hokuyo UTM-30LX-EW 激光雷達，其可視角度為270°，可測量的最大距離為30 m。計算平臺為HP OMEN-15 筆記本電腦，其搭載6 核心12 線程Intel Core i7 8750H 處理器，操作系統(tǒng)為Ubuntu 16.04 LTS，整個系統(tǒng)外觀如圖5（a）所示。

算法使用Python 在ROS 中實現(xiàn)。實驗室真實環(huán)境如圖5（b）所示，使用的地圖如圖5（c）所示。三維柵格地圖的分辨率是0.05 m×0.05 m×0.5°。

圖5 實驗室真實環(huán)境、地圖Fig.5 Environment of lab & map

本文進行了兩項實驗。第一項實驗的目的是測試位置跟蹤能力。第二項實驗測試的是機器人從綁架問題中恢復的能力。

由于SAMCL 不支持270°的激光雷達，無法在完全相同的條件下進行iSAMCL 與SAMCL 的對比，因此本實驗無法得知iSAMCL 與SAMCL 在定位性能上的區(qū)別，實驗結(jié)果將與里程計以及文獻[3]中給出的數(shù)據(jù)進行對比，以說明iSAMCL 在270°視角激光雷達上的可行性。

3.2 位置跟蹤實驗

實驗過程中，使用鍵盤控制小車在實驗室中運行并分別記錄定位算法給出的位姿以及里程計給出的位姿，并使用ROS 中的可視化工具rviz 實時觀察，過程如圖6所示。

實驗中記錄的軌跡如圖7所示，其中實線是由iSAMCL 產(chǎn)生的軌跡，而虛線是由里程計產(chǎn)生的軌跡。機器人到達終點后，使用激光測距儀測出機器人在環(huán)境中的實際位置，與算法給出的位置對比，得出x，y，θ，三個維度的誤差并記錄。進行20 次實驗，得到的平均誤差如表1所示。文獻[3]中給出的位置跟蹤實驗平均誤差如表2所示。對比可以看出，定位算法的誤差基本處于相同的數(shù)量級內(nèi)，而本實驗由于實驗室環(huán)境較小，因此顯得里程計漂移較小。這一結(jié)果能夠說明iSAMCL 在位置跟蹤問題上的可行性。

圖6 位置跟蹤實驗過程Fig.6 Position tracking experiment

圖7 里程計與iSAMCL 軌跡對比Fig.7 Comparison between tracks of odometer and iSAMCL

表1 SAMCL 綁架恢復最終位置平均誤差Tab.1 Average error of final position for iSAMCL

表2 SAMCL 綁架恢復最終位置平均誤差Tab.2 Average error of final position for SAMCL

3.3 綁架問題實驗

綁架問題的測試方法為當機器人準確獲取自身的定位后，人為地將機器人搬離原有位置一段距離，造成里程計的失效，測試定位算法能否恢復正確位置。

圖8 展示了機器人從綁架問題中恢復的過程。2 s 時刻發(fā)生綁架，此時發(fā)生綁架，粒子的權(quán)重分散，導致了的減小，算法分出粒子在SER 進行采樣，粒子顯得較為分散。如圖8（a）所示。通過幾輪迭代，粒子逐漸確定了機器人的正確位姿，如圖8（b）（c）所示。使用激光測距儀測量機器人最終位置，并與定位算法給出的位置進行比較，記錄誤差。

圖8 綁架問題恢復過程Fig.8 Rescuing from kidnapping problem

經(jīng)過20 次實驗后計算平均誤差，得到的結(jié)果如表3所示。文獻[3]中給出的實驗結(jié)果如表4所示。對比發(fā)現(xiàn)，定位算法的誤差相差較小，這一結(jié)果基本能夠證明iSAMCL 在綁架問題中的可行性。

表3 iSAMCL 綁架恢復最終位置平均誤差Tab.3 Average error of recovered location for iSAMCL

表4 SAMCL 綁架恢復最終位置平均誤差Tab.4 Average error of recovered location for SAMCL

4 結(jié)語

本文提出了一個改進的自適應蒙特卡洛定位算法，這個算法繼承了自適應蒙特卡洛定位算法的優(yōu)點，即高實時性，并通過改進預處理，使其能夠適用于270°視角的激光雷達。本文進行了2 個實驗，驗證了iSAMCL 算法的可行性。