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

摘要

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

主权项

一种里程计辅助车载捷联惯导系统行进间初始对准方法，其特征在于依次包括粗对准和精对准两部分，所述的粗对准包括以下步骤：(11)车辆发动机开启，人员上下车辆做准备工作，惯性测量单元实时地同步采集车辆角速率和比力信息；(12)运行系统程序，开始进行快速粗对准；(13)粗对准结束后，得到捷联惯导系统初始姿态矩阵，车辆开始启动，里程计实时采集车辆运动信息；所述的精对准包括以下步骤：(21)捷联惯导系统利用粗对准得到的姿态矩阵进行速度、位置以及姿态导航解算，同时里程计根据捷联惯导系统提供的初始车辆姿态矩阵进行航位推算，计算得到准确的速度、姿态信息；(22)将里程计航位推算得到的速度、姿态信息作为量测基准，与捷联惯导系统解算得到的速度、姿态相减作差，以所得差值作为卡尔曼滤波的量测值，列写卡尔曼滤波的状态方程和量测方程；卡尔曼滤波的状态方程为：$\stackrel{·}{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}\delta \stackrel{·}{p}\\ \delta {\stackrel{·}{v}}^n\\ {\phi }^{·}\\ {ϵ}^{·}\\ {▿}^{·}\\ \delta {\stackrel{·}{p}}_D\\ \stackrel{·}{{\phi }_D}\\ \delta {\stackrel{·}{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}\delta p\\ {\mathrm{\delta v}}^n\\ \phi \\ ϵ\\ ▿\\ {\mathrm{\delta p}}_D\\ {\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{\omega }_{ie}{\mathrm{cosLv}}_N+\frac{v_E{v}_N}{R_N+h}{\sec }^{2}L+2{\omega }_{ie}\sin {\mathrm{Lv}}_u& 0& 0\\ -(2{\omega }_{ie}\cos L+\frac{v_E}{R_N+h}{\sec }^{2}L)v_E& 0& 0\\ -{\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{\omega }_{ie}\sin L+\frac{v_E}{R_N+h}\tan L& -(2{\omega }_{ie}\cos L+\frac{v_E}{R_N+h})\\ -(2{\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({\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\\ -{\omega }_{ie}\sin L& 0& 0\\ ({\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& {\omega }_{ie}\sin L+\frac{v_E}{R_N+h}\tan L& -({\omega }_{ie}\cos L+\frac{v_E}{R_N+h})\\ -({\omega }_{ie}\sin L+\frac{v_E}{R_N+h})& 0& -\frac{v_N}{R_M+h}\\ {\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\\ -{\omega }_{ie}\sin L_D& 0& 0\\ ({\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}=[M(v_D^n\times )-({\omega }_{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]\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}{\phi }_{DU}+v_{DU}{\phi }_{DN}-v_{DE}{\mathrm{\delta k}}_D\\ {\mathrm{\delta v}}_N-v_{DE}{\phi }_{DU}+v_{DU}{\phi }_{DE}-v_{DN}{\mathrm{\delta k}}_D\\ {\mathrm{\delta v}}_U-v_{DE}{\phi }_{DN}+v_{DN}{\phi }_{DE}-v_{DU}{\mathrm{\delta k}}_D\\ -\frac{T_{12}{T}_{32}}{T_{12}^2+T_{22}^2}{\phi }_E-\frac{T_{32}{T}_{22}}{T_{12}^2+T_{22}^2}{\phi }_N+{\phi }_U+\frac{R_{12}{R}_{32}}{R_{12}^2+R_{22}^2}{\phi }_{DE}+\frac{R_{32}{R}_{22}}{R_{12}^2+R_{22}^2}{\phi }_{DN}-{\phi }_{DU}\\ -\frac{T_{22}}{\sqrt{1-T_{32}^2}}{\phi }_E+\frac{T_{12}}{\sqrt{1-T_{32}^2}}{\phi }_N+\frac{R_{22}}{\sqrt{1-R_{32}^2}}{\phi }_{DE}-\frac{R_{12}}{\sqrt{1-R_{32}^2}}{\phi }_{DN}\\ \frac{T_{21}{T}_{33}-T_{23}{T}_{31}}{T_{31}^2+T_{33}^2}{\phi }_E+\frac{T_{13}{T}_{31}-T_{11}{T}_{33}}{T_{31}^2+T_{33}^2}{\phi }_N-\frac{R_{21}{R}_{33}-R_{23}{R}_{31}}{R_{31}^2+R_{33}^2}{\phi }_{DE}-\frac{R_{13}{R}_{31}-R_{11}{R}_{33}}{R_{31}^2+R_{33}^2}{\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)将状态方程和量测方程离散化，根据给定状态初值以及估计误差初值利用导航计算机递推得到任意时刻的状态估计值，反馈修正捷联惯导系统，完成初始对准。