第8卷第3期 智能系统学报 Vol.8 No.3 2013年6月 CAAI Transactions on Intelligent Systems Jun.2013 D0I:10.3969/i.issn.1673-4785.201210019 网络出版地址：http://www.cnki.net/kcms/detail/23.1538.TP.20130515.0836.001.html 采用Leader-follower和模糊反馈机制的 机器鱼队形控制 单素素，纪志坚，翟海川 (青岛大学自动化工程学院，山东青岛266071) 摘要：在Leader-follower框架下的多机器鱼系统队形控制中，针对Leader和Follower相对独立及落后机器鱼离队等 问题，在传统的Leader--follower队形控制算法中引入模糊反馈控制器进行反馈调节，使得领航机器鱼Leader能定时 检查Follower的位置，将多机器鱼系统的队形控制问题转化为具有反馈调节的Follower跟踪Leader的位置和方向的 问题，以增强多机器鱼队形控制的稳定性，更好地协同作业完成任务.仿真试验验证了所提出的方法能较好地实现多 机器鱼系统的队形控制： 关键词：多机器鱼系统：队形控制：Leader-follower:模糊反馈控制器：反馈调节：协同作业 中图分类号：TP242.6文献标志码：A文章编号：1673-4785(2013)03-0247-07 中文引用格式：单素素，纪志坚，翟海川.采用Leader--follower和模糊反馈机制的机器鱼队形控制[J].智能系统学报，2013,8(3)： 247-253. 英文引用格式：SHAN Susu,JI Zhijian,ZHAI Haichuan.The formation control of multi-robot fish based on leader-follower and fuzzy feedback mechanism[J].CAAI Transactions on Intelligent Systems,2013,8(3):247-253. The formation control of multi-robot fish based on leader-follower and fuzzy feedback mechanism SHAN Susu,JI Zhijian,ZHAI Haichuan (College of Automation Engineering,Qingdao University,Qingdao 266071,China) Abstract:In the formation control of multi-robot fish under the leader-follower framework,the fuzzy feedback con- troller was introduced to traditional leader-follower formation control algorithm in efforts to cope with the relative in- dependence of the leader and followers,as well as the desertion of the laggard robot fishes.The goal was to ensure that the pilot robotic fish could check the position of followers regularly.In order to enhance the stability of the for- mation control of multi-robot fish and complete the task of collaborative operation,the formation control problem of multi-robot fish system was transformed into the problem of followers'tracking the position and orientation of the leader with the addition of the feedback regulation.Simulations verify that the proposed method can better realize the formation control of the robotic fish system. Keywords:multi-robot fish system;formation control;leader-follower;fuzzy feedback controller;feedback regula- tion;collaborative operation 在多机器鱼系统中协作是必不可少的，而机器 问题，是多机器鱼协作的基础).无论是从提升机器 鱼的队形控制则是一个具有典型性和通用性的协作 鱼的智能化水平还是从加快其实用化进程来说，多 机器鱼队形控制研究都有着及其重要的意义和价 收稿日期：2012-10-12.网络出版日期：2013-05-15. 值2 基金项目：国家自然科学基金资助项目(61075114,41076062). 多条机器鱼通过队形控制策略，能在某种环境 通信作者：纪志坚.E-mail:jizhijian@pku.og.cn
第 ８ 卷第 ３ 期 智 能 系 统 学 报 Ｖｏｌ．８ №．３ ２０１３ 年 ６ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ｊｕｎ． ２０１３ ＤＯＩ：１０．３９６９ ／ ｊ．ｉｓｓｎ．１６７３⁃４７８５．２０１２１００１９ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ＴＰ．２０１３０５１５．０８３６．００１．ｈｔｍｌ 采用 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 和模糊反馈机制的 机器鱼队形控制 单素素，纪志坚，翟海川 （青岛大学 自动化工程学院，山东 青岛 ２６６０７１） 摘 要：在 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 框架下的多机器鱼系统队形控制中，针对 Ｌｅａｄｅｒ 和 Ｆｏｌｌｏｗｅｒ 相对独立及落后机器鱼离队等 问题，在传统的 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队形控制算法中引入模糊反馈控制器进行反馈调节，使得领航机器鱼 Ｌｅａｄｅｒ 能定时 检查 Ｆｏｌｌｏｗｅｒ 的位置，将多机器鱼系统的队形控制问题转化为具有反馈调节的 Ｆｏｌｌｏｗｅｒ 跟踪 Ｌｅａｄｅｒ 的位置和方向的 问题，以增强多机器鱼队形控制的稳定性，更好地协同作业完成任务．仿真试验验证了所提出的方法能较好地实现多 机器鱼系统的队形控制． 关键词：多机器鱼系统；队形控制；Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ；模糊反馈控制器；反馈调节；协同作业 中图分类号：ＴＰ２４２．６ 文献标志码：Ａ 文章编号：１６７３⁃４７８５（２０１３）０３⁃０２４７⁃０７ 中文引用格式：单素素，纪志坚，翟海川．采用 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 和模糊反馈机制的机器鱼队形控制［ Ｊ］．智能系统学报， ２０１３， ８（ ３）： ２４７⁃２５３． 英文引用格式：ＳＨＡＮ Ｓｕｓｕ， ＪＩ Ｚｈｉｊｉａｎ， ＺＨＡＩ Ｈａｉｃｈｕａｎ．Ｔｈｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ｂａｓｅｄ ｏｎ ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ ａｎｄ ｆｕｚｚｙ ｆｅｅｄｂａｃｋ ｍｅｃｈａｎｉｓｍ［Ｊ］． ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ， ２０１３， ８（３）： ２４７⁃２５３． Ｔｈｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ｂａｓｅｄ ｏｎ ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ ａｎｄ ｆｕｚｚｙ ｆｅｅｄｂａｃｋ ｍｅｃｈａｎｉｓｍ ＳＨＡＮ Ｓｕｓｕ， ＪＩ Ｚｈｉｊｉａｎ， ＺＨＡＩ Ｈａｉｃｈｕａｎ （Ｃｏｌｌｅｇｅ ｏｆ Ａｕｔｏｍａｔｉｏｎ Ｅｎｇｉｎｅｅｒｉｎｇ， Ｑｉｎｇｄａｏ Ｕｎｉｖｅｒｓｉｔｙ， Ｑｉｎｇｄａｏ ２６６０７１， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎ ｔｈｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ｕｎｄｅｒ ｔｈｅ ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ ｆｒａｍｅｗｏｒｋ， ｔｈｅ ｆｕｚｚｙ ｆｅｅｄｂａｃｋ ｃｏｎ⁃ ｔｒｏｌｌｅｒ ｗａｓ ｉｎｔｒｏｄｕｃｅｄ ｔｏ ｔｒａｄｉｔｉｏｎａｌ ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ａｌｇｏｒｉｔｈｍ ｉｎ ｅｆｆｏｒｔｓ ｔｏ ｃｏｐｅ ｗｉｔｈ ｔｈｅ ｒｅｌａｔｉｖｅ ｉｎ⁃ ｄｅｐｅｎｄｅｎｃｅ ｏｆ ｔｈｅ ｌｅａｄｅｒ ａｎｄ ｆｏｌｌｏｗｅｒｓ， ａｓ ｗｅｌｌ ａｓ ｔｈｅ ｄｅｓｅｒｔｉｏｎ ｏｆ ｔｈｅ ｌａｇｇａｒｄ ｒｏｂｏｔ ｆｉｓｈｅｓ． Ｔｈｅ ｇｏａｌ ｗａｓ ｔｏ ｅｎｓｕｒｅ ｔｈａｔ ｔｈｅ ｐｉｌｏｔ ｒｏｂｏｔｉｃ ｆｉｓｈ ｃｏｕｌｄ ｃｈｅｃｋ ｔｈｅ ｐｏｓｉｔｉｏｎ ｏｆ ｆｏｌｌｏｗｅｒｓ ｒｅｇｕｌａｒｌｙ． Ｉｎ ｏｒｄｅｒ ｔｏ ｅｎｈａｎｃｅ ｔｈｅ ｓｔａｂｉｌｉｔｙ ｏｆ ｔｈｅ ｆｏｒ⁃ ｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ａｎｄ ｃｏｍｐｌｅｔｅ ｔｈｅ ｔａｓｋ ｏｆ ｃｏｌｌａｂｏｒａｔｉｖｅ ｏｐｅｒａｔｉｏｎ， ｔｈｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｐｒｏｂｌｅｍ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ｓｙｓｔｅｍ ｗａｓ ｔｒａｎｓｆｏｒｍｅｄ ｉｎｔｏ ｔｈｅ ｐｒｏｂｌｅｍ ｏｆ ｆｏｌｌｏｗｅｒｓ’ ｔｒａｃｋｉｎｇ ｔｈｅ ｐｏｓｉｔｉｏｎ ａｎｄ ｏｒｉｅｎｔａｔｉｏｎ ｏｆ ｔｈｅ ｌｅａｄｅｒ ｗｉｔｈ ｔｈｅ ａｄｄｉｔｉｏｎ ｏｆ ｔｈｅ ｆｅｅｄｂａｃｋ ｒｅｇｕｌａｔｉｏｎ． Ｓｉｍｕｌａｔｉｏｎｓ ｖｅｒｉｆｙ ｔｈａｔ ｔｈｅ ｐｒｏｐｏｓｅｄ ｍｅｔｈｏｄ ｃａｎ ｂｅｔｔｅｒ ｒｅａｌｉｚｅ ｔｈｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ｔｈｅ ｒｏｂｏｔｉｃ ｆｉｓｈ ｓｙｓｔｅｍ． Ｋｅｙｗｏｒｄｓ： ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ｓｙｓｔｅｍ； ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ； ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ； ｆｕｚｚｙ ｆｅｅｄｂａｃｋ ｃｏｎｔｒｏｌｌｅｒ； ｆｅｅｄｂａｃｋ ｒｅｇｕｌａ⁃ ｔｉｏｎ； ｃｏｌｌａｂｏｒａｔｉｖｅ ｏｐｅｒａｔｉｏｎ 收稿日期：２０１２⁃１０⁃１２． 网络出版日期：２０１３⁃０５⁃１５． 基金项目：国家自然科学基金资助项目（６１０７５１１４，４１０７６０６２）． 通信作者：纪志坚． Ｅ⁃ｍａｉｌ： ｊｉｚｈｉｊｉａｎ＠ ｐｋｕ．ｏｒｇ．ｃｎ． 在多机器鱼系统中协作是必不可少的，而机器 鱼的队形控制则是一个具有典型性和通用性的协作 问题，是多机器鱼协作的基础［１］ ．无论是从提升机器 鱼的智能化水平还是从加快其实用化进程来说，多 机器鱼队形控制研究都有着及其重要的意义和价 值［２⁃３］ ． 多条机器鱼通过队形控制策略，能在某种环境
·248· 智能系统学报 第8卷 约束下，始终保持一定的队形4)前进.这种策略一 状态，确保它必须向目标位置行进.构建Leader的运 般包含2个步骤：1)依据所处环境，确定各机器鱼 动模型6，如图1所示。 的目标位置：2)再由控制策略得到控制命令，机器 鱼在控制命令的约束下便能以一定的队形运行到目 Goal 标位置目前，大致有3种方法可以应用于机器鱼队 形控制，即Leader--follower算法[8】、基于行为算法列 和虚拟结构法[o.其中，Leader--follower算法是将队 形中的机器鱼划分为Leader和Follower2个角色， Follower的运行状态由Leader决定，进而实现队形 控制.这种控制方法比较简单，但因Leader与Follo- ver之间没有信息反馈，相对独立，在复杂环境中 图1 Leader的运动模型 Follower在跟踪过程中容易丢失[).一些文献中给出 Fig.I Movement model of Leader 了针对该算法缺点的解决方法，文献[11]应用精确 设定O点为机器鱼的参考原点，根据Leader的 反馈线性化方法设计控制器来确保编队的渐近稳 运动模型，并考虑机器鱼有效旋转半径d的影响，可 定.本文则是在Leader-follower算法的基础上引入模 得式(1)【6： 糊反馈控制器，使得Follower能够实时地将队形信 息通过模糊控制器反馈给Leader,确保Leader及时 dw,sin o-v,cos 调整自身运行状态，使落后或离队的Follower可以 v,sin dw,cos o (1) 9= -10 通过反馈调节作用重新回归队伍，以更好地实现队 r r 形控制。 式中：r为机器鱼参考原点与目标点的距离，”为机 器鱼中心线与目标点的夹角， 1基本的Leader-follower队形控制策略 控制的最终目标是使Leader到达目标点，即r= 由多条机器鱼组成一个群体中，一条机器鱼为 0,p=0.可以将控制策略定义为： Leader,其他的鱼均为Follower,各个Follower与 =-&r; Leader都有一定的相对位置关系，并同时跟踪Lead- (2) er的位置及方向[].Leader可以控制整个系统的运 9=-2P 动趋势，其状态由所有Follower共享[)]，由此实现 式中：a,、α2是给定的比例系数，二者均为正数.将式 多机器鱼的队形控制. (1)、(2)联立可以解得控制变量为： 基本的Leader--follower队形控制策略还衍生出 a sin o a,ocos 很多扩展形式，比如单一Leader的l-p算法、多个 -r; reos o-d Leader的l-l算法以及跟踪链算法等.目前，基于 a(r-dcos o)+doa sin Leader-follower队形控制中多采用l-p控制方法[) rcos o-d 而编队队形中按跟踪的拓扑结构，又可分为串联编 考虑到(vs,0)的限制，控制变量应为： 队和并联编队.在串联编队结构中，每个Follower根 据与它相邻近的机器鱼来确定自己的位置：并联编 0,=了0， 一10ms≤10，≤0mx; 队结构中，Follower则是根据Leader的位置去确定 -10 tmas 0,<一Wm 自身位置.文献[15]对比分析了这2种拓扑结构的 Vt Uimaxi 稳定性，在群体中只存在单一Leader时，并联编队 v,=301, 一Umx≤飞，≤Vms; 的稳定度要优于串联编队.在本文中，采用并联结构 来对机器鱼进行控制. -Vma,U <-Vimas" 1.2 队形保持 l.1 Leader向目标点运行 本文采用l-p控制的方法来保持队形.它是 在Leader-follower队形控制策略中，Leader作 通过控制2条机器鱼之间的相对距离！和相对角度 为这个队形的参照物，对整个队列能否准确运行到 p,使之达到一对理想值(L4,P),从而形成和保持一 目标位置起决定作用.在这里先分析Leader的运行 个良好的编队队形，如图2所示
约束下，始终保持一定的队形［４⁃７］ 前进．这种策略一 般包含 ２ 个步骤：１） 依据所处环境，确定各机器鱼 的目标位置；２） 再由控制策略得到控制命令，机器 鱼在控制命令的约束下便能以一定的队形运行到目 标位置．目前，大致有 ３ 种方法可以应用于机器鱼队 形控制，即 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 算法［８］ 、基于行为算法［９］ 和虚拟结构法［１０］ ．其中，Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 算法是将队 形中的机器鱼划分为 Ｌｅａｄｅｒ 和 Ｆｏｌｌｏｗｅｒ ２ 个角色， Ｆｏｌｌｏｗｅｒ 的运行状态由 Ｌｅａｄｅｒ 决定，进而实现队形 控制．这种控制方法比较简单，但因 Ｌｅａｄｅｒ 与 Ｆｏｌｌｏ⁃ ｗｅｒ 之间没有信息反馈，相对独立，在复杂环境中 Ｆｏｌｌｏｗｅｒ 在跟踪过程中容易丢失［８］ ．一些文献中给出 了针对该算法缺点的解决方法，文献［１１］应用精确 反馈线性化方法设计控制器来确保编队的渐近稳 定．本文则是在 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 算法的基础上引入模 糊反馈控制器，使得 Ｆｏｌｌｏｗｅｒ 能够实时地将队形信 息通过模糊控制器反馈给 Ｌｅａｄｅｒ，确保 Ｌｅａｄｅｒ 及时 调整自身运行状态，使落后或离队的 Ｆｏｌｌｏｗｅｒ 可以 通过反馈调节作用重新回归队伍，以更好地实现队 形控制． １ 基本的 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队形控制策略 由多条机器鱼组成一个群体中，一条机器鱼为 Ｌｅａｄｅｒ，其 他 的 鱼 均 为 Ｆｏｌｌｏｗｅｒ， 各 个 Ｆｏｌｌｏｗｅｒ 与 Ｌｅａｄｅｒ 都有一定的相对位置关系，并同时跟踪 Ｌｅａｄ⁃ ｅｒ 的位置及方向［１２］ ．Ｌｅａｄｅｒ 可以控制整个系统的运 动趋势，其状态由所有 Ｆｏｌｌｏｗｅｒ 共享［１３］ ，由此实现 多机器鱼的队形控制． 基本的 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队形控制策略还衍生出 很多扩展形式，比如单一 Ｌｅａｄｅｒ 的 ｌ⁃φ 算法、多个 Ｌｅａｄｅｒ 的 ｌ⁃ｌ 算法以及跟踪链算法等． 目前，基于 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队形控制中多采用 ｌ⁃φ 控制方法［１４］ ． 而编队队形中按跟踪的拓扑结构，又可分为串联编 队和并联编队．在串联编队结构中，每个 Ｆｏｌｌｏｗｅｒ 根 据与它相邻近的机器鱼来确定自己的位置；并联编 队结构中，Ｆｏｌｌｏｗｅｒ 则是根据 Ｌｅａｄｅｒ 的位置去确定 自身位置．文献［１５］对比分析了这 ２ 种拓扑结构的 稳定性，在群体中只存在单一 Ｌｅａｄｅｒ 时，并联编队 的稳定度要优于串联编队．在本文中，采用并联结构 来对机器鱼进行控制． １．１ Ｌｅａｄｅｒ 向目标点运行 在 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队形控制策略中， Ｌｅａｄｅｒ 作 为这个队形的参照物，对整个队列能否准确运行到 目标位置起决定作用．在这里先分析 Ｌｅａｄｅｒ 的运行 状态，确保它必须向目标位置行进．构建 Ｌｅａｄｅｒ 的运 动模型［１６］ ，如图 １ 所示． 图 １ Ｌｅａｄｅｒ 的运动模型 Ｆｉｇ．１ Ｍｏｖｅｍｅｎｔ ｍｏｄｅｌ ｏｆ Ｌｅａｄｅｒ 设定 Ｏ 点为机器鱼的参考原点，根据 Ｌｅａｄｅｒ 的 运动模型，并考虑机器鱼有效旋转半径 ｄ 的影响，可 得式（１） ［１６］ ： ｒ · ＝ ｄｗｔ ｓｉｎ φ － ｖｔ ｃｏｓ φ； φ · ＝ ｖｔ ｓｉｎ φ ｒ ＋ ｄｗｔ ｃｏｓ φ ｒ － ｗｔ ． ì î í ï ï ï ï （１） 式中：ｒ 为机器鱼参考原点与目标点的距离，φ 为机 器鱼中心线与目标点的夹角． 控制的最终目标是使 Ｌｅａｄｅｒ 到达目标点，即ｒ ＝ ０，φ＝ ０．可以将控制策略定义为： ｒ · ＝ － α１ ｒ； φ · ＝ － α２φ． ì î í ïï ïï （２） 式中：α１ 、α２ 是给定的比例系数，二者均为正数．将式 （１）、（２）联立可以解得控制变量为： ｗｔ ＝ α１ ｓｉｎ φ ＋ α２φｃｏｓ φ ｒｃｏｓ φ － ｄ ｒ； ｖｔ ＝ α１ (ｒ － ｄｃｏｓ φ) ＋ ｄφα２ ｓｉｎ φ ｒｃｏｓ φ － ｄ ｒ． ì î í ï ïï ï ï 考虑到（ｖｔｍａｘ，ｗｔｍａｘ）的限制，控制变量应为： ｗｔ ＝ ｗｔｍａｘ， ｗｔ， － ｗｔｍａｘ， ì î í ï ï ï ï ｗｔ ＞ ｗｔｍａｘ； － ｗｔｍａｘ ≤ ｗｔ ≤ ｗｔｍａｘ； ｗｔ ＜ － ｗｔｍａｘ ． ｖｔ ＝ ｖｔｍａｘ， ｖｔ， － ｖｔｍａｘ， ì î í ï ï ï ï ｖｔ ＞ ｖｔｍａｘ； － ｖｔｍａｘ ≤ ｖｔ ≤ ｖｔｍａｘ； ｖｔ ＜ － ｖｔｍａｘ ． １．２ 队形保持 本文采用 ｌ⁃φ 控制的方法［１４］ 来保持队形．它是 通过控制 ２ 条机器鱼之间的相对距离 ｌ 和相对角度 φ，使之达到一对理想值（ｌ ｄ ，φｄ ），从而形成和保持一 个良好的编队队形，如图 ２ 所示． ·２４８· 智 能 系 统 学 报 第 ８ 卷
第3期 单素素，等：采用Leader-follower和模糊反馈机制的机器鱼队形控制 ·249· 一ma≤,≤t书ms; <-Vmas' 由此可以看出，在l-算法中，只要给定领航机 器鱼的角速度、线速度、位置和航向，跟随者就可以 保证向着与领航机器鱼相对距离为1和相对方向为 P的位置移动，实现队形的保持 2基于Leader-follower算法引入反馈 调节的多机器鱼队形控制 图2基于lp的队形保持 2.1基本思想 Fig.2 Formation keeping based on 在传统的Leader-follower算法中，机器鱼系统 下面以一个由2条机器鱼组成的系统为例，给 的队形控制简单且稳定，但Leader和Follower相对 出该算法的详细说明.在控制过程中，领航机器鱼 独立，在复杂环境中易出现离队现象.本文在保留 Leader的状态表示为R,(xo,o,o,wo),跟随机器鱼 Leader--follower算法优点的基础上引入反馈环节，使 Follower的状态表示为R,(x,y,,,),机器鱼线速 机器鱼队伍在复杂环境中能更好地保持队形并避开 度的正方向为机器鱼前行的方向，角速度的正方向 障碍物，多机器鱼的队形控制更加有效.具有反馈调 为逆时针方向.根据队形要求，Follower相对于Lead- 节的队形控制结构如图3所示 er的相对参数为(l,p),如图2所示，可得式 (3)[161: ET i=vcosr-du,sin r -vocosdsin Leader ES (vosin dcos -ysin r -du,cosr). E E E Follower(1) Follower(2) Follower(n) (3) 式中：wo、e,分别为Leader和Follower的角速度，O。、 模糊控制器 O,分别为Leader和Follower运动方向与正x轴的夹 图3具有反馈调节的Leader-follower队形控制结构 角，由几何知识可知r=p+0。-0,l4、p。分别为队形 Fig.3 Leader-follower formation control structure with 要求中Follower相对于Leader的理想距离和方位 feedback regulation 角.为使2条机器鱼保持一定的队形，控制的最终目 2.2基本原理 标为(l-l)→0，(p-p)→0.将控制策略定义为： 本文设计的具有反馈调节的Leader-follower队 i=B,(l4-): 形控制的基本原理如下： (4) 1)开始时Leader接受任务T,然后根据外界环 o=B(o-o). 境E规划运动路径，并将自身的运行状态R。(xoyo 将式(3)、(4)联立可解得控制变量为： vo,wo)发送给每个Follower.. w,=-C [Bl(-)-rosin 2)每个Follower根据接收到的信息计算自己在 队形中的相对位置P:=(x,y)(i=1,2,…,n),结合 dwocos lwo psin r]; 外界环境E,根据队形控制策略产生控制变量，即 v=p dw,tan r. 式中p=B,(L-）0-duoin p (:,0:） 3)队形信息P=(P1P2,…P)通过模糊控制器 cos r 反馈给Leader,其中p,为Follower(I)的相对位置. 同样，考虑到(mx,0mx)的限制，控制变量应 正常运行时Leader不将队形反馈信息作为自己运 变为： 行的依据，仅依靠Follower自身调整运行状态来提 1,>1Umm; 高队形保持率.当个别机器鱼有离队现象时，Leader 10,= -0mx≤10，≤0mx; 开始低速前进，同时离队的Follower开始高速前进， 10,<一10mx 使得落后的机器鱼能够回归队伍，防止Follower掉
图 ２ 基于 ｌ⁃φ 的队形保持 Ｆｉｇ．２ Ｆｏｒｍａｔｉｏｎ ｋｅｅｐｉｎｇ ｂａｓｅｄ ｏｎ ｌ⁃φ 下面以一个由 ２ 条机器鱼组成的系统为例，给 出该算法的详细说明．在控制过程中，领航机器鱼 Ｌｅａｄｅｒ 的状态表示为 Ｒ０（ｘ０ ，ｙ０ ，ｖ０ ，ｗ０ ），跟随机器鱼 Ｆｏｌｌｏｗｅｒ 的状态表示为 Ｒｔ（ｘｔ，ｙｔ，ｖｔ，ｗｔ），机器鱼线速 度的正方向为机器鱼前行的方向，角速度的正方向 为逆时针方向．根据队形要求，Ｆｏｌｌｏｗｅｒ 相对于 Ｌｅａｄ⁃ ｅｒ 的 相 对 参 数 为 （ ｌ， φ）， 如 图 ２ 所 示， 可 得 式 （３） ［１６］ ： ｌ · ＝ ｖｔ ｃｏｓ ｒ － ｄｗｔ ｓｉｎ ｒ － ｖ０ ｃｏｓ φ ＋ ｄｗ０ ｓｉｎ φ； φ · ＝ １ ｌ （ｖ０ ｓｉｎ φ ＋ ｄｗ０ ｃｏｓ φ － ｖｔ ｓｉｎ ｒ － ｄｗｔ ｃｏｓ ｒ － ｌｗ０）． ì î í ï ï ï ï （３） 式中：ｗ０ 、ｗｔ 分别为 Ｌｅａｄｅｒ 和 Ｆｏｌｌｏｗｅｒ 的角速度，θ０ 、 θｔ 分别为 Ｌｅａｄｅｒ 和 Ｆｏｌｌｏｗｅｒ 运动方向与正 ｘ 轴的夹 角，由几何知识可知 ｒ ＝ φ＋θ０ －θｔ，ｌ ｄ 、φｄ 分别为队形 要求中 Ｆｏｌｌｏｗｅｒ 相对于 Ｌｅａｄｅｒ 的理想距离和方位 角．为使 ２ 条机器鱼保持一定的队形，控制的最终目 标为（ｌ－ｌ ｄ ）→０，（φ－φｄ ）→０．将控制策略定义为： ｌ · ＝ β１（ｌ ｄ － ｌ）； φ · ＝ β２（φｄ － φ）． ì î í ïï ï （４） 将式（３）、（４）联立可解得控制变量为： ｗｔ ＝ － ｃｏｓ ｒ ｄ ［β２ ｌ（φｄ － φ） － ｖ０ ｓｉｎ φ － ｄｗ０ ｃｏｓ φ ＋ ｌｗ０ ＋ ρｓｉｎ ｒ］； ｖｔ ＝ ρ ＋ ｄｗｔ ｔａｎ ｒ． ì î í ï ï ï ï 式中：ρ ＝ β１（ｌ ｄ －ｌ）＋ｖ０ ｃｏｓ φ－ｄｗ０ ｓｉｎ φ ｃｏｓ ｒ ． 同样，考虑到（ ｖｔｍａｘ，ｗｔｍａｘ ） 的限制，控制变量应 变为： ｗｔ ＝ ｗｔｍａｘ， ｗｔ， － ｗｔｍａｘ， ì î í ï ï ïï ｗｔ ＞ ｗｔｍａｘ； － ｗｔｍａｘ ≤ ｗｔ ≤ ｗｔｍａｘ； ｗｔ ＜ － ｗｔｍａｘ ． ｖｔ ＝ ｖｔｍａｘ， ｖｔ， － ｖｔｍａｘ， ì î í ï ï ïï ｖｔ ＞ ｖｔｍａｘ； － ｖｔｍａｘ ≤ ｖｔ ≤ ｖｔｍａｘ； ｖｔ ＜ － ｖｔｍａｘ ． 由此可以看出，在 ｌ⁃φ 算法中，只要给定领航机 器鱼的角速度、线速度、位置和航向，跟随者就可以 保证向着与领航机器鱼相对距离为 ｌ 和相对方向为 φ 的位置移动，实现队形的保持． ２ 基于 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 算法引入反馈 调节的多机器鱼队形控制 ２．１ 基本思想 在传统的 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 算法中，机器鱼系统 的队形控制简单且稳定，但 Ｌｅａｄｅｒ 和 Ｆｏｌｌｏｗｅｒ 相对 独立，在复杂环境中易出现离队现象．本文在保留 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 算法优点的基础上引入反馈环节，使 机器鱼队伍在复杂环境中能更好地保持队形并避开 障碍物，多机器鱼的队形控制更加有效．具有反馈调 节的队形控制结构如图 ３ 所示． 图 ３ 具有反馈调节的 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队形控制结构 Ｆｉｇ．３ Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｓｔｒｕｃｔｕｒｅ ｗｉｔｈ ｆｅｅｄｂａｃｋ ｒｅｇｕｌａｔｉｏｎ ２．２ 基本原理 本文设计的具有反馈调节的 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 队 形控制的基本原理如下： １）开始时 Ｌｅａｄｅｒ 接受任务 Ｔ，然后根据外界环 境 Ｅ 规划运动路径，并将自身的运行状态 Ｒ０（ｘ０ ，ｙ０ ， ｖ０ ，ｗ０ ）发送给每个 Ｆｏｌｌｏｗｅｒ． ２）每个 Ｆｏｌｌｏｗｅｒ 根据接收到的信息计算自己在 队形中的相对位置 ｐｉ ＝ （ｘｉ，ｙｉ）（ｉ ＝ １，２，…，ｎ），结合 外界环境 Ｅ，根据队形控制策略产生控制变量，即 （ｖｉ，ｗｉ）． ３）队形信息 Ｐ ＝ （ｐ１ ，ｐ２ ，…，ｐｎ ）通过模糊控制器 反馈给 Ｌｅａｄｅｒ，其中 ｐ１ 为 Ｆｏｌｌｏｗｅｒ（１） 的相对位置． 正常运行时 Ｌｅａｄｅｒ 不将队形反馈信息作为自己运 行的依据，仅依靠 Ｆｏｌｌｏｗｅｒ 自身调整运行状态来提 高队形保持率．当个别机器鱼有离队现象时，Ｌｅａｄｅｒ 开始低速前进，同时离队的 Ｆｏｌｌｏｗｅｒ 开始高速前进， 使得落后的机器鱼能够回归队伍，防止 Ｆｏｌｌｏｗｅｒ 掉 第 ３ 期 单素素，等：采用 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 和模糊反馈机制的机器鱼队形控制 ·２４９·
.250. 智能系统学报 第8卷 队.在特殊情况下，如等待时间t>t,而Follower仍不 2.3反馈控制器的设计 能跟上Leader,这时Leader会放弃故障机器鱼，恢 本文以一个由2条机器鱼所组成的系统为例进 复正常运行 行反馈控制器的设计.所设计的反馈控制器有4个 队形保持算法流程图如图4所示. 输入变量，分别对应2个输出变量，而且这2个输出 变量是并列关系.首先使用1时刻跟随者与领航者 (开始 之间的理想距离与实际距离之差L(t)=L-L,以及 差值变化率△L(t)作为输人变量，领航机器鱼在t Leader接受任务 时刻的速度档位V(t)作为输出变量.其中，L(t)的 Leader发送自 论域为[-30,30]，模糊集表示为{NB,NS,ZE,PS, 身运行状态 PB}:差值变化率△L(t)的论域为[-6,6]，模糊集 N 收到丢失信息 表示为{NB,NS,ZE,PS,PB};输出变量V(t)的论域 高Y 为[1,15]，模糊集表示为{VS,LS,ME,LF,VF}.输 低速前进 入输出各模糊控制变量的隶属度函数如图5所示. 发送数据包 NB NS ze PB 低速前时间 N N Olwe是否跟E Y后 -30-20-100 102030L(0 正常运行 (a)输入变量L(t) N 到达日标点一 Y (结束) NB NS ZE1 PS PB (a)Leader 开始 △L(t) Followert收到 开始信息 (b)输入变量△L(t) 检测到Leader Y VS ME VE TN 发送丢失信息 跟踪Leader 根据收到的数 据包高速前进 6 81012460 高速前进时 (c)输出变量V(t) 间>1， 图5距离输入输出变量的隶属度函数 Fig.5 Membership function of the position input varia- N 跟上Leader ble and position output variable 其次，以t时刻跟随者与领航者之间的理想方 结束 位角与实际方位角之差(t)=P。-p,以及差值变化 (b)Follower 率△(t)作为输入变量，领航机器鱼t时刻的方向 图4队形保持算法流程 档位P(t)作为输出变量.对差值(t)进行模糊化可 Fig.4 Algorithm flowchart of keeping formation 以得到{NB,NS,ZE,PS,PB}:对差值变化△(t)进
队．在特殊情况下，如等待时间 ｔ＞ｔ ｓ 而 Ｆｏｌｌｏｗｅｒ 仍不 能跟上 Ｌｅａｄｅｒ，这时 Ｌｅａｄｅｒ 会放弃故障机器鱼，恢 复正常运行． 队形保持算法流程图如图 ４ 所示． （ａ）Ｌｅａｄｅｒ （ｂ）Ｆｏｌｌｏｗｅｒ 图 ４ 队形保持算法流程 Ｆｉｇ．４ Ａｌｇｏｒｉｔｈｍ ｆｌｏｗｃｈａｒｔ ｏｆ ｋｅｅｐｉｎｇ ｆｏｒｍａｔｉｏｎ ２．３ 反馈控制器的设计 本文以一个由 ２ 条机器鱼所组成的系统为例进 行反馈控制器的设计．所设计的反馈控制器有 ４ 个 输入变量，分别对应 ２ 个输出变量，而且这 ２ 个输出 变量是并列关系．首先使用 ｔ 时刻跟随者与领航者 之间的理想距离与实际距离之差 Ｌ（ ｔ） ＝ ｌ ｄ －ｌ，以及 差值变化率 ΔＬ（ ｔ） 作为输入变量，领航机器鱼在 ｔ 时刻的速度档位 Ｖ（ｔ）作为输出变量．其中，Ｌ（ ｔ） 的 论域为［ －３０，３０］，模糊集表示为｛ＮＢ，ＮＳ，ＺＥ，ＰＳ， ＰＢ｝；差值变化率 ΔＬ（ ｔ） 的论域为［ －６，６］，模糊集 表示为｛ＮＢ，ＮＳ，ＺＥ，ＰＳ，ＰＢ｝；输出变量Ｖ（ｔ）的论域 为［１，１５］，模糊集表示为｛ＶＳ，ＬＳ，ＭＥ，ＬＦ，ＶＦ｝．输 入输出各模糊控制变量的隶属度函数如图 ５ 所示． （ａ）输入变量 Ｌ（ｔ） （ｂ）输入变量 ΔＬ（ｔ） （ｃ）输出变量 Ｖ（ｔ） 图 ５ 距离输入输出变量的隶属度函数 Ｆｉｇ．５ Ｍｅｍｂｅｒｓｈｉｐ ｆｕｎｃｔｉｏｎ ｏｆ ｔｈｅ ｐｏｓｉｔｉｏｎ ｉｎｐｕｔ ｖａｒｉａ⁃ ｂｌｅ ａｎｄ ｐｏｓｉｔｉｏｎ ｏｕｔｐｕｔ ｖａｒｉａｂｌｅ 其次，以 ｔ 时刻跟随者与领航者之间的理想方 位角与实际方位角之差 θ（ｔ）＝ φｄ －φ，以及差值变化 率 Δθ（ ｔ）作为输入变量，领航机器鱼 ｔ 时刻的方向 档位 Ｐ（ｔ）作为输出变量．对差值 θ（ｔ）进行模糊化可 以得到｛ＮＢ，ＮＳ，ＺＥ，ＰＳ，ＰＢ｝；对差值变化 Δθ（ｔ） 进 ·２５０· 智 能 系 统 学 报 第 ８ 卷
第3期 单素素，等：采用Leader-follower和模糊反馈机制的机器鱼队形控制 ·251- 行模糊化可得{NB,NS,ZE,PS,PB}:输出变量P(t) 表1机器鱼速度档位控制规则表 的论域为[-7,7]，其模糊集为{LB,LS,ME,S, Table 1 Speed stalls control rule table of multi-robot fish RB}.输入输出各模糊控制变量的隶属度函数如图6 V(1) 所示 AL(t)L(t)=L(t)=L(1)= L(t)=L(t)= NB NS ZE PS PB NB VS VS VS ME LF NB NS ZE PS PB NS VS VS LS ME LF ZE LS LS ME LF LF PS LS ME LF VE VF PB LS ME VE VE VE 表2机器鱼方向档位控制规则表 -60 -15 Table 2 Direction stalls control rule table of multi-robot fish (a)输入变量0(t) P(t) △0(t)0(t)=0(t)= (t)=0(t)=0(t)= nB NS ZE PS PB NB LB LB LS me RS NB ZE PS PB NS LB LB LS ME RS ZE LB LS ME RS RB PS LS ME RS RB RB PB LS ME RS RB RB -10-5 0 5 75440 3仿真实验与分析 (b)输入变量△9(t) 在实验仿真过程中，以2条机器鱼在复杂环境 中保持一定队形，并运动到指定位置为目标.首先 Leader根据环境信息自主规划运动路径，并将自身 LB LS 1ME RS RB 的状态信息传递给各个Follower.然后每个Follower 分别计算自己在队形中的位置并将自身的信息反馈 给Leader.最终在正常运行状态下，Leader不受队形 反馈信息的影响，队形仅依靠Follower自行调整速 度和方位角来保持. P(n 利用MATLAB工具，根据各控制变量的隶属度 (c)输出变量P(t) 函数及模糊规则建立模糊反馈控制器，并将其导入 图6方位角输入输出变量的隶属度函数 到Simulink工作区，搭建出反馈控制环节.实验中， Fig.6 Membership function of the orientation input 在5s时加入扰动，使Follower与Leader之间的距离 variable and orientation output variable 和方位角均偏离理想值：在10s时加入扰动仅使距 离偏离理想值：在12s时再次加入扰动仅使方位角 由于模糊控制器的2个输出变量是并列关系， 偏离理想值.得到Leader的速度档位和方向档位波 因此需要建立2个并列的规则库，其模糊推理规则 形图如图7所示. 可以分别表示为： 15 If L(t)and AL(t),then V(t) If 0(t)and 0(t)then P(t). 10 经过多次试验后，可以得到相应的模糊控制规 则表，如表1和2所示.在经过模糊化和模糊推理建 立模糊规则后，基于最大隶属度法，将得到的输出变 量进行清晰化.这样就可以得到精确的速度档位和 10 15 20 方向档位值，即领航机器鱼为了更好地保持队形即 t/s 将执行的2个游动指令. (a)速度档位
行模糊化可得｛ＮＢ，ＮＳ，ＺＥ，ＰＳ，ＰＢ｝；输出变量 Ｐ（ｔ） 的论域为［ － ７，７］，其模糊集为｛ ＬＢ， ＬＳ，ＭＥ，ＲＳ， ＲＢ｝．输入输出各模糊控制变量的隶属度函数如图 ６ 所示． （ａ）输入变量 θ（ｔ） （ｂ）输入变量 Δθ（ｔ） （ｃ）输出变量 Ｐ（ｔ） 图 ６ 方位角输入输出变量的隶属度函数 Ｆｉｇ． ６ Ｍｅｍｂｅｒｓｈｉｐ ｆｕｎｃｔｉｏｎ ｏｆ ｔｈｅ ｏｒｉｅｎｔａｔｉｏｎ ｉｎｐｕｔ ｖａｒｉａｂｌｅ ａｎｄ ｏｒｉｅｎｔａｔｉｏｎ ｏｕｔｐｕｔ ｖａｒｉａｂｌｅ 由于模糊控制器的 ２ 个输出变量是并列关系， 因此需要建立 ２ 个并列的规则库，其模糊推理规则 可以分别表示为： Ｉｆ Ｌ（ｔ） ａｎｄ ΔＬ（ｔ），ｔｈｅｎ Ｖ（ｔ） ； Ｉｆ θ（ｔ） ａｎｄ Δθ（ｔ），ｔｈｅｎ Ｐ（ｔ） ． 经过多次试验后，可以得到相应的模糊控制规 则表，如表 １ 和 ２ 所示．在经过模糊化和模糊推理建 立模糊规则后，基于最大隶属度法，将得到的输出变 量进行清晰化．这样就可以得到精确的速度档位和 方向档位值，即领航机器鱼为了更好地保持队形即 将执行的 ２ 个游动指令． 表 １ 机器鱼速度档位控制规则表 Ｔａｂｌｅ １ Ｓｐｅｅｄ ｓｔａｌｌｓ ｃｏｎｔｒｏｌ ｒｕｌｅ ｔａｂｌｅ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ ΔＬ（ｔ） Ｖ（ｔ） Ｌ（ｔ）＝ ＮＢ Ｌ（ｔ）＝ ＮＳ Ｌ（ｔ）＝ ＺＥ Ｌ（ｔ）＝ ＰＳ Ｌ（ｔ）＝ ＰＢ ＮＢ ＶＳ ＶＳ ＶＳ ＭＥ ＬＦ ＮＳ ＶＳ ＶＳ ＬＳ ＭＥ ＬＦ ＺＥ ＬＳ ＬＳ ＭＥ ＬＦ ＬＦ ＰＳ ＬＳ ＭＥ ＬＦ ＶＦ ＶＦ ＰＢ ＬＳ ＭＥ ＶＦ ＶＦ ＶＦ 表 ２ 机器鱼方向档位控制规则表 Ｔａｂｌｅ ２ Ｄｉｒｅｃｔｉｏｎ ｓｔａｌｌｓ ｃｏｎｔｒｏｌ ｒｕｌｅ ｔａｂｌｅ ｏｆ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｉｓｈ Δθ（ｔ） Ｐ（ｔ） θ（ｔ）＝ ＮＢ θ（ｔ）＝ ＮＳ θ（ｔ）＝ ＺＥ θ（ｔ）＝ ＰＳ θ（ｔ）＝ ＰＢ ＮＢ ＬＢ ＬＢ ＬＳ ＭＥ ＲＳ ＮＳ ＬＢ ＬＢ ＬＳ ＭＥ ＲＳ ＺＥ ＬＢ ＬＳ ＭＥ ＲＳ ＲＢ ＰＳ ＬＳ ＭＥ ＲＳ ＲＢ ＲＢ ＰＢ ＬＳ ＭＥ ＲＳ ＲＢ ＲＢ ３ 仿真实验与分析 在实验仿真过程中，以 ２ 条机器鱼在复杂环境 中保持一定队形，并运动到指定位置为目标．首先 Ｌｅａｄｅｒ 根据环境信息自主规划运动路径，并将自身 的状态信息传递给各个 Ｆｏｌｌｏｗｅｒ．然后每个 Ｆｏｌｌｏｗｅｒ 分别计算自己在队形中的位置并将自身的信息反馈 给 Ｌｅａｄｅｒ．最终在正常运行状态下，Ｌｅａｄｅｒ 不受队形 反馈信息的影响，队形仅依靠 Ｆｏｌｌｏｗｅｒ 自行调整速 度和方位角来保持． 利用 ＭＡＴＬＡＢ 工具，根据各控制变量的隶属度 函数及模糊规则建立模糊反馈控制器，并将其导入 到 Ｓｉｍｕｌｉｎｋ 工作区，搭建出反馈控制环节．实验中， 在５ ｓ时加入扰动，使 Ｆｏｌｌｏｗｅｒ 与 Ｌｅａｄｅｒ 之间的距离 和方位角均偏离理想值；在 １０ ｓ 时加入扰动仅使距 离偏离理想值；在 １２ ｓ 时再次加入扰动仅使方位角 偏离理想值．得到 Ｌｅａｄｅｒ 的速度档位和方向档位波 形图如图 ７ 所示． （ａ）速度档位 第 ３ 期 单素素，等：采用 Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ 和模糊反馈机制的机器鱼队形控制 ·２５１·