智能空間中的服務機器人路徑規(guī)劃

薛英花，田國會，吳 皓，吉艷青

(1.山東大學控制科學與工程學院，山東 濟南 250061;2.山東財政學院計算機信息工程學院，山東 濟南 250014)

1996年日本東京大學的Hashimoto實驗室提出了“智能空間”的概念.此后，智能空間作為一種新的技術手段，在國內(nèi)外都得到了廣泛研究［1-4］.將智能空間技術應用于服務機器人系統(tǒng)，可以有效地解決許多機器人靠自身無法解決或難于解決的問題，使得服務機器人應用變得輕松可行［5］.

路徑規(guī)劃是服務機器人順利完成智能空間中各種服務(如物品抓取、目標跟蹤)的基本環(huán)節(jié)之一，定義為按照某一性能指標搜索一條從起始狀態(tài)到目標狀態(tài)的最優(yōu)或近似最優(yōu)的無碰路徑.路徑規(guī)劃可分為靜態(tài)路徑規(guī)劃和動態(tài)路徑規(guī)劃2類［6-8］.全局規(guī)劃需要環(huán)境中的全部先驗信息，而局部規(guī)劃則強調避碰行為，雖實時性好，但是由于缺乏規(guī)劃，有時會產(chǎn)生局部極值點或振蕩，無法保證機器人順利地到達目標點.智能空間的環(huán)境是部分未知的，一方面，智能空間中的整體布局已知，如沙發(fā)、茶幾、電視機柜的擺放等;另一方面，環(huán)境中存在著不可預知的動態(tài)障礙物，如人、另外的機器人等.單獨使用全局規(guī)劃和局部規(guī)劃都不能滿足智能空間中路徑規(guī)劃的安全性、實時性和高效性要求.

本文首先分析了柵格地圖的不足，建立了環(huán)境信息更加豐富的危險度地圖;然后采用了分層的路徑規(guī)劃方法，既有效利用了已知環(huán)境信息，又實現(xiàn)了實時避障.本文充分利用了智能空間的多傳感器和通信網(wǎng)絡，從多角度獲取動態(tài)障礙物的信息，使服務機器人可以對環(huán)境做出快速反映，安全及時地完成任務.

1 環(huán)境模型的建立

柵格法是一種常用的環(huán)境建模方法，由于其表示直觀、實現(xiàn)簡單而得到了廣泛應用［9］.圖1為根據(jù)環(huán)境中障礙物的疏密建立的柵格地圖.

圖1 柵格地圖Fig.1 Grids map

柵格地圖只能表示某處障礙物的有無，不能提供更為詳細的其他環(huán)境信息，如該柵格距離障礙物的遠近等.為了能提供更充分的環(huán)境信息，采用危險度地圖來表示環(huán)境［10］.危險度地圖是在柵格地圖的基礎上，充分考慮周圍障礙物與柵格的距離和障礙物的疏密程度而建立的能充分體現(xiàn)該處危險程度的地圖.

設柵格地圖的行數(shù)為r，列數(shù)為c.柵格的取值用 gij(1≤i≤r，1≤j≤c)表示，危險度用 dij表示.若gij=0，說明該柵格本身就是障礙物，是機器人不可通過區(qū)域，因此將其危險度置為最大;若gij=1，說明該柵格為自由柵格，這時應根據(jù)其周圍障礙物的多少和距離遠近來計算該柵格的危險度，如式(1)所示:

式中:s為窗口尺寸因子，實際窗口大小為(2s+1)×(2s+1);Rsign(m，n)為點(i+m，j+n)處的障礙物標志，當g(i+m，j+n)=0，即該點為障礙物時，Rsign(m，n)=1，否則取0.

由圖2可知，危險度地圖包含了比柵格地圖更豐富的環(huán)境信息，除了能表示障礙物的有無外，還能表示任意位置的危險程度，為機器人路徑規(guī)劃提供了更有效的環(huán)境表示.

圖2 危險度地圖Fig.2 Danger dergree map

2 改進的PSO靜態(tài)路徑規(guī)劃

粒子群優(yōu)化算法(particle swarm optimization，PSO)是一種進化計算技術，具有個體數(shù)目少、計算簡單、魯棒性好等優(yōu)點.PSO初始化為一群隨機粒子，然后通過迭代尋找最優(yōu)解［11］.粒子根據(jù)下面的公式來完成自己的速度和位置的進化:

式中:Vi(t)是粒子i在第t代的速度，Xi(t)是粒子i在第t代的位置，粒子的速度V和位置X均為m維向量，pbesti為粒子本身個體極值，gbest為全局極值，r是介于(0，1)之間的隨機數(shù)，c1和c2是學習因子，ω為慣性因子.

圖3 路徑規(guī)劃建模Fig.3 Model of path planning

定義n個m維粒子，粒子每一維位置分量xij(1≤i≤n，1≤j≤m)分別對應圖3中垂直于SG的直線yj，則一個粒子就對應一條路徑P.

在圖3中，定義起始點 S為 p0，目標點 G為pm+1，則路徑P的長度和危險度可分別用式(2)、(3)表示:

式中:lpjpj+1為點pj與點pj+1間的距離;dpj為路徑點pj的危險度;dS為起始點S的危險度;dG為目標點G的危險度;Dp越大，表示該路徑的危險度越高.

取路徑長度和路徑危險度的加權和作為粒子的適應度函數(shù)，則第i個粒子的適應度函數(shù)Fi表示為

式中:wl和wd是LPi和DPi的加權因子，為大于等于零的實數(shù)，lij，i(j+1)表示粒子i第j維和第j+1維間的長度，dij表示粒子i第j維的危險度.該適應度函數(shù)可充分反映路徑的長度和危險程度，使粒子在迭代過程中能自動避開危險度高的區(qū)域，選擇一條既安全又長度較短的路徑.

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

智能空間為機器人動態(tài)路徑規(guī)劃提供了豐富的信息.由于機器人本身攜帶超聲等距離傳感器，因此能夠檢測出進入其探測區(qū)域的障礙物，如圖4中的沙發(fā)和另一機器人(或人).結合已知的先驗地圖(即全局地圖)，可知沙發(fā)為固定障礙物，不做任何處理，而另一障礙物在先驗地圖中并不存在，故為動態(tài)障礙物.另外，文中假定障礙物為圓形.由于智能空間中的動態(tài)障礙物一般為人或其他機器人，所以該假設不失一般性.單獨依靠超聲只能檢測機器人與障礙物的距離，無法得到障礙物的大小等信息;利用智能空間中的頂棚攝像頭可識別出動態(tài)障礙物，再作一個包含動態(tài)障礙物的最小外切圓作為動態(tài)障礙物在二維地圖中的覆蓋范圍，并把相關數(shù)據(jù)通過智能空間網(wǎng)絡系統(tǒng)傳遞給機器人，這樣機器人就可獲得比較完備的動態(tài)障礙物信息.

圖4 智能空間中的動態(tài)障礙物檢測Fig.4 Dynamic obstacle detection in intelligent space

智能空間中的動態(tài)路徑規(guī)劃包含3個基本行為，即跟蹤靜態(tài)路徑的行為、避障的行為和目標制導的行為［12］.其中，避障的行為采用基于動態(tài)危險度地圖的加權A*算法實現(xiàn)，該方法實現(xiàn)簡單、實時性強.

3.1 跟蹤靜態(tài)路徑的行為

若在智能空間中未發(fā)現(xiàn)動態(tài)障礙物，由改進的粒子群算法規(guī)劃出的靜態(tài)路徑就是一條最優(yōu)路徑，機器人只需跟蹤這條靜態(tài)路徑即可.目前路徑跟蹤的方法主要是根據(jù)靜態(tài)路徑劃分出一些局部目標點，使機器人能夠沿著已經(jīng)規(guī)劃好的路徑前進.因此，根據(jù)上一節(jié)改進的粒子群優(yōu)化算法得到機器人靜態(tài)路徑，將各路徑點也就是各粒子的最優(yōu)位置作為路徑跟蹤的局部目標點.該方法簡單易行，能迅速得到局部目標點，很好地滿足了自主機器人導航的實時性要求.

3.2 避障行為

設定安全距離ρs，當機器人檢測到有動態(tài)障礙物進入其探測區(qū)域時，計算機器人與動態(tài)障礙物之間的距離ρ.當ρ＜ρs時表示障礙物已進入機器人周圍的危險區(qū)域，轉入避障行為，否則繼續(xù)跟蹤靜態(tài)路徑.

如前所述，智能空間可獲得動態(tài)障礙物的位置、大小等信息，將這些信息添加到已有的靜態(tài)地圖中去，就可生成動態(tài)地圖，再按照第1節(jié)所述的方法，建立包含動態(tài)障礙物的危險度地圖，即動態(tài)危險度地圖，如圖5所示.由于動態(tài)障礙物的位置可能會隨時發(fā)生變化，因此智能空間和機器人必須實時監(jiān)控動態(tài)障礙物的狀態(tài)，定時刷新動態(tài)危險度地圖，以保持對動態(tài)障礙物的跟蹤.

圖5 動態(tài)危險度地圖Fig.5 Dynamic danger degree map

在動態(tài)危險度地圖的基礎上，采用改進的加權A*算法進行避障.取評價函數(shù)為

即用實際耗散g(n)與啟發(fā)函數(shù)h(n)的加權和作為評價函數(shù).式中:權值wg和wh在搜索過程中可動態(tài)調整，n(n=1，2，…，8)是機器人周圍8個柵格節(jié)點中的某一個，g(n)是從當前節(jié)點(即機器人的當前位置)到節(jié)點n的實際代價，h(n)是從節(jié)點n到路徑終點的估計代價，h(n)體現(xiàn)了搜索的啟發(fā)信息［13］.

為了在避障的同時能自主選擇危險度低的路徑，在g(n)中加入了表示路徑節(jié)點危險度的信息，即取機器人當前節(jié)點到節(jié)點n的路徑長度ln和路徑危險度dn的加權和作為 g(n)，即g(n)=wlln+wdwn.圖6為機器人周圍的局部動態(tài)危險度地圖，機器人左上方節(jié)點的危險度為255，即該處有障礙物，由于此處的危險度遠遠高出其他位置，故此節(jié)點不會被選中.無障礙物區(qū)域的危險度為0～1(已進行歸一化).

圖6 局部動態(tài)危險度地圖Fig.6 Local dynamic danger degree map

取h(n)為n節(jié)點到路徑終點G的歐氏距離，即

式中:(xn，yn)是柵格 n的中心點坐標，(xG，yG)是路徑終點G的坐標.

這樣，總的評價函數(shù)為

由式(4)可知，當一個節(jié)點的危險度越小，與機器人當前位置越近，與終點距離越短，那么整個的啟發(fā)函數(shù)就越小，此節(jié)點就更容易選中.這樣就可以保證在進行下一個節(jié)點選取的時候，選擇一個相對于其他節(jié)點既安全又路徑較短的柵格節(jié)點.

3.3 目標制導行為

機器人避障時，通常會偏離原始靜態(tài)路徑，目標制導行為是為了使機器人能夠以最小的代價到達目的地.一般來說，當機器人偏離靜態(tài)路徑不遠時，可以引導機器人尋回原來靜態(tài)路徑，從而繼續(xù)跟蹤靜態(tài)路徑;當機器人已經(jīng)遠遠偏離靜態(tài)路徑時，繼續(xù)跟蹤靜態(tài)路徑可能比重新規(guī)劃的路徑所需代價更高，這時機器人應當重新規(guī)劃路徑，以便迅速到達目的地.

綜合考慮了機器人可能遇到的各種情況，制定的目標制導策略如下:

1)計算剩余路徑的長度lleave，若lleave小于設定的閾值l0，則重新規(guī)劃后續(xù)路徑，否則轉2);

2)判斷當前路徑點與靜態(tài)路徑點的偏離程度ldeparture，若ldeparture小于閾值l1，則繼續(xù)跟蹤靜態(tài)路徑，否則轉3);

3)判斷當前路徑點與靜態(tài)路徑點間是否有障礙物，若有則重新規(guī)劃后續(xù)路徑，否則轉4);

4)判斷原靜態(tài)路徑是否逐漸趨近于當前路徑點(即偏離程度會越來越小)，若是，則繼續(xù)跟蹤靜態(tài)路徑，否則重新規(guī)劃后續(xù)路徑.

尋回靜態(tài)路徑和重新規(guī)劃后續(xù)路徑仍采用改進的PSO算法，因為需要規(guī)劃的路徑相對較短，故粒子數(shù)和粒子維數(shù)都很少，所需時間也大大縮短.

4 實驗仿真結果

硬件環(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ù)設置為:n=30，m=19，wl=1.0，wd=0.3.

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

圖7為靜態(tài)路徑規(guī)劃仿真結果.其中圖7(a)為常規(guī)PSO算法的路徑規(guī)劃結果，圖7(b)為本文方法得到的路徑，經(jīng)過多次實驗得到其性能對比如表1所示.

圖7 靜態(tài)路徑規(guī)劃仿真Fig.7 Static path planning diagram

表1 常規(guī)PSO算法與本文算法性能對比Table 1 Performance comparison of conventional PSO and modified algorithm

由表1可知，利用改進的粒子群優(yōu)化算法得到的路徑雖然比常規(guī)PSO算法略長，但是路徑的危險度卻大大降低，且本文算法耗時不到常規(guī)算法的1/2，極大提高了算法的收斂速度.由圖7(c)和(d)可知該算法對一般智能空間環(huán)境和陷阱環(huán)境亦能得到最優(yōu)路徑.

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

圖8為動態(tài)路徑規(guī)劃的仿真結果，其中動態(tài)障礙物處于t=10時的位置，即若不進行避障，服務機器人沿靜態(tài)路徑行進時將與動態(tài)障礙物發(fā)生碰撞.

圖8 動態(tài)路徑規(guī)劃仿真Fig.8 Dynamic path planning diagram

圖8(a)為機器人避障結束后選擇尋回原始靜態(tài)路徑策略時獲得的動態(tài)路徑.機器人開始時跟蹤靜態(tài)路徑前進;t=5時機器人探測到有動態(tài)障礙物進入其危險區(qū)域，啟動避障行為;t=11時動態(tài)障礙物離開機器人的危險區(qū)域，避障結束，開始尋找原始路徑;t=15時找到原始靜態(tài)路徑，此后一直跟蹤原始靜態(tài)路徑直到目標點.表2為該奔向目標策略下的性能.

圖8(b)為機器人避障結束后選擇重新規(guī)劃后續(xù)路徑策略獲得的動態(tài)路徑.機器人開始時跟蹤靜態(tài)路徑;t=5時機器人轉入避障行為;t=11時避障結束，此時機器人根據(jù)目標制導策略，選擇重新規(guī)劃后續(xù)路徑并沿新規(guī)劃的路徑到達目標點.表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可知，不同的奔向目標策略都可以得到最優(yōu)路徑，且避障迅速，在避障結束后能快速靈活地進行后續(xù)路徑規(guī)劃;另外，與靜態(tài)路徑相比，動態(tài)路徑的長度有所增加，但是路徑的危險度反而有所降低，這主要是由于本文的動態(tài)避障算法更注重安全性.

5 結束語

安全性是路徑規(guī)劃中最為重要的一個問題.針對智能空間環(huán)境的特點，提出一種層次化路徑規(guī)劃方法，克服了常規(guī)路徑規(guī)劃算法對安全性重視不夠的缺點.本文設計的智能空間中的路徑規(guī)劃有以下4個特點:

1)能充分利用智能空間中的豐富信息，及時快速地獲取動態(tài)障礙物的完備資料;

2)實時性好，能迅速避開動態(tài)障礙物;

3)避障結束后機器人能根據(jù)當前狀況靈活快速地選擇不同的目標制導策略;

4)動態(tài)路徑規(guī)劃安全可靠且路徑長度較短.

因此，本文設計的智能空間中的動態(tài)路徑規(guī)劃方案是完全可行的.當然，本文的工作也有一些不足之處，如尚未考慮智能空間中有多個障礙物的情況和機器人的實際運行速度等問題，這些問題將在今后的研究中進一步解決.

［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］田國會，李曉磊，趙守鵬，等.家庭服務機器人智能空間技術研究與進展［J］.山東大學學報:工學版，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］席裕庚，張純剛.一類動態(tài)不確定環(huán)境下機器人的滾動路徑規(guī)劃［J］.自動化學報，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］樸松昊，洪炳熔.一種動態(tài)環(huán)境下移動機器人的路徑規(guī)劃方法［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.