0 引　言

在最近的几十年中，大量的海洋探索工具被开发出来，扩展了人类的研究范围，获得了关于海洋的更多知识，使得深海的矿藏能够得到有效利用^[1-2]。其中，AUV（Autonomous underwater vehicle）是一种广泛使用的潜水器，它有高速和活动范围大的优势^[3-4]。大部分AUV在水中是中性浮力的，为了维持中性浮力，AUV需要安装很多浮力材料，增加了AUV的体积及AUV在水中航行时的阻力。

本文针对一种负浮力四倾转推进器AUV（negative-buoyancy quad tilt-rotor autonomous underwater vehicle, NQTAUV），进行了抗干扰姿态跟踪控制设计。NQTAUV没有安装浮力材料，因此在水中有剩余重量。NQTAUV质量为 $m$ ，浸没于水中受到浮力为 $B$ ，且满足 $B < mg$ ， $g$ 为重力加速度。有悬停、倾转、水平巡航3种工作模式。在悬停模式下，依靠垂直向下的推进器产生升力克服剩余重量；在倾转模式下，推进器由垂直向下旋转至朝向后方，推动AUV不断水平加速。NQTAUV安装有水翼，随着水平分量推力的增加，AUV速度加快，机翼产生的流体升力克服水中剩余重量^[5-6]，实现水平巡航。

NQTAUV有独特的机身倾转悬停（body-tilt-hover, BTH）功能，即在机身实现大幅度俯仰角机动的时候，能够悬停在水中一个位置点，而不产生水平的分力，不会引起水平的位移。对于普通AUV来说，一般是在艇体尾部有一个推进器，部分在艇体上还有垂向和横向的推进器，实现垂向和水平机动。不过，这些推进器一般固定在艇体上，只能提供固定方向的力或力矩，不能提供矢量推力，所以不能做机身倾转悬停。然而，NQTAUV可以借助自身的矢量推力进行机身倾转悬停^[7-8]。BTH是一项有用的功能。比如，AUV前部一般安装有摄像机，用以提供视频信息。通常来讲，为了更大的视角，摄像机安装在一个二自由度云台上。然而，当云台损坏后，相机的视角被限制，此时，可以采用BTH功能，操作手可以重新获得原来的视角。不过，当工作在BTH模式时，因为舵机会随着俯仰角而转动，引起转动惯量的变化，同时产生一定的干扰力矩，且不能计算或测量得到干扰量幅值。因此，需要更鲁棒的姿态控制器，实现姿态的跟踪。此外，在BTH模式下，水动力矩也会随着机身的旋转运动而改变。因此，研究干扰的估计和补偿策略，以及在干扰下的姿态跟踪，是符合实际应用需要的。

为了进行干扰下的控制，学者们进行了大量研究。Li^[9]设计了四自由度非线性滑动状态观测器，建立了ROV的洋流模型和简化的脐带干扰力模型，基于简化的索力模型表征电缆的干扰力，并实时估计缆的干扰力，提高了观测精度，减少了抖振。Yuan^[10]设计了一种非线性自抗扰控制方案控制气动肌肉执行器，采用连续离散扩展状态观测器估计总扰动，然后使用非线性复合控制器，在气动平台上进行了实验，验证了整个系统的可控性。Cui^[11]对路径跟踪问题，设计了自适应神经网络控制器，考虑了外部干扰、控制输入非线性和模型不确定性等因素。Liu^[12]用非线性干扰观测器估算了作用在飞行器上的风效应，然后将风信息与控制器结合，设计了一种抗干扰导航算法，实现风干扰下的实时路径跟踪，并理论分析了复合控制器的全局渐近稳定性，进行了软件在环的仿真。对小型飞机进行的飞行仿真试验，验证了其性能。可以看出，在以往的研究中，对于干扰观测的研究取得了大量进展，部分控制器的稳定性得到了证明。不过，这些算法在实际应用中存在调参数量多的问题，不利于控制器实际的部署和调整。

姿态跟踪控制是NQTAUV的重要功能，一般作为路径跟踪等更高一级控制的内环，它的性能决定了上层控制的精度^[13]。干扰力矩对姿态控制精度影响很大，为了补偿干扰对姿态跟踪控制的影响，设计调试简单，易于部署的抗干扰控制器，本文提出了基于干扰观测器的姿态跟踪控制器设计框架，并进行了姿态跟踪实验。

首先，基于修正的罗德里格斯参数（modified Rodrigues parameters, MRPs）姿态表示法建立NQTAUV的运动学和动力学模型，并进一步推导出姿态跟踪误差模型。其次，设计干扰观测器，估计作用在姿态上的干扰；设计姿态跟踪控制系统，并证明控制系统的稳定性。最后，进行实时姿态跟踪实验，验证了所提方法的控制性能。

1 NQTAUV数学模型 1.1 运动学和动力学

在姿态跟踪控制中用到了3个坐标系：地球坐标系 ${F_e}$ 、本体坐标系 ${F_b}$ 和目标坐标系 ${F_d}$ 。坐标系示意图如图1所示。

BTH模式示意图如图2所示。

表示刚体姿态的方法有多种，其中欧拉角是三参数姿态表示法，因为简单、直观，被广泛应用。然而，欧拉角在 $ \pm {90^ \circ }$ 有奇异；MPRs也是三参数姿态表示法，不过在 $ \pm {180^ \circ }$ 才有奇异点，能够表示更大范围的机动^[14]。因此，本文选取MRPs表示姿态。

NQTAUV的运动学和动力学方程为：

$\dot \sigma = G(\sigma )\omega $

(1)

$ J\dot \omega {\rm{ }} = - S(\omega )J\omega - {{ I}_A}\dot \omega - { C}(\omega )\omega - D(\omega ) + \tau $

(2)

其中： $\sigma = \overline {\bf{e}} tan(\Phi /4)$ 是MRPs，且满足 $\overline {{e}} \in {\mathbb{R}^3}$ ； $||\overline {\bf{e}} || = 1$ ； $\omega $ 为机体角速度向量； $J$ 为刚体相对于机体坐标系的惯性张量； ${{ I}_A}$ 为附加质量矩阵； ${ C}(\omega ) = S({I_A}\omega )$ 为水动力科氏力和向心力矩阵； ${ D}(\omega )$ 为水动力阻尼矩阵； $\tau $ 为控制输入力矩向量。 $S(\lambda )$ 的定义如下：

$ S(\lambda ) = \left[ {\begin{array}{*{20}{c}} 0&{ - {\lambda _3}}&{{\lambda _2}} \\ {{\lambda _3}}&0&{ - {\lambda _1}} \\ { - {\lambda _2}}&{{\lambda _1}}&0 \end{array}} \right] {\text{，}} $

(3)

其中： $\lambda = {[{\lambda _1},{\lambda _2},{\lambda _3}]^{\rm T}}$ ，且有性质 ${\lambda ^{\rm T}}S(\lambda ) \equiv 0$ 。 $G(\sigma )$ 满足：

$ G(\sigma ) = \frac{1}{4}[(1 - {\sigma ^T}\sigma )I + 2S(\sigma ) + 2\sigma {\sigma ^{\rm T}}] {\text{，}} $

(4)

$G(\sigma )$ 有性质：

$ \sigma G(\sigma ) = \frac{1}{4}(1 + {\sigma ^T}\sigma )\sigma {\text{。}} $

(5)

1.2 姿态跟踪误差模型

在姿态跟踪问题中，定义目标姿态 $[{\sigma _d},{\omega _d},{\dot \omega _d}]$ ，因此，相对姿态就是：

$ \tilde \sigma = \sigma \otimes \sigma _d^{ - 1} = \frac{{{\sigma _d}({\sigma ^{\rm T}}\sigma - 1) + \sigma (1 - \sigma _d^{\rm T}{\sigma _d}) - 2S({\sigma _d})\sigma }}{{1 + \sigma _d^{\rm T}{\sigma _d}{\sigma ^{\rm T}}\sigma + 2\sigma _d^{\rm T}\sigma }} {\text{，}} $

(6)

相对角速度为：

$ \tilde \omega = \omega - \tilde { R}{\omega _d} {\text{，}} $

(7)

其中， $\tilde { R}$ 表示相对姿态矩阵，具体推导如下：

$ \tilde { R} = \frac{{(1 - 6{{\tilde \sigma }^{\rm T}}\tilde \sigma + {{({{\tilde \sigma }^{\rm T}}\tilde \sigma )}^2})I + 8\tilde \sigma {{\tilde \sigma }^{\rm T}} - 4(1 - {{\tilde \sigma }^{\rm T}}\tilde \sigma )S(\tilde \sigma )}}{{{{(1 + {{\tilde \sigma }^{\rm T}}\tilde \sigma )}^2}}}{\text{，}} $

(8)

$\tilde { R}$ 的导数为：

$ \dot {\tilde { R}} = - S(\tilde \omega )\tilde { R} {\text{，}} $

(9)

$\tilde \omega $ 的导数为：

$ \dot {\tilde \omega} = \dot \omega - (\dot{ \tilde { R}}{\omega _d} + \tilde { R}{{\dot {\tilde \omega}} _d}) = \dot \omega - [ - S(\tilde \omega )\tilde { R}{\omega _d} + \tilde{ R}{{\dot{ \tilde \omega}} _d}] {\text{，}} $

(10)

因此，姿态跟踪误差模型为：

$ \dot {\tilde \sigma} = G(\tilde \sigma )\tilde \omega {\text{，}} $

(11)

$ \begin{split} \dot {\tilde \omega} = & {J^{ - 1}}[ - S(\omega )J\omega - {{ I}_A}\dot \omega - { C}(\omega )\omega - { D}(\omega ) + \tau ] - \\ &( - S(\tilde \omega )\tilde R{\omega _d} + \tilde R{{\dot {\tilde \omega}} _d}){\text{，}} \end{split} $

(12)

其中， $\omega = \tilde \omega + \tilde { R}{\omega _d}$ 。

用反馈线性化消去系统动态中的非线性项：

$ \begin{split} \tau = & \nu + [S(\omega )J\omega + {I_A}\dot \omega + C(\omega )\omega + { D}(\omega )] +\\ & J( - S(\tilde \omega )\tilde R{\omega _d} + \tilde R{{\dot {\tilde \omega}} _d}) {\text{，}} \end{split} $

(13)

其中， $\nu $ 是要设计的控制量，那么公式（12）变成：

$ \dot {\tilde \omega} = {J^{ - 1}}(\nu + d) {\text{。}} $

(14)

其中： $d$ 为干扰，且 $d$ 来源于很多方面。首先，推进器的旋转会对俯仰产生干扰力矩；其次，当NQTAUV在水中水平巡航的时候，水翼也会产生额外的力和力矩。在没有速度传感器的情况下，水动力矩不能被准确计算。在此，将所有干扰当做一个复合的干扰，估计这个复合干扰 $d$ 。记 $d' = {J^{ - 1}}d$ ，方程（14）变成：

$ \dot {\tilde \omega} = {J^{ - 1}}\nu + d' {\text{。}} $

(15)

2 控制器和干扰观测器设计

基于干扰观测器的控制框架如图3所示。

2.1 干扰观测器设计

作用在NQTAUV上的干扰不能被直接测量，因此需要引入干扰观测器估计这个干扰。假设干扰是常值，记 $\hat d'$ 是 $d'$ 的估计，估计误差记为 $\tilde d' = d' - \hat d'$ 。基于方程（15），设计干扰观测器为：

$ \dot z{\rm{ }} = - l(\tilde \omega )[z + p(\tilde \omega ) + {J^{ - 1}}\nu ] {\text{，}} $

(16)

$ \hat d'{\rm{ }} = z + p(\tilde \omega ) {\text{，}} $

(17)

其中， $l(\tilde \omega )$ 和 $p(\tilde \omega )$ 满足关系式：

$ l(\tilde \omega ) = \frac{{\partial p(\tilde \omega )}}{{\partial \tilde \omega }} {\text{。}} $

(18)

干扰观测误差的导数为：

$ \begin{split} & \dot {\tilde d'} = \dot d' - \dot {\hat d'} = 0 - [\dot z + \frac{{\partial p(\tilde \omega )}}{{\partial \tilde \omega }}\dot {\tilde \omega} ]= \\ & l(\tilde \omega )[z + p(\tilde \omega ) + {J^{ - 1}}\nu ] + l(\tilde \omega )\dot {\tilde \omega} = \\ & l(\tilde \omega )[\hat d' + {J^{ - 1}}\nu ] - l(\tilde \omega )({J^{ - 1}}\nu + d') =\\ & - l(\tilde \omega )[d' - \hat d']= \\ & - l(\tilde \omega )\tilde d' {\text{。}} \end{split} $

(19)

同时，因为 $\tilde d = J\tilde d'$ ，故得到：

$ \dot {\tilde d }= - l(\tilde \omega )\tilde d {\text{。}} $

(20)

选取 $l(\tilde \omega )$ ，使得 $\tilde d$ 是渐进稳定的，那么 $\hat d = J\hat d'$ 可以渐进稳定到 $d$ 。选取：

$ l(\tilde \omega ) = {k_3},{k_3} > 0 {\text{，}} $

(21)

故得到：

$ p(\tilde \omega ) = \int l (\tilde \omega ){\rm d}\tilde \omega = {k_3}\tilde \omega {\text{。}} $

(22)

2.2 控制器设计

选取李雅普诺夫函数：

$ {V_1}{\rm{ }} = 2{k_1}\ln (1 + {\tilde \sigma ^{\rm T}}\tilde \sigma ) + \frac{1}{2}{\tilde \omega ^{\rm T}}J\tilde \omega {\text{，}} $

(23)

其中 ${k_1} > 0$ ， ${V_1}$ 的导数为：

$ \begin{split} {{\dot V}_1} =& 4{k_1}\frac{{{{\tilde \sigma }^{\rm T}}\dot {\tilde \sigma} }}{{1 + {{\tilde \sigma }^T}\tilde \sigma }} + {{\tilde \omega }^{\rm T}}J\dot {\tilde \omega} = \\ & {k_1}{{\tilde \omega }^{\rm T}}\tilde \sigma + {{\tilde \omega }^{\rm T}}(\nu + d) = {{\tilde \omega }^{\rm T}}({k_1}\tilde \sigma + \nu + d) {\text{，}} \end{split} $

(24)

假设干扰 $d$ 由干扰观测器估计出来，且估计值是 $\hat d$ ，则估计误差是 $\tilde d = d - \hat d$ ，选取：

$ \nu = - {k_1}\tilde \sigma - {k_2}\tilde \omega - \hat d {\text{，}} $

(25)

其中 ${k_2} > 0$ ，得到：

$ {{\dot V}_1}{\rm{ }} = - {k_2}{{\tilde \omega }^{\rm T}}\tilde \omega + {{\tilde \omega }^{\rm T}}\tilde d \leqslant - {k_2}||\tilde \omega |{|^2} + ||\tilde \omega ||||\tilde d|| {\text{。}} $

(26)

2.3 稳定性分析

用李雅普诺夫稳定性理论分析整个系统的稳定性。选取李雅普诺夫函数：

$ {V_2} = \frac{1}{2}{\tilde d^2} {\text{，}} $

(27)

${V_2}$ 的导数为：

$ {{\dot V}_2} = \tilde d\dot {\tilde d} = - {k_3}{{\tilde d}^2} {\text{，}} $

(28)

又选取李雅普诺夫函数 ${V_3} = {V_1} + {V_2}$ ，求导得到：

$ {\dot V_3} = {\dot V_1} + {\dot V_2} \le - {k_2}||\tilde \omega |{|^2} + ||\tilde \omega ||||\tilde d|| - {k_3}{\tilde d^{'2}} {\text{。}} $

(29)

选取 ${k_2},{k_3}$ ，满足 ${k_2}{k_3} \geqslant \dfrac{1}{4}$ ，则 ${\dot V_3} \leqslant 0$ 。故由李雅普诺夫稳定性理论，系统是全局渐进稳定的。

3 实验结果和分析 3.1 测试平台

实验采用浸入水中的三自由度测试平台进行。NQTAUV本体通过一个球铰连接在固定的基座上，使得本体能够实现俯仰和横摇各 $ \pm {30^ \circ }$ 的活动范围，首向角可实现 ${360^ \circ }$ 的活动范围。倾转推进器可以进行 ${360^ \circ }$ 的旋转，最大旋转速度6.9 rad/s，最大提供 $15\; \rm N$ 的推力。姿态传感器采用MPU9250，提供三轴的角速度、加速度和磁场强度，经过卡尔曼滤波融合后得到姿态角和角速度。控制电路板采用STM32 F401RE，具有512 kB内存，84 MHz频率。控制器控制频率为100 Hz。NQTAUV的机械参数见表1。

3.2 姿态跟踪实验

控制器和观测器参数选取为 ${k_1} = 3.58,{k_2} = 0.67, $ ${k_3} = 10$ 。对于BTH模式，典型姿态跟踪测试信号是俯仰轴的正弦信号，因此，定义目标姿态正弦信号为：

$ {\sigma _d} = \left[ {\begin{array}{*{20}{c}} 0 \\ {\tan\left(\dfrac{{\text{π}} }{{72}}\right)\sin\left(\frac{{\text{π}}}{{10}}t\right)} \\ 0 \end{array}} \right]{\text{。}} $

(30)

在 $x$ 和 $y$ 轴施加约0.008 N.m的常值干扰力矩。为了验证所提干扰观测器和姿态跟踪控制器的有效性，在实验的 $0 \sim 50\;$ s内，将干扰观测器关闭；在 $50 \sim 100\;$ s内，将干扰观测器激活。实验结果如图4～图6所示。其中，图4显示姿态跟踪效果，图5显示控制力矩，图6显示干扰估计值。

从图4可以看出，当干扰观测器关闭时，能跟踪正弦跟踪信号，但是会产生大约0.02的姿态偏移量，且有大约1 s的相位滞后。同时可以看出，姿态波动幅值很大，说明受干扰影响很大。当干扰观测器激活后，姿态偏移量为0，相位滞后也被消除，姿态波动非常小，说明干扰被补偿掉，实际姿态能够精确跟踪目标姿态。

从图5可以看出，在激活干扰观测器后，控制输出有一个整体的增加，说明对常值干扰力矩进行了补偿。

从图6可以看出，当干扰观测器激活后，干扰估计值呈现波动，与正弦信号相位相吻合。在正弦姿态跟踪时，NQTAUV受到周期性的水动力，其周期和相位与本体运动相同，由于目标姿态信号周期为20 s，因此这是缓慢变化的干扰。这也说明干扰观测器不但可以估计常值干扰，还可以估计缓慢变化的干扰。

综上，干扰观测器能够准确估计常值干扰，对缓慢变化的干扰也有准确的估计。所提的姿态跟踪控制器能够准确跟踪目标姿态。

4 结　语

本文针对负浮力四倾转推进器水下机器人，设计了抗干扰控制框架。基于修正的罗德里格斯参数，导出了负浮力四倾转推进器水下机器人姿态跟踪的数学模型。然后设计了干扰观测器来估计干扰，并基于此干扰观测器设计了姿态跟踪控制器。控制框架的稳定性得到了证明。最后，通过实时姿态跟踪实验，比较干扰观测器激活和关闭2种不同情况下的姿态跟踪表现，验证了控制框架的性能。所提出的基于干扰观测器的控制框架，抗干扰能力强，参数数量少，调节简单，易于实际部署。