CN102519485B - 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法 - Google Patents

Abstract

一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法，包括建立捷联惯性导航系统初始对准状态方程；建立捷联惯性导航系统引入陀螺信息的初始对准量测方程；构建用于捷联惯性导航系统初始对准的卡尔曼滤波器；采集捷联惯性导航系统信息，完成第一位置的初始对准滤波；采集捷联惯性导航系统信息，完成第二位置的初始对准滤波。本发明提出了一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法，在捷联惯性导航系统二位置对准方法的基础上，将等效东向陀螺信息引入初始对准观测方程，从而实现对惯性器件随机常值误差的快速估计，实现方位角的快速对准，其最终对准精度与常规二位置对准方法类似，对准时间远远低于常规二位置对准方法。

Description

一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法

技术领域

本发明属于惯性导航技术领域，是一种用于捷联惯性导航系统的初始对准方法，适用于捷联惯性导航系统工作前的自主初始对准。

背景技术

捷联惯性导航系统初始对准的任务是获取捷联惯性导航系统工作的初始姿态矩阵。根据捷联惯性导航系统初始对准的过程，可将初始对准分为两个阶段：粗对准和精对准。解析粗对准是最为常用的自主式粗对准方法，它利用地球自转角速度和重力加速度在导航系和载体坐标系上投影的变换关系计算出初始姿态矩阵；在粗对准基础上，建立小姿态角的捷联惯性导航系统自对准误差方程，利用线性卡尔曼滤波实现捷联惯性导航系统的自主对准这一过程称为精对准。

对准精度和对准速度是捷联惯性导航系统初始对准的最重要的两项性能指标。捷联惯性导航系统自对准系统状态模型维数为10，采用单一固定位置对准时系统的可观测性矩阵秩为7，系统不满秩存在部分状态量不能观测或可观测度较低。初始对准的方位失准角收敛速度和精度都较低，且部分惯性器件误差不能进行估计。通过改变载体姿态或转动惯性测量单元的方法可以提高系统状态的可观测性，从而对更多状态变量进行估计，提高方位失准角的估计精度。

二位置初始对准方法基于捷联惯性导航系统自对准方法，在捷联惯性导航系统自对准基本收敛的情况下引入第二个位置，提高初始对准系统的可观性，估计惯性器件误差，从而提高初始对准的精度。由于，第二个位置的引入需在第一个位置基本收敛情况下才能进行，同时引入第二位置后，卡尔曼滤波器还需要一定的时间收敛，其对准精度的代价是延长了初始对准的时间。

本发明针对常规二位置初始对准方法中提高了对准精度而延长了初始对准时间的问题，提出了一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法。在捷联惯性导航系统自对准原理的基础上，将陀螺信息引入捷联惯性导航系统二位置对准过程。从而有效估计惯性器件随机常值误差，明显提高捷联惯性导航系统初始对准的速度。

发明内容

本发明的目的是提供一种捷联惯性导航系统自主初始对准的方法，该方法可以实现捷联惯性导航系统的高精度快速自主初始对准。

本发明为实现上述目的，采用如下技术方案：

本发明是一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法，相对于固定位置的捷联惯性导航系统自主对准方法，该方法不要求引入外部辅助手段，通过引入捷联惯性导航系统自身惯性器件的信息，达到提高初始对准精度、缩短对准时间的目的。其特征在于包括下列步骤：

(1)建立捷联惯性导航系统初始对准状态方程；

(2)建立捷联惯性导航系统引入陀螺信息的初始对准量测方程；

(3)构建用于捷联惯性导航系统初始对准的卡尔曼滤波器；

(4)采集捷联惯性导航系统信息，完成第一位置的初始对准滤波；

(5)采集捷联惯性导航系统信息，完成第二位置的初始对准滤波。

本发明所述的建立捷联惯性导航系统初始对准状态方程方法如下：捷联惯性导航系统小失准角自对准是在粗对准之后，假设三个失准角均为小角度情况下进行的，本发明所述初始对准均指精对准过程。

捷联惯性导航系统姿态和速度误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=(I-C_n^c){\mathrm{\ω}}_{\mathrm{in}}^n+{\mathrm{\δ\ω}}_{\mathrm{in}}^n-C_b^c{\mathrm{\ϵ}}^b---\left(1\right)$

$=(I-C_c^n)({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)+({\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)-C_b^n{\mathrm{\ϵ}}^b$

式中，φ为平台失准角，I为单位阵；n为导航坐标系；i为惯性坐标系，c为计算坐标系，e为地球坐标系，b为机体坐标系；

为n系相对i系的姿态角速率在n系的投影，

为e系相对i系的姿态角速率在n系的投影，

为n系相对e系的姿态角速率在n系的投影；

为n系到c系的姿态转换矩阵，

为b系到c系的姿态转换矩阵；

为角速率误差；ε^b为机体陀螺误差。

$\mathrm{\δ}{\stackrel{\·}{V}}^n=(C_n^c-I)f^n-({2\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×V^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δV}}^n+C_b^n{\▿}^b---\left(2\right)$ 式中，δVⁿ为速度误差；fⁿ为比力；Vⁿ为速度；

为机体加速度计误差。

假定三个失准角均为小角度，则有：

$C_n^c=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& -{\mathrm{\φ}}_N\\ -{\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& -{\mathrm{\φ}}_E& 1\end{array}\right]=I-[\mathrm{\φ}\×]---\left(3\right)$

式中，φ_E，φ_N，φ_U分别为东向、北向和天向失准角，下标E，N，U分别代表东向、北向和天向； $[\mathrm{\φ}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right].$

将(3)式代入(1)式和(2)式，忽略小量，将惯性器件误差均假设为随机常值误差，并将惯性器件误差扩展入系统状态。不考虑高度通道，由此，可以得到捷联惯性导航系统小失准角下的初始自对准系统误差方程。取系统状态为：

$X={\left[\begin{array}{cccccccccc}{\mathrm{\δv}}_E& {\mathrm{\δv}}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_{\mathrm{bx}}& {\▿}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}\end{array}\right]}^T---\left(4\right)$

式中，δv_E，δv_N为水平速度误差，下标E，N分别代表东向和北向；

为加速度计随机常值误差，ε_bx，ε_by，ε_bz为陀螺随机常值误差，下标中b代表机体系，x，y，z分别代表机体系三个轴向。

则有初始对准状态方程：

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

$=\left[\begin{array}{cc}{A}_1& A_2\\ 0_{5\×5}& 0_{5\×5}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{A}_2\\ 0_{5\×5}\end{array}\right]W\left(t\right)---\left(5\right)$

式中，A(t)为系统状态系数矩阵，B(t)为噪声系数矩阵，W(t)为系统噪声，t为时间表示连续方程。

$A_1=\left[\begin{array}{ccccc}0& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -g& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{nL}& 0& g& 0& 0\\ 0& 0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ 0& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ 0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0\end{array}\right]---\left(6\right)$

式中，L为当地纬度，g为当地重力加速度，ω_ie为地球自转角速率。

$A_2=\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}\end{array}\right]---\left(7\right)$

式中，C_ij(i，j＝1，2，3)为

各项元素。

系统噪声为：

W(t)＝[W_ax W_ay W_gx W_gy W_gz]^T    (8)

式中，W服从N(0，Q)分布，Q为系统过程噪声协方差阵；W_ax，W_ay为加速度计白噪声，W_gx，W_gy，W_gz为三轴陀螺白噪声，下标中a指代加速度计，g指代陀螺仪，x，y，z分别代表机体系三个轴向。

本发明所述的建立捷联惯性导航系统引入陀螺信息的初始对准量测方程方法如下：

计算系与导航系的姿态转换矩阵为式(3)，则地球自转角速度在计算系上的投影为：

${\mathrm{\ω}}_{\mathrm{ie}}^c=C_c^n{\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& -{\mathrm{\φ}}_N\\ -{\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& -{\mathrm{\φ}}_E& 1\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]$   (9)

$=\left[\begin{array}{c}{\mathrm{\φ}}_U{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\φ}}_N{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ {\mathrm{\φ}}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L+{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\φ}}_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]$

式中，

为地球自转角速度在计算系上的投影。

陀螺输出在计算系的投影为：

${\mathrm{\ω}}_{\mathrm{ib}}^c={\mathrm{\ω}}_{\mathrm{ie}}^c+{\mathrm{\ω}}_{\mathrm{eb}}^c\≈C_b^c{\mathrm{\ω}}_{\mathrm{ib}}^b---\left(10\right)$

式中，

为b系相对i系的姿态角速率在c系的投影，

为b系相对e系的姿态角速率在c系的投影，

为b系相对i系的姿态角速率在b系的投影。

捷联惯性导航系统初始对准中载体基本保持静止，因此考虑

等效陀螺的随机常值误差可以表示为：

$\left[\begin{array}{c}{\mathrm{\ϵ}}_E^c\\ {\mathrm{\ϵ}}_N^c\\ {\mathrm{\ϵ}}_U^c\end{array}\right]=C_b^c\left[\begin{array}{c}{\mathrm{\ϵ}}_{\mathrm{bx}}\\ {\mathrm{\ϵ}}_{\mathrm{by}}\\ {\mathrm{\ϵ}}_{\mathrm{bz}}\end{array}\right]=\left[\begin{array}{c}{C}_{11}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{12}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{13}{\mathrm{\ϵ}}_{\mathrm{bz}}\\ C_{21}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{22}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{23}{\mathrm{\ϵ}}_{\mathrm{bz}}\\ C_{31}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{32}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{33}{\mathrm{\ϵ}}_{\mathrm{bz}}\end{array}\right]---\left(11\right)$

式中，

为等效陀螺随机常值误差在东向、北向和天向的分量。

本发明将等效东向陀螺敏感到的地球自转角速度信息引入初始对准滤波器，加快方位失准角的对准速度。根据式(11)等效东向陀螺输出为：

${\mathrm{\ω}}_E^c={\mathrm{\φ}}_U{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\φ}}_N{\mathrm{\ω}}_{\mathrm{ie}}\sin L+C_{11}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{12}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{13}{\mathrm{\ϵ}}_{\mathrm{bz}}---\left(12\right)$

将等效东向陀螺敏感到的地球自转角速度信息引入初始对准滤波器，并将等效东向陀螺输出与理想情况下的输出之差作为新的观测量，设置观测噪声为高斯白噪声，列写观测方程如下：

$Y\left(k\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& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& C_{11}& C_{12}& C_{13}\end{array}\right]X\left(k\right)+V\left(k\right)---\left(13\right)$

式中，Y(k)为观测量，k为k时刻，取值为0，1，2，…，n表示离散方程；X(k)为k时刻的状态X；V(k)为量测噪声，服从N(0，R)分布，R为量测噪声协方差阵。

本发明所述的构建用于捷联惯性导航系统初始对准的卡尔曼滤波器方法如下：根据捷联惯性导航系统初始对准状态方程和捷联惯性导航系统引入陀螺信息的初始对准量测方程设计卡尔曼滤波器。

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{K-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Y_k=H_k{X}_k+V_k\end{array}\right\}---\left(14\right)$

式中，X_k为系统状态，定义见(4)式，下标k和k-1分别为k和k-1时刻；Y_k为k时刻观测量，H_k为k时刻观测系数矩阵，V_k为k时刻量测噪声定义见(13)式；Φ_k，k-1为状态矢量X从时刻k-1转移到时刻k的转移矩阵，Г_k-1为噪声系数矩阵，Φ_k，k-1和Г_k-1分别定义如下：

${\mathrm{\Φ}}_{k,k-1}=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[A\left(t_k\right)T\right]}^n/n!---\left(15\right)$

${\mathrm{\Γ}}_{k-1}=\left\{\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{n!}{\left(A\left(t_k\right)T\right)}^{n-1}\left]\right\}B\left(t_k\right)T---\left(16\right)$

式中，T为离散时间周期，n为自然数。

则卡尔曼滤波方程为：

$\left.\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1/k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1/k-1}{\mathrm{\Φ}}_{k,k-1}+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T\\ {\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k[Y_k-H_k{\hat{X}}_{k/k-1}]\\ K_k=P_{k/k-1}{H}_k^T{\left[H_k{P}_{k/k-1}{H}_k^T{R}_k\right]}^{-1}\\ P_{k/k}=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}^T\end{array}\right\}---\left(17\right)$

式中，

为k-1时刻状态估计值，

为k-1时刻状态一步预测值，P_k/k-1为k-1时刻状态一步预测误差方差阵，K_k为增益，P_k/k为k时刻状态估计误差方差阵。

本发明所述的采集捷联惯性导航系统信息，完成第一位置的初始对准滤波方法如下：

根据捷联惯性导航系统实际器件性能，设置捷联惯性导航系统初始工作地理经度、纬度和高度，设置捷联惯性导航系统初始工作姿态横滚角、俯仰角和航向角，设置卡尔曼滤波器系统状态噪声协方差阵矩阵Q和系统观测噪声协方差阵矩阵R，设置卡尔曼滤波器状态初值X₀，设置卡尔曼滤波器误差协方差矩阵P₀。由导航计算机采集惯性器件输出信息，完成初始对准在第一个位置的卡尔曼滤波解算。

本发明所述的采集捷联惯性导航系统信息，完成第二位置的初始对准滤波方法如下：

捷联惯性导航系统完成第一个位置的卡尔曼滤波解算，卡尔曼滤波基本进入收敛状态。捷联惯性导航系统绕天向轴旋转180°，并对三个姿态角度进行设置，横滚角和俯仰角在原有角基础上取负，航向角为原航向角加180°。此时进行第二个位置的卡尔曼滤波解算，从而完成捷联惯性导航系统的整个初始对准过程。

本发明的方法具有如下优点：不需要引入其它辅助设备，提高了初始对准收敛精度、缩短了收敛时间。算法简单易行，基本不增加系统算法的复杂性和计算量。对以上发明的有益效果说明如下：

设计陀螺精度为0.01°/h，加速度计零偏1e-4g，为便于比较两种方案的对准效果，惯性器件随机常值误差设定为定值：陀螺随机常值取0.01deg/h，加速度计随机常值取1e^-4g。姿态误差设置为1°，1°，1°(横滚、俯仰和航向)。初始姿态角为0°，0°，90°(横滚、俯仰和航向)。初始位置为115.9°、28.68°、500米(经度、纬度和高度)。仿真步长0.1秒，时间1000秒。

引入陀螺信息的二位置捷联惯性导航系统初始对准方法在捷联惯性导航系统二位置初始对准过程中引入了惯性系统自身惯性器件的信息，如图1所示，从而快速实现对陀螺、加速度计等惯性器件误差的估计，提高捷联惯性导航系统初始对准速度。仿真中将常规二位置对准方法同本发明提出的引入陀螺信息的二位置对准方法(简称改进二位置)结果进行对比，其中常规二位置法在250秒第1位置基本收敛后进行第2位置的对准，改进二位置法在50秒处(本例在50秒处切换，实际可以更早)第1位置完全收敛后进行第2位置的对准。

本发明给出了相关惯性器件误差的估计曲线和失准角估计曲线，并与捷联惯性导航系统常规二位置初始对准方法进行对比，如从图2-图7所示。引入陀螺信息辅助二位置初始对准后，航向失准角的收敛速度有了极大的提高，解决了捷联惯性导航系统快速自主对准问题。

附图说明

图1是引入陀螺信息的二位置初始对准方法示意图。

图2是两种方法横滚角误差对比。

图3是两种方法俯仰角误差对比。

图4是两种方法航向角误差对比。

图5是两种方法等效东向陀螺估计效果对比。

图6是两种方法X轴加表随机常值误差估计效果对比。

图7是两种方法Y轴加表随机常值误差估计效果对比。

具体实施方式

下面结合附图对发明的技术方案进行详细说明：

首先根据捷联惯性导航系统的工作原理建立捷联惯性导航系统初始对准状态方程；然后建立捷联惯性导航系统引入陀螺信息的初始对准量测方程；根据捷联惯性导航系统初始对准状态方程和量测方程，构建用于捷联惯性导航系统初始对准的卡尔曼滤波器；在以上基础上，采集捷联惯性导航系统信息，完成第一位置的初始对准滤波；最后，采集捷联惯性导航系统信息，完成第二位置的初始对准滤波。

1)建立捷联惯性导航系统初始对准状态方程

捷联惯性导航系统小失准角自对准是在粗对准之后，假设三个失准角均为小角度情况下进行的，本发明所述初始对准均指精对准过程。目前，一般采用线性卡尔曼滤波器完成捷联惯性导航系统的精对准过程。

捷联惯性导航系统姿态和速度误差方程为：

$\stackrel{\·}{\mathrm{\φ}}=(I-C_n^c){\mathrm{\ω}}_{\mathrm{in}}^n+{\mathrm{\δ\ω}}_{\mathrm{in}}^n-C_b^c{\mathrm{\ϵ}}^b$       (1)

$=(I-C_c^n)({\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)+({\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)-C_b^n{\mathrm{\ϵ}}^b$

式中，φ为平台失准角，I为单位阵；n为导航坐标系；i为惯性坐标系，c为计算坐标系，e为地球坐标系，b为机体坐标系；

为n系相对i系的姿态角速率在n系的投影，

为e系相对i系的姿态角速率在n系的投影，

为n系相对e系的姿态角速率在n系的投影；

为n系到c系的姿态转换矩阵，

为b系到c系的姿态转换矩阵；

为角速率误差；ε^b为机体陀螺误差。

$\mathrm{\δ}{\stackrel{\·}{V}}^n=(C_n^c-I)f^n-({2\mathrm{\δ\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×V^n-({2\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δV}}^n+C_b^n{\▿}^b$

(4)

式中，δVⁿ为速度误差；fⁿ为比力；Vⁿ为速度；为机体加速度计误差。

假定三个失准角均为小角度，则有：

$C_n^c=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& -{\mathrm{\φ}}_N\\ -{\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& -{\mathrm{\φ}}_E& 1\end{array}\right]=I-[\mathrm{\φ}\×]---\left(3\right)$

式中，φ_E，φ_N，φ_U分别为东向、北向和天向失准角，下标E，N，U分别代表东向、北向和天向； $[\mathrm{\φ}\×]=\left[\begin{array}{ccc}0& -{\mathrm{\φ}}_U& {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U& 0& -{\mathrm{\φ}}_E\\ -{\mathrm{\φ}}_N& {\mathrm{\φ}}_E& 0\end{array}\right].$

将(3)式代入(1)式和(2)式，忽略小量，将惯性器件误差均假设为随机常值误差，并将惯性器件误差扩展入系统状态。不考虑高度通道，由此，可以得到捷联惯性导航系统小失准角下的初始自对准系统误差方程。取系统状态为：

$X={\left[\begin{array}{cccccccccc}{\mathrm{\δv}}_E& {\mathrm{\δv}}_N& {\mathrm{\φ}}_E& {\mathrm{\φ}}_N& {\mathrm{\φ}}_U& {\▿}_{\mathrm{bx}}& {\▿}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bx}}& {\mathrm{\ϵ}}_{\mathrm{by}}& {\mathrm{\ϵ}}_{\mathrm{bz}}\end{array}\right]}^T---\left(4\right)$

式中，δ_vE，δ_vN为水平速度误差，下标E，N分别代表东向和北向；

为加速度计随机常值误差，ε_bx，ε_bv，ε_bz为陀螺随机常值误差，下标中b代表机体系，x，y，z分别代表机体系三个轴向。

则有初始对准状态方程：

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

$=\left[\begin{array}{cc}{A}_1& A_2\\ 0_{5\×5}& 0_{5\×5}\end{array}\right]X\left(t\right)+\left[\begin{array}{c}{A}_2\\ 0_{5\×5}\end{array}\right]W\left(t\right)---\left(5\right)$

式中，A(t)为系统状态系数矩阵，B(t)为噪声系数矩阵，W(t)为系统噪声，t为时间表示连续方程。

$A_1=\left[\begin{array}{ccccc}0& {2\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& -g& 0\\ -2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{nL}& 0& g& 0& 0\\ 0& 0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L& -{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ 0& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& 0& 0\\ 0& 0& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0\end{array}\right]$

(6)

式中，L为当地纬度，g为当地重力加速度，ω_ie为地球自转角速率。

$A_2=\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}\end{array}\right]---\left(7\right)$

式中，C_ij(i，j＝1，2，3)为

各项元素。

系统噪声为：

W(t)＝[W_ax W_ay W_gx W_gy W_gz]^T    (8)

式中，W服从N(0，Q)分布，Q为系统过程噪声协方差阵；W_ax，W_ay为加速度计白噪声，W_gx，W_gy，W_gz为三轴陀螺白噪声，下标中a指代加速度计，g指代陀螺仪，x，y，z分别代表机体系三个轴向。

2)建立捷联惯性导航系统引入陀螺信息的初始对准量测方程计算系与导航系的姿态转换矩阵为式(3)，则地球自转角速度在计算系上的投影为：

${\mathrm{\ω}}_{\mathrm{ie}}^c=C_c^n{\mathrm{\ω}}_{\mathrm{ie}}^n=\left[\begin{array}{ccc}1& {\mathrm{\φ}}_U& -{\mathrm{\φ}}_N\\ -{\mathrm{\φ}}_U& 1& {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N& -{\mathrm{\φ}}_E& 1\end{array}\right]\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]$   (9)

$=\left[\begin{array}{c}{\mathrm{\φ}}_U{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\φ}}_N{\mathrm{\ω}}_{\mathrm{ie}}\sin L\\ {\mathrm{\φ}}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin L+{\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ -{\mathrm{\φ}}_E{\mathrm{\ω}}_{\mathrm{ie}}\cos L+{\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]$

式中，

为地球自转角速度在计算系c上的投影。

陀螺输出在计算系的投影为：

${\mathrm{\ω}}_{\mathrm{ib}}^c={\mathrm{\ω}}_{\mathrm{ie}}^c+{\mathrm{\ω}}_{\mathrm{eb}}^c\≈C_b^c{\mathrm{\ω}}_{\mathrm{ib}}^b---\left(10\right)$

式中，为b系相对i系的姿态角速率在c系的投影，为b系相对e系的姿态角速率在c系的投影，为b系相对i系的姿态角速率在b系的投影。

捷联惯性导航系统初始对准中载体基本保持静止，因此考虑

等效陀螺的随机常值误差可以表示为：

$\left[\begin{array}{c}{\mathrm{\ϵ}}_E^c\\ {\mathrm{\ϵ}}_N^c\\ {\mathrm{\ϵ}}_U^c\end{array}\right]=C_b^c\left[\begin{array}{c}{\mathrm{\ϵ}}_{\mathrm{bx}}\\ {\mathrm{\ϵ}}_{\mathrm{by}}\\ {\mathrm{\ϵ}}_{\mathrm{bz}}\end{array}\right]=\left[\begin{array}{c}{C}_{11}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{12}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{13}{\mathrm{\ϵ}}_{\mathrm{bz}}\\ C_{21}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{22}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{23}{\mathrm{\ϵ}}_{\mathrm{bz}}\\ C_{31}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{32}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{33}{\mathrm{\ϵ}}_{\mathrm{bz}}\end{array}\right]---\left(11\right)$

式中，

为等效陀螺随机常值误差在东向、北向和天向的分量。

对于理想情况，IMU(Inertial Measurement Unit，IMU)的等效东向陀螺敏感到的地球自转角速度分量理论上应为零。实际中由于陀螺随机噪声、速度误差和惯性导航系统失准角的影响，等效东向陀螺输出包含由失准角、速度误差和陀螺随机噪声等引起的各种误差量。显然等效东向陀螺信息中包含机体系三个陀螺的所有信息，从降低卡尔曼滤波器计算量的角度考虑，此处仅将等效东向陀螺信息引入初始对准过程。与采用加速度计进行水平对准的方法类似，本发明将等效东向陀螺敏感到的地球自转角速度信息引入初始对准滤波器，加快方位失准角的对准速度。根据式(11)等效东向陀螺输出为：

${\mathrm{\ω}}_E^c={\mathrm{\φ}}_U{\mathrm{\ω}}_{\mathrm{ie}}\cos L-{\mathrm{\φ}}_N{\mathrm{\ω}}_{\mathrm{ie}}\sin L+C_{11}{\mathrm{\ϵ}}_{\mathrm{bx}}+C_{12}{\mathrm{\ϵ}}_{\mathrm{by}}+C_{13}{\mathrm{\ϵ}}_{\mathrm{bz}}---\left(12\right)$

将等效东向陀螺敏感到的地球自转角速度信息引入初始对准滤波器，并将等效东向陀螺输出与理想情况下的输出之差作为新的观测量，设置观测噪声为高斯白噪声，列写观测方程如下：

$Y\left(k\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& 0& -{\mathrm{\ω}}_{\mathrm{ie}}\sin L& {\mathrm{\ω}}_{\mathrm{ie}}\cos L& 0& 0& C_{11}& C_{12}& C_{13}\end{array}\right]X\left(k\right)+V\left(k\right)---\left(13\right)$

式中，Y(k)为观测量，k为k时刻，取值为0，1，2，…，n表示离散方程；X(k)为k时刻的状态X；V(k)为量测噪声，服从N(0，R)分布，R为量测噪声协方差阵。

3)构建用于捷联惯性导航系统初始对准的卡尔曼滤波器根据捷联惯性导航系统初始对准状态方程和捷联惯性导航系统引入陀螺信息的初始对准量测方程设计卡尔曼滤波器。

$\left.\begin{array}{c}{X}_k={\mathrm{\Φ}}_{k,k-1}{X}_{K-1}+{\mathrm{\Γ}}_{k-1}{W}_{k-1}\\ Y_k=H_k{X}_k+V_k\end{array}\right\}---\left(14\right)$

式中，X_k为系统状态，定义见(4)式，下标k和k-1分别为k和k-1时刻；Y_k为k时刻观测量，H_k为k时刻观测系数矩阵，V_k为k时刻量测噪声定义见(13)式；Φ_k，k-1为状态矢量X从时刻k-1转移到时刻k的转移矩阵，Г_k-1为噪声系数矩阵，Φ_k，k-1和Г_k-1分别定义如下：

${\mathrm{\Φ}}_{k,k-1}=\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}{\left[A\left(t_k\right)T\right]}^n/n!---\left(15\right)$

${\mathrm{\Γ}}_{k-1}=\left\{\underset{n=0}{\overset{\∞}{\mathrm{\Σ}}}\right[\frac{1}{n!}{\left(A\left(t_k\right)T\right)}^{n-1}\left]\right\}B\left(t_k\right)T---\left(16\right)$

式中，T为离散时间周期，n为自然数。

则卡尔曼滤波方程为：

$\left.\begin{array}{c}{\hat{X}}_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{\hat{X}}_{k-1/k-1}\\ P_{k/k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1/k-1}{\mathrm{\Φ}}_{k,k-1}+{\mathrm{\Γ}}_{k,k-1}{Q}_{k-1}{\mathrm{\Γ}}_{k,k-1}^T\\ {\hat{X}}_{k/k}={\hat{X}}_{k/k-1}+K_k[Y_k-H_k{\hat{X}}_{k/k-1}]\\ K_k=P_{k/k-1}{H}_k^T{\left[H_k{P}_{k/k-1}{H}_k^T{R}_k\right]}^{-1}\\ P_{k/k}=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}^T\end{array}\right\}---\left(17\right)$

式中，为k-1时刻状态估计值，

为k-1时刻状态一步预测值，P_k/k-1为k-1时刻状态一步预测误差方差阵，K_k为增益，P_k/k为k时刻状态估计误差方差阵。

4)采集捷联惯性导航系统信息，完成第一位置的初始对准滤波根据捷联惯性导航系统实际器件性能，设置捷联惯性导航系统初始工作地理经度、纬度和高度，设置捷联惯性导航系统初始工作姿态横滚角、俯仰角和航向角，设置卡尔曼滤波器系统状态噪声协方差阵矩阵Q和系统观测噪声协方差阵矩阵R，设置卡尔曼滤波器状态初值X₀，设置卡尔曼滤波器误差协方差矩阵P₀。由导航计算机采集惯性器件输出信息，完成初始对准在第一个位置的卡尔曼滤波解算。

5)采集捷联惯性导航系统信息，完成第二位置的初始对准滤波捷联惯性导航系统完成第一个位置的卡尔曼滤波解算，卡尔曼滤波基本进入收敛状态。捷联惯性导航系统绕天向轴旋转180°，并对三个姿态角度进行设置，横滚角和俯仰角在原有角基础上取负，航向角为原航向角加180°。此时进行第二个位置的卡尔曼滤波解算，从而完成捷联惯性导航系统的整个初始对准过程。

综上所述，对该发明进行效果分析。图2-图3为水平姿态角估计效果，常规二位置方法和改进二位置方法在第1个位置处都能以很快的速度收敛，且该位置对准精度相当。两种方法进入第2个位置后，最终的对准精度也是类似的，但是，常规二位置方法受航向角收敛速度影响在250秒后才进入第2个位置。

图4为两种方法的航向角误差对比，显然改进二位置方法的第1个位置能在很短时间内就进入收敛状态，由于其在第1个位置达到收敛状态花费时间较短，因此可以迅速切换至第2个位置进行对准，且在第2个位置处依然能保持较快收敛速度。图4中常规二位置对准方法在250秒左右进入第1个位置的收敛状态，在500秒左右进入第二个位置的收敛状态，其最终对准精度与改进二位置方法类似。

图5所示为两种方法的等效东向陀螺估计效果的对比。在第1个位置时等效东向陀螺随机常值误差不可观测，常规方法在250秒之前对等效东向陀螺随机常值误差不能进行有效估计，250秒后切换到第2个位置，等效东向陀螺随机常值误差的可观性提高，在500秒左右完成了等效东向陀螺随机常值误差的估计。改进二位置方法在第1个位置处与常规方法类似，等效东向陀螺随机常值误差也不能进行估计，但是切换到第2个位置后，能迅速估计出等效东向陀螺随机常值误差。

图6-图7为X轴和Y轴加速度计随机常值误差估计效果，其效果与两个水平姿态角估计效果是相对应的，两种方法估计的精度和速度是类似的，只是常规二位置方法受航向角收敛速度影响，不能迅速切换进入第2个位置。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。

Claims (1)

1.一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法，其特征在于包括下列步骤： 

（1）建立捷联惯性导航系统初始对准状态方程； 

（2）建立捷联惯性导航系统引入陀螺信息的初始对准量测方程； 

（3）构建用于捷联惯性导航系统初始对准的卡尔曼滤波器； 

（4）采集捷联惯性导航系统信息，完成第一位置的初始对准滤波； 

（5）采集捷联惯性导航系统信息，完成第二位置的初始对准滤波； 

步骤（1）所述的建立捷联惯性导航系统初始对准状态方程方法如下： 

捷联惯性导航系统小失准角自对准是在粗对准之后，假设三个失准角均为小角度情况下进行的；

捷联惯性导航系统姿态和速度误差方程为： 

式中，φ为平台失准角，I为单位阵；n为导航坐标系；i为惯性坐标系，c为计算坐标系，e为地球坐标系，b为机体坐标系；

为n系相对i系的姿态角速率在n系的投影，

为e系相对i系的姿态角速率在n系的投影，

为n系相对e系的姿态角速率在n系的投影；

为n系到c系的姿态转换矩阵，

为b系到c系的姿态转换矩阵；为角速率误差；ε^b为机体陀螺误差； 

式中，δVⁿ为速度误差；fⁿ为比力；Vⁿ为速度；▽^b为机体加速度计误差； 

假定三个失准角均为小角度，则有： 

式中，φ_E,φ_N,φ_U分别为东向、北向和天向失准角，下标E,N,U分别代表东向、北向和天 向；

将(3)式代入(1)式和(2)式，忽略小量，将惯性器件误差均假设为随机常值误差，并将惯性器件误差扩展入系统状态， 不考虑高度通道，由此，可以得到捷联惯性导航系统小失准角下的初始自对准系统误差方程， 取系统状态为： 

X＝[δv_E  δv_N  φ_E  φ_N  φ_U  ▽_bx  ▽_by  ε_bx  ε_by  ε_bz]^T         (4) 

式中，δv_E,δv_N为水平速度误差，下标E,N分别代表东向和北向；▽_bx,▽_by为加速度计随机常值误差，ε_bx,ε_by,ε_bz为陀螺随机常值误差，下标中b代表机体系，x,y,z分别代表机体系三个轴向； 

则有初始对准状态方程： 

式中，A(t)为系统状态系数矩阵，B(t)为噪声系数矩阵，W(t)为系统噪声，t为时间表示连续方程；

式中，L为当地纬度，g为当地重力加速度，ω_ie为地球自转角速率； 

式中，C_ij(i,j＝1,2,3)为

各项元素； 

系统噪声为： 

W(t)＝[W_ax  W_ay  W_gx  W_gy  W_gz  ]^T                 (8) 

式中，W服从N(0,Q)分布，Q为系统过程噪声协方差阵；W_ax,W_ay为加速度计白噪声，W_gx,W_gy,W_gz为三轴陀螺白噪声，下标中a指代加速度计，g指代陀螺仪，x,y,z分别代表机体 系三个轴向； 

步骤（2）所述的建立捷联惯性导航系统引入陀螺信息的初始对准量测方程方法如下： 

计算系与导航系的姿态转换矩阵为式(3)，则地球自转角速度在计算系上的投影为： 

式中，为地球自转角速度在计算系上的投影； 

陀螺输出在计算系的投影为： 

式中，

为b系相对i系的姿态角速率在c系的投影，

为b系相对e系的姿态角速率在c系的投影，

为b系相对i系的姿态角速率在b系的投影； 

捷联惯性导航系统初始对准中载体基本保持静止，因此考虑

＝ 0 ；

等效陀螺的随机常值误差可以表示为： 

式中，

为等效陀螺随机常值误差在东向、北向和天向的分量； 

该方法将等效东向陀螺敏感到的地球自转角速度信息引入初始对准滤波器，加快方位失准角的对准速度， 根据式(11)等效东向陀螺输出为： 

将等效东向陀螺敏感到的地球自转角速度信息引入初始对准滤波器，并将等效东向陀螺输出与理想情况下的输出之差作为新的观测量，设置观测噪声为高斯白噪声，列写观测方程如下： 

式中，Y(k)为观测量，k为k时刻，取值为0,1,2,…,n表示离散方程；X(k)为k时刻的状态X；V(k)为量测噪声，服从N(0,R)分布，R为量测噪声协方差阵； 

步骤（3）所述的构建用于捷联惯性导航系统初始对准的卡尔曼滤波器方法如下： 

根据捷联惯性导航系统初始对准状态方程和捷联惯性导航系统引入陀螺信息的初始对准量测方程设计卡尔曼滤波器； 

式中，X_k为系统状态，定义见(4)式，下标k和k-1分别为k和k-1时刻；Y_k为k时刻观测量，H_k为k时刻观测系数矩阵，V_k为k时刻量测噪声定义见(13)式；Φ_k,k-1为状态矢量X从时刻k-1转移到时刻k的转移矩阵，Γ_k-1为噪声系数矩阵，Φ_k,k-1和Γ_k-1分别定义如下： 

式中，T为离散时间周期，n为自然数，A(t_k)为k时刻系统状态系数矩阵，B(t_k)为k时刻噪声系数矩阵； 

则卡尔曼滤波方程为： 

式中，为k-1时刻状态估计值，

为k-1时刻状态一步预测值，P_k/k-1为k-1时刻状态一步预测误差方差阵，P_k-1/k-1为k-1时刻状态估计误差方差阵，Γ_k,k-1为k-1到k时刻系统噪声系数矩阵，

为Γ_k,k-1的转置矩阵，Q_k-1为k-1时刻系统噪声协方差矩阵，

为k时刻的最优滤波值，K_k为增益，

为K_k的转置矩阵，Y_k为k时刻观测量，H_k为k时刻观测系数矩阵，

为H_k的转置矩阵，R_k为k时刻系统量测噪声协方差矩阵，P_k/k为k时刻状态估计误差方差阵，I为单位阵； 

步骤（4）所述的采集捷联惯性导航系统信息，完成第一位置的初始对准滤波方法如下： 

根据捷联惯性导航系统实际器件性能，设置捷联惯性导航系统初始工作地理经度、纬度和高度，设置捷联惯性导航系统初始工作姿态横滚角、俯仰角和航向角，设置卡尔曼滤波器系统状态噪声协方差阵矩阵Q和系统观测噪声协方差阵矩阵R，设置卡尔曼滤波器状态初值X₀，设置卡尔曼滤波器误差协方差矩阵P₀；由导航计算机采集惯性器件输出信息，完成初始对准在第一个位置的卡尔曼滤波解算； 

步骤（5）所述的采集捷联惯性导航系统信息，完成第二位置的初始对准滤波方法如下： 

捷联惯性导航系统完成第一个位置的卡尔曼滤波解算，卡尔曼滤波基本进入收敛状态；捷联惯性导航系统绕天向轴旋转180°，并对三个姿态角度进行设置，横滚角和俯仰角在原有角基础上取负，航向角为原航向角加180°， 此时进行第二个位置的卡尔曼滤波解算，从而完成捷联惯性导航系统的整个初始对准过程。 

Images