朱 冰，韓嘉懿，趙 健，劉 帥，鄧偉文

（1.吉林大學(xué)，汽車仿真與控制國(guó)家重點(diǎn)實(shí)驗(yàn)室，長(zhǎng)春 130022； 2.北京航空航天大學(xué)交通科學(xué)與工程學(xué)院，北京 100083）

前言

路徑規(guī)劃是智能汽車最為重要的核心技術(shù)之一，可根據(jù)宏觀地圖信息找到一條從初始位姿到目標(biāo)位姿的連續(xù)無(wú)碰撞路徑［1］。在實(shí)際道路環(huán)境中，智能汽車路徑規(guī)劃策略還須滿足車輛動(dòng)力學(xué)約束，同時(shí)具備較高的計(jì)算效率，及時(shí)規(guī)劃出合理的期望路徑。

快速搜索隨機(jī)樹（rapidly-exploring random tree，RRT）算法是解決路徑規(guī)劃問(wèn)題的常用算法，它是一種基于均勻隨機(jī)采樣的增量式路徑規(guī)劃算法，具有概率完備性［2］。但RRT算法擴(kuò)張效率低、路徑抖動(dòng)明顯、易陷入局部區(qū)域，因此研究者提出了多種改進(jìn)方案。

Xue等［3］借鑒人工勢(shì)場(chǎng)思想，利用目標(biāo)點(diǎn)和隨機(jī)點(diǎn)作用于隨機(jī)樹上最近節(jié)點(diǎn)的“引力”矢量和來(lái)確定節(jié)點(diǎn)擴(kuò)展方向，同時(shí)加入雙向搜索策略，提高了算法的搜索效率。這類方法利用目標(biāo)位置信息減少計(jì)算資源的浪費(fèi)［4］，同時(shí)考慮了車輛的軌跡曲率約束。但是過(guò)度依賴目標(biāo)點(diǎn)信息可能導(dǎo)致隨機(jī)樹在擴(kuò)張過(guò)程中長(zhǎng)時(shí)間陷入局部陷阱。馮來(lái)春等［5］提出結(jié)合A*算法和目標(biāo)偏向策略引導(dǎo)隨機(jī)樹擴(kuò)張，以使隨機(jī)樹的擴(kuò)張更有指向性，從而提高搜索效率，但是需要額外建立柵格化地圖。

Karaman等［6］將價(jià)值最低的相鄰區(qū)域內(nèi)的樹節(jié)點(diǎn)作為父節(jié)點(diǎn)，在每次迭代過(guò)程中對(duì)該區(qū)域內(nèi)的樹節(jié)點(diǎn)重新連接，從而保證已生成路徑總是較優(yōu)的。Jeong等［7］提出的Q-RRT*算法在隨機(jī)樹新節(jié)點(diǎn)局部重構(gòu)的過(guò)程中選取多級(jí)父節(jié)點(diǎn)進(jìn)行比較，采用代價(jià)最低的節(jié)點(diǎn)作為新的父節(jié)點(diǎn)，從而使生成路徑的長(zhǎng)度更小。這些方法具有漸進(jìn)最優(yōu)性，但是計(jì)算量較大。

以上算法在避障處理時(shí)，大多采用判斷新生成的隨機(jī)樹連接是否與障礙物存在交點(diǎn)的方式。如果沒(méi)有交點(diǎn)意味不會(huì)發(fā)生碰撞，則接受該新節(jié)點(diǎn)并更新隨機(jī)數(shù)。反之則撤銷該連接。通過(guò)該方法可使隨機(jī)樹盡可能地在自由空間中探索，然而當(dāng)障礙分布較為簡(jiǎn)單時(shí)，該方法效率較低，若能提前發(fā)現(xiàn)隨機(jī)樹擴(kuò)張方向上的障礙物，然后據(jù)此規(guī)劃避障路徑，則能有效提高算法的計(jì)算效率［8］。為此，本文中提出一種利用安全場(chǎng)引導(dǎo)的改進(jìn)RRT*算法，將安全場(chǎng)強(qiáng)作為選擇節(jié)點(diǎn)的權(quán)重，使隨機(jī)樹的擴(kuò)展方向能以足夠大的概率指向遠(yuǎn)離障礙物的方向，提高收斂速度，減少計(jì)算量。

本文中首先根據(jù)實(shí)車駕駛數(shù)據(jù)建立了基于安全距離模型的安全場(chǎng)；在此基礎(chǔ)上，提出具備安全場(chǎng)引導(dǎo)、角度約束等策略的改進(jìn)RRT*算法；最后，通過(guò)離線仿真驗(yàn)證算法的有效性。

1 基于安全距離模型的安全場(chǎng)

1.1 安全場(chǎng)模型

駕駛?cè)笋{駛過(guò)程中避障意圖的觸發(fā)及減速跟車過(guò)程中的制動(dòng)操作，均與車輛間的相對(duì)行駛狀態(tài)以及駕駛?cè)肆?xí)性有關(guān)［9］，可引入“安全距離”對(duì)這一過(guò)程進(jìn)行表征。所謂“安全距離”指后車通過(guò)采取一定強(qiáng)度的制動(dòng)措施，使后車與前車等速并不發(fā)生碰撞所需要的初始相對(duì)距離［10］。對(duì)安全距離模型進(jìn)行擴(kuò)展，可得到能夠定量評(píng)價(jià)行駛安全性的安全場(chǎng)模型。

車輛制動(dòng)過(guò)程中減速度隨時(shí)間變化的關(guān)系如圖1所示［11］。

圖1 制動(dòng)過(guò)程車輛減速度變化曲線

圖中：tb1表示駕駛?cè)朔磻?yīng)時(shí)間，期間主車制動(dòng)減速度為0；tb2表示提高制動(dòng)減速度的過(guò)程，期間制動(dòng)減速度以斜率k線性升高直到設(shè)定的目標(biāo)制動(dòng)減速度abg；tb3表示制動(dòng)減速度不變的制動(dòng)維持過(guò)程，持續(xù)到與前車相對(duì)車速變?yōu)?為止。

為保證安全性，通常要求制動(dòng)結(jié)束后兩車之間要存有一定的安全距離裕度Sbs，可以得到主車開(kāi)始制動(dòng)時(shí)與前車之間應(yīng)有的制動(dòng)安全距離Sbw：

式中：Δv為開(kāi)始制動(dòng)時(shí)兩車間相對(duì)速度；k為駕駛?cè)苏Ｐ旭偁顟B(tài)下的平均制動(dòng)減速度變化率。k、Sbs和abg為模型參數(shù)，可通過(guò)實(shí)車駕駛數(shù)據(jù)分析提取得到。

據(jù)式（1），當(dāng)兩車相對(duì)距離下降到Sbw時(shí)開(kāi)始以目標(biāo)制動(dòng)減速度abg制動(dòng)，可實(shí)現(xiàn)前車留有距離裕度Sbs的避障狀態(tài)。在緊急制動(dòng)情況下，abg為接近路面附著條件允許的最大制動(dòng)減速度，而在非緊急情況下，駕駛?cè)送ǔＲ孕∮谧畲笾苿?dòng)減速度的減速度進(jìn)行制動(dòng)。

將式（1）中的abg設(shè)為變量，依據(jù)駕駛?cè)祟愋瓦x定k值，則對(duì)于任意的abg，利用式（1）總能根據(jù)相對(duì)速度Δv計(jì)算出一個(gè)對(duì)應(yīng)的臨界制動(dòng)安全距離Sbw。此時(shí)abg、Δv和Sbw3者的關(guān)系可用圖2表示。

由圖2可知，避撞制動(dòng)時(shí)機(jī)主要與Δv和abg有關(guān)。因此用abg來(lái)表征行駛安全性，即某一強(qiáng)度的目標(biāo)制動(dòng)減速度會(huì)讓駕駛員感到危機(jī)并造成速度損失，從而使其產(chǎn)生避障意圖。

圖2 安全距離模型

將式（1）中的Sbw和 Δv作為自變量，abg作為因變量，即得到安全場(chǎng)，如圖3所示。圖3反映了在一定的Δv和Sbw下主車避免碰撞所需的abg?？梢钥闯?，當(dāng)相對(duì)距離縮小和相對(duì)速度增大時(shí)，主車所需采取的制動(dòng)減速度增強(qiáng)，代表行駛安全性降低，該情況與實(shí)際駕駛經(jīng)驗(yàn)相符。

圖3 安全場(chǎng)三維視圖

為建立符合駕駛?cè)肆?xí)性、能反映車輛間行駛環(huán)境安全性的安全場(chǎng)，同時(shí)描述駕駛?cè)藢?duì)碰撞危險(xiǎn)的敏感和接受程度，采用實(shí)車試驗(yàn)的方法采集駕駛?cè)笋{駛數(shù)據(jù)，在此基礎(chǔ)上分析提取構(gòu)建安全場(chǎng)所需的k、Sbs和abg3個(gè)參數(shù)。

1.2 駕駛?cè)藬?shù)據(jù)采集試驗(yàn)

搭建如圖4所示的駕駛?cè)笋{駛數(shù)據(jù)采集平臺(tái)，并設(shè)計(jì)了如表1所示的跟車和換道兩種工況。其中vh0和vt0分別為主車和前車初始速度。跟車工況和換道工況可以反映出不同駕駛?cè)藢?duì)不同相對(duì)速度下安全距離的把握和減速制動(dòng)過(guò)程中所采用的制動(dòng)減速度。

平臺(tái)主體由主車和交通車組成，兩車各自搭載高精度慣性導(dǎo)航設(shè)備RT 3002／RT Range以及配套V2V和基站通信設(shè)備，連同車輛CAN總線數(shù)據(jù)可以獲得包括車輛位置、速度和加速度在內(nèi)的車輛狀態(tài)信息。

圖4 駕駛?cè)笋{駛數(shù)據(jù)采集平臺(tái)

表1 駕駛?cè)笋{駛數(shù)據(jù)采集試驗(yàn)工況

跟車工況中，前車依據(jù)速度歷程70→50→30 km／h行駛（在各階段維持勻速），主車按照自身駕駛習(xí)慣跟車行駛。換道工況中，在兩車產(chǎn)生一定距離后，主車以指定速度勻速追趕前車并在感知到前方低速車輛造成的潛在行駛危險(xiǎn)時(shí)按照自身駕駛習(xí)慣切換車道避障。

試驗(yàn)共采集了87名駕駛?cè)说臄?shù)據(jù)。根據(jù)跟車工況駕駛?cè)酥苿?dòng)過(guò)程中的主車速度變化可以獲取不同風(fēng)格駕駛?cè)肆?xí)慣的車輛減速度；而換道工況中駕駛?cè)藫Q道開(kāi)始時(shí)刻與前車的平均車間時(shí)距則能夠反映出不同駕駛?cè)藢?duì)危險(xiǎn)敏感程度的差異［12］。

根據(jù)實(shí)車試驗(yàn)數(shù)據(jù)進(jìn)行聚類得到了3類不同風(fēng)格駕駛?cè)说陌踩珗?chǎng)參數(shù)，實(shí)際應(yīng)用過(guò)程中可以根據(jù)駕駛?cè)笋{駛風(fēng)格選取對(duì)應(yīng)參數(shù)，如表2所示。

表2 安全場(chǎng)參數(shù)

2 基于安全場(chǎng)改進(jìn)RRT*算法

RRT算法［13］將初始點(diǎn)Pstart作為隨機(jī)樹T的父節(jié)點(diǎn)，然后通過(guò)迭代更新使新產(chǎn)生的節(jié)點(diǎn)Pnew與目標(biāo)點(diǎn)Pgoal足夠接近。迭代過(guò)程如下所述：在空間中隨機(jī)生成點(diǎn)Prand，根據(jù)價(jià)值函數(shù)計(jì)算與Prand連接的節(jié)點(diǎn)代價(jià)值，將該值最小的點(diǎn)Pnearest選為父節(jié)點(diǎn)；從Pnearest向Prand延展出步長(zhǎng)D獲得新節(jié)點(diǎn)Pnew；判斷Pnearest與Pnew的之間是否存在障礙物，若不存在，則在隨機(jī)樹上加入Pnew，反之則忽略Pnew，進(jìn)行下一次迭代。

RRT*算法相較于RRT算法加入了新節(jié)點(diǎn)的父節(jié)點(diǎn)重選策略以及鄰域內(nèi)局部樹節(jié)點(diǎn)的重構(gòu)策略，使所生成的路徑更加平滑，從而縮短路徑長(zhǎng)度［14］。RRT*算法具有概率完備性，無(wú)須事先對(duì)環(huán)境建立柵格地圖，且可擴(kuò)展性好，但該算法計(jì)算量大，盲目擴(kuò)張策略導(dǎo)致計(jì)算資源的浪費(fèi)，同時(shí)難以顧及終點(diǎn)的角度要求。因此本文中將安全場(chǎng)作為引導(dǎo)并加入角度約束策略對(duì)RRT*算法進(jìn)行改進(jìn)，使其能更好地應(yīng)用于智能汽車路徑規(guī)劃。

2.1 角度約束策略

針對(duì)汽車航向約束，提出了一種新的節(jié)點(diǎn)擴(kuò)展策略：設(shè)搜索空間中生成隨機(jī)點(diǎn)Prand最近的節(jié)點(diǎn)為Pnearest，其父節(jié)點(diǎn)為Pparent。利用歐氏距離檢索出距離Prand最近的點(diǎn)Pnearest，根據(jù)Pparent到Pnearest的矢量方向和范圍［-θmax，θmax］按照式（2）和式（3）生成一系列點(diǎn)Pcan，i作為備選點(diǎn)集，再以安全場(chǎng)作為引導(dǎo)（在2.2節(jié)中介紹）從中選取一個(gè)點(diǎn)作為新的樹節(jié)點(diǎn)。

式中：θi為點(diǎn)Pnearest到點(diǎn)Pcan，i的矢量與地圖橫軸的夾角；θnearest為點(diǎn)Pparent到點(diǎn)Pnearest的矢量與橫軸的夾角；θcan，i為在［-θmax，θmax］內(nèi)均布的第i個(gè)角度；θres為角分辨率；rand為0到1的隨機(jī)數(shù)；floor表示向下取整；（xcan，i，ycan，i）為點(diǎn)Pcan，i的坐標(biāo)；（xnearest，ynearest）為點(diǎn)Pnearest的坐標(biāo)；D為擴(kuò)展步長(zhǎng)。

與先產(chǎn)生樹節(jié)點(diǎn)、再通過(guò)角度約束篩選隨機(jī)節(jié)點(diǎn)的方法不同，本策略在樹節(jié)點(diǎn)生成時(shí)直接約束其擴(kuò)展角度，使得每次迭代產(chǎn)生的樹節(jié)點(diǎn)連接均滿足曲率約束。此外，還能使路徑符合起始點(diǎn)和目標(biāo)點(diǎn)的角度約束。

2.2 安全場(chǎng)引導(dǎo)策略

依照駕駛?cè)藢?duì)障礙物的避撞傾向，利用前述構(gòu)建的安全場(chǎng)，根據(jù)節(jié)點(diǎn)到障礙物之間的相對(duì)狀態(tài)計(jì)算式（3）產(chǎn)生的一系列備選點(diǎn)的安全場(chǎng)強(qiáng)，將安全場(chǎng)強(qiáng)作為選擇節(jié)點(diǎn)的權(quán)重系數(shù)，使隨機(jī)樹的擴(kuò)展方向能以足夠大的概率指向遠(yuǎn)離障礙物的方向，從而實(shí)現(xiàn)提前發(fā)現(xiàn)隨機(jī)樹延伸方向上的障礙物并據(jù)此規(guī)劃避障路徑，提升計(jì)算效率。

如圖5所示，為便于說(shuō)明，根據(jù)式（3）在θnearest的［-θmax，θmax］內(nèi)產(chǎn)生了3個(gè)備選點(diǎn)，記為Pcan，i，i∈｛1，2，3｝。以Pnearest為端點(diǎn)，向Pcan，i作射線。當(dāng)前方存在障礙物時(shí)，可得到備選點(diǎn)沿射線方向到障礙物的距離Sbw，i。根據(jù)主車速度得到相對(duì)障礙物的速度Δv。在此基礎(chǔ)上即可利用前述安全場(chǎng)得出臨界制動(dòng)減速度abg，i，即為安全場(chǎng)強(qiáng)。安全場(chǎng)強(qiáng)越大（絕對(duì)值）代表由該備選點(diǎn)所產(chǎn)生的路徑安全性越低，該備選點(diǎn)被選取的概率越應(yīng)降低。圖5中距離Sbw，3最大，計(jì)算得出的安全場(chǎng)最低，因而Pcan，3被選定為新樹節(jié)點(diǎn)的概率最大，引導(dǎo)隨機(jī)樹繞開(kāi)障礙物。

圖5 安全場(chǎng)引導(dǎo)策略

安全場(chǎng)強(qiáng)表示可能路徑上的碰撞風(fēng)險(xiǎn)，當(dāng)存在多重障礙物導(dǎo)致場(chǎng)景較為復(fù)雜時(shí)，若單純地按照安全場(chǎng)強(qiáng)選擇最安全的節(jié)點(diǎn)可能會(huì)導(dǎo)致錯(cuò)失最短的連通路徑。因此在兼顧避障能力的基礎(chǔ)上，通過(guò)概率選點(diǎn)的方法保留對(duì)安全場(chǎng)強(qiáng)較低區(qū)域的探索能力。以均值μ為0，方差σ為abg的方差，按照3σ原則設(shè)置飽和上限。采用式（4）計(jì)算權(quán)重wcan，i。利用式（5）對(duì)備選點(diǎn)Pcan，i的權(quán)重wcan，i做歸一化得到該點(diǎn)被選為樹節(jié)點(diǎn)的概率pcan，i，其中N為備選點(diǎn)數(shù)。

將全部備選點(diǎn)按照其概率大小作為區(qū)間長(zhǎng)度順序排布在［0，1］范圍內(nèi)，然后生成一個(gè)［0，1］范圍內(nèi)的隨機(jī)數(shù)，該隨機(jī)數(shù)所處區(qū)間的所屬備選點(diǎn)即被選作為新樹節(jié)點(diǎn)。

以圖5的場(chǎng)景為例，障礙物垂直于X軸，Pnearest到障礙物的距離為40 m，Pparent到Pnearest的矢量與X軸夾角為345°，θmax設(shè)為45°，θres設(shè)為3°。車速為30 km／h。按上述方法計(jì)算得到各位角度處備選點(diǎn)的概率，如圖6所示。備選節(jié)點(diǎn)所處半徑越大代表概率越大。

圖6 備選節(jié)點(diǎn)的被選概率與節(jié)點(diǎn)延伸方向的關(guān)系

在約束角度［0°，30°］∪［300°，0°）范圍內(nèi)，避障潛能越高的備選點(diǎn)的被選定概率越高，因此有助于降低算法在障礙物周圍區(qū)域的無(wú)效擴(kuò)張，提高算法效率。

綜上，安全場(chǎng)引導(dǎo)改進(jìn)RRT*算法流程如圖7所示。當(dāng)算法生成連接起點(diǎn)和目標(biāo)點(diǎn)的路徑后，可以通過(guò)剪枝和五次B樣條曲線進(jìn)行處理得到最終軌跡［15］。

圖7 算法流程圖

3 仿真試驗(yàn)

為驗(yàn)證本文提出的安全場(chǎng)改進(jìn)RRT*算法，設(shè)計(jì)了兩種工況，并在加入貪婪策略的情況下對(duì)RRT、RRT*和改進(jìn)RRT*在600 m×600 m的場(chǎng)景地圖內(nèi)分別進(jìn)行了20次仿真試驗(yàn)。其中，安全場(chǎng)改進(jìn)RRT*采用表2中一般型駕駛?cè)藬?shù)據(jù)作為安全場(chǎng)參數(shù)。

3.1 單個(gè)障礙物場(chǎng)景

在起始點(diǎn)（圓點(diǎn)，坐標(biāo)（100，300），航向0°）和目標(biāo)點(diǎn)（星形，坐標(biāo)（500，300），航向90°）之間設(shè)置垂直于橫軸的單個(gè)狹長(zhǎng)障礙物對(duì)算法的避障能力進(jìn)行測(cè)試。仿真得到的路徑規(guī)劃結(jié)果和統(tǒng)計(jì)結(jié)果分別如圖8和表3所示。

圖8 工況1仿真結(jié)果圖

表3 工況1仿真結(jié)果統(tǒng)計(jì)數(shù)據(jù)

圖8中分別為RRT、RRT*和改進(jìn)RRT*在場(chǎng)景地圖中生成的原始路徑，黑色粗實(shí)線為經(jīng)過(guò)平滑處理后得到的路徑。由圖8（a）可以看出，RRT在起始點(diǎn)附近的空間內(nèi)擴(kuò)展出大量的節(jié)點(diǎn)，當(dāng)接近障礙物時(shí)，在其左側(cè)生成了大量分枝，經(jīng)過(guò)了大量迭代次數(shù)才成功連通至障礙物右側(cè)，整體收斂速度慢。RRT*產(chǎn)生的路徑較RRT平順并且也更短，但RRT*附加的策略增加了每次迭代中的計(jì)算量，如表3所示，RRT*相較于RRT耗費(fèi)更多時(shí)間進(jìn)行搜索。

本文中提出的改進(jìn)RRT*利用安全場(chǎng)引導(dǎo)并借助角度約束策略，使得隨機(jī)數(shù)上的大部分節(jié)點(diǎn)提前繞行了障礙物并保證了路徑平滑。從表3中可以看出，該方法在平均迭代次數(shù)、算法生成節(jié)點(diǎn)數(shù)和搜索用時(shí)上均優(yōu)于另外兩種算法。綜上所述，對(duì)于單個(gè)障礙物場(chǎng)景，在3種算法均能夠規(guī)劃出可行路徑的基礎(chǔ)上，本文中提出的改進(jìn)RRT*效率更高。

3.2 多重障礙物場(chǎng)景

通過(guò)在地圖中擺放多種障礙物形成包含狹窄通道，局部小區(qū)域和大轉(zhuǎn)彎在內(nèi)的場(chǎng)景。設(shè)定起始點(diǎn)（圓點(diǎn)，坐標(biāo)（150，75），航向180°）和目標(biāo)點(diǎn)（星形，坐標(biāo)（450，75），航向180°）。圖9所示為RRT和RRT*的規(guī)劃路徑。兩種方法在20次試驗(yàn)中均未能在迭代3 000次內(nèi)成功規(guī)劃出路徑。

圖9 RRT和RRT*仿真結(jié)果圖

圖10 改進(jìn)RRT*算法仿真結(jié)果圖

圖10所示為改進(jìn)RRT*算法成功規(guī)劃出的路徑。在20次試驗(yàn)中共有14次在迭代3 000次內(nèi)成功搜索出路徑，成功率達(dá)70%。統(tǒng)計(jì)結(jié)果如表4所示，平均迭代次數(shù)為1 575.3次，遠(yuǎn)小于給定的3 000次。與RRT和RRT*相比，本文中提出的改進(jìn)RRT*擴(kuò)展效率較高，在搜索時(shí)借助安全場(chǎng)實(shí)現(xiàn)了提前避障的能力，可較快地規(guī)劃出路徑。同時(shí)，在多次仿真中計(jì)算出了多種路徑，表明算法在提高了規(guī)劃速度和成功率的同時(shí)保留了較好的空間探索能力。

表4 工況2仿真結(jié)果統(tǒng)計(jì)數(shù)據(jù)

綜上所述，本文中提出的基于安全場(chǎng)的改進(jìn)RRT*算法能夠以更高的成功率快速地規(guī)劃出滿足軌跡曲率約束的路徑。

4 結(jié)論

本文中針對(duì)RRT算法和RRT*算法計(jì)算效率低、對(duì)車輛角度約束滿足性差以及易陷入局部區(qū)域等問(wèn)題，提出了一種基于安全場(chǎng)改進(jìn)RRT*算法的路徑規(guī)劃方法。首先建立了安全場(chǎng)模型，并通過(guò)實(shí)車駕駛數(shù)據(jù)采集與分析，獲取了安全場(chǎng)參數(shù)；隨后通過(guò)加入角度約束和安全場(chǎng)引導(dǎo)策略，提出了改進(jìn)RRT*算法；最后，通過(guò)對(duì)RRT、RRT*和改進(jìn)RRT*進(jìn)行對(duì)比測(cè)試，驗(yàn)證了本文中提出的路徑規(guī)劃方法具備生成滿足航向角約束的平滑路徑的能力，同時(shí)在搜索效率和成功率方面有著明顯優(yōu)勢(shì)。