一種領航-跟隨型多移動機器人編隊控制方法*

王蓀馨，王經國

(1.西安理工大學信息技術與裝備工程學院，西安 710082；2.烽火通信科技股份有限公司，武漢 430205)

0 前言

多移動機器人編隊是多無人系統技術領域的一個重要分支，在聯合偵查、群體盯梢、協作救援、合作搬運、傳感網絡等軍事和民用領域具有廣闊的應用背景。多移動機器人編隊是多臺移動機器人自主協作組成一個較為精準的編隊或構型以完成特定的作業任務，并在執行作業任務、目標跟蹤、躲避障礙物時盡可能保持預期的編隊幾何隊形不變。由于受傳感器感知范圍、單體作業半徑、設備搭載能力、通訊距離等因素制約，在執行大范圍、長距離、動態復雜等環境下的復雜作業任務時，傳統單臺機器人難以勝任。因此，多移動機器人編隊則成為解決這類復雜問題的有效途徑。

目前，多移動機器人編隊控制方法主要包括：行為法(Behavior-based)、虛擬結構法(Virtual-structure)和領航-跟隨法(Leader-follower)。基于行為的多機編隊方法無法準確描述系統整體行為，且不能保證系統控制的穩定性；而虛擬結構法則存在系統靈活性不足的缺陷；領航-跟隨型編隊控制法[1]具有數學分析簡單、易保持隊形、通信壓力小等優點，被廣泛應用于多無人系統編隊，如：野外作戰系統編隊、無人機編隊和水下艦船編隊等諸多應用領域。

領航-跟隨(leader-follower)型編隊控制方法在具體執行多機編隊作業任務時，領航機器人(Leader)負責目標跟蹤、路徑規劃、主動避障等任務，而跟隨機器人(follower)負責主動跟隨領航者的運動軌跡，并盡可能保持預定的理想編隊幾何隊形。文獻[2]將多機編隊控制問題劃分為軌跡跟蹤、機器人控制和隊形保持3個子問題，通過控制相對距離-相對角度、相對距離-相對距離來實現多機編隊控制。文獻[3]為了自主形成期望的編隊隊形，采用一種綜合的軌跡跟蹤控制方法來確保系統跟蹤誤差穩定。針對復雜環境下的領航-跟隨型編隊控制問題，文獻[4]采用模糊控制方法來確保多機編隊系統自主避障和保持隊形，文獻[5]提出了一種模糊邏輯控制方法來對非完整輪式移動機器人進行隊形控制，文獻[6]提出了一種綜合運動學和計算力矩的輸出反饋控制率來完成預期幾何隊形的自主控制。

綜上所述，在領航-跟隨型編隊控制研究領域，現有研究較好地解決了確定性目標和靜態環境下的編隊控制、軌跡跟蹤等問題，但針對未知復雜環境條件下的多機編隊研究甚少，特別是領航機器人如何在線進行路徑規劃、實時自主避障等方面亟待進行更深入研究。為此，本文針對在未知復雜環境條件下領航-跟隨型編隊控制中的路徑規劃、編隊形成和隊形保持3類問題，采用人工勢場法進行在線局部路徑規劃，運用滑膜控制方法自主調節跟隨機器人的實時位姿，以確保在無碰撞的條件下完成從初始位置到目標位置的多機編隊運動。

1 領航-跟隨型編隊控制問題及其解決方案

1.1 領航-跟隨型編隊控制問題

在領航-跟隨型編隊控制問題中，選定一臺機器人作為領航者，主要負責整個編隊的路徑規劃任務，而其余機器人則被視為跟隨者。跟隨機器人負責實施跟蹤領航者，并盡可能與領航機器人之間保持隊形所需的距離和角度，從而確保整個多機編隊按照預期的理想編隊隊形進行無碰撞運動，并最終到達目標位置，其運動場景如圖1所示。

圖1 領航-跟隨型多機編隊問題場景

1.2 領航-跟隨型編隊控制的整體解決方案

針對領航-跟隨型編隊控制問題，本文提出了圖2所示的整體解決方案。該解決方案包括3個主要階段：

(1)初始化階段：給定多機編隊隊形任務，確定領航機器人和跟隨機器人角色，確定各機器人的初始位姿，建立領航-跟隨機器人編隊運動學模型。

(2)領航機器人在線路徑規劃：在任意i時刻，領航機器人依據人工勢場法計算i+1時刻的自身位姿和跟隨機器人的理想位姿；

(3)跟隨機器人軌跡跟蹤：在i+1時刻，各跟隨機器人依據給定的理想位姿，運用滑膜運動控制器實時調節該時刻機器人的線速度和角速度，從而確保當前時刻跟隨機器人保持預期幾何隊形并跟隨領航機器人進行編隊運動。

圖2 領航-跟隨型編隊控制整體解決方案

2 領航-跟隨型編隊控制的運動學模型

2.1 輪式移動機器人的運動學模型

本文采用輪式移動機器人，而輪式移動機器人的無滑動滾動特征決定了系統滿足非完整約束條件。因此，對于具有非完整性約束的雙輪移動機器人，其運動學模型如圖3所示，表達式為

圖3 雙輪驅動的移動機器人運動學模型示意圖

式中，(xc,yc)是機器人的中心坐標；θ是車身軸線與x軸的夾角；(v,ω)T為移動機器人的2個主要控制參數，其中，v、ω分別是機器人的線速度和角速度。

2.2 領航-跟隨型編隊控制的運動學模型

以單機運動學模型為基礎，圖4給出了包含三角形編隊隊形、三臺機器人、雙輪移動機器人的領航-跟隨型編隊控制運動學模型。在領航-跟隨型編隊控制問題中，編隊的主軌跡由領航機器人自主規劃確定，隊形形成和保持則由跟隨機器人的期望位姿參數來確定。

圖4 領航-跟隨型編隊控制的運動學模型示意圖

圖4中，R1為領航機器人，R2和R3為2臺跟隨機器人，而Rv則是R2為了形成和保持預期隊形而假設的虛擬機器人；R1和Rv之間期望的距離和方位角為(ρ,φ)T。結合R1的位姿并經幾何坐標變換，即可得到Rv的期望位姿。

式中，R1位姿為(x1,y1,θ1)T，R2期望位姿為(xv,yv,θv)T。

在領航-跟隨編理想控制條件下，Rv和R1的線速度和角速度應該保持相等(即vv=v1，ωv=ω1)。而R2的跟蹤位姿誤差表示為

(xe,ye,θe)T=(x-xv,y-yv,θ-θv)T

3 基于改進型人工勢場法的領航機器人在線局部路徑規劃

在領航-跟隨型編隊控制中，首先需要解決承擔整個編隊引領任務的領航機器人的路徑規劃問題，即：領航機器人進行在線局部路徑規劃，引導其后的跟隨機器人，形成和保持預期的編隊隊形，從起點到目標點進行無碰撞的編隊運動，從而完成既定的作業任務。

人工勢場法將機器人的實際運動空間抽象為物理中的勢場，機器人在該勢場中同時受目標引力場和障礙物斥力場的共同作用。通過人工勢場法這類在線局部路徑規劃方法，可以引導整個多機編隊在運動過程中實時躲避障礙物并順利抵達目標位置。因此，相對于目前大多數路徑規劃算法，由于人工勢場法具有結構簡單、依賴信息少等優勢，特別是該方法在避障方面的獨特優勢，使得非常適合未知復雜環境下的在線局部路徑規劃任務。然而，人工勢場法所具有的引力和斥力疊加效應使得其存在“目標不可達”的弊端[7]。

為此，針對領航機器人在復雜未知環境下的在線路徑規劃需求，以及人工勢場法自身存在的目標不可達問題，本文在標準人工勢場法[7]的基礎上，提出了一種改進型人工勢場法如圖5所示。改進型人工勢場法使得目標點成為全局勢場中的最小點，而斥力場在目標位置附近盡可能趨近于零。

圖5 改進型人工勢場法示意圖

領航機器人R1在外部環境中受到的總勢場為Utotal(X)=Uatt(X)+Urep(X)，而領航機器人在總勢場中對應的合力Ftotal(X)為Ftotal(X)=Fatt(X)+Frep(X)。

在運用人工勢場法進行在線路徑規劃時，領航機器人根據Ftotal(X)的方向和自身的行駛速度v，來計算下一時刻領航機器人和跟隨機器人的軌跡坐標，從而引導整個編隊進行無碰撞運動。以下結合圖5對總勢場計算和合力計算2個公式中涉及的變量進行詳細解釋。

在圖5中，領航機器人R1的位置表示為X=(x,y)，則R1和目標之間的引力勢函數為

式中，Xg為目標位置，(X-Xg)為機器人R1和目標之間的距離；Katt為引力位置增益系數，則領航機器人在引力場中受到目標的引力為Fatt(X)=-Uatt(X)=-katt|X-Xg|。

考慮到機器人與目標之間的相對距離，R1和障礙物之間的斥力勢函數為

式中，(X-Xg)為機器人和目標之間的相對距離；(X-X0)為機器人和障礙物之間的距離；η是斥力位置增益系數；ρ0是障礙物的影響距離；n為任意實數。

與標準人工勢場法中的斥力勢函數相比較，這里的改進型人工勢場法的斥力函數計算利用了機器人與目標之間的相對距離，即：當機器人接近目標點時，(X-Xg)逐漸變小，相應的斥力場也隨之減小，從而保證了整個勢場僅在目標點Xg最小。因此，機器人在勢場中對應的斥力計算為

Frep(X)=-

其中，

4 基于滑膜運動控制的跟隨機器人軌跡跟蹤方法

通過基于人工勢場法的領航機器人在線路徑規劃，即可得到整個編隊的運動軌跡。編隊系統的主要任務是設計各跟隨機器人的運動控制器，依此引導跟隨機器人按照預期的理想位姿和領航機器人形成并保持既定的編隊隊形。在多機編隊軌跡跟蹤控制器設計方法中，目前主要包括：自適應控制法、非線性狀態反饋控制法、反步控制法和滑膜控制法等。與其他控制方法相比較而言，滑膜控制方法中的滑膜模態設計和系統參數與擾動無關，而且具有較好的魯棒性[8]。因此，本文引入滑膜運動控制器來執行跟隨機器人的軌跡跟蹤控制任務。

在圖4所示的領航-跟隨型編隊控制運動學模型中，采用人工勢場法可以確定出領航機器人的位姿(x,y,θ)，為了形成和保持預期編隊隊形，各跟隨機器人需要實時保持和領航機器人之間的距離l和方位角φ。而通過跟隨機器人的運動控制器來實時調節其線速度和角速度(v,ω)T，從而盡可能確保各個跟隨機器人的實際位姿與其理想位姿的誤差趨近于零。針對跟隨機器人的軌跡跟蹤控制器設計問題，結合所提出的領航-跟隨型編隊控制的運動學模型，采用基于雙冪指數趨近率的滑膜運動控制器為

這里仍然結合圖4對滑膜運動控制器進行說明。假設在i時刻，領航機器人的位姿為(x1,y1,θ1)，由于跟隨機器人與領航機器人應保持距離l和觀測角φ，經過坐標變換即可求得跟隨機器人在全局坐標系下的理想位姿(xv,yv,θv)為

跟隨機器人的理想位姿對應的虛擬機器人的運動學方程為

依據文獻[9]，跟隨機器人R2從位姿(x2,y2,θ2)T移動到虛擬機器人位姿(xv,yv,θv)T的位姿誤差方程為

通過反步法并結合Lyapunov定理，求得切換函數為

根據雙冪次趨近控制率，使得s1→0，s2→0，實現xe收斂到0且θe收斂到-arctan(vrye)，從而實現ye→0，θe→0。根據文獻[8]，求得跟隨機器人R2的滑膜運動控制器為

則在i+1時刻，根據滑膜運動控制器求得跟隨機器人R2的位姿(x2,i+1,y2,i+1,θ2,i+1)T為

x2,i+1=x2,i+t·v2,i·cos(θ2,i+1+t·ω2,i)

y2,i+1=y2,i+t·v2,i·sin(θ2,i+1+t·ω2,i)

θ2,i+1=θi+t·ω2,i

其中，t為時刻i和時刻i+1的時間間隔。

依據該計算方法，同理可計算得到的跟隨機器人R3的滑膜運動控制器。

綜上所述，在領航-跟隨型編隊控制問題中，領航機器人基于改進型人工勢場法進行在線路徑規劃，從而確定出從起點到目標點的無碰撞路徑；編隊中各個跟隨機器人在滑膜運動控制器的自主調節下促使多機形成并保持既定編隊隊形，并以此隊形運動至目標點。

5 領航-跟隨型編隊控制方法的仿真實驗

為了驗證該領航-跟隨型編隊控制方法的有效性，實驗選用三角形編隊隊形、3臺移動機器人，在兩類實驗場景條件下，運用MATLAB軟件對編隊控制方法中的在線路徑規劃和軌跡跟蹤控制方法進行仿真驗證。

(1)實驗場景1。在無障礙物環境下，領航

機器人R1采用基于改進型人工勢場法進行在線局部路徑規劃，兩臺跟隨機器人R2和R3采用滑膜運動控制器。三臺機器人從起始點到目標點進行三角形編隊運動。仿真實驗結果如圖6～圖8所示。

從圖6可知，三臺機器人從初始的隨機狀態，經過滑膜運動控制器對兩臺跟隨機器人的運動控制，快速形成了預期的三角形編隊隊形，兩臺跟隨機器人自主跟隨領航機器人以穩定的編隊隊形從起始點順利到達了目標點。另外，從圖7和圖8可知：兩臺跟隨機器人能快速移動到理想位姿，并以較小運動誤差(接近于0)保持編隊隊形進行整體移動。

圖6 實驗場景1環境下領航-跟隨編隊在線路徑規劃實驗結果

圖7 實驗場景1環境下跟隨機器人R2的線速度、角速度和運動誤差

圖8 實驗場景1環境下跟隨機器人R3的線速度、角速度和運動誤差

(2)實驗場景2。在靜態障礙物環境下，運用本文提出的在線路徑規劃和滑膜運動控制器進行編隊實驗。實驗結果如圖9～圖11所示。

從圖9可知，在障礙物環境下，領航機器人仍能規劃出一條無碰撞路徑，并帶領2臺跟隨機器人以較小的隊形誤差從起始點順利抵達目標點。從圖10和11實驗結果可知：由于滑膜運動控制器的實時調節機器人的位姿，在路線中存在較多障礙物的環境下，仍能通過運動控制器的自我調姿，并以較小的運動誤差保持預期的編隊隊形。

圖9 實驗場景2環境下領航-跟隨編隊在線路徑規劃實驗結果

圖10 實驗場景2環境下跟隨機器人R2的線速度、角速度和運動誤差

圖11 實驗場景2環境下跟隨機器人R3的線速度、角速度和運動誤差

6 結論

本文將改進型人工勢場法和滑膜運動控制有機融合，提出了一種一體化、集成化的領航-跟隨型編隊控制方法，有效解決了領航機器人的在線路徑規劃和跟隨機器人的軌跡跟蹤兩類問題，最后的兩類工況仿真實驗也驗證了該方法的可行性和有效性。這種集成了在線路徑規劃和軌跡跟蹤的一體化多機編隊控制方法，有助于實現多移動機器人編隊成形、隊形保持和在線避障等作業任務。該方法在多無人系統自主編隊、多移動機器人協作搬運、智慧物流等領域具有潛在應用價值。后續將以集成化的領航-跟隨型編隊控制方法為核心，設計制作移動機器人實物，并在接近實際工況環境下進行實證研究。