基于改進RRT算法的機器人三維路徑規(guī)劃方法

中圖分類號：TP242 文獻標(biāo)識碼：A 文章編號：2096-3998（2025）04-0060-09

摘要：針對快速擴展隨機樹（RRT）算法在基于三維空間的復(fù)雜環(huán)境中收斂速度極慢，在最大迭代次數(shù)較低時難以找到路徑，得到的最終路徑質(zhì)量較差的問題，提出了一種用于三維空間的機器人路徑規(guī)劃的改進RRT算法。在RRT算法的基礎(chǔ)上添加了目標(biāo)偏向采樣策略、節(jié)點優(yōu)化策略、動態(tài)步長策略和路徑修剪策略。通過目標(biāo)偏向采樣策略，使隨機采樣點以一定的概率指向目標(biāo)點，其余隨機采樣點引入人工勢場法原理再進一步優(yōu)化，讓隨機樹具有目標(biāo)導(dǎo)向性；節(jié)點優(yōu)化策略判斷節(jié)點是否處在障礙物區(qū)域，從而選取合適的隨機節(jié)點，減少碰撞檢測概率；采用動態(tài)步長策略提高了尋徑效率，減少冗余點以便于后續(xù)的路徑修剪；利用路徑修剪策略對最終路徑進一步優(yōu)化。在三維地圖環(huán)境中進行仿真實驗驗證，結(jié)果表明，與傳統(tǒng)的RRT算法和RRT*算法相比較，改進的RRT算法性能更好，提高了路徑質(zhì)量并縮短了尋徑時間。

隨著機器人技術(shù)的快速發(fā)展，機器人在各個領(lǐng)域得到廣泛的應(yīng)用，例如智能制造、健康醫(yī)療、倉儲物流、農(nóng)業(yè)種植、軍事安全、交通運輸、教育、家庭服務(wù)、娛樂等領(lǐng)域都有機器人的身影[1]。路徑規(guī)劃是機器人領(lǐng)域的核心問題。傳統(tǒng)固定基座以及基座連接單一導(dǎo)軌的機械臂只能用于簡單環(huán)境下小范圍內(nèi)的作業(yè)要求，擁有較高自由度的機械臂無法發(fā)揮出它的最大價值。因此機器人對生成無碰撞路徑的自主運動規(guī)劃技術(shù)的需求變得更加迫切[2]。

機器人路徑規(guī)劃是指在給定的環(huán)境中，機器人如何從起始位置移動到目標(biāo)位置，同時避開障礙物且滿足機器人運動學(xué)等條件的過程。目前，常用的路徑規(guī)劃算法主要分為基于搜索、采樣和智能的路徑規(guī)劃算法[3]。基于搜索的路徑規(guī)劃算法包括 Dijkstra算法[4]、A*算法[5]等。基于采樣的路徑規(guī)劃算法包括快速擴展隨機樹（Rapidly-exploringRandom Trees，RRT）[6]、RRT-Connect[7] PRM^[8] 等?；谥悄芩惴ǖ穆窂揭?guī)劃算法包括蟻群算法[9]、遺傳算法[1]等。對于高自由度的機器人路徑規(guī)劃來說，快速精確的找出一條最優(yōu)路徑或漸進最優(yōu)路徑尤為重要，基于隨機采樣的路徑規(guī)劃算法不會隨著維數(shù)的增加而導(dǎo)致其規(guī)劃效率降低，適合應(yīng)用于機器人。

RRT是一種經(jīng)典的基于采樣的路徑規(guī)劃算法，具有搜索能力強，尋徑速度較快，在高維空間能夠有效找到路徑等特點，但由于其采樣方式的隨機性，造成迭代次數(shù)高、搜索速度慢、內(nèi)存開銷大，會產(chǎn)生過多冗余節(jié)點，最終路徑較為復(fù)雜等。針對以上問題，許多學(xué)者進行了不同程度上的改進。Kuffner 等[12]提出RRT-Connect算法，通過同時從起始點和目標(biāo)點擴展兩棵樹來加快路徑搜索速度，增加了找到路徑的概率，當(dāng)兩個樹接近時，將它們連接起來形成完整路徑。Karaman 等[13]提出RRT*算法，通過重新連接樹上的節(jié)點來優(yōu)化路徑，減少路徑長度，然后不斷迭代和更新路徑，使得最終路徑逐漸接近最優(yōu)解。Gammell等[14]提出Informed-RRT*算法，通過動態(tài)調(diào)整樹的探索范圍，僅在有可能生成更優(yōu)路徑的區(qū)域進行擴展，從而減少計算量。王冠強等[15提出增設(shè)第三、四棵擴展樹、同時限定采樣點選區(qū)范圍的RRT-Connect算法，加快了算法的收斂速度。

雖然上述研究都對RRT算法做出了進一步改進，但改進后的算法依然有隨機性高、存在冗余路徑、路徑質(zhì)量差等問題，基于此，本文提出了改進RRT算法。

1傳統(tǒng)RRT算法

RRT算法是一種強大而靈活的路徑規(guī)劃方法，具有概率完備性，能夠在復(fù)雜環(huán)境中快速找到可行路徑。該算法通過構(gòu)建一個隨機樹來探索空間，逐漸找到從起點到目標(biāo)點的路徑[16]。從起始點作為根節(jié)點開始，在空間中隨機采樣，選擇距離隨機點最近的節(jié)點作為該隨機節(jié)點的父節(jié)點，按照指定步長擴展新節(jié)點，并對兩點進行碰撞檢測，若檢測通過則將新節(jié)點加入樹中，不通過則需重新采樣，反復(fù)循環(huán)直到擴展樹達到距離目標(biāo)點所設(shè)閾值，在有限的迭代次數(shù)里總能找到路徑。RRT算法節(jié)點擴展的原理如圖1所示。

RRT算法的偽代碼如下所示。其中，iterMax為最大迭代次數(shù)，step為固定步長， Thr 為距離目標(biāo)點閾值。

圖1RRT算法節(jié)點擴展原理圖

2改進RRT算法

2.1 主要步驟

針對傳統(tǒng)RRT算法存在的上述問題，本文在傳統(tǒng)RRT的基礎(chǔ)上提出一種改進的RRT算法，結(jié)合目標(biāo)偏向采樣策略、節(jié)點優(yōu)化策略、動態(tài)步長策略和路徑修剪策略提升算法性能。

改進RRT算法的主要步驟：

（1）初始化。確定機器人障礙物空間 C_obj 與非障礙物空間 C_free 、初始點 X_start 與目標(biāo)點 X_goal 信息、距離目標(biāo)點閾值 Thr 、最大迭代次數(shù)iterMax，按照環(huán)境信息設(shè)置步長基數(shù) T_step 、偏置概率 P₀ 、人工勢場法（APF）引力系數(shù) ε 和斥力系數(shù) σ 以及障礙物影響距離 ρ₀ 。

（2）生成隨機采樣點。按照目標(biāo)偏置采樣策略與節(jié)點優(yōu)化策略，生成隨機節(jié)點。

（3）尋找樹中距離隨機采樣點 X_rand 最近節(jié)點 X_near （4）按照一個步長基數(shù) T_step 沿著最近節(jié)點 X_near 指向隨機采樣點 X_rand 方向擴展新節(jié)點 X_new 。

（5）X_new 與 X_rand 之間進行碰撞檢測，碰撞則需返回到第二步重新采樣，無碰撞則采用動態(tài)步長策略，改變步長的大小，直至碰撞檢測不通過返回兩次前的步長大小作為該次擴展的步長，生成新節(jié)點 X_new 。

（6）判斷新節(jié)點 X_new 距離目標(biāo)點 X_goal 是否小于閾值，小于直接連接兩點，找到路徑，否則跳到第二步往復(fù)循環(huán)。（7）路徑修剪。路徑搜索成功后進行該步，對最終路徑進行剪枝處理，剔除該路徑的冗余節(jié)點，優(yōu)化了原始路徑。

2.2 目標(biāo)偏向采樣策略

由于RRT算法在采樣過程中有很強的隨機性，為減少冗余節(jié)點，使隨機樹具有目標(biāo)導(dǎo)向性，故在算法的隨機采樣過程中加入目標(biāo)偏置采樣策略，首先以一定的概率 P₀（00lt;1） 讓隨機采樣點 X_rand 就是目標(biāo)點 X_goal 以 （1-P₀） 的概率在地圖上生成采用節(jié)點優(yōu)化策略的隨機點后，結(jié)合人工勢場法的引力函數(shù)與斥力函數(shù)生成新的隨機節(jié)點Xnewrand o

概率偏置采樣選取隨機節(jié)點的方式：

式中， p 是rand函數(shù)在（O，1）區(qū)間任意的值，Sample為空間中的任意一點。

人工勢場法（APF）是一種局部路徑規(guī)劃算法，通過為障礙物和目標(biāo)點設(shè)置虛擬的勢場來指導(dǎo)運動，其勢場法思想如圖2所示。

圖2人工勢場法思想

隨機點 X_rand=Sample 時，結(jié)合人工勢場法的性質(zhì)，受到目標(biāo)點的引力和障礙物的斥力，離障礙物越近斥力越大，當(dāng)與障礙物超過一定距離后斥力為0;隨機點離目標(biāo)點越遠引力越大，反之越小。引力與斥力的計算方法：

F_grav=ερ（q_goal-q），

式中， ε 是引力常數(shù)， S（q_goal-q） 表示隨機點與目標(biāo)點的距離， σ 是斥力常數(shù)， ρ（q，q_obj） 表示隨機點與障礙物的距離， ρ_ρ0 表示障礙物影響距離。

最終，隨機節(jié)點 X_rand 融合人工勢場法思想后更新隨機節(jié)點 X_rand^′ 原理如圖3所示，具體表達式為

式中，當(dāng) ρ（q，q_obj）lt;ρ₀ 時， X_new 需要減去斥力， s 表示每次迭代步長。

引入概率目標(biāo)偏置和人工勢場法后，擴展樹可以有效地朝著目標(biāo)點行進且遠離了障礙物，加快了搜索效率，減少了過多冗余點。

2.3 節(jié)點優(yōu)化策略

由于傳統(tǒng)RRT算法隨機采樣點的隨機性，導(dǎo)致過多的采樣點位于障礙物空間中，不僅影響了算法效率，還增加了目標(biāo)偏向采樣策略中APF算法的斥力函數(shù)的使用次數(shù)。因此引入節(jié)點優(yōu)化策略和障礙物膨脹策略，對Sample進行篩選，若Sample處于障礙物空間或是障礙物膨脹過后的空間則以 90% 的概率重新采樣，優(yōu)化隨機采樣點的同時保證了算法的概率完備性。在整個工作空間進行采樣如圖4所示。

圖3改進隨機點原理圖

本策略使得大部分隨機采樣點避開了不合適的位置，提升了隨機采樣點的質(zhì)量，更適用于結(jié)合人工勢場法，減少冗余點的生成，還進一步提高了碰撞檢測通過概率，在障礙物過多的復(fù)雜環(huán)境中效果更佳明顯，極大的節(jié)省了計算資源和時間。最終新節(jié)點的選取方式為

圖4隨機點采樣

式中， 為 X_rand^′ 與 X_near 之間的歐氏距離。

2.4 動態(tài)步長策略

傳統(tǒng)RRT算法在擴展樹擴展過程中都是按照指定步長擴展新節(jié)點，導(dǎo)致擴展樹在障礙物較少的情況下以較慢的速度進行搜索，當(dāng)遇到障礙物過多的場景時，可能會找不到合適的路徑，增加碰撞檢測次數(shù)，嚴(yán)重影響算法的效率。

針對以上問題，本文提出一種基于碰撞檢測的動態(tài)步長方法，改變步長 step 按照步長基數(shù) T_step 以step=step+T_step 的方式進行擴展，更新 X_new 的位置，反復(fù)進行碰撞檢測調(diào)節(jié)步長的大小，直至碰撞檢測不通過返回兩次前步長的值，此時 step=step-2T_step 為最終步長，其偽代碼如下：

采用動態(tài)步長后的擴展樹如圖5所示。

圖5采用動態(tài)步長后擴展樹

對于基于目標(biāo)偏置策略的RRT算法與基于目標(biāo)偏置策略和動態(tài)步長策略的RRT算法，分別進行100次路徑規(guī)劃仿真，實驗數(shù)據(jù)見表1。圖6和圖7中的（a）為基于目標(biāo)偏置策略的RRT算法，（b）為基于目標(biāo)偏置策略和動態(tài)步長策略的RRT算法。對比可知，采用動態(tài)步長策略后，刪除了大量的冗余節(jié)點，縮短了尋徑時間，提升了路徑質(zhì)量。

表1二者實驗數(shù)據(jù)對比

圖6擴展樹對比

圖7最終路徑對比

（a）RRT最終路徑

（b）動態(tài)步長RRT最終路徑

2.5 路徑修剪策略

由于RRT算法在路徑規(guī)劃時的隨機性，因此該算法得不到最優(yōu)路徑。為提升機器人的工作效率，故還需對最終路徑作進一步優(yōu)化，保證路徑成本最低。本文基于三角不等式原理 （a+bgt;c，a，bc 為三角形的任意一條邊）修剪冗余節(jié)點，降低路徑成本。如圖8所示的路徑中，從后面的節(jié)點依次向前判斷，節(jié)點5連接其父節(jié)點的父節(jié)點節(jié)點3路徑不可行不可修改，接下來判斷節(jié)點4，節(jié)點4連接其父節(jié)點的父節(jié)點節(jié)點2該路徑可行，故舍去節(jié)點3，此時判斷節(jié)點4與節(jié)點2的父節(jié)點節(jié)點1，仍滿足無碰撞路徑，故舍去結(jié)點2，最終路徑為節(jié)點1—節(jié)點4—節(jié)點5。

圖8路徑修剪策略

在路徑修剪策略執(zhí)行完成后，原路徑的冗余點被刪除，縮短了路徑長度，具有顯著的優(yōu)化效果。

3 三維地圖仿真實驗

3.1 實驗準(zhǔn)備

為了驗證改進后的RRT算法在三維空間中的性能，本文在處理器為13thGenIntel（R）Core（TM）i7-13700F，主頻為 2.10GHz 的計算機上進行仿真。設(shè)置地圖大小為 1 000×1 000×1 000 ，起始點坐標(biāo)為（0，0，0），目標(biāo)點坐標(biāo)為（900，900，900），設(shè)置簡單不規(guī)則障礙物環(huán)境模型和復(fù)雜不規(guī)則障礙物環(huán)境模型兩種，分別采用RRT算法、RRT*和改進的RRT算法在兩個地圖上進行10O次的仿真實驗，取平均值進行對比。

3.2 簡單不規(guī)則障礙物環(huán)境下的運動規(guī)劃

簡單不規(guī)則障礙物環(huán)境包括3個長方體障礙物、2個圓柱體障礙物、2個球體障礙物。其中，偏置采樣概率 P₀ 設(shè)為0.5，步長基數(shù) T_step 設(shè)為5，引力系數(shù) ε 設(shè)為0.001，斥力系數(shù) σ 設(shè)為2000以及障礙物影響距離 ρ₀ 設(shè)為100，距離障礙物閾值Thr設(shè)為10，最大迭代次數(shù)iterMax為 10 000 。

RRT算法、RRT*算法和改進的RRT算法在該環(huán)境下的路徑規(guī)劃仿真結(jié)果擴展樹和最終路徑如圖9所示，藍色為擴展樹的分支，紅色為最終路徑，改進后的RRT算法的綠色為修剪后的路徑。在圖9中可以看出，改進的RRT算法相較于其他算法，冗余節(jié)點較少，搜索速度快，路徑更短。

（a）簡單不規(guī)則障礙物環(huán)境下RRT算法

（b）簡單不規(guī)則障礙物環(huán)境下RRT*算法

（c）簡單不規(guī)則障礙物環(huán)境下改進RRT算法

圖9簡單不規(guī)則障礙物環(huán)境仿真實驗結(jié)果

RRT算法、RRT*算法和改進的RRT算法在該環(huán)境下進行100組重復(fù)實驗，實驗數(shù)據(jù)如表2所示。改進的RRT算法相較于RRT算法平均運行時間減少了約 97.1% ，路徑長度縮短了約 70.2% ，節(jié)點數(shù)減少了約 99.3% ，傳統(tǒng)的RRT算法在此地圖環(huán)境規(guī)定的最大迭代次數(shù)上的尋徑成功率僅為 64% 。改進的RRT算法相較于RRT*算法平均運行時間減少了約 89.4% ，路徑長度縮短了約 5.1% ，節(jié)點數(shù)減少了約 97.4% 。

表2簡單不規(guī)則障礙物環(huán)境下三者實驗對比

3.3復(fù)雜不規(guī)則障礙物環(huán)境下的運動規(guī)劃

相較于簡單不規(guī)則障礙物環(huán)境，復(fù)雜不規(guī)則障礙物環(huán)境在其基礎(chǔ)上增加了7個長方體障礙物、5個圓柱體障礙物及5個球體障礙物。同理將RRT算法、RRT*算法和改進的RRT算法在該環(huán)境下進行路徑規(guī)劃，其中，偏置采樣概率 P₀ 設(shè)為0.5，步長基數(shù) T_step 設(shè)為5，引力系數(shù) ε 設(shè)為0.0001，斥力系數(shù) σ 設(shè)為200，障礙物影響距離 ρ₀ 設(shè)為100，距離障礙物閾值 Thr 設(shè)為10。由于環(huán)境復(fù)雜且RRT算法的隨機性過于強，最大迭代次數(shù)iterMax設(shè)為30000。

RRT算法、RRT*算法和改進的RRT算法在復(fù)雜不規(guī)則環(huán)境下的路徑規(guī)劃仿真結(jié)果擴展樹和最終路徑如圖10所示，圖中可以看出，改進的RRT算法相較于其他算法，冗余節(jié)點較少，搜索速度快，路徑更短。三者進行100組試驗后，實驗數(shù)據(jù)如表3所示。改進的RRT算法相較于RRT算法平均運行時間減少了約 98.5% ，路徑長度縮短了約 70.9% ，節(jié)點數(shù)減少了約 99.7% ，傳統(tǒng)的RRT算法在此地圖環(huán)境規(guī)定的最大迭代次數(shù)上的尋徑成功率僅為 3% 。改進的RRT算法相較于RRT*算法平均運行時間減少了約 81.1% ，路徑長度縮短了約 3.8% ，節(jié)點數(shù)減少了約 93% 。

（c）復(fù)雜不規(guī)則障礙物環(huán)境下改進RRT算法

圖10復(fù)雜不規(guī)則障礙物環(huán)境仿真實驗結(jié)果

表3復(fù)雜不規(guī)則障礙物環(huán)境下三者實驗對比

綜上，改進的RRT算法在簡單與復(fù)雜環(huán)境都保持良好，在運行時間、路徑長度以及平均節(jié)點數(shù)上均優(yōu)于RRT算法和RRT*算法，其中RRT算法由于其隨機性會隨著環(huán)境的復(fù)雜程度升高或者空間維數(shù)的增大而需大量采樣點，采樣效率極低，在規(guī)定的時間或是最大迭代次數(shù)內(nèi)難以找到路徑。

4 結(jié)束語

本文針對RRT算法在三維空間中冗余點多、尋徑時間長、路徑過于曲折以及在復(fù)雜環(huán)境中難以找到路徑等問題，提出一種基于目標(biāo)偏執(zhí)策略、采樣點優(yōu)化策略、動態(tài)步長策略和路徑修剪策略的改進的RRT算法。通過在簡單和復(fù)雜兩種環(huán)境下分別進行100組路徑規(guī)劃仿真實驗，實驗結(jié)果表明在三維環(huán)境下，改進的RRT算法較于傳統(tǒng)的RRT算法平均運行時間減少了約 97.8% ，平均路徑長度縮短了約70.6% ，平均節(jié)點數(shù)減少了約 99.5% ，在任意環(huán)境下都能較快地找到路徑，能夠很好地解決傳統(tǒng)RRT算法的上述問題。雖然改進的RRT算法最終路徑得到了優(yōu)化，但其節(jié)點的前后轉(zhuǎn)折過大，需要機器人有很大的加速度，因此后續(xù)還需進行路徑平滑處理，以滿足機器人運動學(xué)，更高效地應(yīng)用在機器人實際的路徑規(guī)劃中。

［參考文獻]

[1]劉佰陽，鄭宇，魏琳，等.面向模型未知的冗余機器人運動規(guī)劃方案[J].蘭州大學(xué)學(xué)報（自然科學(xué)版），2023，59（4）：506-511.

[2] 唐永興，朱戰(zhàn)霞，張紅文，等.機器人運動規(guī)劃方法綜述[J].航空學(xué)報，2023，44（2）：181-212.

[3] 崔煒，朱發(fā)證.機器人導(dǎo)航的路徑規(guī)劃算法研究綜述[J].計算機工程與應(yīng)用，2023，59（19）：10-20.

[4]KATONA K，NEAMAH HA，KORONDI P.Obstacle Avoidanceand Path Planning Methods for Autonomous NavigationofMobileRobot[J].Sensors，2024，24（11） ：79-84.

[5] 賈慶軒，陳鋼，孫漢旭，等.基于 A^* 算法的空間機械臂避障路徑規(guī)劃[J].機械工程學(xué)報，2010，46（13）：109-115.

[6] CHEN Q，JIANG H，ZHENG Y.Summary of Rapidly-Exploring Random Tree Algorithm in Robot Path Planning[J]. Com-puter Engineering and Application，2019，55（16） ：10-17.

[7]朱波，姜官武，王旭亮，等.基于改進RRT-Connect算法的移動機器人路徑規(guī)劃[J].組合機床與自動化加工技術(shù)，2024（8） ：33-37.

[8］封澳，楊錦宇，謝玉陽，等.基于改進PRM算法的機器人路徑規(guī)劃[J].計算機技術(shù)與發(fā)展，2024，34（2）：127-33.

[9]劉志海，高龍，韓文鈺，等.基于改進遺傳算法的移動機器人路徑規(guī)劃研究[J].制造業(yè)自動化，2024，46（5）：26-30.

[10] WEIT，LONG C.Path planningformobilerobot basedonimproved geneticalgorithm[J].JournalofBeijing UniversityofAeronauticsand Astronautics，2020，46（4） ：703-711.

[11] 韋玉海，張輝，劉理，等.基于AMRRT-Connect 算法的移動機器人路徑規(guī)劃[J].武漢大學(xué)學(xué)報（工學(xué)版），2022，55（5） ：531-538.

[12]KUNERJJ，LAVALLE S M. RRT-connect：Anefficientapproach to single-query path planning[J].Proceedings2000ICRAMillnnium Conference IEEE International Conference on Robotics and Automation Symposia Proceedings（Cat No00CH37065），2000，19（5） ：995-1001.

[13] KARAMAN S，WALTER M R，PEREZ A，et al. Anytime motion planning using the RRT[C]//2011 IEEE InternationalConference On Robotics And Automation，2011.

[14] GAMMELL JD，SRINIVASA S S，BARFOOT T D.Informed RRT*：Optimal Sampling-based Path Planing Focused viaDirect Sampling of an Admissible Elipsoidal Heuristic[Z/OL].[2024-08-10].https：//arxiv.org/abs/1404.2334.

[15] 王冠強，張馳洲，陳明松，等.融合RRT-Connect 和DWA算法的室內(nèi)移動機器人單目標(biāo)點導(dǎo)航任務(wù)研究[J].中南大學(xué)學(xué)報（自然科學(xué)版），2023，54（11）：4326-4337.

[16] 周瑞紅，李彩虹，張耀玉，等.基于改進RRT算法的移動機器人路徑規(guī)劃[J].山東理工大學(xué)學(xué)報（自然科學(xué)版），2024，38（5） ：54-60.

［責(zé)任編輯：李莉］

Abstract：To address the issues of extremely slow convergence speed of the Rapidly-exploring Random Tree（RRT）algorithm in complex three-dimensional space environments，dificulty in finding paths when the maximum number of iterations is low，and poor quality of the final path obtained，this paper proposes an improved RRT algorithm for robot path planning in 3D space. Specificall，based on RRT algorithm，we add target bias sampling strategy， node optimization strategy， dynamic step size strategy and path pruning strategy. Through the target bias sampling strategy，the random sampling points point to the target point with a certain probability，and the other random sampling points are further optimized by introducing the principleof artificial potential field method，so that the random tree has the target orientation.The node optimization strategy determines whetherthe node is in the obstacle area，and then selects the appropriate random node to reduce the colision detection probability.Dynamic step size strategy is adopted to improve the efciency of path finding， reduce redundant points and facilitate subsequent path pruning. The path pruning strategy is used to further optimize the final path.The simulation results show that compared with the traditional RRT algorithm and RRT algorithm，the improved RRT algorithm has better performance，significantly improving the path quality and shortening the path searching time.

Key words：path planning；RRT; artificial potential field method; dynamic step size; path pruning