汪明慧 王秋杰

摘要：多機(jī)器人運(yùn)動(dòng)控制主要包括編隊(duì)、避障和向目標(biāo)移動(dòng)三個(gè)方面，文章研究基于群集控制的多機(jī)器人的避障和編隊(duì)協(xié)調(diào)控制方法，針對(duì)復(fù)雜障礙物環(huán)境下群集控制的局部零勢(shì)能點(diǎn)問(wèn)題進(jìn)行優(yōu)化設(shè)計(jì)，并設(shè)計(jì)基于虛擬領(lǐng)航結(jié)構(gòu)的編隊(duì)協(xié)調(diào)控制Leader-follower的編隊(duì)控制中因leader出現(xiàn)故障無(wú)法保持隊(duì)形的問(wèn)題。通過(guò)Matlab平臺(tái)和ROS系統(tǒng)的Gazebo平臺(tái)進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證了算法的有效性。

關(guān)鍵詞： 多機(jī)器人系統(tǒng)；避障控制；編隊(duì)控制；群集控制

中圖分類號(hào)：TP399? ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2023）35-0006-04

開(kāi)放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID） ：<G：＼飛翔打包文件一＼電腦2023年第三十五期＼2.00xs202335＼Image＼image9.jpeg>

0 引言

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

本文采用群集控制方法來(lái)實(shí)現(xiàn)多機(jī)器人的群集運(yùn)動(dòng)，將針對(duì)復(fù)雜障礙物環(huán)境下機(jī)器人運(yùn)動(dòng)存在的兩個(gè)問(wèn)題進(jìn)行優(yōu)化設(shè)計(jì)，增強(qiáng)多機(jī)器人群集系統(tǒng)的環(huán)境自適應(yīng)能力。其一，當(dāng)機(jī)器人運(yùn)動(dòng)環(huán)境障礙物密集度程度增加，有復(fù)雜凹型障礙物，群集控制會(huì)出現(xiàn)“勢(shì)場(chǎng)陷阱”問(wèn)題。其二，采用Leader-follower編隊(duì)方式的多機(jī)器人可以通過(guò)群集控制進(jìn)行編隊(duì)并保持隊(duì)形向目標(biāo)前進(jìn)，但如果領(lǐng)航者機(jī)器人出現(xiàn)故障，后面以它為參考點(diǎn)的跟隨者機(jī)器人也將掉隊(duì)，編隊(duì)無(wú)法繼續(xù)保持。本文將提出群集控制的優(yōu)化模型解決這兩個(gè)問(wèn)題。

1 多機(jī)器人群集控制模型

群集控制是假設(shè)在各個(gè)機(jī)器人的周圍存在一個(gè)分布三個(gè)區(qū)域的勢(shì)場(chǎng)，這三個(gè)區(qū)域分別為排斥區(qū)域[Rr]、一致區(qū)域[Rm]和吸引區(qū)域[Ra]，且各個(gè)機(jī)器人的區(qū)域范圍一致。當(dāng)機(jī)器人A處于機(jī)器人B的排斥區(qū)域[Rr]時(shí)，機(jī)器人A將會(huì)受到來(lái)自機(jī)器人B的斥力，由于各機(jī)器人劃分的區(qū)域一致，所以機(jī)器人受到的力將是相互的，即機(jī)器人B也會(huì)受到來(lái)自機(jī)器人A的斥力；同理，若機(jī)器人處于吸引區(qū)域[Ra]則會(huì)相互受到引力；而處于一致區(qū)域[Rm]則彼此不受到力的影響。每個(gè)機(jī)器人的勢(shì)場(chǎng)根據(jù)隊(duì)形的位置確定，當(dāng)期望隊(duì)形發(fā)生改變，勢(shì)場(chǎng)也會(huì)隨之發(fā)生改變。該勢(shì)場(chǎng)能有效地保證各機(jī)器人保持一個(gè)合適的距離，機(jī)器人形成的隊(duì)形將會(huì)十分穩(wěn)定。但是有時(shí)機(jī)器人可能會(huì)因?yàn)楸舜藸砍抖鴮?dǎo)致整體隊(duì)伍無(wú)法前進(jìn)。本文將通過(guò)引入虛擬領(lǐng)航者的方式解決，同時(shí)虛擬領(lǐng)航者還可以解決領(lǐng)航者出現(xiàn)狀況導(dǎo)致跟隨者們跟不上領(lǐng)航者的問(wèn)題。本文設(shè)計(jì)的虛擬領(lǐng)航者同樣存在勢(shì)場(chǎng)，但它不會(huì)受到其他力的影響，只會(huì)受到目標(biāo)位置的引力影響并對(duì)其他機(jī)器人產(chǎn)生力。

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

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

式（1）中，[vi]表示第[i]個(gè)機(jī)器人的速度的模，或者說(shuō)步長(zhǎng)；[Vmax]則表示機(jī)器人的最大速度。

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

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

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

則根據(jù)[vi=pi]可定義如下機(jī)器人i的運(yùn)動(dòng)方程：

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

在式（3）中，[pi∈Rn]，[pj∈Rn]，[pL∈Rn]分別表示機(jī)器人[i]，[j]及虛擬領(lǐng)航者的位置；函數(shù)[gi?]為虛擬領(lǐng)航者與機(jī)器人[i]的關(guān)系函數(shù)，函數(shù)[gij?]為機(jī)器人[i]機(jī)器人[j]的關(guān)系函數(shù)。

對(duì)于[gi?]和[gij?]的構(gòu)造，其表達(dá)式如下：

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

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

在式（4）和式（5）中，[ai∈R+]表示速度參數(shù)，速度參數(shù)是一個(gè)用來(lái)調(diào)節(jié)人工勢(shì)場(chǎng)函數(shù)所產(chǎn)生的力的大小的參數(shù)，會(huì)對(duì)隊(duì)伍的大小產(chǎn)生影響。它能靈活地調(diào)節(jié)機(jī)器人[i]的勢(shì)場(chǎng)函數(shù)的強(qiáng)度；[pi-pL]表示機(jī)器人[i]與虛擬領(lǐng)航者之間的實(shí)際距離；[δi∈R+]表示機(jī)器人[i]與虛擬領(lǐng)航者之間的平衡距離。當(dāng)[pi-pL>δi]即[gipi-pL<0]時(shí)，虛擬領(lǐng)航者對(duì)機(jī)器人[i]的勢(shì)場(chǎng)力表現(xiàn)為引力；相反，當(dāng)[pi-pL<δi]即[gipi-pL>0]時(shí)，虛擬領(lǐng)航者對(duì)機(jī)器人[i]的勢(shì)場(chǎng)力表現(xiàn)為斥力。函數(shù)[gij?]與[gi?]同理。

2 多機(jī)器人避障控制

在多機(jī)器人的群集運(yùn)動(dòng)過(guò)程中，由群集內(nèi)聚性運(yùn)動(dòng)分析和聚合群集內(nèi)成員行為的分析設(shè)計(jì)出群集控制律，可以使群集機(jī)器人實(shí)現(xiàn)基本的避碰避障。但對(duì)于復(fù)雜障礙物，還需要有針對(duì)性的避障策略才能解決。本文重點(diǎn)研究機(jī)器人在包含凹型障礙物的復(fù)雜環(huán)境運(yùn)動(dòng)時(shí)，機(jī)器人進(jìn)入勢(shì)場(chǎng)的“陷阱”的解決方案。

多機(jī)器人在有復(fù)雜障礙物的環(huán)境下進(jìn)行運(yùn)動(dòng)是采用一種利用勢(shì)場(chǎng)原理的虛擬物理力方式進(jìn)行模擬，主要分為兩個(gè)方面：一方面是集群內(nèi)部個(gè)體間的相互作用；另一方面是附近環(huán)境對(duì)各個(gè)機(jī)器人的影響作用，即環(huán)境障礙對(duì)智能體個(gè)體產(chǎn)生的排斥作用和目標(biāo)位置對(duì)個(gè)體產(chǎn)生的吸引作用。勢(shì)場(chǎng)法是一種應(yīng)用廣泛的借助虛擬物理力的路徑規(guī)劃方法，將目標(biāo)位置看成勢(shì)能低點(diǎn)，將環(huán)境障礙勢(shì)能高點(diǎn)，機(jī)器人在運(yùn)動(dòng)過(guò)程中，會(huì)受到目標(biāo)位置產(chǎn)生的引力以及環(huán)境障礙產(chǎn)生的斥力。令引力和斥力的合力方向作為對(duì)象的前進(jìn)方向，則可以實(shí)現(xiàn)機(jī)器人既向目標(biāo)位置移動(dòng)又遠(yuǎn)離障礙的避障效果。這些引力和斥力，是一種虛擬物理力。

智能體與目標(biāo)位置之間的歐幾里得距離決定了引力的大小。相對(duì)于傳統(tǒng)勢(shì)場(chǎng)，人工勢(shì)場(chǎng)中目標(biāo)位置的勢(shì)能較低，引力勢(shì)能的大小與智能體和目標(biāo)位置的距離呈正相關(guān)。當(dāng)距離越大時(shí)，智能體所受的引力作用越大，否則越小。[Pr=（xr， yr）]表示機(jī)器人的位置信息，[Po=（xo， yo）表示環(huán)境障礙的位置信息，Pg=（xg， yg）]表示目標(biāo)點(diǎn)的位置信息。目標(biāo)位置對(duì)機(jī)器人產(chǎn)生的引力場(chǎng)為[Uatt]，障礙物對(duì)機(jī)器人產(chǎn)生的斥力場(chǎng)為[Urep]。引力勢(shì)場(chǎng)函數(shù)表示為：

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

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

對(duì)勢(shì)場(chǎng)函數(shù)求負(fù)梯度，得引力[Fatt]：

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

環(huán)境障礙將會(huì)對(duì)智能體造成一個(gè)與相對(duì)位置成反比的斥力，即智能體所受到的排斥作用將隨著距離的減小而增大，反之則越小。斥力勢(shì)場(chǎng)函數(shù)定義為：

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

式（8）中，[λ2]為斥力增益系數(shù)；[Pr-Po]為機(jī)器人與環(huán)境障礙的歐幾里得距離；[ρ0]為環(huán)境障礙對(duì)機(jī)器人產(chǎn)生作用的影響半徑。當(dāng)機(jī)器人與環(huán)境障礙間的距離超出影響半徑[ρ0]時(shí)，機(jī)器人不會(huì)受到來(lái)自環(huán)境障礙的斥力作用。

同理，對(duì)斥力勢(shì)場(chǎng)函數(shù)求解負(fù)梯度斥力[Frep]的表達(dá)式為：

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

當(dāng)機(jī)器人周圍有n個(gè)障礙物時(shí)，機(jī)器人所受到的合力[Ftotal]的表達(dá)式為：

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

合力[Ftotal]的方向即為機(jī)器人的運(yùn)動(dòng)方向，通常情況下能順利躲避環(huán)境障礙抵達(dá)目標(biāo)位置。

但勢(shì)場(chǎng)法具有如下一些局限性：

1） 需要實(shí)時(shí)計(jì)算。使用人工勢(shì)場(chǎng)法作為路徑規(guī)劃方法時(shí)，機(jī)器人需要根據(jù)自身與障礙物和目標(biāo)點(diǎn)的距離來(lái)實(shí)時(shí)計(jì)算所受到的合力。

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

3） 無(wú)法抵達(dá)目標(biāo)位置。由式（7）可得，當(dāng)智能體與目標(biāo)位置之間的相對(duì)位置越近，目標(biāo)位置造成的吸引作用力越??；而式（9）則表明當(dāng)智能體與環(huán)境障礙之間的相對(duì)位置越近，環(huán)境障礙造成的排斥作用力將越大。假如在目標(biāo)位置旁邊有障礙且在障礙影響范圍內(nèi)時(shí)，機(jī)器人所受到的排斥作用可能將大于吸引作用，造成機(jī)器人因振蕩或停滯而難以抵達(dá)目標(biāo)位置。

這些場(chǎng)景機(jī)器人都易發(fā)生停滯或振蕩現(xiàn)象，對(duì)人工勢(shì)場(chǎng)來(lái)說(shuō)是一個(gè)勢(shì)能極小點(diǎn)，一般對(duì)此稱機(jī)器人落入了“勢(shì)場(chǎng)陷阱”。解決方法可以通過(guò)打破引力和斥力在方向和數(shù)值上的平衡。本文采用對(duì)斥力進(jìn)行偏轉(zhuǎn)的方法來(lái)使機(jī)器人逃離“勢(shì)場(chǎng)陷阱”。

當(dāng)機(jī)器人所受斥力[Frep]和引力[Fatt]的夾角形成一個(gè)較大的鈍角或平角時(shí)，就存在陷入“勢(shì)場(chǎng)陷阱”的可能，若將斥力[Frep]的方向偏轉(zhuǎn)一個(gè)角度[θ]，使[F]與[Fatt]之的夾角小于直角，則機(jī)器人可脫離“勢(shì)場(chǎng)陷阱”。[θ]是[Frep]的偏轉(zhuǎn)角度，[α]是目標(biāo)位置到環(huán)境障礙與目標(biāo)位置到機(jī)器人的連線夾角，[β]是環(huán)境障礙到機(jī)器人與目標(biāo)位置到機(jī)器人的連線夾角，[l]是機(jī)器人到環(huán)境障礙的距離，[Frep]經(jīng)過(guò)偏轉(zhuǎn)得[F'rep]，使合力由[F]變換為[F']。

偏轉(zhuǎn)角[θ]的大小主要與角[β]和距離[l]有關(guān)，其表達(dá)式為：

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

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

式（12）中，設(shè)常數(shù)[θ0]為90°，μ是偏轉(zhuǎn)角調(diào)整系數(shù)，其值為[θ0ρ02]。當(dāng)[d]不變，距離減小或距離不變，[d]減小時(shí)，偏轉(zhuǎn)角度變大。

目標(biāo)不可達(dá)問(wèn)題的主要原因是目標(biāo)位置附近的環(huán)境障礙給機(jī)器人提供的排斥力大于機(jī)器人所受的引力。如果目標(biāo)位置被環(huán)境障礙包圍，在環(huán)境障礙的斥力勢(shì)場(chǎng)籠罩范圍內(nèi)，即使存在能容納機(jī)器人通過(guò)的空間，機(jī)器人也難以通過(guò)。

假如在保證安全的前提下，斥力增益系數(shù)隨機(jī)器人與環(huán)境障礙的距離增加而遞減，那么機(jī)器人將能夠抵達(dá)被環(huán)境障礙斥力勢(shì)場(chǎng)包圍的目標(biāo)位置。另外，對(duì)于機(jī)器人駛過(guò)的障礙以及目標(biāo)位置之后的障礙，可以不必再給機(jī)器人提供斥力。因此，本文對(duì)斥力增益系數(shù)[λ2]進(jìn)行調(diào)整，表達(dá)式如下：

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

式（13）中，[λ0]是原系數(shù)基數(shù)，[S]是機(jī)器人與環(huán)境障礙之間的距離，[r]是非負(fù)實(shí)數(shù)。

3 多機(jī)器人編隊(duì)協(xié)調(diào)控制

由于群集控制的編隊(duì)算法一直保持剛性結(jié)構(gòu)缺乏靈活性，本文設(shè)計(jì)基于虛擬領(lǐng)航結(jié)構(gòu)的多機(jī)器人編隊(duì)控制算法。由于輪式機(jī)器人到達(dá)設(shè)定的虛擬結(jié)構(gòu)位置所需路程不一定達(dá)到最優(yōu)，使得編隊(duì)的形成較慢，且需要更多的時(shí)間。本文采用任務(wù)指定方法對(duì)各機(jī)器人在虛擬結(jié)構(gòu)中的位置進(jìn)行分配。步驟如下：

1） 分別計(jì)算第[j]個(gè)機(jī)器人到[j]個(gè)虛擬節(jié)點(diǎn)的距離，形成距離矩陣Q。

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

其中[qij]為機(jī)器人[i]到虛擬節(jié)點(diǎn)[j]的距離，即

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

2） 在矩陣 Q中找到最小的元素 [Q（ij）]，即虛擬點(diǎn)[j]為第[i]個(gè)機(jī)器人的預(yù)定節(jié)點(diǎn)。

3） 刪除 Q 矩陣中的第[i]行第[j]列，得到新的矩陣，反復(fù)循環(huán)，直到 Q矩陣內(nèi)只剩下一個(gè)元素。最終可以得到各機(jī)器人與虛擬節(jié)點(diǎn)的對(duì)應(yīng)關(guān)系。

虛擬領(lǐng)導(dǎo)者和虛擬節(jié)點(diǎn)作為領(lǐng)導(dǎo)層，虛擬領(lǐng)導(dǎo)者進(jìn)行路徑規(guī)劃。運(yùn)動(dòng)過(guò)程中，跟隨機(jī)器人在虛擬節(jié)點(diǎn)的引力、障礙物的斥力和隊(duì)形力的合力下，向著各自期望的虛擬領(lǐng)導(dǎo)者，即對(duì)應(yīng)的虛擬節(jié)點(diǎn)移動(dòng)，以提高編隊(duì)避障中各機(jī)器人的靈活性。

虛擬領(lǐng)導(dǎo)者以自身為參考點(diǎn)，生成期望的虛擬結(jié)構(gòu)并在運(yùn)動(dòng)過(guò)程中形成和維持隊(duì)形；跟隨機(jī)器人以期望的虛擬結(jié)構(gòu)中各參考點(diǎn)為目標(biāo)移動(dòng)，最終形成相應(yīng)的隊(duì)形。移動(dòng)過(guò)程中，障礙物較少情況下多機(jī)器人進(jìn)行編隊(duì)移動(dòng)，在較多障礙物情況下，每個(gè)機(jī)器人作為個(gè)體使用人工勢(shì)場(chǎng)法進(jìn)行避障，同時(shí)在虛擬結(jié)構(gòu)的引導(dǎo)下盡量保持隊(duì)形。

當(dāng)動(dòng)態(tài)障礙物向多機(jī)器人集群發(fā)起碰撞時(shí)，處于leader位置的領(lǐng)航機(jī)器人會(huì)因遭到碰撞而失聯(lián)，無(wú)法繼續(xù)帶領(lǐng)剩余的機(jī)器人前行，此時(shí)每個(gè)機(jī)器人會(huì)作出相應(yīng)的障礙物檢測(cè)，通過(guò)比較各個(gè)檢測(cè)結(jié)果選出當(dāng)前的最佳虛擬領(lǐng)航者來(lái)帶領(lǐng)整個(gè)隊(duì)伍前行。

本文將虛擬結(jié)構(gòu)和leader-follower的結(jié)合。虛擬結(jié)構(gòu)的特點(diǎn)是將多個(gè)機(jī)器人的位姿記錄起來(lái)，視作一個(gè)剛性結(jié)構(gòu)，突出整個(gè)機(jī)器集群的隊(duì)形，放大化了整個(gè)隊(duì)伍的隊(duì)形反饋機(jī)制，但是其缺少靈活性，在某些環(huán)境下隊(duì)形的維持需要依靠大量的控制指令，造成較大的資源浪費(fèi)。通過(guò)領(lǐng)航者-跟隨者與虛擬結(jié)構(gòu)相結(jié)合，將圖拓?fù)渑c各個(gè)節(jié)點(diǎn)反饋控制協(xié)議交互，完成多機(jī)器人的協(xié)同控制與容錯(cuò)控制，提高復(fù)雜環(huán)境下的容錯(cuò)率和靈活性。

編隊(duì)控制中主要通過(guò)位置控制、位移控制、距離控制來(lái)實(shí)現(xiàn)隊(duì)形的控制。本文采用位移控制方案，通過(guò)領(lǐng)航者跟隨者以及分布式控制實(shí)現(xiàn)多機(jī)器人的編隊(duì)控制，核心思想為計(jì)算出編隊(duì)隊(duì)形時(shí)各機(jī)器人的所在位置，從而計(jì)算出領(lǐng)航[leader]和跟隨者[followers]的相對(duì)位置，控制隊(duì)形穩(wěn)定關(guān)鍵在于控制多機(jī)器人之間的速度[V]、角度[ω]；其中編隊(duì)系統(tǒng)的一階連續(xù)模型表示為[Xi=Ui]，其中[Xi]和[Ui]屬于[Rn]，[n]表示狀態(tài)量的維度，[Xi]表示節(jié)點(diǎn)i的狀態(tài)量，[Ui]表示節(jié)點(diǎn)i輸入量，通過(guò)上文圖論知識(shí)我們可得當(dāng)且僅當(dāng)拓?fù)鋱DG只具有一棵生成樹(shù)時(shí)，系統(tǒng)才可以達(dá)到一致性。采用一致性算法表示為

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

其中令[?]> 0，機(jī)器人[i]和機(jī)器人[j]之間的相對(duì)位置表示為[rij]，[k]表示為在某一時(shí)刻k。在機(jī)器群集剛開(kāi)始前行時(shí)，默認(rèn)機(jī)器人0作為領(lǐng)航者[leader]，通過(guò)控制領(lǐng)航者與目標(biāo)點(diǎn)之間的距離、領(lǐng)航者速度、領(lǐng)航者入度大小實(shí)現(xiàn)整個(gè)隊(duì)伍的前進(jìn)，領(lǐng)航者0的一致性控制算法為

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

4 仿真實(shí)驗(yàn)

在MATLAB環(huán)境下對(duì)前面設(shè)計(jì)的控制算法進(jìn)行仿真實(shí)現(xiàn)。如圖1（a） ，黑色不規(guī)則物體為凹型障礙物，此環(huán)境是容易造成勢(shì)場(chǎng)陷阱導(dǎo)致機(jī)器人無(wú)法逃離，在優(yōu)化后的群集控制算法下，機(jī)器人能逃離勢(shì)場(chǎng)陷阱。仿真圖中不同顏色的圓圈代表不同機(jī)器人個(gè)體，不同顏色的曲線代表各機(jī)器人的運(yùn)動(dòng)軌跡。可以看出，本文設(shè)計(jì)的優(yōu)化模型能解決傳統(tǒng)群集控制存在的勢(shì)場(chǎng)陷阱問(wèn)題。如圖1（b） ，是檢驗(yàn)本文設(shè)計(jì)的基于虛擬結(jié)構(gòu)體的一致性編隊(duì)協(xié)調(diào)算法，虛擬領(lǐng)導(dǎo)者進(jìn)行路徑規(guī)劃，跟隨機(jī)器人在虛擬節(jié)點(diǎn)的引力、障礙物的斥力和隊(duì)形力的合力下，向著虛擬領(lǐng)導(dǎo)者，即對(duì)應(yīng)的虛擬節(jié)點(diǎn)移動(dòng)，此設(shè)計(jì)提高編隊(duì)避障中各機(jī)器人的靈活性。

本文還在ROS環(huán)境對(duì)基于群集控制的多機(jī)器人運(yùn)動(dòng)控制算法進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證算法的有效性，仿真結(jié)果如圖2。圖2左邊為gazebo的仿真可視區(qū)，右邊為啟動(dòng)命令終端。仿真顯示優(yōu)化后的群集控制算法能使所有的機(jī)器人移動(dòng)在一起，避免碰撞并向目標(biāo)移動(dòng)。

5 結(jié)束語(yǔ)

本文研究基于群集控制的多機(jī)器人的避障控制和編隊(duì)協(xié)調(diào)控制。針對(duì)群集控制會(huì)出現(xiàn)的“勢(shì)場(chǎng)陷阱”問(wèn)題，設(shè)計(jì)了斥力偏轉(zhuǎn)策略解決人工勢(shì)場(chǎng)下局部零勢(shì)能點(diǎn)問(wèn)題。并針對(duì)Leader-follower的編隊(duì)控制會(huì)因leader出現(xiàn)故障編隊(duì)無(wú)法繼續(xù)保持的問(wèn)題，采用基于虛擬領(lǐng)航結(jié)構(gòu)的編隊(duì)協(xié)調(diào)控制算法，通過(guò)領(lǐng)航者-跟隨者與虛擬結(jié)構(gòu)相結(jié)合，將圖拓?fù)渑c各個(gè)節(jié)點(diǎn)反饋控制協(xié)議交互，提高復(fù)雜環(huán)境下的容錯(cuò)率和靈活性。通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了算法的有效性。

參考文獻(xiàn)：

[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ī)器人系統(tǒng)分布式群集控制[J].控制理論與應(yīng)用，2014，31（10）：1393-1403.

[4] 董慧.基于偏微分方程約束的機(jī)器人群集運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)[J].計(jì)算機(jī)測(cè)量與控制，2023，31（4）：130-135.

[5] Sakai D，F(xiàn)ukushima 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.

【通聯(lián)編輯：朱寶貴】