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

Abstract

环境干扰下的水下无人航行器航迹跟踪控制优化方法，本发明涉及水下无人航行器航迹跟踪控制优化方法。本发明的目的是为了解决现有UUV的航迹跟踪控制的准确率低的问题。通过以下技术方案实现：步骤一、建立UUV水平面数学模型；步骤二、在UUV水平面数学模型的基础上进行PID控制，实现UUV航迹跟踪控制；步骤三、在UUV航迹跟踪控制的基础上对PID控制进行改进，确定非线性控制律及观测器。本发明应用于水下无人航行器领域。

Description

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

技术领域

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

背景技术

无人水下航行器(UUV)近年来成为各国研究的热门话题，被应用于水下作业的各种领域。无论是海洋军事领域还是日常海下应用作业，都离不开对于UUV的应用与控制，传统UUV航迹控制采用经典PID控制，经典控制理论中的PID控制是历史最久，应用最广，适应性最强的一种控制方法。是比例积分微分控制的简称，其原理就是对于系统误差的控制。这里所说的“误差”就是期望值与输出值的差值。将误差值作为PID控制器的输入，PID控制器表示为：Kp*误差系数+Ki*误差积分+Kd*误差微分。通过这个式子可以看出只要误差存在，PID控制器就会对误差进行调整，直至误差变为零。PID控制方法具有以下优点:PID控制结构简单、工作可靠、稳定性好、调整方便，因而成为实际控制应用的主要控制技术之一。但是在实际的系统运行中，UUV航行速度不可测，即未考虑UUV航行速度对UUV航迹跟踪控制的影响，导致UUV航迹跟踪控制的准确率低。

发明内容

本发明的目的是为了解决现有UUV航迹跟踪控制的准确率低的问题，而提出了环境干扰下的水下无人航行器航迹跟踪控制优化方法。

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

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

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

步骤三、在UUV航迹跟踪控制的基础上对PID控制进行改进，确定非线性控制律及观测器。

发明效果

采用本发明的环境干扰下的水下无人航行器航迹跟踪控制优化方法，

根据UUV航行速度不可测，设计非线性控制律—观测器，本发明观测器采用了Luenberger型观测器，利用反馈的偏差来消除偏差，将Luenberger型观测器应用到控制系统中非线性部分，针对控制系统中的线性部分设计Luenberger型观测器的增益矩阵，并选择充分大的增益来补偿系统的非线性部分，只依赖UUV的艏向对UUV进行定点跟踪控制，观测器对非线性控制系统提出UUV速度的估计值，使UUV的艏向能够逐渐趋近于期望的艏向，并最终达到误差为零，得到UUV航行速度，非线性控制律—Luenberger型观测器的误差动态方程包观测器增益、刚体科氏矩阵估计值、水动力阻尼矩阵估计值、未知干扰量估计值，R(ψ)为旋转矩阵、未知干扰量估计值一阶导数和UUV航行速度，考虑了UUV航行速度对UUV航迹跟踪控制的影响，提高了UUV的航迹跟踪控制的准确率，使UUV的航迹跟踪控制的准确率提高了20-85％。

附图说明

图1为本发明流程图；

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

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

图3a为改进算法后的UUV跟踪误差图，——为非线性输出反馈控制下的期望轨迹与实际航行轨迹误差，横坐标为时间，时间单位为秒(s)，纵坐标为行迹误差，纵坐标为航迹误差，航迹误差单位为rad；

图3b为改进算法后的UUV轨迹跟踪图，——为期望航迹，-----为实际航迹，横坐标Y为(m)，纵坐标X为米(m)。

具体实施方式

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

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

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

步骤三、在UUV航迹跟踪控制的基础上对PID控制进行改进，确定非线性控制律及观测器。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一中建立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]$

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

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

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

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

其中，M_RB为刚体的质量矩阵，V为载体坐标系下角速度矩阵，为载体坐标系下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.$

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

式中，m_IQ为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]$

要建立精确的UUV水动力，阻力和阻力矩矩阵是一项非常有难度的任务，特别是速度范围变化比较大的UUV，找到符合任意速度的水动力系数是很难以实现的。而且，当UUV的纵向速度比较大时，水动力的非线性特征就会表现得十分明显。同时，UUV各个自由度之间的耦合性也会增强。本文所使用的水动力系数是由水池实验和经验公式得到的，当UUV速度范围在2-4m/s时，能够确保其正确性。

结合上面的论证与简化，对于本文的研究对象，水动力阻尼表达式可表示如下：

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＝5，I_z＝8.0， $N_{\left|r\right|v}=u{Y}_{\stackrel{.}{r}},$ N_r＝5，N_|r|r＝15；

向量的形式表示为：

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

其中，τ为执行机构的控制力矩阵，τ_u为纵向推力，τ_q为纵倾力矩，τ_r为转艏力矩；

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

$\left\{\begin{array}{c}\stackrel{.}{u}=(-d_{11}u+{\mathrm{\τ}}_u)/m_{11}\\ \stackrel{.}{v}=(A{m}_{66}-B{m}_{26})/(m_{22}{m}_{66}-m_{26}^2)\\ \stackrel{.}{r}=(B{m}_{22}-A{m}_{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\sin \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_IQ为UUV质量，I为0、1、2、3、4、5、6、7、8或9，Q为0、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；

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

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

PID控制算法：

ψ_e＝ψ_d-ψ

式中，ψ_d为实时期望艏向，是通过编程人为设定的期望值，ψ为UUV航行时的实际艏向值，ψe为艏向误差；

UUV艏向的PID控制规律为： $\mathrm{\ψ}=k_p\·{\mathrm{\ψ}}_e+k_i\·\∫{\mathrm{\ψ}}_e+k_d\·{\stackrel{.}{\mathrm{\ψ}}}_e$

式中，k_p为比例增益，k_i为积分增益，k_d为微分增益，为ψ的一阶导数，均是通过调节时人为给定的参数。

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

具体实施方式四：本实施方式与具体实施方式一、二或三不同的是：所述步骤三中在UUV航迹跟踪控制的基础上对PID控制进行改进，设计非线性控制律及观测器；具体过程为：

非线性控制律的确定：

定义误差变量：

$\left\{\begin{array}{c}\mathrm{\ϵ}=\underset{0}{\overset{t}{\∫}}(\mathrm{\ψ}-{\mathrm{\ψ}}_d)d_{\mathrm{\τ}}\\ z_1=\mathrm{\ψ}-{\mathrm{\ψ}}_d\\ z_2=r-{\mathrm{\α}}_1\end{array}\right.$

式中的z₁为艏向角的误差，ε为艏向角误差的积分变量，α₁为镇定函数，r为角速度；z₂为镇定艏摇角差；

UUV航迹跟踪控制的误差动态方程为：

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\ϵ}}=z_1\\ {\stackrel{.}{z}}_1=\stackrel{.}{\mathrm{\ψ}}=r=z_2+{\mathrm{\α}}_1\\ {\stackrel{.}{z}}_2=\stackrel{.}{r}-{\stackrel{.}{\mathrm{\α}}}_1\end{array}\right.$

式中，为ε的一阶导数，为z₁的一阶导数，为z₂的一阶导数，为r的一阶导数，α₁为镇定函数，为α₁的一阶导数，为ψ的一阶导数；

选取镇定函数：

α₁＝-k_εε-k_z1z₁

k_ε为艏相角积分误差设计参数k_z1为艏相角设计参数；

可知，

${\stackrel{.}{\mathrm{\α}}}_1=k_{\mathrm{\ϵ}}\stackrel{.}{\mathrm{\ϵ}}-k_{z1}{\stackrel{.}{z}}_1$

得到了非线性控制律的表达式为：

${\mathrm{\τ}}_r=-\hat{w}+D{\mathrm{\α}}_1+M_A{\stackrel{.}{\mathrm{\α}}}_1-z_1-k_{z2}{z}_2$

式中，k_z2为设计参数，为未知的扰动量的估计值，M_A为附加质量的惯性矩阵，D为线性阻尼矩阵，τ_r为转艏力矩；

观测器的确定：

设UUV位置与艏向角的估计值为为v的估计值；

其中，为UUV位置与艏向角的估计值，为估计的x轴位姿，为估计的y轴位姿，为估计的UUV航行的实时艏向；

则定义UUV的速度与位置观测误差为：

$\left\{\begin{array}{c}\widetilde{\mathrm{\η}}=\mathrm{\η}-\widehat{\mathrm{\η}}\\ \tilde{v}=v-\hat{v}\end{array}\right.$

式中，为位置观测误差，为速度观测误差，η为UUV的位置矩阵；

根据UUV跟踪控制系统设计Luenberger型观测器：

所述，Luenberger型观测器为隆伯格型观测器；

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\χ}}=R\left(\mathrm{\ψ}\right)v\\ M_A\stackrel{.}{v}=-C_{\mathrm{RB}}\left(V\right)v-C_A\left(V\right)-D\left(V\right)v+{\mathrm{\τ}}_c+w\\ \stackrel{.}{w}=-\frac{\mathrm{\λ}}{r_w}w\end{array}\right.$

式中，R(ψ)为旋转矩阵，C_RB(V)为刚体的科氏矩阵，D(V)为水动力阻尼矩阵，C_A(V)为水动力科氏矩阵，为时间常数，w为未知扰动，为纵向速度的旋转矩阵，纵向角速度，τ_c为水动力矩，w为未知扰动量，为扰动量一阶导数；

可以得到观测器的误差动态方程：

$\left\{\begin{array}{c}\stackrel{.}{\widehat{\mathrm{\χ}}}=R\left(\mathrm{\ψ}\right)\hat{v}+K_1\widetilde{\mathrm{\χ}}\\ M_A\stackrel{.}{v}=-C_{\mathrm{RB}}\left(\hat{v}\right)\hat{v}-D\left(\hat{v}\right)\hat{v}+{\mathrm{\τ}}_c+\hat{w}\\ \stackrel{.}{\hat{w}}=-K_2\hat{w}+M_{A}R{\left(\mathrm{\ψ}\right)}^{-1}{K}_1\widetilde{\mathrm{\χ}}\end{array},+M_{A}R{\left(\mathrm{\ψ}\right)}^{-1}{K}_1\widetilde{\mathrm{\χ}}\right.$

式中，K₁为观测器增益，K₂为是时间常数，为纵向速度的旋转矩阵估计值，为纵向速度估计值，为刚体科氏矩阵估计值，为水动力阻尼矩阵估计值，为未知干扰量估计值，R(ψ)为旋转矩阵，为未知干扰量估计值一阶导数，为纵向速度的旋转矩阵误差，λ为扰动波长，r_w为扰动角。

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

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

实施例1

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

步骤一、UUV模型的建立；

步骤二、在UUV模型的基础上进行PID控制实现UUV航迹跟踪控制；

PID控制算法：

ψ_e＝ψ_d-ψ

式中，ψ_d为实时期望艏向，是通过编程人为设定的期望值，ψ为UUV航行时的实际艏向值，ψe为艏向误差；

UUV艏向的PID控制规律为： $\mathrm{\ψ}=k_p\·{\mathrm{\ψ}}_e+k_i\·\∫{\mathrm{\ψ}}_e+k_d\·{\stackrel{.}{\mathrm{\ψ}}}_e$

式中，k_p为比例增益，k_i为积分增益，k_d为微分增益，为ψ的一阶导数，均是通过调节时人为给定的参数；k_p＝8，k_i＝0.5，k_d＝25；

步骤三、在UUV航迹跟踪控制的基础上对PID控制进行改进，即带观测器的非线性输出反馈控制；

非线性控制律的确定：

定义误差变量：

$\left\{\begin{array}{c}\mathrm{\ϵ}=\underset{0}{\overset{t}{\∫}}(\mathrm{\ψ}-{\mathrm{\ψ}}_d)d_{\mathrm{\τ}}\\ z_1=\mathrm{\ψ}-{\mathrm{\ψ}}_d\\ z_2=r-{\mathrm{\α}}_1\end{array}\right.$

式中的z₁为艏向角的误差，ε为艏向角误差的积分变量，α₁为镇定函数，r为角速度；z₂为镇定艏摇角差；

UUV航迹跟踪控制的误差动态方程为：

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\ϵ}}=z_1\\ {\stackrel{.}{z}}_1=\stackrel{.}{\mathrm{\ψ}}=r=z_2+{\mathrm{\α}}_1\\ {\stackrel{.}{z}}_2=\stackrel{.}{r}-{\stackrel{.}{\mathrm{\α}}}_1\end{array}\right.$

式中，为ε的一阶导数，为z₁的一阶导数，为z₂的一阶导数，为r的一阶导数，α₁为镇定函数，为α₁的一阶导数，为ψ的一阶导数；

选取镇定函数：

α₁＝-k_εε-k_z1z₁

k_ε为艏相角积分误差设计参数，k_z1为艏相角设计参数；取k_ε＝3.45，k_z1＝6.8，K₁＝10，K₂＝0.56；

可知，

${\stackrel{.}{\mathrm{\α}}}_1=k_{\mathrm{\ϵ}}\stackrel{.}{\mathrm{\ϵ}}-k_{z1}{\stackrel{.}{z}}_1$

得到了非线性控制律的表达式为：

${\mathrm{\τ}}_r=-\hat{w}+D{\mathrm{\α}}_1+M_A{\stackrel{.}{\mathrm{\α}}}_1-z_1-k_{z2}{z}_2$

式中，k_z2为设计参数，为未知的扰动量的估计值，M_A为附加质量的惯性矩阵，D为线性阻尼矩阵，τ_r为转艏力矩；

观测器的确定：

设UUV位置与艏向角的估计值为为v的估计值；

其中，为UUV位置与艏向角的估计值，为估计的x轴位姿，为估计的y轴位姿，为估计的UUV航行的实时艏向；

则定义UUV的速度与位置观测误差为：

$\left\{\begin{array}{c}\widetilde{\mathrm{\η}}=\mathrm{\η}-\widehat{\mathrm{\η}}\\ \tilde{v}=v-\hat{v}\end{array}\right.$

式中，为位置观测误差，为速度观测误差，η为UUV的位置矩阵；

根据UUV跟踪控制系统设计Luenberger型观测器：

所述，Luenberger型观测器为隆伯格型观测器；

$\left\{\begin{array}{c}\stackrel{.}{\mathrm{\χ}}=R\left(\mathrm{\ψ}\right)v\\ M_A\stackrel{.}{v}=-C_{\mathrm{RB}}\left(V\right)v-C_A\left(V\right)-D\left(V\right)v+{\mathrm{\τ}}_c+w\\ \stackrel{.}{w}=-\frac{\mathrm{\λ}}{r_w}w\end{array}\right.$

式中，R(ψ)为旋转矩阵，C_RB(V)为刚体的科氏矩阵，D(V)为水动力阻尼矩阵，C_A(V)为水动力科氏矩阵，为时间常数，w为未知扰动，为纵向速度的旋转矩阵，纵向角速度，τ_c为水动力矩，w为未知扰动量，为扰动量一阶导数；

可以得到观测器的误差动态方程：

$\left\{\begin{array}{c}\stackrel{.}{\widehat{\mathrm{\χ}}}=R\left(\mathrm{\ψ}\right)\hat{v}+K_1\widetilde{\mathrm{\χ}}\\ M_A\stackrel{.}{v}=-C_{\mathrm{RB}}\left(\hat{v}\right)\hat{v}-D\left(\hat{v}\right)\hat{v}+{\mathrm{\τ}}_c+\hat{w}+M_{A}R{\left(\mathrm{\ψ}\right)}^{-1}{K}_1\widetilde{\mathrm{\χ}}\\ \stackrel{.}{\hat{w}}=-K_2\hat{w}+M_{A}R{\left(\mathrm{\ψ}\right)}^{-1}{K}_1\widetilde{\mathrm{\χ}}\end{array}\right.$

式中，K₁为观测器增益，K₂为是时间常数，K₁＝10，K₂＝0.56，为纵向速度的旋转矩阵估计值，为纵向速度估计值，为刚体科氏矩阵估计值，为水动力阻尼矩阵估计值，为未知干扰量估计值，R(ψ)为旋转矩阵，为未知干扰量估计值一阶导数，为纵向速度的旋转矩阵误差，λ为扰动波长，r_w为扰动角；

得到附图说明中图2(a)、图2(b)、图3(a)和图3(b)的仿真结果，从仿真结果可以看出带观测器的非线性控制规律同样可以达到PID的控制效果，即UUV成功跟踪上一条我所设定的直线轨迹，并达到误差为零；得到UUV航行速度，考虑了UUV航行速度对UUV航迹跟踪控制的影响，提高了UUV的航迹跟踪控制的准确率。

Claims (4)

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

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

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

步骤三、在UUV航迹跟踪控制的基础上对PID控制进行改进，确定非线性控制律及观测器。

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

（1）运动学方程的建立：

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

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

用向量形式表示为：

其中，

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

（2）动力学方程的建立：

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

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

其中的参数：

式中，m_IQ为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；

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

其中，

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

τ=[τ_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为转艏力矩；

其中，

式中，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_v=36，Y_uv=0， Y_r=5，I_z=8.0，N_r=5，N_|r|r=15；

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

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

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

而且

式中，A为质量参数，B为附加质量参数，m_IQ为UUV质量，I为0、1、2、3、4、5、6、7、8或9，Q为0、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。

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

PID控制算法为：

ψ_e＝ψ_d-ψ

式中，ψ_d为实时期望艏向，是通过编程人为设定的期望值，ψ为UUV航行时的实际艏向值，ψe为艏向误差；

UUV艏向的PID控制规律为：

式中，k_p为比例增益，k_i为积分增益，k_d为微分增益，为ψ的一阶导数，均是通过调节时人为给定的参数。

4.根据权利要求3所述环境干扰下的水下无人航行器航迹跟踪控制优化方法，其特征在于，所述步骤三中在UUV航迹跟踪控制的基础上对PID控制进行改进，设计非线性控制律及观测器；具体过程为：

非线性控制律的确定：

定义误差变量：

式中的z₁为艏向角的误差，ε为艏向角误差的积分变量，α₁为镇定函数，r为纵摇角速度；z₂为镇定艏摇角差；

UUV航迹跟踪控制的误差动态方程为：

式中，为ε的一阶导数，为z₁的一阶导数，为z₂的一阶导数，为r的一阶导数，α₁为镇定函数，为α₁的一阶导数，为ψ的一阶导数；

选取镇定函数：

α₁＝-k_εε-k_z1z₁

kε为艏相角积分误差设计参数k_z1为艏相角设计参数；

可知，

得到了非线性控制律的表达式为：

式中，k_z2为设计参数，为未知的扰动量的估计值，M_A为附加质量的惯性矩阵，D为线性阻尼矩阵，τ_r为转艏力矩；

观测器的确定：

设UUV位置与艏向角的估计值为 为v的估计值；

其中，为UUV位置与艏向角的估计值，为估计的x轴位姿，为估计的y轴位姿， 为估计的UUV航行的实时艏向；

则定义UUV的速度与位置观测误差为：

式中，为位置观测误差，为速度观测误差，η为UUV的位置矩阵；

根据UUV跟踪控制系统设计Luenberger型观测器：

所述，Luenberger型观测器为隆伯格型观测器；

式中，R(ψ)为旋转矩阵，C_RB(V)为刚体的科氏矩阵，D(V)为水动力阻尼矩阵，C_A(V)为水动力科氏矩阵，为时间常数，w为未知扰动，为纵向速度的旋转矩阵，为UUV纵向角速度的一阶导数，τ_c为水动力矩，w为未知扰动量，为扰动量一阶导数；

可以得到观测器的误差动态方程：

式中，K₁为观测器增益，K₂为是时间常数，为纵向速度的旋转矩阵估计值，为UUV纵向速度估计值，为刚体科氏矩阵估计值，为水动力阻尼矩阵估计值，为未知干扰量估计值，R(ψ)为旋转矩阵，为未知干扰量估计值一阶导数，为纵向速度的旋转矩阵误差，λ为扰动波长，r_w为扰动角。