關(guān)鍵詞：RRT算法；目標(biāo)偏置策略；障礙物膨脹策略；啟發(fā)式貪心思想；三次B樣條曲線

0 引言

近些年來，隨著科技的進(jìn)步，汽車無人駕駛技術(shù)得到了迅速發(fā)展。新一代智能汽車研究已成為產(chǎn)業(yè)革命的重要方向[1]。無人駕駛汽車具有提高道路安全性、減少交通事故和擁堵、降低交通成本、提高出行舒適度、滿足特定出行需求、節(jié)能環(huán)保等優(yōu)點(diǎn)，為人類出行帶來更加安全、高效、便捷和環(huán)保的體驗(yàn)。因此，對無人駕駛汽車領(lǐng)域的研究變得越發(fā)重要，引起了眾多科研人員的廣泛關(guān)注。

路徑規(guī)劃是無人車智能駕駛技術(shù)中的關(guān)鍵技術(shù)之一。為了使無人車根據(jù)預(yù)定地圖到達(dá)指定目的地，需要在地圖上進(jìn)行路徑規(guī)劃，并采用合適的路徑規(guī)劃算法。常見的全局路徑規(guī)劃算法包括Dijkstra算法[2]、A*算法[3]、蟻群算法[4]、RRT 算法[5] 等。其中，RRT（Rapidly-exploring Random Tree） 算法是一種基于樹結(jié)構(gòu)的隨機(jī)采樣路徑規(guī)劃算法。它通過不斷隨機(jī)采樣狀態(tài)空間中的點(diǎn)，并將其連接到樹上已有節(jié)點(diǎn)的方式，逐步生成一棵覆蓋整個(gè)狀態(tài)空間的樹。RRT算法具有簡單易實(shí)現(xiàn)、適應(yīng)性強(qiáng)等優(yōu)點(diǎn)。然而，其缺點(diǎn)也較為明顯，對RRT算法的缺點(diǎn)，盡管國內(nèi)外學(xué)者對其進(jìn)行了多種改進(jìn)，以提升其在不同場景下的表現(xiàn)，但仍存在一些共性問題。

文獻(xiàn)[5]采用雙向改進(jìn)RRT算法并結(jié)合人工勢場法，解決了RRT算法在隨機(jī)性和計(jì)算量較大的問題，從而提高了算法的尋優(yōu)精度和路徑規(guī)劃速度。文獻(xiàn)[6] 將目標(biāo)導(dǎo)向策略引入RRT，以解決路徑規(guī)劃中隨機(jī)性較大、容易陷入停滯狀態(tài)的問題，通過增加目標(biāo)點(diǎn)的采樣概率，加快了路徑規(guī)劃的收斂速度，提高了效率。文獻(xiàn)[7]將人工勢場法與RRT結(jié)合，利用環(huán)境信息引導(dǎo)隨機(jī)樹向目標(biāo)點(diǎn)生長，避免陷入局部最優(yōu)。盡管這一方法改善了路徑生成過程，但由于選用的地圖過于簡單，缺乏代表性，其效果在復(fù)雜環(huán)境中可能有限。文獻(xiàn)[8]將改進(jìn)的RRT與動(dòng)態(tài)窗口法相結(jié)合，以防止陷入局部最優(yōu)并實(shí)現(xiàn)動(dòng)態(tài)避障，并融合人工勢場法來調(diào)整擴(kuò)展方向，提高了算法的搜索效率和路徑質(zhì)量。然而，最終生成的路徑依然不夠平滑，不適合實(shí)際應(yīng)用中的移動(dòng)機(jī)器人行駛。文獻(xiàn)[9]提出通過計(jì)算從起點(diǎn)到終點(diǎn)的方向向量，并將其作為引導(dǎo)向量，使隨機(jī)樹的生長方向更加朝向目標(biāo)點(diǎn)，減少了節(jié)點(diǎn)擴(kuò)展的隨機(jī)性，提高了搜索效率。然而，隨機(jī)樹擴(kuò)展時(shí)仍容易陷入障礙物包圍圈，導(dǎo)致路徑規(guī)劃失敗。

針對RRT算法搜索效率不高、生成路徑不平滑等缺點(diǎn)，本文提出了一種改進(jìn)的RRT算法。具體優(yōu)化過程如下：首先，加入目標(biāo)偏置策略和障礙物膨脹策略，以減小RRT算法擴(kuò)展的隨機(jī)性，在提升搜索效率的同時(shí)，保證規(guī)劃路徑的安全和穩(wěn)定性。其次，引入貪心算法思想，剔除規(guī)劃路徑中的冗余節(jié)點(diǎn)，使生成的路徑更加簡短和高效。最后，使用三次B樣條曲線對生成的路徑進(jìn)行平滑處理，使規(guī)劃出的路徑更有利于無人車的行駛。

1 傳統(tǒng)RRT 算法

RRT算法具有簡單、高效、無須預(yù)處理和適用于非線性約束等優(yōu)點(diǎn)，因此被廣泛應(yīng)用于無人駕駛領(lǐng)域。RRT算法的核心思想是通過在搜索空間中隨機(jī)采樣，逐步擴(kuò)展樹結(jié)構(gòu)，從而找到從起點(diǎn)Sinit到目標(biāo)點(diǎn)Sgoal的路徑，其擴(kuò)展方式如圖1所示。

算法的主要步驟包括：

（1） 初始化：創(chuàng)建一棵樹 T，樹的根節(jié)點(diǎn)為起點(diǎn)Sinit。

（2） 隨機(jī)采樣：在搜索空間中隨機(jī)生成一個(gè)點(diǎn)Srand。

（3） 找到最近節(jié)點(diǎn)：在樹 T 中找到離 Srand 最近的節(jié)點(diǎn) Snear。

（4） 擴(kuò)展新節(jié)點(diǎn)：從 Snear 沿著 Srand的方向，以固定步長 A生成新節(jié)點(diǎn) Snew。

（5） 碰撞檢測：檢查從 Snear 到 Snew 的路徑是否與障礙物發(fā)生碰撞。如果沒有碰撞，則將 Snew 添加到樹T 中。

（6） 檢查是否到達(dá)目標(biāo)：如果 Snew 到目標(biāo)點(diǎn)的距離小于預(yù)設(shè)的閾值，則認(rèn)為找到了路徑。

（7） 重復(fù)：重復(fù)上述步驟，直到找到路徑或者達(dá)到最大迭代次數(shù)。

2 改進(jìn)RRT 路徑規(guī)劃算法

2.1 障礙物虛擬膨脹策略

傳統(tǒng)的RRT算法在規(guī)劃路徑時(shí)，路徑可能會(huì)緊貼障礙物的邊緣，這對于實(shí)際應(yīng)用中的無人車來說并不理想。無人車具有一定的寬度，如果按照傳統(tǒng)RRT算法規(guī)劃出的路徑行駛，容易發(fā)生碰撞或剮蹭。為了更符合無人車的實(shí)際運(yùn)動(dòng)要求，我們需要改進(jìn)RRT算法，使其在規(guī)劃路徑時(shí)能夠考慮無人車的寬度，從而提高車輛在行駛過程中的安全性。

為此，本文提出了一種障礙物虛擬膨脹策略。在進(jìn)行延伸節(jié)點(diǎn)的碰撞檢測時(shí)，將延伸節(jié)點(diǎn)與障礙物的虛擬半徑進(jìn)行比較。當(dāng)延伸點(diǎn)與障礙物圓心之間的距離小于虛擬半徑時(shí)，則認(rèn)為發(fā)生碰撞，剔除該延伸節(jié)點(diǎn)。圖2為障礙物虛擬膨脹示意圖，其中rt為真實(shí)半徑，rv為虛擬半徑。

2.2 目標(biāo)偏置策略

傳統(tǒng)RRT算法的搜索效率低下，搜索時(shí)間過長。然而，目標(biāo)偏置策略可以有效降低算法搜索的隨機(jī)性，減少對不必要空間的搜索，從而極大提高算法的搜索效率，節(jié)省大量搜索時(shí)間。因此，在RRT算法中融入目標(biāo)偏置策略進(jìn)行隨機(jī)采樣是一種有效的選擇。加入目標(biāo)偏置策略后，隨機(jī)點(diǎn)的生成方式如式（1） 所示。

式中設(shè)定了兩個(gè)概率值Pg和Ptarget。其中，Pg為按照均勻概率分布隨機(jī)生成的概率值，范圍在0到1之間。Ptarget 為事先設(shè)定好的閾值，通常是一個(gè)較小的值。在每次迭代中，筆者隨機(jī)生成一個(gè)Pg，當(dāng)Pg小于Ptarget時(shí)，選擇目標(biāo)點(diǎn)Sgoal為采樣點(diǎn)；否則，則在整個(gè)地圖范圍內(nèi)隨機(jī)生成一個(gè)采樣點(diǎn)。

2.3 結(jié)合啟發(fā)式貪心思想剔除冗余節(jié)點(diǎn)

在通過前面兩種策略對RRT算法進(jìn)行改進(jìn)之后，規(guī)劃出的路徑仍然存在許多冗余路徑。為此，本文結(jié)合啟發(fā)式貪心思想，對路徑中的冗余節(jié)點(diǎn)進(jìn)行剔除，使生成的路徑更加簡短，從而有利于無人車高效行駛。冗余節(jié)點(diǎn)去除策略的具體步驟如下：

1） 將起點(diǎn)設(shè)置為起始索引，將路徑中的最后一個(gè)點(diǎn)設(shè)置為終點(diǎn)索引，并記錄檢測次數(shù)為節(jié)點(diǎn)數(shù)量減1。

2） 從終點(diǎn)向起點(diǎn)遍歷路徑中的每個(gè)節(jié)點(diǎn)。對于每個(gè)節(jié)點(diǎn)，檢查其與父節(jié)點(diǎn)之間的路徑是否與障礙物虛擬相交。如果相交，則將終點(diǎn)索引減1，并繼續(xù)向上檢測父節(jié)點(diǎn)；如果不相交，則將該節(jié)點(diǎn)添加到最優(yōu)路徑中，并更新起始索引和檢測次數(shù)。

3） 一旦找到一條最優(yōu)路徑段（即不與障礙物虛擬相交的路徑段），將其添加到最優(yōu)路徑中。在找到最優(yōu)路徑段之后，將該節(jié)點(diǎn)作為新的起始節(jié)點(diǎn)，從該節(jié)點(diǎn)向上重新檢測父節(jié)點(diǎn)的路徑段。

4） 當(dāng)檢測次數(shù)為0 時(shí)，表示已經(jīng)遍歷完所有節(jié)點(diǎn)，最優(yōu)路徑生成完成。

在完成啟發(fā)式貪心思想優(yōu)化后，最終得到的最優(yōu)路徑將不再包含不必要的節(jié)點(diǎn)。冗余節(jié)點(diǎn)剔除策略的具體實(shí)現(xiàn)如圖3所示。

其中，虛線代表去除冗余節(jié)點(diǎn)后的優(yōu)化路徑。

2.4 采用三次B 樣條曲線平滑路徑

在結(jié)合啟發(fā)式貪心思想去除冗余節(jié)點(diǎn)之后，生成的路徑雖然與原始RRT算法相比平滑了很多，但仍存在曲率不連續(xù)的問題，這會(huì)影響無人車行駛的流暢性和穩(wěn)定性。為了提高無人車行駛的平順性，需要對得到的折線路徑進(jìn)行平滑處理。B樣條曲線是一種廣泛應(yīng)用于路徑平滑的方法，具有幾何不變性和凹凸性等優(yōu)點(diǎn)。為使規(guī)劃路徑具有較好的連續(xù)性，使用B樣條曲線對路徑進(jìn)行平滑處理。k階 B樣條曲線的方程式如式（2） 所示：

公式中：ei ，i=0，1，2，3…n 是B樣條曲線的控制頂點(diǎn)，也就是路徑中的節(jié)點(diǎn)；Li，k（z）是第i 個(gè)k 次B樣條基函數(shù)，由一個(gè)稱為節(jié)點(diǎn)向量的參數(shù)序列U={ p0 ，...，p } m所確定。根據(jù)這些節(jié)點(diǎn)向量的不同分布形式，可以將B樣條曲線劃分為均勻B樣條曲線、 準(zhǔn)均勻 B 樣條曲線和非均勻 B 樣條曲線。其中，準(zhǔn)均勻B樣條曲線保留了貝塞爾曲線在兩個(gè)端點(diǎn)處的性質(zhì)，樣條曲線在端點(diǎn)處的切線為兩個(gè)端點(diǎn)的連線，更適合應(yīng)用于折線路徑的光滑處理?；瘮?shù)確定了B樣條曲線的連續(xù)性和相應(yīng)的次數(shù)，其遞推公式為：

由遞推公式可以看出，高次的B樣條基函數(shù)是用低次的B樣條基函數(shù)的線性組合來表示的，所有的Li，k（z）均為兩個(gè)（k-1） 次B 樣條基函數(shù)Li，k-1（z）和Li+1，k-1（z）的線性組合。

綜合分析車輛的運(yùn)動(dòng)學(xué)模型和行駛環(huán)境，同時(shí)結(jié)合算法的運(yùn)算復(fù)雜度，筆者選用三次準(zhǔn)均勻B樣條曲線對路徑進(jìn)行平滑處理，從而可以得到一條曲率連續(xù)的平滑路徑。擬合三次B樣條曲線段的示意圖如圖4 所示：

2.5 改進(jìn)RRT 路徑規(guī)劃算法流程

將前文提到的幾種改進(jìn)策略全部融入傳統(tǒng)RRT 算法之后，繪制出完整的改進(jìn)RRT算法流程圖。本文改進(jìn)RRT算法的流程圖如圖5所示。

3 實(shí)驗(yàn)仿真驗(yàn)證

為驗(yàn)證改進(jìn)RRT算法的有效性和適應(yīng)性，本實(shí)驗(yàn)在MATLAB R2023仿真平臺(tái)上進(jìn)行。在二維環(huán)境下，每種算法分別進(jìn)行50 次實(shí)驗(yàn)，并取平均值將改進(jìn)RRT算法與傳統(tǒng)RRT算法進(jìn)行對比。起點(diǎn)設(shè)置為 [0，0]，終點(diǎn)為[10， 10]，步長ρ 為 0.5，閾值設(shè)置為 0.1，最大迭代次數(shù)設(shè)置為 5 000。傳統(tǒng)RRT算法及其改進(jìn)算法在地圖中的路徑仿真結(jié)果如圖6至圖10所示。

從圖中可以看出，相較于傳統(tǒng)RRT算法，加入改進(jìn)點(diǎn)1之后，規(guī)劃的路徑更遠(yuǎn)離障礙物，提高了無人車行駛的安全性。同時(shí)，在加入改進(jìn)點(diǎn)1和2之后，RRT 算法的探索空間明顯減少，從而提高了搜索效率。進(jìn)一步地，加入改進(jìn)點(diǎn)1、2和3后，路徑中的冗余節(jié)點(diǎn)顯著減少，路徑變得更加流暢。最終，筆者使用三次B 樣條曲線對路徑進(jìn)行了平滑處理。

傳統(tǒng)RRT算法與本文最終改進(jìn)算法的仿真結(jié)果如表1所示。從表1中可以明顯看出，在擴(kuò)展節(jié)點(diǎn)數(shù)大幅減少的情況下，平均時(shí)間減少了78.1%，路徑長度減少了18.6%。

4 結(jié)束語

本文主要針對傳統(tǒng)RRT算法在路徑規(guī)劃過程中存在的收斂速度慢、搜索隨機(jī)性高、冗余節(jié)點(diǎn)過多以及路徑安全性較差等問題進(jìn)行了優(yōu)化處理。改進(jìn)算法首先對障礙物采取虛擬膨脹，使得在隨機(jī)樹的生成過程中更加遠(yuǎn)離障礙物，從而生成的路徑更加安全可靠。其次，采用目標(biāo)偏置策略，減小搜索范圍，提高搜索效率。再次，結(jié)合啟發(fā)式貪心思想，對路徑上的冗余節(jié)點(diǎn)進(jìn)行剔除，消除路徑中過大的轉(zhuǎn)彎角，減少路徑長度。最后，采用三次B樣條曲線對生成的路徑進(jìn)行平滑處理，降低路徑曲率的突變性，增加車輛的平穩(wěn)性。

通過在MATLAB進(jìn)行仿真驗(yàn)證，結(jié)果表明改進(jìn)后的RRT算法具有較高的可行性和有效性，對無人車的路徑規(guī)劃具有一定的參考價(jià)值。未來，筆者將繼續(xù)深入研究全局路徑規(guī)劃，期望將該算法優(yōu)化得更加完善，同時(shí)對局部路徑規(guī)劃算法進(jìn)行優(yōu)化。隨后，將優(yōu)化后的局部路徑規(guī)劃算法與改進(jìn)的RRT算法結(jié)合起來，通過一系列實(shí)驗(yàn)驗(yàn)證和分析，以進(jìn)一步驗(yàn)證這種融合算法在實(shí)際場景中進(jìn)行路徑規(guī)劃的有效性。