移動機器人的混合式路徑規(guī)劃算法研究

鐘燦燦，陳萬米

(上海大學機電工程與自動化學院，上海 200444)

0 引言

路徑規(guī)劃是移動機器人研究的一個重要方向，指的是在移動機器人的運動環(huán)境中，為移動機器人尋找一條從起始點到終止點的無碰撞路徑[1-4]。針對該方向，國內(nèi)外學者提出了許多改進算法。柵格法對傳統(tǒng)的八鄰域節(jié)點進行改進，縮短了路徑長度，顯著減少了時間[5]。人工勢場法提出自適應調(diào)節(jié)算法，很好地克服了目標不可達的問題[6]。行為分解法提出自適應神經(jīng)模糊控制系統(tǒng)對隸屬度函數(shù)、模糊控制規(guī)則進行優(yōu)化，以達到更好的控制效果[7]。但這些改進算法中全局路徑規(guī)劃器反應很差，局部路徑規(guī)劃器易陷入局部死區(qū)陷阱[8]，不能使移動機器人很好地完成任務。

為了更加有效地解決移動機器人的路徑優(yōu)化問題，將全局路徑規(guī)劃算法與局部路徑規(guī)劃算法相結(jié)合，構(gòu)造混合式路徑規(guī)劃算法，彌補了柵格法、人工勢場法和行為分解法的缺點，并充分利用各自的優(yōu)點來解決移動機器人的路徑優(yōu)化問題。

1 問題描述

目前，移動機器人的導航技術(shù)雖然取得了不錯的成績，但是大部分導航技術(shù)研究中的環(huán)境比較結(jié)構(gòu)化。當移動機器人的工作環(huán)境為非結(jié)構(gòu)化的家庭環(huán)境時，其中存在的墻壁、家具、門等大量障礙物，以及房門關(guān)閉與否，都會嚴重影響移動機器人的工作，造成導航技術(shù)無法達到預期的效果[9]。對此，將慎思式行為與反應式行為相結(jié)合，把全局路徑規(guī)劃放到慎思層中，把局部路徑規(guī)劃和執(zhí)行放到反應層中，可以有效地解決這個問題。

慎思層路徑規(guī)劃利用已知的全局靜態(tài)環(huán)境地圖信息，為移動機器人的路徑規(guī)劃提供較優(yōu)序列。但其規(guī)劃時間較長，對實時信息的反應速度較慢。反應層路徑規(guī)劃能對環(huán)境中的突發(fā)情況作出快速反應，但缺乏智能性，因此需要與慎思層規(guī)劃方法結(jié)合使用。

1.1 慎思層路徑規(guī)劃

慎思層具有執(zhí)行規(guī)劃與高級決策等功能，需要高級的信息處理方式。慎思層的主要作用是負責全局路徑規(guī)劃。全局路徑規(guī)劃主要包括兩個方面：環(huán)境地圖的構(gòu)建和路徑搜索策略。

1.1.1 環(huán)境地圖的構(gòu)建

假設移動機器人的工作環(huán)境為普通的室內(nèi)環(huán)境，且已知整體的環(huán)境信息，采用傳統(tǒng)的柵格法進行地圖構(gòu)建。這種方法將移動機器人的工作環(huán)境按照固定柵格的大小進行劃分，采用八鄰域模型的運動方式。柵格劃分的數(shù)學模型如式(1)所示。

G(i,j)={(xG,yG)|xG≥(i-1)×l,xG

(1)

式中：G(i,j)為直角坐標系法表示的柵格單元，i為柵格地圖中的第i行，j為柵格地圖中的第j列；(xG,yG)為全局環(huán)境坐標系下的坐標；l為柵格的長度。

模型建立后，需要對其進行狀態(tài)信息編碼。每個柵格的狀態(tài)信息碼如式(2)所示。

(2)

式中：0表示該柵格沒有障礙物，為自由區(qū)域；1表示該柵格中存在障礙物，為障礙物區(qū)域；x表示該柵格為連接兩個相鄰子工作區(qū)域的關(guān)鍵區(qū)域，為動態(tài)區(qū)域，具有不確定性。

首先，將環(huán)境信息進行柵格化處理，確定哪些柵格狀態(tài)信息碼為0或x；然后，將狀態(tài)信息碼為0或x的相鄰區(qū)域連接起來，構(gòu)成一個“連接圖”；最后，找出初始點與目標點所在的單元柵格，并在連接圖中搜索連接初始點和目標點單元柵格的路徑。采用合適的路徑搜索策略，能夠找到最優(yōu)的路徑。

1.1.2 路徑搜索策略

在基于柵格法的路徑規(guī)劃算法中，所用到的搜索算法為廣度優(yōu)先搜索(breadth first search,BFS)。該搜索方法從起點到終點尋找到的路徑由兩兩相鄰的、不重復的柵格組成。建立一個路徑鏈表P，將目標柵格添加到P中。以P鏈表中的當前柵格為中心，從8個方向上搜索相鄰柵格中狀態(tài)信息碼為0或x的柵格，開通數(shù)條路徑鏈表(如P1、P2，...，Pn),標記狀態(tài)信息碼為x的單元柵格為動態(tài)柵格，并進行搜索[10]。判斷是否將起點柵格添加到P中，并選出P0～Pn中的最小值。該值即較短路徑Pbest。

1.2 反應層路徑規(guī)劃

反應層負責對慎思層規(guī)劃的路徑進行局部路徑規(guī)劃，細化整個全局路徑，彌補全局路徑規(guī)劃需要掌握全部障礙物信息和不可預見事件的缺點。因此，在反應層中用到人工勢場法或行為分解法。

1.2.1 人工勢場法

子目標點或目標點對移動機器人產(chǎn)生的作用力就像磁鐵一樣，吸引著移動機器人，可把它想象為引力。而障礙物與移動機器人的作用如同磁鐵同極一樣，將其視為斥力。移動機器人在引力與斥力的合力驅(qū)動下運動，由子目標點或目標點的引力場和障礙物的斥力場共同合成的總勢場決定移動機器人的運動方向[11]。人工勢場法流程如圖1所示。

圖1 人工勢場法流程圖

人工勢場法極易出現(xiàn)目標不可達的情況，具有不穩(wěn)定性。

1.2.2 行為分解法

為了解決移動機器人易陷入局部死區(qū)陷阱問題，采用行為分解法?；谛袨榉纸夥ǖ木植柯窂揭?guī)劃實時感知環(huán)境信息，更易處理一些非結(jié)構(gòu)化的環(huán)境。行為分解法將整個局部路徑規(guī)劃分為奔向目標行為、避障行為和搜索行為等[7]。

①奔向目標行為。

奔向目標行為是指在當前移動機器人發(fā)現(xiàn)目標點或者子目標點之間的連線上，不存在會與障礙物發(fā)生碰撞的情況時，移動機器人的行為。

當xt≠xr時：

(3)

當xt=xr時：

(4)

則奔向目標時機器人的轉(zhuǎn)向角αt為：

(5)

式中：(xr,yr)為移動機器人在全局環(huán)境坐標系下的位置;θr為移動機器人的運動方向與橫坐標的夾角;(xt,yt)為目標點在全局環(huán)境坐標系下的位置;θt為目標點與橫坐標的夾角。

奔向目標行為結(jié)束條件：在奔向目標的過程中，當滿足其他行為激活條件時，移動機器人切換為其他行為條件；當滿足其他行為的結(jié)束條件時，重新切換為奔向目標行為。

②避障行為。

通過超聲波傳感器檢測移動機器人周圍的障礙物信息。如果與障礙物的距離小于最近的距離dmin=0.1 m，調(diào)用避障行為函數(shù)，使移動機器人沿著障礙物邊緣運行，實行避障行為。當移動機器人環(huán)境滿足下述情況1或情況2時，移動機器人需切換到避障行為。

移動機器人車身前面有9個超聲波傳感器，分別為S1、S2、…、S9。S5處于移動機器人的正前方，超聲波傳感器的檢測范圍為5 cm～5 m。

情況1：障礙物在移動機器人前進方向的右側(cè)。

條件1：S6∨S7∨S8∨S9≤dmax。

條件2：min(S5,S6,S7,S8,S9)≥dmin。

條件3：(S1=∞)∧(S2=∞)∧(S3=∞)∧(S4=∞)。

條件1～條件3中：“∧表示“與”；∨ 表示“或”；Si表示第i個超聲波檢測到的距離值，i=1，2，…，9。條件1成立說明移動機器人前進方向的右側(cè)出現(xiàn)障礙物且距離障礙物較近。條件2成立說明移動機器人還沒有碰到障礙物。條件3成立說明移動機器人的左側(cè)沒有障礙物[12]。

情況2：障礙物在移動機器人的左側(cè)或正前方。

條件1：S1∨S2∨S3∨S4≤dmax。

條件2：min(S1,S2,S3,S4,S5)≥dmin。

條件3：(S6=∞)∧(S7=∞)∧(S8=∞)∧(S9=∞)。

當情況2滿足條件1～條件3時，說明移動機器人左側(cè)存在障礙物，應向右旋轉(zhuǎn)以避免碰撞。

避障行為結(jié)束條件：條件1或條件2被滿足。其中：條件1為Si=∞，i=1，2，…，9；條件2為子目標。目標點或障礙物位于移動機器人的兩側(cè)。

當障礙物在移動機器人的右側(cè)，目標點位于前進方向的左側(cè)時，由于移動機器人左側(cè)沒有障礙物，移動機器人放棄沿障礙物行走的行為，向左旋轉(zhuǎn)并切換到奔向目標行為。

③搜索行為。

行為分解法流程如圖2所示。

圖2 行為分解法流程圖

當動態(tài)柵格區(qū)域中存在障礙物時，需要判斷是否存在可行的區(qū)域。如果不存在可行的區(qū)域，那么表示兩個相鄰的子工作區(qū)域是死區(qū)，需上傳報警信息，移動機器人停止運動。當動態(tài)柵格區(qū)域存在障礙物且存在可運行的區(qū)域時，移動機器人沿著障礙物一側(cè)邊緣運行。移動機器人駛離動態(tài)柵格區(qū)域，由搜索行為切換為奔向目標行為[13]。

反應層接收到子目標點的位置信息，進行局部路徑規(guī)劃，初始狀態(tài)為奔向目標行為。檢測到障礙物并小于設定的最小距離時調(diào)用避障行為，到達分段點由序列發(fā)生器提供下一子目標的位置信息，在動態(tài)柵格區(qū)域采用搜索行為。循環(huán)往復，直到到達目標點。當不同的行為處理不同的事物時，多個行為之間往往為了爭奪控制器而發(fā)生沖突。行為分解法中采用基于行為優(yōu)先級的仲裁方法來處理不同行為之間的優(yōu)先級問題[14]。

2 算法設計

針對兩個相鄰子工作區(qū)域易陷入死區(qū)陷阱的問題，使用慎思層與反應層相結(jié)合的混合式規(guī)劃方法，把基于柵格法的規(guī)劃應用于慎思層中，把人工勢場法或行為分解法放到反應層中。移動機器人路徑規(guī)劃總流程如圖3所示。

圖3 移動機器人路徑規(guī)劃總流程圖

移動機器人路徑規(guī)劃步驟如下。

①構(gòu)建環(huán)境地圖，將移動機器人的工作環(huán)境進行柵格化處理。

②對柵格信息進行BFS，尋找較優(yōu)路徑，并將標記狀態(tài)信息碼為x的動態(tài)柵格作為子目標點。

③如果全局路徑規(guī)劃中不含有動態(tài)柵格，則直接進行單一的局部路徑規(guī)劃。否則，以動態(tài)柵格作為分割點，將較優(yōu)路徑分割成幾個路徑，每段路徑的結(jié)尾為一個子目標點，每個子目標點都可以作為一個要達到的目標，然后通過排序器下傳到反應層。

④當檢測到動態(tài)區(qū)域存在障礙物時，說明兩個相鄰的子工作區(qū)域可能是封閉的狀態(tài)，或者半封閉的狀態(tài)。若動態(tài)區(qū)域障礙物之間的最大距離大于移動機器人的寬度，則證明存在可行區(qū)域，處于半封閉狀態(tài)。若存在可行區(qū)域，移動機器人要搜索可行區(qū)域。如果處于完全封閉的狀態(tài)，則移動機器人上傳報警信息，停止路徑規(guī)劃。

⑤反應層采用人工勢場法或行為分解法進行局部路徑規(guī)劃，到達子目標點，給出下一子目標點。返回步驟④，循環(huán)直到到達目標點。

3 試驗分析

對基于柵格法與人工勢場法和基于柵格法與行為分解法進行驗證。在Vrep平臺中建立了室內(nèi)移動機器人導航模型，仿真環(huán)境為30×30區(qū)域。圖4所示為移動機器人部分三維環(huán)境模型。由圖4可知，移動機器人在有限的區(qū)域中運動。其中：Pioneer3代表移動機器人；Bill代表目標點；Plant代表靜態(tài)障礙物；Cupboard代表墻壁。上、下兩個Cupboard之間的區(qū)域為動態(tài)柵格區(qū)域，假設為兩個相鄰房間的房門區(qū)域。

圖4 移動機器人部分三維環(huán)境模型

移動機器人從起點Pioneer3(1.805 5,2.800 0)出發(fā)。慎思層利用柵格法進行柵格化之后，對運行環(huán)境信息進行全局路徑規(guī)劃，為反應層提供子目標點。反應層利用局部路徑規(guī)劃方法執(zhí)行慎思層規(guī)劃的路徑，快速響應移動機器人工作環(huán)境中碰到的各種障礙物，例如Plant與Cupboard，到達目標點Bill(7.150 0,5.005 0)。

表1所示為移動機器人的環(huán)境信息數(shù)據(jù)。

表1 環(huán)境信息數(shù)據(jù)

3.1 封閉的區(qū)域環(huán)境

假設動態(tài)柵格區(qū)域全被Plant占領(lǐng)，則認為兩個相鄰的子工作區(qū)域處于封閉的狀態(tài)，無法連通。當移動機器人到達分段的動態(tài)柵格區(qū)域時，檢測動態(tài)柵格里的障礙物信息。如果移動機器人沒有檢測到障礙物或者存在可運行的區(qū)域，繼續(xù)規(guī)劃下一段局部路徑。如果移動機器人沒有檢測到可行的區(qū)域，即動態(tài)柵格區(qū)域被障礙物Plant占領(lǐng)，則認為處于全封閉狀態(tài)。方法1與方法2在封閉的區(qū)域環(huán)境下的運行軌跡如圖5所示。如未檢測到可行區(qū)域，則上傳報警信息，停止路徑規(guī)劃。

圖5 在封閉的區(qū)域環(huán)境下的運行軌跡

3.2 半封閉的區(qū)域環(huán)境

當動態(tài)柵格區(qū)域未全被障礙物Plant占領(lǐng)，認為兩個相鄰的子工作區(qū)域處于半封閉的狀態(tài)，可以連通。

3.2.1 不可達情況

方法1與方法2在目標點被障礙物包圍時的運行軌跡如圖6所示。移動機器人在動態(tài)柵格區(qū)域檢測并運行到可行進路線，但檢測到目標點Bill被障礙物包圍，無法到達目標點。此時，上傳報警信息，停止路徑規(guī)劃。

圖6 在目標點被障礙物包圍時的運行軌跡

3.2.2 可達情況

混合式路徑規(guī)劃算法的移動機器人運行軌跡如圖7所示。

圖7 混合式路徑規(guī)劃算法的移動機器人運行軌跡

由圖7(a)可知，方法1的運動距離為7.500 m，運動所花時間60.100 s。依靠單一傳感器的效果不穩(wěn)定。為更好地達到避障效果，在移動機器人車身前面放置視覺傳感器。視覺傳感器觀測界面如圖8所示。分別為在軌跡點a1、a2、a3、a4處所觀察到的環(huán)境信息。

圖8 視覺傳感器觀測界面

由圖7(b)可知，方法2的運行長度為7.900 m，移動機器人在運行軌跡b1點處駛出了動態(tài)柵格區(qū)域，在原地旋轉(zhuǎn)了一定角度搜尋目標點，所花時間為70.040 s。

為驗證算法的有效性，每組試驗重復20次。表2所示為兩種算法的仿真結(jié)果。由表2可知，本文提出的混合式路徑規(guī)劃算法能夠取得較好的效果，移動機器人能夠規(guī)劃出一條較優(yōu)路徑，且路徑比較短。移動機器人并不會在起始點所在的子工作區(qū)域內(nèi)徘徊，而是直接朝向動態(tài)柵格所在的位置移動，規(guī)劃出的路徑符合人類的行走習慣。

表2 兩種算法的仿真結(jié)果

4 結(jié)論

為了研究移動機器人在非結(jié)構(gòu)環(huán)境下的路徑優(yōu)化問題，在Vrep平臺建立了移動機器人導航模型，提出慎思層與反應層相結(jié)合的混合式路徑規(guī)劃算法。在柵格法中對柵格狀態(tài)標記作出了改進，在基于柵格法與行為分解法的混合式路徑規(guī)劃算法中根據(jù)模型特點構(gòu)造避障行為與搜索行為[15]，并對兩種算法進行驗證。仿真試驗結(jié)果表明，混合式路徑規(guī)劃算法能很好地指揮移動機器人到達目標點。在設計的混合式路徑規(guī)劃算法中，移動機器人反應層采用行為分解法進行局部路徑規(guī)劃時，如果某些控制器不具有科學性，就會影響移動機器人的控制判斷。在兩種算法的驗證中，設計的障礙物都是靜止的狀態(tài)，未考慮動態(tài)障礙物。這兩個問題都是現(xiàn)在研究的熱點領(lǐng)域，后續(xù)研究將繼續(xù)改進移動機器人的路徑規(guī)劃算法，進一步提高算法的實時性與魯棒性。