CN101216321A - 捷联惯性导航系统的快速精对准方法 - Google Patents

Abstract

一种捷联惯性导航系统的快速精对准方法，属捷联惯性导航系统的对准方法。该方法包括如下步骤：采集惯性测量组件的输出信号，得到运载体的角速度和比力信息；捷联惯性导航系统解析式粗对准；捷联惯性导航系统解算；捷联惯性导航系统精对准；滤波输出及修正；精对准完成。本方法针对低成本捷联惯性导航系统，可实现性和可操作性高，成本低，提高了捷联惯性导航系统的对准精度和快速性，并且实现了航向对准。

Description

捷联惯性导航系统的快速精对准方法

一、技术领域

本发明涉及一种低成本捷联惯性导航系统的导航技术，尤其涉及一种低成本捷联惯性导航系统的快速精对准方法。

二、背景技术

捷联惯性导航系统(Strapdown Inertial Navigation System简称SINS)是根据牛顿提出的相对惯性空间的力学定律，利用惯性测量组件(简称IMU)感受运载体的角速度和加速度，通过积分运算得到运载体的姿态、速度和位置等导航参数。IMU作为捷联惯性导航系统的核心部件，由敏感轴相互正交的三个陀螺仪和三个加速度计构成，其性能和成本紧密相关。近年来，用低成本的固态IMU实现中高性能的捷联惯性导航系统和组合导航系统逐步成为导航系统的发展方向。

捷联惯性导航系统的初始对准在系统进入导航工作状态之前进行，其目的是确定初始时刻运载体的姿态和航向从而建立机体坐标系与导航坐标系之间的初始姿态矩阵，是保证捷联惯性导航系统正常工作的前提。初始对准的对准精度和对准时间是两项重要指标，直接影响惯性导航系统整体的导航精度和反应时间。对准过程分为粗对准和精对准两步：粗对准阶段依靠重力矢量及地球角速率矢量的测量值，直接估计机体坐标系与导航坐标系之间的初始姿态矩阵；精对准阶段通过对惯性测量组件输出信号的处理或其他外测设备，精确校准计算导航坐标系与真实导航坐标系间的失准角，建立准确的初始姿态矩阵。捷联惯性导航系统要求对准精度高，对准时间短。

对于由低精度IMU构成的低成本捷联惯性导航系统来说，IMU的精度低导致航向难以完成自对准；传统的以速度误差作为观测量的静基座精对准系统可观性和可观度差，是制约其对准精度和快速性的重要原因，特别是方位失准角的观测度低，导致方位失准角的估计效果很差。已有文献的常规思路是利用精密的位置转台提供位置基准进行多位置对准来提高系统的可观性和可观度，但该方法需要在精密的位置转台上进行，无法实现工程应用。因此，在实际工程应用中如何充分利用系统本身的可观测信息或者引入低成本的外观测设备，建立具有更高可观性和可观度的静基座精对准系统，对于提高初始对准的对准精度和反应能力乃至整个捷联惯性导航系统的性能具有非常重要的军事意义和实用价值。

三、发明内容

本发明的目的是针对低成本捷联惯性导航系统的初始对准提供一种实用性强、成本低并能有效提高系统对准精度和快速性的精对准方法。

为了达到上述的发明目的，本发明包括下列步骤：

(A)采集惯性测量组件的输出信号，得到运载体的角速度和比力信息；

(B)捷联惯性导航系统解析式粗对准步骤

在粗对准时，利用加速度计测量重力加速度矢量g在机体坐标系b中的分量；利用陀螺仪测量地球角速率矢量Ω在机体坐标系b中的分量，则机体坐标系相对导航坐标系的姿态矩阵通过如下方式进行估计，C_n ^b导航坐标系n相对机体坐标系b的姿态矩阵，C_b ⁿ为机体坐标系b相对导航坐标系n的姿态矩阵。

重力加速度矢量和地球角速率矢量的变换式为

$g^b=C_n^b{g}^n---\left(1\right)$

${\mathrm{\Ω}}^b=C_n^b{\mathrm{\Ω}}^n---\left(2\right)$

定义第三个矢量V＝g×Ω，则其变换式为

$V^b=C_n^b{V}^n---\left(3\right)$

由于 $C_b^n={\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T,$ 则由式(1)、式(2)和式(3)的矢量关系得机体坐标系b相对导航坐标系n的姿态矩阵：

$C_b^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {\left({\mathrm{\Ω}}^n\right)}^T\\ {\left(V^n\right)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(g^b\right)}^T\\ {\left({\mathrm{\Ω}}^b\right)}^T\\ {\left(V^b\right)}^T\end{array}\right]---\left(4\right)$

则由步骤(A)和式(4)获得载体初始姿态矩阵C_b ⁿ估计值；

(C)捷联惯性导航系统解算步骤

利用步骤(A)得到的角速度和比力信息，以及由步骤(B)得到的载体初始姿态矩阵即式(4)，按捷联惯性导航算法的流程，解算出运载体的姿态角、航向角和速度信息。

导航坐标系选择东北天地理坐标系，则机体坐标系相对于地理坐标系的角速度在机体系的三个轴向分量依次为绕横滚轴的角速度ω_x，绕俯仰轴的角速度ω_y，绕方位轴的角速度ω_z，计算周期为Δt，Δt时间内机体转过的角度Δθ的三个分量依次为横滚轴角增量Δθ_x，俯仰轴角增量Δθ_y，方位轴角增量Δθ_z，则转过的角度以矩阵方式表示为：

$\mathrm{\Δ\θ}=\left[\begin{array}{ccc}0& {-\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z& 0& {-\mathrm{\Δ\θ}}_x\\ {-\mathrm{\Δ\θ}}_y& {\mathrm{\Δ\θ}}_x& 0\end{array}\right]=\left[\begin{array}{ccc}0& {-\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& {-\mathrm{\ω}}_x\\ {-\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\mathrm{\Δt}---\left(5\right)$

t时刻载体的姿态矩阵为

$C_b^n\left(t\right)=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(6\right)$

式(6)中，γ为载体的横滚角，θ为载体的俯仰角，ψ为载体的航向角。那么，t+Δt时刻的姿态矩阵由 $C_b^n=(t+\mathrm{\Δt})=-\mathrm{\Δ\θ}\·C_b^n\left(t\right)$ 从t时刻的姿态矩阵递推求得，在姿态矩阵递推的初始时刻，即t＝0时，C_b ⁿ(0)的值由步骤(B)中的式(4)确定。将C_b ⁿ(t+Δt)简写为 $\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$ 则t+Δt时刻，捷联惯性导航系统的横滚角γ_I、俯仰角θ_I和航向角ψ_I姿态信息由下式获得：

$\left\{\begin{array}{c}{\mathrm{\γ}}_I={\mathrm{tg}}^{-1}\left(\frac{C_{31}}{C_{33}}\right)\\ {\mathrm{\θ}}_I={\sin }^{-1}\left(C_{32}\right)\\ {\mathrm{\ψ}}_I={\mathrm{tg}}^{-1}\left(\frac{C_{12}}{C_{22}}\right)\end{array}\right.---\left(7\right)$

上述式中：

C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγ

C₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγ

C₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ

在姿态解算的基础上，根据当前的姿态和比力，获得地理坐标系中的运动加速度a_E，a_N，a_U，其中，a_E是东向的运动加速度，a_N是北向的运动加速度，a_U是天向的运动加速度。运载体的速度由式(8)递推得到，v_E(t)表示t时刻运载体东向的速度，v_N(t)表示t时刻运载体北向的速度，v_U(t)表示t时刻运载体天向的速度，v_E(t+Δt)，v_N(t+Δt)，v_U(t+Δt)分别表示t+Δt时刻东向、北向和天向的速度，即：

$\left\{\begin{array}{c}{v}_E(t+\mathrm{\Δt})=v_E\left(t\right)+a_E\mathrm{\Δt}\\ v_N(t+\mathrm{\Δt})=v_N\left(t\right)+a_N\mathrm{\Δt}\\ v_U(t+\mathrm{\Δt})=v_U\left(t\right)+a_U\mathrm{\Δt}\end{array}\right.---\left(8\right)$

(D)捷联惯性导航系统精对准步骤，包括建立精对准系统的状态方程和观测方程，获取精对准系统的观测量，进行卡尔曼滤波状态估计。

①建立精对准系统的状态方程和观测方程

在东北天地理坐标系下，静基座捷联惯性导航系统初始对准的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

式(9)中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态转移矩阵和系统噪声系数矩阵；W(t)为系统的噪声向量。

系统的状态向量为：

X(t)＝[δv_E δv_N φ_E φ_N  φ_U __x __y ε_x ε_y ε_z]^T    (10)

系统的噪声向量为：

W(t)＝[w_ax w_at w_gx w_gy w_gz]^T    (11)

式(10)和式(11)中，δv_E、δv_N表示系统东向和北向的速度误差，φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角，__x、__y表示x轴和y轴加速度计的误差，ε_x、ε_y、ε_z表示三轴陀螺仪的误差；w_ax、w_ay表示x轴和y轴加速度计的测量白噪声，w_gx、w_gy、w_gz表示三轴陀螺仪的测量白噪声。

系统的状态转移矩阵为：

$A\left(t\right)=\left[\begin{array}{cccccccccc}0& 2{\mathrm{\Ω}}_U& 0& -g& 0& C_{11}& C_{12}& 0& 0& 0\\ -2{\mathrm{\Ω}}_U& 0& g& 0& 0& C_{21}& C_{22}& 0& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& -{\mathrm{\Ω}}_N& 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& -{\mathrm{\Ω}}_U& 0& 0& 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& {\mathrm{\Ω}}_N& 0& 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(12\right)$

系统的噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]---\left(13\right)$

式(12)和式(13)中，Ω_N、Ω_U表示地球角速率矢量在地理坐标系北向和天向的分量，C_ij为机体坐标系相对地理坐标系的姿态矩阵C_b ⁿ的第i行和第j列元素，即

C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγ

C₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγ

C₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ

建立姿态角误差、航向角误差观测量对数学平台失准角的观测关系，实质上，两者存在着如下的转换关系：

$C_b^n=C_p^n{C}_b^p---\left(14\right)$

式(14)中，p代表数学平台坐标系，在东北天地理坐标系下有

$C_b^n=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(15\right)$

$C_b^p=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\′\sin \mathrm{\θ}\′\sin \mathrm{\γ}\′+\cos \mathrm{\ψ}\′\cos \mathrm{\γ}\′& \sin \mathrm{\ψ}\′\cos \mathrm{\θ}\′& -\sin \mathrm{\ψ}\′\sin \mathrm{\θ}\′\cos \mathrm{\γ}\′+\cos \mathrm{\ψ}\′\sin \mathrm{\γ}\′\\ \cos \mathrm{\ψ}\′\sin \mathrm{\θ}\′\sin \mathrm{\γ}\′-\sin \mathrm{\ψ}\′\cos \mathrm{\γ}\′& \cos \mathrm{\ψ}\′\cos \mathrm{\θ}\′& -\cos \mathrm{\ψ}\′\sin \mathrm{\θ}\′\cos \mathrm{\γ}\′-\sin \mathrm{\ψ}\′\sin \mathrm{\γ}\′\\ -\cos \mathrm{\θ}\′\sin \mathrm{\γ}\′& \sin \mathrm{\θ}\′& \cos \mathrm{\θ}\′\cos \mathrm{\γ}\′\end{array}\right]---\left(16\right)$

式(15)和式(16)中：γ、θ、ψ分别为运载体理想情况下的横滚角、俯仰角、航向角；γ′、θ′、ψ′分别为运载体实际情况下的横滚角、俯仰角和航向角。定义δγ、δθ和δψ分别为横滚角、俯仰角和航向角误差，则有如下的关系：

$\left\{\begin{array}{c}\mathrm{\δ\γ}=\mathrm{\γ}\′-\mathrm{\γ}\\ \mathrm{\δ\θ}=\mathrm{\θ}\′-\mathrm{\θ}\\ \mathrm{\δ\ψ}=\mathrm{\ψ}\′-\mathrm{\ψ}\end{array}\right.---\left(17\right)$

通过数学平台的失准角φ建立数学平台系与地理坐标系之间的方向余弦矩阵为：

$C_p^n=\left[\begin{array}{ccc}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]---\left(18\right)$

将式(15)、式(16)、式(18)代入式(14)，展开过程中代入式(17)并忽略δγ、δθ和δψ的二阶小量，令式(14)左右两端矩阵元素相等得：

$\left\{\begin{array}{c}\mathrm{\δ\γ}=-\frac{\sin \mathrm{\ψ}}{\cos \mathrm{\θ}}\·{\mathrm{\φ}}_E-\frac{\cos \mathrm{\ψ}}{\cos \mathrm{\θ}}\·{\mathrm{\φ}}_N\\ \mathrm{\δ\θ}=-\cos \mathrm{\ψ}\·{\mathrm{\φ}}_E+\sin \mathrm{\ψ}\·{\mathrm{\φ}}_N\\ \mathrm{\δ\ψ}=-\tan \mathrm{\θ}\sin \mathrm{\ψ}\·{\mathrm{\φ}}_E-\tan \mathrm{\θ}\cos \mathrm{\ψ}\·{\mathrm{\φ}}_N+{\mathrm{\φ}}_U\end{array}\right.---\left(19\right)$

式(19)即为姿态角误差、航向角误差与数学平台失准角之间的观测关系，式(18)和式(19)中φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角。

视量测噪声为白噪声，建立系统的观测方程如下：

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

式(20)中，Z(t)为t时刻系统的观测向量，H(t)为系统的观测矩阵，V(t)为观测白噪声向量。

系统的观测向量为：

Z(t)＝[δv_E δv_N δγ δθ δψ]^T    (21)

系统的观测矩阵为：

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& -\sin \mathrm{\ψ}/\cos \mathrm{\θ}& -\cos \mathrm{\ψ}/\cos \mathrm{\θ}& 0& 0& 0& 0& 0& 0\\ 0& 0& -\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0& 0& 0& 0& 0& 0\\ 0& 0& -\tan \mathrm{\θ}\sin \mathrm{\ψ}& -\tan \mathrm{\θ}\cos \mathrm{\ψ}& 1& 0& 0& 0& 0& 0\end{array}\right]---\left(22\right)$

②获取精对准系统的观测量

在静基座条件下，以步骤(C)中式(8)输出的惯性导航系统东向和北向的速度信息作为速度误差观测量，即

δv_E＝v_E                       (23)

δv_N＝v_N

获取姿态误差观测量的特征在于利用加速度计倾角传感器测量原理估计运载体的姿态，即

在地理坐标系下，加速度计输出的比力为

$f_n={\stackrel{\·}{v}}_{\mathrm{en}}+({2\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})\×v_{\mathrm{en}}-g_n=-g_n=\left[\begin{array}{c}0\\ 0\\ G\end{array}\right]---\left(24\right)$

式(24)中的v_en为地理坐标系n相对于地球坐标系e的速度，

为地理坐标系n相对于地球坐标系e的加速度，ω_en为地理坐标系n相对于地球坐标系e的角速度，g_n为地理坐标系下的重力加速度矢量，G为重量加速度矢量的大小。

加速度计的实际测量输出为f_b，而

$f_b=C_n^b{f}_n---\left(25\right)$

将式(24)代入式(25)计算横滚角γ_D和俯仰角θ_D得：

${\mathrm{\γ}}_D=-\arctan \left(\frac{f_x}{f_z}\right),$ ${\mathrm{\θ}}_D=\arcsin \left(\frac{f_y}{g}\right)---\left(26\right)$

式(26)中的f_x，f_y，f_z为加速度计的实际测量输出f_b的三个轴向分量。

则以步骤(C)输出的惯性导航系统姿态即式(7)和加速度计估计的姿态即式(26)两者之差作为姿态误差观测量，即

$\left\{\begin{array}{c}\mathrm{\δ\γ}={\mathrm{\γ}}_I-{\mathrm{\γ}}_D\\ \mathrm{\δ\θ}={\mathrm{\θ}}_I-{\mathrm{\θ}}_D\end{array}\right.---\left(27\right)$

以步骤(C)输出的惯性导航系统航向ψ_I和外部磁传感器输出的航向ψ_M两者之差作为航向误差观测量，即

δψ＝ψ_I-ψ_M                          (28)

综合式(23)、式(27)和式(28)，得该精对准系统的观测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ \mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{v}_E\\ v_N\\ {\mathrm{\γ}}_I-{\mathrm{\γ}}_D\\ {\mathrm{\θ}}_I-{\mathrm{\θ}}_D\\ {\mathrm{\ψ}}_I-{\mathrm{\ψ}}_M\end{array}\right]---\left(29\right)$

③进行卡尔曼滤波状态估计：在步骤①和②基础上利用标准的卡尔曼滤波方程进行迭代计算，对式(10)中各状态量进行估计。

(E)滤波输出及修正步骤：在步骤(C)获得的运载体姿态角、速度信息中，扣除步骤(D)估计出的系统速度误差、姿态角误差，得到滤波输出的姿态角、速度导航参数，同时将步骤(A)输出的角速度和比力信息扣除步骤(D)估计出的加速度计误差和陀螺仪误差作为步骤(C)的输入信号；

(F)精对准完成步骤：重复步骤(A)、(C)、(D)、(E)进行迭代计算，直至滤波器收敛，由步骤(D)滤波输出的姿态角和航向角建立精确的初始姿态矩阵，同时即实现了航向的对准。

本发明的方法具有以下优点：(1)仅增加磁传感器作为外观测设备，非常经济实惠，适用于低成本捷联惯性导航系统；(2)无需提供精密的位置转台，工程可实现性、可操作性高、成本低；(3)有效提高了低成本捷联惯性导航系统的对准精度和快速性，并且实现了航向的对准。

四、说明书附图

图1捷联惯性导航系统精对准结构图；

图1中符号名称：v_E、v_N分别表示惯性导航系统输出的东向和北向的速度；γ_I、θ_I、ψ_I分别表示惯性导航系统输出的横滚角、俯仰角和航向角；γ_D、θ_D分别表示加速度计估计的载体横滚角和俯仰角；ψ_M表示外部磁传感器输出的航向角。

图2东向失准角估计误差对比曲线；

图3北向失准角估计误差对比曲线；

图4方位失准角估计误差对比曲线；

五、具体实施方式

本发明的精对准系统如图1所示，主要目的是改进传统的以速度误差作为观测量的精对准方法在可观性和可观度方面的不足，同时避免现有文献中的多位置对准方法存在的实用性、可操作性方面的问题，针对低成本捷联惯性导航系统提出一种成本低、实用性好、精度高、对准时间短的精对准方法，同时实现低成本捷联惯性导航系统的航向对准。为了达到这个目的，需要完成如下的工作：

(1)惯性测量组件(简称IMU)信号采集步骤：以一定的采样速率采集IMU的输出信号，得到运载体的角速度和比力信息；

(2)捷联惯性导航系统解析式粗对准步骤

在粗对准时，加速度计测量的是重力加速度矢量g在机体坐标系b中的分量，陀螺仪测量的是地球角速率矢量Ω在机体坐标系b中的分量。这两个矢量在导航坐标系n中的分量已知，且为常值。则机体坐标系相对地理坐标系的姿态矩阵可通过如下方式进行估计，C_n ^b为导航坐标系n相对机体坐标系b的姿态矩阵，C_b ⁿ为机体坐标系b相对导航坐标系n的姿态矩阵。

重力加速度矢量和地球角速率矢量的变换式为

$g^b=C_n^b{g}^n---\left(1\right)$

${\mathrm{\Ω}}^b=C_n^b{\mathrm{\Ω}}^n---\left(2\right)$

定义第三个矢量V＝g×Ω，则其变换式为

$V^b=C_n^b{V}^n---\left(3\right)$

由于 $C_b^n={\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T,$ 则由式(1)、式(2)和式(3)的矢量关系得机体坐标系b相对导航坐标系n的姿态矩阵：

$C_b^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {\left({\mathrm{\Ω}}^n\right)}^T\\ {\left(V^n\right)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(g^b\right)}^T\\ {\left({\mathrm{\Ω}}^b\right)}^T\\ {\left(V^b\right)}^T\end{array}\right]---\left(4\right)$

则由步骤(1)和式(4)获得载体初始姿态矩阵C_b ⁿ的估计值；

(3)捷联惯性导航系统解算步骤

利用步骤(1)得到的角速度和比力信息，以及由步骤(2)得到的载体初始姿态矩阵即式(4)按捷联惯性导航算法的流程，解算出运载体的姿态、航向和速度信息。捷联惯性导航系统解算的初始姿态和航向由步骤(2)确定，初始速度和位置由外部输入。

导航坐标系选择东北天地理坐标系，则机体坐标系相对于地理坐标系的角速度在机体系的三个轴向分量依次为绕横滚轴的角速度ω_x，绕俯仰轴的角速度ω_y，绕方位轴的角速度ω_z，计算周期为Δt，Δt时间内机体转过的角度Δθ的三个分量依次为横滚轴角增量Δθ_x，俯仰轴角增量Δθ_y，方位轴角增量Δθ_z，则转过的角度以矩阵方式表示为：

$\mathrm{\Δ\θ}=\left[\begin{array}{ccc}0& {-\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z& 0& {-\mathrm{\Δ\θ}}_x\\ {-\mathrm{\Δ\θ}}_y& {\mathrm{\Δ\θ}}_x& 0\end{array}\right]=\left[\begin{array}{ccc}0& {-\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& {-\mathrm{\ω}}_x\\ {-\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\mathrm{\Δt}---\left(5\right)$

t时刻载体的姿态矩阵为

$C_b^n\left(t\right)=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(6\right)$

式(6)中，γ为载体的横滚角，θ为载体的俯仰角，ψ为载体的航向角。

那么，t+Δt时刻的姿态矩阵由 $C_b^n(t+\mathrm{\Δt})=-\mathrm{\Δ\θ}\·C_b^n\left(t\right)$ 从t时刻的姿态矩阵递推求得，在姿态矩阵递推的初始时刻，即t＝0时，C_b ⁿ(0)的值由步骤(2)中的式(4)确定。将C_b ⁿ(t+Δt)简写为 $\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$ 则t+Δt时刻，捷联惯性导航系统的横滚角γ_I、俯仰角θ_I和航向角ψ_I姿态信息由下式获得：

$\left[\begin{array}{c}{\mathrm{\γ}}_I={\mathrm{tg}}^{-1}\left(\frac{C_{31}}{C_{33}}\right)\\ {\mathrm{\θ}}_I={\sin }^{-1}\left(C_{32}\right)\\ {\mathrm{\ψ}}_I={\mathrm{tg}}^{-1}\left(\frac{C_{12}}{C_{22}}\right)\end{array}\right]---\left(7\right)$

上述式中：

C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγ

C₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγ

C₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ

在姿态解算的基础上，根据当前的姿态和比力，获得地理坐标系中的运动加速度a_E，a_N，a_U，其中，a_E是东向的运动加速度，a_N是北向的运动加速度，a_U是天向的运动加速度。运载体的速度由式(8)递推得到，v_E(t)表示t时刻运载体东向的速度，v_N(t)表示t时刻运载体北向的速度，v_U(t)表示t时刻运载体天向的速度，v_E(t+Δt)，v_N(t+Δt)，v_U(t+Δt)分别表示t+Δt时刻东向、北向和天向的速度。

$\left\{\begin{array}{c}{v}_E(t+\mathrm{\Δt})=v_E\left(t\right)+a_E\mathrm{\Δt}\\ v_N(t+\mathrm{\Δt})=v_N\left(t\right)+a_N\mathrm{\Δt}\\ v_U(t+\mathrm{\Δt})=v_U\left(t\right)+a_U\mathrm{\Δt}\end{array}\right.---\left(8\right)$

(4)捷联惯性导航系统精对准步骤

①设计精对准方法

在静基座条件下，同时以系统的速度误差、姿态误差和航向误差作为观测量，建立精对准系统的状态方程和观测方程，通过卡尔曼滤波对数学平台的失准角进行估计。

②建立精对准系统的状态方程和观测方程

在东北天地理坐标系下，静基座捷联惯性导航系统初始对准的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

式(9)中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态转移矩阵和系统噪声系数矩阵；W(t)为系统的噪声向量。

系统的状态向量为：

X(t)＝[δv_E δv_N φ_E φ_N φ_U __x __y ε_x ε_y ε_z]^T    (10)

系统的噪声向量为：

W(t)＝[w_ax w_ay w_gx w_gy w_gz]^T    (11)

式(10)和式(11)中，δv_E、δv_N表示系统东向和北向的速度误差，φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角，__x、__y表示x轴和y轴加速度计的误差，ε_x、ε_y、ε_z表示三轴陀螺仪的误差；w_ax、w_ay表示x轴和y轴加速度计的测量白噪声，w_gx、w_gy、w_gz表示三轴陀螺仪的测量白噪声。

系统的状态转移矩阵为：

$A\left(t\right)=\left[\begin{array}{cccccccccc}0& 2{\mathrm{\Ω}}_U& 0& -g& 0& C_{11}& C_{12}& 0& 0& 0\\ -2{\mathrm{\Ω}}_U& 0& g& 0& 0& C_{21}& C_{22}& 0& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& -{\mathrm{\Ω}}_N& 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& -{\mathrm{\Ω}}_U& 0& 0& 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& {\mathrm{\Ω}}_N& 0& 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(12\right)$

系统的噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]---\left(13\right)$

式(12)和式(13)中，Ω_N、Ω_U表示地球角速率矢量在地理坐标系北向和天向的分量，C_ij为机体坐标系相对地理坐标系的姿态矩阵C_b ⁿ的第i行和第j列元素，即

C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγ

C₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγ

C₃₁＝-cosθsinγ                C₃₂＝sinθ    C₃₃＝cosθcosγ

同时以系统的速度误差、姿态角误差、航向角误差作为卡尔曼滤波的观测量，由于姿态角误差、航向角误差描述了机体坐标系与地理坐标系之间的关系，而系统状态方程中的失准角描述了数学平台与地理坐标系之间的关系，其特征在于如何建立姿态角误差、航向角误差观测量对数学平台失准角的观测关系。

实质上，两者存在着如下的转换关系：

$C_b^n=C_p^n{C}_b^p---\left(14\right)$

式中：p代表数学平台坐标系，在东北天地理坐标系下有

$C_b^n=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(15\right)$

$C_b^p=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\′\sin \mathrm{\θ}\′\sin \mathrm{\γ}\′+\cos \mathrm{\ψ}\′\cos \mathrm{\γ}\′& \sin \mathrm{\ψ}\′\cos \mathrm{\θ}\′& -\sin \mathrm{\ψ}\′\sin \mathrm{\θ}\′\cos \mathrm{\γ}\′+\cos \mathrm{\ψ}\′\sin \mathrm{\γ}\′\\ \cos \mathrm{\ψ}\′\sin \mathrm{\θ}\′\sin \mathrm{\γ}\′-\sin \mathrm{\ψ}\′\cos \mathrm{\γ}\′& \cos \mathrm{\ψ}\′\cos \mathrm{\θ}\′& -\cos \mathrm{\ψ}\′\sin \mathrm{\θ}\′\cos \mathrm{\γ}\′-\sin \mathrm{\ψ}\′\sin \mathrm{\γ}\′\\ -\cos \mathrm{\θ}\′\sin \mathrm{\γ}\′& \sin \mathrm{\θ}\′& \cos \mathrm{\θ}\′\cos \mathrm{\γ}\′\end{array}\right]---\left(16\right)$

式中：γ、θ、ψ分别为运载体理想情况下的横滚角、俯仰角、航向角；γ′、θ′、ψ′分别为运载体实际情况下的横滚角、俯仰角和航向角。定义δγ、δθ和δψ分别为横滚角、俯仰角和航向角误差，则有如下的关系：

$\left\{\begin{array}{c}\mathrm{\δ\γ}=\mathrm{\γ}\′-\mathrm{\γ}\\ \mathrm{\δ\θ}=\mathrm{\θ}\′-\mathrm{\θ}\\ \mathrm{\δ\ψ}=\mathrm{\ψ}\′-\mathrm{\ψ}\end{array}\right.---\left(17\right)$

通过数学平台的失准角φ(设定为小量)，建立数学平台系与地理坐标系之间的方向余弦矩阵为：

$C_p^n=\left[\begin{array}{ccc}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]---\left(18\right)$

将式(15)、式(16)、式(18)代入式(14)，展开过程中代入式(17)并忽略δγ、δθ和δψ的二阶小量，令式(14)左右两端矩阵元素相等得：

$\left\{\begin{array}{c}\mathrm{\δ\γ}=-\frac{\sin \mathrm{\ψ}}{\cos \mathrm{\θ}}\·{\mathrm{\φ}}_E-\frac{\cos \mathrm{\ψ}}{\cos \mathrm{\θ}}\·{\mathrm{\φ}}_N\\ \mathrm{\δ\θ}=-\cos \mathrm{\ψ}\·{\mathrm{\φ}}_E+\sin \mathrm{\ψ}\·{\mathrm{\φ}}_N\\ \mathrm{\δ\ψ}=-\tan \mathrm{\θ}\sin \mathrm{\ψ}\·{\mathrm{\φ}}_E-\tan \mathrm{\θ}\cos \mathrm{\ψ}\·{\mathrm{\φ}}_N+{\mathrm{\φ}}_U\end{array}\right.---\left(19\right)$

式(19)即为姿态角误差、航向角误差与数学平台失准角之间的观测关系，式(18)和式(19)中φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角。

视量测噪声为白噪声，建立系统的观测方程如下：

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

式(20)中，Z(t)为t时刻系统的观测向量，H(t)为系统的观测矩阵，V(t)为观测白噪声向量。

系统的观测向量为：

Z(t)＝[δv_E δv_N δγ δθ δψ]^T    (21)

系统的观测矩阵为：

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& -\sin \mathrm{\ψ}/\cos \mathrm{\θ}& -\cos \mathrm{\ψ}/\cos \mathrm{\θ}& 0& 0& 0& 0& 0& 0\\ 0& 0& -\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0& 0& 0& 0& 0& 0\\ 0& 0& -\tan \mathrm{\θ}\sin \mathrm{\ψ}& -\tan \mathrm{\θ}\cos \mathrm{\ψ}& 1& 0& 0& 0& 0& 0\end{array}\right]---\left(22\right)$

③获取精对准系统的观测量

在静基座条件下，以步骤(3)中式(8)输出的惯性导航系统东向和北向的速度信息作为速度误差观测量，即

δv_E＝v_E                     (23)

δv_N＝v_N

获取姿态误差观测量的特征在于利用加速度计倾角传感器测量原理估计运载体的姿态，即

在地理坐标系下，加速度计输出的比力为

$f_n={\stackrel{\·}{v}}_{\mathrm{en}}+({2\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})\×v_{\mathrm{en}}-g_n=-g_n=\left[\begin{array}{c}0\\ 0\\ G\end{array}\right]---\left(24\right)$

式(24)中的v_en为地理坐标系n相对于地球坐标系e的速度，

为地理坐标系n相对于地球坐标系e的加速度，ω_en为地理坐标系n相对于地球坐标系e的角速度，g_n为地理坐标系下的重力加速度矢量，G为重量加速度矢量的大小。

加速度计的实际测量输出为f_b，而

$f_b=C_n^b{f}_n---\left(25\right)$

将式(24)代入式(25)计算横滚角γ_D和俯仰角θ_D得：

${\mathrm{\γ}}_D=-\arctan \left(\frac{f_x}{f_z}\right),$ ${\mathrm{\θ}}_D\text{=arcsin}\left(\frac{f_y}{g}\right)---\left(26\right)$

式(26)中的f_x，f_y，f_z为加速度计的实际测量输出f_b的三个轴向分量。

则以步骤(3)输出的惯性导航系统姿态即式(7)和加速度计估计的姿态即式(26)两者之差作为姿态误差观测量，即

$\left\{\begin{array}{c}\mathrm{\δ\γ}={\mathrm{\γ}}_I-{\mathrm{\γ}}_D\\ \mathrm{\δ\θ}={\mathrm{\θ}}_I-{\mathrm{\θ}}_D\end{array}\right.---\left(27\right)$

获取航向误差观测量的特征在于利用外部磁传感器输出的航向提供航向参考基准，则以步骤(3)输出的惯性导航系统航向ψ_I和外部磁传感器输出的航向ψ_M两者之差作为航向误差观测量，即

δψ＝ψ_I-ψ_M                 (28)

综合式(23)、式(27)和式(28)，得该精对准系统的观测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ \mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{v}_E\\ v_N\\ {\mathrm{\γ}}_I-{\mathrm{\γ}}_D\\ {\mathrm{\θ}}_I-{\mathrm{\θ}}_D\\ {\mathrm{\ψ}}_I-{\mathrm{\ψ}}_M\end{array}\right]---\left(29\right)$

④进行卡尔曼滤波状态估计：在步骤②和③基础上利用标准的卡尔曼滤波方程进行迭代计算，对(10)式中各状态量进行估计。

(5)滤波输出及修正步骤：在步骤(3)获得的运载体姿态角、速度信息中，扣除步骤(4)估计出的系统速度误差、姿态角误差，得到滤波输出的姿态角、速度等导航参数，同时将步骤(1)输出的角速度和比力信息扣除步骤(4)估计出的加速度计误差和陀螺仪误差作为步骤(3)的输入信号；

(6)精对准完成步骤：重复步骤(1)、(3)、(4)、(5)进行迭代计算，直至滤波器收敛，由步骤(4)滤波输出的姿态角和航向角建立精确的初始姿态矩阵，同时即实现了航向的对准。

有益效果

本发明充分利用系统所有的可观测信息，同时将速度误差、姿态角误差和航向角误差作为观测量进行精对准：不仅利用加速度计倾角传感器的测量原理对姿态角进行估计，增加了精对准系统的观测信息，同时引入磁传感器作为外观测设备提供航向基准，解决了方位失准角不可观的问题。该精对准系统的结构如图1所示，该精对准方法大大提高了系统的可观性和可观度，有效缩短了对准时间，提高了对准精度，从而全面提高了低成本捷联惯性导航系统的整体性能。

对本发明的有益效果说明如下：

(1)可观性和可观度的分析与比较

捷联惯性导航系统在静基座对准时可以近似为线性定常系统，其可观性矩阵为：Q＝[H HA HA² HA³ HA⁴]^T。其中，A为系统的状态转移矩阵，H为系统的观测矩阵。

比较传统的以速度误差作为观测量的精对准方法与本发明中同时以速度误差、姿态角误差和航向角误差作为观测量的方法，可观性矩阵的秩如表1所示。

表1可观性矩阵秩的比较

序号	观测量	rank(Q)
			1	δvE、δvN	7
2	δvE、δvN、δγ、δθ、δψ	10

由表1可知，本发明的方法可观性矩阵满秩，精对准系统变为完全可观测系统。

采用基于可观性矩阵特征值和特征向量的判别方法，可以考察线性定常系统中状态的可观度。可观性矩阵Q的正定对称矩阵Q^TQ经单位化后的特征值λ(0≤λ≤1)指示了其特征向量所对应的状态向量(或线性组合)的可观度。λ越大，其特征向量所对应的状态向量(或线性组合)的可观度越高。

比较传统的以速度误差作为观测量的精对准方法和本发明的方法对应的可观性矩阵单位化特征值和特征向量的情况，如表2所示(特征值和状态之间的对应关系由特征向量指示)。

表2状态的可观度的比较

λ		λ1	λ2	λ3	λ4	λ5	λ6	λ7	λ8	λ9	λ10
												对应状态		εx和εy	_y	εx和εz	_x	φU	εy	φE	φN	δvE	δvN
序号	1	6.56e-7	0.0046	3.81e-16	2.49e-10	0.002	3.79e-6	0.988	0.988	1.00	1.00
												2	0.014	0.111	0.0105	0.187	0.602	0.003	0.997	0.997	1.00	1.00

由表2可知，本发明的方法中，与方位失准角φ_U相对应的特征值λ₅明显增大，φ_U的可观度大大提高；与器件误差相对应的特征值λ₁～λ₄、λ₆也得到了不同程度的提高，器件误差的可观度有较大改善。

因此，系统可观度的分析与秩的检验结果一致，本发明的精对准方法充分利用外部可观测信息有效提高了系统的观测性能，特别是方位失准角的观测能力。

(2)设定仿真条件如下：捷联惯性导航系统经过粗对准；卡尔曼滤波器模型中状态变量的初始值X(0)＝0；陀螺常值漂移取为1.0°/h，随机漂移取为1.0°/h；加速度计零偏取为1×10^-4g，随机偏差取为1×10^-4g；磁传感器的误差取方差为0.3°的高斯白噪声；惯导所处位置的纬度为L＝32.0°。

分别利用传统的以速度误差作为观测量的精对准方法与本发明中同时以速度误差、姿态角误差和航向角误差作为观测量的方法，对捷联惯性导航系统进行精对准的仿真计算。其对比曲线如图2至图4所示，其中图2为东向失准角估计误差对比曲线，图3为北向失准角估计误差对比曲线，图4为方位失准角估计误差对比曲线。

结果表明，本发明的方法能够大大加快数学平台失准角的收敛速度。特别是方位失准角，传统的方法对方位失准角的估计几乎没有效果，本发明的方法引入航向角误差这一观测量使得方位失准角φ_U的观测度得到了大幅度提高，大大缩短了对准时间、提高了对准精度，其收敛时间减小到200s左右，估计误差减小到750″左右。此外，充分利用可观测信息，系统变为完全可观测系统，更好的估计了陀螺仪和加速度计的误差，从而使东向失准角φ_E和北向失准角φ_N的收敛时间大大加快、估计精度也有一定的提高。

Claims (1)

1.一种捷联惯性导航系统的快速精对准方法，其特征在于该方法包括下列步骤：

(A)采集惯性测量组件的输出信号，得到运载体的角速度和比力信息；

(B)捷联惯性导航系统解析式粗对准步骤

在粗对准时，利用加速度计测量重力加速度矢量g在机体坐标系b中的分量；利用陀螺仪测量地球角速率矢量Ω在机体坐标系b中的分量，则机体坐标系相对导航坐标系的姿态矩阵通过如下方式进行估计，C_n ^b为导航坐标系n相对机体坐标系b的姿态矩阵，C_b ⁿ为机体坐标系b相对导航坐标系n的姿态矩阵，

重力加速度矢量和地球角速率矢量的变换式为

$g^b=C_n^b{g}^n---\left(1\right)$

${\mathrm{\Ω}}^b=C_n^b{\mathrm{\Ω}}^n---\left(2\right)$

定义第三个矢量V＝g×Ω，则其变换式为

$V^b=C_n^b{V}^n---\left(3\right)$

由于 $C_b^n={\left(C_n^b\right)}^{-1}={\left(C_n^b\right)}^T,$ 则由式(1)、式(2)和式(3)的矢量关系得机体坐标系b相对导航坐标系n的姿态矩阵：

$C_b^n={\left[\begin{array}{c}{\left(g^n\right)}^T\\ {\left({\mathrm{\Ω}}^n\right)}^T\\ {\left(V^n\right)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left(g^b\right)}^T\\ {\left({\mathrm{\Ω}}^b\right)}^T\\ {\left(V^b\right)}^T\end{array}\right]---\left(4\right)$

则由步骤(A)和式(4)获得载体初始姿态矩阵C_b ⁿ的估计值；

(C)捷联惯性导航系统解算步骤

利用步骤(A)得到的角速度和比力信息，以及由步骤(B)得到的载体初始姿态矩阵即式(4)，按捷联惯性导航算法的流程，解算出运载体的姿态角、航向角和速度信息，

导航坐标系选择东北天地理坐标系，则机体坐标系相对于地理坐标系的角速度在机体系的三个轴向分量依次为绕横滚轴的角速度ω_x，绕俯仰轴的角速度ω_y，绕方位轴的角速度ω_z，计算周期为Δt，Δt时间内机体转过的角度Δθ的三个分量依次为横滚轴角增量Δθ_x，俯仰轴角增量Δθ_y，方位轴角增量Δθ_z，则转过的角度以矩阵方式表示为：

$\mathrm{\Δ\θ}=\left[\begin{array}{ccc}0& {-\mathrm{\Δ\θ}}_z& {\mathrm{\Δ\θ}}_y\\ {\mathrm{\Δ\θ}}_z& 0& {-\mathrm{\Δ\θ}}_x\\ {-\mathrm{\Δ\θ}}_y& {\mathrm{\Δ\θ}}_x& 0\end{array}\right]=\left[\begin{array}{ccc}0& {-\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& {-\mathrm{\ω}}_x\\ {-\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]\mathrm{\Δt}---\left(5\right)$

t时刻载体的姿态矩阵为：

$C_b^n\left(t\right)=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(6\right)$

式(6)中，γ为载体的横滚角，θ为载体的俯仰角，ψ为载体的航向角，那么，t+Δt时刻的姿态矩阵由 $C_b^n=(t+\mathrm{\Δt})=-\mathrm{\Δ\θ}\·C_b^n\left(t\right)$ 从t时刻的姿态矩阵递推求得，在姿态矩阵递推的初始时刻，即t＝0时，C_b ⁿ(0)的值由步骤(B)中的式(4)确定，将C_b ⁿ(t+Δt)简写为 $\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right],$ 则t+Δt时刻，捷联惯性导航系统的横滚角γ_I、俯仰角θ_I和航向角ψ_I姿态信息由下式获得：

$\left\{\begin{array}{c}{\mathrm{\γ}}_I={\mathrm{tg}}^{-1}\left(\frac{C_{31}}{C_{33}}\right)\\ {\mathrm{\θ}}_I={\sin }^{-1}\left(C_{32}\right)\\ {\mathrm{\ψ}}_I={\mathrm{tg}}^{-1}\left(\frac{C_{12}}{C_{22}}\right)\end{array}\right.---\left(7\right)$

上述式中：

C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγ

C₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγ

C₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ

在姿态解算的基础上，根据当前的姿态和比力，获得地理坐标系中的运动加速度a_E，a_N，a_U，其中，a_E是东向的运动加速度，a_N是北向的运动加速度，a_U是天向的运动加速度，运载体的速度由式(8)递推得到，v_E(t)表示t时刻运载体东向的速度，v_N(t)表示t时刻运载体北向的速度，v_U(t)表示t时刻运载体天向的速度，v_E(t+Δt)，v_N(t+Δt)，v_U(t+Δt)分别表示t+Δt时刻东向、北向和天向的速度，即：

$\left\{\begin{array}{c}{v}_E(t+\mathrm{\Δt})=v_E\left(t\right)+a_E\mathrm{\Δt}\\ v_N(t+\mathrm{\Δt})=v_N\left(t\right)+a_N\mathrm{\Δt}\\ v_U(t+\mathrm{\Δt})=v_U\left(t\right)+a_U\mathrm{\Δt}\end{array}\right.---\left(8\right)$

(D)捷联惯性导航系统精对准步骤，包括建立精对准系统的状态方程和观测方程，获取精对准系统的观测量，进行卡尔曼滤波状态估计，

①建立精对准系统的状态方程和观测方程

在东北天地理坐标系下，静基座捷联惯性导航系统初始对准的状态方程为：

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

式(9)中，X(t)为t时刻系统的状态向量；A(t)，G(t)分别为系统状态转移矩阵和系统噪声系数矩阵；W(t)为系统的噪声向量，

系统的状态向量为：

X(t)＝[δv_E δv_N φ_E φ_N φ_U __x __y ε_x ε_y ε_z]^T    (10)

系统的噪声向量为：

W(t)＝[w_ax w_ay w_gx w_gy w_gz]^T    (11)

式(10)和式(11)中，δv_E、δv_N表示系统东向和北向的速度误差，φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角，__x、__y表示x轴和y轴加速度计的误差，ε_x、ε_y、ε_z表示三轴陀螺仪的误差；w_ax、w_ay表示x轴和y轴加速度计的测量白噪声，w_gx、w_gy、w_gz表示三轴陀螺仪的测量白噪声，

系统的状态转移矩阵为：

$A\left(t\right)=\left[\begin{array}{cccccccccc}0& 2{\mathrm{\Ω}}_U& 0& -g& 0& C_{11}& C_{12}& 0& 0& 0\\ -2{\mathrm{\Ω}}_U& 0& g& 0& 0& C_{21}& C_{22}& 0& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& -{\mathrm{\Ω}}_N& 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& -{\mathrm{\Ω}}_U& 0& 0& 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& {\mathrm{\Ω}}_N& 0& 0& 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]---\left(12\right)$

系统的噪声系数矩阵为：

$G\left(t\right)=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& 0& 0& 0\\ C_{21}& C_{22}& 0& 0& 0\\ 0& 0& C_{11}& C_{12}& C_{13}\\ 0& 0& C_{21}& C_{22}& C_{23}\\ 0& 0& C_{31}& C_{32}& C_{33}\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]---\left(13\right)$

式(12)和式(13)中，Ω_N、Ω_U表示地球角速率矢量在地理坐标系北向和天向的分量，C_ij为机体坐标系相对地理坐标系的姿态矩阵C_b ⁿ的第i行和第j列元素，即

C₁₁＝sinψsinθsinγ+cosψcosγ C₁₂＝sinψcosθ C₁₃＝-sinψsinθcosγ+cosψsinγ

C₂₁＝cosψsinθsinγ-sinψcosγ C₂₂＝cosψcosθ C₂₃＝-cosψsinθcosγ-sinψsinγ

C₃₁＝-cosθsinγ                C₃₂＝sinθ      C₃₃＝cosθcosγ

建立姿态角误差、航向角误差观测量对数学平台失准角的观测关系，实质上，两者存在着如下的转换关系：

$C_b^n=C_p^n{C}_b^p---\left(14\right)$

式(14)中，p代表数学平台坐标系，在东北天地理坐标系下有

$C_b^n=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]---\left(15\right)$

$C_b^p=\left[\begin{array}{ccc}\sin \mathrm{\ψ}\′\sin \mathrm{\θ}\′\sin \mathrm{\γ}\′+\cos \mathrm{\ψ}\′\cos \mathrm{\γ}\′& \sin \mathrm{\ψ}\′\cos \mathrm{\θ}\′& -\sin \mathrm{\ψ}\′\sin \mathrm{\θ}\′\cos \mathrm{\γ}\′+\cos \mathrm{\ψ}\′\sin \mathrm{\γ}\′\\ \cos \mathrm{\ψ}\′\sin \mathrm{\θ}\′\sin \mathrm{\γ}\′-\sin \mathrm{\ψ}\′\cos \mathrm{\γ}\′& \cos \mathrm{\ψ}\′\cos \mathrm{\θ}\′& -\cos \mathrm{\ψ}\′\sin \mathrm{\θ}\′\cos \mathrm{\γ}\′-\sin \mathrm{\ψ}\′\sin \mathrm{\γ}\′\\ -\cos \mathrm{\θ}\′\sin \mathrm{\γ}\′& \sin \mathrm{\θ}\′& \cos \mathrm{\θ}\′\cos \mathrm{\γ}\′\end{array}\right]---\left(16\right)$

式(15)和式(16)中：γ、θ、ψ分别为运载体理想情况下的横滚角、俯仰角、航向角；γ′、θ′、ψ′分别为运载体实际情况下的横滚角、俯仰角和航向角，定义δγ、δθ和δψ分别为横滚角、俯仰角和航向角误差，则有如下的关系：

$\left\{\begin{array}{c}\mathrm{\δ\γ}=\mathrm{\γ}\′-\mathrm{\γ}\\ \mathrm{\δ\θ}=\mathrm{\θ}\′-\mathrm{\θ}\\ \mathrm{\δ\ψ}=\mathrm{\ψ}\′-\mathrm{\ψ}\end{array}\right.---\left(17\right)$

通过数学平台的失准角φ建立数学平台系与地理坐标系之间的方向余弦矩阵为：

$C_p^n=\left[\begin{array}{ccc}1& {-\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 1& {-\mathrm{\φ}}_E\\ {-\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 1\end{array}\right]---\left(18\right)$

将式(15)、式(16)、式(18)代入式(14)，展开过程中代入式(17)并忽略δγ、δθ和δψ的二阶小量，令式(14)左右两端矩阵元素相等得：

$\left\{\begin{array}{c}\mathrm{\δ\γ}=-\frac{\sin \mathrm{\ψ}}{\cos \mathrm{\θ}}\·{\mathrm{\φ}}_E-\frac{\cos \mathrm{\ψ}}{\cos \mathrm{\θ}}\·{\mathrm{\φ}}_N\\ \mathrm{\δ\θ}=-\cos \mathrm{\ψ}\·{\mathrm{\φ}}_E+\sin \mathrm{\ψ}\·{\mathrm{\φ}}_N\\ \mathrm{\δ\ψ}=-\tan \mathrm{\θ}\sin \mathrm{\ψ}\·{\mathrm{\φ}}_E-\tan \mathrm{\θ}\cos \mathrm{\ψ}\·{\mathrm{\φ}}_N+{\mathrm{\φ}}_U\end{array}\right.---\left(19\right)$

式(19)即为姿态角误差、航向角误差与数学平台失准角之间的观测关系，式(18)和式(19)中φ_E、φ_N、φ_U表示数学平台的东向、北向和方位失准角，

视量测噪声为白噪声，建立系统的观测方程如下：

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

式(20)中，Z(t)为t时刻系统的观测向量，H(t)为系统的观测矩阵，V(t)为观测白噪声向量，

系统的观测向量为：

Z(t)＝[δv_E δv_N δγ δθ δψ]^T    (21)

系统的观测矩阵为：

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& -\sin \mathrm{\ψ}/\cos \mathrm{\θ}& -\cos \mathrm{\ψ}/\cos \mathrm{\θ}& 0& 0& 0& 0& 0& 0\\ 0& 0& -\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0& 0& 0& 0& 0& 0\\ 0& 0& -\tan \mathrm{\θ}\sin \mathrm{\ψ}& -\tan \mathrm{\θ}\cos \mathrm{\ψ}& 1& 0& 0& 0& 0& 0\end{array}\right]---\left(22\right)$

②获取精对准系统的观测量

在静基座条件下，以步骤(C)中式(8)输出的惯性导航系统东向和北向的速度信息作为速度误差观测量，即

δv_E＝v_E                  (23)

δv_N＝v_N

获取姿态误差观测量的特征在于利用加速度计倾角传感器测量原理估计运载体的姿态，即

在地理坐标系下，加速度计输出的比力为

$f_n={\stackrel{\·}{v}}_{\mathrm{en}}+({2\mathrm{\ω}}_{\mathrm{ie}}+{\mathrm{\ω}}_{\mathrm{en}})\×v_{\mathrm{en}}-g_n=-g_n=\left[\begin{array}{c}0\\ 0\\ G\end{array}\right]---\left(24\right)$

式(24)中的v_en为地理坐标系n相对于地球坐标系e的速度，为地理坐标系n相对于地球坐标系e的加速度，ω_en为地理坐标系n相对于地球坐标系e的角速度，g_n为地理坐标系下的重力加速度矢量，G为重量加速度矢量的大小，

加速度计的实际测量输出为f_b，而

$f_b=C_n^b{f}_n---\left(25\right)$

将式(24)代入式(25)计算横滚角γ_D和俯仰角θ_D得：

${\mathrm{\γ}}_D=-\arctan \left(\frac{f_x}{f_z}\right),$ ${\mathrm{\θ}}_D=\arcsin \left(\frac{f_y}{g}\right)---\left(26\right)$

式(26)中的f_x，f_y，f_z为加速度计的实际测量输出f_b的三个轴向分量，

则以步骤(C)输出的惯性导航系统姿态即式(7)和加速度计估计的姿态即式(26)两者之差作为姿态误差观测量，即

$\left\{\begin{array}{c}\mathrm{\δ\γ}={\mathrm{\γ}}_I-{\mathrm{\γ}}_D\\ \mathrm{\δ\θ}={\mathrm{\θ}}_I-{\mathrm{\θ}}_D\end{array}\right.---\left(27\right)$

以步骤(C)输出的惯性导航系统航向ψ_I和外部磁传感器输出的航向ψ_M两者之差作为航向误差观测量，即

δψ＝ψ_I-ψ_M                         (28)

综合式(23)、式(27)和式(28)，得该精对准系统的观测量为：

$Z\left(t\right)=\left[\begin{array}{c}{\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ \mathrm{\δ\γ}\\ \mathrm{\δ\θ}\\ \mathrm{\δ\ψ}\end{array}\right]=\left[\begin{array}{c}{v}_E\\ v_N\\ {\mathrm{\γ}}_I-{\mathrm{\γ}}_D\\ {\mathrm{\θ}}_I-{\mathrm{\θ}}_D\\ {\mathrm{\ψ}}_I-{\mathrm{\ψ}}_M\end{array}\right]---\left(29\right)$

③进行卡尔曼滤波状态估计：在步骤①和②基础上利用标准的卡尔曼滤波方程进行迭代计算，对式(10)中各状态量进行估计；

(E)滤波输出及修正步骤：在步骤(C)获得的运载体姿态角、速度信息中，扣除步骤(D)估计出的系统速度误差、姿态角误差，得到滤波输出的姿态角、速度导航参数，同时将步骤(A)输出的角速度和比力信息扣除步骤(D)估计出的加速度计误差和陀螺仪误差作为步骤(C)的输入信号；

(F)精对准完成步骤：重复步骤(A)、(C)、(D)、(E)进行迭代计算，直至滤波器收敛，由步骤(D)滤波输出的姿态角和航向角建立精确的初始姿态矩阵，同时即实现了航向的对准。

Images