一種偏向目標(biāo)型的RRT算法實(shí)現(xiàn)

1、一種偏向目標(biāo)型的rrt算法實(shí)現(xiàn)摘要：本文針對(duì)基本快速擴(kuò)展隨機(jī)樹(rrt)算法存在搜索過(guò)于平均、效率低下、用時(shí)較長(zhǎng)的缺陷，提出了一種偏向目標(biāo)型的改進(jìn)型rrt算法。這種算法在生成隨機(jī)點(diǎn)時(shí)以一定概率選擇最終目標(biāo)點(diǎn)作為局部目標(biāo)點(diǎn)，使樹的擴(kuò)展有一個(gè)趨向于最終目標(biāo)點(diǎn)的趨勢(shì)，從而加快了算法的收斂速度，優(yōu)化了規(guī)劃路徑。最后通過(guò)matlab程序仿真，在二維平面上驗(yàn)證了改進(jìn)型算法相對(duì)于基本算法的優(yōu)越性。關(guān)鍵詞：路徑規(guī)劃、rrt算法、偏向目標(biāo)型一. 引言機(jī)器人學(xué)是當(dāng)今科學(xué)技術(shù)研究的熱點(diǎn)問(wèn)題，它匯聚了各個(gè)尖端學(xué)科最先進(jìn)的研究成果。科學(xué)家們從上世紀(jì)40年代開始著手研制機(jī)器人到如今，機(jī)器人的發(fā)展主要經(jīng)歷了三次技術(shù)變革。從

2、最簡(jiǎn)單的第一代機(jī)器人到現(xiàn)在的第三代智能機(jī)器人，機(jī)器人從只會(huì)機(jī)械的執(zhí)行命令逐漸演變成利用各種先進(jìn)的傳感器自動(dòng)的學(xué)習(xí)環(huán)境，適應(yīng)環(huán)境，并完成人類下達(dá)的任務(wù)。路徑規(guī)劃問(wèn)題是機(jī)器人研究中的重要的組成部分，它的重點(diǎn)就是要使機(jī)器人自主并且安全的從起始位姿移動(dòng)到目標(biāo)位姿。機(jī)器人路徑規(guī)劃主要分為全局路徑規(guī)劃和局部路徑規(guī)劃兩大方面。全局路徑規(guī)劃是一種利用環(huán)境全局信息的方法，它通常將周邊環(huán)境信息存儲(chǔ)在一張地圖中，并且利用這張地圖尋找可行路徑。全局路徑規(guī)劃的優(yōu)點(diǎn)是有利于找到全局可行解和最優(yōu)解，但是它的運(yùn)算時(shí)間長(zhǎng)，不適用于快速變化的動(dòng)態(tài)環(huán)境。常用的全局路徑規(guī)劃方法有柵格法、可視圖法、拓?fù)浞ê妥杂煽臻g法等。局部路徑規(guī)劃只

3、考慮機(jī)器人當(dāng)前能探測(cè)到的環(huán)境信息，運(yùn)算速度快、反應(yīng)迅速，非常適用于動(dòng)態(tài)環(huán)境。但其缺點(diǎn)是算法可能無(wú)法收斂，不能保證機(jī)器人一定能夠到達(dá)目標(biāo)點(diǎn)，而且找到的可行路徑可能會(huì)偏離最短路徑。常用的局部路徑規(guī)劃算法有人工勢(shì)場(chǎng)法、模糊邏輯法、神經(jīng)網(wǎng)絡(luò)法和遺傳算法等等。rrt算法是最近幾年才發(fā)展起來(lái)的，并且應(yīng)用比較普遍的一種路徑規(guī)劃算法。它在處理非完整約束的路徑規(guī)劃問(wèn)題時(shí)具有相當(dāng)大的優(yōu)勢(shì)，因?yàn)樗梢詫⒏鞣N約束集成到算法本身之中，因此對(duì)環(huán)境要求較低。而且該算法概率完備，在理論上肯定能找到可行路徑。但其缺點(diǎn)是搜索過(guò)于平均，算法效率較低，而且規(guī)劃出的路徑往往偏離最短路徑。本文針對(duì)rrt算法存在的缺陷，提出了一種改進(jìn)的偏

4、向目標(biāo)型的rrt算法，該算法有效的解決了搜索過(guò)于平均的問(wèn)題，提高了算法效率，而且規(guī)劃出的路徑是更接近于最短路徑的次優(yōu)路徑。二. rrt算法的基本原理rrt算法在路徑規(guī)劃時(shí)以狀態(tài)空間中的一個(gè)初始點(diǎn)作為根節(jié)點(diǎn)，通過(guò)隨機(jī)采樣逐漸增加葉節(jié)點(diǎn)的方式，生成一個(gè)隨機(jī)擴(kuò)展樹。當(dāng)隨機(jī)樹中的葉節(jié)點(diǎn)中包含了目標(biāo)點(diǎn)或目標(biāo)區(qū)域時(shí)，便可在隨機(jī)樹中找到一條從根節(jié)點(diǎn)到目標(biāo)點(diǎn)的路徑。rrt的擴(kuò)展方式如下圖1所示。圖 1 rrt算法擴(kuò)展過(guò)程圖1中t表示當(dāng)前存在的擴(kuò)展樹，表示隨機(jī)點(diǎn)，表示離隨機(jī)點(diǎn)最近的一個(gè)樹節(jié)點(diǎn)，然后在和的連線上以步長(zhǎng)step為單位截取一個(gè)新節(jié)點(diǎn)，如果沒(méi)碰到障礙物，則將加入到擴(kuò)展樹t中。重復(fù)以上步驟直到到達(dá)目標(biāo)區(qū)域

5、則算法結(jié)束，此時(shí)可在樹t中找到一條起點(diǎn)到目標(biāo)點(diǎn)的路徑。為了便于計(jì)算機(jī)編程實(shí)現(xiàn)，我們將rrt的構(gòu)建過(guò)程歸納為以下兩個(gè)表格。其中表1為各參數(shù)意義，表2為rrt構(gòu)建流程。表 1 rrt算法中的各參數(shù)意義s所有空間無(wú)障礙空間有k個(gè)節(jié)點(diǎn)的隨機(jī)樹起始點(diǎn)目標(biāo)點(diǎn)step步長(zhǎng)dis(q1,q2)s中兩點(diǎn)之間的距離表 2 rrt算法構(gòu)建流程1=2判斷|-|step,若是，轉(zhuǎn)到步驟8，不是，轉(zhuǎn)到步驟33生成隨機(jī)點(diǎn)4找出使dis(,)dis(q, )5在和的連線上求使dis(,)=step,并且，若存在這樣的，轉(zhuǎn)到步驟6，若不存在，轉(zhuǎn)到步驟36在擴(kuò)展樹上增加節(jié)點(diǎn)，7判斷|-|step,若是，轉(zhuǎn)到步驟8，不是，轉(zhuǎn)到步驟

6、38結(jié)束三. 改進(jìn)的rrt算法雖然rrt算法概率完備，在理論上總能找出一條可行路徑。但是由于其擴(kuò)展新節(jié)點(diǎn)的方式是在全空間隨機(jī)產(chǎn)生的，一方面造成擴(kuò)展樹在全空間分布過(guò)于平均，算法效率較低；另一方面規(guī)劃的路徑質(zhì)量不高，通常偏離最短路徑較大。針對(duì)以上缺陷，我們希望對(duì)基本的rrt算法進(jìn)行改進(jìn)。經(jīng)過(guò)分析我們得知造成rrt算法上述缺陷的根源是在擴(kuò)展新節(jié)點(diǎn)時(shí)，隨機(jī)點(diǎn)是在全空間隨機(jī)產(chǎn)生的。借鑒啟發(fā)式算法的靈感，我們可以在確定隨機(jī)點(diǎn)時(shí)讓隨機(jī)點(diǎn)以一定的概率等于目標(biāo)點(diǎn)。這樣樹的擴(kuò)展就有一個(gè)趨于目標(biāo)點(diǎn)的趨勢(shì)，而不是在全空間隨機(jī)分布，從而提高了算法的搜索效率，而且由于樹節(jié)點(diǎn)的擴(kuò)展是趨向于目標(biāo)點(diǎn)的，理論上規(guī)劃出來(lái)的路徑也會(huì)

7、更加接近于最短路徑。還應(yīng)注意的是改進(jìn)后的算法在概率上依然是完備的。通過(guò)以上分析，我們知道改進(jìn)的rrt算法流程和基本的rrt算法流程大致相同，只要把第二節(jié)中表2的第3個(gè)步驟進(jìn)行如下的改寫即可。（1） 生成隨機(jī)點(diǎn)；（2） 給定一個(gè)0到1的偏置變量bias，生成一個(gè)0到1的隨機(jī)數(shù)rand；（3） 如果randbias,則=；否則， 不變。四. matlab仿真實(shí)驗(yàn)1.定義環(huán)境模型本文仿真是在二維平面上進(jìn)行的，將整個(gè)平面劃分為了有障礙部分和無(wú)障礙物部分，如下圖2所示。圖 2環(huán)境模型示意圖地圖尺寸為100*100，其中白色區(qū)域代表無(wú)障礙空間，機(jī)器人可以隨意行走；黑色區(qū)域表示障礙物空間，機(jī)器人不能通行。為

8、了仿真的方便，這里的障礙物都選擇為圓形，這不影響算法驗(yàn)證的可靠性。2.定義節(jié)點(diǎn)數(shù)據(jù)結(jié)構(gòu) 分析可知擴(kuò)展樹的每個(gè)節(jié)點(diǎn)有三個(gè)必要要素，分別是節(jié)點(diǎn)的x，y坐標(biāo)以及其父節(jié)點(diǎn)。因此我們定義節(jié)點(diǎn)的數(shù)據(jù)結(jié)構(gòu)如下：其中表示節(jié)點(diǎn)在二維平面上的坐標(biāo)，表示節(jié)點(diǎn)的父節(jié)點(diǎn)序號(hào)。考慮到起點(diǎn)作為根結(jié)點(diǎn)，其沒(méi)有父節(jié)點(diǎn)，因此定義它的父節(jié)點(diǎn)序號(hào)為0。3.仿真結(jié)果對(duì)比（1）分布障礙地圖 起點(diǎn)（5，80），終點(diǎn)（90，70）圖 3 路徑規(guī)劃圖，rrt算法(左)，改進(jìn)rrt算法(右) 上圖中藍(lán)色的“*”點(diǎn)表示擴(kuò)展樹的所有節(jié)點(diǎn)，紅色的曲線表示規(guī)劃得到的路徑。通過(guò)圖3的對(duì)比明顯可以看出在分布障礙地圖中，偏向目標(biāo)型rrt算法得到的擴(kuò)展樹節(jié)點(diǎn)數(shù)

9、更少，這說(shuō)明算法的效率更高。觀察路徑曲線可以看出偏向目標(biāo)型rrt算法得到的路徑更優(yōu)。（2）狹窄通道地圖 起點(diǎn)（1，1），終點(diǎn)（90，90）圖 4路徑規(guī)劃圖，rrt算法(左)，改進(jìn)rrt算法(右) 從圖4中可以看出在狹窄通道地圖中，我們可以明顯的觀察到基本rrt算法在搜索時(shí)表現(xiàn)出來(lái)的搜索過(guò)于平均，算法效率低下的缺陷，而且規(guī)劃得到的路徑也偏離最優(yōu)路徑較大。而改進(jìn)后的偏向目標(biāo)型rrt算法體現(xiàn)出了強(qiáng)烈的趨向于目標(biāo)點(diǎn)趨勢(shì)，而且規(guī)劃得到的路徑也非常接近于最優(yōu)路徑。（3）復(fù)雜隨機(jī)地圖起點(diǎn)（1，1），終點(diǎn)（90，90） 圖 5路徑規(guī)劃圖，rrt算法(左)，改進(jìn)rrt算法(右) 通過(guò)圖5，我們也可以看出改進(jìn)后的

10、rrt算法在復(fù)雜隨機(jī)地圖中也表現(xiàn)優(yōu)異，證明了偏向目標(biāo)型rrt算法的優(yōu)越性。五. 總結(jié)本文針對(duì)基本rrt算法存在搜索過(guò)于平均，算法效率低下，規(guī)劃路徑偏離最短路徑較大的缺陷，分析其缺陷原因在于隨機(jī)點(diǎn)的確定在全空間分布過(guò)于平均導(dǎo)致的。借鑒啟發(fā)式算法的思想，我們提出了一種確定隨機(jī)點(diǎn)的新方法，即讓隨機(jī)點(diǎn)以一定的概率等于目標(biāo)點(diǎn)，這樣就使隨機(jī)樹的擴(kuò)展有一種趨向于目標(biāo)點(diǎn)的趨勢(shì)，從而解決了隨機(jī)點(diǎn)分布過(guò)于平均的缺點(diǎn)。最后通過(guò)matlab仿真對(duì)兩種算法的結(jié)果對(duì)比分析得到，改進(jìn)后的偏向目標(biāo)型rrt算法相對(duì)于基本rrt算法，無(wú)論在算法效率還是路徑質(zhì)量，都體現(xiàn)出了很大的優(yōu)越性。參考文獻(xiàn)1王全.基于rrt的全局路徑規(guī)劃方法

11、及其應(yīng)用研究d.國(guó)防科學(xué)技術(shù)大學(xué),2014. 2李加?xùn)|.基于rrt算法的非完整移動(dòng)機(jī)器人運(yùn)動(dòng)規(guī)劃d.華東理工大學(xué),2014. 3馮林,賈菁輝.基于對(duì)比優(yōu)化的rrt路徑規(guī)劃改進(jìn)算法j.計(jì)算機(jī)工程與應(yīng)用,2011,47(3):210-213,228. 4賈菁輝.移動(dòng)機(jī)器人的路徑規(guī)劃與安全導(dǎo)航d.大連理工大學(xué),2009. 5周培培.未知環(huán)境下機(jī)器人路徑規(guī)劃算法的研究d.青島科技大學(xué),2014. 6李猛.基于智能優(yōu)化與rrt算法的無(wú)人機(jī)任務(wù)規(guī)劃方法研究d.南京航空航天大學(xué),2012. 7王濱,金明河,謝宗武等.基于啟發(fā)式的快速擴(kuò)展隨機(jī)樹路徑規(guī)劃算法j.機(jī)械制造,2007,45(12):1-4. 8宋金

12、澤,戴斌,單恩忠等.一種改進(jìn)的rrt路徑規(guī)劃算法j.電子學(xué)報(bào),2010,38(z1):225-228.9喬永興.自主式移動(dòng)機(jī)器人的路徑規(guī)劃d.廣西大學(xué),2003. 10李磊,葉濤,譚民等.移動(dòng)機(jī)器人技術(shù)研究現(xiàn)狀與未來(lái)j.機(jī)器人,2002,24(5):475-480.11陳剛,沈林成.復(fù)雜環(huán)境下路徑規(guī)劃問(wèn)題的遺傳路徑規(guī)劃方法j.機(jī)器人,2001,23(1):40-44.附錄本文中使用的matlab程序%主程序function biasgoal_rrtmap=createmap(1); %創(chuàng)建有障礙物的模擬地圖，輸入?yún)?shù)為不同的地圖類型step=5; %步長(zhǎng)startnode=1,1,0; %起點(diǎn)

13、endnode=90,90,0; %終點(diǎn)tree=startnode; %根結(jié)點(diǎn)if(norm(startnode(1,1:2)-endnode(1,1:2)=step)&(collision(startnode,endnode,map)=0) path=startnode(1,1:2);endnode(1,1:2);else success=0; while success=0 tree,flag=extendtree(tree,endnode,step,map); success=flag; endendpath=findpath(tree);plotmap(map,path,tree);

14、%創(chuàng)建地圖，有三種不同類型的地圖function map=createmap(num)if num=1 %分布障礙地圖 map.barnum=3; map.llcd=0;0; %地圖左下角坐標(biāo) map.urcd=100;100; %地圖右上角坐標(biāo) radius=20;15;20; %障礙物半徑 barcenter=33,75;38,30;75,50; %障礙物中心點(diǎn)坐標(biāo) for i=1:map.barnum map.radius(i)=radius(i); map.cx(i)=barcenter(i,1); map.cy(i)=barcenter(i,2); endendif num=2 %狹

15、窄通道地圖 map.barnum=2; map.llcd=0;0; map.urcd=100;100; radius=23.75;23.75; barcenter=50,76.25;50,23.75; for i=1:map.barnum map.radius(i)=radius(i); map.cx(i)=barcenter(i,1); map.cy(i)=barcenter(i,2); endendif num=3 %復(fù)雜隨機(jī)地圖 map.barnum=100; map.llcd=0;0; map.urcd=100;100; maxradius=2.5; for i=1:map.barnu

16、m map.radius(i)=maxradius*rand;map.cx(i)=map.llcd(1)+map.radius(i)+(map.urcd(1)-map.llcd(1)-2*map.radius(i)*rand;map.cy(i)=map.llcd(2)+map.radius(i)+(map.urcd(2)-map.llcd(2)-2*map.radius(i)*rand; endend%基本rrt算法程序function newtree,flagreach=extendtree(tree,endnode,step,map)flag=1;while flag randpoint=

17、(map.urcd(1)-map.llcd(1)*rand,(map.urcd(2)-map.llcd(2)*rand; distmp=tree(:,1:2)-ones(size(tree,1),1)*randpoint; mindis,minnum = min(diag(distmp*distmp); pvector=randpoint-tree(minnum,1:2); newpoint=tree(minnum,1:2)+pvector/norm(pvector)*step; if collision(newpoint,tree(minnum,1:2),map)=0 newnode=new

18、point,minnum; newtree=tree;newnode; flag=0; endendif(norm(newpoint-endnode(1,1:2)=step)&(collision(newpoint,endnode(1,1:2),map)=0) flagreach=1;else flagreach=0;end%改進(jìn)后的偏向目標(biāo)型rrt算法程序function newtree,flagreach=biasextendtree(tree,endnode,step,map)flag=1;bias=0.5;while flag randpoint=(map.urcd(1)-map.ll

19、cd(1)*rand,(map.urcd(2)-map.llcd(2)*rand; if randbias randpoint=endnode(1:2); end distmp=tree(:,1:2)-ones(size(tree,1),1)*randpoint; mindis,minnum = min(diag(distmp*distmp); pvector=randpoint-tree(minnum,1:2); newpoint=tree(minnum,1:2)+pvector/norm(pvector)*step; if collision(newpoint,tree(minnum,1:

20、2),map)=0 newnode=newpoint,minnum; newtree=tree;newnode; flag=0; endendif(norm(newpoint-endnode(1,1:2)=step)&(collision(newpoint,endnode(1,1:2),map)=0) flagreach=1;else flagreach=0;end%檢測(cè)新節(jié)點(diǎn)和每一個(gè)障礙物是否有碰撞，若有則返回1function collision_flag = collision(node, parent, map);collision_flag = 0;for sigma = 0:.2:1, p = sigma*node(1:2) + (1-sigma)*parent(1:2); for i=1:map.barnum, if (norm(p(1);p(2)-map.cx(i); map.cy(i)=map.radius(i), collision_flag = 1; break; end endend%隨機(jī)擴(kuò)展樹完成后，根據(jù)每個(gè)節(jié)點(diǎn)父節(jié)點(diǎn)的序號(hào)找出可行路徑function path=findpath(tree)lastnodenum=size(tree,1);path=tree(