CN108549394A - 一种基于领航者和虚拟领航者的多auv直线编队控制方法 - Google Patents

Abstract

本发明属于船舶运动控制领域，具体涉及一种基于领航者和虚拟领航者的多AUV直线编队控制方法。本发明包括如下步骤：(1)根据使命任务，实现多AUV的路径规划，获得领航者需跟随的路径(2)建立参考坐标系(3)建立如下AUV动力学模型(4)构建固定坐标系下的跟随者AUV的误差模型(5)设计AUV运动控制器。本发明提出了领航‑跟随法和虚拟领航‑跟随法相结合的编队控制策略，在多AUV组成的群体中，指定某个AUV为领航者，剩余的AUV作为它的跟随者，跟随者以期望的距离间隔跟随领航者的位置。通过反步法实现AUV路径跟踪控制，跟随者实时接收且只需接收领航者的纵向位姿信息，减少AUV在多种信息通讯时出现的混乱现象，增强了编队控制的稳定可靠性。

Description

一种基于领航者和虚拟领航者的多AUV直线编队控制方法

技术领域

本发明属于船舶运动控制领域，具体涉及一种基于领航者和虚拟领航者的多AUV直线编队控制方法。

背景技术

随着能源的枯竭，开发海洋也越来越迫切，所以各国都开始研究水下航行器代替人去探测海底资源，而同时，水下航行器在军事上也发挥着越来越重要的作用。于是如何开发利用自主水下航行器AUV成了各国的研究热点。而随着AUV的发展，单个AUV在控制与通讯方面能力不足，无法执行大范围，复杂环境下的任务，于是有不少学者开始研究多AUV来解决这些问题。

多AUV编队队形控制一般分为两步：首先确定各自主水下航行器的期望位置；然后通过控制器生成控制命令，驱动AUV以期望队形运动到期望位置。到目前为止，研究多AUV编队的方法主要有4种：分别为跟随领航者法、基于行为法、虚拟结构法和强化学习法。

本发明采用的是跟随领航者法与虚拟领航跟随法的结合，跟随领航者法的基本思想是：在多AUV组成的群体中，指定某个AUV为领航者，剩余的AUV作为它的跟随者，跟随者以期望的距离间隔跟随领航者的位置。根据领航者AUV与跟随者AUV之间的相对位置关系，可以形成不同的编队队形。并且跟随领航者法可以仅仅给定领航者AUV的行为或轨迹位置就可以控制整个AUV队形。

发明内容

本发明提出一种基于领航者和虚拟领航者相结合的多AUV的直线编队方法，减少AUV在多种信息通讯时出现的混乱现象，增强了编队控制的稳定可靠性。本发明主要包括如下步骤：

一种基于领航者和虚拟领航者的多AUV直线编队控制方法，具体包括如下步骤：

(1)根据使命任务，实现多AUV的路径规划，获得领航者需跟随的路径；

(2)建立参考坐标系：以步骤(1)中获取的路径的起点为路径坐标系的原点，记为0；以路径所在的直线为坐标轴的纵轴，记为X轴；垂直于路径且经过路径起点的直线为坐标轴的横轴，记为Y轴；AUV重心为G，速度向量为U_G，在固定坐标系下X轴和Y轴上投影为AUV受到的外力F在固定坐标系下X轴和Y轴上的投影为F_X，F_Y；受到的外力矩为T；运动坐标系的原点取在AUV重心G处；纵轴取在AUV纵中剖面内，指向船首，记为x轴；横轴与AUV纵中剖面垂直，指向右舷，记为y轴。速度向量为U_G，在固定坐标系下x轴和y轴上投影为u，v；AUV受到的外力F在固定坐标系下x轴和y轴上的投影为F_x，F_y；受到的外力矩为T；因外力矩而获得的角速度为r；

(3)建立如下AUV动力学模型：

在固定坐标系中，AUV的动力学模型为：

式中：φ为AUV速度与固定坐标系的夹角；ψ为X轴与运动坐标系的夹角，V_t为AUV的合速度，且假设u_G恒不为0，定义β为侧滑角，β＝φ-ψ＝tan^-1(v/u)；

在运动坐标系中，水体为理想流体时，AUV动力学模型为：

式中：mur，-mvr为AUV转动时的离心惯性力；N为绕水平面旋转的力矩；F_T为AUV纵向受到水平推力，Γ为垂直水平面方向转艏力矩，F_w为水平面对AUV的阻力，N为垂直水平面方向的力矩，F_w在x轴和y轴上的分力为F_xw，F_yw；m_u；m_vr；m_v；m_ur分别为实际运动中的质量；

(4)构建固定坐标系下的跟随者AUV的误差模型；

(5)设计AUV运动控制器。

步骤(2)具体包括：

领航者与跟随者的纵向距离为d，领航者与跟随者的横向距离D；在纵向距离上用基于领航-跟随法的纵向距离误差控制模型，在横向距离采用基于虚拟领航-跟随法的跟随者横向距离误差控制模型；

跟随者纵向距离误差控制模型如下：

在固定坐标系中，AUV_i为领航者，其它AUV为跟随者；AUV_i在固定坐标系下的位姿为(X_i，Y_i，φ_i)；第j个跟随者的坐标为(X_j，Y_j，φ_i)；第j个跟随者与领航者在纵向距离上的理想位姿关系为：

X_i-X_j＝d_j；

Y_j-Y_i＝D_j；

式中：D_j，d_j为跟随者直线路径与领航者直线路径距离长度；

纵向距离误差e_j为：

e_j＝X_i-X_j-d_j；

当时，领航者与跟随者在横向上完成编队控制；

跟随者横向距离误差控制模型如下：

领航者与跟随者在期望横向距离上：

Y_j-Y_i＝D_j；

在固定坐标系中，跟随者到直线Y＝D_j的距离为横向距离误差E_j，横向距离误差E_j为：

E_j＝Y_i-D_j；

φ_ej＝φ_j-φ_sj；

其中，AUV的前向距离为L，φ_j为第j个跟随者的艏向角，φ_sj为期望艏向角，φ_ej为艏向角误差，即横向距离误差。

步骤(5)具体包括：

(5.1)设计领航者AUV_i运动控制器：

领航者AUV_i的转艏力矩Γ_i为：

式中：m_ri为AUV在垂直水平面运动的实际质量；N_i为AUV运动时受到其它因素垂直水平面方向的力矩；k_ψ，k_r分别为艏向角，角速度系数；r_i为领航者AUV_i的角速度；AUV_i横向误差为E＝Y_i；领航者AUV_i期望艏向角为领航者AUV_i艏向角误差φ_e＝φ_i-φ_s；

AUV纵向受到的水平推力F_T为：

式中：m_ui为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xwi为AUV运动时会受到其它因素在水平面对AUV的阻力F_wi在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u_i为领航者AUV_i的纵向速度；

(5.2)设计跟随者AUV运动控制器：

第j个跟随者AUV的转艏力矩Γ_j为：

式中：m_rj为第j个跟随者AUV在垂直水平面运动的实际质量；N_j为第j个跟随者AUV运动时受到其它因素垂直水平面方向的力矩；k_ψ，k_r分别为艏向角，角速度系数；r_j为第j个跟随者AUV的角速度；

在纵向上第j个跟随者AUV受到的水平推力F_Tj为：

式中：m_uj第j个跟随者为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xwj为第j个跟随者AUV运动时受到其它因素在水平面的阻力F_wj在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u_j为第j个跟随者的纵向速度；u_cj为第j个跟随者AUV的期望速度，其中u_cj由下面公式给出：

u_cj＝u_d-g(e_j)；

式中：e_j为第j个跟随者AUV纵向距离误差；其中a满足：

附图说明

图1为坐标系的描述；

图2为领航-跟随法编队队形的纵向误差；

图3为虚拟领航-跟随法编队队形的横向误差；

图4为大地坐标系下的编队直线轨迹仿真结果；

图5为大地坐标系下编队梳子型轨迹仿真结果。

具体实施方式

本发明实现的具体方式如下：

步骤1：根据使命任务，实现多AUV的路径规划，获得领航者需跟随的路径。

步骤2：建立参考坐标系

研究AUV运动问题前，必须线建立表达AUV运动的固定参考坐标系和运动坐标系，以步骤1中获取的路径的起点为路径坐标系的原点，记为0；以路径所在的直线为坐标轴的纵轴，记为X轴；垂直于路径且经过路径起点的直线为坐标轴的横轴，记为Y轴。AUV重心为G，速度向量为U_G，在固定坐标系下X轴和Y轴上投影为AUV受到的外力F在固定坐标系下X轴和Y轴上的投影为F_X，F_Y；受到的外力矩为T。

运动坐标系的原点取在AUV重心G处；纵轴取在AUV纵中剖面内，指向船首，记为x轴；横轴与AUV纵中剖面垂直，指向右舷，记为y轴。速度向量为U_G，在固定坐标系下x轴和y轴上投影为u，v；AUV受到的外力F在固定坐标系下x轴和y轴上的投影为F_x，F_y；受到的外力矩为T；因外力矩而获得的角速度为r。

步骤3：建立AUV数学模型

固定坐标系的X轴与运动坐标系的夹角为ψ；AUV速度与固定坐标系夹角为φ；假设u_G恒不为0，则定义侧滑角β，则

β＝φ-ψ＝tan^-1(v/u)

AUV的动力学模型为：

式中：V_t为AUV的合速度，且

在运动坐标系中，如果水体为理想流体，即忽略流体黏性，根据牛顿定律，AUV动力学模型为：

式中：m为AUV质量；mur，-mvr为AUV转动时的离心惯性力；AUV绕水平向旋转的转动惯量为J；AUV在x轴和y轴方向受到的外力为F_x，F_y；N为绕水平面旋转的力矩。

实际流体有黏性，会出现流体惯性阻力效应，相当于AUV质量增加。针对欠驱动AUV，只会在AUV纵向受到水平推力F_T和垂直水平面方向转艏力矩Γ，AUV运动时会受到其它因素，主要是水体阻力在水平面对AUV的阻力F_w和垂直水平面方向的力矩N，其中F_w在x轴和y轴上的分力为F_xw，F_yw。AUV动力学模型又可以写为：

式中：m_u；m_vr；m_v；m_ur分别为实际运动中的质量。

步骤4：构建固定坐标系下的跟随者AUV的误差模型

将领航者与跟随者的距离分为纵向距离d和横向距离D。在纵向距离上采用领航-跟随法，在横向距离采用了虚拟领航-跟随法。

(1)跟随者纵向距离误差控制模型

在固定坐标系中，假定AUV_i为领航者，其它AUV为跟随者。在编队队形中AUV_i将其位姿纵坐标通过水声通信传感器发送给跟随者，跟随者实时采集领航者的纵向坐标信息。AUV_i在固定坐标系下的位姿为(X_i，Y_i，φ_i)；第j个跟随者的坐标为(X_j，Y_j，φ_j)；第j个跟随者与领航者在纵向距离上的理想位姿关系为：

X_i-X_j＝d_j

Y_j-Y_i＝D_j

式中：D_j，d_j为跟随者直线路径与领航者直线路径距离长度。

纵向距离误差e_j为：

e_j＝X_i-X_j-d_j

当时，领航者与跟随者在横向上完成编队控制。

(2)跟随者纵向距离误差控制模型

建立纵向距离误差模型，采用了虚拟领航-跟随法，由步骤4中(1)可知领航者与跟随者在期望横向距离上：

Y_j-Y_i＝D_j

在固定坐标系中，编队路径在纵轴上，当多AUV实现编队控制后，让领航者运动到坐标系纵轴上，即则跟随者将在Y＝D_j直线上，即此时有则领航者与跟随者在纵向上完成编队控制。在本专利中，让跟随者到直线Y＝D_j的距离为横向距离误差E_j，则横向距离误差E_j为：

E_j＝Y_j-D_j

假定AUV的前向距离为L，则期望艏向角φ_sj为：

将横向距离误差转换成艏向角误差φ_ej，则：

φ_ej＝φ_j-φ_sj

式中：φ_j为第j个跟随者的艏向角。

步骤5：AUV运动控制器的设计

(1)领航者AUV_i运动控制器的设计

领航者AUV_i，可以看作一个特殊的跟随者，不存在纵向误差，但有横向误差，且领航者AUV_i横向误差为E＝Y_i；领航者AUV_i期望艏向角为领航者AUV_i艏向角误差φ_e＝φ_i-φ_s。艏向角误差可以通过改变领航者AUV_i的转艏力矩Γ_i来控制：

式中：m_ri为AUV在垂直水平面运动的实际质量；N_i为AUV运动时受到其它因素，主要是水体阻力垂直水平面方向的力矩；k_ψ，k_r分别为艏向角，角速度系数；r_i为领航者AUV_i的角速度。

在AUV速度控制上，通常会给AUV一个期望纵向速度u_d；通过控制AUV纵向受到水平推力F_T来控制AUV速度。

式中：m_ui为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xwi为AUV运动时会受到其它因素，主要是水体阻力在水平面对AUV的阻力F_wi在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u_i为领航者AUV_i的纵向速度。

(2)跟随者AUV运动控制器的设计

对于跟随者AUV艏向角误差可以通过改变第j个跟随者AUV的转艏力矩Γ_j来控制：

式中：m_rj为第j个跟随者AUV在垂直水平面运动的实际质量；N_j为第j个跟随者AUV运动时受到其它因素，主要是水体阻力垂直水平面方向的力矩；k_ψ，k_r分别为艏向角，角速度系数；r_j为第j个跟随者AUV的角速度。

在纵向上，可通过控制跟随者AUV的速度来减少纵向距离误差：

式中：m_uj为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xwj为AUV运动时会受到其它因素，主要是水体阻力在水平面对AUV的阻力F_wj在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u_j为第i个跟随者的纵向速度；u_cj为跟随者AUV的期望速度，其中u_cj由下面公式给出：

u_cj＝u_d-g(e_j)

式中：e_j为第j个跟随者AUV纵向距离误差；在这里函数g(x)对任意的x∈R满足如下条件：

可以让：

其中a满足

跟随者实时接收且只需接收领航者的纵向位姿信息，减少AUV在多种信息通讯时出现的混乱现象，增强了编队控制的稳定可靠性。

以下结合附图和两个案例对本发明技术方案进行详细描述。

步骤1：本案例中实现3个AUV的直线编队，根据使命任务首先路径规划出领航者需跟随的路径，图4(a)是由原点(0，0)到点(1000，1000)，图5(a)(X，Y)＝(0，0)→(1000，0)→(1000，500)→(0，500)→(0，1000)→(1000，1000)→…。此案例，以三角形编队为例介绍，其中领航者与跟随者AUV纵向距离为d₂＝d₃＝20，横向距离D₂＝50，D₃＝-50，领航者与跟随者之间直线编队结构如图1所示。

步骤2：建立参考坐标系

以步骤1中获取的路径的起点为路径坐标系的原点，记为0；以路径所在的直线为坐标轴的纵轴，记为X轴；垂直于路径且经过路径起点的直线为坐标轴的横轴，记为Y轴。AUV重心为G，速度向量为U_G，在固定坐标系下X轴和Y轴上投影为AUV受到的外力F在固定坐标系下X轴和Y轴上的投影为F_X，F_Y；受到的外力矩为T。

建立AUV运动坐标系，运动坐标系的原点取在AUV重心G处；纵轴取在AUV纵轴剖面内，指向船首，记为x轴；横轴与AUV纵轴剖面垂直，指向右舷，记为y轴。速度向量为U_G，在固定坐标系下x轴和y轴上投影为u，v；AUV受到的外力F在固定坐标系下x轴和y轴上的投影为F_x，F_y；受到的外力矩为T；因外力矩而获得的角速度为r。

步骤3：建立AUV数学模型

固定坐标系的X轴与运动坐标系的夹角为ψ；AUV速度与固定坐标系夹角为φ；假设u_G恒不为0，则定义侧滑角β，则

β＝φ-ψ＝tan^-1(v/u)

AUV的动力学模型为：

式中：V_t为AUV的合速度，且

在运动坐标系中，如果水体为理想流体，即忽略流体黏性，根据牛顿定律，AUV动力学模型为：

式中：m为AUV质量；mur，-mvr为AUV转动时的离心惯性力；AUV绕水平面旋转的转动惯量为J；AUV在x轴和y轴方向受到的外力为F_x，F_y；N为绕水平面旋转的力矩。

实际流体有黏性，会出现流体惯性阻力效应，相当于AUV质量增加。针对欠驱动AUV，只会在AUV纵向受到水平推力F_T和垂直水平面方向转艏力矩Γ，AUV运动时会受到其它因素，主要是水体阻力，在水平面对AUV的阻力F_w和垂直水平面方向的力矩N，其中F_w在x轴和y轴上的分力为F_xw，F_yw。AUV动力学模型又可以写为：

式中：m_u；m_vr；m_v；m_ur分别为实际运动中的质量。

步骤4：构建固定坐标系下的跟随者AUV的误差模型

将领航者与跟随者的距离分为纵向距离d和横向距离D。在纵向距离上采用领航-跟随法，在横向距离采用了虚拟领航-跟随法。

(1)跟随者纵向距离误差控制模型

在固定坐标系中，假定AUV₁为领航者，AUV₂和AUV₃为跟随者。在编队队形中AUV₁将其位姿纵坐标通过通信传感器发送给跟随者，而跟随者也实时采集领航者的纵向坐标信息。AUV1在固定坐标系下的位姿为(X₁，Y₁，φ₁)；跟随者的坐标分别为(X₂，Y₂，φ₂)和(X₃，Y₃，φ₃)；跟随者与领航者在纵向距离上的理想位姿关系为

X₁-X₂＝d₂和Y₂-Y₁＝D₂

X₁-X₃＝d₃和Y₃-Y₁＝D₃

其中D₂，D₃，d₂，d₃为距离长度。则跟随者纵向距离误差为

e₂＝X₁-X₂-d₂和e₃＝X₁-X₃-d₃

当和时，可以看作领航者与跟随者在横向上完成编队控制。

(2)跟随者纵向距离误差控制模型

建立纵向距离误差模型，采用了虚拟领航-跟随法，由步骤4中(1)可知领航者与跟随者在期望横向距离上Y₂-Y₁＝D₁和Y₃-Y₁＝D₃

在固定坐标系中，编队路径在纵轴上，当多AUV实现编队控制后，让领航者运动到坐标系纵轴上，即则跟随者将在Y＝D₂和Y＝D₃直线上，即和此时有和则可以看作领航者与跟随者在纵向上完成编队控制。在本专利中，让两个跟随者到直线Y＝D₂和Y＝D₃的距离为横向距离误差E₂，E₃，所以横向距离误差为

E₂＝Y₂-D₂和E₃＝Y₃-D₃

假定AUV的前向距离为L，则跟随者期望艏向角φ_s2，φ_s3为

于是将横向距离误差转换成艏向角误差φ_e2，φ_e3

φ_e2＝φ₂-φ_s2，φ_e3＝φ₃-φ_s3

其中φ₂，φ₃为两个跟随者的当前艏向角

步骤5：AUV运动控制器的设计

(1)领航者AUV₁运动控制器的设计

对于领航者AUV₁，可以看作一个特殊的跟随者，不存在纵向误差，但有横向误差，且领航者AUV₁横向误差为E₁＝Y₁；领航者AUV₁期望艏向角为领航者AUV₁艏向角误差φ_e1＝φ₁-φ_s1。艏向角误差可以通过改变领航者AUV₁的转艏力矩Γ₁来控制

其中m_r1为AUV在垂直水平面运动的实际质量；N₁为AUV运动时受到其它因素，主要是水体垂直水平面方向的力矩；k_ψ，k_r分别为艏向角，角速度系数；r₁为领航者AUV₁的角速度。

在AUV速度控制上，通常会给AUV一个期望纵向速度u_d；通过控制AUV纵向受到水平推力F_T来控制AUV速度。

其中m_u1为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xw1为AUV运动时会受到其它因素，主要是水体阻力在水平面对AUV的阻力F_w1在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u₁为领航者AUV₁的纵向速度。

(2)跟随者AUV运动控制器的设计

对于跟随者AUV艏向角误差可以通过改变两个跟随者AUV的转艏力矩Г₂，Γ₃来控制：

其中m_r2，m_r2为两个跟随者AUV在垂直水平面运动的实际质量；N₂，N₃为两个跟随者AUV运动时受到其它因素，主要是水体垂直水平面方向的力矩；k_ψ，k_r分别为艏向角，角速度系数；r₂，r₃为两个跟随者AUV的角速度。

在纵向上，可通过控制跟随者AUV的速度来减少纵向距离误差：

其中m_u2，m_u3为两个跟随者AUV在沿运动坐标系纵轴方向运动的实际质量；F_xw2，F_xw3为两个跟随者AUV运动时会受到其它因素，主要是水体阻力在水平面对AUV的阻力F_w2，F_w3在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u₂，u₃分别为两个跟随者的纵向速度；u_c2，u_c3为跟随者AUV的期望速度，其中u_c2，u_c3由公式给出：

u_c2＝u_d-g(e₂)，u_c3＝u_d-g(e₃)

其中e₂，e₃分别为两个跟随者AUV纵向距离误差；在这里函数g(x)对任意的x∈R满足如下条件

设定：

其中a满足

在说明书中，系数k_ψ，k_rk_u都是需要在实际实验中调试得到最佳值。D_j，d_j，u_d为可以由实际仿真验证中所要求设计的已知值。本发明仿真验证结果如图4和图5。

图4仿真中，3个AUV以三角形为队形进行直线编队运动，约150秒后完全达到编队状态。由图4(a)，细实线为步骤1所设计的路径，可以看到3个AUV很快的从初始位置运动到期望编队位置，且在120秒后达到稳定编队，如图4(e)，可以看到3个AUV与其期望路径误差为0，如图4(b)，3个AUV进行瞬时取点，即十字所在点，可以看到，AUV之间达到所期望的编队。由图4(c)和图4(d)，3个AUV的速度和艏向角均在稳定后达到相同值，形成了稳定的三角形编队。

图5为3个AUV以三角形为队形进行梳子型直线编队运动，梳子型编队是在直线上基础上添加了路径切换，即编队运动到梳子型的一个中间点时，切换路径坐标系的起点和终点，重新建立路径坐标系，让AUV在新的路径坐标系下形成编队。由图5(f)可以看到AUV是能进行编队运动的，而在路径切换上会出现误差，原因是跟随者AUV在路径切换是落后领航者AUV20m，使得3个AUV同时切换路径时，领航者基本无误差，而跟随者一个需要减速，一个需要加速，且与切换后直线误差突然增加，需要重新进行编队。由图5(b)和图5(c)可知速度重新达到稳定，误差也趋于0，所以在约200m后，3个AUV再次完成编队控制。而图5(d)，图5(f)可以看到跟随者AUV1分别在转折点外侧和内测相对于领航者AUV进行改变直线路径，且均能顺利完成，可以认定AUV直线编队成功。

Claims (3)

1.一种基于领航者和虚拟领航者相结合的多AUV直线编队控制方法，具体包括如下步骤：

(1)根据使命任务，实现多AUV的路径规划，获得领航者需跟随的路径；

(2)建立参考坐标系：以步骤(1)中获取的路径的起点为路径坐标系的原点，记为0；以路径所在的直线为坐标轴的纵轴，记为X轴；垂直于路径且经过路径起点的直线为坐标轴的横轴，记为Y轴；AUV重心为G，速度向量为U_G，在固定坐标系下X轴和Y轴上投影为AUV受到的外力F在固定坐标系下X轴和Y轴上的投影为F_X,F_Y；受到的外力矩为T；运动坐标系的原点取在AUV重心G处；纵轴取在AUV纵中剖面内，指向船首，记为x轴；横轴与AUV纵中剖面垂直，指向右舷，记为y轴。速度向量为U_G，在固定坐标系下x轴和y轴上投影为u,v；AUV受到的外力F在固定坐标系下x轴和y轴上的投影为F_x,F_y；受到的外力矩为T；因外力矩而获得的角速度为r；

(3)建立如下AUV动力学模型：

在固定坐标系中，AUV的动力学模型为：

式中：φ为AUV速度与固定坐标系的夹角；ψ为X轴与运动坐标系的夹角，V_t为AUV的合速度，且假设u_G恒不为0，定义β为侧滑角，β＝φ-ψ＝tan^-1(v/u)；

在运动坐标系中，水体为理想流体时，AUV动力学模型为：

式中：mur,-mvr为AUV转动时的离心惯性力；N为绕水平面旋转的力矩；F_T为AUV纵向受到水平推力，Γ为垂直水平面方向转艏力矩，F_w为水平面对AUV的阻力，N为垂直水平面方向的力矩，F_w在x轴和y轴上的分力为F_xw,F_yw；m_u；m_vr；m_v；m_ur分别为实际运动中的质量；

(4)构建固定坐标系下的跟随者AUV的误差模型；

(5)设计AUV运动控制器。

2.根据权利要求1所述的一种基于领航者和虚拟领航者的多AUV直线编队控制方法，其特征在于，步骤(2)具体包括：

领航者与跟随者的纵向距离为d，领航者与跟随者的横向距离D；在纵向距离上用基于领航-跟随法的纵向距离误差控制模型，在横向距离采用基于虚拟领航-跟随法的跟随者横向距离误差控制模型；

跟随者纵向距离误差控制模型如下：

在固定坐标系中，AUV_i为领航者，其它AUV为跟随者；AUV_i在固定坐标系下的位姿为(X_i,Y_i,φ_i)；第j个跟随者的坐标为(X_j,Y_j,φ_j)；第j个跟随者与领航者在纵向距离上的理想位姿关系为：

X_i-X_j＝d_j；

Y_j-Y_i＝D_j；

式中：D_j,d_j为跟随者直线路径与领航者直线路径距离长度；

纵向距离误差e_j为：

e_j＝X_i-X_j-d_j；

当时，领航者与跟随者在横向上完成编队控制；

跟随者横向距离误差控制模型如下：

领航者与跟随者在期望横向距离上：

Y_j-Y_i＝D_j；

在固定坐标系中，跟随者到直线Y＝D_j的距离为横向距离误差E_j，横向距离误差E_j为：

E_j＝Y_j-D_j；

φ_ej＝φ_j-φ_sj；

其中，AUV的前向距离为L，φ_j为第j个跟随者的艏向角，φ_sj为期望艏向角，φ_ej为艏向角误差，即横向距离误差。

3.根据权利要求1所述的一种基于领航者和虚拟领航者的多AUV直线编队控制方法，其特征在于，步骤(5)具体包括：

(5.1)设计领航者AUV_i运动控制器：

领航者AUV_i的转艏力矩Γ_i为：

式中：m_ri为AUV在垂直水平面运动的实际质量；N_i为AUV运动时受到其它因素垂直水平面方向的力矩；k_ψ,k_r分别为艏向角，角速度系数；r_i为领航者AUV_i的角速度；AUV_i横向误差为E＝Y_i；领航者AUV_i期望艏向角为领航者AUV_i艏向角误差φ_e＝φ_i-φ_s；

AUV纵向受到的水平推力F_T为：

式中：m_ui为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xwi为AUV运动时会受到其它因素在水平面对AUV的阻力F_wi在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u_i为领航者AUV_i的纵向速度；

(5.2)设计跟随者AUV运动控制器：

第j个跟随者AUV的转艏力矩Γ_j为：

式中：m_rj为第j个跟随者AUV在垂直水平面运动的实际质量；N_j为第j个跟随者AUV运动时受到其它因素垂直水平面方向的力矩；k_ψ,k_r分别为艏向角，角速度系数；r_j为第j个跟随者AUV的角速度；

在纵向上第j个跟随者AUV受到的水平推力F_Tj为：

式中：m_uj第j个跟随者为AUV在沿运动坐标系纵轴方向运动的实际质量；F_xwj为第j个跟随者AUV运动时受到其它因素在水平面的阻力F_wj在固定坐标系沿纵轴的分量；k_u为纵向速度的系数；u_j为第j个跟随者的纵向速度；u_cj为第j个跟随者AUV的期望速度，其中u_cj由下面公式给出：

u_cj＝u_d-g(e_j)；

式中：e_j为第j个跟随者AUV纵向距离误差；其中a满足：