基于ROS的移動機器人自動導航系統設計

秦國威，陳孟元

(1.蚌埠學院 電子與電氣工程學院，安徽 蚌埠 233030；2.安徽工程大學 電氣工程學院，安徽 蕪湖 241000)

隨著社會地突飛猛進，在眾多學者的孜孜努力下，機器人在20世紀終于得以成功問世。機器人的誕生對我國的工農業、制造業、服務業等領域都產生了非常深遠的影響[1]。移動機器人作為其中最典型的代表之一，被廣泛地應用在醫療、餐飲等服務行業，為使移動機器人能夠提供更加高質量的服務，對移動機器人開展一系列的智能化、自主化研究就顯得具有非常遠大的意義和價值[2-3]。

自主導航是移動機器人需完成的一項最基本的工作。簡而言之，即移動機器人在復雜或陌生的環境中建立環境地圖，自主規劃出一條安全高效無碰撞的最優路徑，并且沿著該路徑行進到目標點。歸納起來，移動機器人自主導航主要包含以下三方面工作：第一，地圖構建問題；第二，定位問題；第三，路徑規劃問題。三方面之間的關系如圖1所示。地圖構建問題即SLAM[4]，作為移動機器人自主導航的首要任務，是實現自主導航的堅實基礎。目前，ROS下SLAM建圖方法主要有以下三種。(1)基于圖優化的cartographer算法[5]，此算法是Goole公司于2016年10月推出的圖優化SLAM算法，故又被稱為Graph-based SLAM，其需要里程計、慣導(IMU)等傳感器。圖優化的思想就是在機器人運行過程中，將不同時刻的位姿抽象為若干個點(pose)，在不同位置上觀測所產生的約束抽象為點與點之間的邊。構圖完成后，從多方面調整移動機器人的位姿去滿足這些邊構成的約束，從全圖的角度進行優化。由于cartographer算法沒有集成到ROS中，因此，ROS環境下若要使用此算法，還需要采用源碼的方式進行安裝，過程較為繁瑣。(2)基于粒子算法的Gmapping算法，此算法是一種基于粒子濾波的SLAM方法，眾多的加權粒子表示路徑的后驗概率，每個粒子都代表一個重要性因子。機器人在建圖過程中，由于粒子基數比較大，而且還不斷地被重采樣，導致計算量和內存消耗的持續增加，因此此算法建圖的效率偏低，此外，由于該算法需要使用多種傳感器獲取信息，導致所建地圖的準確性和魯棒性也備受影響。(3)基于高斯牛頓法的Hector-SLAM：此方法僅利用激光雷達獲取場景的地圖信息，完全擺脫里程計數據，僅采用圖優化的方法進行建圖，此算法非常有利于在特定的非結構化的復雜環境下進行地圖構建，而且靈活性高、擴展性強。本文綜合考慮，采用此算法建圖。本文在環境建圖建成后，接著就需要在地圖中確定移動機器人的位置和姿態，即定位。在ROS環境下移動機器人定位大部分都采用蒙特卡洛(MCL)算法[6]，但此算法存在計算量大、實時性差等問題，由此本文采用一種基于KLD重采樣的自適應AMCL算法[7]，能夠實時在線自適應調整粒子數，減少算法的計算量。定位完成后，下一步就需要規劃出一條安全且無碰撞的路徑，即路徑規劃[8]。在眾多的路徑規劃算法中，A*算法是一種基于柵格地圖的算法[9]，相比于D*算法，對于已知的環境場景，能夠規劃出更加有效平滑地路徑；相比于Dijkstra算法，A*算法采用了啟發式搜索模式，大大降低了搜索節點的數量，從而提高了搜索節點的效率。由此可看出A*算法在路徑規劃方面具有非常大的優勢。

圖1 自主導航各模塊關系圖

綜上，本文在ROS環境環境下所設計實現的自動導航系統，首先利用Hector-SLAM完成環境地圖構建，其次使用基于KLD重采樣的自適應AMCL算法確定移動機器人自身位置，最后利用改進型A*算法和DWA[10]動態窗口法完成機器人路徑規劃和運行控制。本文研究內容對于移動機器人導航領域研究具有非常遠大的意義和實用價值[11]。

1 環境地圖構建

移動機器人要在陌生環境中實現完全自主導航，必須首先獲取周圍的環境信息，明確自身所處的位置，進而建立環境地圖模型。當下，機器人均是通過自身攜帶的傳感器進行在線測量和位置估計，在陌生且復雜的環境中利用相關的SLAM算法來完成地圖構建。基于此，本文采用一種Hector-SLAM圖優化算法完成環境地圖的建模。

Hector-SLAM建圖分為兩步，第一步利用激光雷達獲取外界的環境信息，第二步則是與先驗激光數據信息進行匹配，進而完成概率柵格地圖的構建。整個過程不需要里程計的參與，僅依賴于高頻率高精度的激光，在第二步中利用Gauss-Newton法解決掃描匹配問題。本文主要針對2D SLAM子系統，開展Hector-SLAM算法研究，Hector-SLAM原理框架如圖2所示。

圖2 Hector-SLAM原理圖框架

1.1 地圖獲取

利用Hector-SLAM算法進行地圖構建，即為依賴激光雷達進行環境掃描，將連續的環境信息進行采樣離散化。同時為了克服離散性對精度的不利影響，采用雙線性插值法進行柵格占有概率估計。如圖3為雙線性插值濾波示意圖。

圖3 雙線性插值濾波示意圖

對于給定的地圖坐標Pm，M(Pm)表示該柵格的占有值，為了方便計算，其周圍的四個點坐標簡單表示為(P00,P01,P10,P11)，由此沿橫軸和縱軸進行雙線性插值，如式(1)所示。

(1)

將Pm在x方向、y方向求偏導如式(2)所示。

(2)

將(1)、(2)兩式聯合，連續柵格地圖坐標Pm點梯度值如式(3)所示。

(3)

1.2 掃描匹配

在環境柵格地圖構建完成后，下一步需要將當前采集的激光掃描幀與已建立的柵格地圖進行匹配，進而完成移動機器人的位姿估計。在整個過程中，依據激光雷達采集的第一幀數據直接進行建圖，之后采集的數據需要與已建地圖逐一對齊匹配。利用高斯-牛頓梯度法進行最優解的求解，如式(4)所示。

(4)

式中：η=(Px，Py，β)為柵格地圖旋轉矩陣，即機器人的最佳位姿。Sj(η)表征激光雷達的端點坐標Sj=(Sj,x，Sj,y)T在世界坐標系下的位置，如式(5)所示。

(5)

式中：M(Sj(η))表征坐標為Sj(η)的環境柵格地圖值，依據高斯-牛頓梯度法的高級思想，設定一個初始化數值η，根據下式對增量Δη進行估計，如式(6)所示。

(6)

對式(6)中M(Sj(η+Δη))進行一階泰勒展開，且對Δη求偏導如式(7)所示。

(7)

由此，Δη問題可轉化為函數極值問題進行處理。令上式為0可得Δη的解如式(8)所示。

(8)

式中：H為Hession矩陣如式(9)所示。

(9)

式中：▽M(Sj(η))表征的地圖梯度值已在地圖獲取部分進行闡述，如式(3)所示。

上述的Hector-SLAM算法采用高精度激光傳感器，完全不依賴里程計的數據信息，適用的范圍非常廣泛，尤其在移動機器人SLAM領域，廣泛用于解決環境地圖建模問題。

2 基于KLD重采樣的AMCL定位

移動機器人在獲取地圖信息后，接著就需要在復雜的環境中確定自身所處的位置。在本文研究中，機器人在利用Hector-SLAM算法獲取的先驗環境地圖信息、上一時刻的位姿估計以及激光雷達的數據信息后，進而經過信息的變換、處理，最終依賴當前時刻機器人所處的姿態來最終完成自主定位。本文采用基于KLD重采樣的AMCL(自適應蒙特卡洛定位)算法完成整個定位過程。

2.1 MCL算法

原始的蒙特卡洛(MCL)定位算法廣泛應用于估計移動機器人在已知環境地圖信息下的位姿狀態。移動機器人的運動學方程和觀測方程如式(10)所示。

xt=ft(xt-1,ut-1)

zt=ht(xt,vt)

(10)

式中：xt∈Rnx，表征系統在t時刻的運動狀態；zt∈Rnz分別表征系統在t時刻的觀測狀態；ft、ht分別表征系統的狀態的轉移函數和觀測函數；ut、vt分別表征系統的控制噪聲和觀測噪聲。

MCL定位是概率確定位置的結果，可將其理解在環境地圖m和系統初始化狀態x0給定的條件下，利用一組有限的離子集St來估計機器人位姿狀態xt的后驗信度，后驗概率密度的分布如式(11)所示。

(11)

(12)

在不同的時間間隔內，MCL算法基于重要性采集理論，首先采用權重粒子集St，接著對權重粒子集進行重采樣方式處理，處理的新粒子用于姿態更新，由此得出當前時刻系統狀態后驗信度分布bel(xt)的離散估計，最后，系統狀態的最小方差用粒子集替代估計。式(13)為運動狀態xt的后驗概率密度估計。

(13)

式中：δ表示樣本估計概率。

從MCL(蒙特卡洛)算法中可以明確得知，系統的樣本容量是一層不變的，同時缺少系統的觀測信息zt，忽略了移動機器人所處場景的觀測信息對狀態估計的修正作用，進而使得采樣粒子集分布在觀測似然的尾部，導致粒子集退化問題，最終使得移動機器人定位存在偏差。

2.2 基于KLD重采樣的AMCL算法

考慮到原始的MCL算法在定位方面存在的問題，本文采用基于KLD重采樣的AMCL算法完成移動機器人定位，致力于服務機器人更好的完成自主導航工作。

2.2.1 KLD采樣

粒子濾波效率和定位的精度由以下兩方面的因素決定。一是后驗信度分布與信度函數之間的似然性，二是信度函數的準確分布情況。本文所采用的KLD重采樣對采樣部分進行調整改進，是自適應定位的關鍵技術。KLD采樣的核心思想如下，在粒子濾波的每一次迭代中，以概率1-δ使真實后驗概率和基于樣本的估計信度之間的誤差是否小于ε，由此來確定重采樣樣本數目，該誤差是通過計算最大似然估計與真實的后驗分布之間的差值，即K-L距離來確定的。K-L距離用來表示兩個概率分布p和q之間的逼近誤差，如式(14)所示。

(14)

式中：k表示K-L距離，且當p，q兩個似然分布保持相同且全部為0時，是概率分布之間差異的衡量標準。

q的似然概率估計值λn在粒子數n趨向于正無窮大時服從差分分布，如式(15)所示。

(15)

p=(K(p,q)≤ε)=1-δ

(16)

但是，計算n的值非常困難，由概率論與梳理統計分析所學知識可知：當n的數量滿足一定值時，就一定可以保證真實概率密度與估計概率密度之間的K-L距離小于閾值ε，從而可以進一步保證兩者之間的逼近誤差最小，再接著利用wilson-hilferty變換近似可以得出n的近似值如式(17)所示。

(17)

式中：z1-δ服從標準正態分布，上界為1-δ。

在重采樣過程中，對權重大的粒子數進行復制，反之，進行忽略拋棄。在粒子的復制過程中采用KLD采樣的方法來確定下一次重采樣的粒子數。由此可以看出，采用重采樣的粒子處理方式可以實現粒子的實時調整，計算量得以大大減小。

2.2.2 基于KLD的自適應AMCL算法

基于KLD的自適應AMCL算法流程如下。

第二步：將粒子集權重作歸一化處理。

第四步：以粒子集的數學期望值作為當前t時刻下移動機器人位置和方向即位姿的輸出，位姿與數學期望關系式如式(18)所示。

(18)

式中：E[ ]為數學期望。

3 路徑規劃

移動機器人自主導航在完成環境地圖建模，自身定位兩部分工作之后，還剩下最后一個環節，即機器人路徑規劃。本文采用改進型A*算法，使機器人在所處陌生場景中完成初始點到目標點的全局路徑規劃，采用DWA動態窗口法完成局部路徑規劃、避障及運動控制等工作。

3.1 傳統的A*算法

A*算法作為一種啟發式搜索方法，是非常常用的移動機器人路徑規劃算法，其關鍵評價函數定義如式(19)所示。

f(n)=s(n)+h(n)

(19)

式中：n表征當前位置的節點；f(n)表征初始節點經過節點n到目標節點的總估計代價函數；s(n)表征初始點到目標點的實際代價函數；h(n)表征初始節點到目標節點的估計代價。h(n)是啟發代價函數中最重要的部分，其取值即可決定算法的性能，正確地選擇h(n)能夠大大提高A*算法的精準度。A*算法利用曼哈頓舉例作為h(n)的代價函數，公示如式(20)所示。

h(n)=|x1-x0|+|y1-y0|

(20)

式中：(x0,y0)表征初始節點所在柵格中心的坐標，(x1,y1)表征目標節點所在柵格中心的坐標。

傳統A*算法采用的是八領域搜索法，如圖4所示。通過當前柵格中心節點向臨近8個方向擴散，由此會產生很多轉折點，從運動學的角度來說，傳統的A*算法在實際場景中的應用效果顯然不理想。

圖4 八領域搜索示意圖

3.2 改進的A*算法

根據傳統的A*算法的八領域搜索方向：x方向、y方向、對角線方向，將A*算法啟發函數h(n)采用的曼哈頓距離分解成這三個方向的線性組合，經過采用這種方法處理后，即可更準確地計算出柵格地圖上任意兩點的實際代價，并且還可使得算法搜尋出最優路徑。改進后的A*啟發函數如式(21)所示。

(21)

式中：dx(n)=|x1-x0|，dy(n)=|x1-x0|。

A*算法h(n)啟發函數經過優化后，對于實際移動代價的估算更加精確。

4 ROS仿真實驗

在ROS系統下，移動機器人完成自主導航的流程如圖5所示。首先要在ubantu系統下將ROS安裝完畢，接著調用ROS環境下的三個功能包，共同組成了導航棧的核心。首先，需要調用的時hector-SLAM功能包，根據hector-SLAM算法以及激光傳感器獲得的數據完成環境地圖的建模；其次，需調用amcl定位功能包，根據基于KLD重采樣的AMCL算法，評估機器人在地圖中的位置，完成定位工作；最后，需調用Move-base包，使用A*算法以及DWA動態窗口法實現初始點到目標點的路徑規劃以及機器人的自主運行。下面就逐一討論分析建圖、定位、路徑規劃的的虛擬仿真效果。

圖5 導航流程圖

4.1 Hector-SLAM建圖仿真

Hector-SLAM算法僅需要激光掃描儀采集的數據就可完成地圖的構建，由于Hector-SLAM不需要里程計等傳感器，使其應用場景非常廣泛，如空中無人機、視覺設備等。在模擬化的ROS環境下進行仿真，首先要在ROS中機器人主要訂閱“/scan”激光數據和“/syscommand”系統命令話題，由此向外發布機器人的位姿信息及柵格地圖信息。其次通過ROS系統啟動launch腳本功能，與此同時，使用鍵盤方向按鍵控制機器人在環境中的行進，在移動機器人行進的過程中，環境地圖被完整構建出來，與真實場景基本吻合，未出現打滑、重影等情況。地圖構建的過程如圖6所示，柵格地圖白色區域為空閑區域，一圈黑邊表示障礙物區域，黑邊上的紅點為激光點云，淺灰色區域為未知區域。

圖6 環境地圖

4.2 路徑規劃仿真

在ROS環境下機器人要完成路徑規劃，首先需要機器人不管在何處都能估算出自身所處的位置，接著進行路徑的規劃。此路徑規劃工作可通過調用ROS中的兩個功能包來完成。第一，調用AMCL功能包，對底層的蒙特卡洛算法進行改進，實現基于KLD重采樣的AMCL算法，以準確評估出機器人所處的位置。第二，調用Move-base功能包，其中全局路徑規劃采用A*算法來完成，局部路徑規劃采用DWA動態窗口法，準確評估出最優路徑。如圖7所示，為移動機器人定位的結果。開始粒子云在初始位姿處隨機分布，隨著機器人的移動，粒子逐漸收斂到正確位姿。

圖7 定位過程圖

通過上述使用多種算法后，實現了SLAM環境地圖的構建。由此接下來可以通過將地圖加載在ROS框架上實現移動機器人路徑規劃以及自主導航仿真實驗。此實驗通過Gazebo進行機器人的虛擬導航仿真。在Gazebo中啟動導航節點，最終的路徑規劃導航過程如圖8所示，圖中的紅藍區域表示障礙物膨脹區域，以此使得機器人更加有效實現避障。機器人的起點在左下角，終點在右上角，行進過程中規劃的路徑逐漸縮短，最終順利到達終點。

圖8 路徑規劃及導航過程圖

5 結語

本文基于ROS的移動機器人自動導航系統設計研究為目的，著重對機器人環境感知能力、SLAM建圖、AMCL定位、路徑規劃算法等開展了一系列討論研究。通過在ROS模擬化場景下搭建導航框架，最終實現移動機器人自主導航功能。下一步將繼續利用已有的硬件條件，在真實的場景中，基于ROS搭建自主導航系統，完成真實場景下的導航實驗。