基于改進OpenPlanner 算法的移動機器人局部路徑規劃

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

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

0 引言

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

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

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

1 傳統OpenPlanner 算法

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

1.1 狀態采樣

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

圖1 局部路徑簇分段結構Fig. 1 Sections of local path cluster

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

1.2 局部路徑簇生成

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

1.3 最優局部路徑篩選

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

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

評價函數的各加權項含義如下。

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

式中：z為單個模擬周期內局部路徑條數；ldis_resolution為相鄰局部路徑出段間距。

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

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

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

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

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

3） 障礙物代價cobstacle（i）：度量單個模擬周期內各條局部路徑與障礙物接近程度的代價。障礙物代價的計算過程分為2 個階段：第1 階段計算各條局部路徑平滑段和入段的碰撞情況，如圖5（a）所示，如果在以O為圓心的圓形碰撞檢測區域內存在障礙物輪廓點，則該條路徑被剔除；第2 階段計算在第1 階段中未發生碰撞的局部路徑出段的障礙物代價，該代價包括障礙物橫向代價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 算法

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

2.1 狀態采樣改進

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

圖6 改進OpenPlanner 算法局部路徑簇分段結構Fig. 6 Sections of local path cluster of improved OpenPlanner algorithm

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

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

2.2 評價函數改進

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

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

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

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

3 實驗與分析

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

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

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

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

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

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

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

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

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

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

4 結論

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

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

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