0 引　言

部分自主水下机器人（Autonomous Underwater Vehicle，AUV）会安装超过其自身自由度数量的控制设备（如槽道桨、X型舵等）以满足水下探测与作业在精度方面的要求^{[1 − 3]}，这些冗余的推进器共同构成了过驱动系统。过驱动系统为AUV提供了较高的机动性和可靠性，也为AUV带来了推力分配问题。

如图1所示，在航行过程中，AUV的控制算法会根据自身的位置、期望的位置、速度、当前的海洋环境等信息给出航行器所需的力和力矩。推力分配算法则旨在将航行器多个自由度所需的力和力矩合理分配到各个推进器上，使航行器切实获取到控制算法计算出的力和力矩。当控制算法给出的推力与力矩较小时，求出可行的解并不困难，此时推力分配算法应聚焦于如何求出使整体能耗更少的解。当控制算法给出的推力与力矩较大时，求出的解可能超过推进器的推力限制（或舵的舵角限制），称之为推力饱和问题，此时推力分配算法应聚焦于如何合理调节舵角和各个推进器的推力以尽可能使实际值接近目标值。

广泛应用于推力分配上的算法主要为直接法^[4]和伪逆法^[5]。直接法通过人为指定各个推进器在推力分配中的占比来增加额外约束从而进行求解。如李新飞等^[6]在直接逻辑分配法中额外引入了各个推进器在单一方向上提供相同大小的力这一条件进行求解。直接法算法简洁可靠，但在节能方面效果较差且无法解决推力饱和问题。伪逆法以各推进器的推力为目标函数，可以计算出在需求的推力和力矩约束下使目标函数达到最小值的解。但伪逆法难以将各个推进器的约束限制纳入计算，无法实现可达集的全覆盖^[7]。使用伪逆法进行求解在遇到推力饱和问题时需要对求解结果进行放缩，放缩得到的结果与需求值有着较大差距且未能完全发挥推进器的效能。在伪逆法的基础上，张瀚文等^[8]通过更改伪逆法的权重系数对ROV进行容错推力分配，Sun等^[9]使用多步分配法对X舵ROV进行推力分配，孙功武等^[10]将模糊控制与伪逆法相结合对电动-液压混合推进型ROV进行推力分配。经过改进的伪逆法可实现更复杂的需求，但难以在算法中将推进器推力大小、推进角度限制纳入计算。二次规划算法^[11]和序列二次规划算法^[12]能够寻找在可行域约束下的最优解，但对可行域和约束条件均有一定要求。智能算法的适应范围更加广泛，能在计算时考虑更加复杂的约束条件，如Zhao等^[13]使用自适应遗传算法设计半潜船动力定位系统，徐海阳等^[14]提出的基于极限学习机的推力分配方法等。

本文针对某型过驱动AUV，根据其水动力系数建立动力学及运动学仿真模型，利用其推进器的空间布置及舵效参数构建推力转换矩阵，使用反步法对仿真模型进行控制以获取航行器在一段时间内的推力和力矩需求。在推力分配方面使用改进的广义既约梯度法（GRG）来求解推力分配问题。通过切换不同的目标函数和约束来分别获取能耗目标和差值目标下的最优解，并与伪逆法的求解结果进行对比。

1 推力分配问题 1.1 研究对象

某型AUV主体结构如图2所示，其分别在艏部和艉部各布置有1个垂推和1个侧推，推进器形式为槽道浆，艉部带有主推，舵为十字型舵，无差分控制功能。针对某型AUV，其在横滚方向能够自平衡，忽略横滚方向自由度，推力分配算法的控制输入为由控制器产生的5个自由度方向AUV所需的力与力矩，控制输出为4个槽道推进器、十字舵及主推，输入变量个数小于输出变量，为过驱动系统。

1.2 问题描述

在AUV航行过程中，当控制器给出的推力和扭矩不会造成推力饱和时，推力分配算法应当在满足控制器要求和推进器推力限制的前提下，尽可能输出使得整体能耗最小的解。也即：

$\begin{split} \text{min} \quad & f_p(x) = \sum p_i(x_i), \quad i=3, \ldots, 7 \\ \text{s.t.} \quad & h_j(x) = d_j, \quad j = 1, \ldots, 5 \\ & {\boldsymbol{l}} \leqslant {\boldsymbol{x}} \leqslant {\boldsymbol{u}}。\end{split}$

(1)

针对某型号AUV，定义$\boldsymbol{x}={\left[{x}_{1},\dots {x}_{7}\right]}^{\rm T}$分别为水平舵侧向力、垂直舵侧向力、艏垂推推力、艏侧推推力、艉垂推推力、艉侧推推力和主推推力，${p}_{i}\left({x}_{i}\right)$为各推进器所消耗功率，${{\boldsymbol{h}}}_{j}\left(x\right)$为舵和推进器在进退、横移、升沉、纵倾、回转5个自由度方向上所产生的力和力矩，${{\boldsymbol{d}}}_{j}$为期望获得的力与力矩，$\boldsymbol{l}$和$\boldsymbol{u}$为舵和推进器推力的上下限。

当控制器给出的力超出推进器所能达到的极限时，${h}_{j}\left(x\right)=d$在$\mathit{l}\leqslant \mathit{x}\leqslant \mathit{u}$范围内无解，引入偏差量$\boldsymbol{s}={\left[{s}_{1},\dots {s}_{5}\right]}^{\rm T}$，此时推力分配算法的优化目标应使偏差量达到最小，即：

$\begin{split} \text{min} \quad & f_s(s) = {\boldsymbol{s}}^{\rm T}{\boldsymbol{s}} ,\\ \text{s.t.} \quad & h_j(x) +s_j= d, \quad j = 1, \ldots, 5 \\ & {\boldsymbol{l}} \leqslant {\boldsymbol{x}} \leqslant {\boldsymbol{u}}。\end{split}$

(2)

1.3 伪逆法

忽略舵所产生的纵向力，以航行器和舵推力的平方和作为最小化目标，假设不存在舵角和推进器推力限制，式(1)可化为：

$\begin{split} \text{min} \quad & f_p(x) = {\boldsymbol{x}}^{\rm T}{\boldsymbol{Wx}}, \\ \text{s.t.} \quad & {\boldsymbol{Bx}} ={\boldsymbol{d}}。\end{split}$

(3)

式中：$\boldsymbol{B}$为推力系数矩阵。

通过拉格朗日乘子法求取$\boldsymbol{B}$的伪逆矩阵${\boldsymbol{B}}_{\mathit{w}}^{+}$，取$\mathit{W}=\mathit{I}$，则

${\boldsymbol{B}}_{\mathit{w}}^+={\boldsymbol{B}}^{\rm T}{\left(\boldsymbol{B}{\boldsymbol{B}}^{\rm T}\right)}^{-1} ,$

(4)

解出

$\mathit{x}={\boldsymbol{B}}_{\mathit{w}}^+\mathit{d}。$

(5)

2 与灰狼优化算法相结合的广义既约梯度法

各种优化算法均有其适用条件，如伪逆法不适用于不等式约束问题，线性二次规划法不适用于非线性约束问题等。相比于其他优化算法，广义既约梯度法的适用范围更广，其等式约束可为非线性方程，迭代求解时可考虑变量的上下限，是求解非线性规划问题的最有效的方法之一^[15]。使用广义既约梯度法的求解流程如图3所示。

具体计算步骤如下^[15]（以式(1)为例）：

步骤1　用伪逆法获取初始可行点${x}^{\left(1\right)}$，指定收敛条件$\varepsilon > 0$。

步骤2　将$x$划分为基变量${x}_{B}$与非基变量${x}_{N}$，当推力分配推力饱和时，取偏差量$s$为基变量，当推力分配未发生推力饱和时，取x₃～x₇为基变量，便于计算。

步骤3　计算$\mathit{h}\left(x\right)$的Jacobi矩阵和目标函数的微分

$\dfrac{\partial h}{\partial x} \stackrel{\text{def}}= \begin{bmatrix} \dfrac{\partial h_1}{\partial x_1} & \cdots & \dfrac{\partial h_1}{\partial x_7} \\ \vdots & \ddots & \vdots \\ \dfrac{\partial h_5}{\partial x_1} & \cdots & \dfrac{\partial h_5}{\partial x_7} \end{bmatrix} = \left( \dfrac{\partial h}{\partial x_B}, \dfrac{\partial h}{\partial x_N} \right)，$

(6)

${\nabla }_{{\mathit{x}}_{\mathit{B}}}{\mathit{f}}_{\mathit{p}}={\left(\frac{\partial {f}_{p}}{\partial {x}_{3}},\dots ,\frac{\partial {f}_{p}}{\partial {x}_{7}}\right)}^{\rm T} ,$

(7)

${\nabla }_{{\mathit{x}}_{\mathit{N}}}{\mathit{f}}_{\mathit{p}}={\left(\frac{\partial {f}_{p}}{\partial {x}_{1}},\frac{\partial {f}_{p}}{\partial {x}_{2}}\right)}^{\rm T}。$

(8)

步骤4　计算既约梯度

${r}\left({\mathit{x}}_{\mathit{N}}\right)=\frac{\mathrm{d}{\mathit{f}}_{\mathit{p}}}{\mathrm{d}{\mathit{x}}_{\mathit{N}}}={\nabla }_{{\mathit{x}}_{\mathit{N}}}{\mathit{f}}_{\mathit{p}}-\left[{\left(\frac{\partial \mathit{h}}{\partial {\mathit{x}}_{\mathit{B}}}\right)}^{-1}\frac{\partial \mathit{h}}{\partial {\mathit{x}}_{\mathit{N}}}\right]{\nabla }_{{\mathit{x}}_{\mathit{B}}}{\mathit{f}}_{\mathit{p}}。$

(9)

步骤5　获取搜索方向${d}_{N}$（式(39)中$k$代表第$k$个非基变量）

${\mathit{d}}_{\mathit{N}}=\left\{\begin{aligned} &0, \begin{array}{l}{{x}_{N}}_{k}={l}_{{N}_{k}}且{r}_{k}\left(x_N\right) > 0\\ 或{{x}_{N}}_{k}={u}_{{N}_{k}}\mathrm{且}{r}_{k}\left({x}_{N}\right) < 0，\end{array}\\ &-{r}_{k}\left({x}_{N}\right)，其他。\end{aligned}\right.$

(10)

步骤6　当$\|{\mathit{d}}_{\mathit{N}}\| < \varepsilon$时，停止计算，否则沿${\mathit{d}}_{\mathit{N}}$方向进行一维搜索，获取到$\mathit{x}$取值范围内使目标函数值更小的可行解，返回步骤3。

${\overline{\mathit{x}}}_{\mathit{N}}={\mathit{x}}_{\mathit{N}}+\lambda {\mathit{d}}_{\mathit{N}} 。$

(11)

原广义既约梯度法在步骤6进行一维搜索时使用方法较为低效^[15]，先取$\lambda ={\lambda }_{{\mathrm{max}}}$进行计算，若结果不符合条件，则以$0.5\lambda$代替$\lambda$再次进行计算，反复迭代直至获取符合判定条件的解。为提高一维搜索效率，减少整体迭代次数，在此引入灰狼优化算法进行计算，灰狼优化算法计算该问题的步骤如下^[16]：

步骤1　初始化“狼”的位置$\mathit{w}$：

${\mathit{w}}_{\mathit{r}}={\mathit{x}}_{\mathit{N}}+\left(r-1\right){\lambda }_{{\mathrm{max}}}{\mathit{d}}_{\mathit{N}}/num 。$

(12)

式中：$r$为“狼”的序号，$r=[\mathrm{1,2},3,\dots ,num]$；$num$为“狼群”数量。

步骤2　计算“狼”适应度，选出对应目标函数${f}_{p}$最小的前3只“狼”，记为${w}_{\alpha }、{w}_{\beta }、{w}_{\delta }$。

步骤3　生成系数矩阵$\boldsymbol{A}$和位移矢量$\boldsymbol{D}$

${{\boldsymbol{A}}}_{t}={a}_{t}(2{r}_{1}-1) ，$

(13)

${{\boldsymbol{D}}}_{t}=\left|2{r}_{2}{w}_{t}-{w}_{r}\right|。$

(14)

式中：${r}_{1}、{r}_{2}$为$\left[\mathrm{0,1}\right]$上的随机数；$t$为$\alpha 、\beta 、\delta$，$a$为从2随迭代次数线性递减到0的系数。

步骤4　更新除${w}_{\alpha }、{w}_{\beta }、{w}_{\delta }$外其他“狼”的位置${w}_{r}$

${w}_{r}=\frac{1}{3}(\sum {w}_{t}-\sum {A}_{t}{D}_{t})。$

(15)

3 某型AUV仿真建模 3.1 某型AUV坐标系定义

参照潜艇体系^[17]建立右手坐标系如图4所示。图中$E-XYZ$为定系，$B-xyz$为动系。设动系的原点$B$与AUV的浮心重合，随AUV移动而移动，$Bx$轴指向AUV正前方，$Bz$轴指向AUV正下方，$By$轴按照右手法则确定方向。定系与动系在AUV起始点处重合。六自由度运动及AUV位置的符号定义如下：

X、Y、Z分别为AUV浮心在定系下的纵向、横向、垂向坐标；$\psi 、\theta 、\varphi$分别为AUV的偏航角、俯仰角、横滚角；$u\mathrm{、}v\mathrm{、}w$分别为AUV沿$Gx\mathrm{、}Gy\mathrm{、}Gz$轴的进退、横移、升沉运动速度；$p\mathrm{、}q\mathrm{、}r$分别为AUV沿$Gx\mathrm{、}Gy\mathrm{、}Gz$轴的横倾、纵倾、回转角速度。

3.2 运动学及动力学方程

某型AUV在横倾方向能自平衡，忽略横滚方向自由度，建立AUV五自由度运动学方程^[17]如下：

${{\mathit{R}}_{\mathit{b}}^{\mathit{n}}=\left[\begin{array}{ccc}\cos\psi \cos\theta & -\sin\psi \cos\varphi +\cos\psi \sin\theta \sin\varphi \\ & \sin\psi \sin\varphi +\cos\psi \sin\theta \cos\varphi \\ \sin\psi \cos\theta & \cos\psi \cos\varphi +\sin\psi \sin\theta \sin\varphi \\ & -\cos\psi \sin\varphi +\sin\psi \sin\theta \cos\varphi \\ -\sin\theta & \cos\theta \sin\varphi \\ & \cos\theta \cos\varphi \end{array}\right] ，}$

(16)

${\mathit{T}}_{\mathbf{\Theta }}=\left[\begin{array}{cc}\mathrm{sin}\varphi \mathrm{tan}\theta & \mathrm{cos}\varphi \mathrm{tan}\theta \\ \mathrm{cos}\varphi & -\mathrm{sin}\varphi \\ \mathrm{sin}\varphi /\mathrm{cos}\theta & \mathrm{cos}\varphi /\mathrm{cos}\theta \end{array}\right] ，$

(17)

$\mathit{J}\left(\mathit{\eta }\right)=\left[\begin{array}{cc}{\mathit{R}}_{\mathit{b}}^{\mathit{n}}& 0\\ 0& {\mathit{T}}_{\mathit{\Theta }}\end{array}\right]，$

(18)

$\dot{\mathit{\eta }}=\mathit{J}\left(\mathit{\eta }\right)\mathit{V}，$

(19)

$\mathit{\eta }={\left[\begin{array}{ccccc}X& Y& Z& \theta & \psi \end{array}\right]}^{\rm T}，$

(20)

$\mathit{V}={\left[\begin{array}{ccccc}u& v& w& q& r\end{array}\right]}^{\rm T} 。$

(21)

建立AUV五自由度动力学方程^[18]如下：

$\left(m+{\lambda }_{11}\right)\dot{u}= {T}_{x}-\frac{1}{2}\rho {U}^{2}S({{C}_{xS}-C}_{x}^{{\delta }_{r}}{\delta }_{r}^{2}-{C}_{x}^{{\delta }_{e}}{\delta }_{e}^{2})-\Delta G\mathrm{sin}\theta ，$

(22)

${\begin{split}\left(m + {\lambda }_{22}\right)\dot{v} + \left(m{x}_{g} - {\lambda }_{26}\right)\dot{r} = & {T}_{y} - mur + \frac{1}{2}\rho {U}^{2}S ({C}_{y}^{\beta }\beta + {C}_{y}^{{\delta }_{r}}{\delta }_{r} + \\ & {C}_{y}^{\stackrel-{r}}\stackrel-{r}) +\Delta G\cos\theta \sin\varphi，\end{split}}$

(23)

${\begin{split}\left(m + {\lambda }_{33}\right)\dot{w} - \left(m{x}_{g} + {\lambda }_{35}\right)\dot{q} = & {T}_{z}+muq - \frac{1}{2}\rho {U}^{2}S ({C}_{z}^{\alpha }\alpha + {C}_{z}^{{\delta }_{e}}{\delta }_{e} + \\ & {C}_{z}^{\stackrel-{q}}\stackrel-{q}) +\Delta G\mathrm{cos}\theta \mathrm{cos}\varphi ，\end{split}}$

(24)

${\begin{split} \left(J_{y y} + \lambda_{55}\right) \dot{q} - \left(m x_{g} + \lambda_{53}\right) \dot{w} = M_{y} - m x_{g} u q + \dfrac{1}{2} \rho U^{2} S L\left(m_{y}^{\alpha} \alpha + \right. \\ \left.m_{y}^{\delta_{e}} \delta_{e}+m_{y}^{q} \bar{q}\right)-G\left(z_{g} \sin \varphi+x_{g} \cos \theta \cos \varphi\right)，\end{split}}$

(25)

${\begin{split}\left({J}_{zz} + {\lambda }_{66}\right)\dot{r} + \left(m{x}_{g} - {\lambda }_{62}\right)\dot{v} = {M}_{z} - m{x}_{g}ur - \dfrac{1}{2}\rho {U}^{2}SL\left({m}_{z}^{\beta }\beta + \right.\\ \left. {m}_{z}^{{\delta }_{r}}{\delta }_{r}+{m}_{z}^{\stackrel-{r}}\stackrel-{r}\right) +G\left({x}_{g}\cos\theta \sin\varphi +{y}_{g}\sin\theta \right) 。\end{split}}$

(26)

对式(22)～式(26)进行整合，如式(27)～式(30)所示。

$\begin{array}{c}\mathit{M}=\\ \left[\begin{array}{ccccc} m+{\lambda }_{11}& 0& 0& 0& 0 \\ 0& m+{\lambda }_{22}& 0& 0& m{x}_{g}-{\lambda }_{26} \\ 0& 0& m+{\lambda }_{33}& -\left(m{x}_{g}+{\lambda }_{35}\right)& 0 \\ 0& 0& -\left(m{x}_{g}+{\lambda }_{53}\right)& {J}_{yy}+{\lambda }_{55}& 0 \\ 0& m{x}_{g}-{\lambda }_{62}& 0& 0& {J}_{zz}+{\lambda }_{66} \end{array}\right] ，\end{array}$

(27)

${\mathit{\tau }}_{\mathit{G}}=\left[\begin{array}{c}-\Delta G\sin\theta \\ \Delta G\cos\theta \sin\varphi \\ \Delta G\cos\theta \cos\varphi \\ -G({z}_{g}\sin\varphi +{x}_{g}\cos\theta \cos\varphi )\\ G\left({x}_{g}\cos\theta \sin\varphi +{y}_{g}\sin\theta \right)\end{array}\right]，$

(28)

${\mathit{\tau }}_{\mathit{F}}=\left[\begin{array}{c}-{C}_{xS}\dfrac{1}{2}\rho {U}^{2}S\\ -mur+\dfrac{1}{2}\rho {U}^{2}S\left({C}_{y}^{\beta }\beta +{C}_{y}^{\stackrel-{r}}\stackrel-{r}\right)\\ muq-\dfrac{1}{2}\rho {U}^{2}S\left({C}_{z}^{\alpha }\alpha +{C}_{z}^{\stackrel-{q}}\stackrel-{q}\right)\\ -m{x}_{g}uq+\dfrac{1}{2}\rho {U}^{2}SL\left({m}_{y}^{\alpha }\alpha +{m}_{y}^{\stackrel-{q}}\stackrel-{q}\right)\\ -m{x}_{g}ur-\dfrac{1}{2}\rho {U}^{2}SL\left({m}_{z}^{\beta }\beta +{m}_{z}^{\stackrel-{r}}\stackrel-{r}\right)\end{array}\right]，$

(29)

${\mathit{\tau }}_{\mathit{T}}=\left[\begin{array}{c}{T}_{x}+\dfrac{1}{2}\rho {U}^{2}S({C}_{x}^{{\delta }_{r}}{\delta }_{r}^{2}+{C}_{x}^{{\delta }_{e}}{\delta }_{e}^{2})\\ {T}_{y}+\dfrac{1}{2}\rho {U}^{2}S{C}_{y}^{{\delta }_{r}}{\delta }_{r}\\ {T}_{z}-\dfrac{1}{2}\rho {U}^{2}S{C}_{z}^{{\delta }_{e}}{\delta }_{e}\\ {M}_{y}+\dfrac{1}{2}\rho {U}^{2}SL{m}_{y}^{{\delta }_{e}}{\delta }_{e}\\ {M}_{z}-\dfrac{1}{2}\rho {U}^{2}SL{m}_{z}^{{\delta }_{r}}{\delta }_{r}\end{array}\right]。$

(30)

整合后式(22)～式(26)可写作：

$\dot{\mathit{V}}={\mathit{M}}^{-1}({\mathit{\tau }}_{\mathit{F}}+{\mathit{\tau }}_{\mathit{G}}+{\mathit{\tau }}_{\mathit{T}}) 。$

(31)

式中：$m$为AUV质量；${\lambda }_{ij}$为AUV在$i$方向上的运动为$j$方向带来的附加质量（$i\mathrm{、}j$取值为$1～6$；分别代表$u\mathrm{、}v\mathrm{、}w\mathrm{、}p\mathrm{、}q\mathrm{、}r$）；${J}_{yy}、{J}_{zz}$分别为AUV相对于$By、Bz$轴的惯性矩；${x}_{g}\mathrm{、}{y}_{g}\mathrm{、}{z}_{g}$为AUV重心在动系下的坐标；${\delta }_{e}$为水平舵舵角；${\delta }_{r}$为垂直舵舵角；$\rho$为海水密度；${U}^{2}={u}^{2}+{v}^{2}+{w}^{2}$；$\Delta G$为AUV重力与浮力的差值($\Delta G=G-B$）,$S$为AUV的横截面积；L为AUV长度；$\stackrel{-}{r}=rl/U$；$\stackrel{-}{q}=ql/U$；$\alpha = \mathrm{arctan}(w/u)$；$\beta =\mathrm{arctan} (v/\sqrt{{u}^{2}+{w}^{2}})$；T_x、T_y、T_z、M_y、M_z为五自由度方向由推进器产生的力；其余参数为水动力系数。

3.3 槽道推进器

普通螺旋桨的推力$T$、扭矩$Q$及功率$P$计算式为：

$T={K}_{T}\rho {n}^{2}{D}^{4},$

(32)

$Q={K}_{Q}\rho {n}^{2}{D}^{5} ,$

(33)

$\begin{array}{c}P =2{\text{π}} {K}_{Q}{T}^{1.5}/{K}_{T}^{1.5}{\rho }^{0.5}D。\end{array}$

(34)

式中：${K}_{T}$为推力系数；${K}_{Q}$为扭矩系数；$n$为转速；$D$为螺旋桨直径。

与常规螺旋桨不同，槽道推进器在受来流影响时会发生推力损失^[19]。为研究某型AUV在航行时槽道桨的推力与槽道桨所消耗的功率之间的关系，使用Fluent进行仿真。仿真结果归一化处理后如图5所示，在2 kn来流情况下，艏垂推所需求的扭矩仍和转速平方成正比，但其产生的推力不再与转速的平方成正比。

使用Fluent对其他槽道桨进行仿真，仿真结果显示，来流对艏垂推和艉侧推带来的推力影响较大，艏侧推和艉垂推位于平行中体附近，受来流影响较小，仍可按照式(34)通过推力计算功率大小。整理仿真结果绘制艏垂推和艉侧推功率随推力变化的曲线如图6所示。

3.4 运动控制器设计

航行时，推力分配算法需要根据控制器给出的推力与力矩需求进行推力分配计算，为获取某型AUV航行时的推力与力矩需求，现以反步法设计某型AUV的航行控制器。

反步控制是常用的AUV控制方法之一^{[20 − 22]}。其通过设计多级状态方程，逐级求解最后得出令非线性系统趋于稳定的控制输出，原理简单、实时性好。为获取AUV在航行时所需求的推力与力矩，本文基于李亚普洛夫理论，采用反步法设计某型AUV的五自由度控制器。

定义位置跟踪误差：

${{z}}_{1}=\mathit{\eta }-{\mathit{\eta }}_{\mathit{d}}。$

(35)

选取第1个李亚普洛夫函数：

${{V}}_{1}=\dfrac{1}{2}{{z}}_{1}^{\rm T}{{z}}_{1} 。$

(36)

求导得：

${\dot{\mathit{V}}}_{1}={z}_{1}^{\rm T}\times \left(\dot{\mathit{\eta }}-\dot{{\mathit{\eta }}_{\mathit{d}}}\right) ={z}_{1}^{\rm T}\times (\mathit{J}\left(\mathit{\eta }\right)\mathit{V}-\dot{{\mathit{\eta }}_{\mathit{d}}})。$

(37)

以速度为虚拟输入，镇定函数${z}_{\alpha }$设为：

${\mathit{z}}_{\mathit{\alpha }}=-\mathit{J}{\left(\mathit{\eta }\right)}^{-1}({\mathit{k}}_{1}{\mathit{z}}_{1}-\dot{{\mathit{\eta }}_{\mathit{d}}})。$

(38)

定义速度跟踪误差：

${\mathit{z}}_{2}=\mathit{V}-{\mathit{z}}_{\mathit{\alpha }}。$

(39)

选取第二个李亚普洛夫函数：

${\mathit{V}}_{2}={\mathit{V}}_{1}+\dfrac{1}{2}{\mathit{z}}_{2}^{\rm T}{\mathit{z}}_{2} 。$

(40)

求导得：

${\begin{split}\dot{{\mathit{V}}_{2}}= \text{-}{\mathit{k}}_{1}{\mathit{z}}_{1}^{\rm T}{\mathit{z}}_{1}+{\mathit{z}}_{2}^{\rm T}\left(\mathit{J}{\left(\mathit{\eta }\right)}^{\rm T}{\mathit{z}}_{1}+{\mathit{M}}^{-1}\left({\mathit{\tau }}_{\mathit{F}}+{\mathit{\tau }}_{\mathit{G}}+{\mathit{\tau }}_{{\rm T}}\right)-\dot{\mathit{\alpha }}\right)。\end{split}}$

(41)

取${\tau }_{T}$为控制输入，为使$\dot{{V}_{2}} < 0$恒成立，${\tau }_{T}$应满足

$\mathit{J}{\left(\mathit{\eta }\right)}^{\rm T}{\mathit{z}}_{1}+{\mathit{M}}^{-1}\left({\mathit{\tau }}_{\mathit{F}}+{\mathit{\tau }}_{\mathit{G}}+{\mathit{\tau }}_{\mathit{T}}\right)-\dot{\mathit{\alpha }}=-{\mathit{k}}_{2}{\mathit{z}}_{2}。$

(42)

解得

${\mathit{\tau }}_{\mathit{T}}=\mathit{M}\cdot\left(\dot{{\mathit{z}}_{\mathit{\alpha }}}-{\mathit{k}}_{2}{\mathit{z}}_{2}-\mathit{J}{\left(\mathit{\eta }\right)}^{\rm T}{\mathit{z}}_{1}\right)-{\mathit{\tau }}_{\mathit{F}}-{\mathit{\tau }}_{\mathit{G}}。$

(43)

使用反步法控制AUV仿真模型进行螺旋下潜，如图7所示，期望轨迹与实际轨迹吻合良好。

3.5 推力分配模型

根据某型AUV的推进器布局及舵效参数可得：

${h}_{j}\left(x\right)=\left\{\begin{array}{l}{x}_{7}-2({C}_{x}^{e}{x}_{1}^{2}/{\left({C}_{z}^{e}\right)}^{2}+{C}_{x}^{r}{x}_{2}^{2}/({{C}_{y}^{r})}^{2})/\rho {U}^{2}S,\\ {x}_{2}+{x}_{4}+{x}_{6},\\ {x}_{1}+{x}_{3}+{x}_{5},\\ L{m}_{y}^{e}x\left(1\right)/{C}_{y}^{e}-{e}_{3}{x}_{3}+{e}_{5}{x}_{5},\\ L{m}_{z}^{r}x\left(2\right)/{C}_{z}^{r}+{e}_{4}{x}_{4}-{e}_{6}{x}_{6}。\end{array}\right.$

(44)

式中：${e}_{3}\sim{e}_{6}$为各推进器到浮心纵向距离。为便于计算艏垂推和艉侧推功率与推力之间的关系，采用多项式对图6曲线进行拟合，结果如图8所示。设艏垂推和艉侧推拟合获取的多项式方程分别为${p}_{3}\left({x}_{3}\right)$和${p}_{6}\left({x}_{6}\right)$。艏侧推、艉垂推和主推的功率则采用式(34)进行计算，即${p}_{k}\left({x}_{k}\right)=2{\text{π}} {K}_{Qk}{x}_{k}^{1.5}/{K}_{Tk}^{1.5}{\rho }^{0.5}{D}_{k}$，k=4,5,6。

4 仿真验证

以反步法操控仿真模型，使其在纵向以2 kn航速前进，在横向和垂向沿正弦曲线运动，设计式(35)中的${\eta }_{d}$如下：

$\left\{\begin{array}{l}{\eta }_{d}\left(1\right)={v}_{x}t，\\ {\dot{\eta }}_{d}\left(2\right)=\left(2A{\text{π}} \cos\right(2{\text{π}} t/T\left)\right)/T，\\ {\eta }_{d}\left(3\right)=A\sin(2{\text{π}} t/T)，\\ {\eta }_{d}\left(4\right)=-a\tan\left(2A{\text{π}} \cos\right(2{\text{π}} t/T)/T/{v}_{x})，\\ {\eta }_{d}\left(5\right)=a\tan\left(2A{\text{π}} \cos\right(2{\text{π}} t/T)/T/{v}_{x})。\end{array} \right.$

(45)

式中：v_x = 1.0288 m/s；A = 20 m；T = 200 s；t为时间变量。以该方程作为反步法的控制输入中的${\eta }_{d}$计算AUV航行过程中的需求的五自由度力与力矩，分别使用伪逆法和广义既约梯度法对反步法输出的力与力矩进行推力分配计算，再对计算结果进行合成以获取AUV实际能获取到的五自由度力与力矩。伪逆法在进行推力分配计算时无法考虑推进器的推力限制，在合成前需根据推进器能产生的推力上限对伪逆法的计算结果进行放缩。同时，为避免反步法在AUV所处的位置点与期望的位置点相距过远时发生积分爆炸，限定AUV最多将距离当前纵向位置前方3 m处的期望轨迹点（${\eta }_{d}$）作为反步法的控制输入。最终AUV运动轨迹如图9所示。

图中，×为期望轨迹，即${\eta }_{d}$生成的轨迹，实线为使用广义既约梯度法作为推力分配算法运行的轨迹，虚线为使用伪逆法作为推力分配算法运行的轨迹。由图可见，广义既约梯度法的运行轨迹与期望轨迹吻合良好，而伪逆法的运行轨迹发生了明显的变形。

以时间为横坐标，以单自由度实际位置与期望位置的差值为纵坐标绘制曲线，结果如图10所示。可见，以伪逆法作为推力分配算法无法以2 kn航速沿着轨迹航行，实际运行时速度低于2 kn航速，进退方向的差值在不断增加，而以广义既约梯度算法作为推力分配算法的运行结果能使AUV保持纵向航速的情况下满足该工况的机动需求。

以AUV运行时的在固定坐标系上的纵向位移为横坐标，以纵向位置处对应的轨迹期望值（${\eta }_{d}$）为纵坐标，绘制曲线如图11所示。可见，以广义既约梯度法作为推力分配算法的运行结果在各维度与期望值吻合良好，以伪逆法作为推力分配算法的运行结果与期望值偏差较大。

由于图9所示算例对AUV的机动性能要求较为极限，且以广义既约梯度法作为推力分配算法的运行轨迹与以伪逆法作为推力分配算法的运行轨迹并不一致，难以对比二者在能耗上的区别。取A = 10 m再次运行仿真模型，结果如图12所示。可见A = 10 m时2种推力分配算法的运行轨迹均与期望轨迹相吻合。对2种推力分配算法的能耗进行统计，能耗计算公式：

${f}_{p}\left(x\right)=\sum {p}_{i}\left({x}_{i}\right),i=3,\dots ,6 。$

(46)

式中：${p}_{3}\left({x}_{3}\right)\sim{p}_{6}\left({x}_{6}\right)$分别代表艏垂推功率、艏侧推功率、艉垂推功率、艉侧推功率。忽略舵板角度变化带来的AUV纵向阻力带变化，AUV在进退方向上仅有一个主推，无能耗优化空间，不纳入能耗统计。艏垂推和艉侧推的功率通过拟合图6获取的多项式方程根据推力进行计算。艏侧推和艉垂推功率计算式为：

${p}_{i}\left({x}_{i}\right)=2{\text{π}} {K}_{Qi}{x}_{i}^{1.5}/{K}_{Ti}^{1.5}{\rho }^{0.5}{D}_{i}\text{，}i=\mathrm{4,6} 。$

(47)

绘制航行过程中2种算法的能耗曲线如图13所示。可见以广义既约梯度算法作为推力分配算法所产生的能耗始终少于以伪逆法作为推力分配算法所产生的能耗。选取算例进行遍历，发现广义既约梯度算法的计算结果已是全局最优解。

取t = 100 s处的工况为代表进行分析，结果如表1所示。经过广义既约梯度算法优化后，沿各方向上产生的力与力矩的总和并未发生变化，而能耗减少了15%。

统计获取伪逆法和广义既约梯度法在Matlab上计算出一次结果的平均运行时长，结果如表2所示，相比于伪逆法，广义既约梯度法在算法复杂度上有所增加，但其运算速度和所消耗的算力资源对于大型AUV上搭载的计算器而言仍处于可接受范围内。

5 结　语

为搭建仿真平台、获取仿真数据，建立了AUV运动学和动力学模型、设计反步控制算法，模拟获取一系列AUV运动时所需求的力与力矩数据；通过建立AUV槽道桨的Fluent仿真模型获取槽道桨能耗与推力之间关系。完成仿真平台的搭建后，分别以伪逆法和广义既约梯度法作为AUV的推力分配算法，控制AUV沿着既定轨迹航行。2次的仿真结果显示，相比于伪逆法，广义既约梯度法能适用于更加复杂的推力分配问题，在节能与减少推力饱和方面问题更具优势，能更好的发挥AUV的机动性、延长AUV的续航能力。