CN103743414B - 一种里程计辅助车载捷联惯导系统行进间初始对准方法 - Google Patents

Abstract

本发明公开一种里程计辅助车载捷联惯导系统行进间初始对准方法，包括粗对准和精对准两部分。本发明可以有效地解决车辆行进间初始对准问题，既不需要车辆作特殊机动或停车，也不需要借助路标点等外界辅助信息，仅靠里程计就能辅助捷联惯导系统实现快速高精度对准。

Description

一种里程计辅助车载捷联惯导系统行进间初始对准方法

技术领域

本发明属于惯性导航初始对准技术领域，具体涉及一种里程计辅助车载捷联惯导系统行进间初始对准方法。

背景技术

车载捷联惯导系统进入导航工作之前需要进行初始对准，精确性和快速性是初始对准的两个重要性能指标。现代战争中为了增强战车的生存能力，要求车载捷联惯导系统能够在运动状态下完成初始对准，这对于提高车辆快速机动能力具有重要意义。车辆运动时受运动加速度干扰影响，在短时间内完成初始对准一直是初始对准领域研究的热点之一。车辆行进间对准通常需要利用外部设备（在陆用导航领域，通常使用GPS或里程计）提供载体运动信息实现对惯性导航系统的补偿与修正。GPS信号容易受遮挡且战时不可利用，里程计能够测量车辆在地面行驶速度和距离，具有完全自主和信号不易受干扰的特点，因而被广泛采用。

车载捷联惯导系统中，为了提高车辆快速机动能力，发动机开启后惯导系统要能够迅速进入导航工作状态。当车辆受到晃动干扰（如人员走动、货物卸载等会造成车体发生较为明显的晃动），参考矢量（特别是地球自转角速度）的测量精度会明显下降。传统解析式初始对准算法在这种条件下对准精度会受到影响，甚至无法完成粗对准。

发明内容

发明目的：为解决现有技术中存在的不足，本发明提供了一种里程计辅助车载捷联惯导系统行进间初始对准方法。

技术方案：本发明的一种里程计辅助车载捷联惯导系统行进间初始对准方法，依次包括粗对准模块和精对准模块两部分，

所述的粗对准模块包括以下步骤：

（11）车辆发动机开启、人员上下车辆做准备工作，惯性测量单元实时地同步采集车辆角速率和比力信息；

（12）运行系统程序，开始进行快速粗对准；

（13）粗对准结束后，得到捷联惯导系统初始姿态矩阵，车辆开始启动，里程计实时采集车辆运动信息；

所述的精对准模块包括以下步骤：

（21）捷联惯导系统利用粗对准得到的姿态矩阵进行速度、位置以及姿态导航解算，同时里程计根据捷联惯导系统提供的初始车辆姿态矩阵进行航位推算，计算得到较为准确的速度、姿态信息；

（22）将里程计航位推算得到的速度、姿态信息作为量测基准，与捷联惯导解算得到的速度、姿态相减作差，以所得差值作为卡尔曼滤波的量测值，列写卡尔曼滤波的状态方程和量测方程；

（23）将状态方程和量测方程离散化，根据给定状态初值以及估计误差初值利用导航计算机递推得到任意时刻的状态估计值，反馈修正捷联惯导系统，完成初始对准。

进一步的，所述粗对准模块应用惯性凝固假设，将比力信息投影到载体惯性坐标系中，得到重力加速度相对惯性空间随地球旋转而引起的方向变化信息，对其积分可以克服车辆发动机振动以及人员上下车等引起的随机干扰影响，求解得到粗略姿态矩阵

进一步的，所述精对准模块应用卡尔曼滤波估计系统误差，里程计与捷联惯导系统中的陀螺仪进行航位推算，将航位推算得到的车辆速度和姿态信息作为卡尔曼滤波量测基准，与捷联惯导解算得到的速度、姿态相减，误差值作为卡尔曼滤波器输入，估计得到系统误差后反馈修正捷联惯导系统，求解得到精确姿态矩阵

进一步的，所述步骤（22）中的卡尔曼滤波的状态方程为：

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

其中，t为时间变量，F(t)是系统状态转移矩阵，W(t)是系统过程噪声序列，状态向量取为 $X={\left[\begin{array}{cccccccc}\mathrm{\δ}{p}^T& \mathrm{\δ}{v}^{\mathrm{nT}}& {\mathrm{\φ}}^T& {\mathrm{\ϵ}}_g^{\mathrm{bT}}& {\▿}_a^{\mathrm{bT}}& \mathrm{\δ}{p}_D^T& {\mathrm{\φ}}_D^T& \mathrm{\δ}{k}_D\end{array}\right]}^T,$ 状态向量中对应的变量分别为捷联惯导位置误差向量δp、速度误差向量δvⁿ、数学平台误差向量φ、陀螺仪常值漂移向量加速度计常值偏置向量里程计航位推算位置误差向量δp_D、航位推算数学平台误差向量φ_D和里程计刻度系数误差δk_D；

$\left[\begin{array}{c}\mathrm{\δ}\stackrel{\·}{p}\\ \mathrm{\δ}{\stackrel{\·}{v}}^n\\ \stackrel{\·}{\mathrm{\φ}}\\ \stackrel{\·}{\mathrm{\ϵ}}\\ \stackrel{\·}{\▿}\\ \mathrm{\δ}{\stackrel{\·}{p}}_D\\ {\stackrel{\·}{\mathrm{\φ}}}_D\\ \mathrm{\δ}{\stackrel{\·}{k}}_D\end{array}\right]=\left[\begin{array}{cccccccc}{F}_{11}& F_{12}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ F_{21}& F_{22}& F_{23}& 0_{3\×3}& F_{24}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ F_{31}& F_{32}& F_{33}& F_{34}& 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& 0_{3\×1}\\ 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}& F_{41}& F_{42}& F_{43}\\ 0_{3\×3}& 0_{3\×3}& 0_{3\×3}& F_{51}& 0_{3\×3}& F_{52}& F_{53}& F_{54}\\ 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×3}& 0_{1\×1}\end{array}\right]\left[\begin{array}{c}\mathrm{\δp}\\ \mathrm{\δ}{v}^n\\ \mathrm{\φ}\\ \mathrm{\ϵ}\\ \▿\\ \mathrm{\δ}{p}_D\\ {\mathrm{\φ}}_D\\ \mathrm{\δ}{k}_D\end{array}\right]+\left[\begin{array}{c}{0}_{3\×1}\\ w_a\\ w_g\\ 0_{3\×1}\\ 0_{3\×1}\\ 0_{3\×1}\\ w_g\\ 0_{1\×1}\end{array}\right]$

式中，w_a、w_g分别为加速度计和陀螺仪在载体坐标系下的噪声，即是均值为0、方差为Q(t)、呈正态分布的白噪声；

$F_{11}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{v_E\tan L}{R_N+h}\sec L& 0& 0\\ 0& 0& 0\end{array}\right],F_{12}=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{21}=\left[\begin{array}{ccc}2{\mathrm{\ω}}_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_u& 0& 0\\ -(2{\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E& 0& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L{v}_E& 0& \frac{2g}{R}\end{array}\right]$

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

$F_{23}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right],F_{24}=C_b^n$

$F_{31}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ ({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)& 0& 0\end{array}\right],F_{32}=\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]$

$F_{33}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h})& 0& -\frac{v_N}{R_M+h}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}& \frac{v_N}{R_M+h}& 0\end{array}\right]$

$F_{34}=-C_b^n$

$F_{41}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{v_{\mathrm{DE}}\tan L_D}{R_N+h}\sec L_D& 0& 0\\ 0& 0& 0\end{array}\right],F_{42}=\left[\begin{array}{ccc}\frac{v_{\mathrm{DU}}}{R_M+h_D}& 0& -\frac{v_{\mathrm{DE}}}{R_M+h_D}\\ 0& -\frac{\sec L_D}{R_N+h_D}{v}_{\mathrm{DU}}& \frac{\sec L_D}{R_N+h_D}{v}_{\mathrm{DN}}\\ -v_{\mathrm{DN}}& v_{\mathrm{DE}}& 0\end{array}\right]$

$F_{43}={\left[\begin{array}{ccc}\frac{v_{\mathrm{DN}}}{R_M+h_D}& \frac{\sec L_D}{R_N+h_D}{v}_{\mathrm{DE}}& v_{\mathrm{DU}}\end{array}\right]}^T$

$F_{51}=-C_{\mathrm{Db}}^n,F_{52}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L_D& 0& 0\\ ({\mathrm{\ω}}_{\mathrm{ie}}\cos L_D+\frac{v_{\mathrm{DE}}{\sec }^2{L}_D}{R_N+h_D})& 0& 0\end{array}\right]$

$F_{53}=[M(v_D^n\×)-({\mathrm{\ω}}_{\mathrm{in}}^n\×)],M=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h_D}& 0\\ \frac{1}{R_N+h_D}& 0& 0\\ \frac{\tan L_D}{R_N+h_D}& 0& 0\end{array}\right]$

$F_{54}={\left[\begin{array}{ccc}-\frac{v_{\mathrm{DN}}}{R_M+h_D}& \frac{v_{\mathrm{DE}}}{R_N+h_D}& \frac{v_{\mathrm{DE}}}{R_N+h_D}\tan L_D\end{array}\right]}^T$

式中，v_E、v_N、v_U分别表示捷联解算得车辆东向、北向、天向速度值；L表示捷联解算得车辆所在点纬度值；h表示捷联解算得车辆所在点高度值；R_M和R_N分别表示子午面曲率半径和卯酉圈曲率半径；ω_ie表示地球自转角速度；v_DE、v_DN、v_DU分别表示航位推算得车辆东向、北向、天向速度值；L_D表示航位推算得车辆所在点纬度值；h_D表示航位推算得车辆所在点高度值；表示捷联惯导姿态更新矩阵；表示航位推算姿态更新矩阵；

f_E为加速度计所测比力分量在导航坐标系中东向投影分量；

f_N为加速度计所测比力分量在导航坐标系中北向投影分量；

f_U为加速度计所测比力分量在导航坐标系中天向投影分量。

进一步的，所述步骤（22）中的卡尔曼滤波的量测方程的计算过程如下：

利用航位推算获得的车辆速度、姿态辅助捷联惯导系统完成精对准。将捷联惯导输出的速度、姿态与航位推算获得的对应信息相减作为辅助对准的量测，即：

Z=[ψ_S-ψ_D,θ_S-θ_D,γ_S-γ_D,v_SE-v_DE,v_SN-v_DN,v_SU-v_DU]^T

式中，ψ_S、θ_S和γ_S分别为捷联惯导输出车辆航向角、俯仰角和横摇角，ψ_D、θ_D和γ_D分别为里程计航位推算输出航向角、俯仰角和横摇角，v_SE、v_SN和v_SU分别为捷联惯导输出车辆速度，v_DE、v_DN和v_DU分别为里程计输出车辆速度；

根据捷联惯导、航位推算的姿态角误差与数学平台失准角之间关系，可以推导获得量测方程为：

$Z=\left[\begin{array}{c}\mathrm{\δ}{v}_E-v_{\mathrm{DN}}{\mathrm{\φ}}_{\mathrm{DU}}+v_{\mathrm{DU}}{\mathrm{\φ}}_{\mathrm{DN}}-v_{\mathrm{DE}}\mathrm{\δ}{k}_D\\ \mathrm{\δ}{v}_N+v_{\mathrm{DE}}{\mathrm{\φ}}_{\mathrm{DU}}-v_{\mathrm{DU}}{\mathrm{\φ}}_{\mathrm{DE}}-v_{\mathrm{DN}}\mathrm{\δ}{k}_D\\ \mathrm{\δ}{v}_U-v_{\mathrm{DE}}{\mathrm{\φ}}_{\mathrm{DN}}+v_{\mathrm{DN}}{\mathrm{\φ}}_{\mathrm{DE}}-v_{\mathrm{DU}}\mathrm{\δ}{k}_D\\ -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}{\mathrm{\φ}}_E-\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}{\mathrm{\φ}}_N+{\mathrm{\φ}}_U+\frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}{\mathrm{\φ}}_{\mathrm{DE}}+\frac{R_{32}+R_{22}}{R_{12}^2+R_{12}^2}{\mathrm{\φ}}_{\mathrm{DN}}-{\mathrm{\φ}}_{\mathrm{DU}}\\ -\frac{T_{22}}{\sqrt{1-T_{32}^2}}{\mathrm{\φ}}_E+\frac{T_{12}}{\sqrt{1-T_{32}^2}}{\mathrm{\φ}}_N+\frac{R_{22}}{\sqrt{1-R_{32}^2}}{\mathrm{\φ}}_{\mathrm{DE}}-\frac{R_{12}}{\sqrt{1-R_{32}^2}}{\mathrm{\φ}}_{\mathrm{DN}}\\ \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}{\mathrm{\φ}}_E+\frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}{\mathrm{\φ}}_N-\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}{\mathrm{\φ}}_{\mathrm{DE}}-\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}{\mathrm{\φ}}_{\mathrm{DN}}\end{array}\right]$

结合系统状态X，可将辅助对准的量测方程写成如下形式：

Z(t)=H(t)X(t)+V(t)

$H\left(t\right)=\left[\begin{array}{cccccccccccc}& 1& 0& 0& 0& 0& 0& & 0& v_{\mathrm{DU}}& -v_{\mathrm{DN}}& -v_{\mathrm{DE}}\\ & 0& 1& 0& 0& 0& 0& & -v_{\mathrm{DU}}& 0& v_{\mathrm{DE}}& -v_{\mathrm{DN}}\\ & 0& 0& 1& 0& 0& 0& & v_{\mathrm{DN}}& -v_{\mathrm{DE}}& 0& -v_{\mathrm{DU}}\\ 0_{6\×3}& 0& 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}& 1& 0_{6\×9}& \frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}& \frac{R_{32}{R}_{22}}{R_{12}^2+R_{22}^2}& -1& 0\\ & 0& 0& 0& -\frac{T_{22}}{\sqrt{1-T_{32}^2}}& \frac{T_{12}}{\sqrt{1-T_{32}^2}}& 0& & \frac{R_{22}}{\sqrt{1-R_{32}^2}}& -\frac{R_{12}}{\sqrt{1-R_{32}^2}}& 0& 0\\ & 0& 0& 0& \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}& \frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}& 0& & -\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}& -\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}& 0& 0\end{array}\right]$

其中，H(t)表示量测矩阵，可以由状态方程和量测方程之间关系直接求得，V(t)为量测噪声向量，其方差为R(t)，记捷联惯导姿态更新矩阵里程计航位推算更新矩阵 $C_{\mathrm{Db}}^n={\left[R_{\mathrm{ij}}\right]}_{3\×3}.$

有益效果：本发明的一种里程计辅助车载捷联惯导行进间初始对准方法，与现有技术相比具有以下优点：

（1）以惯性空间中不同时刻的重力加速度作为参考矢量的解析粗对准算法，对晃动干扰有着较好的适应性，本发明能够克服车辆晃动等引起的角运动干扰影响，通过短时间静止可以迅速建立粗略姿态矩阵缩短系统准备时间。

（2）由于捷联惯导系统速度误差随时间不断积累，捷联姿态矩阵误差会随导航时间增加而变差，里程计速度测量误差稳定，将里程计与陀螺仪进行航位推算，可以得到更加精确的姿态矩阵，本发明利用航位推算得到的速度和姿态作为卡尔曼滤波量测基准信息，和传统的以速度误差为量测的卡尔曼滤波方法相比，此方法的对准精度更高。

（3）由于作为速度、姿态基座的信息更为准确，和传统的以速度误差为量测的卡尔曼滤波方法相比，滤波收敛速度更快，有效地缩短了对准时间。

综上所述，本发明可以有效地解决车辆行进间初始对准问题，既不需要车辆作特殊机动或停车，也不需要借助路标点等外界辅助信息，仅靠里程计就能辅助捷联惯导系统实现快速高精度对准，具有自主性高、抗干扰性强、机动性好等突出优点，对于战场环境下武器系统的快速反应、机动作战具有重要意义。

附图说明

图1为本发明中的行进间初始对准流程图；

图2为本发明的粗对准原理图；

图3为本发明的精对准结构图；

图4为初始对准效果图，其中实线代表本方法，点线代表传统的速度组合卡尔曼滤波方法。

具体实施方式

下面对本发明技术方案结合附图和实施例进行详细说明。

如图1至图4所示，本发明的一种里程计辅助车载捷联惯导系统行进间初始对准方法，包括以下步骤：车辆发动机开启，人员上下车辆做准备工作，惯性测量单元实时同步采集角速率信息和比力信息；启动系统程序，开始进行粗对准；40s粗对准结束后，发动车辆开始运动，利用得到的粗略姿态矩阵以及采集得到的里程计路程增量信息进行航位推算，得到车辆速度、姿态信息，同时捷联惯导系统利用陀螺仪和加速度计信息进行速度、位置以及姿态等导航解算；里程计航位推算得到的车辆速度、姿态信息作为卡尔曼滤波器量测基准，与捷联惯导解算出的速度、姿态信息相减作差，以此差值作为卡尔曼滤波器量测值，进行卡尔曼滤波估计得到捷联惯导系统速度、姿态误差，通过闭环反馈修正捷联惯导误差，得到准确的车辆姿态信息，完成初始对准任务。

本发明具体包括以下步骤：

步骤一、开启发动机，车辆进入启动准备阶段，此时捷联惯导系统进入工作状态，实时同步地采集三轴陀螺仪的角速度信息和三轴加速度计的比力信息，完成车载捷联惯导系统的预热准备工作后车辆继续静止40s，进行粗对准；

步骤二、为了消除振动、晃动等引起的干扰加速度影响，选用基于重力加速度解析粗对准方法，完成粗对准，得到粗略姿态矩阵；

步骤三、此方法应用惯性凝固假设，将比力信息投影到载体惯性坐标系中，得到重力加速度相对于惯性空间随地球旋转而引起的方向变化信息，将其积分，消除部分干扰加速度，求解得到粗略姿态矩阵；

步骤三中的具体方法如下：

定义3个新的坐标系：经线地球坐标系(e₀系)、经线地心惯性坐标系(i₀系)和载体惯性坐标系

经线地球坐标系(e₀系)，轴与地球自转轴重合，轴在赤道平面内指向对准开始时刻惯组所在点经线，轴在赤道平面内，轴构成右手坐标系，与地球固连，随地球一同转动；

经线地心惯性坐标系(i₀系)，该坐标系是在粗对准的起始时刻将经线地球坐标系在惯性空间凝固后形成的坐标系；

载体惯性坐标系该坐标系是在粗对准的起始时刻将载体坐标系在惯性空间凝固后形成的坐标系；

捷联姿态矩阵可表示为：式中，为经线地球坐标系e₀到导航坐标系n的转换矩阵，可由车辆所在地地理位置精确求得，为经线惯性坐标系i₀到经线地球坐标系e₀的转换矩阵，该矩阵为时间t的函数，为载体坐标系b到载体惯性坐标系的转换矩阵，可利用陀螺仪输出通过姿态跟踪算法实时求解，为载体惯性坐标系与经线地心惯性坐标系i₀之间的转换矩阵，该矩阵不随时间变化且与载体的运动状态无关，为一常值矩阵，因此可以通过在这两个惯性空间内分别测量两个不共线矢量来求取，该矩阵的求取是此算法的关键之处。

1)矩阵和矩阵的求解

矩阵为e₀系到n系的转换矩阵。可由对准过程中车辆所在地纬度精确求得；

$C_{e_0}^n=\left[\begin{array}{ccc}0& 1& 0\\ -\sin L& 0& \cos L\\ \cos L& 0& \sin L\end{array}\right]$

矩阵为i₀系到e₀系的转换矩阵，可由对准时间精确求得，t₀表示对准初始时刻点；

$C_{i_0}^{e_0}=\left[\begin{array}{ccc}\cos \left[{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)\right]& \sin \left[{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)\right]& 0\\ -\sin \left[{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)\right]& \cos \left[{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)\right]& 0\\ 0& 0& 1\end{array}\right]$

2)矩阵的求解

由系的定义知对准开始时刻系与b系重合，即因此可以利用陀螺仪输出的b系相对系的角速度信息，通过四元数更新求解。

3)矩阵的求解

矩阵为系到i₀系的转换矩阵，该矩阵是常值矩阵。在惯性坐标系中观察地球重力加速度时，由于地球自转的作用其运动轨迹构成一个圆锥面。所以在不同时刻重力矢量在惯性坐标系内是不共线的。只要求得两个不同时刻的以及与其相对应的即可根据双矢量定姿原理求取矩阵。

车辆静止时，存在发动机振动以及人员等干扰影响。此时加速度计的输出在载体惯性坐标内的投影为：

${\tilde{f}}^{i_{b_0}}=C_b^{i_{b_0}}{\tilde{f}}^b---\left(1\right)$

式中，加速度计输出比力包括三部分：重力加速度g^b，车辆发动机振动等引起的干扰加速度加速度计零位偏置，即

${\tilde{f}}^b=-g^b+a_D^b+{\▿}^b---\left(2\right)$

式(2)中加速度计零位偏置很小，可以忽略这部分影响，在Δt_k=t_k-t₀内对式(1)进行积分得：

${\∫}_{t_0}^{t_k}{\tilde{f}}^{i_{b_0}}\mathrm{dt}={\∫}_{t_0}^{t_k}{C}_b^{i_{b_0}}{\tilde{f}}^b\mathrm{dt}---\left(3\right)$

记 ${\tilde{V}}^{i_{b_0}}={\∫}_{t_0}^{t_k}{\tilde{f}}^{i_{b_0}}\mathrm{dt},$ 将式(2)带入式(3)中有：

$\begin{array}{c}{\tilde{V}}^{i_{b_0}}={\∫}_{t_0}^{t_k}(-C_b^{i_{b_0}}{g}^b+C_b^{i_{b_0}}{a}_D^b)\mathrm{dt}\\ ={\∫}_{t_0}^{t_k}(-C_{i_0}^{i_{b_0}}{C}_b^{i_0}{g}^b+C_b^{i_{b_0}}{a}_D^b)\mathrm{dt}\\ ={\∫}_{t_0}^{t_k}(-C_{i_0}^{i_{b_0}}{g}^{i_0}+C_b^{i_{b_0}}{a}_D^b)\mathrm{dt}\end{array}---\left(4\right)$

当车辆存在发动机振动引起的环境干扰时，一般可视这种干扰误差为周期性变化量，在较长一段时间内这种干扰加速度正负积分相互抵消，于是：

${\∫}_{t_0}^{t_k}{C}_b^{i_{b_0}}{a}_D^b\mathrm{dt}=0---\left(5\right)$

记 $V^i={\∫}_{t_0}^{t_k}-g^{i_0}\mathrm{dt},$ 式(4)可以简化为：

${\tilde{V}}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{V}^{i_0}---\left(6\right)$

由于

$g^{i_0}=\left[\begin{array}{c}-g\cos L\cos \left[{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)\right]\\ -g\cos L\sin \left[{\mathrm{\ω}}_{\mathrm{ie}}(t-t_0)\right]\\ -g\sin L\end{array}\right]---\left(7\right)$

在Δt_k=t_k-t₀内，对式(7)进行积分得：

$V^i=\left[\begin{array}{c}\frac{g\cos L\sin \left[{\mathrm{\ω}}_{\mathrm{ie}}(t_k-t_0)\right]}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ -\frac{g\cos L\left\{\cos \right[{\mathrm{\ω}}_{\mathrm{ie}}(t_k-t_0)]-1\}}{{\mathrm{\ω}}_{\mathrm{ie}}}\\ g\sin L(t_k-t_0)\end{array}\right]---\left(8\right)$

可以根据加速度计输出积分求得，在式(6)中取两个不同时刻，t₁时刻和t₂时刻(t₁<t₂)，由双矢量定姿原理得：

$\begin{array}{c}{\tilde{V}}_{t_1}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{V}_{t_1}^{i_0}\\ {\tilde{V}}_{t_2}^{i_{b_0}}=C_{i_0}^{i_{b_0}}{V}_{t_2}^{i_0}\end{array}---\left(9\right)$

式(9)中两向量作叉乘，得：

${\tilde{V}}_{t_1}^{i_{b_0}}\&times;{\tilde{V}}_{t_2}^{i_{b_0}}=C_{i_0}^{i_{b_0}}(V_{t_1}^{i_0}\&times;V_{t_2}^{i_0})---\left(10\right)$

因此，可按照式(11)求解：

$C_{i_{b_0}}^{i_0}={\left[\begin{array}{c}{\left(V_{t_1}^{i_0}\right)}^T\\ {\left(V_{t_2}^{i_0}\right)}^T\\ {(V_{t_1}^{i_0}\&times;V_{t_2}^{i_0})}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left({\tilde{V}}_{t_1}^{i_{b_0}}\right)}^T\\ {\left({\tilde{V}}_{t_2}^{i_{b_0}}\right)}^T\\ {({\tilde{V}}_{t_1}^{i_{b_0}}\&times;{\tilde{V}}_{t_2}^{i_{b_0}})}^T\end{array}\right]---\left(11\right)$

该算法巧妙地应用惯性凝固假设，建立了载体坐标系使车体相对坐标系的姿态阵初值成为单位矩阵，从而使更新成为可能。通过积分可以将车辆振动引起的线速度平滑掉，能够取得比较好的对准效果，航向角误差基本能够控制在2°范围以内，满足粗对准精度要求；

步骤四、粗对准结束后，得到粗略姿态矩阵，启动车辆，利用里程计输出路程增量以及陀螺仪输出角速率进行航位推算。与捷联惯导相比，航位推算时载体系运动速度为直接测量值，其误差有界且能保证较高的精度，从长期效果来看航位推算的速度误差发散速度远小于惯导系统，导航精度更高。

下面给出航位推算姿态更新算法：

由于惯导速度误差随时间不断积累，捷联姿态矩阵误差会随导航时间增加而变差。里程计测量误差比较稳定，利用里程计与捷联惯导系统中的陀螺仪进行高精度航位推算可以得到更加精确的姿态矩阵，从而可以提高航位推算精度；

与捷联惯导姿态四元数更新算法类似，航位推算的姿态四元数更新算法为：

$q_{\mathrm{Dbk}}^n=q_{\mathrm{Dbk}-1}^n\&CenterDot;q_{\mathrm{Dbk}}^{\mathrm{bk}-1}---\left(12\right)$

式中：是t_k时刻的姿态四元数，是t_k-1时刻的姿态四元数，是以n系为参考坐标系时b系从t_k-1时刻到t_k时刻的变换四元数，它的计算需要用到b系相对于n系的转动角速度设η_k是从t_k-1时刻到t_k时刻b系相对于n系的角速度变换所对应的等效旋转矢量，则有：

${\mathrm{\&eta;}}_k={\&Integral;}_{t_{k-1}}^{t_k}{\mathrm{\&omega;}}_{\mathrm{nb}}^b\left(\mathrm{\&tau;}\right)\mathrm{d\&tau;}={\&Integral;}_{t_{k-1}}^{t_k}\{{\mathrm{\&omega;}}_{\mathrm{ib}}^b\left(t\right)-{\left[C_{\mathrm{Db}}^n\left(t\right)\right]}^T{\mathrm{\&omega;}}_{\mathrm{in}}^n\left(t\right)\}\mathrm{dt}---\left(13\right)$

式中：利用里程计的输出进行姿态更新与捷联惯导姿态更新的主要不同之处在于的计算。设航位推算的速度为 $v_D^n={\left[\begin{array}{ccc}{v}_{\mathrm{DE}}^n& v_{\mathrm{DN}}^n& v_{\mathrm{DU}}^n\end{array}\right]}^T,$ 航位推算纬度和高度分别为L_D和h_D，则的计算公式为：

${\mathrm{\&omega;}}_{\mathrm{ie}}^n{\left[\begin{array}{ccccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}& \cos L_D& {\mathrm{\&omega;}}_{\mathrm{ie}}& \sin L_D\end{array}\right]}^T$

${\mathrm{\&omega;}}_{\mathrm{en}}^n={\left[\begin{array}{ccc}-\frac{v_{\mathrm{DN}}^n}{R_M+h_D}& \frac{v_{\mathrm{DE}}^n}{R_N+h_D}& \frac{v_{\mathrm{DE}}^n\tan L_D}{R_N+h_D}\end{array}\right]}^T$

下面给出航位推算速度、位置更新算法：

假设里程计安装坐标系与捷联惯导坐标重合（安装误差已经补偿），根据里程计输出得到载体系下载车速度，利用航位推算的姿态矩阵将其转换到导航坐标系下，可推导获得速度解算算法：

$v_{\mathrm{DK}}^n=C_{\mathrm{Db}\left(k\right)}^n{\left[\begin{array}{ccc}0& \mathrm{\&Delta;}{S}_k/T_D& 0\end{array}\right]}^T,(k=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;)---\left(14\right)$

式中：为第k时刻导航系中载体速度，ΔS_k为第k时刻里程计测得的位移增量。

利用姿态矩阵将里程计输出的路程增量由载体坐标系转换到导航坐标系，即采用路程增量变换法，推导获得位置解算算法：

$\left\{\begin{array}{c}{L}_{\mathrm{Dk}}=L_{\mathrm{Dk}-1}+\frac{\mathrm{\&Delta;}{S}_{\mathrm{DNk}}^n}{R_M+h_{\mathrm{Dk}-1}}\\ {\mathrm{\&lambda;}}_{\mathrm{Dk}}={\mathrm{\&lambda;}}_{\mathrm{Dk}-1}+\frac{\mathrm{\&Delta;}{S}_{\mathrm{DEk}}^n\sec L_{\mathrm{Dk}-1}}{R_N+h_{\mathrm{Dk}-1}}\\ h_{\mathrm{Dk}}=h_{\mathrm{Dk}-1}+\mathrm{\&Delta;}{S}_{\mathrm{DUk}}^n\end{array}\right.(k=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;)---\left(15\right)$

式中：L_Dk-1、λ_Dk-1和h_Dk-1以及L_Dk、λ_Dk和h_Dk分别为k-1时刻和k时刻航位推算的纬度、经度和高度；

步骤五、导航计算机根据陀螺仪和加速计敏感的角速度和加速度进行捷联解算，分别得到车辆的速度与姿态，将捷联解算得到的车辆速度、姿态信息与里程计航位推算得到的速度、姿态信息相减作为卡尔曼滤波器的量测信息；

步骤六、以此差值作为卡尔曼滤波的量测量，列写卡尔曼滤波的状态方程和量测方程；

1)状态方程

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

其中，t为时间变量，F(t)是系统状态转移矩阵，W(t)是系统过程噪声序列，状态向量取为 $X={\left[\begin{array}{cccccccc}\mathrm{\&delta;}{p}^T& \mathrm{\&delta;}{v}^{\mathrm{nT}}& {\mathrm{\&phi;}}^T& {\mathrm{\&epsiv;}}_g^{\mathrm{bT}}& {\&dtri;}_a^{\mathrm{bT}}& \mathrm{\&delta;}{p}_D^T& {\mathrm{\&phi;}}_D^T& \mathrm{\&delta;}{k}_D\end{array}\right]}^T,$ 状态向量中对应的变量分别为捷联惯导位置误差向量δp、速度误差向量δvⁿ、数学平台误差向量φ、陀螺仪常值漂移向量加速度计常值偏置向量里程计航位推算位置误差向量δp_D、航位推算数学平台误差向量φ_D和里程计刻度系数误差δk_D；

$\left[\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{p}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n\\ \stackrel{\&CenterDot;}{\mathrm{\&phi;}}\\ \stackrel{\&CenterDot;}{\mathrm{\&epsiv;}}\\ \stackrel{\&CenterDot;}{\&dtri;}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{p}}_D\\ {\stackrel{\&CenterDot;}{\mathrm{\&phi;}}}_D\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{k}}_D\end{array}\right]=\left[\begin{array}{cccccccc}{F}_{11}& F_{12}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;1}\\ F_{21}& F_{22}& F_{23}& 0_{3\&times;3}& F_{24}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;1}\\ F_{31}& F_{32}& F_{33}& F_{34}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;1}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& F_{41}& F_{42}& F_{43}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& F_{51}& 0_{3\&times;3}& F_{52}& F_{53}& F_{54}\\ 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;1}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;p}\\ \mathrm{\&delta;}{v}^n\\ \mathrm{\&phi;}\\ \mathrm{\&epsiv;}\\ \&dtri;\\ \mathrm{\&delta;}{p}_D\\ {\mathrm{\&phi;}}_D\\ \mathrm{\&delta;}{k}_D\end{array}\right]+\left[\begin{array}{c}{0}_{3\&times;1}\\ w_a\\ w_g\\ 0_{3\&times;1}\\ 0_{3\&times;1}\\ 0_{3\&times;1}\\ w_g\\ 0_{1\&times;1}\end{array}\right]$

式中，w_a、w_g分别为加速度计和陀螺仪在载体坐标系下的噪声，是均值为0、方差为Q(t)、呈正态分布的白噪声；

$F_{11}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{v_E\tan L}{R_N+h}\sec L& 0& 0\\ 0& 0& 0\end{array}\right],F_{12}=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]$

$F_{21}=\left[\begin{array}{ccc}2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L{v}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L{v}_u& 0& 0\\ -(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E& 0& 0\\ -2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L{v}_E& 0& \frac{2g}{R}\end{array}\right]$

$F_{22}=\left[\begin{array}{ccc}\frac{v_N}{R_M+h}\tan L-\frac{v_U}{R_M+h}& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L& -(2{\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\\ -(2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L)& -\frac{v_U}{R_M+h}& -\frac{v_N}{R_M+h}\\ 2({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})& 2\frac{v_N}{R_M+h}& 0\end{array}\right]$

$F_{23}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right],F_{24}=C_b^n$

$F_{31}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L& 0& 0\\ ({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)& 0& 0\end{array}\right],F_{32}=\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]$

$F_{33}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h}\tan L& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{v_E}{R_N+h})& 0& -\frac{v_N}{R_M+h}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{v_E}{R_N+h}& \frac{v_N}{R_M+h}& 0\end{array}\right]$

$F_{34}=-C_b^n$

$F_{41}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{v_{\mathrm{DE}}\tan L_D}{R_N+h}\sec L_D& 0& 0\\ 0& 0& 0\end{array}\right],F_{42}=\left[\begin{array}{ccc}\frac{v_{\mathrm{DU}}}{R_M+h_D}& 0& -\frac{v_{\mathrm{DE}}}{R_M+h_D}\\ 0& -\frac{\sec L_D}{R_N+h_D}{v}_{\mathrm{DU}}& \frac{\sec L_D}{R_N+h_D}{v}_{\mathrm{DN}}\\ -v_{\mathrm{DN}}& v_{\mathrm{DE}}& 0\end{array}\right]$

$F_{43}={\left[\begin{array}{ccc}\frac{v_{\mathrm{DN}}}{R_M+h_D}& \frac{\sec L_D}{R_N+h_D}{v}_{\mathrm{DE}}& v_{\mathrm{DU}}\end{array}\right]}^T$

$F_{51}=-C_{\mathrm{Db}}^n,F_{52}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L_D& 0& 0\\ ({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L_D+\frac{v_{\mathrm{DE}}{\sec }^2{L}_D}{R_N+h_D})& 0& 0\end{array}\right]$

$F_{53}=[M(v_D^n\&times;)-({\mathrm{\&omega;}}_{\mathrm{in}}^n\&times;)],M=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h_D}& 0\\ \frac{1}{R_N+h_D}& 0& 0\\ \frac{\tan L_D}{R_N+h_D}& 0& 0\end{array}\right]$

$F_{54}={\left[\begin{array}{ccc}-\frac{v_{\mathrm{DN}}}{R_M+h_D}& \frac{v_{\mathrm{DE}}}{R_N+h_D}& \frac{v_{\mathrm{DE}}}{R_N+h_D}\tan L_D\end{array}\right]}^T$

v_E、v_N、v_U分别表示捷联解算得车辆东向、北向、天向速度值；L表示捷联解算得车辆纬度值；h表示捷联解算得车辆高度值；R_M和R_N分别表示子午面曲率半径和卯酉圈曲率半径；ω_ie表示地球自转角速度；v_DE、v_DN、v_DU分别表示航位推算得车辆东向、北向、天向速度值；L_D表示航位推算得车辆纬度值；h_D表示航位推算得车辆高度值；表示捷联惯导姿态更新矩阵；表示航位推算姿态更新矩阵。

f_E为加速度计所测比力分量在导航坐标系中东向投影分量；

f_N为加速度计所测比力分量在导航坐标系中东向投影分量；

f_U为加速度计所测比力分量在导航坐标系中东向投影分量。

2)量测方程

利用航位推算获得的车辆速度、姿态辅助捷联惯导系统完成精对准。将捷联

惯导输出的速度、姿态与航位推算获得的对应信息相减作为辅助对准的量测，即：

Z=[ψ_S-ψ_D,θ_S-θ_D,γ_S-γ_D,v_SE-v_DE,v_SN-v_DN,v_SU-v_DU]^T

式中，ψ_S、θ_S和γ_S分别为捷联惯导输出车辆航向角、俯仰角和横摇角，ψ_D、θ_D和γ_D分别为里程计航位推算输出航向角、俯仰角和横摇角，v_SE、v_SN和v_SU分别为捷联惯导输出车辆速度，v_DE、v_DN和v_DU分别为里程计输出车辆速度；

根据捷联惯导、航位推算的姿态角误差与数学平台失准角之间关系，可以推导获得量测方程为：

$Z=\left[\begin{array}{c}\mathrm{\&delta;}{v}_E-v_{\mathrm{DN}}{\mathrm{\&phi;}}_{\mathrm{DU}}+v_{\mathrm{DU}}{\mathrm{\&phi;}}_{\mathrm{DN}}-v_{\mathrm{DE}}\mathrm{\&delta;}{k}_D\\ \mathrm{\&delta;}{v}_N+v_{\mathrm{DE}}{\mathrm{\&phi;}}_{\mathrm{DU}}-v_{\mathrm{DU}}{\mathrm{\&phi;}}_{\mathrm{DE}}-v_{\mathrm{DN}}\mathrm{\&delta;}{k}_D\\ \mathrm{\&delta;}{v}_U-v_{\mathrm{DE}}{\mathrm{\&phi;}}_{\mathrm{DN}}+v_{\mathrm{DN}}{\mathrm{\&phi;}}_{\mathrm{DE}}-v_{\mathrm{DU}}\mathrm{\&delta;}{k}_D\\ -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}{\mathrm{\&phi;}}_E-\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}{\mathrm{\&phi;}}_N+{\mathrm{\&phi;}}_U+\frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}{\mathrm{\&phi;}}_{\mathrm{DE}}+\frac{R_{32}+R_{22}}{R_{12}^2+R_{12}^2}{\mathrm{\&phi;}}_{\mathrm{DN}}-{\mathrm{\&phi;}}_{\mathrm{DU}}\\ -\frac{T_{22}}{\sqrt{1-T_{32}^2}}{\mathrm{\&phi;}}_E+\frac{T_{12}}{\sqrt{1-T_{32}^2}}{\mathrm{\&phi;}}_N+\frac{R_{22}}{\sqrt{1-R_{32}^2}}{\mathrm{\&phi;}}_{\mathrm{DE}}-\frac{R_{12}}{\sqrt{1-R_{32}^2}}{\mathrm{\&phi;}}_{\mathrm{DN}}\\ \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}{\mathrm{\&phi;}}_E+\frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}{\mathrm{\&phi;}}_N-\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}{\mathrm{\&phi;}}_{\mathrm{DE}}-\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}{\mathrm{\&phi;}}_{\mathrm{DN}}\end{array}\right]$

结合系统状态X，可将辅助对准的量测方程写成如下形式

Z(t)=H(t)X(t)+V(t)

$H\left(t\right)=\left[\begin{array}{cccccccccccc}& 1& 0& 0& 0& 0& 0& & 0& v_{\mathrm{DU}}& -v_{\mathrm{DN}}& -v_{\mathrm{DE}}\\ & 0& 1& 0& 0& 0& 0& & -v_{\mathrm{DU}}& 0& v_{\mathrm{DE}}& -v_{\mathrm{DN}}\\ & 0& 0& 1& 0& 0& 0& & v_{\mathrm{DN}}& -v_{\mathrm{DE}}& 0& -v_{\mathrm{DU}}\\ 0_{6\&times;3}& 0& 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}& 1& 0_{6\&times;9}& \frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}& \frac{R_{32}{R}_{22}}{R_{12}^2+R_{22}^2}& -1& 0\\ & 0& 0& 0& -\frac{T_{22}}{\sqrt{1-T_{32}^2}}& \frac{T_{12}}{\sqrt{1-T_{32}^2}}& 0& & \frac{R_{22}}{\sqrt{1-R_{32}^2}}& -\frac{R_{12}}{\sqrt{1-R_{32}^2}}& 0& 0\\ & 0& 0& 0& \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}& \frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}& 0& & -\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}& -\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}& 0& 0\end{array}\right]$

其中，H(t)表示量测矩阵，可以由状态方程和量测方程之间关系直接求得，V(t)为量测噪声向量，其方差为R(t)，记捷联惯导姿态更新矩阵里程计航位推算更新矩阵

步骤七、将卡尔曼滤波的状态方程和量测方程离散化，完成初始对准，进入导航状态。

将状态方程和量测方程离散化：

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

Z_k=H_kX_k+V_k

(1)Φ_k+1,k的计算方法

${\mathrm{\&Phi;}}_{k+1,k}\&ap;I+\mathrm{\&Delta;T}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{F}_{i-1}+O\left(\mathrm{\&Delta;}{T}^2\right)\&ap;I+\mathrm{\&Delta;T}\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{F}_{i-1}$

式中，滤波器计算周期为T，对于时变连续系统，将计算周期T分隔成N个时间间隔ΔT，即T=NΔT，按各个ΔT时间间隔中系统阵近似为常值矩阵来计算。

(2)Q_k的计算方法

$Q_k=(Q+{\mathrm{\&Phi;}}_{k+1,k}Q{\mathrm{\&Phi;}}_{k+1,k}^T)\frac{T}{2}$

应用标准卡尔曼滤波方程，X_k的估计按下述方法求解：

$\left\{\begin{array}{c}{\hat{X}}_{k/k-1}{\mathrm{\&Phi;}}_{k,k-1}{\hat{X}}_{k-1}\\ P_{k/k-1}={\mathrm{\&Phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&Phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T\\ K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+R_k)}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k\left(Z_k{H}_k{\hat{X}}_{k/k-1}\right)\\ P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k\end{array}\right.$

只要给定初值和P₀，根据k时刻的量测Z_k，就可以递推算得k时刻的状态估计 ${\hat{X}}_k,(k=\mathrm{1,2,3}\&CenterDot;\&CenterDot;\&CenterDot;).$

实施例：

在一组仿真环境下，对发明进行Matlab仿真实验：

模拟车辆启动过程，车载惯导系统在振动环境下进行粗对准，40s粗对准完成后，车辆以0.5m/s²的加速度加速启动20s，然后以10m/s的速度作近似匀速运动，精对准时间为10min。

车辆在发动机开启环境下主要受到发动机的高频振动引起的速度变化影响，建立数学模型：

x,y,z分别表示车辆右、前、上三个位置，根据车辆发动机特性，设置参数如下：A_RHx=0.1m/s，A_RHy=0.1m/s，A_RHz=0.2m/s；f_RHx=50Hz，f_RHy=50Hz，f_RHz=100Hz；为[02π]上服从均匀分布的随机相位。

车辆所处的地理经度λ=118.4°，纬度L=32.1°；陀螺仪常值漂移为0.01°/h，随机漂移为0.01°/h；加速度计常值偏置为1×10^-4g，随机偏置为1×10^-4g；里程计刻度系数误差为0.3%。

如上所述，尽管参照特定的优选实施例已经表示和表述了本发明，但其不得解释为对本发明自身的限制。在不脱离所附权利要求定义的本发明的精神和范围前提下，可对其在形式上和细节上作出各种变化。

Claims (3)

1.一种里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于依次包括粗对准和精对准两部分，

所述的粗对准包括以下步骤：

(11)车辆发动机开启，人员上下车辆做准备工作，惯性测量单元实时地同步采集车辆角速率和比力信息；

(12)运行系统程序，开始进行快速粗对准；

(13)粗对准结束后，得到捷联惯导系统初始姿态矩阵，车辆开始启动，里程计实时采集车辆运动信息；

所述的精对准包括以下步骤：

(21)捷联惯导系统利用粗对准得到的姿态矩阵进行速度、位置以及姿态导航解算，同时里程计根据捷联惯导系统提供的初始车辆姿态矩阵进行航位推算，计算得到准确的速度、姿态信息；

(22)将里程计航位推算得到的速度、姿态信息作为量测基准，与捷联惯导系统解算得到的速度、姿态相减作差，以所得差值作为卡尔曼滤波的量测值，列写卡尔曼滤波的状态方程和量测方程；

卡尔曼滤波的状态方程为：

$\stackrel{\&CenterDot;}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

其中，t为时间变量，F(t)是系统状态转移矩阵，W(t)是系统过程噪声序列，状态向量取为状态向量中对应的变量分别为捷联惯导位置误差向量δp、速度误差向量δvⁿ、数学平台误差向量φ、陀螺仪常值漂移向量加速度计常值偏置向量里程计航位推算位置误差向量δp_D、航位推算数学平台误差向量φ_D和里程计刻度系数误差δk_D；

$\left[\begin{array}{c}\mathrm{\&delta;}\stackrel{\&CenterDot;}{p}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{v}}^n\\ {\mathrm{\&phi;}}^{\&CenterDot;}\\ {\mathrm{\&epsiv;}}^{\&CenterDot;}\\ {\&dtri;}^{\&CenterDot;}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{p}}_D\\ \stackrel{\&CenterDot;}{{\mathrm{\&phi;}}_D}\\ \mathrm{\&delta;}{\stackrel{\&CenterDot;}{k}}_D\end{array}\right]=\left[\begin{array}{cccccccc}{F}_{11}& F_{12}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;1}\\ F_{21}& F_{22}& F_{23}& 0_{3\&times;3}& F_{24}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;1}\\ F_{31}& F_{32}& F_{33}& F_{34}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;1}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& F_{41}& F_{42}& F_{43}\\ 0_{3\&times;3}& 0_{3\&times;3}& 0_{3\&times;3}& F_{51}& 0_{3\&times;3}& F_{52}& F_{53}& F_{54}\\ 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;3}& 0_{1\&times;1}\end{array}\right]\left[\begin{array}{c}\mathrm{\&delta;}p\\ {\mathrm{\&delta;v}}^n\\ \mathrm{\&phi;}\\ \mathrm{\&epsiv;}\\ \&dtri;\\ {\mathrm{\&delta;p}}_D\\ {\mathrm{\&phi;}}_D\\ {\mathrm{\&delta;k}}_D\end{array}\right]+\left[\begin{array}{c}{0}_{1\&times;3}\\ w_a\\ w_g\\ 0_{3\&times;1}\\ 0_{3\&times;1}\\ 0_{3\&times;1}\\ w_g\\ 0_{1\&times;1}\end{array}\right]$

式中，w_a、w_g分别为加速度计和陀螺仪在载体坐标系下的噪声，即是均值为0、方差为Q(t)、呈正态分布的白噪声；

$\begin{array}{cc}{F}_{11}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{v_E\tan L}{R_N+h}\sec L& 0& 0\\ 0& 0& 0\end{array}\right]& F_{12}=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]\end{array}$

$F_{21}=\left[\begin{array}{ccc}2{\mathrm{\&omega;}}_{ie}{\mathrm{cosLv}}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\mathrm{\&omega;}}_{ie}\sin {\mathrm{Lv}}_u& 0& 0\\ -(2{\mathrm{\&omega;}}_{ie}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E& 0& 0\\ -{\mathrm{\&omega;}}_{ie}\sin {\mathrm{Lv}}_E& 0& \frac{2g}{R}\end{array}\right]$

$F_{22}=\left[\begin{array}{ccc}\frac{v_N}{R_M+h}\tan L-\frac{v_U}{R_M+h}& 2{\mathrm{\&omega;}}_{ie}\sin L+\frac{v_E}{R_N+h}\tan L& -(2{\mathrm{\&omega;}}_{ie}\cos L+\frac{v_E}{R_N+h})\\ -(2{\mathrm{\&omega;}}_{ie}\sin L+\frac{v_E}{R_N+h}\tan L)& -\frac{v_U}{R_M+h}& -\frac{v_N}{R_M+h}\\ 2({\mathrm{\&omega;}}_{ie}\cos L+\frac{v_E}{R_N+h})& 2\frac{v_N}{R_M+h}& 0\end{array}\right]$

$\begin{array}{cc}{F}_{23}=\left[\begin{array}{ccc}0& -f_U& f_N\\ f_U& 0& -f_E\\ -f_N& f_E& 0\end{array}\right]& F_{24}=C_b^n\end{array}$

$\begin{array}{cc}{F}_{31}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\&omega;}}_{ie}\sin L& 0& 0\\ ({\mathrm{\&omega;}}_{ie}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)& 0& 0\end{array}\right]& F_{32}=\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]\end{array}$

$F_{33}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{ie}\sin L+\frac{v_E}{R_N+h}\tan L& -({\mathrm{\&omega;}}_{ie}\cos L+\frac{v_E}{R_N+h})\\ -({\mathrm{\&omega;}}_{ie}\sin L+\frac{v_E}{R_N+h})& 0& -\frac{v_N}{R_M+h}\\ {\mathrm{\&omega;}}_{ie}\cos L+\frac{v_E}{R_N+h}& \frac{v_N}{R_M+h}& 0\end{array}\right]$

$F_{34}=-C_b^n$

$\begin{array}{cc}{F}_{41}=\left[\begin{array}{ccc}0& 0& 0\\ \frac{v_{DE}\tan L_D}{R_N+h}\sec L_D& 0& 0\\ 0& 0& 0\end{array}\right]& F_{42}=\left[\begin{array}{ccc}\frac{v_{DU}}{R_M+h_D}& 0& -\frac{v_{DE}}{R_M+h_D}\\ 0& -\frac{\sec L_D}{R_N+h_D}{v}_{DU}& \frac{\sec L_D}{R_N+h_D}{v}_{DN}\\ -v_{DN}& v_{DE}& 0\end{array}\right]\end{array}$

$F_{43}={\left[\begin{array}{ccc}\frac{v_{DN}}{R_M+h_D}& \frac{\sec L_D}{R_N+h_D}{v}_{DE}& v_{DU}\end{array}\right]}^T$

$\begin{array}{cc}{F}_{51}=-C_{Db}^n& F_{52}=\left[\begin{array}{ccc}0& 0& 0\\ -{\mathrm{\&omega;}}_{ie}\sin L_D& 0& 0\\ ({\mathrm{\&omega;}}_{ie}\cos L_D+\frac{v_{DE}{\sec }^2{L}_D}{R_N+h_D})& 0& 0\end{array}\right]\end{array}$

$\begin{array}{cc}{F}_{53}=\&lsqb;M(v_D^n\&times;)-({\mathrm{\&omega;}}_{in}^n\&times;)\&rsqb;& M=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h_D}& 0\\ \frac{1}{R_N+h_D}& 0& 0\\ \frac{\tan L_D}{R_N+h_D}& 0& 0\end{array}\right]\end{array}$

$F_{54}={\left[\begin{array}{ccc}-\frac{v_{DN}}{R_M+h_D}& \frac{v_{DE}}{R_N+h_D}& \frac{v_{DE}}{R_N+h_D}\tan L_D\end{array}\right]}^T$

式中，v_E、v_N、v_U分别表示捷联解算得车辆东向、北向、天向速度值；L表示捷联解算得车辆所在点纬度值；h表示捷联解算得车辆所在点高度值；R_M和R_N分别表示子午面曲率半径和卯酉圈曲率半径；ω_ie表示地球自转角速度；v_DE、v_DN、v_DU分别表示航位推算得车辆东向、北向、天向速度值；L_D表示航位推算得车辆所在点纬度值；h_D表示航位推算得车辆所在点高度值；表示捷联惯导姿态更新矩阵；表示航位推算姿态更新矩阵；

f_E为加速度计所测比力分量在导航坐标系中东向投影分量；

f_N为加速度计所测比力分量在导航坐标系中北向投影分量；

f_U为加速度计所测比力分量在导航坐标系中天向投影分量；

上述卡尔曼滤波的量测方程的计算过程如下：

利用航位推算获得的车辆速度、姿态辅助捷联惯导系统完成精对准，将捷联惯导输出的速度、姿态与航位推算获得的对应信息相减作为辅助对准的量测信息，即：

Z＝[ψ_S-ψ_D,θ_S-θ_D,γ_S-γ_D,v_SE-v_DE,v_SN-v_DN,v_SU-v_DU]^T

式中，ψ_S、θ_S和γ_S分别为捷联惯导输出车辆航向角、俯仰角和横摇角，ψ_D、θ_D和γ_D分别为里程计航位推算输出航向角、俯仰角和横摇角，v_SE、v_SN和v_SU分别为捷联惯导输出车辆速度，v_DE、v_DN和v_DU分别为里程计输出车辆速度；

根据捷联惯导、航位推算的姿态角误差与数学平台失准角之间关系，可以推导获得量测方程为：

$Z=\left[\begin{array}{c}{\mathrm{\&delta;v}}_E-v_{DN}{\mathrm{\&phi;}}_{DU}+v_{DU}{\mathrm{\&phi;}}_{DN}-v_{DE}{\mathrm{\&delta;k}}_D\\ {\mathrm{\&delta;v}}_N-v_{DE}{\mathrm{\&phi;}}_{DU}+v_{DU}{\mathrm{\&phi;}}_{DE}-v_{DN}{\mathrm{\&delta;k}}_D\\ {\mathrm{\&delta;v}}_U-v_{DE}{\mathrm{\&phi;}}_{DN}+v_{DN}{\mathrm{\&phi;}}_{DE}-v_{DU}{\mathrm{\&delta;k}}_D\\ -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}{\mathrm{\&phi;}}_E-\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}{\mathrm{\&phi;}}_N+{\mathrm{\&phi;}}_U+\frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}{\mathrm{\&phi;}}_{DE}+\frac{R_{32}{R}_{22}}{R_{12}^2+R_{22}^2}{\mathrm{\&phi;}}_{DN}-{\mathrm{\&phi;}}_{DU}\\ -\frac{T_{22}}{\sqrt{1-T_{32}^2}}{\mathrm{\&phi;}}_E+\frac{T_{12}}{\sqrt{1-T_{32}^2}}{\mathrm{\&phi;}}_N+\frac{R_{22}}{\sqrt{1-R_{32}^2}}{\mathrm{\&phi;}}_{DE}-\frac{R_{12}}{\sqrt{1-R_{32}^2}}{\mathrm{\&phi;}}_{DN}\\ \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}{\mathrm{\&phi;}}_E+\frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}{\mathrm{\&phi;}}_N-\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}{\mathrm{\&phi;}}_{DE}-\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}{\mathrm{\&phi;}}_{DN}\end{array}\right]$

结合系统状态X，可将辅助对准的量测方程写成如下形式：

Z(t)＝H(t)X(t)+V(t)

$H\left(t\right)=\left[\begin{array}{cccccccccccc}& 1& 0& 0& 0& 0& 0& & 0& v_{DU}& -v_{DN}& -v_{DE}\\ & 0& 1& 0& 0& 0& 0& & -v_{DU}& 0& v_{DE}& -v_{DN}\\ & 0& 0& 1& 0& 0& 0& & v_{DN}& -v_{DE}& 0& -v_{DU}\\ 0_{6\&times;3}& 0& 0& 0& -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}& -\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}& 1& 0_{6\&times;9}& \frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}& \frac{R_{32}{R}_{22}}{R_{12}^2+R_{22}^2}& -1& 0\\ & 0& 0& 0& -\frac{T_{22}}{\sqrt{1-T_{32}^2}}& \frac{T_{12}}{\sqrt{1-T_{32}^2}}& 0& & \frac{R_{22}}{\sqrt{1-R_{32}^2}}& -\frac{R_{12}}{\sqrt{1-R_{32}^2}}& 0& 0\\ & 0& 0& 0& \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}& \frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}& 0& & -\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}& -\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}& 0& 0\end{array}\right]$

其中，H(t)表示量测矩阵，可以由状态方程和量测方程之间关系直接求得，V(t)为量测噪声向量，其方差为R(t)，记捷联惯导姿态更新矩阵里程计航位推算更新矩阵

(23)将状态方程和量测方程离散化，根据给定状态初值以及估计误差初值利用导航计算机递推得到任意时刻的状态估计值，反馈修正捷联惯导系统，完成初始对准。

2.根据权利要求1所述的里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于：所述粗对准应用惯性凝固假设，将比力信息投影到载体惯性坐标系中，得到重力加速度相对惯性空间随地球旋转而引起的方向变化信息，对其积分可以克服车辆发动机振动以及人员上下车等引起的随机干扰影响，求解得到粗略姿态矩阵

3.根据权利要求1所述的里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于：所述精对准应用卡尔曼滤波估计系统误差，里程计与捷联惯导系统中的陀螺仪进行航位推算，将航位推算得到的车辆速度和姿态信息作为卡尔曼滤波量测基准，与捷联惯导系统解算得到的速度、姿态相减，误差值作为卡尔曼滤波器输入，估计得到系统误差后反馈修正捷联惯导系统，求解得到精确姿态矩阵