煤礦救援機器人路徑規劃研究 朱洪波

關鍵詞：煤礦救援機器人；路徑規劃；雙向A*算法；分層平滑；動態窗口法；動態加權因子；歸正因子；動態避障

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

0引言

礦難發生后，災區環境復雜，倉促救援可能導致人員傷亡。 煤礦救援機器人具有在危險區域內感知周圍環境的能力，從而顯著提高煤礦救災行動的效率，最大限度地減少人員傷亡[1]。

在礦井災后復雜環境中如何為煤礦救援機器人快速規劃出一條從指定起點至終點的無碰撞最優路徑是眾多學者考慮的重點，也是實現煤礦救援機器人自主完成各項任務的前置基礎。根據煤礦機器人對環境信息的掌握程度可將煤礦救援機器人路徑規劃分為動態局部路徑規劃和靜態全局路徑規劃[2]。局部路徑規劃算法適用于動態環境，注重煤礦救援機器人的動態避障能力。常用的局部路徑規劃算法主要包括動態窗口法（Dynamic WindowApproach，DWA）[3-4]、人工勢場法（Artificial PotentialField，APF）[5-6] 等。全局路徑規劃常用于靜態環境，實時性較差。目前常用的全局路徑規劃算法主要包括A*算法[7]、蟻群算法（Ant Colony" Optimization，ACO） [8]、遺傳算法（Genetic Algorithm， GA） [9-10]、迪杰斯特拉算法（Dijkstra's Algorithm， Dijkstra）[11-12]、快速擴展隨機樹算法（Rapidly?exploring Random Tree，RRT）[13-14]、跳點搜索算法（Jump Point Search， JPS）[15-16]等。DWA 考慮了速度和加速度的限制，是一種基于預測控制理論的方法，其優點在于能夠實現未知環境中的實時動態避障，且規劃出的路徑較為光滑。A*算法引入了啟發式信息作為決策輔助，在路徑規劃過程中不需要遍歷整個地圖，從而降低了計算復雜度并提升了路徑搜索效率。因此，這2 種算法在煤礦井下路徑規劃中得到廣泛應用。但DWA在大尺度復雜環境中缺乏中間引導點而無法得到最優路徑，甚至無法到達目標點；傳統A*算法在煤礦救援機器人路徑規劃的應用中存在拐點多、路徑不平滑等問題，不利于機器人的運行。針對以上算法在煤礦井下救援環境中的不足，許多學者對其進行了優化與改進。文獻[17]通過提取若干路徑節點作為關鍵點改進了A*算法，有效解決了路徑冗余點和轉折次數多的問題，但并未考慮路徑搜索效率和安全性。文獻[18]通過改進A*算法的領域搜索條件，并利用三次樣條曲線擬合路徑，在一定程度上提高了路徑搜索效率和平滑性，但路徑安全性差且無法進行實時避障。文獻[19]通過引入曲率相似度評價因子改進了DWA，有效提升了路徑的安全性，但仍存在大量冗余路段。文獻[20-21]通過提取全局路徑的關鍵點作為DWA的臨時目標點，有效解決了全局路徑規劃算法實時性差、局部路徑規劃算法易陷入陷阱和局部最優的問題，但算法實時尋路效率低且存在冗余路段。

煤礦救援機器人在采用雙向A*算法進行路徑規劃時，因缺乏方向引導導致正反向搜索不相交，從而降低搜索效率。另外，在煤礦未知環境中，現有全局路徑規劃算法融合DWA 在完成全局路徑規劃后，需依賴全局路徑的關鍵點引導DWA 再次從起點到終點進行二次路徑規劃，導致實時尋路效率低，并引入冗余路段。針對上述問題，本文提出一種基于分層平滑優化雙向A*（ Hierarchical Smoothing Optimization A*，HSTA*）引導DWA（HSTA*?G?DWA）的煤礦救援機器人路徑規劃方法。首先將動態加權因子、歸正因子和碰撞約束函數的調整機制引入雙向A*算法中，以提升路徑搜索的方向性、路徑搜索效率及路徑的安全性；然后采用分層平滑優化方式去除冗余節點和冗余路段，并通過三次B樣條曲線對路徑進行進一步平滑處理，以生成安全光滑的初始全局路徑；最后引導DWA 優化生成局部修正的實時動態避障路徑，從而實現安全、平滑、高效的煤礦井下路徑規劃。

1分層平滑優化雙向A*算法

1.1環境建模

在平面坐標軸XOY 創建尺寸為42×42的環境地圖，如圖1所示， 其中黑色方格表示不可到達的障礙物區域，白色方格為煤礦救援機器人可自由行走的無障礙物區域。

1.2動態加權和碰撞約束的雙向A*路徑規劃方法

針對雙向A*算法的代價函數會導致正反向搜索路徑不相交及路徑搜索效率和安全性低等問題，提出了一種動態加權因子和歸正因子修正的代價函數與碰撞約束的雙向A*路徑規劃方法，對正反向代價函數進行改進。以正向代價函數為例，改進公式為

同理可得反向代價函數。

1.3分層平滑優化策略

為了消除非光滑轉角、冗余節點及冗余路段，設計并引入了分層平滑優化策略，其可分為內層冗余點、轉折角優化和外層路徑平滑生成優化，如圖2所示。

內層優化的目的是消除路徑中的冗余節點和不必要的轉彎，以減少路徑的拐角和長度。

1）冗余點精簡。首先對直線路段上的節點進行精簡，只保留使路徑方向改變的節點，并將精簡后的節點存入節點列表中，記節點列表的集合為x=x[x₁，x₂……x_n]。

2）轉折角精簡。若節點x₀和x₄能直接連線且連線路徑與最近障礙物距離d大于安全距離，則x₀和x₄之間的x₁， x₂和x₃被當作冗余轉折角去除，從起點到終點依次遍歷整個集合，剔除所有的冗余轉折角（圖2（a））。

由于內層優化后的路徑是分段的，不夠平滑，煤礦救援機器人在拐點處可能會因為方向的變化無法正常救援，影響煤礦救援機器人的穩定性，所以需通過外層優化（圖2（b））對路徑進行進一步平滑處理，使路徑平滑且安全。

1） 在空間M 中定義起始節點與目標節點，并采用動態加權因子和歸正因子對雙向A*算法的代價函數進行改進。

2） 將起始節點和目標節點分別添加至open1 列表和open2 列表中，作為正向搜索和反向搜索的初始節點。

3） 對正向搜索和反向搜索的初始節點進行鄰域擴展，此時起始節點的坐標分別被加入到close1 列表和close2 列表中。

4） 通過計算正向和反向的代價函數值，將子節點分別按順序存入2 個open 列表中，并分別更新當前節點。

5） 判斷正向和反向擴展中選出的最優節點是否為同一節點，若不是，則繼續在正反2 個方向上進行鄰域擴展，重復上述過程，直到找到最終目標節點。若是，則表明雙向搜索已相遇，路徑搜索成功。

6） 利用分層平滑優化策略消除路徑中的冗余路徑節點和轉折節點，采用三次B 樣條曲線對路徑進行平滑處理，得到初始全局路徑。

7） 若煤礦救援機器人按照初始全局路徑行駛過程中未探測到未知障礙物，則煤礦救援機器人按照初始全局路徑行駛。

8） 若煤礦救援機器人按照初始全局路徑行駛過程中探測到未知障礙物時，則利用全局路徑引導DWA 實現局部動態避障。

9） 如果煤礦救援機器人處于靜態場景，則引導煤礦救援機器人回到全局路徑繼續行駛，并跟隨初始全局路徑運動；若不處于靜態場景，則繼續利用全局路徑引導DWA 完成局部動態避障。

10） 如果機器人沒有到達目標節點，則繼續重復步驟7）—步驟9）；若到達目標節點，則路徑規劃完成。

3實驗分析

3.1HSTA*?G?DWA算法仿真實驗

為驗證HSTA*?G?DWA算法的正確性與有效性，在 Matlab 中進行算法的仿真實驗驗證。在靜態場景下，將HSTA*?G?DWA算法和雙向A*算法、傳統A*算法進行仿真對比分析，所得實驗結果如圖6所示，數據見表1。

由圖6 和表1可知，在靜態環境下， HSTA*?G?DWA 算法路徑轉折角數較傳統A*算法、雙向A*算法分別降低了77.78%， 75%，搜索時間分別降低了81.82%，64.63%。說明HSTA*?G?DWA 算法規劃路徑更加平滑且效率更高。

在煤礦未知場景下將HSTA*?G?DWA 算法與RRT 算法、改進4 領域A*算法進行仿真對比分析，所得實驗結果如圖7 所示，數據見表2。

由圖7 和表2 可知，在存在未知靜態和動態障礙物的轉彎、連轉、轉巷3 種特殊復雜煤礦環境中，HSTA*?G?DWA 算法在路徑長度、路徑平滑性及路徑安全性方面均優于RRT 算法和改進4 領域A*算法，且HSTA*?G?DWA 算法可以實現實時避障。

在煤礦環境下將HSTA*?G?DWA算法與傳統A*算法、Dijkstra算法和文獻[20]融合算法進行對比分析，所得實驗結果如圖8所示，數據見表3。

由圖8 和表3 可知，在相同煤礦環境下，傳統A*算法和Dijkstra 算法路徑平滑性和安全性較差且不能實現實時避障，當煤礦環境中出現未知障礙物時可能會導致路徑規劃失敗。HSTA*?G?DWA 算法規劃路徑長度較文獻[20]融合算法降低了2.45%，搜索時間降低了70.48%，這是因為HSTA*?G?DWA算法無需在完成全局路徑規劃后使煤礦救援機器人依賴全局路徑的關鍵點作為中間引導點，而是再次利用DWA 從起點到終點進行二次路徑規劃并行駛至終點，當煤礦救援機器人按照全局路徑行駛過程中探測到未知障礙物時，按照局部避障路徑引導優化生成機制生成避障與返回全局路徑的局部動態修正路徑行駛，安全避障后回到全局路徑繼續行駛至終點。

3.2實驗室環境實驗

為進一步驗證HSTA*?G?DWA 算法的有效性和可行性，利用煤礦救援機器人進行實驗室環境實驗，實驗室環境如圖9 所示，其中圖9（a）為不存在未知障礙物的靜態環境，圖9（b）為存在未知障礙物的未知環境（臨時增加靜態障礙物替代未知障礙物）。在機器人操作系統（Robot Operating System，ROS）中完成煤礦救援機器人的自身定位和地圖構建，并完成路徑軌跡規劃，所得靜態環境路徑如圖10 所示，其中綠色實線為煤礦救援機器人從起點到終點的路徑。

由圖10可看出，在靜態環境下，HSTA*?G?DWA算法可以有效地規劃出一條安全平滑的路徑，煤礦救援機器人能夠按照所規劃的路徑行駛，并安全避開障礙物后到達終點。

為驗證HSTA*?G?DWA 算法的實時避障能力，在實驗室環境中分別臨時增加3 個未知障礙物，所得實驗結果如圖11所示。

由圖11可看出，隨著煤礦救援機器人向終點不斷運動，在煤礦救援機器人探測到第1個未知障礙物時，利用DWA 對局部路徑進行調整，安全避開第1個障礙物后回到全局路徑繼續運動。當煤礦救援機器人再次探測到第2 個和第3 個未知障礙物時，通過相同的方式進行避障調整直至到達終點。

為了進一步驗證本文所提算法的優越性， 將HSTA*?G?DWA 算法與傳統A*算法分別在靜態環境與未知環境中進行對比分析，其中傳統A*算法實驗結果如圖12所示。

由圖10—圖12 可看出，在靜態環境下，煤礦救援機器人采用傳統A*算法進行路徑規劃時，機器人行駛的路徑比較彎折，非光滑轉折角數為10個，機器人邊緣距障礙物的最小距離為0.11 m，機器人到達終點所用時間為27.54 s。當采用HSTA*?G?DWA 算法進行路徑規劃時，煤礦救援機器人行駛的路徑較為光滑，機器人邊緣距障礙物的最小距離為0.82 m，路徑安全性提高，機器人到達終點所用時間為11.36 s，較傳統A*算法縮短了58.75%。在未知環境中，煤礦救援機器人采用傳統A*算法進行路徑規劃時，無法成功避開環境中存在的未知障礙物，機器人邊緣距障礙物的最小距離為0，且路徑較為彎折。當采用HSTA*?G?DWA算法進行路徑規劃時，煤礦救援機器人行駛路徑較為光滑，且能夠成功避開環境中出現的未知障礙物并到達終點，機器人邊緣距障礙物的最小距離為0.86m。

4結論

1）在雙向A*算法中引入動態加權因子、碰撞約束函數和歸正因子的調整機制，提高了路徑的搜索效率和安全性。仿真結果表明，經過上述優化后，雙向A*算法規劃的路徑能夠確保煤礦救援機器人與巷道壁、障礙物之間保持安全距離，且路徑搜索時間較傳統A*算法和雙向A*算法分別減少了81.82%和64.63%。

2）采用分層平滑優化策略消除路徑中的冗余點和轉折角，減少節點數量和路徑長度，提高路徑平滑性。仿真結果表明，經路徑平滑處理后，路徑轉折角數較雙向A*算法和傳統A*算法分別減少了75% 和77.8%，路徑平滑性得到改善。

3）構建局部避障路徑引導優化生成機制，提升了實時尋路效率。仿真結果表明，在相同煤礦環境下相比于現有融合算法，HSTA*?G?DWA算法路徑搜索時間和路徑長度分別減少了70.48% 和2.45%。

4）對煤礦救援機器人在存在未知靜態和動態障礙物的轉彎、連轉、轉巷3種巷道中進行尋路搜索仿真實驗。結果表明，在不同巷道環境下，HSTA*?G?DWA算法規劃路徑長度較RRT 算法和改進A*算法分別平均減少了10.34% 和14.28%，并且具有更好的路徑平滑性，可以實時避開環境中出現的未知障礙物。

5）在實驗室環境下驗證了HSTA*?G?DWA算法的可行性，然而在真實復雜的煤礦救援場景中，需要考慮到煤礦救援機器人的防爆和通信性能等方面。未來的研究將綜合考慮上述因素對算法進行改進范疇，以提高煤礦救援機器人在復雜煤礦救援環境中的應用價值。