薛英花,田國會(huì),吳 皓,吉艷青
(1.山東大學(xué)控制科學(xué)與工程學(xué)院,山東 濟(jì)南 250061;2.山東財(cái)政學(xué)院計(jì)算機(jī)信息工程學(xué)院,山東 濟(jì)南 250014)
1996年日本東京大學(xué)的Hashimoto實(shí)驗(yàn)室提出了“智能空間”的概念.此后,智能空間作為一種新的技術(shù)手段,在國內(nèi)外都得到了廣泛研究[1-4].將智能空間技術(shù)應(yīng)用于服務(wù)機(jī)器人系統(tǒng),可以有效地解決許多機(jī)器人靠自身無法解決或難于解決的問題,使得服務(wù)機(jī)器人應(yīng)用變得輕松可行[5].
路徑規(guī)劃是服務(wù)機(jī)器人順利完成智能空間中各種服務(wù)(如物品抓取、目標(biāo)跟蹤)的基本環(huán)節(jié)之一,定義為按照某一性能指標(biāo)搜索一條從起始狀態(tài)到目標(biāo)狀態(tài)的最優(yōu)或近似最優(yōu)的無碰路徑.路徑規(guī)劃可分為靜態(tài)路徑規(guī)劃和動(dòng)態(tài)路徑規(guī)劃2類[6-8].全局規(guī)劃需要環(huán)境中的全部先驗(yàn)信息,而局部規(guī)劃則強(qiáng)調(diào)避碰行為,雖實(shí)時(shí)性好,但是由于缺乏規(guī)劃,有時(shí)會(huì)產(chǎn)生局部極值點(diǎn)或振蕩,無法保證機(jī)器人順利地到達(dá)目標(biāo)點(diǎn).智能空間的環(huán)境是部分未知的,一方面,智能空間中的整體布局已知,如沙發(fā)、茶幾、電視機(jī)柜的擺放等;另一方面,環(huán)境中存在著不可預(yù)知的動(dòng)態(tài)障礙物,如人、另外的機(jī)器人等.單獨(dú)使用全局規(guī)劃和局部規(guī)劃都不能滿足智能空間中路徑規(guī)劃的安全性、實(shí)時(shí)性和高效性要求.
本文首先分析了柵格地圖的不足,建立了環(huán)境信息更加豐富的危險(xiǎn)度地圖;然后采用了分層的路徑規(guī)劃方法,既有效利用了已知環(huán)境信息,又實(shí)現(xiàn)了實(shí)時(shí)避障.本文充分利用了智能空間的多傳感器和通信網(wǎng)絡(luò),從多角度獲取動(dòng)態(tài)障礙物的信息,使服務(wù)機(jī)器人可以對(duì)環(huán)境做出快速反映,安全及時(shí)地完成任務(wù).
柵格法是一種常用的環(huán)境建模方法,由于其表示直觀、實(shí)現(xiàn)簡單而得到了廣泛應(yīng)用[9].圖1為根據(jù)環(huán)境中障礙物的疏密建立的柵格地圖.
圖1 柵格地圖Fig.1 Grids map
柵格地圖只能表示某處障礙物的有無,不能提供更為詳細(xì)的其他環(huán)境信息,如該柵格距離障礙物的遠(yuǎn)近等.為了能提供更充分的環(huán)境信息,采用危險(xiǎn)度地圖來表示環(huán)境[10].危險(xiǎn)度地圖是在柵格地圖的基礎(chǔ)上,充分考慮周圍障礙物與柵格的距離和障礙物的疏密程度而建立的能充分體現(xiàn)該處危險(xiǎn)程度的地圖.
設(shè)柵格地圖的行數(shù)為r,列數(shù)為c.柵格的取值用 gij(1≤i≤r,1≤j≤c)表示,危險(xiǎn)度用 dij表示.若gij=0,說明該柵格本身就是障礙物,是機(jī)器人不可通過區(qū)域,因此將其危險(xiǎn)度置為最大;若gij=1,說明該柵格為自由柵格,這時(shí)應(yīng)根據(jù)其周圍障礙物的多少和距離遠(yuǎn)近來計(jì)算該柵格的危險(xiǎn)度,如式(1)所示:
式中:s為窗口尺寸因子,實(shí)際窗口大小為(2s+1)×(2s+1);Rsign(m,n)為點(diǎn)(i+m,j+n)處的障礙物標(biāo)志,當(dāng)g(i+m,j+n)=0,即該點(diǎn)為障礙物時(shí),Rsign(m,n)=1,否則取0.
由圖2可知,危險(xiǎn)度地圖包含了比柵格地圖更豐富的環(huán)境信息,除了能表示障礙物的有無外,還能表示任意位置的危險(xiǎn)程度,為機(jī)器人路徑規(guī)劃提供了更有效的環(huán)境表示.
圖2 危險(xiǎn)度地圖Fig.2 Danger dergree map
粒子群優(yōu)化算法(particle swarm optimization,PSO)是一種進(jìn)化計(jì)算技術(shù),具有個(gè)體數(shù)目少、計(jì)算簡單、魯棒性好等優(yōu)點(diǎn).PSO初始化為一群隨機(jī)粒子,然后通過迭代尋找最優(yōu)解[11].粒子根據(jù)下面的公式來完成自己的速度和位置的進(jìn)化:
式中:Vi(t)是粒子i在第t代的速度,Xi(t)是粒子i在第t代的位置,粒子的速度V和位置X均為m維向量,pbesti為粒子本身個(gè)體極值,gbest為全局極值,r是介于(0,1)之間的隨機(jī)數(shù),c1和c2是學(xué)習(xí)因子,ω為慣性因子.
圖3 路徑規(guī)劃建模Fig.3 Model of path planning
定義n個(gè)m維粒子,粒子每一維位置分量xij(1≤i≤n,1≤j≤m)分別對(duì)應(yīng)圖3中垂直于SG的直線yj,則一個(gè)粒子就對(duì)應(yīng)一條路徑P.
在圖3中,定義起始點(diǎn) S為 p0,目標(biāo)點(diǎn) G為pm+1,則路徑P的長度和危險(xiǎn)度可分別用式(2)、(3)表示:
式中:lpjpj+1為點(diǎn)pj與點(diǎn)pj+1間的距離;dpj為路徑點(diǎn)pj的危險(xiǎn)度;dS為起始點(diǎn)S的危險(xiǎn)度;dG為目標(biāo)點(diǎn)G的危險(xiǎn)度;Dp越大,表示該路徑的危險(xiǎn)度越高.
取路徑長度和路徑危險(xiǎn)度的加權(quán)和作為粒子的適應(yīng)度函數(shù),則第i個(gè)粒子的適應(yīng)度函數(shù)Fi表示為
式中:wl和wd是LPi和DPi的加權(quán)因子,為大于等于零的實(shí)數(shù),lij,i(j+1)表示粒子i第j維和第j+1維間的長度,dij表示粒子i第j維的危險(xiǎn)度.該適應(yīng)度函數(shù)可充分反映路徑的長度和危險(xiǎn)程度,使粒子在迭代過程中能自動(dòng)避開危險(xiǎn)度高的區(qū)域,選擇一條既安全又長度較短的路徑.
智能空間為機(jī)器人動(dòng)態(tài)路徑規(guī)劃提供了豐富的信息.由于機(jī)器人本身攜帶超聲等距離傳感器,因此能夠檢測出進(jìn)入其探測區(qū)域的障礙物,如圖4中的沙發(fā)和另一機(jī)器人(或人).結(jié)合已知的先驗(yàn)地圖(即全局地圖),可知沙發(fā)為固定障礙物,不做任何處理,而另一障礙物在先驗(yàn)地圖中并不存在,故為動(dòng)態(tài)障礙物.另外,文中假定障礙物為圓形.由于智能空間中的動(dòng)態(tài)障礙物一般為人或其他機(jī)器人,所以該假設(shè)不失一般性.單獨(dú)依靠超聲只能檢測機(jī)器人與障礙物的距離,無法得到障礙物的大小等信息;利用智能空間中的頂棚攝像頭可識(shí)別出動(dòng)態(tài)障礙物,再作一個(gè)包含動(dòng)態(tài)障礙物的最小外切圓作為動(dòng)態(tài)障礙物在二維地圖中的覆蓋范圍,并把相關(guān)數(shù)據(jù)通過智能空間網(wǎng)絡(luò)系統(tǒng)傳遞給機(jī)器人,這樣機(jī)器人就可獲得比較完備的動(dòng)態(tài)障礙物信息.
圖4 智能空間中的動(dòng)態(tài)障礙物檢測Fig.4 Dynamic obstacle detection in intelligent space
智能空間中的動(dòng)態(tài)路徑規(guī)劃包含3個(gè)基本行為,即跟蹤靜態(tài)路徑的行為、避障的行為和目標(biāo)制導(dǎo)的行為[12].其中,避障的行為采用基于動(dòng)態(tài)危險(xiǎn)度地圖的加權(quán)A*算法實(shí)現(xiàn),該方法實(shí)現(xiàn)簡單、實(shí)時(shí)性強(qiáng).
若在智能空間中未發(fā)現(xiàn)動(dòng)態(tài)障礙物,由改進(jìn)的粒子群算法規(guī)劃出的靜態(tài)路徑就是一條最優(yōu)路徑,機(jī)器人只需跟蹤這條靜態(tài)路徑即可.目前路徑跟蹤的方法主要是根據(jù)靜態(tài)路徑劃分出一些局部目標(biāo)點(diǎn),使機(jī)器人能夠沿著已經(jīng)規(guī)劃好的路徑前進(jìn).因此,根據(jù)上一節(jié)改進(jìn)的粒子群優(yōu)化算法得到機(jī)器人靜態(tài)路徑,將各路徑點(diǎn)也就是各粒子的最優(yōu)位置作為路徑跟蹤的局部目標(biāo)點(diǎn).該方法簡單易行,能迅速得到局部目標(biāo)點(diǎn),很好地滿足了自主機(jī)器人導(dǎo)航的實(shí)時(shí)性要求.
設(shè)定安全距離ρs,當(dāng)機(jī)器人檢測到有動(dòng)態(tài)障礙物進(jìn)入其探測區(qū)域時(shí),計(jì)算機(jī)器人與動(dòng)態(tài)障礙物之間的距離ρ.當(dāng)ρ<ρs時(shí)表示障礙物已進(jìn)入機(jī)器人周圍的危險(xiǎn)區(qū)域,轉(zhuǎn)入避障行為,否則繼續(xù)跟蹤靜態(tài)路徑.
如前所述,智能空間可獲得動(dòng)態(tài)障礙物的位置、大小等信息,將這些信息添加到已有的靜態(tài)地圖中去,就可生成動(dòng)態(tài)地圖,再按照第1節(jié)所述的方法,建立包含動(dòng)態(tài)障礙物的危險(xiǎn)度地圖,即動(dòng)態(tài)危險(xiǎn)度地圖,如圖5所示.由于動(dòng)態(tài)障礙物的位置可能會(huì)隨時(shí)發(fā)生變化,因此智能空間和機(jī)器人必須實(shí)時(shí)監(jiān)控動(dòng)態(tài)障礙物的狀態(tài),定時(shí)刷新動(dòng)態(tài)危險(xiǎn)度地圖,以保持對(duì)動(dòng)態(tài)障礙物的跟蹤.
圖5 動(dòng)態(tài)危險(xiǎn)度地圖Fig.5 Dynamic danger degree map
在動(dòng)態(tài)危險(xiǎn)度地圖的基礎(chǔ)上,采用改進(jìn)的加權(quán)A*算法進(jìn)行避障.取評(píng)價(jià)函數(shù)為
即用實(shí)際耗散g(n)與啟發(fā)函數(shù)h(n)的加權(quán)和作為評(píng)價(jià)函數(shù).式中:權(quán)值wg和wh在搜索過程中可動(dòng)態(tài)調(diào)整,n(n=1,2,…,8)是機(jī)器人周圍8個(gè)柵格節(jié)點(diǎn)中的某一個(gè),g(n)是從當(dāng)前節(jié)點(diǎn)(即機(jī)器人的當(dāng)前位置)到節(jié)點(diǎn)n的實(shí)際代價(jià),h(n)是從節(jié)點(diǎn)n到路徑終點(diǎn)的估計(jì)代價(jià),h(n)體現(xiàn)了搜索的啟發(fā)信息[13].
為了在避障的同時(shí)能自主選擇危險(xiǎn)度低的路徑,在g(n)中加入了表示路徑節(jié)點(diǎn)危險(xiǎn)度的信息,即取機(jī)器人當(dāng)前節(jié)點(diǎn)到節(jié)點(diǎn)n的路徑長度ln和路徑危險(xiǎn)度dn的加權(quán)和作為 g(n),即g(n)=wlln+wdwn.圖6為機(jī)器人周圍的局部動(dòng)態(tài)危險(xiǎn)度地圖,機(jī)器人左上方節(jié)點(diǎn)的危險(xiǎn)度為255,即該處有障礙物,由于此處的危險(xiǎn)度遠(yuǎn)遠(yuǎn)高出其他位置,故此節(jié)點(diǎn)不會(huì)被選中.無障礙物區(qū)域的危險(xiǎn)度為0~1(已進(jìn)行歸一化).
圖6 局部動(dòng)態(tài)危險(xiǎn)度地圖Fig.6 Local dynamic danger degree map
取h(n)為n節(jié)點(diǎn)到路徑終點(diǎn)G的歐氏距離,即
式中:(xn,yn)是柵格 n的中心點(diǎn)坐標(biāo),(xG,yG)是路徑終點(diǎn)G的坐標(biāo).
這樣,總的評(píng)價(jià)函數(shù)為
由式(4)可知,當(dāng)一個(gè)節(jié)點(diǎn)的危險(xiǎn)度越小,與機(jī)器人當(dāng)前位置越近,與終點(diǎn)距離越短,那么整個(gè)的啟發(fā)函數(shù)就越小,此節(jié)點(diǎn)就更容易選中.這樣就可以保證在進(jìn)行下一個(gè)節(jié)點(diǎn)選取的時(shí)候,選擇一個(gè)相對(duì)于其他節(jié)點(diǎn)既安全又路徑較短的柵格節(jié)點(diǎn).
機(jī)器人避障時(shí),通常會(huì)偏離原始靜態(tài)路徑,目標(biāo)制導(dǎo)行為是為了使機(jī)器人能夠以最小的代價(jià)到達(dá)目的地.一般來說,當(dāng)機(jī)器人偏離靜態(tài)路徑不遠(yuǎn)時(shí),可以引導(dǎo)機(jī)器人尋回原來靜態(tài)路徑,從而繼續(xù)跟蹤靜態(tài)路徑;當(dāng)機(jī)器人已經(jīng)遠(yuǎn)遠(yuǎn)偏離靜態(tài)路徑時(shí),繼續(xù)跟蹤靜態(tài)路徑可能比重新規(guī)劃的路徑所需代價(jià)更高,這時(shí)機(jī)器人應(yīng)當(dāng)重新規(guī)劃路徑,以便迅速到達(dá)目的地.
綜合考慮了機(jī)器人可能遇到的各種情況,制定的目標(biāo)制導(dǎo)策略如下:
1)計(jì)算剩余路徑的長度lleave,若lleave小于設(shè)定的閾值l0,則重新規(guī)劃后續(xù)路徑,否則轉(zhuǎn)2);
2)判斷當(dāng)前路徑點(diǎn)與靜態(tài)路徑點(diǎn)的偏離程度ldeparture,若ldeparture小于閾值l1,則繼續(xù)跟蹤靜態(tài)路徑,否則轉(zhuǎn)3);
3)判斷當(dāng)前路徑點(diǎn)與靜態(tài)路徑點(diǎn)間是否有障礙物,若有則重新規(guī)劃后續(xù)路徑,否則轉(zhuǎn)4);
4)判斷原靜態(tài)路徑是否逐漸趨近于當(dāng)前路徑點(diǎn)(即偏離程度會(huì)越來越小),若是,則繼續(xù)跟蹤靜態(tài)路徑,否則重新規(guī)劃后續(xù)路徑.
尋回靜態(tài)路徑和重新規(guī)劃后續(xù)路徑仍采用改進(jìn)的PSO算法,因?yàn)樾枰?guī)劃的路徑相對(duì)較短,故粒子數(shù)和粒子維數(shù)都很少,所需時(shí)間也大大縮短.
硬件環(huán)境為 Intel(R)Core(TM)2 E4700@2.60 GHz、RAM 4.0 GB,軟件環(huán)境為 Microsoft Windows Vista Home Premium、Matlab 7.4.0(R2007a).參數(shù)設(shè)置為:n=30,m=19,wl=1.0,wd=0.3.
圖7為靜態(tài)路徑規(guī)劃仿真結(jié)果.其中圖7(a)為常規(guī)PSO算法的路徑規(guī)劃結(jié)果,圖7(b)為本文方法得到的路徑,經(jīng)過多次實(shí)驗(yàn)得到其性能對(duì)比如表1所示.
圖7 靜態(tài)路徑規(guī)劃仿真Fig.7 Static path planning diagram
表1 常規(guī)PSO算法與本文算法性能對(duì)比Table 1 Performance comparison of conventional PSO and modified algorithm
由表1可知,利用改進(jìn)的粒子群優(yōu)化算法得到的路徑雖然比常規(guī)PSO算法略長,但是路徑的危險(xiǎn)度卻大大降低,且本文算法耗時(shí)不到常規(guī)算法的1/2,極大提高了算法的收斂速度.由圖7(c)和(d)可知該算法對(duì)一般智能空間環(huán)境和陷阱環(huán)境亦能得到最優(yōu)路徑.
圖8為動(dòng)態(tài)路徑規(guī)劃的仿真結(jié)果,其中動(dòng)態(tài)障礙物處于t=10時(shí)的位置,即若不進(jìn)行避障,服務(wù)機(jī)器人沿靜態(tài)路徑行進(jìn)時(shí)將與動(dòng)態(tài)障礙物發(fā)生碰撞.
圖8 動(dòng)態(tài)路徑規(guī)劃仿真Fig.8 Dynamic path planning diagram
圖8(a)為機(jī)器人避障結(jié)束后選擇尋回原始靜態(tài)路徑策略時(shí)獲得的動(dòng)態(tài)路徑.機(jī)器人開始時(shí)跟蹤靜態(tài)路徑前進(jìn);t=5時(shí)機(jī)器人探測到有動(dòng)態(tài)障礙物進(jìn)入其危險(xiǎn)區(qū)域,啟動(dòng)避障行為;t=11時(shí)動(dòng)態(tài)障礙物離開機(jī)器人的危險(xiǎn)區(qū)域,避障結(jié)束,開始尋找原始路徑;t=15時(shí)找到原始靜態(tài)路徑,此后一直跟蹤原始靜態(tài)路徑直到目標(biāo)點(diǎn).表2為該奔向目標(biāo)策略下的性能.
圖8(b)為機(jī)器人避障結(jié)束后選擇重新規(guī)劃后續(xù)路徑策略獲得的動(dòng)態(tài)路徑.機(jī)器人開始時(shí)跟蹤靜態(tài)路徑;t=5時(shí)機(jī)器人轉(zhuǎn)入避障行為;t=11時(shí)避障結(jié)束,此時(shí)機(jī)器人根據(jù)目標(biāo)制導(dǎo)策略,選擇重新規(guī)劃后續(xù)路徑并沿新規(guī)劃的路徑到達(dá)目標(biāo)點(diǎn).表3為該策略下的性能.
表2 尋回靜態(tài)路徑策略Table 2 Strategy of looking for the static path after avoiding
表3 重新規(guī)劃后續(xù)路徑策略Table 3 Strategy of planning the remaining path after avoiding
由表2和表3可知,不同的奔向目標(biāo)策略都可以得到最優(yōu)路徑,且避障迅速,在避障結(jié)束后能快速靈活地進(jìn)行后續(xù)路徑規(guī)劃;另外,與靜態(tài)路徑相比,動(dòng)態(tài)路徑的長度有所增加,但是路徑的危險(xiǎn)度反而有所降低,這主要是由于本文的動(dòng)態(tài)避障算法更注重安全性.
安全性是路徑規(guī)劃中最為重要的一個(gè)問題.針對(duì)智能空間環(huán)境的特點(diǎn),提出一種層次化路徑規(guī)劃方法,克服了常規(guī)路徑規(guī)劃算法對(duì)安全性重視不夠的缺點(diǎn).本文設(shè)計(jì)的智能空間中的路徑規(guī)劃有以下4個(gè)特點(diǎn):
1)能充分利用智能空間中的豐富信息,及時(shí)快速地獲取動(dòng)態(tài)障礙物的完備資料;
2)實(shí)時(shí)性好,能迅速避開動(dòng)態(tài)障礙物;
3)避障結(jié)束后機(jī)器人能根據(jù)當(dāng)前狀況靈活快速地選擇不同的目標(biāo)制導(dǎo)策略;
4)動(dòng)態(tài)路徑規(guī)劃安全可靠且路徑長度較短.
因此,本文設(shè)計(jì)的智能空間中的動(dòng)態(tài)路徑規(guī)劃方案是完全可行的.當(dāng)然,本文的工作也有一些不足之處,如尚未考慮智能空間中有多個(gè)障礙物的情況和機(jī)器人的實(shí)際運(yùn)行速度等問題,這些問題將在今后的研究中進(jìn)一步解決.
[1]BROOKS R A.The intelligent room project[C]//Proceedings of the Second International Conference on Cognitive Technology.Fukushima,Japan,1997:271-278.
[2]JOHANSON B,WINOGRAD T,F(xiàn)OX A.Interactive workspaces[J].Computer,2003,36(4):99-101.
[3]NIITSUMA M,HASHIMOTO H.Spatial memory as an aid system for human activity in intelligent space[J].IEEE Transactions on Industrial Electronics,2007,54(2):1122-1131.
[4]SHI Yuanchun,XIE Weikai,XU Guangyou.The smart classroom:merging technologies for seamless tele-education[J].Pervasive Computing,2003,2(2):47-55.
[5]田國會(huì),李曉磊,趙守鵬,等.家庭服務(wù)機(jī)器人智能空間技術(shù)研究與進(jìn)展[J].山東大學(xué)學(xué)報(bào):工學(xué)版,2007,37(5):53-59.TIAN Guohui,LI Xiaolei,ZHAO Shoupeng,et al.Research and development of intelligent space technology for a home service robot[J].Journal of Shandong University:Engineering Science,2007,37(5):53-59.
[6]席裕庚,張純剛.一類動(dòng)態(tài)不確定環(huán)境下機(jī)器人的滾動(dòng)路徑規(guī)劃[J].自動(dòng)化學(xué)報(bào),2002,28(2):161-174.XI Yugeng,ZHANG Chungang.Rolling path planning of mobile robot in a kind of dynamic uncertain environment[J].Acta Automatica Sinica,2002,28(2):161-174.
[7]TOMPKINS P,STENTZ A,WETTERGREEN D.Missionlevel path planning and re-planning for rover exploration[J].Robotics and Autonomous Systems,2006,54(2):174-183.
[8]MORA M C,TORNERO J.Path planning and trajectory generation using multi-rate predictive artificial potential fields[C]//Proceedings of the IEEE/RSJ International Conference on IntelligentRobotsand Systems. Nice,F(xiàn)rance,2008:2990-2995.
[9]LI Jigong,F(xiàn)ENG Yiwei,GUO Ge.Real-time path planning based on certainty grids map in complex environments[C]//Proceedings of the IEEE International Conference on Integration Technology.Shenzhen,China,2007:525-529.
[10]XUE Yinghua,TIAN Guohui,HUANG Bin.Optimal robot path planning based on danger degree map[C]//Proceedings of the IEEE International Conference on Automation and Logistics.Shenyang,China,2009:1040-1045.
[11]EBERHART R C,SHI Y.Particle swarm optimization:developments,applications and resources[C]//Proceedings Congress on Evolutionary Computation.Piscataway,USA,2001:81-86.
[12]樸松昊,洪炳熔.一種動(dòng)態(tài)環(huán)境下移動(dòng)機(jī)器人的路徑規(guī)劃方法[J].機(jī)器人,2003,25(1):18-21,43.PIAO Songhao,HONG Bingrong.A path planning approach to mobile robot under dynamic environment[J].Robot,2003,25(1):18-21,43.
[13]RUSSELL S,NORVIG P.Artificial intelligence:a modern approach[M].2nd ed.Beijing:Pearson Education Asia Limited and Posts& Telecom Press,2004:76-105.