基于優(yōu)化RRT算法的智能車輛路徑規(guī)劃策略

摘要：為了解決快速搜索隨機樹（RRT）算法存在收斂速度慢、生成路徑質(zhì)量差等問題，提出一種融合算法，即將人工勢場法以及A*算法的代價函數(shù)融合到RRT算法中。使用改進后的RRT算法生成路徑，并將其所搜尋到的路徑作為初始解傳遞給A*算法，進而重新規(guī)劃出一條更優(yōu)路徑，并結(jié)合車輛運動學(xué)約束，利用貝塞爾曲線平滑路徑。選取兩種地圖場景進行仿真實驗，實驗結(jié)果表明，提出的改進策略可以更快地生成質(zhì)量更高且更符合汽車實際行駛的路徑。仿真結(jié)果證明了該算法能有效提升RRT算法的收斂速度和路徑質(zhì)量。

關(guān)鍵詞：RRT算法；人工勢場法；A*算法；貝塞爾曲線

中圖分類號：TP242.6文獻標志碼：A

0引言（Introduction）

路徑規(guī)劃一直是汽車自動駕駛領(lǐng)域的研究重點，它在自動駕駛中的作用是實現(xiàn)具體的行程調(diào)度［1］。目前，針對路徑規(guī)劃已有很多成熟的算法，常見的算法有Dijkstra算法［2］、A*算法［3］、遺傳算法［4］、人工勢場法［5］、RRT算法［6］等。其中，RRT算法的核心思想是通過隨機擴展樹結(jié)構(gòu)，快速地探索搜索空間以達到搜尋路徑的目的。RRT算法可以在高維空間中進行路徑規(guī)劃，并且適用于復(fù)雜的環(huán)境，但由于其搜索本質(zhì)具有隨機性，它僅能確保盡快找到路徑，卻難以生成最優(yōu)的路徑［7］，因此大量學(xué)者對RRT算法進行了深入研究，例如，葉鴻達等［8］提出了一種基于A*引導(dǎo)的RRT算法，A*可以很好地引導(dǎo)RRT算法中隨機樹的生長，但是A*對地圖的要求過于嚴格，因此難以用于實際。李金良等［9］提出了安全場的概念，并且結(jié)合實車的測試數(shù)據(jù)，基于距離建立安全場，對RRT算法進行改進，提高了搜索的效率和成功率，但該算法的性能在很大程度上依賴于所用數(shù)據(jù)且魯棒性不高。

針對經(jīng)典RRT算法存在的問題，結(jié)合現(xiàn)有學(xué)者的部分研究成果，本研究對隨機點的生成添加兩個生成代價，一個是A*算法的綜合代價值，另一個是人工勢場法的合力。只有當新節(jié)點滿足這兩個判定指標時，才會減少生成的冗余節(jié)點并加快收斂速度。將生成的初始路徑傳遞給A*算法，重新搜索一條更優(yōu)的路徑，過濾掉多余的枝條，使最終生成的路徑更優(yōu)。結(jié)合車輛運動學(xué)約束，利用貝塞爾曲線平滑路徑，3c521f991f288277348d9b4d54591c06最終得到一條更加符合實際的車輛行駛路徑。

1經(jīng)典RRT算法（ClassicalRRTalgorithm）

經(jīng)典的RRT算法原理如圖1所示。傳統(tǒng)的RRT算法的邏輯是從初始位置Xstart出發(fā)，通過在狀態(tài)空間中隨機采樣的方式增加新的節(jié)點，繞過在狀態(tài)空間中的障礙物，當新生成的節(jié)點是目標點Xgoal或者出現(xiàn)在目標區(qū)域內(nèi)時，最終生成一個隨機樹。進入循環(huán)之后，有以下幾個步驟［6］。

Step1：在狀態(tài)空間中隨機采集節(jié)點Xrand。

Step2：查找節(jié)點集合T中距離Xrand最近的節(jié)點Xnear。

Step3：令Xnear沿著指向Xrand的方向，生長規(guī)定步長得到新節(jié)點Xnew。

Step4：對Xnear與Xnew的連線進行碰撞檢測，若發(fā)生碰撞，則進行下一輪循環(huán)，若沒有發(fā)生碰撞，則將新節(jié)點Xnew放入節(jié)點集合T中7565a70e5237ea09bcde0620522fda68，并且設(shè)置Xnear為Xnew的父節(jié)點。

[JP3]Step5：若Xnew等于Xgoal，或者Xnew到Xgoal的距離小于設(shè)定的閾值（即新節(jié)點到達目標范圍），則路徑成功找到，即退出循環(huán)。

按照RRT算法的邏輯，影響生成路徑好壞的主要因素是隨機點的生成與否，而隨機點的生成通常是毫無規(guī)律的，大部分情況都需要較大的計算量完成路徑搜索，因此想生成最優(yōu)的路徑十分困難，所以需要對生成的路徑進行優(yōu)化。

2優(yōu)化RRT算法（OptimizetheRRTalgorithm）

針對RRT算法的收斂速度慢、冗余節(jié)點多的問題，提出一種基于人工勢場法和A*算法的判定隨機節(jié)點生成策略，當且僅當隨機點滿足約束條件時，才會生成，使得隨機點的生成在具有較強啟發(fā)性的同時，還能在遇到某些特定場景時保持其本身的隨機性，并將RRT算法生成的路徑作為A*算法的初始解，再利用A*算法生成一條更加合理的路徑。由于在RRT算法中已經(jīng)計算過初始解中節(jié)點的代價值，因此A*算法不需要重復(fù)計算。對于A*算法生成后的路徑，利用貝塞爾曲線進行路徑優(yōu)化，在此基礎(chǔ)上結(jié)合車輛運動學(xué)約束，使得生成的路徑不僅更優(yōu)且符合汽車的實際行駛路徑，最終得到一條質(zhì)量較高的路徑。

經(jīng)典的RRT算法在附近一片區(qū)域內(nèi)隨機選擇一個點并按照步長進行擴展，即隨機點的生成完全是由隨機點所處的位置決定的，沒有任何的約束，具備較大的隨機性。這樣會導(dǎo)致隨機樹生長盲目，產(chǎn)生很多無意義的節(jié)點?；诖藛栴}，可以利用A*算法中的代價函數(shù)和人工勢場法中的勢場所產(chǎn)生的力對隨機點的生成進行判定和限制，減少冗余節(jié)點的生成，并且縮短迭代的時間。

根據(jù)A*算法中代價函數(shù)的計算方法，F(xiàn)（n）為節(jié)點n的綜合優(yōu)先級，G（n）是起點距離當前節(jié)點n的代價，H（n）是當前節(jié)點n到終點的預(yù)計代價［3］。圖3表示將綜合優(yōu)先級作為隨機點生成的判定條件之一，對新生成的隨機點進行代價檢測，選擇使用綜合優(yōu)先級F（n）小于其父節(jié)點值加上一個步長距離的點作為擴展點。這里G（n）是步長與當前路徑上所生成隨機點個數(shù)的乘積，具體為圖3中的實線。其中預(yù)計代價H（n）的計算方法有很多，為了保證RRT算法中節(jié)點生成具有隨機性，選擇使用能夠向任何方向生長的歐幾里得距離，為圖3中的長短虛線。當隨機點Xrand2的綜合代價大于其父節(jié)點加上一個步長的距離，即h2大于h1，則舍棄該點。

根據(jù)人工勢場法的基本思想，在障礙物周圍構(gòu)建障礙物斥力勢場，在目標點周圍構(gòu)建引力勢場，類似于物理學(xué)中的電磁場。被控對象在這兩組勢場組成的復(fù)合場中受到斥力作用和引力作用，引力和斥力的合力指引著被控對象運動，搜索出無碰撞的避障路徑［10］。但是，處于勢場中的被控對象可能會陷入局部最小值從而導(dǎo)致方向不明確和路徑生成失敗的情況，所以需要對人工勢場法的函數(shù)做一部分的修改和完善。引力計算公式如下：

Fatt（q）=-η[WTHX]ρ[WTBX]（q，qg）[JZ）][JY]（1）

其中：η為正比例增益系數(shù)，[WTHX]ρ[WTBX]（q，qg）是一個矢量，表示從當前點到目標點的距離，方向為從當前位置指向目標點位置。斥力計算公式如下：

Freq（q）=〖JB（{〗〖HL（2：1，Z;2，Z〗k[JB（（]〖SX（〗1〖〗[WTHX]ρ[WTBX]（q，q0）〖SX）〗-〖SX（〗1〖〗[WTHX]ρ[WTBX]0〖SX）〗[JB））]〖SX（〗1〖〗[WTHX]ρ[WTBX]2（q，q0）〖SX）〗，〖〗0≤[WTHX]ρ[WTBX]（q，q0）≤[WTHX]ρ[WTBX]0

0，〖〗[WTHX]ρ[WTBX]（q，q0）≥[WTHX]ρ[WTBX]0〖HL）〗〖JB）〗[JZ）][JY]（2）

其中：k為正比例系數(shù)，[WTHX]ρ[WTBX]（q，q0）是一個矢量，方向是從障礙物指向當前點的位置，表示的是當前點到障礙物的距離。障礙物產(chǎn)生斥力的條件是當前點在障礙物的影響區(qū)域內(nèi)，若當前點與障礙物的距離大于設(shè)定閾值，則不產(chǎn)生斥力。當前位置所受到的合力，即引力與斥力之和?，F(xiàn)添加一個斥力，即遇到目標不可達的情況時，添加一個由車輛指向目標點方向上的力，讓斥力的受力方向發(fā)生改變，等于變相地增加引力而削減斥力。修改后的斥力計算公式如下：

將修改后的斥力計算公式與引力計算公式相加就是當前所受的力，此時不考慮力的方向，僅考慮其大小。如圖4所示，若新的隨機點受到的合力大于其父節(jié)點，則舍棄。

2.2利用A*算法進行優(yōu)化路徑

RRT算法是一種用于搜索無向環(huán)境中連續(xù)狀態(tài)空間中路徑的隨機算法，與之相比，A*算法是一種啟發(fā)式算法，主要用于在圖中找到從起點到終點的最佳路徑。因此，可以將RRT算法作為A*算法的初始解，然后通過A*算法對RRT算法生成的路徑進行進一步優(yōu)化。這樣做的目的在于利用了RRT算法可以在復(fù)雜環(huán)境中快速生成近似路徑的優(yōu)勢，而A*算法可以在這條路徑的基礎(chǔ)上進行更精確的優(yōu)化，將兩個算法的優(yōu)勢有機結(jié)合，帶來更有效的路徑規(guī)劃解決方案。核心策略為在柵格化地圖之后，使用A*算法對RRT算法生成的初始解進行進一步搜索，A*算法會在給定的搜索空間中基于啟發(fā)式信息和路徑代價評估函數(shù)，盡可能地找到最佳路徑。一旦A*算法完成搜索，得到了一條路徑之后，就可以刪除那些冗余的枝條而只留下A*算法找到的路徑，然后對這條路徑進行處理。

A*算法的主要策略如下。

Step1：初始化open_list和close_list，并將RRT算法搜索到的初始路徑傳入open_list中。

Step2：遍歷open_list，若其不為空，從其中選擇優(yōu)先級最高的點n，判斷節(jié)點n是否為終點，若是終點，則進入Step5，若不是，則進入Step3。

Step3：將節(jié)點n從open_list中刪除，并加入close_list中，遍歷其附近節(jié)點，判斷附近節(jié)點m是否在close_list中，若在其中，則選取下一個附近節(jié)點，若不在其中，則進入Step4。

Step4：若附近節(jié)點m也不在open_list中時，則設(shè)置節(jié)點m的父節(jié)點為節(jié)點n，并計算節(jié)點m的代價值，添加到open_list中回到Step2。

Step5：回溯路徑并輸出。

2.3貝塞爾曲線平滑路徑

貝塞爾曲線是一種數(shù)學(xué)曲線，常用于圖形設(shè)計和計算機圖形學(xué)中，它的作用是在A*算法搜尋到路徑之后，將離散的路徑點連接，從而獲得更加平滑的路徑。貝塞爾曲線可以使用n個控制點{P1，P2，…，Pn}控制曲線的形狀，曲線經(jīng)過起點P1和終點Pn，但不經(jīng)過中間點P2～Pn-1［11］。這里將A*算法搜尋到的所有路徑點，按照每3個為一組，組成若干個二階貝塞爾曲線。在二階貝塞爾之后繼續(xù)遞歸得到n階貝塞爾的公式，進而一次性將所有的路徑點都考慮進去。其中，貝塞爾曲線的公式如下：

p（t）=∑〖DD（〗n〖〗i=0〖DD）〗PiBi，n（t），0≤t≤1[JZ）][JY]（4）

[JB（{]Bi，n（t）=Cini（1-t）n-i

Cin=〖SX（〗n！〖〗i！（n-i）！〖SX）〗[JB）]（i=0，1，…，n）[JZ）][JY]（5）

二階貝塞爾曲線公式如下：

B（t）=（1-t）2P0+2t（1-t）P1+t2P2，t∈［0，1］[JZ）][JY]（6）

2.4融合車輛運動學(xué)的約束函數(shù)

在生成的路徑中，會出現(xiàn)有一部分位置的曲率小于汽車的最小轉(zhuǎn)彎半徑所要求的最小值，所以需要對生成后的路徑進行約束，使路徑滿足車輛轉(zhuǎn)向特性要求。假設(shè)車輛在平面內(nèi)做勻速運動且車輛做純滾動式運動，在任何行駛時段，車輛的速度都一定指向縱軸的垂直方向。將車輛簡化為二自由度的自行車模型，如圖5所示，l是軸距，φ是橫擺角，δ是車輛前輪轉(zhuǎn)角，（X，Y）是后軸軸心坐標。為了驗證優(yōu)化算法的可行性，在VisualStudio2022中進行算法的仿真驗證。為了驗證其改進效果，將經(jīng)典RRT算法、基于密集節(jié)點過濾的RRT算法與本文算法布置在同一張地圖上進行仿真實驗，簡單場景下的仿真結(jié)果如圖6所示，復(fù)雜場景下的仿真結(jié)果如圖7所示。將引力系數(shù)設(shè)為0.5，斥力系數(shù)設(shè)為0.5，RRT擴展步長設(shè)為50。設(shè)置兩種地圖場景，一種簡單、一種復(fù)雜，地圖大小均為750×750。圖6、圖7中較細的線條為搜尋后生長的枝條，其中加粗的線條為生成的最終路徑。

由圖6、圖7可知，經(jīng)典RRT算法的路徑分支繁多，難以快速地找到路徑，而基于密集節(jié)點過濾的RRT算法能有效地減少分支。本文提出的改進RRT算法可以很好地避免多分支情況的出現(xiàn)，還可以在保留經(jīng)典RRT采樣隨機性的同時，給予隨機點較強的目標啟發(fā)性，并且減少搜索時間和縮短路徑長度，同時更加符合車輛的運動學(xué)規(guī)律。根據(jù)生成的最終曲線，可以確定路徑的質(zhì)量也得到了提高。為了確保算法的科學(xué)性，本研究分別在簡單場景下和復(fù)雜場景下，對表1中的3種算法進行了100次仿真實驗，具體的實驗結(jié)果分別如表1和表2所示。

通過表1和表2，可以很明顯看出，不管是在簡單場景還是復(fù)雜場景中，本文算法的路徑規(guī)劃時間、路徑長度、所生成的隨機點的個數(shù)均是最少的。并且相比于密集節(jié)點過濾的RRT算法，本文算法在簡單場景中的規(guī)劃速度提升了62%左右，隨機點減少了50%左右；在復(fù)雜場景中的規(guī)劃速度提升了51%左右，隨機點減少了57%左右。此外，在使用貝塞爾曲線平滑路徑后，路徑長度得到明顯縮短。

4結(jié)論（Conclusion）

基于智能車輛的路徑規(guī)劃算法的研究中，本文對于RRT算法、A*算法、人工勢場法進行了深入研究，通過仿真實驗，揭示了經(jīng)典RRT算法存在的問題，包括冗余節(jié)點多、隨機點生成無規(guī)律及生成路徑質(zhì)量差等。為了解決以上問題，首先在RRT算法中融合人工勢場法和A*算法，明顯減少了迭代的次數(shù)和迭代所需要的時間，生成的隨機點具備一定的方向性；其次通過A*算法生成了一條質(zhì)量更高的路徑，并利用貝塞爾曲線平滑路徑；最后對所生成路徑進行車輛運動學(xué)約束，使生成的路徑更加滿足車輛的實際行駛要求。經(jīng)過大量的仿真實驗，證明了優(yōu)化后RRT算法具有一定的可行性和科學(xué)性。

參考文獻（References）

［1］采國順，劉昊吉，馮吉偉，等.智能汽車的運動規(guī)劃與控制研究綜述［J］.汽車安全與節(jié)能學(xué)報，2021，12（3）：279＼|297.

［2］馬新國，馬希青.融合改進RRT和Dijkstra算法的機器人動態(tài)路徑規(guī)劃［J］.組合機床與自動化加工技術(shù)，2023（2）：5＼|9.

［3］郭昭烽，謝玲，劉紅英，等.移動機器人路徑規(guī)劃算法仿真及實現(xiàn)［J］.軟件工程，2022，25（4）：25＼|29.

［4］孫俊麗.談遺傳算法［J］.辦公自動化，2019，24（12）：48＼|49.

［5］LIUJ，QINXL，QIBL，eta1.3DonlinepathplanningofUAVbasedonimproveddifferentialevolutionandmodelpredictivecontrol［J］.Internationaljournalofinnovativecomputing，informationandcontrol，2020，16（1）：315＼|329.

［6］笪晨，宋天麟，施維.多策略模式下RRT算法的優(yōu)化［J］.組合機床與自動化加工技術(shù)，2022（12）：128＼|131，135.

［7］譚波，羅均，羅雨松，等.改進R_{a5bf9f4dd2099b8e75e23f4d9e0cbf8cb097d00314a12c82d2abccb1a86fa2bf}RT算法的機器人路徑規(guī)劃［J］.重慶大學(xué)學(xué)報，2023，46（9）：13＼|22.

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

［9］李金良，舒翰儒，劉德建，等.基于改進RRT路徑規(guī)劃算法［J］.組合機床與自動化加工技術(shù)，2021（2）：22＼|24，29.

［10］PANGBZ，DAIW，HUXT，etal.Multipleairroutecrossingwaypointsoptimizationviaartificialpotentialfieldmethod［J］.Chinesejournalofaeronautics，2021，34（4）：279＼|292.

［11］韓浩，王舜燕.基于混合貝塞爾曲線模型的車道檢測算法［J］.計算機工程與設(shè)計，2018，39（3）：732＼|737.