李金良，舒翰儒，劉德建，徐 磊

(山東科技大學(xué)機(jī)械電子工程學(xué)院，山東 青島 266560)

0 引言

隨著人工智能的不斷推進(jìn)，機(jī)器人技術(shù)得到了飛速的發(fā)展，智能化成為未來機(jī)器人領(lǐng)域的新航標(biāo)。其中，機(jī)器人技術(shù)重要的研究領(lǐng)域—路徑規(guī)劃，也開始迅速發(fā)展。

路徑規(guī)劃主要研究在障礙物空間環(huán)境下，機(jī)器人自主搜尋一條從起點(diǎn)到目標(biāo)點(diǎn)的可行路徑或最優(yōu)路徑[1]。機(jī)器人的路徑規(guī)劃算法有圖搜索法[2]、可視圖法[3]、人工勢(shì)場(chǎng)法[4]、概率路圖法[5]、快速擴(kuò)展隨機(jī)樹法等。因快速擴(kuò)展隨機(jī)樹法更適合移動(dòng)機(jī)器人和多自由度工業(yè)機(jī)器人路徑規(guī)劃而被廣泛應(yīng)用及改進(jìn)。

王中玉等[6]改進(jìn)傳統(tǒng)A*算法的評(píng)價(jià)函數(shù)，減少了一定的計(jì)算量；通過改變?cè)u(píng)價(jià)函數(shù)的權(quán)重比例，減少了不必要的點(diǎn)的搜索，但在三維空間中依然存在計(jì)算量大的問題。宋宇等[7]通過人工勢(shì)場(chǎng)法和RRT算法相結(jié)合，以合力方向引導(dǎo)搜索樹的擴(kuò)張，可以減少采樣點(diǎn)的個(gè)數(shù)和搜索路徑長(zhǎng)度，但搜索時(shí)間明顯增加。代彥輝等[8]通過引入起始點(diǎn)到目標(biāo)點(diǎn)的方向向量，使得搜索樹擴(kuò)展具有方向性，雖然大大減少了算法的迭代次數(shù)和搜索時(shí)間，但極容易陷入障礙物包圍圈內(nèi)，從而導(dǎo)致算法的失敗。

本文提出一種目標(biāo)偏向的RRT算法，結(jié)合上述一些算法作出以下改進(jìn)：基本RRT算法搜索具有盲目性，因此引入目標(biāo)偏向策略[9]，以減少采樣點(diǎn)的數(shù)目；通過對(duì)采樣點(diǎn)距離和速度約束，減少搜索范圍，提高計(jì)算效率；對(duì)規(guī)劃后的路徑采用貪心算法[10]簡(jiǎn)化路徑并對(duì)其用三次B樣條曲線[11]進(jìn)行優(yōu)化。

1 RRT算法

RRT算法是以空間中起點(diǎn)為根節(jié)點(diǎn)，通過特殊的增量方式進(jìn)行擴(kuò)展，生成一棵隨機(jī)擴(kuò)展樹，判定隨機(jī)擴(kuò)展樹上的采樣點(diǎn)是否包含目標(biāo)點(diǎn)或在目標(biāo)范圍內(nèi)，直到該條件滿足，隨機(jī)擴(kuò)展樹就很快覆蓋起點(diǎn)和終點(diǎn)的所有位姿空間，當(dāng)樹長(zhǎng)好后，路徑也就找到了，而且起點(diǎn)到終點(diǎn)有且僅有一條路徑。基本RRT算法擴(kuò)展示意圖如圖1所示。

圖1 RRT算法擴(kuò)展示意圖

RRT算法步驟：在空間環(huán)境中，定義一個(gè)起點(diǎn)xinit；在空間環(huán)境中，隨機(jī)確定一個(gè)點(diǎn)xrand；若點(diǎn)xrand不在障礙區(qū)范圍內(nèi)，則連接點(diǎn)xinit和點(diǎn)xrand，得到一條連接直線L；若直線L沒有和障礙物發(fā)生碰撞，則沿著直線L，從xinit向xrand的方向擴(kuò)展一定的距離λ，得到新的點(diǎn)xnew，該點(diǎn)被添加到已經(jīng)存在的擴(kuò)展樹上；重復(fù)以上步驟，直到目標(biāo)點(diǎn)被添加在擴(kuò)展樹上或在一定誤差范圍內(nèi)，于是可以尋找到起點(diǎn)到終點(diǎn)唯一一條路徑。

在上述算法需要引起注意的是：在隨機(jī)擴(kuò)展樹上，初始的隨機(jī)數(shù)只包含一個(gè)根節(jié)點(diǎn)；隨機(jī)選擇一個(gè)采樣點(diǎn)，在目標(biāo)點(diǎn)和這個(gè)采樣點(diǎn)中創(chuàng)建一個(gè)新的節(jié)點(diǎn)(如果這個(gè)新節(jié)點(diǎn)和障礙物之間發(fā)生碰撞則取消該節(jié)點(diǎn)，如果沒有碰撞則把這個(gè)節(jié)點(diǎn)加入到隨機(jī)擴(kuò)展樹中)；在擴(kuò)展過程中為了控制算法，加入了運(yùn)行時(shí)間上限和搜索節(jié)點(diǎn)個(gè)數(shù)上限條件，如果沒有在規(guī)定條件內(nèi)達(dá)到目標(biāo)點(diǎn)，判定算法失敗。

2 改進(jìn)RRT算法

在基本RRT算法的基礎(chǔ)上，采用目標(biāo)偏向策略減少樹擴(kuò)展的隨機(jī)性；添加采樣點(diǎn)時(shí)角度約束，減少計(jì)算量；并對(duì)規(guī)劃后的節(jié)點(diǎn)進(jìn)行貪心策略，簡(jiǎn)化搜索路徑長(zhǎng)度。

改進(jìn)RRT算法步驟：

第1步：在空間地圖中，定義起點(diǎn)xinit，終點(diǎn)xgoal，偏向概率p，擴(kuò)展步長(zhǎng)λ，最大迭代次數(shù)kmax。

剛戀愛那會(huì)兒，林藍(lán)與大趙也有說不完的話。那時(shí)他倆都是初入職場(chǎng)的小白領(lǐng)，林藍(lán)和父母住，大趙在離林家兩站路的地方租了一間出租屋。每次約會(huì)大趙送林藍(lán)回家最有意思：往往都走到門口了，兩個(gè)人覺得剛剛聊的話題還沒說完，于是又手牽手往回走，由林藍(lán)送大趙回家去，大趙怎么會(huì)允許女朋友送自己呢？于是到地兒又把林藍(lán)送回來。

第2步：檢測(cè)xinit與xgoal之間的距離是否小于λ，若條件成立，則對(duì)擴(kuò)展樹采用貪心策略剪枝，生成新的路徑節(jié)點(diǎn)；若條件不成立，進(jìn)行下一步。

第3步：以一定的偏向概率p選擇采樣點(diǎn)xrand，并根據(jù)改進(jìn)度量函數(shù)確定就近點(diǎn)xnear。

第4步：在xnew和xrand之間的連線L上找到新節(jié)點(diǎn)，xnew沿著直線L，從xinit向xrand的方向擴(kuò)展一定的距離λ，得到新的節(jié)點(diǎn)xnew。

第5步：判斷擴(kuò)展樹上的所有節(jié)點(diǎn)是否達(dá)到終點(diǎn)，若不能，返回第3步；若能，進(jìn)行下一步。

第6步：對(duì)已經(jīng)生成的擴(kuò)展樹采用貪心策略，生成新的路徑點(diǎn)。

圖2為改進(jìn)RRT算法流程圖。

圖2 改進(jìn)RRT算法流程圖

2.1 擴(kuò)展步長(zhǎng)

在RRT算法中，xnew是由xnear向xrand擴(kuò)展一定步長(zhǎng)得到的，下式為采樣點(diǎn)求得公式：

(1)

其中，λ為擴(kuò)展步長(zhǎng)，d(xrand,xnear)為點(diǎn)xrand與點(diǎn)xnear之間的歐幾里得距離。

2.2 目標(biāo)偏向策略

基本RRT算法的采樣點(diǎn)的范圍很大，且隨機(jī)性強(qiáng)，隨機(jī)樹在擴(kuò)展方向上具有盲目性，缺乏一定的引導(dǎo)性，導(dǎo)致算法搜索時(shí)間較長(zhǎng)。所以，采用目標(biāo)偏向策略，以一定的概率P把目標(biāo)點(diǎn)作為采樣點(diǎn)進(jìn)行隨機(jī)樹擴(kuò)展，提高隨機(jī)樹向目標(biāo)點(diǎn)的擴(kuò)展的概率，以達(dá)到減少采樣點(diǎn)的個(gè)數(shù)，加速隨機(jī)擴(kuò)展樹的形成。

2.3 度量函數(shù)

度量函數(shù)的目的是在隨機(jī)擴(kuò)展樹上找到一點(diǎn)xnear離采樣點(diǎn)xrand最近的哪一個(gè)。其中，基本RRT算法的度量函數(shù)是歐幾里得距離。在改進(jìn)的RRT算法中除了考慮兩點(diǎn)之間的距離，還引入了角度約束，使得節(jié)點(diǎn)的確立對(duì)整條路徑平滑有著重要作用。

由于距離和角度的量綱不同，所以將其進(jìn)行歸一化處理。下式中，D(d)為規(guī)劃后的距離函數(shù)，G(θ)為規(guī)劃后的偏角函數(shù)。在已形成的搜索擴(kuò)展樹上的臨近的節(jié)點(diǎn)，使得M函數(shù)取得最小值的節(jié)點(diǎn)，即可作為xnear。如圖2所示節(jié)點(diǎn)選擇圖。

圖3 改進(jìn)RRT算法節(jié)點(diǎn)擴(kuò)展圖

D(d)=(dmax-d)/dmax

(2)

G(θ)=(θmax-θ)/θmax

(3)

(4)

(5)

M(xi,xj)=D(d)d(i,j)+G(θ)Δθ(i,j)

(6)

其中，xnear坐標(biāo)為(xi,yi)，xrand坐標(biāo)為(xj,yj)，dmax和θmax為搜索最大值。

2.4 路徑的剪枝及平滑

基本RRT算法采樣點(diǎn)的選擇具有隨機(jī)性，生成的路徑節(jié)點(diǎn)眾多，存在一些不必要的冗余節(jié)點(diǎn)。在障礙物較多的復(fù)雜空間下，隨著采樣節(jié)點(diǎn)的增多，會(huì)導(dǎo)致機(jī)器人運(yùn)動(dòng)無法得到有效的跟蹤。為此，改進(jìn)RRT算法對(duì)已生成的路徑節(jié)點(diǎn)進(jìn)行了剪枝，刪除了冗余節(jié)點(diǎn)，最后通過貪心算法對(duì)剪枝后的路徑進(jìn)行簡(jiǎn)化。

具體修剪步驟：對(duì)初次規(guī)劃出的節(jié)點(diǎn)集合，從起點(diǎn)xinit開始，依次遍歷后面的節(jié)點(diǎn)xk，如果兩個(gè)節(jié)點(diǎn)的連線沒有與障礙物發(fā)生碰撞，則刪除點(diǎn)xinit與點(diǎn)xk之間的所有節(jié)點(diǎn)；如果兩個(gè)節(jié)點(diǎn)的連線與障礙物發(fā)生碰撞，則要從點(diǎn)xi的父節(jié)點(diǎn)開始重復(fù)上述過程，直到終點(diǎn)xgoal添加到新路徑中，最后將新的節(jié)點(diǎn)連接成路徑。

上述形成的路徑并不符合機(jī)器人運(yùn)動(dòng)學(xué)規(guī)律，因此在此基礎(chǔ)上對(duì)曲線進(jìn)行平滑。B樣條曲線的曲率具有一定的連續(xù)性，能夠更好地控制曲線的平滑度，并在實(shí)際工程中得到廣泛應(yīng)用。

K次B樣條曲線表示：

(7)

其中，pi為曲線控制的節(jié)點(diǎn)，Bi,k(u)為K次B樣條基函數(shù)，節(jié)點(diǎn)向量U=[u0,u1,…,un+k+1]確定K次分段曲線。

三次B樣條曲線的基函數(shù)：

(8)

3 仿真驗(yàn)證

為了驗(yàn)證改進(jìn)RRT算法的可行性和優(yōu)越性，通過MATLAB語(yǔ)言進(jìn)行了仿真驗(yàn)證。通過基本RRT算法與改進(jìn)RRT算法在步長(zhǎng)λ=0.4的條件下，得到在地圖1、地圖2、地圖3路徑規(guī)劃的長(zhǎng)度、節(jié)點(diǎn)個(gè)數(shù)以及搜索時(shí)間。起點(diǎn)坐標(biāo)(0,0)，終點(diǎn)坐標(biāo)(10,10)，紅色代表最終規(guī)劃出的可行路徑，圖4為各個(gè)算法在不同地圖的路徑規(guī)劃圖。

(a) 地圖1基本RRT算法 (b) 地圖2基本RRT算法 (c) 地圖3基本RRT算法

(d) 地圖1改進(jìn)RRT算法 (e) 地圖2改進(jìn)RRT算法 (f) 地圖3改進(jìn)RRT算法圖4 不同算法路徑規(guī)劃圖

在仿真結(jié)果中，如表1、表2、表3所示仿真數(shù)據(jù)，基本RRT算法和改進(jìn)RRT算法在搜索時(shí)間、搜索路徑長(zhǎng)度、迭代次數(shù)上有著明顯的差異。改進(jìn)RRT算法在這三個(gè)維度衡量標(biāo)準(zhǔn)下有著更顯著的優(yōu)勢(shì)。隨著地圖障礙物的增加，改進(jìn)后的RRT算法在路徑規(guī)劃中更加具有優(yōu)勢(shì)，在地圖3中，與基本RRT算法相比：搜索時(shí)間縮短53.8%，路徑長(zhǎng)度縮短了14%，迭代次數(shù)減少了74.9%。以此證明了改進(jìn)RRT算法的可行性和優(yōu)越性。

表1 不同算法搜索時(shí)間表

表2 不同算法搜索路徑長(zhǎng)度表

表3 不同算法迭代次數(shù)表

4 軌跡平滑仿真

為了使得路徑更符合機(jī)器人運(yùn)動(dòng)學(xué)規(guī)律，將改進(jìn)RRT算法規(guī)劃出的路徑進(jìn)行三次B樣條曲線擬合，如圖5所示。

(a) 地圖1路徑優(yōu)化 (b) 地圖2路徑優(yōu)化 (c) 地圖3路徑優(yōu)化圖5 三次B樣條曲線優(yōu)化圖

5 結(jié)論

針對(duì)靜態(tài)全局空間下機(jī)器人的路徑規(guī)劃問題，提出了一種改進(jìn)RRT算法。該算法通過搜索范圍的約束、剪枝運(yùn)算以及貪心算法的應(yīng)用達(dá)到了對(duì)路徑的修正，最后通過三次B樣條曲線對(duì)路徑進(jìn)行優(yōu)化，使其更符合機(jī)器人運(yùn)動(dòng)學(xué)規(guī)律。

通過仿真驗(yàn)證，與基本RRT算法對(duì)比，證明了改進(jìn)RRT算法的正確性和可行性，對(duì)機(jī)器人運(yùn)動(dòng)規(guī)劃具有一定的參考價(jià)值。