三维环境下基于反步法的多机器人编队控制

冯 磊，肖伸平

（湖南工业大学 电气与信息工程学院，湖南 株洲 412007）

三维环境下基于反步法的多机器人编队控制

冯 磊，肖伸平

（湖南工业大学 电气与信息工程学院，湖南 株洲 412007）

针对两轮式移动机器人在复杂环境下的编队控制问题，提出一种基于虚构领航法和反步法，并结合人工势场法策略的多机器人避障编队算法。首先，详细分析多机器人系统在三维空间下的编队模型，并利用空间投影方法将其映射到二维平面进行分析。其次，将运动学模型转化为链式形式，并通过正则坐标变换,将误差系统形式转换成串联非线性系统。然后运用Backstepping方法构造轮式机器人追踪系统的Lyapunov函数，设计出针对轮式机器人的轨迹跟踪控制器。再结合人工势场法避障策略，完成多机器人复杂环境下的编队任务。最后，通过多机器人轨迹跟踪的两组仿真实验，验证了所提出方法的有效性。

三维空间；人工势场法；反步法；李雅普诺夫函数；编队控制

1 研究背景

近年来，随着机器人技术的发展，多机器人的稳定控制和轨迹跟踪问题越来越受到国内外学者的关注[1]。相对于稳定问题，轨迹跟踪是一个更实际的控制问题。而编队往往将面对复杂的环境，因此，在障碍物环境下，迫切需要寻找一条从起始位置到达目标位置的避障路径。而路径规划中的人工势场法以其数学计算简单明了而被广泛应用。目前，国内外学者进行编队研究的机器人主要有地面自主移动机器人、水下自主式机器人、卫星和无人飞行器等[2-3]；多机器人的控制算法主要包括虚拟结构法、领航跟随法、图论法和基于行为的方法[4-6]。

在当前的研究中，文献[7]综合路径跟踪法和虚拟结构法，实现了多机器人系统的动态编队控制。其缺点是其虚拟结构运动的队形要求限制了该方法的应用范围，难以实现灵活的队形控制。文献[8]根据领航机器人和跟随机器人之间的相对运动模型，构造了一种鲁棒编队控制器，有效地解决了多机器人对象模型中的参数不确定性问题。文献[9]中基于图论法和李雅普诺夫函数提出一种多智能体分布式编队控制的理论框架，通过分布式控制所有机器人来实现整个系统的编队控制。文献[10]提出一种带队形反馈的分布式动态编队控制算法，该算法将虚结构法和行为法相融合，通过分段设定编队的虚构位置达到编队控制的目的。但未明确指出达到整体行为的局部控制规则，很难保证队形控制的稳定性。文献[11]基于人工势场法提出一种编队控制器，可在不完整的传感器信息或者机器人出现故障的情况下应用。但是其势场函数的设计比较困难，而且存在局部极值点的问题。

尽管机器人编队控制研究近年来有了较大进展，但仍有不少问题有待于进一步研究。比如：没有统一有效的框架描述机器人控制与编队算法，缺乏适应性和灵活性。稳定性是编队控制的重要因素，有些控制方法从理论上很难得到保证。实际运动环境中机器人往往呈现动态的性质，因此三维环境下移动机器人编队避障控制问题的研究具有一定的迫切性。

本文基于轮式移动机器人三维空间下的编队运动学模型，将其映射到二维平面进行分析，并结合虚构法和领航跟随法，将领航机器人的位姿参数转换生成虚拟机器人的位姿参数，再利用反步法和李雅普诺夫函数设计该虚拟位姿的轨迹跟踪控制器，结合人工势场法，以解决多机器人在复杂环境下的编队避障控制问题。本文设计了2组仿真实验，实现复杂环境下的经典三角形和动态变换队形的编队避障控制，验证了控制器的性能及所提方法策略的有效性与稳定性。

2 人工势场法原理

人工势场法是将复杂环境信息转化为斥力场和引力场模型，并通过此模型来找出一条从初始点到目标点的路径的方法，是路径规划传统算法中较高效且成熟的规划方法。

设目标的位置信息为qo=[xoyo]T，机器人的位置信息为q=[x y]T，第i障碍物的位置信息为qg(i)=[xg(i) yg(i)]T，则机器人的斥力势场函数可表达为[12]：

引力势场函数的表达式为

斥力函数的表达式为

引力函数的表达式为

式（1）～（4）中：d为机器人和障碍物间的最短距离；

d0为障碍物的影响距离范围；

kr为斥力正比例因子；

ka为引力比例系数。

3 总体方案设计

两轮式移动机器人模型的三维坐标结构如图1所示。其中，x, y, z为机器人在三维空间的X，Y和Z坐标；为机器人的俯仰角；为机器人的横摇角；为机器人的方位角。

通过对机器人模型在三维空间中的分析与计算，并对x, y, z和求导，得到的运动学模型为

式中w和v为机器人的角速度和线速度。

再根据空间投影方法，将运动模型映射到x-y坐标系统。在本研究中，为便于设计和分析，将两轴的中心和机器人的重心重合，因此轮式机器人的运动学模型可以简化为

式中 v′=vcos，为机器人在x-y坐标下的线速度。

多机器人模型的三维坐标编队结构如图2所示。编队队伍的主要轨迹为领航机器人的运动轨迹，每个跟随机器人的期望位姿参数决定其编队队形，机器人均为同样的运动学模型。

图2中，R1是领航机器人，Rv为期望虚拟机器人，相对领航者R1的期望距离与角度分别为和，R2和R3为跟随机器人。[x1y1z1α1β1γ1]T，[xfyfzfαfβfγf]T和[xvyvzvαvβvγv]T分别为R1，R2和Rv的构成向量。

Rv的前进速度和角速度与R1保持一致。为了简化控制器的设计,通过坐标变换后，机器人R2的运动学模型表达式转换成链式形式，并对其求导可得:

式中：u1, u2为R2变换后的控制输入变量；

x1, x2, x3为R2变换后的状态变量。

同理，虚拟机器人Rv的运动学模型表达式转换成如下链式形式：

式中：uv1, uv2为Rv变换后的控制输入；

xv1, xv2, xv3为Rv变换后的状态变量。

此时，R2控制输入表达式的变换方程可表示为

式中v, w为R2在x-y坐标下的线速度和角速度。

变换后的跟踪误差可定义为xei=xi-xvi，i=1, 2, 3；并对xei进行求导可以得出：

由此，将机器人的编队问题转化为跟随机器人对虚拟机器人轨迹的跟踪问题，因此需要寻找合适的控制律u1, u2，使得跟踪误差xei渐近收敛到0。

4 控制器设计及控制策略

首先，由式（10）形成的控制系统通过坐标变换转换成一个三角形式，形成如下控制律，并对其求导可得:

式中z1, z2, z3均为误差状态变量。

然后通过使用反步法，逐步递推出各子系统的控制量。

令虚拟控制量z2的期望值为f1，误差变量可定义为=z2-f1(z1) ，通过观察并计算得知，当z3=0时，f1(z1) =0是系统的稳定方程。

综合上述递推出的各子系统控制量，可以得到如下控制律：

课题组采用的避障策略为人工势场法，其核心思想是：将机器人在物理上的运动转化为一种其在虚拟的人工受力场中的运动。目标点对机器人产生引力,障碍物对机器人产生斥力，将所有的引力和斥力的分力进行合并，最终合力作为机器人的控制力，从而实现机器人安全绕过障碍物到达目标位置。

课题组采用的编队控制策略为：在该编队控制算法基础上，结合人工势场法，实现多机器人在复杂环境下编队控制的任务。首先将三维空间下的编队运动学模型映射到二维平面进行分析；然后结合本编队控制器算法完成三维环境下的编队任务；当遇到外部障碍物的干扰时，智能切换为本文所提出的人工势场法避障策略；避障策略区域范围及障碍物影响范围可依据具体情况设定；当机器人脱离避障策略区域范围后，自动切换成编队控制模式，继续实现编队任务。

5 控制系统仿真

通过2个仿真实验来验证本文算法的有效性及可行性。仿真实验中，各轮式机器人控制器的参数为：k1=0.8，k2=1.2，控制周期T=50.0 ms。仿真机器人都装配有虚拟红外传感器与无线传感器，可以实时检测障碍物的位置并获得机器人的位置与角度等状态信息，R1为领航机器人，Rv为虚拟机器人，R2～R5为跟随机器人。

仿真实验1。图3中，5个机器人从不同的初始位置出发，在三维空间中，途中受到障碍物外部干扰的情况下，进行三角形编队,并且保持队形进行直线运动。每个机器人的初始位置、运动轨迹和编队参数如图3a所示，R2～R5机器人的编队参数分别是[0.5, -π/6]，[0.5, π/6]，[1.0, -π/6]和[1.0, π/6]。各跟随机器人的编队位置误差和角度误差如图3b和3c所示。R1领航机器人的前进速度和转向角速度为常量，进入编队稳定状态时，各机器人的前进速度和转向角速度都相同。从仿真图中可以看出，多机器人在三维空间中受到障碍物外部干扰的情况下，能快速回归编队状态，且保持极小误差，验证出跟随机器人的跟踪性能的有效性、稳定性和容错性。

仿真2。图4中，4个机器人从不同的初始位置出发，在三维空间中，途中受到障碍物外部干扰的情况下，机器人 R1做圆周运动，其余跟随机器人完成编队及变换队形的任务。首先进行菱形编队，然后切换为并排一字队形，最后切换为正方形队形。每个机器人的初始位置、运动轨迹和编队参数如图4a所示，R2, R3和R4机器人的编队参数分别为[0.5, -π/4]，[0.5, π/4]，[0.707, 0]。各跟随机器人的编队位置误差和角度误差如图4b和图4c所示。从图中可以看出，三维空间中，多机器人不仅在切换队形时，即使当前位姿相对期望位姿会有较大的误差，都能快速平滑地完成变换队形的编队控制。而且在存在外部干扰的情况下，依然能完成动态编队的任务，并保持较小的误差。由实验结果可知，该控制器可以实现复杂环境下的动态变换队形的编队控制。

6 结语

课题组在前期研究的基础上，完成控制器在三维环境中，存在障碍物外部干扰的情况下的多机器人编队避障控制的研究，并提出一种控制策略和控制算法，最后进行了2组仿真验证实验。仿真结果表明，提出的编队控制算法和避障策略，能顺利完成复杂环境下的动态编队及避障控制，容错性强，精度高，具有良好的可靠性。后继将对在动态障碍物环境下如何实现优化路径规划和通信失败对编队控制的影响等内容进行进一步研究。

[1]GANGULI A，CORTES J，BULLO F.Multirobot Rendezvous with Visibility Sensors in Nonconvex Environments[J].IEEE Trans on Robotics，2009，25(2)：340-352.

[2]杨 维，朱文球，张长隆.基于RGB-D相机的无人机快速自主避障[J].湖南工业大学学报，2015，29(6)：74-79.

YANG Wei，ZHU Wenqiu，ZHANG Changlong.UAV Autonomous Obstacle Avoidance Based on RGB-D Camera[J].Journal of Hunan University of Technology，2015，29(6)：74-79.

[3]王保防，张瑞雷，李 胜，等.基于轨迹跟踪车式移动机器人编队控制[J].控制与决策，2015，30(1)：176-180.

WANG Baofang，ZHANG Ruilei，LI Sheng，et al.Based on the Trajectory Tracking Vehicle Type Mobile Robot Formation Control[J].Control and Decision，2015，30(1)：176-180.

[4]WANG X，YU C，LIN Z.A Dual Quaternion Solution to Attitude and Position Control for Rigid-Body Coordination[J].IEEE Trans on Robotics，2012，28(5)：1162-1170.

[5]张瑞雷，李 胜，陈庆伟.车式移动机器人动态编队控制方法[J].机器人，2013，35(6)：651-656.

ZHANG Ruilei，LI Sheng，CHEN Qingwei.Dynamic Formation Control Method for Vehicle Type Mobile Robot[J].Robot，2013，35(6)：651-656.

[6]李 胜，马国梁，胡维礼.基于 Backstepping 方法的轮式移动机器人轨迹追踪控制[J].东南大学学报（自然科学版），2005，35(2)：248-252.

LI Sheng，MA Guoliang，HU Weili.Trajectory Tracking Control of Wheeled Mobile Robot Based on Backstepping Method[J].Journal of Southeast University（Natural Science Edition），2005，35(2)：248-252.

[7]TIAN Y，ZHANG Y.High-Order Consensus of Heterogeneous Multi-Agent Systems with Unknown Communication Delays[J].Automatica，2012，48(6)：1205-1212.

[8]杨 帆，刘士荣，董德国.编队控制中的机器人行为与基于服务的运动行为结构设计[J].机器人，2012，34(1)：120-128.

YANG Fan，LIU Shirong，DONG Deguo.Robot Formation Control Based on Motion Behavior and Structure Design Services[J].Robot，2012，34(1)：120-128.

[9]OLFATI S R，MURRAY R M.Distributed Structural Stabilization and Tracking for Formations of Dynamics Multi-Agents[C]// Proceedings of the 41st IEEE Conference on Decision and Control.Las Vegas：[s.n.]，2002：209-215.

[10]SHEN Kuihua，TAN Yuegang.A Solving Approach of Partial Minimum Problem on Artificial Potential Field Theory[J].Transaction of University of Science and Technology of Wuhan，2005，27(8)：77-79.

[11]DUDENHOEFFER D D，JONES M P.A Formation Behavior for Large-Scale Micro-Robot Force Deployment[C]// Proceedings of the 2000 Winter Simulation Conference.Orlando：[s.n.]，2000：972-982.

(责任编辑：申 剑)

Multi-Robot Formation Control Based on Backstepping Under the 3D Environment

FENG Lei，XIAO Shenping
（School of Electrical and Information Engineering，Hunan University of Technology，Zhuzhou Hunan 412007，China）

In view of the existing problems in the formation of dual-wheeled mobile robots under threedimensional environment, an obstacle avoidance multi-robot formation algorithm, based on methods of fictitious navigation and backstepping, and combined with the artificial potential field method, has thus been proposed.First, an analysis has been made of the formation model of the multi-robot system in three-dimensional environment, followed by an analysis of the spatial projection of the model onto a two-dimensional plane.Second, the kinematics model will be transformed into a chained form, where the error system will be transformed into a series nonlinear system through a canonical coordinate transformation.Third, Lyapunov function of the wheeled robot tracking system will be constructed by using backstepping method and a design of the trajectory tracking controller for wheeled robots will be obtained.The multi-robot formation in complicated environments, combined with the artificial potential field obstacle avoidance strategy, will be accomplished.Finally, the effectiveness of the proposed method will be verified by two groups of simulation experiments of multi-robot trajectory tracking.

three-dimensional space；method of artificial potential field；backstepping；Lyapunov function；formation control

TP273

A

1673-9833(2017)01-0069-06

10.3969/j.issn.1673-9833.2017.01.013

2016-12-18

国家自然科学基金资助项目（61672225），国家火炬计划基金资助项目（2015GH712901），湖南省教育厅科研基金资助重点项目（14A038）

冯 磊（1991-）男，河南焦作人，湖南工业大学硕士生，主要研究方向为智能检测与控制，E-mail：1341497188@qq.com

肖伸平（1965-）男，湖南东安人，湖南工业大学教授，主要从事时滞系统鲁棒控制，电力系统稳定性分析及网络化控制等方面的研究，E-mail：xsph_519@163.com