王欽釗， 程金勇， 李小龍

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

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

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

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

1 問題描述

1.1 機器人編隊群體模型

假設n個機器人在二維平面上運動，將機器人視為質(zhì)點并采用常用的二階智能體模型，則機器人i(i=1,2,…,n)在二維平面中的動力學方程為

(1)

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

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

(2)

1.2 編隊運動整體目標

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

(3)

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

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

2 群集一致性基礎理論模型

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

(4)

3 多機器人編隊避障控制策略

3.1 群集編隊控制算法主體思想

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

圖1 群集編隊控制策略示意圖

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

領航者采用人工勢場[6]方法對路徑進行初步規(guī)劃，實現(xiàn)編隊導航。目標對領航者施加的引力和障礙物對領航者施加的斥力形成合力控制領航者的運動。引力和斥力場模型分別為

(5)

(6)

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

引力和斥力均為引力場和斥力場函數(shù)的負梯度，表達式分別為

(7)

(8)

領航者受到的合力

F=Fatt+Frep

(9)

決定其下一步的運動方向。

3.3 群集編隊避障控制器設計

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

(10)

1) 避碰控制

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

圖2 隊形勢場函數(shù)

隊形勢場函數(shù)ψ1和隊形力f1定義為

(11)

(12)

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

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

(13)

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

2) 避障控制

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

(14)

圖3為避障勢場函數(shù)。

圖3 避障勢場函數(shù)

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

(15)

(16)

式中：e2為機器人i指向投影點的單位方向向量。

3) 群集編隊控制

虛擬領航者相當于跟隨機器人的目標，跟隨機器人要保持與虛擬領航者的速度、位置的一致性，定義一致性控制器

(17)

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

4) 死鎖跳出控制

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

fi=kτn

，

(18)

(19)

圖4 勢場旋轉(zhuǎn)力示意圖

綜上可得編隊控制器為

(20)

4 穩(wěn)定性證明

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

證明：定義系統(tǒng)中的跟隨機器人i的運動學誤差方程為

(21)

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

(22)

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

(23)

對其求導得

可知

(24)

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

5 仿真試驗

試驗考慮5個機器人的編隊運動，首先設置相應的編隊參數(shù)：n=5，k0=11，k1=26，k2=12，c0=3，c1=6，k=10。

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

圖5 多機器人編隊運動軌跡

圖6 跟隨機器人與虛擬領航者的位置誤差

圖7 跟隨機器人速度與角速度變化曲線

圖8 跟隨機器人的相對距離

圖9 跟隨機器人受到的合力

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

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

6 結(jié)論

利用虛擬領航者和領航者的相對位置關系表

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

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

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

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

[4] 曲正剛，曹喜濱，張澤旭.人工勢場和虛擬領航者相結(jié)合的多智能體編隊控制[J].哈爾濱工業(yè)大學學報，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] 陳浩，雷斌，高全杰.基于領導者群體機器人編隊導航控制[J].武漢科技大學學報，2016,39(3)：220-223.

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

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

[9] 劉丹.基于拓撲連通性的多智能體系統(tǒng)目標追蹤群集控制[D].秦皇島：燕山大學，2011.

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