0 引　言

海洋蕴含着丰富的矿产资源、生物资源、可再生能源等。然而，深海环境极端复杂。自主水下航行器（Autonomous Underwater Vehicle, AUV）以其航程远、可重复使用等特点，成为了人类认识和探索海洋领域必不可缺的工具。精确的路径跟踪控制是AUV实现自主航行的关键技术之一，受到国内外学者的广泛关注。

Jose等^[1]针对编队AUV的路径跟踪控制，将多个分层任务结合在基于零空间的行为算法中，以安全引导六自由度AUV跟踪控制。Habib Choukri等^[2]提出一种改进的自抗扰控制器，用于在海浪和海流引起的显著变化扰动下AUV的路径跟踪控制，实现在周期性干扰的情况下进行高性能跟踪。Tanakitkorn等^[3]针对工作在一定航速范围内的过驱动型悬停AUV，提出了一种滑模航向控制方法，即使受到外部干扰，也能产生一致和稳健的性能，王宁等^[4]提出一种基于有限时间观测器的制导控制方法。该方法能在有限时间内精确获取未知侧滑角，显著提升了跟踪的控制精度。Liang等^[5]利用自适应积分视线制导和非线性迭代滑模控制器解决了欠驱动AUV在复杂曲线下的路径跟踪问题。王伟等^[6]针对容错控制，提出一种容错转向样机系统，实现动力学控制、自主故障检测与容错控制。

现有的研究工作大多围绕特定控制算法或策略开展，如滑模控制^{[7 − 8]}、PID控制^{[9 − 10]}、反步控制^{[11 − 12]}等。但这些算法未能综合考虑AUV路径跟踪控制的各项关键性能指标，同时也伴随着抖振、鲁棒性差、响应时间长等局限性与待改进之处。考虑到AUV运动系统具有时变性、强耦合性、非线性特征及控制约束条件，而模型预测控制(MPC)具有前馈-反馈特性、自适应能力强等优点，并能解决其他控制方法不易解决的多变量约束问题。因此，本文选择MPC作为控制算法解决AUV的三维路径跟踪控制问题。

1 基础知识与问题描述 1.1 基础知识

引理1　^[13]考虑如下系统：

$\left\{ \begin{gathered} {{\dot \varepsilon }_1} = {\varepsilon _2} - {P_1}{\text{si}}{{\text{g}}^{{{(n + 1)} \mathord{\left/ {\vphantom {{(n + 1)} 2}} \right. } 2}}}({\varepsilon _1})，\\ {{\dot \varepsilon }_2} = - h - {P_2}{\text{si}}{{\text{g}}^n}({\varepsilon _1})。\\ \end{gathered} \right.$

(1)

式中：${P_1},{P_2} > 0$；$0 < n < 1$；$h$ 连续有界且可微；${\text{si}}{{\text{g}}^m}(x) = {\left| x \right|^m}{{\mathrm{sgn}}} (x)$。可以满足上述条件的系统，是有限时间稳定的，即能够在有限时间内稳定并收敛至平衡点。

1.2 问题描述

AUV在实际的水下航行中具有强耦合、强非线性和复杂度高等特点，因此在建立AUV模型时，不能忽略系统内部未建模动态和外部干扰，并要考虑障碍物的约束。本文忽略横摇运动的影响，建立五自由度数学模型如下：

$\left\{\begin{array}{l} \dot{{\boldsymbol{\eta}}}={\boldsymbol{R}}_{g-v^{\prime}} {\boldsymbol{v}} ，\\ {\bf{M}} \dot{{\boldsymbol{v}}}={\boldsymbol{\tau}}-{\bf{C}}({\boldsymbol{v}}) {\boldsymbol{v}}-{\bf{D}}({\boldsymbol{v}}) {\boldsymbol{v}}-{\boldsymbol{g}}({\boldsymbol{\eta}})+{\boldsymbol{\tau}}_{\delta}。\end{array}\right.$

式中：η为AUV的位姿状态；${\boldsymbol{v}}$为载体坐标系下的速度状态；${\boldsymbol{\tau}}$为控制输入；${\boldsymbol{\tau}}_{\delta}$为未知外部环境干扰；$\boldsymbol{R}_{y, x}$为旋转矩阵；${\boldsymbol{M}}$为广义质量正定矩阵；${\boldsymbol{C}}({\boldsymbol{v}})$为科里奥利向心力矩阵；${\boldsymbol{D}}({\boldsymbol{v}})$为流体水动力阻尼参数矩阵；${\boldsymbol{g}}({\boldsymbol{\eta}})$为重浮力产生的恢复力（力矩）参数向量。

${{\boldsymbol{R}}_{y,{{z}}}} = \left[ {\begin{array}{*{20}{c}} {\cos \psi \cos \theta }&{ - \sin \psi }&{\cos \psi \sin \theta }&0&0 \\ {\sin \psi \cos \theta }&{\cos \psi }&{\sin \psi \sin \theta }&0&0 \\ { - \sin \theta }&0&{\cos \theta }&0&0 \\ 0&0&0&1&0 \\ 0&0&0&0&{1/\cos \theta } \end{array}} \right]，$

${\boldsymbol{M}}({\boldsymbol{v}}) = {\text{diag}}({m_{11}},{m_{22}},{m_{33}},{m_{44}},{m_{55}})，$

${\boldsymbol{D}}({\boldsymbol{v}}) = {\text{diag}}({d_{11}},{d_{22}},{d_{33}},{d_{44}},{d_{55}})，$

${\boldsymbol{g}}({\boldsymbol{\eta}} ) = {[0,0,0,\rho g\nabla G{M_L}\sin (\theta ),0]^{\text{T}}}，$

${\boldsymbol{C}}({\boldsymbol{v}}) = \left[ {\begin{array}{*{20}{c}} 0&0&0&{{m_{33}}w}&{ - {m_{22}}v} \\ 0&0&0&0&{{m_{11}}u} \\ 0&0&0&{ - {m_{11}}u}&0 \\ { - {m_{33}}w}&0&{{m_{11}}u}&0&0 \\ {{m_{22}}v}&{ - {m_{11}}u}&0&0&0 \end{array}} \right] 。$

本文引入虚拟向导概念，在参数化的期望路径上引入Serret-Frenet坐标系，跟踪误差模型：

$\left\{ \begin{gathered} {{\dot x}_e} = U\cos {\psi _e}\cos {\theta _e} + {y_e}{{\dot \psi }_F}\cos {\theta _F} - {z_e}{{\dot \theta }_F} - {u_{tar}} ，\\ {{\dot y}_e} = U\sin {\psi _e}\cos {\theta _e} - {x_e}{{\dot \psi }_F}\cos {\theta _F} - {z_e}{{\dot \psi }_F}\sin {\theta _F} ，\\ {{\dot z}_e} = - U\sin {\theta _e} + {y_e}{{\dot \psi }_F}\sin {\theta _F} + {x_e}{{\dot \theta }_F} ，\\ {{\dot \theta }_e} = q - \dot \alpha - {c_c}(\varpi )\dot \varpi ，\\ {{\dot \psi }_e} = r/\cos \theta + \dot \beta - {c_t}(\varpi )\dot \varpi 。\\ \end{gathered} \right.$

(2)

假设1　欠驱动AUV的期望路径，连续可微，并且期望路径的一阶导数和二阶导数均有界。

假设2　假设系统内部的不确定项和外部扰动构成的混合不确定项$f$及其变化率有界，满足$\left\| {\dot f} \right\| \leqslant {M_f}$ （${M_f}$为有界正常数）。

考虑欠驱动AUV的动力学模型和路径跟踪误差模型，在假设成立的前提下，设计基于非线性模型预测控制的欠驱动AUV三维路径跟踪避障控制方法，实现欠驱动AUV在有界未知环境干扰和模型未知的条件下，仍能有效跟踪期望路径并避免与障碍物之间发生碰撞。

2 三维路径跟踪控制方法设计

本文将控制器设计分为三部分：制导部分、控制部分以及有限时间扩张状态观测器部分。本文设计的制导律在遇到障碍物时由变速视线制导（SVLOS）切换到人工势场制导，通过计算势场合力对AUV的影响，设计制导律来躲避障碍物。

1）构建全局引力势场

引力势场引导着AUV抵达或跟踪目标点。主要与目标和AUV的实时位置和速度差相关。其中，AUV受到来自势场引力作用的强弱与相对距离${\rho _{{x_s}}}({{\boldsymbol{p}}_o},{{\boldsymbol{p}}_g})$和速度$\rho_{v_{s}}({\boldsymbol{v}}_{0}, {\boldsymbol{v}}_{g})$的大小正相关。AUV的位置和速度向量为${{\boldsymbol{p}}_o} = [{p_{ox}},{p_{oy}}]$和${{\boldsymbol{v}}_o} = [{v_{ox}},{v_{oy}}]$，${{\boldsymbol{p}}_g} = [{p_{gx}},{p_{gy}}]$和${{\boldsymbol{v}}_g} = [{v_{gx}},{v_{gy}}]$为目标的位置和速度向量。引力势场函数$U_{a t i}({\boldsymbol{p}}, {\boldsymbol{v}})$可描述为：

${U_{att}}({\boldsymbol{p}},{\boldsymbol{v}}) = \frac{1}{2}{k_{ap}}\rho _{{p_{og}}}^2({{\boldsymbol{p}}_o},{{\boldsymbol{p}}_g}) + \frac{1}{2}{k_{av}}\rho _{{v_{og}}}^2({{\boldsymbol{v}}_o},{{\boldsymbol{v}}_g})。$

(3)

式中：$k_{s p}$和$k_{\operatorname{sw}}$为大于0的权重系数。相对距离和相对速度如下式：

$\left\{ \begin{split} & {\rho _{{p_{og}}}}({p_o},{p_g}) = \sqrt {{{\left( {{p_{ox}} - {p_{gx}}} \right)}^2} + {{\left( {{p_{oy}} - {p_{gy}}} \right)}^2}} ，\\ & {\rho _{{v_{og}}}}({v_o},{v_g}) = \sqrt {{{\left( {{v_{ox}} - {v_{gx}}} \right)}^2} + {{\left( {{v_{oy}} - {v_{gy}}} \right)}^2}} 。\end{split} \right.$

(4)

吸引力$\boldsymbol{F}_{\mathrm{att}}$计算式为：

${\boldsymbol F_{att}} = {\boldsymbol F_{attp}} + {\boldsymbol F_{attv}} = {k_{ap}}{\rho _{{p_{og}}}}{\vec n{{p_{og}}}} + {k_{av}}{\rho _{{v_{og}}}}{\vec n{{v_{og}}}} 。$

(5)

式中：F_attp为位置差产生的吸引力，用单位向量${\vec n{{p_{og}}}}$表示；F_attv为速度差产生的吸引力；单位向量$\vec n{{v_{og}}}$为速度产生的吸引力方向。

2）构建全局斥力势场

人工势场法以其易于表达、计算小、易于实现和高实时性而在机器人路径规划中得到广泛应用。传统斥力势场存在局部极小值和目标不可达的问题，需作进一步改进。

如图1所示，根据传统人工势场的不足，改进后的斥力势场在设计中考虑了欠驱动AUV与障碍物之间的距离因素，改进后的斥力势场将障碍物环境分为3个区域。当禁止区（PZ）存在障碍物时，导航方案将不考虑目标的引力，并不受任何约束尽可能地避开障碍物。当欠驱动AUV进入避碰操纵区（CZ）时，AUV将躲避区域内的静态障碍物。探测区（DZ）表示欠驱动AUV的探测范围。改进的斥力势场函数如下式：

$\begin{aligned} & U_{rep}({\boldsymbol{p}},{\boldsymbol{v}})= \\ & \left\{\begin{aligned} & \frac{1}{2}k_{rp}\left(\rho_{p_{og}}^{-1}-R_1^{-1}\right)^2,\text{ 0}\leqslant\rho_{p_{og}} < R_1，\\ & \frac{1}{2}k_{rp}\left(\rho_{p_{og}}^{-1}-R_2^{-1}\right)^2\rho_{p_{og}}^2+\chi, \\ & R_1\leqslant\rho_{p_{og}} < R_2\text{,}\left|\theta_{p_{ob}}-\theta_{v_{ob}}\right|\leqslant\mathit{\Phi}_1，\\ & \frac{1}{2}k_{rp}\left(\rho_{p_{og}}^{-1}-R_3^{-1}\right)^2\rho_{p_{og}}^2, \\ & R_2\leqslant\rho_{p_{og}} < R_3\text{,}\left|\theta_{p_{ob}}-\theta_{v_{ob}}\right|\leqslant\mathit{\Phi}_2，\\ & 0,\text{ others}。\end{aligned}\right.\end{aligned}$

(6)

式中：$\chi=k_{rv}e^{ \left\| v_o-v_b \right\| }\sin\left((\mathit{\Phi}\text{π})\mathord{\left/\vphantom{\text{π})(2\Phi_1)}\right.}(2\mathit{\Phi}_1)\right)$；$R_1、R_2、R_3$分别为划分后避碰区域的影响半径；$k_{\eta}>0$,$k_{n}>0$为斥力势场的正调节参数。欠驱动AUV与障碍物的相对距离为：${\rho _{{p_{og}}}} = \sqrt {{{({p_{ox}} - {p_{bx}})}^2} + {{({p_{oy}} - {p_{by}})}^2}}$。$\mathit{\Phi}_1、\mathit{\Phi}_2$为划分后碰撞区所设定的临界值；$\mathit{\Phi}=\theta_{p_{ob}}-\theta_{v_{ob}}$为欠驱动AUV的碰撞角；${\theta _{{p_{ob}}}} = \arctan \left( {{{{p_{{y_{ob}}}}} / {{p_{{x_{ob}}}}}}} \right)$和${\theta _{{v_{ob}}}} = \arctan \left( {{{{v_{{y_{ob}}}}} / {{v_{{x_{ob}}}}}}} \right)$为向量${p_{ob}}({p_{{x_{ob}}}},{p_{{y_{ob}}}})$和向量${v_{ob}}({v_{{x_{ob}}}}, {v_{{y_{ob}}}})$的方向角。

通过对斥力势场函数${U_{rep}}(p,v)$梯度求解，得到斥力${F_{rep}}$函数表达式：

$F_{rep}=\left\{\begin{aligned} & F_{repp1},\text{ }0\leqslant\rho_{p_{ob}} < R_1，\\ & F_{repp2}+F_{repp3}+F_{repv1}+F_{repv2}, \\ & R_1\leqslant\rho_{p_{og}} < R_2\text{,}\left|\theta_{p_{ob}}-\theta_{v_{ob}}\right|\leqslant\mathit{\Phi}_1，\\ & F_{repp4}+F_{repp5}, \\ & R_2\leqslant\rho_{p_{og}} < R_3\text{,}\left|\theta_{p_{ob}}-\theta_{v_{ob}}\right|\leqslant\mathit{\Phi}_2。\end{aligned}\right.$

(7)

斥力函数的具体设计如下式：

$\left\{ \begin{gathered} {F_{repp1}} = - {k_{rp}}\left( {\rho _{{p_{og}}}^{ - 1} - R_1^{ - 1}} \right)\left( {{1 / {\rho _{{p_{og}}}^2}}} \right){{\vec n}_{{p_{ob}}}}, \\ {F_{repp2}} = - {k_{rp}}\left( {\rho _{{p_{og}}}^{ - 1} - R_2^{ - 1}} \right)\left( {{{\rho _{{p_{og}}}^2} / {\rho _{{p_{og}}}^2}}} \right){{\vec n}_{{p_{ob}}}}，\\ {F_{repp3}} = {k_{rp}}{\left( {\rho _{{p_{og}}}^{ - 1} - R_2^{ - 1}} \right)^2}{\rho _{{p_{og}}}}{{\vec n}_{{p_{ob}}}} ,\\ {F_{repp4}} = - {k_{rp}}\left( {\rho _{{p_{og}}}^{ - 1} - R_3^{ - 1}} \right)\left( {{{\rho _{{p_{og}}}^2}/ {\rho _{{p_{og}}}^2}}} \right){{\vec n}_{{p_{ob}}}} ,\\ {F_{repp5}} = {k_{rp}}{\left( {\rho _{{p_{og}}}^{ - 1} - R_3^{ - 1}} \right)^2}{\rho _{{p_{og}}}}{{\vec n}_{{p_{ob}}}}。\\ \end{gathered} \right.$

(8)

式中：${F_{reppi}},i = 1,2,3,4,5$为有关欠驱动AUV和障碍物之间相对位置的斥力；单位向量${\vec n_{{p_{ob}}}}$表示欠驱动AUV指向障碍物的方向。

$\left\{\begin{split}&F_{repv1}=-k_{rv}e^{ \left\| v_o-v_b \right\| }\cos\left(\frac{\text{π}}{2}\frac{\mathit{\Phi}}{\mathit{\Phi}_1}\right)\vec{l}_{p_{ob}}，\\ & F_{repv2}=-k_{rv}e^{ \left\| v_o-v_b \right\| }\cos\left(\frac{\text{π}}{2}\frac{\mathit{\Phi}}{\mathit{\Phi}_1}\right)\vec{l}_{v_{ob}}。\end{split}\right.$

(9)

式中：${F_{repv1}}$,${F_{repv2}}$与AUV和障碍物之间的相对速度有关，其方向由单位向量${\vec l_{{p_{ob}}}} = \left( { - \sin {\theta _{{p_{ob}}}},\cos {\theta _{{p_{ob}}}}} \right)$和${\vec l_{{v_{ob}}}} = \left( {\sin {\theta _{{v_{ob}}}}, - \cos {\theta _{{v_{ob}}}}} \right)$表示。

3）结合SVLOS获取参考路径的制导律

出于实际AUV能耗考虑，本文人工势场设计中只考虑水平面避障角制导，即在一个方向执行避障操作，可以有效节省能耗，又不会因为避障转向方向优先级问题产生算法错误。在一个由静止的障碍物和目标形成的人工势场中，欠驱动AUV在任何时刻，任何一个位置所受到来自障碍物产生的斥力和目标产生的引力的共同作用下，通过动态虚拟制导点形成一个光滑且安全合理的局部期望路径。具体表达式如下：

$\left\{ \begin{gathered} {{\dot x}_d} = {u_d}\cos {\psi _d}，\\ {{\dot y}_d} = {u_d}\sin {\psi _d} ，\\ {\psi _d} = {\theta _{{F_{{\mathrm{total}}}}}}。\\ \end{gathered} \right.$

(10)

式中：$x_d、y_d、\psi_d$为虚拟制导点在的位置坐标和艏向角；${u}_{{d}}$为虚拟制导点的设定速度；$F_{\mathrm{total}}=F\mathit{\mathit{_{\mathrm{\mathit{att}}}}}+F_{\mathrm{\mathit{rep}}}$是人工势场对欠驱动AUV产生的合力；${\theta _{{F_{{\mathrm{total}}}}}}$表示合力${F_{{\mathrm{total}}}}$的方向角。如果$\left\| {{F_{{\mathrm{total}}}}} \right\| > 0$，虚拟制导点的状态如式(10)，否则虚拟制导点的状态更换为SVLOS形式。

2.1 SVLOS制导技术设计

为了改进传统LOS跟踪固定速度所欠缺的灵活性，设计SVLOS制导律的设计如下：

${u_d} = {k_u}\sqrt {y_e^2 + z_e^2 + \Delta _\psi ^2} 。$

(11)

式中：$k_u$为增益正参数；${\Delta _\psi } > 0$为水平面的前视距离。应用SVLOS设计的期望值为：

$\left\{ \begin{gathered} {\theta _{LOS}} = \arctan \left( {{{{z_e}} \mathord{\left/ {\vphantom {{{z_e}} {{\Delta _\theta }}}} \right. } {{\Delta _\theta }}}} \right)，\\ {\psi _{LOS}} = - \arctan \left( {{{{y_e}} \mathord{\left/ {\vphantom {{{y_e}} {{\Delta _\psi }}}} \right. } {{\Delta _\psi }}}} \right)。\\ \end{gathered} \right.$

(12)

式中：${\Delta _\theta } = \sqrt {\Delta _\psi ^2 + y_e^2}$，为垂直面的前视距离。

重新设计的攻角$\alpha_{d}$和漂角$\beta_{d}$，期望纵倾角$\theta_{d}$和期望艏向角$\psi_{d}$设计为：

$\left\{ \begin{gathered} {\theta _d} = {\theta _F} + {\theta _{LOS}} + {\alpha _d}，\\ {\psi _d} = {\psi _F} + {\psi _{LOS}} - {\beta _d} 。\\ \end{gathered} \right.$

(13)

角度误差可改写为${\theta _e} = \theta - {\theta _d}$和${\psi _e} = \psi - {\psi _d}$。假设在SVLOS制导律的作用下，欠驱动AUV的实际纵倾角$\theta$和艏向角$\psi$能跟踪上期望纵倾角$\theta_{d}$和期望艏向角$\psi_{A}$，即${\theta _e} = {\psi _e} = 0$。使期望路径参数定义为：

${u_{tar}} = {U_d}\cos {\theta _e}\cos {\psi _e} + {k_t}{x_e}。$

(14)

式中：$k_{t}>0$代表增益正参数。

2.1.1 制导控制器稳定性

构造如下Lyapunov函数$\nabla_{1}$：

${V_1} = \frac{1}{2}(x_e^2 + y_e^2 + z_e^2)。$

(15)

假设$U=U_{4}>0$，对$\nabla_{1}$求导可得：

${\dot V_1} = {x_e}{\dot x_e} + {y_e}{\dot y_e} + {z_e}{\dot z_e} 。$

(16)

根据路径跟踪误差模型和前述定义可知：

$\begin{split} {{\dot V}_1} =& - {k_t}x_e^2 + {y_e}{U_d}\sin {\psi _e}\cos {\theta _e} - {z_e}{U_d}\sin {\theta _e}= \\ & - {k_t}x_e^2 - \frac{{{U_d}}}{{\sqrt {y_e^2 + \Delta _\psi ^2} }}\frac{{{\Delta _\theta }}}{{\sqrt {z_e^2 + \Delta _\theta ^2} }}y_e^2 - \frac{{{U_d}}}{{\sqrt {z_e^2 + \Delta _\theta ^2} }}z_e^2 \leqslant \\ & - 2{k_{{V_1}}}{V_1} \leqslant 0。\end{split}$

(17)

式中：$k_{t_{1}}>0$，根据引理1可知，所设计的期望角，能够使跟踪误差最终能收敛到0，即AUV与虚拟目标点的距离能够收敛。

接着，使用误差变量$\theta_{\varepsilon}$和$\psi_{E}$，构造Lyapunov函数$V_{2}$如下式：

${V_2} = \frac{1}{2}\theta _e^2 + \frac{1}{2}\psi _e^2。$

(18)

构造期望纵倾角速度$q_{d}$和期望艏向角速度$r_{A}$为：

$\left\{ \begin{gathered} {q_d} = {{\dot \theta }_d} - {k_\theta }(\theta - {\theta _d})，\\ {r_d} = \cos \theta ({{\dot \psi }_d} - {k_\psi }(\psi - {\psi _d})) 。\\ \end{gathered} \right.$

(19)

式中：${k_\theta } > 0$，${k_\psi } > 0$。令$q = {q_d}$、$r = {r_d}$对V₂求导得：

$\begin{split} {{\dot V}_2} =& {\theta _e}\left( {{{\dot \theta }_d} - q} \right) + {\psi _e}\left({{\dot \psi }_d} - \frac{r}{{\cos \theta }}\right) =\\ & - {k_\theta }\theta _e^2 - {k_\psi }\psi _e^2 \leqslant - 2{k_{{V_2}}}{V_2} \leqslant 0。\end{split}$

(20)

式中：${k_{{V_2}}} = \min [{k_\theta },{k_\psi }] > 0$。通过证得${\dot V_1} \leqslant 0$，${\dot V_2} \leqslant 0$，由Lyapunov稳定性理论可知误差方程大范围渐进稳定。

2.2 有限时间扩张状态观测器设计

由于在实际工况中，通常很难精确地获得真实欠驱动AUV的准确模型。故针对控制器模型参数未知，含有模型不确定项的前提下，设计一种有限时间扩张状态观测器补偿系统模型参数未知和环境干扰构成的总扰动项。

首先，依据欠驱动AUV的数学模型：

$\dot{\boldsymbol{v}}=\boldsymbol{M}^{-1}\left(\boldsymbol{\tau}_{\delta}-\boldsymbol{C}(\boldsymbol{v})\boldsymbol{v}-\boldsymbol{D}(\boldsymbol{v})\boldsymbol{v}-\boldsymbol{g}(\boldsymbol{\eta})\right)+\boldsymbol{M}^{-1}\boldsymbol{\tau}。$

(21)

令$\boldsymbol{f}=\boldsymbol{M}^{-1}\left(\boldsymbol{\tau}_t-\boldsymbol{C}(\boldsymbol{v})\boldsymbol{v}-\boldsymbol{D}(\boldsymbol{v})\boldsymbol{v}-g(\eta)\right)$为扰动混合项，作为总扰动项，$\boldsymbol{B}=M^{-1}$。则式(21)可以重新表示为$\hat{\boldsymbol{v}}=\boldsymbol{f}+\boldsymbol{B}\boldsymbol{\tau}$。

定义有限时间扩张状态观测器（FESO）的状态变量$x_{1}=v$, $x_{2}=f$，构造了如下有限时间扩张状态观测器的积分串联系统为：

$\left\{\begin{gathered}\dot{\boldsymbol{x}}_1=\boldsymbol{x}_2+\boldsymbol{\mu}，\\ \dot{\boldsymbol{x}}_2=\boldsymbol{h}。\\ \end{gathered}\right.$

(22)

式中：${\boldsymbol{ \mu}} = {\boldsymbol{B\tau}}$；${\boldsymbol{h}} = {[{h_u},{h_v},{h_w},{h_q},{h_r}]^{\text{T}}}$为扩张状态f的未知变量的导数。

定理 1 为了抑制欠驱动AUV的内部和外部总扰动项，提高欠驱动AUV路径跟踪系统的鲁棒性，将混合扰动项视为扩张状态，对于增广系统有限时间扩张状态观测器可以设计为：

$\left\{ \begin{gathered} {{\dot \hat {\boldsymbol{x}}}_1} = {{\hat {\boldsymbol{x}}}_2} - {{\boldsymbol{L}}_1}{\text{si}}{{\text{g}}^{{3 \mathord{\left/ {\vphantom {3 4}} \right. } 4}}}({{\hat {\boldsymbol{x}}}_1} - {{\boldsymbol{x}}_1}) + {\boldsymbol{\mu}}，\\ {{\dot \hat {\boldsymbol{x}}}_2} = - {{\boldsymbol{L}}_2}{\text{si}}{{\text{g}}^{{3 \mathord{\left/ {\vphantom {3 4}} \right. } 4}}}({{\hat {\boldsymbol{x}}}_1} - {{\boldsymbol{x}}_1})。\\ \end{gathered} \right.$

(23)

式中：${\hat {\boldsymbol{x}}_1}$为状态速度$v$的观测值；${\hat {\boldsymbol{x}}_2}$为混合扰动项f的观测值；$\boldsymbol{L}_1、\boldsymbol{L}_2$为观测器参数。

证明　定义观测误差${\boldsymbol{e}}_1、{\boldsymbol{e}}_2$如下：

$\left\{ \begin{gathered} {{\boldsymbol{e}}_1} = {{\hat {\boldsymbol{x}}}_1} - {\boldsymbol{v}} , \\ {{\boldsymbol{e}}_2} = {{\hat {\boldsymbol{x}}}_2} - {\boldsymbol{f}}。\\ \end{gathered} \right.$

(24)

结合式(24)对观测器误差求导可以得到：

$\left\{ \begin{gathered} {{\dot {\boldsymbol{e}}}_1} = - {{\boldsymbol{L}}_1}{\text{si}}{{\text{g}}^{{3 \mathord{\left/ {\vphantom {3 4}} \right. } 4}}}({{\boldsymbol{e}}_1}) + {{\boldsymbol{e}}_2} ，\\ {{\dot {\boldsymbol{e}}}_2} \in - {{\boldsymbol{L}}_2}{\text{si}}{{\text{g}}^{{3 \mathord{\left/ {\vphantom {3 4}} \right. } 4}}}({{\boldsymbol{e}}_1}) - {\boldsymbol{h}} + [ - {{\boldsymbol{M}}_f},{{\boldsymbol{M}}_f}] 。\\ \end{gathered} \right.$

(25)

利用引理1所设计的有限时间扩张状态观测器可以在有限时间内收敛到0。

${\hat {\boldsymbol{x}}_1} \equiv {\boldsymbol{v}},{\hat {\boldsymbol{x}}_2} \equiv {\boldsymbol{f}}({\boldsymbol{\eta}} ,{\boldsymbol{v}}) 。$

(26)

由上即可证得，观测误差可以在有限时间内稳定，证毕。

2.3 NMPC跟踪控制器

本文采用NMPC方法，使纵荡速度u、纵倾角$\theta$和艏向角$\psi$跟踪上前文所设计的参考制导律，来求得最优控制力和力矩，使得欠驱动AUV完成三维路径跟踪任务，

离散时间形式的代价函数（目标函数）二次型的形式如下：

$\begin{aligned} \min J(k)=& \sum_{i-0}^{N-1}\left(\|e(k+f \mid k)\|_{Q}^{2}+\|{\boldsymbol{u}}(k+i \mid k)\|_{R}^{2}\right)+ \\ & \left\|x\left(N, x_{k}\right)-x_{\operatorname{ref}}(N+k)\right\|_{\rho}^{2}。\end{aligned}$

(27)

$\begin{array}{ll}\text{ s.t. } \overline{{\boldsymbol{x}}}\left(i+1,\overline{{\boldsymbol{x}}}_k\right)={\boldsymbol{x}}\left(i,{\boldsymbol{x}}_k\right)+Tf，\\ \overline{{\boldsymbol{x}}}\left(i+1,\overline{{\boldsymbol{x}}}_k\right)\in {\boldsymbol{X}},{\boldsymbol{u}}\left(i,x_k\right)\in {\boldsymbol{U}}，\\ e(k+1)=\overline{{\boldsymbol{x}}}_{k+1}-{\boldsymbol{x}}_{{\mathrm{ref}}}，\\ \overline{{\boldsymbol{x}}}\left(N,\overline{{\boldsymbol{x}}}_k\right)-x_{{\mathrm{ref}}}(N+k)\in {\boldsymbol{E}}_r。\end{array}$

(28)

式中：${\boldsymbol{Q}} \in \mathbb{R}^{{n_x} \times {n_x}}$、$\boldsymbol{R} \in \mathbb{R}^{{n_u} \times {n_u}}$、$P \in \mathbb{R}^{{n_x} \times {n_x}}$为权重系数矩阵；$x \in \mathbb{R}^{n_{x}}$为实际问题的状态变量；$x_{{\mathrm{ref}}} \in \mathbb{R}^{n_{x}}$是跟踪目标状态；$x_{\mathrm{{r e f}}}(i+k)$为$k$时刻(求解时刻)开始的期望输出状态；${\boldsymbol{u}} \in \mathbb{R}^{u}$为求解过程中的预测控制输入；$\hat{\tau}\in \mathbb{R}^{5}$为FESO求出的扰动补偿。${\boldsymbol{X}}=\left({\boldsymbol{x}}_{k.{\text {min }}}, {\boldsymbol{x}}_{k.\text { max }}\right)$为在状态可行集下的约束；${\boldsymbol{U}}=({\boldsymbol u}_{k,{\mathrm{min}}}, {\boldsymbol u}_{k,{\mathrm{max}}})$为输入可行集下的约束。通过设计合适的权重矩阵，可以促进控制器实现出色的路径跟踪性能。

3 仿真研究

AUV的初始位置为：${[0{\text{ m}},8{\text{ m}},0{\text{ m}}]^{\text{T}}}$；参考路径为${\text{path}}(\varpi ) = {[4\varpi ,8\cos (0.3\varpi ), - 5\varpi ]^{\text{T}}}$，采样时间$T = 0.1{\text{s}}$，步长$N = 10$，仿真时间为200 s；斥力及引力系数：${k_{ap}} = 10$、${k_{av}} = 40$ 、${k_{rp}} = 400$ 、${k_{rv}} = 200$，外部环境干扰设置为：${\tau _\delta } = [3.5\sin \left( {0.5T + 0.3{\text{π}} } \right){\text{ N}},0.8\cos \left( {0.5T + 0.1{\text{π}} } \right){\text{ N}},$$0.8 \cos \left( {0.5T + 0.1{\text{π}} } \right){\text{ N}}, 3.5\sin \left( {0.5T + 0.2{\text{π}} } \right){\text{ N}} \cdot {\text{m}}$, $3.5\sin \times$$( 0.5T +0.2{\text{π}} ){\text{ N}} \cdot {\text{m}}{]^{\text{T}}}$；障碍物坐标为$[15{\text{ m}},6{\text{ m}}, - 18{\text{ m}}]^{\text{T}}$；障碍物区域半径设定为${R_1} = 3{\text{ m}}$、${R_2} = 5{\text{ m}}$、${R_3} = 8{\text{ m}}$；${\Delta _\psi } = 1.5$，${k_u} = 0.5$，${k_t} = 1$，${k_\theta } = 1$，${k_\psi } = 1$；控制输入约束为：$0 \leqslant {\tau _u} \leqslant 145{\text{ N}}$，$\left|\tau_r\right|\leqslant40\text{ N}\cdot\text{m}$；有限时间状态扩张观测器的参数矩阵设定为：${L_1} = 100{\text{ diag}} ([1,1, 1,1,1])$，${L_2} = 1000{\text{ diag}}([1,1,1, 1,1])$；权重矩阵设定为：$P = {\text{ diag}}([150,150,150,180,180,2,15, 15,5,5])$，$Q ={\text{ diag}} ([80,\ 80,\ 90,\ 100,\ 100,\ 1,\ 8,\ 8,\ 2,\ 2])$，$R = {\text{ diag}} ([0.005, 0.01,0.005])$。仿真结果如图2～图7所示。

由图2可知，在相同初始条件下，本文提出的改进人工势场法结合SVLOS-NMPC控制器与传统人工势场法在三维空间下路径跟踪避障控制对比发现：在遭遇障碍物时，基于本文控制算法下的欠驱动AUV能迅速地对障碍物做出反应，其路径跟踪过程流畅且无明显抖动。

从图3可以看出，在改进的人工势场法作用下，欠驱动AUV能有效地避开障碍物，并在进入操作避碰区域时执行避障，有效缩短了避障时间，提高任务执行效率。相比之下，传统人工势场法的路径跟踪控制器虽然同样能有效躲避障碍物，但在避障响应开始和结束时间都远没有改进方法更有效。

由图6可以看出，在有控制力约束下相较于传统人工势场法在避障过程中保证了控制力的平滑变化，未产生明显的抖振，降低了螺旋桨与方向舵的损耗，也保证了跟踪控制的稳定性和鲁棒性。

图7表示本文所提出的FESO对未知动力学模型参数项和外部环境干扰的总扰动项观测曲线。可以看出，所提出的FESO，可以实现对总扰动项的精准估计。

4 结　语

本文针对欠驱动AUV内部模型未知、外部未知环境干扰以及有障碍物约束的三维路径跟踪控制与安全避碰控制问题，提出了基于人工势场法和有限时间扩张状态观测器的NMPC路径跟踪控制器。首先，使用人工势场法设计制导律来躲避障碍物，设计有限时间扩张状态观测器来估计混合扰动项。最后，为验证欠驱动AUV三维路径跟踪避障控制系统的稳定性，进行了对比仿真实验。结果显示，在模型存在建模误差和模型未知的情况下，FESO-NMPC控制器可以有效解决模型未知问题，在遭遇障碍物阻拦的情况下，能有效地跟踪改进制导律来躲避障碍物。从而验证了所设计的控制器方法的有效性。