張志偉， 馬小平， 白亞騰， 雷震亞， 李佳明

（1. 中國礦業(yè)大學 信息與控制工程學院，江蘇 徐州 221000；2. 中國中煤能源集團有限公司，北京 100083）

0 引言

煤礦作業(yè)環(huán)境復雜多變，存在各種危險因素，傳統(tǒng)人工作業(yè)效率低且安全難以保障。移動機器人能夠自動規(guī)劃路徑和執(zhí)行[1]，可有效代替部分人工作業(yè)。

路徑規(guī)劃是移動機器人自主導航的核心，也是技術(shù)難點[2]。路徑規(guī)劃主要指機器人基于環(huán)境信息和全局位姿確定從一個地點到另一個地點的最佳路徑或軌跡，以實現(xiàn)特定的任務或目標。具體可分為全局路徑規(guī)劃和局部路徑規(guī)劃[3-4]。全局路徑規(guī)劃按照相關(guān)評價指標規(guī)劃出始末點間的最優(yōu)路徑[5]，該過程通常不考慮障礙物的具體位置和動態(tài)變化，而是在地圖或環(huán)境模型的基礎上計劃一個整體路徑，它描述了移動機器人應該遵循的大致方向和路線。局部路徑規(guī)劃依賴搭載的傳感器實時獲取的環(huán)境信息規(guī)劃出可供執(zhí)行的實時路徑，它關(guān)注如何繞開障礙物、避免碰撞、調(diào)整速度和方向，以保證移動機器人在復雜環(huán)境中安全、高效地移動?，F(xiàn)有的全局路徑規(guī)劃算法已較為成熟，在大部分場景下求解速度快且魯棒性較好；而針對煤礦復雜環(huán)境，要保證移動機器人避開障礙物，局部路徑規(guī)劃算法就顯得更加重要。常用的局部路徑規(guī)劃算法有人工勢場法、動態(tài)窗口法（Dynamic Window Approach，DWA）和人工神經(jīng)網(wǎng)絡法等。人工勢場法將場景中的障礙物和目標點分別看作對移動機器人有斥力和引力的物體，移動機器人沿著斥力和引力的合力避開障礙物，到達終點[6-8]。如S. M. H. Rostami[9]等通過改進成本函數(shù)使得機器人避開障礙物，而不會停在局部最優(yōu)點；J. Batista[10]等通過遺傳算法對人工勢場參數(shù)進行動態(tài)優(yōu)化調(diào)整，實現(xiàn)碰撞避免和更高的目標精度。DWA 在速度空間采集多組線速度與角速度的交叉組合數(shù)據(jù)，模擬并評估未來的運動路徑，對比生成路徑與參考路徑貼合程度，剔除有障礙物路徑，最終采用最優(yōu)路徑作為規(guī)劃結(jié)果[11-14]。如王永雄等[15]提出了稠密場景內(nèi)的自適應DWA，可根據(jù)障礙物稠密程度自動調(diào)整目標函數(shù)權(quán)值；魏立新等[16]對DWA 進行改進并與蟻群算法融合，提高了DWA 響應時間。人工神經(jīng)網(wǎng)絡是一種自適應非線性動力學系統(tǒng)[17]，由大量神經(jīng)元互相連接組成。如M. Duguleana 等[18]使用Q-learning 算法和人工神經(jīng)網(wǎng)絡解決不確定工作空間內(nèi)的路徑規(guī)劃問題；Xue Yang[19]使用非主導排序遺傳算法解決靜態(tài)環(huán)境下的多智能體路徑規(guī)劃問題；S. Josef 等[20]在強化學習網(wǎng)絡中加入自注意模塊，以增強對局部環(huán)境中重要信息的感知，實現(xiàn)了在離散障礙物和連續(xù)地形的路徑規(guī)劃；F. Leiva 等[21]使用深度確定性策略梯度算法，依據(jù)傳感器獲取一個可變的2D 點云，實現(xiàn)在無圖情況下的路徑規(guī)劃。

然而，上述局部路徑規(guī)劃算法只實現(xiàn)了移動機器人在場景內(nèi)自由移動，但局部路徑生成并未考慮場景內(nèi)道路限制，對于一些規(guī)則化的結(jié)構(gòu)道路并不適用。OpenPlanner 算法很好地解決了該問題，該算法以全局路徑（即行車道中心線）為參考線，在道路內(nèi)橫縱向均勻采樣生成局部路徑簇，使得移動機器人可沿結(jié)構(gòu)道路移動。但傳統(tǒng)OpenPlanner 算法未考慮移動機器人最大轉(zhuǎn)向曲率約束，生成的局部路徑無法被移動機器人所跟蹤。因此，本文基于傳統(tǒng)OpenPlanner 算法，提出了改進OpenPlanner 算法，并用于移動機器人局部路徑規(guī)劃。采用雙層局部路徑簇，同時在評價函數(shù)中引入曲率代價，不僅可擴大解空間，還使得優(yōu)化過程篩選出的最優(yōu)局部路徑能夠滿足移動機器人最大轉(zhuǎn)向曲率約束，從而使移動機器人能平順、安全地跟蹤局部路徑。

1 傳統(tǒng)OpenPlanner 算法

OpenPlanner 算法包含用于全局路徑規(guī)劃、任務規(guī)劃和局部路徑規(guī)劃的開源自動駕駛導航框架[22]。其中局部路徑規(guī)劃的基本思想：首先分別根據(jù)橫向采樣距離和縱向采樣距離進行狀態(tài)采樣，生成初始局部路徑簇，然后基于梯度下降方法完成路徑簇的平滑處理，最后綜合考慮參考線代價、過渡代價和障礙物代價篩選出最優(yōu)的局部路徑，重復迭代以上過程，直到移動機器人行駛至目標位置。

1.1 狀態(tài)采樣

OpenPlanner 算法以全局路徑為參考線，接收移動機器人當前位姿及局部障礙物信息，實時生成局部路徑簇并篩選出最優(yōu)的局部路徑。局部路徑簇是OpenPlanner 算法的關(guān)鍵，其分段結(jié)構(gòu)如圖1 所示，主要由平滑段、入段和出段構(gòu)成。平滑段為移動機器人后軸中心至平滑采樣點的線段，其長度決定了局部路徑轉(zhuǎn)向的平滑性，各條局部路徑的平滑段重合。入段為連接平滑段末端和各狀態(tài)采樣點的線段，狀態(tài)采樣點位置由入段橫向采樣距離和入段縱向采樣距離決定。出段為各狀態(tài)采樣點與最遠局部規(guī)劃點的連線，可通過全局參考路徑的平移得到。移動機器人沿參考線的狀態(tài)采樣過程如圖2 所示，其中點M 為移動機器人后軸中心，點N 為路徑簇平滑段和入段的銜接點，點A—E 分別為局部路徑簇入段的末狀態(tài)采樣點，點A'—E'分別為路徑簇出段的末狀態(tài)采樣點，這些點共同決定了路徑簇的具體構(gòu)型。

圖1 局部路徑簇分段結(jié)構(gòu)Fig. 1 Sections of local path cluster

圖2 沿參考線的狀態(tài)采樣過程Fig. 2 State sampling process along reference line

1.2 局部路徑簇生成

由于狀態(tài)采樣后的初始局部路徑簇在分段連接點處存在較大轉(zhuǎn)折，所以需進行路徑平滑處理。OpenPlanner 算法將路徑平滑轉(zhuǎn)換為無約束的優(yōu)化問題，采用梯度下降方法求解平滑后的局部路徑簇。該問題的優(yōu)化變量為第k（k=0，1，···，n-1，n為路徑點數(shù)量）個路徑點坐標（xk，yk），代價函數(shù)為式中：csmooth為平滑度代價，促使局部路徑趨向于直線，路徑越平滑，csmooth越??；clength為長度代價，保證各路徑點間距分布均勻，各路徑點間距分布越均勻，clength越小；cdeviation為偏移代價，確保優(yōu)化后路徑基本維持原路徑的幾何形狀，對應點之間距離越小，與原路徑偏移越??；Pk為第k個路徑點的笛卡爾坐標；Pk_ref為平滑后第k個路徑點的笛卡爾坐標。

1.3 最優(yōu)局部路徑篩選

算法運行時，通過狀態(tài)采樣和梯度下降生成光滑的局部路徑簇，并基于包含參考線代價、過渡代價和障礙物代價的評價函數(shù)完成最優(yōu)路徑篩選。局部路徑的評價函數(shù)為

式中：i為局部路徑索引；σ為評價系數(shù)；ω1，ω2，ω3分別為參考線代價ccenter（i）、過渡代價ctransition（i）和障礙物代價cobstacle（i）的權(quán)重系數(shù)。

評價函數(shù)的各加權(quán)項含義如下。

1） 參考線代價ccenter（i）：度量單個模擬周期內(nèi)各條局部路徑出段偏離行車道參考線距離的代價。

式中：z為單個模擬周期內(nèi)局部路徑條數(shù)；ldis_resolution為相鄰局部路徑出段間距。

參考線代價計算原理如圖3 所示。

圖3 參考線代價計算原理Fig. 3 Calculation principle of reference line cost

2） 過渡代價ctransition（i）：度量單個模擬周期內(nèi)移動機器人當前位姿與各條局部路徑出段距離的代價。該項用于限制最優(yōu)路徑的頻繁更換，確保移動機器人平穩(wěn)地轉(zhuǎn)向。

式中q為與移動機器人距離最近的局部路徑索引。過渡代價計算原理如圖4 所示。

圖4 過渡代價計算原理Fig. 4 Calculation principle of transition cost

3） 障礙物代價cobstacle（i）：度量單個模擬周期內(nèi)各條局部路徑與障礙物接近程度的代價。障礙物代價的計算過程分為2 個階段：第1 階段計算各條局部路徑平滑段和入段的碰撞情況，如圖5（a）所示，如果在以O為圓心的圓形碰撞檢測區(qū)域內(nèi)存在障礙物輪廓點，則該條路徑被剔除；第2 階段計算在第1 階段中未發(fā)生碰撞的局部路徑出段的障礙物代價，該代價包括障礙物橫向代價cobstacle_longitude（i）和縱向代價cobstacle_lateral（i），含有m個障礙物輪廓點的代價計算公式為

圖5 障礙物代價計算原理Fig. 5 Calculation principle of obstacle cost

式中：lj為移動機器人位置和第j（j=0，1，···，m-1）個障礙物輪廓點位置在路徑上的投影點距離；dj為第j個障礙物輪廓點到參考路徑的垂直距離。

2 改進OpenPlanner 算法

傳統(tǒng)OpenPlanner 算法的狀態(tài)采樣階段忽略了移動機器人實際運行狀態(tài)的影響，同時未考慮移動機器人最大轉(zhuǎn)向曲率約束，可能產(chǎn)生不滿足最大轉(zhuǎn)向曲率的局部路徑。因此，本文從狀態(tài)采樣和評價函數(shù)2 個方面對傳統(tǒng)OpenPlanner 算法進行改進。

2.1 狀態(tài)采樣改進

為擴大最優(yōu)局部路徑的解空間， 改進OpenPlanner 算法采用雙層局部路徑簇設計：次層局部路徑簇的平滑段與首層局部路徑簇的平滑段重合；次層局部路徑簇的入段縱向采樣距離為首層局部路徑簇的1.5 倍，入段橫向采樣距離保持不變；根據(jù)最遠局部路徑規(guī)劃距離平移參考線得到次層局部路徑簇的出段。改進OpenPlanner 算法的局部路徑簇分段結(jié)構(gòu)如圖6 所示。

圖6 改進OpenPlanner 算法局部路徑簇分段結(jié)構(gòu)Fig. 6 Sections of local path cluster of improved OpenPlanner algorithm

改進OpenPlanner 算法在實際運行時，首層局部路徑簇入段縱向采樣距離llon與行駛速度v呈分段線性關(guān)系：

式中：llon_min為最小縱向采樣距離；vmin為最小速度；a為速度影響因子；vmax為最大速度；llon_max為最大縱向采樣距離。

2.2 評價函數(shù)改進

曲率代價能確保最優(yōu)的局部路徑滿足移動機器人最大轉(zhuǎn)向曲率約束，同時曲率代價越小，移動機器人行駛越平順、安全，因此將曲率代價引入評價函數(shù)，改進后局部路徑的評價函數(shù)為

式中：ccurve（i）為局部路徑的曲率代價，由該條局部路徑上各采樣點曲率之和得到；ω4為曲率代價ccurve（i）的權(quán)重系數(shù)。

式中：κλ為局部路徑上采樣點曲率；λ為采樣點索引（λ=1，2，···，t-2，t為局部路徑上采樣點數(shù)量）。

局部路徑上采樣點曲率κλ可由該點坐標（xb，yb）與左右相鄰2 個點坐標（xa，ya），（xc，yc）通過外接圓求曲率法得到：

3 實驗與分析

為驗證改進OpenPlanner 算法的可行性和有效性，在Gazebo 仿真環(huán)境下搭建了有障礙物和無障礙物2 種場景， 對傳統(tǒng)OpenPlanner 算法和改進OpenPlanner 算法進行對比實驗。在2 種場景下設置相同起點，與障礙物距離相同，局部規(guī)劃路徑簇長度為15 m。傳統(tǒng)OpenPlanner 算法的評價函數(shù)權(quán)重系數(shù)ω1=2.5，ω2=1.0，ω3=1.0，改進OpenPlanner 算法的評價函數(shù)權(quán)重系數(shù)ω1=2.5，ω2=0.5，ω3=1.0，ω4=0.5。算法運行環(huán)境為Ubuntu18.04 操作系統(tǒng)，處理器為Intel（R） Core（TM）i5-10400F，主頻為2.9 GHz，內(nèi)存為8 GiB。

傳統(tǒng)OpenPlanner 算法和改進OpenPlanner 算法在有障礙物、無障礙物場景下的局部路徑規(guī)劃仿真結(jié)果分別如圖7、圖8 所示?？煽闯? 種算法均可滿足避障需求，但改進OpenPlanner 算法篩選的局部路徑轉(zhuǎn)向更加平緩。

圖7 有障礙物場景下局部路徑規(guī)劃仿真結(jié)果Fig. 7 Simulation results of local path planning in scenario with obstacle

圖8 無障礙物場景下局部路徑規(guī)劃仿真結(jié)果Fig. 8 Simulation results of local path planning in scenario without obstacle

傳統(tǒng)OpenPlanner 算法和改進OpenPlanner 算法在不同場景下多次仿真實驗的平均數(shù)據(jù)見表1?？煽闯龈倪MOpenPlanner 算法雖然在無障礙物和有障礙物場景下規(guī)劃耗時較傳統(tǒng)OpenPlanner 算法分別增加了30.9%和36.2%，但算法單次規(guī)劃耗時仍小于1 ms，實時性能夠滿足移動機器人的要求；在無障礙物場景下，傳統(tǒng)OpenPlanner 算法規(guī)劃出的局部路徑最大曲率為0.280 8 m-1，超過了移動機器人最大轉(zhuǎn)向曲率0.219 3 m-1，導致局部路徑無法被移動機器人所跟蹤； 與傳統(tǒng)OpenPlanner 算法相比， 改進OpenPlanner 算法生成的局部路徑平均曲率更小，無障礙物場景下局部路徑的平均曲率下降了31.3%，有障礙物場景下局部路徑的平均曲率下降了6.2%，更有利于移動機器人平順、安全地行駛。

表1 OpenPlanner 算法改進前后實驗結(jié)果Table 1 Experimental results before and after improvement of OpenPlanner algorithm

在仿真環(huán)境中采用純追蹤控制算法，對傳統(tǒng)OpenPlanner 算法和改進OpenPlanner 算法分別進行局部路徑跟蹤實驗，對比結(jié)果如圖9 所示?？煽闯鰝鹘y(tǒng)OpenPlanner 算法規(guī)劃的局部路徑和移動機器人實際行駛路徑無法重合，這是由于規(guī)劃的局部路徑存在部分路徑曲率超過最大轉(zhuǎn)向曲率限制，使得大曲率路徑段無法被跟蹤，并在后續(xù)跟蹤中造成累計誤差；而改進OpenPlanner 算法考慮了局部路徑曲率代價，規(guī)劃的局部路徑曲率并未超過最大轉(zhuǎn)向曲率約束，使得移動機器人能完全跟蹤改進OpenPlanner算法規(guī)劃的局部路徑。

圖9 局部路徑跟蹤對比Fig. 9 Comparison of local path tracking

為驗證改進OpenPlanner 算法的實際效果，搭建了搭載激光雷達的雙輪履帶實驗車進行實物實驗。以行人為動態(tài)障礙物，在距離行人3 m 處開始避障規(guī)劃，如圖10 所示。實驗結(jié)果表明，移動機器人能夠準確發(fā)現(xiàn)行人且計算出左側(cè)不滿足通行要求，然后快速執(zhí)行右轉(zhuǎn)的避讓操作，整個跟蹤過程移動機器人動作執(zhí)行平順。

圖10 改進OpenPlanner 算法實物實驗Fig. 10 Physical experiment of improved OpenPlanner algorithm

4 結(jié)論

1） 為擴大最優(yōu)局部路徑解空間，設計了雙層局部路徑簇，其中首層局部路徑簇入段縱向采樣距離與行駛速度呈分段線性變化，次層局部路徑簇入段縱向采樣距離為首層局部路徑簇的1.5 倍。

2） 在評價函數(shù)中引入曲率代價，既確保了最優(yōu)路徑滿足移動機器人的最大轉(zhuǎn)向曲率約束，又降低了局部路徑的平均曲率。

3） 在無障礙物和有障礙物場景下，改進OpenPlanner 算法篩選的最優(yōu)局部路徑平均曲率較傳統(tǒng)OpenPlanner 算法分別下降了31.3%和6.2%，局部路徑能夠被移動機器人較好地跟蹤。