CN113741433B - 一种水面无人船的分布式编队方法 - Google Patents

Abstract

本发明公开了一种水面无人船的分布式编队方法，包括以下步骤：1)基于虚拟结构法建立无人船集群数学模型；2)根据无人船集群数学模型获得第i艘无人船跟踪误差动态模型；3)根据编队任务进行无人船航行路径和速度规划；4)根据第i艘无人船跟踪误差动态模型和无人船速度规划，确定纵荡速度和艏摇角速度的虚拟控制量，进而获得第i艘驱动无人船运动的推进力和力矩向量的虚拟控制量；5)根据步骤4)中的驱动无人船运动的推进力和力矩向量的虚拟控制量，控制无人船完成编队任务。本发明路径规划中将无人艇与目标点或障碍物的距离直接与无人艇的速度挂钩，可快速得到无人艇的规划航行路径。

Description

一种水面无人船的分布式编队方法

技术领域

本发明涉及无人船路径规划技术，尤其涉及一种水面无人船的分布式编队方法。

背景技术

随着海洋空间中水面及水下作业任务逐渐向复杂化、规模化方向发展，特别是无人智能集群式海洋军事作业任务是今后海洋军事行动模式的重要发展方向，水面无人船集群(Unmanned Surface Vehicle Swarm，USVs)作为海洋智能机器人技术发展到一定阶段的产物已获得广泛关注和研究。水面无人船集群从最初军事领域扩展至民用领域，比如海洋水文与环境监测、水下地形测绘、污染物追踪与调查等。

为此，就需要针对无人船集群的编队规划以及控制，设计合适的方法，从而保证无人船编队自主协同控制器能够实现无人船编队的集结、队形保持、队形自主变换、动态任务分配、容错控制等任务。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种水面无人船的分布式编队方法。

本发明解决其技术问题所采用的技术方案是：一种水面无人船的分布式编队方法，包括以下步骤：

1)基于虚拟结构法建立无人船集群数学模型；

1.1)建立惯性坐标系O_E-X_EY_EZ_E，其坐标原点O_E为地球上的任意一点，O_EX_E轴沿水平面指向无人船运动的主航向，O_EY_E轴同样位于水平面内，且与O_EX_E轴相垂直，其正向按右手法则确定；O_EZ_E轴与O_EX_EY_E平面垂直，指向地心；

非惯性坐标系O_b-X_bY_bZ_b，其坐标原点O_b为无人船的中心，O_bX_b轴在舯纵剖面内且平行无人船基线，并指向船艏；O_bY_b轴平行于无人船基面，指向右舷；O_bZ_b轴垂直于O_bX_bY_b平面，指向龙骨；

1.2)确定无人船的运动参数；

位移η₁在坐标系O_E-X_EY_EZ_E的分量表示为(x，y，z)；姿态角η₂在坐标系 O_E-X_EY_EZ_E的分量表示为(φ，θ，ψ)；线速度在坐标系O_b-X_bY_bZ_b的分量表示为(u，v，w)；角速度在坐标系O_b-X_bY_bZ_b的分量为(p，q，r)；力在坐标系O_b-X_bY_bZ_b的分量表示为(X，Y，Z)；力矩在坐标系O_b-X_bY_bZ_b的分量表示为(K，M，N)；

1.3)对于n艘水面无人船组成的集群，第i艘无人船的数学模型为：

其中，模型各变量的实际意义在步骤1.2)中已给出。此处展开六个状态的一阶微分方程，表示系统位姿和速度等状态变量随时间的变化，作为研究无人船运动的数学模型。u_i，v_i)为第i艘无人船的线速度在坐标系O_b-X_bY_bZ_b的X，Y 轴分量；r_i为第i艘无人船的角速度在坐标系O_b-X_bY_bZ_b的Z轴分量；ψ_i为第i 艘无人船的姿态角在坐标系O_E-X_EY_EZ_E的Z轴分量；

M_i为第i艘无人船自身的惯性和水动力附加的惯性构成的惯性矩阵；

D_i为第i艘无人船的一阶水动力阻尼矩阵；

τ_i＝[τ_ui 0 τ_ri]^T为第i艘驱动无人船运动的推进力和力矩向量；

2)根据第i艘无人船的数学模型获得第i艘无人船跟踪误差动态模型：

其中，v_ei＝v_i-v_vi，α_i＝m_11i/m_22i，β_i＝d_22i/m_22i；

3)根据编队任务进行无人船航行路径和速度规划，设在t时刻，第i艘无人船距离目标点的距离是D_gi，距离目标点的角度是θ_gi，D_l为目标点的影响作用距离，无人船i距离障碍物的距离为D_ic，c＝1,2,3，……N，N为无人艇工作环境中障碍物的个数，无人艇所在的位置为(x_i,y_i)，障碍物所在的位置为(x_c,y_c)，D_r是障碍物的影响作用距离的临界值；

则当D_ic≤D_r时，：

其中，T_i是障碍物对无人艇速度的影响系数，V”_x与V”_y分别为无人艇在障碍物作用下x方向与y方向上的速度分量，K_m为速度排斥因子；

当无人艇与目标点的距离小于距离阈值D_t时则判断无人艇已到达目标点位置；

4)根据第i艘无人船跟踪误差动态模型和无人船速度规划，确定纵荡速度和艏摇角速度的虚拟控制量，进而获得第i艘驱动无人船运动的推进力和力矩向量的虚拟控制量；

其中，τ_ui和τ_ri分别为驱动第i艘无人船运动的推进力和转向力矩；k和δ均为控制器设计参数；u_ei和r_ei分别为第i艘无人船纵荡速度误差和艏摇角速度误差；和/>分别表示第i艘无人船纵荡速度和艏摇角速度的虚拟控制量随时间的变化；w_ei为第i艘无人船航向角偏差与视线导航角之差。

5)根据步骤4)中的控制量，控制无人船完成编队任务。

按上述方案，所述步骤2)中考虑到无人船集群在实际的航行过程中会受到外界环境干扰，第i艘无人船跟踪误差动态模型修正为：

其中，τ_Euiτ_Eviτ_Eri分别为第i艘无人船在纵荡、横荡以及艏摇三个自由度上的不确定干扰，包括风干扰力以及波浪干扰力的合力(合力矩)。

按上述方案，所述步骤4)中考虑到无人船集群在实际的航行过程中会受到外界环境干扰，将第i艘驱动无人船运动的推进力和力矩向量的虚拟控制量修正为：

其中，为第i艘无人船纵荡干扰自适应律，/>为第i艘无人船艏摇干扰自适应律。用以补偿实际的航行过程中受到的风、浪、流等外界环境的干扰。

本发明产生的有益效果是：

1、本发明路径规划中将无人艇与目标点或障碍物的距离直接与无人艇的速度挂钩，简化流程，可快速得到无人艇的规划航行路径。

2、本发明基于针对实际航行过程中的不确定干扰设计了自适应控制律控制器，实现了无人船集群自适应控制。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的坐标系与无人艇的运动示意图；

图3是本发明实施例的虚拟结构与无人艇跟踪误差示意图；

图4是本发明实施例的基于虚拟结构法的无人艇编队控制示意图；

图5是本发明实施例的编队航行轨迹仿真示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，一种水面无人船的分布式编队方法，包括以下步骤：

1)基于虚拟结构法建立无人船集群数学模型；

1.1)如图2所示，建立惯性坐标系O_E-X_EY_EZ_E，其坐标原点O_E为地球上的任意一点，O_EX_E轴沿水平面指向无人船运动的主航向，O_EY_E轴同样位于水平面内，且与O_EX_E轴相垂直，其正向按右手法则确定；O_EZ_E轴与O_EX_EY_E平面垂直，指向地心；

非惯性坐标系O_b-X_bY_bZ_b，其坐标原点O_b为编队中任一无人船的中心，O_bX_b轴在舯纵剖面内且平行无人船基线，并指向船艏；O_bY_b轴平行于无人船基面，指向右舷；O_bZ_b轴垂直于O_bX_bY_b平面，指向龙骨；

1.2)确定无人船的运动参数；

位移η₁在坐标系O_E-X_EY_EZ_E的分量表示为(x，y，z)；姿态角η₂在坐标系 O_E-X_EY_EZ_E的分量表示为(φ，θ，ψ)；线速度在坐标系O_b-X_bY_bZ_b的分量表示为(u，v，w)；角速度在坐标系O_b-X_bY_bZ_b的分量为(p，q，r)；力在坐标系O_b-X_bY_bZ_b的分量表示为(X，Y，Z)；力矩在坐标系O_b-X_bY_bZ_b的分量表示为(K，M，N)；

坐标系中的运动变量参数可记为向量的形式：

η＝[η₁ η₂]^Tη₁＝[x y z]^Tη₂＝[φ θ ψ]^T

υ＝[υ₁ υ₂]^Tυ₁＝[u v w]^Tυ₂＝[p q r]^T

τ＝[τ₁ τ₂]^Tτ₁＝[X Y Z]^Tτ₂＝[K M N]^T

上面两组变量之间存在着的变换关系即为无人船的运动学方程：

其中J₁(η₂)，J₂(η₂)分别为变换矩阵，表示为：

本申请中只涉及无人船的水平面运动，因而可以把无人船的六自由度空间运动简化成一种只有三个自由度的水平面运动问题。考虑欠驱动无人船，假设其左右对称，且没有侧推器。根据上述分析，可以得到欠驱动无人船水平面运动数学模型，现将其写成矩阵方程的形式：

其中，M是惯性矩阵，由无人船自身的惯性和水动力附加的惯性构成；C(υ) 是科氏力和向心力矩阵，同样包含了无人船以及水动力的附加部分；D为一阶水动力阻尼矩阵，D_n(υ)为高阶水动力阻尼矩阵；τ＝[τ_u 0 τ_r]^T为驱动无人船运动的推进力和力矩向量；τ_E＝[τ_Eu τ_Ev τ_Er]^T为风、浪、流等环境干扰向量。

1.3)对于n艘水面无人船组成的集群，第i艘无人船的数学模型为：

其中，模型各变量的实际意义在步骤1.2)中已给出，(u_i，v_i)为第i艘无人船的线速度在坐标系O_b-X_bY_bZ_b的X，Y轴分量；r_i为第i艘无人船的角速度在坐标系O_b-X_bY_bZ_b的Z轴分量；ψ_i为第i艘无人船的姿态角在坐标系O_E-X_EY_EZ_E的Z轴分量；

M_i为第i艘无人船自身的惯性和水动力附加的惯性构成的惯性矩阵；

D_i为第i艘无人船的一阶水动力阻尼矩阵；

τ_i＝[τ_ui 0 τ_ri]^T为第i艘驱动无人船运动的推进力和力矩向量；

2)虚拟结构与无人船跟踪误差如图3所示，根据第i艘无人船的数学模型获得第i艘无人船跟踪误差动态模型：

其中，v_ei＝v_i-v_vi，α_i＝m_11i/m_22i，β_i＝d_22i/m_22i；

3)根据编队任务进行无人船航行路径和速度规划，设在t时刻，第i艘无人船距离目标点的距离是D_gi，距离目标点的角度是θ_gi，D_l为目标点的影响作用距离，无人艇距离障碍物的距离为D_c，c＝1,2,3，……N，N为无人艇工作环境中障碍物的个数，无人艇所在的位置为(x_i,y_i)，障碍物所在的位置为(x_c,y_c)，D_r是障碍物的影响作用距离的临界值；

则当D_c≤D_r时，：

其中，T_i是障碍物对无人艇速度的影响系数，V”_x与V”_y分别为无人艇在障碍物作用下x方向与y方向上的速度分量，K_m为速度排斥因子；

当无人艇与目标点的距离小于距离阈值D_t时则判断无人艇已到达目标点位置；

根据人工势场法的思想，本发明将对其进行改进，即将无人艇与目标点或障碍物的距离直接与无人艇的速度挂钩，这样就能大大简化算法流程，从而快速得到无人艇的航行路径。依据改进的方法，设在t时刻，无人艇距离目标点的距离是D_g，距离目标点的角度是θ_g，D_l为目标点的影响作用距离，则只考虑目标点吸引的作用下，当D_g≤D_l时下一时刻无人艇的速度表达式为：

其中V'_x与V'_y分别为无人艇在目标点作用下x方向与y方向上的速度分量，K_n为速度吸引因子，当D_g＞D_l时，D_g恒等于100。此时设无人艇距离障碍物的距离为D_i(i＝1、2、3...N)，N为无人艇工作环境中障碍物的个数，无人艇所在的位置为 (x_c,y_c)，障碍物所在的位置为(x_i,y_i)，D_r是障碍物的影响作用距离的临界值，则当D_i≤D_r时有以下各式成立：

其中，T_i是计算障碍物对无人艇速度影响的过渡值，V”_x与V”_y分别为无人艇在障碍物作用下x方向与y方向上的速度分量，K_m为速度排斥因子。

当D_i＞D_r时T_i恒等于0。

则在目标点和障碍物的作用下无人艇的实时速度V表示为：

对于如何判断无人艇到达目标点选择D_t作为距离阈值，当无人艇与目标点的距离小于D_t时则判断无人艇已到达目标点位置。

4)根据第i艘无人船跟踪误差动态模型和无人船速度规划，确定纵荡速度和艏摇角速度的虚拟控制量，进而获得第i艘驱动无人船运动的推进力和力矩向量的虚拟控制量；

如图4所示，将集群控制器设计分为运动学和动力学两个部分，在运动学控制器设计部分，将u_i和r_i看作输入，设计纵荡速度和艏摇角速度的虚拟控制量；在动力学控制器设计部分，对实际输入量τ_ui和τ_ri进行设计。

结合李雅普诺夫函数可以得到：

当考虑到无人船集群在实际的航行过程中会受到风、浪、流等外界环境干扰时，集群中第i艘水面无人船的数学模型需改写为：

假设不确定干扰τ_Ei有界，但上界未知，即|τ_Ei|≤τ_Eimax＜∞，基于李雅普诺夫直接法，设计自适应律

其中，γ_hi(1≤h≤4)均为正常数，为横荡速度v_i的估计量，/>为估计误差，

得到水面无人船集群的鲁棒自适应控制器：

5)根据步骤4)中的控制量，控制无人船完成编队任务。

仿真计算信息如下：在初始状态下，给定艇1的坐标为(-30,5)，艏向角为0，纵荡速度、横荡速度、艏摇角速度都为0；给定艇2的坐标为(-20,10)，艏向角为0，纵荡速度、横荡速度、艏摇角速度都为0；给定艇3的坐标为(-10,10)，艏向角为0，纵荡速度、横荡速度、艏摇角速度都为0；用矩阵表示如下：

[x₁(0) y₁(0) ψ₁(0) u₁(0) v₁(0) r₁(0)]＝[-30 5 0 0 0 0]

[x₂(0) y₂(0) ψ₂(0) u₂(0) v₂(0) r₂(0)]＝[-20 10 0 0 0 0]

[x₃(0) y₃(0) ψ₃(0) u₃(0) v₃(0) r₃(0)]＝[-10 -10 0 0 0 0]

设定初始编队为“一”，在前50s编队以u_d＝2m/s的速度运动，在50到100s 阶段将速度降低为1m/s，在随后的时间内，编队开始调头，调头完成以后继续以1m/s的速度航行，假设不确定时变干扰为：

选择控制器参数为：

k_1i＝0.47、k_2i＝0.05、k_3i＝0.95、k_4i＝k_5i＝0.98、δ_i＝π/3、γ_1i＝0.3、γ_2i＝γ_3i＝γ_4i＝0.5。仿真结果如图5。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (3)

1.一种水面无人船的分布式编队方法，其特征在于，包括以下步骤：

1)基于虚拟结构法建立无人船集群数学模型；

1.1)建立惯性坐标系O_E-X_EY_EZ_E，其坐标原点O_E为地球上的任意一点，O_EX_E轴沿水平面指向无人船运动的主航向，O_EY_E轴位于水平面内，且与O_EX_E轴相垂直，其正向按右手法则确定；O_EZ_E轴与O_EX_EY_E平面垂直，指向地心；

非惯性坐标系O_b-X_bY_bZ_b，其坐标原点O_b为无人船的中心，O_bX_b轴在舯纵剖面内且平行无人船基线，并指向船艏；O_bY_b轴平行于无人船基面，指向右舷；O_bZ_b轴垂直于O_bX_bY_b平面，指向龙骨；

1.2)确定无人船的运动参数；

位移η₁在坐标系O_E-X_EY_EZ_E的分量表示为(x，y，z)；姿态角η₂在坐标系O_E-X_EY_EZ_E的分量表示为(φ，θ，ψ)；线速度在坐标系O_b-X_bY_bZ_b的分量表示为(u，v，w)；角速度在坐标系O_b-X_bY_bZ_b的分量为(p，q，r)；力在坐标系O_b-X_bY_bZ_b的分量表示为(X，Y，Z)；力矩在坐标系O_b-X_bY_bZ_b的分量表示为(K，M，N)；

1.3)对于n艘水面无人船组成的集群，根据无人船的运动参数建立第i艘无人船的数学模型，获得无人船集群数学模型；

所述步骤1.3)中根据无人船的运动参数建立第i艘无人船的数学模型为：

此处展开六个状态的一阶微分方程，表示系统位姿和速度变量随时间的变化，作为研究无人船运动的数学模型；

其中，(u_i，v_i)为第i艘无人船的线速度在坐标系O_b-X_bY_bZ_b的X，Y轴分量；r_i为第i艘无人船的角速度在坐标系O_b-X_bY_bZ_b的Z轴分量；ψ_i为第i艘无人船的姿态角在坐标系O_E-X_EY_EZ_E的Z轴分量；

M_i为第i艘无人船自身的惯性和水动力附加的惯性构成的惯性矩阵；

D_i为第i艘无人船的一阶水动力阻尼矩阵；

τ_i＝[τ_ui 0 τ_ri]^T为第i艘驱动无人船运动的推进力和力矩向量；

2)根据无人船集群数学模型获得第i艘无人船跟踪误差动态模型；

其中第i艘无人船跟踪误差动态模型如下：

其中，v_ei＝v_i-v_vi，α_i＝m_11i/m_22i，β_i＝d_22i/m_22i；

3)根据编队任务进行无人船航行路径和速度规划，设在t时刻，第i艘无人船距离目标点的距离是D_gi，距离目标点的角度是θ_gi，D_l为目标点的影响作用距离，无人船i距离障碍物的距离为D_ic，c＝1,2,3，……N，N为无人艇工作环境中障碍物的个数，无人艇所在的位置为(x_i,y_i)，障碍物所在的位置为(x_c,y_c)，D_r是障碍物的影响作用距离的临界值；

则当D_ic≤D_r时，

其中，T_i是障碍物对无人艇速度的影响系数，V″_x与V″_y分别为无人艇在障碍物作用下x方向与y方向上的速度分量，K_m为速度排斥因子；

当无人艇与目标点的距离小于距离阈值D_t时则判断无人艇已到达目标点位置；

4)根据第i艘无人船跟踪误差动态模型和无人船速度规划，确定纵荡速度和艏摇角速度的虚拟控制量，进而获得第i艘驱动无人船运动的推进力和力矩向量的虚拟控制量；

其中，τ_ui和τ_ri分别为驱动第i艘无人船运动的推进力和转向力矩；k和δ均为控制器设计参数；u_ei和r_ei分别为第i艘无人船纵荡速度误差和艏摇角速度误差；和/>分别表示第i艘无人船纵荡速度和艏摇角速度的虚拟控制量随时间的变化；w_ei为第i艘无人船航向角偏差与视线导航角之差；

5)根据步骤4)中的驱动无人船运动的推进力和力矩向量的虚拟控制量，控制无人船完成编队任务。

2.根据权利要求1所述的水面无人船的分布式编队方法，其特征在于，所述步骤2)中，考虑到无人船集群在实际的航行过程中会受到外界环境干扰，第i艘无人船跟踪误差动态模型修正为：

其中，τ_Euiτ_Eviτ_Eri分别为第i艘无人船在纵荡、横荡以及艏摇三个自由度上的不确定干扰。

3.根据权利要求1所述的水面无人船的分布式编队方法，其特征在于，所述步骤4)中考虑到无人船集群在实际的航行过程中会受到外界环境干扰，将第i艘驱动无人船运动的推进力和力矩向量的虚拟控制量修正为：

其中，为第i艘无人船纵荡干扰自适应律，/>为第i艘无人船艏摇干扰自适应律，用以补偿实际的航行过程中受到的风、浪、流外界环境的干扰。