基于MI-RRT*算法的路徑規劃研究*

于強，彭昭鴻，黎旦，李利彬，高藝成

?仿真技術?

基于MI-RRT*算法的路徑規劃研究*

于強1，2，彭昭鴻2，黎旦1，李利彬1，高藝成1

（1.哈爾濱工程大學 智能科學與工程學院，黑龍江 哈爾濱 150000； 2.青島哈爾濱工程大學創新發展中心，山東 青島 266000）

針對Informed-RRT(rapidly-exploring random tree)*算法收斂速度慢、優化效率低和生成路徑無法滿足實際需求等問題，開展了基于MI-RRT* (Modified Informed-RRT*)算法的路徑規劃研究，通過引入貪心采樣和自適應步長的方法提高算法的收斂率，減少路徑生成時間、降低內存占用；利用最小化Snap曲線優化的方法使路徑平滑的同時動力也變化平緩，達到節省能量的效果，并提供實際可執行的路徑。最后通過多組不同復雜度的實驗環境表明，較Informed-RRT*算法MI-RRT*算法穩定性更高、所得規劃路徑平滑可執行，并且能夠減少20%的迭代次數和25% 的搜索時間，得出在開闊以及密集環境中MI-RRT*算法較Informed-RRT*和RRT*算法有明顯的優勢。

Informed-RRT*算法；貪心采樣；自適應步長；MI-RRT*；最小化Snap曲線優化；RRT*算法

0　引言

近年來，隨著科技快速發展和機器人技術水平的不斷提高，移動機器人、無人飛行器在代替人從事消防救援、莊稼灌溉、餐飲配送、物流分揀等領域得到廣泛運用。路徑規劃作為移動機器人、無人飛行器的一種核心技術，其基本思想是：在給定的初始狀態和目標狀態下，找出一個路徑可行解使得機器人在與障礙物無碰撞的前提條件下從初始狀態到達目標狀態。路徑規劃算法有很多，常用的局部路徑規劃算法有人工勢場法［1］、蟻群算法［2-3］、A*算法［4］、遺傳算法［5］、粒子群算法［6］等。

在全局路徑規劃采樣算法中，快速隨機搜索樹算法（rapidly-exploring random tree，RRT）對于解決路徑規劃問題，有著規劃速度快、環境建模要求低、動態環境適應性強等優點，其缺點是搜索時盲目性大、耗時長、計算復雜度高、易陷入死區和存在局部最小值問題［7］。近年來針對RRT算法的改進受到了國內外學者的廣泛研究，2011年Karaman Sertac等針對 RRT算法生成路徑質量不高、非最優等問題，提出了RRT*算法［8］。RRT*算法繼承RRT概率完備性的同時也保證了算法的漸近最優性，但是該算法搜索效率較低，進行較大范圍搜索時花費的時間過長；2014年Gammell Jonathan D等針對最優RRT*效率低下，與單一查詢性質不一致等問題，提出了Informed-RRT*算法［9］，該方法保持了與RRT*相同的完整性和最優性的概率，并且路徑規劃能力顯著優于RRT*算法；2019年Ryu Hyejeong等針對Informed-RRT*算法在復雜環境中減少計算時間，提出了一種網格圖骨架化方法用于生成初始解決方案的改進Informed-RRT*算法［10］，這種方法可以有效獲得初始解決方案并提高算法收斂率；2020年張玉偉等針對Informed-RRT*算法在復雜環境重復規劃穩定性差、收斂速度慢的問題，提出IBI-RRT*（informed bi-directional RRT*）的路徑規劃算法［11］，該算法結合樹的雙向搜索策略加快對狀態子集的探索，提高算法的收斂速度；2022年DAI Jun等針對Informed-RRT*在機器人自主導航中生成路徑質量差、導航效率低的問題提出運用貪婪算法以及潛在父節點重構的方法實現對Informed-RRT*算法的優化，并運用動態窗口法對改進算法在ROS（robot operating system）平臺上進行了仿真實驗［12］。針對Informed-RRT* 開闊環境收斂率低、在障礙物附近地生長不具有靈活性，生成的路徑沒有考慮動力平緩變化且不夠平滑等問題，本文提出了MI-RRT*（modified informed- RRT*）算法，通過引入貪心采樣的方法提高算法在不同環境下的收斂率，減少了通過復雜采樣所增加的時間；運用自適應步長的方法對障礙物周圍的采樣點進行精確采樣，減少采樣點以降低內存占用；以及利用最小化Snap曲線優化的方法所得路徑平滑的同時動力變化平緩，達到節省能量的效果。

1　基于MI-RRT*路徑規劃算法設計

1.1　Informed-RRT*原理

Informed-RRT*算法有效地解決了RRT*效率低下、單一查詢性質不一致等問題，并保留了RRT*的概率完整性和最優性。無論配置空間的維度如何，Informed-RRT*算法都能夠在有限時間內找到近似最優解的可行路徑，路徑規劃能力優于RRT*算法。

圖1 　采樣狀態子集空間

1.2　主要設計思路

Informed-RRT*相較于RRT*算法提高了算法的收斂率、解決了單一查詢性質不一致的問題，但仍然有缺陷需要改善：①Informed-RRT*的改進方式是通過縮小采樣范圍，不斷采樣使得路徑逐漸優化，因此在一些簡單環境，或者目標點周圍沒有障礙物時仍然需要大量采樣才能找到最優路徑；②Informed-RRT*與RRT算法一樣是以固定步長作為生長樹進行隨機搜索，當步長過大時，在進行障礙物附近的點進行搜索時隨機點被舍棄的概率增大，步長過小會導致生長樹不能對空間進行高效率的搜索，開闊空曠環境搜索速率降低；③算法生成的路徑是通過路徑點直接相連，生成的路徑不夠平滑，這使得在實際情況中很難得到應用。針對Informed-RRT*算法的這些問題，本文提出了MI-RRT（informed bi-directional RRT*）*算法，通過引入貪心采樣［13-14］、自適應步長［15］以及最小化Snap曲線優化［16］等方法對Informed-RRT*算法進行改進優化。

1.3　貪心采樣與自適應步長

1.3.1貪心采樣

貪心采樣是一種解決在某些空曠的環境地圖中求最優解問題更簡單、更迅速的設計技術［13］。該方法的實現原理是以一定幾率對目標點進行采樣，判斷當前節點與目標點之間是否有障礙物，若有則放棄此次采樣進行普通采樣，若無障礙物則將終點直接作為采樣對象，生成可執行的路徑。通過這種采樣的方式，在目標點與當前點直接可達時，直接將目標點選為下一次采樣點，縮短了到達目標點所消耗的大量時間。

圖2 　貪心算法改進前后示意圖

1.3.2自適應步長

圖3 　貪心算法改進前后示意圖

Fig. 3 Tree growth near obstacles

圖4 　遠離障礙物樹生長

1.4　最小化Snap曲線優化

1.4.1最小化Snap軌跡生成

Informed-RRT*算法對最后生成的最優路徑的軌跡點進行直線連接，沒有考慮實際運動物體的動力學模型，這使得搜索得到的路徑粗糙，并不符合移動機器人、無人飛行器實際執行情況，因此需要將直線平滑處理。為了使生成的路徑能更好地使用曲線擬合的方式進行描述，針對這一情況，MI-RRT*算法使用最小化Snap［16］曲線優化的方法對最后的軌跡點進行了曲線擬合處理。Snap是加速度的二階導數，將其作為被控量處理相當于最小化角加速度即動力的微分［17］。這種方法能將原直線變得平滑，同時也能使動力變化盡可能地平緩，以達到節省能量的效果。

多項式優化中的約束條件被施加在每個路徑段的端點上。這些端點在空間中已知位置、速度、加速度。根據相鄰點之間的位置、速度、加速度以及相鄰點之間需要連續可以構成一個等式約束，合并所有等式約束可變成聯合優化問題的一組線性等式約束：

1.4.2通道約束

由于最小化Snap生成的曲線存在凸包現象，為了限制曲線路徑和直線的偏離，可以添加通道約束來限制一段路徑上的所有控制點都在相應的多維數據集中，由于本次實驗仿真在二維環境下進行，因此多維數據集采用控制點向外擴展形成的方形集，當所有控制點都生成一個方形集時，此時曲線產生的凸包一定在所生成的方形集內，這就意味著整個軌跡段被限制在方形集內。通道約束限制方法：

為了實現通道約束需要構建2個不等式約束：

1.4.3時間分配

1.5　算法設計與步驟

圖5 　算法流程圖

算法1：

下面介紹各個函數功能。

2　仿真校驗

為了驗證MI-RRT*算法的有效性，本實驗采用PyCharm Community Edition 2021.2.1環境下進行路徑規劃仿真校驗，二維實驗環境為800×600單位大小的柵格空間地圖，通過改變環境中障礙物數量和大小，建立3幅不同的環境地圖，Map1為簡單地圖、Map2為稀疏開闊地圖、Map3為密集復雜地圖分別對算法可靠性進行驗證，地圖相關數據如表1所示。

表1 　地圖數據

本節通過對比分析RRT*算法、Informed RRT*算法和MI-RRT*算法在3種不同地圖環境下運行得出仿真結果。圖6～8均是迭代5 000次所得到的結果，圖中a）為Informed-RRT*算法在地圖中運行得出的路徑圖，b）為MI-RRT*算法在地圖中運行得出的路徑圖。

圖6 　Map1中算法運行結果

圖7 　Map2中算法運行結果

圖8 　Map3中算法運行結果

對比Map1仿真結果可以看出，Informed-RRT*算法在目標點周圍沒有障礙物的情況下仍然需要大量的迭代才能得出路徑，但MI-RRT*算法在越過障礙物對目標點直接可達時能快速得出路徑，因此減少了大量迭代次數，提高了算法收斂率。

在簡單開闊地圖Map2中，Informed-RRT*算法對開闊環境進行采樣時由于步長固定，對采樣的次數多，迭代次數隨之增加，MI-RRT*在開闊環境進行采樣隨著距離障礙物的遠近選取合適的采樣步長，在較少的采樣次數下就能到達目標點。

Map3中可以看出，Informed-RRT*算法在采用與圖6，7相同步長的情況下仿真出的結果離最優結果相差較大，MI-RRT*算法在采用自適應步長情況下能對狹小空間障礙物附近的點進行采集從而得出趨于最優的路徑。對比可以看出，MI-RRT*得出的路徑較平滑，具有可執行性，并且由于對Snap優化達到了節省能量的效果。

表2所列為RRT*，Informed RRT*，MI-RRT*在地圖Map1，Map2，Map3初次運行得出可行解的具體數據，通過實驗數據可以觀測出，MI-RRT*相較其他2種算法，在相同地圖環境下在初次得到可行解的迭代次數、時間以及路徑長度等方面都有性能的提升；同時隨著地圖環境復雜度的增加，MI-RRT*所消耗時間、迭代次數同樣增加，但仍然低于RRT*，Informed RRT*算法消耗時間、迭代次數，并且得出路徑長度也優于其他2種算法。表3數據為不同算法在迭代100～5 000次所得路徑長度數據圖，從不同地圖中可以看出，MI-RRT*通過較少的迭代次數就得到趨于最優路徑的解，隨著迭代次數的增加在前一路徑的基礎上路徑逐漸優化。

圖9為RRT*，Informed RRT*，MI-RRT*3種算法在地圖Map3收斂情況對比圖，從圖中可以看出，路徑長度隨時間變化的趨勢圖，MI-RRT*與前2種算法相比收斂速度快、所得到的路徑短。

表2 　算法初次運行得出可行解數據

表3 　Map1～Map3運行結果數據

圖9 　3種算法的收斂情況對比圖

綜上所述，通過生成路徑具體情況，迭代次數，收斂時間等不同方面進行了比較，可以看出3種算法收斂率大小關系為MI-RRT*算法>Informed-RRT*算法>RRT*算法。

3　結束語

本文在Informed-RRT*算法基礎上進行改進，提出了一種MI-RRT*算法。通過貪心采樣的方法提高了算法的收斂率，減少了復雜采樣所增加的時間；采用自適應步長的方法，對障礙物附近的點進行采樣，減少采樣點降低對采樣點存儲而占用的內存空間；利用最小化Snap曲線優化的方法使直線平滑的同時達到節省能量的效果。與Informed-RRT*算法相比，MI-RRT*算法穩定性更高，迭代次數方面降低20%、搜索時間方面縮短25%。MI-RRT*能解決在不同環境下的路徑規劃，但現如今任何一個單獨的算法都不足以解決實際問題中的所有路徑規劃問題，未來可以對多種路徑規劃算法進行融合達到算法優勢互補的目的，以及將路徑規劃算法擴展到三維空間進行研究。

［1］ KUMAR P B， RAWAT H， PARHI D R. Path Planning of Humanoids Based on Artificial Potential Field Method in Unknown Environments［J］. Expert Systems： The International Journal of Knowledge Engineering， 2019，36（1）：1-12.

［2］ 歐陽志宏，郭強. 改進蟻群算法的無人機突防航路規劃［J］. 現代防御技術，2018， 46（1）：74-78，114.

OUYANG Zhihong， GUO Qiang.Improved Ant Colony Algorithm for UAV Penetration Route Planning ［J］. Modern Defence Technology， 2018， 46（1）：74-78， 114.

［3］ 趙玉新，劉利強，李剛. 無人智能運載器航路規劃方法及其應用［M］. 北京：科學出版社，2014.

ZHAO Yuxing，LIU Liqiang，LI Gang. Route Planning Method of Unmanned Intelligent Vehicle and Its Application ［M］. Beijing： Science Press，2014.

［4］ 孔慧芳，盛陽. 基于改進A算法的AGV路徑規劃研究［J］. 現代制造工程，2021 （10）：60-64.

KONG Huifang，SHENG Yang.Research on AGV Path Planning Based on Improved Aalgorithm ［J］. Modern Manufacturing Engineering， 2021 （10）：60-64.

［5］ LAMINI C， BENHLIMA S， ELBEKRI A. Genetic Algorithm Based Approach for Autonomous Mobile Robot Path Planning［J］.Procedia Computer Science， 2018：180-189.

［6］ SONG Baoye， WANG Zidong， ZOU Lei. On Global Smooth Path Planning for Mobile Robots Using a Novel Multimodal Delayed PSO Algorithm ［J］. Cognitive Computation， 2017（1）：5-17.

［7］ 霍鳳財，遲金，黃梓健，等.移動機器人路徑規劃算法綜述［J］. 吉林大學學報（信息科學版），2018 （6）：639-647.

HUO Fengcai，CHI Jin， HUANG Zijian， et al. A Survey of Path Planning Algorithms for Mobile Robots ［J］.Journal of Jilin University （Information Science Edition），2018（6）： 639-647.

［8］ KARAMAN S， FRAZZOLI E. Sampling-Based Algorithms for Optimal Motion Planning ［J］. International Journal of Robotics Research， 2011（7）：846-894.

［9］ GAMMELL J D， SRINIVASA S S， BARFOOT T D. Informed RRT*： Optimal Sampling-Based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal Heuristic ［C］∥IEEE/RSJ International Conference on Intelligent Robots and Systems， 2014：2997-3004.

［10］ RYU H， PARK Y. Improved Informed RRT* Using Gridmap Skeletonization for Mobile Robot Path Planning ［J］. International Journal of Precision Engineering and Manufacturing， 2019（11）：2033-2039.

［11］ 張玉偉，左云波，吳國新，等. 基于改進Informed-RRT*算法的路徑規劃研究［J］. 組合機床與自動化加工技術，2020 （7）：21-25.

ZHANG Yuwei，ZUO Yunbo， WU Guoxing， et al. Research on Path Planning Based on Improved Informed-RRT* Algorithm ［J］.Combined Machine Tools and Automated Processing Technology，2020 （7）：21-25.

［12］ DAI Jun， LI Dongfang， ZHAO Junwei， et al. Autonomous Navigation of Robots Based on the Improved Informed-RRT* Algorithm and DWA ［J］. Journal of Robotics， 2022，2022：1-9.

［13］ 代軍，李志明，李艷琴，等. 基于改進Informed-RRT*算法的機器人路徑規劃［J］. 河南理工大學學報（自然科學版），2021（1）：1-11.

DAI Jun，LI Zhiming，LI Yanqin， et al. Robot Path Planning Based on Improved Informed-RRT* Algorithm ［J］.Journal of Henan University of Technology（Natural Science Edition）， 2021（1）：1-11.

［14］ 朱奕航. 四旋翼無人機路徑規劃及跟蹤控制技術［D］.南京：南京航空航天大學，2020.

ZHU Yihang. Four-Rotor UAV Path Planning and Tracking Control Technology ［D］. Nanjing： Nanjing University of Aeronautics and Astronautics，2020.

［15］ 陳晉音，施晉，杜文耀，等. 基于MB-RRT*的無人機航跡規劃算法研究［J］. 計算機科學，2017 （8）：198-206.

CHENG Jinyin，SHI jin， DU Wenyao， et al. Research on UAV Track Planning Algorithm Based on MB-RRT* ［J］. Computer Science， 2017 （8）：198-206.

［16］ MELLINGER D， KUMAR V. Minimum Snap Trajectory Generation and Control for Quadrotors ［C］∥2011 IEEE International Conference on Robotics and Automation. Shanghai，China： IEEE International Conference on Robotics and Automation （ICRA 2011），2011： 2520-2525.

［17］ 韓曉微，劉宏宇，石澤亮，等. 移動機器人最小化snap與A*的聯合軌跡優化［J］. 沈陽大學學報（自然科學版），2021 （4）：321-327.

HAN Xiaowei，LIU Hongyu， SHI Zeliang， et al.Joint Trajectory Optimization of Minimizing Snap and A* for Mobile Robots ［J］. Journal of Shenyang University（Natural Science Edition）， 2021 （4）：321-327.

［18］ RICHTER C， BRY A， ROY N. Polynomial Trajectory Planning for Aggressive Quadrotor Flight in Dense Indoor Environments［C］∥Proceedings of the 16th International Symposium on Robotics Research （ISSR）， 2016： 114， 649-666.

Research of Path Planning Based on MI-RRT*Algorithm

YUQiang1，2，PENGZhaohong2，LIDan1，LI Libin1，GAOYicheng1

（1.College of Intelligent Systems Science and Engineering， Harbin Engineering University， Harbin 266000， China； 2.Qingdao Innovation and Development Center of Harbin Engineering University， Qingdao 266000， China）

Aiming at the problems of the Informed-RRT（rapidly-exploring random tree）* algorithm， such as slow convergence speed， low optimization efficiency and inability to execute the generated path， path planning research based on MI-RRT* （modified informed-RRT*） algorithm is carried out. The method of greedy sampling and adaptive step size is introduced to improve the convergence rate of the algorithm， reduce the path generation time and the memory usage. Minimum Snap curve is used to make the path smooth and the power change smoothly， so as to achieve the effect of saving energy and the path to generate the executable. Several sets of different experimental environments are run to show that the MI-RRT* algorithm is more advantageous than the original algorithm， the resulting planned path is smooth executable and it can reduce the number of iterations by 20% and the search time by 25%. Compared with the open and dense environment， the MI-RRT* algorithm has obvious advantages over the Informed-RRT* ，RRT* algorithms.

Informed-RRT*（rapidly-exploring random tree） algorithm；reedy sampling；adaptive step size；modifiend informed-RRT*（MI-RRT*）；minimum snap；RRT* algorithm

10.3969/j.issn.1009-086x.2023.04.015

TP301.6；TP391.9

A

1009-086X（2023）-04-0116-10

于強, 彭昭鴻, 黎旦, 等.基于MI-RRT*算法的路徑規劃研究[J].現代防御技術,2023,51(4):116-125.

YU Qiang,PENG Zhaohong,LI Dan,et al.Research of Path Planning Based on MI-RRT* Algorithm[J].Modern Defence Technology,2023,51(4):116-125.

2022 -06 -21 ;

2022 -12 -06

于強(1977-)，男，遼寧省東港人。講師，博士，研究方向為陀螺儀及導航技術、航行器路徑規劃、磁探測等方面。

266000 山東省青島市黃島區哈爾濱工程大學青島創新發展基地 E-mail：yuqiang@hrbeu.edu.cn