一种基于领导跟随结构的无人水面艇编队控制方法与流程

本发明涉及无人水面艇的编队控制领域，具体涉及一种基于领导跟随结构的无人水面艇编队控制方法。

背景技术：

无人水面艇是一种具有在实际海洋环境下自主航行的能力，并能够自主完成环境感知、目标探测等任务的水面航行器。无人水面艇具有广阔的应用前景，可用于海洋资源的研究、勘探、开采与运输，恶劣海况(如海上大风、巨浪、热带风暴等)的探测与预警，海洋地质环境的勘探与监控，海洋水文的观测，以及海洋气象研究等领域。

就目前的无人水面艇技术水平而言，单个无人水面艇在信息的获取、处理及控制能力等方面是有限的。面对复杂的工作任务以及多变的海洋工作环境，单个无人水面艇的执行能力可能显得不足。利用多个无人水面艇组成的群体系统，将复杂的工作任务进行分解成各个简单的子任务，多个无人水面艇并行地执行各自的子任务，并通过各个无人水面艇的相互通信、协调、协作来提高整个工作的效率，可完成单个无人水面艇无法或难以完成的工作。受到自然界生物群体队形的启发，无人艇编队控制是一个典型的协调运动控制问题。多个无人水面艇编队控制指通过设计合适的控制策略，使得多个无人水面艇组成的编队系统的各个无人艇之间能够保持一个期望的相对位置和姿态，并维持队形的协同运动，完成特定的任务。无人水面艇编队系统通常比单个无人水面艇系统具有优越性，可以通过群体的编队协同合作来弥补单个无人艇能力的不足，扩大完成任务的能力范围，完成单个无人水面艇难以完成的复杂任务。

本发明针对多个全驱动无人水面艇的编队控制问题，提出基于领导者--跟随者(leader-follower)的编队控制方法解决任一跟随者与其领导者防止碰撞和保持连接的问题。根据领导者与跟随者的安全距离和通讯连接范围，设计跟随者的位置输出跟踪误差约束条件及跟踪转换函数，并构造性地设计编队控制器，确保任一无人水面艇的位置跟踪误差在任何时刻均不违反位置输出跟踪误差约束条件，即保证了任一跟随者与其领导者始终保持一定的安全距离并处于其领导者的通讯连接范围内，从而解决了编队控制中任一跟随者与其领导者防止碰撞和保持连接的问题。

技术实现要素：

本发明的目的是针对上述现有技术的不足，提供了一种基于领导跟随结构的无人水面艇编队控制方法，该方法针对无人水面艇编队控制中的防止碰撞和保持连接问题，将这两个问题转化为位置跟踪误差的约束问题，提出基于领导者-跟随者结构的编队控制方法，设计编队控制器使得任一无人水面艇的位置跟踪误差在任何时刻均不违反该约束条件，解决了防止碰撞和保持连接的问题。

本发明的目的可以通过如下技术方案实现：

一种基于领导跟随结构的无人水面艇编队控制方法，所述方法包括以下步骤：

步骤(1)、建立多个无人水面艇的动态模型；

步骤(2)、依据领导者-跟随者的编队控制方式，根据领导者与跟随者的安全距离和通讯连接范围设计跟随者的位置来输出跟踪误差约束条件；

步骤(3)、设计跟踪误差转换函数，将跟踪误差不等式约束转换成等式约束；

步骤(4)、应用动态面控制技术设计虚拟控制器：结合动态面控制技术与逐步后推控制器设计技术避免虚拟控制器的求导，从而避免控制器的输入包含不可测的加速度信息；

步骤(5)、设计扰动观测器补偿外界未知干扰：应用扰动观测器的方法，设计扰动观测器对外界未知干扰进行估计，并设计相应的前馈扰动补偿控制器；

步骤(6)、应用李雅普诺夫稳定性理论设计编队跟踪控制器：设计编队跟踪控制器，应用李雅普诺夫稳定性理论严格证明闭环系统的稳定性，并确保编队控制中任一跟随者与其领导者始终保持一定的安全距离并处于其领导者的通讯连接范围内。

进一步地，步骤(1)中，所述多个无人水面艇的动态模型为：

其中，ui表示第i个无人水面艇的纵向速度，vi表示第i个无人水面艇的横荡速度，ri表示第i个无人水面艇的转向角速度，ψi(t)为第i个无人水面艇的航向角，i＝1、2、3……n，表示ψi(t)的导数，表示第i个无人水面艇在x轴方向的速度，表示第i个无人水面艇在y轴方向的速度，mi表示无人水面艇的质量矩阵，表示第i个无人水面艇在u，v，r方向上的加速度构成的向量，c(vi)表示科氏力矩阵，vi＝[ui,vi,ri]^t，d(vi)表示阻尼矩阵，τi＝[τui,τvi,τri]^t，τui表示第i个无人水面艇纵向的推力，τvi表示第i个无人水面艇横荡方向的推力，τri表示第i个无人水面艇转向的力矩，τwi＝[τwui,τwvi,τwri]^t，τwui表示第i个无人水面艇在纵向方向受到的外部时变扰动，τwvi表示第i个无人水面艇在横荡方向受到的外部时变扰动，τwri表示第i个无人水面艇在转向角方向受到的外部时变扰动。

进一步地，所述步骤(2)的具体过程为：将多个无人水面艇依次编号为：1、2、3……n，任一无人水面艇i作为跟随者与它的领导者无人水面艇i-1的可视距离为夹角为在整个运动控制过程中满足如下约束条件：di,col<di(t)<di,con，以使领导者与跟随者保持安全距离同时又在通讯连接范围内，其中i＝1、2、3……n，当i＝0时，表示给定的第一艘无人水面艇的参考轨迹，(xi(t)，yi(t))为第i个无人水面艇的位置，(xi-1(t)，yi-1(t))为第i-1个无人水面艇的位置，di,col>0为无人水面艇之间防止碰撞的最小安全距离，di,con为无人水面艇之间保持通讯连接的最大允许距离，且di,con>di,col>0，引入任一无人水面艇i与其领导者无人水面艇i-1之间的期望距离di,des，并定义任一无人水面艇i与它的领导者无人水面艇i-1的的跟踪误差edi(t)＝di(t)-di,des、角度误差eψi(t)＝ψi-1(t)-ψi(t)，其中di,con>di,des>di,col>0，ψi-1(t)为第i-1个无人水面艇的航向角，ψi(t)为第i个无人水面艇的航向角，将跟随者与其领导者之间的距离约束转换为跟踪误差约束：di,col-di,des<edi(t)<di,con-di,des。

进一步地，步骤(2)中，所述跟踪误差约束条件设计如下：

其中，edi(t)表示任一无人水面艇i与它的领导者无人水面艇i-1的跟踪误差，eψi(t)表示任一无人水面艇i与它的领导者无人水面艇i-1的角度误差，edi(t)表示edi(t)的下界性能函数，表示edi(t)的上界性能函数，eψi(t)表示eψi(t)的下界性能函数，表示eψi(t)的上界性能函数，di,des表示任一无人水面艇i与其领导者无人水面艇i-1之间的期望距离，di,col表示无人水面艇之间防止碰撞的最小安全距离，edi,∞表示性能函数edi(t)的稳态值，κdi表示性能函数edi(t)的收敛速度，di,con表示无人水面艇之间保持通讯连接的最大允许距离，表示性能函数的稳态值，eψi,0表示性能函数eψi(t)的初始值，eψi,∞表示性能函数eψi(t)的稳定值，κψi表示性能函数eψi(t)的收敛速度。

进一步地，步骤(3)中设计的误差跟踪转换函数为：

其中，zji表示第i个无人水面艇的转换误差，γji表示第i个无人水面艇性能函数的上界除以下界，表示自然底数的zji次幂，表示自然底数的-zji次幂，表示第i个无人水面艇性能函数的下界除以上界，当且仅当zji＝0时，tji(zji,γji)＝0；将跟踪误差不等式约束转换成如下等式约束：

其中：

得到如下转换误差：

进一步地，步骤(4)中，引入动态面控制技术并设计虚拟控制器的一阶滤波器为：

其中，αfi＝[αf1i,αf2i,αf3i]^t为滤波虚拟控制输入向量，αf1i表示α1i的滤波虚拟控制，αf2i表示α2i的滤波虚拟控制，αf3i表示α3i的滤波虚拟控制，表示αfi的导数，αi＝[α1i,α2i,α3i]^t为虚拟控制输入向量，α1i表示纵向速度ui的虚拟控制器，α2i表示横荡速度vi的虚拟控制器，α3i表示转向角速度ri的虚拟控制器，μi＝diag[μ1,μ2,μ3]为滤波器时间常数矩阵，μ1表示设计滤波虚拟控制αf1i的滤波器时间常数，μ2表示设计滤波虚拟控制αf2i的滤波器时间常数，μ3表示设计滤波虚拟控制αf3i的滤波器时间常数，i∈1、2、3……n,j＝d、ψ；αfi(0)表示滤波虚拟控制αfi的初始值，αi(0)表示虚拟控制器αi的初始值，虚拟控制器αi设计如下：

其中，ψi(t)为第i个无人水面艇的航向角，表示第i-1个无人水面艇的航向角的导数，为第i个无人水面艇和第i-1个无人水面艇之间的视角，kzdi>0表示虚拟控制器α1i的设计参数，zdi表示第i个无人水面艇的距离转换误差，i∈1、2、3……n,j＝d、ψ；表示第i-1个无人水面艇在x轴方向的速度，表示第i-1个无人水面艇在y轴方向的速度，kzψi>0表示虚拟控制器α3i的设计参数，zψi表示第i个无人水面艇的角度转换误差。

进一步地，步骤(5)中的扰动观测器设计如下：

其中，eαi＝αfi-αi，αfi＝[αf1i,αf2i,αf3i]^t为滤波虚拟控制输入向量，αi＝[α1i,α2i,α3i]^t为虚拟控制输入向量，z2i＝vi-αfi＝[z21i,z22i,z23i]^t，vi＝[ui,vi,ri]^t，z21i表示纵向速度ui与滤波虚拟控制αf1i的差，z22i表示横荡速度vi与滤波虚拟控制αf2i的差，z23i表示转向角速度ri与滤波虚拟控制αf3i的差，kd1i＝diag[kd11i,kd12i,kd13i]为对角矩阵，kd11i表示第一个扰动观测器的设计参数，kd12i表示第二个扰动观测器的设计参数，kd13i表示第三个扰动观测器的设计参数，mi表示无人水面艇的质量矩阵，c(vi)表示科氏力矩阵，d(vi)表示阻尼矩阵，τi＝[τui,τvi,τri]^t，τui表示第i个无人水面艇纵向的推力，τvi表示第i个无人水面艇横荡方向的推力，τri表示第i个无人水面艇转向的力矩，μi＝diag[μ1,μ2,μ3]为滤波器时间常数矩阵，μ1表示滤波虚拟控制αf1i的滤波器时间常数，μ2表示滤波虚拟控制αf2i的滤波器时间常数，μ3表示滤波虚拟控制αf3i的滤波器时间常数，ξ1i＝[ξ11i,ξ12i,ξ13i]^t是扰动观测器的状态变量，ξ11i表示第一个扰动观测器的状态变量，ξ12i表示第二个扰动观测器的状态变量，ξ13i表示第三个扰动观测器的状态变量，为外界未知扰动的估计值。

进一步地，步骤(6)中设计的编队跟踪控制器如下：

其中，k2i＝diag[k21i,k22i,k23i]为对角矩阵，k21i表示编队控制器τ1的设计参数，k22i表示编队控制器τ2的设计参数，k23i表示编队控制器τ3的设计参数。

本发明与现有技术相比，具有如下优点和有益效果：

1、本发明通过扰动观测器来补偿风浪流等外界未知干扰，使得本设计方法对外界干扰具有较强的鲁棒性。

2、本发明采用动态面控制技术设计跟踪编队控制器，避免使用领导者的加速度，提高了设计方案的实用性。

3、本发明采用的控制器设计方案，使得任一无人水面艇的位置跟踪误差在任何时刻均不违反位置输出跟踪误差约束条件，确保了任一跟随者与其领导者始终保持一定的安全距离并处于其领导者的通讯连接范围内，从而解决了编队控制中任一跟随者与其领导者防止碰撞和保持连接的问题。

附图说明

图1为本发明实施例多个无人水面艇的领导者-跟随者编队结构示意图。

图2为本发明实施例多个无人水面艇编队控制系统结构框图。

图3为本发明实施例一组无人水面艇与其领导者的可视距离变化示意图。

图4为本发明实施例一组无人水面艇的航向角跟踪误差eψi(t)的示意图。

图5为本发明实施例一组无人水面艇编队控制的位置输出轨迹示意图。

图6为本发明实施例无人水面艇纵向的推力τui的示意图。

图7为本发明实施例无人水面艇横荡方向的推力τvi的示意图。

图8为本发明实施例无人水面艇转向的力矩τri的示意图。

具体实施方式

下面结合实施例及附图对本发明作进一步详细的描述，但本发明的实施方式不限于此。

实施例：

本实施例提供了一种基于领导跟随结构的无人水面艇编队控制方法，该方法针对无人水面艇编队控制中的防止碰撞和保持连接问题，将这两个问题转化为位置跟踪误差的约束问题，提出基于领导者-跟随者结构的编队控制方法，多个无人水面艇的领导者-跟随者编队结构示意图如图1所示，多个无人水面艇编队控制系统结构框图如图2所示，所述方法具体包括以下步骤：

步骤(1)、建立多个无人水面艇的动态模型；

所述多个无人水面艇的动态模型为：

其中，ui表示第i个无人水面艇的纵向速度，vi表示第i个无人水面艇的横荡速度，ri表示第i个无人水面艇的转向角速度，ψi(t)为第i个无人水面艇的航向角，i＝1、2、3……n，表示ψi(t)的导数，表示第i个无人水面艇在x轴方向的速度，表示第i个无人水面艇在y轴方向的速度，表示第i个无人水面艇在u，v，r方向上的加速度构成的向量，vi＝[ui,vi,ri]^t，τi＝[τui,τvi,τri]^t，τui表示第i个无人水面艇纵向的推力，τvi表示第i个无人水面艇横荡方向的推力，τri表示第i个无人水面艇转向的力矩，τwi＝[τwui,τwvi,τwri]^t，τwui表示第i个无人水面艇在纵向方向受到的外部时变扰动，τwvi表示第i个无人水面艇在横荡方向受到的外部时变扰动，τwri表示第i个无人水面艇在转向角方向受到的外部时变扰动，mi表示无人水面艇的质量矩阵，c(vi)表示科氏力矩阵，d(vi)表示阻尼矩阵，其中：

τwi＝[2+4sin(0.05t),-3+2cos(0.05t),2-5sin(0.05t)]^t

在本实施例中，选取5个(i＝1,2,3,4,5)相同的无人水面艇动态模型，无人艇的系统参数分别为：

m11i＝25.8kg，m22i＝33.8kg，m23i＝m32i＝1.0948kg，

m33i＝2.76kg，c13(vi)＝-m22ivi-m23iri，

d22(vi)＝0.8612+36.2823*|vi|+0.805*|ri|，

d23(vi)＝-0.1079+0.845*|vi|+3.45*|ri|，

d32(vi)＝-0.1052-5.0437*|vi|-0.13*|ri|，

d33(vi)＝1.9-0.08*|vi|+0.75*|ri|，i＝1,2,3,4,5。

艇体长度为li＝1.225m。

步骤(2)、依据领导者-跟随者的编队控制方式，根据领导者与跟随者的安全距离和通讯连接范围设计跟随者的位置来输出跟踪误差约束条件，任一无人水面艇i与它的领导者无人水面艇i-1的可视距离为夹角为在整个运动控制过程中满足如下约束条件：di,col<di(t)<di,con，以使领导者与跟随者保持安全距离同时又在通讯连接范围内，当i＝0时，表示给定的第一艘无人水面艇的参考轨迹，(xi(t)，yi(t))为第i个无人水面艇的位置，(xi-1(t)，yi-1(t))为第i-1个无人水面艇的位置，di,col>0为无人水面艇之间防止碰撞的最小安全距离，di,con为无人水面艇之间保持通讯连接的最大允许距离，且di,con>di,col>0，引入任一无人水面艇i与其领导者无人水面艇i-1之间的期望距离di,des，并定义任一无人水面艇i与它的领导者无人水面艇i-1的的跟踪误差edi(t)＝di(t)-di,des、角度误差eψi(t)＝ψi-1(t)-ψi(t)，其中di,con>di,des>di,col>0，ψi-1(t)为第i-1个无人水面艇的航向角，ψi(t)为第i个无人水面艇的航向角，将跟随者与其领导者之间的距离约束转换为跟踪误差约束：di,col-di,des<edi(t)<di,con-di,des，本实施例中，di,con＝6m，di,des＝5m，di,col＝4m，所期望的参考轨迹设计为：

当t≤60s时，所期望的参考轨迹为直线运动：xd＝3t，yd＝ψd＝0；

当t>60s时，所期望的参考轨迹为如下圆周运动：

xd＝180+60sin(0.05(t-60))，

yd＝60(1-cos(0.05(t-60)))，

ψd＝0.05(t-60)。

以ηi＝[xi(t),yi(t),ψi(t)]^t表示无人水面艇的位置(xi(t)，yi(t))和航向角ψi(t)，无人水面艇的初始位置及航向角分别选择为η1(0)＝[0,5,0]^t，η2(0)＝[0,10,0]^t，η3(0)＝[0,15,0]^t，η4(0)＝[0,20,0]^t，η5(0)＝[0,25,0]^t，初始速度选择为vi(0)＝[0,0,0]^t，i＝1,2,3,4,5；

任一跟随的无人水面艇i与它的领导者无人水面艇i-1的跟踪误差约束条件设计如下：

其中，edi(t)表示任一无人水面艇i与它的领导者无人水面艇i-1的跟踪误差，eψi(t)表示任一无人水面艇i与它的领导者无人水面艇i-1的角度误差，本实施例中

步骤(3)、设计跟踪误差转换函数，经过转换函数变换后，得到如下误差方程：

任一跟随的无人水面艇i与它的领导者无人水面艇i-1的可视距离变化示意图如图3所示，任一无人水面艇的航向角跟踪误差eψi(t)如图4所示。

步骤(4)、应用动态面控制技术设计虚拟控制器，引入动态面控制技术并设计虚拟控制器的一阶滤波器为：

其中，αfi＝[αf1i,αf2i,αf3i]^t为滤波虚拟控制输入向量，αi＝[α1i,α2i,α3i]^t为虚拟控制输入向量，本实施例中的滤波器时间常数矩阵设计为μi＝diag[0.1,0.1,0.1]，虚拟控制器αi设计如下：

其中，为第i个无人水面艇和第i-1个无人水面艇之间的视角，kzdi＝kzψi＝4，zdi表示第i个无人水面艇的距离转换误差，表示第i-1个无人水面艇在x轴方向的速度，表示第i-1个无人水面艇在y轴方向的速度，zψi表示第i个无人水面艇的角度转换误差。

步骤(5)、设计扰动观测器补偿外界未知干扰，扰动观测器设计如下：

其中，eαi＝αfi-αi，αfi＝[αf1i,αf2i,αf3i]^t为滤波虚拟控制输入向量，αi＝[α1i,α2i,α3i]^t为虚拟控制输入向量，z2i＝vi-αfi＝[z21i,z22i,z23i]^t，vi＝[ui,vi,ri]^t，z21i表示纵向速度ui与滤波虚拟控制αf1i的差，z22i表示横荡速度vi与滤波虚拟控制αf2i的差，z23i表示转向角速度ri与滤波虚拟控制αf3i的差，kd1i＝diag[6,6,6]为对角矩阵，τi＝[τui,τvi,τri]^t，τui表示第i个无人水面艇纵向的推力，τui的示意图如图6所示，τvi表示第i个无人水面艇横荡方向的推力，τvi的示意图如图7所示，τri表示第i个无人水面艇转向的力矩，τri的示意图如图8所示，ξ1i＝[ξ11i,ξ12i,ξ13i]^t是扰动观测器的状态变量，观测器的状态初始值为ξ1i(0)＝[0.5,0.5,0.5]^t，为外界未知扰动的估计值。

步骤(6)、应用李雅普诺夫稳定性理论设计编队跟踪控制器：设计编队跟踪控制器，应用李雅普诺夫稳定性理论严格证明闭环系统的稳定性，并确保编队控制中任一跟随者与其领导者始终保持一定的安全距离并处于其领导者的通讯连接范围内，设计的编队跟踪控制器如下：

其中，k2i＝diag[6,6,6]，一组无人水面艇编队控制的位置输出轨迹示意图如图5所示。

以上所述，仅为本发明专利较佳的实施例，但本发明专利的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明专利所公开的范围内，根据本发明专利的技术方案及其发明专利构思加以等同替换或改变，都属于本发明专利的保护范围。