龍建全,張皓然,梁艷陽

(西南科技大學(xué)信息工程學(xué)院，四川綿陽 621010)

0 引言

在機器人運行的研究中，路徑規(guī)劃成為了研究的重點之一。路徑規(guī)劃即從初始位置找到一條通向目標(biāo)位置的無碰撞路徑，同時兼顧環(huán)境約束和移動體自身存在的約束[1]。傳統(tǒng)的路徑規(guī)劃算法有A*、人工勢場、蟻群算法等。而這些算法存在自身的問題，例如A*算法要求的計算量較大[2]，人工勢場法容易使機器人陷入局部最小值中[3]，蟻群算法收斂速度慢[4]。這些算法不能適應(yīng)多障礙物條件下的求解，不能適用于機器人的運行。許多學(xué)者在機器人領(lǐng)域，尤其在面向復(fù)雜環(huán)境下的路徑規(guī)劃成為當(dāng)今研究的重點范圍。S. M. LaValle等于1999年提出了基于隨機采樣的快速擴展隨機樹(Rapidly exploring Random Trees，RRT)[5]，相比于傳統(tǒng)的路徑規(guī)劃算法，它的優(yōu)勢能在復(fù)雜環(huán)境下找到一個收斂的解，不需要對工作空間做一些預(yù)處理，而傳統(tǒng)方法有些需要對環(huán)境做預(yù)處理，部分傳統(tǒng)算法不一定能找到收斂解。搜索速度相比傳統(tǒng)算法要快。但RRT算法也存在一些問題，例如均勻采樣搜索的范圍大搜索時間較長，得到收斂解的時間長。在一些狹窄路口不能找到解，容易陷入局部最小。傳統(tǒng)的RRT算法并沒有結(jié)合車輛的非完整約束，使得求出的路徑不可行。針對不同模型的車輛度量函數(shù)的選擇也會制約算法的搜索效率。算法生成的路徑不平滑，拐點眾多等問題。

針對上述問題，國內(nèi)外有學(xué)者提出相應(yīng)的優(yōu)化算法，W. Wang等提出了“橋測試”方法[6]，該方法主要通過在起點、終點和橋測試點同時生成樹，通過多樹的不斷結(jié)合生成路徑，但是在復(fù)雜環(huán)境下得到收斂解的時間過長。劉多能提出的方法是通過人為的主觀判斷，在狹窄路口設(shè)置虛擬點來引導(dǎo)擴展，雖然在狹窄路口數(shù)量少時，人為可以判斷，但是狹窄路口過多就不利于人為判斷，會造成算法收斂時間過長[7]。I.A.Sucan等人提出了在動力學(xué)模型的基礎(chǔ)上進(jìn)行動態(tài)規(guī)劃[8]。尹高揚提出了根據(jù)無人機動力學(xué)模型下的RRT擴展，應(yīng)用于非完整積分約束下的路徑規(guī)劃[9]。劉華偉提出了陷阱環(huán)境下設(shè)置虛擬目標(biāo)點的引導(dǎo)來解決局部最小的情況[10]。杜明博提出一種基于連續(xù)曲率的路徑規(guī)劃，讓汽車運動的曲率保持連續(xù)[11]。一些學(xué)者也結(jié)合一些擬合函數(shù)例如B樣條[11]、貝塞爾曲線[12]和樹枝修剪的后期處理技術(shù)來解決路徑不平滑的問題[13-14]。

本文針對多路口的復(fù)雜環(huán)境下小車的路徑規(guī)劃，提出一種人工設(shè)置多虛擬目標(biāo)引導(dǎo)區(qū)域的bi-RRT路徑規(guī)劃算法[15](bi-RRT with mutiply artifical guided region，bi-RRT-MAGR)。首先根據(jù)路口設(shè)置路標(biāo)點，在根據(jù)其連通域通過Dijkstra找出以這些點連接的最短路徑構(gòu)建采樣區(qū)域集合，滿足小車非完整約束的處理，來解決RRT存在不足的問題。最后通過仿真實驗來驗證其合理性、高效性和正確性。

1 非完整約束模型

非完整約束系統(tǒng)通常受到空間位置以及其運動速度的影響，使速度的積分不能轉(zhuǎn)換為系統(tǒng)空間的位置。由于其不可積的約束存在，使得其運動的控制與規(guī)劃變得復(fù)雜。對于一般的非完整約束系統(tǒng)：

(1)

(2)

則判定該系統(tǒng)為完整約束系統(tǒng)，否則為非完整約束系統(tǒng)[16-17]。

圖1 非完整約束小車模型

圖1為典型的非完整約束小車模型,(x,y)為當(dāng)前小車在全局坐標(biāo)系下的位置，θ為小車主軸方向與全局坐標(biāo)下x軸方向的夾角，L為小車車長(即前輪與后輪之間的長度)，ρ為小車的轉(zhuǎn)彎半徑，v為小車的速度，φ為小車與小車主軸方向的夾角(轉(zhuǎn)彎角)。小車的車輪只能做純滾動、無滑動的運動，無論任何時候小車的方向一定指向小車的主軸。在Δt近于零時，小車車體所受的約束為

(3)

也可以表示為

(4)

由圖可知，vi為車體速度的積分。

(5)

(6)

小車的狀態(tài)轉(zhuǎn)移方程如下：

(7)

(8)

小車的轉(zhuǎn)向角和速度必須滿足不能超過其最大轉(zhuǎn)向角和最大速度的約束條件(如式(8))。

通過結(jié)合非完整約束小車的模型，給定xt的位置、方向角以及轉(zhuǎn)向角，小車速度v，小車車長L和時間差 Δt=dt，得到xx+Δt的相應(yīng)的位置和方向角，生成方式如式(9)(函數(shù)generate)：

(9)

2 多引導(dǎo)域的bi-RRT算法

本文提出一種混合策略的bi-RRT，首先通過人為在狹窄路口的入出口設(shè)置虛擬目標(biāo)點，并用Dijkstra算法[18]根據(jù)這些虛擬目標(biāo)點找出他們最短路徑的集合并用作bi-RRT樹擴展中的采樣區(qū)域。該算法能使bi-RRT迅速通過狹窄路口和大幅減少算法的搜索時間，以提高算法的效率。

2.1 基本的RRT算法

X?Rd，X為工作空間,Xobs?X為障礙物區(qū)域，Xfree=X/Xobs為自由區(qū)域，Xinit為起始點位置，Xgoal為目標(biāo)點位置。Xnew為新生成節(jié)點，Xclosest為最臨近點。路徑規(guī)劃問題本質(zhì)在于找到一條collisionfree pathσ，v表示小車速度，連接閾值為threshold，連接角度閾值為thresholda ngle，V為節(jié)點集合，E為邊的集合，Tree為擴展樹。

Sample：采樣函數(shù)從Xfree返回一個獨立的值Xrand作為采樣點。

Nearest：給定一個點Xclosest∈V，使得T(Xclosest,Xrand)值最小。

Steer：擴展函數(shù)返回一個值Xnew，根據(jù)式(9)。

CollisionCheck ：檢測兩節(jié)點間是否有障礙物碰撞。給定2個點Vertex1，Vertex2，根據(jù)公式Vertex=λ·Vertex1+(1-λ)·Vertex2，λ∈[0,1]來判斷Vertex是否與障礙物碰撞。

PathToGoal：存在節(jié)點Xnew∈V，使得‖Xnew-Xgoal‖

Steer：函數(shù)通過最臨近點Xclosest，通過φ的增量變化，生成多條路徑path，再根據(jù)歐式距離找出離隨機點Xrand最近的一個點作為新節(jié)點Xnew。

RRT偽代碼如下：

V←{Xinit,Xgoal};E←φ；Tree←(V,E)；

fori=1 toNdo

Xrand←Sample；

Xclosest←Nearest(Xrand,Tree)；

Xnew←Steer(Xclosest，Xrand)；

if no CollisionCheck(Xclosest,Xnew)

Tree←Xnew∪Tree；

if PathToGoal(Xnew，Xgoal)

Tree←Xgoal∪Tree；

傳統(tǒng)的RRT是一種單查詢方式的搜索樹，通過給定的Xinit和Xgoal，在工作空間生成隨機點Xrand，根據(jù)歐氏距離找到樹上的最臨近點Xclosest，在以給定的參數(shù)生成新節(jié)點Xnew，通過不斷的迭代，直到新生成的Xnew與Xgoal的歐氏距離小于閾值時，通過Xgoal尋找其父節(jié)點回溯到Xinit，就可以得到一條完整路徑。

2.2 引導(dǎo)點的選擇

首先是通過人為主觀的在狹窄路口設(shè)置虛擬目標(biāo)點如圖2，0和9分別為起始點與目標(biāo)點，1～8是人為設(shè)置的虛擬目標(biāo)點。

圖2 人為設(shè)置的虛擬導(dǎo)航點示意圖

定義1 加入起始點和終點來構(gòu)建點之間的權(quán)值矩陣W，dij作為兩個連通域間的歐氏距離，用∞表示兩個非連通域，規(guī)定入口與入口不為連通域，入口與出口互為連通域(入口與出口間不能有其他入出口)。

定義2 在多路口環(huán)境下通過人為設(shè)置路標(biāo)點(Landmark)，通過Dijkstra找出節(jié)點間的最短路標(biāo)索引，可定義為向量Landmark={Lm1,Lm2,…,Lmn}，n為索引出來的路標(biāo)點個數(shù)：

Dijkstra算法步驟

(1)集合S={0}，0為起始點，V={0,1,2,…9}，U=V-S。

(2)從U中選取一個距離0結(jié)點最近的點k，把k加入到S，并從U中刪除。

(3)如果d0u>d0k+dku，則d0u=d0k+dku。

(4)重復(fù)步驟(2)、(3)直到所有點都在集合S中。

Dijkstra是一種根據(jù)點的連通域來求解的算法，通過輸入起始點和相應(yīng)的權(quán)值矩陣[18]。根據(jù)算法特性和多次的迭代求出一組最短路徑的點集合Landmark。

ForwardRegion(Landmark)函數(shù)：根據(jù)找出的Landmark，建立連續(xù)的采樣區(qū)間，當(dāng)擴展樹到達(dá)第1個節(jié)點時，僅在第1個節(jié)點與第2個節(jié)點間長l寬w的矩形范圍采樣。當(dāng)存在節(jié)點到達(dá)第2個節(jié)點時，則采用第2個節(jié)點與第3個節(jié)點間的矩形范圍采樣，依次類推(見圖3)。生成隨機點的角度與當(dāng)前矩形主軸的角度小于α?xí)r，成功生成隨機點，如圖4所示。

圖3 采樣區(qū)域連接圖

圖4 重疊采樣區(qū)域

ReverseRegion(Landmark)函數(shù)：根據(jù)找出的Landmark，建立連續(xù)的采樣區(qū)間，當(dāng)擴展樹到達(dá)倒數(shù)第1個節(jié)點時，僅在倒數(shù)第1個節(jié)點與倒數(shù)第2個節(jié)點間長l寬w的矩形范圍采樣。當(dāng)存在節(jié)點到達(dá)倒數(shù)第2個節(jié)點時，則采用倒數(shù)第2個節(jié)點與倒數(shù)第3個節(jié)點間的矩形范圍采樣，依次類推。生成隨機點的角度與當(dāng)前矩形主軸的角度小于α?xí)r，成功生成隨機點。

采樣函數(shù)Samplefrom Landmark偽代碼

if Treea

ForwardRegion(Landmark)

if abs(Sample-mainangle)<α

samplesuccess

if Treeb

ReverseRegion(Landmark)

if abs(SampleAngle-mainangle)<α

samplesuccess

根據(jù)上述Dijkstra找出的最短路標(biāo)集合，對于以起始點生長的樹Treea，選擇路標(biāo)集合Landmark中的第1個路標(biāo)和第2個路標(biāo)構(gòu)建采樣區(qū)域。當(dāng)Treea存在節(jié)點到第1個路標(biāo)點的歐氏距離小于閾值，則把第2個路標(biāo)點和第3個路標(biāo)構(gòu)建采樣區(qū)域，如果Treea存在節(jié)點到第2個路標(biāo)點的歐氏距離小于閾值，則把第3個路標(biāo)點和第4個路標(biāo)構(gòu)建區(qū)域，依次類推下去。對于以目標(biāo)點生成樹Treeb，則以最后1個路標(biāo)點和倒數(shù)第2個路標(biāo)點構(gòu)建采樣區(qū)域，當(dāng)Treeb存在節(jié)點到最后1個節(jié)點的歐氏距離小于閾值時，則以倒數(shù)第2個路標(biāo)點和倒數(shù)第3個路標(biāo)點構(gòu)建區(qū)域，依次類推，不斷地產(chǎn)生采樣空間。而本文提出的路標(biāo)點構(gòu)建的采樣空間，能引導(dǎo)機器人快速的向狹窄路口靠近并通過它同時大幅減少收斂時間，提高算法的效率。

2.3 bi-RRT-MAGR算法

通過上述對小車模型的分析、引導(dǎo)點的選擇、采樣空間的確定以及對基本的RRT算法的了解。提出了一種基于多引導(dǎo)點區(qū)域的bi-RRT算法。

bi-RRT-MAGR的偽代碼：

Va←{Xinit}；Vb←{Xgoal};Ea，Eb←φ；

Treea←(Va,Ea)；Treeb←(Vb,Eb)；

fori=1 to toNdo

if Threea

Xrand←SamplefromLandmark

Xclosest←Nearest(Xrand，Tree)；

Xnew←Steer(Xclosest，Xrand)；

if no CollisionCheck(Xclosest,Xnew)

Treea←Treea∪Xnew

if Treeb

Xrand←SamplefromLandmark

Xclosest←Nearest(Xrand,Tree)；

Xnew←Steer(Xclosest，Xrand)；

if no CollisionCheck(Xclosest,Xnew)

Treeb←Treeb∪Xnew

if PathToGoal(Treea,Treeb)

break

Tree←Treeb∪Treea

以起始點init開始構(gòu)建Treea和目標(biāo)點Xgoal構(gòu)建Treeb，設(shè)置最大迭代次數(shù)N，Treea和Treeb通過SamplefromLandmark函數(shù)獲取相應(yīng)的采樣區(qū)域，以Nearest函數(shù)找出相應(yīng)樹的最鄰近點，再根據(jù)最臨近點通過Steer函數(shù)不斷生成新節(jié)點，判斷新生成的節(jié)點與最臨近點之間是否存在碰撞，如果不存在則相應(yīng)的樹加入新節(jié)點，直到Treea和Treeb兩棵樹存在節(jié)點滿足函數(shù)PathToGoal并判斷兩點是否存在碰撞，若否定則搜索結(jié)束。在根據(jù)兩棵樹的連接點分別進(jìn)行父節(jié)點的索引直到回溯到起始點和目標(biāo)點，將兩棵樹的點集合合并，最后得到一條完整的路徑。

3 仿真實驗與分析

根據(jù)上述算法設(shè)計，本文在Inter Core i5-7500，主頻3.4 GHz PC機上采用MATLAB 2016a上進(jìn)行算法編程仿真測試。設(shè)定小車速度為v=5 m/s，時間差Δt=0.5 s，小車車長L=7.5 m，最大轉(zhuǎn)彎角φmax=0.7rad。地圖1的起始點(400，50)，目標(biāo)點(50，500)。地圖2的起始點(400，50)，目標(biāo)點(50，950)。圖5、圖6圖中，地圖(0，0)位置在左上角，豎直向下為x軸正方向，橫向向右為y軸正方向,通過Dijkstra找出的路標(biāo)索引點見表1。

表1 通過Dijkstra找出的路標(biāo)索引點

圖5 地圖1情況下算法生成路徑圖

圖6 地圖2情況下生成路徑圖

在圖5和圖6，左邊為搜索路徑，右邊為最終路徑。由圖5對比得，在地圖1的情況下，狹窄路口數(shù)量不多，RRT在搜索范圍幾乎充滿整個空間，搜索時間相對較長，bi-RRT相對于RRT在搜索效率上和收斂次數(shù)上提升了1倍，通過表2得出本文改進(jìn)的算法，在搜索效率上相對bi-RRT提高了1倍。由圖6對比，在地圖2的情況下RRT搜索由于不能正確的找到路口，路徑會搜索更長，而本文提出的算法就是按照Dijkstra索引出的最短路標(biāo)點構(gòu)建的采樣區(qū)域，在路徑搜索上相對于RRT、bi-RRT要短一點，主要原因是RRT，bi-RRT在搜索上具有隨機性，他們既能向其他距離比較遠(yuǎn)的路口搜索，也可以向距離較近的搜索，而本文會向路標(biāo)索引處大范圍搜索，所以平均距離短一點。通過表3在搜索效率上相比于RRT提高了7～8倍，相對bi-RRT提高了3～4倍。根據(jù)地圖1與地圖2的仿真實驗，本文所提算法相對其他兩種算法在搜索效率和收斂次數(shù)更加高效和合理。

表2 地圖1情況下算法對比

表3 地圖2情況下算法對比圖

4 結(jié)論

本文針對非完整約束小車的路徑規(guī)劃和RRT算法所出現(xiàn)的隨機性太強，獲得路徑的時間長，搜索范圍廣和收斂慢等問題，提出了基于虛擬目標(biāo)區(qū)域?qū)Ш降腷i-RRT算法，該算法繼承了bi-RRT的隨機性、兼顧小車的非完整約束。通過多路標(biāo)區(qū)域的引導(dǎo)使算法不會在路口等狹窄位置重復(fù)搜索，使搜索快速通過路口，最后產(chǎn)生合理的路徑，使得規(guī)劃出來的路徑更加滿足實際的需要。通過仿真實驗驗證該算法的有效性，對于提高小車在實際路徑規(guī)劃具有實際意義。