改進(jìn)RRT算法的無(wú)人駕駛車(chē)輛路徑規(guī)劃研究

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

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

1 引言

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

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

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

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

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

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

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

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

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

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

3.1 RRT算法

基礎(chǔ)RRT算法的原理，如圖2所示。將起始點(diǎn)作為根節(jié)點(diǎn)，在所在環(huán)境區(qū)域內(nèi)生成一個(gè)隨機(jī)點(diǎn)，然后尋找隨機(jī)樹(shù)中與該隨機(jī)點(diǎn)距離最近的樹(shù)節(jié)點(diǎn)作為父節(jié)點(diǎn)，沿著父節(jié)點(diǎn)到隨機(jī)點(diǎn)的方向擴(kuò)展單位步長(zhǎng)形成子節(jié)點(diǎn)，為搜索的新節(jié)點(diǎn)。擴(kuò)展過(guò)程無(wú)障礙碰撞，則此次擴(kuò)展成功，子節(jié)點(diǎn)加入隨機(jī)樹(shù)集合中；若發(fā)生碰撞，則擴(kuò)展失敗，重新生成新的隨機(jī)點(diǎn)。擴(kuò)展成功后，判斷子節(jié)點(diǎn)是否進(jìn)入目標(biāo)點(diǎn)區(qū)域，若未達(dá)到重復(fù)上述搜索過(guò)程直至尋找到目標(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ī)樹(shù)重新布線兩個(gè)計(jì)算過(guò)程。每次迭代生成新節(jié)點(diǎn)后，在以新節(jié)點(diǎn)為中心，定義長(zhǎng)度為半徑的圓形范圍內(nèi)搜索的臨界節(jié)點(diǎn)，作為潛在的父節(jié)點(diǎn)，計(jì)算起始點(diǎn)經(jīng)過(guò)臨近點(diǎn)到新節(jié)點(diǎn)的代價(jià)，若出現(xiàn)比當(dāng)前路徑代價(jià)更低的新路徑，則進(jìn)行父節(jié)點(diǎn)替換，刪除原路徑，完成父節(jié)點(diǎn)的重選過(guò)程。接著，進(jìn)行隨機(jī)樹(shù)重新布線過(guò)程，檢查臨近點(diǎn)，若存在起始點(diǎn)經(jīng)過(guò)新節(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ī)樹(shù)的擴(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ū)域，過(guò)程如圖3所示。

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

Informed RRT*算法是對(duì)已得路徑的優(yōu)化，將優(yōu)化范圍限制在橢圓區(qū)域內(nèi)，從而減少隨機(jī)樹(shù)冗余的分支，提高路徑質(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)無(wú)人車(chē)向障礙物方向行駛，這是具有巨大危險(xiǎn)隱患的，僅僅通過(guò)基礎(chǔ)RRT算法的避障特性是遠(yuǎn)遠(yuǎn)不夠的，因此需要更為嚴(yán)謹(jǐn)?shù)恼系K物檢測(cè)策略，提高路徑可行性。

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

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

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

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

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

改進(jìn)算法分別以λ=4，λ=8構(gòu)建兩類(lèi)障礙物檢測(cè)圓，引入此策略能夠?qū)崿F(xiàn)大范圍的障礙物檢測(cè)，并保證車(chē)輛在足夠充裕的空間內(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)致無(wú)效節(jié)點(diǎn)的生成，無(wú)法進(jìn)行有目的性的搜索擴(kuò)展，忽視局部路徑細(xì)節(jié)變化，導(dǎo)致無(wú)效規(guī)劃時(shí)間增加，這大大降低算法的搜索效率。引入目標(biāo)指向性采樣策略實(shí)現(xiàn)采樣過(guò)程的優(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ī)樹(shù)向其他方向無(wú)效擴(kuò)展，但由于改變的采樣概率分布，無(wú)法保證避障特性和指向特性同時(shí)處于可控范圍之內(nèi)，易出現(xiàn)陷入局部最小值，無(wú)法收斂的情況。解決思路為借助上文障礙檢測(cè)函數(shù)，進(jìn)行障礙物檢測(cè)，若障礙物檢測(cè)圓中無(wú)障礙物執(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)通過(guò)狹窄區(qū)域和多障礙物環(huán)境時(shí)，其避障繞障能力下降，搜索效率大幅度下降。這里通過(guò)變步長(zhǎng)策略，結(jié)合障礙檢測(cè)策略，獲悉障礙物與路徑相對(duì)位置，執(zhí)行相應(yīng)的步長(zhǎng)進(jìn)行節(jié)點(diǎn)擴(kuò)展，充分利用地圖信息，提高算法局部避障能力，變步長(zhǎng)依據(jù)與障礙物相對(duì)位置將步長(zhǎng)分為大，中，小三類(lèi)步長(zhǎng)，當(dāng)一類(lèi)障礙檢測(cè)圓中無(wú)障礙物，無(wú)人車(chē)與障礙物距離較遠(yuǎn)，采用大步長(zhǎng)進(jìn)行擴(kuò)展；當(dāng)障礙物處于兩類(lèi)障礙圓之間時(shí)，采用中步長(zhǎng)進(jìn)行擴(kuò)展；當(dāng)父節(jié)點(diǎn)與障礙物距離小于二類(lèi)障礙物檢測(cè)圓半徑時(shí)，采用小步長(zhǎng)進(jìn)行擴(kuò)展：

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

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

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

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

由圖6 可知，隨機(jī)點(diǎn)Node1 距離Node3 距離最近，但其超過(guò)了節(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)重選過(guò)程的基礎(chǔ)上進(jìn)一步改進(jìn)，在增加新節(jié)點(diǎn)與目標(biāo)點(diǎn)的距離代價(jià)值，并引入調(diào)節(jié)系數(shù)進(jìn)行臨近=節(jié)點(diǎn)總代價(jià)的計(jì)算?？偞鷥r(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ò)展過(guò)程，提高路徑質(zhì)量，減少收斂時(shí)間。

4.3 后處理

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

改進(jìn)算法引入啟發(fā)式采樣策略后，搜索效率得到了明顯的提升，但搜索得到的路徑仍會(huì)出現(xiàn)無(wú)效的分支路徑，局部范圍內(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連線上是否存在障礙，若無(wú)障礙則進(jìn)行下一節(jié)點(diǎn)的判定，直至出現(xiàn)節(jié)點(diǎn)qi與起始點(diǎn)qinit連線存在障礙，將qi加入新路徑節(jié)點(diǎn)集合，并作為新的起點(diǎn)重復(fù)上述擇優(yōu)判定過(guò)程，直至遍歷全部原有節(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ù)，車(chē)輛在跟蹤該路徑時(shí)，航向角會(huì)發(fā)生突變，嚴(yán)重影響車(chē)輛平順性和安全性，故對(duì)路徑進(jìn)行平滑處理，才能得到真正滿(mǎn)足車(chē)輛跟蹤要求的最佳路徑。這里采用三次B樣條曲線對(duì)路徑進(jìn)行平滑性擬合處理，如圖7所示。

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

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

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

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

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

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

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

Reeds-Shepp 曲線同時(shí)滿(mǎn)足起始位姿和終止位姿的最短路徑?？紤]車(chē)輛倒車(chē)的情況，可實(shí)現(xiàn)任意位姿抵達(dá)目標(biāo)點(diǎn)，保證無(wú)人駕駛車(chē)輛抵達(dá)終點(diǎn)時(shí)最終的航向角度，并且曲線由半徑固定的圓弧和直線組合的特性也能夠滿(mǎn)足車(chē)輛最小轉(zhuǎn)彎半徑的要求。Reeds-Shepp 曲線可以描述的路徑類(lèi)型分為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)車(chē)輛的最終位姿要求，并兼具曲率的連續(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)大量減少，避障繞障能力得到提升，通過(guò)狹窄區(qū)域能力提升，實(shí)現(xiàn)環(huán)境信息利用的最大化，大大提高了算法的規(guī)劃效率，得到路徑長(zhǎng)度實(shí)現(xiàn)最優(yōu)并兼具平滑性，符合車(chē)輛追蹤要求，并能解決目標(biāo)點(diǎn)的車(chē)輛航向問(wè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)稱(chēng)為路徑百分比），縱軸為曲率的絕對(duì)值。通過(guò)對(duì)比可以發(fā)現(xiàn)，基礎(chǔ)RRT算法及其衍生算法所得路徑的曲率突變情況明顯，改進(jìn)后的算法所得路徑曲率連續(xù)且平滑，且曲率最大值為0.047，滿(mǎn)足車(chē)輛最大曲率要求，從而保證了車(chē)輛追蹤規(guī)劃路徑過(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)航向問(wèn)題。改進(jìn)后的算法大幅度提高了搜索效率，總節(jié)點(diǎn)數(shù)減少了約80%，規(guī)劃時(shí)間縮短了約60%，路徑長(zhǎng)度減少了約40%，滿(mǎn)足平滑性要求及車(chē)輛最大曲率要求全方面提高了路徑質(zhì)量。（2）利用Matlab軟件進(jìn)行三種工況下的仿真實(shí)例驗(yàn)證，分析了仿真數(shù)據(jù)，驗(yàn)證了改進(jìn)后算法的有效性和優(yōu)越性，具備普遍性和準(zhǔn)確性。