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

毛清華， 姚麗杰， 薛旭升

（1.西安科技大學 機械工程學院，陜西 西安 710054；2.陜西省礦山機電裝備智能檢測與控制重點實驗室，陜西 西安 710054）

0 引言

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

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

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

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

1.1 環(huán)境建模

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

在數學上以可通行區(qū)域為0、障礙物區(qū)域為1的元素M（i，j）（（i，j）為柵格點坐標）構成的矩陣表示柵格地圖，如圖1所示。白色柵格表示煤礦巷道可通行區(qū)域；黑色柵格表示煤層或障礙物，定向鉆機無法通行。

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

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

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

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

式中：(xn,yn)為當前節(jié)點坐標；(xgoal,ygoal)為目標節(jié)點坐標。

1.3 A*算法搜索策略

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

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

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

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

2.1 安全擴展策略

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

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

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

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

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

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

3） 若該父節(jié)點鄰域內存在障礙物節(jié)點，則將該節(jié)點的代價設為無窮大，將其標定為危險節(jié)點，不作為擴展節(jié)點。

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

如果在路徑搜索過程中判斷危險節(jié)點，會延長路徑規(guī)劃時間。因此在程序運行前，先對柵格地圖進行預處理，遍歷柵格地圖中的可通行區(qū)域，將其存儲在矩陣中。

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

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

傳統(tǒng)A*算法中，g(n)與h(n)的權重比為1∶1。為h(n)增加權重系數w(n)，若w(n)=2，則g(n)與h(n)的權重比變?yōu)?∶2，使得啟發(fā)函數偏向于估計代價h(n)，從而提高路徑搜索效率。基于該方法，A*算法啟發(fā)函數可表示為

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

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

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

2.3 路徑平滑策略

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

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

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

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

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

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

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

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

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

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

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

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

3 改進A*算法與DWA融合算法

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

3.1.1 履帶式定向鉆機運動模型

履帶式定向鉆機與兩輪差速驅動/四輪驅動均存在類似的非全向約束，通過控制兩側履帶的相對速度實現轉向，通過線速度、角速度描述其運動。履帶式定向鉆機運動模型如圖6所示，v(t)，ω(t)分別為t時刻定向鉆機在巷道坐標系xoy下的線速度與角速度，θ(t)為t時刻定向鉆機姿態(tài)角。在采樣周期?t內，定向鉆機可視為做勻速直線運動。

圖6 定向鉆機簡化模型Fig.6 Simplified model of directional drilling rig

定向鉆機位姿為

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

3.1.2 速度采樣

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

定向鉆機自身最大、最小速度約束為

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

定向鉆機最大、最小加速度約束為

式中：vc,ωc分別為定向鉆機當前時刻的線速度和角速度；分別為定向鉆機當前時刻的最大線減速度和最大角減速度；分別為定向鉆機當前時刻的最大線加速度和最大角加速度。

定向鉆機制動距離約束為

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

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

3.1.3 評價函數

DWA的評價函數綜合考慮定向鉆機與目標節(jié)點的角度偏差、線速度、模擬軌跡末端與最近障礙物距離。對上述3個量進行歸一化處理，并分別賦予權重后相加，得到軌跡評價函數：

式中：σ（·）為歸一化函數；A(v,ω)為方位角評價函數，用于評價鉆機軌跡末端朝向與目標節(jié)點之間的角度差；L(v,ω)為定向鉆機當前線速度評價函數；α，β，γ為各項權重。

3.2 融合算法

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

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

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

4 仿真及結果分析

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

4.1 基于20×20柵格的改進A*算法仿真

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

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

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

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

在20×20的煤礦巷道柵格地圖中，對優(yōu)化啟發(fā)函數前后的A*算法進行仿真，結果如圖9所示。可看出采用自適應權重優(yōu)化方法優(yōu)化啟發(fā)函數后，A*算法減少了路徑搜索節(jié)點，搜索時間為0.024 7 s，較傳統(tǒng)A*算法的0.038 8 s減少36.3%，搜索效率顯著提高。

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

4.1.3 路徑平滑前后對比

在20×20的煤礦巷道柵格地圖中，剔除冗余節(jié)點前后的路徑規(guī)劃結果如圖10所示。可看出剔除冗余節(jié)點后，路徑長度為19.571 m，較剔除冗余節(jié)點前的23 m減小14.9%，且減少了不必要的轉彎，規(guī)劃出的路徑更為平滑。

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

采用分段三次Hermite插值方法對剔除冗余節(jié)點后的路徑進行二次平滑，結果如圖11所示。可看出二次平滑后的路徑基本平滑，更符合定向鉆機的運動特性。

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

4.2 基于50×50柵格的改進A*算法仿真

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

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

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

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

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

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

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

圖13 50×50柵格地圖內改進A*算法與DWA融合算法路徑規(guī)劃結果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ī)劃算法，若采樣次數過少，PRM算法路徑規(guī)劃可能失敗，RRT*算法路徑規(guī)劃效果較差；融合算法規(guī)劃路徑長度較改進A*算法稍有增加，但平滑性和路徑長度優(yōu)于PRM算法和RRT*算法規(guī)劃路徑，更便于定向鉆機行駛；融合算法與未知障礙物保持了安全距離，可在全局路徑最優(yōu)的基礎上，使定向鉆機安全繞過未知障礙物并到達目標節(jié)點。

5 結論

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

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

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

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

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