基于改进自抗扰的自治水下机器人运动控制系统研究

Research on Motion Control System of Autonomous Underwater Vehicle Based on Improved Active Disturbance Rejection

摘要: 自治水下机器人控制系统是一个具有时变性、不确定性以及强耦合性的非线性复杂系统，很难建立起准确的数学模型。针对干扰下AUV的运动控制问题，提出了一种改进型自抗扰控制器：在使用fal函数的自抗扰控制器基础上，提出了具有更好平滑性的改进非线性函数nal，并且基于nal函数设计了改进后的扩张状态观测器和非线性状态误差反馈控制器。最后通过Matlab/Simulink对AUV的航向控制模型进行仿真。仿真结果表明，与传统的自抗扰控制器相比，改进型自抗扰控制器能够有效提高扰动的估计能力，增强系统抗干扰能力。

Abstract: Autonomous underwater vehicle is a nonlinear and complex system with time-varying, uncertain, and strong coupling characteristics, making it difficult to establish an accurate mathematical model. Aiming at the motion control problem of AUV under interference, an improved self disturbance rejection controller is proposed: based on the self disturbance rejection controller using the fal function, an improved nonlinear function nal with better smoothness is proposed, and an improved Extended State Observer and Nonlinear State Error Feedback Controller are designed based on the nal function. Finally, the heading control model of the AUV was simulated using Matlab/Simulink. The simulation results show that compared with traditional self disturbance rejection controllers, the improved self disturbance rejection controller can effectively improve the estimation ability of disturbances and enhance the system’s anti-interference ability.

1. 引言

随着海洋科学和人工智能的快速发展，自治水下机器人(autonomous underwater vehicle, AUV)作为智能水下机器人，由于其快速、灵活的特点，成为了人们探索各种水域的必备工具。因此，为了更好地完成海洋资源的调查[1] [2] [3]，AUV的运动控制研究迫在眉睫。

自治水下机器人作为海洋探索中的重要一环，在过去几十年中进行了大量的研究。由于工作环境多变且难以模拟，机器人各个自由度运动之间存在的强耦合等原因，使得AUV在控制上一直都存在着难点。为了达到AUV的控制器需要的稳定性、可靠性、精确性和抗干扰性的要求，国内外学者在控制算法方面进行了许多研究与工作。常见的AUV控制算法有：文献[4]中采用比例–积分–微分(proportional-integral-differential, PID)控制，是如今各行业中应用最广的控制方法，能够不依赖于模型且控制结构简单，但由于控制器的参数固定以及外界环境多变，容易导致误差大和响应时间长的问题。文献[5]采用自适应控制，能够感知环境变化并且控制的自身参数可以随时做出智能调整，从而达到最优工作状态。但是其信息处理方式较为简单从而导致控制精度下降。文献[6]运用的神经网络智能算法，适用于强非线性和强耦合系统的控制，对于外界或自身内部的微小变化适应性好，但是对于参数的确定和结构上存在很大的难度。文献[7]采用模糊控制，它的最大特点就是不需要建立被控对象的数学模型，根据积累的经验和知识转换成语言，然后去描述规则，进而根据规则去控制系统。但其相关参数的择取难度大。

自抗扰控制器(active disturbance rejection controller, ADRC)由韩京清研究并提出，在PID的基础上进行改进，采用非线性函数使得该控制器具有较高的精度和响应速度，大大提高了AUV的抗干扰能力[8]。所以近年来，各国对自抗扰控制器的研究越来越多，其中：文献[9]针对于ADRC中的扩张状态观测器(extended state observer, ESO)模块的收敛性进行了分析；文献[10]将ADRC应用于机器人的轨迹跟踪；文献[11]改进了ESO和非线性状态误差反馈控制器(nonlinear state error feedback, NLSEF)模块，提高了在受到噪声干扰时，无人机的稳定性。

为了提高AUV运动控制系统的性能表现，本文提出一种改进型的自抗扰控制器。构建出新型的非线性函数，用于解决传统非线性函数在分断点处不光滑的问题，从而提高系统的抗干扰能力，并削弱AUV运动过程中受到的干扰，最后通过Matlab/Simulink进行仿真验证，结果表明改进后的自抗扰控制系统拥有更好的控制效果。

2. 水下机器人的运动与建模

为了完成AUV在水下的运动，首先要为水下机器人建立合适的坐标系，如图1所示。第一个坐标系为固定坐标系E-ξηζ，它将坐标原点选在水平面，以水下机器人的主航向为正，Eη轴位于Eξ所在平面，即Eξ轴顺时针旋转90˚，Eζ轴竖直向下，与Eξη面垂直。第二个坐标系为运动坐标系O-xyz，它固定在AUV艇体上。将水下机器人的浮心选择为运动坐标系的原点，纵轴Ox平行于艇体基线指向艇首，横轴Oy平行于基面指向右舷，垂轴Oz指向艇底。这里的两个坐标系都是右手系[12] [13] [14]。

Figure 1. Fixed coordinates and moving coordinates

图1. 固定坐标系与运动坐标系

AUV的运动由6自由度组成，各个自由度均有相对应的运动状态与参数，分别定义为沿着Ox、Oy、Oz轴方向移动的进退、横移、潜浮，以及绕Ox、Oy、Oz轴方向旋转的横滚、俯仰和偏航。

根据刚体运动牛顿–欧拉方程和拉格朗日公式，AUV的动力学方程可表示为：

$M\dot{V}+C\left(V\right)V+D\left(V\right)V+G\left(\eta \right)=T$ (1)

其中，$V={\left[\begin{array}{cccccc}u& v& w& p& q& r\end{array}\right]}^{\text{T}}$ 表示在运动坐标系下的线速度和角速度矢量，$\eta ={\left[\begin{array}{cccccc}x& y& z& \phi & \theta & \psi \end{array}\right]}^{\text{T}}$ 表示在固定坐标系下的位置以及姿态矢量。M是惯性矩阵，包括由流体动力引起的附加重力；C(V)是哥式力和向心力矩阵；D(V)是水阻力与动力矩阵；G(η)是重力和浮力矩阵；T是外部施加的力和力矩，包括推进器推力和外部扰动。

本文所研究的AUV由沈阳工业大学研制，动力装置采用推进器控制，拥有四个可旋转的推进器。AUV可以通过接收传感器传来的位姿信息控制舵机，从而控制推进器的推力和旋转角度，使其达到期望位置与姿态，产生的推力及力矩向量可表示为：

$T=LU+W$ (2)

$U={\left[\begin{array}{cccc}{T}_1& T_2& T_3& T_4\end{array}\right]}^{\text{T}}$ (3)

式中，L是转换矩阵，U是推进器的推力矢量，T₁~T₄是推进器1~4的推力，W是来自外界的扰动。本文仅考虑固定推进器情况，将T₁~T₄水平放置，对AUV的水平面运动进行研究。

AUV在水下航行过程中所受干扰较复杂，航向和深度控制相互耦合，且模型存在不确定性，使其控制变得较为复杂，所以在实际控制过程中，采用将水平面运动和垂直面运动解耦的方法[15]，即在水平面运动时涉及到的参数为u、v、r及它们的导数，得到的水平面运动方程如下所示：

$\{\begin{array}{l}\dot{x}=u\cos \psi -v\sin \psi \hfill \\ \dot{y}=u\sin \psi +v\cos \psi \hfill \\ \dot{\psi }=r\hfill \end{array}$ (4)

$\{\begin{array}{l}\left(m-X_{\dot{u}}\right)\dot{u}=\left(m-Y_{\dot{v}}\right)vr-Y_{\dot{v}}{r}^2+\left(X_u+X_{u\left|u\right|}\left|u\right|\right)u+T_u\hfill \\ \left(m-Y_{\dot{v}}\right)\dot{v}=Y_{\dot{r}}\dot{r}-\left(m-X_{\dot{u}}\right)ur+\left(Y_v+Y_{v\left|v\right|}\left|v\right|\right)v\hfill \\ \left(I_z-N_{\dot{r}}\right)\dot{r}=N_{\dot{v}}\dot{v}-\left(X_{\dot{u}}-Y_{\dot{v}}\right)uv+\left(N_r+N_{r\left|r\right|}\left|r\right|\right)r+T_r\hfill \end{array}$ (5)

3. 改进型自抗扰控制器设计

3.1. 自抗扰控制器设计

自抗扰控制是一种强大的抗扰动技术，包括跟踪微分器(tracking differentiator, TD)、ESO和NLSEF三部分，主要原理是通过TD过渡后提供平滑的输入信号，再利用ESO进行实时动态估计和补偿不确定干扰，最后通过NLSEF对TD和ESO的输出进行组合得到最终的控制量。自抗扰控制的基本结构如图2所示。

3.1.1. 跟踪微分器TD

输入信号可以通过TD分为两个输出，分别为输入信号的过渡信号v₁与微分信号v₂。基于最大速度合成功能，ADRC建立了可实现输入信号快速跟踪和同步差分的最大速度反馈系统，构建了TD模块[16]。因此，TD提供了平滑的输入信号，减少了超调，提高了系统的稳定性，并减少了稳态误差。TD的离散算法如下所示：

$\{\begin{array}{l}{v}_1\left(k+1\right)=v_1\left(k\right)+h{v}_2\left(k\right)\\ v_2\left(k+1\right)=v_2\left(k\right)+hfh\\ fh=fhan\left(v_1\left(k\right)-v\left(t\right),v_2\left(k\right),r_0,h_0\right)\end{array}$ (6)

r₀为速度因子；h表示运算时的采样步长；h₀表示滤波因子；fhan函数为离散域内最速控制函数，定义如式(7)所示：

$\{\begin{array}{l}d=r_0{h}_0\hfill \\ d_0=h_{0}d\hfill \\ y=v_1+h_0{v}_2\hfill \\ a_0=\sqrt{d^2+8r_0\left|y\right|}\hfill \\ a=\{\begin{array}{l}{v}_2+\frac{a_0-d}{2}sign\left(y\right),\left|y\right|>d_0\\ v_2+\frac{y}{h_0},\left|y\right|\le d_0\end{array}\hfill \\ fhan=-\{\begin{array}{l}{r}_{0}sign\left(a\right),\left|a\right|>d\\ r_0\frac{a}{d},\left|a\right|\le d\end{array}\hfill \end{array}$ (7)

Figure 2. ADRC basic structure

图2. ADRC基本结构

3.1.2. 扩张状态观测器ESO

在TD的基础上设计ESO，基于需要对信号进行准确的追踪，ESO控制器常用于微分控制器之后，是自抗扰控制器中最关键的部分，用于解决扰动观测的核心问题。

$\{\begin{array}{l}e\left(k\right)=z_1\left(k\right)-y\left(k\right)\hfill \\ z_1\left(k+1\right)=z_1\left(k\right)+h\left(z_2\left(k\right)-{\beta }_{01}e\left(k\right)\right)\hfill \\ z_2\left(k+1\right)=z_2\left(k\right)+h\left(z_3\left(k\right)-{\beta }_{02}fal\left(e,{\alpha }_1,{\delta }_0\right)+b_{0}u\right)\hfill \\ z_3\left(k+1\right)=z_3\left(k\right)-h\left({\beta }_{03}fal\left(e,{\alpha }_2,{\delta }_0\right)\right)\hfill \end{array}$ (8)

其中，fal函数的定义为：

$fal\left(e,a,\delta \right)=\{\begin{array}{l}\frac{e}{{\delta }^{1-a}},\left|e\right|\le \delta \\ sign\left(e\right){\left|e\right|}^a,\left|e\right|>\delta \end{array}$ (9)

ESO会将对象的输出响应到一个新的状态变量中，该状态变量可以将所有内部和外部扰动共同归结为综合扰动，并估计该扰动[17] [18] [19] [20]。然后，根据综合扰动进行补偿。ESO不依赖于生成扰动模型，也不需要直接测量来观察扰动以获得估计，并且仅通过控制系统的输入和输出信息来估计扰动。

3.1.3. 非线性状态误差反馈控制器NLSEF

NLSEF是一个独立于控制对象的非线性控制器，能够将TD和ESO的输出进行非线性组合。

$\{\begin{array}{l}{e}_1=v_1\left(k\right)-z_1\left(k\right)\\ e_2=v_2\left(k\right)-z_2\left(k\right)\\ u_0={\beta }_{1}fal\left(e_1,a_3,\delta \right)+{\beta }_{2}fal\left(e_2,a_4,\delta \right)\end{array}$ (10)

利用反馈控制率z₃进行补偿，最终可以得到控制量u：

$u=u_0-z_3/b_0$ (11)

3.2. 非线性函数的改进

非线性函数是ADRC的核心部分，极大的决定了自抗扰控制器的控制性能。目前常用的非线性函数为fal函数。

对传统fal函数进行仿真，如图3所示，当δ = 0.25，α取0、0.25、0.5、0.75和1时传统非线性函数fal的特性曲线；如图4所示，当α = 0.25，δ取0.01、0.05、0.25、0.5和0.75时传统非线性函数fal的特性曲线。

Figure 3. When δ = 0.25, α takes different values of nonlinear function characteristic curve

图3. δ = 0.25时，α取不同值时非线性函数特性曲线

Figure 4. When α = 0.25, δ takes different values of nonlinear function characteristic curve

图4. α = 0.25时，δ取不同值时非线性函数特性曲线

通过仿真结果可以看出非线性函数中的参数α和δ对fal函数的影响：参数α影响fal函数的不光滑程度，并且参数α越大，fal函数的不光滑程度越小；参数δ影响fal函数的线性区间大小，并且参数δ越大，fal函数的线性区间越大。

对fal进行求导可以得到式(12)和式(13)：

$fa{l}^{\prime }\left(e,a,\delta \right)=\{\begin{array}{l}\alpha e^{\alpha -1},e>\delta \\ \frac{1}{{\delta }^{1-\alpha }},0<e\le \delta \end{array}$ (12)

$\alpha e^{\alpha -1}\ne \frac{1}{{\delta }^{1-\alpha }},e=\delta$ (13)

结合图3和图4可以得到以下结论：虽然fal函数是连续的，但是在分段点处是不可导的，这使得函数曲线连续性和平滑性较差。所以需要在fal函数的基础上进行改进，设计连续光滑的非线性函数nal使曲线更光滑，满足“大误差，小增益；小误差，大增益”的要求，从而改善控制器性能。

1) 当$\left|e\right|\le \delta$ 时，由于δ一般取值小于1，在此区间内采用sin函数和sinh函数拟合代替传统非线性函数中的指数函数，将拟合函数中的e用sin函数代替，e³用sinh函数代替，构建nal函数为：

$nal\left(e,a,\delta ,k\right)={\eta }_1\sin \left(e\right)+{\eta }_2{e}^2+{\eta }_3\sinh \left(e\right)$ (14)

新型非线性函数nal需要满足连续可导的条件，所以当$\left|e\right|=\delta$ 时，式(15)成立，代入式(14)可得式(16)：

$\{\begin{array}{l}nal\left(e,a,\delta ,k\right)={\delta }^{\alpha },e=\delta \\ nal\left(e,a,\delta ,k\right)=-{\delta }^{\alpha },e=-\delta \\ na{l}^{\prime }\left(e,a,\delta ,k\right)=\alpha {\delta }^{\alpha -1},e=\pm \delta \end{array}$ (15)

$\{\begin{array}{l}{\eta }_1=\frac{\alpha \cdot {\delta }^{\alpha -1}\cdot \sinh \left(\delta \right)-{\delta }^{\alpha }\cdot \cosh \left(\delta \right)}{\cos \left(\delta \right)\cdot \sinh \left(\delta \right)-\sin \left(\delta \right)\cdot \cosh \left(\delta \right)}\\ {\eta }_2=0\\ {\eta }_3=\frac{{\delta }^{\alpha -1}\left(\delta -\alpha \cdot \tan \left(\delta \right)\right)}{\sinh \left(\delta \right)-\tan \left(\delta \right)\cdot \cosh \left(\delta \right)}\end{array}$ (16)

2) 引入常数k，当$\delta <\left|e\right|<k$ 时，与fal函数相同，表达式为：

$nal\left(e,a,\delta ,k\right)=sign\left(e\right){\left|e\right|}^{\alpha }$ (17)

3) 当$k\le \left|e\right|$ 时，nal函数表达式为：

$nal\left(e,a,\delta ,k\right)=sign\left(e\right){\left|k\right|}^{\alpha }$ (18)

所以得到的新型非线性函数nal的表达式可以写成：

$nal\left(e,a,\delta ,k\right)=\{\begin{array}{l}{\eta }_1\sin \left(e\right)+{\eta }_3\sinh \left(e\right),\left|e\right|\le \delta \\ sign\left(e\right){\left|e\right|}^{\alpha },\delta <\left|e\right|<k\\ sign\left(e\right){\left|k\right|}^{\alpha },k\le \left|e\right|\end{array}$ (19)

$\{\begin{array}{l}{\eta }_1=\frac{\alpha \cdot {\delta }^{\alpha -1}\cdot \sinh \left(\delta \right)-{\delta }^{\alpha }\cdot \cosh \left(\delta \right)}{\cos \left(\delta \right)\cdot \sinh \left(\delta \right)-\sin \left(\delta \right)\cdot \cosh \left(\delta \right)}\\ {\eta }_3=\frac{{\delta }^{\alpha -1}\left(\delta -\alpha \cdot \tan \left(\delta \right)\right)}{\sinh \left(\delta \right)-\tan \left(\delta \right)\cdot \cosh \left(\delta \right)}\end{array}$ (20)

为了验证nal非线性函数，对其进行仿真。当α = 0.2、δ = 0.5和k = 1时，传统非线性函数fal与改进型非线性函数nal的特性曲线仿真结果如图5所示；将函数与误差e的比值称作误差增益，两种函数的误差增益曲线如图6所示。

从图5可以看出：nal函数在函数的分段点处拥有的平滑性和连续性都更优越。从图6可以看出，在误差接近0时，nal函数有较大的误差增益，表明nal函数的调节能力较大，提高了系统的调节速度；在误差较大时，nal函数有较小的误差增益，表明nal函数的调节力度变小，可以减少系统的超调。

4. 实验仿真与结果分析

为了验证本文设计的改进型ADRC控制器的控制效果，设定目标航向角为45˚，在Matlab/Simulink软件上对AUV的水平面运动模型进行仿真。

Figure 5. fal and nal function characteristic curves

图5. fal和nal函数特性曲线

Figure 6. Error gain curve

图6. 误差增益曲线

Figure 7. The change curve of the AUV’s course angle without interference

图7. AUV在无干扰下的航向角变化曲线

Figure 8. The change curve of AUV’s course angle under interference force

图8. AUV在干扰力下的航向角变化曲线

图7显示了无干扰时，AUV在传统自抗扰控和改进型自抗扰这两种不同控制方法下的航向角变化曲线。可以看出采用改进型自抗扰控制器的超调更小，达到稳定的时间更短，具有更高的精度。

图8显示了AUV在航向角达到稳定后，于t = 40 s时受到干扰力的航向角变化曲线，可以看出采用改进型自抗扰控制器的航向角波动幅值较小，且能更快的恢复到目标航向值，有更好的抑制扰动的效果。

5. 结论

本文分析了AUV在水平面上的运动模型，以航向控制为例，搭建了自抗扰控制器并将其改进：采用光滑的非线性函数改进ESO和NLSEF模块。通过对传统自抗扰控制器和改进型自抗扰控制器分别在有无干扰的情况下进行航向角控制仿真实验，可以看出采用改进型自抗扰控制器的系统，在航向角控制过程中超调更小，且在干扰状态下航向角变化更小，具有更好的抗干扰能力，表明在AUV的航向控制中采用改进型自抗扰技术能够更好地完成控制任务。

基金项目

辽宁省自然资源厅海洋领域新技术转化与应用项目。

NOTES

^*通讯作者。