李宗剛,郭志龍,石慧榮,謝廣明

(1.蘭州交通大學(xué)機(jī)電工程學(xué)院,蘭州 730070；2.北京大學(xué)工學(xué)院系統(tǒng)與控制研究中心,北京 100871)

?

基于分布式估計(jì)與模糊控制的多機(jī)器魚編隊(duì)控制

李宗剛1,2,郭志龍1,石慧榮1,謝廣明2

(1.蘭州交通大學(xué)機(jī)電工程學(xué)院,蘭州 730070；2.北京大學(xué)工學(xué)院系統(tǒng)與控制研究中心,北京 100871)

針對(duì)多仿生機(jī)器魚編隊(duì)控制問題，提出了一種分布式的分層控制方案。首先，各機(jī)器魚以一致性算法為數(shù)據(jù)融合模塊對(duì)編隊(duì)中心位姿進(jìn)行分布式估計(jì)，據(jù)此獲得在編隊(duì)中的期望位姿，作為路徑規(guī)劃模塊的輸入；其次，各機(jī)器魚基于速度與方向模糊控制器，實(shí)時(shí)跟蹤期望軌跡，形成和保持編隊(duì)。所提方法實(shí)現(xiàn)了期望位置分布式估計(jì)與機(jī)器魚個(gè)體路徑規(guī)劃的分離，為不同種類機(jī)器魚的合作控制問題提供了新思路。實(shí)驗(yàn)結(jié)果表明了所提方法的有效性。

仿生機(jī)器魚;編隊(duì);一致性算法;模糊控制;軌跡跟蹤

0　引言

近年來，隨著海洋重要性的日益凸顯，海洋中的地圖構(gòu)建、生態(tài)監(jiān)測(cè)、水中偵測(cè)等領(lǐng)域的需求使得多水下機(jī)器人系統(tǒng)的協(xié)調(diào)控制問題成為多機(jī)器人系統(tǒng)研究的熱點(diǎn)[1-9]。針對(duì)水環(huán)境的動(dòng)態(tài)特性所造成位姿的不確定性，Zhang等[3]利用提出的極限環(huán)方法控制機(jī)器魚位姿和避免碰撞，利用模糊控制器控制方向，研究了多魚協(xié)作搬運(yùn)任務(wù)。文獻(xiàn)[4]研究了多機(jī)器魚協(xié)作爆破問題，其中利用遺傳算法優(yōu)化機(jī)器魚之間的權(quán)重因子，進(jìn)而利用改進(jìn)的匈牙利算法實(shí)現(xiàn)了任務(wù)分配。Shao等[5]利用機(jī)器魚的機(jī)構(gòu)特點(diǎn)，利用曲率坐標(biāo)系建立了個(gè)體動(dòng)力學(xué)模型，進(jìn)而基于領(lǐng)航者-跟隨者方法研究了多機(jī)器魚編隊(duì)控制問題。Zou等[6]利用基于徑向基神經(jīng)網(wǎng)絡(luò)的滑?？刂品椒ㄑ芯苛硕鄼C(jī)器魚的軌跡跟蹤問題。Zhao等[7]基于領(lǐng)航者-跟隨者方法研究了多自主機(jī)器魚的編隊(duì)的控制問題。其中，機(jī)器魚線速度和角速度分別通過模糊控制器和期望Bezier軌線曲率優(yōu)化方法進(jìn)行控制。Hu等[10]提出了一種基于勞動(dòng)分工方法的多自主機(jī)器魚搬運(yùn)控制策略。此外，文獻(xiàn)[8]～[9]基于行為技術(shù)研究了多水下機(jī)器人編隊(duì)控制問題。需要指出的是，文獻(xiàn)[3]～[9]中的協(xié)調(diào)控制算法都是集中式的，也即機(jī)器魚之間不存在信息交互,文獻(xiàn)[10]的算法是分布式的，其特點(diǎn)更近于通過公平競(jìng)爭(zhēng)的方式完成協(xié)作任務(wù)。

針對(duì)空中或地面多機(jī)器人系統(tǒng)，基于領(lǐng)航者-跟隨者方法和一致性算法的編隊(duì)控制策略得到了廣泛研究[11-22]。受此啟發(fā)，本文以三關(guān)節(jié)仿鲹科機(jī)器魚為對(duì)象，提出了一種分層的多仿生機(jī)器魚分布式編隊(duì)控制算法。其中，以期望隊(duì)形的幾何中心作為虛擬領(lǐng)導(dǎo)者，首先每個(gè)機(jī)器魚利用一致性算法對(duì)編隊(duì)中心位姿進(jìn)行分布式估計(jì)，進(jìn)而基于估計(jì)信息和編隊(duì)要求確定自身下一時(shí)刻的位姿，最后利用模糊控制器控制機(jī)器魚的線速度和角速度，實(shí)現(xiàn)對(duì)期望位姿的精確跟蹤。所提方法將一致性算法作為數(shù)據(jù)融合模塊，利用領(lǐng)航者-跟隨者模式實(shí)現(xiàn)編隊(duì)控制。

1　預(yù)備知識(shí)

對(duì)所考慮的多機(jī)器魚系統(tǒng)，令pi(t)=[xi(t),yi(t),θi(t)]T∈R3表示機(jī)器魚vi的位姿信息，則必有

(1)

其中，θi表示機(jī)器魚頭尾軸與x軸的夾角，υi∈[0,υmax]表示機(jī)器魚巡游速度，ωi∈[0,ωmax]表示機(jī)器魚轉(zhuǎn)彎角速度。機(jī)器魚之間的信息交換拓?fù)錆M足假設(shè)1。

假設(shè)1設(shè)有向圖G為N條機(jī)器魚組成的多智能體系統(tǒng)的信息交換拓?fù)?，則G含有生成樹，且在任意時(shí)刻至少有一個(gè)根節(jié)點(diǎn)可以接收到參考軌跡的位姿信息ξ0(t)。

2　多仿生機(jī)器魚編隊(duì)控制

考慮N個(gè)多仿鲹科三關(guān)節(jié)機(jī)器魚的編隊(duì)控制問題，機(jī)器魚的體波曲線由Lighthill方程

ybody(x,t)=(c1x+c2x2)sin(kx+ωt)

(2)

控制。則所考慮多仿生機(jī)器魚編隊(duì)控制問題可描述為：在沿期望軌跡運(yùn)動(dòng)的過程中，N條機(jī)器魚頭部參考點(diǎn)的連線形成并保持期望的隊(duì)形，同時(shí)所有機(jī)器魚頭尾軸的方向最終保持一致。分析可知，各機(jī)器魚在編隊(duì)形成與保持的過程中，通過直線巡游中的轉(zhuǎn)彎行為調(diào)整位姿，其巡游速度和轉(zhuǎn)彎角速度由式(1)和式(3)給出。為此，提出一種基于個(gè)體的編隊(duì)形成與保持方法，機(jī)器魚vi的位姿控制流程如圖1所示。

2.1編隊(duì)參考點(diǎn)一致性估計(jì)

在編隊(duì)形成與保持過程中，多采用個(gè)體之間的位姿偏置量確定編隊(duì)形狀[15-16，18-20]。受此啟發(fā)，本文以編隊(duì)幾何中心為參考點(diǎn)，確定各機(jī)器魚在編隊(duì)中的相對(duì)位姿，如圖2所示。

由于所考慮的多機(jī)器魚系統(tǒng)信息交換拓?fù)錆M足假設(shè)1，為了實(shí)現(xiàn)編隊(duì)，各機(jī)器魚必須基于局部信息實(shí)現(xiàn)編隊(duì)任務(wù)。由于機(jī)器魚vi在編隊(duì)中的位姿是確定的，因此可以根據(jù)其當(dāng)前位姿估計(jì)出編隊(duì)參考點(diǎn)位姿ζi(t)，并同相鄰機(jī)器魚共享這一信息?；谶@一事實(shí)，利用一致性算法對(duì)編隊(duì)參考點(diǎn)進(jìn)行分布式估計(jì)，協(xié)議為

(3)

i=1,…,N，ζi(t),ζj(t)分別表示第i條和第j條機(jī)器魚所估計(jì)的參考點(diǎn)位姿；ξ0(t)表示編隊(duì)所要跟蹤的軌跡，當(dāng)機(jī)器魚i能夠獲得所跟蹤軌跡的位姿信息時(shí)，bi0=1，否則為0。易知，式(4)是典型的一階一致性算法，由文獻(xiàn)[19]中的定理3.1可知，式(4)描述的多機(jī)器魚編隊(duì)參考點(diǎn)位姿估計(jì)算法，當(dāng)信息交換拓?fù)錆M足假設(shè)1時(shí)，當(dāng)t→∞時(shí)，必有ζi(t)→ζ0(t)成立。

需要指出的是，一致性算法(4)僅作為數(shù)據(jù)融合模塊存在，通過分布式方法為各機(jī)器魚的軌跡規(guī)劃提供支持。由于水中環(huán)境的動(dòng)態(tài)特性帶來的不確定性，使得很難對(duì)機(jī)器魚進(jìn)行精確控制，多機(jī)器魚組成的編隊(duì)也很難保持。這一點(diǎn)也是實(shí)現(xiàn)水中多仿生機(jī)器魚編隊(duì)控制的難點(diǎn)。

2.2多機(jī)器魚編隊(duì)形成與保持

(4)

在編隊(duì)保持和軌跡跟蹤階段，機(jī)器魚以巡游轉(zhuǎn)彎行為保持其在編隊(duì)中的位姿，此時(shí)體波方程(3)在由式(1)確定的υi和ωi確定后起作用。為了實(shí)現(xiàn)編隊(duì)保持和跟蹤期望軌跡的目標(biāo)，需要設(shè)計(jì)精確控制機(jī)器魚由式(1)確定的直線巡游速度υi和轉(zhuǎn)彎角速度ωi的控制律。

3　多機(jī)器魚編隊(duì)路徑跟蹤模糊控制器

如何克服水環(huán)境的動(dòng)態(tài)特性對(duì)多機(jī)器魚編隊(duì)形成、保持與軌跡跟蹤帶來的不確定性，是實(shí)現(xiàn)上述任務(wù)的關(guān)鍵。受文獻(xiàn)[3],[21]～[24]的啟發(fā)，采用模糊控制方法對(duì)機(jī)器魚的運(yùn)動(dòng)方向和速度進(jìn)行控制。

由圖4可知，機(jī)器魚vi與其期望位置之間的距離為

(5)

易得期望位姿的方向角為

則機(jī)器魚vi與其期望位姿之間的方向角之差為

(6)

控制目標(biāo)即是使得由式(6)及式(7)確定的誤差信號(hào)為零。機(jī)器魚vi的角速度ωi及速度υi的模糊控制器結(jié)構(gòu)如圖5所示。

與此相對(duì)應(yīng)，輸出υi由3條模糊控制規(guī)則獲得，即IfdeiisS, thenυiisS;IfdeiisM, thenυiisM;IfdeiisL, thenυiisF。

每條模糊規(guī)則的激活度ηi由Mamdani推理規(guī)則確定，即ηi=min(μ(dei)),i=1,2,3,μ(dei)為模糊變量Ldei值的隸屬度函數(shù)值，則機(jī)器魚vi的巡游速度可由“重心法”去模糊得到，即

(7)

下面討論機(jī)器魚vi的方向控制問題，其方向模糊控制器輸入為角度誤差θei，由于-π≤θei(t)<π，故將其分為7個(gè)等級(jí)，對(duì)應(yīng)模糊集記為

分別表示負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大，其隸屬度函數(shù)如圖7a所示。同理，方向模糊控制器的輸出變量為角速度ωi，其模糊集記為

分別表示負(fù)大、負(fù)中、負(fù)小、零、正小、正中、正大，隸屬度函數(shù)如圖6b所示。

與此相對(duì)應(yīng)，輸出ωi由以下模糊規(guī)則得到：IfθeiisPB,thenωiisPB;IfθeiisPM,thenωiisPM;IfθeiisPS,thenωiisPS;IfθeiisZE,thenωiisZE;IfθeiisNS,thenωiisNS;IfθeiisNM,thenωiisNM;IfθeiisNB,thenωiisNB。

每條模糊規(guī)則的激活度μi由Mamdani推理規(guī)則確定，即μi=min(μ(θei)),i=1,…,7,μ(θei)為模糊變量Lθei值的隸屬度函數(shù)值，則機(jī)器魚vi的角速度可由“重心法”去模糊得到，即

(8)

機(jī)器魚vi通過模糊控制器獲得其下一時(shí)刻的巡游速度υi和角速度ωi之后，即依據(jù)式(3)選擇合適模態(tài)進(jìn)行游動(dòng)，以達(dá)到保持編隊(duì)和跟蹤期望軌跡的目標(biāo)。

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

4.1單仿生機(jī)器魚路徑跟蹤

如圖9所示，機(jī)器魚以逆時(shí)針方向游動(dòng)，其軌跡基本能夠跟蹤參考橢圓軌跡。圖10，圖11以及圖12分別是機(jī)器魚跟蹤橢圓軌跡時(shí)的線速度，運(yùn)動(dòng)方向和角速度的變化曲線。

由上述各圖可知，機(jī)器魚編隊(duì)的線速度基本維持在10～16cm/s的恒定范圍內(nèi)。12s時(shí)，由于運(yùn)動(dòng)滯后的原因發(fā)生跳變，但角度及角速度保持穩(wěn)定；在24s時(shí)，游動(dòng)到圖8所示的b點(diǎn)，運(yùn)用游動(dòng)中的轉(zhuǎn)彎方式進(jìn)行轉(zhuǎn)彎，其線速度增大，以增加回?cái)[的力度，為跟蹤橢圓軌跡，運(yùn)動(dòng)方向瞬間由3rad轉(zhuǎn)為-3rad，此時(shí)，由于速度和運(yùn)動(dòng)方向的改變，其轉(zhuǎn)彎角速度也瞬間降為-7rad/s,為最小。之后，在恒定速度下，運(yùn)動(dòng)方向趨于0，在50s時(shí)，游動(dòng)到圖8所示的d點(diǎn)，發(fā)生另一次游動(dòng)中轉(zhuǎn)彎。之后游動(dòng)到a點(diǎn)，完成一個(gè)周期的編隊(duì)巡游。由圖可見，機(jī)器魚的速度和角速度變化都符合模糊控制規(guī)則的設(shè)計(jì)要求，表明在模糊控制器的作用下，機(jī)器魚能夠穩(wěn)定地跟蹤期望的參考軌跡。

4.2多機(jī)器魚編隊(duì)形成與保持

考慮3條仿生機(jī)器魚的三角編隊(duì)及軌跡跟蹤問題，實(shí)驗(yàn)環(huán)境為的室內(nèi)水池，機(jī)器魚初始位置在水池左側(cè)隨機(jī)布置，編隊(duì)中心參考軌跡為直線。3條仿生機(jī)器魚在水池中的運(yùn)動(dòng)場(chǎng)景如圖13所示。由圖可知，3條仿生魚在8s時(shí)已形成期望隊(duì)形，并在后續(xù)過程中保持了隊(duì)形。圖14為3條機(jī)器魚的運(yùn)動(dòng)軌跡圖，可以看出，編隊(duì)在跟蹤一直線時(shí)表現(xiàn)比較穩(wěn)定。圖15給出了各機(jī)器魚在游動(dòng)中的線速度變化情況，其中紅線代表編隊(duì)中心的期望速度。由圖可知，為了保持編隊(duì)，機(jī)器魚的游動(dòng)速度始終有微小的波動(dòng)。

圖16、圖17和圖18分別給出了機(jī)器魚的方向、角速度以及實(shí)際的誤差曲線。由圖可知，各機(jī)器魚線速度，角速度的變化基本趨于一致，表明3條機(jī)器魚能夠快速地形成編隊(duì)，并且在保持編隊(duì)構(gòu)型的前提下，實(shí)現(xiàn)了對(duì)參考軌跡曲線的精確跟蹤。

5　結(jié)論

針對(duì)多仿生機(jī)器魚的編隊(duì)控制問題，基于分層思想，提出了一種分布式的控制算法。在該方案中，首先每條機(jī)器魚采用一致性算法模塊對(duì)編隊(duì)中心信息進(jìn)行分布式估計(jì)，并據(jù)此求得其下一時(shí)刻在編隊(duì)中的期望位姿；其次，各機(jī)器魚采用模糊控制器確定自己的巡游速度和轉(zhuǎn)彎速度，從而實(shí)現(xiàn)對(duì)期望位姿的跟蹤，實(shí)驗(yàn)結(jié)果表明算法是有效的。所提方法兼顧了編隊(duì)算法的分布性和機(jī)器魚本身的動(dòng)力學(xué)特性，實(shí)現(xiàn)了一致性算法與機(jī)器人動(dòng)力學(xué)的分離，為不同種類機(jī)器人之間的合作控制問題提供了一個(gè)新的思路。在后續(xù)工作中，將綜合考慮編隊(duì)參考點(diǎn)信息的變化率及其不確定性，以及模糊控制器輸入變量的變化率信息，設(shè)計(jì)更靈敏和可靠的合作控制算法，提高多仿生機(jī)器魚協(xié)作編隊(duì)任務(wù)的穩(wěn)定性和準(zhǔn)確性。

[1]Zhang Y, Streitlien K, Bellingham J G, et al. Acoustic doppler velocimeter flow measurement from an autonomous underwater vehicle with applications to deep ocean convection[J]. Journal of Atmospheric and Oceanic Technology, 2001, 18(12): 2038-2051.

[2] Leonard N E, Fiorelli E. Virtual leaders, artificial potentials and coordinated control of groups[C]// Proceedings of the 40th IEEE Conference on Decision and Control. Orlando,Florida,USA,2001, 3: 2968-2973.

[3] Zhang D D, Wang L, Yu J Z, et al. Coordinated transport by multiple biomimetic robotic fish in underwater environment[J]. IEEE Transactions on Control Systems Technology, 2007, 15(4): 658-671.

[4] Zhang D D, Wang L, Yu J Z. Geometric topology based cooperation for multiple robots in adversarial environments [J]. Control Engineering Practice, 2008, 16(9): 1092-1100.

[5] Shao J Y, Yu J Z, Wang L. Formation control of multiple biomimetic robotic fish[C]// Proceedings of the 2006 IEEE/RSJ International Conference on Intelligent Robots and Systems. Beijing, China, 2006: 2503-2508.

[6] Zou K M, Wang C, Xie G M, et al. Cooperative control for trajectory tracking of robotic fish[C]. 2009 American Control Conference. St Louis, MO, USA, 2009: 5504-5509.

[7] Zhao W, Hu Y H, Wang L. Leader-following formation control of multiple vision-based autonomous robotic fish[C]∥Proceedings of the 48th IEEE Conference on Decision and Control, 2009 held jointly with the 2009 28th Chinese Control Conference. Shanghai, China, 2009, 579-584.

[8] 孟憲松, 徐宏根, 張銘鈞, 等. 基于分解策略的多機(jī)器人編隊(duì)控制方法[J]. 哈爾濱工程大學(xué)學(xué)報(bào)，2006, 27(2): 276-280.

Meng Xiansong, Xu Honggen, Zhang Mingjun, et al. Formation control for multiple robots based on a strategy of decomposition [J]. Journal of Harbin Engineering University, 2006, 27(2): 276-280.

[9] 范士波, 王宏健. 多自主水下航行器編隊(duì)控制技術(shù)研究[J]. 中國造船，2008, 50(4): 92-101.

Fan Shibo, Wang Hongjian. Research on formation technology for autonomous underwater vehicles[J]. Ship Building of China, 2008, 50(4): 92-101.

[10] Hu Y H, Wang L, Liang J H, et al. Cooperative box-pushing with multiple autonomous robotic fish in underwater environment [J]. IET Control Theory and Applications, 2011,5(11): 2015-2022.

[11] Shao J Y, Xie G M, Yu J Z, et al. Leader-following formation control of multiple mobile robots[C]∥Proceedings of the 2005 IEEE Internatio-nal Symposium on Mediterrean Conference on Control and Automation, IEEE.Limassol, Cyprus, 2005: 808-813.

[12] Chen Y, Zhuang Y, Wang W. Cooperative control for formations of mobile robots under the nonholonomic constraints[C]∥Proceeding of the 6th World Congress on Intelligent Control and Automation, IEEE.Dalian, China， 2006, 2: 9042-9046.

[13] Fax J A, Murray R M. Information flow and cooperative control of vehicle formations[J]. IEEE Transactions on Automatic Control, 2004, 49(9): 1465-1476.

[14] Lin Z Y, Francis B, Maggiore M. Necessary and sufficient graphical conditions for formation control of unicycles[J]. IEEE Transactions on Automatic Control, 2005, 50(1): 121-127.

[15] Porfiri M, Roberson D G, Stilwell D J. Tracking and formation control of multiple autonomous agents: a two-level consensus approach[J]. Automatica, 2007, 43(8): 1318-1328.

[16] 張博, 羅建軍, 袁建平. 一種基于信息一致性的衛(wèi)星編隊(duì)協(xié)同控制策略[J]. 航空學(xué)報(bào), 2010, 31(5): 1004-1013.

Zhang Bo, Luo Jianjun, Yuan Jianping. A satellite formation cooperative control strategy based on information consensus[J]. Acta Aeronautica et Astronaut Ica Sinica, 2010, 31(5): 1004-1013.

[17] Sheng L, Pan Y J, Gong X. Consensus formation control for a class of networked multiple mobile robot systems[J]. Journal of Control Science and Engineering-Special Issue on Advances in Methods for Control over Networks, 2012: 1-12.

[18] Cao Y C, Ren W, Sorensen N, et al. Experiments in consensus-based distributed cooperative control of multiple mobile robots[C]∥Proceedings of the 2007 IEEE International Conference on Mechatronics and Automation. Harbin, China， 2007: 2819-2824.

[19] Ren W, Sorensen N. Distributed coordination architecture for multi-robot formation control[J]. Robotics and Autonomous Systems, 2008, 56: 324-333.

[20] Fang L, Antsaklis P J A. Decentralized formation tracking of multi-vehicle systems with consensus-based controllers[J]. Advances in Unmanned Aerial Vehicles, Intelligent Systems, Control and Automation: Science and Engineering, 2007, 33: 455-471.

[21] Bazoula A, Djouadi M S, Maaref H. Formation control of multi-robots via fuzzy logic technique[J]. International Journal of Computers, Communications & Control, 2008, 3(3): 179-184.

[22] Amoozgar M H, Alipour K, Sadati S H. A fuzzy logic-based formation controller for wheeled mobile robots[J]. Industrial Robot: an International Journal, 2011, 38(3): 269-281.

[23] Liao Y, Huang J, Zeng Q. Preview fuzzy control method for intelligent vehicle path tracking[C] ∥Proceeding of 2010 IEEE International Conference on Progress in Informatics and Computing (PIC), IEEE, 2010, 2: 1211-1214.

[24] Antonelli G, Chiaverini S, Fusco G. A fuzzy-logic-based approach for mobile robot path tracking[J]. IEEE Transactions on, Fuzzy Systems, 2007, 15(2): 211-221.

(責(zé)任編輯李進(jìn))

Formation Control of Multiple Robotic Fish Based on Distributed Estimation and Fuzzy Control

LI Zonggang1,2， GUO Zhilong1， SHI Huirong1, XIE Guangming2

(1.School of Mechanic and Electronic Engineering, Lanzhou Jiaotong University, Lanzhou 730070, China;2.Center for Systems and Control, College of Engineering, Peking University, Beijing 100871, China)

This paper proposed a distributed and hierarchical scheme for formation control of multiple robotic fish. Firstly, each robotic fish takes consensus algorithm as a distributed data fusion modular to estimate the center of the formation, and then calculate its desired position in the formation at the next time. Such estimated results are the input variables of its path planning modular. Secondly, each robotic fish tracking its desired position by employing the fuzzy controller of linear speed and angular speed in order to form and maintain formation. The proposed method separates the distributed estimation and the path planning and behavior of robotic fish, such a property gives a new method for the cooperative control of multiple heterogeneous robotic fish. The results of experiment shows the validity of the proposed method.

biomimetic robotic fish; formation control; consensus; fuzzy control; trajectory tracking

1672-3813(2016)03-0040-07;DOI:10.13306/j.1672-3813.2016.03.006

2015-06-05；

2015-12-31

國家自然科學(xué)基金(61064008);教育部科學(xué)技術(shù)研究重點(diǎn)項(xiàng)目(211185);甘肅省自然科學(xué)基金(1112RJZA042，1208RJZA166)

李宗剛(1975-)，男，寧夏西吉人，博士，副教授，主要研究方向?yàn)閺?fù)雜系統(tǒng)動(dòng)力學(xué)分析與控制、智能仿生機(jī)器人動(dòng)力學(xué)及控制、智能信息處理。

TP273

A