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

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

中圖分類號：TP242；TP18" "文獻(xiàn)標(biāo)志碼：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

【引用格式】 張明月， 王軍. 基于改進(jìn)雙向快速擴(kuò)展隨機(jī)樹算法的智能汽車路徑規(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)境各不相同。為了應(yīng)對多樣化的場景應(yīng)用需求，避免發(fā)生碰撞，許多學(xué)者提出了不同的路徑規(guī)劃算法和相應(yīng)的改進(jìn)算法。目前，常見的路徑規(guī)劃方法主要有迪杰斯特拉（Dijkstra）算法[1]、A*算法[2]、遺傳算法[3]、蟻群算法[4]、概率路線圖法（Probabilistic Roadmap Method，PRM）[5]、快速擴(kuò)展隨機(jī)樹（Rapidly-exploring Random Tree，RRT）算法[6]等。很多傳統(tǒng)的路徑規(guī)劃算法存在運(yùn)算時間過長、路徑規(guī)劃較為復(fù)雜等問題。

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

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

2 改進(jìn)RRT-Connect算法的構(gòu)建

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

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

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

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

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

2.3 基于人工勢場的擴(kuò)展方式改進(jìn)

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

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

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

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

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

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

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

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

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

B樣條曲線可以保證曲線的每一段都是平滑的，且在有限階內(nèi)連續(xù)可導(dǎo)，保證了B樣條基函數(shù)可以通過局部支撐達(dá)到全局支撐。三次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 改進(jìn)RRT-Connect算法仿真驗證

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

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

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

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

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

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

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

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

4 結(jié)束語

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

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

參考文獻(xiàn)

[1]" "宋江一， 李丹， 陳文博. 融合Dijkstra和PID算法的室內(nèi)移動機(jī)器人局部路徑規(guī)劃[J]. 安徽工業(yè)大學(xué)學(xué)報（自然科學(xué)版）， 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*引導(dǎo)域的RRT智能車輛路徑規(guī)劃算法[J]. 計算機(jī)系統(tǒng)應(yī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]" "李艷生， 萬勇， 張毅， 等. 基于人工蜂群-自適應(yīng)遺傳算法的倉儲機(jī)器人路徑規(guī)劃方法[J]. 儀器儀表學(xué)報， 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]" "梁凱， 毛劍琳. 基于改進(jìn)蟻群算法的室內(nèi)移動機(jī)器人路徑規(guī)劃[J]. 電子測量技術(shù)， 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)國， 李廣文. 基于改進(jìn)PRM算法的路徑規(guī)劃研究[J]. 計算機(jī)應(yīng)用研究， 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]" "印峰， 謝青松. 基于改進(jìn)RRT*算法的移動機(jī)器人路徑規(guī)劃研究[J]. 湘潭大學(xué)學(xué)報（自然科學(xué)版）， 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]" "葉鴻達(dá)， 黃山， 涂海燕. 基于改進(jìn)Bi-RRT*算法的移動機(jī)器人路徑規(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] 裴以建， 楊超杰， 楊亮亮. 基于改進(jìn)RRT*的移動機(jī)器人路徑規(guī)劃算法[J]. 計算機(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ī)械臂運(yùn)動[J]. 自動化技術(shù)與應(yīng)用， 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)東. 基于改進(jìn)RRT-Connect算法的機(jī)械臂路徑規(guī)劃[J]. 計算機(jī)應(yīng)用與軟件， 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] 趙惠， 李慶黨， 張明月. 基于改進(jìn)RRT算法的機(jī)械臂路徑規(guī)劃方法[J]. 電子測量技術(shù)， 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] 朱毅， 張濤， 宋靖雁. 非完整移動機(jī)器人的人工勢場法路徑規(guī)劃[J]. 控制理論與應(yīng)用， 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]. 沈陽理工大學(xué)學(xué)報， 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]. 計算機(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.

（責(zé)任編輯 斛 畔）

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