基于改進A*算法的煤礦救援機器人路徑規(guī)劃

姜媛媛 豐雪艷

摘要：路徑規(guī)劃是煤礦救援機器人研究的重要內容之一。針對災后煤礦環(huán)境非結構化的特點，以及傳統(tǒng) A*算法規(guī)劃的路徑長度非最短、拐彎次數(shù)多和平滑度較差等問題，提出一種基于改進 A*算法的煤礦救援機器人路徑規(guī)劃方法。對真實環(huán)境中的地圖信息進行二值化處理，構建柵格地圖；判斷當前點與目標點的相對位置，利用改進 A*算法進行路徑規(guī)劃，得到一條從當前點到目標點的路徑；利用 Douglas-Peucker（D?P）算法提取路徑上的關鍵節(jié)點，采用三次樣條插值函數(shù)對關鍵節(jié)點進行擬合，完成對路徑的平滑處理。改進 A*算法將傳統(tǒng) A*算法的8鄰域搜索擴展為有目的性的13鄰域搜索，在進行路徑搜索時，先對當前點和目標點的位置關系進行判斷，從而減少路徑節(jié)點，減小路徑長度，提升路徑平滑度。Matlab仿真結果表明：與8鄰域 A*算法、24鄰域 A*算法、48鄰域 A*算法相比，改進 A*算法在路徑長度、拐彎次數(shù)、平滑度等方面有一定優(yōu)化，更適用于煤礦救援機器人路徑規(guī)劃；與 Fuzzy算法相比，改進 A*算法路徑規(guī)劃所用時間更短，規(guī)劃的路徑長度更短，拐彎次數(shù)更少。

關鍵詞：煤礦救援機器人；路徑規(guī)劃；A*算法；鄰域擴展；Douglas-Peucker算法；路徑平滑；三次樣條插值函數(shù)

中圖分類號： TD774??? 文獻標志碼： A

Path planning of coal mine rescue robot based on improved A* algorithm

JIANG Yuanyuan1，2， FENG Xueyan1

（1. School of Electrical and Information Engineering， Anhui University of Science and Technology， Huainan 232000，China；2. Institute of Environmental Friendly Materials and Occupational Health （Wuhu），Anhui University of Science and Technology， Wuhu 241003， China）

Abstract： Path planning is one of the important contents of research on coal mine rescue robots. A path planning method for coal mine rescue robots based on improved A* algorithm is proposed to address the unstructured features of post disaster coal mine environments and the problems of non-shortest path length， multiple turns， and poor smoothness of path planned by traditional A* algorithm. The method constructs raster maps by binarizing map information in real environments， determines the relative position between the current point and the target point， and uses the improved A* algorithm for path planning. Then a path from the current point to the target point is obtained. Douglas-Pucker （D-P） algorithm is used to extract key nodes on the path， and cubic spline interpolation function is used to fit the key nodes， thereby completing the smooth processing of the path. The improved A* algorithm expands the traditional A* algorithm's 8 neighborhood search to a purposeful 13 neighborhood search. When conducting path search， the position relationship between the current point and thetarget point is first determined， thereby reducing path nodes and length， and improving path smoothness. The Matlab simulation results show that compared with the 8 neighborhood A* algorithm， 24 neighborhood A* algorithm， and 48 neighborhood A* algorithm， the improved A* algorithm has certain optimizations in pathlength， number of turns and smoothness. It is more suitable for path planning of coal mine rescue robots. Compared with the Fuzzy algorithm， the improved A* algorithm achieve shorter path planning time， shorter planned pathlength， and fewer turns.

Key words： coal mine rescue robot; path planning; A* algorithm; neighborhood extension; Douglas-Pucker algorithm; smooth path; cubic spline interpolation function

0 引言

煤礦救援機器人可在危險區(qū)域對環(huán)境進行探測，對礦工實施救援，對于提高煤礦災后救援效率、減少二次傷亡具有重要意義[1]。路徑規(guī)劃是煤礦救援機器人研究的重要內容之一。從起點到終點規(guī)劃一條連續(xù)且與路徑上的障礙物不發(fā)生任何碰撞的路徑，可使煤礦救援機器人在頂板坍塌及爆炸后散落物遍布的非結構化環(huán)境中快速到達事故現(xiàn)場并實施救援。

傳統(tǒng)的全局路徑規(guī)劃算法有 Dijkstra 算法[2]、快速擴展隨機樹算法[3]、A*算法[4]等，局部路徑規(guī)劃算法有動態(tài)窗口法[5]、人工勢場法[6]等。其中具有啟發(fā)式思想的 A*算法被認為是進行靜態(tài)全局路徑規(guī)劃最有效的方法，但其規(guī)劃出的路徑存在拐角過大、擴展節(jié)點較多等缺點[7]。文獻[8]將傳統(tǒng) A*算法的 8鄰域搜索擴展為24鄰域搜索，改進后的 A*算法增加了搜索方向，規(guī)劃的路徑長度有所減小，但以犧牲時間為代價，擴展后的24鄰域搜索方向不具有目的性。文獻[9]通過基于象限判別的改進 A*算法尋找從起始點到目標點的最佳路徑。文獻[10]將傳統(tǒng)的 A*算法改進為7×7的 A*算法，在擴展搜索鄰域的基礎上去除與搜索鄰域同方向的子節(jié)點，并將改進 A*算法與動態(tài)窗口法相結合，實現(xiàn)了機器人實時避障。文獻[11]優(yōu)化了 A*算法的啟發(fā)搜索函數(shù)，在保留關鍵點的基礎上剔除所規(guī)劃路徑上的冗余節(jié)點和多余轉折點。文獻[12]采用 Douglas-Peucker（D?P）算法提取傳統(tǒng) A*算法所規(guī)劃路徑的關鍵節(jié)點，通過三次樣條插值函數(shù)對規(guī)劃的路徑進行平滑，改善了傳統(tǒng) A*算法所規(guī)劃的路徑節(jié)點多、拐彎次數(shù)多的問題，但因為傳統(tǒng)8鄰域搜索單次搜索的鄰域范圍小，所以需要搜索的次數(shù)多，計算量也會相應增大，不適用于規(guī)模較大的地圖。

針對上述問題，本文提出一種基于改進 A*算法搜索鄰域的煤礦救援機器人路徑規(guī)劃方法，在傳統(tǒng) A*算法的基礎上，從原來的8鄰域搜索擴展為有目的性的13鄰域搜索，并采用 D?P 算法提取路徑中的關鍵節(jié)點，再用三次樣條插值函數(shù)對基于關鍵節(jié)點的整段路徑進行擬合處理，得到一條相對平滑的路徑。

1 煤礦救援機器人路徑規(guī)劃方法

1.1 改進 A*算法

A*算法在 Dijkstra 算法的基礎上引入評估函數(shù)，是一種啟發(fā)式搜索算法[13-14]，評估函數(shù)的引入提高了搜索效率。 A*算法的表達式為

式中：F（n）為節(jié)點n的代價評估函數(shù)；G（n）為煤礦救援機器人從起始點行駛到當前點n所花費的實際代價； H（n）為煤礦救援機器人從當前點n行駛到目標點將要花費的預估代價。

H（n）可采用歐幾里得距離、曼哈頓距離、對角線距離表示，本文采用歐幾里得距離表示，其公式為

式中：（xg，yg）為目標點 g 的坐標；（xn，yn）為當前點 n的坐標。

傳統(tǒng)的 A*算法一般采用8鄰域搜索方式搜索下一個要擴展的節(jié)點，其鄰域搜索的角度被限定為45o 的整數(shù)倍，傳統(tǒng) A*算法搜索方式如圖1所示。

為解決傳統(tǒng) A*算法8鄰域搜索存在的問題，本文將傳統(tǒng) A*算法的8鄰域搜索擴展為有目的性的13鄰域搜索，在進行路徑搜索時，先對當前點和目標點的位置關系進行判斷。首先定義一個向量P：

當P1≥0且P2≤0時，目標點位于當前點的第一象限；當P1≤0且P2≤0時，目標點位于當前點的第二象限；P1≤0且P2≥0時，目標點位于當前點的第三象限；P1≥0且P2≥0時，目標點位于當前點的第四象限。以目標點位于當前點的第四象限為例，位置關系如圖2所示，xoy為節(jié)點所在坐標系。

當目標點位于當前點的不同象限時，改進 A*算法的搜索鄰域如圖3所示。改進 A*算法可以有目的性地向目標點進行擴展搜索，相比于傳統(tǒng)的8鄰域 A*算法，雖然搜索時間略有增加，但是路徑節(jié)點減少，路徑長度減小，平滑度有所提升，更適用于煤礦救援機器人在復雜非結構化環(huán)境中的救援工作。

1.2 路徑平滑算法

D?P 算法是一種經典的直線簡化算法。該算法通過設置合適的距離閾值?，有效地識別曲線特征點[15-16]。為了優(yōu)化路徑，本文采用 D?P 算法提取改進 A*算法搜索路徑中的關鍵節(jié)點，再采用三次樣條插值函數(shù)對 D?P算法提取的關鍵節(jié)點進行擬合，從而完成對路徑的平滑處理。

D?P 算法提取關鍵節(jié)點的步驟如下[17]：

1）將路徑上的起點A 和終點B 連接，形成一條線段 AB。

2）找出路徑上與線段 AB距離最遠的點并記為 C ，計算點 C到線段 AB 的距離，記為 L。

3）將預先設置的距離閾值ε與距離 L 進行比較，如果ε大于 L，則將線段 AB 作為該段路徑的近似。

4）如果ε小于 L，則連接點A 和點 C，連接點 B 和點 C，點 C 即為要提取的關鍵節(jié)點，分別對線段 AC 和 BC 重復步驟1）?步驟3）的處理。

5）處理完每段路徑后，依次連接所提取的關鍵節(jié)點，形成一條拐彎次數(shù)較少的新路徑。

樣條插值常用于工業(yè)設計，三次樣條插值是其中應用較廣泛的一種，其在保留分段低次插值多項式優(yōu)點的基礎上，提高了插值函數(shù)的光滑性。三次樣條插值函數(shù)的定義如下：給定區(qū)間[a，b]上的 m 個節(jié)點 X（X1，X2，···， Xm）和這些點上的函數(shù)值 f（Xi ）= Yi （i =1，2，··· ， m），若 S（X）滿足以下條件，則稱 S（X）為函數(shù)f（X）關于節(jié)點X1，X2，…，Xm的三次樣條插值函數(shù)[18-19]：① S（Xi ）= Yi；②在每個小區(qū)間[Xi，Xi+1]上都是三次多項式；③ S （X），S ′（X），S ′′（X）在[a，b]內連續(xù)。

若S ′′（X）在[a，b]內連續(xù)，則還需滿足以下條件：

1.3 路徑規(guī)劃方法流程

在進行路徑規(guī)劃前，首先，對真實環(huán)境中的地圖信息進行二值化處理[20-21]，構建柵格地圖；其次，判斷當前點與目標點的相對位置，利用改進 A*算法進行路徑規(guī)劃，得到一條從當前點到目標點的路徑；再次，利用 D?P算法提取路徑上的關鍵節(jié)點；最后，利用三次樣條插值函數(shù)對路徑進行平滑處理。具體流程如圖4所示。

2 仿真結果與分析

當?shù)V井發(fā)生瓦斯爆炸、粉塵爆炸等災害事故后，煤礦環(huán)境會被爆炸所產生的沖擊波破壞，導致巷道內散落很多大小不一、形狀各異的煤塊和施工器材。為了驗證改進 A*算法及路徑平滑算法在該環(huán)境下的有效性，利用Matlab R2022a進行了2組仿真實驗。在簡單環(huán)境下將改進 A*算法與8鄰域 A*算法、24鄰域 A*算法、48鄰域 A*算法進行對比，用不同形狀的黑色圖形代表災后煤礦環(huán)境中形狀各異的障礙物。由于礦井內存在一些不規(guī)則的復雜障礙物，再次在復雜環(huán)境下進行仿真實驗。環(huán)境地圖大小均為500 m×500 m。

簡單環(huán)境下起點為（70 m，40 m），終點為（450 m，480 m），改進 A*算法和不同鄰域 A*算法所生成的路徑及平滑后的路徑如圖5所示，各算法性能對比見表1。由表1可知，在起點和終點相同的情況下，改進 A*算法較傳統(tǒng)的8鄰域 A*算法搜索所用的時間略長，但路徑長度減小了2.45%，拐彎次數(shù)減少了41.18%，平滑后路徑長度減小了1.27%，路徑節(jié)點數(shù)減少了22.92%。24鄰域 A*算法搜索的路徑與本文算法搜索的路徑長度、路徑節(jié)點數(shù)相同，路徑平滑前后的拐彎次數(shù)也相同，但搜索時間有所增加。與本文算法相比，48鄰域 A*算法搜索所用的時間略長，但搜索的路徑長度略減小，拐彎次數(shù)略減少，平滑后的路徑比本文算法長。

復雜環(huán)境下起點為（30 m，40 m），終點為（450 m，480 m），改進 A*算法和不同鄰域 A*算法所生成的路徑及平滑后的路徑如圖6所示，各算法性能對比見表2。由表2可知，在起點和終點相同的情況下，改進 A*算法較傳統(tǒng)的8鄰域 A*算法搜索所用的時間略長，但路徑長度減小了4.28%，拐彎次數(shù)減少了26.67%，平滑后路徑長度減小了3.82%，路徑節(jié)點數(shù)減少了28.1%。24鄰域 A*算法搜索的路徑與本文算法搜索的路徑長度、路徑節(jié)點數(shù)相同，路徑平滑前后的拐彎次數(shù)也相同，但搜索時間有所增加。與本文算法相比，48鄰域 A*算法搜索所用的時間略長，拐彎次數(shù)相同，但路徑長度略有減小，而平滑后的路徑長度、拐彎次數(shù)與本文算法一致。綜合表1、表2可知，本文算法更適用于煤礦救援機器人路徑規(guī)劃。

在同一環(huán)境下將本文算法和 Fuzzy 算法進行對比，設煤礦救援機器人的起始點為（100 m，40 m），目標點為（400 m，460 m），對比結果如圖7所示，算法性能測試對比見表3。由圖7可知，本文算法所規(guī)劃的路徑拐彎次數(shù)只有3，路徑較為平滑。 Fuzzy 算法所規(guī)劃的路徑雖然也相對平滑，但拐彎角度過大，不利于煤礦救援機器人實施災后救援工作。由表3可知，本文算法所規(guī)劃的路徑長度明顯小于 Fuzzy 算法所規(guī)劃的路徑，所用時間更短，路徑拐彎次數(shù)更少。減少拐彎次數(shù)可降低煤礦救援機器人工作時進行加減速以調整運動方向的頻率，而路徑長度的減小則可節(jié)省救援所用時間，提高救援效率。

3 結論

1）改進 A*算法在傳統(tǒng) A*算法的基礎上，將可搜索鄰域個數(shù)從8個增加到13個，減少了無用搜索，能夠有效解決傳統(tǒng) A*算法規(guī)劃的路徑節(jié)點數(shù)多、長度非最短的問題。

2）采用 D?P 算法提取改進 A*算法搜索路徑中的關鍵節(jié)點，再采用三次樣條插值函數(shù)對 D?P 算法提取的關鍵節(jié)點進行擬合，從而完成對路徑的平滑處理，解決了傳統(tǒng) A*算法規(guī)劃的路徑平滑度較差、拐彎次數(shù)較多的問題。

3）Matlab仿真結果表明：與8鄰域 A*算法、24鄰域 A*算法、48鄰域 A*算法相比，改進 A*算法在路徑長度、拐彎次數(shù)、平滑度等方面有一定優(yōu)化，更適用于煤礦救援機器人路徑規(guī)劃；與 Fuzzy 算法相比，改進 A*算法路徑規(guī)劃所用時間更短，規(guī)劃的路徑長度更短，拐彎次數(shù)更少。

參考文獻（References）：

[1] 朱華，由韶澤.新型煤礦救援機器人研發(fā)與試驗[J].煤炭學報，2020，45（6）：2170-2181.

ZHU Hua，YOU Shaoze. Research and experiment of a new type of coal mine rescue robot[J]. Journal of China Coal Society，2020，45（6）：2170-2181.

[2] 徐興，俞旭陽，趙蕓，等.基于改進遺傳算法的移動機器人全局路徑規(guī)劃[J].計算機集成制造系統(tǒng)，2022，28（6）：1659-1672.

XU Xing，YU Xuyang，ZHAO Yun，et al. Global path planning of mobile robot based on improved genetic algorithm[J]. Computer? Integrated Manufacturing Systems，2022，28（6）：1659-1672.

[3] 王梓強，胡曉光，李曉筱，等.移動機器人全局路徑規(guī)劃算法綜述[J].計算機科學，2021，48（10）：19-29.

WANG Ziqiang，HU Xiaoguang，LI Xiaoxiao，et al. Overview of global path planning algorithms for mobile robots[J]. Computer Science，2021，48（10）：19-29.

[4] SHANG E，DAI Bin，NIE Yiming，et al. An improved A-star based path planning algorithm for autonomous land vehicles[J]. International Journal of Advanced Robotic Systems，2020，17（5）：1-13.

[5] 張瑜，宋荊洲，張琪祁.基于改進動態(tài)窗口法的戶外清掃機器人局部路徑規(guī)劃[J].機器人，2020，42（5）：617-625.

ZHANG Yu，SONG Jingzhou，ZHANG Qiqi. Local path planning of outdoor cleaning robot based on an improved DWA[J]. Robot，2020，42（5）：617-625.

[6] MIN Huasong，LIN Yunhan，WANG Sijing，et al. Path planning of mobile robot by mixing experience with modified artificial potential field method[J]. Advances in Mechanical Engineering，2015，7（12）：1-17.

[7] 郭曉靜，楊卓橙.基于鄰域拓展的靜態(tài)路徑規(guī)劃A*算法研究[J].計算機工程與應用，2022，58（8）：168-174.

GUO Xiaojing， YANG Zhuocheng. Improved A* algorithm based on neighbor extension in static environment[J].? Computer? Engineering? and Applications，2022，58（8）：168-174.

[8] 崔寶俠，王淼弛，段勇.基于可搜索24鄰域的A*算法路徑規(guī)劃[J].沈陽工業(yè)大學學報，2018，40（2）：180-184.

CUI Baoxia，WANG Miaochi， DUAN Yong. Path planning for A* algorithm based on searching 24 neighborhoods[J]. Journal of Shenyang University of Technology，2018，40（2）：180-184.

[9] 劉小佳，狄夢然，梁利東，等.基于象限判別下的改進 A*算法路徑規(guī)劃[J].常州工學院學報，2020，33（2）：26-30，35.

LIU Xiaojia，DI Mengran，LIANG Lidong，et al. On the improved path planning of A* algorithm based on quadrant discrimination[J]. Journal of Changzhou Institute of Technology，2020，33（2）：26-30，35.

[10] 槐創(chuàng)鋒，郭龍，賈雪艷，等.改進A*算法與動態(tài)窗口法的機器人動態(tài)路徑規(guī)劃[J].計算機工程與應用，2021，57（8）：244-248.

HUAI Chuangfeng，GUO Long，JIA Xueyan，et al. Improved A* algorithm and dynamic window method for robot dynamic path planning[J]. Computer Engineering and Applications，2021，57（8）：244-248.

[11] 程傳奇，郝向陽，李建勝，等.融合改進A*算法和動態(tài)窗口法的全局動態(tài)路徑規(guī)劃[J].西安交通大學學報，2017，51（11）：137-143.

CHENG Chuanqi，HAO Xiangyang，LI Jiansheng，et al. Global dynamic path planning based on fusion of improved A* algorithm and dynamic window approach [J]. Journal of Xi'an Jiaotong University，2017，51（11）：137-143.

[12] 陶德俊，姜媛媛，劉延彬，等.煤礦救援機器人路徑平滑算法研究[J].工礦自動化，2019，45（10）：49-54.

TAO Dejun，JIANG Yuanyuan，LIU Yanbin，et al. Research on path smoothing algorithm of coal mine rescue robot[J]. Industry and Mine Automation，2019，45（10）：49-54.

[13] 李梟揚，周德云，馮琦.基于分級規(guī)劃策略的A*算法多航跡規(guī)劃[J].系統(tǒng)工程與電子技術，2015，37（2）：318-322.

LI Xiaoyang，ZHOU Deyun，F(xiàn)ENG Qi. Multiple routes planning for A* algorithm based on hierarchical planning[J]. Systems Engineering and Electronics，2015，37（2）：318-322.

[14] FU Bing， CHEN Lin， ZHOU Yuntao， et al. An improved A* algorithm for the industrial robot path planning with high success rate and short length[J].Robotics & Autonomous Systems，2018，106：26-37.

[15] LI Chengming， GUO Peipei， WU Pengda， et al.Extraction of terrain feature lines from elevation contours using a directed adjacent relation tree [J]. International Journal of Geo-Information，2018，7（5）：163-177.

[16] ZHAO Liangbin， SHI Guoyou. A method? for simplifying ship trajectory based on improved Douglas- Peucker algorithm[J]. Ocean Engineering，2018，166（15）：37-46.

[17] 楊敏，陳媛媛，金澄，等.保持移動速度特征的軌跡線化簡方法[J].測繪學報，2017，46（12）：2016-2023.

YANG Min，CHEN Yuanyuan，JIN Cheng，et al. Amethod of speed-preserving trajectory simplification[J]. Acta Geodaetica et Cartographica Sinica，2017，46（12）：2016-2023.

[18] 胡崢楠，佘鋒.一種基于樣條插值的局部路徑規(guī)劃模型與實現(xiàn)[J].微型電腦應用，2020，36（11）：106-110.

HU Zhengnan，SHE Feng. A local path planning model and implementation based on spline interpolation[J]. Microcomputer Applications，2020，36（11）：106-110.

[19] 高曉，楊志強，庫新勃，等.基于三次樣條插值實現(xiàn)無人機高動態(tài)運動軌跡插值[J].全球定位系統(tǒng)，2020，45（1）：37-42.

GAO Xiao，YANG Zhiqiang，KU Xinbo，et al.3D- coordinate interpolation for UAV high dynamic positioning based on cubic spline interpolation[J]. GNSS World of China，2020，45（1）：37-42.

[20] 張金澤.水面無人艇路徑規(guī)劃及避障策略的研究[D].大連：大連海事大學，2022.

ZHANG Jinze. Research on path planning and obstacle avoidance strategy of unmanned surface vehicle[D]. Dalian：Dalian Maritime University，2022.

[21] 劉勝，張豪，晏齊忠，等.基于ACO?SA算法的變電站巡檢機器人路徑規(guī)劃[J].南方電網技術，2022，16（9）：75-82.

LIU Sheng，ZHANG Hao，YAN Qizhong，et al. Path planning of inspection robot in substation based on ACO-SA algorithm[J]. Southern? Power System Technology，2022，16（9）：75-82.