[发明专利]一种惯性导航初始基准的估计与补偿方法

摘要

本发明公开了一种惯性导航初始基准的估计与补偿方法，通过卫星导航提供的位置、速度信息，和惯性导航计算得到的位置、速度信息采用卡尔曼滤波的方式，估计惯性导航初始基准误差，修正时结合姿态控制要求采用程序角调姿的方式，对姿态角进行修正，同时对速度和位置进行修正，最终实现提高姿态角解算精度，提高中制导与末制导交班精度的目的。

主权项

1.一种惯性导航初始基准的估计与补偿方法，惯性导航系统接收卫星导航提供的位置、速度信息，以及惯性导航系统计算得到的位置、速度信息，其特征在于，该方法包括如下步骤：步骤a)建立初始姿态误差估计状态方程：步骤b)建立姿态初值误差估计观测方程：Z＝HX+V；步骤c)计算观测量，将惯性导航速度和位置与卫星导航给出的速度和位置作差作为观测量：步骤d)将状态方程和观测方程离散化：X_k＝Φ_k,k‑1X_k‑1+W_k‑1Z_k＝H_kX_k+V_k；步骤e)使用卡尔曼滤波方程计算：P_k＝(I‑K_kH_k)P_k|k‑1；步骤f)姿态误差角的修正姿态角偏差：修正后的姿态角：程序角计算：ψ_cx＝ψ_cx‑δψ·kγ_cx＝γ_cx‑δγ·k；步骤g)速度和位置误差的修正导航速度修正：V＝V‑W·Φ₀，导航位置修正：X＝X‑S·Φ₀；其中，所述步骤a中：X＝[φ_x0 φ_y0 φ_z0 δV_x0 δV_y0 δV_z0 δX₀ δY₀ δZ₀]^TF＝O_9×9；初始姿态误差角：φ_x0、φ_y0、φ_z0，速度初始误差：δV_x0、δV_y0、δV_z0，位置初始误差：δX₀、δY₀、δZ₀；其中，所述步骤b中：其中，所述I_3×3为3行3列的单位矩阵，W_x、W_y、W_z为视速度，S_x、S_y、S_z为视位置；其中，V_xINS，V_yINS，V_zINS为惯性导航速度，X_INS，Y_INS，Z_INS为惯性导航位置，V_xGNSS，V_xGNSS，V_xGNSS为卫星导航给出的速度，X_GNSS，Y_GNSS，Y_GNSS为卫星导航给出的位置；其中，所述步骤d中，Φ_k,k‑1＝I_9×9，Q_k‑1＝O_9×9，H_k＝H；其中，所述步骤f中，ψ₀、γ₀为当前俯仰角、当前偏航角，当前滚动角，δψ，δγ为俯仰姿态角偏差、偏航姿态角偏差、滚动姿态角偏差；ψ_cx、γ_cx为俯仰程序角、偏航程序角、滚动程序角。