CN117389312A - 一种基于模型的水下航行器抗衡滚三维跟踪控制方法 - Google Patents

Abstract

本发明属于水下航行器控制方法技术领域，具体涉及一种基于模型的水下航行器抗衡滚三维跟踪控制方法。包括如下步骤，建立水下航行器运动模型、坐标系，定义运动向量、建立运动学模型和动力学模型；基于非线性复合LOS制导律的运动学控制律，建立跟踪位置误差模型、三维LOS制导律、运动学控制律；非线性饱和约束下的艉舵控制分配器，分别建立虚拟速度误差模型、自适应快速终端滑模控制律、动力学控制律；设计基于抗饱和非线性控制分配器，建立优化目标函数，优化求解舵角控制指令；本申请的基于模型的水下航行器抗衡滚三维跟踪控制方法，在不同初始姿态下都产生较好的跟踪效果，能够同时控制水下航行器的横滚、俯仰和偏航姿态。

Description

一种基于模型的水下航行器抗衡滚三维跟踪控制方法

技术领域

本发明属于水下航行器控制方法技术领域，具体涉及一种基于模型的水下航行器抗衡滚三维跟踪控制方法。

技术背景

水下航行器的价值主要体现在海洋学调查、水文和气象调查、海洋环境监视和测量、海底地形探察和测绘、情报/监视/侦察、反水雷、反潜战等军事用途，水下航行器的控制精度是完成使命任务的基础技术、核心技术，特别是对于机动目标跟踪、时敏目标打击、集群协同编队等跟踪控制任务。在控制精度方面，专利CN116300982A公开了水下航行器及其路径跟踪控制方法和装置，将水下航行器的非零时变横滚角、攻角和侧滑角所引起的非线性干扰作为水下航行器的运动学不确定项，并预估该运动学不确定项；从而提高了水下航行器路径跟踪控制精度，但是该方法只进行了仿真验证。

相比较于十字型舵水下航行器，X字型舵水下航行器具有安全性高、机动性好、可靠性高、舵效高、噪声低等特点，因此X字型舵水下航行器已经被越来越多国家的海军和科研单位所应用，其任意X舵操纵面运动都会对其横滚、纵倾和偏航产生影响，虽然X字型舵水下航行器操纵特性复杂，但是极大提高了机动能力、操纵能力、容错能力等。针对X舵控制，专利CN108776428A公开了一种自主水下航行器四通道耦合控制方法，根据传感器数据分别进行纵平面控制计算、水平面控制计算和横滚控制计算，随后进行舵角分配计算，将控制力矩分配到四个舵面上，得到操舵角度，但是所提出的控制器本质上采用的是PID算法。

发明内容

本发明的目的在于，基于X型舵水下航行器的运动模型，提供一种三维跟踪控制方法，能够同时镇定横滚误差、俯仰误差和偏航误差，通过该控制方法计算相应执行机构指令，控制水下航行器稳定航行和有效作业。控制方法主要包括：速度控制算法、姿态控制算法、控制分配算法。

为实现上述目的，本发明采用如下技术方案。

一种基于模型的水下航行器抗衡滚三维跟踪控制方法，包括如下步骤

步骤一、建立水下航行器运动模型

分别建立惯性坐标系{I}，随体坐标系{B}，Serret-Frenet坐标系{F}和和速度坐标系{V}；定义水下航行器运动向量，包括：

在{I}坐标系下的姿态向量η＝[P,Θ]^·，其中P＝[x,y,z]^·为位置向量，Θ＝[φ,θ,ψ]^·为姿态向量；在{B}坐标系下的线速度v＝[u,v,w]^·和角速度ω＝[p,q,r]^·；在{V}坐标系下的合速度向量U_V＝[U,0,0]^·，其中

基于刚体的牛顿欧拉公式和拉格朗日公式，建立环形器运动学和动力学模型：

其中，M表示附加质量矩阵，C(v)为科里奥利和向心矩阵，D(v)为阻尼矩阵，g(η)为重力和浮力矢量，t为输入控制力和力矩，d为时变的未知扰动，J(η)为η和ν的雅可比旋转矩阵；

建立三维空间中的六自由度运动学模型和动力学模型；其中六自由度运动学模型为：

动力学模型为：

其中，

其中，m_ii(i＝1,2,3,4,5,6)为附加质量，X_(×)，Y_(×)，Z_(·)，K_(×)，M_(·)和N_(·)为水动力系数；对前述基于模型的水下航行器抗衡滚三维跟踪控制方法的进一步完善或优化，所述水动力系数采用经验公式法、计算流体力学CFD仿真模拟法、模型辨识法、约束船模实验法以及实船试验法来估计或测量。

步骤二、基于非线性复合LOS制导律的运动学控制律

为了镇定水下航行器的姿态误差，建立控制器位置误差：

其中旋转矩阵为表示从{F}坐标系到{I}坐标系，绕{I}坐标系的z^I轴旋转再绕{I}坐标系的x^I轴旋转得到{F}坐标系；

求导得式中，U_d＝[U_d,0,0]^·为期望合速度向量，且设χ_e和υ_e为LOS制导律的前视角，则跟踪的位置误差模型为：

设计正定的李雅普诺夫函数，并对其求导为：

为得到负定的公式，设计三维跟踪制导律如下：

其中，k_s>0，k_e>0和k_h>0是制导律的增益系数，前视距离Δ_e>0，Δ_h>0；联立后得到：

设水下航行器姿态误差和目标姿态为Θ_e＝[φ_e,θ_e,ψ_e]^·和Θ_e＝Θ-Θ_d，真实环境中的水动力角为[α,β]，航向角为[υ,χ]，采用坐标系的等价关系转换推导期望的姿态角[φ,θ,ψ]，其中攻角为α＝arctan(w/u)，漂角为则期望姿态角与水动力角和期望航向角的关系为：

其中，

为镇定水下航行器姿态误差，设正定李雅普诺夫函数如下：

V₂＝(1-cosφ_e)+(1-cosθ_e)+(1-cosψ_e)

得到基于非线性复合LOS制导律的运动学控制律：

所述步骤二中，还包括基于自适应快速积分终端滑模控制的动力学控制律设计的步骤，具体包括：

S1、采用一阶虚速度滤波避免微分爆炸，所述滤波器为滤波误差为用于跟踪的虚速度误差为

联立后有

S2、优化动力学控制率来稳定虚速度误差；采用自适应快速终端滑模控制器，设计滑模面如下：

其中，α_u>0和β_u>0为待设计常数，q_u和p_u为正奇数，且0<q_u/p_u<1；

所述动力学控制率如下：

其中，(·)^*为标称的水动力系数，和为未知环境扰动的估计值，sat()为辅助控制率的饱和函数；

对于纵向控制律，设计正定的李雅普诺夫函数为

基于水下航行器在环境中的参数摄动边界为±20％，有

令其中ζ_u>0，为参数摄动的边界，通过滑模控制进行自适应调整；

基于上述设定ζ_p，ζ_q和ζ_r可表示为：

通过李雅普诺夫函数求导，化简后得到：

得到自适应扰动控制律为

步骤三、非线性饱和约束下的艉舵控制分配器

基于步骤二得到期望的力和力矩，通过舵角对力和力矩进行解算，考虑控制精度、执行器饱和约束、能量消耗的优化求解，建立优化目标函数如下

其中，ε表示消耗能量和控制精度的权重参数，s为力或力矩控制误差，δ_max和δ′_max为水下航行器的最大舵角和最大转舵速率；

为了求解上述最优目标函数，采用序列二次规划算法，能够完成舵角控制分配，SQP算法具体介绍如下：

任意的非线性问题都能被线性化为一个二次规划问题，具体为：

其中，E和I分别为等式集的约束和不等式的约束，上式具有一个特殊解(p_k,l_k)且满足如下条件：

通过牛顿迭代法，KKT矩阵为：

使用阻尼BFGS方法近似估计Hessian矩阵，设参数r_k为：

r_k＝θ_ky_k+(1-θ_k)B_ks_k

其中，s_k＝x_k+1-x_k，B_k是一个不对称正定矩阵，θ_k为：

因此，B_k更新如下：

通过序列二次规划算法求解优化目标函数，得到具体的控制分配舵角，进而将舵角控制指令发送给控制器，实现水下航行器的三维跟踪控制。

其有益效果在于：

本申请的基于模型的水下航行器抗衡滚三维跟踪控制方法在迹跟踪控制过程中，能够在不同初始姿态下都产生很好的跟踪效果，且控制方案的姿态误差收敛，能够同时控制水下航行器横滚、俯仰和偏航姿态。

附图说明

图1是不同初始位置的螺旋跟踪示意图；

图2是不同初始位置的螺旋轨迹跟踪示意图；

图3是水下航行器跟踪的位置误差示意图；

图4是水下航行器跟踪的姿态误差示意图；

图5是水下航行器跟踪控制的控制力和力矩示意图；

图6是水下航行器最优分配的舵角变化示意图；

图7是跟踪控制过程中的航迹与期望路径的变化曲线；

图8是跟踪控制过程中的深度变化曲线和期望变化曲线；

图9是跟踪控制过程中的横滚误差变化曲线；

图10是跟踪控制过程中的俯仰误差变化曲线；

图11是跟踪控制过程中的偏航误差变化曲线；

图12是跟踪控制过程中的速度变化曲线。

具体实施方式

以下结合具体实施例对本发明作详细说明。

对于航行智能操纵控制技术，首先需要对其问题和符号进行描述。如图1所示，{I}，{B}，{F}和{V}分别是惯性坐标系，随体坐标系，Serret-Frenet坐标系和和速度坐标系。设水下航行器的姿态为η＝[P,Θ]^·，其中P＝[x,y,z]^·为位置向量，Θ＝[φ,θ,ψ]^·为姿态向量，位置向量和姿态向量都表示在{I}坐标系下，可以通过前文提到的惯性测量单元测得。水下航行器的线速度v＝[u,v,w]^·和角速度ω＝[p,q,r]^·定在{B}坐标系下，可以通过前文提到的多普勒测速仪测测得。设水下航行器定义在{V}坐标系下的合速度为U_V＝[U,0,0]^·，其中设水下航行器的重心Q与{B}坐标系重合，并且设虚拟目标点P与{F}动态坐标系的原点在任意时刻重合。

在运动控制的过程中，将水下航行器视为一个均质的刚体，基于刚体的牛顿欧拉公式和拉格朗日公式，其运动学和动力学模型构建如下：

其中，M，C(v)，D(v)，g(η)，t和d分别为附加质量矩阵，科里奥利和向心矩阵，阻尼矩阵，重力和浮力矢量，输入控制力和力矩，时变的未知扰动；J(η)为η和ν的雅可比旋转矩阵。根据控制维度的不同，可以将运动模型分为平面内和三维空间内分别研究，三维空间中的六自由度运动学模型为：

动力学模型为：

其中，

公式(3)中各参数表示如下，m_ii(i＝1,2,3,4,5,6)为附加质量，X_(·)，Y_(·)，Z_(·)，K_(·)，M_(·)和N_(·)为水动力系数。上述水动力参数可以采用经验公式法、计算流体力学CFD仿真模拟法、模型辨识法、约束船模实验法以及实船试验法等来估计或测量水下航行器水动力学系数，本方法采用经验公式法和计算流体力学CFD仿真模拟法估计水动力学系数。

本申请分别设计了基于非线性复合LOS制导律的运动学控制律和基于自适应快速终端滑模控制的动力学控制律，两者级联控制实现水下航行器的跟踪控制，并结合抗饱和非线性控制分配器，实现复杂环境下的跟踪控制。

其中，基于非线性复合LOS制导律的运动学控制律设计原理如图2所示：

为了镇定水下航行器的姿态误差，首先考虑控制器位置误差，具体为：

其中，旋转矩阵表示从{F}坐标系到{I}坐标系，绕{I}坐标系的z^I轴旋转再绕{I}坐标系的x^I轴旋转得到{F}坐标系，则旋转矩阵为

对公式(12)求导得：

其中，期望合速度向量U_d＝[U_d,0,0]^·，设χ_e和υ_e为LOS制导律的前视角，跟踪的位置误差模型为：

设计正定的李雅普诺夫函数，并对其求导为：

为了得到负定的公式(16)，三维跟踪的制导律设计如下：

其中，k_s>0，k_e>0和k_h>0是制导律的增益系数，前视距离Δ_e>0，Δ_h>0。将公式(17)带入公式(16)，得到：

因此，通过调整参k_s，k_e和k_h数保证当且仅当s，e和h，通过公式(17)可以镇定跟踪的位置误差。接下来需要开展运动学控制率研究，设水下航行器姿态误差和目标姿态为Θ_e＝[φ_e,θ_e,ψ_e]·和Θ_e＝Θ-Θ_d。对于Θ_d的求解，本方法考虑到真实环境中的水动力角[α,β]和航向角的[υ,χ]非线性关系，采用坐标系的等价关系转换，进而推导期望的姿态角[φ,θ,ψ]，其中攻角和漂角分别定义为α＝arctan(w/u)和因此，期望姿态角与水动力角和期望航向角的关系为：

其中，

为了镇定水下航行器的姿态误差设正定李雅普诺夫函数如下：

V₂＝(1-cosφ_e)+(1-cosθ_e)+(1-cosψ_e) (20)

因此，基于非线性复合LOS制导律的运动学控制律为：

其中，k_p>0，k_q>0和k_r>0为控制增益，将公式(21)带入公式(20)为：

通过设计参数k_p，k_q和k_r数保证当且仅当φ_e，θ_e和ψ_e，因此，负定且满足李雅普诺夫第二稳定性定理。

基于自适应快速积分终端滑模控制的动力学控制律设计

为了避免对运动学控制律求导数的微分爆炸问题，采用了一阶虚速度滤波，具体的低通滤波器设计为：

因此，滤波误差为用于跟踪的虚速度误差为：

将公式(3)带入公式的导数为：

接下来设计动力学控制律t_u，t_p，t_q和t_r来稳定虚速度误差。本方法采用自适应快速终端滑模控制器，设计滑模面如下：

其中，α_u>0和β_u>0为待设计常数，q_u和p_u为正奇数，且满足0<q_u/p_u<1。设计动力学控制率如下：

其中，(·)^*为标称的水动力系数，和为未知环境扰动的估计值，sat()为辅助控制率的饱和函数。以纵向控制律为例，设计正定的李雅普诺夫函数为：

考虑到水下航行器在环境中的参数摄动问题，根据现有研究一般认为摄动的边界为±20％，即因此，设计ξ_u如下：

其中，ζ_u>0，为参数摄动的边界，可以通过滑模控制进行自适应调整。基于上述推导，ζ_p，ζ_q和ζ_r为：

将公式(29)化简为：

因此，设计自适应扰动控制律设计为：

开展水下航行器控制系统综合稳定性分析，构造李雅普诺夫函数如下：

V＝V₁+V₂+V_3u+V_3p+V_3q+V_3r (33)

对公式(33)求导并带入化简得：

根据推导，不论取什么值，均满足因此公式(34)始终满足李雅普诺夫稳定性定理，能过够使得水下航行器的姿态趋近于期望姿态。

非线性饱和约束下的艉舵控制分配器设计

在得到期望的力和力矩后，需要通过舵角对力和力矩进行解算，同时考虑控制精度、执行器饱和约束、能量消耗等多方面优化求解，具体的目标函数如下：

其中，ε表示消耗能量和控制精度的权重参数，s为力或力矩控制误差，δ_max和δ′_max为水下航行器的最大舵角和最大转舵速率。通过上述目标函数，采用序列二次规划算法，能够完成舵角控制分配，SQP算法具体介绍如下：

任意的非线性问题都能被线性化为一个二次规划问题，具体为：

其中，E和I分别为等式集的约束和不等式的约束，上式具有一个特殊解(p_k,l_k)且满足如下条件：

通过牛顿迭代法，KKT矩阵为：

使用阻尼BFGS方法近似估计Hessian矩阵，设参数r_k为：

r_k＝θ_ky_k+(1-θ_k)B_ks_k

其中，s_k＝x_k+1-x_k，B_k是一个不对称正定矩阵，θ_k为：

因此，B_k更新如下：

在迹跟踪控制过程中，不同初始姿态的性能如图3所示，所有的结果都有很好的跟踪效果。图4、图5描述了控制方案的姿态误差，姿态误差是收敛的，特别是在横滚自由度上。说明该方法能够同时控制X型舵水下航行器横滚、俯仰和偏航姿态。图6为该过程中的控制力和力矩，图7为对应四个舵叶的舵角变化。

在上位机软件中规划了一条次使命，待试验结束后读取记录的数据，绘制跟踪控制过程中的航迹变化曲线、深度变化曲线、横滚误差变化曲线、俯仰误差变化曲线、偏航误差变化曲线、速度变化曲线。本次航行的任务信息为：

路径点1:东经120.560100,北纬31.892197

路径点2:东经120.559856,北纬31.892216

路径点3:东经120.559126,北纬31.892262

路径点4:东经120.559325,北纬31.891700

路径点5:东经120.560021,北纬31.891979

路径点6:东经120.560282,北纬31.892079

航行深度：1m；航行速度：0.8m/s与1.0m/s切换

从上述曲线可以得出水下航行器稳定航行且能够跟踪预设路径，特别是横滚姿态误差得到了有效的控制。因此，经过试验验证所提出方法稳定有效。

如图1所示，展示了跟踪控制过程中的航迹变化曲线、期望路径点构成的直线以及起点和终点，图2展示了跟踪控制过程中的深度变化曲线和期望深度，说明了所提出方法在航向跟踪、深度跟踪上都有较好的结果。为了更好的展示跟踪控制效果，图3展示了跟踪控制过程中的横滚误差变化曲线，图4展示了跟踪控制过程中的俯仰误差变化曲线，图5展示了跟踪控制过程中的偏航误差变化曲线，进一步说明了说提出的方法具有较好的镇定误差的能力，特对别对于横滚误差。图6展示了跟踪控制过程中的偏航误差变化曲线跟踪控制过程中的速度变化曲线，分别跟踪0.8m/s和1.0m/s两种期望速度，都能达到较好的控制结果。

最后应当说明的是，以上实施例仅用以说明本发明的技术方案，而非对本发明保护范围的限制，尽管参照较佳实施例对本发明作了详细地说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的实质和范围。

Claims (3)

1.一种基于模型的水下航行器抗衡滚三维跟踪控制方法，其特征在于，包括如下步骤

步骤一、建立水下航行器运动模型

分别建立惯性坐标系{I}，随体坐标系{B}，Serret-Frenet坐标系{F}和和速度坐标系{V}；定义水下航行器运动向量，包括：

在{I}坐标系下的姿态向量η＝[P,Θ]，其中P＝[x,y,z]为位置向量，Θ＝[φ,θ,ψ]为姿态向量；在{B}坐标系下的线速度v＝[u,v,w]和角速度ω＝[p,q,r]；在{V}坐标系下的合速度向量U_V＝[U,0,0]，其中

基于刚体的牛顿欧拉公式和拉格朗日公式，建立环形器运动学和动力学模型：

其中，M表示附加质量矩阵，C(ν)为科里奥利和向心矩阵，D(ν)为阻尼矩阵，g(η)为重力和浮力矢量，τ为输入控制力和力矩，d为时变的未知扰动，J(η)为η和ν的雅可比旋转矩阵；

建立环形器三维空间中的六自由度运动学模型和动力学模型；其中六自由度运动学模型为：

动力学模型为：

其中，

其中，m_ii(i＝1,2,3,4,5,6)为附加质量，X_(·)，Y_(·)，Z_(·)，K_(·)，M_(·)和N_(·)为水动力系数；

步骤二、基于非线性复合LOS制导律的运动学控制律

为了镇定水下航行器的姿态误差，建立控制器位置误差：

其中旋转矩阵为表示从{F}坐标系到{I}坐标系，绕{I}坐标系的z^I轴旋转再绕{I}坐标系的x^I轴旋转得到{F}坐标系；

求导得式中，U_d＝[U_d,0,0]为期望合速度向量，且设χ_e和υ_e为LOS制导律的前视角，则轨迹跟踪的位置误差模型为：

设计正定的李雅普诺夫函数，并对其求导为：

为得到负定的公式，设计三维跟踪制导律如下：

其中，k_s＞0，k_e＞0和k_h＞0是制导律的增益系数，前视距离Δ_e＞0，Δ_h＞0；联立后得到：

设水下航行器姿态误差和目标姿态为Θ_e＝[φ_e,θ_e,ψ_e]和Θ_e＝Θ-Θ_d，真实环境中的水动力角为[α,β]，航向角为[υ,χ]，采用坐标系的等价关系转换推导期望的姿态角[φ,θ,ψ]，其中攻角为α＝arctan(w/u)，漂角为则期望姿态角与水动力角和期望航向角的关系为：

其中，

为镇定水下航行器姿态误差，设正定李雅普诺夫函数如下：

V₂＝(1-cosφ_e)+(1-cosθ_e)+(1-cosψ_e)

得到基于非线性复合LOS制导律的运动学控制律：

步骤三、非线性饱和约束下的艉舵控制分配器；

基于步骤二得到期望的力和力矩，通过舵角对力和力矩进行解算，考虑控制精度、执行器饱和约束、能量消耗的优化求解，建立优化目标函数如下：

其中，ε表示消耗能量和控制精度的权重参数，s为力或力矩控制误差，δ_max和δ′_max为水下航行器的最大舵角和最大转舵速率；

基于优化目标函数进行进行优化求解，得到三维运动参数，完成水下航行器的三维跟踪控制。

2.根据权利要求1所述的基于模型的水下航行器抗衡滚三维跟踪控制方法，其特征在于，所述步骤二中，还包括基于自适应快速积分终端滑模控制的动力学控制律设计的步骤，具体包括：

S1、采用一阶虚速度滤波避免微分爆炸，所述滤波器为滤波误差为用于轨迹跟踪的虚速度误差为

联立后有

S2、优化动力学控制率来稳定虚速度误差；采用自适应快速终端滑模控制器，设计滑模面如下：

其中，α_u＞0和β_u＞0为待设计常数，q_u和p_u为正奇数，且0＜q_u/p_u＜1；

所述动力学控制率如下：

其中，(·)^*为标称的水动力系数，和为未知环境扰动的估计值，sat()为辅助控制率的饱和函数；

对于纵向控制律，设计正定的李雅普诺夫函数为

基于水下航行器在环境中的参数摄动边界为±20％，有

令其中ζ_u＞0，为参数摄动的边界，通过滑模控制进行自适应调整；

基于上述设定ζ_p，ζ_q和ζ_r可表示为：

化简后得到：

得到自适应扰动控制律为

3.根据权利要求1所述的基于模型的水下航行器抗衡滚三维跟踪控制方法，其特征在于，所述水动力系数采用经验公式法、计算流体力学CFD仿真模拟法、模型辨识法、约束船模实验法以及实船试验法来估计或测量。