CN104932517A - 环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法 - Google Patents

Abstract

环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法，本发明涉及水下无人航行器航迹跟踪动态面控制优化方法。本发明的目的是为了解决现有系统不能达到动态面控制技术对被控对象精确数学模型的要求，以及该系统抵抗扰动能力低的问题。通过以下技术方案实现的：步骤一、建立UUV水平面数学模型；步骤二、在UUV水平面数学模型的基础上进行动态面控制，得到UUV航迹跟踪控制律；步骤三、在UUV航迹跟踪控制律的基础上对动态面控制进行改进，得出动态面自抗扰控制器。本发明应用于航行器领域。

Description

环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法

技术领域

本发明涉及水下无人航行器航迹跟踪动态面控制优化方法。

背景技术

无人水下航行器(UUV)近年来成为各国研究的热门话题，被应用于水下作业的各种领域。无论是海洋军事领域还是日常海下应用作业，都离不开对于UUV的应用与控制，动态面控制技术能够有效避免“微分爆炸”现象，对于一类非线性系统，在系统模型已知的前提下，设计的控制器具有明显的优越性。但是在通常情况下，系统不能达到动态面控制技术对被控对象精确数学模型的要求，以及该系统抵抗扰动能力低。

发明内容

本发明的目的是为了解决现有系统不能达到动态面控制技术对被控对象精确数学模型的要求，以及该系统抵抗扰动能力低的问题，而提出了环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法。

上述的发明目的是通过以下技术方案实现的：

步骤一、建立UUV水平面数学模型；

步骤二、在UUV水平面数学模型的基础上进行动态面控制，得到UUV航迹跟踪控制律；

步骤三、在UUV航迹跟踪控制律的基础上对动态面控制进行改进，得出动态面自抗扰控制器。

发明效果

采用本发明的环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法，将动态面和自抗扰各自的优势结合，并应用到UUV的水平面运动控制，利用自抗扰技术对被控系统进行标准化，将系统的不确定性或者扰动扩张成一维状态，对其进行观测，再利用动态面设计控制律，最后对该控制律进行相应补偿。设计了基于动态面自抗扰的UUV艏向控制器，自抗扰控制(ADRC)技术对被控系统的数学模型要求不高，只与系统的输入和输出有关。该控制技术根据系统的输入和输出可以实时估计系统的所有状态，包括扰动状态，并且具有标准自抗扰形式的系统也符合动态面控制技术的形式，将动态面控制技术和自抗扰控制技术结合，利用自抗扰控制技术实时估计系统的状态和系统的内外总扰动，在自抗扰的形式上设计动态面控制器，最后将系统内外扰动补偿到控制系统中。达到了动态面控制技术对被控对象精确数学模型的要求，增强了系统抵抗内外界扰动的能力，将抵抗扰动的能力提高30％-50％。

附图说明

图1为本发明流程图；

图2a为动态面控制下的UUV跟踪误差图，——为动态面控制下的期望轨迹与实际航行轨迹误差，横坐标仿真时间单位为秒(s)，纵坐标为米(m)；

图2b为动态面控制下的UUV轨迹跟踪图，-----为期望航迹，——为实际航迹，横坐标Y为(m)，纵坐标X为米(m)；

图3a为改进算法后的UUV跟踪误差图，——为动态面自抗扰控制下的期望轨迹与实际航行轨迹误差，为动态面控制下的期望轨迹与实际航行轨迹误差，横坐标为时间，时间单位为秒(s)，纵坐标为航迹误差，航迹误差单位为米(m)；

图3b为改进算法后的UUV轨迹跟踪图，-----为期望航迹，——为动态面自抗扰控制下的实际航迹，为动态面控制下的实际航迹，横坐标Y为(m)，纵坐标X为米(m)。

具体实施方式

具体实施方式一：结合图1说明本实施方式，环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法具体是按照以下步骤进行的：

步骤一、建立UUV水平面数学模型；

步骤二、在UUV水平面数学模型的基础上进行动态面控制，得到UUV航迹跟踪控制律；

步骤三、在UUV航迹跟踪控制律的基础上对动态面控制进行改进，得出动态面自抗扰控制器。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立UUV水平面数学模型；具体过程为：

(1)运动学方程的建立：

设UUV在大地坐标系下的重心位姿矩阵为η＝[x,y,ψ]^T，大地坐标系下的角速度矩阵为载体坐标系下角速度矩阵为V＝[u,v,r]^T；

根据大地坐标系与载体坐标系之间的转换关系，得出运动学方程：

$\left\{\begin{array}{c}\stackrel{\·}{x}=u\cos \mathrm{\ψ}-v\sin \mathrm{\ψ}\\ \stackrel{\·}{y}=u\sin \mathrm{\ψ}+v\cos \mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.$

用向量形式表示为：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\Θ}\right)V$

其中， $J\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

式中，J(Θ)为向量系数表示矩阵，η为重心位姿矩阵，x为x轴重心的位姿，y为y轴重心的位姿，ψ为UUV航行的实时艏向角，为大地坐标系下角速度矩阵，为x轴角速度，为y轴角速度，为沿艏向角方向的速度，V为载体坐标系下UUV的角速度矩阵，u为艏摇角速度，v为UUV纵向角速度，r为UUV纵摇角速度，T为矩阵转置符号；

(2)动力学方程的建立：

UUV刚体受力与力矩满足方程：

$M_{\mathrm{RB}}\stackrel{\·}{V}+C_{\mathrm{RB}}\left(V\right)V={\mathrm{\τ}}_H$

式中，M_RB为刚体的质量矩阵，V为载体坐标系下UUV的角速度矩阵，为载体坐标系下UUV的角速度矩阵的一阶导数，C_RB(V)为刚体的科氏矩阵，τ_H为总的水动力、水动力矩和执行机构所产生的力与力矩；

其中，

$M_{\mathrm{RB}}=\left[\begin{array}{ccc}m& 0& 0\\ 0& m& m{x}_G\\ 0& {\mathrm{mx}}_G& 0\end{array}\right]$

$C_{\mathrm{RB}}\left(V\right)=\left[\begin{array}{ccc}0& 0& -c_{61}\\ 0& 0& -c_{62}\\ c_{61}& c_{62}& 0\end{array}\right]$

其中的参数：

$\left\{\begin{array}{c}{c}_{61}=m(x_{G}r+v)\\ c_{62}=-\mathrm{mu}\end{array}\right.$

式中，m为UUV质量，x_G为UUV重心距x轴距离，r为UUV纵摇角速度，c_EG为刚体科氏参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9；

${\mathrm{\τ}}_H=-M_A\stackrel{\·}{V}-C_A\left(V\right)V-D\left(V\right)V-g\left(\mathrm{\Θ}\right)+\mathrm{\τ}$

式中，τ_H为总的水动力、水动力矩和执行机构所产生的力与力矩，M_A为附加质量的惯性矩阵，D(V)为水动力阻尼矩阵，C_A(V)为水动力科氏矩阵，g(Θ)为浮力重力矩阵，τ为执行机构的控制力矩阵；

其中，

$M_A=\left[\begin{array}{ccc}-X_{\stackrel{\·}{u}}& 0& 0\\ 0& -Y_{\stackrel{\·}{v}}& -Y_{\stackrel{\·}{r}}\\ 0& -N_{\stackrel{\·}{v}}& -N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(V\right)=\left[\begin{array}{ccc}0& 0& a2\\ 0& 0& -a1\\ -a2& a1& 0\end{array}\right]$

D(V)＝D_l+uD_u+D_nl(V)

$g\left(\mathrm{\Θ}\right)=\left[\begin{array}{c}0\\ (x_{G}W-x_{B}B)\\ 0\end{array}\right]$

τ＝[τ_u τ_q τ_r]^T

式中，为UUV纵向角速度在x轴下的惯性参数，为UUV横向角速度在y轴下的惯性参数，为UUV艏摇角速度在y轴下的惯性参数，为UUV横向角速度在北向轴下的惯性参数，为UUV艏摇角速度在北向轴下的惯性参数，aQ为水动力科氏参数，Q为1或2，其中，D_l为线性阻尼矩阵，D_nl(V)为非线性阻尼矩阵，D_u为随着纵向速度改变而改变的阻尼矩阵，τ为执行机构的控制力矩阵，τ_u为纵向推力，τ_q为纵倾力矩，τ_r为转艏力矩，W为重力参数，B为附加质量参数，X_B为附加距离；

其中，

$D_l=\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& -Y_r\\ 0& -N_v& N_r\end{array}\right]$

$D_u=\left[\begin{array}{ccc}0& 0& 0\\ 0& Y_{\mathrm{uv}}& -Y_{\mathrm{ur}}\\ 0& -N_{\mathrm{uv}}& N_{\mathrm{ur}}\end{array}\right]$

$D_{\mathrm{nl}}\left(V\right)=\left[\begin{array}{ccc}{X}_{\left|u\right|u}\left|u\right|& 0& 0\\ 0& Y_{\left|v\right|v}\left|v\right|& -Y_{\left|v\right|r}\left|v\right|\\ 0& -N_{\left|r\right|v}\left|r\right|& N_{\left|r\right|r}\left|r\right|\end{array}\right]$

式中，X_u为UUV纵向角速度在x轴下的线性阻尼参数，Y_v为UUV横向角速度在y轴下的线性阻尼参数，Y_r为UUV艏摇角速度在y轴下的线性阻尼参数，N_v为UUV横向角速度在北向轴下的线性阻尼参数，N_r为UUV艏摇角速度在北向轴下的线性阻尼参数，Y_uv为UUV横向角速度在y轴下的非线性阻尼参数，Y_ur为UUV艏摇角速度在y轴下的非线性阻尼参数，N_uv为UUV横向角速度在北向轴下的非线性阻尼参数，N_ur为UUV艏摇角速度在北向轴下的非线性阻尼参数，X_|u|u为UUV纵向角速度在x轴D_nl(V)矩阵内的参数，Y_|v|v为UUV横向角速度在y轴D_nl(V)矩阵内的参数，Y_|v|r为UUV艏摇角速度在y轴D_nl(V)矩阵内的参数，N_|r|v为UUV横向角速度在北向轴D_nl(V)矩阵内的参数，N_|r|r为UUV艏摇角速度在北向轴D_nl(V)矩阵内的参数，

m＝40， $N_{\stackrel{\·}{v}}=2.2,$ N_v＝36，Y_uv＝0， $X_{\stackrel{\·}{u}}=-1.42,$ $X_{u_r}=0.1,$ $X_{\left|u_r\right|u_r}=8.2,$ $X_{\mathrm{ur}}=u{X}_{\stackrel{\·}{u}},$ $Y_{\stackrel{\·}{v}}=-38.4,$ $Y_{v_r}=10,$ $Y_{\left|v_r\right|v_r}=200,$ $N_{\mathrm{uv}}=u(Y_{\stackrel{\·}{v}}-X_{\stackrel{\·}{u}}),$ $Y_{\stackrel{\·}{r}}=-2.5,$ Y_r＝5，I_z＝8.0， $N_{\left|r\right|v}=u{Y}_{\stackrel{\·}{r}},$ N_r＝5，N_|r|r＝15；

得出UUV水平面数学模型，其微分方程组的形式如下式：

$\left\{\begin{array}{c}\stackrel{\·}{u}=(-d_{11}u+{\mathrm{\τ}}_u)/m_{11}\\ \stackrel{\·}{v}=({\mathrm{Am}}_{66}-{\mathrm{Bm}}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{\·}{r}=({\mathrm{Bm}}_{22}-{\mathrm{Am}}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{\·}{x}=u\cos \left(\mathrm{\ψ}\right)-v\sin \left(\mathrm{\ψ}\right)\\ \stackrel{\·}{y}=u\sin \left(\mathrm{\ψ}\right)+v\cos \left(\mathrm{\ψ}\right)\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.$

其中，A＝-d₂₂v+(d₂₆-uc₂₆)r，

B＝(d₆₂-uc₆₂)v-d₆₆r+τ_r

而且 $\left\{\begin{array}{cc}{m}_{11}=m-X_{\stackrel{\·}{u}}& d_{11}=X_u+X_{\left|u\right|u}\left|u\right|\\ m_{22}=m=Y_{\stackrel{\·}{v}}& d_{22}=Y_v+Y_{\left|v\right|v}\left|v\right|\\ m_{26}=-Y_{\stackrel{\·}{r}}& d_{26}=Y_r\\ m_{66}=I_z-N_{\stackrel{\·}{r}}& d_{62}=N_v\\ c_{26}=m-X_{\stackrel{\·}{u}}& d_{66}=N_r+N_{\left|r\right|r}\left|r\right|\\ c_{62}=X_{\stackrel{\·}{u}}-Y_{\stackrel{\·}{v}}& \end{array}\right.$

式中，A为质量参数，B为附加质量参数，m_EG为UUV质量，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，d_EG为水动力阻尼参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，c_EG为刚体科氏参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，I_z为转动惯量。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤二中在UUV水平面数学模型的基础上进行动态面控制，得到UUV航迹跟踪控制律；具体过程为：

动态面控制算法为：

UUV航行的实时艏向角为ψ，期望艏向角为ψ_d，UUV纵摇角速度为r，

(1)定义第一个动态面S₁₁为：

S₁₁＝ψ-ψ_d

对S₁₁求导：

${\stackrel{\·}{S}}_{11}=\stackrel{\·}{\mathrm{\ψ}}-{\stackrel{\·}{\mathrm{\ψ}}}_d$

式中，为S₁₁的一阶导数，为ψ的一阶导数，为ψ_d的一阶导数；

结合UUV艏向控制模型：

${\stackrel{\·}{S}}_{11}=r-{\stackrel{\·}{\mathrm{\ψ}}}_d$

为了镇定上述系统，将r看作新的虚拟控制量，并

设计镇定函数

${\stackrel{\‾}{\mathrm{\α}}}_{12}={\stackrel{\·}{\mathrm{\ψ}}}_d-k_{11}{S}_{11}$

式中，k₁₁为待选取参数，且k₁₁>0；

具体实现形式如下：

${\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)$

式中，τ₁₂为待设计的滤波器时间常数，α₁₂为滤波器输出，为滤波器输出的导数，且有τ₁₂>0，为镇定函数；

得到：

${\stackrel{\·}{\mathrm{\α}}}_{12}=({\stackrel{\‾}{\mathrm{\α}}}_{12}-{\mathrm{\α}}_{12})/{\mathrm{\τ}}_{12}$

(2)定义第二个动态面S₁₂为：

S₁₂＝r-α₁₂

对S₁₂求导：

${\stackrel{\·}{S}}_{12}=\stackrel{\·}{r}-{\stackrel{\·}{\mathrm{\α}}}_{12}$

选取为控制1变量，则有：

$\stackrel{\·}{r}={\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}$

式中，k₁₂为待设计的控制器参数；

结合UUV艏向控制模型，得出UUV航迹跟踪控制律为：

${\mathrm{\τ}}_s=\frac{1}{m_{22}}({\mathrm{Am}}_{26}-{\mathrm{Cm}}_{22}+(m_{22}{m}_{66}-m_{26}^2)({\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}))$

式中，A＝-d₂₂v_r+(d₂₆-u_rc₂₆-mu_c)r，C＝(d₆₂-u_rc₆₂)v_r-d₆₆r，C为质量化简参数，v_r为横向速度，u_r为纵向速度，u_c为相对纵向速度，k₁₂为待设计的控制器参数，且有k₁₂>0，τ_s为UUV航迹跟踪控制律。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一、二或三不同的是：所述步骤三中在UUV航迹跟踪控制律的基础上对动态面控制进行改进，得出动态面自抗扰控制器；具体过程为：

(1)制备跟踪微分器：

设期望艏向值为v_ψ，过渡值为v₁₁，得到微分值v₁₂，

跟踪微分器离散形式为：

$\left\{\begin{array}{c}\mathrm{fh}=\mathrm{fhan}(v_{11}\left(k\right)-v_{\mathrm{\ψ}}\left(k\right),v_{12}\left(k\right),r_1,h_1)\\ v_{11}(k+1)=v_{11}\left(k\right)+h_1\·v_{12}\left(k\right)\\ v_{12}(k+1)=v_{12}\left(k\right)+h_1\·\mathrm{fh}\end{array}\right.$

式中，r₁为调节因子(r₁的大小决定期望跟踪的快慢)，h₁为仿真步长，fhan为最速控制综合函数，fh为综合函数，k为计数参数，v_ψ为期望艏向值；

(2)制备扩张观测器：

针对转换后的UUV艏向控制模型，该子系统为二阶非线性系统，故设计一个三阶扩张状态观测器即可实现系统所有状态，包括扰动状态的观测。

对于UUV艏向控制模型，u₁＝τ_r，y₁＝ψ，即输入u₁和输出y₁分别为转艏力矩τ_r和UUV航行的实时艏向角ψ，z₁₁为观测器对艏向角的估计值，z₁₂为转艏角速度的估计值，z₁₃为作用于UUV艏向控制模型总扰动的估计值；

扩张观测器离散算法为：

$\left\{\begin{array}{c}{e}_{12}\left(k\right)=z_{11}\left(k\right)-y_1\left(k\right)\\ \mathrm{fe}=\mathrm{fal}(e_{12}\left(k\right),0.5,{\mathrm{\δ}}_1)\\ {\mathrm{fe}}_1=\mathrm{fal}(e_{12}\left(k\right),0.25,{\mathrm{\δ}}_1)\\ z_{11}(k+1)=z_{11}\left(k\right)+h_1(z_{12}\left(k\right)-{\mathrm{\β}}_{11}e\left(k\right))\\ z_{12}(k+1)=z_{12}\left(k\right)+h_1(z_{13}\left(k\right)-{\mathrm{\β}}_{12}\mathrm{fe}+b_{10}{u}_1\left(k\right))\\ z_{13}(k+1)=z_{13}\left(k\right)+h_1(-{\mathrm{\β}}_{13}{\mathrm{fe}}_1)\end{array}\right.$

式中，e₁₂(k)为艏向时刻误差；z₁₁(k)为观测器对艏向角的估计值的离散形式当前值，y₁(k)为艏向输出的离散形式，δ₁为浮动变量，fal(e₁₂(k),0.5,δ₁)为大小为0.5的具有线性段的连续的幂次函数，fal(e₁₂(k),0.25,δ₁)为大小为0.25的具有线性段的连续的幂次函数，fe为fal(e₁₂(k),0.5,δ₁)的简化函数，fe₁为fal(e₁₂(k),0.25,δ₁)的简化函数，函数fal(e,α,δ)为原点附近具有线性段的连续的幂次函数，z₁₁(k+1)为观测器对艏向角的估计值的离散形式采样后一时刻值，h₁为步长，β₁₁为扩张观测器的第一个待定参数，e(k)为估计误差的离散形式，z₁₂(k+1)为转艏角速度的估计值的离散形式采样后一时刻值，z₁₂(k)为转艏角速度的估计值的离散形式当前值，h₁(z₁₃(k)-β₁₂fe+b₁₀u₁(k))为转艏角速度离散表示式，z₁₃(k+1)为作用于UUV艏向控制模型总扰动的估计值的离散形式采样后一时刻值，z₁₃(k)为作用于UUV艏向控制模型总扰动的估计值的离散形式当前值，h₁(-β₁₃fe₁)为总扰动的的表示式；β₁₂为扩张观测器的第二个待定参数，b₁₀为补偿参数，u₁(k)为转艏力矩的离散形式，β₁₃为扩张观测器的第三个待定参数；

(3)制备动态面控制器

UUV的状态为x_1i，由跟踪微分器得出期望状态v_1i，i取值范围为1、2、3、4、5、6、7、8或9；

(a)定义第一个动态面S₁₁为：

S₁₁＝x₁₁-v₁₁

式中，x₁₁为第一动态面虚拟控制变量；

对S₁₁求导：

${\stackrel{\·}{S}}_{11}=x_{12}-v_{12}$

将实际控制量x₁₂作为虚拟控制变量，并选取镇定函数

${\stackrel{\‾}{\mathrm{\α}}}_{12}=v_{12}-k_{11}{s}_{11}$

式中，k₁₁为待选取参数，且满足k₁₁>0，

结合动态面，引入一阶低通滤波器：

${\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)$

其中，τ₁₂为待设计的滤波器时间常数，且

(b)定义第二个动态面S₁₂为：

S₁₂＝x₁₂-α₁₂

对S₁₂求导：

${\stackrel{\·}{S}}_{12}={\stackrel{\·}{x}}_{12}-{\stackrel{\·}{\mathrm{\α}}}_{12}$

为了镇定该系统，选取实际控制量x'₁₂，且实际控制量x₁₂满足：

${\stackrel{\·}{x}}_{12}^{\′}={\stackrel{\·}{\mathrm{\α}}}_{11}-k_{12}{S}_{12}$

即：

$u_{10}={\stackrel{\·}{\mathrm{\α}}}_{11}-k_{12}{S}_{12}$

式中，为镇定值导数，k₁₂为待设计控制器的参数，且有k₁₂>0，u₁₀为控制2变量；

(4)扰动补偿

将动态面得出的控制2变量u₁₀对UUV艏向控制模型总扰动的估计值z₁₃进行补偿，得出控制3变量u₁，表达式如下：

u₁＝u₁₀-z₁₃/b₁₀或者u₁＝(u₁₀-z₁₃)/b₁₀

式中，b₁₀为补偿参数；

至此，得出动态面自抗扰控制器的具体形式如下：

$\left\{\begin{array}{c}{e}_{11}=v_{11}-v_{\mathrm{\ψ}}\\ \mathrm{fh}=\mathrm{fhan}(e_{11},v_{12},r_1,h_1)\\ v_{11}=v_{11}+h_1{v}_{12}\\ v_{12}=v_{12}+h_1\·\mathrm{fh}\\ e_{12}=z_{11}-y_1,\mathrm{fe}=\mathrm{fal}(e_{12},0.5,{\mathrm{\δ}}_1),{\mathrm{fe}}_1=\mathrm{fal}(e_{12},0.25,{\mathrm{\δ}}_1)\\ z_{11}=z_{11}+h_1(z_{12}-{\mathrm{\β}}_{11}{e}_{12})\\ z_{12}=z_{12}+h_1(z_{13}-{\mathrm{\β}}_{12}\·\mathrm{fe}+b_{10}{u}_1)\\ z_{13}=z_{13}+h_1(-{\mathrm{\β}}_{13}\·{\mathrm{fe}}_1)\\ S_{11}=x_{11}-v_{11}\\ {\stackrel{\‾}{\mathrm{\α}}}_{12}=v_{12}-k_{11}{S}_{11},{\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)\\ S_{12}=x_{12}-{\mathrm{\α}}_{12}\\ u_{10}={\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}\\ u_1=(u_{10}-z_{13})/b_{10}\end{array}\right.$

式中，e₁₁为观测器对艏向角误差，fhan为连续的幂次函数，fh为综合函数，e₁₂为艏向时刻误差，y₁为实时艏向输出，fe为fal(e₁₂,0.5,δ₁)的简化函数，fal(e₁₂,0.5,δ₁)为大小为0.5的具有线性段的连续的幂次函数，fe₁为fal(e₁₂(k),0.25,δ₁)的简化函数，fal(e₁₂,0.25,δ₁)为大小为0.25的具有线性段的连续的幂次函数，h₁(z₁₂-β₁₁e₁₂)为观测器对艏向角的离散表示式，h₁(z₁₃-β₁₂·fe+b₁₀u₁)为转艏角速度离散表示式，h₁(-β₁₃·fe₁)为总扰动的的表示式，k₁₁为待选取参数，k₁₂为待设计控制器的参数，u₁为控制3变量，u₁₀为控制2变量。

其它步骤及参数与具体实施方式一、二或三相同。

采用以下实施例验证本发明的有益效果：

实施例1

环境干扰下的水下无人航行器航迹跟踪控制优化方法具体是按照以下步骤进行的：

步骤一、建立UUV水平面数学模型；

设UUV在大地坐标系下的重心位姿矩阵为η＝[x,y,ψ]^T，大地坐标系下的角速度矩阵为载体坐标系下角速度矩阵为V＝[u,v,r]^T；

根据大地坐标系与载体坐标系之间的转换关系，得出运动学方程：

$\left\{\begin{array}{c}\stackrel{\·}{x}=u\cos \mathrm{\ψ}-v\sin \mathrm{\ψ}\\ \stackrel{\·}{y}=u\sin \mathrm{\ψ}+v\cos \mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.$

用向量形式表示为：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\Θ}\right)V$

其中， $J\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

式中，J(Θ)为向量系数表示矩阵，η为重心位姿矩阵，x为x轴重心的位姿，y为y轴重心的位姿，ψ为UUV航行的实时艏向，为大地坐标系下角速度矩阵，为x轴角速度，为y轴角速度，为ψ轴角速度，V为载体坐标系下UUV的角速度矩阵，u为艏摇角速度，v为UUV纵向角速度，r为UUV纵摇角速度，T为矩阵转置符号；

(3)动力学方程的建立：

UUV刚体受力与力矩满足方程：

$M_{\mathrm{RB}}\stackrel{\·}{V}+C_{\mathrm{RB}}\left(V\right)V={\mathrm{\τ}}_H$

其中，M_RB为刚体的质量矩阵，V为载体坐标系下UUV的角速度矩阵，为载体坐标系下UUV的角速度矩阵的一阶导数，C_RB(V)为刚体的科氏矩阵，τ_H为总的水动力、水动力矩和执行机构所产生的力与力矩；

$M_{\mathrm{RB}}=\left[\begin{array}{ccc}m& 0& 0\\ 0& m& m{x}_G\\ 0& {\mathrm{mx}}_G& 0\end{array}\right]$

$C_{\mathrm{RB}}\left(V\right)=\left[\begin{array}{ccc}0& 0& -c_{61}\\ 0& 0& -c_{62}\\ c_{61}& c_{62}& 0\end{array}\right]$

其中的参数：

$\left\{\begin{array}{c}{c}_{61}=m(x_{G}r+v)\\ c_{62}=-\mathrm{mu}\end{array}\right.$

式中，m为UUV质量，x_G为UUV重心距x轴距离，r为UUV纵摇角速度，c_EG为刚体科氏参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9；

${\mathrm{\τ}}_H=-M_A\stackrel{\·}{V}-C_A\left(V\right)V-D\left(V\right)V-g\left(\mathrm{\Θ}\right)+\mathrm{\τ}$

其中，τ_H为总的水动力、水动力矩和执行机构所产生的力与力矩，M_A为附加质量的惯性矩阵，D(V)为水动力阻尼矩阵，C_A(V)为水动力科氏矩阵，g(Θ)为浮力重力矩阵，τ为执行机构的控制力矩阵；

其中，

$M_A=\left[\begin{array}{ccc}-X_{\stackrel{\·}{u}}& 0& 0\\ 0& -Y_{\stackrel{\·}{v}}& -Y_{\stackrel{\·}{r}}\\ 0& -N_{\stackrel{\·}{v}}& -N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(V\right)=\left[\begin{array}{ccc}0& 0& a2\\ 0& 0& -a1\\ -a2& a1& 0\end{array}\right]$

D(V)＝D_l+uD_u+D_nl(V)

$g\left(\mathrm{\Θ}\right)=\left[\begin{array}{c}0\\ (x_{G}W-x_{B}B)\\ 0\end{array}\right]$

τ＝[τ_u τ_q τ_r]^T

式中，为UUV纵向角速度在x轴下的惯性参数，为UUV横向角速度在y轴下的惯性参数，为UUV艏摇角速度在y轴下的惯性参数，为UUV横向角速度在北向轴下的惯性参数，为UUV艏摇角速度在北向轴下的惯性参数，aQ为水动力科氏参数，Q为1或2，其中，D_l为线性阻尼矩阵，D_nl(V)为非线性阻尼矩阵，D_u为随着纵向速度改变而改变的阻尼矩阵，τ为执行机构的控制力矩阵，τ_u为纵向推力，τ_q为纵倾力矩，τ_r为转艏力矩；

其中，

$D_l=\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& -Y_r\\ 0& -N_v& N_r\end{array}\right]$

$D_u=\left[\begin{array}{ccc}0& 0& 0\\ 0& Y_{\mathrm{uv}}& -Y_{\mathrm{ur}}\\ 0& -N_{\mathrm{uv}}& N_{\mathrm{ur}}\end{array}\right]$

$D_{\mathrm{nl}}\left(V\right)=\left[\begin{array}{ccc}{X}_{\left|u\right|u}\left|u\right|& 0& 0\\ 0& Y_{\left|v\right|v}\left|v\right|& -Y_{\left|v\right|r}\left|v\right|\\ 0& -N_{\left|r\right|v}\left|r\right|& N_{\left|r\right|r}\left|r\right|\end{array}\right]$

式中，X_u为UUV纵向角速度在x轴下的线性阻尼参数，Y_v为UUV横向角速度在y轴下的线性阻尼参数，Y_r为UUV艏摇角速度在y轴下的线性阻尼参数，N_v为UUV横向角速度在北向轴下的线性阻尼参数，N_r为UUV艏摇角速度在北向轴下的线性阻尼参数，Y_uv为UUV横向角速度在y轴下的非线性阻尼参数，Y_ur为UUV艏摇角速度在y轴下的非线性阻尼参数，N_uv为UUV横向角速度在北向轴下的非线性阻尼参数，N_ur为UUV艏摇角速度在北向轴下的非线性阻尼参数，X_|u|u为UUV纵向角速度在x轴D_nl(V)矩阵内的参数，Y_|v|v为UUV横向角速度在y轴D_nl(V)矩阵内的参数，Y_|v|r为UUV艏摇角速度在y轴D_nl(V)矩阵内的参数，N_|r|v为UUV横向角速度在北向轴D_nl(V)矩阵内的参数，N_|r|r为UUV艏摇角速度在北向轴D_nl(V)矩阵内的参数，

m＝40， $N_{\stackrel{\·}{v}}=2.2,$ N_v＝36，Y_uv＝0， $X_{\stackrel{\·}{u}}=-1.42,$ $X_{u_r}=0.1,$ $X_{\left|u_r\right|u_r}=8.2,$ $Y_{\mathrm{ur}}=u{X}_{\stackrel{\·}{u}},$ $Y_{\stackrel{\·}{v}}=-38.4,$ $Y_{v_r}=10,$ $Y_{\left|v_r\right|v_r}=200,$ $N_{\mathrm{uv}}=u(Y_{\stackrel{\·}{v}}-X_{\stackrel{\·}{u}}),$ $Y_{\stackrel{\·}{r}}=-2.5,$ Y_r＝5I_z＝8.0， $N_{\left|r\right|v}=u{Y}_{\stackrel{\·}{r}},$ N_r＝5，N_|r|r＝15；

得出UUV水平面数学模型，其微分方程组的形式如下式：

$\left\{\begin{array}{c}\stackrel{\·}{u}=(-d_{11}u+{\mathrm{\τ}}_u)/m_{11}\\ \stackrel{\·}{v}=({\mathrm{Am}}_{66}-{\mathrm{Bm}}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{\·}{r}=({\mathrm{Bm}}_{22}-{\mathrm{Am}}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{\·}{x}=u\cos \left(\mathrm{\ψ}\right)-v\sin \left(\mathrm{\ψ}\right)\\ \stackrel{\·}{y}=u\sin \left(\mathrm{\ψ}\right)+v\cos \left(\mathrm{\ψ}\right)\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.$

其中，A＝-d₂₂v+(d₂₆-uc₂₆)r，

B＝(d₆₂-uc₆₂)v-d₆₆r+τ_r

而且 $\left\{\begin{array}{cc}{m}_{11}=m-X_{\stackrel{\·}{u}}& d_{11}=X_u+X_{\left|u\right|u}\left|u\right|\\ m_{22}=m=Y_{\stackrel{\·}{v}}& d_{22}=Y_v+Y_{\left|v\right|v}\left|v\right|\\ m_{26}=-Y_{\stackrel{\·}{r}}& d_{26}=Y_r\\ m_{66}=I_z-N_{\stackrel{\·}{r}}& d_{62}=N_v\\ c_{26}=m-X_{\stackrel{\·}{u}}& d_{66}=N_r+N_{\left|r\right|r}\left|r\right|\\ c_{62}=X_{\stackrel{\·}{u}}-Y_{\stackrel{\·}{v}}& \end{array}\right.$

式中，A为质量参数，B为附加质量参数，m_EG为UUV质量，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，d_EG为水动力阻尼参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，c_EG为刚体科氏参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，I_z为转动惯量；

步骤二、在UUV水平面数学模型的基础上进行动态面控制，实现UUV航迹跟踪控制；

动态面控制算法：

UUV艏向的动态面控制规律为： ${\mathrm{\τ}}_s=\frac{1}{m_{22}}({\mathrm{Am}}_{26}-{\mathrm{Cm}}_{22}+(m_{22}{m}_{66}-m_{26}^2)({\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}))$

式中，A＝-d₂₂v_r+(d₂₆-u_rc₂₆-mu_c)r，C＝(d₆₂-u_rc₆₂)v_r-d₆₆r，具体参数的含义已经在UUV水平面模型中给出，k₁₁，τ₁₂，k₁₂，k₂₁均为控制参数，是人为给定的量，仿真实例中：k₁₁＝0.5、τ₁₂＝0.4、k₁₂＝1、k₂₁＝2

步骤三、在UUV航迹跟踪控制的基础上对动态面控制进行改进，在自抗扰的形式上设计动态面控制器。具体是按照以下步骤进行的：

(1)跟踪微分器设计

(2)扩张观测器设计

(3)动态面控制器设计

(4)扰动补偿

得到最终控制器的具体形式如下：

$\left\{\begin{array}{c}{e}_{11}=v_{11}-v_{\mathrm{\ψ}}\\ \mathrm{fh}=\mathrm{fhan}(e_{11},v_{12},r_1,h_1)\\ v_{11}=v_{11}+h_1{v}_{12}\\ v_{12}=v_{12}+h_1\·\mathrm{fh}\\ e_{12}=z_{11}-y_1,\mathrm{fe}=\mathrm{fal}(e_{12},0.5,{\mathrm{\δ}}_1),{\mathrm{fe}}_1=\mathrm{fal}(e_{12},0.25,{\mathrm{\δ}}_1)\\ z_{11}=z_{11}+h_1(z_{12}-{\mathrm{\β}}_{11}{e}_{12})\\ z_{12}=z_{12}+h_1(z_{13}-{\mathrm{\β}}_{12}\·\mathrm{fe}+b_{10}{u}_1)\\ z_{13}=z_{13}+h_1(-{\mathrm{\β}}_{13}\·{\mathrm{fe}}_1)\\ S_{11}=x_{11}-v_{11}\\ {\stackrel{\‾}{\mathrm{\α}}}_{12}=v_{12}-k_{11}{S}_{11},{\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)\\ S_{12}=x_{12}-{\mathrm{\α}}_{12}\\ u_{10}={\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}\\ u_1=(u_{10}-z_{13})/b_{10}\end{array}\right.$

仿真实验中，所设计的动态面控制器参数为k₁₁＝4、k₁₂＝1、τ₁₁＝0.4、k₂₁＝1；动态面自抗扰控制器的相关参数为r₁＝r₂＝0.8、h₁＝h₂＝0.02、δ₁＝δ₂＝0.1、β₁₁＝100、β₁₂＝300、β₁₃＝1000、β₂₁＝100、β₂₂＝1000、b₁₀＝0.06、b₂₀＝0.025、k₁₁＝1.2、k₁₂＝1、τ₁₂＝0.1、k₂₁＝4；

得到图2a、图2b、图3a和图3b的仿真结果，从仿真结果看出两种控制方式均使UUV成功跟踪上一条我所设定的期望轨迹，同时改进后的动态面自抗扰控制提高了UUV抵抗海流干扰的能力，在有海流干扰的情况下，观察图3a,虚线为动态面控制下的航行误差，实线为动态面自抗扰的航迹误差，明显发现动态面自抗扰控制下UUV的航迹误差数值和波动更小，UUV的航行更加稳定，直观跟踪效果可从图3b中看出。所以通过仿真结果表明改进后的动态面自抗扰控制提高了UUV的抗干扰能力。

Claims (4)

1.环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法，其特征在于，环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法具体是按照以下步骤进行的：

步骤一、建立UUV水平面数学模型；

步骤二、在UUV水平面数学模型的基础上进行动态面控制，得到UUV航迹跟踪控制律；

步骤三、在UUV航迹跟踪控制律的基础上对动态面控制进行改进，得出动态面自抗扰控制器。

2.根据权利要求1所述环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法，其特征在于，所述步骤一中建立UUV水平面数学模型；具体过程为：

(1)运动学方程的建立：

设UUV在大地坐标系下的重心位姿矩阵为η＝[x,y,ψ]^T，大地坐标系下的角速度矩阵为载体坐标系下角速度矩阵为V＝[u,v,r]^T；

根据大地坐标系与载体坐标系之间的转换关系，得出运动学方程：

$\left\{\begin{array}{c}\stackrel{\·}{x}=u\cos \mathrm{\ψ}-v\sin \mathrm{\ψ}\\ \stackrel{\·}{y}=u\sin \mathrm{\ψ}+v\cos \mathrm{\ψ}\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.$

用向量形式表示为：

$\stackrel{\·}{\mathrm{\η}}=J\left(\mathrm{\Θ}\right)V$

其中， $J\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ \sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

式中，J(Θ)为向量系数表示矩阵，η为重心位姿矩阵，x为x轴重心的位姿，y为y轴重心的位姿，ψ为UUV航行的实时艏向角，为大地坐标系下角速度矩阵，为x轴角速度，为y轴角速度，为沿艏向角方向的速度，V为载体坐标系下UUV的角速度矩阵，u为艏摇角速度，v为UUV纵向角速度，r为UUV纵摇角速度，T为矩阵转置符号；

(2)动力学方程的建立：

UUV刚体受力与力矩满足方程：

$M_{\mathrm{RB}}\stackrel{\·}{V}+C_{\mathrm{RB}}\left(V\right)V={\mathrm{\τ}}_H$

式中，M_RB为刚体的质量矩阵，V为载体坐标系下UUV的角速度矩阵，为载体坐标系下UUV的角速度矩阵的一阶导数，C_RB(V)为刚体的科氏矩阵，τ_H为总的水动力、水动力矩和执行机构所产生的力与力矩；

其中，

$M_{\mathrm{RB}}=\left[\begin{array}{ccc}m& 0& 0\\ 0& m& {\mathrm{mx}}_G\\ 0& {\mathrm{mx}}_G& 0\end{array}\right]$

$C_{\mathrm{RB}}\left(V\right)=\left[\begin{array}{ccc}0& 0& -c_{61}\\ 0& 0& -c_{62}\\ c_{61}& c_{62}& 0\end{array}\right]$

其中的参数：

$\left\{\begin{array}{c}{c}_{61}=m(x_{G}r+v)\\ c_{62}=-\mathrm{mu}\end{array}\right.$

式中，m为UUV质量，x_G为UUV重心距x轴距离，r为UUV纵摇角速度，c_EG为刚体科氏参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9；

${\mathrm{\τ}}_H=-M_A\stackrel{\·}{V}-C_A\left(V\right)V-D\left(V\right)V-g\left(\mathrm{\Θ}\right)+\mathrm{\τ}$

式中，τ_H为总的水动力、水动力矩和执行机构所产生的力与力矩，M_A为附加质量的惯性矩阵，D(V)为水动力阻尼矩阵，C_A(V)为水动力科氏矩阵，g(Θ)为浮力重力矩阵，τ为执行机构的控制力矩阵；

其中，

$M_A=\left[\begin{array}{ccc}-X_{\stackrel{\·}{u}}& 0& 0\\ 0& -Y_{\stackrel{\·}{v}}& -Y_{\stackrel{\·}{r}}\\ 0& -N_{\stackrel{\·}{v}}& -N_{\stackrel{\·}{r}}\end{array}\right]$

$C_A\left(V\right)=\left[\begin{array}{ccc}0& 0& a2\\ 0& 0& -a1\\ -a2& a1& 0\end{array}\right]$

D(V)＝D_l+uD_u+D_nl(V)

$g\left(\mathrm{\Θ}\right)=\left[\begin{array}{c}0\\ (x_{G}W-x_{B}B)\\ 0\end{array}\right]$

τ＝[τ_u τ_q τ_r]^T

式中，为UUV纵向角速度在x轴下的惯性参数，为UUV横向角速度在y轴下的惯性参数，为UUV艏摇角速度在y轴下的惯性参数，为UUV横向角速度在北向轴下的惯性参数，为UUV艏摇角速度在北向轴下的惯性参数，aQ为水动力科氏参数，Q为1或2，其中，D_l为线性阻尼矩阵，D_nl(V)为非线性阻尼矩阵，D_u为随着纵向速度改变而改变的阻尼矩阵，τ为执行机构的控制力矩阵，τ_u为纵向推力，τ_q为纵倾力矩，τ_r为转艏力矩，W为重力参数，B为附加质量参数，X_B为附加距离；

其中，

$D_l=\left[\begin{array}{ccc}{X}_u& 0& 0\\ 0& Y_v& -Y_r\\ 0& -N_v& N_r\end{array}\right]$

$D_u=\left[\begin{array}{ccc}0& 0& 0\\ 0& Y_{\mathrm{uv}}& -Y_{\mathrm{ur}}\\ 0& -N_{\mathrm{uv}}& N_{\mathrm{ur}}\end{array}\right]$

$D_{\mathrm{nl}}\left(V\right)=\left[\begin{array}{ccc}{X}_{\left|u\right|u}\left|u\right|& 0& 0\\ 0& Y_{\left|v\right|v}\left|v\right|& -Y_{\left|v\right|r}\left|v\right|\\ 0& -N_{\left|r\right|v}\left|r\right|& N_{\left|r\right|r}\left|r\right|\end{array}\right]$

式中，X_u为UUV纵向角速度在x轴下的线性阻尼参数，Y_v为UUV横向角速度在y轴下的线性阻尼参数，Y_r为UUV艏摇角速度在y轴下的线性阻尼参数，N_v为UUV横向角速度在北向轴下的线性阻尼参数，N_r为UUV艏摇角速度在北向轴下的线性阻尼参数，Y_uv为UUV横向角速度在y轴下的非线性阻尼参数，Y_ur为UUV艏摇角速度在y轴下的非线性阻尼参数，N_uv为UUV横向角速度在北向轴下的非线性阻尼参数，N_ur为UUV艏摇角速度在北向轴下的非线性阻尼参数，X_|u|u为UUV纵向角速度在x轴D_nl(V)矩阵内的参数，Y_|v|v为UUV横向角速度在y轴D_nl(V)矩阵内的参数，Y_|v|r为UUV艏摇角速度在y轴D_nl(V)矩阵内的参数，N_|r|v为UUV横向角速度在北向轴D_nl(V)矩阵内的参数，N_|r|r为UUV艏摇角速度在北向轴D_nl(V)矩阵内的参数，

m＝40， $N_{\stackrel{\·}{v}}=2.2,$ N_v＝36，Y_uv＝0， $X_{\stackrel{\·}{u}}=-1.42,X_{u_r}=0.1,X_{\left|u_r\right|u_r}=8.2,Y_{\mathrm{ur}}=u{X}_{\stackrel{\·}{u}},$ $Y_{\stackrel{\·}{v}}=-38.4,Y_{v_r}=10,Y_{\left|v_r\right|v_r}=200,N_{\mathrm{uv}}=u(Y_{\stackrel{\·}{v}}-X_{\stackrel{\·}{u}}),Y_{\stackrel{\·}{r}}=-2.5,$ Y_r＝5，I_z＝8.0， $N_{\left|r\right|v}=u{Y}_{\stackrel{\·}{r}},$ N_r＝5， $N_{\stackrel{\·}{r}}=-8.9,N_{\left|r\right|r}=15;$

得出UUV水平面数学模型，其微分方程组的形式如下式：

$\left\{\begin{array}{c}\stackrel{\·}{u}=(-d_{11}u+{\mathrm{\τ}}_u)/m_{11}\\ \stackrel{\·}{v}=({\mathrm{Am}}_{66}-{\mathrm{Bm}}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{\·}{r}=({\mathrm{Bm}}_{22}-{\mathrm{Am}}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{\·}{x}=u\cos \left(\mathrm{\ψ}\right)-v\sin \left(\mathrm{\ψ}\right)\\ \stackrel{\·}{y}=u\sin \left(\mathrm{\ψ}\right)+v\cos \left(\mathrm{\ψ}\right)\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.$

其中，A＝-d₂₂v+(d₂₆-uc₂₆)r，

B＝(d₆₂-uc₆₂)v-d₆₆r+τ_r

而且 $\left\{\begin{array}{cc}{m}_{11}=m-X_{\stackrel{\·}{u}}& d_{11}=X_u+X_{\left|u\right|u}\left|u\right|\\ m_{22}=m-Y_{\stackrel{\·}{v}}& d_{22}=Y_v+Y_{\left|v\right|v}\left|v\right|\\ m_{26}=-Y_{\stackrel{\·}{r}}& d_{26}=Y_r\\ m_{66}=I_z-N_{\stackrel{\·}{r}}& d_{62}=N_v\\ c_{26}=m-X_{\stackrel{\·}{u}}& d_{66}=N_r+N_{\left|r\right|r}\left|r\right|\\ c_{62}=X_{\stackrel{\·}{u}}-Y_{\stackrel{\·}{v}}& \end{array}\right.$

式中，A为质量参数，B为附加质量参数，m_EG为UUV质量，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，d_EG为水动力阻尼参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，c_EG为刚体科氏参数，E为1、2、3、4、5、6、7、8或9，G为1、2、3、4、5、6、7、8或9，I_z为转动惯量。

3.根据权利要求2所述环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法，其特征在于，所述步骤二中在UUV水平面数学模型的基础上进行动态面控制，得到UUV航迹跟踪控制律；具体过程为：

动态面控制算法为：

UUV航行的实时艏向角为ψ，期望艏向角为ψ_d，UUV纵摇角速度为r，

(1)定义第一个动态面S₁₁为：

S₁₁＝ψ-ψ_d

对S₁₁求导：

${\stackrel{\·}{S}}_{11}=\stackrel{\·}{\mathrm{\ψ}}-{\stackrel{\·}{\mathrm{\ψ}}}_d$

式中，为S₁₁的一阶导数，为ψ的一阶导数，为ψ_d的一阶导数；

结合UUV艏向控制模型：

${\stackrel{\·}{S}}_{11}=r-{\stackrel{\·}{\mathrm{\ψ}}}_d$

设计镇定函数

${\stackrel{\‾}{\mathrm{\α}}}_{12}={\stackrel{\·}{\mathrm{\ψ}}}_d-k_{11}{S}_{11}$

式中，k₁₁为待选取参数，且k₁₁>0；

具体实现形式如下：

${\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)$

式中，τ₁₂为待设计的滤波器时间常数，α₁₂为滤波器输出，为滤波器输出的导数，且有τ₁₂>0，为镇定函数；

得到：

${\stackrel{\·}{\mathrm{\α}}}_{12}=({\stackrel{\‾}{\mathrm{\α}}}_{12}-{\mathrm{\α}}_{12})/{\mathrm{\τ}}_{12}$

(2)定义第二个动态面S₁₂为：

S₁₂＝r-α₁₂

对S₁₂求导：

${\stackrel{\·}{S}}_{12}=\stackrel{\·}{r}-{\stackrel{\·}{\mathrm{\α}}}_{12}$

选取为控制1变量，则有：

$\stackrel{\·}{r}={\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}$

式中，k₁₂为待设计的控制器参数；

结合UUV艏向控制模型，得出UUV航迹跟踪控制律为：

${\mathrm{\τ}}_s=\frac{1}{m_{22}}({\mathrm{Am}}_{26}-{\mathrm{Cm}}_{22}+(m_{22}{m}_{66}-m_{26}^2)({\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}))$

式中，A＝-d₂₂v_r+(d₂₆-u_rc₂₆-mu_c)r，C＝(d₆₂-u_rc₆₂)v_r-d₆₆r，C为质量化简参数，v_r为横向速度，u_r为纵向速度，u_c为相对纵向速度，k₁₂为待设计的控制器参数，且有k₁₂>0，τ_s为UUV航迹跟踪控制律。

4.根据权利要求3所述环境干扰下的水下无人航行器航迹跟踪动态面控制优化方法，其特征在于，所述步骤三中在UUV航迹跟踪控制律的基础上对动态面控制进行改进，得出动态面自抗扰控制器；具体过程为：

(1)制备跟踪微分器：

设期望艏向值为v_ψ，过渡值为v₁₁，得到微分值v₁₂，

跟踪微分器离散形式为：

$\left\{\begin{array}{c}\mathrm{fh}=\mathrm{fhan}(v_{11}\left(k\right)-v_{\mathrm{\ψ}}\left(k\right),v_{12}\left(k\right),r_1,h_1)\\ v_{11}(k+1)=v_{11}\left(k\right)+h_1\·v_{12}\left(k\right)\\ v_{12}(k+1)=v_{12}\left(k\right)+h_1\·\mathrm{fh}\end{array}\right.$

式中，r₁为调节因子，h₁为仿真步长，fhan为最速控制综合函数，fh为综合函数，k为计数参数，v_ψ为期望艏向值；

(2)制备扩张观测器：

对于UUV艏向控制模型，u₁＝τ_r，y₁＝ψ，即输入u₁和输出y₁分别为转艏力矩τ_r和UUV航行的实时艏向角ψ，z₁₁为观测器对艏向角的估计值，z₁₂为转艏角速度的估计值，z₁₃为作用于UUV艏向控制模型总扰动的估计值；

扩张观测器离散算法为：

$\left\{\begin{array}{c}{e}_{12}\left(k\right)=z_{11}\left(k\right)-y_1\left(k\right)\\ \mathrm{fe}=\mathrm{fal}(e_{12}\left(k\right),0.5,{\mathrm{\δ}}_1)\\ {\mathrm{fe}}_1=\mathrm{fal}(e_{12}\left(k\right),0.25,{\mathrm{\δ}}_1)\\ z_{11}(k+1)=z_{11}\left(k\right)+h_1(z_{12}\left(k\right)-{\mathrm{\β}}_{11}e\left(k\right))\\ z_{12}(k+1)=z_{12}\left(k\right)+h_1(z_{13}\left(k\right)-{\mathrm{\β}}_{12}\mathrm{fe}+b_{10}{u}_1\left(k\right))\\ z_{13}(k+1)=z_{13}\left(k\right)+h_1(-{\mathrm{\β}}_{13}{\mathrm{fe}}_1)\end{array}\right.$

式中，e₁₂(k)为艏向时刻误差；z₁₁(k)为观测器对艏向角的估计值的离散形式当前值，y₁(k)为艏向输出的离散形式，δ₁为浮动变量，fal(e₁₂(k),0.5,δ₁)为大小为0_.5的具有线性段的连续的幂次函数，fal(e₁₂(k),0.25,δ₁)为大小为0.25的具有线性段的连续的幂次函数，fe为fal(e₁₂(k),0.5,δ₁)的简化函数，fe₁为fal(e₁₂(k),0.25,δ₁)的简化函数，函数fal(e,α,δ)为原点附近具有线性段的连续的幂次函数，z₁₁(k+1)为观测器对艏向角的估计值的离散形式采样后一时刻值，h₁为步长，β₁₁为扩张观测器的第一个待定参数，e(k)为估计误差的离散形式，z₁₂(k+1)为转艏角速度的估计值的离散形式采样后一时刻值，z₁₂(k)为转艏角速度的估计值的离散形式当前值，h₁(z₁₃(k)-β₁₂fe+b₁₀u₁(k))为转艏角速度离散表示式，z₁₃(k+1)为作用于UUV艏向控制模型总扰动的估计值的离散形式采样后一时刻值，z₁₃(k)为作用于UUV艏向控制模型总扰动的估计值的离散形式当前值，h₁(-β₁₃fe₁)为总扰动的的表示式；β₁₂为扩张观测器的第二个待定参数，b₁₀为补偿参数，u₁(k)为转艏力矩的离散形式，β₁₃为扩张观测器的第三个待定参数；

(3)制备动态面控制器

(a)定义第一个动态面S₁₁为：

S₁₁＝x₁₁-v₁₁

式中，x₁₁为第一动态面虚拟控制变量；

对S₁₁求导：

${\stackrel{\·}{S}}_{11}=x_{12}-v_{12}$

将实际控制量x₁₂作为虚拟控制变量，并选取镇定函数

${\stackrel{\‾}{\mathrm{\α}}}_{12}=v_{12}-k_{11}{S}_{11}$

式中，k₁₁为待选取参数，且满足k₁₁>0，

结合动态面，引入一阶低通滤波器：

${\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)$

其中，τ₁₂为待设计的滤波器时间常数，且

(b)定义第二个动态面S₁₂为：

S₁₂＝x₁₂-α₁₂

对S₁₂求导：

${\stackrel{\·}{S}}_{12}={\stackrel{\·}{x}}_{12}-{\stackrel{\·}{\mathrm{\α}}}_{12}$

选取实际控制量x'₁₂，且实际控制量x₁₂满足：

${\stackrel{\·}{x}}_{12}^{\′}={\stackrel{\·}{\mathrm{\α}}}_{11}-k_{12}{S}_{12}$

即：

$u_{10}={\stackrel{\·}{\mathrm{\α}}}_{11}-k_{12}{S}_{12}$

式中，为镇定值导数，k₁₂为待设计控制器的参数，且有k₁₂>0，u₁₀为控制2变量；

(4)扰动补偿

将动态面得出的控制2变量u₁₀对UUV艏向控制模型总扰动的估计值z₁₃进行补偿，得出控制3变量u₁，表达式如下：

u₁＝u₁₀-z₁₃/b₁₀或者u₁＝(u₁₀-z₁₃)/b₁₀

式中，b₁₀为补偿参数；

至此，得出动态面自抗扰控制器的具体形式如下：

$\left\{\begin{array}{c}{e}_{11}=v_{11}-v_{\mathrm{\ψ}}\\ \mathrm{fh}=\mathrm{fhan}(e_{11},v_{12},r_1,h_1)\\ v_{11}=v_{11}+h_1{v}_{12}\\ v_{12}=v_{12}+h_1\·\mathrm{fh}\\ e_{12}=z_{11}-y_1,\mathrm{fe}=\mathrm{fal}(e_{12},0.5,{\mathrm{\δ}}_1),{\mathrm{fe}}_1=\mathrm{fal}(e_{12},0.25,{\mathrm{\δ}}_1)\\ z_{11}=z_{11}+h_1(z_{12}-{\mathrm{\β}}_{11}{e}_{12})\\ z_{12}=z_{12}+h_1(z_{13}-{\mathrm{\β}}_{12}\·\mathrm{fe}+b_{10}{u}_1)\\ z_{13}=z_{13}+h_1(-{\mathrm{\β}}_{13}\·{\mathrm{fe}}_1)\\ S_{11}=x_{11}-v_{11}\\ {\stackrel{\‾}{\mathrm{\α}}}_{12}=v_{12}-k_{11}{S}_{11},{\mathrm{\τ}}_{12}{\stackrel{\·}{\mathrm{\α}}}_{12}+{\mathrm{\α}}_{12}={\stackrel{\‾}{\mathrm{\α}}}_{12},{\mathrm{\α}}_{12}\left(0\right)={\stackrel{\‾}{\mathrm{\α}}}_{12}\left(0\right)\\ S_{12}=x_{12}-{\mathrm{\α}}_{12}\\ u_{10}={\stackrel{\·}{\mathrm{\α}}}_{12}-k_{12}{S}_{12}\\ u_1=(u_{10}-z_{13})/b_{10}\end{array}\right.$

式中，e₁₁为观测器对艏向角误差，fhan为连续的幂次函数，fh为综合函数，e₁₂为艏向时刻误差，y₁为实时艏向输出，fe为fal(e₁₂,0.5,δ₁)的简化函数，fal(e₁₂,0.5,δ₁)为大小为0.5的具有线性段的连续的幂次函数，fe₁为fal(e₁₂(k),0.25,δ₁)的简化函数，fal(e₁₂,0.25,δ₁)为大小为0.25的具有线性段的连续的幂次函数，h₁(z₁₂-β₁₁e₁₂)为观测器对艏向角的离散表示式，h₁(z₁₃-β₁₂·fe+b₁₀u₁)为转艏角速度离散表示式，h₁(-β₁₃·fe₁)为总扰动的的表示式，k₁₁为待选取参数，k₁₂为待设计控制器的参数，u₁为控制3变量，u₁₀为控制2变量。