CN102679978B - 一种旋转式捷联惯性导航系统静基座初始对准方法 - Google Patents

Abstract

本发明属于惯性导航技术领域，为了克服现有的初始对准方法中存在的计算量大、收敛速度和精度不理想的问题，本发明提出了一种旋转式捷联惯性导航系统静基座初始对准方法，该方法采用低阶的卡尔曼滤波状态方程模型和相应的自对准外观测量获取方法有效的降低了对准计算过程中的计算量，此外通过旋转IMU（惯性测量单元）优化了卡尔曼滤波性能，从而改善了惯导系统静基座初始对准性能。

Description

一种旋转式捷联惯性导航系统静基座初始对准方法

技术领域

本发明涉及一种旋转式捷联惯导系统在静基座条件下的初始对准方法，属于惯性导航技术领域。

背景技术

捷联惯导系统的初始对准是在较短的时间内，以一定的精度确定载体的初始位置、初始速度以及载体的初始姿态角。捷联惯性导航系统的初始对准通常分为两个阶段：粗对准阶段和精对准阶段。在粗对准阶段，根据重力加速度矢量和地球自转角速率矢量的测量值，直接估算出载体坐标系到导航坐标系的方向余弦矩阵，估算值与真实值存在偏差，但应为小量；精对准可在粗对准基础上采用最小二乘法或卡尔曼滤波完成。

按惯导系统基座的运动状态，惯性导航初始对准可分为动基座对准和静基座对准。动基座对准是在载体运动状态下进行的，而静基座对准是载体未进行线运动情况下的对准，此时载体经纬度位置不变，地速为零。

按对准过程中信息的获取途径，惯性导航初始对准又可分为自对准和传递对准。传递对准就是利用高精度的主惯导系统输出的导航参数或其他设备（如GPS、里程计等）提供的载体运动参数信息，作为外观测量，通过状态滤波技术估计出子惯导系统状态量的值；而自对准是从惯导系统自身的惯性器件输出值中提取载体运动参数信息作为外观测量，通过状态滤波技术估计出状态量的值。其中，传递对准主要应用于动基座对准，而自对准主要应用于静基座对准。

在精对准阶段，惯导系统静基座对准通常采用的方法是基于卡尔曼滤波的状态量估计法，所采用的系统方程为：

$\stackrel{.}{X}=\mathrm{AX}+\mathrm{GW}$

其中，A为系统状态矩阵，W为系统噪声向量，X为系统的状态向量，G为噪声系数矩阵，将这些参数写成矩阵形式分别为：

X＝[δV_eδV_nφ_eφ_nφ_uΔ_xΔ_yΔ_zε_xε_yε_z]^T

$A=\left[\begin{array}{ccccc}0& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -g& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& g& 0& 0\\ 0& -\frac{1}{R}& 0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ \frac{1}{R}& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ \frac{\tan L}{R}& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0\\ & & 0_{6\×11}& & \end{array}{C}_{5\×6}\right]$

W＝[w_axw_ayw_azw_gxw_gyw_gz]^T

$G=\left[\begin{array}{c}{C}_{5\×6}\\ 0_{6\×6}\end{array}\right]$

$C_{5\×6}=\left[\begin{array}{cccccc}{c}_{11}& c_{12}& c_{13}& 0& 0& 0\\ c_{21}& c_{22}& c_{23}& 0& 0& 0\\ 0& 0& 0& c_{11}& c_{12}& c_{13}\\ 0& 0& 0& c_{21}& c_{22}& c_{23}\\ 0& 0& 0& c_{31}& c_{32}& c_{33}\end{array}\right]$

上式中，L为载体纬度位置，R为地球半径，ω_ie为地球自转角速率，c₁₁,c₁₂,…,c₃₃为载体姿态矩阵的各元素，下标e、n、u分别表示地理坐标系的东、北、天方向，下标x、y、z分别表示捷联惯导坐标系的坐标轴方向，δV_i表示载体速度误差，φ_i表示载体姿态失准角，Δ_j、ε_j分别表示加速度计和陀螺的零偏，w_aj、w_gj分别表示加速度计和陀螺的噪声；其中i的取值为e、n、u；j的取值为x、y、z。

在精对准过程中，系统根据粗对准确定的姿态阵和陀螺的实时更新姿态矩阵，在此基础上解算出实时速度，由于载体无移动，所以此速度实际上是速度误差，可将其视为外观测量，从而得到系统的观测方程：

$Z=\mathrm{HX}+W_V$

$=\left[\begin{array}{ccccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]X+\left[\begin{array}{c}{w}_{\mathrm{ve}}\\ w_{\mathrm{vn}}\end{array}\right]$

上式中，Z为系统的观测向量，H为系统的观测矩阵，W_V为系统的观测噪声向量，w_ve，w_vn分别为东、北向速度的观测噪声。

将所求得的状态方程和观测方程进行卡尔曼滤波，估计出姿态角的误差和惯性器件的零偏，即可完成初始对准。

上述方案存在的局限性如下：速度外观测量初始对准方法根据粗对准确定的姿态阵和陀螺的实时更新姿态矩阵，在此基础上需要解算出实时速度作为速度误差，因此导航计算机必须执行复杂的导航解算算法，其中需进行划船效应与涡卷效应等误差的补偿，否则将降低速度解算精度进而降低对准精度；在静基座下方位失准角与各惯性器件零偏的可观测度都很低，导致收敛速度和精度很不理想，而通过载体运动提高可观测度的方法对于一些特殊应用背景（如武器系统平台）无法满足。

发明内容

为了克服现有的速度外观测量初始对准方法中存在的计算量大、收敛速度和精度不理想的问题，本发明提出了一种旋转式捷联惯性导航系统静基座初始对准方法，该方法采用低阶的卡尔曼滤波状态方程模型和相应的自对准外观测量获取方法有效的降低了对准计算过程中的计算量，此外通过旋转IMU（惯性测量单元）优化了卡尔曼滤波性能，从而完成了惯性导航系统静基座初始对准。

一种旋转式捷联惯性导航系统静基座初始对准方法，该方法实现的具体步骤如下：

步骤一、获得旋转式捷联惯性导航系统的载体所在位置的经度λ、纬度L；

步骤二、根据所确定的经度、纬度信息以及惯导系统惯性器件的输出进行粗对准，得到惯性测量单元姿态矩阵的近似估计值；

步骤三、使旋转式捷联惯性导航系统的转轴按一定旋转方案旋转；其中，将内环轴、外环轴旋转的角速率分别记为ω₁、ω₂；

步骤四、按照步骤三所确定的旋转方案旋转的同时，利用粗对准得到的姿态矩阵和惯性测量单元的陀螺实时输出的角速度信息，以更新惯性测量单元的姿态矩阵；

步骤五、通过卡尔曼滤波完成静基座对准，具体步骤如下：

①在载体无移动的静基座条件下，加速度计输出为：

${\stackrel{\→}{f}}^p=C_b^p{C}_N^b{g}^N+\mathrm{\Δ}+w_a$

其中，为载体的姿态矩阵，g^N＝[00-g]^T，为导航坐标系下的重力加速度向量，w_a＝[w_az,w_ay,w_az]^T为加速度计噪声向量，Δ＝[Δ_x,Δ_y,Δ_z]^T为加速度计零偏向量，为惯性导航系统按照步骤三所确定的旋转方案旋转时的旋转方向余弦矩阵；

②根据不同的旋转系统结构建立加速度计输出的数学模型，在加速度计输出的数学模型中，简化粗对准后失准角所对应的三角函数以及忽略失准角在东、北方向上的分量φ_e,φ_n的二阶小量，从而得到加速度计输出的线性化近似数学模型 ${\tilde{f}}^p\≈\left[\begin{array}{c}{\mathrm{gm}}_{11}+{\mathrm{\φ}}_e{\mathrm{gm}}_{12}+{\mathrm{\φ}}_n{\mathrm{gm}}_{13}\\ {\mathrm{gm}}_{21}+{\mathrm{\φ}}_e{\mathrm{gm}}_{22}+{\mathrm{\φ}}_n{\mathrm{gm}}_{23}\\ {\mathrm{gm}}_{31}+{\mathrm{\φ}}_e{\mathrm{gm}}_{32}+{\mathrm{\φ}}_n{\mathrm{gm}}_{33}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right],$ 其中g为当地重力加速度，m₁₁，m₁₂,…m₃₃为旋转方案所对应的观测方程中粗对准得到的惯性测量单元坐标系下重力加速度分量系数，以及重力加速度与姿态失准角耦合量的系数；

③从加速度计的输出中减去重力加速度的估计值，得到失准角与重力加速度的耦合量、加速度计零偏以及加速度计噪声之和，作为卡尔曼滤波的外观测量，观测方程如下：

$Z={\tilde{f}}^p-\begin{array}{}\end{array}\left[\begin{array}{c}{\mathrm{gm}}_{11}\\ {\mathrm{gm}}_{21}\\ {\mathrm{gm}}_{31}\end{array}\right]=\left[\begin{array}{ccccccccc}{\mathrm{gm}}_{12}& {\mathrm{gm}}_{13}& 0& 1& 0& 0& 0& 0& 0\\ {\mathrm{gm}}_{22}& {\mathrm{gm}}_{23}& 0& 0& 1& 0& 0& 0& 0\\ {\mathrm{gm}}_{32}& {\mathrm{gm}}_{33}& 0& 0& 0& 1& 0& 0& 0\end{array}\right]X+\begin{array}{}\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right]\end{array}$

其中，X为系统的状态向量，Z为系统观测向量；

④根据所求得的观测方程以及卡尔曼滤波状态方程进行卡尔曼滤波，通过卡尔曼滤波估计载体姿态失准角误差、加速度计和陀螺零偏，用姿态失准角误差的估计值对载体姿态矩阵进行修正，待卡尔曼滤波达到稳态后，以此时的载体姿态矩阵作为导航解算的初始载体姿态矩阵，从而完成静基座精对准；

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

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u\\ {\stackrel{\·}{\mathrm{\Δ}}}_x\\ {\stackrel{\·}{\mathrm{\Δ}}}_y\\ {\stackrel{\·}{\mathrm{\Δ}}}_z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{ccccccccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& 0& c_{11}& c_{12}& c_{13}\\ {-\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0& 0& 0& 0& c_{21}& c_{22}& c_{23}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 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& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{gx}}\\ w_{\mathrm{gy}}\\ w_{\mathrm{gz}}\end{array}\right]$

上式中，L为载体纬度位置，R为地球半径，ω_ie为地球自转角速率，c₁₁,c₁₂，…,c₃₃为载体姿态矩阵的各元素，下标e、n、u分别表示地理坐标系的东、北、天方向，下标x、y、z分别表示捷联惯导坐标系的坐标轴方向，δV_i表示载体速度误差，φ_i表示载体姿态失准角，Δ_j、ε_j分别表示加速度计和陀螺的零偏，w_aj、w_gj分别表示加速度计和陀螺的噪声；其中i的取值为e、n、u；j的取值为x、y、z。

有益效果

(1)卡尔曼滤波器的运算量与其阶数的三次方成正比。若系统状态方程阶数为n，观测方程阶数为m，则完成一次递推计算需要完成4n³+(1+4m)n²+(2m²+2m)n+m³次乘除运算和4n³+(4m-2)n²-(2m+1)n+m³次加法运算。本发明的卡尔曼滤波状态方程为9阶，观测方程为3阶，而背景技术中所述的传统方法所用状态方程为11阶，观测方程为2阶。由此可以计算出本发明的卡尔曼滤波乘除法、加法运算量分别下降至速度外观测量初始对准方法（传统方法）的64.28%和61.19%。

(2)速度观测量初始对准方法（传统方法）根据粗对准确定的姿态阵和陀螺的实时更新姿态矩阵，在此基础上需要解算出实时速度作为速度误差；而按照本发明方法提取的外观测量是姿态角误差与加速度计零偏的线性组合，外观测量是姿态角误差与加速度计零偏，因此无需速度解算，可以进一步节省初始对准运算量。

(3)在捷联式惯导系统中引入IMU旋转机构，可以通过旋转使IMU处于角运动之中，角运动可以提高初始对准可观测性，从而改善卡尔曼滤波的收敛速度与精度，旋转IMU可以进行在捷联式惯导系统中无法实现的角运动机动方式，因此在改善卡尔曼滤波性能方面的效果尤为显著。

附图说明

图1(a)～(e)分别为仿真实验①中，捷联式惯导系统与应用本发明的静基座初始对准方法的旋转式惯导系统进行初始对准卡尔曼滤波过程中各状态量的估计误差;

图2(a)～(e)分别为仿真实验②中，捷联式惯导系统与应用本发明的静基座初始对准方法的旋转式惯导系统进行初始对准卡尔曼滤波过程中各状态量的估计误差;

图3(a)～(e)分别为仿真实验③中，捷联式惯导系统与应用本发明的静基座初始对准方法的旋转式惯导系统进行初始对准卡尔曼滤波过程中各状态量的估计误差。

具体实施方式

为了进一步说明本发明的目的和优点，下面结合附图和实施例详细描述本发明的具体实施方式。

步骤一、通过全球导航卫星系统或其他途径获得旋转式捷联惯性导航系统的载体所在位置的经度λ、纬度L；将载体位置信息导入导航计算机，此时静基座条件下载体的地速度应为零。

步骤二、根据所确定的经度、纬度信息以及惯导系统惯性器件的输出进行粗对准，得到惯性测量单元姿态矩阵的近似估计值；

在本步骤中，常用的方法为解析式粗对准方法，求解公式如下：

${\hat{C}}_p^N={\left[\begin{array}{c}{\left(g^N\right)}^T\\ {\left({\mathrm{\ω}}_{\mathrm{ie}}^N\right)}^T\\ {(g^N\×{\mathrm{\ω}}_{\mathrm{ie}}^N)}^T\end{array}\right]}^{-1}\left[\begin{array}{c}{\left({\tilde{g}}^p\right)}^T\\ {\left({\widetilde{\mathrm{\ω}}}_{\mathrm{ie}}^p\right)}^T\\ {({\tilde{g}}^p\×{\widetilde{\mathrm{\ω}}}_{\mathrm{ie}}^p)}^T\end{array}\right]---\left(1\right)$

上式中，为IMU姿态矩阵的粗对准计算值，g为当地重力加速度，ω_ie为地球自转角速率，g^N＝[00-g]^T，为导航坐标系下的重力加速度向量，为导航坐标系下地球自转角速度向量。为IMU坐标系下重力加速度的测量值，即为静基座下加速度计的输出值；为IMU坐标系下地球自转加速度的测量值，即为静基座下陀螺的输出值。

步骤三、使旋转式捷联惯性导航系统的转轴按一定旋转方案旋转；其中，将内环轴、外环轴旋转的角速率分别记为ω₁、ω₂；

其中，单轴旋转式惯导系统旋转方案选择下述方案之一：

a.单向连续旋转；（仅限于有导电滑环的旋转式惯导系统）

b.连续旋转，每旋转一周改变转向；

双轴旋转式惯导系统旋转方案选择下述方案之一：

a.内环轴、外环轴单向连续旋转；（仅限于有导电滑环的旋转式惯导系统）

b.内环轴、外环轴连续旋转，每旋转一周改变转向；

c.内环轴、外环轴单向交替旋转，每个轴旋转一周则停止同时开始旋转另一轴，如此循环往复；（仅限于有导电滑环的旋转式惯导系统）

d.内环轴、外环轴变向交替旋转，第一轴旋转一周后停止，然后由第二轴旋转一周，然后再由第一轴反向旋转一周，然后再由第二轴反向旋转一周，如此循环往复；

e.内环轴、外环轴变向交替旋转，第一轴旋转一周后再反向旋转一周，然后停止，然后由第二轴旋转一周后再反向旋转一周，如此循环往复；

上述各方案中ω₁和ω₂的范围为0.6°/s～60°/s。

步骤四、按照步骤三所确定的旋转方案旋转的同时，利用粗对准得到的姿态矩阵和惯性测量单元的陀螺实时输出的角速度信息，求出矩阵微分方程的数值解，以更新惯性测量单元的姿态矩阵；

在本步骤中，卡尔曼滤波精对准时，需要载体姿态的实时输出，实施方式为利用粗对准所得姿态矩阵和陀螺输出的角速度信息，通过求矩阵微分方程的数值解，以更新IMU的姿态矩阵

${\stackrel{.}{C}}_p^N=C_p^N{\mathrm{\Ω}}_{\mathrm{Np}}^p$

其中，表示姿态矩阵的变化率，取的初值等于惯性测量单元姿态矩阵的粗对准计算值 为IMU相对于导航坐标系的陀螺输出的角速度的反对称矩阵形式。

${\mathrm{\ω}}_{\mathrm{Np}}^p={\left[\begin{array}{ccc}{\mathrm{\ω}}_x& {\mathrm{\ω}}_y& {\mathrm{\ω}}_z\end{array}\right]}^T$

${\mathrm{\Ω}}_{\mathrm{Np}}^p=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right]$

其中，ω_x、ω_y、ω_z分别为陀螺输出的角速度沿IMU坐标系x，y，z轴的分量。

矩阵微分方程数值求解用毕卡逼近算法（适合输出为角增量的陀螺）或龙格－库塔算法（适合输出为角速度的陀螺）。姿态更新可以采用四元数算法或等效旋转矢量算法。

在此给出采用二阶龙格－库塔算法求解的实现方法：

$C_p^N(t+T)=C_p^N\left(t\right)+\frac{T}{2}\{C_p^N\left(t\right){\mathrm{\Ω}}_{\mathrm{Np}}^p\left(t\right)+[C_p^N\left(t\right)+{\mathrm{TC}}_p^N\left(t\right){\mathrm{\Ω}}_{\mathrm{Np}}^p\left(t\right)\left]{\mathrm{\Ω}}_{\mathrm{Np}}^p(t+T)\right\}$

式中T为惯性器件采样周期。

步骤五、通过卡尔曼滤波完成静基座对准，具体步骤如下：

①在载体无移动的静基座条件下，加速度计输出为：

${\stackrel{\→}{f}}^p=C_b^p{C}_N^b{g}^N+\mathrm{\Δ}+w_a$

其中，为载体的姿态矩阵，g^N＝[00-g]^T，为导航坐标系下的重力加速度向量的，w_a＝[w_az,w_ay,w_az]^T为加速度计噪声向量，Δ＝[Δ_x,Δ_y,Δ_z]^T为加速度计零偏向量，为惯性导航系统按照步骤三所确定的旋转方案旋转时的旋转方向余弦矩阵；

②根据不同的旋转系统结构建立加速度计输出的数学模型，在加速度计输出的数学模型中，简化粗对准后失准角所对应的三角函数以及忽略失准角在东、北方向上的分量φ_e,φ_n的二阶小量，从而得到加速度计输出的线性化近似数学模型 ${\tilde{f}}^p\≈\left[\begin{array}{c}{\mathrm{gm}}_{11}+{\mathrm{\φ}}_e{\mathrm{gm}}_{12}+{\mathrm{\φ}}_n{\mathrm{gm}}_{13}\\ {\mathrm{gm}}_{21}+{\mathrm{\φ}}_e{\mathrm{gm}}_{22}+{\mathrm{\φ}}_n{\mathrm{gm}}_{23}\\ {\mathrm{gm}}_{31}+{\mathrm{\φ}}_e{\mathrm{gm}}_{32}+{\mathrm{\φ}}_n{\mathrm{gm}}_{33}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right],$ 其中g为当地重力加速度，m₁₁，m₁₂,…m₃₃为旋转方案所对应的观测方程中粗对准得到的惯性测量单元坐标系下重力加速度分量系数，以及重力加速度与姿态失准角耦合量的系数；

③从加速度计的输出中减去重力加速度的估计值，得到失准角与重力加速度的耦合量、加速度计零偏以及加速度计噪声之和，作为卡尔曼滤波的外观测量，观测方程如下：

$Z={\tilde{f}}^p-\begin{array}{}\end{array}\left[\begin{array}{c}{\mathrm{gm}}_{11}\\ {\mathrm{gm}}_{21}\\ {\mathrm{gm}}_{31}\end{array}\right]=\left[\begin{array}{ccccccccc}{\mathrm{gm}}_{12}& {\mathrm{gm}}_{13}& 0& 1& 0& 0& 0& 0& 0\\ {\mathrm{gm}}_{22}& {\mathrm{gm}}_{23}& 0& 0& 1& 0& 0& 0& 0\\ {\mathrm{gm}}_{32}& {\mathrm{gm}}_{33}& 0& 0& 0& 1& 0& 0& 0\end{array}\right]X+\begin{array}{c}\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right]\end{array}$

其中，X为系统的状态向量，Z为系统观测向量；

④根据所求得的观测方程以及卡尔曼滤波状态方程进行卡尔曼滤波，通过卡尔曼滤波估计载体姿态失准角误差，用姿态失准角误差的估计值对载体姿态矩阵进行修正，待卡尔曼滤波达到稳态后，以此时的载体姿态矩阵作为导航解算的初始载体姿态矩阵，从而完成静基座精对准；

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

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u\\ {\stackrel{\·}{\mathrm{\Δ}}}_x\\ {\stackrel{\·}{\mathrm{\Δ}}}_y\\ {\stackrel{\·}{\mathrm{\Δ}}}_z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{ccccccccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& 0& c_{11}& c_{12}& c_{13}\\ {-\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0& 0& 0& 0& c_{21}& c_{22}& c_{23}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 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& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{gx}}\\ w_{\mathrm{gy}}\\ w_{\mathrm{gz}}\end{array}\right]$

上式中，L为载体纬度位置，R为地球半径，ω_ie为地球自转角速率，c₁₁,c₁₂，…,c₃₃为载体姿态矩阵的各元素，下标e、n、u分别表示地理坐标系的东、北、天方向，下标x、y、z分别表示捷联惯导坐标系的坐标轴方向，δV_i表示载体速度误差，φ_i表示载体姿态失准角，Δ_j、ε_j分别表示加速度计和陀螺的零偏，w_aj、w_gj分别表示加速度计和陀螺的噪声；其中i的取值为e、n、u；j的取值为x、y、z。

本步骤中，首先对于单轴旋转式惯导系统，在载体无移动的静基座条件下，加速度计输出为：

${\stackrel{\→}{f}}^p=C_b^p{C}_N^b{g}^N+\mathrm{\Δ}+w_a$

其中，为载体的姿态矩阵，设载体的俯仰角、姿态角、方位角分别为θ,γ,ψ，w_a＝[w_az,w_ay,w_az]^T为加速度计噪声向量，Δ＝[Δ_x,Δ_y,Δ_z]^T为加速度计零偏向量，则载体的姿态矩阵表达式为：

$C_N^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}& 0& -\sin \mathrm{\γ}\\ 0& 1& 0\\ \sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\θ}& \sin \mathrm{\θ}\\ 0& -\sin \mathrm{\θ}& \cos \mathrm{\θ}\end{array}\right]\left[\begin{array}{ccc}\cos \mathrm{\ψ}& \sin \mathrm{\ψ}& 0\\ -\sin \mathrm{\ψ}& \cos \mathrm{\ψ}& 0\\ 0& 0& 1\end{array}\right]$

$=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \cos \mathrm{\ψ}\sin \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]$

为旋转矩阵，即载体与IMU之间的方向余弦矩阵，对于单轴旋转的旋转式惯导系统，其旋转矩阵的表达式为：

$a.C_p^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& {\sin \mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]$

$b.C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]$

$c.C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]$

其中，a、b、c式分别对应于绕IMU的x、y、z轴旋转的单轴旋转式惯导系统的旋转矩阵表达式，α_i(i＝x,y,z)为IMU绕转轴i旋转的角位置。

对于双轴旋转的旋转式惯导系统，其旋转矩阵的表达式为：

A. $C_p^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_x\\ \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_z& -\sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x\end{array}\right]$

B. $C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_z& \mathrm{son}{\mathrm{\α}}_x\sin {\mathrm{\α}}_z\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_x\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z\cos {\mathrm{\α}}_z\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_z\end{array}\right]$

C. $C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \cos {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& -\sin {\mathrm{\α}}_y\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ \sin {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_y\end{array}\right]$

D. $C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& 0\\ -\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y\cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_z& -\sin {\mathrm{\α}}_y\cos {\mathrm{\α}}_z\\ -\cos {\mathrm{\α}}_y\sin {\mathrm{\α}}_z& \cos {\mathrm{\α}}_z& \sin {\mathrm{\α}}_y\sin {\mathrm{\α}}_z\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]$

E. $C_p^b=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_y\\ \cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\cos {\mathrm{\α}}_y\end{array}\right]$

F. $C_p^b=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_y& 0& -\sin {\mathrm{\α}}_y\\ 0& 1& 0\\ \sin {\mathrm{\α}}_y& 0& \cos {\mathrm{\α}}_y\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ 0& -\sin {\mathrm{\α}}_x& \cos {\mathrm{\α}}_x\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\sin {\mathrm{\α}}_y& -\cos {\mathrm{\α}}_x\sin {\mathrm{\α}}_y\\ 0& \cos {\mathrm{\α}}_x& \sin {\mathrm{\α}}_x\\ \sin {\mathrm{\α}}_y& -\sin {\mathrm{\α}}_x\cos {\mathrm{\α}}_y& \cos {\mathrm{\α}}_x{\cos \mathrm{\α}}_y\end{array}\right]$

其中，各式所对应的旋转式系统的结构分别为：

A对应的旋转式系统的结构为：z轴为内环轴，x轴为外环轴；

B对应的旋转式系统的结构为：x轴为内环轴，z轴为外环轴；

C对应的旋转式系统的结构为：z轴为内环轴，y轴为外环轴；

D对应的旋转式系统的结构为：y轴为内环轴，z轴为外环轴；

E对应的旋转式系统的结构为：y轴为内环轴，x轴为外环轴；

F对应的旋转式系统的结构为：x轴为内环轴，y轴为外环轴。

以A式所对应结构的旋转式惯导系统为例，推导加速度计输出的数学模型：

${\tilde{f}}^p=$

$\left[\begin{array}{c}-g[\cos ({\mathrm{\φ}}_e+\mathrm{\θ})\sin ({\mathrm{\φ}}_n+\mathrm{\γ})\cos {\mathrm{\α}}_z+\sin ({\mathrm{\φ}}_e+\mathrm{\θ}){\cos \mathrm{\α}}_x{\sin \mathrm{\α}}_z-\cos ({\mathrm{\φ}}_n+\mathrm{\γ})\cos ({\mathrm{\φ}}_e+\mathrm{\θ}){\sin \mathrm{\α}}_x{\sin \mathrm{\α}}_z]\\ -g[\cos ({\mathrm{\φ}}_e+\mathrm{\θ})\sin ({\mathrm{\φ}}_n+\mathrm{\γ}){\sin \mathrm{\α}}_z-\sin ({\mathrm{\φ}}_e+\mathrm{\θ}){\cos \mathrm{\α}}_x{\cos \mathrm{\α}}_z+\cos ({\mathrm{\φ}}_n+\mathrm{\γ})\cos ({\mathrm{\φ}}_e+\mathrm{\θ}){\sin \mathrm{\α}}_x{\cos \mathrm{\α}}_z]\\ g[\sin ({\mathrm{\φ}}_e+\mathrm{\θ}){\sin \mathrm{\α}}_x+\cos ({\mathrm{\φ}}_n+\mathrm{\γ})\cos ({\mathrm{\φ}}_e+\mathrm{\θ}){\cos \mathrm{\α}}_x]\end{array}\right]$

$+\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right]$

由于粗对准后误差角为小量，所以可以对其中三角函数进行简化：

sin(φ_e+θ)＝sinφ_ecosθ+cosφ_esinθ≈φ_ecosθ+sinθ

cos(φ_e+θ)＝cosφ_ecosθ-sinφ_esinθ≈cosθ-φ_esinθ

sin(φ_n+γ)＝sinφ_ncosγ+cosφ_nsinγ≈φ_ncosγ+sinγ

cos(φ_n+γ)＝cosφ_ncosγ-sinφ_nsinγ≈cosγ-φ_nsinγ

因此，加速度计输出的数学模型可以简化为：

由于φ_e,φ_n均为小量，因此忽略二阶小量可得：

$\left[\begin{array}{c}{\mathrm{gm}}_{11}+{\mathrm{\φ}}_e{\mathrm{gm}}_{12}+{\mathrm{\φ}}_n{\mathrm{gm}}_{13}+{\mathrm{\φ}}_e{\mathrm{\φ}}_n{\mathrm{gm}}_{14}\\ {\mathrm{gm}}_{21}+{\mathrm{\φ}}_e{\mathrm{gm}}_{22}+{\mathrm{\φ}}_n{\mathrm{gm}}_{23}+{\mathrm{\φ}}_e{\mathrm{\φ}}_n{\mathrm{gm}}_{24}\\ {\mathrm{gm}}_{31}+{\mathrm{\φ}}_e{\mathrm{gm}}_{32}+{\mathrm{\φ}}_n{\mathrm{gm}}_{33}+{\mathrm{\φ}}_e{\mathrm{\φ}}_n{\mathrm{gm}}_{34}\end{array}\right]\≈\left[\begin{array}{c}{\mathrm{gm}}_{11}+{\mathrm{\φ}}_e{\mathrm{gm}}_{12}+{\mathrm{\φ}}_n{\mathrm{gm}}_{13}\\ {\mathrm{gm}}_{21}+{\mathrm{\φ}}_e{\mathrm{gm}}_{22}+{\mathrm{\φ}}_n{\mathrm{gm}}_{23}\\ {\mathrm{gm}}_{31}+{\mathrm{\φ}}_e{\mathrm{gm}}_{32}+{\mathrm{\φ}}_n{\mathrm{gm}}_{33}\end{array}\right]$

其中，g为当地重力加速度，各系数m_ij的表达式为：

m₁₁＝cosθcosγsinα_xsinα_z-sinθcosα_xsinα_z-cosθsinγcosα_z

m₁₂＝sinθsinγcosα_z-cosθcosα_xsinα_z-sinθcosγsinα_xsinα_z

m₁₃＝-cosθcosγcosα_z-cosθsinγsinα_xsinα_z

m₁₄＝sinθcosγcosα_z+sinθsinγsinα_xsinα_z

m₂₁＝sinθcosα_xcosα_z-cosθsinγsinα_z-cosθcosγsinα_xcosα_z

m₂₂＝cosθcosα_xcosα_z+sinθsinγsinα_z+sinθcosγsinα_xcosα_z

m₂₃＝cosθsinγsinα_xcosα_z-cosθcosγsinα_z

m₂₄＝sinθcosγsinα_z-sinθsinγsinα_xcosα_z

m₃₁＝sinθsinα_x+cosθcosγcosα_x

m₃₂＝cosθsinα_x-sinθcosγcosα_x

m₃₃＝cosθsinγcosα_x

m₃₄＝sinθsinγcosα_x

由此，观测方程可以写成：

$Z={\tilde{f}}^p-\left[\begin{array}{c}{\mathrm{gm}}_{11}\\ {\mathrm{gm}}_{21}\\ {\mathrm{gm}}_{31}\end{array}\right]\left[\begin{array}{ccccccccc}{\mathrm{gm}}_{12}& {\mathrm{gm}}_{13}& 0& 1& 0& 0& 0& 0& 0\\ {\mathrm{gm}}_{22}& {\mathrm{gm}}_{23}& 0& 0& 1& 0& 0& 0& 0\\ {\mathrm{gm}}_{32}& {\mathrm{gm}}_{33}& 0& 0& 0& 1& 0& 0& 0\end{array}\right]X+\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right]---\left(2\right)$

同理可推导出其他结构的旋转式惯导系统观测方程中系数m_ij的表达式，如表1所示。

表1各种旋转式惯导系统结构所对应观测方程的m系数表达式

其中，θ、γ分别为载体的俯仰角、横滚角，α_i为系统旋转机构中与IMU的敏感轴i重合的转轴(其中i可以为x,y,z)。旋转式惯导系统结构a～c分别为代表IMU绕其x,y,z轴旋转的结构；旋转式惯导系统结构A～F的所代表结构分别为：A.z轴为内环轴，x轴为外环轴；B.x轴为内环轴，z轴为外环轴；C.z轴为内环轴，y轴为外环轴；D.y轴为内环轴，z轴为外环轴；E.y轴为内环轴，x轴为外环轴；F.x轴为内环轴，y轴为外环轴。

此外，在本步骤的精对准状态滤波过程中，本发明所用卡尔曼滤波的状态方程为：

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u\\ {\stackrel{\·}{\mathrm{\Δ}}}_x\\ {\stackrel{\·}{\mathrm{\Δ}}}_y\\ {\stackrel{\·}{\mathrm{\Δ}}}_z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{ccccccccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& 0& c_{11}& c_{12}& c_{13}\\ {-\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0& 0& 0& 0& c_{21}& c_{22}& c_{23}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 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& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{gx}}\\ w_{\mathrm{gy}}\\ w_{\mathrm{gz}}\end{array}\right]$

上式中，L为载体纬度位置，R为地球半径，ω_ie为地球自转角速率，c₁₁,c₁₂，…,c₃₃为载体姿态矩阵的各元素，下标e、n、u分别表示地理坐标系的东、北、天方向，下标x、y、z分别表示捷联惯导坐标系的坐标轴方向，δV_i表示载体速度误差，φ_i表示载体姿态失准角，Δ_j、ε_j分别表示加速度计和陀螺的零偏，w_aj、w_gj分别表示加速度计和陀螺的噪声；其中i的取值为e、n、u；j的取值为x、y、z。

对式(1)和(2)组成的系统通过卡尔曼滤波进行精对准。在滤波过程中利用IMU旋转可以提高初始对准可观测性，优化初始对准性能。

下面通过matlab软件仿真实验说明IMU旋转对本发明对静基座初始对准卡尔曼滤波性能的改善。

设载体位于北纬30度，初始航向为北偏东30度，陀螺、加速度计零偏分别为0.01°/h和g×10^-4，惯性器件白噪声标准差分别为0.005°/h和0.5g×10^-4。以三种旋转方案为例，进行卡尔曼滤波初始对准仿真，与IMU无旋转的情况（相当于采用本发明的状态方程和观测方程进行初始对准的捷联式惯导系统）进行比较：（这里只给出了旋转式系统相对于捷联式系统的初始对准卡尔曼滤波性能改善的状态量的滤波估计误差仿真曲线。）

①旋转方案为z轴连续旋转，角速率ω＝6°/s，每旋转一周改变转向。卡尔曼滤波初始对准仿真结果与捷联式系统情况对比如图1所示。

②旋转式惯导系统结构如表1中A式所示，角速率ω＝6°/s，旋转方案为双轴连续旋转，每旋转一周改变转向。卡尔曼滤波初始对准仿真结果与捷联式系统情况对比如图2所示。

③旋转式惯导系统结构如表1中A式所示，角速率ω＝6°/s，旋转方案为双轴交替单周旋转，每旋转一周改变转向。卡尔曼滤波初始对准仿真结果与捷联式系统情况对比如图3所示。

仿真实验表明：本发明的方法可以提高多数状态量(φ_e,φ_n,φ_u,Δ_x,Δ_y)初始对准卡尔曼滤波的收敛速度与精度。

Claims (5)

1.一种旋转式捷联惯性导航系统静基座初始对准方法，其特征在于：该方法实现的具体步骤如下：

步骤一、获得旋转式捷联惯性导航系统的载体所在位置的经度λ、纬度L；

步骤二、根据所确定的经度、纬度信息以及惯导系统惯性器件的输出进行粗对准，得到惯性测量单元姿态矩阵的近似估计值；

步骤三、使旋转式捷联惯性导航系统的转轴按一定旋转方案旋转；其中，将内环轴、外环轴旋转的角速率分别记为ω₁、ω₂；

步骤四、按照步骤三所确定的旋转方案旋转的同时，利用粗对准得到的姿态矩阵和惯性测量单元的陀螺实时输出的角速度信息，以更新惯性测量单元的姿态矩阵；

步骤五、通过卡尔曼滤波完成静基座对准，具体步骤如下：

①在载体无移动的静基座条件下，加速度计输出为：

${\stackrel{\→}{f}}^p=C_b^p{C}_N^b{g}^N+\mathrm{\Δ}+w_a$

其中，为载体的姿态矩阵，g^N＝[0 0 -g]^T，为导航坐标系下的重力加速度向量，w_a＝[w_az,w_ay,w_az]^T为加速度计噪声向量，Δ＝[Δ_x,Δ_y,Δ_z]^T为加速度计零偏向量，为惯性导航系统按照步骤三所确定的旋转方案旋转时的旋转方向余弦矩阵；

②根据不同的旋转系统结构建立加速度计输出的数学模型，在加速度计输出的数学模型中，简化粗对准后失准角所对应的三角函数以及忽略失准角在东、北方向上的分量φ_e,φ_n的二阶小量，从而得到加速度计输出的线性化近似数学模型 ${\tilde{f}}^p\≈\left[\begin{array}{c}{\mathrm{gm}}_{11}+{\mathrm{\φ}}_e{\mathrm{gm}}_{12}+{\mathrm{\φ}}_n{\mathrm{gm}}_{13}\\ {\mathrm{gm}}_{21}+{\mathrm{\φ}}_e{\mathrm{gm}}_{22}+{\mathrm{\φ}}_n{\mathrm{gm}}_{23}\\ {\mathrm{gm}}_{31}+{\mathrm{\φ}}_e{\mathrm{gm}}_{32}+{\mathrm{\φ}}_n{\mathrm{gm}}_{33}\end{array}\right]+\left[\begin{array}{c}{\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\end{array}\right]+\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right],$ 其中g为当地重力加速度，m₁₁,m₁₂,...m₃₃为旋转方案所对应的观测方程中粗对准得到的惯性测量单元坐标系下重力加速度分量系数，以及重力加速度与姿态失准角耦合量的系数；

③从加速度计的输出中减去重力加速度的估计值，得到失准角与重力加速度的耦合量、加速度计零偏以及加速度计噪声之和，作为卡尔曼滤波的外观测量，观测方程如下：

$Z={\tilde{f}}^p-\left[\begin{array}{c}{\mathrm{gm}}_{11}\\ {\mathrm{gm}}_{21}\\ {\mathrm{gm}}_{31}\end{array}\right]\left[\begin{array}{ccccccccc}{\mathrm{gm}}_{12}& {\mathrm{gm}}_{13}& 0& 1& 0& 0& 0& 0& 0\\ {\mathrm{gm}}_{22}& {\mathrm{gm}}_{23}& 0& 0& 1& 0& 0& 0& 0\\ {\mathrm{gm}}_{32}& {\mathrm{gm}}_{33}& 0& 0& 0& 1& 0& 0& 0\end{array}\right.wX+\left[\begin{array}{c}{w}_{\mathrm{ax}}\\ w_{\mathrm{ay}}\\ w_{\mathrm{az}}\end{array}\right]$

其中，X为系统的状态向量，Z为系统观测向量；

④根据所求得的观测方程以及卡尔曼滤波状态方程进行卡尔曼滤波，通过卡尔曼滤波估计载体姿态失准角误差、加速度计和陀螺零偏，用姿态失准角误差的估计值对载体姿态矩阵进行修正，待卡尔曼滤波达到稳态后，以此时的载体姿态矩阵作为导航解算的初始载体姿态矩阵，从而完成静基座精对准；

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

$\left[\begin{array}{c}{\stackrel{\·}{\mathrm{\φ}}}_e\\ {\stackrel{\·}{\mathrm{\φ}}}_n\\ {\stackrel{\·}{\mathrm{\φ}}}_u\\ {\stackrel{\·}{\mathrm{\Δ}}}_x\\ {\stackrel{\·}{\mathrm{\Δ}}}_y\\ {\stackrel{\·}{\mathrm{\Δ}}}_z\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{ccccccccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& 0& c_{11}& c_{12}& c_{13}\\ -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0& 0& 0& 0& c_{21}& c_{22}& c_{23}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 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& 0& 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathrm{\φ}}_e\\ {\mathrm{\φ}}_n\\ {\mathrm{\φ}}_u\\ {\mathrm{\Δ}}_x\\ {\mathrm{\Δ}}_y\\ {\mathrm{\Δ}}_z\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]+\left[\begin{array}{ccc}{c}_{11}& c_{12}& c_{13}\\ c_{21}& c_{22}& c_{23}\\ c_{31}& c_{32}& c_{33}\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{w}_{\mathrm{gx}}\\ w_{\mathrm{gy}}\\ w_{\mathrm{gz}}\end{array}\right]$

上式中，L为载体纬度位置，ω_ie为地球自转角速率，c₁₁,c₁₂,...,c₃₃为载体姿态矩阵的各元素，下标e、n、u分别表示地理坐标系的东、北、天方向，下标x、y、z分别表示捷联惯导坐标系的坐标轴方向，φ_i表示载体姿态失准角，Δ_j、ε_j分别表示加速度计和陀螺的零偏，w_aj、w_gj分别表示加速度计和陀螺的噪声；其中i的取值为e、n、u；j的取值为x、y、z。

2.如权利要求1所述的一种旋转式捷联惯性导航系统静基座初始对准方法，其特征在于：步骤二中，粗对准的方法采用解析式粗对准方法。

3.如权利要求1所述的一种旋转式捷联惯性导航系统静基座初始对准方法，其特征在于：步骤三中，旋转方案的选择如下：

单轴旋转式惯导系统旋转方案选择下述方案之一：

a.单向连续旋转，该旋转方案仅限于有导电滑环的旋转式惯导系统；

b.连续旋转，每旋转一周改变转向；

双轴旋转式惯导系统旋转方案选择下述方案之一：

a.内环轴、外环轴单向连续旋转，该旋转方案仅限于有导电滑环的旋转式惯导系统；

b.内环轴、外环轴连续旋转，每旋转一周改变转向；

c.内环轴、外环轴单向交替旋转，每个轴旋转一周则停止同时开始旋转另一轴，如此循环往复，该旋转方案仅限于有导电滑环的旋转式惯导系统；

d.内环轴、外环轴变向交替旋转，第一轴旋转一周后停止，然后由第二轴旋转一周，然后再由第一轴反向旋转一周，然后再由第二轴反向旋转一周，如此循环往复；

e.内环轴、外环轴变向交替旋转，第一轴旋转一周后再反向旋转一周，然后停止，然后由第二轴旋转一周后再反向旋转一周，如此循环往复。

4.如权利要求1所述的一种旋转式捷联惯性导航系统静基座初始对准方法，其特征在于：步骤三中，ω₁和ω₂的范围为0.6°/s～60°/s。

5.如权利要求1所述的一种旋转式捷联惯性导航系统静基座初始对准方法，其特征在于：步骤四中，姿态矩阵的更新采用四元数算法或等效旋转矢量算法。