融合簡化可視圖和A*算法的礦用車輛全局路徑規劃算法

【編者按】礦山無人駕駛技術可顯著提高礦山生產效率、保障礦山生產安全，是智能化礦山的核心建設內容之一。目前，露天礦山無人駕駛技術已取得較大進展并實現初步商用，地下礦山無人駕駛由于環境惡劣、設備性能受限，技術發展稍顯遲緩，但亦在技術架構、感知設備、礦井車聯網、定位導航、路徑規劃方面取得了較大進展。為促進礦山無人駕駛理論及技術發展，提升礦山運輸無人駕駛水平，推進智能礦山建設，《工礦自動化》編輯部特邀西安科技大學張傳偉教授擔任客座主編，中國礦業大學胡青松教授、中煤科工集團常州研究院有限公司周李兵副研究員擔任客座副主編，于2024 年第10 期組織出版“礦山無人駕駛技術”專題。在專題刊出之際，衷心感謝各位專家學者的大力支持！

文章編號：1671?251X（2024）10?0012?09

DOI：10.13272/j.issn.1671-251x.2024070048

關鍵詞：井下無人駕駛；全局路徑規劃；簡化可視圖；A*算法；路徑平滑

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

0引言

隨著科技的不斷進步，煤礦井下運輸作業正逐漸向自動化和智能化轉型[1]。然而，煤礦井下環境的復雜性給運輸作業帶來了巨大挑戰。無人駕駛技術是實現礦用車輛安全高效智能化運輸的重要技術手段，也是煤炭行業智能化發展的必然趨勢，可使車輛能夠在極端環境下代替人的操作自主完成未知環境感知、車輛定位和導航，從而控制車輛運動[2-3]。煤礦井下全局路徑規劃是指在煤礦井下復雜環境中為運輸機器人或自動化運輸設備設計出一條從起點到終點的最優路徑[4-5]。路徑規劃是無人駕駛技術的核心，深入研究礦用無人駕駛車輛的路徑規劃問題對提升煤礦井下運輸效率具有重大意義[6-7]。

全局路徑規劃側重于在已知環境地圖的基礎上計算一條從起點到終點的最優路徑，通常涉及到圖搜索算法，如A*算法、Dijkstra 算法等[8]，這些算法能夠在地圖上搜索出一條避開所有已知障礙物的路徑。崔寶俠等[9]對傳統的A*算法進行了改進， 將8 鄰域搜索擴展為24 鄰域搜索，減少了規劃路徑的長度，但增加了計算時間。程傳奇等[10]對A*算法的啟發式搜索函數進行了優化，在保留路徑規劃過程中關鍵節點的同時，去除了冗余節點和不必要的轉折點， 從而提高了路徑規劃的效率和準確性。C.Richter 等[11]提出了一種基于學習啟發式函數的路徑搜索方法，提高了對未知空間的可見性，但這種方法在復雜三維環境中的適用性有限。H. Oleynikova 等[12]結合全局規劃和局部探索方法，實現了復雜環境中的安全行駛，但在已知空間范圍內選擇中間目標時較為保守，導致執行時間較長。黃迎港等[13]針對復雜環境設計了復合策略，以應對隨機事件問題，增強了環境感知能力。針對井下巷道環境中的路徑規劃，袁曉明等[14] 提出了一種集成先進感知、精確定位和智能路徑規劃的煤礦輔助運輸機器人技術方案，但該方案可能面臨成本高、計算復雜、環境適應性及傳感器性能受限等挑戰。黃友銳等[15]提出了結合膜計算和Informed RRT 的路徑規劃方法，通過調整步長和并行計算優化狹窄空間的路徑搜索，提高了搜索效率和路徑平滑性，但存在參數敏感性和泛化能力有限的問題。薛光輝等[16]提出了一種改進的人工勢場算法，用于解決煤礦井下機器人在狹窄巷道中的目標不可達和路徑振蕩問題，通過建立巷道邊界勢場、引入斥力勢場調節因子和轉角限制系數，提高了路徑規劃的安全性和效率。夏靜慧等[17] 提出了一種改進人工勢場算法，通過優化引力和斥力勢場模型并引入道路邊界斥力勢場，有效解決了傳統算法中的局部最小值和路徑偏離問題，提高了礦車在復雜礦區環境中的避障和行進效率，但參數選擇可能需要進行大量實驗。

為了提高礦用車輛在狹窄、彎曲及有未知障礙物的井下巷道中的路徑規劃效率，將環境地圖的建立和路徑搜索的過程融合在一起，提出了一種融合簡化可視圖（Simplified Visibility Graph，SVG）和A*算法的全局路徑規劃算法DVGA*。通過動態生成環境地圖并簡化，得到可視圖；將車輛在不同視點下選取的可視切點依次存入OPEN表，結合A*算法的估價函數在OPEN表中選取最短路徑節點放入CLOSED表并存儲路徑，同時刪除CLOSED表中的其余節點，從而對鏈表進行動態更新，循環此過程，直到OPEN表中出現終點；使用路徑平滑算法對鏈表中的路徑節點進行優化，得到最優路徑。

可視圖是一種圖結構，其節點代表障礙物頂點，邊表示節點之間可視連通性[18-19]。如果2 個節點之間的連線沒有被任何障礙物阻擋，則在這2 個節點之間添加一條邊。這種方法將環境轉換為一個圖結構，其中節點集合包括起點、目標點和障礙物頂點，邊集合則是這些點之間的可視直線段。構建可視圖時，將起點與視野內所有障礙物輪廓點相連并判斷連線是否穿過障礙物，若穿過則為無用路徑節點，若不穿過則計算其余輪廓點與終點的距離，選擇距離最短的輪廓點進行視野更新，并刪除之前的障礙物輪廓點和連線，達到簡化路徑節點和可視邊的效果。

動態切線可視圖通過可視切線表示車輛的可行路徑。在動態環境中，當障礙物的位置或形狀發生變化時，動態切線可視圖能夠快速更新，以反映新的可見性關系。隨著車輛的移動，通過傳感器不斷更新可見區域，并根據新的環境信息利用某種搜索算法調整路徑。最終，車輛通過不斷探索和更新路徑，逐步接近目標點并找到一條安全路徑。動態切線可視化如圖2 所示，黑色多邊形表示障礙物。

2融合SVG 和A*算法的DVGA*算法

2.1A*算法

全局路徑規劃算法主要分為啟發式搜索和智能算法。A*算法[8]是一種用于圖搜索和路徑查找的啟發式搜索算法，結合了Dijkstra算法的優點和啟發式搜索的靈活性，通過對路徑進行估價，找到從起點到目標點的最短路徑。A*算法基于啟發函數構造了估價函數，既考慮了新節點到起點的代價，又考慮了新節點到目標點的代價。

F（n） = g（n）+h（n） （2）

式中："F（n）為起點經過當前節點到目標點的估價函數；g（n）為當前節點到目標點的實際代價；"h（n）為啟發函數，是從當前節點到達目標點最佳路徑的估計代價。

2.2路徑生成

DVGA*算法原理如圖3所示，其中實線表示障礙物的邊可見，虛線表示障礙物的邊不可見。對于礦用車輛來說，其路徑是由可視切線構成的，沒有先驗地圖的情況下，車輛只能依靠傳感器獲得當前環境信息。隨著車輛視點位置變化，障礙物的可見性也在發生變化。車輛視點在不同位置可看到的障礙物邊界也在不斷發生變化。當車輛的視點位于點時，在可視范圍內，傳感器只能檢測到彼此互不遮擋的障礙物1 和障礙物2，因此可視切線圖包含４條可視切線和４個可視切點。為了走出當前障礙區且不與障礙物相碰，車輛根據A*算法進行計算，最終選擇可視切線圖中的可視切點O₁作為子目標點。車輛將可視切點O₁作為新的視點，在以O₁為視點的新視窗內，車輛視點范圍內可視切線圖包含6 條可視切線和6 個可視切點，如圖3（b）所示。同理，按照A*算法策略，選取其中一條路徑，依此類推，直至到達目標點。

DVGA*算法的具體步驟如下：

1）創建空鏈表OPEN和CLOSED，將起點O_start放人CLOSED表中。

2）通過車輛視點確定起點到車輛視點范圍內障礙物1和障礙物2的可視切點{A₁和A₂，B₁和B₂}，將確定的可視切點插入待擴展列表OPEN巾，若車輛想要越過障礙物1和障礙物2，必然要通過OPEN表中現存的任一可視切點。

3）根據DVGA*算法的估價函數計算OPEN表中待擴展可視節點的評價值，選擇F＇（O_i）值最小的節點（即估計路徑最短的可視切點）來擴展。

4）將選出的擴展可視切點A₂添加到CLOSED表中作為最優路徑點，并將其邊存儲為路徑。

5）清空OPEN表中除A₂外的其他節點，將A₂作為車輛的下一個視點，重復步驟2）一步驟4）。

6）若車輛視點范圍內出現了終點O_gogal，直接將其插入CLOSED表中，路徑搜索結束。

DVGA*算法路徑搜索偽代碼如下。

DVGA*算法與A*算法的主要區別：A*算法的OPEN表保留了之前步驟的所有已擴展節點，而DVGA*算法的OPEN表只保存當前擴展節點的后續待擴展節點，刪除了之前步驟中的已擴展節點，大大減少了OPEN表中保存的節點數量，從而降低了計算量和耗時。

2.3路徑平滑

CLOSED表中保存的是一條從起點到終點的路徑，受啟發函數的限制，獲取的路徑不保證為最短路徑。為了有效移除路徑中的冗余節點，使用路徑平滑算法消除冗余節點。路徑平滑流程如圖4所示。

對于路徑上的每個節點，檢查是否可直線到達下一個節點。如果CLOSED表中第 i+1 號節點為終點則算法結束，否則通過SMOOTH 算法[20]判斷第i 號節點到第i+2 號節點之間是否存在直線可達性，即2 個點之間能否連成不穿過障礙物的直線，如果存在則認為這2 個節點可直接相連，第 i+1 號節點是可從路徑中刪除的冗余節點。如果第 i 號節點與第i+2 號節點不存在直線可達性，則將 i 增加1，檢查下一個節點。不斷重復這個過程，直到路徑中的第 i+1號節點為終點，算法結束。最終輸出的路徑即為平滑后的路徑，刪除了所有不必要的中間節點，從而更直接和簡潔。路徑平滑前后對比如圖5所示。

3全局路徑規劃仿真實驗及井下試驗

3.1全局路徑規劃仿真

為了驗證DVGA*算法的有效性，設置模擬環境地圖，其中包含6 個隨機生成的障礙物，障礙物占環境地圖總面積的40%，起點和終點分別為S 和G。應用4 種算法在二維環境中進行路徑規劃仿真實驗，結果如圖6 所示。第1 種算法為完整可視圖+A*算法（CVGA*），建立完整可視圖后，使用A*算法搜索最優路徑，可看出該算法在前期可視地圖的構建上消耗了大量時間，而其中大部分可視邊與搜索出的最終路徑無關，可視邊數量的增加導致A*算法搜索效率低下。第2 種算法為DVG+A*算法（SVG?A*），使用文獻[21]中的方法建立SVG 后，再使用A*算法搜索最優路徑， 由于障礙物影響搜索路徑， 所以SVG?A*算法的效率不高。第3 種算法為SVGCA*算法[8]，利用穿越線篩選可視點并同步生成可視圖，該算法需要在可視點之間構建穿越線來確定是否存在障礙物并確定可視點，雖然擴展的節點數有所減少，但是算法迭代次數較多。第4 種算法為DVGA*算法，通過車輛視點直接確定障礙物的可視切點，相比SVG?A*算法減少了OPEN表中的節點數量，動態生成可視圖并通過平滑算法使路徑搜索更加高效。

模擬環境下4 種算法的路徑規劃數據見表1。CVGA*算法過于繁瑣且耗時長，SVG?A*算法可視圖構建和路徑查找加快，但仍不滿足實時應用要求。SVGCA*算法縮短了可視圖的構建時間和OPEN表長度，算法執行時間僅為SVG?A*算法的20%。DVGA*算法的擴展節點數量和OPEN表長度遠小于其他3 種算法，因此路徑搜索時間顯著縮短。DVGA*算法構建的可視圖耗時不到SVG?A*算法的20%，OPEN表長度最短。無論是時間復雜度，還是空間復雜度，DVGA*算法都優于其他3 種算法。

3.2全局路徑規劃實驗

為了進一步驗證DVGA*算法在實際應用中的可靠性和有效性，選取智能小車為實驗對象開展全局路徑規劃實驗。智能小車如圖7 所示，底盤系統由前置阿克曼式轉向機構、后置獨立雙電動機及驅動器組成，為小車提供驅動力。工控機搭載ROS 操作系統，同時嵌入路徑規劃算法，通過CAN 總線與車輛底盤進行交互通信，將運行指令發送到控制底盤，驅動器下發指令驅動小車電動機旋轉，進而實現小車的定位和路徑規劃。實驗硬件設備信息見表2。

選擇樓道作為實驗場地，樓道結構簡單、特征點少，與井下巷道環境的整體分布較為相似，并能模擬場景退化問題，如圖8 所示。

在已知全局地圖上添加2 個未知物體，一個是靜態的矩形箱子A，另一個是勻速運動的障礙物B，設置起點和目標點。對CVGA*， SVG?A*， SVGCA*及DVGA*算法進行對比實驗，點云地圖構建及路徑規劃結果如圖9 所示。圖9（a）和圖9（b）表明，在躲避靜態障礙物A 時， CVGA*和SVG?A*算法規劃的軌跡在巷道轉角的轉彎半徑較大；圖9（c）和圖9（d）表明， SVGCA*和DVGA*算法在巷道轉角的轉彎路徑很平滑，但SVGCA*算法避障距離較大。在躲避動態障礙物B 時，CVGA* 算法的避障路徑彎曲、易發生碰撞且沒有到達目標點，SVG?A*算法的避障距離過大， SVGCA*算法的避障距離比SVG?A*算法小，而DVGA*算法在小范圍內躲避障礙物B 后很快到達了目標點，整體路徑較為平滑。

DVGA*算法規劃路徑局部放大如圖10 所示，可看出靜態障礙物A 處避障半徑較小，在巷道轉角和動態障礙物B 處路徑更加平滑，避障路徑更加有效，同時避障前后路徑都近似直線，運行更加穩定。

采用4 種算法實驗時智能小車軌跡對比如圖11所示， t 為時間， x， y 為移動距離，ω為航偏角。由圖11（a）可看出，4 種算法在x 方向的移動距離相近，CVGA*算法在y 方向沒能到達預設的目標點且加速度存在多次突變，運行不穩定，其余算法在y 方向表現出更好的穩定性。由圖11（b）可看出，采用DVGA*算法時航偏角變化穩定、迅速，小車行駛更加平穩，路徑規劃更加靈活。

分別應用4 種算法進行30 次路徑規劃實驗，得到平均路徑長度、平均規劃時間和到達目標點的成功次數，見表3?？煽闯鱿噍^于CVGA*， SVG?A*和SVGCA*算法， DVGA*算法平均規劃時間分別減少了38.18% ，24.69% 和16.05% ；平均路徑長度分別縮短了10.79 % ，6.26% 和2.86% ；同時，DVGA*算法成功次數最多。上述結果表明，DVGA*算法提高了小車在復雜環境下路徑規劃的成功率，增強了算法對環境的適應能力，路徑規劃更快。

3.3煤礦井下巷道全局路徑規劃試驗

實際的煤礦井下會出現巷道狹窄、彎曲及有未知障礙物的情況，為進一步測試DVGA*算法在真實復雜環境中的性能，開展煤礦井下巷道全局路徑規劃試驗。井下巷道環境如圖12 所示。相比長直廊道，該巷道存在直道、拐彎、分叉及2 種不同寬度。從起點出發直行20 m，經過一個曲率較小的拐彎后到達寬巷道，巷道右側存在緊靠墻壁的管道設備，其直徑為1 m，長度為2 m，設備前方12 m 處存在分叉路口。進入窄巷道并經過曲率較大的拐彎后恢復到直行巷道，巷道頂部每隔10 m 有一個照明光源。

應用SVGCA*和DVGA*算法分別進行路徑規劃試驗，規劃路徑如圖13所示。在巷道寬度變換區域和躲避靜態障礙物A 時，相比SVGCA*算法，DVGA*算法規劃的路徑更加平滑；在經過一個較大的半U 型彎道和曲率較大的拐彎時，為躲避動態障礙物B，SVGCA*算法規劃出曲率較大的彎曲路徑并經過一段距離后才恢復到近似直線的路徑上來，而DVGA*算法在避開動態障礙物B 的同時及時進行路徑糾正，保證了路徑規劃的時效性和穩定性。

應用SVGCA*和DVGA*算法進行井下試驗時智能小車軌跡對比如圖14 所示?？煽闯? 種算法規劃出的路徑前半段基本吻合，小車在經過較寬巷道和曲率較小的拐彎時都可高效完成行駛和避障；后半段巷道變窄，在躲避動態障礙物B 后，SVGCA*算法規劃路徑在y 方向存在偏移，導致小車行駛不穩定，由圖14（b）可明顯看出SVGCA*算法規劃路徑波動更明顯，方向變化更加頻繁，不如DVGA*算法規劃路徑平滑。

應用SVGCA*和DVGA*算法分別進行10 次規劃試驗并對規劃時間和路徑長度求平均值，統計成功次數，試驗結果見表4?？煽闯鲈趶碗s多變的巷道環境中DVGA*算法的規劃時間和路徑長度相比SVGCA*算法分別減少了11.51% 和1.54%，規劃的路徑可更加高效地到達目標點。由于SVGCA*算法只考慮了起點到終點穿越線上的障礙物，在井下巷道試驗中相比實驗室實車實驗略微縮小了與DVGA*算法的差距，但DVGA*算法規劃路徑的整體效果仍優于SVGCA*算法，具有更高的環境適應性和穩定性。

4結論

1） 提出了一種融合DVG 和A*算法的全局路徑規劃算法DVGA*。在構建真實環境點云地圖的基礎上，連接車輛在不同視點下的可視切點動態生成SVG，并在構建過程中搜索相關可視邊以發現最優路徑。通過僅保留當前擴展節點的后續節點，并丟棄之前步驟中已處理節點的其他分支，刪除無關可視邊和重復點，使得每次搜索的節點數量減少，有效減少了存儲需求，從而降低計算量和執行時間。通過路徑平滑算法進一步減少路徑節點數量，從而提高路徑規劃效率。

2） 實驗結果表明， 與完整可視圖+A*算法、SVG+A*算法及SVGCA*算法對比，DVGA*算法對復雜長距離路徑的規劃時間最短，平均路徑長度分別縮短了10.79 % ， 6.26% 和2.86%，對于狹窄、彎曲、存在未知障礙物的井下復雜環境路徑規劃具有更強的適應性和更高的規劃成功率。

3） 井下試驗結果表明：在巷道寬度變換區域和躲避靜態障礙物時，相比SVGCA*算法，DVGA*算法規劃的路徑更加平滑；躲避動態障礙物時，DVGA*算法能夠及時進行路徑糾正，保證了路徑規劃的時效性和穩定性；在復雜多變的巷道環境中DVGA*算法的規劃時間和路徑長度相比SVGCA*算法分別減少了11.51% 和1.54%，具有更高的環境適應性和穩定性。