多移动机器人系统具有广泛的应用前景,是近年来机器人研究热点,一些研究成果已经在工业实际中得到广泛应用。已从传统的工业领域扩展到医疗服务、教育娱乐、勘探勘测、生物工程、柔性制造、救灾救援等新领域,并快速发展[1]。其中,多机器人编队控制是多机器人研究的一个难点,尤其是在观测信息非常有限的情况下,多机器人的编队控制具有很强的非线性系统特性。
目前,多机器人编队控制算法主要包括虚拟结构(virtual structure)法 [2]、基于行为(behavior-based)的方法[3]、领航-跟随(leader-follower)法[4,5,6] 、人工势场法[7]等。文献[4]提出的编队控制方法能实现避碰和跟踪参考轨迹。文献[5]提出了适用于2个机器人距离-方位-方向控制方法和适用于3个机器人的距离-距离-方向控制方法,解决编队控制问题。文献[6] 提出了一种不确定环境下多机器人的动态编队控制方法。文献[4,5,6]都是至少需要距离-角度信息或者更多信息。鉴于此,基于纯角度信息[8, 9, 10, 11, 12]的机器人编队研究成为了一个比较新的研究热点。文献[8, 9, 10]提出了3个或者4个机器人组成的编队分布控制规律,分布控制规律使编队全局稳定。文献[11]研究了机器人的队形控制,并提出了3个机器人编队控制规律。文献[12]基于输出扩展雅克比矩阵的秩,研究了非线性系统的可观测性。
本文提出了一种通过跟随机器人(followers)观测领航机器人(leader)方位信息进行机器人相对定位,并通过反馈用于跟随机器人的实时运动调节,以进行编队控制的方法,该方法对机器人数量和编队形状没有特殊要求,且能使编队保持稳定。
1 leader-followers编队控制及其可观测性 1.1 leader-follower机器人运动模型设(x,y)是机器人在世界坐标系中的位置坐标,θ是机器人在世界坐标系中的方向角。ρ1是跟随机器人质心到leader机器人质心的距离。 Φ1是follower机器人y轴到leader机器人质心的视角,leader(R1)的坐标位置向量是[x1 y1 θ1],follower(R2)的坐标位置向量是[x2 y2 θ2],leader和follower的控制输入分别是线速度和角速度,即[ν1 ω1]和[ν2 ω2]。leader-follower的方向角差值是α1=θ1-θ2 ,leader-follower坐标关系如图 1。
 |
| 图 1 leader-follower坐标关系 |
leader-follower机器人编队运动系统模型如(1)式所示。
式中：状态向量
输入向量U
[ν2 ω2 ν1 ω1]T
输出向量y=[y1 y2]T= [Φ1 α1]T。
对(1)式机器人运动模型扩展,可以得到n个followers的情况。
状态向量:
输入向量:
输出向量:
对(1)式状态方程
进行分解,得到独立函数表示形式,(1)式机器人编队运动模型也相应的变换成(2)式。
n个followers的情况,leader-followers机器人编队运动系统模型如(3)式。
根据文献[13],以秩评价标准来确定非线性系统的局部弱可观测性。
李导数计算:
任何函数的零阶李导数是其本身:
hk(s)关于fνi 一阶李导数定义为:
hk(s)关于fνi 二阶李导数Lfνifνi2 hk(s)定义为:
二阶混合李导数Lfνifνi2 hk(s)关于fνi 和fνj 定义为:
基于李导数的定义,由各阶李导数梯度构成的行向量观测矩阵定义为(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经过一系列的初等行变换,
由文献[12]命题3知,各阶李导数梯度等于相同阶数h(s)对时间导数的梯度。
定理2 当leader和follower2个机器人满足:
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)式中的
(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.1 UKF滤波算法系统输入向量为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。
 |
| 图 2 移动机器人仿真场景 |
leader-followers机器人编队经过分段直线-曲线组合轨迹1、正弦形轨迹2及螺旋形轨迹3。仿真实验效果见图 3~图 5。
 |
| 图 3 仿真结果 |
 |
| 图 4 仿真结果 |
 |
| 图 5 仿真结果 |
仿真中,采用基于纯角度信息的编队控制方法,followers根据leader的运动情况进行实时自我调整;采用UKF滤波算法对leader-followers机器人系统状态进行估计;采用输入-输出状态反馈闭环控制规律来稳定编队。
3.1 初始条件机器人编队采用直线-曲线组合轨迹1时,leader线速度和角速度:
机器人编队采用正弦轨迹2时,leader线速度和角速度:
机器人编队采用螺旋轨迹3时,leader线速度和角速度:
leader和follower初始位置及方向角:
UKF和反馈控制相关参数:
对于轨迹1:
从图 3a)可见,R1 、R2、R3 机器人沿直线-曲线组合轨迹运动时保持了比较理想的编队。
从图 3b)可见,机器人编队沿曲线轨迹1运动时,观测角估计误差几乎是零,即使在运动轨迹发生变化时,最大误差是0.000 518 6。
从图 3c)可见,机器人编队在曲线轨迹1运动时,方向角估计误差也很小,即使在运动轨迹发生变化时,最大误差是-0.003 94。
从图 3d)可见,机器人沿轨迹1运动时,一直沿着直线或曲线运动时,速度变化不大,但当运动轨迹发生变化时,速度的方向和大小变化比较明显;沿着直线轨迹运行时,跟随机器人的速度接近领航机器人的初始速度。
对于轨迹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 结 论本文提出了一种跟随机器人(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) |
| Cited By in Cnki (70) | Click to display the text | |
| [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 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [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) |
| Cited By in Cnki (18) | Click to display the text | |
| [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 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [10] | Bishop A N. Distributed Bearing-Only Quadrilateral Formation Control[C]//Proceedings of the 18th IFAC WorldCongress, Milano, Italy, 2011: 4507-4512 |
| Click to display the text | |
| [11] | Eren T. Formation Shape Control Based on Bearing Rigidity [J]. International Journal of Control, 2012,85(9): 1361-1379 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [13] | Hermann R, Krener A J. Nonlinear Controllability and Observability[J]. IEEE Transactions on Automatic Control, 1977, 22(5): 728-740 |
| Click to display the text | |
| [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 |
| Click to display the text | |
| [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 |
| Click to display the text |