CN101571394A - 基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法 - Google Patents

Abstract

本发明提供的是一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法。根据SINS的输出与地球自转角速度和重力加速度的关系初步确定SINS的初始姿态，利用卡尔曼滤波方法估计出失准角确定当地北向；建立光纤陀螺误差模型，估计出导航系下北向陀螺漂移；惯性测量单元(IMU)顺时针转动90度后估计出导航系下南向陀螺漂移；得到IMU水平陀螺漂移后进行补偿；对误差补偿后的系统采用惯性测量单元绕载体方位轴旋转状态下的初始对准方案，确定出系统的初始捷联矩阵，计算出载体初始时刻的姿态。本发明具有自主、精度高的特点，适合用于各种中高精度的捷联惯性导航系统。

Description

基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法

(一)技术领域

本发明涉及的是一种测量方法，尤其涉及的是一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法。

(二)背景技术

捷联惯性导航系统(SINS)是将陀螺仪、加速度计等惯性元件固连在载体上，根据牛顿力学定律，通过对这些惯性元件采集的信息进行处理，得到载体的姿态、速度、位置、加速度、角速度和角加速度等全导航信息的完全自主导航设备。由于其具有重量轻、可靠性高、便于维护、全天候和完全自主等优点得到越来越广泛的应用。

标定技术本质上是一种误差补偿技术。所谓误差补偿技术就是建立惯性元件的误差数学模型，通过一定的试验来确定模型系数，进而通过软件算法来消除误差。目前惯性元件的标定技术已经比较成熟，系统标定指从惯导系统精度出发，考虑到由惯性元件构成惯导系统时安装轴向不垂直以及载体运动环境的复杂恶劣性等因素的影响，建立惯性元件的误差数学模型，最后实现误差补偿的过程。系统级标定则利用陀螺仪和加速度计的输出进行导航解算，以导航误差作为观测量来标定系统的误差参数。

根据SINS的基本原理，SINS在进入导航状态前需要获得初始信息，包括初始位置、速度和姿态。其中初始姿态的精度就是SINS进入导航状态时的初始对准精度，因此在捷联系统进入导航状态前必须首先完成初始姿态的确定。

按照基座的运动状态的不同可以将初始对准分为静基座对准和动基座对准。其中静基座对准是指在载体静止的情况下，捷联惯导系统进行初始对准。其对准方法主要包括解析法、罗经回路法、方位估算法等。所谓动基座对准是指在载体机动和外界扰动的条件下，捷联系统完成初始对准。动基座对准经常采用外部信息对系统自身的状态变量进行估计，并在滤波稳定后进行姿态修正。

由于静基座下进行以速度为观测量的组合对准，系统可观测度不高，尤其是地理坐标系下的东向陀螺漂移不可观测，导致了方位失准角的可观测度难以提高，达不到提高姿态精度的目的，也不能在初始姿态计算的同时实现对陀螺仪的标定。

(三)发明内容

本发明的目的在于提供一种使用两次对准技术并利用旋转机构对光纤陀螺偏差进行在线标定后并补偿，从而得到更加精确的初始姿态的基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法。

本发明的目的是这样实现的：

包括如下步骤：

(1)对捷联惯导系统进行预热准备；

(2)通过全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(3)采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理，采用解析法完成系统的粗对准，初步确定载体的姿态；

(4)粗对准结束后进入精对准阶段，采用卡尔曼滤波技术估计出载体的方位失准角，确定当地北向；

(5)建立光纤陀螺误差模型，对误差模型进行分析，找出待求误差系数；

(6)保持惯性测量单元(IMU)在第一位置上不动，采用卡尔曼滤波技术估计出导航坐标系上北向光纤陀螺陀螺漂移ε₁；

(7)惯性测量单元顺时针转动90度后，固定在第二位置，采用卡尔曼滤波技术估计出导航坐标系上南向光纤陀螺陀螺漂移ε₂；

(8)利用步骤(6)、(7)获得的陀螺漂移ε₁、ε₂得到惯性测量单元坐标系上光纤陀螺漂移ε_x、ε_y，

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_x={\mathrm{\ϵ}}_2\\ {\mathrm{\ϵ}}_y={\mathrm{\ϵ}}_1\end{array}\right.;$

(9)将步骤(8)获得的光纤陀螺漂移带入到陀螺误差模型补偿中，对陀螺误差进行修正，剔除各项误差得到精确的输出值；

(10)对误差补偿后的系统采用卡尔曼滤波方法进行初始对准，确定出系统的初始捷联矩阵，计算出载体初始时刻的姿态。

本发明与现有技术相比的优点在于：

(1)本发明采用两次对准和一次标定补偿技术，首先通过一次对准确定出当地北向，建立光纤陀螺误差模型，利用旋转机构改变IMU位置对光纤陀螺进行标定，确定误差系数并进行补偿，补偿后的惯导系统再进行一次初始对准，得到更为精确的捷联系统初始姿态。

(2)本发明利用导航坐标系下北向陀螺漂移始终可以观测这一原则，通过旋转机构转动90度，使IMU的两个陀螺在不同时刻分别位于导航系的南北向。利用卡尔曼滤波在这两个位置上进行估计，第一位置上估计出的北向漂移为IMU坐标系oy_s轴上的陀螺漂移ε_y，第二位置上估计出来的北向陀螺漂移为IMU坐标系ox_s轴上的陀螺漂移ε_x。

(四)附图说明

图1为本发明的一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法流程图；

图2为本发明的载体姿态角示意图。

(五)具体实施方式

下面结合附图举例对本发明做更详细地描述：

(1)捷联惯导系统预热准备，具体准备时间根据不同系统而不同；

(2)通过外部设备确定载体的初始位置参数，将它们装订至导航计算机中；

(3)采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理，采用解析法完成系统的粗对准，初步确定载体的姿态；

(4)粗对准结束后进入精对准阶段，采用卡尔曼滤波技术估计出载体的方位失准角，确定当地北向；

(5)建立光纤陀螺误差模型，对误差模型进行分析，找出待求误差系数；

(6)保持惯性测量单元(IMU)在第一位置上不动，采用卡尔曼滤波技术估计出导航坐标系上北向光纤陀螺陀螺漂移ε₁；

(7)旋转机构顺时针转动90度后，固定在第二位置，采用卡尔曼滤波技术估计出导航坐标系上南向光纤陀螺陀螺漂移ε₂；

(8)利用步骤(6)、(7)获得的陀螺漂移ε₁、ε₂得到IMU坐标系上光纤陀螺漂移ε_x、ε_y。

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_x={\mathrm{\ϵ}}_2\\ {\mathrm{\ϵ}}_y={\mathrm{\ϵ}}_1\end{array}\right.$

(9)将步骤(8)获得的光纤陀螺漂移带入到陀螺误差模型补偿中，对陀螺误差进行修正，剔除各项误差得到精确的输出值；

(10)对误差补偿后的系统采用卡尔曼滤波方法进行初始对准，确定出系统的初始捷联矩阵，计算出载体初始时刻的姿态。

本发明还可以包括如下特征：

1、粗对准过程中步骤(3)、(9)建立载体坐标系b和计算地理坐标系n′之间的转换矩阵T_b ⁿ′：

根据加速度计的输出的重力矢量g以及陀螺仪输出与地球自转角速率ω_ie的测量值初步确定此时载体的姿态信息完成系统的粗对准，建立载体坐标系b和计算地理坐标系n′之间的转换矩阵T_b ⁿ′：

对于某一纬度有：

g^t＝[0 0 -g]^T     (1)

用g和ω_ie可以构成一个新的矢量E，即：E＝g×ω_ie，再根据导航系与载体系之间的方向余弦矩阵可以得：

$\left\{\begin{array}{c}{g}^b=T_{n^{\′}}^b{g}^{n^{\′}}\\ {\mathrm{\ω}}_{\mathrm{ie}}^b=T_{n^{\′}}^b{\mathrm{\ω}}_{\mathrm{ie}}^{n^{\′}}\\ E^b=T_{n^{\′}}^b{E}^{n^{\′}}\end{array}\right.---\left(3\right)$

考虑捷联矩阵的正交性，即：

${\left(T_{n^{\′}}^b\right)}^{-1}={\left(T_{n^{\′}}^b\right)}^T=T_b^{n^{\′}}---\left(4\right)$

其中g^b、ω_ie ^b用陀螺和加速度的输出近似代替。

2、精对准过程中步骤(3)、(9)中用到的系统状态方程和量测方程为：

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

Z(t)＝H(t)X(t)+v(t)                    (7)

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；v(t)表示系统的量测噪声；

W=[a_x  a_y  ω_x  ω_y  ω_z  0  0  0  0  0]^T    (9)

以水平速度误差为观测量得到系统量测矩阵为：

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

其中δ_Ve、δV_n分别表示东向、北向的速度误差；分别为东向、北向、天向失准角；

分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；

系统的状态转移矩阵和观测矩阵为：

$A\left(t\right)=\left[\begin{array}{cc}{F}_{5\×5}& T_{5\×5}\\ 0_{5\×5}& 0_{5\×5}\end{array}\right],$ $B\left(t\right)=\left[\begin{array}{cc}{T}_{5\×5}& 0_{5\×5}\\ 0_{5\×5}& 0_{5\×5}\end{array}\right]---\left(11\right)$

其中：

$F_{5\×5}=\left[\begin{array}{cc}{F}_{2\×2}& F_{2\×3}\\ F_{3\×2}& F_{3\×3}\end{array}\right]---\left(12\right)$

$F_{2\×2}=\left[\begin{array}{cc}\frac{V_n\tan L}{R_n}& 2{\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right]---\left(\text{13}\right)$

$F_{2\×3}=\left[\begin{array}{ccc}0& {-f}_u& f_n\\ f_u& 0& -f_e\end{array}\right]---\left(14\right)$

$F_{3\×2}=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]---\left(15\right)$

$F_{3\×3}=\left[\begin{array}{ccc}0& {\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\mathrm{\ω}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{V_n}{R_m}\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_n}{R_m}& 0\end{array}\right]---\left(16\right)$

V_e、V_n分别表示东向、北向的速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_e、f_n、f_u分别表示为导航坐标系下东向、北向、天向的比力。

令捷联矩阵表示为：

$T_s^{n^{\′}}=T_b^{n^{\′}}{T}_s^b=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]---\left(17\right)$

则

$T_{5\×5}=\left[\begin{array}{ccccc}{T}_{11}& T_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& T_{11}& T_{12}& T_{13}\\ 0& 0& T_{21}& T_{22}& T_{23}\\ 0& 0& T_{31}& T_{32}& T_{33}\end{array}\right]---\left(18\right)$

3、步骤(6)、(7)中用到的卡尔曼滤波模型等同于两次对准时采用的滤波模型，当旋转机构转动90度到达第二位置时，IMU坐标系与载体坐标系的转换关系为：

$T_s^b=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]---\left(19\right)$

捷联矩阵表示为：

$T_s^{n^{\′}}=T_b^{n^{\′}}{T}_s^b=\left[\begin{array}{ccc}{-T}_{12}& T_{11}& T_{13}\\ {-T}_{22}& T_{21}& T_{23}\\ {-T}_{32}& T_{31}& T_{33}\end{array}\right]---\left(20\right)$

4、步骤(10)中利用卡尔曼滤波估计出的失准角

计算导航坐标系n和计算地理坐标系n′之间的转换矩阵T_n′ ⁿ：

IMU坐标系与导航坐标系之间的转换矩阵记为：

$T_s^n=T_{n^{\′}}^n{T}_s^{n^{\′}}=\left[\begin{array}{ccc}{T}_{11}^n& T_{12}^n& T_{13}^n\\ T_{21}^n& T_{22}^n& T_{23}^n\\ T_{31}^n& T_{32}^n& T_{33}^n\end{array}\right]---\left(22\right)$

根据转换矩阵确定载体航向角

俯仰角θ、倾斜角γ，其定义如附图2所示。

又因为

对比式(22)、(23)得到载体姿态角主值：

航向角

定义域为(0°，360°)，俯仰角θ定义域为(-90°，90°)，倾斜角γ定义域为(-180°，180°)，得到载体姿态真值：

θ＝θ_主                (26)

Claims (4)

1、一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法，其特征是：

(1)对捷联惯导系统进行预热准备；

(2)通过全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；

(3)采集光纤陀螺仪和石英加速度计输出的数据并对数据进行处理，采用解析法完成系统的粗对准，初步确定载体的姿态；

(4)粗对准结束后进入精对准阶段，采用卡尔曼滤波技术估计出载体的方位失准角，确定当地北向；

(5)建立光纤陀螺误差模型，对误差模型进行分析，找出待求误差系数；

(6)保持惯性测量单元在第一位置上不动，采用卡尔曼滤波技术估计出导航坐标系上北向光纤陀螺陀螺漂移ε₁；

(7)惯性测量单元顺时针转动90度后，固定在第二位置，采用卡尔曼滤波技术估计出导航坐标系上南向光纤陀螺陀螺漂移ε₂；

(8)利用步骤(6)、(7)获得的陀螺漂移ε₁、ε₂得到惯性测量单元坐标系上光纤陀螺漂移ε_x、ε_y，

$\left\{\begin{array}{c}{\mathrm{\ϵ}}_x={\mathrm{\ϵ}}_2\\ {\mathrm{\ϵ}}_y={\mathrm{\ϵ}}_1\end{array}\right.;$

(9)将步骤(8)获得的光纤陀螺漂移带入到陀螺误差模型补偿中，对陀螺误差进行修正，剔除各项误差得到精确的输出值；

(10)对误差补偿后的系统采用卡尔曼滤波方法进行初始对准，确定出系统的初始捷联矩阵，计算出载体初始时刻的姿态。

2、根据权利要求1所述的一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法，其特征在于所述的初步确定载体的初始捷联矩阵的方法为：根据加速度计的输出的重力矢量g以及陀螺仪输出与地球自转角速率ω_ie的测量值初步确定此时载体的姿态信息完成系统的粗对准，建立载体坐标系b和计算地理坐标系n′之间的转换矩阵T_b ⁿ′：

对于某一纬度

g^t＝[0 0 -g]^T

用g和ω_ie构成一个新的矢量E，即：E＝g×ω_ie，再根据导航系与载体系之间的方向余弦矩阵得：

$\left\{\begin{array}{c}{g}^b=T_{n^{\′}}^b{g}^{n^{\′}}\\ {\mathrm{\ω}}_{\mathrm{ie}}^b=T_{n^{\′}}^b{\mathrm{\ω}}_{\mathrm{ie}}^{n^{\′}}\\ E^b=T_{n^{\′}}^b{E}^{n^{\′}}\end{array}\right.$

考虑捷联矩阵的正交性，即：

${\left(T_{n^{\′}}^b\right)}^{-1}={\left(T_{n^{\′}}^b\right)}^T=T_b^{n^{\′}}$

其中g^b、ω_ie ^b用陀螺和加速度的输出近似代替。

3、根据权利要求2所述的一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法，其特征在于卡尔曼滤波模型等同于两次对准时采用的滤波模型，当旋转机构转动90度到达第二位置时，IMU坐标系与载体坐标系的转换关系为：

$T_s^b=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]$

捷联矩阵表示为：

$T_s^{n^{\′}}=T_b^{n^{\′}}{T}_s^b=\left[\begin{array}{ccc}-T_{12}& T_{11}& T_{13}\\ -T_{22}& T_{21}& T_{23}\\ -T_{32}& T_{31}& T_{33}\end{array}\right].$

4、根据权利要求3所述的一种基于旋转机构的光纤捷联惯性导航系统初始姿态确定方法，其特征在于利用卡尔曼滤波估计出的失准角

计算导航坐标系n和计算地理坐标系n′之间的转换矩阵T_n′ ⁿ：

IMU坐标系与导航坐标系之间的转换矩阵记为：

$T_s^n=T_{n^{\′}}^n{T}_s^{n^{\′}}=\left[\begin{array}{ccc}{T}_{11}^n& T_{12}^n& T_{13}^n\\ T_{21}^n& T_{22}^n& T_{23}^n\\ T_{31}^n& T_{32}^n& T_{33}^n\end{array}\right]$

根据转换矩阵确定载体航向角俯仰角θ、倾斜角γ，又因为

得到载体姿态角主值：

航向角定义域为(0°，360°)，俯仰角θ定义域为(-90°，90°)，倾斜角γ定义域为(-180°，180°)，得到载体姿态真值：

θ＝θ_主

Images