CN114115262A - 基于方位角信息的多auv执行器饱和协同编队控制系统和方法 - Google Patents

Abstract

本发明公开了一种基于方位角信息的多AUV执行器饱和协同编队控制系统和方法，涉及多AUV编队控制技术领域，基于方位角信息的多AUV执行器饱和协同编队控制系统，包括领航者AUV和跟随者AUV，以及感知模块、通信模块、通讯模块、计算处理模块、估计器模块和控制器模块。基于方位角信息的多AUV执行器饱和协同编队控制方法包括：多AUV系统模型的建立；编队控制目标的确定；协同编队控制策略。本发明可适用于多AUV系统，且无需使用AUV的位置信息，更适用于GPS无法使用的水下场景；在满足跟踪者AUV执行器饱和约束的前提下，驱动跟随者AUV实现控制目标。

Description

基于方位角信息的多AUV执行器饱和协同编队控制系统和 方法

技术领域

本发明涉及多AUV编队控制技术领域，尤其涉及一种基于方位角信息的多AUV执行器饱和协同编队控制系统和方法。

背景技术

随着海洋工程技术的发展，水下自主航行器(Autonomous Underwater Vehicle,AUV)在海洋资源探索，海事巡逻以及水下搜救等领域逐渐发挥越来越重要的作用。由于单个AUV可以搭载的硬件设备和储备的能量有限，越来越多的研究者开始关注多AUV的协同编队控制问题。多AUV的协同编队合作可以显著提升完成各项任务的效率，同时能够降低单一AUV的能耗，有效节约成本。现有的多AUV协同编队控制方法多采用领航者-跟随者架构。其中，领航者AUV按照预设的轨迹和速度运动，跟随者AUV则需要通过观测与领航者AUV的相对信息追踪领航者AUV，并与领航者AUV共同实现理想编队形状。

现有的适用于多AUV的协同编队控制方法大多需要使用AUV的绝对位置信息。然而，水下恶劣的无线电通信环境极易导致GPS等设备失效，从而使得全局位置信息无法获取，相应的控制方法也无法使用。此外，现有的适于AUV的协同编队控制方法少有考虑执行器饱和的问题。未考虑执行器饱和的控制方法在实现时易导致AUV的驱动电机转速超过安全阈值，从而产生错误的控制信号甚至导致设备损坏。当前涉及避免执行器饱和的控制方法中，多采用预测控制的开环控制方法，或采用基于数据的强化学习和神经网络逼近的最优控制方法实现。然而，该类方法均难以从理论角度验证其稳定性及可行性。同时，采取现有方法，仅能用于解决较为简单的线性系统一致性问题，对于包含非线性动态的AUV系统的各类复杂的协同编队控制问题则难以解决。

因此，本领域的技术人员致力于开发一种基于方位角信息的多AUV执行器饱和协同编队控制系统和方法。

发明内容

有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是如何克服现有协同编队控制方法中基于AUV的全局位置信息难以在水下环境中获取、现有的协同编队控制方法因未考虑AUV的执行器饱和约束因此无法在工程上应用等不足，提出基于方位角信息设计适用于AUV的协同编队控制系统与方法，并且对方法中所包含算法的增益进行调制，实现在满足AUV执行器饱和约束的前提下，驱动跟随者AUV实现控制目标。

为实现上述目的，本发明提供了一种基于方位角信息的多AUV执行器饱和协同编队控制系统，包括领航者AUV和跟随者AUV，以及感知模块、通信模块、计算处理模块、估计器模块和控制器模块，其中：

跟随者AUV搭载有控制器模块、感知模块、估计器模块和通信模块；

领航者AUV搭载有通信模块；

感知模块用以实现跟随者AUV观测邻近AUV的方位角信息，以及跟随者AUV的自身状态信息；

通信模块用以实现领航者AUV和跟随者AUV之间的通信，以及领航者AUV和跟随者AUV与计算处理模块之间的通信；

计算处理模块用以实现最优增益调制的计算过程；

估计器模块基于所设计的估计算法，用以实现每个跟随者AUV对于领航者AUV的未知动态信息和未知参数信息的估计值的迭代更新；

控制器模块通过有线方式接收来自感知模块和估计器模块的信息，实现控制量的计算，以及对跟随者AUV的执行器发送控制信号控制跟随者AUV进行运动。

本发明还提供了一种基于方位角信息的多AUV执行器饱和协同编队控制方法，该方法包括以下步骤：

步骤S1：建立多AUV系统模型：选定适用于领航者AUV和跟随者AUV的世界坐标系和固联于AUV的本体坐标系，分别建立领航者AUV和跟随者AUV的动力学模型；将每个跟随者AUV实际的执行器饱和限制转化为与跟随者AUV运动状态直接相关的状态变量约束；

步骤S2：确定编队控制目标；

步骤S3：进行协同编队控制。

进一步地，步骤S1具体包括：

步骤S1.1：选定领航者AUV和跟随者AUV的下水位置为基准点，结合经线方向、纬线方向，建立领航者AUV和跟随者AUV共同适用的世界坐标系；

步骤S1.2：选定领航者AUV和跟随者AUV本体上的重心或质心为基准点，结合领航者AUV和跟随者AUV的朝向，建立与其固联的本体坐标系；

步骤S1.3：依据领航者AUV和跟随者AUV的运动学特征，分别建立领航者AUV和跟随者AUV的动力学模型；

步骤S1.4：结合跟随者AUV的执行器结构和动力学特征，确定其执行器饱和所导致的跟随者AUV状态的约束范围。

进一步地，步骤S1.4中，确定约束范围的具体方法为：将跟随者AUV的执行器饱和约束等价转化为关于跟随者AUV速度幅值的约束和控制量幅值的约束。

进一步地，步骤S2具体包括：

(1)确定领航者AUV和跟随者AUV的运动轨迹，满足每对相邻的AUV之间的相对位置与理想编队形状对应的方位角关系；

(2)确定领航者AUV和跟随者AUV的运动速度，达到世界坐标系下的理想运动速度。

进一步地，步骤S3具体包括：

步骤S3.1：基于通信模块建立领航者AUV和跟随者AUV之间的通信拓扑关系，基于感知模块建立领航者AUV和跟随者AUV之间的感知拓扑关系，初始化领航者AUV和跟随者AUV在世界坐标系下的自身状态信息，跟随者AUV对于领航者AUV的未知动态信息和未知参数信息的估计值以及领航者AUV和跟随者AUV之间的方位角信息；其中，领航者AUV的运动轨迹和速度均被设定为预期的运动轨迹和速度；

步骤S3.2：步骤S3.1中得到的领航者AUV和跟随者AUV的初始化信息通过通信模块发送给计算处理模块，实现最优增益调制；

步骤S3.3：将计算处理模块得到的最优增益通过通信模块发送给每个跟随者AUV的估计器模块和控制器模块；

步骤S3.4：将跟随者AUV当前时刻的动态信息估计值通过通信模块发送给与其有通信关系的邻居跟随者AUV的估计器模块；

步骤S3.5：通过估计器模块更新跟随者AUV下一时刻的动态信息估计值；

步骤S3.6：采用感知模块，得到跟随者AUV与其有感知拓扑关系的邻居AUV的方位角信息，以及跟随者AUV的自身运动状态信息；

步骤S3.7：基于步骤S3.5和S3.6中得到的信息，采用估计器模块，实现跟随者AUV对于下一时刻参数信息估计值的迭代更新；

步骤S3.8：基于步骤S3.5-S3.7中得到的信息，采用控制器模块实现控制量的解算，并将控制量作用于跟随者AUV的执行器，从而驱动跟随者AUV的运动；

步骤S3.9：重复步骤S3.2至步骤S3.8直至所设定的控制目标得到实现。

进一步地，步骤S3.1中的最优增益调制方法包括：

步骤S3.1.1：优化问题构建：构建由代价函数、系统动态约束、状态变量约束和控制方法有效性约束构成的优化问题P1；

步骤S3.1.2：约束形式转化：对状态变量约束采用约束近似和局部光滑处理的方法将其转化为易处理的光滑函数形式，由此将优化问题1转化为由代价函数、系统动态约束、控制方法有效性约束和状态变量约束的光滑函数形式组成的优化问题P2；

步骤S3.1.3：增益调制算法：通过对光滑近似参数和不等式近似参数的迭代更新，用优化问题P2的最优解逼近得到优化问题P1的最优解。

进一步地，步骤S3.1.1中的优化问题的构建具体包括以下步骤：

S3.1.1.1：代价函数的确定：代价函数由与协同编队任务相关的速度跟踪误差、编队误差、信息估计误差和控制量组成，将上述要素的凸函数进行加权，即可得到具体的代价函数；

S3.1.1.2：系统动态约束的确定：将所有跟随者AUV系统动态与所设计的控制方法相结合，与所有领航者AUV的动态一起构成多AUV闭环系统，形成优化问题的系统方程约束条件；

S3.1.1.3：状态变量约束的确定：建立每个AUV的执行器饱和带来的关于AUV的状态变量约束；

S3.1.1.4：控制方法有效性约束的确定：引入保证所设计的具体估计算法和控制算法有效性的增益约束；

由此，可得到由步骤S3.1.1.1中的代价函数和步骤S3.1.1.2至步骤S3.1.1.4中的约束条件构成的优化问题P1。

进一步地，步骤S3.1.2中的约束近似和局部光滑处理方法具体包括积分降维、光滑近似和不等式近似三步。

进一步地，步骤S3.1.3的增益调制算法包括以下步骤：

S3.1.3.1：输入待优化的增益变量、光滑近似参数和不等式近似参数的初值，以及光滑近似参数和不等式近似参数的取值下界；

S3.1.3.2：若光滑近似参数的取值大于其下界，则求解优化问题P2得到最优解，并跳至步骤S3.1.3.3；若光滑近似参数的取值小于其下界，则跳至步骤S3.1.3.7；

S3.1.3.3：检验优化问题P2的最优解是否为优化问题P1的可行解，若是，则跳至步骤S3.1.3.4；若否，则跳至步骤S3.1.3.5；

S3.1.3.4：将光滑近似参数和不等式近似参数取值分别更新为原来的十分之一，之后跳至步骤S3.1.3.2；

S3.1.3.5：将不等式近似参数的取值更新为原来的二分之一，并跳至步骤S3.1.3.6；

S3.1.3.6：若不等式近似参数的取值小于其下界，则表明没有找到最优的可行解，修改待优化的增益变量和光滑近似参数和不等式近似参数的初值，并跳至步骤S3.1.3.1；若不等式近似参数的取值大于等于其下界，则跳至步骤S3.1.3.2；

S3.1.3.7：优化问题P1的最优解即为最后一次求解优化问题P2得到的最优解。

本发明提出了基于方位角信息的多AUV执行器饱和协同编队控制方法，该方法可适用于多AUV系统，且无需使用AUV的位置信息，更适用于GPS无法使用的水下场景；本发明通过采用最优增益调制方法对于估计算法和控制算法增益进行了调制，使得AUV的执行器饱和约束得以满足，并使相应的暂态性能指标得到优化；其中，采用约束近似和局部平滑方法将执行器饱和对应的无穷多个不等式约束转化为了有限个具有光滑性质的不等式约束，实现了约束条件的降维和平滑；本发明提出了适用于多AUV系统的执行器饱和协同编队控制系统，该系统可用于实现所设计的协同编队控制方法，在满足跟随者AUV执行器饱和约束的前提下，驱动跟随者AUV实现控制目标。

以下将结合附图对本发明的构思、具体结构及产生的技术效果作进一步说明，以充分地了解本发明的目的、特征和效果。

附图说明

图1是本发明的一个较佳实施例的基于方位角信息的多AUV执行器饱和协同编队控制系统框图；

图2是本发明的一个较佳实施例的AUV符号及坐标系示意图；

图3是本发明的一个较佳实施例的AUV通信拓扑及感知拓扑示意图；

图4是本发明的一个较佳实施例的约束近似和局部光滑法示意图；

图5是本发明的一个较佳实施例的AUV编队追踪过程示意图；

图6是本发明的一个较佳实施例的方位角刚性示意图，其中，图6-1(1)为不满足方位角刚性的感知拓扑示意图，图6-1(2)为满足极小方位角刚性的感知拓扑示意图，图6-1(3)为满足方位角刚性的感知拓扑的一般形式；本实施例中设定的感知拓扑满足极小方位角刚性。

具体实施方式

以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。

在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。

以两个领航者AUV和六个跟随者AUV的立方形编队控制为例。最终实现的编队效果如图5所示。

第一步，协同编队控制系统的构建。

如图1所示为一个较佳实施例的基于方位角信息的多AUV执行器饱和协同编队控制系统框图，该系统包括领航者AUV和跟随者AUV，计算处理模块、控制器模块、感知模块、估计器模块、通信模块1和通信模块2。感知模块感知跟随者AUV的状态信息，以及跟随者AUV与其邻近AUV之间的方位角信息，并通过有线方式发送给估计器模块和控制器模块；估计器模块通过通信模块2接收来自邻近AUV对于领航者AUV的未知动态信息和未知参数信息的估计值，完成对未知信息估计值的更新，并将估计值通过有线方式发送给控制器模块；计算处理模块通过通信模块1接收领航者AUV和跟随者AUV的初始状态信息，并将得到的最优增益通过通信模块1发送给对应的控制器模块和估计器模块；控制器模块通过通信模块1接收来自计算处理模块的最优增益，并通过有线方式接收来自感知模块和估计器模块的信息，之后依据具体控制算法解算控制量，并驱动跟随者AUV进行运动。

第二步，实现基于方位角信息的多AUV执行器饱和协同编队控制，包括以下步骤：

一、多AUV系统模型的建立。

如图2所示，为本发明的一个较佳实施例的AUV符号及坐标系示意图。首先建立与领航者AUV和跟随者AUV固联的本体坐标系O_b-X_bY_bZ_b和世界坐标系O_e-X_eY_eZ_e。本体坐标系O_b-X_bY_bZ_b的原点O_b建立在AUV重心处，O_b-X_b轴与AUV朝向一致，O_b-Y_b轴在横向中剖面内指向右手侧，O_b-Z_b轴垂直于横向中剖面指向下。世界坐标系O_e-X_eY_eZ_e的原点定义在AUV下水位置，O_e-X_e轴平行于经线且指向北，O_e-Y_e轴平行于纬线且指向东，两者置于水平面内，O_e-Z_e轴垂直于水平面指向地心。

之后，建立跟随者AUV的动力学模型如下：

该模型为二阶系统形式，其中η_i表示跟随者AUV i在世界坐标系下的位置和相对世界坐标系的运动方向角，J_i表示跟随者AUV i的本体坐标系与世界坐标系的旋转矩阵，v_i表示跟随者AUV i在本体坐标系下的线速度和角速度，M_i为惯性矩阵，C_i表示向心力矩阵，D_i表示阻尼矩阵，g_i表示重力加速度向量，

为控制输入向量。

然后，建立领航者AUV的动力学模型如下：

其中，J_l表示领航者AUV的本体坐标系与世界坐标系的旋转矩阵，v_l表示领航者AUV在本体坐标系下的线速度和角速度，V_l为领航者AUV构成的集合。

然后，确定跟随者AUV执行器饱和所导致的状态信息约束如下：

其中，v_pi(t)和τ_pi(t)分别代表跟随者AUV i在其本体坐标系下的线速度和控制量，v_max和v_min分别为跟随者AUV在其本体坐标系下的线速度上限和下限，τ_max和τ_min分别为跟随者AUV在其本体坐标系下的控制量上限和下限，V_f为跟随者AUV构成的集合，t₀和t_f分别为初始时刻和终止时刻。

然后，对所有AUV按照先领航者再跟随者的顺序进行编号。在本实例中，两个领航者AUV被分别编为1号和2号，其余跟随者AUV被编为3号至8号。

二、编队控制目标的确定。

设定领航者AUV在其本体坐标系下的线速度v_pl，角速度ω_l，作为所有跟随者AUV要达成的理想速度。同时确定领航者AUV初始时刻姿态角[φ_l，θ_l，ψ_l]^T。之后，确定AUV要达成的理想编队形状为立方体形。设定领航者AUV(即1号和2号AUV)之间的运动轨迹已经满足理想方位角约束：

其余跟随者AUV最终实现的理想运动轨迹满足：

三、协同编队估计算法和控制算法的具体设计。

针对本实例中的跟随者AUV动态模型(11.1)和领航者AUV动态模型(11.2)，本发明设计的估计算法和控制算法具体如下：

领航者AUV在世界坐标系下的线速度满足下式：

u_l＝R_lv_pl

其中R_l为运动方向矩阵，且满足

本实例考虑领航者AUV的运动方向R_l时变，但其在本体坐标系下的线速度v_pl为时不变量。

首先为跟随者AUV设计一致性估计算法实现对于领航者AUV运动方向的一致估计。为了讨论方便，将运动方向矩阵R_l和运动方向角速度ω_l中的元素排列成如下向量形式：

之后，得到运动方向估计算法具体如下式：

其中a_ij表示AUV i与AUV j之间的通信标识，即如果两者可以进行通信有a_ij＞0，否则有a_ij＝0；n表示所有参与协同编队任务的AUV总数；

表示AUV i对于领航者AUV运动方向向量β_l的估计值；k_βi＞0为估计器增益。然后，为跟随者设计基于自适应原理的线速度参数估计算法实现对于领航者AUV在本体坐标系下的线速度估计。领航者AUV线速度参数估计算法具体如下式：

其中

为AUV i对领航者运动方向矩阵R_l的估计，N_i为AUV i的邻居集合，

为AUV i与AUV j的方位角，

为理想编队形状对应的方位角。

最后，基于领航者AUV运动方向估计值和其在本体坐标系下的线速度估计值设计控制算法的形式具体如下：

其中k_vi＞0，k_p＞0是控制算法增益，ξ_i包含了AUV i所有的非线性动态，M_1i表示AUVi的质量，R_i表示AUV i的运动方向矩阵，其形式为

其中φ_i，θ_i，ψ_i为AUV i相对世界坐标系的运动方向角。

为运动方向矩阵R_i的导数。

四、协同编队控制。

具体的，协同编队控制过程共分为以下几步：

1、基于通信模块和感知模块，确定跟随者AUV的通信拓扑和观测拓扑如图3所示。初始化所有跟随者AUV的状态信息，跟随者AUV对于未知动态信息β_l的估计值，跟随者AUV对于参数v_pl的估计值，以及领航者AUV和跟随者AUV之间的方位角信息。

2、步骤1中得到的领航者AUV和跟随者AUV的初始化信息通过通信模块发送给计算处理模块，实现最优增益调制；

实现最优增益调制具体需执行以下步骤：

(1)优化问题构建：

1)代价函数的确定。根据编队任务确定需要优化的代价函数。在本发明适用的任务场景中，代价函数可由方向估计误差、编队方位误差、线速度跟踪误差和控制量的二次型构成，因此确定如下所示的代价函数：

2)系统动态约束的确定。构造领航者AUV和跟随者AUV系统动态(11.2)，(11.1)与估计算法(11.4)，(11.5)和控制算法(11.6)结合后形成的闭环系统，得到优化问题的动态方程约束条件，其具体形式如下所示：

3)状态变量约束的确定。考虑每个AUV的执行器饱和所带来的关于AUV状态的约束(11.3)。

4)控制方法有效性约束条件的确定。引入保证估计算法(11.4)和控制算法(11.6)有效性的不等式约束，其具体形式如下：

k＞0 (11.9)

其中k＝[...，k_βi，...k_vi，...，k_p]为所有跟随者AUV的估计算法和控制算法的增益。

由此，可建立由代价函数(11.7)和约束条件(11.3)(11.8)(11.9)组成的优化问题P1。

(2)状态变量约束的形式转化：

约束C2在连续时间段[t₀，t_f]内包含无穷多个不等式，需要对其进行转化得到有限个易处理的光滑函数。本发明采用约束近似和局部光滑方法对形如C2的执行器饱和约束不等式进行转化，具体处理过程分为以下三步：

1)积分降维。通过对不等式(11.3)进行以下积分形式的转换

可将无穷多个不等式转化为以上等式约束。

2)光滑近似。如附图4所示，第(1)步中得到的有限个等式在原点处不具有光滑性质，因此需对上述等式采用局部平滑方法进行处理，即当函数

处于原点附近的区间[-∈，∈]内时，将其用光滑的二次函数曲线近似，具体操作如下：

其中，光滑近似参数∈的具体取值将在求解优化问题P1时确定。

3)不等式近似。最后，通过引入参数δ将上述等式约束转化为如下易处理的不等式约束

不等式近似参数δ的取值也将在求解优化问题P1时确定。

由此，优化问题P1可被转化为由代价函数(11.7)和约束条件(11.8)-(11.10)组成的优化问题2。

(3)优化问题P1的求解：

然后，对于优化问题P2，我们需要求解代价函数(11.7)和约束条件(11.8)，(11.9)及(11.10)对于增益变量和AUV动态变量的梯度值。最后，将第一步中得到的领航者AUV和跟随者AUV初始变量信息导入，并采用最优增益调制算法确定每个跟随者AUV的估计算法和控制算法增益。算法的具体步骤如下：

1)输入：待优化增益的初值k₀，光滑近似参数∈和不等式近似参数δ的初值∈₀，δ₀以及最小值∈_min和δ_min。

2)若∈＞∈_min，则求解优化问题P2得到最优解k^*，并跳至第3)步；若∈≤∈_min，则跳至第7)步。

3)检验优化问题P2的最优解k^*是否为优化问题P1的可行解，若可行，则跳至第4)步；若不可行则跳至第5)步。

4)对参数∈和δ进行更新，即∈＝∈/10，δ＝δ/10，之后跳至第2)步。

5)对参数δ进行更新，即δ＝δ/2，并跳至第6)步

6)若δ＜δ_min，则表明没有找到最优的可行解，修改初值k₀，∈₀，δ₀并跳至第1)步；若δ≥δ_min，则跳至第2)步。

7)输出：优化问题P1的最优解即为最后一次求解优化问题P2得到的最优解k^*。

3、采用通信模块，将计算处理模块得到的最优增益发送给每个跟随者AUV的估计器模块和控制器模块。

4、将跟随者AUV当前时刻的动态信息估计值

通过通信模块发送给与其有通信关系的邻居跟随者AUV的估计器模块。

5、采用估计器模块，更新跟随者AUV下一时刻的动态信息估计值。更新方式遵循估计算法(11.4)式。

6、采用感知模块，实现每个跟随者AUV与其有感知拓扑关系的邻居AUV的方位角信息的观测，以及每个跟随者AUV对于自身的运动状态信息的观测。每个跟随者AUV的运动状态信息包括其运动方向矩阵R_i，本体坐标系下的线速度v_pi，以及AUV在水中所受阻力、重力等非线性信息ξ_i。

7、基于第5步和第6步中得到的信息，采用估计器模块，实现对于跟随者AUV下一时刻参数信息估计值

的迭代更新。更新方式遵循参数估计算法(11.5)。

8、基于第5步至第7步中得到的信息，采用控制器模块实现控制量的解算，并将控制量作用于跟随者AUV的执行器，从而驱动跟随者AUV的运动。控制量的解算方式遵循控制算法(11.6)的形式。

9、重复2-8步直至所设定的控制目标得到实现。

以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (10)

1.一种基于方位角信息的多AUV执行器饱和协同编队控制系统，其特征在于，包括领航者AUV和跟随者AUV，以及感知模块、通信模块、计算处理模块、估计器模块和控制器模块，其中：

所述跟随者AUV搭载有所述控制器模块、所述感知模块、所述估计器模块和所述通信模块；

所述领航者AUV搭载有所述通信模块；

所述感知模块用以实现所述跟随者AUV观测邻近AUV的方位角信息，以及所述跟随者AUV的自身状态信息；

所述通信模块用以实现所述领航者AUV和所述跟随者AUV之间的通信，以及所述领航者AUV和所述跟随者AUV与所述计算处理模块之间的通信；

所述计算处理模块用以实现控制器最优增益调制的计算过程；

所述估计器模块基于所设计的估计算法，用以实现每个所述跟随者AUV对于所述领航者AUV的未知动态信息和未知参数信息的估计值的迭代更新；

所述控制器模块通过有线方式接收来自所述感知模块和所述估计器模块的信息，实现控制量的计算，以及对所述跟随者AUV的执行器发送控制信号控制所述跟随者AUV进行运动。

2.一种基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述方法包括以下步骤：

步骤S1：建立多AUV系统模型：选定适用于所有AUV的世界坐标系和固联于AUV的本体坐标系，分别建立领航者AUV和跟随者AUV的动力学模型；将每个所述跟随者AUV实际的执行器饱和限制转化为与所述跟随者AUV运动状态直接相关的状态变量约束；

步骤S2：确定编队控制目标；

步骤S3：进行协同编队控制。

3.根据权利要求2所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S1具体包括：

步骤S1.1：选定所述领航者AUV和所述跟随者AUV的下水位置为基准点，结合经线方向、纬线方向，建立所述领航者AUV和所述跟随者AUV共同适用的世界坐标系；

步骤S1.2：选定所述领航者AUV和所述跟随者AUV本体上的重心或质心为基准点，结合所述领航者AUV和所述跟随者AUV的朝向，建立与其固联的本体坐标系；

步骤S1.3：依据所述领航者AUV和所述跟随者AUV的运动学特征，分别建立所述领航者AUV和所述跟随者AUV的动力学模型；

步骤S1.4：结合所述跟随者AUV的执行器结构和动力学特征，确定其执行器饱和所导致的所述跟随者AUV状态的约束范围。

4.根据权利要求3所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S1.4中，确定所述约束范围的具体方法为：将所述跟随者AUV的执行器饱和约束等价转化为关于所述跟随者AUV速度幅值的约束和控制量幅值的约束。

5.根据权利要求2所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S2的编队控制目标具体包括：

(1)确定所述领航者AUV和所述跟随者AUV的运动轨迹，满足每对相邻的AUV之间的相对位置与理想编队形状对应的方位角关系；

(2)确定所述领航者AUV和所述跟随者AUV的运动速度，达到所述世界坐标系下的理想运动速度。

6.根据权利要求2所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S3具体包括：

步骤S3.1：基于所述通信模块建立所述领航者AUV和所述跟随者AUV之间的通信拓扑关系，基于所述感知模块建立所述领航者AUV和所述跟随者AUV之间的感知拓扑关系，初始化所述领航者AUV和所述跟随者AUV在世界坐标系下的自身状态信息，所述跟随者AUV对于所述领航者AUV的未知动态信息和未知参数信息的估计值以及所述领航者AUV和所述跟随者AUV之间的方位角信息，其中，所述领航者AUV的运动轨迹和速度均被设定为预期的运动轨迹和速度；

步骤S3.2：步骤S3.1中得到的所述领航者AUV和所述跟随者AUV的初始化信息通过通信模块发送给计算处理模块，实现最优增益调制；

步骤S3.3：将所述计算处理模块得到的最优增益通过所述通信模块发送给每个所述跟随者AUV的估计器模块和控制器模块；

步骤S3.4：将所述跟随者AUV当前时刻的动态信息估计值通过所述通信模块发送给与其有通信关系的邻居跟随者AUV的所述估计器模块；

步骤S3.5：通过所述估计器模块更新所述跟随者AUV下一时刻的动态信息估计值；

步骤S3.6：采用所述感知模块，得到所述跟随者AUV与其有感知拓扑关系的邻居跟随者AUV的方位角信息，以及所述跟随者AUV的自身运动状态信息；

步骤S3.7：基于步骤S3.5和S3.6中得到的信息，采用所述估计器模块，实现所述跟随者AUV对于下一时刻参数信息估计值的迭代更新；

步骤S3.8：基于步骤S3.5-S3.7中得到的信息，采用所述控制器模块实现控制量的解算，并将控制量作用于所述跟随者AUV的执行器，从而驱动所述跟随者AUV的运动；

步骤S3.9：重复所述步骤S3.2至所述步骤S3.8直至所述编队控制目标得到实现。

7.根据权利要求6所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S3.1中的最优增益调制方法包括：

步骤S3.1.1：优化问题构建：构建由代价函数、系统动态约束、状态变量约束和控制方法有效性约束构成的优化问题P1；

步骤S3.1.2：约束形式转化：对状态变量约束采用约束近似和局部光滑处理的方法将其转化为易处理的光滑函数形式，由此将所述优化问题P1转化为由代价函数、系统动态约束、控制方法有效性约束和状态变量约束的光滑函数形式组成的优化问题P2；

步骤S3.1.3：增益调制算法：通过对光滑近似参数和不等式近似参数的迭代更新，用所述优化问题P2的最优解逼近得到所述优化问题P1的最优解。

8.根据权利要求7所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S3.1.1中的优化问题的构建具体包括以下步骤：

S3.1.1.1：代价函数的确定：代价函数由与协同编队任务相关的速度跟踪误差、编队误差、信息估计误差和控制量组成，将上述要素的凸函数进行加权，即可得到具体的代价函数；

S3.1.1.2：系统动态约束的确定：将所有跟随者AUV系统动态与所设计的控制方法相结合，与所有领航者AUV的动态一起构成多AUV闭环系统，形成优化问题的系统方程约束条件；

S3.1.1.3：状态变量约束的确定：建立每个所述跟随者AUV的执行器饱和带来的关于所述跟随者AUV的状态变量约束；

S3.1.1.4：控制方法有效性约束的确定：引入保证所设计的具体估计算法和控制算法有效性的增益约束；

由此，可得到由步骤S3.1.1.1中的代价函数和步骤S3.1.1.2至步骤S3.1.1.4中的约束条件构成的所述优化问题P1。

9.根据权利要求7所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S3.1.2中的约束近似和局部光滑处理方法具体包括积分降维、光滑近似和不等式近似三步。

10.根据权利要求7所述的基于方位角信息的多AUV执行器饱和协同编队控制方法，其特征在于，所述步骤S3.1.3的增益调制算法包括以下步骤：

S3.1.3.1：输入待优化的增益变量、光滑近似参数和不等式近似参数的初值，以及所述光滑近似参数和所述不等式近似参数的取值下界；

S3.1.3.2：若所述光滑近似参数的取值大于其下界，则求解优化问题P2得到最优解，并跳至步骤S3.1.3.3；若所述光滑近似参数的取值小于其下界，则跳至步骤S3.1.3.7；

S3.1.3.3：检验优化问题P2的最优解是否为优化问题P1的可行解，若是，则跳至步骤S3.1.3.4；若否，则跳至步骤S3.1.3.5；

S3.1.3.4：将所述光滑近似参数和所述不等式近似参数取值分别更新为原来的十分之一，之后跳至步骤S3.1.3.2；

S3.1.3.5：将所述不等式近似参数的取值更新为原来的二分之一，并跳至步骤S3.1.3.6；

S3.1.3.6：若所述不等式近似参数的取值小于其下界，则表明没有找到最优的可行解，修改所述待优化的增益变量、所述光滑近似参数和所述不等式近似参数的初值，并跳至步骤S3.1.3.1；若所述不等式近似参数的取值大于等于其下界，则跳至步骤S3.1.3.2；

S3.1.3.7：所述优化问题P1的最优解即为最后一次求解所述优化问题P2得到的最优解。

Images