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

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

中圖分類號：TP242.6文獻(xiàn)標(biāo)志碼：A

0引言（Introduction）

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

其中：η為正比例增益系數(shù)，[WTHX]ρ[WTBX]（q，qg）是一個矢量，表示從當(dāng)前點(diǎn)到目標(biāo)點(diǎn)的距離，方向?yàn)閺漠?dāng)前位置指向目標(biāo)點(diǎn)位置。斥力計(jì)算公式如下：

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）是一個矢量，方向是從障礙物指向當(dāng)前點(diǎn)的位置，表示的是當(dāng)前點(diǎn)到障礙物的距離。障礙物產(chǎn)生斥力的條件是當(dāng)前點(diǎn)在障礙物的影響區(qū)域內(nèi)，若當(dāng)前點(diǎn)與障礙物的距離大于設(shè)定閾值，則不產(chǎn)生斥力。當(dāng)前位置所受到的合力，即引力與斥力之和?，F(xiàn)添加一個斥力，即遇到目標(biāo)不可達(dá)的情況時，添加一個由車輛指向目標(biāo)點(diǎn)方向上的力，讓斥力的受力方向發(fā)生改變，等于變相地增加引力而削減斥力。修改后的斥力計(jì)算公式如下：

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

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

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

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

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

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

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

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

Step5：回溯路徑并輸出。

2.3貝塞爾曲線平滑路徑

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

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

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

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

4結(jié)論（Conclusion）

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

參考文獻(xiàn)（References）

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

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

［3］郭昭烽，謝玲，劉紅英，等.移動機(jī)器人路徑規(guī)劃算法仿真及實(shí)現(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］.組合機(jī)床與自動化加工技術(shù)，2022（12）：128＼|131，135.

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

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

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

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

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