煤礦履帶式定向鉆機(jī)路徑規(guī)劃算法

毛清華， 姚麗杰， 薛旭升

（1.西安科技大學(xué) 機(jī)械工程學(xué)院，陜西 西安 710054；2.陜西省礦山機(jī)電裝備智能檢測(cè)與控制重點(diǎn)實(shí)驗(yàn)室，陜西 西安 710054）

0 引言

煤炭是我國(guó)的主體能源，在能源生產(chǎn)和消費(fèi)結(jié)構(gòu)中一直占據(jù)主體地位，定向鉆機(jī)作為煤礦井下鉆孔施工的主要技術(shù)裝備，在礦井瓦斯災(zāi)害、水害等災(zāi)害防治中發(fā)揮著巨大作用[1-2]。目前，履帶式定向鉆機(jī)在井下行駛和施工主要依靠操作人員遙控實(shí)現(xiàn)，工作環(huán)境惡劣，人員勞動(dòng)強(qiáng)度大，智能化程度較低。2020年八部委聯(lián)合發(fā)布《關(guān)于加快煤礦智能化發(fā)展的指導(dǎo)意見》，提出了煤礦鉆探自動(dòng)化、智能化和遠(yuǎn)程控制的發(fā)展方向[3]。自主行駛是智能化鉆探的基礎(chǔ)，對(duì)于提高鉆探效率和鉆機(jī)智能化程度，減輕人員勞動(dòng)強(qiáng)度，推動(dòng)煤礦安全、高效、智能生產(chǎn)具有重要意義。

路徑規(guī)劃是實(shí)現(xiàn)履帶式定向鉆機(jī)自主行駛的重要環(huán)節(jié)，包括全局路徑規(guī)劃和局部路徑規(guī)劃。全局路徑規(guī)劃是根據(jù)已知環(huán)境信息，從起點(diǎn)出發(fā)，找到一條可行路徑到達(dá)目標(biāo)點(diǎn)；局部路徑規(guī)劃是由傳感器實(shí)時(shí)采集環(huán)境信息進(jìn)行動(dòng)態(tài)路徑規(guī)劃。履帶式定向鉆機(jī)依靠操縱臺(tái)行駛，且體積較大，應(yīng)用于中大型煤礦或巷道，大多情況下行駛于無(wú)障礙環(huán)境，但行駛過程中可能會(huì)遇到行人等動(dòng)態(tài)障礙物。因此，實(shí)現(xiàn)定向鉆機(jī)自主行走還需著重考慮避障的準(zhǔn)確性[4-5]。目前常用的全局路徑規(guī)劃算法包括Dijkstra算法[6-7]、蟻群算法[8-9]、A*算法[10]等，局部路徑規(guī)劃算法有人工勢(shì)場(chǎng)法[11-12]、時(shí)間彈性帶（Time Elastic Band，TEB）算法[13]等。A*算法作為一種有效的靜態(tài)路網(wǎng)路徑規(guī)劃算法，簡(jiǎn)單易實(shí)現(xiàn)，但存在拐點(diǎn)較多、轉(zhuǎn)角大、路徑貼近障礙物等缺點(diǎn)。許多學(xué)者對(duì)其進(jìn)行了改進(jìn)。張偉民等[14]對(duì)A*算法增加擴(kuò)展節(jié)點(diǎn)，設(shè)置距離閾值，并采用五次B樣條曲線擬合得到煤礦救援機(jī)器人的優(yōu)化路徑。趙久強(qiáng)等[15]以移動(dòng)機(jī)器人行駛時(shí)間作為代價(jià)，依據(jù)障礙物信息調(diào)整A*算法的啟發(fā)函數(shù)，使用Floyd算法進(jìn)一步優(yōu)化，提升了路徑平滑度。薛光輝等[16]提出以障礙物大小為基準(zhǔn)設(shè)定柵格尺寸、機(jī)器人占據(jù)多個(gè)柵格的狹長(zhǎng)空間地圖建模方法，利用障礙物碰撞檢測(cè)函數(shù)改進(jìn)A*算法，并完成了狹長(zhǎng)空間的復(fù)雜環(huán)境路徑規(guī)劃。金輝等[17]利用拓?fù)鋱D將燃油消耗與最優(yōu)速度對(duì)應(yīng)，將A*算法的啟發(fā)函數(shù)改進(jìn)為當(dāng)前節(jié)點(diǎn)到終點(diǎn)的最優(yōu)經(jīng)濟(jì)燃油消耗，得到最優(yōu)速度規(guī)劃，提升了燃油經(jīng)濟(jì)性，減少了交叉路口通過時(shí)間。Tian Hao等[18]提出一種核輻射環(huán)境下的改進(jìn)A*算法，在啟發(fā)函數(shù)中加入地形和移動(dòng)速度等因素，將地形對(duì)運(yùn)動(dòng)的影響分為可接近和不可接近，其中可接近部分將地形的影響量化為速度，可求出核輻射復(fù)雜環(huán)境中的最短路徑。Li Dongcheng等[19]優(yōu)化了A*算法搜索策略、步長(zhǎng)和成本函數(shù)，減少了搜索時(shí)間，在Q學(xué)習(xí)的探索機(jī)制中增加了動(dòng)態(tài)探索因子，實(shí)現(xiàn)了無(wú)人機(jī)的全局-局部混合路徑規(guī)劃。除A*算法外，王文飛等[20]基于電勢(shì)能原理改進(jìn)動(dòng)態(tài)窗口法的擴(kuò)展軌跡評(píng)價(jià)函數(shù)，提升了路徑規(guī)劃能力和實(shí)時(shí)性。封碩等[21]將改進(jìn)的雙粒子群算法與路徑規(guī)劃模型相結(jié)合，在復(fù)雜路段尋找最優(yōu)路徑，提高了路徑規(guī)劃成功率，縮短了路徑長(zhǎng)度。Wang Yinchu等[22]提出一種基于深度強(qiáng)化學(xué)習(xí)的機(jī)器人路徑規(guī)劃方法。Wang Fang等[23]在遺傳算法選擇過程中引入自適應(yīng)隨機(jī)檢驗(yàn)方法來(lái)增強(qiáng)初始多樣性，提高了算法的全局搜索能力和收斂速率，并采用Clothoid曲線改善了路徑曲線的連續(xù)性。

上述文獻(xiàn)針對(duì)全局或局部路徑規(guī)劃算法進(jìn)行了優(yōu)化改進(jìn)。當(dāng)環(huán)境發(fā)生變化時(shí)，算法需要從當(dāng)前節(jié)點(diǎn)重新規(guī)劃到目標(biāo)節(jié)點(diǎn)的路徑，耗時(shí)較長(zhǎng)。部分文獻(xiàn)在路徑規(guī)劃階段未考慮機(jī)器人的尺寸等，易與障礙物發(fā)生碰撞。另外，傳統(tǒng)A*算法規(guī)劃的路徑存在拐點(diǎn)，不利于定向鉆機(jī)控制，而局部路徑規(guī)劃算法因未利用全局信息，易出現(xiàn)局部最優(yōu)解。針對(duì)上述問題，提出一種改進(jìn)A*算法和動(dòng)態(tài)窗口法（Dynamic Window Approach，DWA）融合的履帶式定向鉆機(jī)路徑規(guī)劃方法。首先，在傳統(tǒng)A*算法中加入安全距離約束，保證路徑的安全性，針對(duì)啟發(fā)函數(shù)設(shè)計(jì)自適應(yīng)權(quán)重系數(shù)，提高全局路徑的搜索效率；其次，剔除路徑中的冗余節(jié)點(diǎn)，采用分段三次Hermite插值對(duì)路徑進(jìn)行二次平滑處理，便于履帶式定向鉆機(jī)的跟蹤控制；最后，以改進(jìn)A*算法規(guī)劃全局路徑，引導(dǎo)DWA完成局部路徑規(guī)劃，實(shí)現(xiàn)定向鉆機(jī)在煤礦井下巷道中的避障功能。

1 履帶式定向鉆機(jī)路徑規(guī)劃方案

1.1 環(huán)境建模

柵格地圖用于建立二維靜態(tài)或動(dòng)態(tài)運(yùn)行環(huán)境，是路徑規(guī)劃中常用的環(huán)境地圖模型。將煤礦井下環(huán)境簡(jiǎn)化為柵格平面，以柵格為單位記錄煤礦環(huán)境信息，任意一個(gè)柵格與現(xiàn)實(shí)環(huán)境存在一一對(duì)應(yīng)關(guān)系。柵格被賦予狀態(tài)后共同組成環(huán)境地圖模型，柵格劃分越精細(xì)，算法運(yùn)行占用空間越大。

在數(shù)學(xué)上以可通行區(qū)域?yàn)?、障礙物區(qū)域?yàn)?的元素M（i，j）（（i，j）為柵格點(diǎn)坐標(biāo)）構(gòu)成的矩陣表示柵格地圖，如圖1所示。白色柵格表示煤礦巷道可通行區(qū)域；黑色柵格表示煤層或障礙物，定向鉆機(jī)無(wú)法通行。

圖1 柵格地圖模型Fig.1 Grid map model

1.2 A*算法路徑規(guī)劃

結(jié)合廣度優(yōu)先Dijkstra和深度優(yōu)先貪心算法的A*算法是常見的路徑搜索算法。A*算法的啟發(fā)函數(shù)為

式中：f(n)為從起點(diǎn)經(jīng)過當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的總代價(jià)；g(n)為從起點(diǎn)到當(dāng)前節(jié)點(diǎn)n消耗的代價(jià)；h(n)為從當(dāng)前節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)，包括曼哈頓距離h1(n)、歐幾里得距離h2(n)和切比雪夫距離h3(n)。

式中：(xn,yn)為當(dāng)前節(jié)點(diǎn)坐標(biāo)；(xgoal,ygoal)為目標(biāo)節(jié)點(diǎn)坐標(biāo)。

1.3 A*算法搜索策略

A*算法在路徑搜索階段一般采用四鄰域或八鄰域的搜索策略，如圖2所示。四鄰域搜索是對(duì)當(dāng)前節(jié)點(diǎn)東南西北4個(gè)方向進(jìn)行擴(kuò)展，擴(kuò)展鄰域集合可表示為{(xn±1,yn),(xn,yn±1)}，相鄰方向夾角為90°。八鄰域是在四鄰域擴(kuò)展的基礎(chǔ)上增加4個(gè)方向，為當(dāng)前節(jié)點(diǎn)的8個(gè)方向，擴(kuò)展鄰域集合為{(xn±1,yn),(xn,yn±1),(xn±1,yn±1)}，相鄰方向夾角為45°。

圖2 四鄰域和八鄰域搜索Fig.2 Four neighborhood and eight neighborhood search

二維平面中，歐幾里得距離是兩點(diǎn)之間的直線距離；曼哈頓距離是兩點(diǎn)沿著網(wǎng)格線的總距離；切比雪夫距離是兩點(diǎn)在各維度上坐標(biāo)差值的最大絕對(duì)值，沒有考慮路徑走向。相比之下，曼哈頓距離計(jì)算簡(jiǎn)單，適用于四鄰域情況，更符合煤礦環(huán)境的距離表達(dá)要求。因此，選用四鄰域作為A*算法的搜索策略，以曼哈頓距離計(jì)算節(jié)點(diǎn)間的估計(jì)代價(jià)等，將安全距離約束引入傳統(tǒng)A*算法，進(jìn)行啟發(fā)函數(shù)優(yōu)化和路徑平滑。

2 基于改進(jìn)A*算法的路徑規(guī)劃

2.1 安全擴(kuò)展策略

柵格地圖中可能存在路徑中的某一節(jié)點(diǎn)鄰域是障礙物的情況。因?qū)で笞顑?yōu)路徑，規(guī)劃的路徑可能會(huì)貼近障礙物。定向鉆機(jī)作為一種特殊移動(dòng)體，若將其視為質(zhì)點(diǎn)，很可能會(huì)出現(xiàn)規(guī)劃的路徑穿越障礙物的情況，如圖3所示。這將增大定向鉆機(jī)行駛過程中的碰撞風(fēng)險(xiǎn)。因此，在考慮路徑最短之外，應(yīng)考慮定向鉆機(jī)自身的尺寸。本文引入安全擴(kuò)展策略，為定向鉆機(jī)和障礙物之間保留安全行駛距離。

圖3 A*算法規(guī)劃路徑穿越障礙物Fig.3 Planned path of A* algorithm through obstacles

為避免規(guī)劃路徑受鉆機(jī)機(jī)身尺寸影響，將柵格中心點(diǎn)視為鉆機(jī)中心點(diǎn)，在定向鉆機(jī)和巷道壁、障礙物之間添加距離函數(shù)約束，建立安全擴(kuò)展策略（圖4）。

圖4 路徑規(guī)劃安全擴(kuò)展策略Fig.4 Safety extension strategy of path planning

1） 遍歷整體煤礦柵格地圖，以父節(jié)點(diǎn)為中心構(gòu)建正方形柵格區(qū)域。

2） 檢測(cè)構(gòu)建的正方形柵格區(qū)域是否存在煤層或障礙物節(jié)點(diǎn)。

3） 若該父節(jié)點(diǎn)鄰域內(nèi)存在障礙物節(jié)點(diǎn)，則將該節(jié)點(diǎn)的代價(jià)設(shè)為無(wú)窮大，將其標(biāo)定為危險(xiǎn)節(jié)點(diǎn)，不作為擴(kuò)展節(jié)點(diǎn)。

4） 若該父節(jié)點(diǎn)鄰域內(nèi)不存在障礙物節(jié)點(diǎn)，則對(duì)其不做處理。

如果在路徑搜索過程中判斷危險(xiǎn)節(jié)點(diǎn)，會(huì)延長(zhǎng)路徑規(guī)劃時(shí)間。因此在程序運(yùn)行前，先對(duì)柵格地圖進(jìn)行預(yù)處理，遍歷柵格地圖中的可通行區(qū)域，將其存儲(chǔ)在矩陣中。

2.2 啟發(fā)函數(shù)優(yōu)化

啟發(fā)函數(shù)的選擇決定A*算法的搜索效率。大多情況下定向鉆機(jī)行駛于無(wú)障礙的巷道，因此，提高全局路徑的搜索效率非常有必要。在柵格地圖中，當(dāng)前節(jié)點(diǎn)和目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)始終比實(shí)際值低。若啟發(fā)函數(shù)的估計(jì)代價(jià)h(n)=0，則A*算法會(huì)退化為Dijkstra算法。Dijkstra算法會(huì)在起點(diǎn)和目標(biāo)節(jié)點(diǎn)之間找到最優(yōu)路徑，但其搜索范圍變大，耗費(fèi)大量時(shí)間。

傳統(tǒng)A*算法中，g(n)與h(n)的權(quán)重比為1∶1。為h(n)增加權(quán)重系數(shù)w(n)，若w(n)=2，則g(n)與h(n)的權(quán)重比變?yōu)?∶2，使得啟發(fā)函數(shù)偏向于估計(jì)代價(jià)h(n)，從而提高路徑搜索效率?；谠摲椒ǎ珹*算法啟發(fā)函數(shù)可表示為

定向鉆機(jī)在路徑規(guī)劃過程中需兼顧搜索速度和最優(yōu)路徑，w(n)需根據(jù)當(dāng)前節(jié)點(diǎn)位置調(diào)整。h(n)較大時(shí)，為了盡快完成目標(biāo)節(jié)點(diǎn)擴(kuò)展，w(n)應(yīng)變大，從而加快搜索速度；h(n)較小時(shí)，傾向于搜索最優(yōu)路徑，w(n)應(yīng)變小。為此，基于h(n)設(shè)計(jì)自適應(yīng)權(quán)重系數(shù)，同時(shí)將父節(jié)點(diǎn)的影響引入啟發(fā)函數(shù)中，減少A*算法遍歷節(jié)點(diǎn)數(shù)量。優(yōu)化的啟發(fā)函數(shù)為

式中：D為起點(diǎn)到目標(biāo)節(jié)點(diǎn)的歐幾里得距離；(xstart，ystart)為起點(diǎn)坐標(biāo)；H(q)為當(dāng)前節(jié)點(diǎn)的父節(jié)點(diǎn)q到目標(biāo)節(jié)點(diǎn)的曼哈頓距離。

通過對(duì)A*算法的啟發(fā)函數(shù)進(jìn)行自適應(yīng)權(quán)重優(yōu)化，調(diào)整定向鉆機(jī)路徑規(guī)劃過程中起點(diǎn)到當(dāng)前節(jié)點(diǎn)的實(shí)際代價(jià)與當(dāng)前節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)的估計(jì)代價(jià)在評(píng)價(jià)函數(shù)中的權(quán)重占比，隨著定向鉆機(jī)逐漸靠近目標(biāo)節(jié)點(diǎn)，h(n)的權(quán)重系數(shù)逐漸趨近于1，估計(jì)代價(jià)逐漸趨近于0。

2.3 路徑平滑策略

定向鉆機(jī)在搜索出起點(diǎn)到目標(biāo)節(jié)點(diǎn)之間的路徑后，開始向目標(biāo)節(jié)點(diǎn)移動(dòng)。規(guī)劃出的路徑由不同斜率的折線段組成，但因采用四鄰域擴(kuò)展策略，規(guī)劃路徑仍存在轉(zhuǎn)折次數(shù)多的問題，導(dǎo)致鉆機(jī)轉(zhuǎn)向困難，降低了鉆機(jī)行駛效率，需對(duì)路徑進(jìn)行平滑處理。

2.3.1 冗余節(jié)點(diǎn)剔除方法

在柵格地圖中，定義S為路徑上的一點(diǎn)，在S點(diǎn)后存在若干路徑節(jié)點(diǎn)。剔除冗余節(jié)點(diǎn)的基本思想：在生成的路徑列表中，依次連接起點(diǎn)和列表中的各節(jié)點(diǎn)，如果起點(diǎn)和節(jié)點(diǎn)n的連線不經(jīng)過障礙物，且起點(diǎn)和節(jié)點(diǎn)n+1的連線經(jīng)過障礙物，則剔除起點(diǎn)和節(jié)點(diǎn)n之間的若干點(diǎn)，最終提取保留的路徑S→n。在剔除全部冗余節(jié)點(diǎn)后，按順序連接列表中的節(jié)點(diǎn)，生成優(yōu)化后的路徑。冗余節(jié)點(diǎn)剔除方法如圖5所示，其中紅色線表示等距采樣點(diǎn)，用于判斷兩點(diǎn)之間的路徑是否安全無(wú)碰撞。

剔除冗余節(jié)點(diǎn)步驟如下：

1） 基于前文改進(jìn)后的A*算法規(guī)劃出一條全局最優(yōu)路徑，保存路徑節(jié)點(diǎn)等信息。

2） 將上述全局最優(yōu)路徑上的節(jié)點(diǎn)記為集合{p1,p2,···,pN}，p1為路徑起點(diǎn)，pN為路徑目標(biāo)節(jié)點(diǎn)，N為節(jié)點(diǎn)總數(shù)。

3） 從起點(diǎn)開始，遍歷后續(xù)每個(gè)節(jié)點(diǎn)，分別將當(dāng)前節(jié)點(diǎn)與后面節(jié)點(diǎn)相連作為備選路徑，對(duì)備選路徑進(jìn)行碰撞檢測(cè)。若在當(dāng)前節(jié)點(diǎn)的連接路徑發(fā)生碰撞，則保留前一節(jié)點(diǎn)，反之保留當(dāng)前節(jié)點(diǎn)。

4） 若當(dāng)前路徑與地圖中最近障礙物的距離大于設(shè)置的安全距離，則保留該路徑，并刪除中間節(jié)點(diǎn)；若小于安全距離則不做任何操作，保留原來(lái)的路徑，直至到達(dá)目標(biāo)節(jié)點(diǎn)。

2.3.2 規(guī)劃路徑二次平滑

在煤礦環(huán)境下，改進(jìn)A*算法可以快速規(guī)劃出一條安全路徑，剔除冗余節(jié)點(diǎn)后的路徑可減少路徑拐點(diǎn)數(shù)量，但仍存在轉(zhuǎn)折點(diǎn)，需對(duì)路徑進(jìn)行二次平滑。

Hermite插值在區(qū)間生成插值節(jié)點(diǎn)，構(gòu)造出一條光滑的曲線，可以對(duì)路徑進(jìn)行平滑處理，保證路徑的連續(xù)光滑性。但直接使用Hermite插值得到的多項(xiàng)式次數(shù)較高，因此采用分段三次Hermite插值函數(shù)P對(duì)剔除冗余節(jié)點(diǎn)后的路徑進(jìn)行二次平滑處理。

式中：Jn,In為插值基函數(shù)；Yn為路徑節(jié)點(diǎn)n處的函數(shù)值。

3 改進(jìn)A*算法與DWA融合算法

3.1 DWA路徑規(guī)劃算法

3.1.1 履帶式定向鉆機(jī)運(yùn)動(dòng)模型

履帶式定向鉆機(jī)與兩輪差速驅(qū)動(dòng)/四輪驅(qū)動(dòng)均存在類似的非全向約束，通過控制兩側(cè)履帶的相對(duì)速度實(shí)現(xiàn)轉(zhuǎn)向，通過線速度、角速度描述其運(yùn)動(dòng)。履帶式定向鉆機(jī)運(yùn)動(dòng)模型如圖6所示，v(t)，ω(t)分別為t時(shí)刻定向鉆機(jī)在巷道坐標(biāo)系xoy下的線速度與角速度，θ(t)為t時(shí)刻定向鉆機(jī)姿態(tài)角。在采樣周期?t內(nèi)，定向鉆機(jī)可視為做勻速直線運(yùn)動(dòng)。

圖6 定向鉆機(jī)簡(jiǎn)化模型Fig.6 Simplified model of directional drilling rig

定向鉆機(jī)位姿為

式中(x(t),y(t))為t時(shí)刻定向鉆機(jī)位置。

3.1.2 速度采樣

實(shí)際應(yīng)用中要根據(jù)定向鉆機(jī)性能和環(huán)境對(duì)速度矢量空間[v,w]采樣進(jìn)行約束。

定向鉆機(jī)自身最大、最小速度約束為

式中：vmin,vmax分別為定向鉆機(jī)最小線速度和最大線速度；ωmin,ωmax分別為定向鉆機(jī)最小角速度和最大角速度。

定向鉆機(jī)最大、最小加速度約束為

式中：vc,ωc分別為定向鉆機(jī)當(dāng)前時(shí)刻的線速度和角速度；分別為定向鉆機(jī)當(dāng)前時(shí)刻的最大線減速度和最大角減速度；分別為定向鉆機(jī)當(dāng)前時(shí)刻的最大線加速度和最大角加速度。

定向鉆機(jī)制動(dòng)距離約束為

式中d(v,ω)為v,ω下定向鉆機(jī)與障礙物的最小距離。

對(duì)式（9）—式（11）所示的 3個(gè)約束求交集，得到定向鉆機(jī)采樣速度Vr=Vs∩Vd∩Va。

3.1.3 評(píng)價(jià)函數(shù)

DWA的評(píng)價(jià)函數(shù)綜合考慮定向鉆機(jī)與目標(biāo)節(jié)點(diǎn)的角度偏差、線速度、模擬軌跡末端與最近障礙物距離。對(duì)上述3個(gè)量進(jìn)行歸一化處理，并分別賦予權(quán)重后相加，得到軌跡評(píng)價(jià)函數(shù)：

式中：σ（·）為歸一化函數(shù)；A(v,ω)為方位角評(píng)價(jià)函數(shù)，用于評(píng)價(jià)鉆機(jī)軌跡末端朝向與目標(biāo)節(jié)點(diǎn)之間的角度差；L(v,ω)為定向鉆機(jī)當(dāng)前線速度評(píng)價(jià)函數(shù)；α，β，γ為各項(xiàng)權(quán)重。

3.2 融合算法

A*算法是基于先驗(yàn)地圖的路徑規(guī)劃算法，只能應(yīng)用于環(huán)境信息全部已知的情況，若在A*算法規(guī)劃好的路徑上出現(xiàn)障礙物，定向鉆機(jī)無(wú)法躲避，需引入局部避障算法。DWA依靠外部傳感器實(shí)時(shí)檢測(cè)環(huán)境信息進(jìn)行局部路徑規(guī)劃，在采樣周期?t內(nèi)的速度空間[v,ω]中對(duì)速度進(jìn)行采樣，通過評(píng)價(jià)函數(shù)評(píng)價(jià)運(yùn)動(dòng)軌跡，選取對(duì)應(yīng)[v,ω]下的最優(yōu)軌跡完成運(yùn)動(dòng)，但缺少指引點(diǎn)，存在局部最優(yōu)解。

將改進(jìn)A*算法與DWA融合：以改進(jìn)A*算法規(guī)劃全局路徑，指引DWA進(jìn)行局部路徑規(guī)劃，并以最優(yōu)軌跡控制鉆機(jī)向目標(biāo)節(jié)點(diǎn)移動(dòng)，當(dāng)激光雷達(dá)或其他外部傳感器檢測(cè)到未知障礙時(shí)，利用DWA實(shí)現(xiàn)避障。融合算法流程如圖7所示。

圖7 改進(jìn)A*算法與DWA融合算法流程Fig.7 Fusion algorithm flow of improved A* algorithm and dynamic window approach (DWA)

4 仿真及結(jié)果分析

為驗(yàn)證改進(jìn)A*算法和融合算法的有效性，針對(duì)履帶式定向鉆機(jī)不同工況環(huán)境建立柵格地圖，在12th Gen Intel（R） Core（TM） i5-12500H 2.50 GHz計(jì)算機(jī)上，采用Matlab 2022b軟件對(duì)改進(jìn)A*算法、改進(jìn)A*算法與DWA融合算法進(jìn)行仿真驗(yàn)證。

4.1 基于20×20柵格的改進(jìn)A*算法仿真

在Matlab中建立20×20的巷道柵格地圖，對(duì)改進(jìn)A*算法的安全擴(kuò)展策略、啟發(fā)函數(shù)優(yōu)化策略和路徑平滑策略進(jìn)行驗(yàn)證。設(shè)柵格地圖中x，y為橫縱坐標(biāo)，單位柵格長(zhǎng)度為1 m，黑色柵格代表煤層、障礙物，白色柵格代表可通行區(qū)域。

4.1.1 引入安全擴(kuò)展策略前后路徑規(guī)劃對(duì)比

在20×20的煤礦巷道柵格地圖中，對(duì)傳統(tǒng)A*算法和引入安全擴(kuò)展策略后的算法進(jìn)行仿真，結(jié)果如圖8所示?？煽闯鯝*算法引入安全擴(kuò)展策略后，規(guī)劃出的路徑遠(yuǎn)離障礙物，路徑長(zhǎng)度增加，但保證了定向鉆機(jī)與巷道壁、障礙物的安全距離，避免了因路徑貼近障礙物而發(fā)生碰撞。

4.1.2 啟發(fā)函數(shù)優(yōu)化前后路徑規(guī)劃對(duì)比

在20×20的煤礦巷道柵格地圖中，對(duì)優(yōu)化啟發(fā)函數(shù)前后的A*算法進(jìn)行仿真，結(jié)果如圖9所示?？煽闯霾捎米赃m應(yīng)權(quán)重優(yōu)化方法優(yōu)化啟發(fā)函數(shù)后，A*算法減少了路徑搜索節(jié)點(diǎn)，搜索時(shí)間為0.024 7 s，較傳統(tǒng)A*算法的0.038 8 s減少36.3%，搜索效率顯著提高。

圖9 A*算法啟發(fā)函數(shù)優(yōu)化前后20×20 柵格地圖內(nèi)鉆機(jī)路徑規(guī)劃Fig.9 Drilling rig path planning in 20×20 grid before and after optimizing heuristic function in A* algorithm

4.1.3 路徑平滑前后對(duì)比

在20×20的煤礦巷道柵格地圖中，剔除冗余節(jié)點(diǎn)前后的路徑規(guī)劃結(jié)果如圖10所示?？煽闯鎏蕹哂喙?jié)點(diǎn)后，路徑長(zhǎng)度為19.571 m，較剔除冗余節(jié)點(diǎn)前的23 m減小14.9%，且減少了不必要的轉(zhuǎn)彎，規(guī)劃出的路徑更為平滑。

圖10 剔除冗余節(jié)點(diǎn)前后20×20 柵格地圖內(nèi)定向鉆機(jī)路徑規(guī)劃Fig.10 Drilling rig path planning in 20×20 grid before and after deleting redundant nodes

采用分段三次Hermite插值方法對(duì)剔除冗余節(jié)點(diǎn)后的路徑進(jìn)行二次平滑，結(jié)果如圖11所示?？煽闯龆纹交蟮穆窂交酒交?，更符合定向鉆機(jī)的運(yùn)動(dòng)特性。

圖11 二次平滑處理后定向鉆機(jī)規(guī)劃路徑Fig.11 Drilling rig path planning after quadratic smoothing

4.2 基于50×50柵格的改進(jìn)A*算法仿真

考慮煤礦井下巷道實(shí)際環(huán)境，履帶式定向鉆機(jī)主要行駛于井下輔運(yùn)巷、回風(fēng)巷等，因此根據(jù)巷道環(huán)境對(duì)輔運(yùn)巷（直行工況）、由輔運(yùn)巷進(jìn)入輔運(yùn)聯(lián)絡(luò)巷（轉(zhuǎn)彎工況）和由輔運(yùn)巷到達(dá)回風(fēng)巷（轉(zhuǎn)巷工況）3種工況建立柵格地圖，地圖大小為50×50，單位柵格長(zhǎng)度為1 m。分別采用Dijkstra算法、傳統(tǒng)A*算法和改進(jìn)A*算法在不同柵格地圖中進(jìn)行路徑規(guī)劃，仿真結(jié)果如圖12所示，不同算法性能對(duì)比見表1。

表1 改進(jìn) A*算法與其他路徑規(guī)劃算法性能對(duì)比Table 1 Performance comparison between improved A* algorithm and other path planning algorithms

圖12 50 × 50柵格地圖內(nèi)不同算法路徑規(guī)劃結(jié)果Fig.12 Path planning result of different algorithms in 50 × 50 grid map

由圖12和表1可看出，改進(jìn)A*算法的路徑搜索時(shí)間分別較Dijkstra算法和傳統(tǒng)A*算法平均減少88.5%和63.2%，且在路徑長(zhǎng)度和轉(zhuǎn)折點(diǎn)上有不同程度的優(yōu)化，對(duì)轉(zhuǎn)彎和轉(zhuǎn)巷工況的優(yōu)化效果較明顯。改進(jìn)A*算法規(guī)劃出的路徑可與巷道壁及障礙物保持必要的安全距離，保證了定向鉆機(jī)在巷道中行駛的安全性。同時(shí)，改進(jìn)A*算法通過剔除路徑中的冗余節(jié)點(diǎn)和路徑平滑處理，提高了規(guī)劃路徑的質(zhì)量。

4.3 基于50×50柵格的融合算法仿真

在前文建立的50×50柵格地圖中進(jìn)行改進(jìn)A*算法與DWA融合算法仿真驗(yàn)證。在改進(jìn)A*算法規(guī)劃路徑上設(shè)置障礙物，驗(yàn)證融合算法的避障效果，并將本文算法與PRM算法和RRT*算法進(jìn)行對(duì)比分析。仿真結(jié)果如圖13所示。4種算法的性能對(duì)比見表2。

表2 融合算法與其他路徑規(guī)劃算法性能對(duì)比Table 2 Performance comparison between the fusion algorithm and other path planning algorithms

圖13 50×50柵格地圖內(nèi)改進(jìn)A*算法與DWA融合算法路徑規(guī)劃結(jié)果Fig.13 Path planning results of fusion algorithm of the improved A* algorithm and DWA in 50 × 50 grid map

從圖13和表2可看出，在相同的工況環(huán)境下，因PRM和RRT*為基于概率采樣的路徑規(guī)劃算法，若采樣次數(shù)過少，PRM算法路徑規(guī)劃可能失敗，RRT*算法路徑規(guī)劃效果較差；融合算法規(guī)劃路徑長(zhǎng)度較改進(jìn)A*算法稍有增加，但平滑性和路徑長(zhǎng)度優(yōu)于PRM算法和RRT*算法規(guī)劃路徑，更便于定向鉆機(jī)行駛；融合算法與未知障礙物保持了安全距離，可在全局路徑最優(yōu)的基礎(chǔ)上，使定向鉆機(jī)安全繞過未知障礙物并到達(dá)目標(biāo)節(jié)點(diǎn)。

5 結(jié)論

1） 在傳統(tǒng)A*算法中引入安全距離約束，并對(duì)啟發(fā)函數(shù)進(jìn)行自適應(yīng)權(quán)重優(yōu)化。仿真結(jié)果表明，經(jīng)上述優(yōu)化后，A*算法規(guī)劃出的路徑可保證定向鉆機(jī)與巷道壁、障礙物之間保持安全距離，且路徑搜索效率高，路徑搜索時(shí)間較傳統(tǒng)A*算法減少了36.3%。

2） 對(duì)基于四鄰域的A*算法規(guī)劃出的路徑剔除冗余節(jié)點(diǎn)，之后采用分段三次Hermite插值方法進(jìn)行二次路徑優(yōu)化。仿真結(jié)果表明，經(jīng)路徑平滑后，規(guī)劃路徑拐點(diǎn)及轉(zhuǎn)折點(diǎn)減少，路徑長(zhǎng)度較平滑前減小14.9%，定向鉆機(jī)運(yùn)行效率得以提高。

3） 對(duì)鉆機(jī)在直行、轉(zhuǎn)彎、轉(zhuǎn)巷3種工況下的路徑規(guī)劃進(jìn)行仿真，結(jié)果表明，改進(jìn)A*算法的路徑搜索時(shí)間分別較Dijkstra算法和傳統(tǒng)A*算法平均減少88.5%和63.2%。

4） 將改進(jìn)A*算法和DWA融合，在保證全局最優(yōu)的前提下，使得定向鉆機(jī)可動(dòng)態(tài)規(guī)劃出最優(yōu)路徑，避開未知障礙物。仿真結(jié)果表明，在不同工況下，改進(jìn)A*算法規(guī)劃路徑與未知障礙物的距離為0，融合算法規(guī)劃路徑與未知障礙物的最小距離為1.257 m；與PRM算法和RRT*算法相比，融合算法規(guī)劃路徑長(zhǎng)度分別平均減小5.5%和2.9%，路徑平滑性更好。

5） 目前主要通過仿真分析驗(yàn)證了路徑規(guī)劃算法的有效性，未涉及履帶式定向鉆機(jī)的控制，下一步重點(diǎn)研究將提出的路徑規(guī)劃算法融入控制算法，并在煤礦井下進(jìn)行試驗(yàn)驗(yàn)證。