基于分布式估計與模糊控制的多機器魚編隊控制

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

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

?

基于分布式估計與模糊控制的多機器魚編隊控制

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

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

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

仿生機器魚;編隊;一致性算法;模糊控制;軌跡跟蹤

0　引言

近年來，隨著海洋重要性的日益凸顯，海洋中的地圖構建、生態監測、水中偵測等領域的需求使得多水下機器人系統的協調控制問題成為多機器人系統研究的熱點[1-9]。針對水環境的動態特性所造成位姿的不確定性，Zhang等[3]利用提出的極限環方法控制機器魚位姿和避免碰撞，利用模糊控制器控制方向，研究了多魚協作搬運任務。文獻[4]研究了多機器魚協作爆破問題，其中利用遺傳算法優化機器魚之間的權重因子，進而利用改進的匈牙利算法實現了任務分配。Shao等[5]利用機器魚的機構特點，利用曲率坐標系建立了個體動力學模型，進而基于領航者-跟隨者方法研究了多機器魚編隊控制問題。Zou等[6]利用基于徑向基神經網絡的滑模控制方法研究了多機器魚的軌跡跟蹤問題。Zhao等[7]基于領航者-跟隨者方法研究了多自主機器魚的編隊的控制問題。其中，機器魚線速度和角速度分別通過模糊控制器和期望Bezier軌線曲率優化方法進行控制。Hu等[10]提出了一種基于勞動分工方法的多自主機器魚搬運控制策略。此外，文獻[8]～[9]基于行為技術研究了多水下機器人編隊控制問題。需要指出的是，文獻[3]～[9]中的協調控制算法都是集中式的，也即機器魚之間不存在信息交互,文獻[10]的算法是分布式的，其特點更近于通過公平競爭的方式完成協作任務。

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

1　預備知識

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

(1)

其中，θi表示機器魚頭尾軸與x軸的夾角，υi∈[0,υmax]表示機器魚巡游速度，ωi∈[0,ωmax]表示機器魚轉彎角速度。機器魚之間的信息交換拓撲滿足假設1。

假設1設有向圖G為N條機器魚組成的多智能體系統的信息交換拓撲，則G含有生成樹，且在任意時刻至少有一個根節點可以接收到參考軌跡的位姿信息ξ0(t)。

2　多仿生機器魚編隊控制

考慮N個多仿鲹科三關節機器魚的編隊控制問題，機器魚的體波曲線由Lighthill方程

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

(2)

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

2.1編隊參考點一致性估計

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

由于所考慮的多機器魚系統信息交換拓撲滿足假設1，為了實現編隊，各機器魚必須基于局部信息實現編隊任務。由于機器魚vi在編隊中的位姿是確定的，因此可以根據其當前位姿估計出編隊參考點位姿ζi(t)，并同相鄰機器魚共享這一信息。基于這一事實，利用一致性算法對編隊參考點進行分布式估計，協議為

(3)

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

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

2.2多機器魚編隊形成與保持

(4)

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

3　多機器魚編隊路徑跟蹤模糊控制器

如何克服水環境的動態特性對多機器魚編隊形成、保持與軌跡跟蹤帶來的不確定性，是實現上述任務的關鍵。受文獻[3],[21]～[24]的啟發，采用模糊控制方法對機器魚的運動方向和速度進行控制。

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

(5)

易得期望位姿的方向角為

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

(6)

控制目標即是使得由式(6)及式(7)確定的誤差信號為零。機器魚vi的角速度ωi及速度υi的模糊控制器結構如圖5所示。

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

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

(7)

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

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

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

與此相對應，輸出ωi由以下模糊規則得到：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。

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

(8)

機器魚vi通過模糊控制器獲得其下一時刻的巡游速度υi和角速度ωi之后，即依據式(3)選擇合適模態進行游動，以達到保持編隊和跟蹤期望軌跡的目標。

4　實驗

4.1單仿生機器魚路徑跟蹤

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

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

4.2多機器魚編隊形成與保持

考慮3條仿生機器魚的三角編隊及軌跡跟蹤問題，實驗環境為的室內水池，機器魚初始位置在水池左側隨機布置，編隊中心參考軌跡為直線。3條仿生機器魚在水池中的運動場景如圖13所示。由圖可知，3條仿生魚在8s時已形成期望隊形，并在后續過程中保持了隊形。圖14為3條機器魚的運動軌跡圖，可以看出，編隊在跟蹤一直線時表現比較穩定。圖15給出了各機器魚在游動中的線速度變化情況，其中紅線代表編隊中心的期望速度。由圖可知，為了保持編隊，機器魚的游動速度始終有微小的波動。

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

5　結論

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

[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]. 哈爾濱工程大學學報，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] 范士波, 王宏健. 多自主水下航行器編隊控制技術研究[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] 張博, 羅建軍, 袁建平. 一種基于信息一致性的衛星編隊協同控制策略[J]. 航空學報, 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.

(責任編輯李進)

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

國家自然科學基金(61064008);教育部科學技術研究重點項目(211185);甘肅省自然科學基金(1112RJZA042，1208RJZA166)

李宗剛(1975-)，男，寧夏西吉人，博士，副教授，主要研究方向為復雜系統動力學分析與控制、智能仿生機器人動力學及控制、智能信息處理。

TP273

A