0 引　言

水下机器人（Remote Operated Vehicle，ROV）是一种可替代人类长时间、大面积和恶劣水域中进行作业的重要工具^[1]，不仅如此，ROV在无人化军事海上主动权上也发挥重要作用^[2]。但是，ROV是一个六自由度的欠驱动强耦合系统，不同于空中和陆地，在ROV的航行过程中，它会受到很多因素的影响，且干扰系数较大，一旦 ROV受到干扰，其航行就会变得不稳定，很容易出现偏离航向等违背人类意愿的行为。因此，确保控制稳定是ROV广泛应用的基础。

目前，水下机器人的主流控制算法有PID控制^[3]、滑模控制^[4]、神经网络^[5]、反步法^[6]、自抗扰技术(ADRC)^[7]等。对于复杂的非线性系统以及对系统参数变化更为敏感的情况，韩京清^[8]提出的自抗扰控制技术，在保留了PID算法不依靠精确控制目标模型优势的前提下，通过合理安排延迟环节，解决比例-积分-微分控制中存在的响应速率和超调量之间难以协调的问题^[9]。ADRC的核心思想是提出了一种总扰动的概念，将系统内部的耦合、外部扰动、模型的不准确性等不确定因素视为总扰动，在观测出总扰动后在系统扰动来临之前给予提前补偿^[10]。

鄢化彪等^[11]中将ADRC用于四旋翼无人机系统，对非线性函数$fal$进行了改进，得到了在正弦函数基础上构造的$xfal$，将其用于扩张状态观测器设计，最后通过实验表明比其他改进的ADRC方法都有更好抗干扰能力。

本文根据已有书籍以及相关文献建立ROV的动力学模型，并针对本文研究的姿态控制中偏航角控制对模型进行解耦简化；考虑到水下机器人系统在水下时会存在诸多综合干扰的情况，从提高系统的抗干扰性能出发，对ADRC中最为关键的模块ESO的工作原理分析，以二次函数的特性为基础，根据非线性曲线选取的控制工程核心思想，对二次函数进行了适当的修改，构造了一种新型光滑的非线性曲线函数$cfal$以改善ESO的性能，从而提高系统的抗干扰能力。

1 ROV动力学模型建立

假设水下机器人具备以下理论条件：

1）ROV为均匀刚体，具有均匀的质量分布；2）ROV完全对称的机身结构；3）ROV重心与中心在同一位置。可得ROV的刚体动力学模型^[12]：

$ \left\{ \begin{gathered} X = m[it - \nu r + wq - {x_G}\left( {{q^2} + {r^2}} \right) + \\ {y_G}\left( {pq - \dot r} \right) + {z_G}\left( {pr + \dot q} \right)] ，\\ Y = m[\dot \nu - wp + ur - {y_G}\left( {{r^2} + {p^2}} \right) + \\ {z_G}\left( {qr - \dot p} \right) + {x_G}\left( {qp + \dot r} \right)]，\\ Z = m[\dot w - uq + \nu p - {z_G}\left( {{p^2} + {q^2}} \right) + \\ {x_G}\left( {rp - \dot q} \right) + {y_G}\left( {rq + \dot p} \right)]，\\ K = m[{y_G}(w + \nu p - uq) - \\ {z_G}(\dot \nu + ur - wp)] + {I_x}\dot p + ({I_z} - {I_y})qr，\\ M = m[{z_G}\left( {ii + wq - \nu r} \right) - \\ {x_G}\left( {\dot w + \nu p - uq} \right)] + {I_y}\dot q + ({I_x} - {I_z})qr，\\ N = m[{x_G}(\dot \nu + ur - wp) - \\ {y_G}(it + wq - vr)] + {I_z}\dot r + ({I_y} - {I_x})qr。\\ \end{gathered} \right. $

(1)

式中：$ X、Y、Z $分别为机体所受的合外力；$ K、M、N $分别为机体所受的合外力矩；$ u、v、w $分别为ROV的进退、横移、升沉速度；$ {I}_{x}、{I}_{y}、{I}_{z} $分别为ROV各轴的转动惯量；$ p、q、r $分别为ROV的横摇、纵摇、偏航角速度；$ {x}_{G}、{y}_{G}、{z}_{G} $分别为ROV重心各轴上坐标；$m$为ROV质量。

由上式可知，水下机器人的任一自由度的运动都受其他自由度的影响，则说明在ROV的运动控制中存在着严重的耦合。为了达到理想的控制效果，使控制变得简单，达到预期效果，在实际控制中可以采取独立控制的方式进行系统的解耦。例如，ROV在偏航运动时，不进行其他5个自由度的运动。

在本文中水下机器人各转动自由度结构类似，所以各姿态控制器模型几乎相同，更多的是存在数值差异。故本文以水下机器人偏航运动为例，则可进一步简化到水平面运动方程：

$ \left\{ \begin{gathered} {U_X}{\text{cos}}\alpha - {\lambda _{11}}\dot u + {X_{uu}}{u^2} + {X_{vv}}{v^2} + \\ {X_{rr}}{r^2} + {X_{vr}}vr = m(\dot u - vr) ，\\ {U_Y}{\text{cos}}\alpha + {Y_v}v + {Y_r}r + {Y_{v|v|}}v|v| + \\ {Y_{v|r|}}v|r| + {Y_{r|r|}}r|r| - {\lambda _{22}}\dot v = m(\dot v + ur)，\\ {U_N}{l_1} - {\lambda _{66}}\dot r + {N_v}v + {N_r}r + {N_{v|v|}}v|v| + \\ {N_{v|r|}}v|r| + {N_{r|r|}}r|r| = {I_z}\dot r。\\ \end{gathered} \right. $

(2)

式中：$ {U}_{X}、{U}_{Y}、{U}_{N} $为$X$轴、$Y$轴、偏航角运动虚拟控制量；$ {X}_{\left(\text{·}\right)}、{Y}_{\left(\text{·}\right)}、{N}_{\left(\text{·}\right)} $为相关的粘性水动力系数；${\lambda _{\left( {\text{·}} \right)}}$为相关自由度间耦合的惯性水动力系数。其中的水动力系数可根据CFD方法及窦铭昊等^[13]建立的流体模型仿真模拟获得。

在只考虑偏航角通道，并忽略数值较小的高阶耦合水动力项$ {N_{r|r|}}r|r| $，则水下机器人的偏航角通道动力学方程为：

$ {U_N}{l_1} = ({I_z} + {\lambda _{66}})\dot r - {N_r}r。$

(3)

令$ {\psi _1} $为偏航角，${\psi _2}$为偏航角速度，并将内部忽略的耦合、忽略的水动力项以及外部扰动统称为$\Delta $，则有：

$ \left\{ \begin{gathered} {{\dot \psi }_1} = {\psi _2} ，\\ {{\dot \psi }_2} = \frac{{{N_r}{\psi _2}}}{{{I_z} + {\lambda _{66}}}} + \frac{{{l_1}}}{{{I_z} + {\lambda _{66}}}}{U_N} + \Delta，\\ y = {\psi _1} 。\\ \end{gathered} \right. $

(4)

2 扩张状态观测器改进 2.1 传统扩张状态观测器

ADRC中ESO模块是最为关键的一环，其原理是提出将所有干扰估计为新的状态，该估计被称为总扰动。

由式(4)，将$\Delta $扩张为一个新的状态${\psi _3}$，则有：

$ \left\{ \begin{gathered} {{\dot \psi }_1} = {\psi _2}，\\ {{\dot \psi }_2} = {\psi _3} + {b_0}{U_N} + {a_0}，\\ {{\dot \psi }_3} = \omega，\\ y = {\psi _1} 。\\ \end{gathered} \right. $

(5)

式中：$ {b_0} = \displaystyle\frac{{{l_1}}}{{{I_z} + {\lambda _{66}}}} $ ；$ {a_0} = \displaystyle\frac{{{N_r}{\psi _2}}}{{{I_z} + {\lambda _{66}}}} $。

对式(5)，建立观测器如下：

$ \left\{ \begin{gathered} e = {z_1} - y，\\ {{\dot z}_1} = {z_2} - {\beta _1}e ，\\ {{\dot z}_2} = {z_3} - {\beta _2}fal(e,{\alpha _1},\delta ) + {b_0}{U_N} + {a_0} ，\\ {{\dot z}_3} = - {\beta _3}fal(e,{\alpha _2},\delta )。\\ \end{gathered} \right. $

(6)

式中：$e$为实际角度与期望角度误差；$y$为被控系统输出；${b_0}$为输入系数；${\beta _i}$为误差增益系数；$ {z}_{1}、{z}_{2}、{z}_{3} $分别为角度估计、角速度估计、角加速度估计；$fal$为自抗扰控制器中传统的误差增益函数，其形式如下：

$ fal(x,\alpha,\delta)=\left\{\begin{aligned} & \displaystyle\frac{x}{\delta^{(1-\alpha)}}，\ \ \quad\left|x\right|\leqslant\delta，\\ & \mathrm{sign}(x)\left|x\right|^{\alpha}，\ \ \quad\left|x\right| > \delta。\end{aligned}\right. $

(7)

式中：$\alpha $为幂次且为常数；$x$为自变量；$\delta $为线性区间长度且与线性区间斜率成反比且为常数。

2.2 构建cfal函数

1）$ \mathrm{fal} $函数分析

图1为$ \mathrm{fal} $函数在$\delta $为0.2，即线性小误差区间为0.2之内，$\alpha $分别为0.25，0.5，0.75的曲线。如图1可知，提高$\alpha $的值便可加快误差收敛速度，但与此同时提高了线性区间的斜率且区间切换并不够平滑，会导致在原点领域范围内易产生高频抖振现象。这就会出现增益过大从而导致ESO观测不准致使系统整体性能不佳的问题。

2）$ \mathrm{cfal} $曲线的构建

根据于洪国等^[14]的中非线性曲线的构建方法，为此基于二次函数的特性并稍加改进，使其具有非线性曲线的特性，构建$ \mathrm{cfal} $函数：

$ \left\{\begin{gathered}\begin{array}{*{20}{c}}-ax(x-b)，& \quad0 < x < \displaystyle\frac{b}{2}，\end{array} \\ \begin{array}{*{20}{c}}ax(x+b)，& \quad-\displaystyle\frac{b}{2} < x < 0，\end{array} \\ \begin{array}{*{20}{c}}\mathrm{sign}(x)\displaystyle\frac{ab^2}{4}，& \quad|x| > \displaystyle\frac{b}{2}。\end{array} \\ \end{gathered}\right. $

(8)

式中：$ a、b $为可调参数且都大于0。

$ \mathrm{cfal} $函数利用了二次函数关于对称轴对称的性质构造出一个连续光滑的奇函数，且在其对称轴为最大值处斜率为0的特性衔接最大值作为大误差区间的误差增益并且可通过联立方程组求解参数$ a、b $：

$ \left\{ \begin{gathered} \frac{{a{b^2}}}{4} = x，\\ ab = y 。\\ \end{gathered} \right. $

(9)

式中：$x$为大误差区间增益，可保证大误差小增益的特性；$y$为在原点处误差收敛速率，可保证小误差大增益的特性。

通过$a、b$二者的调配参数可使ROV系统在抗干扰能力和抖振之间进行有效的平衡，进一步到达预期控制器的稳定性和收敛性。

为了方便直观比较，如图2、图3为$ \mathrm{fal}（e,0.5,1） $和$ \mathrm{cfal}（e,4,1） $曲线比较；图4为其导数图。

由图3可以看出，相比$ \mathrm{fal} $函数，$ \mathrm{cfal} $在接近原点更能够实现“小误差大增益”的特性。由图2可以清楚看出，在误差较大时，$ \mathrm{cfal} $函数的输出为一个相对较小的常值，并保证了“大误差，小增益”的特性。由图4可看出，$ \mathrm{cfal} $在大误差时，输出增益明显小于$ \mathrm{fal} $的输出增益。

为进一步体现$ \mathrm{cfal} $相比较于$ \mathrm{fal} $更能满足小误差获得大增益，大误差获得小增益的思想，因二者均为奇函数，在此取图2中小误差区间为（0,0.5），大误差区间（0.5,2），求其在各自区间中平均增益变化量，其表达式如下：

$ g = \frac{1}{n}\sum\limits_{i = 0}^n {\frac{{({c_i} - {f_i})}}{{{f_i}}}}。$

(10)

式中：n为离散化总个数；${c_i}$为cfal函数误差增益；$ {f_i} $为fal函数误差增益。

以此得出，在小误差区间误差增益提升了20.6%，在大误差区间误差增益减小了10.0%。

综上所述，若用cfal函数代替fal函数设计ESO理论上可以更加有效的提高系统的抗干扰能力、减小系统的抖振。

2.3 改进ESO的稳定性条件

在系统扰动为0时，计算ESO的误差收敛性。将式(6)中fal函数改成cfal函数得到：

$ \left\{ \begin{gathered} e = {z_1} - y , \\ {{\dot z}_1} = {z_2} - {\beta _1}e , \\ {{\dot z}_2} = {z_3} - {\beta _2}cfal(e,{a_1},{b_1}) + {b_0}{U_N} + {a_0} , \\ {{\dot z}_3} = - {\beta _3}cfal(e,{a_2},{b_2}) 。\\ \end{gathered} \right. $

(11)

将其与式(5)联立后可得：

$ \left\{ \begin{gathered} {{\dot e}_1} = {e_2} - {\beta _1}{e_1} ，\\ {{\dot e}_2} = {e_3} - {\beta _2}cfa{l_1}({e_1}) ，\\ {{\dot e}_3} = - {\beta _3}cfa{l_2}({e_1})。\\ \end{gathered} \right. $

(12)

将式(12)简写后：

$ \boldsymbol{\dot{e}}=-\boldsymbol{A(e)e}。$

(13)

式中：

$ {\boldsymbol{A}({\boldsymbol e})}=\left[\begin{array}{*{20}{c}}\beta_1 & -1 & 0 \\ \beta_2\displaystyle\frac{c\mathrm{fal}_1(e_1)}{e_1} & 0 & -1 \\ \beta_3\displaystyle\frac{c\mathrm{fal}_2(e_1)}{e_1} & 0 & 0\end{array}\right]。$

(14)

易得$ \mathrm{cfal}\in\left[0，ab^2/4\right] $。

引理1^[15]：存在满足角元素为正数的矩阵${\boldsymbol{D}}$，使得矩阵${\boldsymbol{DA(e)}}$为对称正定矩阵，就能够证明式(13)在原点处是Lyapunov渐进稳定的。

因此，只要能够构造矩阵${\boldsymbol{D}}$，证明ESO系统误差式(13)是渐进稳定的。下面推导矩阵${\boldsymbol{D}}$所需要满足的条件。

假设矩阵${\boldsymbol{D}}$：

$ {\boldsymbol{D}} = \left[ {\begin{array}{*{20}{c}} {{d_{11}}} & {{d_{12}}} & {{d_{13}}}\\ { - {d_{21}}} & {{d_{22}}} & {{d_{23}}}\\ { - {d_{13}}} & { - {d_{23}}} & {{d_{33}}} \end{array}} \right]。$

(15)

式中：$ {d}_{11}、{d}_{22}、{d}_{33} $均为大于0的数。

则对矩阵${\boldsymbol{DA(e)}}$进行处理得到：

$ {\boldsymbol{DA(e)}} = \left[ {\begin{array}{*{20}{c}} {{D_{11}}}&{ - {d_{11}}}&{ - {d_{12}}} \\ {{D_{21}}}&{{d_{12}}}&{ - {d_{22}}} \\ {{D_{31}}}&{{d_{13}}}&{{d_{23}}} \end{array}} \right]。$

(16)

式中：$ {D_{11}} =\; {d_{11}}{\beta _1} + {d_{12}}{\beta _2}F + {d_{13}}{\beta _3}F $，$ {D_{21}} = - {d_{12}}{\beta _1} + {d_{22}}{\beta _2}F + {d_{23}}{\beta _3}F $，${D_{31}} = - {d_{13}}{\beta _1} - {d_{23}}{\beta _2}F + {d_{33}}{\beta _3}F $。

若保证式(16)为对称正定矩阵，根据对称正定矩阵的性质，即：

$ \begin{gathered} {d_{22}} = - {d_{13}}, \quad {D_{31}} = - {d_{12}}, \quad {D_{21}} = - {d_{11}}, \quad {D_{11}} > 0, \\ \left| {\begin{array}{*{20}{c}} {{D_{11}}}&{ - {d_{11}}} \\ { - {d_{11}}}&{{d_{12}}} \end{array}} \right| > 0，\quad \left| {\begin{array}{*{20}{c}} {{D_{11}}}&{ - {d_{11}}}&{ - {d_{12}}} \\ { - {d_{11}}}&{{d_{12}}}&{ - {d_{22}}} \\ { - {d_{12}}}&{ - {d_{22}}}&{{d_{23}}} \end{array}} \right| > 0 。\\ \end{gathered} $

在不影响结果的情况下，为了简化推导过程，在不影响结论的情况下，令$ F_i=\mathrm{cfal}_i(e_1)/e_1 $、$B = {\beta _1}{\beta _2} - {\beta _3}$、${d_{11}} = 1$、${d_{22}} = {d_{33}} = \psi $；${F_i}$为原点到函数$ \mathrm{cfal}_i(e_1) $上任意一点之间的斜率，即${F_i} \in \left( {0，ab} \right)$，$\psi $是待定的正数。

联立方程组得：当满足$B > 0$时，使得${\boldsymbol{DA(e)}}$为对称正定矩阵。其中${\boldsymbol{D}}$表现形式如下：

$ {\boldsymbol{D}} = \left[ {\begin{array}{*{20}{c}} 1&{\displaystyle\frac{{{b_2}}}{B} + {\delta_1}}&{ - \psi} \\ {\displaystyle\frac{{ - {b_2}}}{B} - {\delta_1}}&\psi &{\displaystyle\frac{1}{{B{F_1}}} + {\delta_2}} \\ \psi &{\displaystyle\frac{{ - 1}}{{B{F_1}}} - {\delta_2}}&\psi \end{array}} \right] $

(17)

式中：$ \psi 、{\delta }_{1}、{\delta }_{2} $均为无穷小的正数。

当改进的非线性函数中的增益系数$ {\beta }_{1}、{\beta }_{2}、{\beta }_{3} $之间的关系满足${\beta _1}{\beta _2} - {\beta _3} > 0$时，然后，构造出满足引理1条件的矩阵${\boldsymbol{D}}$，使得矩阵${\boldsymbol{DA(e)}}$对称正定，由此便可得水下机器人系统误差式(13)的零解是Lyapunov渐进稳定。

3 ROV姿态控制仿真 3.1 仿真平台及参数

为进一步验证本文所给出的$ \mathrm{cfal} $函数对ESO算法的改进，通过上述控制器的设计思路，在Matlab软件中利用Simulink仿真平台对水下机器人偏航角控制问题进行仿真研究，并与常规PID和自抗扰控制算法进行比较分析。为了保证改进的严谨性。其中基于ADRC的算法中TD模块和NLSEF模块中各参数都选取一致，从而保证单一变量性，仅ESO中非线性曲线函数中参数各异。

由于ROV系统3个姿态角数学模型相似，更多存在数值上的差异，在此以偏航角为例进行完整的抗干扰仿真实验，其中具体参数及系数如表1～表4所示。

3.2 实验设计及结果分析

设ROV初始状态时的偏航角为0°，期望指令为$\varepsilon = \left\{ {\begin{array}{*{20}{c}} {30}，&{t \geqslant 1} \\ 0，&{t \leqslant 1} \end{array}} \right.$，仿真时间20 s。

为模拟真实水下的复杂环境，对3个偏航角控制器输出通道分别添加4种不同的强干扰，为此设计5组实验，作出控制量整体分析。

如图5所示，第1组实验在无干扰的情况下各曲线响应，其目的为了判断各控制器的快速性和超调性。

如图6所示，第2组实验为模拟水下机器人受到突发干扰的情况，对通道加入幅值为8000，脉冲宽度为0.5 s的矩形方波。

如图7所示，第3组实验为模拟水下机器人在水中受到周期性连续干扰的情况，对通道加入幅值为$8\;000\sin (10t)$的正弦波，以验证控制器应对周期性连续干扰的效果。

如图8所示，第4组实验为模拟水下机器人受到不规则的连续干扰，对通道加入白噪声，以验证控制器的抗扰效果。

如图9所示，第5组实验为模拟水下机器人在水中的综合复杂情况，同时对通道加入上述的3种干扰，以验证控制器的综合抗干扰能力。由图9(a)可知，PID控制器在面对干扰时，处于一个失控的状态。而由图9(b)可以看出基于$ \mathrm{cfal} $的ADRC控制器表现出更好的抗干扰能力。

为了更加明确的衡量ROV受到干扰后的情况，针对上述实验采取均方误差（Mean Square Error，MSE）的方式量化，公式为：

$ {MSE} = \frac{1}{N}{\sum\limits_{i = 1}^N {\left( {{{\hat y}_i} - {y_i}} \right)} ^2}。$

(18)

式中：$N$离散化后总个数；${y_i}$为实际角度；${\hat y_i}$为期望角度。

各组实验均为加入干扰后具有代表性的区间（见表5）。

可知，ROV响应速率方面，2款ADRC控制器性能均优于PID，对指令进行有效响应。但$ \mathrm{cfal} $响应更加迅速超调量更小。其原因是$ \mathrm{cfal} $的曲线在区间切换时更加平滑，故收敛也较为平滑，在一些白噪声类似干扰下也不易发生抖振。

在面对扰动时，PID都表现的较差，甚至出现失控状态，这也说明PID这类线性控制器在面对非线性系统时，其控制效果是非常有限的。反观ADRC控制器均能有更好的抗干扰效果。且$ \mathrm{cfal} $比$ \mathrm{fal} $对扰动的影响具有更强的抑制，这是由于$ \mathrm{cfal} $的曲线更好地保证了小误差大增益和大误差小增益的特性，使得ESO对系统的扰动估计更加准确，最终补偿值更贴近于实际总扰动。

最后，如图10作出各控制器在不同干扰时工作区间输出控制量，以便直观看出输出控制量，其中PID控制量过大，此处不作比较。

为此，为明确衡量控制量的减少情况，取2款控制器控制量与时间轴围成的面积进行对比，cfal相对fal的控制量相对减少量并作出表6所示，其中均为响应代表区间。

可见，$ \mathrm{cfal} $与传统方法相比，该改进方法在复杂环境下表现出更强的鲁棒性，有效应对了各种干扰源，确保了系统的稳定性的同时，在计算资源和能源消耗方面也具有一定优势，尤其在长时间运行和大规模任务中能够降低整体成本。

4 结　语

1）对水下机器人的常规模型进行单自由度的简化，减少各自由度之间的耦合问题，使得各自由度独立控制，保证了对其控制的准确性。

2）深入了ADRC算法的ESO模块，从其工作原理和误差特性出发，构建一种易调节参数的新型非线性光滑曲线。且经过严密的可行性分析，证明其稳定性。

3）通过仿真并模拟水下多种干扰以贴近真实情况，验证了本文改进型ESO用于ADRC的抗干扰能力，还展现了其在实际应用中的广泛潜力，尤其适合需要长时间续航或对资源有限制的任务。

4）本文主要针对姿态控制抗干扰能力的仿真研究，仅基于理论上，最终目的也是服务于实际应用，后期将针对具体实验进一步验证该方法的可行性。且在姿态控制稳定的基础上，能否将该控制方法用于轨迹控制也可进一步探究。