一种基于粘滞阻尼振荡模型的多AUV编队控制方法与流程

本发明涉及水下机器人技术领域，具体的说是一种基于粘滞阻尼振荡模型的多AUV编队控制方法。

背景技术：

AUV协同作业是当前水下机器人领域的研究热点。相比于单台AUV，多AUV协同作业的系统可以实现任务分解，从而覆盖较广泛的空间，作业能力更强，在如海底资源勘测、海底地形探测以及海洋作战领域都有广泛应用前景。

AUV编队控制是AUV协同作业的一个基础课题，很多AUV协同作业任务是在AUV编队的基础上完成的。受到水下环境的限制，AUV编队所需要的信息，如其他载体的位置、航速等信息，以及AUV之间的相对方向信息较难获得，并且获得这些信息的延时往往较高，且可靠性较差。但AUV之间的距离信息比较容易获得，且延迟较短，可靠性较高。因此可以基于测距信息完成多AUV之间的编队控制。

技术实现要素：

为使AUV能够在通信受限(载体间可以频繁周期性测距，但不能频繁发送自身航速航向信息)的条件下能够形成期望队形，并维持期望队形航行，本发明要解决的技术问题是借鉴粘滞阻尼振荡模型这一物理模型，提供一种基于粘滞阻尼振荡模型的多AUV编队控制方法。

本发明为实现上述目的所采用的技术方案是：一种基于粘滞阻尼振荡模型的多AUV编队控制方法，包括以下步骤：

初始化编队中的AUV；

利用水声通信机的测距功能测量AUV之间的距离信息，AUV中的领航者向跟随者广播自身速度和方向信息；

AUV根据测距信息完成对相对位置信息的推算和相对距离信息的推算

根据粘滞阻尼振荡模型建立多AUV编队的弹簧阻尼质量块模型，给出AUV 中的跟随者完成编队或维持队形所需要的期望航速和期望航向，映射为主推电机和舵电机的控制信号，控制跟随者完成编队。

所述初始化编队中的AUV，包括以下步骤：

编队中的三个AUV，在初始时刻指定其中一个为领航者L，另外两个为跟随者F₁和F₂；

三个AUV在初始时刻上浮至海面，使用所搭载的GPS获得各自位置信息；

领航者L使用所搭载的无线电通信设备向跟随者F₁、F₂发送自身位置和航速航向信息，两个跟随者使用所搭载的无线电通信设备互相发送各自的位置信息；

跟随者根据获得的领航者位置信息和另一个跟随者的位置信息，给自身的载体方向信息估计器和距离信息估计器赋初值；

三个AUV下潜至同一深度开始定深航行，此时领航者向两个跟随者广播一次自身的航速和航向信息。

所述利用水声通信机的测距功能测量AUV之间的距离信息，具体为：两个跟随者使用水声通信机的测距功能，依次向领航者和另一个跟随者发出测距信号，更新距离领航者和另一跟随者的距离信息。

所述利用水声通信机的测距功能测量AUV之间的距离信息，包括循环的以下步骤：

跟随者F₁向领航者L发出测距信号，获得距离信息|F₁L|；

跟随者F₁向跟随者F₂发出测距信号，获得距离信息|F₁F₂|；

跟随者F₂向领航者L发出测距信号，获得距离信息|F₂L|；

跟随者F₂向跟随者F₁发出测距信号，获得距离信息|F₂F₁|。

所述AUV中的领航者向跟随者广播自身速度和方向信息，具体为：

领航者每隔一定时间向跟随者广播一次自身航速和航向信息；或，

领航者在跟踪的路径点发生变化的情况下才广播自身航速和航向信息。

所述AUV根据测距信息和方向信息给出跟随者完成编队或维持编队需要的期望航速和期望航向，具体为：

经过时间Δt后，下一周期跟随者F_i的期望航速为向量的模值，期望航向为向量的方向角，且向量满足：

式中，为跟随者F_i的速度，虚拟加速度由虚拟弹簧的弹力产生，跟随者F_i受到虚拟弹簧F₁F₂和F_iL产生合力的作用，以及阻尼介质所产生阻力的作用，分别为：

式中，k为弹簧的弹性系数，c为阻尼系数，|DD|为两跟随者之间期望距离，D_iL为代表跟随者F_i(i＝1,2)的质量块与代表领航者L的质量块之间的期望距离， 为阻尼介质与质量块的相对速度，因为阻尼介质与领航者速度同为v_l，

因此质量块所受到的合力为：

由此可知：

所述AUV根据测距信息完成对相对距离信息的推算，具体为：

其中，|F₁F₂|_t+Δt为t+Δt时刻两跟随者之间的距离，|F₁F₂|_t为t时刻两跟随者之间的距离，Δ|F₁F₂|_t为t时刻两跟随者之间实际距离与期望距离之差，|F_iL|_t+Δt为t+Δt时刻跟随者F_i(i＝1,2)与领航者L之间的距离，|F_iL|_t为t时刻跟随者F_i(i＝1,2)与领航者L之间的距离，Δ|F_iL|_t为t时刻跟随者F_i(i＝1,2)与领航者L之间实际距离与期望距离之差，Δt为经过的时间，k采用经验值，取0.1-0.5之间的某数。

所述粘滞阻尼振荡模型为：认为AUV间距离符合期望时，虚拟弹簧不发生 形变；从能量角度，该由质量块，弹簧和阻尼器构成的模型在初始时刻所具有的全部能量为质量块的初始动能和形变弹簧所储存的弹性势能，整个振动过程中阻尼器对质量块做负功，消耗能量，最终整个模型所具有的弹性势能和动能均变为零，即收敛于弹簧不发生弹性形变的状态。

所述多AUV编队的弹簧阻尼质量块模型为：认为编队中的AUV为由弹簧连接的质量块，阻尼由与领航者同速度的阻尼介质产生。规定弹簧不发生弹性形变时编队队形为期望队形，则质量块在弹簧弹力和阻力的作用下发生振荡，并最终收敛于期望队形。

本发明具有以下优点及有益效果：

1、本方法依据测距信息完成编队，所需通信信息量少，适合于通信限制条件下的多AUV编队控制。

2、本方法所研究的编队控制规则是模拟粘滞阻尼振荡模型这一成熟的物理模型得到的，控制规则中的参量可以根据粘滞阻尼振荡模型的特征进行选择，而不需要经过多次实验取用经验值，容易确定比较合理的参数。

附图说明

图1是本发明的编队示意图；

图2是本发明的工作流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

本发明由三台AUV、运行与AUV控制计算机中的航向控制器和载体间距离、方向估计器，以及AUV上搭载的GPS和导航系统，无线电通信设备和水声通信机等设备组成，具体需要如下设备：

有测距功能的水声通信机：安装于AUV上，用于水下机器人间周期性测距，和非周期性地发送自身航速、航向信息。

控制计算机：用于实时监控AUV位姿信息，推算AUV间相对方向信息，实时推算AUV间距离信息和方向信息，并根据AUV间距离信息和方向信息控 制给出AUV期望航速及期望航向。将AUV期望航速和期望航向映射为推进器和舵的控制信号。

推进器、舵等航行动力设备：驱动AUV运动以完成编队。

位姿传感器：获得AUV自身航速、航向信息

GPS：在初始时刻所有AUV均在水面上的时候，获得AUV的位置信息

无线电通信设备：在初始时刻AUV均没有下潜的时候，向其他AUV发送各自的位置信息。

队形及相关物理量的定义如图1所示。

编队控制过程如图2所示，初始时刻三台AUV在水面完成准备工作，然后AUV下潜至同一深度定深航行。AUV定深航行后使用基于粘滞阻尼模型的编队控制规则控制载体完成编队。载体使用水声通信机的测距功能周期性测距，并估算载体间实时方向信息和距离信息，依据此信息根据编队控制规则得到完成编队所需的期望航速和期望航向。将期望航速、航向映射为主推电机和舵电机的控制信息后完成编队。

第1步：完成准备工作

编队中的三个AUV，在初始时刻指定其中一个为领航者，另外两个为跟随者。

三个AUV在初始时刻上浮至海面，使用所搭载的GPS获得各自位置信息；

领航者使用所搭载的无线电通信设备向跟随者发送自身位置和航速航向信息，两个跟随者使用所搭载的无线电通信设备互相发送各自的位置信息；

跟随者根据获得的领航者位置信息和另一个跟随者的位置信息，给自身的载体方向信息估计器和距离信息估计器赋初值；

三个AUV下潜至同一深度开始定深航行，此时领航者向两个跟随者广播一次自身的航速和航向信息。

第2步：完成测距工作

两个跟随者使用水声通信机的测距功能，依次向领航者和另一个跟随者发出测距信号，更新距离领航者和另一跟随者的距离信息。具体包括四个步骤，第 一步，跟随者F₁测量距离|F₁L|；第二步，跟随者F₁测量距离|F₁F₂|；第三步，跟随者F₂测量距离|F₂L|；第四步，跟随者F₂测量距离|F₂F₁|。以上四步为一个周期，每个周期耗时约20s，循环此过程。

领航者每5分钟广播一次自身航速和航向信号，或者只在路径点变化的情况下广播一次自身航速和航向信息。

第3步：航向控制计算机根据距离信息和方向信息给出编队需要的期望航速和期望航向，若控制算法的调用周期为Δt，则下一周期跟随者F_i的期望航速为：

式中，为跟随者F_i的速度，虚拟加速度由虚拟弹簧的弹力产生，分析受力，跟随者F_i受到虚拟弹簧FF和FL_i产生合力的作用，以及阻尼介质所产生阻力的作用，分别为：

式中，k为弹簧的弹性系数，c为阻尼系数。为阻尼介质与质量块的相对速度，因为阻尼介质与领航者速度同为v_l，所以

因此质量块所受到的合力为：

由此可知：

第4步：通过估算更新航向信息和位置信息

由于跟随者F_i只能测得距离信息|F₁F₂|和|F_iL|，而不能测得所需的方向向量和所以需要设计方向估计器。在初始时刻，跟随者可以获得两个跟随者的位置P_Fi(i＝1,2)和领航者的位置P_L，据此可以得到下一周期所有载体的期望速度，进而得到其新的位置，重复此过程得到每个周期所有载体位置的估计值：

根据推算的位置信息计算单位向量:

式中，P_Fi表示需要估算方向信息的跟随者的位置，表示另一跟随者的位置。

方向估计器是一个开环的估计器，误差会累加。一种解决方案是距离较长时间后，载体广播自身位置信息，对估计器进行修正。由于这种控制器本身对方向信息精度要求不高，这种估计器可以满足要求。

由于声信号在水下传播存在延迟，无法提供实时的距离信息，因此需要设计距离估计器，根据跟随者速度另一跟随者期望速度和领航者速度得到下一周期距离估计值：

由于载体可以周期性地测得距离信息，可以保证方向估计器信息的准确性。

所述AUV根据推算得到的相对距离信息和相对位置信息，给出完成或维持队形所需的期望航速和期望航向，具体为：

首先将粘滞阻尼振荡模型应用于基于测距的多水下机器人编队控制中，认为载体间距离符合期望时，虚拟弹簧不发生形变。从能量角度分析该模型，在初始时刻，系统所具有的能量为质量块的初始动能和形变弹簧所储存的弹性势能，整个振动过程中阻尼器对质量块做负功，消耗能量，最终系统的弹性势能和动能均变为零，即收敛于弹簧不发生弹性形变的状态。在此模型中，质量块所受弹簧弹力和阻尼器阻力合力的作用，为：

F＝kx+cv (7)

该系统运动学方程为：

式中，k为弹簧弹性系数，x为质量为m的质量块的位移，c为阻尼器的阻尼系数，v为质量块的速度。

依据此模型，建立多AUV编队的弹簧阻尼质量块模型。即认为编队中的载体为由弹簧连接的质量块，阻尼由与领航者同速度的阻尼介质产生。规定弹簧不发生弹性形变时编队队形为期望队形，则质量块在弹簧弹力和阻力的作用下发生振荡，并最终收敛于期望队形。

进而分析受力，速度为v_fi的跟随者F_i受到弹簧FF和FL_i产生合力的作用，以及阻尼介质所产生阻力的作用，分别为：

式中，k为弹簧的弹性系数，c为阻尼系数。为阻尼介质与质量块的相对速度，因为阻尼介质与领航者速度同为v_l，所以

因此质量块所受到的合力为：

若控制算法的调用周期为Δt，则下一周期跟随者的期望速度为：

最后确定编队控制方法中所用到的参数：控制器的设计模拟弹簧阻尼质量块模型，因此控制器中的量均有确定的物理意义。根据物理模型可以确定其对编队的影响，并通过计算得到参数，而无需选用经验值。上述控制器中需要确定的参数有质量块的质量m，弹簧的弹性系数k和阻尼系数c。质量m对系统性能的影响可以通过调整另外两个参数k和c反映，因此取m＝1以简化分析，此时对模型进行分析，沿着弹簧FL_i和FF的振动为：

ΣM_FLi＝M_FLi+M_FF·cosθ_i (16)

ΣM_FF＝M_FF+M_FL1·cosθ₁+M_FL2·cosθ₂ (17)

M_FLi为由弹簧FL_i所产生的振动，M_FF为由弹簧FF所产生的振动。分析M_FLi，领航者不受虚拟弹力驱动，视为静止，则跟随者沿FL_i的受力为：

符合经典粘滞阻尼模型，由于m＝1，故：

由此得到两个参数固有频率和阻尼比分别为

以及运动方程的解

x(t)＝e^-ct/2f(·) (22)

当ξ＜1时，系统为欠阻尼状态，振荡收敛，当ξ≥1时，系统为临界阻尼或过阻尼状态，缓慢但平滑收敛。由系统上层控制器给出的控制信号应尽量平滑，以方便下次控制器调节，因此应保持临界阻尼甚至欠阻尼状态，即参数满足：

同样分析M_FF，运动方程为：

运动方程的解为：

x(t)＝e^-tc/2f(·) (25)

振荡为临界阻尼甚至过阻尼时参数应满足

由此可知，振动M_FLi和M_FF收敛速度一致，说明沿各弹簧的振动ΣM_FLi和ΣM_FF收敛速度不受叠加影响，且仅与c有关。但ΣM_FLi和ΣM_FF的收敛形式受到叠加的影响。当分量均为欠阻尼状态时，输出平滑，因此k,c应同时满足：

。