水下机器人是海洋资源可持续开发利用的重要技术手段。自主式水下航行器(autonomous underwater vehicle，AUV) 是集能源和推进技术、传感器和信号处理技术，通讯和导航技术、作战和人工干涉技术以及自动控制技术为一体的具有标准化、模块化和自主能力的水下无人平台^[1]。AUV的水下运动有6个自由度，对AUV进行控制操作时，每个操作动作会对其各个自由度产生不同程度的影响，因此AUV运动具有强耦合性和严重非线性的特点^[2]。于是，使潜航器以要求的性能指标运动是有实际意义的。经典控制理论、现代控制理论和智能控制理论中的许多控制方法，都已用在AUV的运动控制问题上^[3]。PID控制是主要的AUV控制方法之一，其不依赖于被控对象模型，是基于误差信号的反馈控制。王智学等^[4]通过5个PID控制器，实现了在有水动力干扰下的AUV五自由度运动控制。但对于复杂的非线性系统仅仅使用常规PID控制是不够的。为了增强控制器的鲁棒性，针对强非线性的AUV系统，张子迎^[5]使用神经网络PID控制技术，使控制系统具有自适应、自学习的能力。Khodayari等^[6]建立的自适应模糊PID控制器，使AUV实现了快速三维路径跟踪，并验证了所提方法在鲁棒性和超调等方面较PID方法有明显的改善。除了在控制过程中根据系统特征引入一些非线性控制方式的智能控制算法，还可以通过对被控对象的误差进行及时的观测和补偿来提升控制性能。我国著名控制论学者韩京清先生于20世纪90年代创立的自抗扰控制理论(active disturbance rejection control，ADRC)继承了经典PID控制理论“基于误差来消除误差的思想”^[7-8]，其核心为扩张状态观测器(extended state observer，ESO)，可以将被控对象中各种未知的不确定性因素归结为总扰动，将系统补偿为标准的积分串联型，辅以简单的比例-微分控制就可以实现误差控制。因非线性自抗扰控制(nonlinear active disturbance rejection control, NLADRC)控制器需整定的参数多且整定过程复杂繁琐。高志强^[9]将NLADRC中的扩张状态观测器和误差反馈控制以线性形式实现，提出了线性自抗扰控制(LADRC)方法，用带宽的概念确定控制器参数，将控制参数由原来的12个大幅度降为4个，使得整个系统便于调试和应用。

LADRC方法控制参数少且易于整定，在自抗扰领域得到了广泛的应用。且目前针对AUV的控制问题研究多是基于单纯升沉、偏航或其他形式的简化模型。以上特点构成了本文的思路。本文使用LADRC方法对AUV的六自由度模型进行深度控制并添加控制器对AUV的运动进行解耦，实验结果与PID方法对比，验证了LADRC方法在AUV控制问题上的有效性，展现了LADRC方法在控制参数较少的情况下有较PID方法更高的控制精度和控制稳定性。

1 AUV的运动与建模 1.1 坐标系与转换矩阵

水下机器人的运动研究分为运动学和动力学2个方面，其中描述机器人位置、速度、姿态等问题属于运动学问题，研究机器人在受力和力矩作用后的位置、姿态变化问题属于动力学问题。为了清晰描述AUV运动，参考有关资料，建立2种坐标系。

固定坐标系E-ξηζ，也称为地面坐标系。原点E一般取在水平面上任意一点，Eξ轴正方向指向AUV前进方向，Eη轴与Eξ轴位于同一水平面内，Eζ垂直于水平面，正方向指向地心。E-ξηζ成右手直角坐标系。运动坐标系O-xyz，为建立在船体上的坐标系，又称船体坐标系。一般运动坐标系的原点取在AUV浮心处，Ox指向艏向，Oy指向右舷，Oz指向AUV底部。

表 1中列出了AUV的速度、角速度、力和力矩在运动坐标系下的参量符号。

固定坐标系与运动坐标系之间关于平移速度的转换表达式：

$ \left[\begin{array}{c} \dot{x} \\ \dot{y} \\ \dot{z} \end{array}\right]=\boldsymbol{J}_{1}\left[\begin{array}{l} u \\ v \\ w \end{array}\right] $

(1)

其中:

$ \boldsymbol{J}_{1}=\left[\begin{array}{ccc} \cos \psi \cos \theta & \cos \psi \sin \theta \sin \varphi-\sin \psi \cos \varphi & \cos \psi \sin \theta \cos \varphi+\sin \psi \sin \varphi \\ \sin \psi \cos \theta & \sin \psi \sin \theta \sin \varphi+\cos \psi \cos \varphi & -\cos \psi \sin \varphi+\sin \psi \sin \theta \cos \varphi \\ -\sin \theta & \cos \theta \sin \varphi & \cos \theta \cos \varphi \end{array}\right] $

式中：x、y、z为固定坐标系描述的AUV位置参数; ψ为艏向角；θ为纵倾角; φ为横倾角为固定坐标系描述的AUV姿态角。

固定坐标系与运动坐标系之间关于转动速度的转换表达式：

$ \left[\begin{array}{c} \dot{\varphi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right]=\boldsymbol{J}_{2}\left[\begin{array}{c} p \\ q \\ r \end{array}\right] $

(2)

式中：

$ \boldsymbol{J}_{2}=\left[\begin{array}{ccc} 1 & \sin \varphi \tan \theta & \cos \varphi \tan \theta \\ 0 & \cos \varphi & -\sin \varphi \\ 0 & \sin \varphi / \cos \theta & \cos \varphi / \cos \theta \end{array}\right] $

根据刚体动力学理论，AUV六自由度运动的一般方程如下：

$ \left\{\begin{array}{l} m\left[(\dot{u}-v r+w q)-x_{g}\left(q^{2}+r^{2}\right)+\right. \\ \qquad \left.y_{g}(p q-\dot{r})+z_{g}(p r+\dot{q})\right]=X \\ m\left[(\dot{v}-w p+u r)-y_{g}\left(r^{2}+p^{2}\right)+\right. \\ \qquad \left.z_{g}(q r-\dot{p})+x_{g}(q p+\dot{r})\right]=Y \\ m\left[(\dot{w}-u q+v p)-z_{g}\left(q^{2}+p^{2}\right)+\right. \\ \qquad \left.y_{g}(r q+\dot{p})+x_{g}(r p-\dot{q})\right]=Z \\ I_{x} \dot{p}+\left(I_{z}-I_{y}\right) q r+m\left[y_{g}(\dot{w}+p v-q u)-\right. \\ \qquad \left.z_{g}(\dot{v}+r u-p w)\right]=K \\ I_{y} \dot{q}+\left(I_{x}-I_{z}\right) r p+m\left[z_{g}(\dot{u}+w q-v r)-\right. \\ \qquad \left.x_{g}(\dot{w}+p v-u q)\right]=M \\ I_{z} \dot{r}+\left(I_{y}-I_{x}\right) p q+m\left[x_{g}(\dot{v}+u r-p w)-\right. \\ \qquad \left.y_{g}(\dot{u}+q w-v r)\right]=N \end{array}\right. $

(3)

式中：m为AUV的质量；I_x、I_y、I_z为AUV的质量m对Ox、Oy、Oz 3个坐标轴的转动惯量；x_g、y_g、z_g为AUV的重心坐标。方程右端X、Y、Z、K、M、N为作用在AUV上的作用力和力矩，包括了流体动力、推力、浮力重力等。

1.2 AUV空间运动受力与方程

目前所使用的潜航器运动方程有很多，但各个方程只是在数学描述和数学处理方法上不同^[5]。例如一种应用广泛的受力建模方法把AUV受力分为流体动力和其他外力2种^[10]，其中流体动力为由于AUV运动而引起运动的水对AUV的反作用力，其他外力包括了推力、浮力和重力等。本文参考的AUV型号为REMUS自主式水下机器人。作用在AUV上的总作用力和力矩的表达形式为^[11]：

$ \left\{\begin{array}{l} X=X_{H S}+X_{u|u|} u|u|+X_{\dot{u}} \dot{u}+X_{w q} w q+ \\ \quad X_{q q} q q+X_{v r} v r+X_{r r} r r+X_{\text {prop }} \\ Y=Y_{H S}+Y_{v|v|} v|v|+Y_{r|r|} r|r|+Y_{\dot{v}} \dot{v}+Y_{\dot{r}} \dot{r}+ \\ \quad Y_{u r} u r+Y_{w p} w p+Y_{p q} p q+Y_{u v} u v+Y_{u u \delta_{r}} u^{2} \delta_{r} \\ Z=Z_{H S}+Z_{w|w|} w|w|+Z_{q|q|} q|q|+Z_{\dot{w}} \dot{w}+Z_{\dot{q}} \dot{q}+ \\ \quad Z_{u q} u q+Z_{v p} v p+Z_{r p} r p+Z_{u w} u w+Z_{u u \delta_{s}} u^{2} \delta_{s} \\ K=K_{H S}+K_{p|p|} p|p|+K_{\dot{p}} \dot{p}+K_{\text {prop }} \\ M=M_{H S}+M_{w|w|} w|w|+M_{q|q|} q|q|+M_{\dot{w}} \dot{w}+ \\ \quad M_{\dot{q}} \dot{q}+M_{u q} u q+M_{v p} v p+M_{r p} r p+M_{u w} u w+M_{u u \delta_s} u^{2} \delta_{s} \\ N=N_{H S}+N_{v|v|} v|v|+N_{r|r|} r|r|+N_{\dot{v}} \dot{v}+N_{\dot r} \dot{r}+ \\ \quad N_{u r} u r+N_{u p} w p+N_{p q} p q+N_{u v} u v+N_{u u \delta_{r}} u^{2} \delta_{r} \end{array}\right. $

(4)

式中：·_HS为流体静力；·_u|u|、·_v|v|、·_w|w|、·_p|p|、·_q|q|、·_r|r|为流体动力阻尼系数；Y_uv、Y_uuδr、Z_uw、Z_uuδs、M_uw、M_uuδs、N_uv、N_uuδr为主体升力、控制鳍升力与升力矩系数；X_prop为螺旋桨推力; K_prop为螺旋桨扭矩；δ_sδ_r为AUV的纵摇鳍角和舵角。其余系数为附加质量系数。将(4)式代入(3)式右端，将其中的加速度项移动到公式左端，整理后可得到非线性运动方程组：

$ \left[\begin{array}{c} \dot{u} \\ \dot{v} \\ \dot{w} \\ \dot{p} \\ \dot{q} \\ \dot{r} \end{array}\right]=\left[\begin{array}{cccccc} m-X_{\dot u} & 0 & 0 & 0 & m z_{g} & -m y_{g} \\ 0 & m-Y_{\dot v} & 0 & -m z_{g} & 0 & m x_{g}-Y_{ \dot r} \\ 0 & 0 & m-Z_{\dot w} & m y_{g} & -m x_{g}-Z_{\dot{q}} & 0 \\ 0 & -m z_{g} & m y_{g} & I_{x x}-K_{\dot{p}} & 0 & 0 \\ m z_{g} & 0 & -m x_{g}-M_{\dot{w}} & 0 & I_{y y}-M_{\dot{q}} & 0 \\ -m y_{g} & m x_{g}-N_{\dot{v}} & 0 & 0 & 0 & I_{z z}-N_{\dot{r}} \end{array}\right]^{-1}\left[\begin{array}{c} \sum X \\ \sum Y \\ \sum Z \\ \sum K \\ \sum M \\ \sum N \end{array}\right] $

(5)

式中：∑X…∑N为除加速度项以外的其余各项。

方程组(5)结合表达式(1)、(2)便可得到AUV的六自由度非线性运动方程组。

1.3 AUV系统模型

由AUV运动方程组可知，AUV运动控制的输入为鳍角δ_s、舵角δ_r和螺旋桨推力X_prop。其中AUV的纵摇鳍角δ_s控制潜航器进行俯仰运动，舵角δ_r控制使潜航器进行偏航运动。文献[11]中实验表明，在螺旋桨转速为1 500 r/min时，REMUS可以保持1.51 m/s的速度前进。AUV系统模型图如图 2所示。AUV控制量的输入影响AUV空间受力后通过六自由度运动方程计算得到速度、角速度输出，再经过坐标转换可得到AUV的位置、姿态角输出。

2 LADRC原理

韩京清教授构建的ADRC主要由跟踪微分器(TD)、扩张状态观测器(ESO)以及非线性状态误差反馈(NLSEF)组成，其基本结构如图 3所示。TD可以跟踪输入信号v得到其过渡过程v₁和各阶微分信号^[12-13]；ESO是ADRC的核心部分，用于实时估计与在线补偿被控系统的总扰动；NLSEF根据系统状态误差e₁~e_n计算控制信号u₀。ADRC在有效抑制扰动影响的同时可以减弱对被控模型的依赖，有较强的鲁棒性。陈增强等^[14]提出的一种自抗扰广义预测控制(ADRC-GPC)算法改善了原GPC算法对被控对象精确数学模型的强依赖性，并使计算量大大减少。

如果ESO和NLSEF都采用线性函数LESO、LSEF^[9]，则可以将ADRC简化为LADRC。

2.1 线性扩张状态观测器

以二阶非线性系统为例：

$ \ddot{y}=f(\dot{y}, y, w, t)+b u $

(6)

式中：y和u为系统的输出和输入控制信号；w为系统的外部扰动；b为控制器增益且部分已知，设已知部分为b₀。则式(6)可写为：

$ \ddot{y}=f+b_{0} u $

(7)

式中：f为包括内扰和外扰的总扰动。选取状态变量x₁=y，x₂= $ \dot y $，x₃=f，则x =[y $ \dot y $ f]^T为包括扰动的扩张状态，式(7)相应转化为扩张状态空间描述^[15]：

$ \left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{\dot x}} = \mathit{\boldsymbol{Ax}} + \mathit{\boldsymbol{B}}u + \mathit{\boldsymbol{E}}\dot f}\\ {\mathit{\boldsymbol{y}} = \mathit{\boldsymbol{Cx}}} \end{array}} \right. $

(8)

其中: $ \boldsymbol{A}=\left[\begin{array}{lll} 0 & 1 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{array}\right] ; \boldsymbol{B}=\left[\begin{array}{l} 0 \\ b_{0} \\ 0 \end{array}\right] ; \boldsymbol{E}=\left[\begin{array}{c} 0 \\ 0 \\ 1 \end{array}\right] ; $ C = [1 0 0]。对应的线性扩张状态观测器为：

$ \left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{\dot z}} = \mathit{\boldsymbol{Az}} + \mathit{\boldsymbol{B}}u + \mathit{\boldsymbol{L}}(y - \mathit{\boldsymbol{Cz}})}\\ {\mathit{\boldsymbol{\hat y}} = \mathit{\boldsymbol{Cz}}} \end{array}} \right. $

(9)

式中：z → x为观测器的状态向量；L为需要设计的观测器增益矩阵。由于在本文中系统输出状态y可通过实时求解AUV运动方程组直接得到，所以本文采用降阶的LESO，删除式(9)中与状态y有关的部分，得到新的观测器方程：

$ \left\{ {\begin{array}{*{20}{l}} {\mathit{\boldsymbol{\dot z}} = {\mathit{\boldsymbol{A}}_R}\mathit{\boldsymbol{z}} + {\mathit{\boldsymbol{B}}_R}u + \mathit{\boldsymbol{L}}\left( {\dot y - {\mathit{\boldsymbol{C}}_R}\mathit{\boldsymbol{z}}} \right)}\\ {\mathit{\boldsymbol{\hat f}} = \mathit{\boldsymbol{Vz}}} \end{array}} \right. $

(10)

其中: $ \boldsymbol{A}_{R}=\left[\begin{array}{ll} 0 & 1 \\ 0 & 0 \end{array}\right] ; \boldsymbol{B}_{R}=\left[\begin{array}{l} b_{0} \\ 0 \end{array}\right] $; C_R=[1 0]; V =[0 1]。

对于n阶系统，观测器增益系数可取^[9]：[l₁ l₂ … l_n+1]=[ω₀α₁ ω₀²α₂ … ω₀ⁿ⁺¹α_n+1]，α_i= $ \frac{(n+1) !}{i !(n+1-i) !} $，于是式(10)中观测器增益矩阵为L =[2ω₀ ω₀²]，其中ω_o为观测器带宽。

2.2 线性状态误差反馈控制律

当LESO可以实时估计并补偿总扰动时，即$ \hat{f} \approx f=f(\dot{y}, y, w, t) $，系统的输入控制量为：

$ u=\frac{u_{0}-\hat{f}}{b_{0}} $

(11)

式中u₀为虚拟控制量。将式(11)代入原系统式(7)中，当b₀≈b时，原系统转变为串联积分型系统：$ \ddot y $=u₀。

高志强等^[9]将线性状态误差反馈控制律简化为PD组合的设计形式。结合本文中降阶的LESO，虚拟控制量表示为：u₀=k_p(r-y)-k_dz₂，其中r为系统给定值。对于需要设计的控制器增益k_p、k_d，经过参数化，选择k_p=ω_c²、k_d=2ω_c，其中ω_c为控制器带宽^[9]。

2.3 参数整定与简化

由以上可知，二阶系统的LADRC需要确定的控制器参数只有b₀、ω_c、ω₀ 3个。对于大部分工程对象，ω_o和ω_o常按ω_o=(3~5)ω_c的关系选择^[15]，这样LADRC方法需整定的参数在一定意义上有所减少。

3 AUV仿真与实验结果分析

进行AUV仿真与运动控制的重要前提是获得AUV的准确的水动力系数。高婷等^[16]提出的一种空间拘束运动模拟方法，经过一次算例就可以得到全部水动力系数，在保证了计算精度的同时极大地缩短了计算时间。本文的实验对象参考了文献[11]中的模型与水动力系数, 文中通过数值计算与实地实验，获得了相对准确的水动力系数。

为了验证模型的可操作性，首先进行AUV手动控制实验。以AUV深度控制为例，进行仿真实验，并考虑AUV在运动时控制量变化后六自由度之间耦合的情况，对控制器进行了优化。

3.1 手动控制实验

手动控制实验以AUV下沉实验为例，假设AUV具有1.54 m/s的初始速度。0~2 s时AUV控制输入δ_s、δ_r均为0°，在2~5 s时控制输入δ_s=8°、δ_r=0°。从图 4可以看出，在AUV下沉过程中，若方向舵没有控制量输入，AUV各自由度之间的耦合会使其产生偏航运动。

3.2 LADRC控制与优化

良好的深度控制可以帮助AUV完成一系列复杂的海底勘探等任务，因此AUV深度控制的控制效果应该在设定深度上拥有一定程度的稳定性。

图 5为LADRC、NLADRC与PID 3种控制方法的深度控制效果，且未考虑偏航控制。PID方法与LADRC方法在到达指定深度后振荡分别为-0.35~0.33 m和-0.21~0.16 m。从图中可以看出，PID方法虽然响应速度较快，但PID方法比LADRC方法超调更大，且不易稳定。NLADRC方法在到达指定深度后的振荡为-0.18~0.17 m。NLADRC方法比LADRC方法响应速度快，但快速的响应会带来明显的超调量，调整NLADRC的控制器参数需要付出大量的时间，且LADRC控制器的控制性能在优化控制器结构之后可以取得更好的控制效果，所以选择LADRC控制器进行进一步的仿真实验。

为了解决潜航器升沉和偏航运动之间的耦合问题，接下来在原LADRC深度控制器的基础上，添加PD偏航控制器，使潜航器在下沉运动过程中，实时计算并消除航向偏差。图 6(a)为添加偏航控制后的时间-深度平面图像，数值分析可以看出添加偏航控制后的LADRC深度控制器在到达指定深度后的震荡幅度从原来的-0.21~0.16 m减小为-0.05~0.02 m，但PID深度控制器的震荡幅度没有改变。图 6(b)为加入偏航控制器之后的时间-偏航平面图，双PID控制下的AUV偏航运动的最终震荡幅度为-0.01~0.01 m，而增添PD控制器的LADRC控制器的最终震荡幅度为-0.001~0.002 m。作为参考，本文研究对象REMUS自主式水下机器人的最大船体直径约为0.2 m，由以上数据和结果可知LADRC方法可以取得较好的控制性能。图 7为2种控制方法控制效果的三维图像。

4 结论

1) LADRC方法在AUV运动控制上是有效的，LESO可以很好地实时估计被控系统的总扰动。

2) 在AUV到达指定深度的稳定阶段，LADRC方法可以使AUV有更高的稳定性，当AUV需要完成高精度要求的工作时，LADRC方法有更高的适用性。

3) 在添加偏航控制器后，LADRC方法的深度控制效果得到明显改善，证明了LADRC方法在AUV六自由度运动解耦问题上的有效性和优势。

4) LADRC方法需整定的参数个数与PID相等，与NLADRC方法相比，LADRC参数整定更加便捷、更易推广和在工程上使用。

在实际应用中，AUV的运动控制问题更为复杂：不同的位置目标，控制器的参数也不尽相同。因此，未来将向着不基于模型的控制器参数自调整方面进行研究。