基于改進(jìn)RRT*的移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃算法

潘思宇,徐向榮

(安徽工業(yè)大學(xué) 機(jī)械工程學(xué)院,安徽 馬鞍山 243002)

基于改進(jìn)RRT*的移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃算法

潘思宇,徐向榮*

(安徽工業(yè)大學(xué) 機(jī)械工程學(xué)院,安徽 馬鞍山 243002)

RRT*(快速搜索隨機(jī)樹)算法在以往研究中存在收斂速度慢、結(jié)果不穩(wěn)定的缺點(diǎn)。針對(duì)此問題,文章在現(xiàn)有RRT*基礎(chǔ)之上提出一種新型改進(jìn)算法。該改進(jìn)算法結(jié)合環(huán)境約束、車輛自身約束和運(yùn)動(dòng)學(xué)約束,舍棄原算法貪心思想并引入啟發(fā)式采樣節(jié)點(diǎn)插入算法,提高路徑規(guī)劃的速度和質(zhì)量;接著對(duì)改進(jìn)算法進(jìn)行理論分析,證明算法具有概率完整性、漸近最優(yōu)性,從理論上保證算法能快速收斂到最優(yōu)路徑。通過各種仿真環(huán)境的測試,驗(yàn)證改進(jìn)算法的有效性、穩(wěn)定性和正確性,也驗(yàn)證理論分析的正確性。

快速搜索隨機(jī)樹;路徑規(guī)劃;移動(dòng)機(jī)器人;機(jī)器人操作系統(tǒng)

0 引言

近年來智能移動(dòng)機(jī)器人在工業(yè)、農(nóng)業(yè)、航空航天及空間探索等方面起到了重要的作用,因而成為學(xué)術(shù)界研究和關(guān)注的熱點(diǎn)問題。移動(dòng)機(jī)器人在任務(wù)空間自由活動(dòng)的過程中,首先需要解決的問題,就是路徑規(guī)劃問題。路徑規(guī)劃是指移動(dòng)機(jī)器人按照某些性質(zhì)指標(biāo)(如最短路徑,時(shí)間最短或者代價(jià)最少等)的要求,規(guī)劃出一條從起始點(diǎn)到目標(biāo)點(diǎn)位置的最優(yōu)或是次優(yōu)的無碰撞路徑,并指示移動(dòng)機(jī)器人按照該路徑行駛[1]。而解決這些問題需要考慮的因素有:任務(wù)空間的復(fù)雜性和不確定性,規(guī)劃算法的有效性、最優(yōu)性和實(shí)時(shí)性以及滿足移動(dòng)機(jī)器人本體的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)特性。

在過去的一段時(shí)間里,為了解決這些問題,國內(nèi)外學(xué)者進(jìn)行了大量的研究,不斷地進(jìn)行探索,提出了很多路徑規(guī)劃的理論和方法。傳統(tǒng)的路徑規(guī)劃算法如蟻群算法、遺傳算法、人工勢場算法等,這些算法在處理簡單的規(guī)劃問題有一定的優(yōu)越性,但是在復(fù)雜環(huán)境下和高維空間中算法的復(fù)雜的會(huì)急劇增加,導(dǎo)致收斂時(shí)間長、求解困難[2]。此外,基于勢場或啟發(fā)函數(shù)的算法,如 A*、D*和人工勢場法等,在處理規(guī)劃問題時(shí)雖然能滿足最優(yōu)性和實(shí)時(shí)性的要求,但是因其并未考慮移動(dòng)機(jī)器人本體的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)限制,使得規(guī)劃的路徑不一定能被移動(dòng)機(jī)器人所執(zhí)行[3]。

其他改進(jìn)算法需要在一個(gè)確定性空間中對(duì)障礙物進(jìn)行確定的建模,在高維空間中容易引發(fā)維度災(zāi)難。為了解決高維規(guī)劃問題,基于采樣的算法因此被引入[4],相比于其他先進(jìn)算法，其主要優(yōu)勢就是避免構(gòu)建顯式的任務(wù)空間,且其已被證明是有效解決路徑規(guī)劃問題的方案[5]，其中,最著名的基于采樣的算法有PRM和RRT,但當(dāng)存在未知障礙物時(shí)PRM算法的效率是極其低下的[6]。RRT算法收斂速度快,同時(shí)能應(yīng)用在存在未知障礙物的環(huán)境中[7],但是,RRT算法中仍然有一些缺點(diǎn)[8-10]:(1)因其采用的是全局均勻隨機(jī)策略,導(dǎo)致其路徑不穩(wěn)定,規(guī)劃出的路徑未必是最優(yōu)路徑,而且無謂地消耗大量資源,收斂速度緩慢;(2)最近節(jié)點(diǎn)選擇算法在解決復(fù)雜問題時(shí)可能會(huì)導(dǎo)致算法失效;(3)因算法的隨機(jī)性生成的路徑過于粗糙,不便于移動(dòng)機(jī)器人實(shí)施;(4)因算法缺少學(xué)習(xí)能力,在擴(kuò)展過程中選擇的節(jié)點(diǎn)可能會(huì)產(chǎn)生使路徑發(fā)生碰撞導(dǎo)致擴(kuò)展失敗的情況,然而在后續(xù)的擴(kuò)展過程中,當(dāng)遇到同一區(qū)域選擇隨機(jī)目標(biāo)點(diǎn)是,這些會(huì)發(fā)生碰撞的節(jié)點(diǎn)和處在障礙物區(qū)域無法實(shí)化的節(jié)點(diǎn)將再次被選取,產(chǎn)生重復(fù)而不必要的運(yùn)算,影響整體算法效率。

由于這些不足的存在,國內(nèi)外的學(xué)者對(duì)此展開了研究,許多RRT的改進(jìn)算法被提出,以適應(yīng)不同的應(yīng)用場景。為了提高節(jié)點(diǎn)的擴(kuò)展效率,2000年Kuffner和LaValle提出了RRT-connect[11],次年,他們提出了雙向搜索樹(Bidirectional-RRT),從起始點(diǎn)和目標(biāo)點(diǎn)同時(shí)出發(fā)并行生成兩棵RRT,直至兩棵樹相遇,加速算法收斂[12]。C.Urmson和R.Simmons(2003)提出了一種啟發(fā)式的偏向算法,該算法引導(dǎo)RRT樹向目標(biāo)區(qū)域生長,使得RRT算法能得到一個(gè)低時(shí)間成本的解決方案[13]。 D.Ferguson和A.Stentz(2006)考慮多次運(yùn)行RRT算法以逐步提高解決方案的質(zhì)量,即使不能保證收斂到最優(yōu)的解決方案,也能保證算法每次運(yùn)行的結(jié)果是一個(gè)比較小的路徑代價(jià)的路徑[14]。Karaman和Frazzoli(2010)首次提出RRT*算法,用來改進(jìn)由基本RRT算法產(chǎn)生的并非概率最優(yōu)解的問題[15]。M.Jordan和A.Perez(2013)提出了B-RRT*,用一個(gè)輕微變異的貪心RRT-connect作為啟發(fā)函數(shù)來連接兩顆隨機(jī)樹[16]。 A.H. Qureshi和 S. Mumtaz(2014)提出了TG-RRT*,利用三角幾何來選取節(jié)點(diǎn),以降低得到最優(yōu)解所需要的迭代次數(shù),從而使得算法快速收斂[17]。Ahmed Hussain Qureshi, Yasar Ayaz(2015)提出了IB-RRT*,利用雙向樹的方法,通過智能樣本插入的啟發(fā)函數(shù)使得算法快速收斂到最優(yōu)路徑[5],C. Wouter Bac和Tim Roorda(2016)提出利用RRT算法在ROS平臺(tái)上做移動(dòng)機(jī)器人的路徑規(guī)劃研究,并應(yīng)用于在密集障礙物環(huán)境中運(yùn)動(dòng)規(guī)劃問題中[18]。

本文在這些問題的基礎(chǔ)上提出了一個(gè)新的算法,采用智能隨機(jī)抽樣的方法以降低得到最優(yōu)解所需要的迭代次數(shù),從而使得算法快速收斂,同時(shí)引入了懲罰函數(shù)來記憶所選取過的節(jié)點(diǎn)以提高算法的效率,同時(shí)考慮移動(dòng)機(jī)器人所受到的運(yùn)動(dòng)學(xué)和動(dòng)力學(xué)限制,使得生成的軌跡可以直接被實(shí)際機(jī)器人所直接實(shí)現(xiàn)。通過仿真實(shí)驗(yàn),證實(shí)了該算法的有效性、最優(yōu)性和實(shí)時(shí)性。

1 移動(dòng)機(jī)器人的非完整性約束模型

Fig.1 Geometrics of mobile robot圖1 移動(dòng)機(jī)器人幾何示意圖

非完整性約束是指含有系統(tǒng)廣義坐標(biāo)導(dǎo)數(shù)且不可積分的約束[19],移動(dòng)機(jī)器人是一個(gè)典型的非完整約束系統(tǒng)。而本文要研究的移動(dòng)機(jī)器人模型,是一種典型的非完整性約束的系統(tǒng)。

移動(dòng)機(jī)器人在任務(wù)空間中狀態(tài)變量為(x,y,θ,v,φ),其中(x,y)為移動(dòng)機(jī)器人后輪軸的中心在系統(tǒng)坐標(biāo)系下的坐標(biāo),θ為移動(dòng)機(jī)器人前進(jìn)方向與x軸的夾角,φ為移動(dòng)機(jī)器人前輪方向與前進(jìn)方向之間的夾角,v為移動(dòng)機(jī)器人的速度,由于受到非完整性約束,所以車輪與地面是點(diǎn)接觸且在接觸點(diǎn)只有純滾動(dòng)沒有相對(duì)的滑動(dòng)。由分析可知:

(1)

從而可得移動(dòng)機(jī)器人所受到的約束方程:

(2)

其中,移動(dòng)機(jī)器人的控制變量為u0(加速度)和u1(車輪的角速度)。移動(dòng)機(jī)器人的最小轉(zhuǎn)彎半徑(最大曲率):

(3)

除了上面的約束條件之外,還有:

(4)

由此可知,沒有考慮機(jī)器人的非完整性約束的規(guī)劃算法,它們不考慮車輛約束,規(guī)劃出來的路徑也不能用于實(shí)際的路徑規(guī)劃當(dāng)中[20]。

2 規(guī)劃算法的描述與實(shí)現(xiàn)

2.1 RRT*算法

RRT*通過在新的節(jié)點(diǎn)附近建立周圍節(jié)點(diǎn)分布圖來改進(jìn)現(xiàn)有搜索樹,然后遍歷周圍的節(jié)點(diǎn)檢查是否存在新的路徑比現(xiàn)有的路徑的代價(jià)更低,如果存在則將現(xiàn)有的路徑換掉。通過這種方式RRT*算法可以保證最終所確認(rèn)的路徑是漸進(jìn)最優(yōu)解。下面是算法的基本流程。

ALGORITHM:RRT*

1.V←{xinit};E←φ;T←(V,E);

2.fori←0toNdo

3.xrand←Sample(i)

4.Xnear←NearVertices(xrand,T)

5.ifXnear=φthen

6.Xnear←NearVertex(xrand,T)

7.Ls←GetSortedList(xrand,Xnear)

8.xmin←ChooseBestParent(Ls)

9.ifxmin≠φthen

10.T←InsertVertex(xrand,xmin,T)

11.T←REWIREVERTICES(xrand,Ls,E)

12.returnT=(V,E)

由ALGORITHM:RRT*所示,RRT*算法以起始點(diǎn)xinit作為隨機(jī)搜索樹的根節(jié)點(diǎn),不斷的從給定無障礙空間Xfree中通過隨機(jī)采樣Sample函數(shù)得到xrand,然后利用NearVertices函數(shù)得出鄰近節(jié)點(diǎn)集合Xnear。鄰近節(jié)點(diǎn)集合的定義是:給定一個(gè)樣本節(jié)點(diǎn),搜索樹T=(V,E),一個(gè)球型空間Βxrand,r以xrand為球心以r為半徑:

Near(xrand,T,r):={v∈V:v∈Βxrand,r}|→Xnear?V，

如果鄰近節(jié)點(diǎn)集合Xnear是空集,則由NearestVertex函數(shù)計(jì)算出最近點(diǎn)xnear補(bǔ)充到鄰近節(jié)點(diǎn)集合Xnear中,即鄰近節(jié)點(diǎn)集合Xnear中只有一個(gè)元素xnear。鄰近節(jié)點(diǎn)集合Xnear中的元素在GetSortedList函數(shù)的作用下按照代價(jià)函數(shù)c(σ)升序排列生成一個(gè)列表Ls,列表Ls每一個(gè)元素都是由三個(gè)變量參數(shù)(x′,c(σ),σ′)所組成,ChooseBestParent函數(shù)遍歷列表Ls,從中選出最優(yōu)父節(jié)點(diǎn)xmin∈Xnear,使在不碰障礙物的前提下,xinit通過xmin到xrand路徑代價(jià)最小。InsertVertex函數(shù)將xmin插入搜索樹T中,REWIREVERTICES函數(shù)進(jìn)行隨機(jī)搜索樹T重組,檢查每一個(gè)節(jié)點(diǎn),如存在x′∈Xnear從起始點(diǎn)xinit通過x′到xrand的路徑代價(jià)小于現(xiàn)存的路徑代價(jià),且是無障礙路徑,就將現(xiàn)存路徑換成新路徑,如條件不滿足,遍歷搜索樹T檢查Xnear中的每一個(gè)節(jié)點(diǎn)。依次重復(fù)上述過程直到搜到一條從起始點(diǎn)到目標(biāo)點(diǎn)的無碰撞路徑。

RRT*算法雖然提供了漸近最優(yōu)性的保證,但是也存在著一些缺點(diǎn):(1)雖然能實(shí)現(xiàn)最優(yōu)解收斂,但實(shí)現(xiàn)最優(yōu)解收斂速度緩慢;(2)由于使用大量的迭代在計(jì)算最優(yōu)路徑,所以需要大量的資源來求解;(3)RRT*算法舍去的隨即采樣點(diǎn),雖然無法直接連接到搜索樹現(xiàn)存的節(jié)點(diǎn)上,但是其可能在目標(biāo)點(diǎn)附近,并且能加快實(shí)現(xiàn)最優(yōu)解,而這樣的節(jié)點(diǎn)不應(yīng)該被舍去。

2.2 改進(jìn)的RRT*算法(即IB-RRT*)

ALGORITHM:IB-RRT*

3.σf←∞;E←φ;

4.Connection←True

5.fori←0toNdo

6.xrand←Sample(i)

10.Connection←False

14.if(flag)then

15.Ta←InsertVertex(xrand,xmin,Ta)

16.Ta←RewireVertex(xrand,xmin,Ta)

17.else

18.Tb←InsertVertex(xrand,xmin,Tb)

19.Tb←RewireVertex(xrand,xmin,Tb)

20.E←Ea∪Eb

21.V←Va∪Vb

22.return({Ta,Tb}=V,E)

(5)

(6)

3 對(duì)改進(jìn)算法的理論分析

在任意的狀態(tài)空間中,一個(gè)算法能收斂到可行解就稱該算法具有概率完備性,能在有限次迭代中找到最優(yōu)解就稱該算法具有漸進(jìn)最有性。因改進(jìn)算法是一種新的算法,需要對(duì)其進(jìn)行收斂性驗(yàn)證,證明其具有收斂性且能收斂到最優(yōu)路徑。

3.1 概率完備性

概率完備性的定義是:如果一個(gè)算法就有概率完備性,則對(duì)于任何一個(gè)存在可行解的路徑規(guī)劃問題(Xfree,xinit,Xgoal)就有

(7)

對(duì)IB-RRT*算法分析,可得對(duì)于任何一個(gè)存在可行解的路徑規(guī)劃問題(Xfree,xinit,Xgoal),始終存在一個(gè)常數(shù)a>0,n0∈Ν,且兩者只取決于Xfree和Xgoal,

?n>n0.

(8)

3.2 漸近最優(yōu)性

漸近最優(yōu)性的定義是:如果一個(gè)算法具有漸近最優(yōu)性,那么對(duì)于任何路徑規(guī)劃問題(Xfree,xinit,Xgoal)都有最優(yōu)解的存在且其代價(jià)函數(shù)c:∑→R≥0會(huì)等于一個(gè)有限的路徑代價(jià)c*,即:

(9)

σ*表示一個(gè)最優(yōu)路徑,定義δ:=min{δ,4rn},其中rn為IB-RRT*算法的鏈接半徑。

對(duì)于n∈N,構(gòu)造一個(gè)序列{Bn}n∈N的球覆蓋住σn記為Bn={Bn,1,Bn,2,…,Bn,Mn}:=coveringBalls(σn,rn,2rn),其中

(10)

(θn)>n}).

(11)

).

(12)

).

(13)

這個(gè)概率可以按照如下的方式來計(jì)算,從均勻分布的次序統(tǒng)計(jì)來說,最小αlogn采樣點(diǎn)均勻分且獨(dú)立的分布在[0,1],其概率分布函數(shù)為:

(14)

其中,Beta(·,·)是Beta函數(shù),最大αlogn采樣點(diǎn)均勻分且獨(dú)立的分布在[0,1],其累積分布函數(shù)是:Fmax(x)=xαlogn.

(15)

其中Gamma(·)是gamma函數(shù),整合上式可得:

(16)

(17)

然后,

(18)

那么事件An發(fā)生的概率:

(19)

Bn中含有球的個(gè)數(shù)可以表示為:

(20)

其中,結(jié)合上面的不等式可以得到:

(21)

3.3 快速收斂到最優(yōu)路徑

4 實(shí)驗(yàn)與仿真

4.1 算法仿真實(shí)驗(yàn)

將進(jìn)行仿真實(shí)驗(yàn)來驗(yàn)證算法的有效性,并作為上一節(jié)中提出的概率完備性,漸近最優(yōu)性提供一個(gè)實(shí)驗(yàn)的驗(yàn)證。通過對(duì)典型障礙物的規(guī)劃實(shí)驗(yàn),對(duì)比原有的RRT[21]、RRT*[15]、B-RRT*[16]算法和本文提出的IB-RRT*的運(yùn)算速度、運(yùn)算效率、迭代次數(shù)、路徑代價(jià)和路徑規(guī)劃的成功率。每一個(gè)算法對(duì)應(yīng)于每一個(gè)障礙物地圖時(shí)都將運(yùn)行程序50次,并記錄以上參數(shù),所有實(shí)驗(yàn)均在一個(gè)有著4 GB內(nèi)存的Corei5的處理器,主頻為2.4 MHz的電腦上運(yùn)行得出的。

4.1.1 二維環(huán)境中的仿真實(shí)驗(yàn)

在二維環(huán)境的仿真中,選用一些比較有代表性的能夠驗(yàn)證算法性能的障礙物地圖(圖2)。

Fig.2 ALG performance in 2-D environment圖2 算法在二維環(huán)境中的規(guī)劃

其中,圖2(a)-(d)為四種規(guī)劃算法在三種不同的規(guī)劃問題中求解的結(jié)果。

4.1.2 三維環(huán)境中的仿真實(shí)驗(yàn)

在三維環(huán)境模型的仿真試驗(yàn)中,采用了三種不同的障礙物環(huán)境對(duì)算法進(jìn)行仿真實(shí)驗(yàn),以探究算法的性能。

表1 計(jì)算最優(yōu)路徑的實(shí)驗(yàn)結(jié)果

Fig.3 ALG performance in 3-D environment圖3 算法在三維環(huán)境中的規(guī)劃(左為隨機(jī)障礙物地圖,中為復(fù)雜障礙物地圖,右為窄道障礙物地圖)

從圖3和表1中可以看出來對(duì)于同一個(gè)路徑規(guī)劃問題,經(jīng)過大量的迭代后,IB-RRT*在任務(wù)空間中搜索到的路徑和RRT*、B-RRT*有許多相似的地方,這也間接的證明了這三種算法都是漸近最優(yōu)的算法,最終都能收斂到最優(yōu)解。但從表1中的數(shù)據(jù)可以看出IB-RRT*在搜索速度和搜索效率上均有顯著提高,在三維隨機(jī)障礙物地圖環(huán)境中,IB-RRT*的平均迭代次數(shù)是21 290次,而B-RRT*的平均迭代次數(shù)是36 128次,是IB-RRT*的1.69倍,而RRT*的平均迭代次數(shù)是108 965次,是IB-RRT*的5.12倍,搜索用時(shí)也從RRT*的18.8 s,B-RRT*的7.4 s降低到IB-RRT*的5.3 s;在三維復(fù)雜障礙物地圖環(huán)境中,IB-RRT*的平均迭代次數(shù)是111 139次,而B-RRT*的平均迭代次數(shù)是498 972次,是IB-RRT*的4.49倍,而RRT*的平均迭代次數(shù)是1 437 342次,是IB-RRT*的12.93倍,搜索用時(shí)也從RRT*的244.1 s,B-RRT*的105.1 s降低到IB-RRT*的26.5 s;在三維窄道障礙物地圖環(huán)境中,IB-RRT*的平均迭代次數(shù)是134 421次,而B-RRT*的平均迭代次數(shù)是551 771次,是IB-RRT*的4.10倍,而B-RRT*的平均迭代次數(shù)是1 290 674次,是IB-RRT*的9.61倍,搜索用時(shí)也從RRT*的218.5 s,B-RRT*的115.9 s降低到IB-RRT*的31.4 s。由此我們可以得知,IB-RRT*在搜索速度方面遠(yuǎn)快于B-RRT*和RRT*,且其搜索路徑成功的概率也大幅提高,算法的穩(wěn)定性增強(qiáng),驗(yàn)證了前文的理論證明是正確。同時(shí)搜索的路徑也相對(duì)其他兩種種算法更為平緩,并且最終生成的可執(zhí)行路徑曲率也是連續(xù)變化的,滿足了車輛的運(yùn)動(dòng)約束要求。

4.2 基于ROS的移動(dòng)機(jī)器人仿真實(shí)驗(yàn)

移動(dòng)機(jī)器人模型如圖4所示,底面直徑為0.25 m,高度為0.3 m。環(huán)境模型如圖5a所示,每個(gè)大房間墻壁長度和寬度均為5 m,高度為2.5 m,厚度為0.2 m;小房間墻壁長度和寬度均為3 m,高度為2.5 m,厚度為0.2 m;四個(gè)門洞高度為2 m,寬度為1 m。模型構(gòu)建完成后,移動(dòng)機(jī)器人調(diào)用ROS中的gmapping功能包完成SLAM,實(shí)現(xiàn)場景地圖的構(gòu)建,最后獲得室內(nèi)場景的地圖(圖6)。

Fig.4 Mobile robot圖4 移動(dòng)機(jī)器人

a) Gazebo 下的SLAM過程 b)RVIZ下的SLAM的過程Fig.5 The process of SLAM圖5 SLAM過程

Fig.6 Map information in RVIZ圖6 RVIZ下顯示的場景地圖信息

這些文件配置好了之后,便可以利用改進(jìn)的RRT算法對(duì)移動(dòng)機(jī)器人進(jìn)行路徑規(guī)劃,在地圖上隨機(jī)選擇一目標(biāo)點(diǎn),并選定位置和姿態(tài),改進(jìn)的RRT算法便會(huì)生成一條無障礙的路徑,如圖7所示，綠色的小軌跡為算法規(guī)劃出來的軌跡。

Fig.7 Process of planning圖7 規(guī)劃過程

圖7給出了IB-RRT*動(dòng)態(tài)規(guī)劃過程。IB-RRT*采用的是實(shí)時(shí)重規(guī)劃,Laserscan-nodelet-manager節(jié)點(diǎn)實(shí)時(shí)感知外界地圖環(huán)境消息,將移動(dòng)機(jī)器人附近的障礙物信息以scan的主題發(fā)布出去,IB-RRT*算法規(guī)劃器接收該消息,并檢測有沒有新的障礙物,如果有新的障礙物IB-RRT*則進(jìn)行實(shí)時(shí)重規(guī)劃,以避開障礙物,圖中的彩色區(qū)域?yàn)橐苿?dòng)機(jī)器人感知到的周圍障礙物的信息,綠色的線為初始規(guī)劃出來的路徑,紅色的線為實(shí)時(shí)重規(guī)劃的路徑。因此,該算法也可以適用于動(dòng)態(tài)環(huán)境的路徑規(guī)劃問題。

5 結(jié)論

以往的規(guī)劃算法中基本沒有考慮到移動(dòng)機(jī)器人自身的約束,可能會(huì)導(dǎo)致雖然已經(jīng)規(guī)劃出路徑,但無法被機(jī)器人所執(zhí)行。本文提出了一種新的RRT算法,它舍棄原算法貪心思想并引入啟發(fā)式采樣節(jié)點(diǎn)插入算法,同時(shí)從起始點(diǎn)和目標(biāo)點(diǎn)開始擴(kuò)展搜索樹,使得算法的收斂速度遠(yuǎn)快于改進(jìn)之前;并進(jìn)行理論分析證明改進(jìn)算法具有概率完備性、漸近最優(yōu)性,且確實(shí)優(yōu)于之前的算法。通過改進(jìn)算法與各種算法在不同的障礙物環(huán)境中進(jìn)行反復(fù)的仿真實(shí)驗(yàn),仿真的結(jié)果也說明了有效快速的解決復(fù)雜環(huán)境下的路徑規(guī)劃問題。但是該算法只能解決低速環(huán)境下的路徑規(guī)劃問題,下一步將對(duì)受滑動(dòng)干擾的移動(dòng)機(jī)器人模型的規(guī)劃問題及高速運(yùn)動(dòng)的規(guī)劃問題,進(jìn)一步的提高算法的實(shí)用性和魯棒性。

[1] 李磊,葉濤,譚民,等.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與未來[J].機(jī)器人,2002,24(5):475-480.DOI:10.1397 3/j. cnki.robot.2002.05.020.

[2] 王勇,蔡自興,周育人.肖赤心約束優(yōu)化進(jìn)化算法[J].軟件學(xué)報(bào),2009,20(1):11-29.DOI:10.3724/SP.J.1001.20 09. 03363.

[3] Song J Z,Dai B,Shan E Z,etal.An Improved RRT Path Planning Algorithm[J].ActaElectronicaSinica,2010,38(B02):225-228.

[4] Elbanhawi M,Simic M.Sampling-based Robot Motion Planning:A Review[J].IEEEAccess,2014,2(1):56-77.DOI:10.1109/ACCESS.2014.2302442.

[5] Qureshi A H,Ayaz Y.Intelligent Bidirectional Rapidly-exploring Random Trees for Optimal Motion Planning in Complex Cluttered Environments[J].RoboticsandAutonomousSystems,2015,68:1-11.DOI: 10.1016/j.robot. 2015.02.007.

[6] Karaman S,Frazzoli E.Sampling-based Algorithms for Optimal Motion Planning[J].TheInternationalJournalofRoboticsResearch,2011,30(7):846-894.DOI:10.1177/0278364911406761.

[7] Park J H,Tai Y W.A Simulation Based Method for Vehicle Motion Prediction[J].ComputerVisionandImageUnderstanding,2015,136:79-91.DOI:10.1016/j.cviu.2015.03.004.

[8] Vonásek V,Saska M,Winkler L,etal.High-level Motion Planning for CPG-driven Modular Robots[J].RoboticsandAutonomousSystems,2015,68:116-128.DOI:10.1016/j.robot.2015.01.006.

[9] Doshi A A,Postula A J,Fletcher A,etal.Development of Micro-UAV with Integrated Motion Planning for Open-cut Mining Surveillance[J].MicroprocessorsandMicrosystems,2015,39(8):829-835.DOI:10.1016/j.micpro. 2015.07.008.

[10] Park K J,Won M.People Tracking and Accompanying Algorithm for Mobile Robot Using Kinect Sensor and Extended Kalman Filter[J].TransactionsoftheKoreanSocietyofMechanicalEngineersA,2014,38(4):345-354.DOI:10.3795/KSME-A.2014.38.4.345.

[11] Kuffner J,RRT-Connect S L V.An Efficient Approach to Single-query Path Planning ieee International Conference on Robotics and Automation[J].SanFrancisco,2000,2:473-479.DOI:10.1109/ROBOT.2000.844730.

[12] LaValle S M,Kuffner J J.Randomized Kinodynamic Planning[J].TheInternationalJournalofRoboticsResearch,2001,20(5):378-400.DOI:10.1177/02783640122067453.

[13] Urmson C,Simmons R G.Approaches for Heuristically Biasing RRT Growth[C]∥IROS，2003,2:1178-1183.DOI:10.1109/IROS.2003.1248805.

[14] Ferguson D,Stentz A.Anytime Rrts[C]∥2006 IEEE/RSJ International Conference on Intelligent Robots and Systems.IEEE,2006:5369-5375.DOI:10.1109/IROS.2006.282100.

[15] Karaman S,Frazzoli E.Incremental Sampling-based Algorithms for Optimal Motion Planning[J].RoboticsScienceandSystemsVI,2010,104.DOI:10.15607/rss.2010.vi.034.

[16] Jordan M,Perez A.Optimal Bidirectional Rapidly-exploring Random Trees,MIT-CSAIL-TR-2013-021[R].Cambridge,UK:Computer Science and Artificial Intelligence Laboratory,2013.

[17] Qureshi A H,Mumtaz S,Iqbal K F,etal.Triangular Geometry based Optimal Motion Planning using RRT*-motion Planner[C]∥2014 IEEE 13th International Workshop on Advanced Motion Control (AMC).IEEE,2014:380-385.DOI:10.1109/AMC.2014.6823312.

[18] Bac C W,Roorda T,Reshef R,etal.Analysis of a Motion Planning Problem for Sweet-pepper Harvesting in a Dense Obstacle Environment[J].BiosystemsEngineering,2015,146:85-97.DOI:10.1016/j.Biosystemseng. 2015.07.004.

[19] 徐娜,陳雄,孔慶生,等.非完整約束下的機(jī)器人運(yùn)動(dòng)規(guī)劃算法[J].機(jī)器人,2011,33(6):666-672.DOI:10.3724/SP.J.1218.2011.00666.

[20] 杜明博,梅濤,陳佳佳,等.復(fù)雜環(huán)境下基于RRT的智能車輛運(yùn)動(dòng)規(guī)劃算法[J].機(jī)器人,2015,37(4):443-450.DOI:10.13973/j.cnki.robot.2015.0443.

[21] LaValle S M.Rapidly-exploring Random Trees:A New Tool Path Planning[R].Ames,USA:Iowa State University,1998.

Improved RRT*-Based Motion Planning Algorithm for Mobile Robot

PAN Siyu,XU Xiangrong*

(School of Mechanical Engineering,Anhui University of Technology,Ma’anshan 243002,China)

RRT*(rapidly-exploring random tree) has the disadvantages of instability and slow convergence rate in previous studies. To solve this problem, a new RRT*-based algorithm, IB-RRT algorithm, is proposed. This algorithm combines the environmental constraints, the constraints of intelligent vehicle and kinematics constraints, discards the bias in the original algorithm and sample insertion heuristic algorithm is introduced to increase the planning speed and quality greatly.Moreover,through theoretical analysis of the improved algorithm, it is proved that the improved algorithm has probability completeness, asymptotic optimality, which ensures that the algorithm can quickly converge to the optimal path in theory. The validity, stability and correctness of this algorithm are verified by the simulation experiments and the correctness of the theoretical analysis is also vevified.

RRT(rapidly-exploring random tree);path planning;mobile robot;ROS

山西大學(xué)學(xué)報(bào)(自然科學(xué)版)40(2):255-261,2017JournalofShanxiUniversity(Nat.Sci.Ed.)

10.13451/j.cnki.shanxi.univ(nat.sci.).2017.02.009

2016-11-22；

2016-12-22

國家外國專家局高端外國專家項(xiàng)目(GDT20153400058)

潘思宇(1992-)，男，安徽阜陽人，碩士生，研究方向?yàn)闄C(jī)器人技術(shù)及應(yīng)用，E-mail：1352674808@qq.com

*通信作者：徐向榮(XU Xiangrong)，E-mail：xuxr@ahut.edu.cn

TP301

A

0253-2395(2017)02-0244-11