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

权利要求书

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

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

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

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

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

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

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

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

x·=ucosψ-vsinψy·=usinψ+vcosψψ·=r]]>

用向量形式表示为：

η·=J(Θ)V]]>

其中，J(Θ)=cosψsinψ0sinψcosψ0001]]>

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

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

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

MRBV·+CRB(V)V=τH]]>

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

其中，

MRB=m000mmxG0mxG0]]>

CRB(V)=00-c6100-c62c61c620]]>

其中的参数：

c61=m(xGr+v)c62=-mu]]>

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

τH=-MAV·-CA(V)V-D(V)V-g(Θ)+τ]]>

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

其中，

MA=-Xu·000-Yv·-Yr·0-Nv·-Nr·]]>

CA(V)=00a200-a1-a2a10]]>

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

g(Θ)=0(xGW-xBB)0]]>

τ＝[τ_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为附加距离；

其中，

Dl=Xu000Yv-Yr0-NvNr]]>

Du=0000Yuv-Yur0-NuvNur]]>

Dnl(V)=X|u|u|u|000Y|v|v|v|-Y|v|r|v|0-N|r|v|r|N|r|r|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，Nv·=2.2,]]>N_v＝36，Y_uv＝0，Xu·=-1.42,Xur=0.1,X|ur|ur=8.2,Yur=uXu·,]]>Yv·=-38.4,Yvr=10,Y|vr|vr=200,Nuv=u(Yv·-Xu·),Yr·=-2.5,]]>Y_r＝5，I_z＝8.0，N|r|v=uYr·,]]>N_r＝5，Nr·=-8.9,N|r|r=15;]]>

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

u·=(-d11u+τu)/m11v·=(Am66-Bm26)/(m22m66-m262)r·=(Bm22-Am26)/(m22m66-m262)x·=ucos(ψ)-vsin(ψ)y·=usin(ψ)+vcos(ψ)ψ·=r]]>

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

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

而且m11=m-Xu·d11=Xu+X|u|u|u|m22=m-Yv·d22=Yv+Y|v|v|v|m26=-Yr·d26=Yrm66=Iz-Nr·d62=Nvc26=m-Xu·d66=Nr+N|r|r|r|c62=Xu·-Yv·]]>

式中，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为转动惯量。