1、第 24卷第 2期 哈 爾 濱 工 程 大 學(xué) 學(xué) 報 V ol. 24 . 2 2003年 4月 Journal of Harbin Engineering University Apr. 2003基于勢場柵格法的機器人全局路徑規(guī)劃王醒策 , 張汝波 , 顧國昌(哈爾濱工程大學(xué) 計算機科學(xué)與技術(shù)學(xué)院 , 黑龍江 哈爾濱 150001摘 要 :綜合勢場法和柵格法的優(yōu)點 , 提出了一個新的全局路徑規(guī)劃方法 勢場柵格法 . 算法在避免局部最 優(yōu)點和降低計算量方面 , 有著良好的效果 ; 并且可以自動確定柵格粒度 . 最后 , 文章分析了影響算法精度的因 素 , 仿真試驗表明此算法有良好的可行性和有

2、效性 .關(guān)鍵詞 :勢場柵格法 ; 全局路徑規(guī)劃 ; 智能機器人中圖分類號 :TP24 文獻標識碼 :A 文章編號 :1006-7043(2003 02-0170-05Potential grid based global path planning for robotsWANG X ing 2ce ,ZHANG Ru 2bo ,G U G uo 2chang(School of C omputer Science and T echnology , Harbin Engineering University , Harbin 150001,China Abstract :The potenti

3、al way is combined with the grid way to form a new potential grid way ,which is very effective in reducing com putational w ork and av oiding local minima , and can automatically create grids. The factors having in 2 fluence on the accuracy of calculation are analyzed and the feasibility and the val

4、idity of the mehtod proposed is veri 2 fied through simulation.K ey w ords :potential grid method ;global path planning ;intelligent robot 全局路徑規(guī)劃技術(shù)是智能機器人領(lǐng)域中的核 心問題之一 , 也是機器人學(xué)中研究人工智能的一 個重要方面 . 解決全局路徑規(guī)劃問題的基本的方 法有 :幾何法 1、 單元分解法 2、 人工勢場法 3和 數(shù)學(xué)分析法等 . 全局路徑規(guī)劃方法的一般步驟為 :1 劃分狀態(tài)空間 ;2 形成包含狀態(tài)空間信息的搜索空間 ;3 在形成的搜索空間

5、上應(yīng)用各種搜索策略 進行搜索 .勢場的方法是由 K hatib 4最先提出的 . 他把 機械手或者是移動機器人在環(huán)境中的運動視為在 一種抽象的人造受力場中運動 :目標點對機器人 產(chǎn)生引力 , 障礙物對機器人產(chǎn)生斥力 , 最后根據(jù)合 力來確定機器人的運動 . 應(yīng)用勢場法規(guī)劃出來的 路徑一般是比較平滑并且安全 , 但是這種方法存 在局部最優(yōu)點問題 (local minima . 為了解決這個 問 題 , 許 多 的 學(xué) 者 進 行 了 研 究 :如 Rim on 5, Shahid 6和 K hosla 7等等 . 他們期望通過建立統(tǒng)一 的勢能函數(shù)來解決這一問題 . 但這就要求障礙物 最好是規(guī)則的

6、 , 否則算法的計算量將很大 , 有時甚 至是無法計算的 .柵格法是由 W. E. H owden 在 1968年提出的 . 他在進行路徑規(guī)劃時采用了柵格 (grid 表示地圖 , 在處理障礙物的邊界時 , 避免了復(fù)雜的計算 . 可以 看出 , 柵格粒度越小 , 障礙物的表示會越精確 , 但 同時會占用大量的存儲空間 , 算法的搜索范圍將 按指數(shù)增加 . 柵格的粒度太大 , 規(guī)劃的路徑會很不 精確 . 所以柵格粒度大小的確定 , 是柵格法中的主 要問題 .本文針對多機器人編隊任務(wù)的需要 , 綜合了 柵格法和勢場法的優(yōu)點 , 提出一種新的方法 , 勢場 柵格法 .一般全局路徑規(guī)劃主要是為了滿足單

7、機器人 的需要 , 這里針對多機器人的編隊任務(wù) , 考慮到路 徑安全性和可靠性要比路徑長短重要得多 , 所以 采用勢場法 . 為了降低勢場法的計算量 , 應(yīng)用柵格 法表示地圖 . 針對勢場法和柵格法的自身的缺陷 , 在整個的路徑規(guī)劃的算法中進行了相應(yīng)的改進 . 算法的特點有以下幾個 :收稿日期 :2002-05-27.作者簡介 :王醒策 (1977- , 女 , 博士研究生 . 研究方向為智能控制 , 路徑規(guī)劃 , 多機器人協(xié)調(diào) . 1 系統(tǒng)根據(jù)地圖中障礙物的疏密自主決定柵 格粒度 . 因為系統(tǒng)規(guī)劃出的路徑還要依靠勢場的作 用 , 所以即使柵格粒度較大時 , 也能規(guī)劃出比較好 的路徑 .2 由

8、于應(yīng)用柵格的方法 , 在計算整個的地圖 中的函數(shù)勢時就只要計算柵格點的時函數(shù) , 根本地 解決了傳統(tǒng)勢場法的大計算量問題 . 整個的算法中 只計算最近障礙物對機器人的排斥 , 終點對機器人 的吸引是由啟發(fā)函數(shù)來完成的 , 避免了局部最優(yōu)點 問題 .1 基于勢場柵格法的路徑規(guī)劃算法應(yīng)用勢場柵格法來規(guī)劃路徑的過程 :在平面 內(nèi) , 首先系統(tǒng)根據(jù)地圖中障礙物的疏密來決定柵格 的粒度 ; 然后求出每一個柵格中心點的勢能值 ; 經(jīng) 過 g 函數(shù)變換后 , 再利用 A 3搜索 , 綜合柵格的 g 函數(shù)結(jié)果與柵格與終點的距離 , 通過啟發(fā)函數(shù)的運 算 , 得到一條自始至終的路徑 .1. 1 柵格粒度的確定系

9、統(tǒng)會根據(jù)地圖中障礙物疏密來決定柵格粒 度的大小 . 在得到地圖之后 , 首先計算地圖中所有 障礙物面積和 . 凸多邊形以某一頂點進行三角剖 分 , 橢圓和其它的不規(guī)則圖形則按照能夠?qū)λ鼈冞M 行最小覆蓋的長方形進行計算 . 然后根據(jù)障礙物的 面積在整個地圖中所占的比例來決定地圖中的柵 格粒度 .柵格粒度大小生成方法 :1 在地圖中任選一個障礙物 .2 判斷此障礙物是否為凸多邊形 ?3 如果是 , 則以某一個頂點為起始點 , 把多邊 形劃分為多個互不相交的三角形 .4 如果不是 , 則找出這個障礙物所有頂點中的 x max , y max , x min , y min , 然 后 分 別 以 (

10、x min , y min 和 (x max , y max 為對角頂點繪制一個長方形 ; 以長方形 某一頂點為起始點 , 把長方形劃分為兩個不相交的 三角形 .5 按照公式 S =2×a ×b ×sin , 計算各三 角形的面積 . 其中 a , b 是以剖分頂點為頂點的三 角形的兩邊 , 是 a , b 所夾的角度 .6 地圖中還有沒計算的障礙物嗎 ? 如果有 , 則 轉(zhuǎn) 1 .7 計算障礙物的面積和 S ob =l S l , 是整個 障礙物集合 , l 是 中的一個元素 .8 計算柵格粒度 :計算 l temp =S 總×l max , 得到 l

11、 =l temp , if l temp >l min ;l min , 其他 .(1 式中 :l max 是定義的柵格的最大邊長 , l min 是柵格的 最小邊長 , l 是最終的柵格邊長 .9 算法結(jié)束 .圖 1給出了不同形狀障礙物的剖分結(jié)果 .圖 1 不同形狀障礙物的剖分Fig. 1 The triangulating of different obstacles1. 2 勢場的構(gòu)造傳統(tǒng)勢場法里勢場構(gòu)造是應(yīng)用引力與斥力共 同對機器人產(chǎn)生作用 . 表示為U art =U o +U g . (2 式中 :U art 表示總勢場 , U o 表示斥力場 , U g 表示引 力場 . 勢

12、場中的力表示為F art =F g +F o . (3 式中 :F g 是引力 , F o 是斥力 , F art 是合力 . 其中 F g =-grad (U g =-x i +y j +z k , (4 F o =-grad (U o =-x i +y j +z k , (5 可能會有 F art =0, 因為引力和斥力共同對機 器人作用 , 當目標點對機器人的引力等于障礙物對 其產(chǎn)生的斥力的時候 , 算法會產(chǎn)生局部極小點 , 需 要引入其它的量對機器人進行控制 .而在這個算法中 , 障礙物產(chǎn)生的斥力場構(gòu)造出 整個勢場 , 表示柵格的安全程度 . 路徑終點對障礙 物的吸引體現(xiàn)在啟發(fā)函數(shù)中路

13、徑預(yù)測長度上 . 機器 人行走的方向由啟發(fā)函數(shù)來決定 , 而非引力和斥力 來決定 . 由于沒有力的矢量計算 , 從根本上解決了 局部最優(yōu)點的問題 . 1 7 1第 2期 王醒策 , 等 :基于勢場柵格法的機器人全局路徑規(guī)劃在算法中先計算勢能值 , 應(yīng)用 g 函數(shù)對勢能 值變換構(gòu)造出勢場 . 這里勢能值定義為柵格中心點 到最近障礙物的距離 , 表示此柵格點的危險程度 . 利用掃描法計算每一個柵格勢能值 . 具體方法為 :1 地圖中選擇一個柵格 ; 2 初始化圓的半徑 r =r 0;3 以柵格中心點 n 為圓心 , 以 r 為半徑畫圓 ; 4 判斷圓與障礙物是否相交 ;5 如果相交 , 則返回柵格

14、中心點的勢能的值為D (n =r -l p ;6 如果不相交 , 則 r =r +l p , 轉(zhuǎn) 3 ;7 地圖中是否還有沒有計算的柵格 ? 若是 , 則轉(zhuǎn) 1 ;8 算法結(jié)束 .這里 r 0是圓半徑的初始值 , l p 是等勢圈的半 徑差 .由圖 2可以看到 , 在某柵格中心點 a 處 , 進行 等勢波樣擴張 , 直到等勢圈與障礙物相交 .圖 2 柵格中的勢能值的選取Fig. 2 The potential value of the grid柵格的勢能值就是柵格與離其最近的障礙物 的距離 . 這是因為對柵格的安全影響最大的是距柵 格最近的障礙物 .D (n =min p dis (n , p

15、 .(6式中 :n 是某柵格 , 是整個的障礙物集合 , p 是 集合中元素 , dis (n , p 是 n 到 p 經(jīng)過掃描法計算出 來的距離 .計算好地圖中的各個柵格節(jié)點勢能值之后 , 進 行 g 函數(shù)變換 , g 函數(shù)為g (n =100×exp (-D (n /40 .(7式中 :D (n 是勢場中柵格 n 的勢能值 . 當柵格 n 距障礙物的距離越大 , 則它的 g 函數(shù)將越小 . 函數(shù) 變換使勢能與柵格中點與路徑終點的距離在大致相等的數(shù)量級上 , 可以共同對節(jié)點的選擇起作用 . 地圖的勢場 , 表示為不同柵格節(jié)點的 g 函數(shù)值 . 總勢場為U art =U o =g (

16、n |g (n =100×exp (-D (n /40 , n (8式中 :是地圖中所有柵格中心點的集合 .圖 4是地圖 3由此算法產(chǎn)生的勢場三維圖 .圖 3 一幅地圖Fig. 3 A map圖 4 地圖 3的勢場三維圖Fig. 4 The 32D map of the figure 3 s potential field1. 3 全局路徑的柵格節(jié)點的選取全局路徑節(jié)點的選取是應(yīng)用 A 3算法進行相 鄰節(jié)點搜索 . 具體的過程是 :在圖中的 a 柵格的周 圍有 8個柵格 . 算法按照啟發(fā)函數(shù) , 在柵格 a 周圍 的 8個柵格中尋找一個啟發(fā)函數(shù)值最小的柵格 (例 如柵格 2 . 然后

17、, 把 a 標注為已走過的柵格 , 畫出 從此柵格到新尋找到的柵格 2之間的路徑 . 然后以 柵格 2為母節(jié)點 , 重復(fù)這一過程 . 如圖 5所示 .啟發(fā)函數(shù) :f (n =g (n +h (n , n =1 8.(9式中 :n 是 a 周圍的 8個柵格節(jié)點中的某一個 ,g (n 是 n 節(jié)點的 g 函數(shù) , h (n 是 n 節(jié)點到路徑終點的估計距離 , 表示為 n 節(jié)點到路徑終點的歐氏 距離 . 271 哈 爾 濱 工 程 大 學(xué) 學(xué) 報 第 24卷 圖 5 全局路徑的節(jié)點的選取Fig. 5 The node selection of global pathh (n =(x n -x en

18、d 2+(y n -y end 2. (10式中 :(x n , y n 是 n 柵格的中心點坐標 , (x end , y end 是規(guī)劃的路徑終點的坐標 .k 為被選中的下一個柵格節(jié)點 .k =min 8n =1(f (n .(11 當某一個柵格中心點與障礙物的距離越大 , 與 終點的距離越短 , 那么整個的啟發(fā)函數(shù)的值將越 小 , 此柵格將更容易被選中 . 這樣就可以保證在每次進行柵格選取的時候 , 選擇一個相對于其它的柵 格距離障礙物最遠而距離終點最近的柵格節(jié)點 . 1. 4 路徑的回溯方法 當利用柵格法來進行全局路徑規(guī)劃的時候 , 必 然會討論路徑的節(jié)點回溯問題 . 首先來介紹一下路

19、 徑的回溯問題 .如圖 6所示 , 路徑選擇圖中的 B 柵格后 , 根據(jù) 啟發(fā)函數(shù)的計算 , 可以得到下一個柵格為柵格 A , 可以看到在 A 柵格除了 B 柵格之外 , 不與任何節(jié) 點連通 , 這樣路徑會進入死胡同 . 但是即使出現(xiàn)這種情況 , 整個地圖可能依然存在著從起點到終點的 路徑 . 因此要進行回溯操作 . 在這種情況下 , 把 A 柵格打上危險標記 , 然后回溯到 B 柵格 , 重新利用 啟發(fā)函數(shù)尋找節(jié)點的計算 . 在新一次的計算中 , 根 據(jù)危險標記濾掉 A 柵格 , 以免引起上述的情況 .圖 6 路徑節(jié)點的回溯Fig. 6 The backup of the path node

20、在算法中 , 柵格需要在兩種情況下打標記 . 一 種情況是 B 柵格進入 A 柵格之后 , 為了防止路徑 的循環(huán) 、 交叉和跳躍 , 為 B 柵格打上標記 , 使得 A 柵格在下一次選擇時 , 不再考慮 B 柵格 . 另一種情 況就是在回溯時 , B 柵格進入 A 柵格之后 , 發(fā)現(xiàn) A 柵格不連通之后 , 為 A 柵格打上標記 , 并解開 B 柵 格由于第一種情況所打的標記 . 這兩種標記中 , 第一種標記是表示路徑所經(jīng)過的柵格 ; 另一種標記是表示柵格的連通性 . 在這里定義柵格的結(jié)構(gòu)類型 時 , 用兩種不同的布爾量來實現(xiàn)兩種標記的區(qū)別 .typedef struct bool m -li

21、antong ; /柵格中路徑經(jīng)過的標記 bool m -around8; /此柵格與其周 圍的柵格的連通情況 ;1. 5 算法的總結(jié)最后來總結(jié)一下整個的算法 :step 1:初始化算法 ;step 2:對地圖進行預(yù)處理 . 根據(jù)障礙物在地圖中的密度 , 生成柵格 ; step 3:應(yīng)用掃描法和 g 函數(shù)計算地圖勢場 ; step 4:設(shè)機器人的起始節(jié)點所在柵格為當前 的路徑搜索柵格 . 判斷當前柵格是否為路徑的終 點 , 是則轉(zhuǎn) step8; step 5:根據(jù)公式 (9 , 利用 A 3算法進行路徑下一節(jié)點的搜索 ; step 6:下一個柵格是否為路徑終點 ? 是則轉(zhuǎn) step8; ste

22、p 7:計算的路徑節(jié)點需要回溯 ? 若是則采用回溯算法 , 否則把搜索到的柵格當作當前柵格 , 返回 step5; step 8:算法結(jié)束 ;2 仿真試驗以及結(jié)果分析根據(jù)以上介紹的算法 , 在 P 800機器上 , 應(yīng)用 VC6. 0編制仿真試驗環(huán)境下 , 進行了相應(yīng)的仿真試驗 .經(jīng)過多次實驗得到 l max =40, l min =5, r 0=l p =5. 圖 7和 8是在不同環(huán)境中的規(guī)劃的結(jié)果 . 圖 7因為障礙物稀疏 , 從路徑的折點就可以看出柵格粒度 較大 , 而圖 8中柵格粒度較小 . 經(jīng)過多次實驗驗證 ,371 第 2期 王醒策 , 等 :基于勢場柵格法的機器人全局路徑規(guī)劃此全

23、局路徑規(guī)劃算法有很高的安全性 , 可以滿足多 機器人編隊的任務(wù)的需要 . 同時算法具有很好的擴 展性 , 對于三維地圖中的全局路徑規(guī)劃也同樣適 用 . 并且對機器人局部路徑規(guī)劃也有一定的借鑒作 用 .圖 7 在稀疏環(huán)境下的全局路徑規(guī)劃結(jié)果Fig. 7 The planning result in the map with sparse obstacles圖 8 在障礙物密集的時候的路徑規(guī)劃結(jié)果Fig. 8 The planning result in the map with dense obstacles 由于在地圖采用了柵格表示法 , 所以在經(jīng)過掃 描法計算出勢場后 , 路徑規(guī)劃的速度會很

24、快 , 算法 的實時性較強 . 整個算法的精度與柵格的粒度的大 小 l 和掃描法中每一層等勢圈之間的半徑差值 l p 有關(guān) . 在這兩方面選取恰當?shù)南禂?shù) , 系統(tǒng)就可以規(guī) 劃出較好的路徑 .3結(jié)束語勢場法由于結(jié)構(gòu)簡單 、 易于實現(xiàn) , 所以在路徑規(guī)劃中被廣泛的采用 . 但是勢場法存在著大計算空 間和局部最優(yōu)點問題 . 針對勢場法的這些缺點 , 應(yīng) 用柵格法與勢場法的結(jié)合 , 降低勢場法的計算復(fù)雜 度 ; 應(yīng)用障礙物構(gòu)造勢場 , 避免了局部最優(yōu)點的問 題 . 此算法在三維空間中的路徑規(guī)劃也同樣有效 , 是一種安全 、 可靠和高效的算法 .參考文獻 :1JANETJ A. The essentia

25、l visibility graph :an approach toglobal m otion planning for autonom ous m obile robotsA.Proc of IEEE Int C on f on R obotics and Automation C.Nag oya , Japan ,1995.2LAZANO 2PEREZ. S patial planning :a con figuration space ap 2proachJ.IEEE T ransaction on C omputers , 1983,32(2 :108-119.3Y OSHIFURMI KIT AM URA. 32D path planning in a dynamic environment using an octree and an artificial potential field A.IROS 9