基于雙向隨機樹改進的智能車輛路徑規劃研究*

施楊洋,楊家富,梅 淼,朱林峰,2

(1.南京林業大學機械電子工程學院，江蘇 南京 210037；2.北方信息控制研究院集團有限公司，江蘇 南京 211106)

1 引言

車輛的路徑規劃[1 - 4]指的是已知車輛的起始點位置、目標點位置和環境中的障礙物分布，按照一定的標準，規劃出一條與障礙物不相碰撞的路徑。近年來新的路徑規劃算法不斷涌現，在路徑規劃領域，最具有代表性和最常見的路徑規劃算法主要有基于地圖的路徑規劃算法、基于仿生學的路徑規劃算法以及基于采樣的路徑規劃算法。

基于采樣的路徑規劃算法有概率圖算法和快速擴展隨機樹算法。快速擴展隨機樹算法是由LaValle等[5,6]提出的一種路徑規劃算法。其優點在于不需要對規劃的空間進行建模，是一種隨機采樣的算法，同時考慮了無人車客觀存在的約束，因此得到了廣泛應用[7]。基本的快速擴展隨機樹算法在進行路徑規劃時存在以下缺點：(1)隨機生成路徑，路徑具有偏差性；(2)隨機樹在搜索過程中無導向性；(3)收斂速度遲緩，搜索效率低。

針對基本快速擴展隨機樹算法的不足，國內外研究者進行了大量的改進研究。偏向搜索和雙向擴展的快速擴展隨機樹算法提高了算法的收斂速度和搜索效率[8,9]，但是并未克服隨機樹生成節點時的隨機性；動態步長的快速擴展隨機樹算法[10]改善了算法的不確定性，提高了避障能力，但針對不同的障礙物步長無法確定；快速擴展隨機樹算法中引入了啟發式函數[11]，使得搜索樹在搜索過程中更具有導向性，但該算法在進行路徑規劃時容易陷入死循環。

本文基于快速擴展隨機樹提出了一種改進的智能車輛路徑規劃算法，采用循環交替迭代搜索方式生成新節點，雙向隨機樹同時擴展，增加車輛的轉彎角度約束，對生成的節點進行優化，對路徑進行平滑處理，改善了快速擴展隨機樹算法在路徑規劃時的不足。

2 基本快速擴展隨機樹算法

基本快速擴展隨機樹算法的擴展示意圖如圖1所示。以初始點xinit作為隨機樹的根節點，搜索自由空間，選取隨機采樣點xrand，在已知的隨機樹上，搜索選擇一個距離隨機采樣點xrand最近的節點xnearest，連接節點xnearest和節點xrand，以一定的步長ρ從xnearest出發生成一個新節點xnew，如果從xnearest到xnew的擴展過程中與障礙物無碰撞發生，則將這個新節點xnew加入到隨機樹中，生成一個隨機樹，當隨機樹中的子節點包含目標點xgoal時，便可以在隨機樹中生成一條由初始點xinit到目標點xgoal的路徑。反之若發生碰撞，則舍棄這次擴展。

Figure 1 Expansion of rapidly-exploring random tree algorithm圖1 基本快速擴展隨機樹算法的擴展

3 車輛轉向模型

智能車輛在行駛的過程中，不僅要保證車輛的行駛安全，還要保證車上乘客的乘坐舒適性，因此智能車輛在進行路徑規劃時，既要保證車輛規避所有的障礙物，又要保證規劃路徑的平滑性。

根據牛頓第二定律以及轉向的幾何關系可以推導出智能車輛進行穩態轉向的方程。為了便于分析智能車輛的轉向狀態，采用如圖2所示的兩輪模型。

Figure 2 Two-wheel steering model圖2 兩輪轉向模型

圖2中δ為前輪輪向角度，當前輪發生轉向時，車身產生離心力，在前后輪上產生的側向反作用力為Fy1和Fy2，并產生相應的側偏角αf和αr。設車輛的質心和轉動的瞬心分別為O和P，則2點間的距離R即為轉彎半徑。r為橫擺角速度，則質心處的速度為vc=rR。β為質心處的側偏角，即質心處車輛行駛方向與X軸之間的夾角。lf為前輪到質心O的距離，lr為后輪到質心O的距離，l為前輪到后輪的距離。前輪速度為vf，后輪速度為vr。

假定汽車只作平面運動，無垂直方向以及繞X軸和Y軸的側傾運動和俯仰運動。汽車速度為一個定值，并忽略空氣阻力和切向力對車輛的影響。以前輪轉角作為唯一輸入，忽略轉向系統對車輛的影響，不考慮左車輪由于載荷變化引起輪胎特性變化和回正力矩的作用，則vc在X軸上的分量為：

u=vccosβ

(1)

車輛在轉彎過程中的β很小，得到cosβ≈1,因此：

u=vc=rR

(2)

vc在Y軸上的分量為：

v=vcsinβ

(3)

由此得到質心的加速度ac為：

(4)

從力和力矩平衡方程式可導出微分運動方程式為：

(5)

(6)

其中，m為整車質量；Iz為車身繞Z軸的轉動慣量。

前后輪的側偏力分別為：

Fy1=Cfαf

(7)

Fy2=Crαr

(8)

其中，Cf、Cr分別為前后輪胎側偏剛度；αf、αr分別為前后輪側偏角。

由幾何關系可求得：

(9)

(10)

代入式(5)和式(6)可得：

(11)

(12)

由式(3)得到β≈sinβ=v/vc,代入式(11)和式(12)整理得到：

(13)

(14)

(15)

(16)

由式(15)和式(16)聯立并消去v，可得：

(17)

由式(17)可得：

(18)

穩定性因素K是影響轉向穩定性能的重要指標。

4 改進快速擴展隨機樹算法

4.1 循環交替迭代搜索

快速擴展隨機樹算法采用基于采樣的搜索方式，具有很強的隨機性。針對基本快速擴展隨機樹算法搜索效率低，搜索過程無導向性，收斂速度遲緩等缺點，本文對基本快速擴展隨機樹算法進行優化。

在生成新節點xnew時采用循環交替迭代的搜索方式：(1)快速擴展隨機樹在擴展過程中首先采用隨機采樣方式生成隨機采樣點xrand，將該隨機采樣點xrand作為隨機樹子節點生成的生長目標點；(2)接著直接將目標點xgoal作為隨機樹子節點的生長目標點。2種搜索方式循環交替迭代搜索，直到搜索到可行路徑，或者達到設定的迭代次數閾值，退出搜索，搜索路徑失敗。改進快速擴展隨機樹算法的流程圖如圖3所示。

Figure 3 Flow chart of improved rapidly-exploring random tree algorithm圖3 改進快速擴展隨機樹算法流程圖

4.2 雙向快速擴展隨機樹算法

針對基本快速擴展隨機樹算法的缺點，采用雙向快速擴展隨機樹算法進行搜索，雙向快速擴展隨機樹算法在自由空間中定義了2棵隨機樹，分別以起始點和目標點為隨機樹根節點，雙向擴展，直到2棵樹相遇為止，即找到了搜索的路徑。

以起始點為根節點的隨機樹搜索自由空間建立隨機樹的同時，以目標點為根節點的隨機樹也開始建立，2棵隨機樹交替生成新節點，檢測該新節點與另一棵隨機樹節點的歐氏距離是否小于設定的閾值。當2個節點的距離小于設定閾值時，將2個節點連接，即2棵隨機樹合并成1棵隨機樹，生成路徑。

4.3 增加角度約束

由車輛的轉向模型可知，穩定性因素K是影響轉向穩定性能的重要指標。K的取值分為3種情況：中性轉向(K=0)，不足轉向 (K>0)，過多轉向(K<0)。以中性轉向為基礎，由式(18)可得：

(19)

根據以上分析，車輛的前輪轉向角受轉彎半徑的影響，為了保證轉向的平穩順利進行，在快速擴展隨機樹進行路徑規劃時，新節點需要滿足一定的角度約束，使生成的路徑更加貼近車輛的運動需求。

設快速擴展隨機樹在生成新節點xnew的角度約束為φ，如圖4所示，當xnew、xnearest和xinit之間角度φ1小于約束的值φ時，則舍棄該生成的新節點，當φ2大于或等于約束的值φ時，則保留該新節點，并將新節點加入到隨機樹中。

Figure 4 Expansion of rapidly-exploring random tree with angle constraints圖4 增加角度約束的快速擴展隨機樹擴展示意圖

5 節點優化

快速擴展隨機樹算法以一定的步長在地圖中擴展，生成的路徑節點過多，路徑彎曲度較大，無法滿足車輛的實際行駛條件，因此對生成的路徑可行點進行優化，刪除多余的節點，以優化生成的路徑。

如圖5所示，數組A存放著通過快速擴展隨機樹算法搜索得到的可通行路徑的可行點，1表示起始點xinit，n表示目標點xgoal，2～(n-1)表示從起始點到目標點的可行點。

優化節點的思想如下所示：

(1)將初始點xinit加入到新的數組B中。

(2)判斷節點n和節點n+1之間有無障礙物，若沒有，則跳過節點n+1，判斷節點n和下一個節點n+2之間有無障礙物，若有障礙物，則將節點n+2加入到數組B中，以n+2為新的節點往后搜索，依此類推。

(3)當搜索到目標點xgoal時，結束搜索，將目標點加入到數組B中。數組B為優化后的節點集合。

Figure 5 Node optimization圖5 節點優化

6 路徑平滑處理

本文選擇B樣條曲線對快速擴展隨機樹規劃生成的路徑進行平滑處理。

通常給定q+n+1個頂點Pi(i=0,1,2,…,q+n),則可定義q+1段n次的參數曲線，B樣條曲線的表達式為：

(20)

其中，0≤t≤1,Pi+k(i=0,1,2,…,q;k=0,1,2,…,n)為控制頂點,n為規定的基函數的次數，Fk,n(t)為n次B樣條曲線基函數。

Fk,n(t)定義如下：

(21)

B樣條曲線是分段定義的。如果給定q+n+1個頂點Pi(i=0,1,2,…,q+n)，則可定義q+1段n次的參數曲線。

B樣條曲線具有幾何不變性、凸包性、保凸性、變差減小性等優點，且控制點的個數與函數的階數相關性小。因此，對于路徑的平滑處理具有良好的效果。

對于快速擴展隨機樹搜索生成的可行路徑，各個可行節點連接時生成折點，為了避免用B樣條曲線處理的路徑與障礙物發生碰撞，在折點的兩端分別生成局部端點，如圖6所示，An-1、An和An+1為路徑的可行點，在折點A2的兩端分別生成局部端點Bn1和Bn2。

Figure 6 Inserting local endpoints圖6 插入局部端點

由于折點所在的角度不同，采用自適應的方式選取局部端點Bn1和Bn2，通過點An-1和點An確定一次函數y(x)：

y(x)=A·x+B

(22)

其中，A和B為常數。

則Bn1的橫坐標滿足：

(23)

其中s為自適應系數。

將Bn1的橫坐標x(Bn1)代入式(22)得到y(Bn1)，求出點Bn1坐標，再求出點Bn2坐標。將局部端點Bn1和Bn2加入到可行點中，對路徑進行平滑處理。

7 仿真及分析

改進的快速擴展隨機樹算法是否符合要求，完成路徑規劃，智能車輛是否能夠準確到達設定好的目標位置，需要通過軟件來進行驗證與分析。本節利用Matlab軟件搭建仿真實驗平臺來對改進后的快速擴展隨機樹算法的正確性進行驗證分析，同時與基本的快速擴展隨機樹算法進行了對比驗證。仿真地圖為500*500的二維空間。

7.1 增加角度約束仿真結果

未增加角度約束的隨機樹擴展示意圖如圖7所示，增加角度約束的隨機樹擴展示意圖如圖8所示。

Figure 7 Simulation result of rapidly-exploring random tree expansion圖7 基本快速擴展隨機樹擴展仿真結果示意圖

Figure 8 Simulation result of random tree expansion with angle constraints圖8 增加角度約束的隨機樹擴展仿真結果示意圖

分析圖7和圖8可知，未增加角度約束的快速擴展隨機樹算法，生成的可行點之間的角度大小無法確定，過小的角度無法滿足車輛的運動學模型；增加了角度約束的快速擴展隨機樹，在擴展過程中可行點之間的角度更加平滑，增加的可行點之間的角度滿足約束要求，生成的路徑更加符合車輛的實際需求。

7.2 基本快速擴展隨機樹算法仿真結果

基本快速擴展隨機樹算法仿真結果如圖9所示。

Figure 9 Simulation result of rapidly-exploring random tree algorithm圖9 基本快速擴展隨機樹算法仿真結果

如圖9所示，起始點為(10，10)，目標點為(480，480)，基本快速擴展隨機樹算法在搜索過程中無導向性，隨機生成新節點，隨機性大，生成的路徑質量差，具有偏差性。

7.3 改進快速擴展隨機樹算法仿真結果

改進快速擴展隨機樹算法的仿真結果如圖10所示。

Figure 10 Simulation result of improved rapidly-exploring random tree algorithm圖10 改進快速擴展隨機樹算法仿真結果

節點優化仿真結果如圖11所示。

Figure 11 Simulation result of node optimization圖11 節點優化仿真結果

路徑平滑仿真結果如圖12所示。

Figure 12 Simulation result of path smoothing圖12 路徑平滑仿真結果

分析圖9和圖10可知，改進的快速擴展隨機樹算法采用循環交替迭代搜索方式，大大改善了基本快速擴展隨機樹算法的隨機性，減少了算法的迭代次數。分析圖10和圖11可知，圖10中未對節點進行優化，雖然生成了可行的路徑，但可行點較多，路徑長度較長，不是最優路徑；圖11中加入了節點優化，縮短了路徑的長度，提高了路徑的可行性。分析圖11和圖12可知，在折點兩端插入局部端點，使路徑折點處更加平滑，提高了快速擴展隨機樹算法的準確性，大大改善了路徑的質量，更加符合車輛行駛的實際條件。

改進的算法與基本快速擴展隨機樹算法在步長一致的前提下，分別仿真50次，計算仿真實驗的平均運行時間、平均路徑長度和平均迭代次數，結果如表1所示。分析表1可以得出，本文算法降低了基本快速擴展隨機樹算法的隨機性，路徑長度較短，減少了算法的迭代次數，加快了算法的收斂速度。改進的快速擴展隨機樹算法新生節點都朝向目標點生長，避免了全局搜索，提高了無人車運動的實時性，而且較易獲得最優路徑，改善了基本快速擴展隨機樹算法的偏差性。

Table 1 Data comparison

8 結束語

本文通過采用循環交替迭代的搜索方式生成新節點，采用雙向隨機樹同時搜索，建立車輛轉向模型，增加車輛的轉彎角度約束，改進了基本快速擴展隨機樹算法，解決了基本快速擴展隨機樹算法隨機性大、收斂速度慢和偏差性的問題。基于車輛轉向模型的算法，更加符合車輛行駛的實際情況，提高了可行性。對生成的路徑進行節點優化，縮短了路徑的長度，對路徑進行平滑度處理，使生成的路徑更加符合車輛的行駛條件。但是，無論是快速擴展隨機樹算法、粒子群算法等還是改進的快速擴展隨機樹算法都是一種模型驅動，有一定的局限性，需要進一步研究，結合數據驅動來進行智能車輛的路徑規劃及避障，最終使智能車輛的技術更加成熟。