第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
４）照相机定位系统。 图 ６ Ｑｂｏｔ 实物图 Ｆｉｇ．６ Ｑｂｏｔ ｐｈｙｓｉｃａｌ ｆｉｇｕｒｅ 图 ７ Ｑｕａｎｓｅｒ 无人工具实验系统 Ｆｉｇ．７ Ｑｕａｎｓｅｒ ｕｎｍａｎｎｅｄ ｔｏｏｌ ｅｘｐｅｒｉｍｅｎｔ ｓｙｓｔｅｍ ＱｕａＲＣ 实时控制软件与 Ｍａｔｌａｂ ／ Ｓｉｍｕｌｉｎｋ 兼容， 可以方便地调用 Ｍａｔｌａｂ ／ Ｓｉｍｕｌｉｎｋ 中的函数。 将设 计的控制器与相应的系统硬件端口 Ｓｉｍｕｌｉｎｋ 模块相 连，编译并下传到 ＱｕａＲＣ 中，实现实时控制。 地面 控制站是通过一台地面控制计算机来实现，包括定 位、多智能体控制等，主控制机通过无线局域网和各 个智能体进行通信，地面控制站主要进行任务规划 和定位等目的，一旦控制系统的控制算法设计完成， 控制站就只起到定位作用。 无人地面机器人是基于 ｉＲｏｂｏｔ 的 Ｃｒｅａｔｅ 平台开发的自主移动机器人 Ｑｂｏｔ， 它带有 ５ 个红外线传感器，可以用于探测障碍物，探 测距离最大可达 。 照相机定位系统通过 ８ 个红外 照相机实现空间的三维定位。 ４．２ 实验原理 系统采用 Ｈｏｓｔ⁃Ｔａｒｇｅｔ 工作模式进行实时控制， 控制器开发在装有 ＭＡＴＬＡＢ ／ Ｓｉｍｕｌｉｎｋ 的主机上实 现，控制器搭建完成后，在 Ｓｉｍｕｌｉｎｋ 下进行编译并且 下载到移动机器人的控制器（ｔａｒｇｅｔ）中实现实时控 制。 主机 Ｈｏｓｔ 可同时控制多个控制器（ Ｔａｒｇｅｔ），从 而可以在 ＭＡＴＬＡＢ 环境下进行多智能体控制算法 的研究。 每个移动机器人上都装有 ３ 个红外线反射 球，使装在墙壁上的 ８ 台红外照相机可以实时捕捉 每个移动机器人的位姿坐标 （ｘ，ｙ，θ） ，通过主机 Ｈｏｓｔ 和无线网络传输给每个移动机器人，跟随机器 人按预先设计好的算法计算出与领航机器人的位置 差，调整方向跟踪领航机器人以实现编队控制。 编 队过程中，领航和跟随机器人通过自身携带的红外 线传感器检测环境中的障碍物，机器人根据检测到 的障碍物情况利用 Ｐｏｌａｒ Ｈｉｓｔｏｇｒａｍ 避障控制策略实 时避开障碍物，实现编队避障控制。 ４．３ 算法实现步骤 １）在主机 Ｈｏｓｔ 上设计领航机器人的跟踪路径。 建立无线局域网。 ２）主机 Ｈｏｓｔ 实时地将照相机定位系统捕捉到 的领航和跟随机器人的位姿信息发送给对应的领航 和跟随机器人，同时将设计好的路径命令发送给领 航机器人。 ３）领航机器人实时接收主机传来的路径命令 和自身位姿信息向目标点运行，当自身携带的红外 线传感器检测到障碍物时，利用设计好的避障算法 进行避障，绕过障碍后继续驶向目标点。 同时，领航 机器人根据 ｌ⁃φ 控制法生成虚拟机器人的轨迹命 令，并发送给跟随机器人。 ４）跟随机器人实时接收主机 Ｈｏｓｔ 发送的自身 位姿信息和领航机器人发送的虚拟机器人轨迹命 令，跟随机器人沿着虚拟跟随机器人的轨迹跟随领 航机器人，同时能够有效避开环境中的障碍物，实现 编队避障控制。 ４．４ 半实物仿真实验及结果分析 设计一个 ３ ｍ × ３ ｍ 的场地，构造机器人编队 避开障碍物到达目标点的仿真环境，利用领航跟 随编队法和 Ｐｏｌａｒ Ｈｉｓｔｏｇｒａｍ 避障法，分别进行了 ３ 个机器人柱形编队避障控制实验和 ３ 个机器人 三角形编队避障控制实验，柱形和三角形编队队 形如图 ８ 所示。 图 ８ 多移动机器人的编队队形 Ｆｉｇ．８ Ｆｏｒｍａｔｉｏｎ ｏｆ ｍｕｌｔｉｐｌｅ ｍｏｂｉｌｅ ｒｏｂｏｔｓ ·２０６· 智 能 系 统 学 报 第 １２ 卷