啟發(fā)式RRT算法的AGV路徑規(guī)劃

楊 瑤，付克昌，蔣 濤，張國良，劉甲甲，孟 易

成都信息工程大學(xué) 控制工程學(xué)院，成都610225

1 引言

近年來，隨著智能車的快速發(fā)展，智能車自身的優(yōu)勢也日漸凸顯。比如：智能車能夠減少駕駛壓力、提高駕駛的安全性、避免交通擁堵和降低對環(huán)境的污染[1]。而路徑規(guī)劃作為智能駕駛技術(shù)[2]和多機(jī)器人協(xié)同技術(shù)[3]的核心，對未知道路的規(guī)劃能力是衡量智能車的重要標(biāo)準(zhǔn)。目前，學(xué)者們對路徑規(guī)劃算法進(jìn)行了大量研究，主要包括：基于隨機(jī)采樣的快速搜索隨機(jī)樹等方法[4]；基于圖搜索的A*算法[5]、D*算法[6]等；基于模型的人工勢場法[7]、動態(tài)窗口法[8]等；基于生物啟發(fā)式的遺傳算法[9]等。其中，1998年，LaValle首次提出RRT算法，采用隨機(jī)采樣的規(guī)劃方式，不需要對狀態(tài)空間進(jìn)行預(yù)處理，搜索速度較快，且具有概率完備性，因此被廣泛運(yùn)用于機(jī)器人的路徑規(guī)劃中。

但傳統(tǒng)的RRT算法仍存在如下缺點(diǎn)：（1）所使用的全局均勻隨機(jī)采樣，使得算法運(yùn)行時(shí)間增加，收斂的速度變慢[10]；（2）采用最近節(jié)點(diǎn)選擇算法往往會導(dǎo)致其在復(fù)雜場景時(shí)所規(guī)劃的路徑失敗[11]；（3）所規(guī)劃的路徑?jīng)]有考慮車輛運(yùn)動學(xué)約束，使得規(guī)劃的路徑不能運(yùn)用于智能車的路徑規(guī)劃中[12]。

針對上述問題，國內(nèi)外學(xué)者都提出不同的改進(jìn)和優(yōu)化方法。在2001年Kuffner和LaValle[13]提出并行生成隨機(jī)樹（RRT-Connect）的方法，以提高該算法的收斂速度，但其未考慮車輛的運(yùn)動學(xué)約束。在2010年Karaman和Frazzoli[14]提出RRT*算法，用于解決RRT算法產(chǎn)生的路徑并非概率最優(yōu)解的問題，但其運(yùn)行效率較低。在2013年，M.Jordan和A.Perez[15]提出B-RRT*，用RRT*算法進(jìn)行起始點(diǎn)和目標(biāo)點(diǎn)的雙向擴(kuò)展，以減少RRT*算法所執(zhí)行的時(shí)間，但其應(yīng)用于移動機(jī)器人中，不能直接用于智能車。在2015年，杜明博，梅濤等人[12]提出連續(xù)曲率的RRT算法，通過加入環(huán)境約束和車輛自身約束，使得所規(guī)劃的路徑能夠較好運(yùn)用于智能車中，但未考慮車輛到障礙物的約束。2017年，馮來春，梁華為等人[16]通過A*算法形成引導(dǎo)域，然后使用RRT算法在其引導(dǎo)域中進(jìn)行擴(kuò)展，用于解決RRT算法隨機(jī)采樣所造成的問題，但其未解決智能車在目標(biāo)點(diǎn)朝向的問題。賀伊琳，高奇等人[17]于2018年在傳統(tǒng)RRT算法基礎(chǔ)上加入采樣可行區(qū)域，并使用三次B樣條曲線進(jìn)行平滑處理，以解決RRT算法所規(guī)劃的路徑曲折不平滑的問題，但未考慮智能車與障礙物的關(guān)系使得所生成的路徑存在著與障礙物過近的情況。2018年張捍東，陳陽，吳玉秀[18]提出將RRT算法與視野域自適應(yīng)滑動窗口相結(jié)合，用于解決在未知環(huán)境下的路徑規(guī)劃，但是其改進(jìn)未考慮智能車的運(yùn)動學(xué)約束，不能直接用于智能車的路徑規(guī)劃。

針對上述優(yōu)化中存在的不足，本文在B-RRT*算法基礎(chǔ)上提出一種適用于智能車的路徑規(guī)劃算法[19]。首先，使用Reeds_Shepp曲線進(jìn)行預(yù)處理以解決車輛在目標(biāo)點(diǎn)朝向的問題，并且使用雙向目標(biāo)偏向采樣策略以提高算法的效率。其次，提出啟發(fā)式滑動窗口的方法以限制B-RRT*算法采樣帶來的不確定因素。同時(shí)，在新生成節(jié)點(diǎn)上加入障礙物約束以解決B-RRT*算法所規(guī)劃的路徑離障礙物過近的問題。然后，在B-RRT*算法重新選擇父節(jié)點(diǎn)和重布隨機(jī)樹的過程中加入車輛最大轉(zhuǎn)角約束使擴(kuò)展樹能夠滿足車輛的運(yùn)動學(xué)約束。最后，使用貝塞爾曲線擬合路徑點(diǎn)使得路徑更加平滑。通過對基本B-RRT*算法進(jìn)行上述改進(jìn)，使得所規(guī)劃的路徑更加安全合理且更適用于車輛的運(yùn)動學(xué)特性。

2 車輛的運(yùn)動學(xué)模型

為了驗(yàn)證算法的適用性，本文采用智能汽車作為研究對象。由于汽車是一種非完整性約束的系統(tǒng)[12]且分析較為復(fù)雜，所以本文只考慮其簡化模型。本文的實(shí)驗(yàn)平臺為野馬E70電動汽車，如圖1所示，其簡化模型如圖2所示。在慣性坐標(biāo)系XOY下，(x,y)為車體在此坐標(biāo)系下的坐標(biāo)位置，φ為車體的橫擺角（航向角），L=1.56 m是智能車的軸距，D=1.835 m表示智能車的寬度，智能車的長度為5.555 m,R是智能車的轉(zhuǎn)彎半徑，θ為車輛縱軸y與橫軸x之間的夾角。由于車輛只能用前輪轉(zhuǎn)向，所以需要滿足阿克曼轉(zhuǎn)向條件(φ＜φmax)。本文只考慮車輛在二維平面上運(yùn)動，不考慮俯仰和側(cè)傾帶來的影響[12]。同時(shí)，假設(shè)在車輛運(yùn)動瞬間，車輛的速度與車體保持平衡，即滿足：

其中，Kmax=0.16m-1車輛最大轉(zhuǎn)折曲率，Rmin=8 m車輛最小轉(zhuǎn)彎半徑，φmax=40°車輛最大轉(zhuǎn)角。

圖1 基于野馬E70電動汽車改造的智能車駕駛平臺

圖2 車輛簡化運(yùn)動學(xué)模型

3 B-RRT＊算法

B-RRT*算法是一種雙向漸近最優(yōu)算法[20]，其過程如下：

（1）初始化：從起始點(diǎn)向目標(biāo)點(diǎn)構(gòu)建擴(kuò)展樹tree1，從目標(biāo)點(diǎn)向起始點(diǎn)構(gòu)建擴(kuò)展樹tree2。

（2）在地圖上生成隨機(jī)點(diǎn)qrand1和qrand2，如果隨機(jī)點(diǎn)qrand1和qrand2落在無障礙物的區(qū)間內(nèi)，則遍歷tree1和tree2找到離隨機(jī)點(diǎn)最近的節(jié)點(diǎn)qnearest1和qnearest2。否則，重新從搜索空間選擇隨機(jī)點(diǎn)。

（3）判斷qrand1和qnearest1，qrand2和qnearest2連線上是否有障礙物。

如果連線上無障礙物，則判斷qrand1和qnearest1，qrand2和qnearest2之間的距離是否小于擴(kuò)展步長（本文設(shè)置為5 m），若距離小于擴(kuò)展步長，則將隨機(jī)點(diǎn)添加到擴(kuò)展樹中。否則，在最近節(jié)點(diǎn)和隨機(jī)點(diǎn)的連線上取擴(kuò)展步長長度的點(diǎn)qnew添加到擴(kuò)展樹上，再將隨機(jī)點(diǎn)添加到擴(kuò)展樹上。

如果連線上有障礙物，則執(zhí)行（2）。

（4）在tree1和tree2分別執(zhí)行重新選擇父節(jié)點(diǎn)和重布隨機(jī)樹的過程。

（5）判斷tree1和tree2之間的距離是否小于閾值。若兩棵樹之間的距離大于閾值（本文設(shè)置為5 m），重復(fù)執(zhí)行（2）～（4）。若小于閾值，則從相遇的tree1和tree2的節(jié)點(diǎn)上，分別反向取到起始點(diǎn)和目標(biāo)點(diǎn)并保存路徑（如圖3的示意圖）。

（6）輸出B-RRT*算法所規(guī)劃的路徑。

圖3 B_RRT*算法示意圖

4 改進(jìn)B-RRT＊算法

與微小型移動機(jī)器人路徑規(guī)劃相比，智能車的路徑規(guī)劃是一種三維的規(guī)劃，不僅要考慮車輛的位置(x,y)而且還考慮車輛到達(dá)目標(biāo)點(diǎn)的航向φ。因?yàn)橹悄苘嚥⒎琴|(zhì)點(diǎn)，所以在智能車的路徑規(guī)劃中，還需要考慮所規(guī)劃的路徑的轉(zhuǎn)折角度是否小于最大轉(zhuǎn)折角度、路徑與障礙物之間的距離是否安全合理等情況。

4.1 預(yù)處理過程

Reeds-Shepp曲線[21]是Reeds和Shepp二人在1990年，針對車輛的運(yùn)動學(xué)提出的最優(yōu)曲線。通過對車輛進(jìn)行運(yùn)動學(xué)建模，得到車輛的最小轉(zhuǎn)彎半徑和起始點(diǎn)與目標(biāo)點(diǎn)的姿態(tài)，生成適用于車輛的運(yùn)動曲線。

在執(zhí)行B-RRT*算法之前或在目標(biāo)點(diǎn)附近，使用Reeds-Shepp曲線進(jìn)行路徑規(guī)劃得到路徑，并判斷路徑上是否與障礙物發(fā)生碰撞。若沒有，則保留Reeds-Shepp曲線所生成的路徑，完成規(guī)劃。

圖4表示了預(yù)處理過程，圖4（a）、圖4（b）分別表示起始點(diǎn)朝向與目標(biāo)點(diǎn)朝向相同和垂直的情況。藍(lán)色路徑為使用Reeds-Shepp曲線預(yù)處理生成的路徑，綠色的路徑為B-RRT*算法規(guī)劃的路徑。從圖4中，能夠看出使用Reeds-Shepp曲線能夠有效解決車輛朝向的問題，并且所生成的軌跡也更加平滑。

圖4 預(yù)處理過程

4.2 啟發(fā)式滑動采樣窗口

由于基本的B-RRT*算法在搜索空間（節(jié)點(diǎn)qrand生成的范圍）隨機(jī)采樣，導(dǎo)致所生成的節(jié)點(diǎn)不合理并使得算法收斂速度的減慢。本文的實(shí)驗(yàn)環(huán)境是校園道路，通過加入滑動窗口使得B-RRT*算法的采樣點(diǎn)能夠更多地集中于道路中，而啟發(fā)式改進(jìn)使得采樣點(diǎn)能夠朝著目標(biāo)點(diǎn)“生長”并能提高算法的收斂速度。因此，本文提出雙向偏向性采樣與啟發(fā)式滑動采樣窗口相融合的方法（如公式（3）所示）。

首先，設(shè)定一個(gè)目標(biāo)偏向概率[21]p1（本文設(shè)置為10%），獲得采樣時(shí)的概率p并代入公式（3）中。若p＜p1，則將目標(biāo)點(diǎn)或起始點(diǎn)直接賦值給qrand1或qrand2，這樣可使隨機(jī)樹能夠較快地向外“生長”。當(dāng)p＞p1時(shí)，在tree1中尋找與目標(biāo)點(diǎn)最近的節(jié)點(diǎn)qnearest1并判斷在該節(jié)點(diǎn)周圍以D為長度的正方形內(nèi)有無障礙物。如果沒有，則在最近節(jié)點(diǎn)建立滑動窗口（窗口長度為2 m應(yīng)略大于智能車輛的寬度），并在該窗口中生成qrand1，以避免采樣的隨機(jī)性。在tree2的“生長”過程中，尋找與起始點(diǎn)最近的節(jié)點(diǎn)qnearest2，進(jìn)行上述過程并建立滑動窗口，約束qrand2的坐標(biāo)（如圖5示意圖所示）。

為了驗(yàn)證啟發(fā)式滑動窗口對B-RRT*算法的改進(jìn)作用，將帶有啟發(fā)式滑動窗口B-RRT*算法和基本B-RRT*算法進(jìn)行對比。圖6中的坐標(biāo)點(diǎn)為所測試的起始點(diǎn)和目標(biāo)點(diǎn)，其中S表示起始點(diǎn)，分別選取G1,G2,G3,G4,G5作為目標(biāo)點(diǎn)，采樣窗口寬度為2 m。圖7表示改進(jìn)B-RRT*算法和基本B-RRT*算法在各個(gè)測試點(diǎn)運(yùn)行的平均時(shí)間（每種算法執(zhí)行20次，取平均值）。

圖5 啟發(fā)式滑動采樣窗口示意圖

圖6 采用啟發(fā)式滑動采樣窗口的B-RRT*算法與基本B-RRT*算法的測試點(diǎn)

圖7 帶有啟發(fā)式滑動采樣窗口的B-RRT*算法和基本B-RRT*算法測試對比

從圖7（a）和（b）中可以看出，加入啟發(fā)式滑動采樣窗口能夠減少B-RRT*算法的迭代次數(shù)，并提升了B-RRT*算法的運(yùn)行時(shí)間。同時(shí)，從圖7（b）中可以看出，啟發(fā)式滑動采樣窗口能夠較好解決B-RRT*算法隨機(jī)采樣所帶來的不確定性并提高算法的收斂性速度，使得B-RRT*算法能夠更好地運(yùn)用于智能車。

4.3 新節(jié)點(diǎn)q new到障礙物安全距離約束

由于定位和地圖本身存在一定的誤差，所以必須保證規(guī)劃的路徑與障礙物之間的距離大于車身寬度，不能使所規(guī)劃的路徑貼合障礙物或離障礙物過近（如圖8表示路徑與障礙物之間的關(guān)系）。但在基本B-RRT*算法中，未考慮路徑和障礙物的距離關(guān)系使得所規(guī)劃的路徑點(diǎn)存在著與離障礙物過近的情況，不能被智能車直接使用。

圖8 規(guī)劃路徑和障礙物示意圖

在基本B-RRT*算法產(chǎn)生新的節(jié)點(diǎn)qnew的過程中，僅僅是通過在qrand和qnearest連線上，以擴(kuò)展步長為長度生成節(jié)點(diǎn)qnew，而未考慮該節(jié)點(diǎn)與障礙物所需的安全距離。本文通過生成qnew周圍的待選節(jié)點(diǎn)（如圖8所示）并將其與障礙物安全距離約束相結(jié)合的方法（如圖9所示），使得所規(guī)劃的路徑與障礙物之間的距離大于安全距離。即d＞Dsafe，其中Dsafe=0.917 5m表示安全距離，取為車輛寬度的一半，d表示規(guī)劃路徑與障礙物之間的距離。

圖9 q new周圍待選節(jié)點(diǎn)的選取示意圖

圖9 中，qnew與周圍的待選節(jié)點(diǎn)poses1之間的距離D=1.835 m。=1 m表示待選節(jié)點(diǎn)poses1與周圍節(jié)點(diǎn)poses2的距離。

圖10流程圖的基本步驟如下：

（1）首先獲得節(jié)點(diǎn)qnew，初始化dmin（初始化定義dmin等于無窮大）。

（2）獲得qnew四周的節(jié)點(diǎn)poses1，依次從中選取pose并判斷是否為障礙物。如果是，則從poses1中重新選取pose。否則在pose節(jié)點(diǎn)四周添加poses2(poses1={A1,B1,C1,D1},poses2={A11,A12,A13,A14,B11,B12,B13,B14,C11,C12,C13,C14,D11,D12,D13,D14})。

圖10 q new到障礙物安全距離約束流程圖

（3）在poses2中依次選擇poses1，并判斷poses1是否為障礙物。如果是，則執(zhí)行（2）。否則，計(jì)算pose到目標(biāo)點(diǎn)距離d。

（4）判斷d和dmin大小，如果dmin＞d，更新qnew和dmin。否則執(zhí)行（2）。

（5）輸出qnew。

為了驗(yàn)證新節(jié)點(diǎn)qnew的障礙物安全距離約束對算法對運(yùn)算成本的影響，將帶有新生節(jié)點(diǎn)安全距離約束的B-RRT*算法和基本B-RRT*算法在圖6處進(jìn)行測試對比。圖11表示改進(jìn)B-RRT*算法和基本B-RRT*算法在各個(gè)測試點(diǎn)運(yùn)行的平均時(shí)間（每種算法執(zhí)行20次，取平均值）。

圖11 加入障礙物安全距離約束對規(guī)劃時(shí)間的影響

從圖11可以看出，對新節(jié)點(diǎn)qnew加入障礙物安全距離約束使得B-RRT*算法的運(yùn)算成本增加，但是通過加入這項(xiàng)約束能夠使得B-RRT*算法所規(guī)劃的路徑與障礙物保持一定的距離，增加智能車在行駛中的安全性。

4.4 車輛最大轉(zhuǎn)角約束

重新選擇父節(jié)點(diǎn)和重布隨機(jī)樹是基本B-RRT*算法的重要部分，但在基本B-RRT*算法中并未考慮車輛的運(yùn)動特性，導(dǎo)致所規(guī)劃的路徑存在著轉(zhuǎn)折次數(shù)較多和轉(zhuǎn)折角度較大等問題不利于智能車的行駛。因此本文通過在重新選擇父節(jié)點(diǎn)和重布隨機(jī)樹過程中加入車輛最大轉(zhuǎn)角約束和障礙物安全距離約束相結(jié)合的方式使得重選父節(jié)點(diǎn)和重布隨機(jī)樹的過程結(jié)果更加合理。

（1）重選父節(jié)點(diǎn)過程

在基本B-RRT算法中，qnearest是新生成節(jié)點(diǎn)qnew的父節(jié)點(diǎn)。然而，在B-RRT*中會以qnew為圓心和R為半徑選擇新的父節(jié)點(diǎn)，并計(jì)算從根節(jié)點(diǎn)到待選父節(jié)點(diǎn)距離和待選父節(jié)點(diǎn)到qnew的距離的總和，選擇最短距離的待選父節(jié)點(diǎn)作為qnew父節(jié)點(diǎn)（如圖12（a）和12（b））。雖然B-RRT*算法能得到相對較短的路徑，但忽略了智能車自身的約束條件。在本論文中，將車輛最大轉(zhuǎn)折曲率作為約束條件，加入重選父節(jié)點(diǎn)的過程中（如圖12（c）所示）。此外，在更新父節(jié)點(diǎn)的過程中還需判斷在父節(jié)點(diǎn)周圍以D為邊長的正方形內(nèi)有無障礙物，只有在該正方形內(nèi)無障礙物才更新父節(jié)點(diǎn)。

圖12 重新選擇父節(jié)點(diǎn)過程

其中，圖12（a）是以R為半徑⑨為圓心的重選選擇父節(jié)點(diǎn)的過程，圖12（b）是使用基本B-RRT*算法執(zhí)行重選父節(jié)點(diǎn)的結(jié)果??⑤?⑧?⑨其路徑的長度為11），圖12（c）是加入車輛最大轉(zhuǎn)角約束（如公式（4））的重新選擇父節(jié)點(diǎn)的結(jié)果??④?⑨其路徑的長度為14）。通過圖12（b）和（c）可知，雖然（b）的路徑比（c）短，但是φ5-8-9＞φmaxφ5-8-9為路徑⑤?⑧?⑨的角度）不滿足路徑點(diǎn)的轉(zhuǎn)角小于智能車最大轉(zhuǎn)角的要求，所以放棄節(jié)點(diǎn)⑧作為節(jié)點(diǎn)⑨的父節(jié)點(diǎn)。

φparent表示待選的父節(jié)點(diǎn)（圈內(nèi)的節(jié)點(diǎn)）的角度，A表示常數(shù)，d1表示樹根到qnew的距離。Cparent表示待選父節(jié)點(diǎn)的代價(jià)值。

其步驟如下：

①獲得節(jié)點(diǎn)qnew并以閾值R為半徑，找出所有待選父節(jié)點(diǎn)。

②依次從待選父節(jié)點(diǎn)qparent中，計(jì)算qparent,qparent的父節(jié)點(diǎn)和qnew所形成角度是否小于車輛的最大轉(zhuǎn)角φmax。如果是，則計(jì)算根節(jié)點(diǎn)到qnew的距離并代入公式（4）得到待選父節(jié)點(diǎn)的代價(jià)值。否則，從待選父節(jié)點(diǎn)中重新獲得新的父節(jié)點(diǎn)qparent。

③從所有待選父節(jié)點(diǎn)中，依次判斷父節(jié)點(diǎn)在D為邊長的正方形內(nèi)有無障礙物。若無障礙物，則更新最小代價(jià)值并保留qnew新的父節(jié)點(diǎn)。否則，繼續(xù)從待選父節(jié)點(diǎn)中選取父節(jié)點(diǎn)。

（2）重布隨機(jī)樹過程

在qnew重新選擇父節(jié)點(diǎn)后，為了進(jìn)一步使得隨機(jī)樹之間連接代價(jià)減小，所以要為隨機(jī)樹重新布線。即以新生成的節(jié)點(diǎn)qnew⑨為圓心，R為半徑并以④為父節(jié)點(diǎn)，圓內(nèi)其他的節(jié)點(diǎn)為子節(jié)點(diǎn)qnew的父節(jié)點(diǎn)除外）。但是由于車輛存在最大轉(zhuǎn)角度約束，因此在重布隨機(jī)樹過程中需要加入車輛約束。圖13（a）表示基本B-RRT*算法重新選擇子節(jié)點(diǎn)的結(jié)果，（b）表示基本B-RRT*算法重新布線的結(jié)果??④?⑨?⑥其路徑為15），（c）表示加入車輛最大轉(zhuǎn)角約束所重新布線的結(jié)果??④?⑨?⑩其路徑為16）。通過圖13（b）和（c）可知雖然（b）的路徑比（c）短，但是φ4-9-6＞φmaxφ4-9-6為路徑④?⑨?⑥的角度）不滿足路徑點(diǎn)的轉(zhuǎn)角小于智能車最大轉(zhuǎn)角的要求，所以放棄節(jié)點(diǎn)⑥作為節(jié)點(diǎn)⑨的子節(jié)點(diǎn)。

其步驟如下：

①獲得節(jié)點(diǎn)qnew并以閾值R為半徑獲得待選子節(jié)點(diǎn)qchild。

②依次從待選子節(jié)點(diǎn)qchild中，計(jì)算qnew,qnew的父節(jié)點(diǎn)和qchild所形成角度是否大于車輛的最大轉(zhuǎn)角φmax。如果是，則計(jì)算根節(jié)點(diǎn)到qchild的距離并代入公式（5）得到待選子節(jié)點(diǎn)的代價(jià)值。否則，從待選子節(jié)點(diǎn)中重新獲得新的子節(jié)點(diǎn)。

③從所有待選子節(jié)點(diǎn)中，依次判斷子節(jié)點(diǎn)在以D為邊長的正方形內(nèi)有無障礙物。若無，則更新最小代價(jià)值和qnew新的子節(jié)點(diǎn)。否則，繼續(xù)從待選子節(jié)點(diǎn)中選取子節(jié)點(diǎn)。

φchild表示待選的子節(jié)點(diǎn)（圈內(nèi)的節(jié)點(diǎn)）的角度，B表示常數(shù)，d2表示樹根到待選子節(jié)點(diǎn)的距離。Cchild表示待選子節(jié)點(diǎn)的代價(jià)值。

圖13 重布隨機(jī)樹過程

為了驗(yàn)證車輛最大轉(zhuǎn)角約束對算法運(yùn)算成本的影響，將帶有車輛最大轉(zhuǎn)角約束的B-RRT*算法和基本B-RRT*算法在圖6的環(huán)境中進(jìn)行測試對比。圖14表示改進(jìn)B-RRT*算法和基本B-RRT*算法在各個(gè)測試點(diǎn)運(yùn)行的平均時(shí)間（每種算法執(zhí)行20次，取平均值）。

圖14 加入車輛最大轉(zhuǎn)角約束對規(guī)劃時(shí)間的影響

從圖14中可以看出，加入車輛最大轉(zhuǎn)角約束使得B-RRT*算法的運(yùn)算時(shí)間略微增加，但所增加的運(yùn)算成本也在可以接受范圍之內(nèi)，而加入這項(xiàng)約束能夠解決B-RRT*算法中重新父節(jié)點(diǎn)和更新子節(jié)點(diǎn)未考慮車輛約束的問題。

4.5 路徑點(diǎn)優(yōu)化和擬合

在加入啟發(fā)式滑動采樣窗口、障礙物安全距離約束和車輛最大轉(zhuǎn)角約束后，可得到相關(guān)的路徑點(diǎn)poses，然后對路徑點(diǎn)進(jìn)行優(yōu)化，其步驟如下。

（1）依次從路徑點(diǎn)poses中取出pose1和pose2并計(jì)算pose1與pose2的距離D1，再判斷D1是否大于擴(kuò)展步長。如果是，則執(zhí)行（2）。否則，繼續(xù)執(zhí)行（1）。

（2）在pose1和pose2連線上，取距離pose1以擴(kuò)展步長為長度的點(diǎn)作為posenew。

（3）對posenew進(jìn)行到障礙物的安全距離約束，使posenew更加合理并將posenew插入poses。

（4）使用貝塞爾曲線對路徑點(diǎn)poses進(jìn)行擬合。

貝塞爾曲線是法國工程師皮埃爾。貝塞爾在1962年提出，其形狀可由曲線的控制點(diǎn)確定。因此，將改進(jìn)B-RRT*算法的路徑點(diǎn)作為貝塞爾曲線的輸入，消除路徑不平滑的情況，如圖15所示。

圖15 使用貝塞爾曲線示意圖

使用不同擬合方法示意圖如圖15所示，其中A點(diǎn)，B點(diǎn)和C點(diǎn)表示改進(jìn)B-RRT*算法的路徑點(diǎn)。從圖15可以看出，使用二次樣條插值所生成的路徑的曲率較大，二次B樣條曲線擬合的起始點(diǎn)是A點(diǎn)和B點(diǎn)連線的中點(diǎn)，目標(biāo)點(diǎn)是B點(diǎn)和C點(diǎn)連線的中點(diǎn)，導(dǎo)致所生成的路徑在起始點(diǎn)和目標(biāo)點(diǎn)處不連續(xù)，而使用二次貝塞爾曲線擬合能夠較好解決上述二次樣條插值和二次B樣條曲線擬合所帶來的問題。

公式（6）為貝塞爾曲線的一般方程，公式（7）是本文所使用的二階貝塞爾曲線，P0,P1,P2為二階貝塞爾曲線的控制點(diǎn)。

5 算法驗(yàn)證

為了驗(yàn)證改進(jìn)B-RRT*算法的有效性，本文分別將RRT、RRT*、B-RRT*和改進(jìn)的B-RRT*算法應(yīng)用于成都信息工程大學(xué)校園的柵格地圖（4846×2816）上（如圖16），柵格大小為0.298 m/pixel，選擇不同的地點(diǎn)進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)基于ROS（開源機(jī)器人操作系統(tǒng)）平臺，并使用Rviz對規(guī)劃結(jié)果進(jìn)行可視化顯示，計(jì)算機(jī)配置為：ubuntu14.04 LTS，處理器intel i5-6500，主頻為3.2 GHz，運(yùn)行內(nèi)存為8 GB。

圖16 學(xué)校的校園地圖

改進(jìn)B-RRT*算法的參數(shù)選?。海?）擴(kuò)展步長和閾值的選擇，當(dāng)“相遇”的兩個(gè)節(jié)點(diǎn)之間的距離小于車身長度時(shí)，則算法收斂，因此本文將擴(kuò)展步長和閾值設(shè)置為5 m便于計(jì)算；（2）為了使更多的采樣點(diǎn)落在智能車的正前方，所以啟發(fā)式滑動采樣窗口長度應(yīng)略大于智能車輛的寬度，本文設(shè)置為2 m；（3）安全距離Dsafe的選擇，因?yàn)楸疚氖且攒囕v后軸上中心點(diǎn)處為車輛位置的參考點(diǎn)，所以安全距離應(yīng)為車寬的一半，本文設(shè)置為0.917 5 m；（4）由于智能車的寬度為1.835 m，所以以車寬為距離獲得待選節(jié)點(diǎn)poses1能夠使得qnew更為合理；（5）D1around的選擇，待選節(jié)點(diǎn)poses1與四周節(jié)點(diǎn)poses2的距離D1around應(yīng)大于安全距離Dsafe以便為智能車提供一定的冗余距離，本文設(shè)置為1 m；（6）其他參數(shù)選擇，車輛的最大轉(zhuǎn)角為φmax=40°。

本次實(shí)驗(yàn)通過在地圖上選擇起始點(diǎn)和終點(diǎn)，使用RRT、RRT*、B-RRT*和改進(jìn)的B-RRT*算法規(guī)劃。通過路徑的長度、到障礙物的距離和轉(zhuǎn)折數(shù)量（轉(zhuǎn)折角度＞φmax=40°，則認(rèn)為是轉(zhuǎn)折）等方式來評價(jià)路徑的好壞。對區(qū)域a和區(qū)域b進(jìn)行路徑規(guī)劃，規(guī)劃結(jié)果見圖17、圖18和圖19，其中藍(lán)色節(jié)點(diǎn)為起始點(diǎn)向目標(biāo)點(diǎn)的采樣節(jié)點(diǎn)，紅色節(jié)點(diǎn)為目標(biāo)點(diǎn)向起始點(diǎn)的采樣節(jié)點(diǎn)。表1、表2和表3分別為連續(xù)轉(zhuǎn)折區(qū)域、直角轉(zhuǎn)折曲線區(qū)域和停車場連續(xù)轉(zhuǎn)折區(qū)域的指標(biāo)對比。

表1 區(qū)域1測試對比

從圖17、圖18和圖19中可以看出，改進(jìn)B-RRT*算法所規(guī)劃出來的路徑較為平滑，且具有連續(xù)的曲率，能夠解決基本RRT、RRT*、B-RRT*算法在初始時(shí)刻路徑不合理的問題，同時(shí)與障礙物形成一定的安全距離并且能夠較好地滿足車輛運(yùn)動學(xué)特性。

圖17 在區(qū)域1測試的結(jié)果

圖18 在區(qū)域2測試的結(jié)果

圖19 在區(qū)域3測試的結(jié)果

表2 區(qū)域2測試對比

表3 區(qū)域3測試對比

通過表1、表2和表3能夠看到雖然基本RRT、RRT*、B-RRT*算法能夠在較短的時(shí)間內(nèi)規(guī)劃出路徑，但是所規(guī)劃的路徑轉(zhuǎn)折角度過大、路徑點(diǎn)貼合障礙物，導(dǎo)致路徑不能被智能車使用。而本文提出的改進(jìn)B-RRT*算法在犧牲較短時(shí)間的基礎(chǔ)上獲得了更加平滑、安全且更符合車輛運(yùn)動學(xué)的路徑。同時(shí)，通過加入對新生節(jié)點(diǎn)安全距離約束和車輛最大轉(zhuǎn)向約束等增加了算法的復(fù)雜度使得算法的運(yùn)行時(shí)間增加，但對處于實(shí)驗(yàn)階段的中低速智能車或者AGV來說所增加的時(shí)間也是在接受的范圍之內(nèi)，并且通過上述對B-RRT*算法的改進(jìn)增加了改算法在實(shí)際應(yīng)用中的可行性。

6 結(jié)論

由于定位和地圖所帶來的誤差，以及車輛運(yùn)動學(xué)約束，使得基本RRT、RRT*和B-RRT算法不適用于本文所測試的環(huán)境。所以，本文提出的改進(jìn)B-RRT*算法克服了基本的RRT、RRT*、B-RRT*算法存在的轉(zhuǎn)折點(diǎn)較多、路線不平滑、規(guī)劃路徑與障礙物過近和曲率不連續(xù)等問題，使得所規(guī)劃的路徑能更加滿足車輛的運(yùn)動學(xué)模型。同時(shí)，通過使用Reeds-Shepp曲線在目標(biāo)點(diǎn)附近重新規(guī)劃能夠解決目標(biāo)點(diǎn)朝向的問題，并通過約束采樣范圍增強(qiáng)了算法的魯棒性。需要指出的是，本文算法主要解決實(shí)驗(yàn)階段中低速智能車或輪式機(jī)器人的規(guī)劃問題。后續(xù)將對該算法進(jìn)行改進(jìn)，使其能夠應(yīng)用在高速行駛的智能車上。