李咏华,张 立,刘嘉睿,黄 峥,胡梓凯,廖世旺
(1.广州大学 机械与电气工程学院,广州 510006;2.广州市城市建设职业学校,广州 510006)
随着智能制造业的快速发展,多机器人的应用需求逐渐提升。相比于单个移动小车,多移动小车系统具有很多显著的优点[1],较大的冗余性提高了容错性和鲁棒性,可变的编队控制保证了灵活性。目前,多移动小车系统在仓储物流[2]、工业制造[3]和应急救灾[4]等领域得到了广泛的应用。
在进行多智能体编队控制时所应用的方法主要包括领航跟随法[5-9]、行为法[10]、人工势场法[11-12]和虚拟结构法[13]4种。基于行为的多智能体编队控制方法具有良好的鲁棒性和灵活性,但是难以用数学分析描述系统而无法实现精确的编队。基于人工势场的多智能体编队控制方法原理简单,适用于可自由运动的智能体编队控制中,但对于非完整约束的情况难度较大。基于虚拟结构的多智能体编队控制方法需要编队队形保持同一个刚性结构,因此灵活性和适应性较差。基于领航跟随的多智能体编队控制方法原理简单,只需得到领航者期望轨迹和跟随者相对位置即可,应用最广泛。
在领航-跟随型的编队控制研究中,张方方等[14]使用基于位置信息的速度补偿算法对经典的领航-跟随法进行改进,并与人工势场法相结合实现三轮差分实体机器人的编队避障问题。王乐乐等[15]通过建立搜索树扩展方向与队形之间的联系,调整编队方向以实现多智能体编队路径规划。伍锡如等[16]通过设计状态观测器,实现了多智能体系统的环形编队控制和目标包围。易国等[17]考虑了领航小车和跟随小车之间的信息相互是局部的情况,为每个跟随小车设计分布式估计算法估计领航小车的状态,再根据跟踪误差实现编队控制,但并未考虑时延等影响,同时会存在网络负载波动等影响。吴垠等[18]设计了不依赖移动小车模型的模糊控制方法,实现了多移动小车的编队控制,但控制律过度依赖领航者模型,外部干扰对系统稳定性的影响较大。
现有的编队控制研究绝大部分针对编队队形不变的情况,但在现实情况中跟随小车与领航小车的相对位置需要根据具体的任务而改变。针对多移动小车的编队控制和队形变换控制问题,采用领航-跟随法。首先,通过坐标变换建立了轮式移动小车的运动学模型、编队控制的运动学模型,每个跟随小车的期望位姿由领航小车当前实际位姿和相对位置决定,基于移动小车的跟踪误差设计了一种趋近律滑模编队控制器,最后通过Matlab/Simulink数值仿真验证该控制器在多移动小车编队控制中的有效性。
在图论中,n个智能小车之间的通信拓扑结构可以用有向图G=(V,E)表示,图中节点0代表领航者。在拓扑结构中,将每个移动小车抽象成节点,则所有节点的集合记为V={v1,v2,…,vn}。图1中的箭头表示智能小车间的通讯方式,其中圆点表示发送端,箭头表示接收端,用e=(i,j)表示有向边,表示智能小车i可以将信息传递给智能小车j,反之则不行。用E⊂N×N表示拓扑结构中所有边的集合。
图1 节点间通讯方式示意图
图G的邻接矩阵为A=[aij]N×N,其中
在本文中,定义每个移动小车只能访问自身的状态信息和领航小车的状态信息,使用有向图G=(V,E)描述领航小车和n-1个跟随小车之间的通讯拓扑结构。通讯拓扑结构如图2所示,其中{1}表示领航小车,{2,3,…,n}表示跟随小车。邻接矩阵A=[aij]N×N表示领航小车和跟随小车间通讯关系的权重系数。
图2 移动小车间通讯拓扑结构示意图
在由n个轮式移动小车组成的领航-跟随型编队系统中,用i={1,2,…,n}表示第i个移动小车,其中第一个为领航小车。假设移动小车的速度较低,可以忽略车轮的横向滑动和行驶过程中所受的横向力。建立以地面为参考的全局坐标系XOY和以移动小车为参考的小车坐标系xoy,其中θi为小车移动方向和水平方向的夹角。移动小车的运动学模型如图3所示。
图3 移动小车的运动学模型示意图
移动小车位姿模型如图4所示,实际位姿坐标为(xi,yi,θi),向前移动时线速度和角速度分别为vi、ωi,小车运动学模型为
图4 移动小车位姿模型
(1)
在XOY坐标下给定的领航小车期望轨迹位姿坐标为(xr,yr,θr),期望轨迹的线速度和角速度分别为vr和ωr。利用平面坐标变换,领航小车的位姿误差方程为
(2)
对式(2)进行微分,结合式(1)整理可以得到领航小车位姿误差微分方程
(3)
为了更好地描述移动小车的编队控制,建立了如图5所示的编队控制运动学模型,其中R1为领航小车,Ri(i={2,3,…,n})为跟随小车,Rvi为Ri在期望队形中的映射小车,线速度和角速度分别为vvi和ωvi。在运动过程中,R1和Rvi之间的相对位置用Δi=[Δix,Δiy]∈R2描述。利用平面坐标变换,得到跟随小车的期望位姿
图5 移动小车编队控制运动学模型示意图
(4)
式中:(x1,y1,θ1)T为领航小车的位姿,(xvi,yvi,θvi)T为跟随小车Ri(i={2,3,…,n})的期望位姿,则跟随小车的位姿误差方程为
(5)
对式(5)进行微分,得
将式(1)代入,可以得到跟随小车位姿误差微分方程
(6)
由n个轮式移动小车组成的领航-跟随型编队系统中,编队控制的目的在于调整各个小车的位置使系统以特定的几何图形移动,即跟随小车和领航小车的线速度和角速度保持一致(即vvi=v1,ωvi=ω1),并且相对位置和方向满足期望的拓扑和物理约束:
(7)
(8)
(9)
为了实现多移动小车的编队控制和队形变换控制,提出了基于有限时间趋近律的编队控制策略,其结构如图6所示。对于领航小车,首先根据式(1)建立小车的运动学模型,将当前实际位姿与期望位姿根据式(2)分析得到位姿误差并输入到编队控制器中,计算得到控制小车跟踪期望轨迹的线速度和角速度。对于跟随小车,同样需要先建立小车的运动学模型,再根据领航小车当前实际位姿和相对位置Δi=[Δix,Δiy]∈R2计算得到跟随小车的期望位姿,最后根据式(5)分析得到位姿误差。
图6 移动小车编队控制结构框图
滑模变结构控制具有响应速度快、物理实现简单等特点[19-20],能更好地应用于移动小车的非线性控制。
引理[19]:对于任何x∈R且|x|→∞,有φ(x)=xsin(arctanx)≥0,当且仅当x=0时“=”成立。
引入反步法,并根据上述引理设计系统滑模切换函数。
(10)
(11)
根据以上分析设计滑模切换函数
(12)
为使系统由于切换时间、空间滞后等因素导致的无法消除的抖振得以削弱,而不影响滑模面趋近速度,采用有限时间趋近律[21]:
(13)
其中
式中:η>0,0<δ<1,ε>0,fal(s,η,δ)为非连续线性函数,δ为fal(s,η,δ)在原点附近正负对称区间的长度。当|s|<1和|s|≥1时取不同的参数k,可使系统在初始阶段趋近速度增大,而在趋近滑模面时降低速度并削弱系统的抖振。
(14)
整理上式可以得到控制律
(15)
为了验证本文所提出的领航跟随型编队控制方法的有效性,分别对由5个移动小车组成的编队进行编队队形控制和队形变换数值仿真实验,期望编队队形共有3种,如图7所示。式(13)中有限时间趋近律滑模控制参数设置为k1=k2=3,δ1=δ2=0.2,η1=η2=0.5,ε1=ε2=0.01。
图7 期望编队队形示意图
图7所示期望编队队形中,R1和Rvi之间的相对位置矩阵为
首先采用由4个跟随小车和1个领航小车组成的多移动小车系统在Matlab/Simulink下进行编队控制数值仿真。
移动小车的初始条件如表1所示。
表1 移动小车的初始条件
领航-跟随型编队控制任务分为3个阶段:Ⅰ)0~9.4 s为第一个阶段,期望编队队形为图7的(1),领航者的期望轨迹为直线,期望线速度为vr=4.8 m/s,期望角速度为ωr=0 rad/s;Ⅱ)9.4~25.1 s为第二个阶段,期望编队队形不变,领航者的期望轨迹为半径为24 m的圆弧,期望线速度为vr=4.8 m/s,期望角速度为ωr=0.2 rad/s;Ⅲ)25.1~34.6 s为第三个阶段,期望编队队形不变,领航者的期望轨迹为直线,期望线速度为vr=4.8 m/s,期望角速度为ωr=0 rad/s。仿真结果如图8—11所示。
图8表示5个移动小车系统在各个运动时刻的队形,每个小车用不同颜色的圆圈表示,圆圈中的箭头表示各移动小车的运动方向。多移动小车系统在0、9.4和25.1 s的运动轨迹开始变化,但在移动过程中保持编队队形不变,5个小车均可收敛至期望队形并沿着期望轨迹运动。
图8 移动小车编队运动轨迹1
图9—11分别表示跟随小车与领航小车航向角、X轴方向和Y轴方向误差,由于开始时小车处于静止状态,并且每个小车位置和航向角与期望值均存在一定误差,因此收敛至期望队形用时较长,为2.61 s。
图9 跟随小车航向角误差曲线1
图10 跟随小车与领航小车X轴方向距离误差曲线1
图11 跟随小车与领航小车Y轴方向距离误差曲线1
图12 干扰下移动小车编队运动轨迹1
图13 干扰下跟随小车航向角误差曲线1
图14 干扰下跟随小车与领航小车X轴方向距离误差曲线1
图15 干扰下跟随小车与领航小车Y轴方向距离误差曲线1
由图12—15可知,12.5~13.5 s加入扰动后各跟随小车的位置和方向都受到了影响,航向角误差最大值为1.064 rad,与领航小车X轴和Y轴方向距离误差为1.496 0 m和0.604 3 m。扰动结束后,跟随小车用时0.84 s便恢复了期望队形,可见该多移动小车编队控制方法在编队控制时具有较好的抗干扰性能。
同样采用由4个跟随小车和1个领航小车组成的多移动小车系统在Matlab/Simulink下进行编队控制和队形变换控制数值仿真。
移动小车的初始条件如表2所示。
表2 移动小车初始条件
领航-跟随型编队任务分为3个阶段:
Ⅰ)0~15.7 s为第一阶段,期望编队队形为图7的(1),领航者的期望轨迹为半径24 m的圆弧:
期望线速度为vr=4.8 m/s,角速度为ωr=0.2 rad/s;
Ⅱ)15.7~30 s为第二个阶段,期望编队队形为图7的(2),领航者的期望轨迹为半径32 m的圆弧:
期望线速度和角速度分别为vr=4.8 m/s和ωr=0.15 rad/s;
Ⅲ)30~50 s为第三个阶段,期望编队队形为图7的(3),领航者的期望轨迹不变。
仿真结果如图16—19所示。
图16为5个移动小车系统在各个运动时刻的队形,每个小车用不同颜色的圆圈表示,圆圈中的箭头表示各移动小车的运动方向。在0、15.7和30 s开始队形变换,5个小车在3个阶段均可收敛到期望队形并沿着期望轨迹运动。
图16 移动小车编队运动轨迹2
图17—19表示跟随小车与领航小车航向角、X轴方向和Y轴方向误差,由于开始时小车处于静止状态,并且每个小车位置和航向角与期望值偏差较大,第一阶段收敛至期望队形ΔⅠ用时较长,为2.00 s。第二阶段编队队形调整为ΔⅡ,编队队形仍为正方形,只是跟随小车和领航小车在X轴和Y轴方向的距离增大,因此调整用时较短,为1.79 s。第三阶段编队队形调整为ΔⅢ,编队队形由正方形变换为菱形,由于队形变化较大,在队形调整过程中跟随小车1和4与领航小车航向角误差偏大,调整时间变长,为1.82 s。
图17 跟随小车航向角误差曲线2
图18 跟随小车与领航小车X轴方向距离误差曲线2
图19 跟随小车与领航小车Y轴方向距离误差曲线2
由图20—23可知,20~21 s加入扰动后各跟随小车的位置和方向同样都受到了影响,航向角误差最大值为0.325 9 rad,与领航小车X轴和Y轴方向距离误差分别为1.452 5 m和0.582 9 m。扰动结束后,跟随小车用时1.59 s便恢复了期望队形,可见该多移动小车编队控制方法在队形变换控制时同样具有较好的抗干扰性能。
图20 干扰下移动小车编队运动轨迹2
由图8—23的实验结果可知,本文设计的编队控制策略能够有效应用于多移动小车的编队控制和队形变化控制中。
图21 干扰下跟随小车航向角误差曲线2
图22 干扰下跟随小车与领航小车X轴方向距离误差曲线2
图23 干扰下跟随小车与领航小车Y轴方向距离误差曲线2
所提出的控制策略能快速实现队形变化,但变化过程角速度较大,导致航向角误差偏大,后续可通过改进算法,增加时间、能耗等指标,提高适应性。