一種基于機器人操作系統(tǒng)的代價地圖自適應膨脹半徑設置方法

龔志力， 谷玉海*， 朱騰騰， 任 斌

(1.北京信息科技大學現(xiàn)代測控技術教育部重點實驗室， 北京 100192；2.北京航天長征飛行器研究所， 北京 100076)

移動機器人自主導航技術作為各項高新技術綜合應用的載體，在各個行業(yè)均展現(xiàn)了巨大的應用價值[1]。在移動機器人進行自主導航前，還需要依賴同步定位與建圖(simultaneous localization and mapping，SLAM)技術完成機器人在未知環(huán)境中的地圖構建。目前主要應用在移動機器人自主導航的地圖類型主要分為拓撲地圖[2]、柵格地圖[3]和語義地圖[4]，三種方法各有其優(yōu)劣和不同的適用范圍。

其中柵格地圖易于實現(xiàn)建模、更新與處理，具有結構簡單、便于存儲計算等優(yōu)點，是目前移動機器人自主導航中研究和應用最廣泛的地圖類型。柵格地圖把環(huán)境信息離散劃分成一系列柵格，用概率值表征該柵格位置下的障礙信息。一般情況下只含簡單的布爾信息，因此在復雜環(huán)境下尋求更優(yōu)路徑有一定局限性。為改善柵格地圖的缺陷，李天成等[5]提出了一種極坐標下扇形柵格地圖的建立方法，構建了一種新型高效的工作空間。岳偉韜等[6]提出了“有義地圖率”的表征柵格地圖準確度的概念，提供了以準確度和信息量評估柵格地圖獲得最佳柵格大小方法。余文凱等[7]基于K-means聚類算法對柵格地圖進行分區(qū)并提出了量化各局部區(qū)域的復雜度的方法。張福海等[8]改善代價地圖與室內環(huán)境的匹配性，提升了地圖更新的速度。

以上研究成果均針對傳統(tǒng)柵格地圖建立方式進行優(yōu)化，使柵格地圖能包含更多環(huán)境信息，以提高自主導航效率。在移動機器人導航中對柵格地圖的優(yōu)化由代價地圖完成。代價地圖對傳統(tǒng)柵格劃分新的區(qū)域，根據(jù)傳感器數(shù)據(jù)實時更新障礙物信息，機器人移動時在障礙物周圍設置一層安全緩沖區(qū)。相比于其他種類地圖，代價地圖更有助于機器人的定位與導航[9]。如今隨著移動機器人自主導航應用范圍的不斷擴大，代價地圖也逐漸表露出了應對復雜環(huán)境時障礙物劃分不靈活、路徑規(guī)劃不理想等不足。

現(xiàn)提出一種基于機器人操作系統(tǒng)(robot operating system, ROS)的代價地圖自適應膨脹半徑設置方法及一種改進A*路徑規(guī)劃算法，優(yōu)化移動機器人自主導航中的地圖構建與路徑規(guī)劃，以提升移動機器人自主導航過程中應對復雜環(huán)境的安全性和魯棒性。

1 代價地圖構建

獲取地圖信息是ROS移動機器人進行自主導航的第一步。代價地圖是由機器人接受各類傳感器采集到的環(huán)境數(shù)據(jù)而創(chuàng)建、更新的二維或三維地圖。通常情況下ROS中的代價地圖采用柵格形式，根據(jù)與障礙物的碰撞情況可以分為三種狀態(tài)：無障礙(Free)、有障礙(Occupied)和未知區(qū)域(Unknown)[10]。

1.1 代價地圖的分層

代價地圖由多個層組成，每個地圖層(Layer)都有其特定的功能，各個地圖層的信息疊加生成最終的代價地圖信息。如圖1所示，靜態(tài)地圖層(Static Layer)是第一層，障礙物層(Obstacle Layer)是第二層，膨脹層(Inflation Layer)是第三層。這三層組合成了疊加各個地圖層后的代價地圖(Master map)，為路徑規(guī)劃模塊提供地圖信息。

圖1 Master map及基礎地圖層Fig.1 Master map and basic layers

1.2 代價地圖的更新

代價地圖以一定的周期進行更新，每個周期中都要依據(jù)更新的傳感器數(shù)據(jù)對代價地圖底層結構進行標記(mark)和清除(clear)操作，再將各層信息投影到master map上得到相應的代價值。mark操作將地圖上障礙物坐標對應的柵格設置為致命代價值，clear操作將清除雷達向外發(fā)射線穿過柵格的代價值設置為無障礙[11]。如存儲的障礙物信息為3D，則需要將每一列障礙物信息投影成2D后才能放入代價地圖中。

更新過程主要分為兩個階段：階段一為各個Layer的更新，階段二為Master map的更新。階段一中，擁有自身實體地圖信息的地圖層會更新從傳感器獲得的新增區(qū)域的代價值，如Static Layer和Obstacle Layer；自身不維護地圖的地圖層則不會更新，如Inflation Layer。階段二中，會將各個地圖層的數(shù)據(jù)逐一添加至Master map。

1.3 代價地圖柵格概率值

代價地圖中的一個柵格在計算機內存中占用一個字節(jié)，可以表示0～255中的任意數(shù)字，稱為柵格代價值。根據(jù)Bresenham算法對障礙物點進行標記[12]，填充機器人傳感器位置到障礙物之間的柵格概率。獲得障礙點的坐標后，需要對障礙點做膨脹處理，即根據(jù)柵格代價值函數(shù)填充障礙物周圍膨脹區(qū)柵格的代價值。

令一柵格qn到距離它最短歐式距離的障礙點的距離為d(qn)，柵格qn的柵格代價值記為c(qn)，其柵格代價值函數(shù)為

c(qn)=

(1)

式(1)中：ra為每個柵格的邊長；ri為機器人地面投影輪廓的內切圓半徑；rc為外接圓半徑；rm為人為規(guī)定的代價地圖膨脹半徑；w為代價值下降權重。由式(1)可得如圖2的柵格代價值分布示意圖。

center cell為機器人輪廓幾何中心柵格

若ra≤d(qn)

若ri≤d(qn)

若ri≤d(qn)

若rm≤d(qn)，珊格代價記為自由空間(Free space)，屬于沒有障礙物的空間。

據(jù)式(1)可知，在對障礙物膨脹處理中，膨脹半徑為定值，選擇的膨脹半徑是否合適在一定程度上決定了避障效果的好壞。

2 自適應膨脹半徑算法

在室內環(huán)境中，利用目前較為成熟的SLAM算法如gmapping，cartographer等能實現(xiàn)較好的導航效果[14]。但在室外環(huán)境處于非結構化環(huán)境中的密集障礙物群時，代價地圖中膨脹半徑設置過大會阻礙局部路徑規(guī)劃，導致不能靈活地通過障礙物群而被困在障礙物之中。膨脹半徑設置過小則會導致規(guī)劃出與障礙物距離過近的危險路徑，增大碰撞的風險。由此，根據(jù)機器人與障礙物的距離，提出一種代價地圖自適應膨脹半徑的設置方法。

2.1 距離自適應膨脹半徑算法

設柵格地圖分辨率為Δ，機器人與任一柵格的距離l轉換成柵格距離d的計算公式為

(2)

(3)

每次柵格地圖更新時記錄距機器人最遠障礙點的距離Dmax，則可計算On的衰減系數(shù)coef。

(4)

結合式(1)可得到距離自適應膨脹半徑柵格代價值函數(shù)s(qn)為

s(qn)=

代價地圖對障礙物邊緣的膨脹是柵格地圖上的障礙點依據(jù)與機器人的柵格距離，對其子節(jié)點迭代填充代價值的過程。

如圖3所示，首先把某一障礙點On作為父節(jié)點，計算子節(jié)點(O1、O2、O3、O4)與On的距離d(qn)，然后由d(qn)計算子節(jié)點的代價值。最后判斷d(qn)

圖3 迭代節(jié)點關系圖Fig.3 Iteration node diagram

算法步驟如下。

(1)構造膨脹點優(yōu)先隊列inflation cells，將所有障礙點依次放入隊列中，得到Dmax。

(3)判斷d(qn)

(4)由s(qn)計算出當前節(jié)點的代價值，與原始代價值比較，取最大值。

(5)從隊列inflation cells中移除當前節(jié)點，返回步驟(2)。

2.2 輪廓自適應膨脹半徑算法

為提高機器人的靈活性，在距離自適應膨脹半徑算法基礎上，針對機器人緊貼障礙物邊緣行駛的情況，提出一種根據(jù)輪廓位置自適應調整膨脹半徑的方法：在未緊貼障礙物行駛時保留更多的安全行駛空間，在緊貼障礙物行駛時再給出最低限度安全行駛空間。

如圖4所示，七宮格為一個7行7列的平面方陣。算法開始時，把當前子柵格放入七宮格的中心位置[15]，即第4行第4列，記為Pn，然后以這一點為中心，檢測其余柵格的概率值。

圖4 七宮格法Fig.4 Seven-grid method

無人車靠近某個障礙物點Pn時，周圍非占據(jù)的柵格數(shù)量減少，被占據(jù)柵格數(shù)量增加。記nF為柵格值為FREE SPACE的柵格數(shù)量，nL為柵格值為LETHAL OBSTACLE的柵格數(shù)量。則可計算衰減因子sp。

(6)

結合式(5)得到自適應膨脹半徑柵格代價值計算函數(shù)f(qn)。

f(qn)=

(7)

計算步驟如下。

(1)在第2.1節(jié)步驟(2)的基礎上判斷當前處理節(jié)點是否為障礙點，如果是，則將其作為七宮格的中心；如不符合，則繼承其On的衰減系數(shù)sp，進行步驟(4)。

(2)依次得到七宮格中的其余48個柵格值，累計得出值為FREE SPACE的柵格數(shù)量nF和值為LETHAL OBSTACLE的柵格數(shù)量nL。

(3)計算衰減因子sp，并將其繼承到所有子節(jié)點。

(4)繼續(xù)進行第2.1節(jié)中的步驟(3)。

2.3 ROS導航中的應用效果

為實現(xiàn)本文自適應膨脹半徑算法，在Gazebo中搭建仿真環(huán)境，改寫ROS中代價地圖功能包進行測試。采用ROS Navigation導航框架，使用的SLAM算法為gmapping，不使用靜態(tài)地圖，在未知環(huán)境中進行實時建圖導航。全局規(guī)劃算法選取經典的A*算法，局部規(guī)劃算法選取動態(tài)窗口法(dynamic window approach，DWA)。在相同的障礙物分布情況下對比膨脹半徑設置效果以及路徑規(guī)劃情況。所使用的程序運行在Ubuntu18.04操作系統(tǒng)中，運行內存為8 GB，主頻為2.3 GHz，ROS版本為Melodic。

Gazebo中自適應膨脹半徑設置方法使用前后效果如圖5所示。

圖5 自適應膨脹半徑方法效果對比Fig.5 Comparison of effect of adaptive inflation radius

Gazebo下設置單一障礙物情景，相同起始點和目標點下使用膨脹半徑算法的路徑導航效果如圖6所示。

圖6 Gazebo中簡單情境導航效果對比Fig.6 Comparison of the effects of simple situation navigation in Gazebo

Gazebo下設置復雜障礙物情景，相同起始點和目標點下使用膨脹半徑算法的路徑導航效果如圖7所示。

圖7 Gazebo中復雜情境導航效果對比Fig.7 Comparison of the effects of complex situation navigation in Gazebo

仿真結果表明，障礙物的膨脹半徑隨與機器人的距離遠近而自適應的調整大小，證明了自適應膨脹半徑算法的有效性。機器人緊靠障礙物行駛時，機器人在保證安全的前提下能預留出更多的局部規(guī)劃空間。復雜障礙物情景下，使用傳統(tǒng)的膨脹半徑算法，全局規(guī)劃路徑從障礙物之中穿過，最終機器人進入障礙物群中，與障礙物發(fā)生碰撞被困，導航失敗；使用改進后的膨脹半徑算法，機器人在遇到復雜障礙物情況時，全局路徑規(guī)劃成功繞過障礙物進行導航，最終到達目標點，導航成功。

3 結合環(huán)境信息的改進A*算法

結合代價地圖自適應膨脹半徑算法，設計了A*路徑規(guī)劃算法的估價函數(shù)f(n)，根據(jù)環(huán)境信息設置動態(tài)衡量啟發(fā)函數(shù)，在無障礙物環(huán)境適當提高搜索效率。對子節(jié)點的選擇進行進一步優(yōu)化，避免路徑通過障礙物頂點。

3.1 動態(tài)衡量啟發(fā)函數(shù)

A*算法是一種啟發(fā)式搜索算法，結合了Dijkstra和廣度優(yōu)先搜索(breadth first search，BFS)兩種算法的優(yōu)勢[16]，在搜索效率和搜索廣度做到平衡。算法中以距離值表征路徑代價。其代價函數(shù)f(n)計算公式為

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

(8)

式(8)中：g(n)為起始點到當前節(jié)點n實際路程的代價函數(shù)，由累計當前節(jié)點與父節(jié)點距離值得到；h(n)為當前節(jié)點到目標點的估計路程代價的啟發(fā)函數(shù)，通常使用當前節(jié)點與目標點的曼哈頓距離或歐式距離，此算法采用歐式距離。針對自適應膨脹半徑方法使得機器人避開復雜環(huán)境的情況，結合環(huán)境信息對啟發(fā)函數(shù)設置了權重函數(shù)w(n)，公式為

(9)

(10)

式中：ds為當前節(jié)點與起始點的距離；dg為當前節(jié)點與目標點的距離；t為同級子節(jié)點中障礙節(jié)點的數(shù)量。

由式(10)可知，權重函數(shù)w(n)∈[1,e]。在無障礙物環(huán)境中，隨接近目標點的過程，啟發(fā)函數(shù)權重逐漸增大，提高了搜索效率。

3.2 子節(jié)點選擇的優(yōu)化

傳統(tǒng)A*算法在柵格地圖上選擇子節(jié)點時，欠缺了對路徑與障礙物位置關系的考慮，使得部分路徑穿過障礙物頂點，增大了發(fā)生碰撞的風險[8]。針對這一問題對子節(jié)點的選擇條件進行優(yōu)化，提高路徑規(guī)劃的安全性。

子節(jié)點和父節(jié)點的位置關系如圖8所示，將處在頂點位置上的4個節(jié)點歸為危險子節(jié)點(子節(jié)點1、3、5、7)。

圖8 節(jié)點位置關系圖Fig.8 Node location relationship diagram

在選子節(jié)點時，若該子節(jié)點為危險子節(jié)點，則檢測其臨近的兩個子節(jié)點是否存在障礙節(jié)點，若存在則放棄該危險子節(jié)點，若不存在則將其作為子節(jié)點。

優(yōu)化子節(jié)點選擇條件后，解決規(guī)劃后的路徑穿過障礙物頂點的問題，避免機器人在運動過程中與障礙物發(fā)生碰撞。

4 仿真實驗與結果分析

在MATLAB中建立60×60大小的柵格地圖作為移動機器人工作環(huán)境，將自適應膨脹半徑算法配合改進A*算法與傳統(tǒng)膨脹半徑算法配合傳統(tǒng)A*算法進行對比仿真測試。

為方便此次對比實驗，設定移動機器人的工作環(huán)境柵格邊長為單位1 m，設傳統(tǒng)膨脹半徑設置方法的膨脹半徑為2 m，最低限度安全行駛半徑為1 m。

柵格中左下角為起點，右上角為目標點。黑色網格為障礙物，藍色網格為障礙物膨脹區(qū)域。模擬密集障礙物分布，隨機改變障礙物位置進行6組對照實驗。其中一組傳統(tǒng)膨脹半徑算法配合傳統(tǒng)A*算法路徑規(guī)劃效果如圖9所示，自適應膨脹半徑算法配合改進A*算法路徑規(guī)劃效果如圖10所示。使用兩種算法的平均實驗路徑參數(shù)如表1所示。

圖9 傳統(tǒng)膨膨脹半徑算法配合傳統(tǒng)A*算法路徑規(guī)劃Fig.9 Conventional inflation map algorithm combined with traditional A* algorithm

圖10 自適應膨脹半徑算法配合改進A*算法路徑規(guī)劃Fig.10 Adaptive expansion radius algorithm and improved A* algorithm path planning

表1 實驗路徑參數(shù)表Table 1 Experimental path parameters

由表1數(shù)據(jù)可知，6組對照實驗中，相比于傳統(tǒng)算法，使用自適應膨脹半徑算法配合改進A*算法后能避開密集障礙物群，轉向次數(shù)和轉角總和減少，路徑不再穿過障礙物頂點。由于需要避免進入密集障礙物群，平均路程增加4.9%。轉向次數(shù)和總和分別減少33.6%和37%。

實驗表明，結合自適應膨脹半徑算法和改進A*算法規(guī)劃后的路徑，犧牲小部分最短路徑，避開了密集障礙物群，在路徑長度相差不大的情況下，轉角次數(shù)和總和減少，根據(jù)環(huán)境信息不同提升了搜索效率。路徑規(guī)劃更平滑，路徑通過的障礙物密度下降，易于機器人行駛通過。

5 結論

通過對基于ROS的代價地圖和A*路徑規(guī)劃算法研究，得到以下結論。

(1)提出一種代價地圖自適應膨脹半徑設置方法，可用于移動機器人全局路徑規(guī)劃時避開復雜障礙物群，局部規(guī)劃時增加可行駛空間。

(2)提出一種改進A*路徑規(guī)劃算法，用于移動機器人導航全局規(guī)劃，增加搜索效率，平滑路徑，降低碰撞風險。

(3)改寫ROS中代價地圖功能包，在Gazebo仿真平臺上對自適應膨脹半徑算法進行導航測試，證明能避開復雜障礙物群。在MATLAB中對自適應膨脹算法配合改進A*算法進行驗證。通過實驗數(shù)據(jù)，證明該方法能提升機器人在復雜環(huán)境中導航的安全性和魯棒性。

(4)自適應膨脹半徑算法計算衰減系數(shù)增加了一定空間復雜度，導航安全較高的路徑增加了部分路程長度。存在權衡上的不足需在未來做進一步研究。