移動機器人自主避障最優路徑規劃算法設計

摘" 要：為確保移動機器人安全高效地到達目標點，移動機器人自主避障規劃算法成為移動機器人研究領域的重要部分。該文采用全局規劃加局部優化的路徑規劃算法進行移動機器人自主避障路徑規劃。首先，在傳統RRT算法的基礎上，優化啟發函數，通過對父節點重新連接進行迭代式地路徑搜索，提出RRT-star算法，將生成的路徑作為機器人的全局規劃結果。將多目標優化的TEB算法進行局部優化，該優化算法實時根據實際情況，添加了速度、加速度和時間對目標函數的約束，讓規劃與控制2個部分的協同性更好。將RRT-star算法和TEB軌跡規劃算法相融合，顯著提高局部路徑的安全性和平滑性，從而為移動機器人的自主導航提供更加可靠的保障。最后，通過仿真驗證算法的可行性和合理性，為移動機器人路徑規劃提供新的思路。

關鍵詞：最優路徑規劃;RRT算法;RRT-star算法;TEB局部規劃算法;自主導航

中圖分類號：TP242" " " 文獻標志碼：A" " " " " 文章編號：2095-2945（2025）11-0032-05

Abstract： In order to ensure that mobile robots reach the target point safely and efficiently， autonomous obstacle avoidance planning algorithms for mobile robots have become an important part of the research field of mobile robots. In this paper， a path planning algorithm based on global planning and local optimization is used to plan autonomous obstacle avoidance paths for mobile robots. First， based on the traditional RRT algorithm， the heuristic function is optimized， and the RRT-star algorithm is proposed by iteratively searching the path reconnecting the parent node， and the generated path is used as the global planning result of the robot. The TEB algorithm for multi-objective optimization is locally optimized. The optimization algorithm adds constraints on the objective function of speed， acceleration and time according to the actual situation in real time， making the two parts of planning and control more collaborative. The RRT-star algorithm and TEB trajectory planning algorithm are integrated to significantly improve the safety and smoothness of local paths， thereby providing a more reliable guarantee for autonomous navigation of mobile robots. Finally， simulation results verify the feasibility and rationality of the algorithm， which provides a new idea for path planning for mobile robots.

Keywords： optimal path planning; RRT algorithm; RRT-star algorithm; TEB local planning algorithm; autonomous" navigation

移動機器人自主避障路徑規劃是現代機器人技術的一個熱門研究領域，規劃算法是讓機器人實現真正的自主導航的基礎。目前已經有多種算法被提出，這些算法被應用于各種場景并得到越來越廣泛的認可和利用。然而，這些算法讓移動機器人自主避障路徑規劃時受環境影響大，且規劃出路徑平滑性差，導致控制機器人的運動變得更加復雜，甚至直接導致移動機器人局部卡死[1-3]。為了解決這些問題，越來越多的學者開始致力于將不同的算法融合，從而提高機器人的性能[4-5]。

與傳統的機器人路徑規劃算法相比，多算法融合的移動機器人路徑規劃具有極大優勢。這些組合算法能夠提升路徑的平滑性、對移動機器人的運動控制更加友好[6]。機器人自主避障最優路徑規劃算法在各個領域都受到了廣泛的重視和應用，并取得了顯著的成效。因此，越來越多的學者開始從事移動機器人自主避障最優路徑規劃多算法融合的研究。

本文提出了一種基于RRT-star全局規劃和TEB（Timed-Elastic-Band）局部優化的路徑規劃算法。這兩者互補性非常強，它們的結合能夠大大提高機器人的運動效率和路徑質量，并且能夠適用于各種實際應用場景。其中，RRT-star算法被用來產生全局路徑，而TEB局部的優化方法則用來消除局部最小點的影響來生成平滑路徑。這種組合算法在性能上表現出色，不僅能夠保證機器人安全避免碰撞，還能夠更加精確地執行任務和快速地檢測環境變化，這種基于RRT-star和TEB局部優化的路徑規劃算法的融合具有一定的研究及應用價值。

1" 全局路徑規劃與局部路徑優化算法設計

1.1" 全局路徑規劃RRT-star算法設計

傳統的全局規劃算法有基于圖搜索的方法，如 A* 算法[7]或 Dijkstra 算法[8]，這些算法在相對簡單的環境下工作良好，面對復雜環境時，其搜索效率以及可靠性會降低，但在實際中，移動機器人的工作環境動態多變、難以預測，本文提出啟發式搜索算法RRT系列的算法[9-10]。RRT算法的生長機制如圖1所示。

通過對父節點的重新連接，RRT-star 依舊采用啟發式搜索策略，但引入了一定的優先級控制，能夠更加高效地把握不確定環境中的最優路徑;RRT-star不僅考慮到沿著樹裂縫線找到目標節點這一基本策略，還計算了其父節點到目標節點的代價，從而在尋找連接目標節點的路徑時更有針對性，使搜索路徑質量更高;RRT-star還充分利用了之前生成的樹信息，為當前決策狀態提供更全面、更有效的參考信息，進而可減少決策時間。RRT-star算法生長機制以及父節點重新選擇如圖2、圖3所示。

1.2" 局部路徑優化TEB算法設計

TEB算法的基本思想是：首先在全局地圖中搜索獲得一條初始連續曲線作為參考路徑，本文采用RRT-star算法規劃出的路徑作為TEB的參考路徑;接下來在每個位姿點上添加時間參數，將位姿和時間參數表示為一個參數向量b=[s1，ΔT1，s2，ΔT2，…，ΔTn-1，sn]T;基于離散的初始節點使用樣條插值算法，在參考路徑上生成平滑的曲線，用曲線的控制點來定義機器人的運動軌跡;根據機器人實際大小、形狀和運動特性，建立碰撞檢測模型，判斷機器人在當前位置上運動軌跡是否與環境中的障礙物相交或接觸;使用非線性規劃求解器對運動軌跡進行優化，將運動的代價函數最小化，保證機器人避障、勻速運動，限制速度和加速度等;根據規劃優化結果，輸出機器人在運動過程中行駛軌跡上的最優控制輸入，實現路徑跟蹤和控制。

針對TEB問題的求解，因為解決非線性問題需要很大的計算量，所以TEB將采用無約束優化技術來處理局部路徑規劃的問題，該技術可以有效地避免復雜的約束條件，將實時的約束作為附加的懲罰項納入目標函數。通過將求解的目標函數進行最小二乘優化，轉換為最小二乘問題，求解最小二乘問題TEB算法采用的是LM（Levenberg-Marquardt）算法[11]，LM算法能夠在求解的魯棒性和效率之間得到一個平衡，通過使用基于圖優化的C++開源庫g2o，處理大型非線性最小二乘問題，并使用稀疏矩陣和高效的求解器來實現TEB多約束問題的高速求解。

2" 仿真平臺搭建與導航邏輯設計

驗證RRT與其父節點重選擇算法RRT-star則直接在Matlab平臺進行仿真驗證。多算法融合仿真采用Gazebo虛擬物理仿真平臺，在該平臺創建移動機器人的物理模型、障礙物以及場地，建立的物理模型能夠與ROS有插件接口，能夠很好地集成在ROS平臺下[12]。創建的Gazebo仿真平臺模型如圖4所示。

本文將利用ROS系統下的節點和話題進行移動機器人的規劃控制，使用Gmapping算法進行機器人的定位與地圖構建，通過接收“cmd_vel”話題以及move_base的導航框架，實現移動機器人的運動，并通過RVIZ可視化機器人的運動狀態。首先通過2D激光雷達進行地圖的信息構建，通過碰撞檢測算法檢測全局規劃路徑上是否有膨脹后障礙物，以及是否有平滑性（曲率）超過所設置的閾值的路徑，沒有則按照全局規劃路徑進行運動控制，如果有，則進行TEB算法局部優化后的路徑運動，當檢測到達目的地的時候，則整個運動規劃結束，如果沒有到達則回到碰撞檢測算法那一步，進行下一階段的規劃控制。2種算法融合下移動機器人自主避障路徑規劃的邏輯圖如圖5所示。

3" 算法有效性驗證

本設計以RRT-star作為全局路徑規劃算法、以TEB作為局部路徑優化算法，在Matlab、ROS平臺下進行了仿真，通過將規劃路徑的時間和規劃軌跡長度以及機器人運動軌跡的長度參數作為參考，驗證這2種算法融合的有效性。

3.1" RRT與RRT-star算法

針對RRT算法的缺點，研究了在RRT算法的基礎上再次對父節點進行搜索，雖然增加了計算算力，但可以從實驗數據發現RRT-star算法能夠全局規劃出更高質量的路徑，規劃出的路徑也更利于機器人的控制系統，極大概率地減少局部卡死的情況存在。Matlab下的仿真結果如圖6和圖7所示，實驗數據見表1。

3.2" RRT-star算法與TEB優化后的仿真實驗

在Gazebo下對TEB優化后的路徑進行機器人運動控制，通過圖8可以看出機器人運動軌跡相比于上述在Matlab下RRT-star規劃出的全局路徑平滑性更好，路徑更短。

對僅使用RRT-star算法作為規劃算法和RRT-star全局規劃+TEB局部優化2種規劃算法融合在ROS系統下進行仿真實驗。仿真結果中，軌跡長度對比圖如圖9所示，機器人運動時間對比分析圖如圖10所示。

通過對機器人運動的時間和運動軌跡長度的分析可以得出，RRT-star在全局優化上存在的隨機性較大，導致隨機生成的路徑也不穩定，對機器人的運動控制增加了難度，而加入TEB局部優化后，機器人的運動軌跡長度和時間都體現出極大的性能提升，不僅讓機器人的運動控制變得更為簡單，還讓機器人的路徑變得更為平滑。

4" 結論

本文設計了RRT-star全局路徑規劃以及TEB局部路徑優化算法，較其他算法而言更符合當前機器人規劃中的自適應性需求，同時還提升了機器人運動控制的簡單性。實驗結果表明，采用RRT-star全局路徑規劃和TEB局部路徑優化的多算法融合的移動機器人自主避障的規劃算法不僅能夠有效降低機器人前進路線的長度，提高路徑平滑度，也能夠適應不同場合帶來的意外或障礙物等影響。因此，它具有極高的適用性和靈活性，而且其高效性也可顯著提升在實際應用中的整體表現，為移動機器人路徑規劃提供了參考。

參考文獻：

[1] 戴年慧.用于輕度失智老人小區導航的移動機器人[D].鄭州：鄭州大學，2021.

[2] 林輝，李猛，姜雨田，等.差速輪式移動機器人控制系統設計[J].科技創新與應用，2024，14（11）：41-44.

[3] 白世杰.基于軌跡預測的室內移動機器人避障方法研究[D].哈爾濱：哈爾濱工業大學，2020.

[4] 周婷.基于多傳感數據的多移動機器人協同避障算法[J].黑龍江工業學院學報（綜合版），2022，22（5）：61-65.

[5] 劉鈺芃，王斌宇，張淞淏，等.基于改進IEKF的室內移動機器人定位算法研究[J].科技創新與應用，2023，13（27）：37-40.

[6] 林吉照.螺旋驅動式平糧機器人姿態控制及系統設計[D].長春：吉林大學，2022.

[7] 宋啟松.移動機器人路徑規劃算法優化研究[D].貴陽：貴州大學，2021.

[8] 祝晨旭，仲志丹，張浩博，等.多主體機器人路徑規劃方法研究[J].制造業自動化，2022，44（6）：72-74.

[9] 程浩，門玉濤，張文昌，等.救援機器人導航技術研究現狀與展望[J].醫療衛生裝備，2022，43（9）：82-90，96.

[10] RASEKHIPOUR Y， KHAJEPOUR A， CHEN S K， et al. A Potential Field-Based Model Predictive Path-Planning Controller for Autonomous Road Vehicles[J]. IEEE Transaction son Intelligent Transportation Systems，2017，18（5）.

[11] LIU J， JI JIE， REN Y， et al. Erratum to： Path Planning for Vehicle Active Collision Avoidance based on Virtual Flow Field[J]. International Journal of Automotive Technology，2022，23（1）：293.

[12] 裴雙成，鐘波.基于Gazebo虛擬世界的草莓采摘機器人仿真研究[J].南方農機，2024，55（5）：57-61.