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

趙以毛，歐陽嘉泰，王冠凌，代廣珍

(安徽工程大學 電氣工程學院，安徽 蕪湖 241000)

隨著機器人技術和信息技術的結合，對室內移動機器人的研究已經成為當前研究的大熱門之一[1]。實現室內移動機器人系統設計的重要關鍵技術是其本身的自主定位與導航的準確性。機器人的定位及導航的精準度決定其在擴展應用層面起到關鍵性作用。但室內環境普遍存在物件擺放復雜多樣、動靜變化因素多等問題，導致移動機器人在室內小環境場景，如家庭、企業工廠等，應用中受到約束。移動機器人最基礎的一項工作就是可以自主導航，簡而言之，當機器人處于一個未知或者陌生環境中，可以獨立自主規劃出一條安全、平滑的最優通行線路，沿著這條路徑到達目標點。地圖構建即SLAM[2]，是機器人成功完成導航的前提。當前，常用的SLAM建圖方法主要有：(1)基于圖像優化的Cartographer算法[3]，此算法是谷歌開發的室內實時SLAM，采用谷歌開發的基于Ceres的非線性優化思想和基于子地圖的全局地圖構建思想，能夠有效規避地圖構建環境中運動物體的干預。由于Cartographer算法沒有集成到ROS中，因此在ROS環境下若要使用此算法，在安裝過程中我們需要從官網下載源碼的方式來對其進行編譯安裝，相比于其它建圖方法其安裝過程復雜。(2)Hector_slam算法基于激光雷達運行，不需運用里程計信息，其采用高斯牛頓方法，構建二維柵格地圖。但此算法對于雷達幀率要求很高且此算法沒有對地圖的修正能力，初值的選擇對結果的影響很大，并且在hector_slam過程中如果轉彎的速度過快，移動機器人建圖會出現打滑的現象，在之后的建圖匹配也都會出現較大差別從而導致地圖構建失敗。(3)Gmapping-SLAM算法，此算法是一種基于粒子濾波的建圖算法，且在ROS系統中此算法已集成其中，是在移動機器人建圖中使用最廣泛的SLAM建圖算法[4]，在對地圖邊緣角落的修正處理上和障礙物的掃描識別標定，以及在對小地圖場景進行建圖時其計算量相對較小且Gmapping構建的二維柵格地圖更加穩定，不易出現漂移的現象。Gmapping算法的建圖效果較好，但同樣也有著兩個缺陷：(1)建圖對機器人位姿要求較高，(2)頻繁的重采樣導致粒子耗散。對比上述幾種建圖方法的優缺點，本文基于Gmapping算法并針對該算法的缺陷，提出了針對性的優化方案，改善建議分布，以及加入KLD重采樣。基于改進的Gmapping算法，本文對室內自主搭建的實驗場地來進行構建地圖。

在建圖與定位完成后，需要機器人可以獨立自主規劃出一條從設置的起始點到目標點并可以自主躲避在前進中遇到的障礙物的最優安全路徑，即路徑規劃[5]。當前，Dijkstra算法[6]、A*算法[7-8]RRT算法[9-11]和基于生物啟發式的遺傳算法[12-13]等是目前流行的全局規劃算法。其中，A*算法采用了啟發式搜索模式，減少了搜索節點的個數，搜索時間降低了，因此大大提高了搜索效率，在全局路徑規劃方面具有非常大的優勢。

本文在ROS(Ubuntu18.04-Melodic版本)環境下，實現了機器人自主導航系統。首先，使用改進的Gmapping-SLAM算法完成對環境地圖構建與定位，然后利用改進的A*算法和TEB算法完成機器人路徑規劃和運動控制，使機器人可以順利到達目標點。

1 基于改進RBPF-Gmapping算法建圖與定位

在不熟悉的或復雜的場景中，移動機器人要想實現完全自主導航，首先必須要獲取周圍場景的信息，確定當前姿勢，構建環境地圖。目前，SLAM算法中應用較為廣泛的是Gmapping算法，主要是其在小場景及邊緣角落處理方面建圖效果較好，計算量較小，精度較高。

1.1 傳統的RBPF-Gmapping算法

Grisetti和Steux等人提出的基于RBPF(Rao-Blackwellized partical filter)的SLAM算法[14]。Gmapping算法的地圖構建主要運用了其理論且基于2D激光雷達使用RBPF算法完成對二維柵格地圖構建的SLAM算法，RBPF首先對位姿估計與地圖估計進行分離，其次估計位姿，最后進行地圖估計，其根據里程計信息u1:t-1=u1,u2,…,ut-1測量值及激光雷達的觀測值z1:t，再通過觀測數據和里程計數據得到機器人位姿x1:t和地圖m的聯合后驗概率估計p(x1:t,m|z1:t,u1:t-1)[15]。

通過RB分解，SLAM模型可表示為：

p(x1:t,m|z1:t,u1:t-1)

=p(x1:t|z1:t,u1:t-1)p(m|x1:t,z1:t)

(1)

式(1)用后驗概率表述出了移動機器人的位姿與地圖信息之間關聯p(x1:t|z1:t,u1:t-1)，為運動軌跡后驗概率。RBPF-SLAM算法是基于Rao-Blackwellized分解的粒子濾波理論估計機器人的軌跡，再利用擴展卡爾曼濾波估計地圖。最后，利用粒子濾波算法對移動機器人進行位姿估計和環境地圖更新。其主要流程為：

(2)權重更新：根據當前觀測信息Z1:t=z1,z2,…,zt，將雷達觀測得到的地圖信息與各個粒子的權值相配比，并利用式(2)更新權值。

(2)

(3)重采樣：在迭代過程中，為了降低粒子退化現象，一些權重低的粒子被丟棄，并復制相同的高權重粒子，避免計算損耗在無意義的低權值粒子中。

(4)更新地圖：通過機器人姿態估計和當前激光雷達觀測信息，通過p(m|x1:t,z1:t)來對地圖進行更新。

Doucet等利用式(3)來制約建議分布q，可以得到用來計算重要性權重的遞歸公式。

q(x1:t|z1:t,u1:t-1)

=q(xt|x1:t-1,z1:t,u1:t-1)·q(x1:t-1|u1:t-2)

(3)

將式(3)代入式(2)展開得到粒子權重為：

(4)

其中，η=1/p(zt|z1:t-1,u1:t-1)表示歸一化因子。

Gmapping的建圖過程如圖1所示。

圖1 Gmapping的建圖過程

1.2 改進RBPF-Gmapping算法

1.2.1 建議分布的改進

對RBPF-SLAM算法來說，選擇正確的建議分布是至關重要。當其所需粒子數越少[16]，其所選建議分布與真實分布越接近。在經過迭代多次之后，粒子集中的每個粒子都會攜帶大量信息數據，從而增加了系統計算的難易程度。在機器人只考慮使用里程計運動模型不使用其它傳感器的情況下，式(4)可轉化為：

(5)

式(5)中運動模型的計算簡單、易于獲得，當在使用傳感器的精度非常高時，激光雷達得到的觀測模型作為建議分布的精度比使用里程計運動模型作為建議分布的精度高，產生這種現象的原因是在后驗概率似然值低的粒子中，其里程計運動模型中的權重值設置較低。因此，本文將激光雷達掃描獲得的觀測信息zt融入到建議分布中。融合后的建議分布可以使采樣分布，在似然函數概率分布范圍內具有較大的值。改進的建議分布優化和粒子權重的計算公式分別定義如下。

(6)

(7)

由于最優重要概率密度函數的形式未知，機器人在不斷向前移動過程中，機器人里程計的誤差，導致其后驗概率分布在采樣時將使用更多的粒子，然而當外部傳感器為高精度激光雷達時，其似然峰值有且只有一個，那么可將傳感器觀測到的信息的似然峰值分布近似為高斯分布，如圖2所示。

圖2 運動模型與觀測模型似然分布值

通過對建議分布改進后，似然函數的峰值很好獲得，此時只需對似然函數的峰值L[i]區域進行采樣，計算公式為：

(8)

(9)

式(7)可以簡化為：

(10)

1.2.2 基于KLD重采樣

上述改進可以直接提高建議分布概率匹配精度，獲得當前數據，使粒子收集更加準確，降低粒子迭代計算的難度和在更新過程中的計算量，但在實際應用中，采集的樣本數和迭代次數較多。為了提高智能機器人的實時反應速度和運行效率，可以對采樣濾波算法做進一步優化。Dieter Fox等人提出了基于KLD采樣的自適應粒子濾波算法[17-18]，其主要思想為：根據粒子群分布的KLD動態調節粒子數，對不同分布的近似程度進行評估。那么，可以在重采樣粒子復制期間加入KLD重采樣，其可根據粒子的離散情況實時調整粒子數量的多少。

本文采用KLD重采樣方法，在其滿足1-θ的條件后，對采樣獲得的粒子集合概率與實際后驗概率進行誤差計算α。誤差α由K(p,q)距離確定的。兩概率之間的逼近誤差如下：

(11)

由式(11)可知，式中：K表示K-L距離，KLD計算值越小，說明概率分布p與概率分布q之間的誤差越小，估計的概率分布與真實概率分布越接近。當粒子數k為固定值時，由概率理論分析，可以保證真實概率密度與估計概率密度之間K-L距離小于閾值α，通過計算可以得到兩個概率分布之間的最小逼近誤差。

(12)

式(12)為粒子數k的表達式和x2分布的分位數。

最后再利用Willson-Hilferty變換近似得到：

(13)

其中，z1-θ是上分位數為1-θ的標準正態分布。

在粒子的復制過程中加入KLD重采樣的方法來對下一階段的粒子數重采樣，可以實現粒子數的實時調整，計算量得以大大減小。

1.3 AMCL定位算法

定位的好壞在導航系統中起到至關重要的作用。機器人在未知室內環境中，通過外部傳感器如激光雷達、慣性測量單元(IMU)、輪式里程計等傳感器獲取環境地圖信息，經過算法處理，最終可以得到機器人在地圖中的位姿。

定位方面使用自適應蒙特卡洛定位(AMCL)算法進行定位，該算法是基于地圖成功建好的前提，其利用粒子濾波器估計機器人的后驗概率分布。其算法過程如圖3所示。

圖3 AMCL算法過程

2 移動機器人路徑規劃

移動機器人自主導航的成功與否的關鍵就是路徑規劃。機器人對自身處于的未知環境的地圖創建和定位完成后，獨立自主規劃出一條安全、平滑的最優通行線路，沿著這條路徑到達目標點，并確保在前進道路上遇到障礙物可以順利避開并準確到達目標點。本文采用的A*和TEB的混合算法來對機器人進行路徑規劃，兩者共同完成機器人處于一個陌生環境中的路徑規劃和自主導航、避障等環節。

2.1 傳統A*算法

實現機器人自主導航功能的關鍵，重要是在已知地圖環境信息后，以及一個或多個性能指標規劃出最短無碰撞路徑。傳統A*算法在得到地圖信息階段時會評估每個位置點，然后從各個位置中找到最優點，再從此處最佳位置繼續向下不斷的搜索接下來的最佳位置，通過對比各個最優解連成一條最優的路徑，這種方式被稱為啟發式搜索。此種搜索方式是從起點開始，首先遍歷初始點周圍相靠近的點，然后再遍歷已經遍歷過的點相鄰近的點，逐步的向外遍歷尋找擴散，直到找到終點，通過這種不斷地搜索可以形成一個點集，形成該點集中路徑點的安全有序的連接即是要找尋的最優路徑。A*算法的模型為：

f(n)=g(n)+h(n)

(14)

式(14)中，f(n)是總的搜索代價，g(n)是從起點到所處最優合理節點n的代價和，h(n)是從當前所處最佳位置節點n到目標節點的最優代價。傳統A*算法采用的是八鄰域搜索法，如圖4所示，然而，基于八鄰域搜索的A*算法生成的搜索路徑中包含很多的導航路線規劃中不需要的多余冗余點，且拐點的轉向角太大，導致規劃產生的過長路徑長度和曲率不連續的路徑，這不利于機器人安全到達目標點。

圖4 八鄰域搜索示意圖

2.2 改進A*算法

針對經典A*算法的八鄰域搜索方向在路徑規劃過程中，需要的時間長，路徑不夠平滑。通過重新設計距離定義的啟發式函數，使改進的A*算法可以在搜索路徑的過程中考慮可通過區域[19]，優化路徑規劃的轉彎角度，使機器人規劃出的全局路徑更加平滑。結合歐氏距離和曼哈頓距離，本文在不同的搜索方向上使用不同的但成比例的距離權值，改進的啟發函數的具體形式如式(15)和式(16)所示，路徑規劃角度的平滑改進如式(17)所示。

h(n)=nK·(|nx-gy|+|ny-gy|)

(15)

(16)

(17)

啟發函數改進后h(n)在當前所處格點的上、下、左、右的不同方位上，采用曼哈頓距離定義，在當前柵格的右上、右下、左上、左下四個不同方位上，采用歐氏距離定義。其中，K為距離加權系數，n為正整數，(x1,y1),(x2,y2),(x3,y3)為開始路徑規劃的三個相鄰節點坐標，β為轉彎角度。啟發函數的改進后，計算成本節省了很多，大大減少了機器人規劃路徑的時間，并且規劃出的全局路徑更加平滑。

2.3 局部路徑規劃

移動機器人的路徑規劃不僅要求利用全局路徑規劃算法獨立自主規劃出一條安全、平滑的最優通行線路，沿著這條路徑到達目標點的路徑，還要求機器人能夠在復雜未知的動態環境中完成對障礙物的自適應避障。如果僅僅只使用全局路徑規劃算法顯然不能滿足機器人在復雜環境下的自主避障功能，此時就需要局部路徑規劃算法與全局路徑規劃算法相互配合、協調以此完成機器人在復雜環境自主導航避障。局部路徑規劃采用TEB算法。TEB是一個多目標優化問題，由于依賴少數幾個連續狀態，因此通常是一個參數較少的局部問題。此外，為了顯示機器人的運動學軌跡信息，每個點與點之間都設置了運動時間，該算法利用“外力”來約束“變形”局部路徑，這些約束主要有如下幾方面。

(1)機器人動力學約束:包括有限控制量、速度、轉向角度、最小轉彎半徑等，通過約束形成一個平滑的運動軌跡。

(2)最短路徑約束:目標函數使機器人路徑上各個位姿點在時間上均勻分布，而不是盲目地追求最短路徑。

(3)路徑跟蹤和避障:考慮了全局路徑、在地圖上找到的障礙物(靜態障礙物)以及激光雷達掃描發現的障礙物(動態障礙物)，障礙力迫使局部路徑遠離障礙物。

使用開源G2O庫(General Graph Optimization)進行測試優化，最終可以獲得到到達目標點的耗時最少、距離最短且可以成功避開障礙物的局部路徑，并且使局部路徑盡可能貼合全局規劃路徑。

根據上述分析，當配置導航功能時，如果TEB參數設置的不符合規劃時，會致使無法規劃出正確的行駛路線，此時機器人會出現抖動、旋轉、停止或者倒退的現象。因此，需將TEB算法中的默認參數進行一定的優化改進，其中一些影響路徑規劃與機器人行駛的重要優化參數如表1所示。

表1 TEB局部路徑相關參數

3 仿真與實際場地實驗

針對建圖定位和導航功能，首先在Gazebo中進行了場景模型搭建，并在Rviz中加載地圖環境并運行鍵盤控制節點進行了地圖構建和導航功能仿真，最終搭建線下的實驗場地中進行導航相關實驗。導航流程如圖5所示。

圖5 導航框架

3.1 建圖仿真實驗

仿真實驗在一臺系統為Ubuntu18.04-Melodic版本環境的計算機(RAM:8.00GB；處理器為：AMD R7-5800X)中操作，使用ROS中的Gazebo與Rviz工具。首先，在Gazebo中配置Word文件，并配置機器人模型URDF文件，地圖環境搭建與機器人模型如圖6所示。其次，打開新終端啟動激光雷達launch節點并調用改進后的Gmapping-SLAM功能包，并根據Gmapping-SLAM算法與激光雷達傳感器獲得的數據信息來完成對當前地圖的構建；運行Rviz節點，對建圖過程進行可視化觀察，可得到如圖7所示的畫面。

圖6 Gazebo仿真環境與機器人模型

圖7 Rviz可視化建圖過程

在ROS環境中進行仿真實驗，首先要在ROS中訂閱機器人激光雷達的“/scan”話題和“/cmd_vel”速度控制話題，同時啟動鍵盤節點腳本使用電腦方向按鍵控制機器人在仿真環境地圖中的前進，機器人前進過程中環境地圖被完整構建出來，與實際環境基本一致，沒有嚴重的漂移、重影、打滑等現象出現。地圖中的黑色圖線表示機器人無法通行的地帶，白色區域為機器人的可通行地帶，地圖邊界上的點為激光雷達掃描點，地圖中剩余區域為未搜索掃描區域。傳統Gmapping建圖和改進Gmapping建圖如圖8(a)、圖8(b)所示，在1，2，3，4處可以看出改進的算法在相同條件下建圖效果更好。

(a)傳統Gmapping建圖效果

3.2 定位與路徑規劃仿真

在機器人移動導航過程中，路徑規劃過程需調用ROS系統中的兩個功能包來實現，第一，調用AMCL定位功能包，如圖9(a)、圖9(b)、圖9(c)、圖9(d)所示，移動機器人定位的結果，觀察Rviz中仿真環境地圖，粒子云在初始位置隨機分布，隨著機器人在不斷前進，粒子云漸漸向正確位姿靠攏。第二，路徑規劃需調用ROS中的Move_base功能包，結合改進后的A*算法及TEB算法，準確評估出最優路徑來實現開始位置到終點位置的路徑規劃和機器人的自主導航運行。

圖9 定位過程圖

在建圖與定位完成后將成功建圖的yaml文件寫入導航框架的launch文件中以實現移動機器人路徑規劃與自主導航實驗，實驗過程中觀察Rviz中仿真環境地圖中的激光雷達掃描點與障礙物的重合程度，同時判斷機器人在當前地圖中的位置；可以看出地圖中的障礙物高度與激光雷達掃描點數據高度匹配。如圖10(a)、圖10(b)、圖10(c)所示。運行導航launch文件和Rviz節點，圖中的凸顯區域為障礙物膨脹層，是防止機器人的外殼與障礙物相撞的區域。機器人的起點在右上角，目標點在黑色方塊點，在這個過程中，規劃的路徑逐漸縮短，機器人自主移動至目標點。

(a)導航起始點

3.3 線下場地導航實驗

在線下場地中，機器人在快速移動中由于地面等原因易發生輪子打滑現象。為此，搭建了線下場地以測試在這種情況下定位和導航功能的準確性和穩定性，此外，實際場地的誤差因素也會干擾其定位的準確性。在機器人不斷向目標點移動過程中，定位進度逐漸準確穩定。實驗場地搭建與機器人如圖11所示，并且在線下場地驗證了經典A*算法與改進A*算法的導航路徑規劃，如圖12(a)，圖12(b)所示。相關數據對比見表2所示。

從表中可知優化后的A*算法規劃出路徑更短更平滑，到達目標點用時更少，速度更快。

圖11 實驗場地和機器人

(a)經典A*算法導航過程

表2 導航算法實驗數據比較

為測試機器人在導航過程中是否能準確避開動態障礙物，在實際場地中放置了另一個機器人作為動態障礙物，如圖13(a)、圖13(b)所示。當障礙機器人進入到機器人雷達掃描的范圍時，機器人會迅速作出反應重新規劃一條最優路徑，以此來規避障礙物機器人。

(a)移動機器人與移動障礙物起點

結語

本文基于ROS的室內移動機器人自主導航系統設計研究為目的。著重對機器人SLAM建圖、AMCL定位、路徑規劃算法等進行討論研究。

(1)在建圖與定位方面，利用改進的Gmapping算法進行建圖，通過激光雷達獲取環境信息，機器人通過AMCL算法進行機器人的定位，行駛中粒子逐漸收斂且精度較高，可以有效地用于地圖構建和導航中。

(2)路徑規劃方面，通過在仿真場景中搭建環境地圖和導航框架，最終實現移動機器人自主導航功能，并在線下搭建實驗場地，利用已有設備對自主導航系統框架進行實驗驗證，在實際場地實驗中機器人可以完成躲避靜態障礙物和動態障礙物的導航實驗且基于改進的A*算法規劃路徑長度，平滑性都優于傳統A*算法。

(3)TEB算法進行局部路徑規劃，當規劃時檢測到障礙物時，可以快速、敏捷地做出反應，并規劃出時間最優路徑，順利抵達導航終點。