第 xx 卷 第 x 期 Vol. xx No. x 控 制 与 决 策 Control and Decision xxxx 年 x 月 Xxx. xxxx 文章编号: 2012-1128 (xxxx) xx-xxxx-xx 一类非完整移动机器人编队控制方法 张瑞雷, 李 胜, 陈庆伟 (南京理工大学 自动化学院，南京 210094) 摘 要: 针对大部分两轮非完整移动机器人轮轴中心与几何中心不重合的特点, 提出一种多机器人协调编队控制算 法. 构造队形参数矩阵确定编队形状, 根据领航机器人和相关队形参数生成虚拟机器人, 把编队控制分解为跟随机器 人对虚拟机器人的轨迹跟踪. 建立虚拟机器人与跟随机器人之间误差系统模型, 利用Lyapunov理论设计相应控制器 实现了队形保持和变换. 应用Microsoft Robotics Developer Studio 4(MRDS4)搭建3D仿真平台, 设计三组实验, 结果进 一步验证了所提方法的有效性. 关键词: 非完整移动机器人; 轨迹跟踪; 编队控制; 队形变换 中图分类号: TP273 文献标识码: A Formation Control for a Kind of Nonholonomic Mobile Robots ZHANG Rui-lei, LI Sheng , CHEN Qing-wei (School of Automation，Nanjing University of Science and Technology，Nanjing 210094，China. Correspondent：ZHANG Rui-lei，E-mail：zhangrelay@qq.com ) Abstract: For the problem of the most nonholonmic mobile robot’s wheel-axle center and geometric center are noncoincidence, the formation coordination control algorithm is proposed in this paper. The shape of formation is determined by the parameter matrix, the virtual robot is generated according the lead robot and related formation parameters, translating the formation control problem into the trajectory tracking problem between the follow robot and virtual robot. The error system model between two robots is builded, the control algorithm is designed via the lyapunov theory for the formation keeping and transforming. Three groups of experiment are designed by the Microsoft Robotics Developer Studio 4, the simulation results show the effectiveness of the presented method. Key words: Nonholonomic mobile robot; Trajectory tracking; Formation control; Formation transform 1 引引引 言言言 近 年 来, 机 器 人 的 协 调 控 制 成 为 多 机 器 人 系 统研究 热点. 多机 器人 通过 协调 能完 成某 些单 个 机器人不能完成的任务, 通常有更高的效率. 编队 控 制 是 一 个 典 型 的 多 机 器 人 协 调 问 题, 在 地 理 勘 测、侦查、救援、扫雷和运输等领域有广阔的应用 前景. 目前, 多机器人的编队控制算法主要包括基 于行为(Behavior-Based) 的方法[1,2]、虚拟结构(Virtual Structure) 法[3,4]和领航-跟随(leader-follower)法[9-12]. 文[2]研究了一组仅有局部感知和少量通讯的分 布式机器人, 通过全局行为的队形控制方法, 实现了 多机器人编队的建立和保持. 文[3] 针对通信范围和 带宽限制, 基于虚拟结构法, 提出了一种统一的分布 式编队控制结构, 结合一致性理论和轨迹跟踪, 实现 了多机器人编队拓扑的切换和控制. 文[4]提出了一种 基于模型预测控制方法的编队控制器, 并将其用于跟 踪虚拟领航机器人. 文[5]将领航-跟随方法应用于潜 水器领域, 文[6-8]研究了一类控制输入受约束非完整 移动机器人的编队控制问题. 文[9]将编队控制问题分 解为轨迹跟踪、机器人控制和队形控制三个子问题, 通过l ϕ和l l控制, 采用输入输出反馈线性化方法 实现了多机器人的编队控制. 文[10]通过在跟随机器 人上安装观测器用于估计领航机器人速度, 仅依靠通 信位置信息实现编队控制, 但是编队精度较差, 领航 收稿日期: xxxx-xx-xx；修回日期: xxxx-xx-xx. 基金项目: 国家自然科学基金(60975075, 61074023). 作者简介: 张瑞雷(1985), 男, 博士生, 从事机器人智能控制、多机器人协调控制的研究；陈庆伟(1963), 男, 教授, 博 士生导师, 主要研究方向智能控制与智能系统. 

2 控 制 与 决 策 第 xx 卷 机器人做复杂运动时, 编队保持不理想. 非完整约束机器人在纯滚动、无滑动下, 使用反 馈线性化的队形控制方法需要选定离轴点, 该点位于 机器人的前进方向, 与质心有一定的偏移量, 当此偏 移量较大时, 引起较大的队形误差, 较小时, 产生不合 适的运动. 文[11]提出的队形控制方法解决了上述问 题, 但其算法实现要保证内三角形的关系. 文[12]在此 基础上提出改进的编队控制算法, 实现了包括线性队 形在内的队形控制. 上述文献研究机器人的运动学模型多为轮轴中 心与几何中心重合的两轮差动机器人, 并且对队形的 控制精度分析较少, 没有讨论不同速度条件对跟踪误 差影响, 跟随机器人与领航者之间也是单向通信的关 系. 本文在此基础上针对轮轴中心与几何中心不重合 的非完整移动机器人, 设计编队控制策略, 实现编队 形状保持和变换. 最后利用MRDS4仿真平台设计实 验进行算法验证. 2 编编编队队队控控控制制制结结结构构构 2.1 队队队形形形描描描述述述 多机器人编队的队形通常有线性、柱形、菱形和 梯形等, 如图1所示 图 1 四个机器人典型队形结构 编队系统一般设定一个主领航机器人, 负责提供编队 的导航, 决定了编队的主轨迹. 如果系统中的两个机 器人存在局部的领航与跟随关系, 则分别称这两个机 器人为领航机器人和跟随机器人. 如图1(c)中,R1为主 领航机器人,R1 R2、R1 R3和R2 R4为三个局部 领航-跟随对. 为了指定队形参数、跟踪的领航机器人 的编号, 定义队形参数矩阵F如下: F = [F1 F2 ... Fn] 3n , (1) T n ρr n ϕr n] 表示机器人Rn的队形参数, 它 式中Fn = [ir n, 与对应 包含3部分, 分别为Rn跟踪的机器人的编号ir n, 一般选 领航机器人保持的期望距离ρr n和期望角度ϕr 定R1为主领航机器人, 定义主领航机器人R1队形参 数F1 = [0 0 0] 2.2 控控控制制制结结结构构构 T . 在控制器设计前, 简要描述一下系统的控制结 构. 系统主要分为两层, 上层为通信协调层, 主要提供 多机器人之间通信和协调信息, 下层为领航-跟随控 制层, 如图2所示: 图 2 领航-跟随控制结构 3 控控控制制制器器器设设设计计计 3.1 问问问题题题描描描述述述 大部分两轮驱动移动机器人的轮轴中心和几何 中心不完全重合, 其结构如图3所示: 图 3 非完整移动机器人结构 其中, C为机器人的轮轴中心, P 为机器人主体的几何 中心, d为两者之间的距离, (x, y)T 为P 点在XY 系中 的坐标,θ为机器人运动方向和X轴间夹角, (v, ω)T 为 机器人的线速度和角速度. 由于此类机器人的轮轴中 心C和主体几何中心P 不重合, 在驱动轮纯滚动、无滑 动的情况下, 该机器人模型受如下非完整约束, 即 此类机器人运动学模型为:  _x sin θ _y cos θ = dω,   cos θ d sin θ  =  _x [ sin θ d cos θ _x cos θ + _y sin θ = v. _y _θ ] . (2) (3) v ω 0 1 3.2 跟跟跟踪踪踪控控控制制制 为描述本文的队形控制算法, 给出如下假设:虚 拟机器人由领航机器人根据队形配置矩阵生成, 其 几何中心与领航机器人几何中心的距离为ρr, 角度 为ϕr, 其运动方向与领航机器人方向一致, 线速度和 角速度与领航机器人相同, 即vv = vl, ωv = ωl. 一组 领航-跟随机器人的结构模型如图4所示, Rl为领航机 器人, Rv为虚拟机器人, Rf 为跟随机器人, 所有机器 人的运动学模型相同. (xl, yl, θl)T 为领航机器人位姿, (xf , yf , θf )T 为跟随机器人位姿, 机器人Rf 的队形参 )(a)(b)(c)(d1R2R3R4R1R2R3R4R1R2R3R4R1R2R3R4R()ril=(,,)lllxyq(,)rrrffR跟随机器人lR领航机器人fF队形参数vR虚拟机器人误差计算),,(vvvyxq),,(eeeyxqfu跟随控制器(,)ffvw(,,)fffxyqwvdqxyXYOPC

第 x 期 张瑞雷 等: 一类非完整移动机器人编队控制方法 3 数Ff = [l ρr ϕr] T . 由图4可得虚拟机器人的位姿Rv:  xv = xl ρr cos(θl + ϕr), yv = yl ρr sin(θl + ϕr), θv = θl. (4) (5) 1 2 [xe d(1 cos θe)]2 + 1 2 [ye + d sin θe + Kθe]2 + (1 cos θe).因此, 该Lyapunov函数V 是正定的, 渐 1 Ky 小的. 对(8)式求导, 并将控制率(7)代入, 整理可得: _V = (1 + e t)fKx[xe d(1 cos θe)]2 vvKyKα (ye + d sin θe + Kθe)2 βvv sin2θe + vv sin θe KyK (ye + d sin θe + Kθe) vv(α + β) sin θe(ye + d [xe d(1 cos θe)]2 + sin θe + Kθe)g e tf 1 2 [ye + d sin θe + Kθe]2 + 1 2 (9) (1 cos θe)g, 1 Ky 若α + β = 1, 则: _V = (1 + e t)fKx[xe d(1 cos θe)]2 vvKyKα tf 1 2 (ye + d sin θe + Kθe)2 βvv KyK [xe d(1 cos θe)]2 + [ye + d sin θe + Kθe]2 sin2θeg e 1 2 (1 cos θe)g, + 1 Ky (10) t+V e _V 负 定, 因 此 Kx, Ky, K, α, β为 正 常 数, 由 此 可 知, 当t ! 1时, V 有 界, 进 一 步 可 知xe, ye, θe有 界, 并 由vl, ωl, _vl, _ωl有界即vv, ωv, _vv, _ωv有界得: V = V1(1 + t有界, 其中: V1 = 2Kx[xed(1 t)2 _V e e _θe + β cos θe)]( _xe d sin θe _θe) sin e _vv sin θe) KyK(ye + d sin θe + Kθe)f2α vv [ _ye + (K + d cos θe) _θe] + α _vv(ye + d sin θe + Kθe)g, 因此, _V ! 0, 可推得xe ! 0, ye ! 0, θe ! 0, 证毕.2 _V 是一致连续的. 由Barbalat引理可知, 当t ! 1时, (2β vv cos θe KyK 4 仿仿仿真真真与与与实实实验验验结结结果果果 Microsoft Robotics Developer Studio 4提供逼真的 物理引擎(PhysX)和完备的接口, 能够精确模拟现实 环境中重力摩擦力等多种环境条件, 也可将仿真数 据导入到Matlab中进行分析, 因此本文利用该软件设 计仿真实验. 选用两种机器人模型进行实验, 分别 为Pioneer3DX和iRobot, 测试算法的有效性和编队控 制精度性能, 仿真场景如图5所示. 图 5 仿真实验场景 图 4 一组机器人编队和领航-跟随对结构 定义如下坐标变换: xe  =  cos θf ye θe sin θf 0 sin θf cos θf 0 0 0 1  ,  xv xf yv yf θv θf 其中, (xv xf , yv yf , θv θf )T 为图4所示X Y 坐 标系下的跟随机器人Rf 与虚拟机器人Rv的位姿误差, (xe, ye, θe)T 为坐标变换后的机器人Rf 位姿误差. 对 误差方程(5)求微分, 并结合式(3)和(4), 整理后可得如 下的误差系统:  _xe = ωf ye vf + vv cos θe + ωvd sin θe, _ye = ωf xe + vv sin θe + ωf d ωvd cos θe, _θe = ωv ωf . (6) (vf , ωf )T 为跟随机器人线速度和角速度. 由此可见, 本 文 将 编 队 控 制 问 题, 转 化 为 跟 随 机 器 人 对 虚 拟 机 器 人 的 轨 迹 跟 踪 问 题, 通 过 构 造 误 差 系 统, 寻 求 合 适 的 控 制 律vf 和ωf , 使 得 误 差 一 致 渐 近 收 敛 到0( lim t!1  vf = vv cos θe + Kx[xe d(1 cos θe)] Kθeωf , jxej = 0, lim t!1 jyej = 0, lim t!1 ωf = ωv + vv[Kyα(ye + d sin θe + Kθe) + jθej = 0), 即 β K sin θe]. 式中, Kx, Ky, K, α, β为正常数. (7) 定定定 理理理 1 假 设 领 航 机 器 人Rl的 线 速 度vl > 0且vl, ωl, _vl, _ωl有界, 跟随机器人Rf 遵循控制率(7), 则 式(6)描述的误差系统全局一致渐近稳定. 证证证 明明明 考 虑 如 下 的Lyapunov函 数(V (xe, ye, θe, t), 下 面简写为V ): V = (1 + e t)f 1 2 [xe d(1 cos θe)]2 + (1 cos θe)g. 1 2 1 Ky d sin θe + Kθe]2 + (8) 显 然, 函 数V 满 足V1 V 2V1, 其 中:V1 = [ye + XYOlRvRrrrjlq'L''LfRfqlyvyfyfxlxvxvqexeylRfR1R2R3R4R012 Pioneer3DXiRobot

4 控 制 与 决 策 第 xx 卷 Pioneer 3DX的d参数为0.17m, iRobot的d参数为0.01m, 控 制 周 期T为50ms, 其 他 相 关 控 制 器:Kx = 1.1, Ky = 1.2, K = 0.7, α = 0.4, β = 0.6. 实验1, 三个iRobot机器人保持三角队形. 图 6 三角形编队实验 √ (xe 2 + ye 编队轨迹如图6(a)所示, 主领航机器人为R1, 跟随机 器人R2和R3快速完成编队, 并跟随R1保持编队. 各跟 随机器人的编队误差( 2), 如图6(b)所示, 可 以看出多机器人系统在较短时间内(< 5.0s)成功的形 成编队, 并以较小的误差(< 0.05m)保持队形, 较大的 初始误差是由于各跟随机器人的初始位置远离队形 配置位置导致. 各机器人的控制输入线速度和角速度, 如图6(c)和(d)所示. 领航机器人的线速度和角速度控 制量是时变的, 当跟随机器人在外侧时线速度较大, 在内侧时线速度较小, 各跟随机器人的角速度与领航 机器人基本相同. 实验2, 五个Pioneer3DX机器人保持T形队形. 图 7 T形编队实验 编队轨迹如图7(a)所示, 跟随机器人在主领航机器 人R1带领下做圆周运动. 由图7(b)可以看出多机器人 系统在较短时间内(< 5.0s)成功的完成编队, 并非常 准确保持队, 误差在完成编队后很小(< 0.01m). 各机 器人的控制输入线速度和角速度, 如图7(c)和(d)所示. 由于领航机器人R1的线速度和角速度控制量是常量, 各跟随机器人的跟踪精度比第一个实验要高. 稳态 时, 机器人R1, R3和R5线速度相同, R4走外圈线速度 较大, R2走内圈线速度较小, 五个机器人角速度相同. 实 验3, 两 种 机 器 人Pioneer3DX和iRobot组 成 混 合编队并实现队形的动态切换. 图 8 混合编队实验 编队轨迹如图8(a)所示, 五个机器人由初始位置, 先完 成柱形编队行驶一段时间后切换为三角形编队, 最后 切换为线性编队. 由图8(b)可知, 机器人能在较短时间 内(< 10.0s)平滑地完成队形切换, 说明该算法具有良 好的动态性能并可以适用于不同参数两轮差动机器 人完成混合编队控制, 并具有较高的编队精度. 5 结结结 论论论 本文提出一种针对轮轴中心与几何中心不重合 的非完整移动机器人动态编队方法. 将非完整移动机 器人的编队控制问题分解为控制领航机器人和控制 编队中的跟随机器人两个子问题, 运用队形参数矩阵 确立多机器人之间的相对位置关系, 通过配置不同的 参数矩阵改变编队队形, 实现了多机器人队形变换. 利用MRDS4软件搭建仿真平台, 由仿真实验可以看 出本文提出的控制算法可靠性好, 保持编队时具有较 高的精度, 能在较短的时间内顺利完成队形变换. 参考文献(References) [1] Tucker Balch, and Ronald C. Arkin. Behavior-Based Formation Control for Multirobot Teams[J]. IEEE Trans. On Robotics and Automation, 1998, 14(6): 926-939. [2] Jakob Fredslund and Maja J. Mataric. A General Algorithm for Robot Formations Using Local Sensing and Minimal Communication[J]. IEEE Trans. On Robotics and Automation, 2002, 18(5): 837-846. 0000222244446666888810101010-4-4-4-4-3-3-3-3-2-2-2-2-1-1-1-10000111122223333x [m]x [m]x [m]x [m]y [m]y [m]y [m]y [m] R1R1R1R1R2R2R2R2R3R3R3R300002020202040404040606060608080808010010010010012012012012000000.50.50.50.511111.51.51.51.5时时时时时时时时 [s][s][s][s]误误误误误误误误 [m][m][m][m] R2-R1R2-R1R2-R1R2-R1R3-R1R3-R1R3-R1R3-R15959595960606060616161610.010.010.010.010.020.020.020.020.030.030.030.030.040.040.040.040.050.050.050.050.060.060.060.06 (a) 编队轨迹 (b) 编队误差 00002020202040404040606060608080808010010010010012012012012000000.20.20.20.20.40.40.40.40.60.60.60.60.80.80.80.81111时时时时时时时时 [s][s][s][s]线线线线线线线线线线线线 [m/s][m/s][m/s][m/s] R1R1R1R1R2R2R2R2R3R3R3R3000020202020404040406060606080808080100100100100120120120120-1-1-1-1-0.5-0.5-0.5-0.500000.50.50.50.511111.51.51.51.52222时时时时时时时时 [s][s][s][s]角角角角线线线线线线线线 [rad/s][rad/s][rad/s][rad/s] R1R1R1R1R2R2R2R2R3R3R3R3 (c) 线速度控制输入 (d)角速度控制输入 0000555510101010-6-6-6-6-4-4-4-4-2-2-2-20000222244446666x [m]x [m]x [m]x [m]y [m]y [m]y [m]y [m] R1R1R1R1R2R2R2R2R3R3R3R3R4R4R4R4R5R5R5R500005050505010010010010000000.50.50.50.511111.51.51.51.5时时时时时时时时 [s][s][s][s]误误误误误误误误 [m][m][m][m] R2-R1R2-R1R2-R1R2-R1R3-R1R3-R1R3-R1R3-R1R4-R1R4-R1R4-R1R4-R1R5-R3R5-R3R5-R3R5-R399.9599.9599.9599.95100100100100100.05100.05100.05100.0500000.50.50.50.511111.51.51.51.5x 10x 10x 10x 10-3-3-3-3 (a) 编队轨迹 (b) 编队误差 00005050505010010010010000000.20.20.20.20.40.40.40.40.60.60.60.60.80.80.80.81111时时时时时时时时 [s][s][s][s]线线线线线线线线线线线线 [m/s][m/s][m/s][m/s] R1R1R1R1R2R2R2R2R3R3R3R3R4R4R4R4R5R5R5R5000050505050100100100100-2-2-2-2-1-1-1-1000011112222时时时时时时时时 [s][s][s][s]角角角角线线线线线线线线 [rad/s][rad/s][rad/s][rad/s] R1R1R1R1R2R2R2R2R3R3R3R3R4R4R4R4R5R5R5R5 (c) 线速度控制输入 (d)角速度控制输入 000020202020404040406060606080808080000055551010101015151515X [m]X [m]X [m]X [m]Y [m]Y [m]Y [m]Y [m] R1R1R1R1R2R2R2R2R3R3R3R3R4R4R4R4R5R5R5R5000010010010010020020020020030030030030040040040040000000.50.50.50.511111.51.51.51.522222.52.52.52.53333时时时时时时时时 [s][s][s][s]误误误误误误误误 [m][m][m][m] R2-R1R2-R1R2-R1R2-R1R3-R1R3-R1R3-R1R3-R1R4-R1R4-R1R4-R1R4-R1R5-R1R5-R1R5-R1R5-R1 (a) 编队轨迹 (b) 编队误差 

第 x 期 张瑞雷 等: 一类非完整移动机器人编队控制方法 5 [3] Wei Ren and Nathan Sorensen. Distributed coordination architecture for multi-robot formation control[J]. Robotics and Autonomous Systems, 2008, 56(4): 324-333. [4] Dongbing Gu and Huosheng Hu. A model predictive controller for robots to follow a virtual leader[J]. Robotica, 2009, 27(6): 905-913. [5] Rongxin Cui, Shuzhi Sam Ge and Bernard Voon En How. Leader-follower formation control of underactuated autonomous underwater vehicles[J]. Ocean Engineering, 2010, 37(17-18): 1491-1512. Jawhar Ghomman, Hasan Mehrjerdi and Maarouf Saad. Formation path following control of unicycle-type mobile robots[J]. Robotics and Autonomous Systems, 2010, 58(5): 727-736. [6] [7] Hasan Mehrjerdi, Jawhar Ghomman and Maarouf Saad. Nonlinear coordination control for a group of mobile robots using a virtual structure[J]. Mechatronics, 2011, 21(7): 1147-1155. [8] Luca C, Fabio M. and Domenico P. Leader-follower formation control of nonholonomic mobile robots with input constraints[J]. Automatica, 2008, 44(5): 1343-1349. [9] Jaydev P. Desai, James P. Ostrowski and Vijay Kumar. Modeling and Control of Formations of Nonholonomic Mobile Robots[J], IEEE Transactions on Robotics and Automation, 2001, 17(6): 905-908. [10] Tove Gustavi and Xiaoming Hu. Observer-based Leader- Following Formation Control Using Onboard Sensor Information[J]. IEEE Transactions on Robotics, 2008, 24(6): 1457-1462. [11] Shao J, Xie G and Wang L. Leader-following formation control of multiple mobile vehicles[J]. Control Theory & Applications, 2007, 1(2): 545-552. [12] Yang Li, Cao Zhiqiang and Tan Min. Dynamic Formation Control for Multiple Robots in Uncertain Environments[J]. Robot, 2010, 32(2):283-288.