陳 敏 李 笑 武交峰

(廣東工業(yè)大學(xué)機(jī)電工程學(xué)院 廣東 廣州 510006)

0 引 言

差動(dòng)機(jī)器人作為一種移動(dòng)機(jī)器人，在物流、交通、軍事等領(lǐng)域應(yīng)用廣泛。路徑規(guī)劃是實(shí)現(xiàn)機(jī)器人智能化的關(guān)鍵技術(shù)，其旨在為機(jī)器人規(guī)劃出一條從初始狀態(tài)到目標(biāo)狀態(tài)的符合約束條件的路徑[1]。傳統(tǒng)規(guī)劃算法如A*算法[2]、人工勢(shì)場(chǎng)法[3]、遺傳算法[4]、粒子群算法[5]等對(duì)于復(fù)雜環(huán)境下受各種約束的機(jī)器人效率低下，求解困難[6]。

LaValle提出快速擴(kuò)展隨機(jī)樹(shù)(RRT)算法[7]，它是一種基于隨機(jī)采樣的規(guī)劃算法，無(wú)需對(duì)狀態(tài)空間顯示建模，規(guī)劃速度快且能考慮機(jī)器人的各種約束，對(duì)解決復(fù)雜環(huán)境下的路徑規(guī)劃問(wèn)題表現(xiàn)出極大的優(yōu)越性[8]。但RRT算法也存在一些固有缺陷，如最近鄰函數(shù)不合理、收斂速度慢、生成的路徑曲折等[9]。

針對(duì)上述不足，國(guó)內(nèi)外學(xué)者對(duì)RRT算法進(jìn)行了各種改進(jìn)，以適用于不同場(chǎng)景。在RRT算法中，理想的最近鄰函數(shù)需解決兩點(diǎn)邊界值問(wèn)題，難以準(zhǔn)確定義[8]，Perez等[10]提出使用LQR控制器作為最近鄰函數(shù)，但求解最優(yōu)控制比較耗時(shí)，不利于實(shí)時(shí)計(jì)算。為了加快收斂速度，Kuffner和LaValle先后提出bi-RRT算法[11]和

RRT-connect算法[12]。bi-RRT算法同時(shí)以初始狀態(tài)和目標(biāo)狀態(tài)為根節(jié)點(diǎn)構(gòu)建兩棵樹(shù)，交替向?qū)Ψ綌U(kuò)展，提高了算法的搜索效率；RRT-connect算法在bi-RRT算法基礎(chǔ)上改進(jìn)了擴(kuò)展步長(zhǎng)以提高節(jié)點(diǎn)擴(kuò)展效率，但同時(shí)增加了每次節(jié)點(diǎn)擴(kuò)展的碰撞檢測(cè)次數(shù)。文獻(xiàn)[13]中引入目標(biāo)偏向RRT算法，節(jié)點(diǎn)以固定概率向目標(biāo)點(diǎn)方向擴(kuò)展，減小了算法的隨機(jī)性，提高了算法的搜索效率，但在復(fù)雜場(chǎng)景中由于探索不足易陷入局部最小點(diǎn)。對(duì)于隨機(jī)采樣導(dǎo)致的路徑曲折問(wèn)題，文獻(xiàn)[14]使用回旋曲線對(duì)初始路徑進(jìn)行平滑，但回旋曲線不存在閉合解，無(wú)法實(shí)時(shí)準(zhǔn)確獲取。文獻(xiàn)[15]采用貝塞爾曲線平滑，但貝塞爾曲線階數(shù)取決于路徑點(diǎn)個(gè)數(shù)，無(wú)法靈活設(shè)置。

針對(duì)RRT算法在差動(dòng)機(jī)器人路徑規(guī)劃中存在的上述問(wèn)題，本文提出一種改進(jìn)RRT算法。首先，考慮差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)約束，在最近鄰函數(shù)中加入角度變化，使規(guī)劃路徑趨于平緩；其次，在節(jié)點(diǎn)擴(kuò)展階段引入啟發(fā)步長(zhǎng)因子，使擴(kuò)展步長(zhǎng)根據(jù)節(jié)點(diǎn)位置和擴(kuò)展方向動(dòng)態(tài)調(diào)整，加快收斂速度的同時(shí)兼顧規(guī)劃成功率；最后，對(duì)規(guī)劃路徑進(jìn)行修剪和平滑處理，以生成差動(dòng)機(jī)器人易于跟蹤控制的路徑。通過(guò)仿真實(shí)驗(yàn)，證實(shí)了該算法的優(yōu)越性和實(shí)用性。

1 差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

差動(dòng)機(jī)器人的簡(jiǎn)化運(yùn)動(dòng)學(xué)模型如圖1所示。差動(dòng)機(jī)器人的狀態(tài)可表示為X=(x,y,θ)，其中：(x,y)為車(chē)輪軸中點(diǎn)在系統(tǒng)坐標(biāo)系下的位置；θ為機(jī)器人行進(jìn)方向與X軸夾角。給定兩個(gè)車(chē)輪的距離L、車(chē)輪半徑r、角速度u=(ul,ur)，其中ul和ur分別為左右兩車(chē)輪角速度，如果ul=ur>0，機(jī)器人向前直行；如果ul=-ur≠0，機(jī)器人沿順時(shí)針?lè)较蛐D(zhuǎn)。差動(dòng)機(jī)器人車(chē)輪只能前后滾動(dòng)，不能側(cè)滑，在運(yùn)動(dòng)過(guò)程中滿足：

(1)

圖1 差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型

非完整約束是指含有系統(tǒng)廣義坐標(biāo)導(dǎo)數(shù)且不可積分的約束[16]。差動(dòng)機(jī)器人是一個(gè)典型的非完整約束系統(tǒng)[17]。由式(1)可知其存在非完整性約束：

dx·sinθ-dy·cosθ=0

(2)

差動(dòng)機(jī)器人狀態(tài)空間為3維，該約束將其可控維度變?yōu)?維。

對(duì)差動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)模型分析可知，路徑規(guī)劃算法需考慮機(jī)器人自身約束，否則所得路徑可能無(wú)法應(yīng)用于實(shí)際環(huán)境中。

2 改進(jìn)RRT算法

2.1 基本RRT算法

機(jī)器人路徑規(guī)劃問(wèn)題可作如下定義[18]：將機(jī)器人狀態(tài)空間表示為C=[0,1]d，其中維度d∈N,d≥2。令Cobs為障礙物空間，則自由空間可表示為Cfree=C/Cobs。機(jī)器人初始狀態(tài)qstart和目標(biāo)狀態(tài)qgoal位于自由空間Cfree中，路徑規(guī)劃問(wèn)題可抽象為一個(gè)三元組(Cfree,qstart,qgoal)，即在自由空間Cfree中找到一條從初始狀態(tài)qstart到目標(biāo)狀態(tài)qgoal的路徑。路徑可表示為函數(shù)σ:[0,1]→Rd，如果對(duì)于路徑中所有的τ∈[0,1]，均有σ(τ)∈Cfree,則稱(chēng)其為無(wú)碰撞路徑；對(duì)于無(wú)碰撞路徑，如果其中σ(0)=qstart、σ(1)=qgoal，則稱(chēng)其為可行路徑。

RRT算法在狀態(tài)空間C中構(gòu)造隨機(jī)樹(shù)T，以初始狀態(tài)qstart為根節(jié)點(diǎn)，在C中迭代隨機(jī)采樣節(jié)點(diǎn)qrand，以樹(shù)狀結(jié)構(gòu)擴(kuò)展直至到達(dá)目標(biāo)狀態(tài)qgoal，然后由目標(biāo)狀態(tài)qgoal回溯到根節(jié)點(diǎn)qstart，得到規(guī)劃路徑。基本RRT算法偽代碼如算法1所示，其中：random_state()在C中隨機(jī)采樣節(jié)點(diǎn)qrand；nearest_neighbor()選取當(dāng)前樹(shù)T中距離qrand最近的節(jié)點(diǎn)qnear；steer()驅(qū)使qnear向qrand方向擴(kuò)展一定步長(zhǎng)到達(dá)節(jié)點(diǎn)qnew；collision()對(duì)qnear和qnew之間的節(jié)點(diǎn)進(jìn)行碰撞檢測(cè)，如果均在Cfree中，則將節(jié)點(diǎn)qnew及對(duì)應(yīng)的邊加入到樹(shù)T中。

算法1RRT算法

1.T.init(qstart);

2. fork=1 toKdo

3.qrand←random_state();

4.qnear←nearest_neighbor(qrand,T);

5.qnew←steer(qnew,qrand);

6. if !collision(qnear,qnew)

7.T.add_node(qnew);

8.T.add_edge(qnear,qnew);

9. end if

10. end for

11. ReturnT

2.2 最近鄰函數(shù)

RRT算法采樣節(jié)點(diǎn)qrand后，需通過(guò)最近鄰函數(shù)從樹(shù)T中找出距離采樣節(jié)點(diǎn)qrand最近的節(jié)點(diǎn)qnear。最近鄰函數(shù)是RRT算法的關(guān)鍵部分，決定著節(jié)點(diǎn)選取的合理性。差動(dòng)機(jī)器人的路徑可用輪軸中點(diǎn)所行進(jìn)的曲線表示，如圖2所示，狀態(tài)間的最短路徑為一條直線(圖中虛線)，可用歐氏距離度量，但方向角也會(huì)影響兩車(chē)輪的轉(zhuǎn)動(dòng)次數(shù)，合理的最近鄰函數(shù)需考慮方向角變化。

圖2 差動(dòng)機(jī)器人直線行進(jìn)示意圖

基本RRT算法采用歐式距離作為最近鄰函數(shù)，未考慮差動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)約束，選取的近鄰節(jié)點(diǎn)qnear不合理。因此，本文考慮差動(dòng)機(jī)器人方向角變化，使用一種能更合理度量其狀態(tài)間代價(jià)的最近鄰函數(shù)：

(3)

式中：d(qrand,qi)為采樣點(diǎn)qrand與節(jié)點(diǎn)qi間的歐式距離；dmax為qrand與樹(shù)中節(jié)點(diǎn)距離最大值；w1和w2為歐式距離與角度的加權(quán)因子，可根據(jù)實(shí)際需求設(shè)定。如圖3所示，qip為節(jié)點(diǎn)qi的父節(jié)點(diǎn)，φ為qipqi與qiqrand的夾角。

圖3 近鄰節(jié)點(diǎn)計(jì)算示意圖

2.3 啟發(fā)步長(zhǎng)因子

在RRT算法節(jié)點(diǎn)擴(kuò)展步驟中，qnew由qnear向qrand擴(kuò)展一定步長(zhǎng)得到，具體擴(kuò)展表達(dá)式為：

(4)

式中：step為擴(kuò)展步長(zhǎng)；d(qrand,qnear)為qrand和qnear狀態(tài)點(diǎn)的歐式距離。

基本RRT算法在環(huán)境中均勻采樣且擴(kuò)展步長(zhǎng)step固定，隨機(jī)性大，收斂速度慢。調(diào)整采樣偏向是加快收斂速度的常用方法，如目標(biāo)偏向RRT算法[13]以固定概率向目標(biāo)點(diǎn)方向擴(kuò)展，能顯著提高搜索效率，在無(wú)障礙物環(huán)境下得到的路徑趨于理想路徑。但在實(shí)際的規(guī)劃任務(wù)中，障礙物或多或少且分布多樣，調(diào)整采樣偏向會(huì)降低算法的探索能力，容易導(dǎo)致規(guī)劃失敗。

為了提高算法搜索效率，同時(shí)保證規(guī)劃成功率，本文保留基本RRT算法全局均勻采樣策略，在其節(jié)點(diǎn)擴(kuò)展步驟中引入啟發(fā)步長(zhǎng)因子γ，使步長(zhǎng)根據(jù)節(jié)點(diǎn)位置和擴(kuò)展方向動(dòng)態(tài)調(diào)整，具體計(jì)算步驟為:

(5)

式中：α為qstart與qgoal關(guān)于qnear的夾角，如圖4所示；Δd為qnear與起始點(diǎn)qstart和目標(biāo)點(diǎn)qgoal之間的距離差：

Δd=|d(qnear,qstart)-d(qnear,qgoal)|

(6)

圖4 節(jié)點(diǎn)擴(kuò)展角度示意圖

如圖5所示，當(dāng)qnear位于雙曲線上時(shí)，Δd滿足圖中所示關(guān)系，qnear在不同位置對(duì)應(yīng)不同步長(zhǎng)。

圖5 節(jié)點(diǎn)擴(kuò)展位置示意圖

由于角度α與距離Δd量綱不同，W(α)與D(Δd)分別對(duì)其作歸一化處理：

(7)

λ1與λ2分別為計(jì)算啟發(fā)步長(zhǎng)因子γ時(shí)W(α)和H(Δd)對(duì)應(yīng)的權(quán)重，為使后續(xù)實(shí)驗(yàn)中算法易于對(duì)比，取λ1+λ2=2，以使平均啟發(fā)步長(zhǎng)與固定步長(zhǎng)step一致。

由式(5)-式(7)可知，啟發(fā)步長(zhǎng)因子γ對(duì)應(yīng)取值范圍為[0,2]，假定λ1=λ2=1，當(dāng)節(jié)點(diǎn)qnear往目標(biāo)點(diǎn)qgoal銳角方向擴(kuò)展(即α<π/2)且同時(shí)距離初始點(diǎn)qstart和目標(biāo)點(diǎn)qgoal較遠(yuǎn)(即位于圖5中陰影部分以外)時(shí)，啟發(fā)步長(zhǎng)因子γ>1，啟發(fā)步長(zhǎng)大于固定步長(zhǎng)step，節(jié)點(diǎn)加速向目標(biāo)點(diǎn)區(qū)域擴(kuò)展。此外，當(dāng)節(jié)點(diǎn)qnear向目標(biāo)點(diǎn)qgoal鈍角方向(即α>π/2)擴(kuò)展且距離初始點(diǎn)qstart或目標(biāo)點(diǎn)qgoal較近(即位于圖5中陰影部分)時(shí)步長(zhǎng)因子γ<1，可提高節(jié)點(diǎn)擴(kuò)展的成功率，同時(shí)緩解目標(biāo)點(diǎn)qgoal附近路徑的抖動(dòng)。

2.4 路徑修剪與平滑

由于RRT算法采樣隨機(jī)，生成的路徑曲折，存在許多不必要的節(jié)點(diǎn)。在障礙物密集的復(fù)雜環(huán)境下，大量的冗余節(jié)點(diǎn)讓差動(dòng)機(jī)器人難以跟蹤。因此，為了得到差動(dòng)機(jī)器人的可執(zhí)行路徑，本文對(duì)生成的初始路徑進(jìn)行修剪，去除不必要的節(jié)點(diǎn)，然后利用B樣條曲線對(duì)修剪后的路徑平滑處理。

具體修剪步驟如圖6所示，對(duì)初始規(guī)劃路徑點(diǎn)集Q，從初始點(diǎn)qstart開(kāi)始，依次遍歷后序路徑點(diǎn)qi，如果兩個(gè)路徑點(diǎn)連線與障礙物無(wú)碰撞，則刪去qstart與qi之間路徑點(diǎn)；如果發(fā)生碰撞，則從碰撞點(diǎn)的父節(jié)點(diǎn)開(kāi)始重復(fù)上述過(guò)程，直至遍歷到目標(biāo)點(diǎn)qgoal，得到剩余路徑點(diǎn)集P′。

圖6 路徑修剪示意圖

B樣條曲線曲率連續(xù)，可根據(jù)控制點(diǎn)局部調(diào)整，在路徑規(guī)劃中應(yīng)用廣泛[9]。因此，本文利用它對(duì)修剪后的路徑點(diǎn)集P′進(jìn)行平滑，以生成差動(dòng)機(jī)器人易執(zhí)行的平滑路徑。k次B樣條曲線可表示為：

(8)

式中：Pi為第i個(gè)控制點(diǎn)的坐標(biāo)值；Bi,k(u)為k次B樣條基函數(shù)，是由節(jié)點(diǎn)向量U=[u0,u1,u2,…,un+k+1]決定的k次分段多項(xiàng)式，可由cox-deBoor遞推公式得到：

(9)

為了使B樣條曲線經(jīng)過(guò)初始點(diǎn)和目標(biāo)點(diǎn)，節(jié)點(diǎn)向量需滿足：

(10)

3 仿真實(shí)驗(yàn)

為了驗(yàn)證改進(jìn)RRT算法的優(yōu)越性和實(shí)用性，進(jìn)行了仿真實(shí)驗(yàn)。鑒于目標(biāo)偏向RRT算法近年被廣泛應(yīng)用，本文將改進(jìn)RRT算法與基本RRT算法、目標(biāo)偏向RRT算法進(jìn)行了性能對(duì)比。仿真實(shí)驗(yàn)通過(guò)C++和QT編程實(shí)現(xiàn)，在個(gè)人PC上完成，操作系統(tǒng)為Ubuntu，處理器為i7，運(yùn)行內(nèi)存為8 GB。

圖7為基本RRT算法和改進(jìn)RRT算法在同一任務(wù)下的規(guī)劃結(jié)果。仿真環(huán)境地圖大小為500×500，圖中左上角和右下角的差動(dòng)機(jī)器人分別對(duì)應(yīng)其初始狀態(tài)和目標(biāo)狀態(tài)。(a)為基本RRT算法規(guī)劃路徑；(b)為改進(jìn)RRT算法的初始規(guī)劃路徑；(c)為改進(jìn)RRT算法修剪后的路徑；(d)為改進(jìn)RRT算法平滑后的路徑。

(a) 基本RRT路徑 (b) 改進(jìn)RRT路徑

(c) 改進(jìn)RRT路徑修剪 (d) 改進(jìn)RRT路徑平滑圖7 基本RRT與改進(jìn)RRT路徑對(duì)比

從圖7的規(guī)劃路徑可以看出，改進(jìn)RRT算法最近鄰函數(shù)由于考慮了角度變化，節(jié)點(diǎn)擴(kuò)展更為平緩，修剪后的路徑去除了多余的節(jié)點(diǎn)，經(jīng)B樣條曲線平滑后曲率連續(xù)，滿足差動(dòng)機(jī)器人的運(yùn)動(dòng)學(xué)約束，易于跟蹤。

為了分析改進(jìn)RRT算法在復(fù)雜環(huán)境下的規(guī)劃性能，在0～60%(包含兩端)間每隔10%設(shè)置了7個(gè)不同障礙物占比的地圖環(huán)境，比較基本RRT、目標(biāo)偏向RRT、改進(jìn)RRT三種算法在這些地圖環(huán)境中的規(guī)劃結(jié)果。由于算法具有隨機(jī)性，為保證規(guī)劃算法評(píng)價(jià)客觀，每個(gè)地圖環(huán)境分別對(duì)三種算法各進(jìn)行了20次測(cè)試，最大迭代次數(shù)為10 000次，步長(zhǎng)step為3。圖8(a)-(c)分別為三種算法在20次測(cè)試下的規(guī)劃時(shí)間、成功率、迭代次數(shù)平均值。

(a) 平均規(guī)劃時(shí)間

(b) 平均成功率

(c) 平均迭代次數(shù)圖8 三種算法規(guī)劃結(jié)果對(duì)比

從圖8結(jié)果可以看出，在高障礙物占比的復(fù)雜環(huán)境下，與基本RRT算法相比，改進(jìn)RRT算法縮短了規(guī)劃時(shí)間和迭代次數(shù)，且成功率高于目標(biāo)偏向RRT算法，提高差動(dòng)機(jī)器人路徑搜索效率的同時(shí)兼顧了搜索成功率。

4 結(jié) 語(yǔ)

RRT算法由于存在最近鄰函數(shù)不合理、收斂速度慢、路徑曲折等問(wèn)題，其難以滿足差動(dòng)機(jī)器人的實(shí)際應(yīng)用需求。對(duì)此，本文提出了一種改進(jìn)RRT算法。仿真實(shí)驗(yàn)表明，該算法提高了路徑搜索效率，規(guī)劃的路徑更為平滑，可進(jìn)一步滿足復(fù)雜環(huán)境下差動(dòng)機(jī)器人路徑規(guī)劃的實(shí)時(shí)性和穩(wěn)定性要求。