基于纯角度观测信息的leader-followers机器人编队控制方法



基于纯角度观测信息的leader-followers机器人编队控制方法

韩青，孙树栋，智睿瑞

(西北工业大学机电学院现代设计与集成制造技术教育部重点实验室，陕西西安710072)

摘要:提出一种纯角度观测信息的leader-followers多机器人编队控制方法．多个跟随机器人(followers)仅观测其领航机器人(leader)角度信息;基于非线性系统可观测性的理论研究，这种纯角度观测信息能够满足leader-followers的可观测性要求;利用无迹卡尔曼滤波算法对leader-followers机器人系统的状态进行估计，根据状态估计结果设计了输入-输出状态反馈控制规律控制跟随机器人运动，以达到理想的编队效果。仿真验证了该方法的可行性。

关键词:算法;角速度;控制;实验;反馈控制;卡尔曼滤波;数学模型; MATLAB;矩阵代数;非线性系统;机器人;轨迹;速度;编队控制; leader-followers编队控制及可观测性;移动机器人;无迹卡尔曼滤波;无迹卡尔曼滤波算法与输入-输出反馈控制规律

多移动机器人系统具有广泛的应用前景，是近年来机器人研究热点，一些研究成果已经在工业实际中得到广泛应用。已从传统的工业领域扩展到医疗服务、教育娱乐、勘探勘测、生物工程、柔性制造、救灾救援等新领域，并快速发展［1］。其中，多机器人编队控制是多机器人研究的一个难点，尤其是在观测信息非常有限的情况下，多机器人的编队控制具有很强的非线性系统特性。

目前，多机器人编队控制算法主要包括虚拟结构(virtual structure)法［2］、基于行为(behaviorbased)的方法［3］、领航-跟随(leader-follower)法［4-6］、人工势场法［7］等。文献［4］提出的编队控制方法能实现避碰和跟踪参考轨迹。文献［5］提出了适用于2个机器人距离-方位-方向控制方法和适用于3个机器人的距离-距离-方向控制方法，解决编队控制问题。文献［6］提出了一种不确定环境下多机器人的动态编队控制方法。文献［4-6］都是至少需要距离-角度信息或者更多信息。鉴于此，基于纯角度信息［8-12］的机器人编队研究成为了一个比较新的研究热点。文献［8-10］提出了3个或者4个机器人组成的编队分布控制规律，分布控制规律使编队全局稳定。文献［11］研究了机器人的队形控制，并提出了3个机器人编队控制规律。文献［12］基于输出扩展雅克比矩阵的秩，研究了非线性系统的可观测性。

本文提出了一种通过跟随机器人(followers)观测领航机器人(leader)方位信息进行机器人相对定位，并通过反馈用于跟随机器人的实时运动调节，以进行编队控制的方法，该方法对机器人数量和编队形状没有特殊要求，且能使编队保持稳定。

1　leader-followers编队控制及其可观测性

1. 1leader-follower机器人运动模型

设(x，y)是机器人在世界坐标系中的位置坐标，θ是机器人在世界坐标系中的方向角。ρ1是跟随机器人质心到leader机器人质心的距离。φ1是follower机器人y轴到leader机器人质心的视角，leader(R1)的坐标位置向量是［x1y1θ1］，follower(R2)的坐标位置向量是［x2y2θ2］，leader和follower的控制输入分别是线速度和角速度，即［ν1ω1］和［ν2ω2］。leader-follower的方向角差值是α1=θ1－θ2，leader-follower坐标关系如图1。

leader-follower机器人编队运动系统模型如(1)式所示。

输出向量y =[ y1y2]T=[φ1α1]T。

对(1)式机器人运动模型扩展，可以得到n个followers的情况。

状态向量:

输入向量:

输出向量:

图1　leader-follower坐标关系

1. 2李导数及leader-followers非线性系统可观测性

n个followers的情况，leader-followers机器人编队运动系统模型如(3)式。

根据文献［13］，以秩评价标准来确定非线性系统的局部弱可观测性。

李导数计算:

任何函数的零阶李导数是其本身:

hk(s)关于fνi一阶李导数定义为:

hk(s)关于fνi二阶李导数(s)定义为:

基于李导数的定义，由各阶李导数梯度构成的行向量观测矩阵定义为(8)式:

式中，表示梯度操作，·表示向量内积，i，k = 1，…，n，j = 2，…，n + 1; p∈N。

引理1对于非线性系统Sn，若由行向量构成的可观测矩阵M满秩(leader-followers系统，rank(M) = 3n，n是followers机器人数量)，则非线性系统是局部弱可观测的。

由(2)式，计算各阶李导数和观测矩阵M。

零阶李导数:

零阶李导数梯度:

一阶李导数:

一阶李导数梯度:

由零阶李导数梯度和一阶李导数梯度构成的行向量矩阵M。

定理1当leader和follower 2个机器人满足:

1)ν1＞0，νj＞0，j = 2，…，n + 1。

2)φj≠0，j = 1，…，n，即测量方位的follower机器人不沿着follower和leader 2个机器人连线做直线运动。

3) leader-follower机器人不能做平行直线运动。

(23)式中矩阵M的秩是3。

证明:在满足3个前提条件的情况下，对(23)式中M经过一系列的初等行变换，

由变换后的结果可以看出，变换后的矩阵有3个线性独立行，因此rank(M) = 3。

由文献［12］命题3知，各阶李导数梯度等于相同阶数h(s)对时间导数的梯度。

定理2当leader和follower 2个机器人满足:

1)ν1＞0，νj＞0，j = 2，…，n + 1。

2)φj= k1，αj= k2，j = 1，…，n，k1，k2是常数，即leader-follower机器人同时做平行直线运动。

(23)式中矩阵M的秩是2。

证明当φj= k1，αj= k2，k1，k2是常数，φj和αj对时间的导数为零，上标(0，…，n)是函数h1(s)对时间的导数阶数。

(23)式中的M由零阶李导数梯度至一阶李导数梯度构成，当然M也可以由更高阶李导数的梯度构成，但是推导发现，二阶李导数梯度及更高阶李导数梯度跟零阶李导数梯度及一阶李导数梯度成线性比例关系，所以，由零阶李导数梯度至更高阶李导数梯度构成M的秩不会改变，即定理1中rank(M)仍是3，定理2中rank(M)还是2。

当follower和leader机器人沿着曲线轨迹运动时，根据定理1，rank(M) = 3，即M满秩，由引理1知，系统Sn是局部弱可观测的。当follower和leader机器人沿着直线轨迹运动，根据定理2，rank(M) = 2，即M不满秩，由引理1知，系统Sn不具有局部弱可观测性。

系统是局部弱可观测的，系统输出能够传递足够丰富的信息，观测器对系统状态产生比较准确的估计，改善编队控制;系统不具有局部弱可观测性，系统输出不能够传递足够丰富的信息，观测器对系统状态产生不准确估计，影响编队控制。

2　UKF滤波算法和输入-输出反馈控制规律

为了实现反馈运动控制，状态估计是必要的，本文选择UKF滤波算法。通过UKF滤波算法和输入-输出状态反馈控制规律的有机结合，获取多机器人方位信息进行运动控制，以达到理想的编队效果。

2. 1UKF滤波算法

系统输入向量为U，输出向量为y，UKF滤波算法用来估计状态S的角度信息，即y = h(s) =[φjαj]T，j = 1，…，n，通过UKF滤波算法，获得了比较准确的角度值。(27)式和(28)式分别是带有噪声的状态方程和观测方程。

D是输出转换矩阵，O和N是零均值、协方差分别是PL和PN白高斯噪声，且s(0)、O和N假定为不相关。对(27)式采用前向欧拉方法进行离散化处理，采样时间为Tc，得到式(29)。

式中Γ(s(k)，u(k) ) = TcF(s) U + s(k)，k∈N。

UKF基于UT变换过程，该算法包括初始化、时间更新和测量更新几个阶段，详细过程见文献［14］。

2. 2输入-输出状态反馈控制

以R1和R2机器人为对象设计控制规律，设计的控制规律同样适用于其他跟随机器人。

对(1)式的状态方程进行变形，得到与其等价的(30)式，并对α1=θ1－θ2两边求导，得到(31)式。

式中: sr［ρ1φ1］T，M2×2和N2×2分别是F的右上角和左上角的子矩阵。

借鉴文献［15］I/O标准线性化技术和文献［16］的方法，提出了应用于机器人编队控制的输入-输出状态反馈控制规律。

C是辅助控制变量，控制增益k1，k2＞0。上标“ide”指理想值。(32)式代入(30)式并结合(31)式，得到简化的闭环运动学方程(34)式。

该输入-输出反馈控制规律可以稳定编队。

3　实验验证

本文通过3个pioneer-3at机器人(1个leader、2 个followers)组成leader-followers编队来验证上述控制方法的有效性。pioneer-3at机器人具有激光雷达、全景相机、声呐、红外等传感器，很容易获取角度信息。为了使仿真更为真实，利用功能强大的Webots 7搭建3D仿真平台，能够精确模拟真实环境，也可以将仿真数据导入MATLAB中进行研究分析。实验仿真场景如图2所示，follower机器人是R2、R3，leader机器人是R1。

leader-followers机器人编队经过分段直线-曲线组合轨迹1、正弦形轨迹2及螺旋形轨迹3。仿真实验效果见图3～图5。

仿真中，采用基于纯角度信息的编队控制方法，followers根据leader的运动情况进行实时自我调整;采用UKF滤波算法对leader-followers机器人系统状态进行估计;采用输入-输出状态反馈闭环控制规律来稳定编队。

图2　移动机器人仿真场景

3. 1初始条件

机器人编队采用直线-曲线组合轨迹1时，leader线速度和角速度:

机器人编队采用正弦轨迹2时，leader线速度和角速度:

机器人编队采用螺旋轨迹3时，leader线速度和角速度: ω1(t)是时间的复杂函数。

leader和follower初始位置及方向角:

UKF和反馈控制相关参数:

s(0) =

3. 2仿真分析

对于轨迹1:

从图3a)可见，R1、R2、R3机器人沿直线-曲线组合轨迹运动时保持了比较理想的编队。

从图3b)可见，机器人编队沿曲线轨迹1运动时，观测角估计误差几乎是零，即使在运动轨迹发生变化时，最大误差是0. 000 518 6。

从图3c)可见，机器人编队在曲线轨迹1运动时，方向角估计误差也很小，即使在运动轨迹发生变化时，最大误差是－0. 003 94。

从图3d)可见，机器人沿轨迹1运动时，一直沿着直线或曲线运动时，速度变化不大，但当运动轨迹发生变化时，速度的方向和大小变化比较明显;沿着直线轨迹运行时，跟随机器人的速度接近领航机器人的初始速度。

图3　仿真结果

对于轨迹2:

从图4a)可见，R1、R2、R3机器人沿轨迹2运动时保持了比较理想的编队。

从图4b)可见，机器人编队沿曲线轨迹2运动时，观测角估计误差几乎是零，即使在运动轨迹发生变化时，最大误差是－0. 000 6。

从图4c)可见，机器人编队沿曲线轨迹2运动时，方向角估计误差最大是－0. 002 862。

从图4d)可见，机器人沿轨迹2运动时，R2的速度变化范围是0. 904 6～1. 402; R3的速度变化范围是0. 848 2～1. 392。

对于轨迹3:

从图5a)可见，R1、R2、R3机器人在轨迹3运动时保持了比较理想的编队。

从图5b)可见，机器人编队沿轨迹3运动时，观测角估计误差几乎是零。

从图5c)可见，机器人编队沿轨迹3运动时，方向角估计误差收敛速度比较快，在t= 1 s时刻几乎收敛到零。

从图5d)可见，机器人沿轨迹3运动时，跟随机器人速度呈现增大趋势。

图4　仿真结果

图5　仿真结果

4　结论

本文提出了一种跟随机器人(followers)观测领航机器人(leader)纯角度信息的leader-followers编队控制方法。基于非线性系统的可观测性理论，分析了机器人编队的可观测性;应用UKF算法对leader-followers系统的状态进行估计;设计了输入-输出状态反馈控制规律稳定编队。仿真结果表明，多个机器人能快速形成编队，并以较小的误差做复杂轨迹运动，保持了比较理想的队形。后续研究将包括实现多机器人队形变换及避障方法。

参考文献:

［1］谭民，王硕．机器人技术研究进展［J］．自动化学报，2013，39(7) : 963-972 Tan M，Wang S．Research progress on robotics［J］．Acta Automatica Sinica，2013，39(7) : 963-972 (in Chinese)

［2］Ren W，Beard R W．Formation Feedback Control for Multiple Spacecraft Via virtual Structures［J］．IEEE Proceedings Control Theory Application，2004，151(3) : 357-368

［3］Long M，Gage A，Murphy R，et al．Application of The Distributed Field Robot Architecture to a Simulated Demining Task［C］∥Proceedings IEEE International Conference on Robotics and Automation，Barcelona，2005: 3193-3200

［4］Mastellone S，Stipanovic D M，Graunke C R，et al．Formation Control and Collision Avoidance for Multi-Agent-Nonholonomic Systems: Theory and Experiments［J］．The International Journal of Robotics Research，2008，27(1) : 107-126

［5］Chen J，Sun D，Yang J，et al．Leader-Follower Formation Control of Multiple Non-Holonomic Mobile Robots Incorporating a Receding-Horizon Scheme［J］．The International Journal of Robotics Research，2010，29(6) : 727-747

［6］杨丽，曹志强，谭民．不确定环境下多机器人的动态编队控制［J］．机器人，2010，32(2) : 283-288 Yang L，Cao Z Q，Tan M．Dynamic Formation Control for Multiple Robots in Uncertain Environments［J］．Robot，2010，32(2) : 283-288 (in Chinese)

［7］Kwon J W，Chwa D．Hierarchical Formation Control Based on a Vector Field Method for Wheeled Mobile Robots［J］．IEEE Trans on Robotics，2012，28(6) : 1335-1345

［8］Basiri M，Bishop A N，Jensfelt P．Distributed Control of Triangular Formations with Angle Only Constraints［J］．Systems＆Control Letters，2010，59(2) : 147-154

［9］Bishop A N．A Very Relaxed Control Law for Bearing-Only Triangular Formation Control［C］∥Proceedings of the 18th IFAC World Congress，Milano，Italy，2011: 5991-5998

［10］Bishop A N．Distributed Bearing-Only Quadrilateral Formation Control［C］∥Proceedings of the 18th IFAC WorldCongress，Milano，Italy，2011: 4507-4512

［11］Eren T．Formation Shape Control Based on Bearing Rigidity［J］．International Journal of Control，2012，85(9) : 1361-1379

［12］Mariottini G L，Morbidi F，Prattichizzo D，et al．Vision-Based Localization for Leader-Follower Formation Control［J］．IEEE Trans on Robotics，2009，25(6) : 1431-1438

［13］Hermann R，Krener A J．Nonlinear Controllability and Observability［J］．IEEE Transactions on Automatic Control，1977，22 (5) : 728-740

［14］Wan E A，Van Der Merwe R．The Unscented Kalmanfilter for Nonlinear Estimation［C］∥Adaptive Systems for Signal Processing，Communications，and Control Symposium，2000: 153-158

［15］Slotine J J E，Li W．Applied Nonlinear Control［M］．Englewood Cliffs，NJ: Prentice-Hall，1991

［16］Das A K，Fierro R，Kumar V，et al．A Vision-Based Formation Control Framework［J］．IEEE Trans on Robotics and Automation，2002，18(5) : 813-825

Bearing-only Leader-Followers Multi-Robot Formation Control

Han Qing，Sun Shudong，Zhi Ruirui

(Contemporary Design and Integrated Manufacturing Technology Key Laboratory of Ministry of Education， School of Mechanical Engineering，Northwestern Polytechnical University，Xi'an 710072，China)

Abstract:A bearing-only formation control method for leader-followers multi-robots is proposed．In the study，follower-robots can only observe the bearing information of the leader-robot．Based on the observability of nonlinear system，studies show that the bearing-only observation meets the leader-followers observability condition for general nonlinear system．The unscented Kalman filter(UKF) is employed to estimate the state of leader-followers’robots．The results are used for followers’movement control via the input-output feedback control law，so that the desired formation of the robots are maintained．Simulation results are presented to demonstrate the feasibility of the approach．

Key words:algorithms，angular velocity，control，experiments，feedback control，Kalman filters，mathematical models，MATLAB，matrix algebra，nonlinear systems，robots，trajectories，velocity; formation control，leader-followers formation control and observability，mobile robot，UKF(Unscented Kalman filter)，UKF filter algorithm and input-output feedback control law

作者简介:韩青(1973—)，西北工业大学讲师、博士研究生，主要从事智能机器人、多机器人协调控制与机电一体化与机电控制技术的研究。

收稿日期:2014-10-08基金项目:国家自然科学基金(51475383、51075337)资助

文章编号:1000-2758(2015) 02-0244-07

文献标志码:A

中图分类号:TP24