啟發式RRT算法的AGV路徑規劃

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

成都信息工程大學 控制工程學院，成都610225

1 引言

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

但傳統的RRT算法仍存在如下缺點：（1）所使用的全局均勻隨機采樣，使得算法運行時間增加，收斂的速度變慢[10]；（2）采用最近節點選擇算法往往會導致其在復雜場景時所規劃的路徑失敗[11]；（3）所規劃的路徑沒有考慮車輛運動學約束，使得規劃的路徑不能運用于智能車的路徑規劃中[12]。

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

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

2 車輛的運動學模型

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

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

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

圖2 車輛簡化運動學模型

3 B-RRT＊算法

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

（1）初始化：從起始點向目標點構建擴展樹tree1，從目標點向起始點構建擴展樹tree2。

（2）在地圖上生成隨機點qrand1和qrand2，如果隨機點qrand1和qrand2落在無障礙物的區間內，則遍歷tree1和tree2找到離隨機點最近的節點qnearest1和qnearest2。否則，重新從搜索空間選擇隨機點。

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

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

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

（4）在tree1和tree2分別執行重新選擇父節點和重布隨機樹的過程。

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

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

圖3 B_RRT*算法示意圖

4 改進B-RRT＊算法

與微小型移動機器人路徑規劃相比，智能車的路徑規劃是一種三維的規劃，不僅要考慮車輛的位置(x,y)而且還考慮車輛到達目標點的航向φ。因為智能車并非質點，所以在智能車的路徑規劃中，還需要考慮所規劃的路徑的轉折角度是否小于最大轉折角度、路徑與障礙物之間的距離是否安全合理等情況。

4.1 預處理過程

Reeds-Shepp曲線[21]是Reeds和Shepp二人在1990年，針對車輛的運動學提出的最優曲線。通過對車輛進行運動學建模，得到車輛的最小轉彎半徑和起始點與目標點的姿態，生成適用于車輛的運動曲線。

在執行B-RRT*算法之前或在目標點附近，使用Reeds-Shepp曲線進行路徑規劃得到路徑，并判斷路徑上是否與障礙物發生碰撞。若沒有，則保留Reeds-Shepp曲線所生成的路徑，完成規劃。

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

圖4 預處理過程

4.2 啟發式滑動采樣窗口

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

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

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

圖5 啟發式滑動采樣窗口示意圖

圖6 采用啟發式滑動采樣窗口的B-RRT*算法與基本B-RRT*算法的測試點

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

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

4.3 新節點q new到障礙物安全距離約束

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

圖8 規劃路徑和障礙物示意圖

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

圖9 q new周圍待選節點的選取示意圖

圖9 中，qnew與周圍的待選節點poses1之間的距離D=1.835 m。=1 m表示待選節點poses1與周圍節點poses2的距離。

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

（1）首先獲得節點qnew，初始化dmin（初始化定義dmin等于無窮大）。

（2）獲得qnew四周的節點poses1，依次從中選取pose并判斷是否為障礙物。如果是，則從poses1中重新選取pose。否則在pose節點四周添加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是否為障礙物。如果是，則執行（2）。否則，計算pose到目標點距離d。

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

（5）輸出qnew。

為了驗證新節點qnew的障礙物安全距離約束對算法對運算成本的影響，將帶有新生節點安全距離約束的B-RRT*算法和基本B-RRT*算法在圖6處進行測試對比。圖11表示改進B-RRT*算法和基本B-RRT*算法在各個測試點運行的平均時間（每種算法執行20次，取平均值）。

圖11 加入障礙物安全距離約束對規劃時間的影響

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

4.4 車輛最大轉角約束

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

（1）重選父節點過程

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

圖12 重新選擇父節點過程

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

φparent表示待選的父節點（圈內的節點）的角度，A表示常數，d1表示樹根到qnew的距離。Cparent表示待選父節點的代價值。

其步驟如下：

①獲得節點qnew并以閾值R為半徑，找出所有待選父節點。

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

③從所有待選父節點中，依次判斷父節點在D為邊長的正方形內有無障礙物。若無障礙物，則更新最小代價值并保留qnew新的父節點。否則，繼續從待選父節點中選取父節點。

（2）重布隨機樹過程

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

其步驟如下：

①獲得節點qnew并以閾值R為半徑獲得待選子節點qchild。

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

③從所有待選子節點中，依次判斷子節點在以D為邊長的正方形內有無障礙物。若無，則更新最小代價值和qnew新的子節點。否則，繼續從待選子節點中選取子節點。

φchild表示待選的子節點（圈內的節點）的角度，B表示常數，d2表示樹根到待選子節點的距離。Cchild表示待選子節點的代價值。

圖13 重布隨機樹過程

為了驗證車輛最大轉角約束對算法運算成本的影響，將帶有車輛最大轉角約束的B-RRT*算法和基本B-RRT*算法在圖6的環境中進行測試對比。圖14表示改進B-RRT*算法和基本B-RRT*算法在各個測試點運行的平均時間（每種算法執行20次，取平均值）。

圖14 加入車輛最大轉角約束對規劃時間的影響

從圖14中可以看出，加入車輛最大轉角約束使得B-RRT*算法的運算時間略微增加，但所增加的運算成本也在可以接受范圍之內，而加入這項約束能夠解決B-RRT*算法中重新父節點和更新子節點未考慮車輛約束的問題。

4.5 路徑點優化和擬合

在加入啟發式滑動采樣窗口、障礙物安全距離約束和車輛最大轉角約束后，可得到相關的路徑點poses，然后對路徑點進行優化，其步驟如下。

（1）依次從路徑點poses中取出pose1和pose2并計算pose1與pose2的距離D1，再判斷D1是否大于擴展步長。如果是，則執行（2）。否則，繼續執行（1）。

（2）在pose1和pose2連線上，取距離pose1以擴展步長為長度的點作為posenew。

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

（4）使用貝塞爾曲線對路徑點poses進行擬合。

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

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

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

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

5 算法驗證

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

圖16 學校的校園地圖

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

本次實驗通過在地圖上選擇起始點和終點，使用RRT、RRT*、B-RRT*和改進的B-RRT*算法規劃。通過路徑的長度、到障礙物的距離和轉折數量（轉折角度＞φmax=40°，則認為是轉折）等方式來評價路徑的好壞。對區域a和區域b進行路徑規劃，規劃結果見圖17、圖18和圖19，其中藍色節點為起始點向目標點的采樣節點，紅色節點為目標點向起始點的采樣節點。表1、表2和表3分別為連續轉折區域、直角轉折曲線區域和停車場連續轉折區域的指標對比。

表1 區域1測試對比

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

圖17 在區域1測試的結果

圖18 在區域2測試的結果

圖19 在區域3測試的結果

表2 區域2測試對比

表3 區域3測試對比

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

6 結論

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