改進RRT算法的機器人路徑規劃

譚波 羅均 羅雨松 胡春暉 卓俊康 白澤鑫 田金濤

doi：?10.11835/j.issn.1000-582X.2022.107

收稿日期：2021-10-14

網絡出版日期：2022-03-02

作者簡介：譚波（1999—），男，碩士研究生，主要從事機器人路徑規劃研究，（E-mail）tanbo@cqu.edu.cn。

通信作者：羅均，男，教授，博士生導師，機械傳動國家重點實驗室主任，主要從事以控制為基礎的機器人運動載體對環境力載荷的抗干擾技術研究，（E-mail）luojun@shu.edu.cn。

摘要：機器人已被廣泛應用于日常生活之中。路徑規劃作為機器人的主要技術之一，優秀的路徑規劃算法能提升機器人的工作效率、降低其使用成本，并為研究機器人的導航打下良好的基礎。RRT（rapidly-exploring random trees）算法具有擴展性強的優點，但存在路徑長度并非最優、光滑性差等不足，為此提出反向尋優和三次樣條曲線插值以改進算法，并在MATLAB和ROS（robot operating system）系統中仿真。結果表明：改進后的RRT算法能降低路徑長度，減少節點數目，提高光滑性，實現了算法的有效性。

關鍵詞：RRT算法；路徑規劃；反向尋優；三次樣條曲線插值；ROS系統

中圖分類號：TP242 ?????????文獻標志碼：A ?????文章編號：1000-582X（2023）09-013-10

Robot path planning based on improved RRT algorithm

TAN Bo， LUO Jun， LUO Yusong， HU Chunhui， ZHUO Junkang， BAI Zexin， TIAN Jintao

（State Key Laboratory of Mechanical Transmissions， Chongqing University， Chongqing 400044， P. R. China）

Abstract： Robots have been widely used in daily life. Path planning is one of the main technologies of robots. An outstanding path planning algorithm can improve the work efficiency of robots， reduce cost， and lay a good foundation for the research of robot navigation. The RRT （rapidly-exploring random trees） algorithm， which is well known for its strong scalability， has some shortcomings， such as suboptimal path length and poor smoothness. An improved algorithm of reverse optimization and cubic spline interpolation is proposed， and then simulated in different scenarios in MATLAB. Taking the restaurant environment as an example in ROS （robot operating system）， the experimental results show that the improved RRT algorithm can decrease the path length and the number of nodes， and improve the smoothness， verifying the effectiveness of the algorithm.

Keywords： RRT algorithm; path planning; reverse optimization; cubic spline interpolation curve; robot operating system

進入21世紀后，隨著前沿科學技術的發展，機器人技術被不少國家列為重點發展的一項高新技術，且相關產業被用作衡量綜合國力強弱的標志^[1]。現實生活中廣泛為人們服務的機器人，其相關的路徑規劃問題面臨著前所未有的機遇，同時也充滿著挑戰，使其成為機器人的核心技術之一^[2-3]。路徑規劃算法的好壞，直接決定事先設定的任務能否圓滿完成并達到理想的效果。優秀的路徑規劃技術不僅能減少資金投入，降低運行時間，而且還能提高機器人的搜索效率，為機器人導航的研究奠定良好的基礎^[4-6]。

快速擴展隨機樹（rapidly-exploring random trees，RRT）算法作為一種基于采樣的路徑規劃算法，地圖不需要做預處理，且可以迅速搜索。但是，RRT也有不足之處，譬如，搜索時較為盲目，在復雜或者動態環境里往往會出現計算極其復雜、所需的時間過長、易于陷入死區等情況。針對以上問題，康亮等^[7]將滾動窗口應用于RRT算法，保留了漸進最優的特點，并且減少了運行時間，增強了算法對未知區域的探索能力；賈李紅等^[8]提出一種融合算法，即將人工勢場法與RRT算法配合使用，該算法有效地解決了RRT算法局部避障的難題，而且使規劃效率大幅度提升；孫欽鵬等^[9]提出了一種適應變步長的調整方法，對新目標點設計出一種新的選擇方式來改進RRT算法，通過仿真實驗證明，改進的RRT算法在復雜和狹窄環境中的平均路徑長度更短。

以上對RRT算法的改進，使搜索能力進一步增強，搜索具有目標性，路徑長度縮短或節點數目減少，但路徑光滑性依舊很差，路徑長度還是很長且不是最優，算法有待進一步改進。因此，筆者提出反向尋優和三次樣條曲線插值改進算法，在MATLAB中仿真驗證并進行實例分析，并在ROS系統中以餐廳為例做進一步仿真分析，驗證了改進算法的有效性。

1RRT算法簡介

1.1RRT算法基本原理

LaValle^[10]在1998年首次提出的RRT算法是一種高效的路徑規劃算法。RRT算法以初始的一個根節點，通過隨機采樣的方法在空間搜索，然后添加一個又一個的葉節點來不斷擴展隨機樹。當目標點進入隨機樹里面后，隨機樹擴展立即停止，此時能找到一條從起始點到目標點的路徑。擴展樹的具體結構如圖1所示。

1.2RRT算法缺點

1）穩定性差。由于擴展樹節點的生長方向是隨機的，所以在相同的任務下重復規劃，也會出現不同的路徑，導致每次規劃的結果都不同，甚至偏離最優路徑。

2）隨機性強。因為擴展樹是在整個區域內隨機搜索，不受目標點的引導和啟發，因此，存在大量無效的擴展節點，使算法具有很強的隨機性。

3）光滑性差。RRT算法生成的路徑是很多折線段相連，并不是一條光滑的曲線。

4）不適合動態環境。RRT算法規劃出一條可行路徑后，當環境中出現動態障礙物時，RRT算法不能有效地檢測，因此，不適合動態環境下的路徑規劃問題^[11-12]。

1.3RRT算法改進思路

針對RRT算法存在的缺點，總結各類改進方法，得到如下幾點常見的改進思路。

1）偏向性方面：為了提高收斂速度和搜索效率，擴展樹生長方向是目標點還是隨機點是依靠隨機概率來確定的，與原始的RRT算法相比，改進后的算法能夠加速收斂。

2）節點選取方面：通過改進父節點選取的方式來改進RRT算法，改進后的算法能夠提高收斂速度，減少節點數目，改善路徑質量。

3）光滑性方面：通過數學中的曲率約束，使生成規劃路徑光滑無節點，使路徑更具有可行性^[13-14]。

在上述改進思路的啟發下，在節點選取方面本研究中提出了反向尋優改進RRT算法，在光滑性方面用三次樣條曲線插值優化。

2RRT算法改進

2.1反向尋優

機器人的路徑若不是最優，就會增加工作時間，降低運作效率。為了改善RRT算法的路徑長度，通過反向尋優的方法來改進RRT算法，如圖2所示。反向尋優方法充分應用兩點之間線段最短這一數學思想，達到縮短路徑長度的目的。圖中A點為起點，B點為終點，原始的RRT算法擴展產生的節點為X_i（i=1，2，…，6），障礙物用黑色矩形表示，原始的RRT算法初步生成的路徑為A與B之間的細實線，圖中A與B之間的粗實線為改進后生成的路徑。

改進后的算法偽代碼如表2所示，具體優化過程如下。首先直接連接起點A和終點B，再判斷A與B連線是否被障礙物覆蓋，若沒有被障礙物覆蓋，則此次搜索最優路徑就是A與B兩點間的連線；若被障礙物覆蓋，則按照節點順序找到B的父節點X₆，判斷A與X₆連線之間是否被障礙物覆蓋，若A與X₆之間沒有被障礙物覆蓋，則直接把X₆當作A的子節點，依次連接A-X₆-B，此路徑作為搜索的最優路徑；若A與X₆連線被障礙物覆蓋，則繼續尋找X₆的父節點，即圖中的X₅，判斷起點A與X₅連線是否被障礙物覆蓋；按照上述方法一直進行下去，優化后得到圖中X₂作為A的子節點，然后再把子節點X₂當作下一步算法改進的起點，不斷地重復以上步驟，當整個路徑優化完成時，得到如圖2所示的最優路徑，即算法結束。

2.2三次樣條曲線插值

RRT算法規劃出的路徑往往不光滑，機器人在節點處需要先減速，然后再加速前行，增加了機器人的能量消耗和工作時間。為了讓路徑更加光滑，使機器人平穩移動，用三次樣條曲線插值優化反向尋優改進算法。三次樣條曲線是一種常用的插值平滑算法，通過一系列的控制點得到一條連續平滑的曲線。

2.2.1 數學基本原理

三次樣條曲線插值是將原始長序列分成若干段，每一段都是一個三次函數，在分段處左右兩邊函數值相等，一階導數和二階導數都連續^[15]。

假設有n+1個節點，把n+1個節點劃分成以下n個區間：

每個分段區間的三次多項式構造形式如下：

所以，S（x）表達式是一個分段函數：

分段函數滿足以下4個條件。

條件1：函數穿過所有已知節點，可得n+1個方程。

條件2：節點處0階連續，即前一段方程在節點處的函數值和后一段方程在相同節點處的函數值都相等，可得n-1個方程。

條件3：在所有節點（除了第1個節點和最后的節點）處1階連續，保證節點處有相同的斜率，原函數曲線上沒有劇烈的跳變，可得n-1個方程。

條件4：在所有節點（除了第1個節點和最后的節點）處2階連續，保證節點處有相同的曲率，意味著每個點的曲率半徑有定義，共n-1個方程。

2.2.2 端點條件

1）自由邊界

首尾兩端沒有任何讓曲線彎曲的力，則要求解線性方程組：

2）固定邊界

在端點有固定的斜率，則要求解線性方程組：

3）非節點邊界

指定樣條曲線的三次微分相等，則要求解線性方程組：

3基于MATLAB仿真分析

將原始的RRT算法、反向尋優改進的RRT算法、三次樣條曲線插值（自由邊界）優化后的RRT算法在MATLAB（R2018a）中仿真分析。實驗地圖為二維場景圖，仿真分為無障礙物、單個障礙物、復雜環境、狹窄通道4個不同的場景，這樣可以使仿真結果更具有可比性，更能驗證算法的有效性。起點坐標（0，0），終點坐標（750，750），搜索步長80。在下面的路徑規劃圖中，白色區域表示算法可覆蓋的區域。

3.1無障礙物情況

在無障礙情況下，3種算法對應的結果如圖3所示。沒有障礙物時，在安全區域中，隨機擴展樹需要擴展大量的節點數，迅速規劃出了一條可行的路徑，但路徑長度不是最優，算法的收斂速度較緩慢，節點較多；反向尋優改進的RRT算法大大減少了隨機擴展樹節點的數量，找到了一條最短路徑，即起點與終點間的連線段，實驗結果表明，改進的RRT算法路徑質量顯著提高；在反向尋優的基礎上，三次樣條曲線插值優化的RRT算法路徑長度變化不大，但光滑性變好。

3.2單個障礙物情況

在單個障礙情況下，3種算法對應的結果如圖4所示。由圖可知，當環境地圖中存在障礙物時，原始RRT算法能很快規劃出一條路徑，但是路徑節點較多，光滑性很差，路徑長度不是最優；反向尋優改進的RRT算法大大減少了節點數量，路徑長度縮短，路徑質量顯著提高；在反向尋優的基礎上，三次樣條曲線插值優化后的RRT算法路徑長度相差不大，但是節點數目進一步減少，光滑性變好。

3.3復雜環境情況

在復雜環境情況下，即存在很多障礙物，3種算法對應的結果如圖5所示。由圖可知，當障礙物很多時，原始RRT算法搜索時間較長，規劃出的路徑節點很多，光滑性很差，路徑長度不是最優；反向尋優改進的RRT算法搜索時間沒有太大變化，但是路徑節點數量明顯減少，路徑長度縮短，顯著提高了路徑的質量；在反向尋優的基礎上，三次樣條曲線插值優化后的RRT算法路徑長度相差不大，但是節點數目進一步減少，光滑性變好。

3.4狹窄通道情況

在狹窄通道情況下，即可搜索區域和障礙物之間構成很多的狹窄通道，3種算法對應結果的如圖6所示。3種算法規劃出的路徑中，由于通道狹窄，擴展樹會在障礙物和可搜索區域之間來回不停地碰撞，導致搜索時間較長。RRT算法擴展節點太多，光滑性非常差，路徑并非最優；而反向尋優改進后的RRT算法由于改變了父節點的選取，規劃出的路徑長度降低，節點數目減少，光滑性相對變好；在反向尋優的基礎上，三次樣條曲線插值優化的RRT算法節點數目進一步減少，光滑性變好，且路徑長度相差甚小。

為了便于進一步和原始RRT算法的相關數據進行比較，驗證反向尋優改進和三次樣條曲線插值優化后的算法的有效性和準確性，在仿真條件下對以上4個不同的場景進行了200次路徑規劃，計算對應算法的平均路徑長度和平均節點數目，結果如表3和4所示。

計算結果顯示原始RRT算法路徑長度較大，且節點數多；反向尋優改進后的算法路徑長度明顯減小，節點數急劇減少，說明光滑性變好，算法改進效果顯著；三次樣條曲線插值在反向尋優的基礎上進行優化后，雖然路徑長度略微增加，但是節點數歸零，光滑性非常好。

結合以上改進算法驗證結果分析，可得以下結論：移動機器人在路徑規劃中如果更偏重于路徑長度的要求，可以選擇反向尋優改進后的算法；如果更偏重于光滑性的要求，使移動機器人行走更加平穩，可以選擇三次樣條曲線插值優化后的算法。

4基于ROS系統的仿真驗證

為進一步驗證改進算法的有效性，以餐廳環境為例在ROS平臺進行仿真，使用gmapping實現機器人的SLAM建圖，如圖7所示。

啟動相應的launch文件，啟動gazebo并加載機器人URDF模型的餐廳環境，啟動rviz并加載了gmapping建立的地圖^[17-18]，如圖8和9所示。

根據現實餐廳障礙物情況，分為空餐廳情況、障礙物較少、障礙物很多3個不同的場景進行仿真，設置相同的起點和終點，并對它們的路徑長度和運行時間進行對比分析。

4.1空餐廳環境

餐廳內部全空時，機器人規劃出的從當前位置到目標位置的有效路徑如圖10所示。從圖中可以看出，機器人在運動過程中的路徑長度較短且光滑。

4.2障礙物較少

在障礙物較少的情況下，機器人迅速規劃出一條路徑，如圖11所示。與空餐廳場景相比，路徑仍然保持光滑，路徑長度幾乎不變，這是由于障礙很少，對路徑長度沒有影響。

4.3障礙物很多

在障礙物很多的情況下，機器人規劃出的路徑如圖12所示。此路徑并非最優，相對于障礙物較少的情況路徑長度明顯增加，其原因在于障礙物太多，機器人需要不停地避開障礙物，因此路徑長度增加。

為了更好地對比以上3種場景，統計路徑長度和運行時間，如表5所示。

分析以上圖可知，障礙物較少的情況相對于空餐廳情況，路徑長度和運行時間略微增加，總體變化不大；在障礙物很多的情況下，路徑長度和運行時間明顯增加，原因在于機器人為躲避較多的障礙物，規劃的路徑并非最優，導致路徑長度和運行時間明顯增加。

5結??論

1）針對RRT算法存在的缺點提出了反向尋優和三次樣條曲線插值改進方法。

2）將改進的RRT算法在MATLAB中不同的場景下進行多次仿真，結果表明，改進后的算法可以縮短路徑長度，減少節點數目，提高光滑性，驗證了改進算法的有效性。

3）以餐廳環境為例在ROS系統中進一步仿真，結果表明，機器人能實現路徑規劃功能，驗證了改進算法的有效性，后續重點研究工作將搭建實物機器人驗證，未來有望應用到送餐機器人中，提高工作效率，降低人工成本，產生經濟效益。

參考文獻

［1］??金碚. 中國制造2025[M]. 北京： 中信出版集團， 2015.

Jin B. Made in China 2025[M]. Beijing： Citic Publishing Group， 2015. （in Chinese）

［2］??王淘淘. 中國機器人產業發展報告： 我國機器人市場進入高速增長期[J]. 今日制造與升級， 2018（8）： 22.

Wang T T. China robot industry development report： Chinas robot market has entered a period of rapid growth[J]. Manufacture & Upgrading Today， 2018（8）： 22. （in Chinese）

［3］??中國電子學會. 機器人簡史[M]. 北京： 電子工業出版社， 2015.

Chinese Society of Electronics. A brief history of robot Brief history of robot[M]. Beijing： Publishing House of Electronics Industry， 2015. （in Chinese）

［4］??Yang L， Qi J T， Xiao J Z， et al. A literature review of UAV 3D path planning[C]//Proceeding of the 11th World Congress on Intelligent Control and Automation， June 29 - July 4， 2014， Shenyang. IEEE， 2014： 2376-2381.

［5］??Karasev V， Ayvaci A， Heisele B， et al. Intent-aware long-term prediction of pedestrian motion[C]//2016 IEEE International Conference on Robotics and Automation （ICRA）， May 16-21， 2016， Stockholm， Sweden. IEEE， 2016： 2543-2549.

［6］??Bakdi A， Hentout A， Boutami H， et al. Optimal path planning and execution for mobile robots using genetic algorithm and adaptive fuzzy-logic control[J]. Robotics and Autonomous Systems， 2017， 89： 95-109.

［7］??康亮， 趙春霞， 郭劍輝. 未知環境下改進的基于RRT算法的移動機器人路徑規劃[J]. 模式識別與人工智能， 2009， 22（3）： 337-343.

Kang L， Zhao C X， Guo J H. Improved path planning based on rapidly-exploring random tree for mobile robot in unknown environment[J]. Pattern Recognition and Artificial Intelligence， 2009， 22（3）： 337-343. （in Chinese）

［8］??賈李紅. 基于GPS的雙向搜索路徑的研究[D]. 安徽淮南： 安徽理工大學， 2017.

Jia L H. Research on two-way search path based on GPS[D]. Huainan， Anhui： Anhui University of Science & Technology， 2017. （in Chinese）

［9］??孫欽鵬， 李猛， 王中華. 基于改進快速擴展隨機樹算法的移動機器人路徑規劃[J]. 濟南大學學報（自然科學版）， 2019， 33（5）： 431-438.

Sun Q P， Li M， Wang Z H. Mobile robot path planning based on improved rapidly-exploring random tree algorithm[J]. Journal of University of Jinan （Science and Technology）， 2019， 33（5）： 431-438. （in Chinese）

［10］??Lavalle S M. Rapidly-exploring random trees： a new tool for path planning[R]. Ames， Iowa： Iowa State University， 1998.

［11］??Chen W B， Wu X B， Lu Y. An improved path planning method based on artificial potential field for a mobile robot[J]. Cybernetics and Information Technologies， 2015， 15（2）： 181-191.

［12］??Zhang C. Path planning for robot based on chaotic artificial potential field method[J]. IOP Conference Series： Materials Science and Engineering， 2018， 317： 012056.

［13］??潘思宇， 徐向榮. 基于改進RRT^*的移動機器人運動規劃算法[J]. 山西大學學報（自然科學版）， 2017， 40（2）： 244-254.

Pan S Y， Xu X R. Mobile robot motion planning algorithm based on improved RRT^*[J]. Journal of Shanxi University：（Natural Science Edition）， 2017， 40（2）： 244-254. （in Chinese）

［14］??李金良， 舒翰儒， 劉德建， 等. 基于改進RRT路徑規劃算法[J]. 組合機床與自動化加工技術， 2021（2）： 22-24，29.

Li J L， Shu H R， Liu D J， et al. Path planning algorithm based on improved RRT[J]. Modular Machine Tool & Automatic Manufacturing Technique， 2021（2）： 22-24，29. （in Chinese）

［15］??Saranrittichai P， Niparnan N， Sudsang A. Robust local obstacle avoidance for mobile robot based on dynamic window approach[C]//2013 10th International Conference on Electrical Engineering/Electronics， Computer， Telecommunications and Information Technology， May 15-17， 2013， Krabi， Thailand. IEEE， 2013： 1-4.

［16］??Choi B， Kim B， Kim E， et al. A modified dynamic window approach in crowded indoor environment for intelligent transport robot[C]//2012 12th International Conference on Control， Automation and Systems， October 17-21， 2012， Jeju， South Korea. IEEE， 2012： 1007-1009.

［17］??于清曉. 輪式餐廳服務機器人移動定位技術研究[D].上海：上海交通大學， 2013.

Yu Q X. Research on mobile localization techniques for wheeled restaurant service robots[D]. Shanghai： Shanghai Jiaotong University， 2013. （in Chinese）

［18］??胡春旭. ROS機器人開發實踐[M]. 北京： 機械工業出版社， 2018.

Hu C X. ROS robot development practice[M]. Beijing： China Machine Press， 2018. （in Chinese）

（編輯??羅敏）