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

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

收稿日期：2021-10-14

網(wǎng)絡(luò)出版日期：2022-03-02

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

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

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

關(guān)鍵詞：RRT算法；路徑規(guī)劃；反向?qū)?yōu)；三次樣條曲線插值；ROS系統(tǒng)

中圖分類號：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世紀后，隨著前沿科學(xué)技術(shù)的發(fā)展，機器人技術(shù)被不少國家列為重點發(fā)展的一項高新技術(shù)，且相關(guān)產(chǎn)業(yè)被用作衡量綜合國力強弱的標志^[1]?，F(xiàn)實生活中廣泛為人們服務(wù)的機器人，其相關(guān)的路徑規(guī)劃問題面臨著前所未有的機遇，同時也充滿著挑戰(zhàn)，使其成為機器人的核心技術(shù)之一^[2-3]。路徑規(guī)劃算法的好壞，直接決定事先設(shè)定的任務(wù)能否圓滿完成并達到理想的效果。優(yōu)秀的路徑規(guī)劃技術(shù)不僅能減少資金投入，降低運行時間，而且還能提高機器人的搜索效率，為機器人導(dǎo)航的研究奠定良好的基礎(chǔ)^[4-6]。

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

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

1RRT算法簡介

1.1RRT算法基本原理

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

1.2RRT算法缺點

1）穩(wěn)定性差。由于擴展樹節(jié)點的生長方向是隨機的，所以在相同的任務(wù)下重復(fù)規(guī)劃，也會出現(xiàn)不同的路徑，導(dǎo)致每次規(guī)劃的結(jié)果都不同，甚至偏離最優(yōu)路徑。

2）隨機性強。因為擴展樹是在整個區(qū)域內(nèi)隨機搜索，不受目標點的引導(dǎo)和啟發(fā)，因此，存在大量無效的擴展節(jié)點，使算法具有很強的隨機性。

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

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

1.3RRT算法改進思路

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

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

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

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

在上述改進思路的啟發(fā)下，在節(jié)點選取方面本研究中提出了反向?qū)?yōu)改進RRT算法，在光滑性方面用三次樣條曲線插值優(yōu)化。

2RRT算法改進

2.1反向?qū)?yōu)

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

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

2.2三次樣條曲線插值

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

2.2.1 數(shù)學(xué)基本原理

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

假設(shè)有n+1個節(jié)點，把n+1個節(jié)點劃分成以下n個區(qū)間：

每個分段區(qū)間的三次多項式構(gòu)造形式如下：

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

分段函數(shù)滿足以下4個條件。

條件1：函數(shù)穿過所有已知節(jié)點，可得n+1個方程。

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

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

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

2.2.2 端點條件

1）自由邊界

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

2）固定邊界

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

3）非節(jié)點邊界

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

3基于MATLAB仿真分析

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

3.1無障礙物情況

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

3.2單個障礙物情況

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

3.3復(fù)雜環(huán)境情況

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

3.4狹窄通道情況

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

為了便于進一步和原始RRT算法的相關(guān)數(shù)據(jù)進行比較，驗證反向?qū)?yōu)改進和三次樣條曲線插值優(yōu)化后的算法的有效性和準確性，在仿真條件下對以上4個不同的場景進行了200次路徑規(guī)劃，計算對應(yīng)算法的平均路徑長度和平均節(jié)點數(shù)目，結(jié)果如表3和4所示。

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

結(jié)合以上改進算法驗證結(jié)果分析，可得以下結(jié)論：移動機器人在路徑規(guī)劃中如果更偏重于路徑長度的要求，可以選擇反向?qū)?yōu)改進后的算法；如果更偏重于光滑性的要求，使移動機器人行走更加平穩(wěn)，可以選擇三次樣條曲線插值優(yōu)化后的算法。

4基于ROS系統(tǒng)的仿真驗證

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

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

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

4.1空餐廳環(huán)境

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

4.2障礙物較少

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

4.3障礙物很多

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

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

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

5結(jié)??論

1）針對RRT算法存在的缺點提出了反向?qū)?yōu)和三次樣條曲線插值改進方法。

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

3）以餐廳環(huán)境為例在ROS系統(tǒng)中進一步仿真，結(jié)果表明，機器人能實現(xiàn)路徑規(guī)劃功能，驗證了改進算法的有效性，后續(xù)重點研究工作將搭建實物機器人驗證，未來有望應(yīng)用到送餐機器人中，提高工作效率，降低人工成本，產(chǎn)生經(jīng)濟效益。

參考文獻

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

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

［2］??王淘淘. 中國機器人產(chǎn)業(yè)發(fā)展報告： 我國機器人市場進入高速增長期[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］??中國電子學(xué)會. 機器人簡史[M]. 北京： 電子工業(yè)出版社， 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］??康亮， 趙春霞， 郭劍輝. 未知環(huán)境下改進的基于RRT算法的移動機器人路徑規(guī)劃[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]. 安徽淮南： 安徽理工大學(xué)， 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］??孫欽鵬， 李猛， 王中華. 基于改進快速擴展隨機樹算法的移動機器人路徑規(guī)劃[J]. 濟南大學(xué)學(xué)報（自然科學(xué)版）， 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^*的移動機器人運動規(guī)劃算法[J]. 山西大學(xué)學(xué)報（自然科學(xué)版）， 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路徑規(guī)劃算法[J]. 組合機床與自動化加工技術(shù)， 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］??于清曉. 輪式餐廳服務(wù)機器人移動定位技術(shù)研究[D].上海：上海交通大學(xué)， 2013.

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

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

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

（編輯??羅敏）