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

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

1 引言

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

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

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

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

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

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

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

3 車輛轉(zhuǎn)向模型

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

根據(jù)牛頓第二定律以及轉(zhuǎn)向的幾何關(guān)系可以推導出智能車輛進行穩(wěn)態(tài)轉(zhuǎn)向的方程。為了便于分析智能車輛的轉(zhuǎn)向狀態(tài)，采用如圖2所示的兩輪模型。

Figure 2 Two-wheel steering model圖2 兩輪轉(zhuǎn)向模型

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

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

u=vccosβ

(1)

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

u=vc=rR

(2)

vc在Y軸上的分量為：

v=vcsinβ

(3)

由此得到質(zhì)心的加速度ac為：

(4)

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

(5)

(6)

其中，m為整車質(zhì)量；Iz為車身繞Z軸的轉(zhuǎn)動慣量。

前后輪的側(cè)偏力分別為：

Fy1=Cfαf

(7)

Fy2=Crαr

(8)

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

由幾何關(guān)系可求得：

(9)

(10)

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

(11)

(12)

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

(13)

(14)

(15)

(16)

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

(17)

由式(17)可得：

(18)

穩(wěn)定性因素K是影響轉(zhuǎn)向穩(wěn)定性能的重要指標。

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

4.1 循環(huán)交替迭代搜索

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

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

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

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

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

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

4.3 增加角度約束

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

(19)

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

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

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

5 節(jié)點優(yōu)化

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

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

優(yōu)化節(jié)點的思想如下所示：

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

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

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

Figure 5 Node optimization圖5 節(jié)點優(yōu)化

6 路徑平滑處理

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

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

(20)

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

Fk,n(t)定義如下：

(21)

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

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

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

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

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

y(x)=A·x+B

(22)

其中，A和B為常數(shù)。

則Bn1的橫坐標滿足：

(23)

其中s為自適應(yīng)系數(shù)。

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

7 仿真及分析

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

7.1 增加角度約束仿真結(jié)果

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

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

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

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

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

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

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

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

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

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

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

節(jié)點優(yōu)化仿真結(jié)果如圖11所示。

Figure 11 Simulation result of node optimization圖11 節(jié)點優(yōu)化仿真結(jié)果

路徑平滑仿真結(jié)果如圖12所示。

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

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

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

Table 1 Data comparison

8 結(jié)束語

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