0 引　言

水下无人航行器(unmanned undersea vehicle, UUV)因具有自主航行能力，可完成海洋/海底环境信息获取，固定/移动目标探测、识别、定位与跟踪以及区域警戒，具有重要的民用军用价值，已成为世界各国海洋装备的重要研究方向^[1]。

对于欠驱动水下无人航行器来说，其只具有舵机、主推力器2种执行机构，只具备姿态与直航速度控制能力，其位置控制相比全驱动的ROV较为困难，也吸引了众多学者进行研究。欠驱动UUV的位置控制问题，主要涉及轨迹跟踪与编队控制，二者均可以一定程度上简化为UUV相对运动目标的位置保持问题，这一“目标”可以是真实目标也可以是虚拟目标，通过对运动目标相对距离的有效控制，便可以实现航行器轨迹跟踪与编队位置保持。

针对位置控制问题，严浙平等^[2]提出了一种基于积分滑模的UUV轨迹跟踪方法，该方法能有效抑制参数摄动对跟踪性能的影响。曹晓明等^[3]提出了基于动态面控制的UUV三维轨迹跟踪方法，该方法在海流的干扰下仍能保持较好的控制性能。于浩淼^[4]提出了利用海流观测器观测海流并进行控制补偿方法，该方法能实现航行器的三维轨迹跟踪。

现有的技术大多将位置控制误差方程转换到本体系下进行分析与设计，模型复杂且非线性项较多。同时，针对全驱动型航行器，在轨迹跟踪上一般要求姿态角也进行跟踪，其对应算法移植至欠驱动型航行器较为困难，因为对于欠驱动航行器来说，姿态角需要用来保持位置稳定并抵抗位置上存在的扰动，在此之外再设计一条姿态轨迹令航行器跟踪不现实。

针对以上位置控制中存在的问题，基于欠驱动无人水下航行器正常航行时小侧滑角、小侧移速度的特性，简化航行器运动学模型，将位置误差转换到惯性系下进行分析，依次设计位置控制环与航速、姿态控制环，将外环位置控制简化为线性系统，避免了复杂的非线性模型，实现航行器的相对位置控制。

1 欠驱动UUV相对位置控制的误差方程

采用文献[5]中的坐标系与航行器运动学方程进行描述，即本体系 ${x_b},{z_b}$ 方向分别指向轴向头部与右舷，对应的地面系 ${x_i},{z_i}$ 指向北向与东向。基于以上坐标定义，运动学方程为：

$ {v_i} = {C_{ib}}({v_b}{\text{)}}。$

(1)

式中： $ {v_i}{\text{ = }}\left[ {\begin{array}{*{20}{c}} {\dot x} \\ {\dot y} \\ {\dot z} \end{array}} \right] $ 为航行器在地面系中的三轴速度； $ {v_b}{\text{ = }}\left[ {\begin{array}{*{20}{c}} u \\ v \\ w \end{array}} \right] $ 为航行器本体系下的三轴速度； ${C_{ib}}{\text{ = }} $ $ \left[ {\begin{array}{*{20}{c}} {\cos \theta \cos \psi }&{ - \sin \theta \cos \psi \cos \varphi + \sin \psi \sin \varphi }&{\sin \theta \cos \psi \sin \varphi + \sin \psi \cos \varphi } \\ {\sin \theta }&{\cos \theta \cos \varphi }&{ - \cos \theta \sin \varphi } \\ { - \cos \theta \sin \varphi }&{\sin \theta \sin \psi \cos \varphi + \cos \psi \sin \varphi }&{ - \sin \theta \sin \psi \sin \varphi + \cos \psi \cos \varphi } \end{array}} \right], $ 为本体系到地面系的坐标转换矩阵， $\psi ,\theta ,\varphi $ 分别为航向角、俯仰角与滚动角。

在这里主要考虑航行器水平面运动，取 $\theta = 0 $ ， $\varphi = 0$ ，忽略垂向通道，同时欠驱动航行器一般纵向速度u满足 $ u \gg v,u \gg w $ ，对应的可以忽略v，w对惯性位置的贡献，那么最终简化的运动学模型为：

$\left\{ \begin{gathered} \dot x = u\cos \psi ，\\ \dot z = - u\sin \psi 。\\ \end{gathered} \right.$

(2)

设目标位置轨线 $ {x}_{d}(t)，{z}_{d}(t) $ 及其一阶导数 $ {\dot{x}}_{d}(t)，{\dot{z}}_{d}(t) $ 已知，则惯性系下相对误差可以写为：

$ \left\{ {\begin{array}{*{20}{c}} {{e_x} = x - {x_d}(t)}，\\ {{e_z} = z - {z_d}(t)}。\end{array}} \right. $

(3)

对式（3）求导可得：

$ \left\{ {\begin{array}{*{20}{c}} {{{\dot e}_x} = u\cos \psi - {{\dot x}_d}(t)}，\\ {{{\dot e}_z} = - u\sin \psi - {{\dot z}_d}(t)} 。\end{array}} \right. $

(4)

式（4）即位置控制方程，通过设计适当的轴向速度u，航向角 $\psi $ ，使得惯性系位置误差 ${e_x}$ ， ${e_z}$ 满足 $\mathop {\lim }\limits_{t \to \infty } {e_x} = 0$ ， $\mathop {\lim }\limits_{t \to \infty } {e_z} = 0$ 。

由式（4）可以看出，为了实现系统稳定，还需设计相应的速度控制器与姿态控制器，对应需要获得速度与姿态的误差方程。

采用文献[5]中所使用的六自由度动力学方程描述欠驱动航行器，依据前述简化条件，进一步忽略重心偏移量及附加质量矩阵中非对角线元素，设正浮力为0，其动力学方程可简化为：

$ \left\{ \begin{gathered} (m + {\lambda _{11}})\dot u = {F_1}{\text{ + }}\frac{1}{2}\rho {u^2}S{C_x}，\\ ({J_y} + {\lambda _{55}})\dot q = \frac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})。\\ \end{gathered} \right. $

(5)

式中：m为航行器质量； $ {\lambda _{{\text{**}}}} $ 为对应的附加质量/惯量； $ {F_1} $ 为推进器推力； $ \rho $ 为水密度；S为航行器最大横截面积； $ {C_x} $ 为阻力系数； ${J_y}$ 为转动惯量；q为航向角速度； $L$ 为航行器长度； $m_y^{\text{*}}$ 为对应水动力系数； $\bar q = \dfrac{{qL}}{u}$ 为无量纲角速度； ${\delta _r}$ 为垂直舵角。

令位置控制环输出的速度命令为 ${u_d}$ ，航向角命令为 ${\psi _d}$ ，对速度误差 ${e_u} = u - {u_d}$ ，航向角误差 ${e_\psi } = \psi - {\psi _d}$ 求导，则误差方程为：

$ {\dot e_u} = \frac{{{F_1}{\text{ + }}\dfrac{1}{2}\rho {u^2}S{C_x}}}{{m + {\lambda _{11}}}} - {\dot u_d} ，$

(6)

$ \left\{ \begin{gathered} {{\dot e}_\psi } = q - {{\dot \psi }_d} ，\\ \dot q = \dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} 。\\ \end{gathered} \right. $

(7)

式中： $ {\dot u_d}{\text{(}}t{\text{) = }}\dfrac{{{u_d}{\text{(}}t{\text{)}} - {u_d}{\text{(}}t - 1{\text{)}}}}{T} $ ， $ {\dot \psi _d}{\text{(}}t{\text{) = }}\dfrac{{{\psi _d}{\text{(}}t{\text{)}} - {\psi _d}{\text{(}}t - 1{\text{)}}}}{T} $ ，T为控制周期。

式（6）即速度误差方程，式（7）为航向误差方程，通过设计适当的推力 $ {F_1} $ 和舵角 $ {\delta _r} $ ，可以使得航行器速度误差 ${e_u}$ ，航向误差 $ {e_\psi } $ 满足 $\mathop {\lim }\limits_{t \to \infty } {e_u} = 0$ ， $\mathop {\lim }\limits_{t \to \infty } {e_\psi } = 0$ 。

2 基于反步法的控制器设计

从式（4）描述的外环控制中输出虚拟控制量 ${f_1}$ ， ${f_2}$ ，从中进行非线性解耦，获得速度命令 ${u_d}$ ，航向角命令 ${\psi _d}$ ，内环生成推力 $ {F_1} $ 和舵角 $ {\delta _r} $ 跟踪速度命令 ${u_d}$ ，航向角命令 ${\psi _d}$ ，其总体控制框图如图1所示。

2.1 位置控制器设计

令虚拟控制量 ${f_1} = u\cos \psi $ ， ${f_2} = u\sin \psi $ ，则位置误差方程为：

$ \left\{ {\begin{array}{*{20}{c}} {{{\dot e}_x} = {f_1} - {{\dot x}_d}(t)} ，\\ {{{\dot e}_z} = - {f_2} - {{\dot z}_d}(t)}。\end{array}} \right. $

(8)

构建Lyapunov函数为：

$ V = \frac{1}{2}e_x^2 + \frac{1}{2}e_z^2 ，$

(9)

其导数为：

$ \dot V = e_x^{}({f_1} - {\dot x_d}) + e_z^{}( - {f_2} - {\dot z_d}) ，$

(10)

只需要取

$ \left\{ \begin{gathered} {f_1} = - {k_1}{e_x} + {{\dot x}_d}，\\ {f_2} = {k_2}{e_z} - {{\dot z}_d} 。\\ \end{gathered} \right. $

(11)

式中， ${k_1} > 0$ 且 ${k_2} > 0$ 。

则使得

$ \dot V = - {k_1}e_x^2 - {k_2}e_z^2 < 0 。$

(12)

证明系统稳定。此时速度命令 ${u_d}$ 与航向角命令 ${\psi _d}$ 可由式（13）进行非线性解耦获得。

$ \begin{gathered} {u_d} = \sqrt {f_1^2 + f_2^2}，\\ {\psi _d} = \arctan \Bigg(\frac{{{f_2}}}{{{f_1}}}\Bigg)。\\ \end{gathered} $

(13)

2.2 速度与航向控制器设计

针对式（6）所描述的速度误差方程，取Lyapunov函数为：

$ {V_1} = \frac{1}{2}e_u^2 ，$

(14)

其导数为：

$ {\dot V_1} = {e_u}\left(\dfrac{{{F_1}{\text{ + }}\dfrac{1}{2}\rho {u^2}S{C_x}}}{{m + {\lambda _{11}}}} - {\dot u_d}\right)。$

(15)

令

$ \frac{{{F_1}{\text{ + }}\dfrac{1}{2}\rho {u^2}S{C_x}}}{{m + {\lambda _{11}}}} - {\dot u_d} = - {k_3}{e_u}，$

(16)

则使得

$ {\dot V_1} = - {k_3}e_u^2 < 0 ，$

(17)

证明速度跟踪系统稳定。此时推力 ${F_1}$ 为：

$ {F_1} = (m + {\lambda _{11}})( - {k_3}{e_u} + {\dot u_d}) - \frac{1}{2}\rho {u^2}S{C_x}，$

(18)

针对式（7）所描述的航向误差模型，取Lyapunov函数为：

$ {V_2} = \frac{1}{2}e_\psi ^2 ，$

(19)

其导数为：

$ {\dot V_2} = e_\psi ^{}(q - {\dot \psi _d}) ，$

(20)

令

$ q - {\dot \psi _d} = - {k_4}{e_\psi } ，$

(21)

则使得 ${\dot V_2} < 0$ ，此时有中间控制变量 ${q_r} = {\dot \psi _d} - {k_4}{e_\psi }$ ，要使得航行器稳定跟踪 ${q_r}$ ，取跟踪误差 $ {e_q} = q - {q_r} $ ，对其求导有：

$ {\dot e_q} = \dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} - {\dot q_r} ，$

(22)

取增广Lyapunov函数：

$ {V_3} = {V_2} + \frac{1}{2}e_q^2 ，$

(23)

对其求导有：

$ \begin{split} {{\dot V}_3} =& e_\psi ^{}({e_q} + {q_r} - {{\dot \psi }_d}) + {e_q}\left(\dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} - {{\dot q}_r}\right) = \\ &- {k_4}e_\psi ^2{\text{ + }}{e_q}\left(\dfrac{{\dfrac{1}{2}\rho {u^2}SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r})}}{{{J_y} + {\lambda _{55}}}} - {{\dot q}_r} + {e_\psi }\right)，\\[-25pt] \end{split} $

(24)

只需要取

$ {\delta _r} = \dfrac{{\dfrac{{( - {k_5}{e_q} - {e_\psi } + {{\dot q}_r})({J_y} + {\lambda _{55}})}}{{\dfrac{1}{2}\rho {u^2}SL}} - m_y^{\bar q}\bar q}}{{m_y^{{\delta _r}}}}，$

(25)

则使得

$ {\dot V_3} = - {k_4}e_\psi ^2 - {k_5}e_q^2 < 0 ，$

(26)

证明系统稳定。

本文只证明了位置控制器、航向控制器和速度控制器稳定，并没有证明级联系统稳定。根据文献[6]，只要内环速度控制器、姿态控制器满足收敛速度远快于外环位置控制的条件，那么分离设计内外控制器也能保证系统稳定，由于欠驱动航行器内环控制系统响应也是快于外环位置控制系统的，这里只证明内外环控制器分别稳定。

建模时忽略了侧滑角以及侧滑速度带来的影响，文献[7]中已经证明，只要侧滑速度有界，那么相当于在Lyapunov函数上增加了一个有界扰动，使得系统从渐进稳定弱化为有界稳定，并不会引起发散。

3 仿真验证 3.1 UUV动力学模型

根据文献[5]，建立UUV水平面运动动力学模型如下：

$ \left\{ \begin{gathered} (m + {\lambda _{11}})\dot u + mwq = {F_1}{\text{ + }}\frac{1}{2}\rho {V_s}^2S{C_x}，\\ (m + {\lambda _{33}})\dot w - muq = \frac{1}{2}\rho {V_s}^2S(C_z^{\bar q}\bar q + C_z^{{\delta _r}}{\delta _r} + C_z^\beta \beta ) ，\\ ({J_y} + {\lambda _{55}})\dot q = \frac{1}{2}\rho {V_s}^2SL(m_y^{\bar q}\bar q + m_y^{{\delta _r}}{\delta _r} + m_y^\beta \beta )。\\ \end{gathered} \right. $

(27)

式中： $ {V_s} = \sqrt {{u^2} + {w^2}} $ ， $\beta $ 为侧滑角，其余参数定义与前述相同。

注意到动力学模型（27）与控制模型（5）的不同，推导控制算法时采用简化的模型（5），仿真验证采用侧滑角未简化的模型（27），进一步通过仿真验证简化侧滑角进行控制推导的可行性。

3.2 轨迹跟踪仿真结果

将某UUV参数代入式（27），取目标轨线为：

$ \left\{ \begin{gathered} {x_1} = 100\sin (0.02t) ，\\ {z_1} = 3t 。\\ \end{gathered} \right. $

(28)

控制参数取 ${k_1} = 0.1$ ， ${k_2} = 0.1$ ， ${k_3} = 0.3$ ， ${k_4} = 4$ ， ${k_5} = 7$ 。其轨迹跟踪结果如图2所示。

可以看出，即使平面轨迹以正弦形式迅速变化，航行器仍然能够较好的跟踪目标轨迹，实现位置控制，其位置误差如图3所示。

可以看到，航行器东向、北向位置误差在双环控制作用下均在50s左右收敛到0并维持稳定，其内环的速度控制误差、航向控制误差如图4所示。

可以看出，速度控制约在20s左右稳定，航向控制约5s左右稳定，满足内环控制速度大于外环控制速度的要求。

从图2～图4的仿真结果中可知，欠驱动无人水下航行器可以采用侧滑角小角度假设的模型（5）进行控制器设计，其双环控制器能够保证轨迹跟踪位置稳定。

3.3 队形保持仿真结果

依据位置误差模型（8）设计的控制律得到的结果是相对轨迹的位置误差为0，实际上也可以增加一个误差偏移量使得位置误差收敛到给定值，结果便是航行器会相对于指定轨迹保持一个固定距离，这一结论和队形保持的需求一致。

设虚拟领航者的目标轨迹为：

$ \begin{split} & {x_2} = 3t，\\ & {z_2} = 2t。\end{split} $

(29)

该领航者轨迹提前装订至所有跟随者中，取跟随者1的相对领航者位置 ${l_1} = \left( {40,30} \right)$ ，跟随者2的相对位置 ${l_2} = \left( {40, - 30} \right)$ ，且在 $t > 200$ 后， ${l'_1} = \left( {0,30} \right)$ ， ${l'_2} = $ $\left( {0, - 30} \right)$ ，其编队轨迹如图5～图7所示，跟随者位置误差如图8和图9所示。

从图5～图9可以看出，采用和轨迹跟踪相同的控制方法，只要增加相对位置偏移，是可以实现编队位置保持的，并且能使得其相对位置误差稳定。图7中后段有一部分点位出现稀疏现象，正是因为200 s时相对位置指令变化，相对位置误差增大，航行器重新调整目标位置造成。同时由于该方法虚拟领航者轨迹为预先装订，只要编队中各从者能保持时钟统一，那么队形保持过程中不需要进行通信修正，对通信的依赖度较小。

4 结　语

针对欠驱动无人水下航行器相对位置保持的需求，提出一种航行器相对目标位置稳定的欠驱动无人水下航行器双环位置控制算法。该方法将航行器的位置控制分解为外环位置环与内环姿态/速度环，实现欠驱动无人水下航行器的相对位置保持。同时将编队位置保持与轨迹跟踪问题均建立为相对位置保持问题，采用相同的控制方法对其进行控制。仿真结果表明，相对位置保持法能有效实现航行器的平面轨迹跟踪与编队位置保持，保证航行器对运动目标的相对位置稳定，此时编队采用虚拟领航者下的主从式编队构型，对通信依赖度小。由于在惯性系下设计位置误差方程，可使得外环控制简化为线性控制，对于现有产品仅需增加线性位置控制环即可实现相对位置保持，亦即实现编队控制与轨迹跟踪，方法快捷简单。