基于改進A＊算法的煤礦救援機器人路徑規劃

張偉民，張 月，張 輝

(1.中國地質大學(武漢) 機械與電子信息學院，湖北 武漢 430074；2.武漢重型機床集團有限公司，湖北 武漢 430074)

煤礦救援機器人(簡稱救援機器人)是在礦井災后救援活動中進行情報收集和判斷，根據災害情況進行救助的一種機器人。它可以代替救援人員進入礦井實施環境監測、人員搜索等任務。為了使煤礦救援機器人快速、順利地到達災害現場并實施救援，規劃出一條安全、無碰撞的路徑非常重要[1]。

救援機器人的路徑規劃是指機器人按照某一種方法在分布有障礙物的地圖中搜索出一條從指定起始點到指定目標點的無碰撞最優路徑[2]。救援機器人的路徑規劃包含全局路徑規劃以及在移動過程中應對局部環境變化的局部路徑規劃。為快速地規劃出效果好的全局路徑，減少救援機器人的整體移動時間，需要采用一個合適的全局規劃算法。

目前常用的全局規劃算法有Dijkstra 算法[3]、A*算法[4]、粒子群算法(Particle Swarm Optimizations，PSO)[5]、遺傳算法[6]等。其中A*算法是在靜態路網中求解最短路最有效的搜索方法，目前使用較為普遍。但傳統A*算法在柵格環境地圖下最多允許機器人向周圍8 個柵格(稱為“鄰接點”)移動，導致傳統A*算法規劃的路徑對于可沿任意方向運動的救援機器人來說存在路徑冗余點過多、累計轉折角度大等問題，并非是最優路徑[7-8]。

目前已有相關文獻針對傳統A*算法的上述問題進行了改進。陶德俊等[9]提出一種基于改進A*算法的煤礦救援機器人路徑平滑算法，該算法利用Douglas-Peucker 算法剔除路徑中的冗余節點，并對提取出的關鍵節點進行平滑處理；雖然路徑平滑問題有所改善，但還有剩余的路徑冗余點。劉夢杰等[10]提出了一種基于雙向A*算法的礦井水災逃生路徑規劃方法，該算法可獲得節點更少、轉折角度更小的逃生路徑，但路徑仍有較為明顯的平滑問題。郭江等[11]提出了一種將Bezier 曲線與A*算法融合的改進算法，該算法對路徑具有良好的平滑效果，但若改變路徑節點位置，則會影響整條路徑的平滑效果。單偉等[12]提出一種使用極多項式曲線進行路徑平滑的改進A*算法，這有利于實現機器人的路徑跟蹤，但未對冗余路徑點進行去除。趙曉等[13]提出一種結合跳點搜索法的改進A*算法，該算法通過減少對某些節點的搜索可提高搜索效率、降低路徑轉折角度，但是由于缺少對某些必要節點的計算導致路徑代價有所增加。

上述文獻對傳統A*算法進行了部分改進，但是最終路徑仍然存在較為明顯的問題。為能同時對路徑的冗余點及路徑的轉折角進行改進，筆者在標準A*算法的基礎上，提出一種改進A*算法。改進A*算法為快速獲得初始路徑，增加救援機器人可能的移動方向，將救援機器人的鄰接點從一層8 個擴展為兩層24 個，即允許機器人最多向周圍24 個柵格移動；通過擴展機器人的移動方向，可以減少算法迭代次數，快速生成初始路徑。獲得初始路徑后，再利用本文采用的去除冗余點方法對路徑去除冗余點，并采用5 次B 樣條曲線對路徑進行平滑處理，以獲得路徑代價更小、累計轉折角度更小的優化路徑。

1 傳統A＊算法

1.1 地圖環境描述

柵格法由于操作簡單、易于實現，被廣泛用來描述機器人環境地圖[14]。柵格法需要計算機利用傳感器收集環境信息，創建環境的二維平面圖M，救援機器人RR看作在M上移動的點狀物體；若M為不規則圖形，則將M填補為規則圖形，并將填補的區域設為障礙物區域。經過柵格化處理的地圖如圖1 所示，設圖中左下角為坐標原點，水平方向為x軸，豎直方向為y軸；白色柵格為“可通行柵格(點)”，允許機器人通過；黑色柵格為“不可通行柵格(環境中的障礙物)”，不允許機器人通過[15]。

圖1 柵格地圖Fig.1 Grid map

記a為M中任意柵格，設柵格集合A={ai}(i=1,2,···,n)構成地圖M，集合Nobs={oj}(j=1,2,···,m)(m＜n)?A組成障礙物柵格，?a∈A在M中的對應坐標為(x,y)。記左下角第一個柵格坐標為[1,1]。在柵格地圖環境下進行路徑規劃是指在M上使得救援機器人RR從指定起始節點node_S沿著一條安全、無碰撞的最優路徑到達指定目標節點node_G。

為便于后續內容展開，本文采用以下設定：

(1)障礙物已基于機器人外形尺寸進行擴張，故機器人可作為一個點在柵格地圖范圍內移動。

(2)機器人不能穿過障礙物柵格的四角。

(3)機器人允許的移動方向用鄰接矩陣表示。A*算法通常允許機器人向周圍最多8 個柵格移動，其鄰接矩陣如下式所示。矩陣中數字“2”表示機器人RR當前所在位置；“1”表示救援機器人允許前往的位置。

1.2 傳統A＊算法

傳統A*算法(以下簡稱A*算法)結合Dijkstra 算法和最佳優先算法(Best First Search，BFS)的思想，在保證可以得到最優路徑的基礎上，同時采用啟發式搜索[16]，以提高算法搜索效率。A*算法假設任意節點均能通過一個代價估計函數計算出該節點的代價值，并在已計算出代價值的節點中選出代價值最小的節點作為算法的下一個擴展點，若目標點被選為下一個擴展點，則表示搜索到最優路徑。

為便于表述，對以下參數進行說明(此處的節點指某一個柵格的中心，其坐標為所在柵格坐標)：

O、C分別為存放未擴展節點、已擴展節點相關信息的集合，即開集、閉集；n為當前擴展節點；n’為當前擴展節點n的某一個鄰接點；c(n’,n) 為當前擴展節點n移動到其鄰接點n’的代價；gx、gy分別為點g在x、y方向的坐標；F(n)、f分別為當前擴展節點n的估計代價值函數(簡稱估價函數)及函數值；G(n)、g分別為從起始節點node_S到當前節點n的實際代價計算函數及函數值；g’為當前擴展節點n已經在O中時的實際代價值；H(n)、h為從當前擴展節點n到目標節點node_G的估計代價函數(啟發式函數)及函數值；H*(n)、h* 為從當前擴展節點n到目標節點node_G的實際代價函數及函數值；Par() 為某節點的父節點在閉集C中的索引；Size() 為獲得某集合行數的函數。

A*算法的搜索過程如下：

(1)初始化開集O、閉集C。

(2)將起始節點node_S加入O，并計算代價F(node_S)。

(3)若O為空集，則表示尋路失敗，算法結束，輸出結果；若O不為空集，則取出O中f值最小的節點n，并把n從O中移到C中。

(4)若n為目標點，則尋路成功，算法結束，輸出結果；否則，處理n的鄰接點：若當前鄰接點n’已經在C中，則直接處理下一個鄰接點；若n’已經在O中，則比較n’當前的實際代價值g和開集O中已保存的實際代價值g’；若g＜g’，則更新O中n’的相關信息；若g≥g’，則不對n’作處理；若n’不在O和C中，則計算相關信息，并將n’加入O中；當所有鄰接點均處理完畢，返回執行第(3)步。

1.3 估價函數F(n)

合適的估價函數F(n)，會使A*算法保證能找到最優路徑。A*算法的估價函數F(n)表示為：

式中：F(n)為從起始節點node_S經過當前節點n到達目標點node_G的估價函數；G(n)為從起始節點node_S到當前節點n的實際代價函數；H(n)為從當前節點n到達目標節點node_G的啟發式函數[17]。

H(n)與H*(n)的關系會對A*算法的搜索速度和搜索結果產生重要影響，故H(n)的選擇是A*算法的關鍵內容之一。H(n)與H*(n)的關系通常存在以下情況：

(1)若啟發式函數恒為0，則F(n)=G(n)，A*算法變為Dijkstra 算法。此時算法總能搜索到最優路徑，但需擴展大量節點，算法效率低。

(2)若H(n)≤H*(n)，則A*算法總能搜索到一條最優路徑。當柵格地圖中無障礙物或障礙物對最優路徑無影響時，存在H(n)=H*(n)，此時A*算法只擴展最優路徑點，這時算法搜索效率很高；當地圖中障礙物對最優路徑產生影響時，存在H(n)＜H*(n)，此時A*需要擴展其他節點以搜索最優路徑。

(3)若H(n)＞H*(n)，A*算法不能保證搜索到最優路徑，但算法搜索節點少、搜索效率高。

(4)若H(n)＞＞H*(n)，此時G(n)對F(n)的影響可忽略，則F(n)=H(n)，A*算法變為BFS 算法。此時算法不能保證會搜索到最優路徑，但搜索節點少，搜索效率高。

常見的代價計算方式有曼哈頓距離、對角距離和歐幾里得距離[18]。采用歐幾里得距離時有：

由于地圖中障礙物的存在，總是會有H(n)≤H*(n)，故A*算法總是可以搜索到最優路徑。本文路徑代價計算均采用歐幾里得距離。

2 改進A＊算法

對于可沿任意方向運動的救援機器人而言，由于A*算法最多允許機器人向周圍8 個柵格移動，故A*算法在柵格地圖環境下規劃的路徑是非最優的。以無障礙地圖為例，A*算法規劃出的路徑如圖2 所示。若機器人最多只能朝周圍8 個柵格移動，則圖2 中的路徑可以稱為“最優的”；但若機器人可沿任意方向移動，則其最優路徑應是起始節點node_S和目標節點node_G的連線。故需要對A*算法進行改進，以獲得對可沿任意方向移動的救援機器人而言的“最優路徑”。

圖2 傳統A*算法搜索結果Fig.2 Search result of conventional A* algorithm

2.1 改進A＊算法方案

在改進A*算法中，定義了RemovePoint函數、Spline函數和SmoothPath函數。RemovePoint函數移除路徑點集合中的冗余點；Spline函數將路徑點按照步長α進行分割；SmoothPath函數采用5 次B 樣條曲線對路徑進行平滑處理。

改進A*算法總體分為5 步：

(1)設置A*算法中救援機器人最多可向周圍24個柵格移動，并生成初始路徑點集合Pori。

(2)調用RemovePoint函數，去除初始路徑點集合Pori中的冗余點，獲得優化路徑點集合Pnew。

(3)調用Spline函數，將路徑點集合Pnew中前后兩路徑點的連線按照一定步長進行分割，獲得分割后的路徑點集合Pdiv。

(4)調用RemovePoint函數，去除路徑點集合Pdiv中的冗余點，獲得優化路徑點集合Pre。

(5)調用SmoothPath函數，對路徑點集合Pre形成的路徑進行平滑處理，獲得最終路徑點集合Pfinal。

2.2 鄰接點擴展

為使移動機器人在使用改進A*算法進行路徑規劃時，更快地獲得初始路徑，本文將改進A*算法允許機器人最多可移動的柵格數量擴展為24 個，此時機器人的鄰接矩陣如下式所示。

采用24 鄰接點的改進A*算法生成的路徑轉折角度更小且能減少搜索路徑時擴展的節點數量，從而降低內存占用，如圖3 所示。

圖3 鄰接點數量為8 和24 時A*算法規劃結果Fig.3 Planning results of A* algorithm with 8 and 24 adjacency points

對比采用8 鄰接點的A*算法和采用24 鄰接點的改進A*算法規劃出的路徑，可看出改進A*算法規劃的路徑路徑點數量更少，路徑轉折次數更少，并且規劃路徑過程中擴展的節點數量(閉集中節點數量)也更少。故采用24 鄰接點可以快速獲得效果更好的初始路徑。

2.3 去除路徑冗余點

去除冗余點流程如圖4 所示，其主要原理是重連路徑點。設重連路徑點時的起始連接點為s、目標連接點為g、新路徑點集合為R(R代表2.1 節中的Pnew或Pre)。初始時，獲取路徑點集合P(P代表2.1 節中的Pori或Pdiv)，并將P中第一個路徑點設為s、第二個路徑點設為g，通過函數CheckPath檢查兩點連線lsg是否經過障礙物，若直線lsg未通過障礙物，則將下一路徑點設為g；若經過障礙物，則將起始連接點s加入新路徑點集合R，并將目標連接點g的前一個連接點設為起始連接點s；重復這一過程直到目標連接點為目標節點node_G。最終輸出新路徑點集合R。

圖4 去除路徑冗余點流程Fig.4 Flow chart of removing redundant points in path

CheckPath函數是去除冗余點時的關鍵步驟，其輸入參數分別是δ、σ、s、g、M，δ是距離閾值，限制周圍柵格與直線lsg的最小距離，σ是采樣步長，即從起始連接點開始，按照r×σ(r=0,1,2,···)距離取點n，并判斷lsn是否通過障礙物。

針對不同的運動方向，CheckPath函數需要檢查的節點不同：

(1)路徑方向為左上到右下時，需要檢查當前節點所在柵格的左側NL和上側NT柵格。

(2)路徑方向為右上到左下時，需要檢查當前節點所在柵格的右側NR和上側NT柵格。

(3)路徑方向為右下到左上時，需要檢查當前節點所在柵格的右側NR和下側NU柵格。

(4)路徑方向為左下到右上時，需要檢查當前節點所在柵格的左側NL和下側NU柵格。

若有一個待檢查柵格為障礙物柵格，則需計算2個距離參數；若2 個待檢查柵格均為障礙物柵格，表示當前連接點g和起始連接點s的連線經過障礙物，需對g的前一個路徑點進行處理。

路徑方向為左上到右下時CheckPath函數的偽代碼如圖5 所示。圖5 中dmin為與直線lsg斜率相同且經過柵格NT左下角(NL右上角)的直線與柵格NT左上角(NL右下角)的最小距離。

當柵格NT為障礙物柵格時，dll為直線lsg到柵格NT左下角的最小距離，dul為直線lsg到柵格NT左上角的最小距離；只有同時滿足dll＞ δ和dul＞ dmin兩個條件，才能保證直線沒有經過柵格NT且與柵格NT的距離滿足要求(代碼第15—第20 行)。

柵格NL原理相同，dur為直線lsg到柵格NL右上角的最小距離，dlr為直線lsg到柵格NL右下角的最小距離；只有同時滿足dur＞δ和dlr＞dmin兩個條件，才能保證直線lsg沒有經過柵格NL且與柵格NL的距離滿足要求(代碼第21—第26 行)。

以圖6 中路徑為例，此時CheckPath函數的輸入點為s、g，當前檢查節點為n，當前檢查柵格為(2,2)。通過三角形面積公式，計算得：

式中：θ為此時的連接起點和目標點所成直線的斜率。

當前柵格的左側柵格NL為障礙物，則需計算NL右上角及右下角到直線lsn(即直線lsg)的距離dur、dlr。若dur＞δ，則能保證直線與柵格右上角點的距離滿足算法要求，但是無法判斷直線lsg是否穿過柵格NL；只有dlr滿足dlr＞dmin，才能保證直線lsg不在柵格NL內。

采用CheckPath進行去除冗余點后的路徑如圖7所示。與原路徑相比，去除冗余點后的路徑代價和路徑累計轉折角度均有所減小。

圖7 去除路徑冗余點后的路徑Fig.7 The path after removing redundant points

2.4 路徑平滑

路徑經過兩次去除冗余點后，其平滑問題得到初步改善，但還需對路徑轉角進行平滑處理。B 樣條曲線能夠對路徑進行布局修改而不影響整條路徑的基本形狀特征，被廣泛應用于路徑平滑和軌跡平滑中[18-20]。同時B 樣條曲線可人為調整曲線階數，通過改變階數可有效改善曲線的平滑效果。在仿真實驗過程中，將B 樣條曲線的階數設置依次設置為3～7 階，其平滑效果如圖8 所示。從圖8 中可知，采用3 次B 樣條曲線平滑后的路徑有較為明顯的轉折；將樣條曲線階數從三階增加到五階時，其路徑平滑效果有明顯改善；將樣條曲線階數從五階增加到七階時，雖然路徑平滑效果有所改善，但是效果沒有從三階到五階的改善效果明顯。故本文選用5 次B 樣條曲線進行路徑平滑。

圖8 不同階數的B 樣條曲線平滑效果Fig.8 Smoothing results of B-spline curves of different orders

B 樣條曲線的總方程為：

式中：Pi為給定的控制點坐標；Fi,k(t)階B 樣條基函數，其表達式為：

將基函數代入到式(5)中，即可得到5 次B 樣條曲線方程：

3 仿真實驗及結果分析

3.1 仿真實驗

為了驗證本文提出的改進A*算法的有效性，使用傳統A*算法和本文提出的改進A*算法分別在5 種尺寸的單位大小柵格組成的隨機地圖環境進行仿真，柵格地圖障礙物密度為20%，實驗環境為：CPU Intel Core i5-4200U，內存12 GB，實驗工具MATLAB 2020a。

圖9 為尺寸為20×20 的隨機柵格地圖環境下，傳統A*算法和改進A*算法的路徑搜索結果對比。通過對比，可看出相同環境下改進A*算法搜索到的路徑相比于傳統A*算法搜索到的路徑代價更小、轉折角度更小。

圖9 傳統A*算法和改進A*算法的仿真結果Fig.9 Simulation result of conventional A* algorithm and improved A* algorithm

仿真實驗在5 種尺寸的隨機地圖分別連續成功運行100 次(可能存在無可行路徑的情況)，并記錄傳統A*算法和改進A*算法運行時的搜索節點數量、路徑代價及路徑轉折角度等3 個數據。

為驗證不同路徑方向的CheckPath函數，不同尺寸地圖的起始點和目標點不同：

(1)地圖尺寸為20×20 時，起始節點為(2,2)，目標節點為(18,18)。

(2)地圖尺寸為30×30 時，起始節點為(27,3)，目標節點為(3,27)。

(3)地圖尺寸為50×50 時，起始節點為(45,45)，目標節點為(5,5)。

(4)地圖尺寸為80×80 時，起始節點為(8,72)，目標節點為(72,8)。

(5)地圖尺寸為100×100 時，起始節點為(10,10)，目標點節為(90,90)。

仿真結果見表1(參數比值為改進A*算法參數值/傳統A*算法參數值)。

表1 改進A＊算法和標準A＊算法的部分參數比值Table 1 Partial parameter ratio between improved A＊algorithm and standard A＊ algorithm

表1 給出了傳統A*算法和改進A*算法在不同尺寸柵格地圖中的搜索節點、路徑代價和路徑轉折角度的比值。地圖是以單位大小柵格組成，圖中的值表示改進A*算法計算得到的結果與傳統A*算法得到的結果的比值。從表1 可以看出，改進A*算法相比于傳統A*算法，搜索過程擴展的節點數量減少20%左右，所得路徑代價減少8%～9%，路徑轉折角度減少75%～80%。“改進A*/傳統A*”比值均小于1，表明改進A*算法優于傳統A*算法。

3.2 結果分析

經過多次仿真實驗得到的結果，分析可得實現路徑優化的主要因素有以下3 點：

(1)擴展鄰接點。通過擴展救援機器人的鄰接點，增加了救援機器人允許的移動方向，導致算法擴展到某一節點時，加入開集O的鄰接點數量更多，從而使得開集中的節點數量增加得更快，擴大了下一次比較f值的節點范圍。即若要在開集中加入相同數量的節點，改進A*算法所需的迭代次數要少于傳統A*算法；因此規劃出相同路徑時，改進A*算法所需的迭代次數更少，并且加入閉集C中的節點數量更少。因此改進A*算法生成初始路徑的速度比傳統A*算法快。理論上可以進一步擴展鄰接點，即再增加鄰接點的層數，但是增加到一定數量時反而會大幅增加每次迭代消耗的時間，從而降低搜索速度，故本文僅將鄰接點數量擴展到24 個。

(2)去除路徑冗余點。通過去除冗余點，減少了初始路徑的代價和轉折角度。改進A*算法首先對初始路徑上的路徑點進行去除；但由于初始路徑中兩路徑點的間距較大，因此路徑仍有優化空間。故在對初始路徑去除冗余點后，再將新的路徑按照一定步長進行分割，獲得間距更小的路徑點集合，并對該路徑點集合去除冗余點。同樣地，理論上來說，可以多次重復進行路徑分割和去除冗余點操作，可使最終路徑接近“最優”；但多次使用路徑分割和去除冗余點反而會增加算法運行時間，并且可能優化效果不明顯；故本文中僅使用了一次，在保證算法搜索效率的同時，盡量減小路徑代價及轉折角度。

(3)路徑平滑。在去除冗余點后，獲得的路徑雖然轉折角度有所減少，但是路徑的轉角處并不是平滑的曲線。因此為實現救援機器人的平穩運動，本文采用了5 次B 樣條曲線對路徑轉角進行進一步平滑。通過5 次B 樣條曲線擬合，可獲得更加平滑的最終路徑，保障救援機器人運動時速度和角度的平穩性。

通過仿真實驗可知，救援機器人采用改進A*算法進行路徑規劃可以獲得代價更小、轉折角度更小的路徑。故改進A*算法滿足使用需求，且具有一定的應用價值。

4 結論

a.本文采用的路徑優化方法不僅可以用于傳統A*算法，也可用于其他全局路徑規劃算法，如Dijkstra 算法、RRT 算法等。以上算法均可以在獲得初始路徑后通過重連路徑點、曲線擬合來獲得代價更小、更平滑的路徑。

b.改進A*算法在傳統A*算法的基礎上，擴展機器人的鄰接點，最多允許機器人向周圍24 個柵格移動，增加了機器人可能的移動方向，可以快速獲得初始路徑；并對初始路徑重復去除冗余點，再利用5 次B 樣條曲線進行路徑平滑，最終獲得路徑代價更小、累計轉折角度更小的優化路徑。

c.通過仿真結果分析可知，改進A*算法得到的最終路徑相比于傳統A*算法，路徑代價和路徑轉折角度都有明顯降低，并且搜索節點數量也有顯著減少(若隨機生成的障礙物柵格集中分布于最優路徑附近，則搜索節點數量不會有顯著變化)。故該改進A*算法能滿足救援機器人對“最優”路徑的需求，可以進行實際應用。

d.改進A*算法可以通過多次調用去除冗余點函數RemovePoint和路徑分割函數Spline，不斷處理路徑，獲得近似為“最優”的優化路徑，但是多次調用函數會占用大量計算機內存，降低計算機處理速度。故后續研究應針對該問題進行進一步優化。同時，為進一步減少生成初始路徑時的擴展節點數量，可采用跳點搜索法(Jump Point Search，JPS)等進行預處理。