樵永鋒，王瀚鑫，周淑文，楊貴軍

（1.東北大學(xué)機(jī)械工程與自動(dòng)化學(xué)院，遼寧 沈陽 110819；2.丹東東方測(cè)控技術(shù)股份有限公司，遼寧 丹東 118002）

1 引言

隨著人工智能領(lǐng)域的興起，無人駕駛車輛已經(jīng)成為未來汽車工業(yè)的發(fā)展趨勢(shì)，無人駕駛技術(shù)［1］也隨之成為當(dāng)下最熱門的研究方向。其中，路徑規(guī)劃［2-3］作為無人駕駛的關(guān)鍵技術(shù)之一，引起了行業(yè)的廣泛關(guān)注。路徑規(guī)劃是無人駕駛車輛從行駛空間中獲取一條滿足自車約束條件的無碰撞可行路徑，而安全性，平穩(wěn)性，高效性則是衡量無人駕駛車輛已規(guī)劃路徑是否最優(yōu)的重要指標(biāo)。

在基于采樣的規(guī)劃算法中，快速擴(kuò)展隨機(jī)樹（RRT）因其算法具有概率完備，適用性強(qiáng)等擴(kuò)展特性而被廣泛的研究和應(yīng)用。然而基礎(chǔ)RRT算法也存在一些明顯的不足之處：路徑搜索過程中節(jié)點(diǎn)盲目擴(kuò)展，無指向性，再現(xiàn)性差，路徑曲折波動(dòng)性大，收斂速度不穩(wěn)定，通過狹窄區(qū)域時(shí)，搜索效率降低。目前，已有一些國(guó)內(nèi)外學(xué)者提出了針對(duì)性的衍生算法［4-5］來解決上述問題。文獻(xiàn)［6］提出了RRT*算法，實(shí)現(xiàn)了采樣空間的約束，解決了路徑非概率最優(yōu)的問題；文獻(xiàn)［7］提出了B-RRT*算法，采用起始點(diǎn)和目標(biāo)點(diǎn)的雙向擴(kuò)展的策略，進(jìn)一步提高了算法的收斂速度；文獻(xiàn)［8］提出了基于連續(xù)曲率的RRT算法，引入自車和環(huán)境約束，有效提高了路徑質(zhì)量；文獻(xiàn)［9］提出了基于采樣點(diǎn)高斯分布的RRT算法，建立期望路徑模型，降低算法隨機(jī)性；文獻(xiàn)［10］提出了基于引導(dǎo)域的改進(jìn)算法，使RRT算法的擴(kuò)展過程在A*算法生成引導(dǎo)域中進(jìn)行，減少RRT算法的盲目性；文獻(xiàn)［11］提出了PI-RRT*算法利用人工勢(shì)場(chǎng)法結(jié)合啟發(fā)集合采樣的最優(yōu)路徑收斂方法，提高了最優(yōu)路徑生成速度。

上述提及的算法在路徑質(zhì)量，搜索效率等方面實(shí)現(xiàn)了優(yōu)化，但經(jīng)上述優(yōu)化后得到的路徑并不適用于無人駕駛車輛追蹤行駛。為使規(guī)劃后的路徑符合實(shí)際駕駛工況，滿足車輛行駛要求，在傳統(tǒng)RRT算法的基礎(chǔ)上提出一種改進(jìn)算法，以啟發(fā)式采樣替代隨機(jī)采樣策略，引導(dǎo)路徑向目標(biāo)點(diǎn)方向擴(kuò)展，設(shè)置轉(zhuǎn)角閾值約束轉(zhuǎn)角范圍，減少無效節(jié)點(diǎn)的生成，同時(shí)結(jié)合自適應(yīng)權(quán)重分配的局部擴(kuò)展方式，避免陷入局部最小值，然后以變步長(zhǎng)的擴(kuò)展方式，提高算法局部避障繞障能力，最后提出節(jié)點(diǎn)優(yōu)化策略，并基于3次B樣條曲線和Reeds-Shepp曲線進(jìn)行路徑后處理，實(shí)現(xiàn)路徑長(zhǎng)度優(yōu)化并解決了路徑平滑和終點(diǎn)航向問題。

2 車輛運(yùn)動(dòng)學(xué)模型

車輛運(yùn)動(dòng)學(xué)是從幾何學(xué)的角度展開車輛運(yùn)動(dòng)的研究，包括行駛空間中的速度和位置等變量隨著時(shí)間而產(chǎn)生的相應(yīng)的變化。在路徑規(guī)劃算法中考慮車輛的運(yùn)動(dòng)學(xué)模型，可以使得規(guī)劃得到的路徑可行性更高，并且能夠保證行駛過程中自車的運(yùn)動(dòng)學(xué)約束，這里采用的車輛運(yùn)動(dòng)學(xué)模型是單車模型，如圖1所示。單車模型是基于阿克曼轉(zhuǎn)向模型的簡(jiǎn)化，將左右兩側(cè)車輪簡(jiǎn)化合并為一個(gè)，只考慮車輛在慣性坐標(biāo)系XOY下運(yùn)動(dòng)，可得簡(jiǎn)化的車輛運(yùn)動(dòng)學(xué)方程如下：

圖1 運(yùn)動(dòng)學(xué)模型Fig.1 Kinematic Model

式中：(X，Y)—后軸軸心坐標(biāo)為(X，Y)；v—后軸軸心速度；φ—車輛橫擺角；δ—車輛前輪等效轉(zhuǎn)角；l—軸距。

根據(jù)前輪等效轉(zhuǎn)角與后軸遵循的曲率半徑之間的幾何關(guān)系，可得下式：

式中：R—后軸曲率半徑。曲率的定義為：

實(shí)際上，車輛轉(zhuǎn)向過程是曲率連續(xù)的弧線，且有最大曲率的約束。由式（3）可知，車輛的轉(zhuǎn)角與路徑曲率相關(guān)，這為下文改進(jìn)算法提供優(yōu)化思路：通過車輛相關(guān)參數(shù)，車輛的轉(zhuǎn)角與路徑曲率相關(guān)，引入轉(zhuǎn)角約束優(yōu)化路徑曲率，實(shí)現(xiàn)路徑曲率最優(yōu)，達(dá)到將車輛運(yùn)動(dòng)學(xué)與規(guī)劃算法結(jié)合的目的，讓規(guī)劃后的路徑更易于車輛追蹤，提高可行性。

3 RRT算法及其衍生算法介紹

3.1 RRT算法

基礎(chǔ)RRT算法的原理，如圖2所示。將起始點(diǎn)作為根節(jié)點(diǎn)，在所在環(huán)境區(qū)域內(nèi)生成一個(gè)隨機(jī)點(diǎn)，然后尋找隨機(jī)樹中與該隨機(jī)點(diǎn)距離最近的樹節(jié)點(diǎn)作為父節(jié)點(diǎn)，沿著父節(jié)點(diǎn)到隨機(jī)點(diǎn)的方向擴(kuò)展單位步長(zhǎng)形成子節(jié)點(diǎn)，為搜索的新節(jié)點(diǎn)。擴(kuò)展過程無障礙碰撞，則此次擴(kuò)展成功，子節(jié)點(diǎn)加入隨機(jī)樹集合中；若發(fā)生碰撞，則擴(kuò)展失敗，重新生成新的隨機(jī)點(diǎn)。擴(kuò)展成功后，判斷子節(jié)點(diǎn)是否進(jìn)入目標(biāo)點(diǎn)區(qū)域，若未達(dá)到重復(fù)上述搜索過程直至尋找到目標(biāo)點(diǎn)。

圖2 基礎(chǔ)RRT算法擴(kuò)展示意圖Fig.2 Extension of the Basic RRT

3.2 RRT＊算法

RRT*算法在RRT算法基礎(chǔ)上，增加了父節(jié)點(diǎn)重選和隨機(jī)樹重新布線兩個(gè)計(jì)算過程。每次迭代生成新節(jié)點(diǎn)后，在以新節(jié)點(diǎn)為中心，定義長(zhǎng)度為半徑的圓形范圍內(nèi)搜索的臨界節(jié)點(diǎn)，作為潛在的父節(jié)點(diǎn)，計(jì)算起始點(diǎn)經(jīng)過臨近點(diǎn)到新節(jié)點(diǎn)的代價(jià)，若出現(xiàn)比當(dāng)前路徑代價(jià)更低的新路徑，則進(jìn)行父節(jié)點(diǎn)替換，刪除原路徑，完成父節(jié)點(diǎn)的重選過程。接著，進(jìn)行隨機(jī)樹重新布線過程，檢查臨近點(diǎn)，若存在起始點(diǎn)經(jīng)過新節(jié)點(diǎn)再到臨近節(jié)點(diǎn)的代價(jià)小于其現(xiàn)存路徑的代價(jià)，則該臨近點(diǎn)將以新節(jié)點(diǎn)為父節(jié)點(diǎn)，進(jìn)行路徑更換。具體算法流程如下：

RRT*算法的核心思想是減少路徑代價(jià)，相比于基礎(chǔ)RRT算法，具備了漸進(jìn)優(yōu)化的特性，即隨迭代次數(shù)的增加，路徑質(zhì)量逐步優(yōu)化，達(dá)到最優(yōu)收斂的目的。

3.3 Informed RRT＊算法

Informed RRT*算法［12］是對(duì)RRT*算法的采樣策略進(jìn)行改進(jìn)。RRT*算法沿用RRT算法的隨機(jī)采樣策略，在環(huán)境空間內(nèi)均勻采樣，Informed RRT*算法則是先得到一個(gè)初始路徑，根據(jù)初始路徑將整個(gè)采樣區(qū)域收縮成橢圓區(qū)域，并且隨著節(jié)點(diǎn)不斷地?cái)U(kuò)展，橢圓的區(qū)域不斷縮小，以限制采樣區(qū)域的方式提高隨機(jī)樹的擴(kuò)展效率。橢圓采樣區(qū)域C定義是在環(huán)境空間為Qfree下，以起始點(diǎn)qinit和目標(biāo)點(diǎn)qgoal作為橢圓的兩個(gè)焦點(diǎn)，初始路徑為σbest，起始點(diǎn)與目標(biāo)點(diǎn)距離為σmin：

在每次迭代優(yōu)化路徑后，都會(huì)出現(xiàn)一條新路徑，若新路徑的長(zhǎng)度更短，則以該路徑長(zhǎng)度作為新的σbest，更新橢圓采樣區(qū)域，過程如圖3所示。

圖3 橢圓采樣區(qū)域更新Fig.3 Update of Ellipse Sampling Area

Informed RRT*算法是對(duì)已得路徑的優(yōu)化，將優(yōu)化范圍限制在橢圓區(qū)域內(nèi)，從而減少隨機(jī)樹冗余的分支，提高路徑質(zhì)量，實(shí)現(xiàn)路徑長(zhǎng)度最優(yōu)。

4 算法改進(jìn)策略

4.1 障礙檢測(cè)策略

由于實(shí)際駕駛環(huán)境中會(huì)存在各種特殊的場(chǎng)景如狹小空間，多障礙物場(chǎng)景等，隨機(jī)采樣點(diǎn)落在障礙物附近，會(huì)引領(lǐng)無人車向障礙物方向行駛，這是具有巨大危險(xiǎn)隱患的，僅僅通過基礎(chǔ)RRT算法的避障特性是遠(yuǎn)遠(yuǎn)不夠的，因此需要更為嚴(yán)謹(jǐn)?shù)恼系K物檢測(cè)策略，提高路徑可行性。

基礎(chǔ)算法將采樣節(jié)點(diǎn)視為質(zhì)點(diǎn)，并未考慮車輛本身的大小，但在實(shí)際駕駛過程中障礙物及車輛均為具有幾何參數(shù)的形體，若不加以考慮，車輛在跟蹤路徑時(shí)有與障礙物碰撞的風(fēng)險(xiǎn)。因此為了滿足避障要求，如圖4所示。對(duì)節(jié)點(diǎn)進(jìn)行膨脹處理，并構(gòu)建障礙物檢測(cè)圓。

圖4 節(jié)點(diǎn)膨脹處理Fig.4 Expansion of Node

節(jié)點(diǎn)膨脹定義為，以無人車中心(x0)，y0為圓心，R0為半徑進(jìn)行膨脹，得到方程為：

式中：R0—車身長(zhǎng)度L的一半。

障礙物檢測(cè)圓的定義是，將膨脹后的圓形區(qū)域進(jìn)行以一定的比例系數(shù)λ進(jìn)行檢測(cè)區(qū)域放大：

改進(jìn)算法分別以λ=4，λ=8構(gòu)建兩類障礙物檢測(cè)圓，引入此策略能夠?qū)崿F(xiàn)大范圍的障礙物檢測(cè)，并保證車輛在足夠充裕的空間內(nèi)進(jìn)行有效避障。

4.2 啟發(fā)式采樣策略

4.2.1 基于權(quán)重分配的目標(biāo)指向性采樣

基礎(chǔ)RRT算法是以環(huán)境地圖隨機(jī)采樣的方式，在隨機(jī)點(diǎn)會(huì)出現(xiàn)盲目擴(kuò)展的情況，會(huì)導(dǎo)致無效節(jié)點(diǎn)的生成，無法進(jìn)行有目的性的搜索擴(kuò)展，忽視局部路徑細(xì)節(jié)變化，導(dǎo)致無效規(guī)劃時(shí)間增加，這大大降低算法的搜索效率。引入目標(biāo)指向性采樣策略實(shí)現(xiàn)采樣過程的優(yōu)化，目標(biāo)指向性采樣策略定義為：

算法初始化的時(shí)候預(yù)設(shè)一個(gè)指向概率數(shù)P0，然后在每個(gè)迭代周期生成隨機(jī)點(diǎn)之后，生成一個(gè)隨機(jī)概率數(shù)Prand，然后進(jìn)行概率數(shù)大小判別，若Prand>P0，目標(biāo)點(diǎn)為此次擴(kuò)展的隨機(jī)點(diǎn)；若Prand≤P0，隨機(jī)點(diǎn)保持不變。

雖然引入指向性的采樣方式，給節(jié)點(diǎn)擴(kuò)展提供一個(gè)指引方向，減少隨機(jī)樹向其他方向無效擴(kuò)展，但由于改變的采樣概率分布，無法保證避障特性和指向特性同時(shí)處于可控范圍之內(nèi)，易出現(xiàn)陷入局部最小值，無法收斂的情況。解決思路為借助上文障礙檢測(cè)函數(shù)，進(jìn)行障礙物檢測(cè)，若障礙物檢測(cè)圓中無障礙物執(zhí)行原有指向性采樣策略；若出現(xiàn)障礙物，則采用權(quán)重分配的目標(biāo)指向性采樣，如圖5所示。

圖5 權(quán)重分配策略Fig.5 Method for Weight Distribution

基于權(quán)重分配的目標(biāo)指向性采樣，當(dāng)Prand≤P0時(shí)，qnew可表示為：

當(dāng)Prand>P0時(shí)，qnew表示為：

式中：μ—權(quán)重分配系數(shù)；ρ—擴(kuò)展步長(zhǎng)。

4.2.2 變步長(zhǎng)策略

基礎(chǔ)RRT算法中采用固定步長(zhǎng)進(jìn)行新節(jié)點(diǎn)的擴(kuò)展，當(dāng)通過狹窄區(qū)域和多障礙物環(huán)境時(shí)，其避障繞障能力下降，搜索效率大幅度下降。這里通過變步長(zhǎng)策略，結(jié)合障礙檢測(cè)策略，獲悉障礙物與路徑相對(duì)位置，執(zhí)行相應(yīng)的步長(zhǎng)進(jìn)行節(jié)點(diǎn)擴(kuò)展，充分利用地圖信息，提高算法局部避障能力，變步長(zhǎng)依據(jù)與障礙物相對(duì)位置將步長(zhǎng)分為大，中，小三類步長(zhǎng)，當(dāng)一類障礙檢測(cè)圓中無障礙物，無人車與障礙物距離較遠(yuǎn)，采用大步長(zhǎng)進(jìn)行擴(kuò)展；當(dāng)障礙物處于兩類障礙圓之間時(shí)，采用中步長(zhǎng)進(jìn)行擴(kuò)展；當(dāng)父節(jié)點(diǎn)與障礙物距離小于二類障礙物檢測(cè)圓半徑時(shí)，采用小步長(zhǎng)進(jìn)行擴(kuò)展：

式中：d—父節(jié)點(diǎn)與障礙物的最短距離；d1—第一類障礙物檢測(cè)圓半徑；d2—第二類障礙物檢測(cè)圓半徑。其中，擴(kuò)展步長(zhǎng)與障礙物始終保有一定的安全距離ds≥2R0。

4.2.3 節(jié)點(diǎn)轉(zhuǎn)角閾值

正常情況下車輛在行駛過程中，由于車輛自身的非完整性約束，其轉(zhuǎn)向角度在相對(duì)較小的范圍內(nèi)平穩(wěn)變化，不會(huì)出現(xiàn)過大轉(zhuǎn)角的情況，基礎(chǔ)RRT算法在節(jié)點(diǎn)隨機(jī)擴(kuò)展的過程沒有考慮無人車的自身非完整性約束，故設(shè)定節(jié)點(diǎn)轉(zhuǎn)角閾值，限制轉(zhuǎn)角范圍，更適用于無人車輛行駛。具體選取策略，如圖6所示。

圖6 節(jié)點(diǎn)轉(zhuǎn)角閾值選取策略Fig.6 Node Corner Threshold Selection Strategy

由圖6 可知，隨機(jī)點(diǎn)Node1 距離Node3 距離最近，但其超過了節(jié)點(diǎn)轉(zhuǎn)角閾值，這里并不將隨機(jī)點(diǎn)舍棄，生成新節(jié)點(diǎn)，而是采用舍棄最近節(jié)點(diǎn)，重新進(jìn)行最短距離判斷，選擇距離稍遠(yuǎn)一些，但在轉(zhuǎn)角閾值范圍的節(jié)點(diǎn)Node2 作為次近點(diǎn)，這樣會(huì)避免由于節(jié)點(diǎn)轉(zhuǎn)角閾值的約束舍棄節(jié)點(diǎn)，導(dǎo)致隨機(jī)采樣數(shù)的增加的情況，提高算法效率。

4.2.4 父節(jié)點(diǎn)重置

改進(jìn)算法在RRT*算法父節(jié)點(diǎn)重選過程的基礎(chǔ)上進(jìn)一步改進(jìn)，在增加新節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離代價(jià)值，并引入調(diào)節(jié)系數(shù)進(jìn)行臨近=節(jié)點(diǎn)總代價(jià)的計(jì)算。總代價(jià)的計(jì)算公式為：

式中：Ct—總代價(jià)；ω1，ω2，ω3—調(diào)節(jié)系數(shù)；Cin—起始點(diǎn)到臨近節(jié)點(diǎn)的代價(jià)值；Lnn—各臨近節(jié)點(diǎn)到新節(jié)點(diǎn)的歐氏距離；Cng—新節(jié)點(diǎn)到目標(biāo)點(diǎn)的預(yù)估代價(jià)值。

將改進(jìn)后總代價(jià)最低的臨界節(jié)點(diǎn)作為替換的父節(jié)點(diǎn)，實(shí)現(xiàn)啟發(fā)式的節(jié)點(diǎn)擴(kuò)展過程，提高路徑質(zhì)量，減少收斂時(shí)間。

4.3 后處理

4.3.1 節(jié)點(diǎn)優(yōu)化策略

改進(jìn)算法引入啟發(fā)式采樣策略后，搜索效率得到了明顯的提升，但搜索得到的路徑仍會(huì)出現(xiàn)無效的分支路徑，局部范圍內(nèi)出現(xiàn)路徑波動(dòng)，造成路線冗余，出現(xiàn)繞遠(yuǎn)的情況，使得路徑長(zhǎng)度并未實(shí)現(xiàn)最優(yōu)，因此需要對(duì)路徑進(jìn)行節(jié)點(diǎn)優(yōu)化，去除多余節(jié)點(diǎn)，優(yōu)化路徑長(zhǎng)度。核心策略為：引用貪婪算法的思想，對(duì)原有路徑的節(jié)點(diǎn)進(jìn)行擇優(yōu)判定，首先將起始點(diǎn)qinit加入新路徑節(jié)點(diǎn)集合中，并將qinit作為判定起點(diǎn)，判斷與起始點(diǎn)qinit連線上是否存在障礙，若無障礙則進(jìn)行下一節(jié)點(diǎn)的判定，直至出現(xiàn)節(jié)點(diǎn)qi與起始點(diǎn)qinit連線存在障礙，將qi加入新路徑節(jié)點(diǎn)集合，并作為新的起點(diǎn)重復(fù)上述擇優(yōu)判定過程，直至遍歷全部原有節(jié)點(diǎn)，最終得到新路徑節(jié)點(diǎn)集合，即為節(jié)點(diǎn)優(yōu)化后的新路徑，將冗余節(jié)點(diǎn)去除，實(shí)現(xiàn)路徑長(zhǎng)度的優(yōu)化。

4.3.2 路徑平滑處理

經(jīng)節(jié)點(diǎn)優(yōu)化后的路徑，實(shí)現(xiàn)路徑長(zhǎng)度最優(yōu)的目標(biāo)，但得到的路徑仍為連續(xù)折線，不夠平滑，曲率并不連續(xù)，車輛在跟蹤該路徑時(shí)，航向角會(huì)發(fā)生突變，嚴(yán)重影響車輛平順性和安全性，故對(duì)路徑進(jìn)行平滑處理，才能得到真正滿足車輛跟蹤要求的最佳路徑。這里采用三次B樣條曲線對(duì)路徑進(jìn)行平滑性擬合處理，如圖7所示。

圖7 三次B樣條曲線Fig.7 Cubic B-Spline Fitting

三次B樣條曲線的階次決定了平滑后曲線具備三階次連續(xù)性，三階連續(xù)的平滑路徑貼合真實(shí)駕駛場(chǎng)景，進(jìn)一步滿足車輛追蹤。B樣條的表達(dá)式為：

式中：Pi+k(i=0，1，...，m，k=0，1，...，n)—控制點(diǎn)；n—基函數(shù)的次數(shù)；Fk，n(t)—n次B 樣條基函數(shù)，也稱為樣條分段混合函數(shù)，定義為：

應(yīng)用上述公式，控制點(diǎn)由RRT算法中的路徑節(jié)點(diǎn)代替，設(shè)定三次B樣條曲線公式：

三次B樣條的擬合曲線為：

4.3.3 目標(biāo)點(diǎn)航向約束

對(duì)于無人駕駛車輛而言，目標(biāo)點(diǎn)通常為工作區(qū)域，停車位等場(chǎng)景，存在倒車等工況，故對(duì)車輛在抵達(dá)目標(biāo)點(diǎn)時(shí)的航向有一定的要求，為了使規(guī)劃的路徑能夠結(jié)合實(shí)際停車位姿的航向信息，對(duì)目標(biāo)點(diǎn)區(qū)域進(jìn)行航向約束。改進(jìn)算法采用Reeds-Shepp曲線進(jìn)行路徑末端的節(jié)點(diǎn)連接，解決目標(biāo)點(diǎn)航向問題。

Reeds-Shepp 曲線同時(shí)滿足起始位姿和終止位姿的最短路徑?？紤]車輛倒車的情況，可實(shí)現(xiàn)任意位姿抵達(dá)目標(biāo)點(diǎn)，保證無人駕駛車輛抵達(dá)終點(diǎn)時(shí)最終的航向角度，并且曲線由半徑固定的圓弧和直線組合的特性也能夠滿足車輛最小轉(zhuǎn)彎半徑的要求。Reeds-Shepp 曲線可以描述的路徑類型分為9 種：C|C|C，CC|C，C|CC，CSC，CCβ|CβC，C|CβCβ|C，C|Cπ2SC，CSCπ2|C，C|Cπ2SCπ2|C。其中，C表示轉(zhuǎn)向，S表示直行，“|”表示正反向的轉(zhuǎn)換，下角標(biāo)表示轉(zhuǎn)向角度。

目標(biāo)點(diǎn)航向約束是在目標(biāo)點(diǎn)為圓心，ε為半徑構(gòu)建圓形區(qū)域，獲得路徑與區(qū)域交點(diǎn)qi的位姿信(xs，ys，φs)，根據(jù)實(shí)際情況給定目標(biāo)位姿信息，利用Reeds-Shepp曲線進(jìn)行交點(diǎn)與目標(biāo)點(diǎn)的連接，實(shí)現(xiàn)車輛的最終位姿要求，并兼具曲率的連續(xù)性。

5 仿真分析

為了驗(yàn)證改進(jìn)算法的優(yōu)越性能，分別對(duì)基礎(chǔ)RRT 算法，RRT*算法，Informed RRT*算法及改進(jìn)算法四種算法分別在簡(jiǎn)單障礙場(chǎng)景，狹窄通道場(chǎng)景，復(fù)雜障礙場(chǎng)景等環(huán)境下進(jìn)行仿真驗(yàn)證，對(duì)所得規(guī)劃路徑進(jìn)行對(duì)比分析，三種場(chǎng)景目標(biāo)點(diǎn)航向角分別設(shè)置為π/2，π，3π/4，以驗(yàn)證目標(biāo)點(diǎn)航向約束的普遍適用性，不同算法中的主要參數(shù)與仿真場(chǎng)景均保持一致。改進(jìn)算法仿真相關(guān)參數(shù)，如表1所示。仿真結(jié)果對(duì)比，如圖8～圖10所示。

表1 仿真所用主要參數(shù)Tab.1 The Main Parameters of Simulation Used

圖8 簡(jiǎn)單障礙場(chǎng)景中仿真結(jié)果對(duì)比Fig.8 Comparison of Simulation Results in Simple Obstacle Scenario

圖9 狹窄通道場(chǎng)景中仿真結(jié)果對(duì)比Fig.9 Comparison of Simulation Results in Narrow Channel Scenario

圖10 復(fù)雜障礙場(chǎng)景中仿真結(jié)果對(duì)比Fig.10 Comparison of Simulation Results in Complex Obstacle Scenario

由圖8～圖10仿真結(jié)果進(jìn)行對(duì)比可知算法的改進(jìn)之處：冗余節(jié)點(diǎn)大量減少，避障繞障能力得到提升，通過狹窄區(qū)域能力提升，實(shí)現(xiàn)環(huán)境信息利用的最大化，大大提高了算法的規(guī)劃效率，得到路徑長(zhǎng)度實(shí)現(xiàn)最優(yōu)并兼具平滑性，符合車輛追蹤要求，并能解決目標(biāo)點(diǎn)的車輛航向問題，且具有普遍適用性。由于算法的隨機(jī)性，每種場(chǎng)景進(jìn)行100次路徑規(guī)劃仿真實(shí)例，對(duì)4種算法得到的仿真結(jié)果進(jìn)行統(tǒng)計(jì)，其中，各個(gè)指標(biāo)結(jié)果均為平均數(shù)值，仿真實(shí)例數(shù)據(jù)，如表2～表4所示。

表2 簡(jiǎn)單障礙場(chǎng)景仿真數(shù)據(jù)Tab.2 Simulation Data in Simple Obstacle Scenario

表3 狹窄通道場(chǎng)景仿真數(shù)據(jù)Tab.3 Simulation Data in Narrow Channel Scenario

表4 復(fù)雜障礙場(chǎng)景仿真數(shù)據(jù)Tab.4 Simulation Data in Complex Obstacle Scenario

根據(jù)表2～表4，將簡(jiǎn)單障礙物場(chǎng)景，狹窄通道場(chǎng)景，復(fù)雜障礙物場(chǎng)景三種場(chǎng)景下仿真實(shí)例數(shù)據(jù)綜合分析可知，改進(jìn)算法平均擴(kuò)展節(jié)點(diǎn)數(shù)為62個(gè)，相較于RRT及其衍生算法，平均擴(kuò)展節(jié)點(diǎn)總數(shù)分別減少了87.45%，81.71%，69.61%；改進(jìn)算法平均迭代次數(shù)為75，分別減少了89.91%，86.44%，82.67%；三種場(chǎng)景下改進(jìn)算法節(jié)點(diǎn)利用效率均實(shí)現(xiàn)了提升，分別是58%，41%，45%；平均規(guī)劃時(shí)間為5.83s，縮短了66.89%，61.34%，49.94%；平均路徑長(zhǎng)度為993.53mm，減少了43.49%，37.98%，31.81%。由此可知，改進(jìn)算法在各項(xiàng)指標(biāo)上都實(shí)現(xiàn)了明顯的提升。以簡(jiǎn)單障礙場(chǎng)景為例，提取四種算法所得路徑，其具體曲率變化情況，如圖11所示。圖中橫軸表示路徑當(dāng)前位置占總路徑的百分比（簡(jiǎn)稱為路徑百分比），縱軸為曲率的絕對(duì)值。通過對(duì)比可以發(fā)現(xiàn)，基礎(chǔ)RRT算法及其衍生算法所得路徑的曲率突變情況明顯，改進(jìn)后的算法所得路徑曲率連續(xù)且平滑，且曲率最大值為0.047，滿足車輛最大曲率要求，從而保證了車輛追蹤規(guī)劃路徑過程時(shí)的平穩(wěn)性和安全性，相比之下，驗(yàn)證了改進(jìn)算法規(guī)劃路徑平滑程度的優(yōu)越性。

圖11 曲率變化Fig.11 Change of Curvature

6 結(jié)論

（1）在RRT 算法的基礎(chǔ)上，設(shè)計(jì)了啟發(fā)式的采樣策略，包括基于權(quán)重分配的指向性采樣，變步長(zhǎng)擴(kuò)展，轉(zhuǎn)角閾值約束等策略，并對(duì)規(guī)劃路徑進(jìn)行后處理，分別采用節(jié)點(diǎn)優(yōu)化策略，三次B樣條曲線進(jìn)行平滑處理，Reed-Shepp曲線進(jìn)行目標(biāo)點(diǎn)的連接，有效地解決了路徑長(zhǎng)度，平滑性及目標(biāo)點(diǎn)航向問題。改進(jìn)后的算法大幅度提高了搜索效率，總節(jié)點(diǎn)數(shù)減少了約80%，規(guī)劃時(shí)間縮短了約60%，路徑長(zhǎng)度減少了約40%，滿足平滑性要求及車輛最大曲率要求全方面提高了路徑質(zhì)量。（2）利用Matlab軟件進(jìn)行三種工況下的仿真實(shí)例驗(yàn)證，分析了仿真數(shù)據(jù)，驗(yàn)證了改進(jìn)后算法的有效性和優(yōu)越性，具備普遍性和準(zhǔn)確性。