一種移動機(jī)器人全局最優(yōu)路徑規(guī)劃算法

1、第37卷第3期2008年5月機(jī)器人ROBO TVol .30,No .3May,2008文章編號:100220446(20080320217206一種移動機(jī)器人全局最優(yōu)路徑規(guī)劃算法孟偲1,王田苗2(1.北京航空航天大學(xué)宇航學(xué)院,北京100083;2.北京航空航天大學(xué)機(jī)器人研究所,北京100083摘要:針對移動機(jī)器人全局最優(yōu)路徑規(guī)劃問題,首先提出一種基于線性操作的遍歷式算法,該算法通過場掃描方式生成步長轉(zhuǎn)換矩陣(ST M ,Step Transf or m Matrix ,并在步長轉(zhuǎn)換矩陣中搜索考慮方向一致的最短路徑,從而可以得到避免不必要路徑轉(zhuǎn)折的全局最短路徑.其次提出了一種評價標(biāo)準(zhǔn)來區(qū)分路徑

2、優(yōu)劣.最后通過仿真與圓形波傳播算法進(jìn)行了路徑規(guī)劃對比實驗,實驗結(jié)果表明本文所提算法在所提出的路徑評價標(biāo)準(zhǔn)下可以獲得比波傳播算法更優(yōu)的路徑.關(guān)鍵詞:移動機(jī)器人;全局路徑規(guī)劃;最優(yōu)路徑中圖分類號:TP24文獻(xiàn)標(biāo)識碼:BA Globa l O ptima l Pa th Pl ann i n g A lgor ith m for M ob ile RobotMENG Cai 1,WANG Tian 2m iao2(1.School of Astronautics,B eihang U niversity,B eijing 100083,China;2.Institute of R obotics,B

3、 eihang U niversity,B eijing 100083,China Abstract:For gl obal op ti m al path p lanning of mobile r obot,a linear operati on based traversal algorith m is p r oposed first 2ly .This algorith m computes the step transfor m matrix (ST M by field scan at first,and then searches the shortest path in th

4、e ST M with considerati on of search directi on coherence .Thus a gl obal shortest path which avoids unnecessary turn is p lanned .Secondly,a new evaluati on criteri on is p r oposed t o distinguish whether paths are excellent or not .Finally,path p lanning comparis on experi m ents are conducted wi

5、th r ound wave p r opagati on algorith m by si m ulati on .The results show that the algorith m can find better path than wave p r opagati on algorith m under the p r oposed criteri on .Keywords:mobile r obot;gl obal path p lanning;op ti m al path基金項目:國防基礎(chǔ)科研計劃資助項目(C2120061357;國家杰出青年科學(xué)基金資助項目(60525314

6、.收稿日期:2007-09-171引言(I n troducti on路徑規(guī)劃是移動機(jī)器人自主導(dǎo)航的關(guān)鍵技術(shù)之一.移動機(jī)器人路徑規(guī)劃是指在有障礙物的工作環(huán)境中,尋找一條從給定起點到終點的適當(dāng)運動路徑,使機(jī)器人在運動過程中能安全、無碰地繞過所有障礙物1.移動機(jī)器人的路徑規(guī)劃方法可分為基于地圖的全局路徑規(guī)劃和基于傳感器的局部路徑規(guī)劃2,3.全局路徑規(guī)劃依據(jù)對環(huán)境模型的表示方式可分為基于圖的方法(如Vor onoi 圖法、Q 2M 圖法等和基于柵格的方法(如D ijkstra 算法、A 3算法、波傳播算法等.其中,柵格法易于實現(xiàn)計算機(jī)的建模、存儲、處理、更新與分析,因而是目前研究和應(yīng)用最為廣泛的路徑

7、規(guī)劃方法4.全局路徑規(guī)劃的任務(wù)是根據(jù)環(huán)境模型找出從起始點到目標(biāo)點的、符合一定性能要求的可行或最優(yōu)路徑5,而所謂的最優(yōu)標(biāo)準(zhǔn)是指路徑最短、耗時最短或代價最小6.波傳播算法(W ave Pr opagati on A lgorithm 是模擬波在水面?zhèn)鞑サ姆绞竭M(jìn)行路徑規(guī)劃的方法.波傳播算法假設(shè)以目標(biāo)點為中心產(chǎn)生波向外擴(kuò)散,當(dāng)波到達(dá)起始點時,則目標(biāo)點到起始點的波傳遞最短途徑的逆序就是搜索路徑.Lat ombe 7和Lengyel 8的方法中采用矩形波進(jìn)行傳遞.矩形波簡單易行,但由于它假設(shè)波會同時到達(dá)對角線柵格和水平與豎直方向柵格,因此造成實際路徑不一定是最短路徑.鑒于矩形波的缺點,Szczerba 9

8、等人提出用圓形波代替矩形波,這樣波在傳遞過程中將首先到達(dá)水平和豎直方向的柵格,然后才會到達(dá)斜向柵格.圓形波解決了不同方向柵格波先后到達(dá)的問題,但增加了實現(xiàn)的復(fù)雜度.Szczerba 等人仍用矩形波方法傳遞圓波,只是將對角線柵格的218機(jī)器人2008年5月波距設(shè)為水平或豎直方向波距的2倍.國內(nèi)方面,李曉敏10基于Szczerba等人的方法提出了雙波傳播算法.波傳播算法同其它全局路徑規(guī)劃算法一樣,雖然可以搜索到基于柵格移動的最短路徑,但不一定是最優(yōu)路徑.全局路徑規(guī)劃一般將機(jī)器人當(dāng)作一個質(zhì)點處理11,12,因而搜索路徑時只考慮機(jī)器人的行走距離代價,而未考慮機(jī)器人在路徑轉(zhuǎn)折處發(fā)生的轉(zhuǎn)向代價.因此現(xiàn)有的

9、路徑規(guī)劃算法基本上是以路徑最短作為路徑最優(yōu)的標(biāo)準(zhǔn)(因為路徑長度基本代表了機(jī)器人移動功耗或時間.但最短路徑可能不止一條,每條路徑的平滑程度也不相同.在柵格環(huán)境中,路徑是由直線段組成的,因而路徑曲折程度可以反映路徑的平滑程度.如果在全局路徑規(guī)劃時考慮機(jī)器人移動方向一致性約束,從而減少路徑上的不必要的轉(zhuǎn)向,就可以使全局路徑更為平滑,從而便于實際機(jī)器人的跟蹤執(zhí)行.本文針對傳統(tǒng)全局路徑規(guī)劃算法僅考慮最短路徑而未考慮轉(zhuǎn)向代價的問題,借鑒波傳播算法提出一種基于場掃描的最短路徑傳播算法.該算法首先通過場掃描的方式生成步長轉(zhuǎn)換矩陣,然后在步長轉(zhuǎn)換矩陣中搜尋路徑,在搜尋路徑時優(yōu)先考慮路徑方向與上次搜索方向一致,從

10、而在確保搜索最短路徑的同時發(fā)生最少的轉(zhuǎn)向行為.2最短路徑場掃描傳播算法(The shortest pa th f i eld scan spread a lgor ith m假設(shè)環(huán)境地圖采用柵格建模,柵格值為可通過(自由空間與不可通過(障礙物.機(jī)器人在柵格中可以進(jìn)行8鄰域移動,即機(jī)器人在地圖中可允許走直線和走斜線,但限定不允許走出邊界和走到障礙物上.本算法的思想是:首先通過場掃描生成以目標(biāo)點為源點或根節(jié)點的類似圓形波算法中距離轉(zhuǎn)換矩陣的步長轉(zhuǎn)換矩陣;其次在步長轉(zhuǎn)換矩陣中尋找路徑,尋找路徑的同時將考慮路徑搜索方向的一致性.2.1步長轉(zhuǎn)換矩陣首先定義柵格之間的步長.對于如圖1所示的以p為中心的33

11、柵格,借用圖像術(shù)語定義:與p近鄰且共邊的處于“十”字線上的4個鄰域柵格稱為N4(p柵格,與p近鄰且共角的處于“”字線上的4個對角線柵格稱為ND(p柵格,N4(p與N D(p共同組成p點的8鄰域柵格N8(p.假設(shè)xN8(p,定義d(xd(x,p表示p到x的步長或距離.對于矩形波,d(x=1;對于Szczerba等人的圓形波算法:d(x=1,xN4(p2,xN D(p本文則定義:d(x=1,xN4(p1.5,xN D(p該定義表示,p到 N4(p的步長為1步,到N D(p的步長為1.5步.圖1相鄰柵格示意圖Fig.1Sche me of adjacent grids所謂步長轉(zhuǎn)換矩陣,是指用于表示柵

12、格圖中任意一點到達(dá)目標(biāo)點所需要的最少步數(shù)的矩陣數(shù)組.根據(jù)上面的定義,步數(shù)的最小單位是半步.步長轉(zhuǎn)換矩陣思想如下:假設(shè)目標(biāo)點為G,S(xS(x,G表示x到G的最短步長,則S(G=0.假設(shè)除G以外的p點到G的最短路徑S(p已知,qN8(p,如q可達(dá),則應(yīng)有:S(qS(p+d(q.為生成步長轉(zhuǎn)換矩陣,可以借鑒波傳遞算法,以G為源點向外一圈圈擴(kuò)展,但波傳遞法需要記錄當(dāng)前波的前沿及新傳播波的前沿.為此,本文借鑒圖像空間濾波方法,在生成步長轉(zhuǎn)換矩陣時,用如圖2(a (d所示的模板在柵格圖中移動,圖2所示模板含義為:p為當(dāng)前柵格,如果S(G已知,則對其鄰域柵格進(jìn)行處理.圖2所示每個模板只對鄰域中的兩個柵格進(jìn)

13、行處理,通過四個掃描方向應(yīng)用不同模板來覆蓋任一點的8鄰域.步長轉(zhuǎn)換矩陣的具體生成過程描述如下:(1首先生成與地圖同大小的步長矩陣S(,并賦全部初始值為(代表步長未知,設(shè)S(G=0;(2用圖2所示的4個場順序?qū)Φ貓D進(jìn)行掃描,在p點步長已知(S(p+d(x,則S(x=S(p+d(x,其中xq,q;(3反復(fù)執(zhí)行第(2步掃描過程,直到S(不再發(fā)生改變.第30卷第3期孟偲等:一種移動機(jī)器人全局最優(yōu)路徑規(guī)劃算法219由于在生成步長轉(zhuǎn)換矩陣時從不同方向?qū)D進(jìn)行反復(fù)掃描,直到步長轉(zhuǎn)換矩陣不再發(fā)生變化為止,因此將其稱為最短路徑的場掃描傳播算法(簡稱為FSS . 圖2場掃描示意圖Fig .2Sche me of

14、field scan2.2步長轉(zhuǎn)換矩陣在獲得步長轉(zhuǎn)換矩陣后,圖上任意一點到G 點的最短步長便已知.這樣機(jī)器人路徑規(guī)劃問題就可轉(zhuǎn)化為機(jī)器人用最少的步數(shù)到達(dá)目標(biāo)點.假設(shè)起始點為R,使機(jī)器人從R 點出發(fā),在步長轉(zhuǎn)換矩陣中從S (R 開始搜尋以步長1.5或1遞減至0的元素序列,序列中元素位置對應(yīng)的柵格點相連后就成為一條規(guī)劃路徑.具體過程為:(1如果起點R 處的步長代價S (R 未知(為,則不存在連通路徑,返回為空;(2如果S (R 已知(,則以R 為起點,設(shè)p =R;(3以p 為中心,在p 的8鄰域N 8(p 中尋找點q,點q 應(yīng)滿足S (q =S (p -d (q ,在初次搜尋時記錄搜索柵格方位,以

15、后優(yōu)先搜索上次柵格所在方位柵格,以q 點為新的p 點重復(fù)步驟(3,直到p =G 或者S (p =0為止;(4按順序連接p 點經(jīng)歷過的點即為搜尋路徑.2.3討論(1如果令d (x,p =1,(x N 8(p ,則上述算法的結(jié)果與矩形波傳播算法相同;但在搜索路徑過程中的第(3步必須先搜索N 4(p 再搜索N D (p ,否則搜索出的路徑將會產(chǎn)生鋸齒現(xiàn)象.(2如果令d (x,p =1,x N 4(p 1.5,x N D (p ,并在搜索路徑過程中的第(3步優(yōu)先搜索N D (p 再搜索N 4(p ,則其結(jié)果與圓形波傳播算法相當(dāng).(3在生成步長轉(zhuǎn)換矩陣時,由于初始時刻只有S (G 已知,故初始掃描可以從

16、G 點開始而不是從柵格圖的四個頂點開始,從而減少無用掃描.2.4算法復(fù)雜性分析及收斂性證明本文提出的算法,其占用空間主要是與地圖同大小的步長轉(zhuǎn)換矩陣S (,假設(shè)柵格地圖大小為N N ,則其需要額外空間為N 2.算法的操作主要是數(shù)組元素值的比較和賦值操作,在生成路徑轉(zhuǎn)換矩陣時對數(shù)組元素的訪問是線性連續(xù)的.關(guān)于算法的收斂性,主要基于以下兩個結(jié)論:結(jié)論1:經(jīng)過有限次掃描后步長轉(zhuǎn)換矩陣將穩(wěn)定不再發(fā)生變化.該結(jié)論證明如下:假設(shè)q 可達(dá)且在步長轉(zhuǎn)換矩陣中未穩(wěn)定,即S (q 尚不是q 到G 的最小步長,而對于所有p N 8(q ,其S (p 已穩(wěn)定,如果不是則沿此逆推.p 是其中距G 步長最小的點,則在下一

17、次掃描中,必然會使S (q =S (p +d (q,p 達(dá)到穩(wěn)定.而柵格地圖范圍有限,所以步長轉(zhuǎn)換矩陣經(jīng)有限次掃描后最終會穩(wěn)定不再發(fā)生變化.結(jié)論2:對于p,如果S (p 0且S (p 220機(jī)器人2008年5月,則必然q N 8(p 滿足S (q =S (p -d (q .結(jié)論2的成立是顯然的,因為S (p 0且S (p ,即p 不為終點且可達(dá),則必然在N 8(p 存在一點q 使得通過該點方能達(dá)到p,既然是通過該點到達(dá)p 的,則必然有S (p =S (q +d (q ,那么對于該點就有S (q =S (p -d (q .由結(jié)論1和2,可知2.1節(jié)中第(3步和2.2節(jié)中第(3步必然都是收斂的.3

18、路徑評價(Pa th eva lua ti on在全局路徑規(guī)劃中,一般以路徑最短作為搜索路徑的最優(yōu)標(biāo)準(zhǔn).但在柵格建模地圖環(huán)境中,最短路徑可能不止一條,單純以路徑長短不能區(qū)分路徑的優(yōu)劣.路徑的優(yōu)劣與路徑的安全性、平滑程度等有關(guān),但對路徑本身而言,除了路徑的長度以外,還可以用路徑的平滑程度加以細(xì)分.如圖3所示兩條路徑長度相同,但圖3(a 所示路徑發(fā)生2次轉(zhuǎn)折,而圖3(b 所示路徑發(fā)生了3次轉(zhuǎn)折,顯然前一條路徑更優(yōu).針對此現(xiàn)象本文提出用二元向量(,來對路徑進(jìn)行評價.設(shè)在柵格圖中以R 為起點、G 為目標(biāo)點的全局路徑為L =n 0,n 1,n 2,.,n N ,其中n i =(x i ,y i ,i =

19、0,1,.,N ,表示路徑的第i 個節(jié)點,x i 、y i 表示n i 節(jié)點的x 坐標(biāo)與y 坐標(biāo).路徑上共有N +1個節(jié)點,且n 0=R 、n N =G .路徑的長短一定程度上代表了機(jī)器人移動消耗的能量和時間.對于2D 路徑規(guī)劃,路徑長度可以定義為所有節(jié)點間向量的2范數(shù)之和,也即歐氏距離:=N -1i =0ni+1-n i 2=N -1i =0(x i+1-x i 2+(y i+1-y i 2路徑長度是衡量一條路徑是否最優(yōu)的一個重要指標(biāo).路徑平滑度是衡量路徑的另一個指標(biāo),對于由直線段組成的路徑,我們用累積轉(zhuǎn)向角來衡量其平 圖3兩條路徑對比示意圖Fig .3Comparis on of t w

20、o paths滑度.累積轉(zhuǎn)向角可以這樣理解,假設(shè)機(jī)器人沿直線移動,到達(dá)轉(zhuǎn)折點后原地轉(zhuǎn)向到下一方向然后繼續(xù)行走.原地轉(zhuǎn)向?qū)囊欢芰亢蜁r間.定義累積轉(zhuǎn)向角:=N -1i =1ni其中,n i 是矢量n i-1n i _與矢量n i n i+1_方向之間的夾角,一般而言它是n i-1n i n i+1的補角,所以n i =-n i-1n i n i+1n i-1n i n i+1=arccosn i-1-n i 2+n i+1-n i 2-n i+1-n i-122n i-1-n i n i+1-n i 對于m 條路徑L i (i =1,.,m ,定義具有以下屬性的路徑為最優(yōu)路徑(設(shè)m in

21、(X i 表示求最小值:(1具有最短路徑長度m in (i 的路徑為最優(yōu)路徑;(2當(dāng)具有m in (i 的路徑不止一條時,則其中具有最小累積轉(zhuǎn)向角m in (i 的路徑為最優(yōu)路徑.4路徑規(guī)劃對比實驗(Co m par ison exper i 2m en ts of pa th pl ann i n g為了驗證本文算法的有效性,在Matlab 環(huán)境下進(jìn)行了仿真試驗.由于本文算法與波傳播算法具有一第30卷第3期孟偲等:一種移動機(jī)器人全局最優(yōu)路徑規(guī)劃算法221定的相似性,所以在實現(xiàn)本文算法的同時也實現(xiàn)了圓形波波傳播算法,其流程圖如圖4所示.為了比較兩種算法,實驗除了給出路徑評價指標(biāo)外,還列出了算法

22、搜索路徑的時間.兩種算法均用Matlab 編程實現(xiàn).測試軟件環(huán)境為W indo ws XP 、Matlab R7.1,硬件環(huán)境為Pentiu m 1.8Hz,512M 內(nèi)存.圖中黑色區(qū)域代表障礙. 圖4圓形波算法流程圖Fig .4Fl owchart of r ound wave p r opagati on alg orith m測試方法為:在相同地圖下,從相同的起始點出發(fā)用本文算法與圓形波波傳播算法分別進(jìn)行搜索.進(jìn)行兩次測試實驗,測試實驗1用了大小為100100的柵格地圖,測試實驗2用了大小為400400的柵格地圖.對比測試實驗結(jié)果分別如圖5和圖6所示,其搜索路徑衡量指標(biāo)及算法所用時間分別

23、如表1和表2所示.表1實驗1測試結(jié)果Table 1Results of exper i m en t 1算法路徑長度累積轉(zhuǎn)向(耗時t (s FSS140.083390 2.48圓形波140.08331352.63表2實驗2測試結(jié)果Table 2Result ofexper i m en t 2算法路徑長度累積轉(zhuǎn)向(耗時t (s FSS582.269118048.77圓形波582.2691945499.87測試實驗結(jié)果討論分析如下:(1在兩次實驗中FSS 算法搜索到的路徑優(yōu)于圓形波波傳播算法,這是因為搜索路徑時FSS 算法首先考慮讓機(jī)器人不改變移動方向,所以使搜索的路徑減少了轉(zhuǎn)折. 圖5對比實驗

24、1Fig .5Comparis on experi m ent 1(2在實驗1中兩種算法耗時相當(dāng),而在實驗2中圓形波波傳播算法耗時則遠(yuǎn)超F(xiàn)SS 算法.這表明地圖規(guī)模增大對波傳播算法耗時的影響大于對FSS 算法的影響.這是因為FSS 是以數(shù)組操作為基礎(chǔ),操作基本是線性的,而波傳播算法在維護(hù)波前沿隊列W FT 時需要進(jìn)行隊列(或鏈表的插入、刪除等操作,當(dāng)涉及規(guī)模變大時,它們之間的差異就變得顯著.(3FSS 算法生成步長轉(zhuǎn)換矩陣時覆蓋整個地圖,而波傳播算法生成距離轉(zhuǎn)換矩陣時可以在波到達(dá)R 點時結(jié)束,其覆蓋范圍可以小于地圖.所以當(dāng)規(guī)模較小或者R 與G 相距較近時,波傳播算法的速度222 機(jī) 器 人 2

25、008 年 5 月 將會優(yōu)于 FSS,這可通過實驗證實 , 但限于篇幅未給 出實驗 . 鑒于此 ,當(dāng)問題規(guī)模較小時可用波傳播法來 生成步長轉(zhuǎn)換矩陣 . 通過上述分析討論 ,可以形成如 下結(jié)論 : FSS算法在搜索最短路徑的同時考慮方向 一致性約束 ,尋找相對更為平滑的最短路徑 ; 由于它 是一種全圖遍歷算法 , 因而它適用于目的就是為得 到全局最優(yōu)路徑尤其是大規(guī)模地圖下尋找最優(yōu)路徑 的任務(wù) . lab 進(jìn)行了仿真實驗并與波傳播算法進(jìn)行了對比 . 實 驗結(jié)果表明本文所提算法可以搜索到相對更為平滑 的最短路徑 . 本文所提算法是一種遍歷式路徑規(guī)劃算法 , 因 而也具有遍歷式算法的特點 ,如需要訪問

26、所有節(jié)點 , 耗時一般會比啟發(fā)式算法長 , 因而適合于更關(guān)注路 徑性能而不是算法耗時的路徑規(guī)劃任務(wù) . 此外該算 法步長代價圖包含了地圖上所有節(jié)點到目標(biāo)點的最 短路徑代價 , 因而也適合于多機(jī)器人需要到同一目 標(biāo)點進(jìn)行協(xié)作的路徑規(guī)劃 . 關(guān)于路徑評價 , 本文只是提出一種簡單的評價 方法來區(qū)分優(yōu)劣 , 且用累積轉(zhuǎn)向角度來表征路徑平 滑程度是否合理也還值得商榷 . 此外關(guān)于路徑評價 應(yīng)該還有更科學(xué)的方法 . 5 結(jié)論與展望 ( Conclusion and perspective 本文首先提出一種考慮路徑轉(zhuǎn)向代價的全局路 徑規(guī)劃算法 , 該算法通過場掃描方式生成步長轉(zhuǎn)換 矩陣并在步長轉(zhuǎn)換矩陣中尋

27、找最短路徑時考慮搜索 方向的一致性 . 然后針對路徑長度相同但優(yōu)劣有不 同的問題 ,提出一種路徑二元評價方法 . 最后用 M at2 參考文獻(xiàn) ( References 1 席裕庚 ,張純剛 . 一類動態(tài)不確定環(huán)境下機(jī)器人的滾動路徑規(guī) 圖 6 對比實驗 2 Fig 6 Comparison experim ent 2 . 9 Szczerba R J, Chen D Z, Uhran J r J J. A grid 2based app roach for finding conditional shortest paths in an unknown environment EB / 10

28、李曉敏 . 智能移動機(jī)器人全局路徑規(guī)劃及仿真 D . 南京 : 南 劃 J . 自動化學(xué)報 , 2002, 28 ( 2 : 161 - 175. 2 蔡自興 ,賀漢根 ,陳虹 . 未知環(huán)境中移動機(jī)器人導(dǎo)航控制研究 的若干問題 J . 控制與決策 , 2002, 17 ( 4 : 385 - 390, 464. 望 J . 系統(tǒng)仿真學(xué)報 , 2005, 17 ( 2 : 439 - 443. 天津 : 南開大學(xué)研究生院 , 2005. 京理工大學(xué) , 2004. 3 張捍東 ,鄭睿 ,岑豫皖 . 移動機(jī)器人路徑規(guī)劃技術(shù)的現(xiàn)狀與展 4 劉作軍 . 基于電路地圖的移動機(jī)器人路徑規(guī)劃方法研究 D . 5 化