一种激光惯组与里程计组合导航方法及系统

摘要

本发明公开了一种激光惯组与里程计组合导航方法及系统，方法包括步骤：S1、采集正常工作状态下N个采样周期内里程计脉冲数，求解惯组与里程计之间的俯仰安装偏角和航向安装偏角；S2、建立状态方程；S3、建立量测方程，其中量测量为惯组在第i个采样周期内导航系下的位移增量，为第i个采样周期内里程计增量在导航系下的位移投影；S4、利用状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。本发明还提供实现上述方法的系统。实施本发明能有效修正组合导航误差量，大幅度提高组合导航精度，尤其适用于惯组处于大角度斜置的情况。

主权项

一种激光惯组与里程计组合导航方法，其特征在于，所述方法包括以下步骤：S1、采集正常工作状态下第i个采样周期内里程计脉冲数N_i，求解惯组与里程计之间的俯仰安装偏角α_θ和航向安装偏角α_ψ，所述为矢量第j个分量，j＝1,2,3；为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵，为预设时间内惯组导航解算的位移增量，K_od为里程计脉冲当量；S2、建立状态方程其中，F为状态转移矩阵，w为系统噪声，$X={\left[\begin{array}{cccccc}{\left(\phi \right)}^T& {\left({\mathrm{\delta v}}^n\right)}^T& {\left(\delta P\right)}^T& {\left({ϵ}_b^b\right)}^T& {\left({▿}_b^b\right)}^T& {{\xi }_{od}}^T\end{array}\right]}^T,$φ为惯导姿态误差，δvⁿ为惯导速度误差，δP为惯导位置误差，为陀螺漂移误差；为加速度计偏置误差，里程计误差相关量ξ_od＝[δK_od,δα_θ，δα_ψ]，其中δK_od为里程计刻度系数误差，δα_θ为俯仰安装偏角误差，δα_ψ为航向安装偏角误差；S3、建立量测方程z＝HX+v，其中v为量测噪声；量测量累积预设采样周期内惯导位移增量与里程计位移增量，并将二者之差作为量测量z，所述为惯组在第i个采样周期内导航系下的位移增量，为第i个采样周期内里程计增量在导航系下的位移投影；量测矩阵$H=\left[\begin{array}{cccccc}-\Sigma \left(C_b^n\left(i\right){\mathrm{\Delta S}}_i^b\right)\times & I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -{\mathrm{\Sigma C}}_b^n\left(i\right)M_i^b\end{array}\right],$I为单位矩阵，$M_i^b=N_i{K}_{od}\left[\begin{array}{ccc}{\mathrm{sin\alpha }}_{\psi }{\mathrm{cos\alpha }}_{\theta }& -{\mathrm{sin\alpha }}_{\psi }{\mathrm{sin\alpha }}_{\theta }& {\mathrm{cos\alpha }}_{\psi }{\mathrm{cos\alpha }}_{\theta }\\ {\mathrm{cos\alpha }}_{\psi }{\mathrm{cos\alpha }}_{\theta }& -{\mathrm{cos\alpha }}_{\psi }{\mathrm{sin\alpha }}_{\theta }& -{\mathrm{sin\alpha }}_{\psi }{\mathrm{cos\alpha }}_{\theta }\\ {\mathrm{sin\alpha }}_{\theta }& {\mathrm{cos\alpha }}_{\theta }& 0\end{array}\right];$S4、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。