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

Abstract

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

Description

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

技术领域

本发明属于航空航天捷联惯性导航技术领域中惯性组合定位领域，更具体地，涉及一种激光惯组与里程计组合导航方法及系统。

背景技术

捷联惯导系统具有反应时间段、可靠性高、自主性好以及全天候应用等优点，广泛应用于航空、航天及车载等军用和民用导航领域，在国防和经济建设中发挥着重要作用。

惯性导航系统能实时地输出载体位置、速度以及姿态等信息，但是其难以克服导航误差随时间累积的缺点。里程计是一种自主的距离信息测量传感器，它与惯性导航系统组合之后能够起到抑制惯导误差发散的作用，因此采用惯性/里程计组合技术能够建立高精度自主定位定向导航系统。

激光惯组精度高、稳定性好，是实现高精度自主定位定向的重要组成。由于死区的存在，激光惯组通常工作在机械抖动偏频模式下，在此模式下激光陀螺噪声较大，难以实现高精度定向能力。因此考虑将激光惯组斜置，使惯组的三个正交轴与水平面保持一个较大的角度，同时使得惯组绕天向轴旋转时工作在速率偏频模式下，此模式下激光惯组避开了死区，且产生的噪声小、定位精度高。

里程计测量的是车辆行进方向的路程，通过建立车体坐标系将里程计一维输出转换为三位输出，再通过车体坐标系到惯组坐标系的转换以及惯组坐标系到导航坐标系的转换得到里程计输出在导航系下的投影，通常情况下车体系到惯组坐标系是小角度，可以按照线性补偿的方式进行转换。激光惯组在斜置状态下，三个正交轴与水平面的夹角都是大角度，若以线性方式对车体系到惯组坐标系之间的转换进行补偿，则会导致误差过大。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种激光惯组与里程计组合导航方法及系统，方法能有效修正组合导航误差量，大幅度提高组合导航精度，尤其适用于惯组处于大角度斜置的情况。

为实现上述目的，按照本发明，提供了一种激光惯组与里程计组合导航方法，所述方法包括以下步骤：

S1、采集正常工作状态下第i个采样周期内里程计脉冲数N_i，求解惯组与里程计之间的俯仰安装偏角α_θ和航向安装偏角α_ψ，

所述 为矢量第j个分量，j＝1,2,3；为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵，为预设时间内惯组导航解算的位移增量，K_od为里程计脉冲当量；

S2、建立状态方程其中，F为状态转移矩阵，w为系统噪声， $X={\left[\begin{array}{cccccc}{\left(\mathrm{\φ}\right)}^T& {\left({\mathrm{\δv}}^n\right)}^T& {\left(\mathrm{\δ}P\right)}^T& {\left({\mathrm{\ϵ}}_b^b\right)}^T& {\left({\▿}_b^b\right)}^T& {{\mathrm{\ξ}}_{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}-\mathrm{\Σ}\left(C_b^n\right(i\left){\mathrm{\ΔS}}_i^b\right)\×& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& -{\mathrm{\Σ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\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\θ}}& 0\end{array}\right];$

S4、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。

相应地，本发明还提供一种激光惯组与里程计组合导航系统，所述系统包括：

第一子模块，用于采集正常工作状态下第i个采样周期内里程计脉冲数N_i，求解惯组与里程计之间的俯仰安装偏角α_θ和航向安装偏角α_ψ，

所述 为矢量第j个分量，j＝1,2,3；为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵，为预设时间内惯组导航解算的位移增量，K_od为里程计脉冲当量；

第二子模块，用于建立状态方程其中，F为状态转移矩阵，w为系统噪声， $X={\left[\begin{array}{cccccc}{\left(\mathrm{\φ}\right)}^T& {\left({\mathrm{\δv}}^n\right)}^T& {\left(\mathrm{\δ}P\right)}^T& {\left({\mathrm{\ϵ}}_b^b\right)}^T& {\left({\▿}_b^b\right)}^T& {{\mathrm{\ξ}}_{od}}^T\end{array}\right]}^T,$ φ为惯导姿态误差，δvⁿ为惯导速度误差，δP为惯导位置误差，为陀螺漂移误差；为加速度计偏置误差，里程计误差相关量ξ_od＝[δK_od,δα_θ，δα_ψ]，其中δK_od为里程计刻度系数误差，δα_θ为俯仰安装偏角误差，δα_ψ为航向安装偏角误差；

第三子模块，用于建立量测方程z＝HX+v，其中v为量测噪声；量测量累积预设采样周期内惯导位移增量与里程计位移增量，并将二者之差作为量测量z，所述为惯组在第i个采样周期内导航系下的位移增量，为第i个采样周期内里程计增量在导航系下的位移投影；量测矩阵 $H=\left[\begin{array}{cccccc}-\mathrm{\Σ}\left(C_b^n\right(i\left){\mathrm{\ΔS}}_i^b\right)\×& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& -{\mathrm{\Σ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\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\θ}}& 0\end{array}\right];$

第四子模块，用于利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：本发明给出惯组与里程计之间安装偏角估计方法，尤其适用于惯组处于大角度斜置条件下的情况，通过安装偏角补偿后能精确计算里程计的位移增量；将预设个采样周期内惯导位移增量与里程计位移增量的差值作为量测值，考虑了里程计的刻度系数误差δK_od及安装偏角误差δα_θ、δα_ψ，有利于通过卡尔曼滤波器进行估计，进而通过估计值修正误差量，能大幅度提高组合导航精度。本发明计算简便，无需额外增加滤波器的维数，易于实现，具有良好的工程应用价值。

附图说明

图1为本发明激光惯组与里程计组合导航方法流程示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明方法原理描述如下：

里程计固定安装在车体上，与车辆的变速轴连接，通过转轴的转动传递至里程计周期转动，里程计以脉冲的形式N_i输出，通过当量K_od(单位：m/脉冲)折算后得到采样时间内里程计测量的路程增量在定义的车体坐标系m系下的投影，记为(上标表示所述路程增量在定义的车体坐标系m系下的投影，下表i表示第i个采样周期)，建立车体坐标系m系与惯组坐标系b系之间的方向余弦矩阵将路程增量投影至惯组坐标系b系得到路程增量在惯组坐标系b下的投影有：

${\mathrm{\ΔS}}_i^b=\left[\begin{array}{c}{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}\end{array}\right]K_{od}{N}_i$

上式中，α_ψ为车体坐标系m系与惯组坐标系b系之间的航向安装偏角，α_θ为车体坐标系m系与惯组坐标系b系之间的俯仰安装偏差角，m系与b系之间的滚转安装偏差角不影响路程增量的投影。根据上式得到路程增量在导航系n系下的投影：

${\mathrm{\ΔS}}_i^n=C_b^n\left(i\right)\left[\begin{array}{c}sin{\mathrm{\α}}_{\mathrm{\ψ}}cos{\mathrm{\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}\end{array}\right]K_{od}{N}_i$

上式中，为第i个采样周期内惯组坐标系b系与导航系n系之间的方向余弦矩阵。由于惯组与里程计都固连于车体上，因此认为安装偏差角α_ψ、α_θ不变，累计预设时间(例如10分钟)内导航系n下的里程计增量有：

${\mathrm{\Σ\ΔS}}_{iOD}^n=\left({\mathrm{\ΣC}}_b^n\right(i\left)K_{od}{N}_i\right)\left[\begin{array}{c}sin{\mathrm{\α}}_{\mathrm{\ψ}}cos{\mathrm{\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}\end{array}\right]$

累计该段时间内惯组导航解算的位移增量考虑高精度惯组的位置误差与行驶的距离相比为小量，将作为实际位移增量的参考值，令：

${\mathrm{\Σ\ΔS}}_{iINS}^n={\mathrm{\Σ\ΔS}}_{iOD}^n$

从而得到：

上式中为计算安装偏角的中间量，进一步得到安装偏角估计值为：

在组合导航过程中将计算得到的安装偏角补偿里程计算后得到里程计的位移增量在惯性导航基础上建立导航参数误差微分方程，选取状态量包括姿态误差、速度误差、位置误差、陀螺误差和加速度计误差，同时将里程计的刻度系数误差δK_od及安装偏角误差δα_θ、δα_ψ列入状态量，建立状态空间微分方程。根据里程计位移增量与惯组位移增量构造量测量，累积预设时间(通常设为1秒)内惯导位移增量与里程计位移增量并将二者之差作为量测值，即：

$z_{Obs}=\mathrm{\Σ}\mathrm{\Δ}{\hat{P}}_i^{INS}-\mathrm{\Σ}\mathrm{\Δ}{\hat{P}}_i^{OD}$

上式中表示预设时间内惯组的位移增量，表示相同预设时间内里程计的位移增量。根据量测量建立量测方程，对状态微分方程离散化后采用卡尔曼滤波技术对各个误差量进行估计，修正姿态、速度、位置及器件误差。

以下对本发明方法具体实现步骤作说明。

图1为本发明方法流程示意图。本发明方法包括以下步骤：

S1、按照采样周期ΔT实时采集正常工作状态下惯组脉冲数与里程计脉冲数N_i，求解里程计与惯组之间的俯仰安装偏角α_θ和航向安装偏角α_ψ，

所述 为第i个采样周期内惯组坐标系b系与导航系n系之间的方向余弦矩阵，K_od为里程计脉冲当量，为预设时间内惯组导航解算的位移增量；

S2、根据状态向量X建立状态微分方程其中，F(t)为状态转移矩阵，w(t)为系统噪声， $X={\left[\begin{array}{cccccc}{\left(\mathrm{\φ}\right)}^T& {\left({\mathrm{\δv}}^n\right)}^T& {\left(\mathrm{\δ}P\right)}^T& {\left({\mathrm{\ϵ}}_b^b\right)}^T& {\left({\▿}_b^b\right)}^T& {{\mathrm{\ξ}}_{od}}^T\end{array}\right]}^T,$ φ为姿态误差，δvⁿ为速度误差，δP为位置误差，为陀螺漂移误差；为加速度计偏置误差，ξ_od＝[δK_od,δα_θ，δα_ψ]，δK_od为里程计刻度系数误差，δα_θ为俯仰安装偏角误差，δα_ψ为航向安装偏角误差；

S3、建立量测方程，其中量测量所述为惯组在单个采样周期ΔT内导航系下的位移增量，为单个采样周期ΔT内里程计增量在导航系下的位移增量，累积预设时间内惯导位移增量与里程计位移增量，将二者之差作为量测量z_Obs；

S4、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，实现组合导航。

其中，上述步骤S1中，本发明一个实施例的实现方式为：将激光惯组与里程计固定安装于车载上(激光惯组三个正交轴与水平面的夹角都是大角度)，车载保持静止，激光惯组通电预热45分钟后开始初始对准，同时采集惯组脉冲数与里程计脉冲数，采样周期为0.01s。对准完成后车辆机动10分钟，保存原始脉冲数。

计算机动时间(10分钟)内惯组累积的位移增量，具体如下：其中ΔT为采样周期间隔，为惯组速度，N为机动时间内惯导更新次数。

计算机动时间内里程计中间变量

${\mathrm{\ΣC}}_b^n\left(i\right)K_{od}{N}_i=K_{od}\underset{i=0}{\overset{N}{\Σ}}{C}_b^n\left(i\right)N_i$

其中，K_od为里程计脉冲当量，N_i表示各采样间隔内里程计输出脉冲数，表示各个采样间隔内的姿态余弦矩阵。

计算得到包含俯仰安装偏角及航向安装偏角的矢量：

从中分离得到俯仰安装偏角和航向安装偏角：

上述步骤S2中，根据状态向量X建立状态微分方程其中，F(t)为状态转移矩阵；w(t)为系统噪声，其中t表示该物理量随时间变化； $w\left(t\right)={\left[\begin{array}{ccc}-{\left(C_b^n\right(i\left){\mathrm{\ϵ}}_w^b\right)}^T& {\left(C_b^n\right(i\left){\▿}_w^b\right)}^T& 0_{9\×1}\end{array}\right]}^T,$ 为第i个采样周期内的姿态余弦矩阵，为陀螺随机噪声，为加速度计随机噪声； $X={\left[\begin{array}{cccccc}{\left(\mathrm{\φ}\right)}^T& {\left({\mathrm{\δv}}^n\right)}^T& {\left(\mathrm{\δ}P\right)}^T& {\left({\mathrm{\ϵ}}_b^b\right)}^T& {\left({\▿}_b^b\right)}^T& {{\mathrm{\ξ}}_{od}}^T\end{array}\right]}^T,$ φ为姿态误差状态向量，φ＝[φ_E φ_N φ_U]^T，φ_E为东向姿态误差，φ_N为北向姿态误差，φ_U为天向姿态误差；δvⁿ为速度误差状态向量， ${\mathrm{\δv}}^n={\left[\begin{array}{ccc}{\mathrm{\δv}}_E^n& {\mathrm{\δv}}_N^n& {\mathrm{\δv}}_U^n\end{array}\right]}^T,$ 为东向速度误差，为北向速度误差，为天向速度误差；δP为位置误差状态向量，δP＝[δL δλ δh]^T，δL为纬度误差、δλ为经度误差、δh为高度误差；为陀螺漂移误差向量， ${\mathrm{\ϵ}}_b^b={\left[\begin{array}{ccc}{\mathrm{\ϵ}}_x^b& {\mathrm{\ϵ}}_y^b& {\mathrm{\ϵ}}_z^b\end{array}\right]}^T,$ 为x陀螺误差，为y陀螺误差，为z陀螺误差；为加速度计偏置误差向量， ${\▿}_b^b=\left[\begin{array}{ccc}{\▿}_x^b& {\▿}_y^b& {\▿}_z^b\end{array}\right],$ 为x加速度计误差，为y加速度计误差，为z加速度计误差；ξ_od为里程计误差相关量，ξ_od＝[δK_od,δα_θ，δα_ψ]，δK_od为里程计刻度系数误差，δα_θ为俯仰安装偏角误差，δα_ψ为航向安装偏角误差。

给出状态向量X各项的微分方程，其中姿态误差微分方程如下：

$\stackrel{\·}{\mathrm{\φ}}=-{\mathrm{\ω}}_{in}^n\×\mathrm{\φ}+M_1{\mathrm{\δv}}^n+M_2\mathrm{\δ}P-C_b^n{\mathrm{\ϵ}}_b^b$

其中，为地球自转角速度与载体运动引起的角速度之和。

M₁、M₂计算公式如下：

$M_1=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]$

其中R_M为地球子午圈半径、R_N为地球卯酉圈半径，h为车载的高度。

M₂＝M₂₁+M₂₂

$M_{21}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{ie}\sin L& 0& 0\\ {\mathrm{\ω}}_{ie}\cos L& 0& 0\end{array}\right],M_{22}=\left[\begin{array}{ccc}0& 0& \frac{v_N^n}{{(R_M+h)}^2}\\ 0& 0& -\frac{v_E^n}{{(R_N+h)}^2}\\ \frac{v_E^n{\sec }^{2}L}{R_N+h}& 0& -\frac{v_E^n\tan L}{{(R_N+h)}^2}\end{array}\right]$

速度误差微分方程为：

$\mathrm{\δ}{\stackrel{\·}{v}}^n=f_{sf}^n\×\mathrm{\φ}+M_3{\mathrm{\δv}}^n+M_4\mathrm{\δ}P+C_b^n{\▿}_b^b$

其中，为比力在导航系下的投影，M₃、M₄计算公式如下：

$M_3=(v^n\×)M_1-\left(\right(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×),$ M₄＝(vⁿ×)(2M₂₁+M₂₂)

其中vⁿ为惯组速度，为地球自转角速度在导航系下的投影，为载体运动引起的角速度在导航系下的投影。

位置误差微分方程为：

$\mathrm{\δ}\stackrel{\·}{P}=M_5{\mathrm{\δv}}^n+M_6\mathrm{\δ}P$

上式中M₅、M₆计算公式如下：

$M_5=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{secL}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right],M_6=\left[\begin{array}{ccc}0& 0& -\frac{v_N^n}{{(R_M+h)}^2}\\ \frac{v_E^n\sec L\tan L}{R_N+h}& 0& -\frac{v_E^{n}secL}{{(R_N+h)}^2}\\ 0& 0& 0\end{array}\right]$

得到状态微分方程：

$\stackrel{\·}{X}=F\left(t\right)X+w\left(t\right)$

得到状态转移矩阵F(t)表示如下：

$F\left(t\right)=\left[\begin{array}{cccccc}-{\mathrm{\ω}}_{in}^n\×& M_1& M_2& -C_b^n& 0_{3\×3}& 0_{3\×3}\\ f_{sf}^n\×& M_3& M_4& 0_{3\×3}& C_b^n& 0_{3\×3}\\ 0_{3\×3}& M_5& M_6& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}\end{array}\right]$

系统噪声状态向量 $w\left(t\right)={\left[\begin{array}{ccc}-{\left(C_b^n{\mathrm{\ϵ}}_w^b\right)}^T& {\left(C_b^n{\▿}_w^b\right)}^T& 0_{9\×1}\end{array}\right]}^T,$ 其中为陀螺随机噪声，为陀螺随机噪声，为惯组坐标系与导航坐标系之间的方向余弦矩阵。

上述步骤S3中，根据里程计位移增量与惯组位移增量构造量测量，建立系统量测方程。

首先计算惯组在采样周期内导航系下的位移增量(可优选梯形构造法)为：

${\mathrm{\ΔP}}_i^{INS}=\frac{\mathrm{\Δ}T}{2}(v_i^n+v_{i-1}^n)$

上式中ΔT为惯组采样周期，在采样周期内里程计增量在导航系下的位移增量为：

${\mathrm{\ΔP}}_i^{OD}=C_b^n\left(i\right){\mathrm{\ΔS}}_i^b$

累积预设时间(如1秒)内惯导位移增量与里程计位移增量，并将二者之差作为量测值，即：

$z_{Obs}=\mathrm{\Σ}\mathrm{\Δ}{\hat{P}}_i^{INS}-\mathrm{\Σ}\mathrm{\Δ}{\hat{P}}_i^{OD}$

建立量测方程：

z＝H(t)X+v(t)

上式中量测矩阵H(t)为：

$H\left(t\right)=\left[\begin{array}{cccccc}-\mathrm{\Σ}\left(C_b^n\right(i\left){\mathrm{\ΔS}}_i^b\right)\×& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& -{\mathrm{\Σ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\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\θ}}& 0\end{array}\right]$

上述步骤S4中，对连续状态微分方程离散化，建立卡尔曼滤波器，根据状态方程和量测方程设定系统噪声矩阵Q_k和量测噪声矩阵R_k初值，设定滤波初值X₀和滤波状态误差初值P₀。根据惯组数据和里程计数据实时进行导航计算，将量测值输入卡尔曼滤波器，通过滤波估计后得到各个误差状态的估计值，利用估计值修正导航参数，以修正后导航参数作为下一次导航初值继续进行导航解算，直至导航过程结束。

首先对连续状态微分方程离散化，得到离散形式的状态方程：

X_k+1＝Φ_k+1,kX_k+W_k

其中：

$\left\{\begin{array}{c}{\mathrm{\Φ}}_{k+1,k}=I+{\mathrm{TF}}_k+\frac{T^2}{2!}{F}_k^2+\frac{T^3}{3!}{F}_k^3+L\\ W_k={\∫}_{t_k}^{t_{k+1}}+{\mathrm{\Φ}}_{k+1,\mathrm{\τ}}w\left(\mathrm{\τ}\right)d\mathrm{\τ}\\ F_k=F\left(t_k\right)\end{array}\right.$

上式中T表示卡尔曼滤波离散化周期，这里优选设置为0.1秒。设置卡尔曼滤波初始值，包括滤波初值X₀和滤波状态误差初值P₀，以及系统噪声矩阵Q_k和量测噪声矩阵R_k初值：

X₀＝0

滤波状态误差初值P₀为对角矩阵，对角线元素如下：

噪声矩阵Q_k为对角矩阵，对角线元素如下：

噪声矩阵R_k为对角矩阵，对角线元素如下

Rk(1,1)	(0.1)^2
		Rk(2,2)	(0.1)^2
Rk(3,3)	(0.1)^2

在离散状态方程基础上建立卡尔曼滤波器，推导状态向量更新计算公式，如下：

a)计算状态一步预测

${\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1}$

b)计算一步预测均方差阵P_k/k-1

$P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}$

c)计算滤波增益矩阵K_k

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}$

d)计算状态最优估计

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k(Z_k-H_k{\hat{X}}_{k/k-1})$

e)计算状态估计均方差阵P_k

P_k＝(I-K_kH_k)P_k/k-1

根据惯组数据和里程计数据实时进行导航计算，得到姿态矩阵速度位置将量测值z_Obs输入卡尔曼滤波器，通过滤波估计后得到各个误差状态的估计值，包括姿态误差估计值速度误差估计值位置误差估计值里程计刻度系数误差及安装偏角误差估计值利用估计值修正导航参数，修正方法如下：

${\hat{C}}_b^n=\[I+\left(\widehat{\mathrm{\φ}}\×\right)\]{\hat{C}}_b^n$

${\hat{v}}^n={\hat{v}}^n-\mathrm{\δ}{\hat{v}}^n$

$\left\{\begin{array}{c}\hat{L}=\hat{L}-\mathrm{\δ}\hat{L}\\ \widehat{\mathrm{\λ}}=\widehat{\mathrm{\λ}}-\mathrm{\δ}\widehat{\mathrm{\λ}}\\ \hat{h}=\hat{h}-\mathrm{\δ}\hat{h}\end{array}\right.$

${\hat{K}}_{od}={\hat{K}}_{od}-\mathrm{\δ}{\hat{K}}_{od}$

${\widehat{\mathrm{\α}}}_{\mathrm{\θ}}={\widehat{\mathrm{\α}}}_{\mathrm{\θ}}-\mathrm{\δ}{\widehat{\mathrm{\α}}}_{\mathrm{\θ}}$

$\widehat{\mathrm{\α}}=\widehat{\mathrm{\α}}-\mathrm{\δ}{\widehat{\mathrm{\α}}}_{\mathrm{\ψ}}$

以反馈校正后的导航参数作为下一次导航初值，继续进行导航解算，直至导航结束。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (2)

1.一种激光惯组与里程计组合导航方法，其特征在于，所述方法包括以下步骤：

S1、采集正常工作状态下第i个采样周期内里程计脉冲数N_i，求解惯组与里程计之间的俯仰安装偏角α_θ和航向安装偏角α_ψ，

所述为矢量第j个分量，j＝1,2,3；为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵，为预设时间内惯组导航解算的位移增量，K_od为里程计脉冲当量；

S2、建立状态方程其中，F为状态转移矩阵，w为系统噪声， $X={\left[\begin{array}{cccccc}{\left(\mathrm{\φ}\right)}^T& {\left({\mathrm{\δv}}^n\right)}^T& {\left(\mathrm{\δ}P\right)}^T& {\left({\mathrm{\ϵ}}_b^b\right)}^T& {\left({\▿}_b^b\right)}^T& {{\mathrm{\ξ}}_{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}-\mathrm{\Σ}\left(C_b^n\left(i\right){\mathrm{\ΔS}}_i^b\right)\×& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& -{\mathrm{\Σ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\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\θ}}& 0\end{array}\right];$

S4、利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。

2.一种激光惯组与里程计组合导航系统，其特征在于，所述系统包括：

第一子模块，用于采集正常工作状态下第i个采样周期内里程计脉冲数N_i，求解惯组与里程计之间的俯仰安装偏角α_θ和航向安装偏角α_ψ，

所述为矢量第j个分量，j＝1,2,3；为第i个采样周期内惯组坐标系与导航坐标系之间的方向余弦矩阵，为预设时间内惯组导航解算的位移增量，K_od为里程计脉冲当量；

第二子模块，用于建立状态方程其中，F为状态转移矩阵，w为系统噪声， $X={\left[\begin{array}{cccccc}{\left(\mathrm{\φ}\right)}^T& {\left({\mathrm{\δv}}^n\right)}^T& {\left(\mathrm{\δ}P\right)}^T& {\left({\mathrm{\ϵ}}_b^b\right)}^T& {\left({\▿}_b^b\right)}^T& {{\mathrm{\ξ}}_{od}}^T\end{array}\right]}^T,$ φ为惯导姿态误差，δvⁿ为惯导速度误差，δP为惯导位置误差，为陀螺漂移误差；为加速度计偏置误差，里程计误差相关量ξ_od＝[δK_od,δα_θ，δα_ψ]，其中δK_od为里程计刻度系数误差，δα_θ为俯仰安装偏角误差，δα_ψ为航向安装偏角误差；

第三子模块，用于建立量测方程z＝HX+v，其中v为量测噪声；量测量累积预设采样周期内惯导位移增量与里程计位移增量，并将二者之差作为量测量z，所述为惯组在第i个采样周期内导航系下的位移增量，为第i个采样周期内里程计增量在导航系下的位移投影；量测矩阵 $H=\left[\begin{array}{cccccc}-\mathrm{\Σ}\left(C_b^n\left(i\right){\mathrm{\ΔS}}_i^b\right)\×& I_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& -{\mathrm{\Σ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\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}& -{\mathrm{cos\α}}_{\mathrm{\ψ}}{\mathrm{sin\α}}_{\mathrm{\θ}}& -{\mathrm{sin\α}}_{\mathrm{\ψ}}{\mathrm{cos\α}}_{\mathrm{\θ}}\\ {\mathrm{sin\α}}_{\mathrm{\θ}}& {\mathrm{cos\α}}_{\mathrm{\θ}}& 0\end{array}\right];$

第四子模块，用于利用所述状态方程和量测方程，进行卡尔曼滤波，以实时修正惯性导航系统参数误差和器件参数误差，完成组合导航。