[发明专利]一种基于反演的水下导航系统动力学模型中扰动估计方法

权利要求书

1.一种基于反演的水下导航系统动力学模型中扰动估计方法，其特征在于：包括以下步骤：

步骤一，根据水下航行器的动力学矢量方程，建立含有未知扰动的非线性系统状态方程和量测方程；步骤一的具体方法如下：

水下航行器有六个自由度运动，为描述航行器的运动情况，引入两种参考坐标系，分别为大地坐标系O-x_Gy_Gz_G和以航行器重心为原点的载体坐标系O-x_By_Bz_B；

航行器刚体动力学矢量表示式为

式中，向量υ为载体坐标系下分解的速度矢量υ＝[V_x,V_y,V_z,p,q,r]^T，其中，V_x是纵荡方向速度，V_y是横荡方向速度，V_z是垂荡方向速度，p是纵摇角速度，q是横摇角速度，r是艏摇角速度；M为系统惯性矩阵，其中包含附加质量；C(υ)为科里奥利向心力矩阵，其中包含附加质量；D(υ)为阻尼系数矩阵；τ为推进系统的主动力和力矩矢量；τ_d为外部环境扰动力矢量；

在大地坐标系下水下航行器的位置向量为η＝[P_x,P_y,P_z,φ,θ,ψ]^T，P_x、P_y、P_z为航行器位置，φ、θ、ψ为航行器的姿态角；在大地坐标系下位置对时间的导数与在载体坐标系下速度的转换关系为

其中，J(η)为转换矩阵；

将式(1)和式(2)的所表示非线性数学模型转化成状态空间模型，即可得到状态方程和量测方程

其中，H＝[0_6×6 I_6×6]

式中，X＝[η^T υ^T]^T为状态变量；w为系统状态方程的高斯白噪声；Z为观测向量；v为量测方程的高斯白噪声；A为系统状态转移矩阵；B为输入量控制矩阵；H为系统量测矩阵；0_6×6为6×6的零矩阵；I_6×6为6×6的单位矩阵；

将式(3)离散化，得到k时刻的离散化系统模型为

其中，Φ为离散化的系统状态转移矩阵，Φ＝exp(A×△T)，△T为采样间隔；Γ为离散化的输入量控制矩阵，

步骤二，采用容积卡尔曼滤波对水下航行器进行状态估计，得到其新息序列；

步骤三，采用递推最小二乘估计方法，由新息序列估计总力大小；

步骤四，采用迭代算法对所求总力进行修正；

步骤五，由总力求扰动力。