[发明专利]一种运动效应影响下的惯性/超短基线组合导航方法

权利要求书

1.一种运动效应影响下的惯性/超短基线组合导航方法，其特征在于，包括如下步骤：

(1)建立惯性/超短基线组合导航的状态方程模型；

步骤(1)中，建立惯性/超短基线组合导航的状态方程模型：

将惯导的位置误差、速度误差、姿态误差作为系统状态量，得：

其中，[φ_E φ_N φ_U]^T表示姿态的失准角误差，[δV_E δV_N δV_U]^T表示速度误差，[δL δλ δh]^T表示位置误差，[ε_x ε_y ε_z]^T表示陀螺仪的零偏，表示加速度计的零偏；

因此，组合导航的状态方程为：

其中，F(t)表示状态转移矩阵，根据捷联惯导误差方程设置，W(t)表示过程噪声矩阵；

(2)建立惯性/超短基线组合导航的方位角量测方程模型；

步骤(2)中，建立惯性/超短基线组合导航的方位角量测方程模型，具体为：

方位角的量测方程表达式为：

其中，Δα和Δβ表示方位角的误差，α_usbl，β_usbl表示超短基线测量得到的方位角，α_ins，β_ins表示利用惯导及应答器位置计算得到的方位角，[*]_1：2表示矩阵的前两行，表示超短基线的安装误差矩阵，表示由导航系到载体系的坐标转换矩阵，表示由地球坐标系到导航系的坐标转换矩阵，p^e为地球系下的相对位置矢量，表示为：

其中，R_N为卯酉圈曲率半径，e为椭圆偏心率，为应答器的位置，为惯导输出的位置；

C_e表示为：

其中，R_Nh＝R_N+h，[L λ h]分别表示经度、纬度和高度；

B_S表示为

其中，S_a＝sin(a)，S_a＝cos(a)，a＝L或λ；

H₁表示为：

其中，表示应答器在基阵坐标系下的相对位置，r表示斜距；

(3)建立基于状步骤(3)中，建立基于状态递推的惯性/超短基线组合导航斜距量测方程模型，具体为：

声信号在发射时刻k以及接收时刻k+i对应的惯导位置为和此时惯导对应的误差状态为和通过状态转移矩阵统一不同时刻的误差状态，得到：

其中，

考虑惯导误差情况下，利用收发时刻惯导位置及应答器位置计算得到的相应斜距表示为：

其中，r₁表示应答器与信号发射时刻船的位置之间的真实距离，r₂表示应答器与信号接收时刻船的位置之间的真实距离，表示r₁的误差，表示r₂的误差，表示k时刻位置误差，表示k+i时刻位置误差，C_e，k+i表示k+i时刻的C_e矩阵值，表示k+i时刻的矩阵值，B_S，k+i表示k+i时刻的B_S矩阵值，表示k+i时刻的p^e矩阵值，C_e，k表示k时刻的C_e矩阵值，表示k时刻的矩阵值，B_S，k表示k时刻的B_S矩阵值，表示k时刻的p^e矩阵值；

超短基线测量得到的往返斜距表示为：

ct＝r₁+r₂-δr

其中，δr表示超短基线测量的误差；

因此，新的基于往返斜距的量测模型有：

其中，H_2-1＝[0_1×6 H_PD1 0_1×6]，

H_2-1＝[0_1×6 H_PD2 0_1×6]，

{*}_7：9表示向量的第7-9列，

态递推的惯性/超短基线组合导航斜距量测方程模型；

(4)根据(1)-(3)建立的状态方程以及量测方程，利用卡尔曼滤波方法进行误差的更新与反馈。

2.根据权利要求书1所述一种运动效应影响下的惯性/超短基线组合导航方法，其特征在于：步骤(4)中，具体为：

综合步骤(2)和(3)，惯性/超短基线的组合导航量测方程为：

Z＝HX+V＝[H_α H_r]^TX+[V₁ V₂]^T

根据新的量测方程，进行卡尔曼滤波更新的步骤为：

(a)状态一步预测：

其中，表示k-1时刻的状态估计，F_k-1表示k-1时刻到k时刻的状态转移矩阵；

(b)状态一步预测均方误差：

其中，Q_k-1表示过程噪声矩阵，P_k-1表示k-1时刻的均方根误差；

(c)滤波增益：

其中，H_k表示k时刻的量测矩阵，R_k表示量测噪声矩阵；

(d)状态估计：

其中，z_k表示观测量；

(e)状态估计均方误差：P_k＝P_k-K_kH_kP_k|(k-1)。