基于雙向隨機(jī)樹(shù)改進(jìn)的智能車輛路徑規(guī)劃研究*

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

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

1 引言

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

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

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

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

2 基本快速擴(kuò)展隨機(jī)樹(shù)算法

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

Figure 1 Expansion of rapidly-exploring random tree algorithm圖1 基本快速擴(kuò)展隨機(jī)樹(shù)算法的擴(kuò)展

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

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

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

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

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

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

u=vccosβ

(1)

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

u=vc=rR

(2)

vc在Y軸上的分量為：

v=vcsinβ

(3)

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

(4)

從力和力矩平衡方程式可導(dǎo)出微分運(yùn)動(dòng)方程式為：

(5)

(6)

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

前后輪的側(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)定性能的重要指標(biāo)。

4 改進(jìn)快速擴(kuò)展隨機(jī)樹(shù)算法

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

快速擴(kuò)展隨機(jī)樹(shù)算法采用基于采樣的搜索方式，具有很強(qiáng)的隨機(jī)性。針對(duì)基本快速擴(kuò)展隨機(jī)樹(shù)算法搜索效率低，搜索過(guò)程無(wú)導(dǎo)向性，收斂速度遲緩等缺點(diǎn)，本文對(duì)基本快速擴(kuò)展隨機(jī)樹(shù)算法進(jìn)行優(yōu)化。

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

Figure 3 Flow chart of improved rapidly-exploring random tree algorithm圖3 改進(jìn)快速擴(kuò)展隨機(jī)樹(shù)算法流程圖

4.2 雙向快速擴(kuò)展隨機(jī)樹(shù)算法

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

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

4.3 增加角度約束

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

(19)

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

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

Figure 4 Expansion of rapidly-exploring random tree with angle constraints圖4 增加角度約束的快速擴(kuò)展隨機(jī)樹(shù)擴(kuò)展示意圖

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

快速擴(kuò)展隨機(jī)樹(shù)算法以一定的步長(zhǎng)在地圖中擴(kuò)展，生成的路徑節(jié)點(diǎn)過(guò)多，路徑彎曲度較大，無(wú)法滿足車輛的實(shí)際行駛條件，因此對(duì)生成的路徑可行點(diǎn)進(jìn)行優(yōu)化，刪除多余的節(jié)點(diǎn)，以優(yōu)化生成的路徑。

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

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

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

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

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

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

6 路徑平滑處理

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

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

(20)

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

Fk,n(t)定義如下：

(21)

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

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

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

Figure 6 Inserting local endpoints圖6 插入局部端點(diǎn)

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

y(x)=A·x+B

(22)

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

則Bn1的橫坐標(biāo)滿足：

(23)

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

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

7 仿真及分析

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

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

未增加角度約束的隨機(jī)樹(shù)擴(kuò)展示意圖如圖7所示，增加角度約束的隨機(jī)樹(shù)擴(kuò)展示意圖如圖8所示。

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

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

分析圖7和圖8可知，未增加角度約束的快速擴(kuò)展隨機(jī)樹(shù)算法，生成的可行點(diǎn)之間的角度大小無(wú)法確定，過(guò)小的角度無(wú)法滿足車輛的運(yùn)動(dòng)學(xué)模型；增加了角度約束的快速擴(kuò)展隨機(jī)樹(shù)，在擴(kuò)展過(guò)程中可行點(diǎn)之間的角度更加平滑，增加的可行點(diǎn)之間的角度滿足約束要求，生成的路徑更加符合車輛的實(shí)際需求。

7.2 基本快速擴(kuò)展隨機(jī)樹(shù)算法仿真結(jié)果

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

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

如圖9所示，起始點(diǎn)為(10，10)，目標(biāo)點(diǎn)為(480，480)，基本快速擴(kuò)展隨機(jī)樹(shù)算法在搜索過(guò)程中無(wú)導(dǎo)向性，隨機(jī)生成新節(jié)點(diǎn)，隨機(jī)性大，生成的路徑質(zhì)量差，具有偏差性。

7.3 改進(jìn)快速擴(kuò)展隨機(jī)樹(shù)算法仿真結(jié)果

改進(jìn)快速擴(kuò)展隨機(jī)樹(shù)算法的仿真結(jié)果如圖10所示。

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

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

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

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

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

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

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

Table 1 Data comparison

8 結(jié)束語(yǔ)

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