基于方向決策RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃

劉想德，何翔鵬，胡 勇，胡小林

(1. 重慶郵電大學(xué)信息無障礙工程與機(jī)器人技術(shù)研發(fā)中心，重慶 400065；2. 重慶工業(yè)大數(shù)據(jù)創(chuàng)新中心有限公司，重慶 400000)

1 引言

移動(dòng)機(jī)器人路徑規(guī)劃[1]是實(shí)現(xiàn)機(jī)器人自主導(dǎo)航的關(guān)鍵技術(shù)之一，其目的是使機(jī)器人在障礙物約束環(huán)境中，通過路徑長度、搜索時(shí)間，平滑度以及避障能力等性能指標(biāo)，求解出一條從起始點(diǎn)到目標(biāo)點(diǎn)的最優(yōu)或近似最優(yōu)路徑。

傳統(tǒng)的移動(dòng)機(jī)器人路徑規(guī)劃算法包括dijkstra算法[2]、遺傳算法[3]、A*算法[4]、粒子群算法[5]等，然而面對(duì)復(fù)雜約束的機(jī)器人應(yīng)用場景，這些算法往往需要大量的計(jì)算力。而Lavalle提出的快速擴(kuò)展隨機(jī)樹(Rapidly-Exploring Random Tree， RRT)算法，在狀態(tài)空間中進(jìn)行隨機(jī)碰撞檢測，將搜索空間逐漸擴(kuò)展至可通行區(qū)域，進(jìn)而搜索出一條從起始點(diǎn)到目標(biāo)點(diǎn)的可行路徑[6-8]，避免了對(duì)規(guī)劃空間建模，運(yùn)行速度快，被廣泛應(yīng)用于移動(dòng)機(jī)器人路徑規(guī)劃中。

傳統(tǒng)RRT算法應(yīng)用于機(jī)器人路徑規(guī)劃時(shí)，由于擴(kuò)展節(jié)點(diǎn)的隨機(jī)性大，導(dǎo)致求解過程中存在冗余搜索，且生成的路徑僅為可通行路徑，路徑長度以及平滑度并非最優(yōu)解。針對(duì)這些問題，Jian-Quan L等人[9]采用雙向RRT算法，從起始點(diǎn)和目標(biāo)點(diǎn)同時(shí)進(jìn)行擴(kuò)展，進(jìn)而提高求解效率。然而雙向RRT算法的求解路徑平滑度低，不符合機(jī)器人運(yùn)動(dòng)學(xué)規(guī)律。朱宏輝等人[10]通過選取最小成本的擴(kuò)展節(jié)點(diǎn)，優(yōu)化了RRT算法的路徑長度，但犧牲了算法的收斂速度，導(dǎo)致搜索時(shí)間增加。賀伊琳等人[11]將目標(biāo)偏向策略引入RRT算法，以固定概率將擴(kuò)展節(jié)點(diǎn)向目標(biāo)點(diǎn)延伸，減小了隨機(jī)性，提高了算法的運(yùn)行效率。值得注意的是，面對(duì)復(fù)雜場景，基于偏向策略的RRT算法極易陷入局部最小狀態(tài)。此外，許多學(xué)者將RRT與其它智能算法混合，從而提高算法的求解效率。康搖等人[12]結(jié)合RRT和滾動(dòng)窗口算法增強(qiáng)了算法搜索未知空間的能力。Zhou F等人[13]結(jié)合RRT和粒子群算法求解最優(yōu)路徑。劉恩海等人[14]提高了RRT算法運(yùn)用在移動(dòng)機(jī)器人時(shí)的避障能力，但是優(yōu)化過程需要大量運(yùn)算，導(dǎo)致算法運(yùn)行效率降低。

本文提出一種基于方向決策的RRT算法，通過引入方向信息決策機(jī)制，引導(dǎo)新節(jié)點(diǎn)趨向目標(biāo)點(diǎn)擴(kuò)展，減小無效迭代次數(shù)，進(jìn)而縮短路徑搜索時(shí)間。同時(shí)，將一種啟發(fā)式采樣策略引入所提算法中，使搜索樹能夠快速跳出局部最小點(diǎn)，利用三次b樣條曲線對(duì)規(guī)劃路徑進(jìn)行平滑處理。最后，通過仿真，驗(yàn)證了本文算法的優(yōu)越性。

2 雙輪差速移動(dòng)機(jī)器人運(yùn)動(dòng)模型

在輪式移動(dòng)機(jī)器人中，雙輪差動(dòng)機(jī)器人由于結(jié)構(gòu)緊湊、轉(zhuǎn)彎靈活、易于控制等特點(diǎn)，被廣泛應(yīng)用于工業(yè)、搜救領(lǐng)域，其運(yùn)動(dòng)方式由旋轉(zhuǎn)運(yùn)動(dòng)和平移運(yùn)動(dòng)組成[15]。

圖1 移動(dòng)機(jī)器人運(yùn)動(dòng)模型

圖1中(a)表示雙輪差動(dòng)機(jī)器人旋轉(zhuǎn)運(yùn)動(dòng)模型。其中，機(jī)器人左輪速度為vl，右輪速度為vr，旋轉(zhuǎn)角度為θ，則機(jī)器人線速度和角速度可分別描述為

(1)

(2)

機(jī)器人運(yùn)動(dòng)半徑為

(3)

機(jī)器人平移運(yùn)動(dòng)模型如圖1中(b)所示。設(shè)t-1時(shí)刻機(jī)器人位姿Pt-1=(xt-1，yt-1，θt-1)，當(dāng)Δt時(shí)間內(nèi)機(jī)器人以線速度v和角速度w移動(dòng)，則t時(shí)刻移動(dòng)機(jī)器人的位姿可表示為

(4)

移動(dòng)機(jī)器人所有時(shí)刻的位姿構(gòu)成了機(jī)器人的實(shí)際運(yùn)行路徑。

3 傳統(tǒng)RRT算法

傳統(tǒng)RRT算法的移動(dòng)機(jī)器人路徑規(guī)劃，采用狀態(tài)空間內(nèi)隨機(jī)采樣的方式擴(kuò)展節(jié)點(diǎn)，將搜索樹從初始點(diǎn)不斷擴(kuò)展，最終延伸至目標(biāo)點(diǎn)。將機(jī)器人起始位置坐標(biāo)作為根節(jié)點(diǎn)vinit，并在狀態(tài)空間內(nèi)隨機(jī)生成一個(gè)隨機(jī)節(jié)點(diǎn)vrand，基于與vrand距離最短原則，搜索樹生成新節(jié)點(diǎn)vnear。為了形成碰撞判斷機(jī)制，vnear與vrand的相連形成線段lnear，在lnear上擴(kuò)展一個(gè)測試節(jié)點(diǎn)vnew，其擴(kuò)展步長為給定常數(shù)，判斷vnew以及l(fā)near是否與障礙物碰撞，若存在碰撞，則放棄當(dāng)前節(jié)點(diǎn)vnear，重新選擇，并循環(huán)障礙物碰撞判斷過程。通過不斷構(gòu)造新的子結(jié)點(diǎn)vnew，直到節(jié)點(diǎn)vnew與目標(biāo)節(jié)點(diǎn)vgoal之間的距離小于一個(gè)擴(kuò)展步長，且沒有發(fā)生障礙物碰撞，則擴(kuò)展隨機(jī)樹構(gòu)建成功。搜索樹的擴(kuò)展過程如圖2所示。

圖2 傳統(tǒng)RRT算法擴(kuò)展過程

4 改進(jìn)RRT算法

4.1 基于方向決策機(jī)制

本文改進(jìn)的RRT算法，為搜索樹上的節(jié)點(diǎn)引入方向變量orinode，首先確定根節(jié)點(diǎn)的orinode參數(shù)值為根節(jié)點(diǎn)vinit與目標(biāo)節(jié)點(diǎn)vgoal的相對(duì)角度偏差值，可表示為

(5)

其中vinit和vgoal的直角坐標(biāo)分別為(xinit，yinit)，(xgoal，ygoal)，Δy=(ygoal-yinit)，Δx=(xgoal-xinit)。

在搜索樹中引入方向變量后，基于啟發(fā)式采樣策略求得隨機(jī)節(jié)點(diǎn)vrand，在搜索樹上檢索與vrand距離最短的節(jié)點(diǎn)vnear。于此同時(shí)，獲取方向變量orinear，以orinear為基準(zhǔn)，在節(jié)點(diǎn)vnear添加三個(gè)探索式節(jié)點(diǎn)vins1、vins2、vins3，添加探索式結(jié)點(diǎn)示意圖如圖3所示。

圖3 探索式結(jié)點(diǎn)示意圖

三個(gè)探索結(jié)點(diǎn)坐標(biāo)可分別表示為

(6)

(7)

然后，計(jì)算探索式節(jié)點(diǎn)與vgoal的距離d，表示為

(8)

其中，探索節(jié)點(diǎn)的直角坐標(biāo)為(xins，yins)，三個(gè)探索式節(jié)點(diǎn)vins1、vins2、vins3與vgoal的距離分別表示為d1、d2、d3(d1

(9)

使用三個(gè)探索節(jié)點(diǎn)與搜索樹中Vnear節(jié)點(diǎn)進(jìn)行碰撞檢測，則反向擴(kuò)展節(jié)點(diǎn)生成vnew，此時(shí)vnew的狀態(tài)如下

(10)

將擴(kuò)展后的vnew節(jié)點(diǎn)加入搜索樹。重復(fù)執(zhí)行以上過程，直至vnew與vgoal之間的距離小于一個(gè)擴(kuò)展步長，且無碰撞發(fā)生。即可以得到一條從起始點(diǎn)到目標(biāo)點(diǎn)之間的較優(yōu)路徑規(guī)劃結(jié)果。

圖4所示為引入方向變量的RRT算法擴(kuò)展過程，虛線表示該探索式節(jié)點(diǎn)與障礙物發(fā)生碰撞，紅色結(jié)點(diǎn)表示節(jié)點(diǎn)vnew，黃色線段為最終求解路徑。

圖4 改進(jìn)RRT算法擴(kuò)展過程

4.2 啟發(fā)式采樣策略

傳統(tǒng)RRT算法在移動(dòng)機(jī)器人的路徑規(guī)劃中，存在以下缺點(diǎn)：

1)由于在狀態(tài)空間中隨機(jī)選取采樣點(diǎn)，求解過程中存在較多冗余擴(kuò)展，導(dǎo)致算法求解效率低下。

2)面對(duì)多障礙物約束環(huán)境，特別是狹窄通道時(shí)，傳統(tǒng)RRT算法的搜索樹擴(kuò)展需要進(jìn)行大量運(yùn)算，才能實(shí)現(xiàn)路徑規(guī)劃。

本節(jié)介紹一種啟發(fā)式采樣策略，保證搜索樹向目標(biāo)節(jié)點(diǎn)擴(kuò)展的同時(shí)，也增強(qiáng)了算法對(duì)復(fù)雜環(huán)境的適應(yīng)性。具體步驟如下：

定義變量vnear′和vnear″分別記錄隨機(jī)樹搜索過程中最后兩次的擴(kuò)展節(jié)點(diǎn)，定義一個(gè)概率閾值變量p。隨機(jī)生成一個(gè)概率值q。如果vnear′等于vnear″，則說明隨機(jī)樹可能陷入局部最小點(diǎn)，則比較p與q的值，然后執(zhí)行隨機(jī)采樣過程，若vnear′≠vnear″，則判斷當(dāng)前最近點(diǎn)vnear′與vgoal，vnear″與vgoal之間是否發(fā)生碰撞，若未發(fā)生碰撞，則直接將vrand設(shè)定為vgoal，反之則執(zhí)行隨機(jī)采樣。

融入啟發(fā)式采樣策略后系統(tǒng)整體流程圖如圖5所示。

圖5 啟發(fā)式采樣策略流程圖

4.3 三次B樣條曲線擬合

利用RRT算法求解移動(dòng)機(jī)器人路徑規(guī)劃，雖然可以得到一條無碰撞發(fā)生的可通行路徑，但生成的路徑存在多個(gè)折點(diǎn)，導(dǎo)致機(jī)器人在移動(dòng)過程中出現(xiàn)卡頓感。

為了優(yōu)化生成路徑，利用三次B樣條插值擬合法對(duì)RRT生成的路徑進(jìn)行平滑處理，提升路徑的連續(xù)性和平滑性。

B樣條曲線定義為給定m+n+1個(gè)平面內(nèi)控制點(diǎn)Pi(i=0，1，…，m+n)，稱n次參數(shù)曲線段，可表示為：

(11)

這些曲線段的總和稱為n次B樣條曲線。其中Gi，n(t)為基函數(shù)，定義為：

(12)

當(dāng)式(12)中n取3時(shí)，即為3次B樣條，3次B樣條算法的基函數(shù)為

(13)

3次B樣條算法曲線方程為：

(14)

利用3次B樣條算法對(duì)改進(jìn)RRT進(jìn)行路徑平滑，圖5中(a)， (b)分別為3次B樣條處理前后的得到的路徑。

可見經(jīng)平滑處理后的路徑連續(xù)性和平滑性得到了優(yōu)化。

圖6 規(guī)劃路徑平滑前后對(duì)比圖

5 實(shí)驗(yàn)結(jié)果與分析

利用Matlab2016b在兩種環(huán)境下對(duì)比傳統(tǒng)RRT算法、基于目標(biāo)偏向RRT算法和改進(jìn)RRT算法的性能。

環(huán)境1下實(shí)驗(yàn)結(jié)果如圖7(a)，(b)，(c)所示，環(huán)境2下實(shí)驗(yàn)結(jié)果如圖7(d)，(e)，(f)所示。環(huán)境地圖尺寸均為800m×800m，黑色區(qū)域表示障礙物，擴(kuò)展步長S為25m，概率閾值p為0.3。環(huán)境1為簡單場景，機(jī)器人起始點(diǎn)坐標(biāo)為(50m，50m)點(diǎn)，目標(biāo)點(diǎn)坐標(biāo)為(750m，750m)點(diǎn)。環(huán)境2為較復(fù)雜場景，機(jī)器人起始點(diǎn)坐標(biāo)為(50m，750m)點(diǎn)，目標(biāo)點(diǎn)坐標(biāo)為(750m，750m)點(diǎn)。藍(lán)色粗線條為不同算法下機(jī)器人的求解規(guī)劃路徑。

圖7 算法路徑規(guī)劃結(jié)果對(duì)比圖

從圖7中可以看到，在環(huán)境1中，傳統(tǒng)的RRT算法由于缺乏導(dǎo)向性，僅通過隨機(jī)采樣的方式拓展節(jié)點(diǎn)，導(dǎo)致出現(xiàn)大量無效拓展，且求解路徑非最優(yōu)解。而基于目標(biāo)偏向的RRT算法雖然可以得到一條較優(yōu)路徑，但是當(dāng)目標(biāo)點(diǎn)和當(dāng)前位置的連接線上存在障礙物時(shí)，會(huì)出現(xiàn)較多無效擴(kuò)展，從圖8中可以看到，在較復(fù)雜場景中，傳統(tǒng)RRT算法和目標(biāo)偏向性RRT算法除上述缺點(diǎn)之外，若環(huán)境中障礙物較為密集時(shí)，會(huì)產(chǎn)生更多的無效擴(kuò)展，而改進(jìn)RRT算法，能夠在進(jìn)行較少迭代后跳出了這片區(qū)域。

圖8 算法迭代次數(shù)對(duì)比圖

圖8(a)、(b)是以上三種算法的迭代次數(shù)對(duì)比圖，圖中可以看出，在兩種環(huán)境下，本文改進(jìn)RRT算法均能在較少迭代次數(shù)內(nèi)收斂。

考慮到RRT算法是一種隨機(jī)性算法，為了驗(yàn)證實(shí)驗(yàn)結(jié)果具有普遍適應(yīng)性，分別在兩種環(huán)境下進(jìn)行30次試驗(yàn)，得到的平均數(shù)據(jù)記錄于表1表示。

表1 不同環(huán)境地圖下算法實(shí)驗(yàn)結(jié)果對(duì)比

在場景1中，相比于傳統(tǒng)RRT算法，改進(jìn)RRT算法的迭代次數(shù)減少62.6%，求解時(shí)間縮短86.9%，路徑長度縮短23.1%；相比目標(biāo)偏向性RRT算法，改進(jìn)RRT算法的迭代次數(shù)減少24.1%，求解時(shí)間縮短48.2%，路徑長度縮短18.1%。場景2中，相比傳統(tǒng)RRT算法，改進(jìn)RRT算法的迭代次數(shù)減少39.5%，求解時(shí)間縮短85.4%，路徑長度縮短8.4%；相比基于目標(biāo)偏向性的RRT算法，迭代次數(shù)減少50.6%，求解時(shí)間縮短79.9%，路徑長度縮短9.4%。

在不同復(fù)雜程度的場景中，分析三種RRT算法的運(yùn)行結(jié)果，可以看出OIS-RRT算法能以更少的迭代次數(shù)，更快的收斂速度求解得到一條更優(yōu)路徑。

6 結(jié)論

為了解決傳統(tǒng)RRT算法應(yīng)用于移動(dòng)機(jī)器人路徑規(guī)劃時(shí)擴(kuò)展隨機(jī)性強(qiáng)，算法運(yùn)行效率低，生成路徑質(zhì)量非最優(yōu)的問題，本文提出一種基于方向決策RRT算法，通過方向變量，引導(dǎo)節(jié)點(diǎn)快速向目標(biāo)點(diǎn)擴(kuò)展；采用啟發(fā)式采樣策略提升算法在復(fù)雜環(huán)境下的適應(yīng)性和避障能力；利用三次B樣條曲線優(yōu)化求解路徑，提高了路徑質(zhì)量。通過仿真，驗(yàn)證了本文改進(jìn)RRT算法能夠有效縮短移動(dòng)機(jī)器人求解路徑規(guī)劃的時(shí)間，并提升求解路徑質(zhì)量。