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

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

1 引言

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

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

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

2 車輛運動學(xué)模型

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

圖1 運動學(xué)模型Fig.1 Kinematic Model

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

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

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

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

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

3.1 RRT算法

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

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

3.2 RRT＊算法

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

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

3.3 Informed RRT＊算法

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

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

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

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

4 算法改進策略

4.1 障礙檢測策略

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

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

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

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

式中：R0—車身長度L的一半。

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

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

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

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

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

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

雖然引入指向性的采樣方式，給節(jié)點擴展提供一個指引方向，減少隨機樹向其他方向無效擴展，但由于改變的采樣概率分布，無法保證避障特性和指向特性同時處于可控范圍之內(nèi)，易出現(xiàn)陷入局部最小值，無法收斂的情況。解決思路為借助上文障礙檢測函數(shù)，進行障礙物檢測，若障礙物檢測圓中無障礙物執(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時，qnew可表示為：

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

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

4.2.2 變步長策略

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

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

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

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

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

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

4.2.4 父節(jié)點重置

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

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

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

4.3 后處理

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

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

4.3.2 路徑平滑處理

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

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

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

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

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

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

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

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

Reeds-Shepp 曲線同時滿足起始位姿和終止位姿的最短路徑。考慮車輛倒車的情況，可實現(xiàn)任意位姿抵達目標(biāo)點，保證無人駕駛車輛抵達終點時最終的航向角度，并且曲線由半徑固定的圓弧和直線組合的特性也能夠滿足車輛最小轉(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)點航向約束是在目標(biāo)點為圓心，ε為半徑構(gòu)建圓形區(qū)域，獲得路徑與區(qū)域交點qi的位姿信(xs，ys，φs)，根據(jù)實際情況給定目標(biāo)位姿信息，利用Reeds-Shepp曲線進行交點與目標(biāo)點的連接，實現(xiàn)車輛的最終位姿要求，并兼具曲率的連續(xù)性。

5 仿真分析

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

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

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

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

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

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

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

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

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

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

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

6 結(jié)論

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