基于改進雙向快速擴展隨機樹算法的智能汽車路徑規(guī)劃研究

【摘要】針對智能汽車路徑規(guī)劃中雙向快速擴展隨機樹（RRT-Connect）算法獲得的路徑不是最優(yōu)解和狹小通道探索性能較差的問題，在分析RRT-Connect算法基本原理的基礎上，對其在擴展策略和路徑平滑等方面進行了改進。首先，引入概率偏向法對選取的隨機點進行篩選，并基于人工勢場進行擴展，以縮短路徑和計算時間，然后，引入三次B樣條曲線對路徑進行優(yōu)化，生成光滑路徑，保證路徑滿足智能汽車的動力學特性，最后，通過仿真驗證改進RRT-Connect算法的性能，結果表明，在簡單障礙物、復雜障礙物和狹窄路徑環(huán)境下，改進的RRT-Connect算法的平均路徑長度和平均耗時均優(yōu)于傳統(tǒng)RRT-Connect算法。

關鍵詞：路徑規(guī)劃 快速擴展隨機樹 概率偏向法 人工勢場

中圖分類號：TP242；TP18" "文獻標志碼：A" "DOI： 10.20104/j.cnki.1674-6546.20240087

Research on Intelligent Vehicle Path Planning Based on Improved RRT-Connect Algorithm

Zhang Mingyue1，2， Wang Jun1

（1. China University of Mining and Technology， Xuzhou 221008; 2. Qingdao University of Science and Technology， Qingdao 266061）

【Abstract】In addressing the issues of suboptimal solutions and poor exploration performance in narrow passages of intelligent car path planning using the Rapidly-exploring Random Tree-Connect （RRT-Connect） algorithm， this paper improves the RRT-Connect algorithm in expansion strategy and path smoothing based on an analysis of the basic principle of the RRT-Connect algorithm. Firstly， in terms of expansion strategy， a probability bias method is introduced to screen random points， and an expansion method based on artificial potential fields is used to shorten paths and reduce computation time. Secondly， regarding path smoothing， a third-order B-spline curve is introduced to optimize the path and generate a smooth path， ensuring that the path meet the dynamic characteristics of intelligent cars. Finally， the superiority of the improved RRT-Connect algorithm is demonstrated through comparative simulation. The results show that in environments with simple obstacles， complex obstacles and narrow paths， the average time and path length of the improved RRT-Connect algorithm are superior to those of the traditional RRT-Connect algorithm.

Key words： Path planning， Rapidly-exploring Random Tree （RRT）， Probability bias method， Artificial potential fields

【引用格式】 張明月， 王軍. 基于改進雙向快速擴展隨機樹算法的智能汽車路徑規(guī)劃研究[J]. 汽車工程師， 2024（10）： 31-36.

ZHANG M Y， WANG J. Research on Intelligent Vehicle Path Planning Based on Improved RRT-Connect Algorithm[J]. Automotive Engineer， 2024（10）： 31-36.

1 前言

在不同場景下，智能汽車面臨的環(huán)境各不相同。為了應對多樣化的場景應用需求，避免發(fā)生碰撞，許多學者提出了不同的路徑規(guī)劃算法和相應的改進算法。目前，常見的路徑規(guī)劃方法主要有迪杰斯特拉（Dijkstra）算法[1]、A*算法[2]、遺傳算法[3]、蟻群算法[4]、概率路線圖法（Probabilistic Roadmap Method，PRM）[5]、快速擴展隨機樹（Rapidly-exploring Random Tree，RRT）算法[6]等。很多傳統(tǒng)的路徑規(guī)劃算法存在運算時間過長、路徑規(guī)劃較為復雜等問題。

其中，RRT算法[6-7]由于采樣邏輯簡單、易于使用等原因，在機器人領域得到了廣泛應用，但該算法存在規(guī)劃路線過于復雜、路徑過長等弊端。為解決RRT算法在狹窄路段效率不佳的問題，Kuffner提出了雙向快速擴展隨機樹（Rapidly-exploring Random Tree-Connect，RRT-Connect）算法[8-9]，規(guī)劃效率大幅提升。Karaman等提出了RRT*算法[10-11]，以解決RRT算法路徑規(guī)劃復雜和陷入局部最優(yōu)的問題。Gammell等在RRT*算法基礎上進行優(yōu)化，提出了Informed-RRT*算法[12-13]，通過引入新的狀態(tài)子集對算法本身的采樣空間進行簡化。

本文基于擴展策略和路徑平滑等方法對RRT-Connect算法進行改進，引入概率偏向法篩選隨機點，并基于人工勢場進行擴展，采用三次B樣條曲線優(yōu)化并生成光滑路徑，最后通過MATLAB仿真驗證改進算法在不同環(huán)境下的具體表現(xiàn)。

2 改進RRT-Connect算法的構建

2.1 基本RRT算法與RRT-Connect算法

RRT算法的主要思想是以路徑規(guī)劃的起點作為根節(jié)點，然后通過隨機采樣的方式增加葉節(jié)點，并不斷生長成一個隨機擴展樹，隨機樹中某一個葉節(jié)點到達目標點附近區(qū)域時，將兩點相連即可獲得從起點位置到目標點位置的路徑。傳統(tǒng)RRT算法在延伸的過程中專注于局部最優(yōu)，往往忽略整體最優(yōu)，導致規(guī)劃路線存在過于復雜和路徑過長等弊端。

RRT-Connect采用了一種貪婪的雙向擴展的隨機樹算法[14]，即在RRT算法隨機生成采樣點并向其延伸的基礎上，使起始點Xinit和目標點Xgoal同時生長，并且在每次擴展的過程中，兩棵樹都嘗試直接向對方父節(jié)點方向延伸。在擴展延伸過程中引入了啟發(fā)算法，使隨機樹可以通過環(huán)境因素自動調節(jié)步長，提高雙樹搜索效率、縮短搜索時間和路徑，RRT算法和RRT-Connect算法的路徑優(yōu)化仿真對比如圖1所示。但由于RRT-Connect算法未能從根本上改進RRT隨機取樣帶來的弊端，其在不同環(huán)境下的工作效率提升效果不夠穩(wěn)定。同時，算法擴展具有很強的隨機性，使其在多障礙的復雜環(huán)境下難以找到合適的擴展方向。

2.2 基于概率偏向法的隨機點篩選優(yōu)化方法

本文引入概率偏向法對選取的隨機點進行篩選。該算法在取點時加入隨機數(shù)閾值[15]：當某次選取的隨機數(shù)大于設定的隨機數(shù)閾值時，則在地圖中隨機選取2個隨機點Xrand，并對Xrand與目標點Xgoal之間的直線距離進行比較，取與目標點Xgoal距離近的點作為此次擴展所用的隨機點Xrand；當選取的隨機數(shù)小于設定的閾值時，則將此次選取的隨機點Xrand更改為算法運行時的目標點Xgoal。通過閾值的設定引導RRT-Connect算法的擴展方向，加快算法的收斂速度，并對最終路徑進行優(yōu)化，以達到縮短路徑長度的目的。

2.3 基于人工勢場的擴展方式改進

利用人工勢場法的思想[16]對新節(jié)點的擴展方式進行改進，即通過在環(huán)境中添加引力點對擴展點進行吸引，進而加速算法收斂。常用的擴展公式為：

[Xnew=Xnear+sXrand-XnearXrand-Xnear] （1）

式中：Xnew為新節(jié)點，Xnear為最近點，s為擴展步長。

本文在此基礎上加入目標點Xgoal對新節(jié)點Xnew的引力，公式調整為：

[Xnew=Xnear+sXrand-XnearXrand-Xnear+kXgoal-XnearXgoal-Xnear] （2）

式中：k為引力系數(shù)，用于控制Xgoal對Xnew的引力。

若由最近點Xnear向新節(jié)點Xnew擴展過程中未觸碰到障礙物，則正常擴展，否則通過減小k對障礙物進行規(guī)避。

2.4 基于三次B樣條曲線的路徑優(yōu)化

RRT-Connect算法在得到有效的可行路徑后，往往無法進一步優(yōu)化，導致最終路徑與最優(yōu)路徑仍存在差距。針對此問題，利用三次B樣條曲線對最終路徑進行平滑處理，以縮短路徑長度。

B樣條曲線可以保證曲線的每一段都是平滑的，且在有限階內連續(xù)可導，保證了B樣條基函數(shù)可以通過局部支撐達到全局支撐。三次B樣條曲線函數(shù)Pi（t）為[17-18]：

[[Pi（t）=j=03Bj，3（t）Ci+j=16t3t2t1-13-313-630-30301410CiCi+1Ci+2Ci+3]][（3）]

式中：t∈[0，1]為參數(shù)，Ci+j為第i段曲線中的第j個控制點，Bj，3（t）為三次B樣條曲線第j個控制點的基函數(shù)。

3 改進RRT-Connect算法仿真驗證

為了驗證改進RRT-Connect算法在不同環(huán)境下的路徑規(guī)劃效果，本文在MATLAB中分別構建存在簡單障礙物、復雜障礙物和僅有狹窄路徑的環(huán)境地圖，地圖大小均為1 000 mm×1 000 mm，設置步長為20 mm、改進RRT-Connect算法的引力系數(shù)為1.3、概率閾值為0.2，進行路徑規(guī)劃仿真。

3.1 簡單障礙物環(huán)境下的性能驗證

本文設置的簡單障礙物環(huán)境及RRT-Connect算法與改進RRT-Connect算法的路徑規(guī)劃仿真對比結果如圖2所示。由圖2可知，RRT-Connect和改進RRT-Connect算法的路徑長度分別為567 mm和532 mm。由圖2c可以看出，經過三次B樣條處理后，生成的最終路徑更優(yōu)。同時，RRT-Connect和改進RRT-Connect算法的運行時間分別為1.300 s和0.747 s，改進RRT-Connect算法用時更短。

圖3所示為多次仿真獲得的算法路徑長度和需要的運行時間，可以看出，改進RRT-Connect算法具有更好的路徑穩(wěn)定性。表1所示為改進RRT-Connect算法與RRT-Connect算法的性能對比結果，可以明顯看出，改進RRT-Connect在所得路徑長度和運行時間方面明顯優(yōu)于傳統(tǒng)的RRT-Connect算法，效率更高。

3.2 復雜障礙物和狹窄路徑環(huán)境下的性能驗證

分別在多障礙物的復雜環(huán)境和狹窄路徑環(huán)境下進行仿真分析，結果如圖4、圖5所示。并取20次仿真后的平均值作為路徑長度和算法耗時的最終數(shù)據(jù)，如表2所示。由圖4可知，多障礙物環(huán)境下，改進的RRT-Connect算法規(guī)劃的路徑更短、更平滑，由表2可知，其平均路徑長度為1 616 mm，平均耗時3.14 s，優(yōu)于RRT-Connect算法。

結合圖5和表2數(shù)據(jù)，在狹窄路徑環(huán)境下，使用改進RRT-Connect算法，平均路徑長度為1 167 mm，平均耗時2.8 s，由圖4和圖5結合表2的數(shù)據(jù)對比，表明改進RRT-Connect算法有效縮短了路徑長度和運行時間。

不同環(huán)境下的仿真結果表明，各環(huán)境下改進RRT-Connect算法生成的最終路徑明顯優(yōu)于RRT-Connect算法，且改進RRT-Connect算法的平均耗時相差較小，即算法的路徑規(guī)劃效率受環(huán)境影響較小，更適合在復雜路況條件下應用。

4 結束語

本文對RRT-Connect算法在擴展策略和路徑平滑等方面進行改進，并利用MATLAB對改進算法和原算法在不同環(huán)境下的性能表現(xiàn)進行了對比分析，從生成路徑的長度、路徑規(guī)劃耗時方面驗證了改進算法的性能。結果表明，相較于RRT-Connect算法，改進算法生成的路徑和平均耗時更短。

本文所做的工作未能覆蓋智能汽車的真實運行環(huán)境，未來需進一步開展實際工況和多動態(tài)障礙物條件下的系統(tǒng)路徑規(guī)劃算法研究。

參考文獻

[1]" "宋江一， 李丹， 陳文博. 融合Dijkstra和PID算法的室內移動機器人局部路徑規(guī)劃[J]. 安徽工業(yè)大學學報（自然科學版）， 2023， 40（1）： 59-64.

SONG J Y， LI D， CHEN W B. Local Path Planning of Indoor Mobile Robot Based on Dijkstra and PID Algorithm[J]. Journal of Anhui University of Technology （Natural Science Edition）， 2023， 40（1）： 59-64.

[2]" "馮來春， 梁華為， 杜明博， 等. 基于A*引導域的RRT智能車輛路徑規(guī)劃算法[J]. 計算機系統(tǒng)應用， 2017， 26（8）： 127-133.

FENG L C， LIANG H W， DU M B， et al. Guiding-Area RRT Path Planning Algorithm Based on A* for Intelligent Vehicle[J]. Computer Systems amp; Applications， 2017， 26（8）： 127-133.

[3]" "李艷生， 萬勇， 張毅， 等. 基于人工蜂群-自適應遺傳算法的倉儲機器人路徑規(guī)劃方法[J]. 儀器儀表學報， 2022， 43（4）： 282-290.

LI Y S， WAN Y， ZHANG Y， et al. Path Planning for Warehouse Robot Based on the Artificial Bee Colony-Adaptive Genetic Algorithm[J]. Chinese Journal of Scientific Instrument， 2022， 43（4）： 282-290.

[4]" "梁凱， 毛劍琳. 基于改進蟻群算法的室內移動機器人路徑規(guī)劃[J]. 電子測量技術， 2019， 42（11）： 65-69.

LIANG K， MAO J L. Path Planning of Indoor Mobile Robot Based on Improved Ant Colony Algorithm[J]. Electronic Measurement Technology， 2019， 42（11）： 65-69.

[5]" "劉洋， 章衛(wèi)國， 李廣文. 基于改進PRM算法的路徑規(guī)劃研究[J]. 計算機應用研究， 2012， 29（1）： 104-106+139.

LIU Y， ZHANG W G， LI G W. Study on Path Planning Based on Improved PRM Method[J]. Application Research of Computers， 2012， 29（1）： 104-106+139.

[6]" "印峰， 謝青松. 基于改進RRT*算法的移動機器人路徑規(guī)劃研究[J]. 湘潭大學學報（自然科學版）， 2022， 44（4）： 22-31.

YIN F， XIE Q S. Research on Mobile Robot Path Planning Based on Improved RRT* Algorithm[J]. Journal of Xiangtan University （Natural Science Edition）， 2022， 44（4）： 22-31.

[7]" "BECERRA I， SUOMALAINEN M， LOZANO E， et al. Human Perception-Optimized Planning for Comfortable VR-Based Telepresence[J]. IEEE Robotics and Automation Letters， 2020， 5（4）： 6489-6496.

[8]" "葉鴻達， 黃山， 涂海燕. 基于改進Bi-RRT*算法的移動機器人路徑規(guī)劃[J]. 電光與控制， 2022， 29（2）： 76-81.

YE H D， HUANG S， TU H Y. Path Planning of Mobile Robot Based on Improved Bi-RRT* Algorithm[J]. Electronics Optics amp; Control， 2022， 29（2）： 76-81.

[9]" KUFFNER J J， LAVALLE S M. RRT-Connect： An Efficient Approach to Single-Query Path Planning[C]// IEEE International Conference on Robotics and Automation. San Francisco， CA， USA： IEEE， 2000.

[10] 裴以建， 楊超杰， 楊亮亮. 基于改進RRT*的移動機器人路徑規(guī)劃算法[J]. 計算機工程， 2019， 45（5）： 285-290+297.

PEI Y J， YANG C J， YANG L L. Path Planning Algorithm for Mobile Robot Based on Improved RRT*[J]. Computer Engineering， 2019， 45（5）： 285-290+297.

[11] KARAMAN S， FRAZZOLI E. Incremental Sampling-Based Algorithms for Optimal Motion Planning[C]// Robotics： Science and Systems. Zaragoza， Spain： The Robotics： Science and Systems Foundation， 2010.

[12] 周恒旭， 程勇， 劉偉才. Dubins-Informed RRT*算法規(guī)劃的機械臂運動[J]. 自動化技術與應用， 2020， 39（10）： 67-74.

ZHOU H X， CHENG Y， LIU W C. Manipulator Motion Planning Based on Dubins-Informed RRT* Algorithm[J]. Technology and Application of Automation， 2020， 39（10）： 67-74.

[13] 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]// 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago， IL， USA： IEEE， 2014.

[14] 韓康， 程衛(wèi)東. 基于改進RRT-Connect算法的機械臂路徑規(guī)劃[J]. 計算機應用與軟件， 2022， 39（3）： 260-265.

HAN K， CHENG W D. Path Planning of Robot Arm Based on Improved RRT Algorithm[J]. Computer Applications and Software， 2022， 39（3）： 260-265.

[15] 趙惠， 李慶黨， 張明月. 基于改進RRT算法的機械臂路徑規(guī)劃方法[J]. 電子測量技術， 2021， 44（16）： 45-49.

ZHAO H， LI Q D， ZHANG M Y. Path Planning Method of Manipulator Based on Improved RRT Algorithm[J]. Electronic Measurement Technology， 2021， 44（16）： 45-49.

[16] 朱毅， 張濤， 宋靖雁. 非完整移動機器人的人工勢場法路徑規(guī)劃[J]. 控制理論與應用， 2010， 27（2）： 152-158.

ZHU Y， ZHANG T， SONG J Y. Path Planning for Nonholonomic Mobile Robots Using Artificial Potential Field Method[J]. Control Theory and Applications， 2010， 27（2）： 152-158.

[17] 于洋， 周佳偉， 馮迎賓， 等. 基于三次B樣條曲線的無人車軌跡優(yōu)化方法研究[J]. 沈陽理工大學學報， 2019， 38（5）： 71-75.

YU Y， ZHOU J W， FENG Y B， et al. Research on Trajectory Optimization of Unmanned Vehicle Based on Cubic B-Spline Interpolation[J]. Journal of Shenyang Ligong University， 2019， 38（5）： 71-75.

[18] 錢東海， 孫林林， 趙偉. 基于三次B樣條曲線的叉車型AGV路徑規(guī)劃研究[J]. 計算機測量與控制， 2022， 30（4）： 177-181+189.

QIAN D H， SUN L L， ZHAO W. Study of Forklift AGV Path Planning Based on Cubic B-Spline Curve[J]. Computer Measurement amp; Control， 2022， 30（4）： 177-181+189.

（責任編輯 斛 畔）

修改稿收到日期為2024年4月24日。