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

鐘燦燦，陳萬米

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，上海 200444)

0 引言

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

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

1 問題描述

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

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

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

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

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

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

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

(1)

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

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

(2)

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

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

1.1.2 路徑搜索策略

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

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

反應(yīng)層負(fù)責(zé)對慎思層規(guī)劃的路徑進(jìn)行局部路徑規(guī)劃，細(xì)化整個(gè)全局路徑，彌補(bǔ)全局路徑規(guī)劃需要掌握全部障礙物信息和不可預(yù)見事件的缺點(diǎn)。因此，在反應(yīng)層中用到人工勢場法或行為分解法。

1.2.1 人工勢場法

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

圖1 人工勢場法流程圖

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

1.2.2 行為分解法

為了解決移動(dòng)機(jī)器人易陷入局部死區(qū)陷阱問題，采用行為分解法。基于行為分解法的局部路徑規(guī)劃實(shí)時(shí)感知環(huán)境信息，更易處理一些非結(jié)構(gòu)化的環(huán)境。行為分解法將整個(gè)局部路徑規(guī)劃分為奔向目標(biāo)行為、避障行為和搜索行為等[7]。

①奔向目標(biāo)行為。

奔向目標(biāo)行為是指在當(dāng)前移動(dòng)機(jī)器人發(fā)現(xiàn)目標(biāo)點(diǎn)或者子目標(biāo)點(diǎn)之間的連線上，不存在會(huì)與障礙物發(fā)生碰撞的情況時(shí)，移動(dòng)機(jī)器人的行為。

當(dāng)xt≠xr時(shí)：

(3)

當(dāng)xt=xr時(shí)：

(4)

則奔向目標(biāo)時(shí)機(jī)器人的轉(zhuǎn)向角αt為：

(5)

式中：(xr,yr)為移動(dòng)機(jī)器人在全局環(huán)境坐標(biāo)系下的位置;θr為移動(dòng)機(jī)器人的運(yùn)動(dòng)方向與橫坐標(biāo)的夾角;(xt,yt)為目標(biāo)點(diǎn)在全局環(huán)境坐標(biāo)系下的位置;θt為目標(biāo)點(diǎn)與橫坐標(biāo)的夾角。

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

②避障行為。

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

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

情況1：障礙物在移動(dòng)機(jī)器人前進(jìn)方向的右側(cè)。

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

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

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

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

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

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

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

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

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

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

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

③搜索行為。

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

圖2 行為分解法流程圖

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

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

2 算法設(shè)計(jì)

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

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

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

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

②對柵格信息進(jìn)行BFS，尋找較優(yōu)路徑，并將標(biāo)記狀態(tài)信息碼為x的動(dòng)態(tài)柵格作為子目標(biāo)點(diǎn)。

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

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

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

3 試驗(yàn)分析

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

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

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

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

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

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

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

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

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

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

3.2.1 不可達(dá)情況

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

圖6 在目標(biāo)點(diǎn)被障礙物包圍時(shí)的運(yùn)行軌跡

3.2.2 可達(dá)情況

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

圖7 混合式路徑規(guī)劃算法的移動(dòng)機(jī)器人運(yùn)行軌跡

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

圖8 視覺傳感器觀測界面

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

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

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

4 結(jié)論

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