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

?仿真技術?

基于MI-RRT*算法的路徑規(guī)劃研究*

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

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

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

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

0　引言

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

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

1　基于MI-RRT*路徑規(guī)劃算法設計

1.1　Informed-RRT*原理

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

圖1 　采樣狀態(tài)子集空間

1.2　主要設計思路

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

1.3　貪心采樣與自適應步長

1.3.1貪心采樣

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

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

1.3.2自適應步長

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

Fig. 3 Tree growth near obstacles

圖4 　遠離障礙物樹生長

1.4　最小化Snap曲線優(yōu)化

1.4.1最小化Snap軌跡生成

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

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

1.4.2通道約束

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

為了實現(xiàn)通道約束需要構建2個不等式約束：

1.4.3時間分配

1.5　算法設計與步驟

圖5 　算法流程圖

算法1：

下面介紹各個函數(shù)功能。

2　仿真校驗

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

表1 　地圖數(shù)據(jù)

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

圖6 　Map1中算法運行結果

圖7 　Map2中算法運行結果

圖8 　Map3中算法運行結果

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

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

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

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

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

表2 　算法初次運行得出可行解數(shù)據(jù)

表3 　Map1～Map3運行結果數(shù)據(jù)

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

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

3　結束語

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

［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］ 歐陽志宏，郭強. 改進蟻群算法的無人機突防航路規(guī)劃［J］. 現(xiàn)代防御技術，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］ 趙玉新，劉利強，李剛. 無人智能運載器航路規(guī)劃方法及其應用［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路徑規(guī)劃研究［J］. 現(xiàn)代制造工程，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］ 霍鳳財，遲金，黃梓健，等.移動機器人路徑規(guī)劃算法綜述［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*算法的路徑規(guī)劃研究［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*算法的機器人路徑規(guī)劃［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］ 朱奕航. 四旋翼無人機路徑規(guī)劃及跟蹤控制技術［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*的無人機航跡規(guī)劃算法研究［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*的聯(lián)合軌跡優(yōu)化［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*算法的路徑規(guī)劃研究[J].現(xiàn)代防御技術,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-)，男，遼寧省東港人。講師，博士，研究方向為陀螺儀及導航技術、航行器路徑規(guī)劃、磁探測等方面。

266000 山東省青島市黃島區(qū)哈爾濱工程大學青島創(chuàng)新發(fā)展基地 E-mail：yuqiang@hrbeu.edu.cn