0 引　言

AUV作为一种高度自动化水下自主航行装备，对海洋开发和探测有着重要的作用和意义。对AUV来说，自主航行控制作为其核心关键技术，是AUV真正实现自治的必要条件，因此也一直是备受关注的研究热点。对AUV自主航行控制技术来说，除基础的航向、深度和姿态控制外，跟踪预设的期望路径点航行是其非常重要和关键的功能。本文主要针对采用首、尾舵控制机构的AUV，研究其空间路径点的跟踪控制算法。

水下潜器的路径跟踪控制问题是近年来备受关注的热门课题，Do K.D^[1]考虑了仅有4个推力器的AUV路径跟踪问题，结合Lyapunov直接法、反演方法以及参数映射等方法提出了一种AUV以定常轴向速度跟踪指定路径的方法。王银涛等^[2]研究了AUV定常速度下的非时间参考路径跟踪问题，将跟踪控制问题分为导引和控制2个部分，导引部分基于视线导引原理输出AUV参考航向，控制部分构造了一种将航向控制器和路径跟踪控制器并行结合的混合型路径跟踪控制器。So-Ryeok Oh等^[3]针对欠驱动船舶的路径点跟踪控制在视线导航法的基础上采用了基于状态空间形式的模型预测控制算法，并通过仿真验证了模型预测控制相较传统的PID控制算法具有更佳的控制效果。王宏健等^[4]研究了欠驱动自主水下航行器的三维路径跟踪控制问题，针对基于虚拟向导建立的三维路径跟踪误差模型，采用滤波反步法设计跟踪控制器，增加了系统对噪声的鲁棒性。朱大奇等^[5]针对水下机器人轨迹跟踪控制的速度跳变问题，提出一种基于生物启发神经动力学模型的自治水下机器人三维轨迹跟踪控制算法，基于水下机器人对算法进行了仿真实验。文献[6-7]提出了自适应区域跟踪控制方法，通过对AUV建模不确定性项进行在线辨识，使AUV能够从任意初始位姿收敛到目标区域，但在目标区域边界上出现容易出现震荡而难以收敛于目标区域中。文献[8]进一步地提出了一种PD神经滑模自适应区域跟踪控制方法，采用径向基函数神经网络对动力学模型中的未知项进行自适应估计，采用滑模控制项对神经网络逼近误差进行补偿。程相勤^[9]针对欠驱动UUV的三维路径跟踪问题提出了一种离散滑模预测的控制方法，提高了欠驱动UUV在海流干扰情况下的空间曲线路径跟踪控制的精度并提升了控制系统的鲁棒性。

本文首先应用视线导航法和垂直面制导算法将路径点跟踪控制问题转化为航向、深度和纵倾的跟踪控制问题，再设计基于状态空间模型的模型预测控制算法使得AUV能收敛到期望航向、深度和纵倾达到预期的路径点跟踪控制效果。

1 系统模型建立

由于AUV在水下的航行是六自由度的空间运动，对于其六自由度的运动学以及动力学建模，可参考一般潜艇以及AUV（自主水下机器人）的建模方法。参考文献[10]中给出的AUV6空间运动坐标系，如图1所示。

结合刚体动力学方程的矢量表达形式，可将AUV的动力学以及运动学方程^[11]表示为：

${M}{\dot { {\nu}}} + {{C}}({{\nu }}){\nu} + {{D}}({\nu }){\nu} + {{g}}({\eta }) = {\tau} + {{g}_{0}} + {\omega }\text{，}$

(1)

$\dot{ \eta} = {{J}}({\eta }){\nu }\text{。}$

(2)

式中： ${{\nu }} = {\left[ {u,v,w,p,q,r} \right]^{\rm{T}}}$ 为AUV在运动坐标系下分解的速度以及角速度， $u{\text{，}}v{\text{，}}w$ 分别为运动坐标系下的纵向、横向和垂向线速度， $p{\text{，}}q{\text{，}}r$ 分别为运动坐标系下的横倾角速度、纵倾角速度和转首角速度； ${\eta } = $ $ [x,y,z,\phi ,\theta ,\psi ]$ 为固定坐标系下的位置和欧拉角， $x{\text{，}}y{\text{，}}$ z分别为固定坐标系下的纵向、横向以及垂向位置， $\varphi {\text{，}}\theta {\text{，}}\psi$ 分别为固定坐标系下的横倾角、纵倾角和航向角； ${{M}}$ 为包含附加质量的系统惯性矩阵， ${{C}}({{\nu }})$ 为包含附加质量的刚体科里奥利向心力矩阵； ${{D}}({{\nu }})$ 为阻尼系数矩阵； ${{g}}({{\eta }})$ 为重力和浮力引起的均衡力矢量； ${{{g}}_{{0}}}$ 为AUV压载水舱提供的均衡力矢量； ${{\omega }}$ 为风、浪、流等外部环境干扰作用力和力矩； ${{\tau }} = [{F_x},{F_y},{F_z},{M_x},$ $ {M_y},{M_z}]$ 为AUV推进系统的力和力矩矢量，考虑到航行的能耗，在不需要精确动力定位航行时，依靠全方位推进器产生纵向推力 ${F_x}$ ，转首力矩 ${M_z}$ 和纵倾力矩 ${M_y}$ 作为航行的主要驱动力更为节省能源，所以可认为系统的被控输入量只有此3项，而其他3项可由控制器生成的控制输入再计算得出，且经计算验证均为较小的值。 ${J}({\eta })$ 为运动坐标系到固定坐标系的转换矩阵，其表达式为：

${J}({\eta } ) = \left[ {\begin{array}{*{20}{c}} {{{J}_{1}}({\eta })}&{{{{0}}_{3 \times 3}}} \\ {{{{0}}_{3 \times 3}}}&{{{J}_{2}}({\eta })} \end{array}} \right]\text{。}$

(3)

${\begin{split}&{{{J}}_{{1}}}({{\eta }}) = \\& \left[\!\!\!\!\! {\begin{array}{*{20}{c}} {\cos \psi \cos \theta }\!\!&\!\!{ - \sin \psi \cos \phi + \cos \psi \sin \theta \sin \phi }\!\!&\!\!{\sin \psi \sin \phi + \cos \psi \sin \theta \cos \phi } \\ {\sin \psi \cos \theta }\!\!&\!\!{\cos \psi \cos \phi + \sin \psi \sin \theta \sin \phi }\!\!&\!\!{ - \cos \psi \sin \phi + \sin \psi \sin \theta \cos \phi } \\ { - \sin \theta }\!\!&\!\!{\cos \theta \sin \phi }\!\!&\!\!{\cos \theta \cos \phi } \end{array}}\!\!\!\!\! \right]\text{，}\end{split}}$

(4)

${{{J}}_{{2}}}({{\eta }}) = \left[ {\begin{array}{*{20}{c}} 1&{\sin \phi \tan \theta }&{\cos \phi \tan \theta } \\ 0&{\cos \phi }&{ - \sin \phi } \\ 0&{\sin \phi /\cos \theta }&{\cos \phi /\cos \theta } \end{array}} \right]\text{。}$

(5)

2 航迹制导算法

针对水下潜器的路径跟踪问题的误差建模主要有2种思路，一种是在Serret-frenet坐标系下建立误差模型，另一种为运用导航视线法将跟踪误差模型简化为简单的航速控制、航向控制以及纵倾控制问题，目前水平面视线导航法已经应用得相当成熟，其原理如图2所示。要驱使AUV收敛到由水平面离散路径点 $(\cdots,{P_{k - 1}},{P_k},{P_{k + 1}},\cdots)$ 构成的路径，则艇体质心至路径段 $({P_{k - 1}},{P_k})$ 的横向距离 ${e_k}$ 需收敛至0，同时航向角 $\psi $ 收敛至 ${\alpha _{k - 1}}$ 。运用视线导航法，首先由艇体质心至 $({P_{k - 1}},{P_k})$ 段路径的垂直距离 ${e_k}$ 以及 $\Delta = {n_1}L$ （ $L$ 为艇长， ${n_1}$ 为一正实数）可计算出视线导引点 ${P_{LOS}}({x_{los}},{y_{los}})$ 在固定坐标系下的坐标，再求取艇体质心至视线导引点 ${P_{LOS}}$ 的视线导航航向角 ${\psi _{los}}$ ，则航向角 $\psi $ 收敛至 ${\psi _{los}}$ 可确保艇体位置收敛到直线路径段 $({P_{k - 1}},{P_k})$ 。当艇体质心位于以路径点 ${P_k}$ 半径为 $R = {n_2}L$ （ ${n_2}$ 为正实数）的圆内，则将导航的目标点切换至下一个目标点 ${P_{k + 1}}$ 。

垂直面航迹制导算法如果采用视线导航算法将垂直面位置偏差解算为指令纵倾的会造成以下问题：1）AUV垂直面内航行对纵倾角有一定限制，采用视线导航法进行解算将造成部分参数难以调整。2）AUV在进行垂直面机动控制的过程中，深度速率的控制必须同时考虑垂向速度和纵倾，采用视线导航法解算指令纵倾角而忽略垂向速度，将引起较大的航迹控制偏差。因此，本文通过分析AUV垂直面运动特性以及仿真验证等方法，采用指令深度速率、指令深度以及指令纵倾三者结合的方式作为垂直面的航迹跟踪控制算法。

其中指令深度与指令纵倾的解算方法如图3所示。当前时刻的指令深度为AUV当前位置点在航迹线上的投影点的垂直位置，其解算方法为：

${\zeta ^*} = \frac{{{z_{k + 1}} - {z_k}}}{{{x_{k + 1}} - {x_k}}}({x_{k + 1}} - \Delta {x_k}) + {z_{k + 1}}\text{，}$

(6)

式中： $\Delta {x_k}$ 为距目标航迹点的纵向位移，指令纵倾为当前航迹线的期望潜浮角 ${\chi _k}: = {\rm{atan}}2({z_{k + 1}} - {z_k},{x_{k + 1}} - $ ${x_k}) $ 。由航迹线解算出的指令深度速率与AUV的当前航速相关，其解算方法为：

${\dot \zeta ^*} = \frac{{{z_{k + 1}} - {z_k}}}{{{x_{k + 1}} - {x_k}}}*V\text{。}$

(7)

3 控制器设计

模型预测控制（Model Predictive Control, MPC）是20世纪70年代发展起来的一类控制算法，算法采用多步预测、滚动优化以及反馈校正等控制策略，使得模型预测控制具有控制效果好、鲁棒性强且对模型精确性要求较低等优点，同时可以考虑到执行机构的约束性。预测控制算法另一个明显的优势便是可以将各类约束条件加入到控制过程当中使得控制系统更具有安全性^[12]，因此基于以上几点考虑，本文设计一种基于状态空间模型的预测控制算法用于AUV深度控制算法。

3.1 线性化模型

由于AUV的非线性模型十分复杂，不宜直接作为控制器设计的模型，故需将AUV的六自由度非线性模型简化成线性模型以便于控制器的设计。本文考虑路径点跟踪控制问题，并假定AUV的纵向速度 $u$ 保持不变，即 $\dot u = 0$ ，同时忽略水平面和垂直面运动的耦合作用，将AUV的非线性模型线性化成状态方程形式的系统模型，下面以垂直面控制为例介绍控制器设计过程。

$\begin{split} & \left[\!\!\! {\begin{array}{*{20}{c}} {\dot w} \\ {\dot q} \\ {\dot \theta } \\ {\dot \zeta } \end{array}} \!\!\!\right] = \left[\!\!\! {\begin{array}{*{20}{c}} {{a_{11}}}&{{a_{12}}}&{{a_{13}}}&0 \\ {{a_{21}}}&{{a_{22}}}&{{a_{23}}}&0 \\ 0&1&0&0 \\ 1&0&{ - u}&0 \end{array}} \!\!\!\right]\left[\!\! {\begin{array}{*{20}{c}} w \\ q \\ \theta \\ \zeta \end{array}} \!\!\!\right] + \left[\!\!\! {\begin{array}{*{20}{c}} {{b_{11}}}&{{b_{12}}} \\ {{b_{21}}}&{{b_{22}}} \\ 0&0 \\ 0&0 \end{array}} \!\!\!\right] \left[\!\!\! {\begin{array}{*{20}{c}} {{\delta _b}} \\ {{\delta _s}} \end{array}} \!\!\!\right] \text{，} \\ & \left[\!\! {\begin{array}{*{20}{c}} \theta \\ \zeta \end{array}} \!\!\!\right] = \left[\!\!\! {\begin{array}{*{20}{c}} 0&0&1&0 \\ 0&0&0&1 \end{array}} \!\!\!\right]\left[\!\!\! {\begin{array}{*{20}{c}} w \\ q \\ \theta \\ \zeta \end{array}} \!\!\!\right] \text{，} \end{split} $

(8)

即

$\begin{split} & {\dot{ x}} = {{Ax}} + {{Bu}}\text{，} \\ &y = {{Cx}} \text{。} \end{split} $

(9)

再将其转换为离散时间的线性状态空间模型：

$\begin{split} &{{x}}(k + 1) = {{{A}}_{{d}}}{{x}}(k) + {{{B}}_{{d}}}{{u}}(k)\text{，} \\ & y(k) = {{{C}}_{{d}}}{{x}}(k) \text{。} \end{split} $

(10)

其中， ${{x}} = {\left[ {w,q,\theta ,\zeta } \right]^{\rm{T}}}$ 是系统的状态量， ${{u}} = {M_z}$ 为系统的控制输入量， $y = \left[\!\! {\begin{array}{*{20}{c}} \theta \\ \zeta \end{array}}\!\!\!\! \right]$ 是被控系统的输出量。 ${{{A}}_{{d}}}$ ， ${{{B}}_{{d}}}$ ， ${{{C}}_{{d}}}$ 分别为离散化后的状态空间模型矩阵。当外部环境存在干扰时，由于典型的状态反馈控制律 ${{u}}(k) = - K{{x}}(k)$ 中不包含积分环节，所以针对状态反馈控制，状态量一般都会存在静差，考虑到实际建模的误差以及控制过程中存在的噪声，为消除稳态时的静差，在基于状态空间方程的预测控制中，采用基于增广状态的状态空间模型，用 $\Delta {{u}}(k)$ （其中 $\Delta {{u}}(k) = {{u}}(k) - $ ${{u}}(k - 1) $ 为控制输入的变化率）代替 ${{u}}(k)$ 作为控制输入量使系统能够实现无静差控制^[13]，则将状态空间方程改写成如下形式：

$\left\{ {\begin{array}{*{20}{l}} {\left[\!\!\! {\begin{array}{*{20}{c}} {{{x}}(k + 1)} \\ {{{u}}(k)} \end{array}}\!\!\! \right] = \left[\!\!\! {\begin{array}{*{20}{c}} {{{{A}}_{{d}}}}&{{{{B}}_{{d}}}} \\ {{0}}&{{I}} \end{array}} \!\!\!\right]\left[\!\!\! {\begin{array}{*{20}{c}} {{{x}}(k)} \\ {{{u}}(k - 1)} \end{array}} \!\!\!\right] + \left[\!\!\! {\begin{array}{*{20}{c}} B \\ I \end{array}} \!\!\!\right]\Delta {{u}}(k)}\text{，} \\ {y(k) = \left[\!\!\! {\begin{array}{*{20}{c}} {{{{C}}_{{d}}}}&{{0}} \end{array}} \!\!\!\right]\left[\!\!\! {\begin{array}{*{20}{c}} {{{x}}(k)} \\ {{{u}}(k - 1)} \end{array}} \!\!\!\right]} \text{，} \\[-25pt] \end{array}} \right.$

(11)

令 $z(k) = \left[\!\!\! {\begin{array}{*{20}{c}} {{{x}}(k)} \\ {{{u}}(k - 1)} \end{array}} \!\!\!\right]$ ， ${{{A}}_{{o}}} = \left[\!\!\! {\begin{array}{*{20}{c}} {{{{A}}_{{d}}}}&{{{{B}}_{{d}}}} \\ {{0}}&{{I}} \end{array}}\!\!\! \right]$ ， ${{{B}}_{{o}}} = \left[\!\!\! {\begin{array}{*{20}{c}} B \\ I \end{array}} \!\!\!\right]$ ， ${{{C}}_{{o}}} = \left[\!\!\! {\begin{array}{*{20}{c}} {{{{C}}_{{d}}}}&{{0}} \end{array}}\!\!\! \right]$ ，即

$\left\{ {\begin{array}{*{20}{l}} {{{z}}(k + 1) = {{{A}}_{{o}}}z(k) + {{{B}}_{{o}}}\Delta {{u}}(k)} \text{，}\\ {{{y}}(k) = {{{C}}_{{o}}}{{z}}(k)} \text{。} \end{array}} \right.$

(12)

3.2 未来状态预测

不考虑测量噪声对状态量的影响，即整个状态向量是可测的，得 $\hat z(k|k) = z(k)$ （其中 ${\hat{ \xi }}(k|k)$ 代表 $k$ 时刻预估的 $k$ 时刻的状态量估计值），设定预测时域为 ${N_y}$ 以及控制时域 ${N_c}$ ，系统的预测模型为：

$\begin{gathered} {{{\vec{ z}}}_k} = {P_x}z(k) + {H_x}\Delta {{{\vec{ u}}}_k} \text{，} \\ {{{\vec{ y}}}_k} = Pz(k) + H\Delta {{{\vec{ u}}}_k} \text{。} \\ \end{gathered} $

(13)

其中：

${\vec {{z}}_k} = \left[\!\!\! {\begin{array}{*{20}{c}} {z(k + 1|k)} \\ \vdots \\ {z(k + {N_c}|k)} \\ {z(k + {N_c} + 1|k)} \\ \vdots \\ {z(k + {N_y}|k)} \end{array}} \!\!\!\right],\; {{{P}}_{{x}}} = \left[\!\!\! {\begin{array}{*{20}{c}} {{{{A}}_{{o}}}} \\ \vdots \\ {{{{A}}_{{o}}}^{{N_c}}} \\ {{{{A}}_{{o}}}^{{N_{c + 1}}}} \\ \vdots \\ {{{{A}}_{{o}}}^{{N_y}}} \end{array}}\!\!\! \right] \text{，}$

$\Delta {{\vec{ u}}_k} = \left[\!\!\! {\begin{array}{*{20}{c}} {\Delta {{u}}(k)} \\ \vdots \\ {\Delta {{u}}(k + {N_c} - 1|k)} \\ 0 \\ \vdots \\ 0 \end{array}}\!\!\! \right],\;{{\vec{ y}}_k} = \left[\!\!\! {\begin{array}{*{20}{c}} {y(k + 1|k)} \\ \vdots \\ {y(k + {N_c}|k)} \\ {y(k + {N_c} + 1|k)} \\ \vdots \\ {y(k + {N_y}|k)} \end{array}}\!\!\! \right]\text{，}$

$ { \begin{aligned}{{{H}}_{{x}}} \! = \left[\!\!\!\! {\begin{array}{*{20}{c}} {{{{B}}_{{o}}}}\!\!\!&\!\!\!0\!\!\!&\!\!\!0\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \\ \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \\ {{{{A}}_{{o}}}^{{N_c} - 1}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{A}}_{{o}}}^{{N_c} - 2}{{{B}}_{{o}}}}\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \\ {{{{A}}_{{o}}}^{{N_c}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{A}}_{{o}}}^{{N_c} - 1}{{{B}}_{{o}}}}\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{A}}_{{o}}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \\ \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \\ {{{{A}}_{{o}}}^{{N_y} - 1}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{A}}_{{o}}}^{{N_y} - 2}{{{B}}_{{o}}}}\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{A}}_{{o}}}^{{N_y} - {N_c}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \end{array}}\!\!\!\! \right]\end{aligned}}\text{，} $

${{P}} = \left[ {\begin{array}{*{20}{c}} {{{{C}}_{{o}}}{{{A}}_{{o}}}} \\ \vdots \\ {{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_c}}} \\ {{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_{c + 1}}}} \\ \vdots \\ {{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_y}}} \end{array}} \right]$ ，

${ \begin{aligned} {{H}} = \left[ {\begin{array}{*{20}{c}} {{{{C}}_{{o}}}{{{B}}_{{o}}}}\!\!\!&\!\!\!0\!\!\!&\!\!\!0\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \\ \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \\ {{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_c} - 1}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_c} - 2}{{{B}}_{{o}}}}\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{C}}_{{o}}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \\ {{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_c}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_c} - 1}{{{B}}_{{o}}}}\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{C}}_{{o}}}{{{A}}_{{o}}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \\ \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \!\!\!&\!\!\! \vdots \\ {{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_y} - 1}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_y} - 2}{{{B}}_{{o}}}}\!\!\!&\!\!\! \cdots \!\!\!&\!\!\!{{{{C}}_{{o}}}{{{A}}_{{o}}}^{{N_y} - {N_c}}{{{B}}_{{o}}}}\!\!\!&\!\!\!{{{{0}}_{({N_y} - {N_c}) \times 1}}} \end{array}} \right]\end{aligned}}$ 。

3.3 约束优化

将性能指标函数定义为：

${J_k}(\Delta {{\vec{ u}}_k}) = \sum\limits_{j = 1}^{{N_y}} {\left\| {y(k + j|k) - {{r}}} \right\|_{{{Q}}(j)}^2} + \sum\limits_{j = 1}^{{N_c} - 1} {\left\| {\Delta u(k + j|k) - {{r}}} \right\|_{{{R}}(j)}^2}\text{，} $

(14)

写成向量形式为：

${J_k} = \left\| {{{{\vec{ y}}}_k} - {{r}}} \right\|_{{Q}}^2 + \left\| {\Delta {{{\vec{ u}}}_k}} \right\|_{{R}}^2\text{。}$

(15)

其中 ${{r}}$ 为设定的参考轨迹，也即由垂直面制导算法得出的动态期望航向角 ${\psi _{los}}$ ，其中 ${{Q}} = {\rm{diag}}({q_1}, \cdots ,{q_{{N_y}}})$ 为 ${N_y}$ 维的输出误差权值矩阵， $R = {\rm{diag}}({r_1}, \cdots ,{r_{{N_c}}})$ 为 ${N_c}$ 维的输入权值矩阵。

对于AUV的路径跟踪控制问题而言，由于执行器的物理约束，转首力矩的大小受到一定的限制，也即控制输入量需满足一定的约束条件，也即 ${{u}}(k)$ 需满足：

$\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{U} \leqslant {{u}}(k) \leqslant \bar U\text{，}$

(16)

其中 $ \underset{\_}{U}，\overline{U}$ 分别表示控制输入量的饱和上下限值。则预测控制问题便转化为实时的在线约束优化问题：

$\begin{split} & \mathop {\min }\limits_{\Delta {{U}}(k)} {J_k} = \left\| {{{{\vec{ y}}}_k} - r} \right\|_Q^2 + \left\| {\Delta {{{\vec{ u}}}_k}} \right\|_R^2 \text{，}\\ & {\rm{s.t.}}\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{U} \leqslant {{u}}(k) \leqslant \bar U \text{。} \end{split} $

(17)

将式（13）代入式（15）并展开得到下式：

$\begin{split} {J_k} =& {({{P}}{{\xi }}(k) - {{r}})^{\rm{T}}}{{Q}}({{P}}{{\xi }}(k) - {{r}}) + 2{({{P}}{{\xi }}(k) - {{r}})^{\rm{T}}}{{QH}}\Delta {{\vec{ u}}_k} + \\&\Delta {{\vec{ u}}_k}^{\rm{T}}({{{H}}^{\rm{T}}}{{QH}} + {{R}})\Delta {{\vec{ u}}_k} \text{。} \\[-10pt] \end{split}$

(18)

式中等号右边第1项为确定值，与优化无关，在计算时可只考虑后2项的影响，并将后2项写成标准的二次型形式，同时将输入量的约束写成向量形式，则可将式（16）写成新的标准二次型约束优化问题，得到：

$\begin{split} & \mathop {\min }\limits_{\Delta {{U}}(k)} {J_k} = \Delta {{{\vec{ u}}}_k}^{\rm{T}}{{S}}\Delta {{{\vec{ u}}}_k} + 2{{{f}}^{\rm{T}}}\Delta {{{\vec{ u}}}_k} \text{，} \\ & {\rm{s.t.}}{{W}}\Delta {{{\vec{ u}}}_k} \leqslant U \text{。} \\ \end{split} $

(19)

其中 ${{S}}\!\! = \!\!{{{H}}^{\rm{T}}}{{QH}} + {{R}}$ ， ${{f}}\!\! =\!\!\! {{{H}}^{\rm{T}}}{{{Q}}^{\rm{T}}}({{P}}{{\xi }}(k) - {{r}})$ ， ${{W}} \!=\! {[ I\quad{ - I}]^{\rm{T}}}$ ， $U = {[ {\bar U}\quad{\underset{\raise0.3em\hbox{$\smash{\scriptscriptstyle-}$}}{U} }]^{\rm{T}}}$ ，则式（18）也即标准的QP（二次规划）问题，采用二次规划算法便可求出式（18）的最优解，假设由式（19）计算出的最优解为 $\Delta {{{U}}_{opt}}(k)$ ，依据模型预测控制的滚动优化策略，取最优解的第1列 $\Delta {u_{opt}}(k)$ 即可作为当前时刻的控制器输出增量：

$\Delta {u_{opt}}(k) = \left[ {\begin{array}{*{20}{c}} {{{{I}}_{{N_c}}}}&{{0}}& \cdots &{{0}} \end{array}} \right]\Delta {{{U}}_{opt}}(k)\text{，}$

(20)

则当前时刻的控制器最优输出值为上一时刻的控制器输出值加上当前时刻的控制器增量值，即

${u_{opt}}(k) = u(k - 1) + \Delta {u_{opt}}(k)\text{。}$

(21)

在此基础上进行滚动优化便可得出整个控制过程的最优控制输入。

4 仿真示例

本文采用Matlab/Simulink仿真工具，首先设定空间离散路径点，并由AUV实时位置信息，采用水平面视线导航算法和垂直面制导算法推算出动态的期望航向角、期望深度、期望纵倾、或深度速率，再由MPC控制器解算出期望控制输入量 $M_z^*$ ，并通过执行器闭环模块得出最终的系统控制输入量，整个控制系统的原理框图如图3所示。

设定控制机构需满足的约束条件为 ${u_{\min }} < u < {u_{\max }},$ $\Delta {u_{\min }} < \Delta u < \Delta {u_{\max }} $ ，预测时域为 ${N_y}$ 为35，控制时域 ${N_c}$ 为4。仿真算例1为垂直面航迹控制，离散路径点序列为[0 0 30] [2000 0 260] [4000 0 60] [6000 0 160] [8000 0 60] [10000 0 160] [12000 0 60] [14000 0 60] [2 000 0 260] m，其路径点跟踪效果仿真示意图如图4所示。

可知，AUV能实现垂直面的路径点跟踪。图5为路径跟踪过程中的纵倾和深度曲线，AUV与设定路径的垂直距离误差仿真结果如图6所示。

仿真算例2为空间路径点跟踪控制，空间路径点序列为[0 0 60]，[2000 2000 140]，[2000 4000 60]，[0 6000 140]，[−2000 4000 60]，[−2000 2000 140]，[0 0 60] m，路径跟踪效果如图7所示。图8和图9分别为水平面和垂直面的航迹偏差。

5 结　语

本文针对AUV的空间路径点跟踪控制问题，采用水平面视线导航法计算AUV指令航向，采用垂直面制导算法计算AUV的垂直面指令深度、指令纵倾和指令深度速率，采用模型预测控制算法用于AUV的航向和深度控制。仿真结果表明，空间航迹控制算法能达到较高的控制精度，采用的指令深度+指令纵倾+指令深度速率的控制方法能使AUV尽快的航行至指令轨迹并跟踪垂直面航迹，分析航迹偏差曲线，可发现只有在航迹切换过程中才有明显的航迹偏差出现，且垂直面切换时潜浮角的改变越小，也即垂直面航迹越平坦时，航迹控制的精度也越高，对应的航迹切换过程中的航迹偏差也越小。