0 引　言

自主水下航行器（Autonomous Underwater Vehicle，AUV）具有成本低、机动性高等优点，是了解海洋和开发利用海洋资源的重要工具，通过搭载不同设备被广泛应用于水下监测、资源调查、地形勘探、油气管线检查^{[1 - 3]}。为了更高效地完成复杂水下作业任务，近年来搭载X舵的AUV逐渐得到推广，该结构凭借其优越的操纵冗余性与机动性^{[4 - 5]}，成为当前的研究热点之一。

X舵AUV在水下航行中存在横滚偏转问题。由于X舵操纵空间力、螺旋桨推进力矩以及水下随机干扰等影响，导致横滚运动震荡甚至产生运动姿态的不稳定，而小型AUV的外形尺寸小，质心和浮心距离短，系统自恢复力矩不足以抑制横滚的影响^{[6 - 7]}，横滚的不稳定在耦合效应下可能进一步诱发航向、纵倾的偏移。在实体实验中，X舵AUV受横滚耦合影响，与期望路径存在误差，甚至可能造成安全事故。此外，AUV在水下航行还面临海流等其他未知干扰的影响，它们随着海况和海域等复杂海洋环境因素的变化而改变，变化方向和幅度存在严重的不确定性，这些不确定扰动和其他影响，加剧了横滚的不稳定性，为AUV的控制设计带来了巨大的挑战。在现有的横滚控制方案里，Ebrahimi等^[8]使用一种耦合鳍叶来进行横滚控制，然而这种方法对AUV的结构有特殊要求。鉴于X舵的舵面都能产生不同方向上的力矩，可以通过分别输入合适舵角，通过对舵角进行差动控制，在横滚方向产生控制力矩，从而抑制AUV的横滚运动，黄哲敏等^[9]在湖试实验中使用反馈线性比例−微分控制器将X舵AUV的横滚角控制在2°内。除此之外，当前有关X舵横滚约束控制的研究与实际应用尚少。因此，要充分发挥X舵的性能优势，综合考虑横滚约束条件下X舵控制研究是值得深入发掘和研究的内容。

另外，国内外研究学者针对AUV的不确定干扰下运动控制问题提出了多种控制方法，如比例−积分−微分控制、线性二次型最优控制（Linear Quadratic Regulator，LQR）、反步控制^[10]、模型预测控制^[11]、自抗扰控制^[12]等。其中，LQR算法具有方便的设计、简单且易于调整的参数等优势。然而，在工程实践中，AUV在航行中除了受到未知海流干扰，还有模型不确定性等问题制约着其控制精度。为了解决这个问题，Yuan等^[13]提出自适应控制线性二次性最优控制（L1AC−LQR）控制算法，结合L1自适应控制（L1 Adaptive Control, L1AC）对控制器进行补偿，优化转向和深度控制性能；Yu等^[14]通过线性二次型调节器设计双环深度跟踪方案，动力学控制器引入扩张状态干扰（Extend State Observer，ESO）观测器对干扰项进行观测，显著提升AUV在模型不确定性和执行器波动下的跟踪精度，然而，控制设计往往以忽略横滚运动为前提，没有充分考虑横滚和其他不确定性的影响。

精确跟踪控制会频繁纠偏动作从而加剧能量消耗，降低执行机构的使用寿命。这个问题与控制器参数选择有关，对于控制器参数整定问题，学者们通过引入先进的优化算法对其进行整定。秦东燕等^[15]利用灰狼算法对控制器参数进行优化以改善控制器精度，但该算法受收敛因子的影响，容易陷入局部最优。陈丽芳等^[16]总结了不同新型群智能算法，经实验验证算法在工程问题中的有效性。其中，鲸鱼优化算法（Whale Optimization Algorithm, WOA）具有操作简单、控制参数少等特点，使其在复杂度较高的情况下具有更好的寻优能力。单泽彪等^[17]提出一种基于鲸鱼优化算法的无人机模糊自抗扰控制方法对控制器进行最优参数调整，具有良好的动态响应。然而，鲸鱼优化算法初始化随机分配，缺少对控制器参数的约束，导致控制器缺少稳定性。对此，Muhssin等^[18]结合决策矩阵的粒子群智能优化算法，实现了Q、R矩阵的正定与半正定。

上述控制算法在应对耦合系统、非线性控制以及参数优化方面已有所优化和改进，但针对随机干扰下的X舵AUV横滚耦合及能耗等多目标约束控制问题在公开的文献中较少，尚缺乏相关方面的控制策略。为此，本文提出一种基于改进鲸鱼多目标约束非线性扩张状态观测器−线性二次型最优（Whale Extend State Observer-Linear Quadratic Regulator，WESO-LQR）优化控制算法。该方法在实现航向与深度稳定控制的同时，可有效抑制横滚扰动，并通过优化打舵频次降低能耗，提升系统控制能效。

1 X舵AUV模型建立 1.1 模型参数

X舵AUV采用分段设计，如图1所示。其包括艏段、通导段、电子舱段、X舵尾部操纵推进段。

设计参数及水动力系数如表1所示。

1.2 坐标轴建立

为了研究AUV操纵运动的规律，确定运动AUV的位置和姿态（方向），本文采用国际水池会议ITTC和SNAME推荐的坐标体系，分别建立固定坐标系$ E-\xi \eta \zeta $和运动坐标系$ O-xyz $，如图2所示。其中，AUV的前进方向、右舷方向、向下方向分别为x轴、y轴和z轴的指向。

重心在z轴上的坐标为$ {z}_{g} $；AUV重心处相对于地球的速度是V，V在坐标系$ O-xyz $上的投影为u、v、w；AUV绕$ x $轴、$ y $轴和$ z $轴上的转动角速度分别为p、q、r；AUV所受合力$ F $在$ O-xyz $上的投影为X、Y、Z；力矩为K、M、N；其他变量包括横滚角$ \varphi $、纵倾角$ \theta $、偏航角$ \psi $。

1.3 X舵AUV舵力分析

本文聚焦X舵对AUV纵倾、偏航以及横滚运动的影响，基于以下假设简化X舵模型：

1）X舵产生的纵向力分量、横向力分量以及垂向力分量对AUV主体动力学的耦合效应可忽略；

2）舵效耦合非线性主要作用于姿态角动态响应，对平移运动自由度的扰动较低。

如图3所示，对4个舵叶重新编号，左下角开始逆时针分别是舵叶1、2、3、4。

将$ {F}_{1}、{F}_{2}、{F}_{3}、{F}_{4} $分解到y、z轴方向上获得分力，尾翼上绕航行器x轴的力将产生横倾力矩，设舵面上合力的作用点到AUV体轴的距离$ {l}_{ki} $(i=1,2,3,4)，分别代表1、2、3、4号舵的横倾力臂，如图4所示。

由4个尾翼产生的横倾力矩$ {\tau }_{K} $为：

$ {\tau }_{K}\text=-{k}_{1}{V}^{2}{\delta }_{1}{l}_{{{k}_{1}}}-{k}_{2}{V}^{2}{\delta }_{2}{l}_{{{k}_{2}}}-{k}_{3}{V}^{2}{\delta }_{3}{l}_{{{k}_{3}}}-{k}_{4}{V}^{2}{\delta }_{4}{l}_{{{k}_{4}}}。$

(1)

尾翼上绕航行器$ y $轴的力将产生纵倾力矩，设舵面上合力的作用点到航行器重心$ {z}_{g} $的距离为$ {l}_{mi} $(i=1,2,3,4)分别代表1、2、3、4号舵的纵倾力臂，如图5所示。

则由4个尾翼产生的纵倾力矩为：

$ {\tau }_{M}\text=\frac{\sqrt{2}}{2}\text{(}{k}_{1}{V}^{2}{\delta }_{1}{l}_{{{m}_{1}}}-{k}_{2}{V}^{2}{\delta }_{2}{l}_{{{m}_{2}}}-{k}_{3}{V}^{2}{\delta }_{3}{l}_{{{m}_{3}}}+{k}_{4}{V}^{2}{\delta }_{4}{l}_{{{m}_{4}}}) 。$

(2)

同理，偏航力矩为：

$ {\tau }_{N}\text=\frac{\sqrt{2}}{2}\text{(}-{k}_{1}{V}^{2}{\delta }_{1}{l}_{{{\text{n}}_{1}}}-{k}_{2}{V}^{2}{\delta }_{2}{l}_{{{\text{n}}_{2}}}+{k}_{3}{V}^{2}{\delta }_{3}{l}_{{{\text{n}}_{3}}}+{k}_{4}{V}^{2}{\delta }_{4}{l}_{{{\text{n}}_{4}}})。$

(3)

式中：$ {l}_{ni} $(i=1,2,3,4)分别为1、2、3、4号舵的偏航力臂。

1.4 X舵AUV六自由度耦合模型

根据黄哲敏等^[9]对螺旋桨式推进器转速和AUV横滚角的研究，推进器产生的横滚扭矩值$ {K}_{T} $为：

$ {K}_{{T}}={K}_{{Q}}\rho n\left| n\right| D_p^{5}。$

(4)

式中：$ {K}_{{Q}} $为螺旋桨转矩系数；$ \rho $为液体密度；$ {D}_{p} $为螺旋桨直径。$ {X}_{T} $推力：

$ {X}_{{T}}={K}_{{p}}\rho n\left| n\right| (1-{t}_{{p}})D_{{p}}^{4}。$

(5)

式中：$ {K}_{{p}} $为螺旋桨推力系数；$ {t}_{{p}} $为推力减额系数。

根据Fossen^[19]对AUV动力学模型的描述方式以及牛顿−欧拉方程和拉格朗日方程，得到X舵AUV六自由度研究模型：

${\left\{\begin{aligned} &m(\dot{u}-vr+wq)={X}_{u|u|}u|u|+{X}_{\dot{u}}\dot{u}+{X}_{wq}wq+{X}_{qq}qq+\\ &{X}_{vr}vr+{X}_{rr}rr-(W-B)\sin \theta +{X}_{\text{T}}+{\sigma }_{X}，\\ &m(\dot{v} - wq+ur) = (W-B)\sin \phi \cos \theta + {Y}_{v|v|}v|v| + {Y}_{r|r|}r|r| + \\ &{Y}_{\dot{v}}\dot{v}+{Y}_{\dot{r}}\dot{r}+{Y}_{ur}ur+{Y}_{wq}wq+{Y}_{pq}pq+{Y}_{uv}uv+{\sigma }_{Y}，\\ & m(\dot{w}-uq+vp)=(W-B)\cos \phi \sin \theta +Z{}_{w|w|}w|w|+\\ &{Z}_{q|q|}q|q| + {Z}_{\dot{w}}\dot{w} + {Z}_{\dot{q}}\dot{q} + {Z}_{uq}uq + {Z}_{vp}vp + {Z}_{rp}rp + {Z}_{uw}uw + {\sigma }_{Z}，\\ &{I}_{xx}\dot{p}+({I}_{zz}-{I}_{yy})qr=({y}_{g}W-{y}_{b}B)\cos \phi \cos \theta -\\ &({z}_{g}W-{z}_{b}B)\sin \phi \cos \theta +{K}_{p|p|}p|p|+{K}_{\dot{p}}\dot{p}+{\tau }_{K}+{K}_{\text{T}}+{\sigma }_{K}，\\ &{I}_{yy}\dot{q}+({I}_{xx}-{I}_{yy})rp={M}_{w|w|}w|w|+{M}_{q|q|}q|q|+{M}_{\dot{w}}\dot{w}+\\ &{M}_{\dot{q}}\dot{q}+{M}_{uq}uq+{M}_{vp}vp+{M}_{rp}rp+{M}_{uw}uw+{\tau }_{M}+{\sigma }_{M}，\\ &{I}_{zz}\dot{r} + ( {I}_{xx} - {I}_{yy} )pq = ( {x}_{g}W - {x}_{b}B )\sin \phi \cos \theta - ( {y}_{g}W -\\ &{y}_{b}B )\sin \theta +{N}_{v|v|}v|v|+{N}_{r|r|}r|r|+{N}_{\dot{v}}\dot{v}+{N}_{\dot{r}}\dot{r}+{N}_{ur}ur+\\ &{N}_{wp}wp+{N}_{pq}pq+{N}_{uv}uv+{\tau }_{N}+{\sigma }_{N}。\end{aligned}\right. }$

(6)

${\left\{\begin{aligned} &\dot{x}=u\cos \psi \cos \theta +v(\cos \psi \sin \theta \sin \varphi -\sin \psi \cos \varphi )+\\ &w(\cos \psi \sin \theta \sin \varphi +\sin \psi \sin \varphi )，\\ &\dot{y}=u\sin \psi \cos \theta +v(\sin \psi \sin \theta \sin \varphi +\cos \psi \cos \varphi )+\\ &w(\sin \psi \sin \theta \cos \varphi -\cos \psi \sin \varphi )，\\ &\dot{z}=-u\sin \theta +v\cos \theta \sin \varphi +w\cos \theta \sin \varphi ，\\ & \dot{\varphi }=p+q\tan \theta \sin \varphi +r\tan \theta \cos \varphi ，\\ &\dot{\theta }=q\cos \varphi -r\sin \varphi ，\\ &\dot{\psi }=(q\sin \varphi +r\cos \varphi )/\cos \varphi。\end{aligned}\right. }$

(7)

式中：m为AUV的质量；W为AUV的重力；$ B $为AUV的浮力；$ {x}_{g}、{y}_{g}、{z}_{g} $和$ {x}_{b}、{y}_{b}、{z}_{b} $分别为AUV的重心位置和浮心位置；$ {X}_{\left(*\right)}、{Y}_{\left(*\right)}、{Z}_{\left(*\right)}、{K}_{\left(*\right)}、{M}_{\left(*\right)}$、$ {N}_{\left(*\right)} $为AUV的一阶水动力系数；$ {X}_{\left(**\right)}、{Y}_{\left(**\right)}、{Z}_{\left(**\right)}$、$ {K}_{\left(**\right)}、{M}_{\left(**\right)}、{N}_{\left(**\right)} $为AUV的耦合水动力系数；$ {\tau }_{\left(*\right)} $为舵力（力矩）；$ {\sigma }_{\left(*\right)} $为海流力等其他的未知随机干扰。

2 X舵AUV控制设计

AUV的深度、航向以及横滚WESO−LQR控制器设计思路如图6所示。

1）动力学控制器。设期望横滚角$ {\varphi }_{d} $、偏航角$ {\psi }_{d} $、深度值$ {\zeta }_{d} $与AUV状态所反馈的横滚角$ \varphi $、偏航角$ \psi $、深度值$ \zeta $之间的差为$ {e}_{\varphi }、{e}_{\psi }、{e}_{\zeta } $。$ {e}_{\varphi }、{e}_{\psi }、{e}_{\zeta } $经过LQR控制器计算输出期望横滚力矩$ {\tau }_{K} $、纵倾力矩$ {\tau }_{M} $、偏航力矩$ {\tau }_{N} $，对期望力矩$ {\tau }_{K}、{\tau }_{M}、{\tau }_{N} $进行基于伪逆法的舵力分配，解算得到AUV舵角的输入$ {\delta }_{1}、{\delta }_{2}、{\delta }_{3}、{\delta }_{4} $。AUV的状态量偏航角$ \psi $、偏航角速度$ r $、横滚角$ \varphi $、横滚角速度$ p $、纵倾角$ \theta $、纵倾角速度$ q $，输入扩张状态观测器中，输出观测量$ {\hat{d}}_{K}、{\hat{d}}_{M}、{\hat{d}}_{N} $补偿进LQR控制律中。

2）控制参数优化。横滚角、偏航和深度的差值$ {e}_{\varphi }、{e}_{\psi }、{e}_{\zeta } $和分配器输出的舵角$ {\delta }_{1}、{\delta }_{2}、{\delta }_{3}、{\delta }_{4} $输入目标函数，输出增益矩阵K到航向、深度以及横滚LQR控制器中，以实现参数优化条件下航向、深度以及横滚角最优控制。

2.1 LQR控制器设计

LQR基于线性化模型，因此在面对非线性耦合系统时，其适用性受到一定限制，需先对式(6)和式(7)进行线性化可得：

$ {\left\{\begin{aligned} &X={X}_{\dot{u}}\dot{u}+{X}_{u|u|}u|u|+{X}_{vr}vr+{X}_{wq}wq+{X}_{T}\text+{\sigma }_{X}，\\ & Y={Y}_{\dot{v}}\dot{v}+{Y}_{\dot{r}}\dot{r}+{Y}_{v}v+{Y}_{r}r\text+{\sigma }_{Y}，\\ &Z={Z}_{0}+{Z}_{\dot{w}}\dot{w}+{Z}_{\dot{q}}\dot{q}+{Z}_{w}w+{Z}_{q}q+{Z}_{\theta }\theta \text+{\sigma }_{Z}，\\ &K={K}_{q|q|}q|q|+{K}_{\dot{q}}\dot{q}+{K}_{T}+{K}_{R}+{\tau }_{K}\text+{\sigma }_{K}，\\ &M={M}_{0}+{M}_{\dot{w}}\dot{w}+{M}_{\dot{q}}\dot{q}+{M}_{w}w+{M}_{q}q+{M}_{\theta }\theta +{\tau }_{M}+{\sigma }_{M}，\\ &N={N}_{\dot{v}}\dot{v}+{N}_{\dot{r}}\dot{r}+{N}_{v}v+{N}_{r}r+{\tau }_{N}\text+{\sigma }_{N}。\end{aligned}\right. }$

(8)

本文将AUV的运动描述为垂荡运动、偏航运动以及横滚运动。对于AUV的垂荡运动，选择状态为$ \left[\dot{w},\dot{q},\dot{\theta },\dot{\zeta }\right] $，模型的状态空间方程为：

$ \left[\begin{matrix}\dot{w}\\ \dot{q}\\ \dot{\theta }\\ \dot{\zeta }\\ \end{matrix}\right]=\left[\begin{matrix}Z{}_{w} & {Z}_{q} & {Z}_{\theta } & 0\\ {M}_{w} & {M}_{q} & {M}_{\theta } & 0\\ 0 & 1 & 0 & 0\\ 1 & 0 & -u & 0\\ \end{matrix}\right]\left[\begin{matrix}w\\ q\\ \theta \\ \zeta \\ \end{matrix}\right]+\left[\begin{matrix}0\\ 1\\ 0\\ 0\\ \end{matrix}\right]{\tau }_{M}+\left[\begin{matrix}{d}_{Z}\\ {d}_{M}\\ 0\\ 0\\ \end{matrix}\right] 。$

(9)

同理，偏航运动状态选择为$ \left[\dot{v},\dot{r},\dot{\psi }\right] $，模型的状态空间方程为：

$ \left[\begin{matrix}\dot{v}\\ \dot{r}\\ \dot{\psi }\\ \end{matrix}\right]=\left[\begin{matrix}{Y}_{v} & {Y}_{r} & 0\\ {N}_{v} & {N}_{r} & 0\\ 0 & 1 & 0\\ \end{matrix}\right]\left[\begin{matrix}v\\ r\\ \psi \\ \end{matrix}\right]+\left[\begin{matrix}0\\ 1\\ 0\\ \end{matrix}\right]{\tau }_{N}+\left[\begin{matrix}{d}_{Y}\\ {d}_{N}\\ 0\\ \end{matrix}\right]。$

(10)

对于横滚运动状态选择为$ \left[\dot{q},\dot{\varphi }\right] $，模型的状态空间方程为：

$ \left[\begin{matrix}\dot{q}\\ \dot{\varphi }\\ \end{matrix}\right]=\left[\begin{matrix}{K}_{qq} & 0\\ 1 & 0\\ \end{matrix}\right]\left[\begin{matrix}q\\ \varphi \\ \end{matrix}\right]+\left[\begin{matrix}1\\ 0\\ \end{matrix}\right]{\tau }_{K}+\left[\begin{matrix}{d}_{K}\\ 0\\ \end{matrix}\right]。$

(11)

对于一个状态空间方程系统为$x(t) = Ax(t) + bu(t)$，LQR控制器，通过增益向量K返回到系统中，LQR控制器的控制律为：$ u\left(t\right)=-{K}x\left(t\right) $。LQR控制器通过设计合适的Q、R矩阵，让代价函数J最小，使得控制器的控制效果最好。

$ J = \int_ {0}^{\infty} \left[ x^{\mathrm{T}}(t){Q} x(t) + u^{\mathrm{T}}(t) {R} u(t) \right] {\mathrm{d}}t。$

(12)

式中：Q为状态变量的n×n半正定加权矩阵；R定义为控制向量输入 u(t)的正定矩阵。根据式Riccati方程求解出矩阵P：

$ A^{\mathrm{T}} P + PA - PBR^{-1}B^{\mathrm{T}} P + Q = 0。$

(13)

最终增益矩阵K的解为：

$ K = R^-1B^{\mathrm{T}} P。$

(14)

由式(14)推出垂直面的状态反馈控制律$ {\tau }_{M} $为：

$ {\tau }_{M}={k}_{w}w+{k}_{q}q+{k}_{\theta }\theta +{k}_{\zeta }(\zeta -{\zeta }_{d})。$

(15)

偏航运动的状态反馈控制律$ {\tau }_{N} $为：

$ {\tau }_{N}={k}_{v}v+{k}_{r}r+{k}_{\psi }(\psi -{\psi }_{d})。$

(16)

横滚运动的状态反馈控制律$ {\tau }_{K} $为：

$ {\tau }_{K}={k}_{p}p+{k}_{\varphi }(\varphi -{\varphi }_{d})。$

(17)

式中：$ {k}_{*} $为LQR控制器参数，仿真的具体参数如表2所示；$ {\zeta }_{d}、{\psi }_{d}、{\varphi }_{d} $分别为期望深度、期望航向角和期望横滚角。

2.2 非线性扩张状态观测器设计

在面对未知海流干扰或其他不确定耦合项，LQR控制无法很好地对系统进行控制容易产生震荡，需要精确的数学模型。通过ESO能对不确定因素进行实时观测，并通过扰动补偿，从而提高控制系统的控制精度。

基于式(6)和式(7)，令 $ {x}_{1}=\psi ，{x}_{2}=r，{x}_{3}=\theta $，$ {x}_{4}=q，{x}_{5}=\varphi ，{x}_{6}=p $。将系统重构为如下具有显示耦合干扰项的模型：

$ \left\{ \begin{array}{l} {\dot{x}}_{1}\text={x}_{2}，\\ {\dot{x}}_{2}={f}_{r}+{b}_{1}{\tau }_{N}+{d}_{N}，\\ {\dot{x}}_{3}={x}_{4}，\\ {\dot{x}}_{4}={f}_{q}+{b}_{2}{\tau }_{M}+{d}_{M}，\\ {\dot{x}}_{5}={x}_{6}，\\ {\dot{x}}_{6}={f}_{p}+{b}_{3}{\tau }_{K}+{d}_{K}。\end{array}\right. $

(18)

式中：$ {b}_{1}、{b}_{2}、{b}_{3} $分别为相应的控制力矩增益；$ {f}_{r}、{f}_{q}、{f}_{p} $展开如下式：

${ \begin{split}&{f}_{r}=\dfrac{({x}_{g}W-{x}_{b}B)\sin \phi \cos \theta -({y}_{g}W - {y}_{b}B)\sin \theta +{N}_{v|v|}{v}_{r}|{v}_{r}|+{N}_{r|r|}r|r|+{N}_{\dot{v}}\dot{v}+{N}_{\dot{r}}\dot{r}+{N}_{ur}{u}_{r}r+{N}_{wp}{w}_{r}p+{N}_{pq}pq+{N}_{uv}{u}_{r}{v}_{r}-({I}_{xx}-{I}_{yy})pq}{{I}_{zz}}，\\ & {f}_{q}=\dfrac{{M}_{w|w|}{w}_{r}|{w}_{r}|+{M}_{q|q|}q|q|+{M}_{\dot{w}}\dot{w}+{M}_{\dot{q}}\dot{q}+{M}_{uq}{u}_{r}q+{M}_{vp}{v}_{r}p+{M}_{rp}rp+{M}_{uw}{u}_{r}{w}_{r}-({I}_{xx}-{I}_{yy})rp}{{I}_{yy}}，\\ & {f}_{p}=\dfrac{({y}_{g}W-{y}_{b}B)\cos \phi \cos \theta -({z}_{g}W-{z}_{b}B)\sin \phi \cos \theta +{K}_{p|p|}p|p|+{K}_{\dot{p}}\dot{p}+{K}_{T}-({I}_{zz}-{I}_{yy})qr}{{I}_{xx}}。\end{split}} $

(19)

设计非线性扩张状态观测器（ESO）模型：

$\left\{ \begin{array}{l} {\dot{\hat{x} }}_{1}={\hat{x}}_{2}-{k}_{1}\left({x}_{1}-{\hat{x}}_{1}\right)，\\ {\dot{\hat{x} }}_{2}={f}_{d}+{b}_{1}{\tau }_{N}+{\hat{d}}_{N}-{k}_{2}{\mathrm{fal}}(\left({x}_{1}-{\hat{x}}_{1}\right),{\alpha }_{1},\delta )，\\ {\dot{\hat{d} }}_{N}=-{k}_{3}{\mathrm{fal}}(\left({x}_{1}-{\hat{x}}_{1}\right),{\alpha }_{2},\delta )。\end{array}\right. $

(20)

$ \left\{ \begin{array}{l} {\dot{\hat{x} }}_{3}={\hat{x}}_{4}-{k}_{4}\left({x}_{4}-{\hat{x}}_{4}\right)，\\ {\dot{\hat{x} }}_{4}={f}_{r}+{b}_{2}{\tau }_{M}+{\hat{d}}_{M}-{k}_{5}{\mathrm{fal}}(\left({x}_{4}-{\hat{x}}_{4}\right),{\alpha }_{3},\delta )，\\ {\dot{\hat{d} }}_{M}=-{k}_{6}{\mathrm{fal}}(\left({x}_{4}-{\hat{x}}_{4}\right),{\alpha }_{4},\delta )。\end{array} \right.$

(21)

$ \left\{ \begin{array}{l} {\dot{\hat{x} }}_{5}={\hat{x}}_{6}-{k}_{7}\left({x}_{5}-{\hat{x}}_{5}\right)，\\ {\dot{\hat{x} }}_{6}={f}_{p}+{b}_{3}{\tau }_{M}+{\hat{d}}_{K}-{k}_{8}{\mathrm{fal}}(\left({x}_{5}-{\hat{x}}_{5}\right),{\alpha }_{5},\delta )，\\ {\dot{\hat{d} }}_{K}=-{k}_{9}{\mathrm{fal}}(\left({x}_{5}-{\hat{x}}_{5}\right),{\alpha }_{6},\delta )。\end{array} \right.$

(22)

式中：$ {\hat{x}}_{*} $为状态量估计值；$ {k}_{*} $为观测器的增益。$ fal\left(\cdot \right) $的具体表达式为：

$ {\mathrm{fal}}(e,\alpha ,\delta )=\left\{\begin{array}{l} |e{|}^{\alpha }\text{sign}(e), |e| \gt \delta，\\ \dfrac{e}{{\delta }^{1-\alpha }}, |e| \leqslant \delta 。\end{array}\right. $

(23)

2.3 舵力分配

由式(15)～式(17)和式(20)，AUV的ESO−LQR控制律为：

$ \left\{ \begin{array}{l} {\tau }_{K}={k}_{p}p+{k}_{\varphi }(\varphi -{\varphi }_{d})+{\hat{d}}_{K}，\\ {\tau }_{M}={k}_{w}w+{k}_{q}q+{k}_{\theta }\theta +{k}_{\zeta }(\zeta -{\zeta }_{\zeta })+{\hat{d}}_{M}，\\ {\tau }_{N}={k}_{v}v+{k}_{r}r+{k}_{\psi }(\psi -{\psi }_{d})+{\hat{d}}_{N}。\end{array} \right.$

(24)

通过伪逆法控制分配给4个舵角，公式为：

$ \left\{\begin{aligned} &\delta_k = {\boldsymbol{B}}^{\mathrm{T}} \left( {\boldsymbol{BB }}^{\mathrm{T}} \right)^{-1} \tau，\text{rank}({\boldsymbol{B}}) = \text{rank}({\boldsymbol{B}}, \tau) ，\\ &\delta_k = \left( {\boldsymbol{B B}}^{\mathrm{T}} \right)^{-1} {\boldsymbol{B}}^{\mathrm{T}} \tau，\text{rank}({\boldsymbol{B}}) \neq \text{rank}({\boldsymbol{B}}, \tau) 。\end{aligned}\right. $

(25)

式中：δ为舵角向量，表示为：

$ \delta=[\delta_1 \;\delta_2 \;\delta_3 \;\delta_4]^{\mathrm{T}}。$

(26)

$ {\delta }_{1}、{\delta }_{2}、{\delta }_{3}、{\delta }_{4} $分别为舵1、舵2、舵3、舵4的舵角，舵角约束在：$ \left[-30,\; 30\right] $，式(25)中的B矩阵为：

$ {\boldsymbol{B}} = \left[ {\begin{array}{*{20}{c}} {{k_1}{V^2}{l_{{k_1}}}}&{{k_2}{V^2}{l_{{k_2}}}}&{{k_3}{V^2}{l_{{k_3}}}}&{{k_4}{V^2}{l_{{k_4}}}} \\ {{k_1}{V^2}{l_{{m_1}}}}&{{k_2}{V^2}{l_{{m_2}}}}&{{k_3}{V^2}{l_{{m_3}}}}&{{k_4}{V^2}{l_{{m_4}}}} \\ {{k_1}{V^2}{l_{{n_1}}}}&{{k_2}{V^2}{l_{{n_2}}}}&{{k_3}{V^2}{l_{{n_3}}}}&{{k_4}{V^2}{l_{{n_4}}}} \end{array}} \right]。$

(27)

式中：$ {k}_{1}、{k}_{2}、{k}_{3}、{k}_{4} $分别为舵1、舵2、舵3、舵4的舵力系数；V为水下航行器的合速度。

2.4 稳定性分析

证明运动误差$ \left({\varphi }_{e},{\theta }_{e},{\psi }_{e}\right) $在动力学控制器可以渐进收敛到0。证明如下：

定义航向通道扩张状态观测器观测误差为$ {e}_{1}= {x}_{1}-{\hat{x}}_{1}，{e}_{2}={x}_{2}-{\hat{x}}_{2}，{\tilde{d}}_{N}={d}_{N}-{\hat{d}}_{N} $，由式(20)得

$ \left\{\begin{aligned} &{\dot{e}}_{1}={\dot{x}}_{1}-{\hat{\dot{x} }}_{1}={e}_{2}+{k}_{1}{e}_{1}，\\ & {\dot{e}}_{2}={\dot{x}}_{2}-{\hat{\dot{x} }}_{2}\text={\tilde{d}}_{N}+{k}_{2}{\mathrm{fal}}\left({e}_{1},{\alpha }_{1},\delta \right)，\\ &{\dot{\tilde{d} }}_{N}=-{\dot{\hat{d} }}_{N}={k}_{3}{\mathrm{fal}}\left({e}_{1},{\alpha }_{2},\delta \right)。\end{aligned}\right. $

(28)

对上述误差系统，选取 Lyapunov 函数：

$ {V}_{1}=\frac{1}{2}e_{1}^{2}+\frac{1}{2}e_{2}^{2}+\frac{1}{2{k}_{3}}\tilde{d}_{N}^{2}。$

(29)

求导得：

$ \begin{split}{\dot{V}}_{1}=&{k}_{1}e_{1}^{2}+{e}_{1}{e}_{2}+{e}_{2}{\tilde{d}}_{N}+{k}_{2}{e}_{2}{\mathrm{fal}}\left({e}_{1},{\alpha }_{1},\delta \right)+\\ &{\tilde{d}}_{N}{\mathrm{fal}}\left({e}_{1},{\alpha }_{2},\delta \right)。\end{split} $

(30)

当$ \left| {e}_{1}\right| \gt \delta $时，$ {{\mathrm{fal}}}(e_1, \alpha, \delta) = |e_1|^\alpha \text{sign}(e_1) $，可得：

$ {\dot{V}}_{1} \leqslant -{c}_{1}{\left| {e}_{1}\right| }^{1+{{\alpha }_{2}}}-{c}_{2}e_{2}^{2} \leqslant 0。$

(31)

当$ \left| {e}_{1}\right| \leqslant \delta $时，$ {\mathrm{fal}}\left({e}_{1},\alpha ,\delta \right)=\displaystyle\frac{{e}_{1}}{{\delta }^{1-\alpha }} $，同样可取增益满足$ {k}_{2}和{k}_{3} $足够大，使得：

$ {\dot{V}}_{1} \leqslant -{c}_{3}e_{1}^{2}-{c}_{4}e_{2}^{2} \leqslant 0。$

(32)

则当$ t\rightarrow \infty \mathrm{时}{，e}_{1},{e}_{2},{\tilde{d}}_{N}\rightarrow 0 $。其他通道的观测器证明相似。姿态跟踪误差表示为：

$ \left\{\begin{aligned} &{\varphi }_{e}=\varphi -{\varphi }_{d}，\\ & {\theta }_{e}=\theta -{\theta }_{d}，\\ &{\psi }_{e}=\psi -{\psi }_{d}。\\ \end{aligned}\right. \quad \left\{\begin{aligned} &{p}_{e}=p-{p}_{d}，\\ &{q}_{e}=q-{q}_{d}，\\ &{r}_{e}=r-{r}_{d}。\\ \end{aligned}\right. $

(33)

假设　期望AUV以安全的姿态定深定航向航行，则$ {\varphi }_{d}=0、{\dot{\varphi }}_{d}=0、{\dot{\psi }}_{d}=0、{\dot{\theta }}_{d}=0 $，稳态下的运动角速度为0，即$ {p}_{d}=0、{q}_{d}=0、{r}_{d}=0 $，设Lyapunov方程为：

$ V=\frac{1}{2}\left(\theta _{e}^{2}+\psi _{e}^{2}+\varphi _{e}^{2}+p_{e}^{2}+q_{e}^{2}+r_{e}^{2}\right)。$

(34)

V正定，根据辅助方程、控制律，求导$ \dot{V} $为：

$ \begin{split}\dot{V}=&\;{\theta }_{e}{\dot{\theta }}_{e}+{\psi }_{e}{\dot{\psi }}_{e}+{\varphi }_{e}{\dot{\varphi }}_{e}+p\dot{p}+q\dot{q}+r\dot{r}=\\ &\;{\theta }_{e}q-\frac{{k}_{\theta }}{{I}_{yy}}{\theta }_{e}q+{\psi }_{e}r-\frac{{k}_{\psi }}{{I}_{zz}}{\psi }_{e}r+{\varphi }_{e}p-\\ &\frac{{k}_{\varphi }}{{I}_{xx}}{\varphi }_{e}p-\frac{{k}_{p}}{{I}_{xx}}{p}^{2}-\frac{{k}_{q}}{{I}_{yy}}{q}^{2}-\frac{{k}_{r}}{{I}_{zz}}{r}^{2}\text+\\ &\frac{{\tilde{d}}_{K}}{{I}_{xx}}p+\frac{{\tilde{d}}_{M}}{{I}_{yy}}q+\frac{{\tilde{d}}_{N}}{{I}_{zz}}r。\end{split} $

(35)

通过选择合适系数$ {k}_{i}\left(i=\theta ,\psi ,\varphi \right) $，使得：

$ 1-\frac{{k}_{\theta }}{{I}_{yy}} \lt 0,1-\frac{{k}_{\psi }}{{I}_{zz}} \lt 0,1-\frac{{k}_{\varphi }}{{I}_{xx}} \lt 0 $

(36)

交叉项抵消，$ {k}_{p}、{k}_{q}、{k}_{r} $均大于0使得正定项大于0，得$ \dot{V} \leqslant 0 $，当且仅当$ {\theta }_{e}、{\psi }_{e}、{\varphi }_{e}、{p}_{e}、{q}_{e}、{r}_{e} $均等于0时，$ V=0 $，否则$ V \gt 0 $。综上所述，运动误差可以渐进收敛到0，系统稳定。

2.5 改进鲸鱼优化算法

鲸鱼优化算法（WOA）在初始化种群时随机生成Q、R矩阵。然而，LQR控制器的稳定性要求Q矩阵必须半正定，R矩阵必须正定。若随机生成的Q矩阵不满足半正定条件或R矩阵不满足正定条件，则对应的控制器参数组合无法保证系统稳定导致算法在无效解空间中进行搜索，浪费计算资源，并可能阻碍算法收敛到真正有效且性能优良的解。因此，为了保证控制器的稳定性，避免在无效参数空间上的搜索，在WOA的优化流程中加入Q、R矩阵的数学属性约束机制。

改进WOA算法步骤如图7所示。种群随机参数：如果Q、R矩阵的特征值小于0则重新随机赋值直至矩阵正定，经过系统模型计算目标函数。迭代开始：开始计算参数a以及系数向量A、C，当概率p<0.5时，开始按气泡网捕食行为更新位置信息；否则判断系数向量绝对值A是否小于1，如果是，按围捕猎物更新位置信息，否则按随机搜索猎物更新位置信息，直至迭代次数达到最大值。

WOA算法分为围捕猎物、气泡网捕食、搜索猎物3个步骤。围捕猎物的位置更新：

$ \left\{\begin{aligned} &D = \left| C \cdot X^*(t) - X(t) \right|^{\mathrm{T}}，\\ &X(t+1)=X^*(t)-A\cdot D。\end{aligned}\right.$

(37)

式中：t为迭代次数；$ {X}^*\left(t\right) $为目前的最优位置向量；A和C均为系数向量，表示为：

$ \left\{\begin{aligned} &A = 2a \times r_1 - a，\\ & C = 2 \times r_2。\end{aligned}\right.$

(38)

在整个迭代过程中a由2线性降到0；$ {r}_{1} $和$ {r}_{2} $是范围$ \left[0,\;1\right] $的随机向量。

气泡网捕食阶段：采用气泡网捕食时，座头鲸与猎物间的位置更新用对数螺旋方程表达：

$ \left\{\begin{aligned} &X(t + 1) = D^* \times e^{j\theta} \cos(2\text{π} t) + X^*(t)，\\ & D' = \left| X^*(t) - X(t) \right|。\end{aligned}\right. $

(39)

式中：$D' $为当前搜索个体与当前最优解的距离；b为螺旋形状参数；l为值域为$ \left[−1{,}1\right] $均匀分布的随机数。WOA根据概率p来选择气泡网捕食或者收缩包围，位置更新公式为：

$ X(t+1)= \left\{\begin{aligned} &X^*(t) - A \cdot D，p \leqslant 0.5，\\ &D' \times e^{bl} \cos(2\text{π} l) + X^*(t)，p \geqslant 0.5。\end{aligned}\right. $

(40)

式中：p为捕食机制的概率，值域为[0，1]的随机数。搜索猎物阶段位置更新公式为：

$ \left\{\begin{aligned} &D{\mathrm{''}} = \left| C \cdot X_{\text{rand}}(t) - X(t) \right| ,\\ &X(t + 1) = X_{\text{rand}}(t) - A \cdot D{\mathrm{''}}。\end{aligned}\right.$

(41)

式中：$ D'' $为当前搜索个体与随机个体的距离。

AUV在未知扰动下的稳定控制，会引起舵机频繁纠偏动作，从而导致能耗增加。而过分降低操舵频次，也会引起跟踪控制误差偏离，因此针对AUV在未知扰动下的多目标约束优化问题，引入积分时间误差积分$ {e}_{\text{ITAE}} $和稳定状态下的激增能耗$ {E}_{i} $作为性能指标^{[20 - 21]}，如下式：

$ E={\lambda }_{1}{e}_{\mathrm{ITAE}}+{\lambda }_{2}{E}_{i}。$

(42)

式中：$ {\lambda }_{1}、{\lambda }_{2} $为各项指标的调节系数。假定$ {P}_{i} $舵机反转的瞬间功率为常值，$ {E}_{i} $为：

$ {E}_{i}={P}_{i}\Delta t{\delta }_{\text{num}}。$

(43)

式中：$ {\delta }_{{\mathrm{num}}} $为动舵次数，即$ \delta _{{n}}^{\mathrm{'}}=0 $且$ \delta _{{n}-1}^{\mathrm{'}}\cdot\delta _{{n}+1}^{\mathrm{'}}< 0 $时计数一次，$ \Delta t $为舵机转子反转的时间，一般取0.3 s。

3 仿真实验 3.1 案例1

为了验证所提控制器在AUV未知干扰和耦合效应干扰下的有效性，分别比较LQR无横滚控制、LQR以及ESO−LQR展开随机干扰下的航向、深度跟踪误差。仿真时间为25 s，转速从0开始加速至1600 r/s；深度从0 m下潜至5 m并保持在5 m；期望航向定为15°。向模型中注入随机干扰，功率0.2，采样时间0.1 s。实验结果如图8～图10所示。分析图8可知，在航行稳定后，具有横滚控制的LQR和ESO−LQR能够有效将横滚角约束在1°内，能够有效抑制横滚的影响；对比图9、图10的LQR无横滚控制和LQR的航向、深度曲线，LQR控制器的航向跟踪误差和深度跟踪误差都小于LQR无横滚控制。这是由于受横滚耦合影响，加剧了AUV航向角和深度的不稳定；而ESO−LQR的航向、深度跟踪误差最小，并且稳态下的抖振最小，控制稳定。3种控制器具体性能指标如表3所示，ESO−LQR在所示的各项指标性能最好。

3.2 案例2

案例1中，针对横滚耦合干扰以及非线性未知海流干扰影响或模型不确定性问题展开实验，所提出ESO−LQR控制方法能够辨识未知干扰，提高跟踪稳定性。然而，AUV稳定控制意味着舵机频繁纠正动作导致能耗的增加。因此，本案例针对舵机频繁纠正动作导致的能耗问题对ESO−LQR进行优化。

设置仿真时间50 s，期望深度5 m，期望航向角0°，优化种群数100，迭代50次。舵角运动曲线如图11所示，以舵1为例，ESO−LQR控制下存在微小抖振，WESO−LQR舵角响应更平缓。控制参数优化前后的对比见表2。舵机动作次数如图12所示，WESO−LQR除舵1打舵频次大幅减少外，其他舵的动舵频次都有不同程度的减少。另外，控制器的性能指标和评价函数如表4所示，WESO−LQR的4个舵机总打舵次数为599次，比ESO−LQR的总打舵次数减少113次；WESO−LQR评价指标较ESO−LQR减少了15.57%。

4 结　语

1）在案例1中，所提出的LQR横滚控制算法，能够有效的约束横滚角，LQR的平均横滚角比LQR无横滚控制降低了96.12%；同时，航向、深度的绝对时间误差分别降低了约23.26%和19.57%，实现横滚控制的同时，提升了航向、深度的控制精度。

2）在案例1中，ESO−LQR的航向和深度绝对时间跟踪误差对比LQR分别降低了19.3%和7.91%，进一步解决了非线性未知海流和模型不确定性的问题，提升了航向和深度的控制精度。

3）在案例2中，WESO−LQR的总打舵频次比ESO−LQR减少了约15.8%，评价指标减少了15.57%。在保持稳定航行的同时，减少打舵频次，从而有效提高航行能效。