面向鏟裝作業場景的自動駕駛礦車路徑規劃方法研究

張納川， 王亞飛，2， 章翼辰， 王煒杰， 梅貴周

（1. 上海交通大學 機械與動力工程學院，上海 200240；2. 上海交通大學 汽車動力與智能控制國家工程研究中心，上海 200240；3. 安徽海博智能科技有限公司，安徽，蕪湖 241200）

為提高礦區生產作業效率、降低礦區運營成本、滿足礦業智能化產業升級需求，各類無人駕駛礦車正被廣泛地應用于露天礦區“智慧礦山”的建設中。其中，礦車自動化鏟裝作業是提高智能化開采效率的重要環節之一。鏟裝作業需要挖機與礦車協調配合，作業能否高效安全地進行主要取決于停車位置的準確性、復雜環境下的車輛繞障性能以及多車序列化作業的連續度等。面對礦區復雜多變的行車環境以及較高的實時性與安全性要求，如何在短時間內完成安全系數較高的路徑規劃便成為了鏟裝作業場景下構建礦車自動駕駛系統的重要課題。

路徑規劃是自動駕駛架構中的重要功能層級，旨在通過分析獲取到的環境信息為車輛構建連接起點位置與終點位置的序列點或曲線，常見的路徑規劃方法包括基于圖搜索的Dijkstra算法［1］、A*算法［2］，基于采樣的RRT算法［3］，基于虛擬力的人工勢場法［4］，基于仿生學的蟻群算法［5］，基于Frenet框架的OTG算法［6］等。然而在實際應用中，上述大多數規劃方法輸出的路徑并未考慮車輛運動學約束，需要經過后續處理才能進一步使用。

針對這一問題，DOLGOV等［7］提出了一種考慮車輛行進最大曲率約束的混合A*算法，該算法能在有障礙物的低速環境下生成符合車輛運動學約束的路徑。然而，該算法的缺點也較為明顯，即計算時間較長，實時性較差，且生成的路徑中存在較多不必要的倒車與轉彎，易產生安全隱患。一直以來，不同作者都在針對混合A*算法的局限性進行優化與補充，KURZER［8］提出的改進算法改進了搜索與剪枝方法，并為代價函數增添了轉向、倒車等附加成本，但規劃時間依然較長。SEDIGHI等［9］提出的引導式算法通過預先快速構建出無約束路徑上的關鍵點作為方向引導，從而輔助混合A*算法進行路徑規劃，明顯降低了規劃時長，但在復雜場景下因未對關鍵點進行篩選而導致輸出路徑不夠平滑。SHENG Weitian等［10］提出的針對泊車場景的改進算法與引導式算法思路類似，即先通過傳統A*算法構建無約束路徑，再提取路徑上障礙物較為密集的狹窄區域進行局部混合A*路徑規劃，最后使用全局混合A*路徑規劃進行連通，提高了路徑的安全性，但在復雜場景下對運算效率的提升有限。TU Kangbin等［11］提出的改進算法限制了部分場景下搜索時的反向擴展，同時將車輛朝向角引入啟發信息中，提高了運算速度，但需要針對不同場景調試具體參數且未解決局部最優問題。ZHANG Songyi等［12］提出的改進算法優化了子節點擴展方法，同時基于曲率、曲率變化、曲線長度、倒車懲罰等設置了代價函數加權系數，生成路徑的平滑性得到提高，但并未顯著提升運算效率。

綜上所述，多數改進算法不能同時兼顧對計算效率和路徑質量的提升，也難以解決大型地圖中面對連續U形障礙物的復雜場景時車輛陷入局部最優的問題。針對混合A*算法的固有問題以及現有改進算法在礦區鏟裝作業場景下的不足，本文提出了一種基于引導式變步長混合A*算法的動態路徑規劃方法，通過建立并提取維諾圖中的關鍵點作為方向引導以避免車輛在U形障礙物處陷入局部最優，同時引入變步長搜索降低規劃時長，有效地提高了運算效率及安全性，并通過開展礦區實車場景試驗驗證了該算法的可用性。

1 混合A*算法結構

混合A*算法是基于傳統A*算法的改進，以KURZER提出的算法為例，其相比于傳統A*算法的改進，一方面在于搜索路徑時考慮了車輛運動學約束，即根據拓展步長、車輛最小轉彎半徑、前輪轉角以及前進后退約束拓展子節點，如圖2所示。

圖2 混合A*算法子節點擴展

另一方面在于代價函數的重新設置，設當前節點n(xn，yn)，起點s(xs，ys)，終點e(xe，ye)，則計算節點代價值的公式為：

式中：ReedsSheppCost為在忽略地圖障礙物分布的情況下當前節點到終點的Reeds-Shepp曲線長度；AStarCost為在真實情況下當前節點到終點的傳統A*算法代價值。由于節點搜索的順序與代價值大小的啟發高度相關，通過上述改動，混合A*算法在搜索時兼顧了對障礙物限制與車輛運動約束的考慮，最終能夠輸出符合車輛運動學的避障路徑。

2 引導式關鍵點

由于露天礦山地理環境的復雜性以及鏟裝作業堆放土石的作業特點，礦車的行駛區域常常會出現包括U形在內的各種形狀的連續障礙物，常規混合A*算法在規劃繞開U形障礙物的行車路徑時花費時間較長，為盡可能提高該過程的計算效率，本文設計了引導式關鍵點用于指導路徑規劃方向，具體流程如下。

2.1 建立維諾圖

維諾圖又稱泰森多邊形或狄利克雷鑲嵌，指在給定平面及生成點的情況下，通過連接兩相鄰生成點直線的垂直平分線構成的數個連續多邊形對平面空間進行分割后得到的圖形。每個多邊形內均有唯一的生成點，且多邊形內任意一點到該區域生成點的距離小于到其他區域生成點的距離。

在路徑規劃中，通常將單個連續障礙物作為生成點構建維諾圖［13］，主要思路為由柵格地圖中的障礙物邊緣向外不斷擴展，計算每個柵格對應最近障礙物的坐標及距離并劃分維諾圖邊緣，最后對維諾圖進行剪枝，在保留連通區域的前提下，盡可能地將兩柵格寬度的邊緣精簡為單柵格寬度。該構建方法耗時較短，通過對新加入或被刪除的障礙物重復上述流程，可以在保證實時性的情況下完成維諾圖的更新，適用于動態規劃。如圖3所示，黑色區域為障礙物，紅色區域即為生成的維諾圖邊緣。

圖3 柵格地圖（左）與維諾圖（右）

2.2 提取關鍵點

設置關鍵點的目的在于給予混合A*路徑規劃大致的避開障礙物的方向引導，以進一步提升后續規劃效率。首先輸入本次路徑規劃的起點與終點，尋找距離起點與終點距離最近的維諾圖邊緣上的點作為路網起點與路網終點，通過Dijkstra算法得到一條從路網起點到路網終點的位于維諾圖邊緣上的路徑；對路徑點進行進一步篩選，由于此前進行過剪枝，所以可通過柵格點連通情況提取關鍵點，統計路徑點四向在維諾圖邊緣上的柵格點數量，如大于等于3則可認為該柵格點為路網連通區域，即維諾圖多邊形頂點。這些頂點到周圍障礙物的距離相等，是該區域避開障礙物相對安全的通行點，因此，將其作為路徑規劃的引導式關鍵點。如圖4所示，綠色柵格點即為提取到的引導式關鍵點。

圖4 關鍵點提取方法

2.3 根據關鍵點規劃路徑

接下來便是完成起點-關鍵點-終點的混合A*路徑規劃。在障礙物連續復雜且并非規律幾何體的情況下，此前提取的關鍵點存在較多冗余，在部分連通區域尤其集中，因此需要進行篩選整合。在目標為關鍵點的規劃過程中，改為使用以關鍵點為中心的九宮格作為終點區域，在搜索過程中將子節點拓展至終點區域時便視為完成當前規劃，并將此時車輛的朝向角與坐標作為下一次規劃的起點姿態。該方法既能為中間點設定符合車輛當前運動狀態的朝向，又能在關鍵點相對集中時將其整合到同一區域，避免了車輛不必要的倒車與轉彎，提高了路徑質量。如圖5所示，綠色柵格及其周圍的藍色柵格的集合構成了每次路徑規劃的終點區域。

圖5 根據關鍵點規劃路徑

3 自適應變步長

為在進一步提高運算效率的同時保證安全性，在計算代價值時引入了自適應變步長搜索，具體流程如下。

3.1 變步長搜索規則

混合A*算法在計算代價值時會引入傳統A*算法的代價值，因此可通過加快傳統A*算法代價的計算過程來加速整體規劃流程。預先將搜索步長設定為1～5的整值，在擴展子節點時以當前節點為中心，擴展周圍到中心歐式距離小于當前步長的柵格。由于規劃過程中關鍵點之間的方位關系十分明顯，反向搜索往往會得到代價值更大的子節點，所以在搜索時不考慮從當前節點到終點反方向上的柵格以降低搜索成本。

為保證安全性，在接近障礙物的區域應縮短步長，進行更精細的搜索；在遠離障礙物的區域應加大步長，進行更快速的搜索。具體方法是在每次擴展子節點時，通過此前構建的維諾圖獲取最近的障礙物距離，并將當前搜索步長設置為小于該距離的最大預設整值，如圖6所示，黃色為當前節點，紫色為終點，黑色為障礙物，藍色為搜索范圍。

圖6 不同位置節點的搜索范圍

3.2 代價函數改進

傳統A*算法代價函數f(n)=g(n)+h(n)中的g(n)為起點到節點的曼哈頓距離，該方法會導致加長搜索步長擴展時距離當前節點更遠的子節點代價大于距離更近的子節點代價，使算法更傾向于將距離較近的子節點作為下一次的擴展節點，導致搜索范圍始終在近處，降低了搜索效率。

為解決這一問題，引入了多步長A*搜索中的權重系數w(n)［14］，設當前計算代價的子節點n(xn，yn)，父節點c(xc，yc)，本次擴展的所有字節點k(xk，yk)，則權重系數計算表達式為：

加入權重系數w(n)后的代價函數為：

該權重系數基于子節點到當前節點的相對距離賦權g(n)，使距離更遠的子節點g(n)值更小，從而被更加優先擴展，充分發揮了加長搜索步長時范圍更廣、速度更快的優勢。

綜上所述，本文提出的引導式變步長混合A*算法的流程如圖7所示。

圖7 引導式變步長混合A*算法流程

4 實車試驗

為驗證算法的可用性，在某礦區一處場地進行了實車試驗。如圖8所示，白色為可通行區域，黑色為山體與石堆，該處場地大小約為788 m×789 m，具有露天礦區障礙物連續復雜且非規律幾何體的特點，在場地東西方向各存在一較大的U形障礙物，適于驗證算法的繞障情況。

圖8 試驗場地及對應的柵格地圖

依照此前的算法流程，首先通過建立維諾圖來提取引導式關鍵點，如圖9所示，紅色區域為維諾圖邊緣，綠色柵格點為引導式關鍵點，由于障礙物本身形狀不規律，可以看到生成的維諾圖邊緣比較復雜，引導式關鍵點在部分復雜區域明顯集中。

圖9 試驗場地維諾圖

將起點設置在左側U形障礙物內部且令車輛面朝障礙物方向（x：289.633，y：528.732，heading：2.49°），將終點設置在右側U形障礙物內部（x：516.231，y：251.173，heading： 195.38°），在ROS操作系統及AMD Ryzen 7 5800H處理器的環境下開展路徑規劃。與此同時，設置KURZER提出的改進混合A*算法及SEDIGHI等提出的引導式混合A*算法作為對照組進行試驗。

試驗結果如圖10～12和表1所示，這里選擇了規劃時長、到障礙物平均距離、路徑曲率變動次數3種評價指標，前者決定計算效率，后兩者決定路徑質量。其中，規劃時長指算法從輸入到輸出的總計算時間，該時間過長會使車輛在規劃完成前出現原地等待現象，導致自動鏟裝作業效率降低；到障礙物平均距離指路徑點到周圍障礙物最小距離的平均值，該值過小表明車輛距離障礙物過近，導致路徑安全性降低；路徑曲率變動指單次轉彎朝向角變化超過20 °或一次前進倒車轉換，次數過多會造成車輛輪胎磨損嚴重且控制誤差提升。

圖1 露天礦區鏟裝作業

表1 試驗結果對比

圖10 Karl Kurzer算法試驗結果

在計算效率方面，KURZER的算法由于缺乏引導與變步長加速，加之地圖本身較大，其規劃時長接近19 min，在實際作業中會導致礦車隊列等待，降低工作效率。SEDIGHI的算法計算效率明顯提升，但由于將部分多余的引導式關鍵點納入規劃且無變步長，計算速度相較于本文提出算法略低。本文提出的引導式變步長混合A*算法相比SEDIGHI的算法運算效率提升近68%，顯著加強了算法的實時性。

在路徑質量方面，能夠明顯看出KURZER的算法輸出路徑到障礙物的距離過近，安全性較低。SEDIGHI的算法輸出路徑由于復雜場景關鍵點較多且未進行篩選整合，因而產生了很多不必要的倒車及轉彎，增大了車輛輪胎的磨損率與車輛誤差。本文提出的引導式變步長混合A*算法相比SEDIGHI的算法到障礙物平均距離增加了11%，路徑曲率變動次數減少了45%，兼顧了安全性與路徑平滑，具有更好的路徑質量。

綜上所述，本文提出的引導式變步長混合A*算法在實時性與安全性上均有更好的表現，適用于鏟裝作業場景自動駕駛礦車路徑規劃。

圖11 ShilanSedighi算法試驗結果

圖12 本文提出的算法試驗結果

5 結論

針對露天礦區鏟裝作業場景復雜多變、路徑規劃安全性與實時性要求高的特點，本文提出了一種基于引導式變步長混合A*算法的礦車路徑規劃方法，在混合A*算法的基礎上引入了引導式關鍵點與自適應變步長的改進策略，解決了礦車在路徑規劃過程中計算時間久、路徑質量差的問題，并通過在實車試驗中與其他改進混合A*算法的對比驗證了該算法的實時性與安全性優勢。在后續研究中，可基于礦區場景中的多車協同鏟裝作業模式進行算法優化，避免同時段工作車輛路徑沖突。此外，還可以基于部分礦車的鉸接結構設計配套的控制方法，進一步提升算法工程應用價值。