室內機器人避碰路徑規劃

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

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

1 引 言

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

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

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

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

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

2 路徑規劃方法

2.1 A*算法

A*算法具有悠久的歷史,該算法在很多領域又被稱為啟發式搜索法.A*算法的函數表達式為:

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

(1)

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

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

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

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

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

圖1為A*算法流程圖.

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

2.2 D*算法

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

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

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

2.3 路徑規劃算法仿真

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

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

2)避開障礙物;

3)盡可能做到路徑上的優化.

針對路徑規劃算法仿真主要是分三步進行:靜態路徑規劃仿真和基于靜態的動態路徑規劃仿真以及靜態和動態結合仿真.

2.4 靜態路徑規劃仿真

靜態路徑規劃仿真模擬機器人對環境已知,起點、目標點、障礙點全部已知的情況,需要在算法中實現規劃出模擬機器人從起點到目標點尋找出最優的路徑.

2.4.1 構建仿真圖

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

2.4.2 模擬仿真路徑規劃

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

2.4.3 路徑規劃算法對比

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

圖2 尋路仿真Fig.2 Pathfinding simulation

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

圖3 復雜環境規劃圖Fig.3 Complex environmental planning map

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

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

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

3 動態路徑規劃仿真

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

3.1 人機交互

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

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

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

圖5 簡單與復雜環境仿真對比Fig.5 Simulation comparison between simple and complex environment

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

b)動態障礙的控制

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

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 動態仿真

動態路徑規劃是靜態路徑規劃的延伸.在仿真中加入既能體現靜態路徑規劃的,也能體現動態路徑規劃的功能;擦除、刷新、設置動態目標與啟動動態目標.圖6為三個不同時刻t1、t2、t3動態障礙點、掃描區域、最優路徑的更新情況.

圖6 動態規劃不同時刻規劃圖Fig.6 Dynamic planning of road maps at different times

4 綜合試驗

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

4.1 模擬仿真平臺

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

圖7 基于實際仿真環境與路徑規劃圖Fig.7 Path planning map based on actual simulation environment

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

4.2 機器人路徑規劃實物驗證

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

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

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

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

表2 平臺實驗結果Table 2 Platform experiment results

4.3 實驗結果分析

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

5 總 結

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