室內(nèi)機器人避碰路徑規(guī)劃

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

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

1 引 言

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

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

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

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

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

2 路徑規(guī)劃方法

2.1 A*算法

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

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

(1)

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

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

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

通過上述的性質(zhì),可以得到,通過A*算法搜索到目標點時,就會有f(goal)=g(goal)<=C成立.

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

圖1為A*算法流程圖.

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

2.2 D*算法

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

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

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

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

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

1)明確起始位置及終點;

2)避開障礙物;

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

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

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

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

2.4.1 構(gòu)建仿真圖

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

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

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

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

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

圖2 尋路仿真Fig.2 Pathfinding simulation

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

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

模擬A*算法:模擬Dijstra法里,不論起點與終點距離多遠,算法總會對整個地圖進行遍歷,這樣既浪費資源又浪費時間.所以在Plan3算法中將會實現(xiàn)遍歷的簡約,只遍歷到目標點,碰到目標點就會停止遍歷,大大的提高了效率和容錯率.

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

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

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

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

3.1 人機交互

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

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

a)設(shè)置人機交互式對話框

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

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

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

在對話框中設(shè)置好了坐標數(shù)值之后,在算法程序中設(shè)置各個障礙點的縱坐標為:

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 動態(tài)仿真

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

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

4 綜合試驗

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

4.1 模擬仿真平臺

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

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

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

4.2 機器人路徑規(guī)劃實物驗證

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

圖8 機器人平臺實驗Fig.8 Robot platform experiment

表1 尋路仿真平均時間對比Table 1 Pathfinding simulation average time comparison

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

表2 平臺實驗結(jié)果Table 2 Platform experiment results

4.3 實驗結(jié)果分析

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

5 總 結(jié)

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