基于多虛擬領(lǐng)航者的多機(jī)器人編隊(duì)控制方法

王欽釗， 程金勇， 李小龍

(陸軍裝甲兵學(xué)院控制工程系， 北京 100072)

近年來，隨著人工智能和機(jī)器人技術(shù)的迅速發(fā)展，多機(jī)器人的協(xié)同技術(shù)受到越來越廣泛的關(guān)注。與單機(jī)器人相比，多機(jī)器人系統(tǒng)具有更強(qiáng)的工作能力和魯棒性。多機(jī)器人協(xié)同在資源探測(cè)、安全巡邏、地震救援、軍事作戰(zhàn)和智能交通等方面都有非常廣闊的應(yīng)用前景[1]。

多機(jī)器人編隊(duì)避障控制，顧名思義就是多個(gè)機(jī)器人在向目標(biāo)運(yùn)動(dòng)的過程中，既能夠保持一定的幾何隊(duì)形，又能適應(yīng)環(huán)境的約束安全避障[2]。目前多機(jī)器人的編隊(duì)控制方法主要有虛擬結(jié)構(gòu)法、人工勢(shì)場(chǎng)法、基于行為法、領(lǐng)導(dǎo)-跟隨法和基于圖論法等。其中：領(lǐng)導(dǎo)-跟隨法采用鏈?zhǔn)酵負(fù)浣Y(jié)構(gòu)，跟隨者跟蹤領(lǐng)航者形成隊(duì)形，但是該算法沒有考慮整體的路徑規(guī)劃和編隊(duì)的避障功能；人工勢(shì)場(chǎng)法中機(jī)器人在勢(shì)場(chǎng)力作用下形成隊(duì)形，但是每種隊(duì)形都需要定義相應(yīng)的勢(shì)場(chǎng)函數(shù)，而勢(shì)場(chǎng)函數(shù)設(shè)計(jì)比較復(fù)雜；基于圖論法采用一致性理論，使得各機(jī)器人能夠達(dá)到距離的恒定和速度的一致，缺點(diǎn)是機(jī)器人之間的相互位置關(guān)系不確定，難以得到明確的隊(duì)形結(jié)構(gòu)。

針對(duì)上述方法的不足，筆者提出領(lǐng)導(dǎo)-跟隨和人工勢(shì)場(chǎng)相結(jié)合的分層群集編隊(duì)避障算法。領(lǐng)導(dǎo)層中，領(lǐng)航者進(jìn)行路徑的預(yù)先規(guī)劃和編隊(duì)導(dǎo)航，以領(lǐng)航者為參考點(diǎn)，通過設(shè)定虛擬領(lǐng)航者的位置和數(shù)目形成明確的隊(duì)形和位置關(guān)系。跟隨層中，設(shè)計(jì)分布式控制器，跟隨機(jī)器人通過虛擬領(lǐng)航者的引導(dǎo)向期望的目標(biāo)位置運(yùn)動(dòng)，同時(shí)在避障避碰策略和群集一致性控制器的作用下實(shí)現(xiàn)編隊(duì)控制，與常規(guī)的一致性方法相比減少了通信量[3]，另外，引入附加勢(shì)場(chǎng)旋轉(zhuǎn)力對(duì)隊(duì)形控制器進(jìn)行調(diào)節(jié)，有利于機(jī)器人及時(shí)跳出局部極小值和死鎖狀態(tài)[4]。利用機(jī)器人編隊(duì)的勢(shì)場(chǎng)能和動(dòng)能構(gòu)建李雅譜諾夫函數(shù)，論證了編隊(duì)的穩(wěn)定性。最后，通過仿真試驗(yàn)驗(yàn)證了該方法的有效性。

1 問題描述

1.1 機(jī)器人編隊(duì)群體模型

假設(shè)n個(gè)機(jī)器人在二維平面上運(yùn)動(dòng)，將機(jī)器人視為質(zhì)點(diǎn)并采用常用的二階智能體模型，則機(jī)器人i(i=1,2,…,n)在二維平面中的動(dòng)力學(xué)方程為

(1)

式中：qi(t)∈R2，pi(t)∈R2，分別為機(jī)器人i在t時(shí)刻的位置向量和速度向量；ui(t)∈R2，為機(jī)器人i在t時(shí)刻的控制輸入。

另外,定義機(jī)器人群體中虛擬領(lǐng)航者r(r=1,2,…,m)的方程為

(2)

1.2 編隊(duì)運(yùn)動(dòng)整體目標(biāo)

跟隨機(jī)器人在控制輸入的作用下能夠形成穩(wěn)定的編隊(duì)跟蹤領(lǐng)航者，并在運(yùn)動(dòng)過程中既避免與鄰近機(jī)器人相撞，又要與鄰域范圍內(nèi)的機(jī)器人保持速度匹配，編隊(duì)能夠保持恒速和恒間距，即滿足條件

(3)

定義1：假設(shè)n個(gè)機(jī)器人分布在二維歐式空間內(nèi)，每個(gè)機(jī)器人的探測(cè)半徑為h，以機(jī)器人i為圓心，探測(cè)半徑內(nèi)的區(qū)域?yàn)闄C(jī)器人i的鄰域，鄰域內(nèi)的其他機(jī)器人j(j=1,2,…,n)為i的鄰接集Ni，表示為

定義2：同一平面的多機(jī)器人用圖論法可以方便直觀地表示機(jī)器人之間的通信拓?fù)浣Y(jié)構(gòu),用無向圖G(t)=(V,h(t),A)表示系統(tǒng)通信結(jié)構(gòu)，其中V={v1,v2,…,vn},為節(jié)點(diǎn)集合，vi代表機(jī)器人i節(jié)點(diǎn)；eij=(vi,vj)∈h(t),為圖的邊集，若機(jī)器人存在通信關(guān)系，則邊(vi,vj)存在，同時(shí)鄰接矩陣A中aij=aji=1，否則aij=aji=0。

2 群集一致性基礎(chǔ)理論模型

1986年，REYNOLDS[5]根據(jù)自然界中群居鳥類的行為特點(diǎn),設(shè)計(jì)了著名的Boids計(jì)算機(jī)模型，最早提出了模擬動(dòng)物行為的群集一致性理論。基礎(chǔ)理論分為3個(gè)部分：1)分離(Separation)，個(gè)體避免與群系統(tǒng)中鄰近的個(gè)體發(fā)生碰撞；2)匹配(Alignment)，目標(biāo)相同的智能體試圖與群系統(tǒng)中鄰近的個(gè)體保持運(yùn)動(dòng)速度匹配；3)聚合(Cohesion)，智能體試圖與鄰近的個(gè)體保持接近，并收斂到合適的位置，朝著一個(gè)共同的目標(biāo)運(yùn)動(dòng)。根據(jù)上述理論，多機(jī)器人運(yùn)動(dòng)控制模型可簡(jiǎn)化為

(4)

3 多機(jī)器人編隊(duì)避障控制策略

3.1 群集編隊(duì)控制算法主體思想

假設(shè)n個(gè)機(jī)器人組成多機(jī)器人系統(tǒng),其組成的群集編隊(duì)控制策略示意圖如圖1所示，其中：f1為隊(duì)形力(f11和f12分別為2個(gè)鄰接機(jī)器人的隊(duì)形力)；f2為障礙力；f3為領(lǐng)航力；fv為速度耗散力。整體編隊(duì)采用分層的思想，領(lǐng)航者和虛擬領(lǐng)航者作為導(dǎo)航層，領(lǐng)航者進(jìn)行路徑的預(yù)先規(guī)劃，主要行為是駛向目標(biāo)和引導(dǎo)編隊(duì)運(yùn)動(dòng)，領(lǐng)航者存儲(chǔ)隊(duì)形結(jié)構(gòu)和局部領(lǐng)航跟隨關(guān)系的隊(duì)形信息庫，運(yùn)動(dòng)過程中由于環(huán)境影響或者指令需要變換隊(duì)形時(shí)，以領(lǐng)航者為參考點(diǎn)，調(diào)整虛擬領(lǐng)航者的位置和數(shù)目，形成期望的編隊(duì)結(jié)構(gòu)。跟隨層中，跟隨機(jī)器人在領(lǐng)航力、障礙力、隊(duì)形力的“合力”和群集一致性控制器的作用下，向著各自期望的虛擬領(lǐng)航者的位置移動(dòng)，該編隊(duì)中機(jī)器人不需要獲取全局信息，僅僅依靠群組的鄰居局部信息即可實(shí)現(xiàn)群集編隊(duì)和狀態(tài)同步，另外在編隊(duì)中增添或減少機(jī)器人時(shí)，只需在隊(duì)形點(diǎn)位置增加相應(yīng)數(shù)目的虛擬領(lǐng)航者即可。

圖1 群集編隊(duì)控制策略示意圖

3.2 領(lǐng)航者路徑規(guī)劃

領(lǐng)航者采用人工勢(shì)場(chǎng)[6]方法對(duì)路徑進(jìn)行初步規(guī)劃，實(shí)現(xiàn)編隊(duì)導(dǎo)航。目標(biāo)對(duì)領(lǐng)航者施加的引力和障礙物對(duì)領(lǐng)航者施加的斥力形成合力控制領(lǐng)航者的運(yùn)動(dòng)。引力和斥力場(chǎng)模型分別為

(5)

(6)

式中：Uatt(X)為引力場(chǎng)函數(shù)；X為領(lǐng)航者位置向量；Xg為目標(biāo)位置向量；ka為引力增益系數(shù)；Urep(X)為斥力場(chǎng)函數(shù)；kr為斥力增益系數(shù)；d為領(lǐng)航者與障礙物相對(duì)距離；d0為斥力場(chǎng)作用距離。

引力和斥力均為引力場(chǎng)和斥力場(chǎng)函數(shù)的負(fù)梯度，表達(dá)式分別為

(7)

(8)

領(lǐng)航者受到的合力

F=Fatt+Frep

(9)

決定其下一步的運(yùn)動(dòng)方向。

3.3 群集編隊(duì)避障控制器設(shè)計(jì)

本節(jié)設(shè)計(jì)分布式控制器，使多機(jī)器人在追蹤虛擬領(lǐng)航者的過程中避免與障礙物或其他機(jī)器人發(fā)生碰撞，最終實(shí)現(xiàn)多機(jī)器人協(xié)調(diào)協(xié)作控制。根據(jù)勢(shì)場(chǎng)理論和REYNOLDS[5]提出的一致性基礎(chǔ)理論，第i個(gè)跟隨機(jī)器人的分布式控制器的輸入為

(10)

1) 避碰控制

為了實(shí)現(xiàn)機(jī)器人之間的避碰，引入隊(duì)形勢(shì)函數(shù)，該函數(shù)是關(guān)于距離的可微、非負(fù)、無界函數(shù)。設(shè)定Ni為鄰接集，機(jī)器人i和j之間的期望距離為dij，實(shí)際距離為x，w為跟隨機(jī)器人之間的作用距離。當(dāng)x>dij時(shí)，體現(xiàn)為引力,將機(jī)器人之間的距離拉近，直至接近dij；當(dāng)x

圖2 隊(duì)形勢(shì)場(chǎng)函數(shù)

隊(duì)形勢(shì)場(chǎng)函數(shù)ψ1和隊(duì)形力f1定義為

(11)

(12)

式中：k1為增益系數(shù)；ψij為隊(duì)形勢(shì)函數(shù);e1為機(jī)器人i指向機(jī)器人j的單位方向向量。

同一編隊(duì)內(nèi)的機(jī)器人避碰時(shí)不僅要考慮到鄰接機(jī)器人之間的位置關(guān)系，同時(shí)還要考慮到彼此之間速度的一致性，體現(xiàn)出編隊(duì)內(nèi)機(jī)器人的運(yùn)動(dòng)狀態(tài)對(duì)同一編隊(duì)內(nèi)其他機(jī)器人的約束作用，對(duì)式(12)添加速度耗散力,改進(jìn)為

(13)

式中：k0為增益系數(shù)。

2) 避障控制

為了增加機(jī)器人避障過程中的安全性，采用閔可夫斯基(Minkowskisum)方法對(duì)障礙物擴(kuò)張?zhí)幚頌檎系K圓[7]。機(jī)器感應(yīng)到障礙物邊緣時(shí)，在障礙物的邊緣使用邊界投影方法在圓上產(chǎn)生投影點(diǎn)，以投影點(diǎn)為基礎(chǔ)產(chǎn)生虛擬的人工勢(shì)場(chǎng)源，能夠使以最差姿態(tài)靠近的機(jī)器人成功避障。s為跟隨機(jī)器人距離障礙物邊緣的距離，障礙物的圓心位置矢量為Ok，障礙圓半徑為Rk，機(jī)器人的位置矢量為qi，投影點(diǎn)的位置矢量為qik，障礙物勢(shì)場(chǎng)作用距離為R。增益系數(shù)k2的大小取決于機(jī)器人慣性向量的大小，數(shù)值過小不利于安全避障，數(shù)值過大則可能超出機(jī)器人的性能，投影點(diǎn)位置矢量為

(14)

圖3為避障勢(shì)場(chǎng)函數(shù)。

圖3 避障勢(shì)場(chǎng)函數(shù)

障礙勢(shì)場(chǎng)函數(shù)和斥力分別為

(15)

(16)

式中：e2為機(jī)器人i指向投影點(diǎn)的單位方向向量。

3) 群集編隊(duì)控制

虛擬領(lǐng)航者相當(dāng)于跟隨機(jī)器人的目標(biāo)，跟隨機(jī)器人要保持與虛擬領(lǐng)航者的速度、位置的一致性，定義一致性控制器

(17)

式中：c0,c1>0，二者均為增益系數(shù)。c0、c1的存在可以非常方便地設(shè)置機(jī)器人對(duì)虛擬領(lǐng)航者的追蹤和自身形成隊(duì)形二者之間的權(quán)重。

4) 死鎖跳出控制

多機(jī)器人采用勢(shì)場(chǎng)思想進(jìn)行編隊(duì)控制時(shí)，機(jī)器人之間或者機(jī)器人和障礙物之間有可能陷入局部極小點(diǎn)，即陷入死鎖狀態(tài)[8]。為解決這個(gè)問題，采用附加旋轉(zhuǎn)力[9]的思想對(duì)控制器進(jìn)行改進(jìn)，如圖4所示，附加勢(shì)場(chǎng)旋轉(zhuǎn)力為

fi=kτn

，

(18)

(19)

圖4 勢(shì)場(chǎng)旋轉(zhuǎn)力示意圖

綜上可得編隊(duì)控制器為

(20)

4 穩(wěn)定性證明

由n個(gè)機(jī)器人組成的多機(jī)器人群體，機(jī)器人模型見式(1)，假設(shè)系統(tǒng)中各跟隨機(jī)器人的初始速度不相同，那么機(jī)器人在虛擬領(lǐng)航者的引導(dǎo)和群集編隊(duì)控制器(20)的作用下能夠?qū)崿F(xiàn)個(gè)體間速度匹配和間距的恒定，且個(gè)體之間不發(fā)生碰撞，系統(tǒng)呈現(xiàn)穩(wěn)定的編隊(duì)運(yùn)動(dòng)。

證明：定義系統(tǒng)中的跟隨機(jī)器人i的運(yùn)動(dòng)學(xué)誤差方程為

(21)

假設(shè)x=[x1x2…xn]T，v=[v1v2…vn]T，u=[u1u2…un]T，計(jì)算過程中不考慮勢(shì)場(chǎng)旋轉(zhuǎn)力，將式(20)、(21)代入式(1)，則跟隨機(jī)器人模型變換為

(22)

為證明穩(wěn)定性,取李雅普諾夫的能量函數(shù)[10]為

(23)

對(duì)其求導(dǎo)得

可知

(24)

與上述結(jié)論中機(jī)器人總能量小于無窮大相矛盾，可得反證法的假設(shè)不成立，跟隨機(jī)器人之間不會(huì)發(fā)生碰撞。由此系統(tǒng)的編隊(duì)穩(wěn)定性得以證明。

5 仿真試驗(yàn)

試驗(yàn)考慮5個(gè)機(jī)器人的編隊(duì)運(yùn)動(dòng)，首先設(shè)置相應(yīng)的編隊(duì)參數(shù)：n=5，k0=11，k1=26，k2=12，c0=3，c1=6，k=10。

領(lǐng)航機(jī)器人以1 m/s的速度沿y=x正方向作直線運(yùn)動(dòng)，5個(gè)機(jī)器人成前三角編隊(duì)運(yùn)動(dòng)。領(lǐng)航者為leader。跟隨機(jī)器人分別為R1、R2、R3、R4，與領(lǐng)航者的期望編隊(duì)距離分別為6、6、12、12 rn。障礙物中心的坐標(biāo)為(25,35)、(8,17)、(25,14) ，障礙物的半徑設(shè)置為3 rn，障礙勢(shì)場(chǎng)作用距離為2 rn，機(jī)器人之間的勢(shì)場(chǎng)力作用范圍為9。圖5為多機(jī)器人編隊(duì)運(yùn)動(dòng)軌跡；圖6為跟隨機(jī)器人與虛擬領(lǐng)航者的位置誤差；圖7為跟隨機(jī)器人速度、角速度變化曲線；圖8為跟隨機(jī)器人的相對(duì)距離；圖9為跟隨機(jī)器人受到的合力。

圖5 多機(jī)器人編隊(duì)運(yùn)動(dòng)軌跡

圖6 跟隨機(jī)器人與虛擬領(lǐng)航者的位置誤差

圖7 跟隨機(jī)器人速度與角速度變化曲線

圖8 跟隨機(jī)器人的相對(duì)距離

圖9 跟隨機(jī)器人受到的合力

從機(jī)器人運(yùn)動(dòng)軌跡中可以看出：多機(jī)器人總體成前三角編隊(duì)運(yùn)動(dòng)，跟隨機(jī)器人實(shí)現(xiàn)了編隊(duì)避障。編隊(duì)過程中，機(jī)器人的間距始終不為零，說明機(jī)器人之間能夠體現(xiàn)出避碰效果。進(jìn)一步分析可知：機(jī)器人之間的間距趨向于期望的編隊(duì)距離。

綜上，在本文提出的機(jī)器人群集編隊(duì)避障算法的作用下，多機(jī)器人實(shí)現(xiàn)了運(yùn)動(dòng)過程中的編隊(duì)控制和避障行為，在避障過程中能夠及時(shí)調(diào)整機(jī)器人的運(yùn)動(dòng)狀態(tài)，實(shí)現(xiàn)避障避碰和編隊(duì)過程的統(tǒng)一，仿真試驗(yàn)驗(yàn)證了算法的正確性。

6 結(jié)論

利用虛擬領(lǐng)航者和領(lǐng)航者的相對(duì)位置關(guān)系表

示機(jī)器人編隊(duì)結(jié)構(gòu)，用鄰接矩陣表示機(jī)器人之間的通信關(guān)系，建立隊(duì)形力、領(lǐng)航力、障礙力模型，用勢(shì)場(chǎng)法思想對(duì)一致性編隊(duì)集群理論進(jìn)行改進(jìn)，新的編隊(duì)控制律能夠使多機(jī)器人達(dá)到期望的隊(duì)形，并能適應(yīng)環(huán)境約束成功避障。用李雅普諾夫穩(wěn)定性理論進(jìn)行分析，在控制器作用下多機(jī)器人能夠穩(wěn)定地編隊(duì)，最后二維平面的編隊(duì)形成、隊(duì)形保持和編隊(duì)避障的試驗(yàn)結(jié)果表明了算法的有效性。由于條件所限，暫時(shí)只能在MATLAB中仿真，將算法應(yīng)用到實(shí)際中將是下一步的研究方向。

[1] 夏梁盛. 基于群集的機(jī)器人編隊(duì)控制[J].火力與指揮控制，2013,38(1)：18-22.

[2] 曹建福，凌志浩，高沖,等.基于群集思想的多智能體編隊(duì)避障算法研究[J].系統(tǒng)仿真學(xué)報(bào)，2014，26(3)：563-567.

[3] 趙海，劉倩，邵氏亮,等.一種面向多智能體群集的避障算法[J].東北大學(xué)學(xué)報(bào)，2014，35(3)：347-351．

[4] 曲正剛，曹喜濱，張澤旭.人工勢(shì)場(chǎng)和虛擬領(lǐng)航者相結(jié)合的多智能體編隊(duì)控制[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2014,46(5)：2-6.

[5] REYNOLDS C W.Flocks,heirds,and schools:a distributed behavioral model[J].Computer graphics，1987，21(4):25-34.

[6] 陳浩，雷斌，高全杰.基于領(lǐng)導(dǎo)者群體機(jī)器人編隊(duì)導(dǎo)航控制[J].武漢科技大學(xué)學(xué)報(bào)，2016,39(3)：220-223.

[7] 潘無為，姜大鵬，龐永杰,等.人工勢(shì)場(chǎng)和虛擬結(jié)構(gòu)相結(jié)合的多水下機(jī)器人編隊(duì)控制[J].兵工學(xué)報(bào)，2017,38(2)：326-334.

[8] 陳世明，丁賢達(dá)，化俞新,等.大規(guī)模機(jī)器人群體的分層編隊(duì)控制算法[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版)，2015,42(10)：52-57.

[9] 劉丹.基于拓?fù)溥B通性的多智能體系統(tǒng)目標(biāo)追蹤群集控制[D].秦皇島：燕山大學(xué)，2011.

[10] 樓曉春，謝宇.一種基于虛擬力的多機(jī)器人編隊(duì)控制系統(tǒng)[J].中國機(jī)械工程，2014，25(24)：3294-3302.