陳 敏 李 笑 武交峰

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

0 引 言

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

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

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

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

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

1 差動機器人運動學(xué)模型

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

(1)

圖1 差動機器人運動學(xué)模型

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

dx·sinθ-dy·cosθ=0

(2)

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

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

2 改進(jìn)RRT算法

2.1 基本RRT算法

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

RRT算法在狀態(tài)空間C中構(gòu)造隨機樹T，以初始狀態(tài)qstart為根節(jié)點，在C中迭代隨機采樣節(jié)點qrand，以樹狀結(jié)構(gòu)擴展直至到達(dá)目標(biāo)狀態(tài)qgoal，然后由目標(biāo)狀態(tài)qgoal回溯到根節(jié)點qstart，得到規(guī)劃路徑。基本RRT算法偽代碼如算法1所示，其中：random_state()在C中隨機采樣節(jié)點qrand；nearest_neighbor()選取當(dāng)前樹T中距離qrand最近的節(jié)點qnear；steer()驅(qū)使qnear向qrand方向擴展一定步長到達(dá)節(jié)點qnew；collision()對qnear和qnew之間的節(jié)點進(jìn)行碰撞檢測，如果均在Cfree中，則將節(jié)點qnew及對應(yīng)的邊加入到樹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é)點qrand后，需通過最近鄰函數(shù)從樹T中找出距離采樣節(jié)點qrand最近的節(jié)點qnear。最近鄰函數(shù)是RRT算法的關(guān)鍵部分，決定著節(jié)點選取的合理性。差動機器人的路徑可用輪軸中點所行進(jìn)的曲線表示，如圖2所示，狀態(tài)間的最短路徑為一條直線(圖中虛線)，可用歐氏距離度量，但方向角也會影響兩車輪的轉(zhuǎn)動次數(shù)，合理的最近鄰函數(shù)需考慮方向角變化。

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

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

(3)

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

圖3 近鄰節(jié)點計算示意圖

2.3 啟發(fā)步長因子

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

(4)

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

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

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

(5)

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

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

(6)

圖4 節(jié)點擴展角度示意圖

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

圖5 節(jié)點擴展位置示意圖

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

(7)

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

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

2.4 路徑修剪與平滑

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

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

圖6 路徑修剪示意圖

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

(8)

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

(9)

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

(10)

3 仿真實驗

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

圖7為基本RRT算法和改進(jìn)RRT算法在同一任務(wù)下的規(guī)劃結(jié)果。仿真環(huán)境地圖大小為500×500，圖中左上角和右下角的差動機器人分別對應(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路徑對比

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

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

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

(b) 平均成功率

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

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

4 結(jié) 語

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