CN100516775C - 一种捷联惯性导航系统初始姿态确定方法 - Google Patents

Abstract

一种捷联惯性导航系统初始姿态确定方法，通过将SINS从初始位置绕空间三维任意轴旋转到任意一个位置，根据位置一上SINS的输出与地球自转角速度和重力加速度的关系，初步确定出SINS的初始姿态。由于SINS位置的改变，提高了系统的可观测性，进一步利用卡尔曼滤波技术，对姿态失准角及陀螺仪常值漂移、加速度计常值进行精确估计，得到SINS更加准确的初始姿态。本发明具有自主、精度高、灵活简便的特点，适用于各种中高精度的捷联惯性导航系统。

Description

一种捷联惯性导航系统初始姿态确定方法

技术领域

本发明涉及一种捷联惯性导航系统(SINS)初始姿态的确定方法，可用于各种中高精度的捷联惯性导航系统的初始姿态确定，特别适合于机载SINS安装时无转位机构，或转位机构精度不高、转角有限的情况。

背景技术

捷联惯性导航系统(SINS)的基本原理是根据牛顿提出的相对惯性空间的力学定律，利用陀螺仪、加速度计测量载体相对惯性空间的线运动和角运动参数，在给定的运动初始条件下，由计算机进行积分运算，连续、实时地提供位置、速度和姿态信息。SINS完全依靠自身的惯性敏感元件，不依赖任何外界信息测量导航参数，因此，它具有隐蔽性好，不受气候条件限制，无信号丢失，不受干扰等优点，是一种完全自主式、全天候的导航系统，已广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在导航定位解算之前必须获得初始信息，包括初始位置、速度和姿态。SINS的初始位置和速度信息较初始姿态容易获得，初始姿态确定后的精度就是SINS进入导航工作状态时的初始精度。因此，SINS开始工作时进行快速精确的初始姿态确定是十分重要的一步。

现有的捷联惯导系统初始姿态确定可分为粗对准和精对准两个阶段。粗对准阶段就是在静基座条件下，将单个位置或两个位置上(固定两位置或任意两位置)的陀螺仪和加速度计输出直接引入计算机，计算出载体的初始姿态。用此方法时，常常忽略陀螺仪与加速度计的误差及外部干扰因素，然而这些因素会导致误差，因此初始姿态计算精度不高。特别地，当采用固定两位置上的陀螺仪和加速度计的输出进行计算时，要求SINS绕Z轴旋转180度或90度，这就需要将SINS安装在一个转位机构上，利用转位机构实现180度或者90度的转动，工程使用时极不方便，而且转位机构的精度不高，转动角度也无法精确测量，降低了初始姿态确定的精度。由于具体工程中SINS安装位置的限制，还存在转动角度无法满足180度或90度要求的情况，此时，现有的固定两位置初始姿态确定方法将无法应用。此外，当利用任意两位置上的陀螺仪和加速度计输出时，需计算反正弦函数，陀螺仪和加速度计本身的误差以及测量误差等，容易造成计算结果不稳定，出现虚数现象，因此常需要进行多次任意两位置上陀螺仪和加速度计输出的采集和计算，对计算结果进行挑选，取平均值后，作为载体的初始姿态。这样一来，初始姿态确定的时间将成倍增加，而且初始姿态计算的精度也无法得到保证。

精对准阶段是在粗对准的基础上进行，利用现代控制理论的状态空间法，对陀螺仪和加速度计输出的数据进行处理。当对单个位置的数据进行处理时，方位失准角的可观度差，收敛速度较慢，所需时间较长，而且陀螺仪和加速度计误差不可观测，因此，无法进行较好的估计，达不到提高初始姿态精度的目的，也不能在初始姿态计算的同时实现对陀螺仪和加速度计的标定。当对固定两个位置的数据进行处理时，如上已叙述，现有的固定两位置初始姿态确定方法存在旋转角度的限制，且对转位机构精度要求较高，具体工程中往往不能应用。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种精确、方便的捷联惯性导航系统初始姿态确定方法。

本发明的技术解决方案为：一种捷联惯性导航系统初始姿态确定方法，具体步骤如下：

(1)SINS预热准备，具体准备时间根据不同的系统而不同；

(2)SINS准备完毕后，保持SINS在初始位置(称为第一位置)静止不动，采集陀螺仪输出和加速度计输出；

(3)利用加速度计输出与重力加速度的关系以及陀螺仪输出与地球自转角速率的关系，计算出第一位置的航向角

俯仰角θ₁和横滚角γ₁；

(4)无需转位机构，通过任意方法将SINS从第一位置绕空间三维任意轴旋转到任意一个位置(称为第二位置)，保持SINS在第二位置静止不动，采集陀螺仪输出和加速度计输出；

(5)采用卡尔曼滤波技术，将

θ₁和γ₁作为初始参数，对两个位置上陀螺仪和加速度计输出的数据进行处理。由于SINS位置的改变，变化了SINS误差模型中的系统矩阵，从而SINS系统的可观测性得到改善，滤波效果得到提高，更好的估计出第二位置时计算地理坐标系n′与真实地理坐标系n之间的误差角(简称为失准角φ_x、φ_y和φ_z)及陀螺仪常值漂移、加速度计常值偏置；

(6)利用估计出的φ_x、φ_y和φ_z计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n′ ⁿ。根据陀螺仪输出的角增量或角速度信息，采用四元数方法，计算载体坐标系b与计算地理坐标系n′之间的转换矩阵C_b ⁿ′。由C_n′ ⁿ和C_b ⁿ′，计算出载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ，再由C_b ⁿ计算第二位置的航向角

俯仰角θ₂和横滚角γ₂，将其作为SINS的初始姿态。

本发明的原理是：SINS在某一个位置保持静止不动时，加速度计输出与重力加速度以及陀螺仪输出与地球自转角速率有如下关系：

$\left[\begin{array}{c}{f}_{x1}\\ f_{y1}\\ f_{z1}\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_{x1}\\ {\mathrm{\ω}}_{y1}\\ {\mathrm{\ω}}_{z1}\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]---\left(2\right)$

其中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为此位置上SINS的X轴、Y轴和Z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率，其在东、北、天方向上的投影为Ω＝[0 ω_iecosL ω_iesinL]；L为当地纬度；C_n ^b为导航坐标系到载体坐标系的转换矩阵， $C_n^b={\left(C_b^n\right)}^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].$

C_n ^b可以表达成此位置处航向角

俯仰角θ₁和横滚角γ₁的表达式，利用(1)式和(2)式可以计算出此位置处的

θ₁和γ₁。计算公式如下：

由(1)、(2)式可得：

$C_{13}=\frac{f_{x1}}{g}$

$C_{23}=\frac{f_{y1}}{g}$

$C_{33}=\frac{f_{z1}}{g}$

$C_{12}=\frac{{\mathrm{\ω}}_{x1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}$

$C_{22}=\frac{{\mathrm{\ω}}_{y1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{y1}\tan L}{g}---\left(3\right)$

$C_{32}=\frac{{\mathrm{\ω}}_{z1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{z1}\tan L}{g}$

C₁₁＝C₂₂C₃₃-C₂₃C₃₂

C₂₁＝-C₁₂C₃₃+C₁₃C₃₂

C₃₁＝C₁₂C₂₃-C₁₃C₂₂

航向角

俯仰角θ₁和横滚角γ₁的主值计算公式为：

θ_1主＝arcsin(C₂₃)    (4)

若航向角

俯仰角θ₁和横滚角γ₁的取值范围定义为[0，2π]、

[-π，+π]，那么

θ₁和γ₁的真值可按如下方法确定。

θ₁＝θ_1主    (5)

由(5)式确定的

θ₁和γ₁即为SINS在此位置上的航向角、俯仰角和横滚角。

由于(1)式和(2)式中未考虑加速度计偏置、陀螺仪漂移以及加速度计和陀螺仪输出信息的测量误差，求得的航向角俯仰角θ₁和横滚角γ₁不能准确描述载体坐标系与当地地理坐标系之间的真实角度关系。因此，应当在此基础上，进一步利用现代估计理论将初始姿态失准角从随机误差和随机干扰中估计出来。

利用卡尔曼滤波技术，将

θ₁和γ₁作为初始参数，可以估计出计算地理坐标系n′与真实地理坐标系n之间的误差角，校正C_b ⁿ′后，可获得更为准确的初始姿态。但是，当卡尔曼滤波对单个位置的加速度计和陀螺仪输出进行处理时，系统不完全可观测，其中，两个加速度计和一个陀螺仪的误差不可观测，因此估计效果差，达不到提高初始姿态精度的目的。本发明通过将SINS从第一位置绕空间三维任意轴旋转到任意一个位置，即改变SINS的位置，来变化SINS误差模型中的系统矩阵，从而改善SINS系统的可观测性，提高卡尔曼滤波效果，更好地估计出失准角及陀螺仪、加速度计的误差。利用估计出的失准角对转换矩阵C_b ⁿ′进行校正，得到更为准确的航向角、俯仰角和横滚角。

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

(1)本发明打破了传统固定两位置初始姿态计算需要通过转位机构将SINS绕Z轴旋转180度或90度的约束，而是无需通过转位机构，将SINS绕任意轴旋转到任意位置，是一种空间三维的任意双位置初始姿态计算方法。避免了因转位机构精度不高，造成的固定两位置初始姿态计算精度的降低，即提高了初始姿态计算的精度。此外，将SINS绕任意轴旋转到任意位置也极大的方便了在实际工程中的应用。

(2)本发明利用两个位置上陀螺仪和加速度计输出的数据，改善SINS系统的可观测性，提高卡尔曼滤波器的滤波效果，更好地估计出失准角及陀螺仪常值漂移、加速度计常值偏置，利用估计出的失准角对姿态矩阵进行校正后，得到更为准确的航向角、俯仰角和横滚角。

(3)本发明能够在初始姿态计算的同时完成测漂和定标任务，不但提高了系统初始姿态计算的精度，而且定标精度也得到了提高，在SINS导航状态给予补偿，可有效地提高导航定位精度。

附图说明

图1为本发明的初始姿态确定方法的流程图；

图2为航向角

俯仰角θ和横滚角γ的示意图，图中Ox_ny_nz_n为导航坐标系，即东北天地理坐标系，Ox_by_bz_b为载体坐标系。其中，图2a表示从导航坐标系Ox_ny_nz_n绕z_n轴逆时针旋转与载体坐标系Ox_by_bz_b重合，即为航向角；图2b表示从导航坐标系Ox_ny_nz_n绕x_n轴逆时针旋转θ与载体坐标系Ox_by_bz_b重合，θ即为俯仰角；图2c表示从导航坐标系Ox_ny_nz_n绕y_n轴逆时针旋转γ与载体坐标系Ox_by_bz_b重合，γ即为横滚角。

具体实施方式

如图1所示，本发明的具体实施方法如下：

1、捷联惯性导航系统的准备

SINS开机后，进入准备状态。

2、第一位置数据采集

SINS准备完毕，保持SINS在初始位置(称为第一位置)静止不动，采集5分钟陀螺仪输出和加速度计输出，如果SINS的精度较低可适当延长采数时间。

3、第一位置姿态计算

导航坐标系取为东北天地理坐标系，第一位置处航向角

俯仰角θ₁和横滚角γ₁的定义如图2a、图2b和图2c所示。

加速度计输出与重力加速度以及陀螺仪输出与地球自转角速率有如下关系：

$\left[\begin{array}{c}{f}_{x1}\\ f_{y1}\\ f_{z1}\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]---\left(6\right)$

$\left[\begin{array}{c}{\mathrm{\ω}}_{x1}\\ {\mathrm{\ω}}_{y1}\\ {\mathrm{\ω}}_{z1}\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ {\mathrm{\ω}}_{\mathrm{ie}}\cos L\\ {\mathrm{\ω}}_{\mathrm{ie}}\sin L\end{array}\right]---\left(7\right)$

式中，f_x1、f_y1和f_z1以及ω_x1、ω_y1和ω_z1分别为第一个位置上SINS的X轴、Y轴和Z轴输出的比力和角速率；g为重力加速度；ω_ie为地球自转角速率，其在东、北、天方向上的投影为Ω＝[0 ω_iecosL ω_iesinL]，L表示当地纬度；C_n ^b为导航系到载体系的转换矩阵，可写为： $C_n^b={\left(C_b^n\right)}^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].$

由(6)、(7)式可得：

$C_{13}=\frac{f_{x1}}{g}$

$C_{23}=\frac{f_{y1}}{g}$

$C_{33}=\frac{f_{z1}}{g}$

$C_{12}=\frac{{\mathrm{\ω}}_{x1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}$

$C_{22}=\frac{{\mathrm{\ω}}_{y1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{y1}\tan L}{g}---\left(8\right)$

$C_{32}=\frac{{\mathrm{\ω}}_{z1}}{{\mathrm{\ω}}_{\mathrm{ie}}\cos L}-\frac{f_{z1}\tan L}{g}$

C₁₁＝C₂₂C₃₃-C₂₃C₃₂

C₂₁＝-C₁₂C₃₃+C₁₃C₃₂

C₃₁＝C₁₂C₂₃-C₁₃C₂₂

航向角

俯仰角θ₁和横滚角γ₁的主值计算公式为：

θ_1主＝arcsin(C₂₃)    (9)

若航向角俯仰角θ₁和横滚角γ₁的取值范围定义为[0，2π]、[-π，+π]，那么

θ₁和γ₁的真值可按如下方法确定。

θ₁＝θ_1主    (10)

由(10)式确定的

θ₁和γ₁即为SINS在第一位置上的航向角、俯仰角和横滚角。

4、第二位置数据采集

通过任意方法将SINS从初始位置绕空间三维任意轴旋转到任意一个位置(称为第二位置)，保持SINS在第二位置静止不动，采集5分钟陀螺仪输出和加速度计输出。

5、对两个位置的数据进行滤波处理

将θ₁和γ₁作为初始参数，利用卡尔曼滤波技术，对两个位置上陀螺仪和加速度计输出的数据进行处理，精确估计出失准角φ_x、φ_y和φ_z(东北天地理坐标系下是φ_E、φ_N和φ_U)及陀螺仪常值漂移、加速度计常值偏置。

(1)SINS初始姿态确定误差模型的建立

导航坐标系取为东北天地理坐标系，位置和垂直速度误差省略，加速度计和陀螺仪的误差视为随机偏置加白噪声过程，此时SINS的误差模型为：

$\left[\begin{array}{c}\mathrm{\δ}{\stackrel{\·}{V}}_E\\ \mathrm{\δ}{\stackrel{\·}{V}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_E\\ {\stackrel{\·}{\mathrm{\φ}}}_N\\ {\stackrel{\·}{\mathrm{\φ}}}_U\\ {\stackrel{\·}{\▿}}_x\\ {\stackrel{\·}{\▿}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_x\\ {\stackrel{\·}{\mathrm{\ϵ}}}_y\\ {\stackrel{\·}{\mathrm{\ϵ}}}_z\end{array}\right]=\left[\begin{array}{cccccccccc}0& 2{\mathrm{\Ω}}_U& 0& -g& 0& T_{11}& T_{12}& 0& 0& 0\\ -2{\mathrm{\Ω}}_U& 0& g& 0& 0& T_{21}& T_{22}& 0& 0& 0\\ 0& 0& 0& {\mathrm{\Ω}}_U& -{\mathrm{\Ω}}_N& 0& 0& T_{11}& T_{12}& T_{13}\\ 0& 0& -{\mathrm{\Ω}}_U& 0& 0& 0& 0& T_{21}& T_{22}& T_{23}\\ 0& 0& {\mathrm{\Ω}}_U& 0& 0& 0& 0& T_{31}& T_{32}& T_{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[\begin{array}{c}\mathrm{\δ}{V}_E\\ \mathrm{\δ}{V}_N\\ {\mathrm{\φ}}_E\\ {\mathrm{\φ}}_N\\ {\mathrm{\φ}}_U\\ {\▿}_x\\ {\▿}_y\\ {\mathrm{\ϵ}}_x\\ {\mathrm{\ϵ}}_y\\ {\mathrm{\ϵ}}_z\end{array}\right]---\left(11\right)$

式中，下标E、N、U分别表示东、北、天。地球自转角速率ω_ie在东、北、天方向上的投影为Ω＝[0 ω_iecosL ω_iesinL]＝[0 Ω_N Ω_U]，L表示当地纬度；下标x、y、z为载体坐标系；T_ij(i＝1，2，3；j＝1，2，3)为姿态矩阵C_b ⁿ中的元素， $C_b^n={\left\{T_{\mathrm{ij}}\right\}}_{i=\mathrm{1,2,3};j=\mathrm{1,2,3}}.$

(2)SINS初始姿态确定卡尔曼滤波模型的建立

对于捷联惯导系统，考虑到陀螺仪随机漂移误差和加速度计的随机偏差，将方程(11)修正为如下形式：

式中，W(t)为N(O，Q)的高斯白噪声；状态变量X_a＝[δV_E δV_N φ_E φ_N φ_U]^T， $X_b={\left[\begin{array}{ccccc}{\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z\end{array}\right]}^T;$ 随机噪声状态矢量 $W^{\′}\left(t\right)={\left[\begin{array}{ccccc}{w}_{{\mathrm{\δV}}_E}& w_{{\mathrm{\δV}}_N}& w_{{\mathrm{\φ}}_E}& w_{{\mathrm{\φ}}_N}& w_{{\mathrm{\φ}}_U}\end{array}\right]}^T,$ 其中，δV_E、δV_N分别为东向和北向速度误差，φ_E、φ_N为水平失准角；φ_U为方位失准角；

为加速度计随机常值偏置，ε_x、ε_y、ε_z是陀螺仪随机常值漂移0_5×5和0_5×1均为指定维数的零矩阵；F和T_i代表的内容如下：

$T_i=\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],$

(12)式为捷联惯导系统初始姿态确定卡尔曼滤波模型的系统方程。为了应用卡尔曼滤波器进行状态向量的最优估计，还需建立系统观测方程。选取两个水平速度误差为观测量，建立的系统观测方程为

其中，η(t)是系统观测噪声向量，为N(O，R)的高斯白噪声过程。

(3)离散卡尔曼滤波模型的建立

根据上述系统方程和观测方程，可建立离散卡尔曼滤波方程如下。

滤波方程：

增益方程：

$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}---\left(15\right)$

预报误差方差方程：

$P_{k,k-1}={\mathrm{\Φ}}_{k,k-1}{P}_{k-1}{\mathrm{\Φ}}_{k,k-1}^T+Q_{k-1}---\left(16\right)$

滤波误差方差方程：

P_k＝[I-K_kH_k]P_k，k-1    (17)

式中，Φ_k，k-1为离散化的状态转移矩阵(系统矩阵)，Q、R分别为系统噪声和观测噪声的协方差矩阵。

(4)滤波初始条件

X(0)＝0_10×1；

P(0)、Q和R对应中等精度SINS的取值如下：

P(0)＝diag{(0.3m/s)²，(0.3m/s)²，(30′)²，(10″)²，(10″)²，

(100μg)²，(100μg)²，(0.1°)²，(0.1°)²，(0.1°)²}；

Q＝diag{(100μg)²，(100μg)²，(0.1°)²，(0.1°)²，(0.1°)²，0，0，0，0，0}；

R＝diag{(0.1m/s)²，(0.1m/s)²}。

6、计算陀螺仪随机常值漂移和加速度计随机常值偏置

滤波器估计出的

即为加速度计随机常值偏置，ε_x、ε_y、ε_z为陀螺仪随机常值漂移。

7、计算载体坐标系b与计算地理坐标系n′之间的转换矩阵C_b ⁿ′

可利用陀螺仪输出的角增量或角速度信息，采用四元数方法计算C_b ⁿ′，计算步骤如下：

(1)利用航向角

俯仰角θ₁和横滚角γ₁初始化第一个位置时的姿态，计算初始转换矩阵C_b ⁿ′和四元数q，计算公式如下：

令 $C_b^{n^{\′}}=\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]$

则有：

$q_0=\±\frac{1}{2}\sqrt{1+T_{11}+T_{22}-T_{33}}$

$q_1=\±\frac{1}{2}\sqrt{1+T_{11}-T_{22}-T_{33}}$

$q_2=\±\frac{1}{2}\sqrt{1-T_{11}+T_{22}-T_{33}}$

$q_3=\±\frac{1}{2}\sqrt{1-T_{11}-T_{22}+T_{33}}$

(2)更新四元数q₀、q₁、q₂、q₃和转换矩阵C_b ⁿ′

$q(n+1)=\{(1-\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^2}{8}+\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^4}{384})I+(\frac{1}{2}-\frac{{\left(\mathrm{\Δ}{\mathrm{\θ}}_0\right)}^2}{48})\left(\mathrm{\Δ\θ}\right)\}q\left(n\right)---\left(19\right)$

其中，

$\mathrm{\Δ\θ}=\left[\begin{array}{cccc}0& -\mathrm{\Δ}{\mathrm{\θ}}_x& -\mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z\\ \mathrm{\Δ}{\mathrm{\θ}}_x& 0& \mathrm{\Δ}{\mathrm{\θ}}_z& -\mathrm{\Δ}{\mathrm{\θ}}_y\\ \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_z& 0& \mathrm{\Δ}{\mathrm{\θ}}_x\\ \mathrm{\Δ}{\mathrm{\θ}}_z& \mathrm{\Δ}{\mathrm{\θ}}_y& -\mathrm{\Δ}{\mathrm{\θ}}_x& 0\end{array}\right]$

$\mathrm{\Δ}{\mathrm{\θ}}_0=\sqrt{\mathrm{\Δ}{\mathrm{\θ}}_x^2+\mathrm{\Δ}{\mathrm{\θ}}_y^2+\mathrm{\Δ}{\mathrm{\θ}}_z^2}$

转换矩阵C_b ⁿ′的更新公式如下：

$C_b^{n^{\′}}=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2(q_1{q}_2-q_0{q}_3)& 2(q_1{q}_3+q_0{q}_2)\\ 2(q_1{q}_2+q_0{q}_3)& q_0^2-q_1^2+q_2^2-q_3^2& 2(q_2{q}_3-q_0{q}_1)\\ 2(q_1{q}_3-q_0{q}_2)& 2(q_2{q}_3+q_0{q}_1)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]---\left(20\right)$

8、计算航向角俯仰角θ₂和横滚角γ₂

利用卡尔曼滤波估计出的φ_E、φ_N、φ_U计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n′ ⁿ。根据C_n′ ⁿ和(20)式计算出的C_b ⁿ′，计算载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ，再由C_b ⁿ计算第二位置的航向角

俯仰角θ₂和横滚角γ₂，将其作为SINS的初始姿态。具体计算步骤如下：

(1)计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n ⁿ′

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

(2)计算载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ

$C_b^n=C_{n^{\′}}^n{C}_b^{n^{\′}}---\left(22\right)$

(3)计算航向角

俯仰角θ₂和横滚角γ₂

航向角

俯仰角θ₂和横滚角γ₂的定义如图2a、图2b和图2c所示。将(22)式计算出的C_b ⁿ记为

$C_b^n=\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(23\right)$

又因为

因此，由(23)式和(24)式，可以确定航向角

俯仰角θ₂和横滚角γ₂的主值，即

θ_2主＝arcsin(T₃₂)    (25)

若航向角

俯仰角θ₂和横滚角γ₂的取值范围分别定义为[0，2π]、

[-π，+π]。那么，θ₂和γ₂的真值可由下式确定：

θ₂＝θ_2主    (26)

由(26)式确定的

θ₂和γ₂即为SINS在第二位置上的航向角、俯仰角和横滚角，将其作为SINS进入导航工作状态的初始姿态。

Claims (1)

1、一种捷联惯性导航系统初始姿态确定方法，其特征在于包括以下步骤：

(1)SINS预热准备完毕，保持SINS在初始位置，称为第一位置，静止不动，采集陀螺仪输出和加速度计输出；

(2)根据加速度计输出与重力加速度的关系以及陀螺仪输出与地球自转角速率的关系，计算出第一位置的航向角

俯仰角θ₁和横滚角γ₁；

(3)将SINS从第一位置绕空间三维任意轴旋转到任意一个位置，称为第二位置，保持SINS在第二位置静止不动，采集陀螺仪输出和加速度计输出；

(4)采用卡尔曼滤波技术，将

θ₁和γ₁作为初始参数，对两个位置上陀螺仪和加速度计输出的数据进行处理，估计出第二位置时计算地理坐标系n′与真实地理坐标系n之间的误差角，简称为失准角φ_x、φ_y和φ_z及陀螺仪常值漂移、加速度计常值偏置；

(5)利用卡尔曼滤波估计出的φ_x、φ_y和φ_z计算真实地理坐标系n与计算地理坐标系n′之间的转换矩阵C_n′ ⁿ，根据陀螺仪输出的角增量或角速度信息，采用四元数方法，计算载体坐标系b与计算地理坐标系n′之间的转换矩阵C_b ⁿ′，由C_n′ ⁿ和C_b ⁿ′，计算出载体坐标系b与真实地理坐标系n之间的转换矩阵C_b ⁿ，再由C_b ⁿ计算第二位置的航向角

俯仰角θ₂和横滚角γ₂，将其作为SINS的初始姿态。

Images