基于吸附行為的多機器人編隊避障算法

汪暢，王桐，高山

哈爾濱工程大學 信息與通信工程學院，黑龍江 哈爾濱 150001

多機器人系統是近年來機器人研究的熱點問題，在軍事、醫療、運輸、安全巡邏、救援救災等領域有著重要的應用前景。多機器人協同的一個經典例子就是多機器人編隊控制，其目標是實現和保持一個預定義的編隊模式，使得多機器人可以協同完成給定的任務。在未知環境下不僅需要有效的編隊控制算法和協同避障算法，保證完成團隊任務，還需要機器人感知周圍的環境信息。

目前，多移動機器人編隊控制方法主要包括領航-跟隨法、虛擬結構法、基于行為法等。其中領航-跟隨法由于其簡單性和可擴展性，被廣泛應用。Desai 等[1]將多機編隊控制問題分為軌跡跟蹤、機器人控制和編隊控制3 個子問題；多機編隊控制是通過控制相對角度、相對距離來實現的；孫玉嬌等[2]建立了在非完整約束條件下輪式機器人的數學模型，然后通過全局坐標轉換將領航-跟隨模型轉換為局部坐標系統誤差模型；吳軍成等[3]針對多機器人在不確定環境下難以保持某種隊形到達預定目標等問題，提出了一種無需全局坐標的多機器人編隊控制算法；有研究者提出基于Lyapunov 方法[4-5]推導系統的全局穩定性，但是領導者和跟隨者之間的定位誤差是有界的，不能收斂到零。

盡管多機器人的領航-跟隨編隊控制取得重大進展，但是在編隊控制設計上，要加強避障算法的研究。由于人工勢場法具有結構簡單、實時性好等特點，被廣泛應用在多機器人編隊中。Kowdiki 等[6]基于領航跟隨法提出一種混合編隊控制技術，并使用人工勢場法為領航智能體進行路徑規劃，保持整個編隊的穩定控制；同時人工勢場法也存在著缺陷，桑雷等[7]指出人工勢場法容易陷入局部極小值等問題；李金芝等[8]通過添加虛擬斥力旋轉勢場，解決了局部極小值問題，實現了多機器人編隊在靜態障礙環境中無碰撞路徑規劃。

現有研究方法，針對未知環境下的多機器人編隊研究較少，特別是如何保持編隊隊形穩定、實時避障、編隊反饋等方面。本文基于領航-跟隨法進行編隊隊形控制，引入輸出反饋控制律和吸附行為，保證編隊的穩定性；通過機器人攜帶的傳感器自主感知并獲取環境信息，采用結合人工勢場法和基于“虛擬引導車”的沿墻行為在未知環境下進行避障，避免陷入局部最小值。最后，通過軟件仿真驗證可行性，并在自主搭建的多移動機器人實驗平臺上進行驗證。

1 領航-跟隨控制算法

1.1 非完整移動機器人運動學模型

由于輪式移動機器人的結構簡單、控制容易和運動穩定等特點，具有較高使用價值和應用前景。本文研究采用輪式移動機器人，具有非完整運動約束的運動模型[9]，運動模型如圖1 所示。

圖1 輪式移動機器人運動模型

定義機器人在環境中的位姿為q=[x,y,θ,γ]T，機器人將繞瞬心ICR 以半徑RB做圓弧運動，可以得到輪式移動機器人的運動學模型為

1.2 領航-跟隨編隊控制算法的改進

1.2.1 基于輸出反饋的編隊控制律

基于l-φ的編隊控制方式只需設置一個領航者，跟隨者根據相對距離和角度進行運動，實現期望隊形的編隊控制，在編隊方式上更為靈活。但是編隊沒有明確的反饋，由于領導者的存在缺乏魯棒性，因此，在此基礎上，利用輸出反饋控制律設計編隊控制方案。

基于輸出反饋控制律如圖2 所示，跟隨者通過計算與領航者之間的位置，得出實際相對距離和角度，再根據系統設定的相對距離和角度，由控制器輸出速度控制的調整參數，使跟隨者調整自身位姿，跟隨領航者運動，并保持相對距離和角度的收斂，即l→ld，φ →φd。

圖2 基于輸出反饋控制律

領航-跟隨算法編隊控制運動模型如圖3 所示。以旋轉中心為基準定義機器人i的位姿為(xi,yi,θi)，分別為第i個機器人的x坐標、y坐標和迎角；vi、wi為旋轉中心處的速度，l為領航者與跟隨者之間的歐氏距離， γ為跟隨者在其參考點處的相對方位角， φ為領航者與跟隨者之間的相對角度，d為旋轉中心與參考點之間的位移，且其正方向表示機器人前進方向。

圖3 領航-跟隨算法編隊控制運動模型

跟隨者的運動學方程為

1.2.2 基于吸附行為的領航-跟隨編隊控制算法

基于輸出反饋控制律的表達式為

式中 α1、 α2為比例控制系數。

跟隨者的控制輸出為

根據跟隨者的控制輸出，通過領航者位姿相關信息與跟隨者位姿相關信息，得出被控對象(跟隨者)在下一周期t+T時刻的控制輸入為(v2(t+T),w2(t+T))，通過改變跟隨者當前的運動狀態，完成編隊任務，使l和 φ收斂并保持系統設定值ld和 φd。

根據文獻[10]提出的相關理論，對編隊系統的穩定性進行分析可知，基于輸出反饋控制律的編隊控制中，若領航者作勻速直線運動或勻速圓周運動，編隊隊形將收斂為固定隊形(l=ld,φ=φd)，且隊形與跟隨者的初始狀態并不影響該固定隊形。

通過基于輸出反饋控制律的編隊控制分析可知，多機器人編隊運動對于l和 φ是漸近穩定的，若領航者運動速度過快將導致跟隨者很難跟上，跟隨者的運動軌跡相對領航者呈現發散狀態，即編隊隊形遭到破壞。

為了使編隊系統能夠穩定地編隊運動，本節將一種吸附行為[11]和基于輸出反饋控制律編隊相結合，提出一種基于吸附行為的領航跟隨算法。通過在領航者附近設置若干吸附區，跟隨者被吸引而直接移動至領航者附近，同時自身也形成吸附區，從而可以應用于更大規模的編隊。步驟描述如下：

1)以領航者的坐標(x0,y0)為中心建立對跟隨者的吸附區，定義為

2)第i個跟隨者以較短的時間朝著領航者的吸附區移動，并到達勢點附近；

2 傳統人工勢場法

2.1 傳統人工勢場法理論

Khatib[12]首次提出人工勢場的概念，移動機器人在虛擬勢場中所受到的力由勢場的負梯度產生，環境中的障礙物對機器人產生斥力，目標對機器人產生引力，引力和斥力共同作用下的合力控制機器人向總勢場下降最快的方向運動。圖4為機器人受力分析。

圖4 機器人受力分析

1）引力勢場函數。

由目標點產生引力場，將機器人抽象為二維空間內的質點，則人工勢場法中引力勢場函數為

式中：Uia為引力勢場函數；Fia為引力，由引力勢場函數的負梯度所得；ka為引力勢能增益系數；d(q,qa)為機器人q與目標點qa之間的距離。

2）斥力勢場函數。

由障礙物產生斥力場，則人工勢場法中斥力勢場函數為

式中：Uir為斥力勢場函數；Fir為斥力，由斥力勢場函數的負梯度所得；kr為斥力勢能增益系數；d(q,qr)為機器人q與障礙物qr之間的距離；dmax為障礙物的斥力勢場形成的作用力最大距離。

3）局限性。

人工勢場法存在局限性，最重要的有2 個：1)目標不可達問題；2)局部最小值問題。目標不可達問題如圖5 所示，當目標位置位于障礙物斥力勢場范圍內，機器人向目標移動時，斥力場迅速增大，引力場迅速減小，因此機器人達不到目標。局部最小值問題如圖6 所示，機器人運動過程中的某一點達到引力和斥力的平衡，機器人誤以為達到目標點而停止不動或在該點徘徊。

圖5 目標不可達問題

圖6 局部最小值問題

2.2 傳統人工勢場法的改進

多機器人根據搭載的傳感器獲取環境信息建立人工勢場，同時將當前生成的路徑與環境信息形成閉環控制，實現在未知環境下的避障。為解決人工勢場法的局限性采用基于行為法中的一種沿墻運動行為[13]和人工勢場法相結合，提出融合沿墻行為的避障策略；利用虛擬引導車的生成/消失條件不斷切換運動狀態，保證機器人在未知環境下有效地避障，同時機器人進行避障不再受初始設置勢場力的影響。

2.2.1 基于虛擬引導車的沿墻行為

沿墻運動行為是指在運動過程中，遇到墻壁或者障礙物時，沿墻壁或障礙物表面進行運動，從而實現無碰撞運動。

由于沿墻行為缺少全局信息時會導致無法擺脫局部最小值問題，因此在基于沿墻行為的避障運動中，引入“虛擬引導車”的概念，如圖7 所示。虛擬引導車是真實機器人在障礙物或墻體表面的投影，位于真實機器人通過正交投影與障礙物表面相交的點，虛擬引導車運動的迎角是機器人所在位置的切線與墻面或障礙物表面形成的角度。在沿墻行為中，為了保持虛擬引導車與真實機器人之間的距離和角度，需要添加虛擬的斥力作用。因此，機器人避障是虛擬引導車的“斥力”與目標點的引力共同作用的結果。

采用沿墻行為需要對墻面進行建模，主要包括定義墻面方向和墻面類型確定2 個方面的工作。

1）墻面方向。

在機器人上使用模擬的激光測距儀，可以比較精確地辨識墻面輪廓，因此可通過均值濾波的方法大致判定墻面走向。如圖7 所示，激光點i(i=1,2,···)的坐標為(oxi,oyi)，激光點i和i+1 之間的矢量連線角度為

墻面的走向為

式中N為反射的激光點個數，N≥2。當N=1 時，ηi無法計算，機器人將按原方向進行運動。當ηwall的方向與機器人的迎角 θveh的夾角大于90°，導致機器人陷入沿墻運動，需要對墻面走向進行修正，得到新的墻面方向為

2）墻面類型確定。

機器人在移動過程中會遇到幾種墻面類型，主要根據機器人能夠掃描到的墻面數量進行區分。一個機器人一次性掃描得到的障礙物信息點序列為若?δi∈(0,2(rveh+D))，則稱機器人面對的障礙物是單面墻體；若?δi∈(2(rveh+D),+∞)，則稱機器人面對障礙物是多面墻體。其中，rveh為當前機器人的最大半徑，D為機器人的安全碰撞距離。

2.2.2 融合沿墻行為的避障算法

基于上述原理，通過將人工勢場法和沿墻行為進行整合設計避障算法。如圖8 所示為融合沿墻行為的避障算法的基本原理。該算法主要由以下4 個步驟組成。

圖8 沿墻運動法與人工勢場法的切換

1)虛擬引導車的生成條件判定。

機器人在避障過程中，如果所受的合力處于平衡狀態，會陷入局部最小值問題，機器人停止運動，此時生成虛擬引導車，將機器人與引導車之間的初始距離記為ds0。

2)沿墻運動狀態的描述。

機器人以沿墻行為方式運動時，首先需要確定墻面的類型和相應的墻面方向η′wall，機器人運動過程中速度保持在v0。沿墻避障運動時，要求機器人實時判斷與虛擬引導車的間距ds，若ds

3)虛擬引導車消失條件的判定。

機器人在避障過程中需要在沿墻運動和人工勢場法之間切換，因此需要對虛擬引導車的消失條件進行判斷，判定條件為：1）當障礙物的作用范圍不對機器人產生影響時，虛擬引導車將會消失；2）當機器人所受虛擬引導車的斥力與引力方向構成銳角時，其已經離開障礙物表面，向目標點移動。

4)以人工勢場法進行運動。

在人工勢場法的作用下，機器人朝目標運動，當遇到障礙物時，切換至沿墻運動行為，重復步驟1)~3)。

3 實驗仿真及分析

3.1 多移動機器人編隊控制仿真

為了證明基于l-φ輸出反饋律改進后的多機器人編隊運動算法的穩定性，在MATLAB 中進行多機器人編隊仿真實驗。將參與編隊運動的5 個機器人分布在仿真環境為15 m×15 m的矩陣區域中，5個機器人的初始位姿分別為V1為領航者，V2～V5為跟隨者，以領航者為參考基準，跟隨者和領航者之間的距離絕對值為‖l12‖ =‖l13‖ =0.7和‖l14‖ = ‖l15‖ =1.4，相對角度的絕對值為‖φ12‖ = ‖φ14‖ =‖φ13‖ = ‖φ15‖ =π/2。

領航-跟隨編隊運動效果如圖9（a）所示，基于吸附行為的領航跟隨編隊運動效果如圖9（b）所示?？梢钥闯鲱I航-跟隨編隊運動中，當領航者大于0.8 m/s 速度時運動，導致整個編隊隊形擴散，即編隊被破壞；而融合吸附行為后，編隊成員的運動軌跡都是比較平滑無振蕩的，而且跟隨者的運動軌跡都是收斂于領航者的運動軌跡，表明基于吸附行為的領航-跟隨算法的編隊系統能保持穩定狀態。如圖9 所示，多機器人編隊系統由5 個編號為1~5 的機器人組成，5 個機器人從起點位置出發前往目標節點。

圖9 改進前后編隊運動軌跡

為進一步驗證改進算法的有效性和準確性，將基于吸附行為的領航-跟隨（attachment behavior based leader-follower, ABLF）控制算法與經典的領航-跟隨（leader-follower, LF）控制算法以及基于虛擬領航者的領航-跟隨（virtual leader leader-follower,VLLF）控制算法進行對比。當領航者以1 m/s 速度運動時，得到如圖10 所示的領航者和跟隨者2在運行過程中相對距離l和相對角度φ的對比結果。

圖10 3 種算法對比仿真

從圖10（a）可以很明顯看出在領航者行駛速度較快時，LF 的領航者與跟隨者的相對距離逐漸拉開，VLLF 由于虛擬領航者的存在，在一定程度上也能保持穩定狀態，但速度過快同樣會導致領航者與跟隨者相對距離變大。而ABLF 算法下領航者與跟隨者的相對距離保持在穩定的0.7 m 左右。如圖10（b） 所示為領航者與跟隨者2 的相對角度的變化，可以看出ABLF 的相對角度穩定在90°左右，而LF 和VLLF 的相對角度則出現跳變，無法保持穩定。

從對比結果可以分析得出ABLF 算法下各成員之間的相對距離和角度穩定在設定值（l=ld，φ=φd），從而形成穩定的編隊運動，證明了基于吸附行為的領航跟隨編隊控制算法具有更高的穩定性和準確性。

3.2 多移動機器人避障仿真

針對傳統的人工勢場函數存在的局限性問題，本文提出的基于沿墻行為的避障策略能夠有效地避免該問題。本文對改進前后的算法進行仿真，驗證其有效性。機器人初始位置隨機分布，仿真環境中隨機設置一些靜態障礙物（黑色塊部分），5 個機器人從起點位置出發前往目標節點。

1) 目標不可達問題。

通過實驗對比發現，如圖11 所示，當障礙物在目標附近，傳統人工勢場法會導致機器人無法到達。而改進后的融合沿墻行為的避障算法可以有效地到達目標點。

圖11 目標不可達問題

2) 局部最小值問題。

如圖12（a）所示移動機器人在傳統的人工勢場函數下進行避障運動，可以看到機器人在障礙物處停止了，即陷入了局部最小值的“陷阱”。如圖12（b）所示是融合沿墻行為的避障運動，經過障礙物附近的運動軌跡與障礙物表面的形狀基本吻合，即進入沿墻行為，如4 號機器人和5 號機器人所示的軌跡；未檢測到障礙物的機器人在目標點的吸引下朝著目標方向移動，如1~3 號機器人的運動軌跡，機器人都成功地避免開障礙物，并未出現停止運動的狀態。仿真結果表明機器人融合沿墻行為的避障策略成功地避免了陷入局部最小值的情況。

圖12 局部最小值問題

3.3 多移動機器人實驗平臺仿真

本文以自主研制的多移動機器人實驗平臺來驗證算法的有效性。該系統由移動機器人和工作站組成，如圖13 所示，所有機器人和工作站均安裝ubuntu16.04 操作系統，并運行ROS Indigo。移動機器人采用上位機與下位機通信模式的軟件架構，其中上位機采用樹莓派4B 作為主控板，下位機采用STM32 作為驅動板，同時配置激光雷達、電機等。

圖13 多移動機器人實驗平臺

機器人以分散的方式控制，工作站不負責控制任何跟隨機器人。每個機器人和工作站運行一個自組織通信節點，從而組成一個自組織網絡。Adhoc 通信節點創建一個服務調用，其中包括發送方和接收方ID、傳輸數據、目的地發布數據主題等。在工作站執行一個Adhoc 通信節點，與機器人通信，并為機器人定義編隊中的角色，領導者或者跟隨者。所需編隊的相對距離和角度從工作站發送給每個跟隨機器人。通信模式如圖14 所示。

圖14 通信模式

實驗1多移動機器人編隊控制實驗驗證。

利用搭建的多移動機器人實驗平臺驗證本文提出算法在實際環境中的有效性。本文設計一個5 m×5 m 的場地，構造移動機器人編隊避障仿真環境，選取3 臺機器人完成三角形隊形實驗。以各移動機器人的中心點為參考，設置領航者和跟隨者之間的期望距離為0.3 m；以領航者為參考點，設置跟隨者與領航者的期望相對角度絕對值為90°。

本文提出的編隊方案測試結果如圖15 所示。領航者速度為1 m/s，根據觀察從圖15（a）~圖15（d）編隊運動過程中的運行軌跡（黑色虛線）可知，移動機器人在行進過程中編隊隊形基本上保持不變。

圖15 基于改進領航跟隨算法的編隊運動

統計領航者與2 個跟隨者編隊運動過程的相對距離和相對角度的結果如圖16 所示。領航者與跟隨者的相對距離和角度與期望的設定值基本吻合，圖16 中出現波動的點表示編隊在進行轉彎動作時出現的正常結果。由實驗結果可以驗證基于改進領航跟隨編隊控制算法的可行性、有效性。

圖16 領航者與跟隨者相對距離和相對角度

實驗2多移動機器人避障算法實驗驗證。

本文考慮多移動機器人在未知障礙環境下的避障仿真實驗，由3 臺移動機器人構成三角編隊，由2 個尺寸為500 mm×450 mm×50 mm 的箱子作為障礙物放置在起點和目標點之間，機器人沿障礙物移動的安全距離為20 mm。

多移動機器人避障視頻截圖如圖17 所示。移動機器人在移動的過程中，不斷探測周圍環境信息，更新領航者位置，通過運動軌跡可以看出，移動機器人在運動過程中會處于局部最小值狀態，通過改進的人工勢場法，移動機器人進行沿墻運動可以擺脫最小值狀態，成功通過障礙區域到達目的地。由實驗結果可以驗證算法的可行性。

圖17 基于改進人工勢場的避障運動

4 結論

1) 本文設計一種基于吸附行為的領航跟隨算法，通過設置若干吸附區有效解決了編隊隊形發散問題，提高了隊形的穩固性。

2）提出在未知環境下移動機器人融合沿墻行為的避障策略，在傳統人工勢場法的基礎上融合基于“虛擬引導車”的沿墻行為，解決了傳統人工勢場局部極小值和目標不可達的局限性。

3）利用軟件仿真驗證算法的有效性，最后通過自主搭建的多移動機器人實驗平臺，完成未知環境下編隊避障實驗，仿真和實驗結果表明本文所提算法可以保持隊形的穩定性。