一种基于地球自转角速率约束的SINS初始对准方法

摘要

一种基于地球自转角速率约束的SINS初始对准方法。该方法采用卡尔曼滤波技术进行对准，在传统速度误差量测模型的基础上，加入地球自转角速率作为约束条件，得到改进的初始对准误差模型。该误差模型提高了静基座对准时系统的可观测性。基于该模型进行卡尔曼滤波，对系统的姿态失准角、速度误差、陀螺仪常值漂移及加速度计常值偏置进行估计，能够更加快速和准确地得到SINS的初始姿态和天向陀螺漂移。本发明具有自主、精度高、灵活简便的特点，适用于各种中高精度的捷联惯性导航系统。

说明书

技术领域

本发明涉及一种基于地球自转角速率约束的SINS初始对准方法，属于 惯性导航技术领域，可广泛应用于各种中高精度SINS的初始对准，有效改 善了系统的可观测性，从而提高了初始对准的精度。

背景技术

捷联惯性导航系统（Strapdown Inertial Navigation System，SINS）的基 本原理是根据牛顿提出的相对惯性空间的力学定律，利用陀螺仪和加速度计 测量载体相对于惯性空间的角运动和线运动参数，根据捷联惯性导航编排方 程，进行相应的积分运算，连续、实时地提供位置、速度和姿态等导航信息。 由于SINS完全依靠自身的惯性敏感元件，不依赖任何外界导航参数，因此， 它具有隐蔽性好，不受气候条件限制，抗干扰能力强和无信号丢失等优点， 是一种完全自主式、全天候的导航系统，已经广泛应用于航空、航天和航海 等各个领域。初始对准是SINS的第一个工作阶段，对准精度对SINS导航工 作时的精度有直接的影响，因此，初始对准是SINS工作过程中十分关键的 一步。

通常，SINS初始对准可以分为粗对准和精对准两个阶段。粗对准阶段 就是在静基座条件下，根据陀螺仪和加速度计输出与已知地球自转角速度和 重力加速度的关系，直接计算出载体的初始姿态，通常粗对准的对准精度较 低，且不能有效获得惯性器件的误差。因此，需要在粗对准完成后，利用相 关技术进行精对准，进一步提高初始对准精度。

精对准阶段是在粗对准的基础上进行的，通常利用现代控制理论中的状 态空间方法，得到惯性导航系统初始对准的误差方程，对陀螺仪和加速度计 输出的数据进行捷联解算，然后进行估计。传统的精对准方法利用水平速度 误差作为量测量，通过卡尔曼滤波等估计方法，可以有效估计出系统中的水 平失准角和北向陀螺漂移。同时，通过系统可观测度分析可知，系统中的方 位失准角、天向陀螺漂移和东向陀螺漂移等状态的可观测度较低，收敛速度 较慢，所需估计时间较长，无法对天向陀螺漂移和东向陀螺漂移误差进行有 效估计。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提出一种基于地球自 转角速率约束的SINS初始对准方法。针对现有初始对准系统可观测度低的 问题，利用SINS在静止情况下只测量地球自转角速率的特点，增加地球自 转角速率测量误差作为观测量，提高了SINS初始对准的可观测度，使得天 向失准角误差能够快速收敛，同时可以快速和准确地估计出北向陀螺和天向 陀螺漂移，提高了SINS初始对准速度和精度，同时也提高了惯性器件的标 定精度。

本发明的技术解决方案为：一种基于地球自转角速率约束的SINS初始 对准方法，具体步骤如下：

（1）SINS预热准备30分钟；

（2）保持SINS静止不动，采集陀螺仪和加速度计的输出；

（3）进行粗对准解算，利用加速度计输出与重力加速度之间的关系以 及陀螺仪输出与地球自转角速度之间的关系，得到初始航向角俯仰角θ₁和横滚角γ₁；

（4）基于地球自转角速率约束的初始对准误差模型，采用卡尔曼滤波 技术，将θ₁和γ₁作为初始姿态参数，对陀螺仪和加速度计输出的数据进 行处理，估计出计算地理坐标系n′与真实地理坐标系n之间的失准角φ_E、φ_N和 φ_U，以及速度误差δv_E和δv_N，陀螺仪常值漂移ε_E、ε_N和ε_U，加速度计常值偏 置和

（5）利用步骤（4）中滤波解算得到的失准角φ_E、φ_N和φ_U计算计算地理 坐标系n′与真实地理坐标系n之间的欧拉角转换矩阵利用步骤（3）中 获得的初始姿态角计算载体坐标系b与计算地理坐标系n′之间的欧拉角转换 阵由转换矩阵和计算得到载体坐标系b与真实地理坐标系n之间的 欧拉角转换矩阵再由计算出更精确的航向角俯仰角θ₂和横滚角γ₂， 将其作为SINS的初始姿态。

本发明的原理是：

（1）粗对准，由于在对准过程中SINS保持静止不动，在不考虑陀螺 仪和加速度计的常值误差与随机误差情况下，利用陀螺仪输出的角速度与地 球自转角速度之间的关系，以及加速度计输出的加速度与当地重力加速度之 间的关系，即可求解出初始的航向角俯仰角θ₁和横滚角γ₁。

（2）精对准，由于粗对准中未考虑陀螺仪和加速度计的常值误差和随 机误差，因此求得的航向角俯仰角θ₁和横滚角γ₁不能够准确描述载体坐 标系与当地地理坐标系之间准确的角度关系。因此，在此基础上，利用卡尔 曼滤波理论将初始姿态失准角从随机误差和随机干扰中估计出来。

利用卡尔曼滤波方法进行估计，必须建立初始对准的系统状态方程和量 测方程。本发明在传统的速度误差量测模型的基础上，基于SINS在静止情 况下只测量地球自转角速率的特点，增加地球自转角速率测量误差作为量测 量，提高了系统的可观测度。以θ₁和γ₁作为初始参数，利用卡尔曼滤波 技术，可估计出计算地理坐标系n′与真实地理坐标系n之间的失准角φ_E、φ_N和 φ_U，以及相应的姿态转换矩阵利用初始姿态角θ₁和γ₁，计算载体坐 标系b与计算地理坐标系n′之间的欧拉角转换阵利用得到的转换矩阵对姿态转换矩阵进行校正，从而计算出更精确的航向角俯仰角θ₂和横 滚角γ₂，将其作为SINS的初始姿态。

本发明与现有技术相比的优点在于：本发明利用SINS在静止情况下只 测量地球自转角速率的特点，增加地球自转角速率测量误差作为量测量，改 善了SINS的可观测性，提高了卡尔曼滤波器的滤波效果，可以有效估计出 天向陀螺漂移，从而更精确地估计出姿态失准角。利用估计出的失准角对姿 态矩阵进行校正后，得到更精确的航向角、俯仰角和横滚角。

附图说明

图1为本发明的初始对准方法流程图；

具体实施方式

如说明书附图1所示，本发明的具体实施包括以下步骤：

1、SINS预热准备

SINS开机后，预热30分钟，使陀螺仪和加速度计的工作状态稳定。

2、采集陀螺仪和加速度计数据

SINS预热准备完毕后，保持SINS静止不动，采集5分钟的陀螺仪和 加速度计的输出数据。

3、粗对准，计算初始姿态角

（1）计算计算地理坐标系与载体坐标系间的欧拉角转换矩阵

导航坐标系取为东北天地理坐标系，可知加速度计输出与重力加速度， 以及陀螺仪输出与地球自转角速度有如下的关系：

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

$\left(\begin{array}{c}{\omega }_{x1}\\ {\omega }_{y1}\\ {\omega }_{z1}\end{array}\right)=C_{n^{\prime }}^b\left(\begin{array}{c}0\\ {\omega }_{\mathrm{ie}}\cos L\\ {\omega }_{\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 ω_ie cosL ω_ie sinL]^T，上 标T表示向量的转置；L表示当地纬度；为计算地理坐标系n′与载体坐标 系b之间的欧拉角转换矩阵。

（2）计算粗对准过程中的俯仰角θ₁、横滚角γ₁和航向角

具体计算过程如下：

由公式（1）和（2）可计算得到计算地理坐标系n′与载体坐标系b之间 的欧拉角转换矩阵

$C_{n^{\prime }}^b={\left(C_b^{n^{\prime }}\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_{13}=\frac{f_{x1}}{g}$$C_{23}=\frac{f_{y1}}{g}$$C_{33}=\frac{f_{z1}}{g}---\left(3\right)$

$C_{12}=\frac{{\omega }_{x1}}{{\omega }_{\mathrm{ie}}\cos L}-\frac{f_{x1}\tan L}{g}$$C_{22}=\frac{{\omega }_{y1}}{{\omega }_{\mathrm{ie}}\cos L}-\frac{f_{y1}\tan L}{g}$$C_{32}=\frac{{\omega }_{z1}}{{\omega }_{\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主＝arctan(C₂₃)

若θ₁和γ₁的取值范围分别定义为[0,2π]、和[-π,+π]，那么航 向角俯仰角θ₁和横滚角γ₁的真实值按如下的方法确定：

θ₁＝θ_1主                        (5)

由上式所确定的θ₁和γ₁即为SINS粗对准的航向角、俯仰角和横滚 角。

4、基于地球自转角速率约束的误差模型，利用卡尔曼滤波技术，估计 出失准角及陀螺仪常值漂移

利用步骤3中得到的航向角俯仰角θ₁和横滚角γ₁作为初始姿态参数， 并建立初始对准的系统状态方程和量测方程。利用卡尔曼滤波技术，估计失 准角φ_E、φ_N和φ_U，以及陀螺仪常值漂移，具体计算步骤如下：

（3.1）建立SINS初始对准的状态方程

导航坐标系取为东北天地理坐标系，位置误差和天向通道的速度误差忽 略，加速度计和陀螺仪的误差为随机偏置加白噪声过程，初始对准的状态方 程如下：

式中，状态变量x₁(t)＝[δv_E δv_N φ_E φ_N φ_U]^T，$x_2\left(t\right)={\left(\begin{array}{ccccc}{▿}_E& {▿}_N& {ϵ}_E& {ϵ}_N& {ϵ}_U\end{array}\right)}^T,$上 标T表示向量的转置；其中，下标E、N、U分别表示导航坐标系下的东向、 北向和天向，δv和φ分别为系统的速度误差和姿态误差，和ε分别为加速 度计的常值偏置和陀螺仪的常值漂移；w(t)＝[w_ax w_ay w_az w_gx w_gy w_gz]^T为系 统高斯白噪声向量，且E[w(t)w^T(t)]＝Q(t)，其中各个元素均为和时间相关的 量，包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪 声，下标中的第一个值a和g分别代表加速度计和陀螺仪，第二个值x、y和 z分别代表载体坐标系的右方、前方和上方。

系统状态转移阵中的F表达式如下：

$F=\left(\begin{array}{ccccc}0& 2{\Omega }_U& 0& -g& 0\\ -2{\Omega }_U& 0& g& 0& 0\\ 0& 0& 0& {\Omega }_U& {-\Omega }_N\\ 0& 0& -{\Omega }_U& 0& 0\\ 0& 0& {\Omega }_N& 0& 0\end{array}\right)---\left(7\right)$

式中，Ω＝[0 ω_ie cosL ω_ie sinL]^T＝[0 Ω_N Ω_U]^T为地球自转角速度ω_ie在东向、北 向和天向上的投影，L表示当地纬度；g为当地的重力加速度。

G(t)为系统噪声转移矩阵，其表达式为：

式中，为载体坐标系b与计算地理坐标系n′之间的欧拉角转换矩阵，为矩阵的前两行元素组成的矩阵，具体可由公式（3）获得。

（3.2）建立SINS初始对准的量测方程

为了应用卡尔曼滤波器进行状态向量的最优估计，还需要建立系统的量 测方程。首先，推导基于地球自转角速率约束的量测方程。

对于地球自转角速率，无误差情况时满足以下公式：

$\omega =\sqrt{{\omega }_E^2+{\omega }_N^2+{\omega }_U^2}---\left(9\right)$

式中，ω_E、ω_N和ω_U为地球自转角速度ω_ie在东向、北向和天向上的投影，定 义为[ω_E ω_N ω_U]＝[0 ω_ie cosL ω_ie sinL]。对公式（9）求解全微分可得：

$\mathrm{\delta \omega }=\frac{\partial \omega }{{\partial \omega }_E}{\mathrm{\delta \omega }}_E+\frac{\partial \omega }{{\partial \omega }_N}{\mathrm{\delta \omega }}_N+\frac{\partial \omega }{{\partial \omega }_U}{\mathrm{\delta \omega }}_U---\left(10\right)$

式中，δω为已知真实地球自转角速率与陀螺测量地球角速率之差。δω_E、 δω_N和δω_U分别为导航坐标系下东向、北向和天向的陀螺仪误差量，其为陀螺 仪常值漂移和随机漂移白噪声之和，因此上式可写为：

$\mathrm{\delta \omega }=\frac{\partial \omega }{{\partial \omega }_E}{ϵ}_E+\frac{\partial \omega }{{\partial \omega }_N}{ϵ}_N+\frac{\partial \omega }{{\partial \omega }_U}{ϵ}_U+\frac{\partial \omega }{{\partial \omega }_E}{u}_E+\frac{\partial \omega }{{\partial \omega }_N}{u}_N+\frac{\partial \omega }{{\partial \omega }_U}{u}_U---\left(11\right)$

式中，ε_E、ε_N、ε_U和u_E、u_N、u_U分别为导航坐标系下东向、北向和天向的陀 螺仪常值漂移和随机漂移白噪声。

分别求解ω_E、ω_N和ω_U的微分得：

$\frac{\partial \omega }{{\partial \omega }_E}=\frac{1}{2}\frac{{2\omega }_E}{\sqrt{{\omega }_E^2+{\omega }_N^2+{\omega }_U^2}}=\frac{{\omega }_E}{\omega }$

$\frac{\partial \omega }{{\partial \omega }_N}=\frac{1}{2}\frac{{2\omega }_N}{\sqrt{{\omega }_E^2+{\omega }_N^2+{\omega }_U^2}}=\frac{{\omega }_N}{\omega }---\left(12\right)$

$\frac{\partial \omega }{{\partial \omega }_U}=\frac{1}{2}\frac{{2\omega }_U}{\sqrt{{\omega }_E^2+{\omega }_N^2+{\omega }_U^2}}=\frac{{\omega }_U}{\omega }$

并且ω＝ω_ie为地球自转角速度，将公式（12）代入（11）中可得简化后 的地球自转角速率量测误差方程：

$\mathrm{\delta \omega }=\left(\begin{array}{ccc}0& \cos L& \sin L\end{array}\right)\left(\begin{array}{c}{ϵ}_E\\ {ϵ}_N\\ {ϵ}_U\end{array}\right)+\begin{array}{c}\left(\begin{array}{ccc}0& \cos L& \sin L\end{array}\right)\left(\begin{array}{c}{u}_E\\ u_N\\ u_U\end{array}\right)---\left(13\right)\end{array}$

选取两个水平速度误差δv_E和δv_N，以及公式（13）获得的地球自转角速 率量测误差δω作为观测量，建立的系统量测方程为：

式中，量测向量z(t)＝[δv_E δv_E δω]^T  ，量测噪声向量 $v\left(t\right)={\left(\begin{array}{ccccc}{v}_{{\mathrm{\delta v}}_E}& v_{{\mathrm{\delta v}}_N}& v_{{ϵ}_E}& v_{{ϵ}_N}& v_{{ϵ}_U}\end{array}\right)}^T$为高斯白噪声，且E[v(t)v^T(t)]＝R(t)，包括东向和 北向速度量测噪声，以及导航坐标系下东向、北向和天向的陀螺仪的白噪声 分量；量测方程中的矩阵S＝[0 cosL sinL]。

（3.3）建立离散卡尔曼滤波模型

根据公式（6）表示的系统状态方程和公式（14）表示的系统量测方程， 可建立离散卡尔曼滤波方程。设t＝t_k-1，t+Δt＝t_k。t_k时刻的线性离散型系统 方程可表示为：

x_k＝Φ_k/k-1x_k-1+Γ_k-1w_k-1

                                             (15)

z_k=H_kx_k+U_kv_k

式中，Φ_k/k-1和Γ_k-1分别为状态转移矩阵F(t)和系统噪声转移阵G(t)的离散化形 式，H_k和U_k分别为量测状态转移阵C(t)和量测噪声转移阵D(t)的离散化形 式。

标准离散型卡尔曼滤波器基本方程为：

状态一步预测求解

${\hat{x}}_{k/k-1}={\Phi }_{k/k-1}{\hat{x}}_{k-1}---\left(16\right)$

一步预测均方误差求解

$P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+{\Gamma }_{k-1}{Q}_{k-1}{{\Gamma }_{k-1}}^T---\left(17\right)$

滤波增益求解

$K_k=P_{k/k-1}{H}_k^T{(H_k{P}_{k/k-1}{H}_k^T+U_k{R}_k{U_k}^T)}^{-1}---\left(18\right)$

状态估计求解

${\hat{x}}_k={\hat{x}}_{k/k-1}+K_k(z_k-H_k{\hat{x}}_{k/k-1})---\left(19\right)$

估计均方误差求解

$P_k=(I-K_k{H}_k)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{U}_k{R}_k{U_k}^T{K}_k^T---\left(20\right)$

式中为状态一步预测，为状态估计，P_k/k-1为一步预测误差方差阵，P_k为估计误差方差阵，K_k为滤波增益阵；Q_k-1和R_k分别为系统噪声和量测噪声 的协方差矩阵。在给定初始值和P₀时，根据t_k时刻的量测z_k，就可以递推 计算得到k时刻的状态估计从而得到估计的失准角φ_E、φ_N和φ_U，以及陀 螺常值漂移。

5、利用估计出的失准角对姿态转换矩阵进行校正，获得更精确的初始 姿态角

利用步骤4中滤波解算所得的失准角φ_E、φ_N和φ_U，获得计算地理坐标系 n′与真实地理坐标系n之间的转换矩阵由转换矩阵和步骤3中获得的 载体坐标系b与计算地理坐标系n′之间的转换矩阵计算得到载体坐标系 b与真实地理坐标系n之间的欧拉角转换矩阵再由计算出更精确的航 向角俯仰角θ₂和横滚角γ₂，将其作为SINS的初始姿态。具体计算步骤 如下：

（4.1）计算计算地理坐标系n′与真实地理坐标系n之间的转换矩阵

$C_{n^{\prime }}^n=\left(\begin{array}{ccc}1& {-\phi }_U& {\phi }_N\\ {\phi }_U& 1& -{\phi }_E\\ -{\phi }_N& {\phi }_E& 1\end{array}\right)---\left(21\right)$

（4.2）计算载体坐标系b与真实地理坐标系n之间的转换矩阵

利用公式（21）得到的计算地理坐标系n′与真实地理坐标系n之间的转 换矩阵和公式（3）得到的载体坐标系b与计算地理坐标系n′之间的转换 矩阵可得

$C_b^n=C_{n^{\prime }}^n{C}_b^{n^{\prime }}=\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(22\right)$

其中，T_ij(i,j=1,2,3)为计算得到的相应值。

（4.3）计算航向角俯仰角θ₂和横滚角γ₂

利用公式（22）得到的载体坐标系b与真实地理坐标系n之间的转换矩 阵可以确定航向角俯仰角θ₂和横滚角γ₂的主值，其表达式如下：

θ_2主＝arctan(T₂₃)

若航向角俯仰角θ₂和横滚角γ₂的取值范围定义为[0,2π]、和 [-π,+π]，那么θ₂和γ₂的真值可由下式确定：

θ₂＝θ_2主                        (24)

由上式所确定的θ₂和γ₂即为SINS精对准后得到的航向角、俯仰角 和横滚角，将其作为SINS进入导航工作状态的初始姿态，初始对准解算完 毕。

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