面向鏟裝作業(yè)場(chǎng)景的自動(dòng)駕駛礦車路徑規(guī)劃方法研究

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

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

為提高礦區(qū)生產(chǎn)作業(yè)效率、降低礦區(qū)運(yùn)營(yíng)成本、滿足礦業(yè)智能化產(chǎn)業(yè)升級(jí)需求，各類無(wú)人駕駛礦車正被廣泛地應(yīng)用于露天礦區(qū)“智慧礦山”的建設(shè)中。其中，礦車自動(dòng)化鏟裝作業(yè)是提高智能化開(kāi)采效率的重要環(huán)節(jié)之一。鏟裝作業(yè)需要挖機(jī)與礦車協(xié)調(diào)配合，作業(yè)能否高效安全地進(jìn)行主要取決于停車位置的準(zhǔn)確性、復(fù)雜環(huán)境下的車輛繞障性能以及多車序列化作業(yè)的連續(xù)度等。面對(duì)礦區(qū)復(fù)雜多變的行車環(huán)境以及較高的實(shí)時(shí)性與安全性要求，如何在短時(shí)間內(nèi)完成安全系數(shù)較高的路徑規(guī)劃便成為了鏟裝作業(yè)場(chǎng)景下構(gòu)建礦車自動(dòng)駕駛系統(tǒng)的重要課題。

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

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

綜上所述，多數(shù)改進(jìn)算法不能同時(shí)兼顧對(duì)計(jì)算效率和路徑質(zhì)量的提升，也難以解決大型地圖中面對(duì)連續(xù)U形障礙物的復(fù)雜場(chǎng)景時(shí)車輛陷入局部最優(yōu)的問(wèn)題。針對(duì)混合A*算法的固有問(wèn)題以及現(xiàn)有改進(jìn)算法在礦區(qū)鏟裝作業(yè)場(chǎng)景下的不足，本文提出了一種基于引導(dǎo)式變步長(zhǎng)混合A*算法的動(dòng)態(tài)路徑規(guī)劃方法，通過(guò)建立并提取維諾圖中的關(guān)鍵點(diǎn)作為方向引導(dǎo)以避免車輛在U形障礙物處陷入局部最優(yōu)，同時(shí)引入變步長(zhǎng)搜索降低規(guī)劃時(shí)長(zhǎng)，有效地提高了運(yùn)算效率及安全性，并通過(guò)開(kāi)展礦區(qū)實(shí)車場(chǎng)景試驗(yàn)驗(yàn)證了該算法的可用性。

1 混合A*算法結(jié)構(gòu)

混合A*算法是基于傳統(tǒng)A*算法的改進(jìn)，以KURZER提出的算法為例，其相比于傳統(tǒng)A*算法的改進(jìn)，一方面在于搜索路徑時(shí)考慮了車輛運(yùn)動(dòng)學(xué)約束，即根據(jù)拓展步長(zhǎng)、車輛最小轉(zhuǎn)彎半徑、前輪轉(zhuǎn)角以及前進(jìn)后退約束拓展子節(jié)點(diǎn)，如圖2所示。

圖2 混合A*算法子節(jié)點(diǎn)擴(kuò)展

另一方面在于代價(jià)函數(shù)的重新設(shè)置，設(shè)當(dāng)前節(jié)點(diǎn)n(xn，yn)，起點(diǎn)s(xs，ys)，終點(diǎn)e(xe，ye)，則計(jì)算節(jié)點(diǎn)代價(jià)值的公式為：

式中：ReedsSheppCost為在忽略地圖障礙物分布的情況下當(dāng)前節(jié)點(diǎn)到終點(diǎn)的Reeds-Shepp曲線長(zhǎng)度；AStarCost為在真實(shí)情況下當(dāng)前節(jié)點(diǎn)到終點(diǎn)的傳統(tǒng)A*算法代價(jià)值。由于節(jié)點(diǎn)搜索的順序與代價(jià)值大小的啟發(fā)高度相關(guān)，通過(guò)上述改動(dòng)，混合A*算法在搜索時(shí)兼顧了對(duì)障礙物限制與車輛運(yùn)動(dòng)約束的考慮，最終能夠輸出符合車輛運(yùn)動(dòng)學(xué)的避障路徑。

2 引導(dǎo)式關(guān)鍵點(diǎn)

由于露天礦山地理環(huán)境的復(fù)雜性以及鏟裝作業(yè)堆放土石的作業(yè)特點(diǎn)，礦車的行駛區(qū)域常常會(huì)出現(xiàn)包括U形在內(nèi)的各種形狀的連續(xù)障礙物，常規(guī)混合A*算法在規(guī)劃繞開(kāi)U形障礙物的行車路徑時(shí)花費(fèi)時(shí)間較長(zhǎng)，為盡可能提高該過(guò)程的計(jì)算效率，本文設(shè)計(jì)了引導(dǎo)式關(guān)鍵點(diǎn)用于指導(dǎo)路徑規(guī)劃方向，具體流程如下。

2.1 建立維諾圖

維諾圖又稱泰森多邊形或狄利克雷鑲嵌，指在給定平面及生成點(diǎn)的情況下，通過(guò)連接兩相鄰生成點(diǎn)直線的垂直平分線構(gòu)成的數(shù)個(gè)連續(xù)多邊形對(duì)平面空間進(jìn)行分割后得到的圖形。每個(gè)多邊形內(nèi)均有唯一的生成點(diǎn)，且多邊形內(nèi)任意一點(diǎn)到該區(qū)域生成點(diǎn)的距離小于到其他區(qū)域生成點(diǎn)的距離。

在路徑規(guī)劃中，通常將單個(gè)連續(xù)障礙物作為生成點(diǎn)構(gòu)建維諾圖［13］，主要思路為由柵格地圖中的障礙物邊緣向外不斷擴(kuò)展，計(jì)算每個(gè)柵格對(duì)應(yīng)最近障礙物的坐標(biāo)及距離并劃分維諾圖邊緣，最后對(duì)維諾圖進(jìn)行剪枝，在保留連通區(qū)域的前提下，盡可能地將兩柵格寬度的邊緣精簡(jiǎn)為單柵格寬度。該構(gòu)建方法耗時(shí)較短，通過(guò)對(duì)新加入或被刪除的障礙物重復(fù)上述流程，可以在保證實(shí)時(shí)性的情況下完成維諾圖的更新，適用于動(dòng)態(tài)規(guī)劃。如圖3所示，黑色區(qū)域?yàn)檎系K物，紅色區(qū)域即為生成的維諾圖邊緣。

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

2.2 提取關(guān)鍵點(diǎn)

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

圖4 關(guān)鍵點(diǎn)提取方法

2.3 根據(jù)關(guān)鍵點(diǎn)規(guī)劃路徑

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

圖5 根據(jù)關(guān)鍵點(diǎn)規(guī)劃路徑

3 自適應(yīng)變步長(zhǎng)

為在進(jìn)一步提高運(yùn)算效率的同時(shí)保證安全性，在計(jì)算代價(jià)值時(shí)引入了自適應(yīng)變步長(zhǎng)搜索，具體流程如下。

3.1 變步長(zhǎng)搜索規(guī)則

混合A*算法在計(jì)算代價(jià)值時(shí)會(huì)引入傳統(tǒng)A*算法的代價(jià)值，因此可通過(guò)加快傳統(tǒng)A*算法代價(jià)的計(jì)算過(guò)程來(lái)加速整體規(guī)劃流程。預(yù)先將搜索步長(zhǎng)設(shè)定為1～5的整值，在擴(kuò)展子節(jié)點(diǎn)時(shí)以當(dāng)前節(jié)點(diǎn)為中心，擴(kuò)展周圍到中心歐式距離小于當(dāng)前步長(zhǎng)的柵格。由于規(guī)劃過(guò)程中關(guān)鍵點(diǎn)之間的方位關(guān)系十分明顯，反向搜索往往會(huì)得到代價(jià)值更大的子節(jié)點(diǎn)，所以在搜索時(shí)不考慮從當(dāng)前節(jié)點(diǎn)到終點(diǎn)反方向上的柵格以降低搜索成本。

為保證安全性，在接近障礙物的區(qū)域應(yīng)縮短步長(zhǎng)，進(jìn)行更精細(xì)的搜索；在遠(yuǎn)離障礙物的區(qū)域應(yīng)加大步長(zhǎng)，進(jìn)行更快速的搜索。具體方法是在每次擴(kuò)展子節(jié)點(diǎn)時(shí)，通過(guò)此前構(gòu)建的維諾圖獲取最近的障礙物距離，并將當(dāng)前搜索步長(zhǎng)設(shè)置為小于該距離的最大預(yù)設(shè)整值，如圖6所示，黃色為當(dāng)前節(jié)點(diǎn)，紫色為終點(diǎn)，黑色為障礙物，藍(lán)色為搜索范圍。

圖6 不同位置節(jié)點(diǎn)的搜索范圍

3.2 代價(jià)函數(shù)改進(jìn)

傳統(tǒng)A*算法代價(jià)函數(shù)f(n)=g(n)+h(n)中的g(n)為起點(diǎn)到節(jié)點(diǎn)的曼哈頓距離，該方法會(huì)導(dǎo)致加長(zhǎng)搜索步長(zhǎng)擴(kuò)展時(shí)距離當(dāng)前節(jié)點(diǎn)更遠(yuǎn)的子節(jié)點(diǎn)代價(jià)大于距離更近的子節(jié)點(diǎn)代價(jià)，使算法更傾向于將距離較近的子節(jié)點(diǎn)作為下一次的擴(kuò)展節(jié)點(diǎn)，導(dǎo)致搜索范圍始終在近處，降低了搜索效率。

為解決這一問(wèn)題，引入了多步長(zhǎng)A*搜索中的權(quán)重系數(shù)w(n)［14］，設(shè)當(dāng)前計(jì)算代價(jià)的子節(jié)點(diǎn)n(xn，yn)，父節(jié)點(diǎn)c(xc，yc)，本次擴(kuò)展的所有字節(jié)點(diǎn)k(xk，yk)，則權(quán)重系數(shù)計(jì)算表達(dá)式為：

加入權(quán)重系數(shù)w(n)后的代價(jià)函數(shù)為：

該權(quán)重系數(shù)基于子節(jié)點(diǎn)到當(dāng)前節(jié)點(diǎn)的相對(duì)距離賦權(quán)g(n)，使距離更遠(yuǎn)的子節(jié)點(diǎn)g(n)值更小，從而被更加優(yōu)先擴(kuò)展，充分發(fā)揮了加長(zhǎng)搜索步長(zhǎng)時(shí)范圍更廣、速度更快的優(yōu)勢(shì)。

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

圖7 引導(dǎo)式變步長(zhǎng)混合A*算法流程

4 實(shí)車試驗(yàn)

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

圖8 試驗(yàn)場(chǎng)地及對(duì)應(yīng)的柵格地圖

依照此前的算法流程，首先通過(guò)建立維諾圖來(lái)提取引導(dǎo)式關(guān)鍵點(diǎn)，如圖9所示，紅色區(qū)域?yàn)榫S諾圖邊緣，綠色柵格點(diǎn)為引導(dǎo)式關(guān)鍵點(diǎn)，由于障礙物本身形狀不規(guī)律，可以看到生成的維諾圖邊緣比較復(fù)雜，引導(dǎo)式關(guān)鍵點(diǎn)在部分復(fù)雜區(qū)域明顯集中。

圖9 試驗(yàn)場(chǎng)地維諾圖

將起點(diǎn)設(shè)置在左側(cè)U形障礙物內(nèi)部且令車輛面朝障礙物方向（x：289.633，y：528.732，heading：2.49°），將終點(diǎn)設(shè)置在右側(cè)U形障礙物內(nèi)部（x：516.231，y：251.173，heading： 195.38°），在ROS操作系統(tǒng)及AMD Ryzen 7 5800H處理器的環(huán)境下開(kāi)展路徑規(guī)劃。與此同時(shí)，設(shè)置KURZER提出的改進(jìn)混合A*算法及SEDIGHI等提出的引導(dǎo)式混合A*算法作為對(duì)照組進(jìn)行試驗(yàn)。

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

圖1 露天礦區(qū)鏟裝作業(yè)

表1 試驗(yàn)結(jié)果對(duì)比

圖10 Karl Kurzer算法試驗(yàn)結(jié)果

在計(jì)算效率方面，KURZER的算法由于缺乏引導(dǎo)與變步長(zhǎng)加速，加之地圖本身較大，其規(guī)劃時(shí)長(zhǎng)接近19 min，在實(shí)際作業(yè)中會(huì)導(dǎo)致礦車隊(duì)列等待，降低工作效率。SEDIGHI的算法計(jì)算效率明顯提升，但由于將部分多余的引導(dǎo)式關(guān)鍵點(diǎn)納入規(guī)劃且無(wú)變步長(zhǎng)，計(jì)算速度相較于本文提出算法略低。本文提出的引導(dǎo)式變步長(zhǎng)混合A*算法相比SEDIGHI的算法運(yùn)算效率提升近68%，顯著加強(qiáng)了算法的實(shí)時(shí)性。

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

綜上所述，本文提出的引導(dǎo)式變步長(zhǎng)混合A*算法在實(shí)時(shí)性與安全性上均有更好的表現(xiàn)，適用于鏟裝作業(yè)場(chǎng)景自動(dòng)駕駛礦車路徑規(guī)劃。

圖11 ShilanSedighi算法試驗(yàn)結(jié)果

圖12 本文提出的算法試驗(yàn)結(jié)果

5 結(jié)論

針對(duì)露天礦區(qū)鏟裝作業(yè)場(chǎng)景復(fù)雜多變、路徑規(guī)劃安全性與實(shí)時(shí)性要求高的特點(diǎn)，本文提出了一種基于引導(dǎo)式變步長(zhǎng)混合A*算法的礦車路徑規(guī)劃方法，在混合A*算法的基礎(chǔ)上引入了引導(dǎo)式關(guān)鍵點(diǎn)與自適應(yīng)變步長(zhǎng)的改進(jìn)策略，解決了礦車在路徑規(guī)劃過(guò)程中計(jì)算時(shí)間久、路徑質(zhì)量差的問(wèn)題，并通過(guò)在實(shí)車試驗(yàn)中與其他改進(jìn)混合A*算法的對(duì)比驗(yàn)證了該算法的實(shí)時(shí)性與安全性優(yōu)勢(shì)。在后續(xù)研究中，可基于礦區(qū)場(chǎng)景中的多車協(xié)同鏟裝作業(yè)模式進(jìn)行算法優(yōu)化，避免同時(shí)段工作車輛路徑?jīng)_突。此外，還可以基于部分礦車的鉸接結(jié)構(gòu)設(shè)計(jì)配套的控制方法，進(jìn)一步提升算法工程應(yīng)用價(jià)值。