第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)可得：要有：基于行为法［３－４］ 、虚拟结构法［１１－１３］和领航跟随 法［１４－１６］ 。 此外，对于多移动机器人系统来说，避障 问题也是编队中需要考虑的一个重要问题。 在有障 碍物的约束环境下，多移动机器人的编队避障控制 会变得更加复杂，因为移动机器人既要保持整体队 形，又要合理躲避障碍物。 对于未知环境下的机器 人避障问题，已经有很多有效的解决方法。 文献 ［１７］提出人工势场法，基本原理是构造机器人和障 碍物、目标点间的力场，机器人将在目标点的引力和 障碍物的斥力的合力作用下运动。 势场法存在若干 缺陷，机器人在相近障碍物间不能找到路径，易在障 碍物前震荡，在狭窄通道中摆动［１８］ 。 为解决这些问 题，文献［１９］ 提出了向量场直方图法（ ｖｅｃｔｏｒ ｆｉｅｌｄ ｈｉｓｔｏｇｒａｍ：ＶＦＨ），该方法将障碍物对机器人的影响 量化为机器人各个角度上的障碍强度值，在障碍强 度值低于阈值的角度范围内选择移动方向。 ＶＦＨ 法存在阈值敏感问题，阈值过小时，一些可行通道被 忽略，阈值过大时，不一定发现前方障碍物［２０］ 。 本文采用领航－跟随编队控制算法来解决多移 动机器人系统的编队控制问题。 为使得多移动机器 人系统能够成功地进行编队和避障，还采用了一种 极坐标系下基于障碍物密度的 Ｐｌｏａｒ Ｈｉｓｔｏｇｒａｍ 避障 算法，该避障策略可有效避免 ＶＦＨ 法中存在阈值敏 感问题。 最后，通过 ３ 个 Ｑｂｏｔ 移动机器人的实验验 证了所提算法的有效性。 １ 两轮差分驱动机器人运动学 本文考察的是 Ｑｕａｎｓｅｒ 公司提供的 Ｑｂｏｔ 移动机 器人，属于差分驱动轮式移动机器人，假设其质心与 两驱动轮轴中心重合。 在轮式移动机器人的理论研 究中，一般都是假设车轮与地面之间点接触，且接触 点和地面之间只有纯滚动没有相对的滑动（包括纵 向与侧向滑动），这种理想条件使得机器人受到非 完整约束。 可根据刚体力学法求得两轮差动移动机 器人的运动学模型。 差分驱动机器人向前或向后运动时，左右驱动 轮的速度相等；当左右驱动轮之间存在速度差时，机 器人将会绕左（或右）驱动轮轴线上某一点旋转，该 点称为瞬时曲率中心 ＩＣＣ （ Ｉｎｓｔａｎｔａｎｅｏｕｓ Ｃｅｎｔｅｒ ｏｆ Ｃｕｒｖａｔｕｒｅ），如图 １ 所示。 机器人的运动轨迹可以通 过改变两驱动轮的速度进行控制。 机器人状态可由左右驱动轮轴中点坐标 （ｘ，ｙ） 和航向角 θ 表示。 图 １ 两轮差分机器人运动学 Ｆｉｇ．１ Ｔｗｏ ｄｉｆｆｅｒｅｎｔｉａｌ ｒｏｂｏｔ ｋｉｎｅｍａｔｉｃｓ 由图 １ 可得： Ｖｒ ＝ ω（Ｒ ＋ ｄ ／ ２） （１） Ｖｌ ＝ ω（Ｒ － ｄ ／ ２） （２） Ｖ ＝ Ｖｒ ＋ Ｖｌ ２ （３） 式中： Ｖｌ 和 Ｖｒ 分别为左右驱动轮的速度， Ｖ 为机器 人线速度（假设机器人速度 Ｖ 大于等于零）， ω 为机 器人绕 ＩＣＣ 的旋转角速度， Ｒ 为左右驱动轮轴心到 曲率中心的距离， ｄ 为左右驱动轮中心之间的距离。 由式（１），（２）变形可得： Ｒ ＝ ｄ（Ｖｒ ＋ Ｖｌ） ２（Ｖｒ － Ｖｌ） （４） ω ＝ （Ｖｒ － Ｖｌ） ／ ｄ （５） 机器人的正向运动学描述了机器人速度与位置 状态之间的关系，在给定左右驱动轮速度和机器人 初始位姿 （ｘ，ｙ，θ）ｔ ＝ ０ 的情况下，可以求得任意时刻 ｔ 时的机器人的位姿 （ｘ，ｙ，θ）ｔ ＝ ｔ 。 下面介绍机器人相对初始状态的位姿［２１］ 。 已知机器人速度 Ｖ（ｔ） 和航向角 θ（ｔ） ，机器人 在 ｔ ＝ ０ 时刻的初始位姿为 （０，０，０） ，受非完整约束 条件下的轮式机器人存在如下关系： ｘ（ｔ） ＝ ∫ ｔ ０ Ｖ（ｔ）ｃｏｓ［θ（ｔ）］ｄｔ ｙ（ｔ） ＝ ∫ ｔ ０ Ｖ（ｔ）ｓｉｎ［θ（ｔ）］ｄｔ θ（ｔ） ＝ ∫ ｔ ０ ω（ｔ）ｄｔ ì î í ï ï ïï ï ï ï （６） 由式（６）可得机器人在 ｔ ＋ δｔ 的位姿： ｘ ∗ ｙ ∗ θ ∗ é ë ê ê êê ù û ú ú úú ＝ ｘ ＋ Ｖｃｏｓ（θ）δｔ ｙ ＋ Ｖｓｉｎ（θ）δｔ θ ＋ ωδｔ é ë ê ê ê ù û ú ú ú （７） 式中： （ｘ，ｙ，θ） 和 （ｘ ∗ ，ｙ ∗ ，θ ∗ ） 分别为 ｔ 时刻和 ｔ ＋ δｔ 时刻机器人的位姿。 由式（７）可知，通过改变线速度 Ｖ 和角速度 ω 可以控制移动机器人的运动轨迹。 对于两轮差动机器人将式（３）和（５）带入方程 （６）可得： 第 ２ 期 罗京，等：多移动机器人的领航－跟随编队避障控制 ·２０３·