基于雙層協調體系的多移動機器人路徑規劃方法

鐘佩思， 徐東方， 李東民， 梁中源， 劉 梅， 陳修龍

(1.山東科技大學先進制造技術研究中心， 青島 266590； 2.山東科技大學機電工程系， 泰安 271000)

多移動機器人系統能夠勝任單機器人所不能完成的復雜任務，因此，多移動機器人領域的研究受到了越來越多的關注[1-2]。其中，協調路徑規劃問題一直是多移動機器人研究的基礎和重點[3]，一種靈活而有效的協調路徑規劃方法，能夠進一步增強系統的可靠性和魯棒性，并提高工作效率。

用于單移動機器人路徑規劃的方法有很多，如拓撲法、遺傳算法[4]、人工勢場及其改進算法[5]等，與單機器人不同，除長度、平滑度和避障能力等問題外，多移動機器人路徑規劃還要考慮機器人之間的協調避碰問題[6]。近年來，對多移動機器人路徑規劃的研究正在不斷深入。孫樹棟等[7]用遺傳算法規劃出能實現協調避碰的多機器人全局路徑，但難以適應復雜的動態環境；周蘭鳳等[8]提出基于知識的遺傳算法，提高了軌跡規劃的效率；Kala[9]設計一種協同進化算法，規劃出了能夠有效協調避碰的多機器人全局路徑組合，但局部協調處理較為復雜；景興建等[10]提出用人工協調場進行多機器人路徑規劃以提高實時避碰能力，但其全局性能較差；吳晉等[11]通過改進人工協調場來應對協調規劃中的運動抖動和“死鎖”現象；Chandrasekhar等[12]采用改進的磷蝦群算法進行協調路徑規劃，但未能驗證在動態障礙物環境中算法的可行性。

可以看出，在目前的多移動機器人路徑規劃方法中，采用全局規劃算法很難應對局部路徑的實時協調，而傾向于局部規劃的方法又難以形成優質的全局路徑。針對這一問題，提出一種基于全局與局部雙層協調的多移動機器人路徑規劃體系，結合改進的免疫協同進化算法和動態窗口法，通過相互協調，力求機器人在沿全局最優路徑行駛時，仍能實現靈活地局部路徑調整，從而進一步提高多移動機器人系統的全局規劃與局部協調能力。

1 雙層協調體系的構建

基于雙層協調體系的多移動機器人路徑規劃方法主要由全局路徑規劃層和局部路徑協調層構成，其體系結構如圖1所示，采用混合式控制方式實現整體調控。路徑規劃前，首先要完成環境地圖構建、各機器人初始位置和目標位置的確定以及優先級編號等前處理工作，其中，機器人的優先級順序由所需完成任務的重要程度來決定。

圖1 體系結構框圖Fig.1 Architecture block diagram

全局路徑規劃層需要從各初始位置到目標位置為每個機器人分別規劃出一條長度最短、轉角最少、能有效避障并盡量減少機器人之間碰撞干涉的全局路徑，該工作由改進的免疫協同進化算法來實現。通過免疫克隆機制能加快種群進化速度，為進一步適應多移動機器人的全局路徑規劃，在原算法的基礎上引入路徑組的概念進行改進。首先為n個機器人分別規劃出多條較優的全局路徑，并將每個機器人對應一條全局路徑組成的集合X={X1,X2,…,Xn}稱為一個路徑組(Xi表示第i個機器人對應的一條全局路徑)，全局路徑規劃的目標函數作為抗原，而在種群中匹配出的所有路徑組均視為抗體，通過對每個抗體進行親和度評價并反復迭代進化，最終能夠規劃出整體效果最優的全局路徑組，機器人將沿著最優路徑組中各自的全局路徑行駛。

全局路徑規劃可以在一定程度上減少機器人之間的碰撞，但在工作過程中，難免出現動態或靜態未知障礙物以及多個機器人相遇的情形，無法保證完全避免碰撞。因此僅靠全局路徑規劃難以應對復雜的工作環境，還需要通過局部路徑協調來對機器人局部路徑進行靈活調整以避免產生碰撞。

局部路徑協調層的工作采用動態窗口法來實現，這是一種基于速度空間的局部避障方法，具有良好的動態處理能力?；陔p層協調體系，可以利用全局路徑信息輔助進行局部路徑協調，以獲得更好的規劃效果。首先根據全局路徑規劃結果對最優路徑組中的各條路徑進行跟蹤預測，當發現機器人之間存在干涉或碰撞趨勢時，便提取碰撞點周圍的環境信息以及各機器人在全局路徑上的運動信息進行局部路徑協調，從而避免碰撞發生；機器人個體對周圍環境也進行實時探測和判斷，當檢測到可能產生碰撞的未知障礙物時，同樣提取周圍工作環境信息并進行局部路徑調整避障。完成局部路徑調整后，機器人重新回到各自的全局路徑上繼續行駛，直至順利到達原定目標位置。

2 全局路徑規劃的實現

2.1 搭建工作環境

假設多移動機器人系統的工作空間S中有n個移動機器人和m個障礙物，且機器人個體在長度或寬度方向上的最大尺寸為d。構建地圖時，把障礙物的邊界向外擴展d/2，則在全局路徑規劃中機器人個體可視為質點。將機器人按優先級高低分別編號為R1, R2,…, Rn，執行的任務越重要，機器人的優先級就越高，相應的下標編號也就越小，而障礙物分別編號為O1, O2,…, Om。所有的障礙物共同組成障礙物空間SO，剩余的空間則稱為自由空間SF。

將移動機器人Ri的起始位置定義為pi,1=(xi,1,yi,1)，目標位置定義為pi,l=(xi,l,yi,l)，l為路徑點的個數，其全局路徑Xi便可通過SF中的點序列{pi,1,pi,2,…,pi,l}來表示，其中pi,j=(xi,j,yi,j)，表示Ri的第j個路徑點坐標值，由于不同機器人的行駛路徑不同，所以路徑點的個數l也會各不相同。為了便于擴展系統中機器人的個數，并減少后期實時路徑跟蹤和預測的計算量與計算時間，規定在全局路徑上，所有移動機器人都以速度vq進行勻速行駛。

2.2 改進免疫協同進化算法

免疫協同進化算法流程如圖1中的全局路徑規劃層所示，為更好地適應多移動機器人系統，在傳統免疫協同進化算法步驟[13]的基礎上進行了改進。

首先為每個機器人隨機生成N條全局路徑，這些路徑共同構成一個n×N矩陣，稱為抗體種群A。之后通過適應度函數對種群中的各條路徑進行評價，選取適應度較高的n×K條路徑構造記憶集Am，而剩下的n×L條路徑則構成一般集Ag，定義適應度評價函數如下：

f(Xi)=α1len(Xi)+α2cor(Xi)+α3aov(Xi)

(1)

式(1)中：len(Xi)表示路徑Xi的總長度，計算公式為

(2)

cor(Xi)表示路徑Xi中連續折線間轉角的總弧度，計算公式為

(3)

aov(Xi)表示路徑Xi與工作空間中障礙物的相交次數；而α1、α2、α3則表示權重系數。

劃分完Am和Ag后，從Am中再選取適應度較高的n×G條路徑進行克隆以獲得克隆集Ac，其中G≤K，其大小與Am的整體適應度成正比。為保證Am不遭到破壞，這里僅對Ac和Ag進行變異，并對Ac進行粒群進化處理[13]，之后對Am、Ag、Ac中的路徑進行疫苗接種。如圖1所示，疫苗主要分為刪除、平滑、交換和避障4類，通過接種疫苗，可以有針對性地優化個體，從而加快收斂速度。

接種疫苗后，再次對Am、Ag、Ac中的路徑進行適應度評價并更新記憶集，最后在更新后的記憶集A′m中通過排列組合匹配路徑組，并通過親和度函數對不同的路徑組進行評價以判斷是否達到規劃要求，將路徑組的親和度評價函數定義為

(4)

式(4)中：C為一較大常數；X={X1,X2,…,Xn}表示從A′m中匹配出來的路徑組；col(Xi)表示Xi與路徑組中其他路徑的相交次數，由于初始條件不確定，路徑相交便存在碰撞的可能；cro(Xi)表示路徑Xi在路徑組中的擁擠程度，用Xi兩側規定范圍D內其他路徑的條數來表示；β1、β2為權重系數。

參照親和度評價結果，若存在滿足要求的最優路徑組，則完成全局路徑規劃，機器人沿各自全局路徑勻速行駛，并由系統進行跟蹤預測，以及時判斷可能產生碰撞的位置；若各路徑組均不滿足規劃要求，則消亡掉當前種群中適應度低的路徑，并更新種群以維護多樣度不變，重復上述步驟繼續迭代進化，直到獲得滿足要求的路徑組為止。

2.3 全局規劃性能驗證

在傳統免疫協同進化算法中，適應度評價被包含進親和度函數中，直接由親和度評價來劃分Am和Ag，并且親和度評價的對象是單條路徑而非路徑組，因此每次評價時，需要將待評路徑與其他機器人所有路徑的親和度都計算一遍，計算量較大，致使迭代速度較慢，而且也不利于機器人個數的擴展。同時也因為評價對象是路徑個體，所以最終獲得的每條路徑親和度可能達到最高，但放一起卻未必是整體親和度最好的一組。

進行變異、進化和疫苗接種前，種群中各條路徑的適應度都相對較低，此時進行親和度評價不僅計算量大，且效果不佳，因此在改進的算法中，先通過適應度函數對Am和Ag進行劃分，之后進行克隆、變異、進化和免疫處理，并再次利用適應度函數對路徑進行評價，更新出適應度相對較好的記憶集A′m后，再對A′m中的路徑進行親和度評價。

在進行親和度評價時，先通過排列組合將A′m中的路徑匹配為不同的路徑組，再利用親和度函數分別評價，并通過反復迭代，尋找親和度最高的路徑組。親和度評價對象是路徑組這個整體概念而不是單條路徑，所以進行評價時只考慮該路徑組中n條路徑間的親和度即可，而不需要將待評路徑與其他機器人所有路徑的協作行為都計算一遍。

通過改進算法，能夠將多機器人全局路徑規劃先簡化為單個機器人的路徑規劃問題，當種群中的路徑都達到較好的適應度后，再匹配路徑組進行多機器人的路徑親和度評價，減少了很多不必要的協作度計算，從而加快了迭代速度。并且由于親和度評價對象是路徑組而非單條路徑，所以能夠保證最終規劃出的n條路徑可以取得最好的整體效果。在相同的軟硬件環境和初始條件下，改進前后算法的迭代速度和迭代效果對比如圖2所示。

從圖2(a)中可以看出，改進后算法的迭代速度要優于原算法，且隨著迭代次數的增加，這種優勢會越來越明顯，而圖2(b)則顯示改進后的算法在迭代至30次左右便逐漸趨于穩定，比改進前算法的收斂速度快，而且最終所獲得的路徑組親和度也更高。這說明對算法的改進可行且有效，能實現更加快速、高質量的多移動機器人全局路徑規劃。

圖2 算法性能對比Fig.2 Comparison of algorithm performance

3 局部路徑協調規劃

3.1 動態窗口法的構建

機器人沿各自全局路徑以vq勻速行駛，根據初始位置和行駛時間可以預測出存在碰撞或干涉的路徑點，當需要進行局部路徑協調時，便啟用動態窗口法進行調整。動態窗口法的主要思想[14]是在速度空間中采樣多組速度并模擬機器人在這些速度下一定時間Δt內的軌跡，得到多組軌跡后，通過對這些軌跡進行評價，選取最優軌跡所對應的速度來驅動機器人進行路徑協調。

(5)

進行速度采樣時，雖然速度空間中存在無窮多組速度，但并不是每組速度都可取，需要根據機器人自身條件和工作環境的限制，將速度采樣控制在一個速度動態窗口V內，該動態窗口可以表示為機器人最值速度窗口Vm、電機加速度性能窗口Ve和安全速度窗口Vs的交集：

(6)

圖3 動態窗口采樣示意圖Fig.3 Dynamic window sampling diagram

基于動態窗口進行速度采樣，并根據機器人運動學模型推算每組速度在時間Δt內對應的軌跡，進而完成對各條軌跡的評價，構造評價函數為

h(v,ω)=γ1hea(v,ω)+γ2dis(v,ω)+

γ3vel(v,ω)

(7)

式(7)中：hea(v,ω)用來評價在當前采樣速度下，機器人到達模擬軌跡末端時的前進方向與目標方向之間的夾角δ，用π-δ表示，即δ越小評分越高；dis(v,ω)表示機器人在當前模擬軌跡上與障礙物之間的最小距離；vel(v,ω)表示當前模擬軌跡對應的線速度大小；γ1、γ2、γ3表示權重系數。

最后基于上述函數的評價結果，選擇評分最高的軌跡對應的采樣速度來驅動移動機器人運動。

3.2 局部路徑協調處理

局部路徑協調過程中，規定機器人的速度方向與大小均由動態窗口法的評價結果來確定，但協調的初始速度和終止速度仍然是全局路徑上對應的速度，從而確保完成局部路徑調整后，機器人回到全局路徑上仍然能按原定速度勻速行駛。

當機器人之間存在碰撞可能時，需要比較這些機器人的優先級，由優先級低的機器人啟用動態窗口法進行局部協調，而優先級最高的機器人將不受影響，繼續沿自身路徑行駛。相比之下，機器人的優先級越低，路徑調整的次序就越靠前，相應的協調范圍也越大，依次類推。位于全局路徑或局部調整中的機器人，均可按此方法進行協調。而對于未知障礙物，則需要機器人自行檢測并判斷，若存在碰撞可能，便及時進行局部路徑調整避障。

當協調避碰的對象為未知障礙物或處于局部調整中的機器人時，由于其運動信息不可預知，直接利用路徑評價函數選擇最合理的(v,ω)來驅動機器人進行協調避碰即可；而當避碰對象為全局路徑上的機器人時，則可利用機器人在全局路徑上的運動信息來輔助局部路徑協調，從而進一步提高整體規劃效果，仍以Rb的局部協調避碰為例，可通過Ra的全局路徑運動信息完善對Rb的路徑評價，在二者行駛軌跡相交前，引入新的路徑評價項：

(8)

式(8)中：t′表示在采樣速度(v,ω)驅使下，Rb的行駛軌跡與Ra全局路徑相交時所需要的時間；(xa,t′，ya,t′)和(xb,t′，yb,t′)分別為行駛軌跡相交時，Ra和Rb所到達的位置坐標，該項權重系數取為γ4。

通過對行駛軌跡相交時機器人之間的位置關系進行評價，能夠挑選出更加合理的運動軌跡，從而提高局部路徑協調效率，待行駛軌跡相交后，再重新啟用原路徑評價函數進行評價調整。通過增加距離評價項，也可使該方法擴展至全局路徑上多個機器人之間協調避碰的情況中。

4 仿真試驗分析

利用MATLAB編程構建多機器人雙層協調路徑規劃體系，隨機生成二維柵格環境地圖，柵格邊長設置為1 m，系統中使用Pioneer3-DX款差分驅動機器人模型，d取為0.5 m，機器人個數設置為4，分別進行優先級編號并分配初始位置與目標位置坐標，在柵格地圖上起點位置和目標位置分別用實心的圓形和方形來表示，由此進行仿真試驗。路徑規劃過程中，各權重系數的取值如表1所示。

表1 權重系數取值Table 1 Calculation results of power

進行全局路徑規劃時，親和度評價函數中常數C取為100，經過迭代進化，最終得到的規劃結果如圖4所示。規劃過程中，種群內各路徑的適應度值變化情況如圖5所示，從A′m中匹配出的各路徑組親和度值變化則如圖6所示?？梢钥闯?，迭代次數達到30次以后，便可獲得趨于穩定規劃結果，最終所獲得的各路徑適應度值較好，且路徑組的親和度也很高。改進后算法的收斂速度快，且全局路徑規劃的結果較為理想。

圖4 全局路徑規劃結果Fig.4 Results of global path planning

圖5 全局路徑適應度值Fig.5 Fitness of global path

圖6 全局路徑親和度Fig.6 Affinity of global path

設定系統的初始條件為機器人從各自起點位置同時出發，在全局路徑上以vq=0.3 m/s勻速行駛，由此進行路徑跟蹤和預測?；诔跏紬l件推算，機器人R1和R2在19.893 s會產生碰撞，此時R2處于圖4中的a點位置，需要在a點附近對R2進行局部路徑協調，R2的優先級比R1低一級，取協調范圍半徑為4vq，并將R1視為以d為半徑的圓形動態障礙物，處理結果如圖7(a)所示；路徑調整致使R2的行駛時間延長，進而導致R2和R3在39.476 s時存在碰撞可能，此時R3處于圖4中的b點位置，則在b點附近同樣需要對R3進行局部路徑協調，處理結果如圖7(b)所示。

圖7 局部路徑協調處理Fig.7 Coordination of local paths

可以看出圖4中R3和R4的全局路徑也存在交點，但是根據初始條件推算，兩個機器人之間不會發生干涉和碰撞，故不需要進行局部路徑協調，對于地圖中的未知障礙物c，需要由機器人自行探測并判斷，以及時進行調整避讓。

完成局部路徑協調處理后，機器人重新回到原全局路徑，以vq繼續向前勻速行駛，若再次出現碰撞趨勢，便重新開啟動態窗口法進行局部路徑調整，直至順利到達目標位置以執行原定任務，基于雙層協調體系的路徑規劃結果如圖8所示。

從圖8中可以看出，基于雙層協調體系的多移動機器人路徑規劃方法所規劃出的各條路徑均具有較好的全局效果，并且可以實現機器人之間靈活有序的協調避碰，對于突然出現的未知障礙物也能做出及時的避障處理，能夠實現預期目的。

圖8 雙層協調路徑規劃結果Fig.8 Results of bilevel coordinated path planning

5 結論

針對多移動機器人的路徑規劃問題，提出一種基于全局與局部雙層協調的路徑規劃方法，通過仿真分析，得到以下結論。

(1)引入路徑組親和度評價方法來改進免疫協同進化算法，提高了全局路徑規劃的效率，為多移動機器人系統規劃出了質量更高并能盡量減少機器人之間干涉碰撞的全局路徑。

(2)利用動態窗口法并結合機器人的優先級機制和全局路徑信息，及時對機器人的局部路徑進行協調處理，有效避免了復雜環境下各機器人之間以及機器人與未知障礙物之間的碰撞。

基于雙層協調體系的多移動機器人路徑規劃方法可以實現機器人在沿全局最優路徑行駛時，仍能進行靈活有序的局部路徑協調，能夠應對動態復雜的工作環境，進一步提高了多移動機器人系統的作業能力。