張 智,翁宗南,蘇 麗,光正慧

(哈爾濱工程大學(xué) 自動(dòng)化學(xué)院,哈爾濱 150001)E-mail:2728397813@qq.com

1 引 言

移動(dòng)機(jī)器人的任意路徑規(guī)劃[1]問(wèn)題是機(jī)器人研究領(lǐng)域中遇到的需要突破的且極具挑戰(zhàn)性的問(wèn)題,更是機(jī)器人研究的核心內(nèi)容之一.在現(xiàn)階段研究的全局路徑規(guī)劃中機(jī)器人所處的環(huán)境大多以不動(dòng)的障礙物為主,但是實(shí)際生活中機(jī)器人需要面對(duì)復(fù)雜的人類(lèi)生活.因此針對(duì)機(jī)器人的研究的工作還有許多關(guān)鍵性的難題亟待突破.很多成熟的算法已經(jīng)可以解決路徑規(guī)劃的相關(guān)問(wèn)題,如人工勢(shì)場(chǎng)法、視圖法、切線圖法、拓?fù)浞?、A*算法[2]、D*算法等.

1)Khatib提出的人工勢(shì)場(chǎng)法的基本思想是基于虛擬力法(參考物理學(xué)中受力分析),對(duì)活動(dòng)環(huán)境中運(yùn)動(dòng)的機(jī)器人進(jìn)行模擬的受力分析.這種方法結(jié)構(gòu)簡(jiǎn)單,但在該人為抽象出來(lái)的場(chǎng)作用下機(jī)器人很可能會(huì)被困在某一平衡點(diǎn)上,從而發(fā)生死鎖現(xiàn)象,這樣可能會(huì)讓移動(dòng)機(jī)器人[4]在到達(dá)目標(biāo)點(diǎn)之前就停留在某個(gè)局部平衡點(diǎn)上從而發(fā)生死鎖.

2)柵格分解法使用大小相同的方格把機(jī)器人可能搜索到的環(huán)境進(jìn)行劃分,劃分后的方格在程序中用數(shù)組代替.劃分后的環(huán)境分為兩類(lèi),機(jī)器人可自由移動(dòng)的區(qū)域,阻礙機(jī)器人運(yùn)動(dòng)的路障區(qū).缺點(diǎn)是表示效率不高.

3)A*算法又名啟發(fā)式(heuristic)算法,是一種靜態(tài)路網(wǎng)中求解最短路最有效的方法,主要搜索過(guò)程:創(chuàng)建兩個(gè)表,OPEN表保存所有已生成而未考察的節(jié)點(diǎn),CLOSED表中記錄已訪問(wèn)過(guò)的節(jié)點(diǎn).遍歷當(dāng)前節(jié)點(diǎn)的各個(gè)節(jié)點(diǎn),將n節(jié)點(diǎn)放入CLOSE中,取n節(jié)點(diǎn)的子節(jié)點(diǎn)X并算X的估價(jià)值.A* 在靜態(tài)路網(wǎng)中非常有效(very efficient for static worlds),但不適于在動(dòng)態(tài)路網(wǎng),環(huán)境如權(quán)重等不斷變化的動(dòng)態(tài)環(huán)境下.D*是動(dòng)態(tài)A*(D-Star,Dynamic A Star)卡內(nèi)基梅隆機(jī)器人中心的Stentz在1994和1995年兩篇文章提出,主要用于機(jī)器人探路,也是火星探測(cè)器采用的尋路算法[3].

本文利用A*(A Star)算法與D*算法的轉(zhuǎn)換思路加以改進(jìn),實(shí)現(xiàn)較快速地規(guī)劃出較優(yōu)的全局路徑,即模擬A*算法,實(shí)現(xiàn)移動(dòng)機(jī)器人在環(huán)境地圖未知的情況下,快速精確的規(guī)劃出最優(yōu)的全局路徑[5],并且保證算法的簡(jiǎn)捷有效性,并基于SLAM[7]激光定位機(jī)器人利用模擬A*動(dòng)態(tài)規(guī)劃算法進(jìn)行避碰路徑規(guī)劃.

2 路徑規(guī)劃方法

2.1 A*算法

A*算法具有悠久的歷史,該算法在很多領(lǐng)域又被稱為啟發(fā)式搜索法.A*算法的函數(shù)表達(dá)式為:

f(n)=g(n)+h(n)

(1)

其中f(n)是節(jié)點(diǎn)n從初始點(diǎn)到目標(biāo)點(diǎn)的估價(jià)函數(shù),g(n)是在狀態(tài)空間中從初始節(jié)點(diǎn)到n節(jié)點(diǎn)的實(shí)際代價(jià),h(n)是從n到目標(biāo)節(jié)點(diǎn)最佳路徑的估計(jì)代價(jià).保證找到最短路徑(最優(yōu)解的)條件,關(guān)鍵在于估價(jià)函數(shù)h(n)的選取:估價(jià)值h(n)<=n到目標(biāo)節(jié)點(diǎn)的距離實(shí)際值,這種情況下,搜索的點(diǎn)數(shù)多,搜索范圍大,效率低.但能得到最優(yōu)解.如果估價(jià)值大于實(shí)際值,搜索的點(diǎn)數(shù)少,搜索范圍小,效率高,但不能保證得到最優(yōu)解.估價(jià)值與實(shí)際值越接近,估價(jià)函數(shù)取得就越好.

1)實(shí)際應(yīng)用中A*算法應(yīng)嚴(yán)格符合h(x)<=h*(x),式(1)中的h*(x)是實(shí)際的啟發(fā)值,h*(x)在現(xiàn)實(shí)生活中往往是不能提前得知的,但是上述要求是容易符合的,只要滿足上述要求,最優(yōu)的路徑規(guī)劃[9]可以被獲得.

2)假設(shè)最短的路徑距離是C,那么在算法運(yùn)行完成之前,在OPEN表里至少有一個(gè)點(diǎn)n,能夠使得f(n)小于等于C.該性質(zhì)可以理解為:因?yàn)樽疃搪窂酱嬖?我們可以將start-a-b-c-…-n-…-end這條路徑設(shè)為最優(yōu).并且此時(shí)的節(jié)點(diǎn)為n,在此節(jié)點(diǎn)之前的所有點(diǎn),都已經(jīng)被記錄在CLOSED表中,此時(shí)的節(jié)點(diǎn)n在OPEN表里.又因?yàn)檫@條路徑是最短的,所以我們可以得到現(xiàn)在OPEN表中n的關(guān)于g(n)的值即我們所要的最小值.

通過(guò)上述的性質(zhì),可以得到,通過(guò)A*算法搜索到目標(biāo)點(diǎn)時(shí),就會(huì)有f(goal)=g(goal)<=C成立.

3)在搜索未知節(jié)點(diǎn)[10]的時(shí)候會(huì)出現(xiàn)多次搜索某一點(diǎn)的問(wèn)題,如果正在搜索的點(diǎn)已經(jīng)包含在CLOSED表中了,并且該點(diǎn)的f取值比CLOSED表里的小,則需要及時(shí)更新該點(diǎn)的f值.假設(shè)h代表的函數(shù)能滿足相容的性質(zhì),這一步就可以省掉了.

圖1為A*算法流程圖.

圖1 A*算法流程圖Fig.1 A star algorithm flow chart

2.2 D*算法

D*是根據(jù)動(dòng)態(tài)A*思想由卡內(nèi)及梅隆機(jī)器人中心的Stentz在20世紀(jì)末刊登的文章中提出來(lái)的,D*算法主要用于解決機(jī)器人路徑搜索[6]的問(wèn)題.在國(guó)內(nèi)外的在火星探測(cè)器中大多數(shù)都是采用了D*尋路算法.

1)用Dijstra算法[8]從目標(biāo)點(diǎn)E向起始點(diǎn)S進(jìn)行搜索.記錄環(huán)境地圖中目標(biāo)點(diǎn)到各個(gè)點(diǎn)的最短距離以及該位置到目標(biāo)點(diǎn)的實(shí)際值距離.任意節(jié)點(diǎn)都記錄了該節(jié)點(diǎn)前一個(gè)結(jié)點(diǎn)距離目標(biāo)點(diǎn)距離的信息,比如存在某一路徑1(3),3(6),6(5),7(9).則1到7的最短路為1-3-6-7.各個(gè)節(jié)點(diǎn)從起始點(diǎn)到目標(biāo)點(diǎn)的信息都已存入OPEN表和CLOSED表.

2)機(jī)器人在環(huán)境中以自己規(guī)劃的最優(yōu)路徑運(yùn)動(dòng),當(dāng)運(yùn)動(dòng)到下一節(jié)點(diǎn)時(shí)最優(yōu)路徑?jīng)]有發(fā)生變化,利用記錄的上一次最優(yōu)路徑信息從起始點(diǎn)向后進(jìn)行搜索.比如機(jī)器人在Y點(diǎn),當(dāng)它掃描到Y(jié)點(diǎn)的下一節(jié)點(diǎn)X點(diǎn)的狀態(tài)發(fā)生改變,機(jī)器人會(huì)即刻調(diào)整它所在的位置到目標(biāo)點(diǎn)的距離值h(Y),h(Y)是機(jī)器人從節(jié)點(diǎn)X運(yùn)動(dòng)到節(jié)點(diǎn)Y新的權(quán)值c(X,Y)與機(jī)器人在節(jié)點(diǎn)X的原來(lái)的實(shí)際值h(X)之和是機(jī)器人將要到達(dá)的下一節(jié)點(diǎn)(到目標(biāo)點(diǎn)方向Y點(diǎn)到X點(diǎn)再到G點(diǎn)),節(jié)點(diǎn)Y是機(jī)器人所在的當(dāng)前點(diǎn).

2.3 路徑規(guī)劃算法仿真

搜索算法的方式主要有兩種,主要分為:盲目搜索和啟發(fā)式搜索,這兩種方式的一個(gè)主要共同點(diǎn):從解空間中尋找出一條從起始點(diǎn)到目標(biāo)節(jié)點(diǎn)的最優(yōu)路徑.路徑規(guī)劃就是機(jī)器人的最優(yōu)路徑規(guī)劃問(wèn)題,即依據(jù)某些優(yōu)化條件(如工作代價(jià)最小、行走路徑最短、行走時(shí)間最短等),在復(fù)雜環(huán)境中找到一個(gè)從起始點(diǎn)到目標(biāo)點(diǎn)能避開(kāi)障礙物的最優(yōu)路徑.所以,應(yīng)注意以下三點(diǎn):

1)明確起始位置及終點(diǎn);

2)避開(kāi)障礙物;

3)盡可能做到路徑上的優(yōu)化.

針對(duì)路徑規(guī)劃算法仿真主要是分三步進(jìn)行:靜態(tài)路徑規(guī)劃仿真和基于靜態(tài)的動(dòng)態(tài)路徑規(guī)劃仿真以及靜態(tài)和動(dòng)態(tài)結(jié)合仿真.

2.4 靜態(tài)路徑規(guī)劃仿真

靜態(tài)路徑規(guī)劃仿真模擬機(jī)器人對(duì)環(huán)境已知,起點(diǎn)、目標(biāo)點(diǎn)、障礙點(diǎn)全部已知的情況,需要在算法中實(shí)現(xiàn)規(guī)劃出模擬機(jī)器人從起點(diǎn)到目標(biāo)點(diǎn)尋找出最優(yōu)的路徑.

2.4.1 構(gòu)建仿真圖

模擬環(huán)境是在電腦上能模仿現(xiàn)實(shí)環(huán)境,設(shè)計(jì)者將實(shí)際環(huán)境抽象成虛擬環(huán)境,并將虛擬環(huán)境用算法語(yǔ)言以形象的圖表或者圖像等的方式展現(xiàn)出來(lái).根據(jù)理論依據(jù),我們所要模擬的機(jī)器人活動(dòng)的環(huán)境應(yīng)該是有起點(diǎn)、有終點(diǎn)、還存在固定的障礙物,通過(guò)數(shù)組對(duì)環(huán)境進(jìn)行模擬在數(shù)組中要對(duì)模擬環(huán)境的各個(gè)點(diǎn)進(jìn)行標(biāo)注,并附上相應(yīng)的含義.如圖2所示.

2.4.2 模擬仿真路徑規(guī)劃

算法描述:從目標(biāo)點(diǎn)開(kāi)始從后向前查找,每次判斷其當(dāng)前點(diǎn)四鄰域中g(shù)值(表示該點(diǎn)距離起始點(diǎn)的距離)最接近當(dāng)前g值的點(diǎn),然后設(shè)置該點(diǎn)為當(dāng)前點(diǎn),并設(shè)置該點(diǎn)的p值(以此判斷是否為最優(yōu)路徑中的點(diǎn))為0,以此例推,直至找到起始點(diǎn).經(jīng)過(guò)優(yōu)化得到相對(duì)復(fù)雜的尋路圖.

2.4.3 路徑規(guī)劃算法對(duì)比

模擬Dijstra算法:在構(gòu)造抽象靜態(tài)地圖的時(shí)候模擬Dijstra的方法,對(duì)構(gòu)造的空間進(jìn)行多次遍歷,判斷每個(gè)點(diǎn)以及四鄰域點(diǎn)的type值然后對(duì)該點(diǎn)進(jìn)行相應(yīng)的處理,將所有的點(diǎn)遍歷到之后即為結(jié)束.

圖2 尋路仿真Fig.2 Pathfinding simulation

Plan1的流程圖簡(jiǎn)單易懂運(yùn)行起來(lái)現(xiàn)象也比較清晰,通過(guò)圖3可以看出該算法的可行性.

圖3 復(fù)雜環(huán)境規(guī)劃圖Fig.3 Complex environmental planning map

模擬A*算法:模擬Dijstra法里,不論起點(diǎn)與終點(diǎn)距離多遠(yuǎn),算法總會(huì)對(duì)整個(gè)地圖進(jìn)行遍歷,這樣既浪費(fèi)資源又浪費(fèi)時(shí)間.所以在Plan3算法中將會(huì)實(shí)現(xiàn)遍歷的簡(jiǎn)約,只遍歷到目標(biāo)點(diǎn),碰到目標(biāo)點(diǎn)就會(huì)停止遍歷,大大的提高了效率和容錯(cuò)率.

模擬A*算法流程圖如圖4.

從圖5可以看出,運(yùn)用模擬A*算法模擬環(huán)境越復(fù)雜所顯現(xiàn)的效果越明顯,同樣是從起始點(diǎn)到達(dá)目標(biāo)點(diǎn),復(fù)雜圖像中掃描過(guò)的區(qū)域與未掃描過(guò)的區(qū)域區(qū)分的較為明顯,而在簡(jiǎn)單圖像中則不然.

3 動(dòng)態(tài)路徑規(guī)劃仿真

動(dòng)態(tài)的路徑規(guī)劃在很多領(lǐng)域都被廣泛應(yīng)用.凡是可簡(jiǎn)化為類(lèi)似D*或動(dòng)態(tài)A*的規(guī)劃問(wèn)題基本上都可以采用動(dòng)態(tài)路徑規(guī)劃的方法來(lái)解決.在動(dòng)態(tài)路徑規(guī)劃中將使用模擬A*算法這種最簡(jiǎn)約最便捷的方法進(jìn)行討論.

3.1 人機(jī)交互

在對(duì)機(jī)器人的操縱中我們通常是通過(guò)一個(gè)界面輸入,而后根據(jù)該輸入實(shí)現(xiàn)對(duì)機(jī)器人的控制,所以輸入控制和機(jī)器人在并不在同一個(gè)平臺(tái)中,因此引入了人機(jī)交互的概念.通過(guò)人機(jī)交互實(shí)現(xiàn)實(shí)時(shí)更新.下面將依次介紹相關(guān)功能.

圖4 模擬A*算法流程圖Fig.4 Simulated A star algorithm flow chart

a)設(shè)置人機(jī)交互式對(duì)話框

圖5 簡(jiǎn)單與復(fù)雜環(huán)境仿真對(duì)比Fig.5 Simulation comparison between simple and complex environment

本課題中人機(jī)交互對(duì)話框是對(duì)動(dòng)態(tài)障礙物進(jìn)行初始化和啟動(dòng)的入口,主要思路是通過(guò)對(duì)話框與動(dòng)態(tài)地圖聯(lián)系起來(lái)然后在對(duì)話框里設(shè)置需要在動(dòng)態(tài)地圖中需要實(shí)現(xiàn)的功能與現(xiàn)象.

b)動(dòng)態(tài)障礙的控制

在對(duì)話框中設(shè)置好了坐標(biāo)數(shù)值之后,在算法程序中設(shè)置各個(gè)障礙點(diǎn)的縱坐標(biāo)為:

D_y1=D_x1+1

(2)

D_y2=D_x2+2

(3)

D_y3=D_x3+3

(4)

D_y4=D_x4+2

(5)

D_y5=D_x5+3

(6)

3.2 動(dòng)態(tài)仿真

動(dòng)態(tài)路徑規(guī)劃是靜態(tài)路徑規(guī)劃的延伸.在仿真中加入既能體現(xiàn)靜態(tài)路徑規(guī)劃的,也能體現(xiàn)動(dòng)態(tài)路徑規(guī)劃的功能;擦除、刷新、設(shè)置動(dòng)態(tài)目標(biāo)與啟動(dòng)動(dòng)態(tài)目標(biāo).圖6為三個(gè)不同時(shí)刻t1、t2、t3動(dòng)態(tài)障礙點(diǎn)、掃描區(qū)域、最優(yōu)路徑的更新情況.

圖6 動(dòng)態(tài)規(guī)劃不同時(shí)刻規(guī)劃圖Fig.6 Dynamic planning of road maps at different times

4 綜合試驗(yàn)

本實(shí)驗(yàn)基于SLAM激光定位機(jī)器人利用模擬A*動(dòng)態(tài)規(guī)劃算法進(jìn)行避碰路徑規(guī)劃.要求在活動(dòng)范圍里中規(guī)劃出一條最優(yōu)的避碰路徑,使機(jī)器人到達(dá)目標(biāo)點(diǎn),同時(shí)基于建立的環(huán)境地圖,實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)定位,由已有的定位功能直觀地反映出算法的可行與否.

4.1 模擬仿真平臺(tái)

為驗(yàn)證基于模擬A*算法與SLAM激光定位系統(tǒng)在移動(dòng)機(jī)器人路徑規(guī)劃中的正確性以及有效性,利用VC++6.0軟件平臺(tái)基于MFC開(kāi)發(fā)了仿真算法.本文中搜索的是四鄰域[11],在仿真環(huán)境中設(shè)計(jì)30*40的仿真地圖,可以在地圖中任意的加入起始點(diǎn)、目標(biāo)點(diǎn)和障礙點(diǎn).

圖7 基于實(shí)際仿真環(huán)境與路徑規(guī)劃圖Fig.7 Path planning map based on actual simulation environment

其中圖7為靜態(tài)圖上顯示的界面,其上設(shè)置好了起始點(diǎn)、目標(biāo)點(diǎn)、障礙物,以及通過(guò)算法仿真自動(dòng)規(guī)劃出的一條最優(yōu)路徑,灰色部分為掃描過(guò)的區(qū)域,白色的部分為未掃描區(qū)域.

4.2 機(jī)器人路徑規(guī)劃實(shí)物驗(yàn)證

本節(jié)要介紹的是驗(yàn)證模擬A*算法應(yīng)用到實(shí)物平臺(tái)上在室內(nèi)環(huán)境下的應(yīng)用情況.圖8所表示的是在實(shí)物演示平臺(tái)上對(duì)機(jī)器人實(shí)時(shí)跟蹤所走過(guò)的路徑記錄,其中左上角為起始點(diǎn),右下角為目標(biāo)點(diǎn),兩側(cè)為墻壁,根據(jù)機(jī)器人平臺(tái)所記錄下的實(shí)際路徑,和算法所規(guī)劃出的路徑基本相似;其中圖8下半部分是實(shí)物機(jī)器人平臺(tái)驗(yàn)證算法實(shí)驗(yàn)的過(guò)程,通過(guò)觀察該過(guò)程機(jī)器人平臺(tái)的路徑選擇與自己設(shè)計(jì)的算法基本一致,即模擬A*算法在實(shí)物平臺(tái)上的應(yīng)用實(shí)驗(yàn)驗(yàn)證成功.

圖8 機(jī)器人平臺(tái)實(shí)驗(yàn)Fig.8 Robot platform experiment

表1 尋路仿真平均時(shí)間對(duì)比Table 1 Pathfinding simulation average time comparison

通過(guò)100次模擬仿實(shí)驗(yàn)結(jié)果得出,在同樣的地圖中模擬Dijstra算法在尋找最優(yōu)路徑用時(shí)平均在5秒左右,而本文模擬A*算法尋找最優(yōu)路徑用時(shí)平均僅有1.8秒左右;此外本文利用SLAM平臺(tái)共進(jìn)行綜合實(shí)驗(yàn)20次,其中有19次實(shí)驗(yàn)結(jié)果的優(yōu)化路徑與仿真結(jié)果基本吻合,只有一次由于人員走動(dòng)影響了雷達(dá)掃描結(jié)果,與仿真結(jié)果有較大出入.

表2 平臺(tái)實(shí)驗(yàn)結(jié)果Table 2 Platform experiment results

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

本文利用VC++6.0軟件平臺(tái)基于MFC開(kāi)發(fā)了仿真算法,在載有SLAM激光掃描雷達(dá)定位的機(jī)器人平臺(tái)進(jìn)行驗(yàn)證算法的可行性和有效性.在試驗(yàn)過(guò)程中,選在了室內(nèi),避免不必要的誤差和干擾,其次選擇了比較規(guī)則的障礙物.在進(jìn)行了多次的試驗(yàn)之后,如圖5所示的靜態(tài)仿真實(shí)驗(yàn)、圖6所示的動(dòng)態(tài)仿真實(shí)驗(yàn)以及圖8 所示的平臺(tái)實(shí)物實(shí)驗(yàn)對(duì)模擬A*進(jìn)行驗(yàn)證得出了表1、表2的結(jié)果,彰顯出該算法具有A*算法的估價(jià)值大于實(shí)際值,搜索的點(diǎn)數(shù)少,搜索范圍小,效率高的特點(diǎn),避免了D*算法不能保證得到最優(yōu)解的缺點(diǎn),當(dāng)然該算法不太適用路程太長(zhǎng)的路徑規(guī)劃,但是對(duì)大部分的路徑規(guī)劃能保持良好的魯棒性.

5 總 結(jié)

本文主要研究基于激光雷達(dá)掃描機(jī)器人的靜態(tài)及動(dòng)態(tài)下避碰路徑規(guī)劃算法,在激光雷達(dá)掃描定位與創(chuàng)建地圖功能完善的前提下,學(xué)習(xí)A*算法、人工勢(shì)場(chǎng)法、D*算法的基礎(chǔ)知識(shí),并對(duì)A*算法進(jìn)行改進(jìn),實(shí)現(xiàn)先開(kāi)發(fā)模擬仿真軟件驗(yàn)證算法的可行性,然后學(xué)習(xí)開(kāi)發(fā)軟件與現(xiàn)有平臺(tái)的接口通信功能.根據(jù)以上實(shí)驗(yàn)結(jié)果可知模擬A*算法在控制機(jī)器人完成路徑規(guī)劃具有一定可行性.