移動機(jī)器人自主避障最優(yōu)路徑規(guī)劃算法設(shè)計(jì)

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

關(guān)鍵詞：最優(yōu)路徑規(guī)劃;RRT算法;RRT-star算法;TEB局部規(guī)劃算法;自主導(dǎo)航

中圖分類號：TP242" " " 文獻(xiàn)標(biāo)志碼：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

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

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

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

1" 全局路徑規(guī)劃與局部路徑優(yōu)化算法設(shè)計(jì)

1.1" 全局路徑規(guī)劃RRT-star算法設(shè)計(jì)

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

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

1.2" 局部路徑優(yōu)化TEB算法設(shè)計(jì)

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

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

2" 仿真平臺搭建與導(dǎo)航邏輯設(shè)計(jì)

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

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

3" 算法有效性驗(yàn)證

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

3.1" RRT與RRT-star算法

針對RRT算法的缺點(diǎn)，研究了在RRT算法的基礎(chǔ)上再次對父節(jié)點(diǎn)進(jìn)行搜索，雖然增加了計(jì)算算力，但可以從實(shí)驗(yàn)數(shù)據(jù)發(fā)現(xiàn)RRT-star算法能夠全局規(guī)劃出更高質(zhì)量的路徑，規(guī)劃出的路徑也更利于機(jī)器人的控制系統(tǒng)，極大概率地減少局部卡死的情況存在。Matlab下的仿真結(jié)果如圖6和圖7所示，實(shí)驗(yàn)數(shù)據(jù)見表1。

3.2" RRT-star算法與TEB優(yōu)化后的仿真實(shí)驗(yàn)

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

對僅使用RRT-star算法作為規(guī)劃算法和RRT-star全局規(guī)劃+TEB局部優(yōu)化2種規(guī)劃算法融合在ROS系統(tǒng)下進(jìn)行仿真實(shí)驗(yàn)。仿真結(jié)果中，軌跡長度對比圖如圖9所示，機(jī)器人運(yùn)動時(shí)間對比分析圖如圖10所示。

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

4" 結(jié)論

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

參考文獻(xiàn)：

[1] 戴年慧.用于輕度失智老人小區(qū)導(dǎo)航的移動機(jī)器人[D].鄭州：鄭州大學(xué)，2021.

[2] 林輝，李猛，姜雨田，等.差速輪式移動機(jī)器人控制系統(tǒng)設(shè)計(jì)[J].科技創(chuàng)新與應(yīng)用，2024，14（11）：41-44.

[3] 白世杰.基于軌跡預(yù)測的室內(nèi)移動機(jī)器人避障方法研究[D].哈爾濱：哈爾濱工業(yè)大學(xué)，2020.

[4] 周婷.基于多傳感數(shù)據(jù)的多移動機(jī)器人協(xié)同避障算法[J].黑龍江工業(yè)學(xué)院學(xué)報(bào)（綜合版），2022，22（5）：61-65.

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

[6] 林吉照.螺旋驅(qū)動式平糧機(jī)器人姿態(tài)控制及系統(tǒng)設(shè)計(jì)[D].長春：吉林大學(xué)，2022.

[7] 宋啟松.移動機(jī)器人路徑規(guī)劃算法優(yōu)化研究[D].貴陽：貴州大學(xué)，2021.

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

[9] 程浩，門玉濤，張文昌，等.救援機(jī)器人導(dǎo)航技術(shù)研究現(xiàn)狀與展望[J].醫(yī)療衛(wèi)生裝備，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ī)器人仿真研究[J].南方農(nóng)機(jī)，2024，55（5）：57-61.