基于群集控制的多移動機器人運動控制研究

汪明慧 王秋杰

摘要：多機器人運動控制主要包括編隊、避障和向目標移動三個方面，文章研究基于群集控制的多機器人的避障和編隊協調控制方法，針對復雜障礙物環境下群集控制的局部零勢能點問題進行優化設計，并設計基于虛擬領航結構的編隊協調控制Leader-follower的編隊控制中因leader出現故障無法保持隊形的問題。通過Matlab平臺和ROS系統的Gazebo平臺進行仿真實驗驗證了算法的有效性。

關鍵詞： 多機器人系統；避障控制；編隊控制；群集控制

中圖分類號：TP399? ? 文獻標識碼：A

文章編號：1009-3044（2023）35-0006-04

開放科學（資源服務）標識碼（OSID） ：<G：＼飛翔打包文件一＼電腦2023年第三十五期＼2.00xs202335＼Image＼image9.jpeg>

0 引言

多移動機器人已廣泛應用于工農業應用管理、空間和海洋探測、環境監測、救災和國防。隨著移動機器人在各領域的發展和應用，對多移動機器人的運動協調提出了更高的要求。在眾多的運動協調算法中，一種新的協調方式——群集控制應運而生，群集控制用 “勢場”統一實現趨向目標、避障和編隊保持，對“勢場”進行精確量化又使得群集運動成為一種可度量的運動協調模式，很多研究在不斷優化群集控制理論[1-4]。Sakai等人[5]研究提出了一種不區分機器人和障礙物的群集控制算法。Gao等人[6]研究具有切換拓撲的多智能體系統的群集控制問題。Zhang[7]提出一個模型預測的群集控制方法，僅基于相鄰測量值設計，建立保證收斂到剛性群的連通性條件。Chen[8]對傳統的群集控制算法進行優化，以解決有界空間的跳躍邊界引起的問題。Valbuena[9]提出了一種適用于多移動機器人的多功能控制律，控制器可以同時集成群集、編隊控制和路徑跟蹤。Dong [10]從二階系統的角度分析多智能體在群集運動過程中的穩定性問題。Chen 等人[11-12]則運用動態圖理論研究群集控制模型的收斂性問題。Yazdani 等人[13]證明了群集控制在多機器人保持編隊和路徑跟蹤上取得較好的控制效果，Hung [14] 研究有領航者的多智能體群集運動編隊控制。

本文采用群集控制方法來實現多機器人的群集運動，將針對復雜障礙物環境下機器人運動存在的兩個問題進行優化設計，增強多機器人群集系統的環境自適應能力。其一，當機器人運動環境障礙物密集度程度增加，有復雜凹型障礙物，群集控制會出現“勢場陷阱”問題。其二，采用Leader-follower編隊方式的多機器人可以通過群集控制進行編隊并保持隊形向目標前進，但如果領航者機器人出現故障，后面以它為參考點的跟隨者機器人也將掉隊，編隊無法繼續保持。本文將提出群集控制的優化模型解決這兩個問題。

1 多機器人群集控制模型

群集控制是假設在各個機器人的周圍存在一個分布三個區域的勢場，這三個區域分別為排斥區域[Rr]、一致區域[Rm]和吸引區域[Ra]，且各個機器人的區域范圍一致。當機器人A處于機器人B的排斥區域[Rr]時，機器人A將會受到來自機器人B的斥力，由于各機器人劃分的區域一致，所以機器人受到的力將是相互的，即機器人B也會受到來自機器人A的斥力；同理，若機器人處于吸引區域[Ra]則會相互受到引力；而處于一致區域[Rm]則彼此不受到力的影響。每個機器人的勢場根據隊形的位置確定，當期望隊形發生改變，勢場也會隨之發生改變。該勢場能有效地保證各機器人保持一個合適的距離，機器人形成的隊形將會十分穩定。但是有時機器人可能會因為彼此牽扯而導致整體隊伍無法前進。本文將通過引入虛擬領航者的方式解決，同時虛擬領航者還可以解決領航者出現狀況導致跟隨者們跟不上領航者的問題。本文設計的虛擬領航者同樣存在勢場，但它不會受到其他力的影響，只會受到目標位置的引力影響并對其他機器人產生力。

多機器人隊伍有[N]個機器人成員，則用[P=（p1，p2，...，pi，...，pN）]和[V=（v1，v2，...，vi，...，vN）]分別表示[N]個機器人成員的位置向量和速度向量。其中，[vi]應滿足如下約束條件：

[vi=vi，? vi≤VmaxVmaxvivi，? vi>Vmax] （1）

式（1）中，[vi]表示第[i]個機器人的速度的模，或者說步長；[Vmax]則表示機器人的最大速度。

當機器人[i]和機器人[j]之間的距離為[δij]時，兩個機器人所受到的引力和斥力相等，則[δij]稱為機器人[i]和機器人[j]之間的平衡距離。因此，當各機器人間的距離等于平衡距離時，它們彼此的相互作用力為零。

在沒有外部環境干擾的情況下，多機器人隊伍中，跟隨者[i]只會受兩種力的影響：一是來自虛擬領航者；二是來自其他跟隨者。所受力的大小與跟隨者[i]和其他跟隨者或者虛擬領航者之間的距離和相對位置有關。根據群集控制的避碰與聚集原則，可建立關于[vi]的速度方程如下：

[vi=gipi-pL+j=1，j≠iNgijpi-pj，? i=1，...，N] （2）

則根據[vi=pi]可定義如下機器人i的運動方程：

[pi=gipi-pL+j=1，j≠iNgijpi-pj，? i=1，...，N] （3）

在式（3）中，[pi∈Rn]，[pj∈Rn]，[pL∈Rn]分別表示機器人[i]，[j]及虛擬領航者的位置；函數[gi?]為虛擬領航者與機器人[i]的關系函數，函數[gij?]為機器人[i]機器人[j]的關系函數。

對于[gi?]和[gij?]的構造，其表達式如下：

[gipi-pL=-aipi-pLpi-pL1-δipi-pL] （4）

[gijpi-pj=-aipi-pjpi-pj1-δijpi-pj] （5）

在式（4）和式（5）中，[ai∈R+]表示速度參數，速度參數是一個用來調節人工勢場函數所產生的力的大小的參數，會對隊伍的大小產生影響。它能靈活地調節機器人[i]的勢場函數的強度；[pi-pL]表示機器人[i]與虛擬領航者之間的實際距離；[δi∈R+]表示機器人[i]與虛擬領航者之間的平衡距離。當[pi-pL>δi]即[gipi-pL<0]時，虛擬領航者對機器人[i]的勢場力表現為引力；相反，當[pi-pL<δi]即[gipi-pL>0]時，虛擬領航者對機器人[i]的勢場力表現為斥力。函數[gij?]與[gi?]同理。

2 多機器人避障控制

在多機器人的群集運動過程中，由群集內聚性運動分析和聚合群集內成員行為的分析設計出群集控制律，可以使群集機器人實現基本的避碰避障。但對于復雜障礙物，還需要有針對性的避障策略才能解決。本文重點研究機器人在包含凹型障礙物的復雜環境運動時，機器人進入勢場的“陷阱”的解決方案。

多機器人在有復雜障礙物的環境下進行運動是采用一種利用勢場原理的虛擬物理力方式進行模擬，主要分為兩個方面：一方面是集群內部個體間的相互作用；另一方面是附近環境對各個機器人的影響作用，即環境障礙對智能體個體產生的排斥作用和目標位置對個體產生的吸引作用。勢場法是一種應用廣泛的借助虛擬物理力的路徑規劃方法，將目標位置看成勢能低點，將環境障礙勢能高點，機器人在運動過程中，會受到目標位置產生的引力以及環境障礙產生的斥力。令引力和斥力的合力方向作為對象的前進方向，則可以實現機器人既向目標位置移動又遠離障礙的避障效果。這些引力和斥力，是一種虛擬物理力。

智能體與目標位置之間的歐幾里得距離決定了引力的大小。相對于傳統勢場，人工勢場中目標位置的勢能較低，引力勢能的大小與智能體和目標位置的距離呈正相關。當距離越大時，智能體所受的引力作用越大，否則越小。[Pr=（xr， yr）]表示機器人的位置信息，[Po=（xo， yo）表示環境障礙的位置信息，Pg=（xg， yg）]表示目標點的位置信息。目標位置對機器人產生的引力場為[Uatt]，障礙物對機器人產生的斥力場為[Urep]。引力勢場函數表示為：

[UattP=0.5λ1Pr-Pg2] （6）

[式（6）中，λ1]為引力增益系數；[Pr-Pg]為智能體與目標位置的歐幾里得距離。

對勢場函數求負梯度，得引力[Fatt]：

[FattPr=-gradUattPr=-λ1Pr-Pg] （7）

環境障礙將會對智能體造成一個與相對位置成反比的斥力，即智能體所受到的排斥作用將隨著距離的減小而增大，反之則越小。斥力勢場函數定義為：

[UrepPr=0.5λ2（1Pr-Po-1ρ0）2，? Pr-Po<ρ00，? Pr-Po≥ρ0] （8）

式（8）中，[λ2]為斥力增益系數；[Pr-Po]為機器人與環境障礙的歐幾里得距離；[ρ0]為環境障礙對機器人產生作用的影響半徑。當機器人與環境障礙間的距離超出影響半徑[ρ0]時，機器人不會受到來自環境障礙的斥力作用。

同理，對斥力勢場函數求解負梯度斥力[Frep]的表達式為：

[FrepPr=-grad[UrepPr]=λ2Pr-Po2（1Pr-Po-1ρ0），? Pr-Po<ρ00，? Pr-Po≥ρ0] （9）

當機器人周圍有n個障礙物時，機器人所受到的合力[Ftotal]的表達式為：

[Ftotal=FattPr+i=1nFrepiPr] （10）

合力[Ftotal]的方向即為機器人的運動方向，通常情況下能順利躲避環境障礙抵達目標位置。

但勢場法具有如下一些局限性：

1） 需要實時計算。使用人工勢場法作為路徑規劃方法時，機器人需要根據自身與障礙物和目標點的距離來實時計算所受到的合力。

2） 局部極小值。由式（10）可得，機器人的前進方位與所受合力方向有關。假如機器人受到的斥力合力與引力共線且方向相反時，機器人可能會出現停滯或振蕩的情況。

3） 無法抵達目標位置。由式（7）可得，當智能體與目標位置之間的相對位置越近，目標位置造成的吸引作用力越小；而式（9）則表明當智能體與環境障礙之間的相對位置越近，環境障礙造成的排斥作用力將越大。假如在目標位置旁邊有障礙且在障礙影響范圍內時，機器人所受到的排斥作用可能將大于吸引作用，造成機器人因振蕩或停滯而難以抵達目標位置。

這些場景機器人都易發生停滯或振蕩現象，對人工勢場來說是一個勢能極小點，一般對此稱機器人落入了“勢場陷阱”。解決方法可以通過打破引力和斥力在方向和數值上的平衡。本文采用對斥力進行偏轉的方法來使機器人逃離“勢場陷阱”。

當機器人所受斥力[Frep]和引力[Fatt]的夾角形成一個較大的鈍角或平角時，就存在陷入“勢場陷阱”的可能，若將斥力[Frep]的方向偏轉一個角度[θ]，使[F]與[Fatt]之的夾角小于直角，則機器人可脫離“勢場陷阱”。[θ]是[Frep]的偏轉角度，[α]是目標位置到環境障礙與目標位置到機器人的連線夾角，[β]是環境障礙到機器人與目標位置到機器人的連線夾角，[l]是機器人到環境障礙的距離，[Frep]經過偏轉得[F'rep]，使合力由[F]變換為[F']。

偏轉角[θ]的大小主要與角[β]和距離[l]有關，其表達式為：

[d=l?sin（β）] （11）

[θ=θ0-μ?d?l] （12）

式（12）中，設常數[θ0]為90°，μ是偏轉角調整系數，其值為[θ0ρ02]。當[d]不變，距離減小或距離不變，[d]減小時，偏轉角度變大。

目標不可達問題的主要原因是目標位置附近的環境障礙給機器人提供的排斥力大于機器人所受的引力。如果目標位置被環境障礙包圍，在環境障礙的斥力勢場籠罩范圍內，即使存在能容納機器人通過的空間，機器人也難以通過。

假如在保證安全的前提下，斥力增益系數隨機器人與環境障礙的距離增加而遞減，那么機器人將能夠抵達被環境障礙斥力勢場包圍的目標位置。另外，對于機器人駛過的障礙以及目標位置之后的障礙，可以不必再給機器人提供斥力。因此，本文對斥力增益系數[λ2]進行調整，表達式如下：

[λ2=0，? β≥90° or α≥90°λ0Sr，? β<90° and α<90°] （13）

式（13）中，[λ0]是原系數基數，[S]是機器人與環境障礙之間的距離，[r]是非負實數。

3 多機器人編隊協調控制

由于群集控制的編隊算法一直保持剛性結構缺乏靈活性，本文設計基于虛擬領航結構的多機器人編隊控制算法。由于輪式機器人到達設定的虛擬結構位置所需路程不一定達到最優，使得編隊的形成較慢，且需要更多的時間。本文采用任務指定方法對各機器人在虛擬結構中的位置進行分配。步驟如下：

1） 分別計算第[j]個機器人到[j]個虛擬節點的距離，形成距離矩陣Q。

[Q=q11q12…q1nq21q22…q2n??…?qn1qn2…qnn]? ? ? ?（14）

其中[qij]為機器人[i]到虛擬節點[j]的距離，即

[qij=（xi0-xj）2+（yi0-yj）2]? ? （15）

2） 在矩陣 Q中找到最小的元素 [Q（ij）]，即虛擬點[j]為第[i]個機器人的預定節點。

3） 刪除 Q 矩陣中的第[i]行第[j]列，得到新的矩陣，反復循環，直到 Q矩陣內只剩下一個元素。最終可以得到各機器人與虛擬節點的對應關系。

虛擬領導者和虛擬節點作為領導層，虛擬領導者進行路徑規劃。運動過程中，跟隨機器人在虛擬節點的引力、障礙物的斥力和隊形力的合力下，向著各自期望的虛擬領導者，即對應的虛擬節點移動，以提高編隊避障中各機器人的靈活性。

虛擬領導者以自身為參考點，生成期望的虛擬結構并在運動過程中形成和維持隊形；跟隨機器人以期望的虛擬結構中各參考點為目標移動，最終形成相應的隊形。移動過程中，障礙物較少情況下多機器人進行編隊移動，在較多障礙物情況下，每個機器人作為個體使用人工勢場法進行避障，同時在虛擬結構的引導下盡量保持隊形。

當動態障礙物向多機器人集群發起碰撞時，處于leader位置的領航機器人會因遭到碰撞而失聯，無法繼續帶領剩余的機器人前行，此時每個機器人會作出相應的障礙物檢測，通過比較各個檢測結果選出當前的最佳虛擬領航者來帶領整個隊伍前行。

本文將虛擬結構和leader-follower的結合。虛擬結構的特點是將多個機器人的位姿記錄起來，視作一個剛性結構，突出整個機器集群的隊形，放大化了整個隊伍的隊形反饋機制，但是其缺少靈活性，在某些環境下隊形的維持需要依靠大量的控制指令，造成較大的資源浪費。通過領航者-跟隨者與虛擬結構相結合，將圖拓撲與各個節點反饋控制協議交互，完成多機器人的協同控制與容錯控制，提高復雜環境下的容錯率和靈活性。

編隊控制中主要通過位置控制、位移控制、距離控制來實現隊形的控制。本文采用位移控制方案，通過領航者跟隨者以及分布式控制實現多機器人的編隊控制，核心思想為計算出編隊隊形時各機器人的所在位置，從而計算出領航[leader]和跟隨者[followers]的相對位置，控制隊形穩定關鍵在于控制多機器人之間的速度[V]、角度[ω]；其中編隊系統的一階連續模型表示為[Xi=Ui]，其中[Xi]和[Ui]屬于[Rn]，[n]表示狀態量的維度，[Xi]表示節點i的狀態量，[Ui]表示節點i輸入量，通過上文圖論知識我們可得當且僅當拓撲圖G只具有一棵生成樹時，系統才可以達到一致性。采用一致性算法表示為

[Ui（K）=?j?Niaij（Xj（k）-Xi（k）-rij（k））]? （16）

其中令[?]> 0，機器人[i]和機器人[j]之間的相對位置表示為[rij]，[k]表示為在某一時刻k。在機器群集剛開始前行時，默認機器人0作為領航者[leader]，通過控制領航者與目標點之間的距離、領航者速度、領航者入度大小實現整個隊伍的前進，領航者0的一致性控制算法為

[U（k）=q+tC（k）+i?Niarij（k）]，[q]和[t]為常數系數。 （17）

4 仿真實驗

在MATLAB環境下對前面設計的控制算法進行仿真實現。如圖1（a） ，黑色不規則物體為凹型障礙物，此環境是容易造成勢場陷阱導致機器人無法逃離，在優化后的群集控制算法下，機器人能逃離勢場陷阱。仿真圖中不同顏色的圓圈代表不同機器人個體，不同顏色的曲線代表各機器人的運動軌跡。可以看出，本文設計的優化模型能解決傳統群集控制存在的勢場陷阱問題。如圖1（b） ，是檢驗本文設計的基于虛擬結構體的一致性編隊協調算法，虛擬領導者進行路徑規劃，跟隨機器人在虛擬節點的引力、障礙物的斥力和隊形力的合力下，向著虛擬領導者，即對應的虛擬節點移動，此設計提高編隊避障中各機器人的靈活性。

本文還在ROS環境對基于群集控制的多機器人運動控制算法進行仿真實驗驗證算法的有效性，仿真結果如圖2。圖2左邊為gazebo的仿真可視區，右邊為啟動命令終端。仿真顯示優化后的群集控制算法能使所有的機器人移動在一起，避免碰撞并向目標移動。

5 結束語

本文研究基于群集控制的多機器人的避障控制和編隊協調控制。針對群集控制會出現的“勢場陷阱”問題，設計了斥力偏轉策略解決人工勢場下局部零勢能點問題。并針對Leader-follower的編隊控制會因leader出現故障編隊無法繼續保持的問題，采用基于虛擬領航結構的編隊協調控制算法，通過領航者-跟隨者與虛擬結構相結合，將圖拓撲與各個節點反饋控制協議交互，提高復雜環境下的容錯率和靈活性。通過仿真實驗驗證了算法的有效性。

參考文獻：

[1] Valbuena Reyes L A，Tanner H G.Flocking，formation control，and path following for a group of mobile robots[J].IEEE Transactions on Control Systems Technology，2015，23（4）：1268-1282.

[2] Saulnier K，Saldana D，Prorok A，et al.Resilient flocking for mobile robot teams[J].IEEE Robotics and Automation Letters，2017，2（2）：1039-1046.

[3] 毛昱天，陳杰，方浩，等.連通性保持下的多機器人系統分布式群集控制[J].控制理論與應用，2014，31（10）：1393-1403.

[4] 董慧.基于偏微分方程約束的機器人群集運動控制系統設計[J].計算機測量與控制，2023，31（4）：130-135.

[5] Sakai D，Fukushima H，Matsuno F.Flocking for multirobots without distinguishing robots and obstacles[J].IEEE Transactions on Control Systems Technology，2017，25（3）：1019-1027.

[6] Gao J Y，Xu X，Ding N，et al.Flocking motion of multi-agent system by dynamic pinning control[J].IET Control Theory & Applications，2017，11（5）：714-722.

[7] Zhang H T，Liu B，Cheng Z M，et al.Model predictive flocking control of the cucker-smale multi-agent model with input constraints[J].IEEE Transactions on Circuits and Systems I：Regular Papers，2016，63（8）：1265-1275.

[8] Z. Chen， H. T. Zhang， M. C. Fan， D. Wang and D. Li.Algorithms and experiments on flocking of multiagents in a bounded space[J].IEEE Transactions on Control Systems Technology，2014，22（4）：1544-1549.

[9] Valbuena Reyes L A，Tanner H G.Flocking，formation control，and path following for a group of mobile robots[J].IEEE Transactions on Control Systems Technology，2015，23（4）：1268-1282.

[10] Dong W J.Flocking of multiple mobile robots based on backstepping[J].IEEE Transactions on Systems，Man，and Cybernetics，Part B （Cybernetics），2011，41（2）：414-424.

[11] Chen H Y，Wang C，Lou Y J.Flocking multiple microparticles with automatically controlled optical tweezers：solutions and experiments[J].IEEE Transactions on Biomedical Engineering，2013，60（6）：1518-1527.

[12] Wang Z Y，Gu D B.Cooperative target tracking control of multiple robots[J].IEEE Transactions on Industrial Electronics，2012，59（8）：3232-3240.

[13] Yazdani S，Haeri M.Flocking of multi-agent systems with multiple second-order uncoupled linear dynamics and virtual leader[J].IET Control Theory & Applications，2016，10（8）：853-860.

[14] Hung S M，Givigi S N.A Q-learning approach to flocking with UAVs in a stochastic environment[J].IEEE Transactions on Cybernetics，2017，47（1）：186-197.

【通聯編輯：朱寶貴】