第12卷第2期 智能系统学报 Vol.12 No.2 2017年04月 CAAI Transactions on Intelligent Systems Apr.2017 D0I:10.11992/is.201507029 网络出版地址：http://www.cmki.net/kcms/detail,/23.1538.tp.20170220.1008.004.html 多移动机器人的领航-跟随编队避障控制 罗京，刘成林，刘飞 (江南大学轻工过程先进控制教育部重，点实验室，江苏无锡214122) 摘要：针对多移动机器人的编队控制问题，提出了一种结合Polar Histogram避障法的领航-跟随协调编队控制算 法。该算法在领航-跟随1如编队控制结构的基础上引入虚拟跟随机器人，将编队控制转化为跟随机器人对虚拟跟 随机器人的轨迹跟踪控制。结合移动机器人自身传感器技术，在简单甚至复杂的环境下为机器人提供相应的路径 运动策略，实现实时导航的目的。以两轮差动Qbt移动机器人为研究对象，搭建半实物仿真平台，进行仿真实验。 仿真结果表明：该方法可以有效地实现多移动机器人协调编队和避障控制。 关键词：多移动机器人：领航-跟随：编队控制：避障 中图分类号：TP273文献标志码：A文章编号：1673-4785(2017)02-0202-11 中文引用格式：罗京，刘成林，刘飞.多移动机器人的领航-跟随编队避障控制[J].智能系统学报，2017,12(2)：202-212. 英文引用格式：LUO Jing,LIU Chenglin,LIU Fei..Piloting-following formation and obstacle avoidance control of multiple mobile robots[J].CAAI transactions on intelligent systems,2017,12(2):202-212. Piloting-following formation and obstacle avoidance control of multiple mobile robots LUO Jing,LIU Chenglin,LIU Fei Key Laboratory of Advanced Process Control for Light Industry(Ministry of Education),Jiangnan University,Wuxi 214122,China) Abstract:In this paper,we propose a piloting-follower coordination formation control algorithm accompanied with a polar histogram obstacle avoidance method;we suggest here that this combined approach addresses the formation control defects that occur when multiple mobile robots must work in unison.Based on the piloting-following forma- tion control structure,we introduce a virtual following robot,thus converting formation control into trajectory track- ing control conducted by the robots that follow the virtual following robot.Using the sensing technology of the mobile robot itself,we achieved real-time navigation of corresponding paths in both simple and complicated environments. By using the two-wheel differential Qbot mobile robot as our research object,we constructed a semi-physical simu- lation platform for our simulation experiments.Simulation results showed that our method was able to effectively a- chieve coordination formation and obstacle avoidance control of multiple robots. Keywords:multiple mobile robots;piloting-following coordination control;formation control;obstacle avoidance 近年来，随着机器人技术、网络通信技术和自动阔的应用前景四。作为多机器人系统协调控制的 控制技术的不断发展，多移动机器人系统协调控制最基础和最重要的研究问题之一，编队控制是指空 研究引起了众多领域研究者的关注，并在军事、空 间分布的多个机器人，达到控制目标的同时，保持期 间探索、交通控制、医疗与服务行业等领域展现了广 望的空间队形，同时要适应环境约束（例如存在障 碍物或者空间的物理限制)2)。 收稿日期：2015-07-22.网络出版日期：2017-02-20. 编队控制所考察的机器人主要包括：地面移动 基金项目：国家自然科学基金项目(61473138,61104092,61134007)；江 苏省自然科学基金项目(BK20151130). 机器人[3)、无人飞行器s-刃、卫星[劉和自主式潜水 通信作者：刘成林.E-mail:liud@jiangnan.cdu.cn 器[9]等。对于期望队形的实现问题，控制算法主
第 １２ 卷第 ２ 期 智 能 系 统 学 报 Ｖｏｌ．１２ №．２ ２０１７ 年 ０４ 月 ＣＡＡＩ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ Ａｐｒ． ２０１７ ＤＯＩ：１０．１１９９２ ／ ｔｉｓ．２０１５０７０２９ 网络出版地址：ｈｔｔｐ： ／ ／ ｗｗｗ．ｃｎｋｉ．ｎｅｔ ／ ｋｃｍｓ／ ｄｅｔａｉｌ ／ ２３．１５３８．ｔｐ．２０１７０２２０．１００８．００４．ｈｔｍｌ 多移动机器人的领航－跟随编队避障控制 罗京，刘成林，刘飞 （江南大学 轻工过程先进控制教育部重点实验室，江苏 无锡 ２１４１２２） 摘 要：针对多移动机器人的编队控制问题，提出了一种结合 Ｐｏｌａｒ Ｈｉｓｔｏｇｒａｍ 避障法的领航－跟随协调编队控制算 法。 该算法在领航－跟随 ｌ⁃φ 编队控制结构的基础上引入虚拟跟随机器人，将编队控制转化为跟随机器人对虚拟跟 随机器人的轨迹跟踪控制。 结合移动机器人自身传感器技术，在简单甚至复杂的环境下为机器人提供相应的路径 运动策略，实现实时导航的目的。 以两轮差动 Ｑｂｏｔ 移动机器人为研究对象，搭建半实物仿真平台，进行仿真实验。 仿真结果表明：该方法可以有效地实现多移动机器人协调编队和避障控制。 关键词：多移动机器人；领航－跟随；编队控制；避障 中图分类号： ＴＰ２７３ 文献标志码：Ａ 文章编号：１６７３－４７８５（２０１７）０２－０２０２－１１ 中文引用格式：罗京，刘成林，刘飞． 多移动机器人的领航－跟随编队避障控制［Ｊ］． 智能系统学报， ２０１７， １２（２）： ２０２－２１２． 英文引用格式：ＬＵＯ Ｊｉｎｇ， ＬＩＵ Ｃｈｅｎｇｌｉｎ， ＬＩＵ Ｆｅｉ． Ｐｉｌｏｔｉｎｇ⁃ｆｏｌｌｏｗｉｎｇ ｆｏｒｍａｔｉｏｎ ａｎｄ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉｐｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ［Ｊ］． ＣＡＡＩ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｉｎｔｅｌｌｉｇｅｎｔ ｓｙｓｔｅｍｓ， ２０１７， １２（２）： ２０２－２１２． Ｐｉｌｏｔｉｎｇ⁃ｆｏｌｌｏｗｉｎｇ ｆｏｒｍａｔｉｏｎ ａｎｄ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉｐｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ＬＵＯ Ｊｉｎｇ， ＬＩＵ Ｃｈｅｎｇｌｉｎ， ＬＩＵ Ｆｅｉ （ Ｋｅｙ Ｌａｂｏｒａｔｏｒｙ ｏｆ Ａｄｖａｎｃｅｄ Ｐｒｏｃｅｓｓ Ｃｏｎｔｒｏｌ ｆｏｒ Ｌｉｇｈｔ Ｉｎｄｕｓｔｒｙ（Ｍｉｎｉｓｔｒｙ ｏｆ Ｅｄｕｃａｔｉｏｎ）， Ｊｉａｎｇｎａｎ Ｕｎｉｖｅｒｓｉｔｙ， Ｗｕｘｉ ２１４１２２， Ｃｈｉｎａ） Ａｂｓｔｒａｃｔ：Ｉｎ ｔｈｉｓ ｐａｐｅｒ， ｗｅ ｐｒｏｐｏｓｅ ａ ｐｉｌｏｔｉｎｇ⁃ｆｏｌｌｏｗｅｒ ｃｏｏｒｄｉｎａｔｉｏｎ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ａｌｇｏｒｉｔｈｍ ａｃｃｏｍｐａｎｉｅｄ ｗｉｔｈ ａ ｐｏｌａｒ ｈｉｓｔｏｇｒａｍ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｍｅｔｈｏｄ； ｗｅ ｓｕｇｇｅｓｔ ｈｅｒｅ ｔｈａｔ ｔｈｉｓ ｃｏｍｂｉｎｅｄ ａｐｐｒｏａｃｈ ａｄｄｒｅｓｓｅｓ ｔｈｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｄｅｆｅｃｔｓ ｔｈａｔ ｏｃｃｕｒ ｗｈｅｎ ｍｕｌｔｉｐｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｍｕｓｔ ｗｏｒｋ ｉｎ ｕｎｉｓｏｎ． Ｂａｓｅｄ ｏｎ ｔｈｅ ｐｉｌｏｔｉｎｇ⁃ｆｏｌｌｏｗｉｎｇ ｆｏｒｍａ⁃ ｔｉｏｎ ｃｏｎｔｒｏｌ ｓｔｒｕｃｔｕｒｅ， ｗｅ ｉｎｔｒｏｄｕｃｅ ａ ｖｉｒｔｕａｌ ｆｏｌｌｏｗｉｎｇ ｒｏｂｏｔ， ｔｈｕｓ ｃｏｎｖｅｒｔｉｎｇ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｉｎｔｏ ｔｒａｊｅｃｔｏｒｙ ｔｒａｃｋ⁃ ｉｎｇ ｃｏｎｔｒｏｌ ｃｏｎｄｕｃｔｅｄ ｂｙ ｔｈｅ ｒｏｂｏｔｓ ｔｈａｔ ｆｏｌｌｏｗ ｔｈｅ ｖｉｒｔｕａｌ ｆｏｌｌｏｗｉｎｇ ｒｏｂｏｔ． Ｕｓｉｎｇ ｔｈｅ ｓｅｎｓｉｎｇ ｔｅｃｈｎｏｌｏｇｙ ｏｆ ｔｈｅ ｍｏｂｉｌｅ ｒｏｂｏｔ ｉｔｓｅｌｆ， ｗｅ ａｃｈｉｅｖｅｄ ｒｅａｌ⁃ｔｉｍｅ ｎａｖｉｇａｔｉｏｎ ｏｆ ｃｏｒｒｅｓｐｏｎｄｉｎｇ ｐａｔｈｓ ｉｎ ｂｏｔｈ ｓｉｍｐｌｅ ａｎｄ ｃｏｍｐｌｉｃａｔｅｄ ｅｎｖｉｒｏｎｍｅｎｔｓ． Ｂｙ ｕｓｉｎｇ ｔｈｅ ｔｗｏ⁃ｗｈｅｅｌ ｄｉｆｆｅｒｅｎｔｉａｌ Ｑｂｏｔ ｍｏｂｉｌｅ ｒｏｂｏｔ ａｓ ｏｕｒ ｒｅｓｅａｒｃｈ ｏｂｊｅｃｔ， ｗｅ ｃｏｎｓｔｒｕｃｔｅｄ ａ ｓｅｍｉ⁃ｐｈｙｓｉｃａｌ ｓｉｍｕ⁃ ｌａｔｉｏｎ ｐｌａｔｆｏｒｍ ｆｏｒ ｏｕｒ ｓｉｍｕｌａｔｉｏｎ ｅｘｐｅｒｉｍｅｎｔｓ． Ｓｉｍｕｌａｔｉｏｎ ｒｅｓｕｌｔｓ ｓｈｏｗｅｄ ｔｈａｔ ｏｕｒ ｍｅｔｈｏｄ ｗａｓ ａｂｌｅ ｔｏ ｅｆｆｅｃｔｉｖｅｌｙ ａ⁃ ｃｈｉｅｖｅ ｃｏｏｒｄｉｎａｔｉｏｎ ｆｏｒｍａｔｉｏｎ ａｎｄ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉｐｌｅ ｒｏｂｏｔｓ． Ｋｅｙｗｏｒｄｓ：ｍｕｌｔｉｐｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ； ｐｉｌｏｔｉｎｇ⁃ｆｏｌｌｏｗｉｎｇ ｃｏｏｒｄｉｎａｔｉｏｎ ｃｏｎｔｒｏｌ； ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ； ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ 收稿日期：２０１５－０７－２２． 网络出版日期：２０１７－０２－２０． 基金项目：国家自然科学基金项目（６１４７３１３８，６１１０４０９２，６１１３４００７）；江 苏省自然科学基金项目（ＢＫ２０１５１１３０）． 通信作者：刘成林． Ｅ⁃ｍａｉｌ：ｌｉｕｃｌ＠ ｊｉａｎｇｎａｎ．ｅｄｕ．ｃｎ． 近年来，随着机器人技术、网络通信技术和自动 控制技术的不断发展，多移动机器人系统协调控制 研究引起了众多领域研究者的关注， 并在军事、空 间探索、交通控制、医疗与服务行业等领域展现了广 阔的应用前景［１］ 。 作为多机器人系统协调控制的 最基础和最重要的研究问题之一，编队控制是指空 间分布的多个机器人，达到控制目标的同时，保持期 望的空间队形，同时要适应环境约束（例如存在障 碍物或者空间的物理限制） ［２］ 。 编队控制所考察的机器人主要包括：地面移动 机器人［３－４］ 、无人飞行器［５－７］ 、卫星［８］ 和自主式潜水 器［９－１０］等。 对于期望队形的实现问题，控制算法主
第2期 罗京，等：多移动机器人的领航-跟随编队避障控制 ·203 要有：基于行为法[3)、虚拟结构法1-)和领航跟随 法[46。此外，对于多移动机器人系统来说，避障 问题也是编队中需要考虑的一个重要问题。在有障 碍物的约束环境下，多移动机器人的编队避障控制 ICC x.Y 会变得更加复杂，因为移动机器人既要保持整体队 形，又要合理躲避障碍物。对于未知环境下的机器 R 人避障问题，已经有很多有效的解决方法。文献 [17]提出人工势场法，基本原理是构造机器人和障 :驱动轮○：万向轮○：支撑轮 碍物、目标点间的力场，机器人将在目标点的引力和 障碍物的斥力的合力作用下运动。势场法存在若干 图1两轮差分机器人运动学 缺陷，机器人在相近障碍物间不能找到路径，易在障 Fig.1 Two differential robot kinematics 碍物前震荡，在狭窄通道中摆动18)。为解决这些问 由图1可得： 题，文献[l9]提出了向量场直方图法(vector field V.=w(R+d/2) (1) histogram:VFH),该方法将障碍物对机器人的影响 y=w(R-d/2) (2) 量化为机器人各个角度上的障碍强度值，在障碍强 v=g+斯 (3) 度值低于阈值的角度范围内选择移动方向。VH 2 法存在阈值敏感问题，阈值过小时，一些可行通道被 式中：V和V,分别为左右驱动轮的速度，V为机器 忽略，阈值过大时，不一定发现前方障碍物[0。 人线速度（假设机器人速度V大于等于零），ω为机 本文采用领航-跟随编队控制算法来解决多移 器人绕ICC的旋转角速度，R为左右驱动轮轴心到 动机器人系统的编队控制问题。为使得多移动机器 曲率中心的距离，d为左右驱动轮中心之间的距离。 人系统能够成功地进行编队和避障，还采用了一种 由式(1)，(2)变形可得： d(V,+V) 极坐标系下基于障碍物密度的Ploar Histogram避障 R= (4) 2(V-V) 算法，该避障策略可有效避免VFH法中存在阈值敏 w=(V.-y)/d (5) 感问题。最后，通过3个Qbot移动机器人的实验验 机器人的正向运动学描述了机器人速度与位置 证了所提算法的有效性。 状态之间的关系，在给定左右驱动轮速度和机器人 1两轮差分驱动机器人运动学 初始位姿(x,y,0),=。的情况下，可以求得任意时刻 t时的机器人的位姿(x,y,θ)，=4。 本文考察的是Quanser公司提供的Qbot移动机 下面介绍机器人相对初始状态的位姿2) 器人，属于差分驱动轮式移动机器人，假设其质心与 已知机器人速度V(t)和航向角(t),机器人 两驱动轮轴中心重合。在轮式移动机器人的理论研 在t=0时刻的初始位姿为(0,0,0)，受非完整约束 究中，一般都是假设车轮与地面之间点接触，且接触 条件下的轮式机器人存在如下关系： 点和地面之间只有纯滚动没有相对的滑动（包括纵 x(t)=V(t)cos[e(t)dt 向与侧向滑动)，这种理想条件使得机器人受到非 完整约束。可根据刚体力学法求得两轮差动移动机 y(t)=[v(t)sin[0(t)]dt (6) 0 器人的运动学模型。 差分驱动机器人向前或向后运动时，左右驱动 0e)=ja()t 轮的速度相等：当左右驱动轮之间存在速度差时，机 由式(6)可得机器人在t+8t的位姿： 器人将会绕左（或右）驱动轮轴线上某一点旋转，该 t* x+Vcos(0)8t 点称为瞬时曲率中心ICC(Instantaneous Center of y+Vsin(0)St (7) Curvature),如图1所示。机器人的运动轨迹可以通 0+wδt 过改变两驱动轮的速度进行控制。 式中：(x,y,0)和(x·,y·,0°)分别为t时刻和t+ 机器人状态可由左右驱动轮轴中点坐标(x,y) 时刻机器人的位姿。由式(7)可知，通过改变线速度V 和角速度。可以控制移动机器人的运动轨迹。 和航向角0表示。 对于两轮差动机器人将式(3)和(5)带入方程 (6)可得：
要有：基于行为法［３－４］ 、虚拟结构法［１１－１３］和领航跟随 法［１４－１６］ 。 此外，对于多移动机器人系统来说，避障 问题也是编队中需要考虑的一个重要问题。 在有障 碍物的约束环境下，多移动机器人的编队避障控制 会变得更加复杂，因为移动机器人既要保持整体队 形，又要合理躲避障碍物。 对于未知环境下的机器 人避障问题，已经有很多有效的解决方法。 文献 ［１７］提出人工势场法，基本原理是构造机器人和障 碍物、目标点间的力场，机器人将在目标点的引力和 障碍物的斥力的合力作用下运动。 势场法存在若干 缺陷，机器人在相近障碍物间不能找到路径，易在障 碍物前震荡，在狭窄通道中摆动［１８］ 。 为解决这些问 题，文献［１９］ 提出了向量场直方图法（ ｖｅｃｔｏｒ ｆｉｅｌｄ ｈｉｓｔｏｇｒａｍ：ＶＦＨ），该方法将障碍物对机器人的影响 量化为机器人各个角度上的障碍强度值，在障碍强 度值低于阈值的角度范围内选择移动方向。 ＶＦＨ 法存在阈值敏感问题，阈值过小时，一些可行通道被 忽略，阈值过大时，不一定发现前方障碍物［２０］ 。 本文采用领航－跟随编队控制算法来解决多移 动机器人系统的编队控制问题。 为使得多移动机器 人系统能够成功地进行编队和避障，还采用了一种 极坐标系下基于障碍物密度的 Ｐｌｏａｒ Ｈｉｓｔｏｇｒａｍ 避障 算法，该避障策略可有效避免 ＶＦＨ 法中存在阈值敏 感问题。 最后，通过 ３ 个 Ｑｂｏｔ 移动机器人的实验验 证了所提算法的有效性。 １ 两轮差分驱动机器人运动学 本文考察的是 Ｑｕａｎｓｅｒ 公司提供的 Ｑｂｏｔ 移动机 器人，属于差分驱动轮式移动机器人，假设其质心与 两驱动轮轴中心重合。 在轮式移动机器人的理论研 究中，一般都是假设车轮与地面之间点接触，且接触 点和地面之间只有纯滚动没有相对的滑动（包括纵 向与侧向滑动），这种理想条件使得机器人受到非 完整约束。 可根据刚体力学法求得两轮差动移动机 器人的运动学模型。 差分驱动机器人向前或向后运动时，左右驱动 轮的速度相等；当左右驱动轮之间存在速度差时，机 器人将会绕左（或右）驱动轮轴线上某一点旋转，该 点称为瞬时曲率中心 ＩＣＣ （ Ｉｎｓｔａｎｔａｎｅｏｕｓ Ｃｅｎｔｅｒ ｏｆ Ｃｕｒｖａｔｕｒｅ），如图 １ 所示。 机器人的运动轨迹可以通 过改变两驱动轮的速度进行控制。 机器人状态可由左右驱动轮轴中点坐标 （ｘ，ｙ） 和航向角 θ 表示。 图 １ 两轮差分机器人运动学 Ｆｉｇ．１ Ｔｗｏ ｄｉｆｆｅｒｅｎｔｉａｌ ｒｏｂｏｔ ｋｉｎｅｍａｔｉｃｓ 由图 １ 可得： Ｖｒ ＝ ω（Ｒ ＋ ｄ ／ ２） （１） Ｖｌ ＝ ω（Ｒ － ｄ ／ ２） （２） Ｖ ＝ Ｖｒ ＋ Ｖｌ ２ （３） 式中： Ｖｌ 和 Ｖｒ 分别为左右驱动轮的速度， Ｖ 为机器 人线速度（假设机器人速度 Ｖ 大于等于零）， ω 为机 器人绕 ＩＣＣ 的旋转角速度， Ｒ 为左右驱动轮轴心到 曲率中心的距离， ｄ 为左右驱动轮中心之间的距离。 由式（１），（２）变形可得： Ｒ ＝ ｄ（Ｖｒ ＋ Ｖｌ） ２（Ｖｒ － Ｖｌ） （４） ω ＝ （Ｖｒ － Ｖｌ） ／ ｄ （５） 机器人的正向运动学描述了机器人速度与位置 状态之间的关系，在给定左右驱动轮速度和机器人 初始位姿 （ｘ，ｙ，θ）ｔ ＝ ０ 的情况下，可以求得任意时刻 ｔ 时的机器人的位姿 （ｘ，ｙ，θ）ｔ ＝ ｔ 。 下面介绍机器人相对初始状态的位姿［２１］ 。 已知机器人速度 Ｖ（ｔ） 和航向角 θ（ｔ） ，机器人 在 ｔ ＝ ０ 时刻的初始位姿为 （０，０，０） ，受非完整约束 条件下的轮式机器人存在如下关系： ｘ（ｔ） ＝ ∫ ｔ ０ Ｖ（ｔ）ｃｏｓ［θ（ｔ）］ｄｔ ｙ（ｔ） ＝ ∫ ｔ ０ Ｖ（ｔ）ｓｉｎ［θ（ｔ）］ｄｔ θ（ｔ） ＝ ∫ ｔ ０ ω（ｔ）ｄｔ ì î í ï ï ïï ï ï ï （６） 由式（６）可得机器人在 ｔ ＋ δｔ 的位姿： ｘ ∗ ｙ ∗ θ ∗ é ë ê ê êê ù û ú ú úú ＝ ｘ ＋ Ｖｃｏｓ（θ）δｔ ｙ ＋ Ｖｓｉｎ（θ）δｔ θ ＋ ωδｔ é ë ê ê ê ù û ú ú ú （７） 式中： （ｘ，ｙ，θ） 和 （ｘ ∗ ，ｙ ∗ ，θ ∗ ） 分别为 ｔ 时刻和 ｔ ＋ δｔ 时刻机器人的位姿。 由式（７）可知，通过改变线速度 Ｖ 和角速度 ω 可以控制移动机器人的运动轨迹。 对于两轮差动机器人将式（３）和（５）带入方程 （６）可得： 第 ２ 期 罗京，等：多移动机器人的领航－跟随编队避障控制 ·２０３·
.204 智能系统学报 第12卷 ke=号o）+o]lcfo]山 本文在！-φ控制法的基础上引入虚拟跟随机器 人，跟随机器人与领航机器人保持期望的队形结构， bo=vo+o]smao]业(8 只要跟随机器人运动到虚拟跟随机器人的位置，就 可以实现期望的队形编队控制2。如图3所示， ow=号ra)-o1a (x,少1,8)为领航机器人位姿，(x,y,9)为跟随机 器人位姿，跟随距离和角度分别为1和p,可得虚拟 同样由式(8)化简可得： 跟随机器人位姿(x,y.,9,)为 x+2[,+V]eos(9)0 [x,=1+lcos(9+p） y,=y+lsin(+ (10) y+ [v,+V,]sin(0)5t (9) 0.=0 0+-1a 拟跟随 由式(8)和式(9)可知，对于两轮差动机器人通 过改变左右驱动轮的速度V和V,就能控制移动机 领航 器人的运动轨迹。本文通过控制V,和V来驱动机 y8) 器人，实现编队控制。 跟随 .0 2领航-跟随编队控制结构 x,) 领航-跟随编队控制法是指定编队中的某一机 (y) 器人作为领航者，其他机器人作为跟随者跟随领航 0 机器人运动。在多移动机器人系统中，一般设定一 图3领航-虚拟跟随队形结构模型 个主领航机器人，负责提供编队的导航，决定编队的 Fig.3 Leader-virtual follower formation structure model 主轨迹。如果系统中的两个机器人存在局部的领航 将(x,水，日，)作为跟随机器人的目标点带入 与跟随关系，则分别称这两个机器人为领航机器人 式(9)可得： 和跟随机器人。 1 领航-跟随法主要有【-l和【-p两种编队模 x+ v,+]cos( 式[2)。-1法至少需要3个机器人，即跟随机器人以 x lcos(0+) 固定的距离！跟随两个领航机器人，保持期望的队 y lsin(+) y+ 2[V,+Vi]sin(0) 形，实现多移动机器人的编队控制。l-φ控制方法的 思想是跟随机器人要以一定的距离和角度跟随领航 v.-v]8 1 0+ 机器人，实现期望队形的编队控制。如图2所示，控 制目标是lim(l4-)=0与lim(p4-p)=0,其中a (11) 式中(x,y,)为跟随机器人当前位姿，由式(11)得： 与P分别为跟随机器人与领航机器人的相隔距离 与相对方位角的期望值，1与”分别为相隔距离与 上+5=2×低，-)+(- 相对方位角的测量值。 4 8.-0 领航 .-V1=d× (12) 式中：x,=x+lcos(0+p),y,=为+lsin(8+p), 日=日。令依-+(.- 跟随 6y0) t =V,6t w,式(12)简化为 V.+V,=2×V (13) (cy0) V.-y,=d×w 由式(13)可得： 图2领航-跟随队形结构模型 V=2×V+dxu 2 Fig.2 Leader-follower formation structure model
ｘ（ｔ） ＝ １ ２ ∫ ｔ ０ ［Ｖｒ（ｔ） ＋ Ｖｌ（ｔ）］ｃｏｓ［θ（ｔ）］ｄｔ ｙ（ｔ） ＝ １ ２ ∫ ｔ ０ ［Ｖｒ（ｔ） ＋ Ｖｌ（ｔ）］ｓｉｎ［θ（ｔ）］ｄｔ θ（ｔ） ＝ １ ｄ ∫ ｔ ０ ［Ｖｒ（ｔ） － Ｖｌ（ｔ）］ｄｔ ì î í ï ï ïï ï ï ïï （８） 同样由式（８）化简可得： ｘ ∗ ｙ ∗ θ ∗ é ë ê ê êê ù û ú ú úú ＝ ｘ ＋ １ ２ ［Ｖｒ ＋ Ｖｌ］ｃｏｓ（θ）δｔ ｙ ＋ １ ２ ［Ｖｒ ＋ Ｖｌ］ｓｉｎ（θ）δｔ θ ＋ １ ｄ ［Ｖｒ － Ｖｌ］δｔ é ë ê ê ê ê ê ê ê ù û ú ú ú ú ú ú ú （９） 由式（８）和式（９）可知，对于两轮差动机器人通 过改变左右驱动轮的速度 Ｖｌ 和 Ｖｒ 就能控制移动机 器人的运动轨迹。 本文通过控制 Ｖｒ 和 Ｖｌ 来驱动机 器人，实现编队控制。 ２ 领航－跟随编队控制结构 领航－跟随编队控制法是指定编队中的某一机 器人作为领航者，其他机器人作为跟随者跟随领航 机器人运动。 在多移动机器人系统中，一般设定一 个主领航机器人，负责提供编队的导航，决定编队的 主轨迹。 如果系统中的两个机器人存在局部的领航 与跟随关系，则分别称这两个机器人为领航机器人 和跟随机器人。 领航 － 跟随法主要有 ｌ⁃ｌ 和 ｌ⁃φ 两种编队模 式［２２］ 。 ｌ⁃ｌ 法至少需要 ３ 个机器人，即跟随机器人以 固定的距离 ｌ 跟随两个领航机器人，保持期望的队 形，实现多移动机器人的编队控制。 ｌ⁃φ 控制方法的 思想是跟随机器人要以一定的距离和角度跟随领航 机器人，实现期望队形的编队控制。 如图 ２ 所示，控 制目标是 ｌｉｍ ｔ→¥ （ｌ ｄ － ｌ） ＝ ０ 与 ｌｉｍ ｔ→¥ （φｄ － φ） ＝ ０，其中 ｌ ｄ 与 φｄ 分别为跟随机器人与领航机器人的相隔距离 与相对方位角的期望值， ｌ 与 φ 分别为相隔距离与 相对方位角的测量值。 图 ２ 领航－跟随队形结构模型 Ｆｉｇ．２ Ｌｅａｄｅｒ⁃ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｓｔｒｕｃｔｕｒｅ ｍｏｄｅｌ 本文在 ｌ⁃φ 控制法的基础上引入虚拟跟随机器 人，跟随机器人与领航机器人保持期望的队形结构， 只要跟随机器人运动到虚拟跟随机器人的位置，就 可以实现期望的队形编队控制［２３］ 。 如图 ３ 所示， （ｘｌ，ｙｌ，θｌ） 为领航机器人位姿， （ｘｆ，ｙｆ，θｆ） 为跟随机 器人位姿，跟随距离和角度分别为 ｌ 和 φ ，可得虚拟 跟随机器人位姿 （ｘｖ，ｙｖ，θｖ） 为 ｘｖ ＝ ｘｌ ＋ ｌｃｏｓ（θｌ ＋ φ） ｙｖ ＝ ｙｌ ＋ ｌｓｉｎ（θｌ ＋ φ） θｖ ＝ θｌ ì î í ï ï ï ï （１０） 图 ３ 领航－虚拟跟随队形结构模型 Ｆｉｇ．３ Ｌｅａｄｅｒ⁃ｖｉｒｔｕａｌ ｆｏｌｌｏｗｅｒ ｆｏｒｍａｔｉｏｎ ｓｔｒｕｃｔｕｒｅ ｍｏｄｅｌ 将 （ｘｖ，ｙｖ，θｖ） 作为跟随机器人的目标点带入 式（９）可得： ｘｌ ＋ ｌｃｏｓ（θｌ ＋ φ） ｙｌ ＋ ｌｓｉｎ（θｌ ＋ φ） θｌ é ë ê ê ê ê ù û ú ú ú ú ＝ ｘ ＋ １ ２ ［Ｖｒ ＋ Ｖｌ］ｃｏｓ（θ）δｔ ｙ ＋ １ ２ ［Ｖｒ ＋ Ｖｌ］ｓｉｎ（θ）δｔ θ ＋ １ ｄ ［Ｖｒ － Ｖｌ］δｔ é ë ê ê ê ê ê ê êê ù û ú ú ú ú ú ú úú （１１） 式中 （ｘ，ｙ，θ） 为跟随机器人当前位姿，由式（１１）得： Ｖｒ ＋ Ｖｌ ＝ ２ × （ｘｖ － ｘ） ２ ＋ （ｙｖ － ｙ） ２ δｔ Ｖｒ － Ｖｌ ＝ ｄ × θｖ － θ δｔ （１２） 式中： ｘｖ ＝ ｘｌ ＋ ｌｃｏｓ（θｌ ＋ φ） ， ｙｖ ＝ ｙｌ ＋ ｌｓｉｎ（θｌ ＋ φ）， θｖ ＝ θｌ 。 令 （ｘｖ － ｘ） ２ ＋ （ｙｖ － ｙ） ２ δｔ ＝ Ｖ ， θｖ － θ δｔ ＝ ω， 式（１２）简化为 Ｖｒ ＋ Ｖｌ ＝ ２ × Ｖ Ｖｒ － Ｖｌ ＝ ｄ × ω （１３） 由式（１３）可得： Ｖｒ ＝ ２ × Ｖ ＋ ｄ × ω ２ ·２０４· 智 能 系 统 学 报 第 １２ 卷
第2期 罗京，等：多移动机器人的领航-跟随编队避障控制 .205. V= 2×V-d×w (14) 2 POD:=wf(di1)+wf(d;)+waf(di) 式(14)即为编队中的跟随机器人的左右驱动 轮输人。 fd:)=[1- min(dod 虚拟跟随机器人的引入，将队形保持转换为跟 (15) 随机器人对于虚拟跟随机器人的跟踪控制，这种队 形保持模型，可描述任意队形结构。 i=argmin POD:,i 1,2,.5 (16) 式中：i=1,2,…,5表示第i个传感器，指向45× 3避障算法 (i-1)°方向，d,表示第i个传感器获得的障碍物 VFH避障算法通过构建向量场直方图来确定 距离，d为传感器所能检测的最大距离，f(d:)为 可行方向（如图4）[。VFH算法采用栅格模型，机 第i个传感器获得的障碍密度值，w1、心2和03为权 器人的工作空间划分为若干连续的二维栅格，将机 重因子(01+02+03=1.0)，且有02>01,02> 器人感知的360°范围划分为n个扇区，每个扇区的 心，表征某方向上的障碍密度值受该方向上的传感 夹角为360°/n,通过VFH法中定义的极线障碍强 器所获得的障碍密度值的影响大于其两侧传感器所 度值计算方法，计算对应扇区的极线障碍强度值 获得的障碍密度值。i为POD最小时i的取值，表示 H(k=0,1,…,n-1)。图4为计算后得到的直方 45×(i-1)°方向为机器人最安全的导航方向。本 图示例，直方图上的每一个H可视为k扇区内障碍 文取w2=0.6667,01=03=0.1667,式(11)中， 物的密度，H越大说明扇区内障碍物越密集。在直 f(d)=f(d6)=0以及dh=1.5m。由图5(a)中的 方图上，设定阈值8，若扇区k的H值低于δ则视为 数据，可以计算出P0D,最小，因此0°方向是最安 无障碍区，若干个无障碍扇区构成候选区(Valley), 全的通过障碍物的方向，机器人将右转90°避开障 图4（以0°~180°为例）中根据直方图和阈值6将 碍物。根据上述描述：该方法构造的极线障碍密度 得到4个候选区，机器人根据候选区的宽度判断其 值计算公式简单，计算量小，而且不存在阈值敏感问 是否可以通过，在所有可以通过的候选区中，选择最 题，因而对复杂的环境具有良好的适应性。 接近目标的一个通过。该方法的不足之处是对阈值 909 90° 敏感，阈值δ过大时可能会碰到障碍物，过小则可能 1209 60 120° 60 一些可行通道将被忽略。 150 1506 30 0e180 210° 330 210 330 0.25. 240° 270°2300 240° 270° (a)障碍物的距离 (b)障碍物密度 459 90° 135° 180° 图5极坐标下的障碍物距离及障碍物密度 图4向量场直方图 Fig.5 Polar plots of range data and obstacle density Fig.4 Vector field histogram 针对以上不足之处，本文利用红外线测距传感 4实验及仿真结果分析 器获取局部环境信息，采用一种基于极坐标系下障 为验证算法的实际有效性，本文以基于iRobot 碍物密度的避障方法Ploar Histogram避障法。该算 的Create平台开发的自主移动机器人Qbot为研究 法根据障碍密度搜索一条最安全的导航方向，利用 对象，其带有5个红外传感器，分别安装在机器人的 红外测距传感器所测数据（即距障碍物距离）来设 0°、45°、90°、135°和180°方向上，如图6所示，搭 计障碍密度值，选择障碍密度最小的方向作为机器 建Quanser无人工具实验系统仿真平台，进行半实 人的安全导航方向。如图5所示，其中图5(a)表示 物仿真实验。 位于0°、45°、90°、135°和180°方向的5个传感 4.1无人工具实验系统 器获得的障碍物距离数据(m),其中90°方向为机 实验系统大致包括以下几部分，如图7所示： 器人的正前方。 1)QuaRe实时控制软件与多智能体任务开发系统： 图5(b)表示相应方向上的极坐标障碍物密度 2)地面控制站； 值(POD),其计算方法如下： 3)无人地面机器人：
Ｖｌ ＝ ２ × Ｖ － ｄ × ω ２ （１４） 式（１４）即为编队中的跟随机器人的左右驱动 轮输入。 虚拟跟随机器人的引入，将队形保持转换为跟 随机器人对于虚拟跟随机器人的跟踪控制，这种队 形保持模型，可描述任意队形结构。 ３ 避障算法 ＶＦＨ 避障算法通过构建向量场直方图来确定 可行方向（如图 ４） ［１９］ 。 ＶＦＨ 算法采用栅格模型，机 器人的工作空间划分为若干连续的二维栅格，将机 器人感知的 ３６０° 范围划分为 ｎ 个扇区，每个扇区的 夹角为 ３６０°／ ｎ ，通过 ＶＦＨ 法中定义的极线障碍强 度值计算方法，计算对应扇区的极线障碍强度值 Ｈｋ（ｋ ＝ ０，１，…，ｎ － １） 。 图 ４ 为计算后得到的直方 图示例，直方图上的每一个 Ｈｋ 可视为 ｋ 扇区内障碍 物的密度， Ｈｋ 越大说明扇区内障碍物越密集。 在直 方图上，设定阈值 δ ，若扇区 ｋ 的 Ｈｋ 值低于 δ 则视为 无障碍区，若干个无障碍扇区构成候选区（Ｖａｌｌｅｙ）， 图 ４（以 ０° ～ １８０° 为例）中根据直方图和阈值 δ 将 得到 ４ 个候选区，机器人根据候选区的宽度判断其 是否可以通过，在所有可以通过的候选区中，选择最 接近目标的一个通过。 该方法的不足之处是对阈值 敏感，阈值 δ 过大时可能会碰到障碍物，过小则可能 一些可行通道将被忽略。 图 ４ 向量场直方图 Ｆｉｇ．４ Ｖｅｃｔｏｒ ｆｉｅｌｄ ｈｉｓｔｏｇｒａｍ 针对以上不足之处，本文利用红外线测距传感 器获取局部环境信息，采用一种基于极坐标系下障 碍物密度的避障方法 Ｐｌｏａｒ Ｈｉｓｔｏｇｒａｍ 避障法。 该算 法根据障碍密度搜索一条最安全的导航方向，利用 红外测距传感器所测数据（即距障碍物距离） 来设 计障碍密度值，选择障碍密度最小的方向作为机器 人的安全导航方向。 如图 ５ 所示，其中图 ５（ａ）表示 位于 ０° 、 ４５° 、 ９０° 、 １３５° 和 １８０° 方向的 ５ 个传感 器获得的障碍物距离数据（ｍ），其中 ９０° 方向为机 器人的正前方。 图 ５（ｂ）表示相应方向上的极坐标障碍物密度 值（ＰＯＤ），其计算方法如下： ＰＯＤｉ ＝ ｗ１ ｆ（ｄｉ－１ ） ＋ ｗ２ ｆ（ｄｉ） ＋ ｗ３ ｆ（ｄｉ＋１ ） ｆ（ｄｉ） ＝ ［１ － ｍｉｎ（ｄｔｈ ，ｄｉ） ｄｔｈ ］ ２ ì î í ï ï ïï （１５） ｉ ＾ ＝ ａｒｇｍｉｎ ＰＯＤｉ，ｉ ＝ １，２，…，５ （１６） 式中： ｉ ＝ １，２，…，５ 表示第 ｉ 个传感器，指向 ４５ × （ｉ － １）° 方向， ｄｉ 表示第 ｉ 个传感器获得的障碍物 距离， ｄｔｈ 为传感器所能检测的最大距离， ｆ（ｄｉ） 为 第 ｉ 个传感器获得的障碍密度值， ｗ１ 、 ｗ２ 和 ｗ３ 为权 重因子（ ｗ１ ＋ ｗ２ ＋ ｗ３ ＝ １．０），且有 ｗ２ ＞ ｗ１ ，ｗ２ ＞ ｗ３ ， 表征某方向上的障碍密度值受该方向上的传感 器所获得的障碍密度值的影响大于其两侧传感器所 获得的障碍密度值。 ｉ ＾ 为 ＰＯＤｉ 最小时 ｉ 的取值，表示 ４５ × （ｉ ＾ － １）° 方向为机器人最安全的导航方向。 本 文取 ｗ２ ＝ ０．６６６ ７， ｗ１ ＝ ｗ３ ＝ ０．１６６ ７，式（１１） 中， ｆ（ｄ０ ） ＝ ｆ（ｄ６ ） ＝ ０ 以及 ｄｔｈ ＝ １．５ ｍ 。 由图 ５（ａ）中的 数据，可以计算出 ＰＯＤ１ 最小，因此 ０° 方向是最安 全的通过障碍物的方向，机器人将右转 ９０° 避开障 碍物。 根据上述描述：该方法构造的极线障碍密度 值计算公式简单，计算量小，而且不存在阈值敏感问 题，因而对复杂的环境具有良好的适应性。 图 ５ 极坐标下的障碍物距离及障碍物密度 Ｆｉｇ．５ Ｐｏｌａｒ ｐｌｏｔｓ ｏｆ ｒａｎｇｅ ｄａｔａ ａｎｄ ｏｂｓｔａｃｌｅ ｄｅｎｓｉｔｙ ４ 实验及仿真结果分析 为验证算法的实际有效性，本文以基于 ｉＲｏｂｏｔ 的 Ｃｒｅａｔｅ 平台开发的自主移动机器人 Ｑｂｏｔ 为研究 对象，其带有 ５ 个红外传感器，分别安装在机器人的 ０° 、 ４５° 、 ９０° 、 １３５° 和 １８０° 方向上，如图 ６ 所示，搭 建 Ｑｕａｎｓｅｒ 无人工具实验系统仿真平台，进行半实 物仿真实验。 ４．１ 无人工具实验系统 实验系统大致包括以下几部分，如图 ７ 所示： １）ＱｕａＲｃ 实时控制软件与多智能体任务开发系统； ２）地面控制站； ３）无人地面机器人； 第 ２ 期 罗京，等：多移动机器人的领航－跟随编队避障控制 ·２０５·
.206 智能系统学报 第12卷 4)照相机定位系统。 Host和无线网络传输给每个移动机器人，跟随机器 人按预先设计好的算法计算出与领航机器人的位置 差，调整方向跟踪领航机器人以实现编队控制。编 135 队过程中，领航和跟随机器人通过自身携带的红外 线传感器检测环境中的障碍物，机器人根据检测到 的障碍物情况利用Polar Histogram避障控制策略实 时避开障碍物，实现编队避障控制。 4.3算法实现步骤 1)在主机Host上设计领航机器人的跟踪路径。 图6Qbot实物图 建立无线局域网。 Fig.6 Qbot physical figure 2)主机Host实时地将照相机定位系统捕捉到 的领航和跟随机器人的位姿信息发送给对应的领航 會照相机定位系统 和跟随机器人，同时将设计好的路径命令发送给领 航机器人。 3)领航机器人实时接收主机传来的路径命令 无人地面机器 和自身位姿信息向目标点运行，当自身携带的红外 e 线传感器检测到障碍物时，利用设计好的避障算法 地面控制站 进行避障，绕过障碍后继续驶向目标点。同时，领航 机器人根据l-φ控制法生成虚拟机器人的轨迹命 QuaRc实时控制软件 +的 令，并发送给跟随机器人。 4)跟随机器人实时接收主机Host发送的自身 图7 Quanser无人工具实验系统 位姿信息和领航机器人发送的虚拟机器人轨迹命 Fig.7 Quanser unmanned tool experiment system 令，跟随机器人沿着虚拟跟随机器人的轨迹跟随领 QuaRC实时控制软件与Matlab/Simulink兼容， 航机器人，同时能够有效避开环境中的障碍物，实现 可以方便地调用Matlab/Simulink中的函数。将设 计的控制器与相应的系统硬件端口Simulink模块相 编队避障控制。 连，编译并下传到QuaRC中，实现实时控制。地面 4.4半实物仿真实验及结果分析 控制站是通过一台地面控制计算机来实现，包括定 设计一个3m×3m的场地，构造机器人编队 位、多智能体控制等，主控制机通过无线局域网和各 避开障碍物到达目标点的仿真环境，利用领航跟 个智能体进行通信，地面控制站主要进行任务规划 随编队法和Polar Histogram避障法，分别进行了 和定位等目的，一旦控制系统的控制算法设计完成， 3个机器人柱形编队避障控制实验和3个机器人 控制站就只起到定位作用。无人地面机器人是基于 三角形编队避障控制实验，柱形和三角形编队队 iRobot的Create平台开发的自主移动机器人Qbot, 形如图8所示。 它带有5个红外线传感器，可以用于探测障碍物，探 测距离最大可达。照相机定位系统通过8个红外 照相机实现空间的三维定位。 4.2实验原理 系统采用Host-Target工作模式进行实时控制， 控制器开发在装有MATLAB/Simulink的主机上实 现，控制器搭建完成后，在Simulink下进行编译并且 下载到移动机器人的控制器(target)中实现实时控 制。主机Host可同时控制多个控制器(Target),从 而可以在MATLAB环境下进行多智能体控制算法 的研究。每个移动机器人上都装有3个红外线反射 (a)柱形 (b)三角形 球，使装在墙壁上的8台红外照相机可以实时捕捉 图8多移动机器人的编队队形 每个移动机器人的位姿坐标(x,y,0),通过主机 Fig.8 Formation of multiple mobile robots
４）照相机定位系统。 图 ６ Ｑｂｏｔ 实物图 Ｆｉｇ．６ Ｑｂｏｔ ｐｈｙｓｉｃａｌ ｆｉｇｕｒｅ 图 ７ Ｑｕａｎｓｅｒ 无人工具实验系统 Ｆｉｇ．７ Ｑｕａｎｓｅｒ ｕｎｍａｎｎｅｄ ｔｏｏｌ ｅｘｐｅｒｉｍｅｎｔ ｓｙｓｔｅｍ ＱｕａＲＣ 实时控制软件与 Ｍａｔｌａｂ ／ Ｓｉｍｕｌｉｎｋ 兼容， 可以方便地调用 Ｍａｔｌａｂ ／ Ｓｉｍｕｌｉｎｋ 中的函数。 将设 计的控制器与相应的系统硬件端口 Ｓｉｍｕｌｉｎｋ 模块相 连，编译并下传到 ＱｕａＲＣ 中，实现实时控制。 地面 控制站是通过一台地面控制计算机来实现，包括定 位、多智能体控制等，主控制机通过无线局域网和各 个智能体进行通信，地面控制站主要进行任务规划 和定位等目的，一旦控制系统的控制算法设计完成， 控制站就只起到定位作用。 无人地面机器人是基于 ｉＲｏｂｏｔ 的 Ｃｒｅａｔｅ 平台开发的自主移动机器人 Ｑｂｏｔ， 它带有 ５ 个红外线传感器，可以用于探测障碍物，探 测距离最大可达 。 照相机定位系统通过 ８ 个红外 照相机实现空间的三维定位。 ４．２ 实验原理 系统采用 Ｈｏｓｔ⁃Ｔａｒｇｅｔ 工作模式进行实时控制， 控制器开发在装有 ＭＡＴＬＡＢ ／ Ｓｉｍｕｌｉｎｋ 的主机上实 现，控制器搭建完成后，在 Ｓｉｍｕｌｉｎｋ 下进行编译并且 下载到移动机器人的控制器（ｔａｒｇｅｔ）中实现实时控 制。 主机 Ｈｏｓｔ 可同时控制多个控制器（ Ｔａｒｇｅｔ），从 而可以在 ＭＡＴＬＡＢ 环境下进行多智能体控制算法 的研究。 每个移动机器人上都装有 ３ 个红外线反射 球，使装在墙壁上的 ８ 台红外照相机可以实时捕捉 每个移动机器人的位姿坐标 （ｘ，ｙ，θ） ，通过主机 Ｈｏｓｔ 和无线网络传输给每个移动机器人，跟随机器 人按预先设计好的算法计算出与领航机器人的位置 差，调整方向跟踪领航机器人以实现编队控制。 编 队过程中，领航和跟随机器人通过自身携带的红外 线传感器检测环境中的障碍物，机器人根据检测到 的障碍物情况利用 Ｐｏｌａｒ Ｈｉｓｔｏｇｒａｍ 避障控制策略实 时避开障碍物，实现编队避障控制。 ４．３ 算法实现步骤 １）在主机 Ｈｏｓｔ 上设计领航机器人的跟踪路径。 建立无线局域网。 ２）主机 Ｈｏｓｔ 实时地将照相机定位系统捕捉到 的领航和跟随机器人的位姿信息发送给对应的领航 和跟随机器人，同时将设计好的路径命令发送给领 航机器人。 ３）领航机器人实时接收主机传来的路径命令 和自身位姿信息向目标点运行，当自身携带的红外 线传感器检测到障碍物时，利用设计好的避障算法 进行避障，绕过障碍后继续驶向目标点。 同时，领航 机器人根据 ｌ⁃φ 控制法生成虚拟机器人的轨迹命 令，并发送给跟随机器人。 ４）跟随机器人实时接收主机 Ｈｏｓｔ 发送的自身 位姿信息和领航机器人发送的虚拟机器人轨迹命 令，跟随机器人沿着虚拟跟随机器人的轨迹跟随领 航机器人，同时能够有效避开环境中的障碍物，实现 编队避障控制。 ４．４ 半实物仿真实验及结果分析 设计一个 ３ ｍ × ３ ｍ 的场地，构造机器人编队 避开障碍物到达目标点的仿真环境，利用领航跟 随编队法和 Ｐｏｌａｒ Ｈｉｓｔｏｇｒａｍ 避障法，分别进行了 ３ 个机器人柱形编队避障控制实验和 ３ 个机器人 三角形编队避障控制实验，柱形和三角形编队队 形如图 ８ 所示。 图 ８ 多移动机器人的编队队形 Ｆｉｇ．８ Ｆｏｒｍａｔｉｏｎ ｏｆ ｍｕｌｔｉｐｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ·２０６· 智 能 系 统 学 报 第 １２ 卷
第2期 罗京，等：多移动机器人的领航-跟随编队避障控制 .207, 实验中设置机器人的红外传感器探测距离 1.0 d=0.4m,控制周期为0.1s,Qbot两驱动轮之间 0.8 0.6 的距离d=252.5mm。 0.4 跟随机器人1 实验13个Qbot移动机器人柱形编队避障实验 0.2 在主机Host上设计领航机器人从任意初始位 0 -0.2 领航机器 置，经中间点(0.9,0.9)、(-0.9,0.9)、(-0.9，-0.9) -0.4 跟随机器人2 和(0.9，-0.9)运动到目标点(0.9,0.9)。障碍物为 -0.6 -0.8 长方体和圆柱体障碍物，位置任意摆放在路径当中。 -1.0 1.0 -0.5 0.5 1.01.5 编队队形为柱形，跟随机器人R,和R2与领航机器 Y/m 人R,的期望相对距离分别为0.4m和0.8m,相对方 (a)编队轨迹 位角都为p=180°。图9是3个Qbot柱形编队实 验过程的部分截图，可以看出两跟随机器人可以跟 (-s.ww) 500 随领航机器人从初始位置，经中间点(0.9,0.9)、(- 0.9,0.9)、(-0.9,-0.9)和(0.9，-0.9)运动到目标点 (0.9,0.9),并且可以顺利避开障碍物。 -500 100 150 200 9-1 s 0-2 500 09,-09) 0.9=0 (0.9,0.9 (0.9,0.9 .mm -500 50 100 150 200 s (b)领航机器人左轮和右轮速度控制输入 1.0 ×10 一实际值 9-5 0 期望值 -1.04 0 100 150 200 t/s 20.*10 实际值 0 期望值 -2.0 图9柱形编队避障过程 50 100 150 200 Fig.9 Cylindrical formation process of obstacle avoidance ts 编队轨迹如图10(a)所示，两跟随机器人R,和 5.0 实际值 0 一期望值 R?跟随领航机器人R,保持柱形编队顺利避开障碍 5. 50 100 150 200 物达到目标点，领航机器人运动的轨迹即为跟随机 tis 器人期望的跟踪轨迹。领航机器人R,和跟随机器 (c)领航机器人的位姿 人R1、R2的左右轮速度控制输入如图10(b)、 (d)和()所示，可以看出各机器人的左右轮速度控 500 制输入是时变的，当机器人转弯或遇障碍物变向时， 各机器人左右轮速度变化较大。领航机器人R,的 50 位姿如图8（©）所示，领航机器人跟踪点(0.9,0.9)、 20 40 60 80 100120140 (-0.9,0.9)、(-0.9,-0.9)以及(0.9，-0.9)最终到达 目标点(0.9,0.9)，其中在遇到障碍物时机器人偏离 期望轨迹存在误差，但随着编队的进行，机器人回归 期望轨迹，误差趋近于0。跟随机器人R,和R2的 位姿如图10(e)和(g)所示，跟随机器人都以较小的 20 40 6080100120140 误差跟随虚拟机器人（即领航机器人的轨迹），完成 轨迹跟踪控制并且可以实时避开障碍物。 (d)跟随机器人1左轮和右轮速度控制输入
实验中设置机器人的红外传感器探测距离 ｄｔｈ ＝ ０．４ ｍ ，控制周期为 ０．１ ｓ ，Ｑｂｏｔ 两驱动轮之间 的距离 ｄ ＝ ２５２．５ ｍｍ 。 实验 １ ３ 个 Ｑｂｏｔ 移动机器人柱形编队避障实验 在主机 Ｈｏｓｔ 上设计领航机器人从任意初始位 置，经中间点（０．９，０．９）、（ －０．９，０．９）、（ －０．９，－０．９） 和（０．９，－０．９）运动到目标点（０．９，０．９）。 障碍物为 长方体和圆柱体障碍物，位置任意摆放在路径当中。 编队队形为柱形，跟随机器人 ＲＦ１ 和 ＲＦ２ 与领航机器 人 ＲＬ 的期望相对距离分别为 ０．４ ｍ 和 ０．８ ｍ，相对方 位角都为 φ ＝ １８０° 。 图 ９ 是 ３ 个 Ｑｂｏｔ 柱形编队实 验过程的部分截图，可以看出两跟随机器人可以跟 随领航机器人从初始位置，经中间点（０．９，０．９）、（ － ０．９，０．９）、（－０．９，－０．９）和（０．９，－０．９）运动到目标点 （０．９，０．９），并且可以顺利避开障碍物。 图 ９ 柱形编队避障过程 Ｆｉｇ．９ Ｃｙｌｉｎｄｒｉｃａｌ ｆｏｒｍａｔｉｏｎ ｐｒｏｃｅｓｓ ｏｆ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ 编队轨迹如图 １０（ａ）所示，两跟随机器人 ＲＦ１ 和 ＲＦ２ 跟随领航机器人 ＲＬ 保持柱形编队顺利避开障碍 物达到目标点，领航机器人运动的轨迹即为跟随机 器人期望的跟踪轨迹。 领航机器人 ＲＬ 和跟随机器 人 ＲＦ１ 、 ＲＦ２ 的左右轮速度控制输入如图 １０ （ ｂ）、 （ｄ）和（ｆ）所示，可以看出各机器人的左右轮速度控 制输入是时变的，当机器人转弯或遇障碍物变向时， 各机器人左右轮速度变化较大。 领航机器人 ＲＬ 的 位姿如图 ８（ｃ）所示，领航机器人跟踪点（０．９，０．９）、 （－０．９，０．９）、（－０．９，－０．９）以及（０．９，－０．９）最终到达 目标点（０．９，０．９），其中在遇到障碍物时机器人偏离 期望轨迹存在误差，但随着编队的进行，机器人回归 期望轨迹，误差趋近于 ０。 跟随机器人 ＲＦ１ 和 ＲＦ２ 的 位姿如图 １０（ｅ）和（ｇ）所示，跟随机器人都以较小的 误差跟随虚拟机器人（即领航机器人的轨迹），完成 轨迹跟踪控制并且可以实时避开障碍物。 （ａ）编队轨迹 （ｂ）领航机器人左轮和右轮速度控制输入 （ｃ）领航机器人的位姿 （ｄ）跟随机器人 １ 左轮和右轮速度控制输入 第 ２ 期 罗京，等：多移动机器人的领航－跟随编队避障控制 ·２０７·
.208 智能系统学报 第12卷 ×10 部分截图，可以看出机器人小组在保持三角形编队 .0 实际值 期望值 的同时也可以顺利避开障碍物。 20 40 60 80 100120140 编队轨迹如图12(a)所示，机器人队形为三角 ×10 形，前进一段时间后，编队遇到障碍物，领航和跟随 2.0r 实际值 机器人各自避开障碍物后，继续以三角形编队到达 期望值 -2.0 目标点。领航机器人R,和跟随机器人R、R2的 020.406080100120140 左右轮速度控制输入如图12(b)、(d)和(f)所示， 50 实际值 可以看出，各机器人的左右轮速度控制输入是时变 0■ 期望值 的，当机器人转弯或遇障碍物变向时，各机器人左右 50 020 40 6080100120140 轮速度变化较大。领航机器人的位姿如图12(c)所 示，领航机器人在初始位置(0,0)跟踪中间点(0， (e)跟随机器人1的位姿 0.6)和(0,1.6)最终到达目标点(0,2.2)，在避障时， 500 uw)/ 梅们 机器人偏离期望跟踪点存在误差，但随着编队的进 行，机器人回归期望跟踪轨迹，误差趋近于0。跟随 500 机器人R,和R2的位姿如图12(e)和(g)所示，跟 6080 100 120140 你 随机器人R,和R2都以较小的误差跟踪虚拟跟随 机器人R。与R的轨迹，同时可以顺利避开障碍 物，完成编队控制。 11-1 11-2 -500 0 20406080100120140 s ()跟随机器人2左轮和右轮速度控制输入 ×103 1.0 实际值 期望值 -0 2040 60 80 100 120140 tis 11-3 114 2.0 实际值 期望值 -2.0 020406080100120140 5.0 实际值 期望值 5.0 20406080100120140 s 图11三角形编队避障过程 (g)跟随机器人2的位姿 Fig.11 Triangle formation process of obstacle avoidance 图103个机器人柱形编队控制实验 Fig.10 Three robots cylindrical formation control experiment 0.5 实验23个Qbot移动机器人三角形编队避障 跟随机器人1 实验。 设计领航机器人R,从初始位置点(0,0)，经中 领航机器人 跟随机器人2 间点(0,0.6)和(0,1.6)最终到达目标点(0,2.2)。 -0.5 引入虚拟跟随机器人R,和R2分别与领航机器人 R,保持相对距离为0.5m与0.5m,相对期望角为 -1.0 135°与-135°。跟随机器人R,和R2分别跟随虚 -0.5 0 0.5 1.0 1.5 2.0 2.5 拟跟随机器人R,与R2。 Y/m 图11是3个Qbt三角形编队避障实验过程的 (a)编队轨迹
（ｅ）跟随机器人 １ 的位姿 （ｆ）跟随机器人 ２ 左轮和右轮速度控制输入 （ｇ）跟随机器人 ２ 的位姿 图 １０ ３ 个机器人柱形编队控制实验 Ｆｉｇ．１０ Ｔｈｒｅｅ ｒｏｂｏｔｓ ｃｙｌｉｎｄｒｉｃａｌ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｅｘｐｅｒｉｍｅｎｔ 实验 ２ ３ 个 Ｑｂｏｔ 移动机器人三角形编队避障 实验。 设计领航机器人 ＲＬ 从初始位置点（０，０），经中 间点（０，０．６） 和（０，１．６） 最终到达目标点（０，２．２）。 引入虚拟跟随机器人 Ｒｖ１ 和 Ｒｖ２ 分别与领航机器人 ＲＬ 保持相对距离为 ０．５ ｍ 与 ０．５ ｍ ，相对期望角为 １３５° 与 － １３５° 。 跟随机器人 ＲＦ１ 和 ＲＦ２ 分别跟随虚 拟跟随机器人 Ｒｖ１ 与 Ｒｖ２ 。 图 １１ 是 ３ 个 Ｑｂｏｔ 三角形编队避障实验过程的 部分截图，可以看出机器人小组在保持三角形编队 的同时也可以顺利避开障碍物。 编队轨迹如图 １２（ ａ）所示，机器人队形为三角 形，前进一段时间后，编队遇到障碍物，领航和跟随 机器人各自避开障碍物后，继续以三角形编队到达 目标点。 领航机器人 ＲＬ 和跟随机器人 ＲＦ１ 、 ＲＦ２ 的 左右轮速度控制输入如图 １２（ｂ） 、（ ｄ）和（ ｆ）所示， 可以看出，各机器人的左右轮速度控制输入是时变 的，当机器人转弯或遇障碍物变向时，各机器人左右 轮速度变化较大。 领航机器人的位姿如图 １２（ｃ）所 示，领航机器人在初始位置（０，０） 跟踪中间点（０， ０．６）和（０，１．６）最终到达目标点（０，２．２），在避障时， 机器人偏离期望跟踪点存在误差，但随着编队的进 行，机器人回归期望跟踪轨迹，误差趋近于 ０。 跟随 机器人 ＲＦ１ 和 ＲＦ２ 的位姿如图 １２（ｅ）和（ｇ）所示，跟 随机器人 ＲＦ１ 和 ＲＦ２ 都以较小的误差跟踪虚拟跟随 机器人 Ｒｖ２ 与 Ｒｖ１ 的轨迹，同时可以顺利避开障碍 物，完成编队控制。 图 １１ 三角形编队避障过程 Ｆｉｇ．１１ Ｔｒｉａｎｇｌｅ ｆｏｒｍａｔｉｏｎ ｐｒｏｃｅｓｓ ｏｆ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ （ａ） 编队轨迹 ·２０８· 智 能 系 统 学 报 第 １２ 卷
第2期 罗京，等：多移动机器人的领航-跟随编队避障控制 .209. 400 5.0 200 -20 01020304050607080 -5.0 10 20 30 40 506070 400 5.0 200 0 -200 0 10 20 30 40 50607080 102030.40506070 s ()跟随机器人2左轮和右轮速度控制输入 (b)领航机器人左轮和右轮速度控制输入 ×10 实际值 -0.5 期望值 100 实际值 -100 10 20304050 6070 50 期望值 迟 04 ×10 0102030 40 50607080 2.0 实际值 ×103 期望值 5.0 实际值 -2.0 期望值 010 20304050 6070 t的 -5.0 0 102030 40 5060 7080 2.0 实际值 tis 0.5 实际值 0 期望值 0 期望值 2.0 10 203040506070 -0. s 01020304050607080 s (g)跟随机器人2的位姿 (c)领航机器人的位姿 图123个机器人三角形编队控制实验 Fig.12 Three robots triangle formation control experiment 5.0 实验33个Qbot移动机器人队形变换避障实验 .m 图13为3个Qbot队形变换的编队避障实验过 0 程的部分截图。 5.0 0 1020 3040506070 600 400 200 13-3 0 20 3040 50 6070 (d)跟随机器人1左轮和右轮速度控制输入 5.0f 实际值 期望值 3-6 0 500 10 20 30 405060 70 tis ×10 2.0r 实际值 0 期望值 -2.04 图13队形变换编队避障过程 10203040506070 Fig.13 Formation transformation process of obstacle avoidance 5.0r 实际值 设计领航机器人R,从初始位置点为(0,0)，经 期望值 中间点(0,0.4)和(0,1.7)最终到达目标点(0,2.2)。 0 10203040506070 无障碍环境下，虚拟跟随机器人R,和R2分别与领 (e)跟随机器人1的位姿 航机器人R,保持相对距离为0.5m与0.5m,对应 的相对期望角为135°与-135°。存在障碍的环境
（ｂ） 领航机器人左轮和右轮速度控制输入 （ｃ） 领航机器人的位姿 （ｄ） 跟随机器人 １ 左轮和右轮速度控制输入 （ｅ）跟随机器人 １ 的位姿 （ｆ）跟随机器人 ２ 左轮和右轮速度控制输入 （ｇ）跟随机器人 ２ 的位姿 图 １２ ３ 个机器人三角形编队控制实验 Ｆｉｇ．１２ Ｔｈｒｅｅ ｒｏｂｏｔｓ ｔｒｉａｎｇｌｅ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｅｘｐｅｒｉｍｅｎｔ 实验 ３ ３ 个 Ｑｂｏｔ 移动机器人队形变换避障实验 图 １３ 为 ３ 个 Ｑｂｏｔ 队形变换的编队避障实验过 程的部分截图。 图 １３ 队形变换编队避障过程 Ｆｉｇ．１３ Ｆｏｒｍａｔｉｏｎ ｔｒａｎｓｆｏｒｍａｔｉｏｎ ｐｒｏｃｅｓｓ ｏｆ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ 设计领航机器人 ＲＬ 从初始位置点为（０，０），经 中间点（０，０．４）和（０，１．７）最终到达目标点（０，２．２）。 无障碍环境下，虚拟跟随机器人 Ｒｖ１ 和 Ｒｖ２ 分别与领 航机器人 ＲＬ 保持相对距离为 ０．５ ｍ 与 ０．５ ｍ ，对应 的相对期望角为 １３５° 与 － １３５° 。 存在障碍的环境 第 ２ 期 罗京，等：多移动机器人的领航－跟随编队避障控制 ·２０９·
.210 智能系统学报 第12卷 下，虚拟跟随机器人R,和R2分别与领航机器人R, 500 保持相对距离为0.3m与0.6m,对应的相对期望角 均为0°。跟随机器人R1和R2分别跟随虚拟跟随 机器人R,与R,。在未发现障碍物时，机器人小组 5000 20406080100120140160180 保持三角形编队队形行驶：当检测到障碍物时，机器 人小组变换为柱形编队，在跟随机器人紧跟领航机 500 器人穿过障碍物后，机器人小组恢复三角形编队到 达目标点。 3个机器人队形变换编队控制实验如图14所示。 -500L 编队轨迹如图14(a)所示：机器人初始以三 0 20406080100120140160180 角形队形前进：在检测到障碍物后，队形变换为 ()跟随机器人1左轮和右轮速度控制输入 柱形通过障碍区：安全避障后，恢复三角形编队 500r 实际值 到达目标点。 期望值 -50 020406080100120140160180 2 ×103 tis 0.2 2.0r 实际值 0.1 跟随机器人1 ww/( 0 期望值 -2.0 0 020406080100120140160180 -0.1 领航机器人 s 跟随机器人2 5.0 -0.2 实际值 0 期望值 -0.3 5.0 -0.4 20406080100120140160180 -0.5 0 0.5 1.01.52.02. t/s Y/m (e)跟随机器人1的位姿 (a)编队轨迹 50 300 200 100 -500 0204060 80100120140160180 -00 0 20406080100120140160180 s 500 400 200 1g-50 020406080,100120140160180 200 t/s 020406080100120140160180 ()跟随机器人2左轮和右轮速度控制输入 实际值 (b)领航机器人左轮和右轮速度控制输入 0 期望值 -50 50 实际值 020406080100120140160180 0 4三 期望值 s ×10 020406080100120140160180 2.0r 实际值 t/s 0 期望值 ×10 ww/K -2.0 实际值 020406080100120140160180 0 期望值 -5.0 5.0 实际值 020406080100120140160180 期望值 -5.0 实际值 020406080100120140160180 0.5 期望值 0 -0.5 (g)跟随机器人2的位姿 0 20406080100120140160180 图143个机器人队形变换编队控制实验 Fig.14 Three robots formation transformation control (c)领航机器人的位姿 experiment
下，虚拟跟随机器人 Ｒｖ１ 和 Ｒｖ２ 分别与领航机器人 ＲＬ 保持相对距离为 ０．３ ｍ 与 ０．６ ｍ ，对应的相对期望角 均为 ０° 。 跟随机器人 ＲＦ１ 和 ＲＦ２ 分别跟随虚拟跟随 机器人 Ｒｖ１ 与 Ｒｖ２ 。 在未发现障碍物时，机器人小组 保持三角形编队队形行驶；当检测到障碍物时，机器 人小组变换为柱形编队，在跟随机器人紧跟领航机 器人穿过障碍物后，机器人小组恢复三角形编队到 达目标点。 ３ 个机器人队形变换编队控制实验如图 １４ 所示。 编队轨迹如图 １４（ ａ） 所示：机器人初始以三 角形队形前进；在检测到障碍物后，队形变换为 柱形通过障碍区；安全避障后，恢复三角形编队 到达目标点。 （ａ） 编队轨迹 （ｂ） 领航机器人左轮和右轮速度控制输入 （ｃ） 领航机器人的位姿 （ｄ） 跟随机器人 １ 左轮和右轮速度控制输入 （ｅ）跟随机器人 １ 的位姿 （ｆ）跟随机器人 ２ 左轮和右轮速度控制输入 （ｇ）跟随机器人 ２ 的位姿 图 １４ ３ 个机器人队形变换编队控制实验 Ｆｉｇ．１４ Ｔｈｒｅｅ ｒｏｂｏｔｓ ｆｏｒｍａｔｉｏｎ ｔｒａｎｓｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｅｘｐｅｒｉｍｅｎｔ ·２１０· 智 能 系 统 学 报 第 １２ 卷
第2期 罗京，等：多移动机器人的领航-跟随编队避障控制 ·211· 领航机器人R,和跟随机器人R:,R2的左右轮速 于服务的运动行为结构设计[J].机器人，2012,34(1)： 度控制输入如图14(b),(d)和(f)所示：各机器人 120-128. 的左右轮速度控制输入是时变的，当机器人转弯或 YANG Fan,LIU Shirong,DONG Deguo.Robot behavior 遇障碍物变向时，各机器人左右轮速度变化较大。 and service-based motion behavior structure design in forma- 领航机器人的位姿如图14(c)所示：领航机器人在 tion control[J].Robot,2012,34(1):120-128. [4]ARRICHIELLO F,CHIAVERINI S,INDIVERI G.et al. 初始位置(0,0)跟踪中间点(0,0.4)和(0,1.7)最终 The null-space-based behavioral control for mobile robots 到达目标点(0,2.2)：避障时，机器人偏离期望跟踪 with velocity actuator saturations[J].The international jour- 点存在偏差，但随着编队的进行，机器人回归期望跟 nal of robotics research,2010,29(10):1317-1337. 踪轨迹，误差趋近于0。跟随机器人R1和R2的位 [5]宋敏，魏瑞轩，沈东，等.基于非线性动态逆的无人机 姿如图14(e)和(g)所示：跟随机器人R和R2都 编队协同控制[J].控制与决策，2011,26(3)：448- 以较小的误差跟踪虚拟跟随机器人R2与R,的轨 452. 迹，完成编队控制。 SONG Min,WEI Ruixuan,SHEN Dong,et al.UAV forma- 从上述实验过程能够看出，机器人小组可以保 tion control based on nonlinear dynamic inversion[J.Con- 持某单一队形完成编队避障控制，也可以根据环境 trol and decision,2011,26(3):448-452. [6]李少斌，陈炎财，杨忠，等.具有通信延迟的多无人机 的约束自主变换队形。机器人小组在选择更安全的 编队飞行控制[J].信息与控制，2012,41(2)：142- 队形通过障碍区后，自主恢复到初始队形，最终顺利 146. 到达目标点，完成编队避障控制。 LI Shaobin,CHEN Yancai,YANG Zhong,et al.Formation 6结束语 fight control of multi-UAVs with communication delay [ Information and control,2012,41(2):142-146. 本文以两轮差动机器人为研究对象，提出了在 [7]魏瑞轩，茹常剑，祁晓明.通信延迟条件下无人机编队 障碍环境中的多移动机器人的编队控制方法，通过 重构的自主安全控制[J].控制理论与应用，2013,30 引入虚拟跟随机器人，将编队控制转化为跟踪控制， (9):1099-1108. 此外，通过编队中的领航和跟随机器人携带的红外 WEI Ruixuan,RU Changjian,QI Xioaming.Autonomous safety control of unmanned aerial vehicle formation reconfig- 线传感器自主感知获取外部环境信息，采用Polar uration under communication delay[J].Control theory Histogram避障法，引导多机器人编队智能地避开障 applications,2013,30(9):1099-1108. 碍物，实现了多移动机器人的编队避障控制，该避障 [8]吴锦杰，刘昆，韩大鹏，等.基于对偶四元数的卫星主 策略不存在阈值敏感性问题，因而对复杂的环境也 从式编队姿轨跟踪的优化控制[J].控制理论与应用， 具有很好的适应性。利用Quanser无人工具实验系 2013,30(9):1069-1078. 统仿真平台，分别对柱形编队、三角形编队以及队形 WU Jinjie,LIU Kun,HAN Dapeng,et al.Optimized track- 变换编队进行实物实验，由仿真实验结果可看出本 ing-control for attitude and orbit of satellite formation flying 文所提出的控制方法的有效性，多移动机器人既可 using dual quaternion[J].Control theory applications, 以保持编队队形，同时各机器人也可以自主避障，实 2013,30(9):1069-1078. 现多移动机器人协调编队和避障控制。 [9]WANG Y,YAN W,LI J.Passivity-based formation control of autonomous underwater vehicles[J].IET control theory 参考文献： applications,2012,6(4):518-525. [10]王银涛，严卫生.多自主水下航行器系统一致性编队跟 [1]FARINELLI A,LOCCHI L,NARDI D.Multirobot systems: 踪控制[J].控制理论与应用，2013,30(3)：379-384. a classification focused on coordination[J].IEEE transac- WANG Yintao,YAN Weisheng.Consensus formation tions on systems,man,and cybernetics,part B(cybernet- tracking control of multiple autonomous underwater vehicle ics),2004.34(5):2015-2028. systems[].Control theory applications,2013,30(3): [2]王祥科，李迅，郑志强.多智能体系统编队控制相关问 379-384. 题研究综述[J].控制与决策，2013,28(11)：1601- [11]REN Wei,SORENSEN N.Distributed coordination archi- 1613. tecture for multi-robot formation control[].Robotics and WANG Xiangke,LI Xun,ZHENG Zhigiang.Survey of de- autonomous systems,2008,56(4):324-333. velopments on multi-agent formation control related problems [12]GHOMMAM J,MEHRJERDI H,SAAD M,et al.Forma- [J].Control and decision,2013,28(11):1601-1613. tion path following control of unicycle-type mobile robots [3]杨帆，刘士荣，董德国.编队控制中的机器人行为与基 [J].Robotics and autonomous systems,2010,58(5):
领航机器人 ＲＬ 和跟随机器人 ＲＦ１ ， ＲＦ２ 的左右轮速 度控制输入如图 １４（ｂ） ，（ｄ）和（ ｆ）所示：各机器人 的左右轮速度控制输入是时变的，当机器人转弯或 遇障碍物变向时，各机器人左右轮速度变化较大。 领航机器人的位姿如图 １４（ｃ）所示：领航机器人在 初始位置（０，０）跟踪中间点（０，０．４）和（０，１．７）最终 到达目标点（０，２．２）；避障时，机器人偏离期望跟踪 点存在偏差，但随着编队的进行，机器人回归期望跟 踪轨迹，误差趋近于 ０。 跟随机器人 ＲＦ１ 和 ＲＦ２ 的位 姿如图 １４（ｅ）和（ｇ）所示：跟随机器人 ＲＦ１ 和 ＲＦ２ 都 以较小的误差跟踪虚拟跟随机器人 Ｒｖ２ 与 Ｒｖ１ 的轨 迹，完成编队控制。 从上述实验过程能够看出，机器人小组可以保 持某单一队形完成编队避障控制，也可以根据环境 的约束自主变换队形。 机器人小组在选择更安全的 队形通过障碍区后，自主恢复到初始队形，最终顺利 到达目标点，完成编队避障控制。 ６ 结束语 本文以两轮差动机器人为研究对象，提出了在 障碍环境中的多移动机器人的编队控制方法，通过 引入虚拟跟随机器人，将编队控制转化为跟踪控制， 此外，通过编队中的领航和跟随机器人携带的红外 线传感器自主感知获取外部环境信息，采用 Ｐｏｌａｒ Ｈｉｓｔｏｇｒａｍ 避障法，引导多机器人编队智能地避开障 碍物，实现了多移动机器人的编队避障控制，该避障 策略不存在阈值敏感性问题，因而对复杂的环境也 具有很好的适应性。 利用 Ｑｕａｎｓｅｒ 无人工具实验系 统仿真平台，分别对柱形编队、三角形编队以及队形 变换编队进行实物实验，由仿真实验结果可看出本 文所提出的控制方法的有效性，多移动机器人既可 以保持编队队形，同时各机器人也可以自主避障，实 现多移动机器人协调编队和避障控制。 参考文献： ［１］ＦＡＲＩＮＥＬＬＩ Ａ， ＬＯＣＣＨＩ Ｌ， ＮＡＲＤＩ Ｄ． Ｍｕｌｔｉｒｏｂｏｔ ｓｙｓｔｅｍｓ： ａ ｃｌａｓｓｉｆｉｃａｔｉｏｎ ｆｏｃｕｓｅｄ ｏｎ ｃｏｏｒｄｉｎａｔｉｏｎ［ Ｊ］． ＩＥＥＥ ｔｒａｎｓａｃ⁃ ｔｉｏｎｓ ｏｎ ｓｙｓｔｅｍｓ， ｍａｎ， ａｎｄ ｃｙｂｅｒｎｅｔｉｃｓ， ｐａｒｔ Ｂ （ ｃｙｂｅｒｎｅｔ⁃ ｉｃｓ）， ２００４， ３４（５）： ２０１５－２０２８． ［２］王祥科， 李迅， 郑志强． 多智能体系统编队控制相关问 题研究综述［ Ｊ］． 控制与决策， ２０１３， ２８ （ １１）： １６０１ － １６１３． ＷＡＮＧ Ｘｉａｎｇｋｅ， ＬＩ Ｘｕｎ， ＺＨＥＮＧ Ｚｈｉｑｉａｎｇ． Ｓｕｒｖｅｙ ｏｆ ｄｅ⁃ ｖｅｌｏｐｍｅｎｔｓ ｏｎ ｍｕｌｔｉ⁃ａｇｅｎｔ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｒｅｌａｔｅｄ ｐｒｏｂｌｅｍｓ ［Ｊ］． Ｃｏｎｔｒｏｌ ａｎｄ ｄｅｃｉｓｉｏｎ， ２０１３， ２８（１１）： １６０１－１６１３． ［３］杨帆， 刘士荣， 董德国． 编队控制中的机器人行为与基 于服务的运动行为结构设计［Ｊ］． 机器人， ２０１２， ３４（１）： １２０－１２８． ＹＡＮＧ Ｆａｎ， ＬＩＵ Ｓｈｉｒｏｎｇ， ＤＯＮＧ Ｄｅｇｕｏ． Ｒｏｂｏｔ ｂｅｈａｖｉｏｒ ａｎｄ ｓｅｒｖｉｃｅ⁃ｂａｓｅｄ ｍｏｔｉｏｎ ｂｅｈａｖｉｏｒ ｓｔｒｕｃｔｕｒｅ ｄｅｓｉｇｎ ｉｎ ｆｏｒｍａ⁃ ｔｉｏｎ ｃｏｎｔｒｏｌ［Ｊ］． Ｒｏｂｏｔ， ２０１２， ３４（１）： １２０－１２８． ［４］ ＡＲＲＩＣＨＩＥＬＬＯ Ｆ， ＣＨＩＡＶＥＲＩＮＩ Ｓ， ＩＮＤＩＶＥＲＩ Ｇ， ｅｔ ａｌ． Ｔｈｅ ｎｕｌｌ⁃ｓｐａｃｅ⁃ｂａｓｅｄ ｂｅｈａｖｉｏｒａｌ ｃｏｎｔｒｏｌ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ｗｉｔｈ ｖｅｌｏｃｉｔｙ ａｃｔｕａｔｏｒ ｓａｔｕｒａｔｉｏｎｓ［Ｊ］． Ｔｈｅ ｉｎｔｅｒｎａｔｉｏｎａｌ ｊｏｕｒ⁃ ｎａｌ ｏｆ ｒｏｂｏｔｉｃｓ ｒｅｓｅａｒｃｈ， ２０１０， ２９（１０）： １３１７－１３３７． ［５］宋敏， 魏瑞轩， 沈东， 等． 基于非线性动态逆的无人机 编队协同控制［ Ｊ］． 控制与决策， ２０１１， ２６ （ ３）： ４４８ － ４５２． ＳＯＮＧ Ｍｉｎ， ＷＥＩ Ｒｕｉｘｕａｎ， ＳＨＥＮ Ｄｏｎｇ， ｅｔ ａｌ． ＵＡＶ ｆｏｒｍａ⁃ ｔｉｏｎ ｃｏｎｔｒｏｌ ｂａｓｅｄ ｏｎ ｎｏｎｌｉｎｅａｒ ｄｙｎａｍｉｃ ｉｎｖｅｒｓｉｏｎ［ Ｊ］． Ｃｏｎ⁃ ｔｒｏｌ ａｎｄ ｄｅｃｉｓｉｏｎ， ２０１１， ２６（３）： ４４８－４５２． ［６］李少斌， 陈炎财， 杨忠， 等． 具有通信延迟的多无人机 编队飞行控制［ Ｊ］． 信息与控制， ２０１２， ４１ （ ２）： １４２ － １４６． ＬＩ Ｓｈａｏｂｉｎ， ＣＨＥＮ Ｙａｎｃａｉ， ＹＡＮＧ Ｚｈｏｎｇ， ｅｔ ａｌ． Ｆｏｒｍａｔｉｏｎ ｆｉｇｈｔ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉ⁃ＵＡＶｓ ｗｉｔｈ ｃｏｍｍｕｎｉｃａｔｉｏｎ ｄｅｌａｙ［ Ｊ］． Ｉｎｆｏｒｍａｔｉｏｎ ａｎｄ ｃｏｎｔｒｏｌ， ２０１２， ４１（２）： １４２－１４６． ［７］魏瑞轩， 茹常剑， 祁晓明． 通信延迟条件下无人机编队 重构的自主安全控制［ Ｊ］． 控制理论与应用， ２０１３， ３０ （９）： １０９９－１１０８． ＷＥＩ Ｒｕｉｘｕａｎ， ＲＵ Ｃｈａｎｇｊｉａｎ， ＱＩ Ｘｉｏａｍｉｎｇ． Ａｕｔｏｎｏｍｏｕｓ ｓａｆｅｔｙ ｃｏｎｔｒｏｌ ｏｆ ｕｎｍａｎｎｅｄ ａｅｒｉａｌ ｖｅｈｉｃｌｅ ｆｏｒｍａｔｉｏｎ ｒｅｃｏｎｆｉｇ⁃ ｕｒａｔｉｏｎ ｕｎｄｅｒ ｃｏｍｍｕｎｉｃａｔｉｏｎ ｄｅｌａｙ ［ Ｊ］． Ｃｏｎｔｒｏｌ ｔｈｅｏｒｙ ＆ ａｐｐｌｉｃａｔｉｏｎｓ， ２０１３， ３０（９）： １０９９－１１０８． ［８］吴锦杰， 刘昆， 韩大鹏， 等． 基于对偶四元数的卫星主 从式编队姿轨跟踪的优化控制［ Ｊ］． 控制理论与应用， ２０１３， ３０（９）： １０６９－１０７８． ＷＵ Ｊｉｎｊｉｅ， ＬＩＵ Ｋｕｎ， ＨＡＮ Ｄａｐｅｎｇ， ｅｔ ａｌ． Ｏｐｔｉｍｉｚｅｄ ｔｒａｃｋ⁃ ｉｎｇ⁃ｃｏｎｔｒｏｌ ｆｏｒ ａｔｔｉｔｕｄｅ ａｎｄ ｏｒｂｉｔ ｏｆ ｓａｔｅｌｌｉｔｅ ｆｏｒｍａｔｉｏｎ ｆｌｙｉｎｇ ｕｓｉｎｇ ｄｕａｌ ｑｕａｔｅｒｎｉｏｎ ［ Ｊ］． Ｃｏｎｔｒｏｌ ｔｈｅｏｒｙ ＆ ａｐｐｌｉｃａｔｉｏｎｓ， ２０１３， ３０（９）： １０６９－１０７８． ［９］ＷＡＮＧ Ｙ， ＹＡＮ Ｗ， ＬＩ Ｊ． Ｐａｓｓｉｖｉｔｙ⁃ｂａｓｅｄ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ ｏｆ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅｓ［Ｊ］． ＩＥＴ ｃｏｎｔｒｏｌ ｔｈｅｏｒｙ ＆ ａｐｐｌｉｃａｔｉｏｎｓ， ２０１２， ６（４）： ５１８－５２５． ［１０］王银涛， 严卫生． 多自主水下航行器系统一致性编队跟 踪控制［Ｊ］． 控制理论与应用， ２０１３， ３０（３）： ３７９－３８４． ＷＡＮＧ Ｙｉｎｔａｏ， ＹＡＮ Ｗｅｉｓｈｅｎｇ． Ｃｏｎｓｅｎｓｕｓ ｆｏｒｍａｔｉｏｎ ｔｒａｃｋｉｎｇ ｃｏｎｔｒｏｌ ｏｆ ｍｕｌｔｉｐｌｅ ａｕｔｏｎｏｍｏｕｓ ｕｎｄｅｒｗａｔｅｒ ｖｅｈｉｃｌｅ ｓｙｓｔｅｍｓ［Ｊ］． Ｃｏｎｔｒｏｌ ｔｈｅｏｒｙ ＆ ａｐｐｌｉｃａｔｉｏｎｓ， ２０１３， ３０（３）： ３７９－３８４． ［１１］ＲＥＮ Ｗｅｉ， ＳＯＲＥＮＳＥＮ Ｎ． Ｄｉｓｔｒｉｂｕｔｅｄ ｃｏｏｒｄｉｎａｔｉｏｎ ａｒｃｈｉ⁃ ｔｅｃｔｕｒｅ ｆｏｒ ｍｕｌｔｉ⁃ｒｏｂｏｔ ｆｏｒｍａｔｉｏｎ ｃｏｎｔｒｏｌ［ Ｊ］． Ｒｏｂｏｔｉｃｓ ａｎｄ ａｕｔｏｎｏｍｏｕｓ ｓｙｓｔｅｍｓ， ２００８， ５６（４）： ３２４－３３３． ［１２］ＧＨＯＭＭＡＭ Ｊ， ＭＥＨＲＪＥＲＤＩ Ｈ， ＳＡＡＤ Ｍ， ｅｔ ａｌ． Ｆｏｒｍａ⁃ ｔｉｏｎ ｐａｔｈ ｆｏｌｌｏｗｉｎｇ ｃｏｎｔｒｏｌ ｏｆ ｕｎｉｃｙｃｌｅ⁃ｔｙｐｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ［Ｊ］． Ｒｏｂｏｔｉｃｓ ａｎｄ ａｕｔｏｎｏｍｏｕｓ ｓｙｓｔｅｍｓ， ２０１０， ５８ （ ５）： 第 ２ 期 罗京，等：多移动机器人的领航－跟随编队避障控制 ·２１１·
