[发明专利]一种基于最大熵自适应抗差估计的USBL斜距修正方法

权利要求书

1.一种基于最大熵自适应抗差估计的USBL斜距修正方法，其特征在于：包括：

步骤1：记录USBL发送请求报文时刻的SINS姿态转移矩阵、速度和位置信息；

在USBL发送请求报文时刻t₁，SINS姿态转移矩阵为速度更新结果为位置更新结果为表示惯导解算地理经度值，表示惯导解算地理纬度值，表示惯导解算高度值；

步骤2：记录USBL接收到应答器报文时刻的方位角，SINS姿态、速度和位置信息；

在应答器报文接收时刻t₂,应答器发出的信号到基阵原点的声线与x轴和y轴的夹角分别为α和β，SINS姿态转移矩阵为速度更新结果为表示载体东向、北向、天向速度,位置更新结果为

步骤3：根据发送时刻和接收时刻SINS的解算结果修正USBL的斜距；

对USBL斜距进行修正，包括如下步骤：

3.1在t₂时刻，应答器在n系下的位置为USBL发送t₁时刻与接收时刻t₂之间，SINS更新了m个周期，则将t₂时刻惯性解算位置递推至t₁时刻

其中，M_PV(m)表示第m个采样周期时的SINS位置更新矩阵，

表示第m个采样周期惯性解算的速度信息，T表示SINS采样间隔；R_Nh＝R_N+h(m)，R_Mh＝R_M+h(m),R_e表示地球长半轴半径，e表示地球偏心率，h(m)表示第m个采样周期惯导解算的高度值，L(m)表示第m个采样周期惯导解算的纬度值。

3.2根据上式中的递推结果，计算的斜距：

3.3在t₁时刻，根据组合的高精度位置，计算发送时刻的斜距：

则应答器在α系下的位置表示为：

其中，表示SINS与USBL之间的安装误差角，表示t₂时刻的姿态转移矩阵，表示n系到e系下的转移矩阵，表示SINS与USBL之间的杆臂误差；

3.4SINS计算的姿态转移矩阵为则将t₂时刻计算的斜距递推至t₁时刻：

3.5根据步骤3.4所计算的斜距，则t₂时刻的斜距修正为：

步骤4：根据步骤3修正的USBL斜距和步骤2采集的方位角、高度角和SINS解算信息，建立SINS/USBL紧组合系统模型；

在t₂时刻，建立系统模型，步骤如下：

4.1系统状态模型为：

其中，F(k)为状态转移矩阵，W(k)为系统噪声，状态向量X(k)为：

其中，

X_USBL＝[θ_x θ_y θ_z δα δβ δD]^T；

其中，δθ＝[θ_x θ_y θ_z]^T表示安装角误差，[δα δβ]^T表示、测角误差，δD表示测距误差，

φⁿ＝[φ_x φ_y φ_z]^T表示载体俯仰角、横滚角、航向角误差；δV＝[δV_E δV_N δV_N]^T表示载体东向、北向、天向速度误差；δP＝[δL δλ δh]^T表示载体地理经度、纬度、高度误差；表示SINS的加速度零偏；[ε_x ε_y ε_z]^T表示SINS的陀螺零偏；

其中，F_USBL＝0_6×6，

其中，

F₂₂＝(Vⁿ×)F₁₂-(a×)

其中，ω_ie表示地球自转角速率。

4.2系统量测模型为：

Z(k)＝HX(k)+V

其中，Z表示系统量测值，H表示量测方程转移矩阵，V表示量测噪声；

根据步骤4.3修正后的斜距以及t₂时刻测量的方位角，USBL量测信息表示为：

利用SINS位置计算得到应答器在a系下的相对位置矢量为则应答器的相对位置矢量和方位角、斜距之间的关系表达式为：

对上式求偏微分得：

其中，矢量为表述方便，记

则矩阵A_TS的表达式为：

根据SINS的位置计算的应答器在a系下的相对位置矢量为：

忽略高阶误差小量，则位置矢量误差为：

其中，

其中，C_λ＝cosλ(t₂)，

则利用SINS位置及应答器位置计算得到的a系下的斜距方位角信息重新表述为：

其中，[α β R]^T为方位角及斜距的真值，

矩阵分别为

其中，

则系统量测为：

其中，H_u＝[0 0 -1]，δU＝[δα δβ δD]^T；

观测矩阵H的表达式为：

H＝[H_a 0_3×3 H_p0_3×6 H_I H_u]；

步骤5：计算动力学模型误差自适应因子和根据最大熵原则计算增益矩阵；

步骤6：对步骤5的模型进行卡尔曼滤波融合，对SINS进行反馈校正，输出导航结果，重复上述步骤，直至导航结束。