CN103674064B - 捷联惯性导航***的初始标定方法 - Google Patents
捷联惯性导航***的初始标定方法 Download PDFInfo
- Publication number
- CN103674064B CN103674064B CN201310656741.0A CN201310656741A CN103674064B CN 103674064 B CN103674064 B CN 103674064B CN 201310656741 A CN201310656741 A CN 201310656741A CN 103674064 B CN103674064 B CN 103674064B
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- navigation system
- strapdown inertial
- error
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
一种捷联惯性导航***的初始标定方法,包括步骤:在捷联惯性导航***保持静止不动的状态下,分别采集陀螺仪和加速度计的输出数据;对捷联惯性导航***进行粗对准解算,得到初始姿态角;构建初始标定***误差的离散***模型,基于所述离散***模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案,利用多个卡尔曼滤波器,并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计,得到北向陀螺仪常值漂移误差的估计值。本发明的技术,可以有效估计出北向陀螺漂移误差参数,利用估计出的高精度陀螺仪漂移误差,为捷联惯性导航***的高精度运行提供了有效支持,有效改善了捷联惯性导航***的可观测性,从而提高了初始标定的精度。
Description
技术领域
本发明涉及惯性导航技术领域,特别是涉及一种捷联惯性导航***的初始标定方法。
背景技术
捷联惯性导航***(Strapdown Inertial Navigation System,SINS)的基本原理是根据牛顿提出的相对惯性空间的力学定律,利用陀螺仪和加速度计测量载体相对于惯性空间的角运动和线运动参数,根据捷联惯性导航编排方程,进行相应的积分运算,连续、实时地提供位置、速度和姿态等导航信息。由于捷联惯性导航***完全依靠自身的惯性敏感元件,不依赖任何外界导航参数,因此,它具有隐蔽性好,不受气候条件限制,抗干扰能力强和无信号丢失等优点,是一种完全自主式、全天候的导航***。因此,被广泛应用于航空、航天和航海等各个领域。初始标定是捷联惯性导航***工作的重要组成部分,标定精度对捷联惯性导航***导航工作时的精度有直接的影响。因此,初始标定是捷联惯性导航***工作过程中十分关键的一步。
通常,捷联惯性导航***初始标定在粗对准完成后进行。粗对准阶段就是在静基座条件下,根据陀螺仪和加速度计输出与已知地球自转角速度和重力加速度的关系,直接计算出载体的初始姿态。初始标定是在粗对准的基础上进行的,通常利用现代控制理论中的状态空间方法,得到惯性导航***初始标定的误差方程,对陀螺仪和加速度计输出的数据进行捷联解算,然后进行估计,获得惯性器件的漂移误差。传统的标定方法利用标准卡尔曼滤波估计方法,对惯性器件的漂移误差进行估计。然而,通过***可观测度分析可知,***中的惯性器件漂移误差的状态可观测度较低,收敛速度较慢,所需估计时间较长,无法对陀螺漂移误差进行精确估计。
发明内容
基于此,有必要针对上述估计时间较长,无法对陀螺漂移误差进行精确估计的问题,提供一种捷联惯性导航***的初始标定方法。
一种捷联惯性导航***的初始标定方法,包括如下步骤:
在捷联惯性导航***保持静止不动的状态下,分别采集陀螺仪和加速度计的输出数据;
根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系,对捷联惯性导航***进行粗对准解算,得到初始姿态角,包括航向角、俯仰角和横滚角;
构建捷联惯性导航***的初始标定***误差的离散***模型,包括状态转移矩阵和量测矩阵;
基于所述离散***模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案,利用多个卡尔曼滤波器,并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计,得到北向陀螺仪常值漂移误差的估计值。
上述捷联惯性导航***的初始标定方法,将陀螺仪的常值漂移误差作为模型的不确定参数,通过将不确定参数量化为多个确定的备选结果,利用多个卡尔曼滤波器分别进行估计。提高了滤波器的收敛速度和精度,进而提高了陀螺仪漂移误差估计的收敛速度和精度,可以有效估计出北向陀螺漂移误差参数,利用估计出的高精度陀螺仪漂移误差,为捷联惯性导航***的高精度运行提供了有效支持。可广泛应用于各种中高精度捷联惯性导航***的初始标定,有效改善了捷联惯性导航***的可观测性,从而提高了初始标定的精度。
附图说明
图1为一个实施例的捷联惯性导航***的初始标定方法流程图。
具体实施方式
下面结合附图对本发明的捷联惯性导航***的初始标定方法的具体实施方式作详细描述。
参见图1所示,图1为一个实施例的捷联惯性导航***的初始标定方法流程图,主要包括如下步骤:
步骤S10,在捷联惯性导航***保持静止不动的状态下,分别采集陀螺仪和加速度计的输出数据。
步骤S20,根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系,对捷联惯性导航***进行粗对准解算,得到初始姿态角;包括航向角俯仰角θ1和横滚角γ1。
步骤S30,构建捷联惯性导航***的初始标定***误差的离散***模型,包括状态转移矩阵和量测矩阵。
步骤S40,基于所述离散***模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案,利用多个卡尔曼滤波器,并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计,得到北向陀螺仪常值漂移误差的估计值。
上述捷联惯性导航***的初始标定方法,将陀螺仪的常值漂移误差作为模型的不确定参数,通过将不确定参数量化为多个确定的备选结果,利用多个卡尔曼滤波器分别进行估计。提高了滤波器的收敛速度和精度,进而提高了陀螺仪漂移误差估计的收敛速度和精度,可以有效估计出北向陀螺漂移误差参数,利用估计出的高精度陀螺仪漂移误差,为捷联惯性导航***的高精度运行提供了有效支持。可广泛应用于各种中高精度捷联惯性导航***的初始标定,有效改善了捷联惯性导航***的可观测性,从而提高了初始标定的精度。
在一个实施例中,所述步骤S10在捷联惯性导航***保持静止不动的状态下,分别采集陀螺仪和加速度计的输出数据的步骤,具体包括如下:
步骤S101,在捷联惯性导航***开机后,预热30分钟,直至陀螺仪和加速度计进入稳定工作状态。
步骤S102,在捷联惯性导航***预热进入稳定工作状态后,保持捷联惯性导航***保持静止不动,采集5分钟的陀螺仪和加速度计的输出数据。
在一个实施例中,所述步骤S20根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系,对捷联惯性导航***进行粗对准解算,得到初始姿态角的步骤,具体包括如下:
步骤S201,根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系计算地理坐标系与载体坐标系间的欧拉角转换矩阵。
步骤S202,根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ1、横滚角γ1和航向角
需要说明的是,上述粗对准及计算初始姿态角过程中,所采用的技术手段并不影响步骤S30的离散***模型的构建过程及步骤S40的北向陀螺仪常值漂移误差的估计值计算过程,步骤S30与步骤S40的过程是在粗对准的基础上完成。
在一个实施例中,所述步骤S30构建捷联惯性导航***的初始标定***误差的离散***模型的步骤,具体包括如下:
步骤S301,在取东北天地理坐标为系导航坐标系,以及忽略位置误差和天向通道的速度误差的条件下建立捷联惯性导航***的初始标定的状态方程。
步骤S302,选取捷联惯性导航***的两个水平误差设为观测量,建立捷联惯性导航***的初始标定的量测方程。
步骤S303,对所述初始标定的状态方程和量测方程进行离散化,获得离散化的初始标定的***误差方程。
在一个实施例中,步骤S40基于所述离散***模型将未知的北向陀螺仪常值漂移量化为多个已知的备选方案,利用多个卡尔曼滤波器,并根据所述初始姿态角对可观测的北向陀螺仪常值漂移进行估计,得到北向陀螺仪常值漂移误差的估计值的步骤,具体包括如下:
步骤S401,基于所述离散***模型,采用多模型估计方式将北向陀螺仪常值漂移误差设为离散***模型的不确定参数,并将所述不确定参数量化为多个确定的备选结果。
优选地,对于上述量化不确定参数获取备选结果的步骤,具体包括:根据估计精度和计算复杂度确定备选结果的个数,并根据陀螺仪的精度水平,量化得到多个确定的备选结果。
步骤S402,利用多个卡尔曼滤波器,并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移分别进行估计,获得北向陀螺仪常值漂移误差的估计值。
在一个实施例中,步骤S402利用多个卡尔曼滤波器,并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移分别进行估计,获得北向陀螺仪常值漂移误差的估计值的步骤,具体包括如下:
步骤a,对所述多个确定的备选结果进行参数初始化处理,获得备选结果的初始状态及其对应多个卡尔曼滤波器的初始概率。
步骤b,以所述初始姿态角为初始值进行捷联解算,确定载体的水平速度,获得北向陀螺仪常值漂移的量测量。
步骤c,根据所述离散***模型和确定的备选结果初始状态计算状态一步预测和一步预测均方误差阵。
步骤d,根据所述离散***模型的量测矩阵和所述一步预测均方误差阵获取新息方差,计算卡尔曼滤波增益和估计均方误差,根据所述北向陀螺仪常值漂移的量测量和状态一步预测获取各个卡尔曼滤波器的新息序列,根据所述新息序列分别求解各个卡尔曼滤波器的状态估计值。
步骤e,根据所述新息方差和各个卡尔曼滤波器的新息序列求解各个卡尔曼滤波器的新息概率,根据所述新息概率对相应的卡尔曼滤波器的初始概率进行更新,根据各个卡尔曼滤波器的状态估计值和更新的各个卡尔曼滤波器的概率获取北向陀螺仪常值漂移误差的估计值。
为了更加清晰本发明的技术方案,下面针对于上述各个实施例的过程作进一步的详细阐述。
在步骤S201中,加速度计输出数据与重力加速度以及陀螺仪输出与地球自转角速度关系如下:
其中,fx1、fy1和fz1以及ωx1、ωy1和ωz1分别为静止位置上的捷联惯性导航***在x轴、y轴和z轴输出的比力和角速率;g为重力加速度;ωie为地球自转角速率在导航坐标系中的投影,其在东、北和天向上的投影为Ω=[0 ωiecosL ωiesinL]T,上标T表示向量的转置;L表示当地纬度;为计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵。
在步骤S202中,根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ1、横滚角γ1和航向角的计算方法,具体包括如下:
所述计算得到计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵步骤包括如下:
航向角俯仰角θ1和横滚角γ1的主值计算公式为:
若θ1和γ1的取值范围分别定义为[0,2π]、和[-π,+π],航向角俯仰角θ1和横滚角γ1的真实值按如下的方法确定:
式中,为欧拉角转换矩阵,所确定的θ1和γ1为捷联惯性导航***粗对准的航向角、俯仰角和横滚角。
在步骤S301中,初始标定的状态方程如下:
式中,状态变量x1(t)=[δvE(t) δvN(t) φE(t) φN(t) φU(t)]T,x2(t)=εN(t),上标T表示向量的转置;其中,δv和φ分别为***的速度误差和姿态误差,ε为陀螺仪的常值漂移;下标E、N、U分别表示导航坐标系下的东向、北向和天向,w(t)=[wax(t) way(t) waz(t) wgx(t) wgy(t) wgz(t)]T为***高斯白噪声向量且E[w(t)wT(t)]=Q(t),其中各个元素均为和时间相关的量,包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪声,下标中的第一个值a和g分别代表加速度计和陀螺仪,第二个值x、y和z分别代表载体坐标系的右方、前方和上方;
***状态转移阵中的F5×5和I5×1表达式分别如下:
式中,Ω=[0 ωiecosL ωiesinL]T=[0 ΩN ΩU]T为地球自转角速度ωie在东向、北向和天向上的投影,L表示当地纬度;g为当地的重力加速度,I5×1=[0 0 0 1 0]T;
G(t)为***噪声转移矩阵,其表达式为:
式中,为载体坐标系b与计算地理坐标系n'之间的欧拉角转换矩阵,为矩阵的前两行元素组成的矩阵。
在步骤S302中,初始标定的量测方程如下:
选取两个水平速度误差δvE和δvN为观测量,建立***的量测方程为:
式中,量测向量为Z(t)=[δvE(t) δvN(t)]T,量测噪声向量为 为高斯白噪声,且E[v(t)vT(t)]=R(t),包括东向和北向速度量测噪声。
在步骤S303中,离散***模型的方程为:
式中,Φk/k-1和Γk-1分别为状态转移矩阵和***噪声转移阵的离散化形式,Hk为量测阵的离散化形式,过程噪声和量测噪声的方差分别为E[w(k)wT(k)]=Q(k)和E[v(k)vT(k)]=R(k)。
对于步骤S402计算北向陀螺仪常值漂移误差的估计值的过程中。
步骤a中的量化后的备选结果的初始化状态值如下:
式中,为备选结果的初始化状态值,i(i=1,2,…,n)为确定备选结果的个数,ε(i)为量化得到的多个确定的备选结果。
初始化各滤波器概率如下:
步骤b中的所述北向陀螺仪常值漂移的量测量如下:
式中,δvEδvN为根据初始标定的状态方程两个水平速度误差选取的观测量。
步骤c中的所述状态一步预测如下:
所述一步预测均方误差阵如下:
式中,Φk/k-1和Γk-1分别为离散***模型的状态转移矩阵和***噪声转移阵,为初始化状态值,Qk-1为过程噪声方差矩阵,Pk-1为***协方差矩阵。
步骤d中的所述新息方差如下:
式中,Hk为离散化的量测矩阵,Pk/k-1为一步预测均方误差阵,Rk为根据陀螺仪、加速度计噪声水平和量测量确定的量测噪声矩阵。
所述卡尔曼滤波增益如下:
所述估计均方误差如下:
所述各个卡尔曼滤波器的新息序列如下:
所述各个卡尔曼滤波器的状态估计值如下:
步骤e中的所述各个卡尔曼滤波器的新息概率如下:
所述各个卡尔曼滤波器的概率的更新如下:
式中,Pk(i)'为根据卡尔曼滤波器的初始概率P0(i)对进行Pk(i)更新后的各个卡尔曼滤波器的概率。
所述北向陀螺仪常值漂移误差的估计值如下:
式中,中第六个元素为北向陀螺仪常值漂移误差的估计值。
基于上述实施例阐述的内容,本发明的捷联惯性导航***的初始标定方法的一个具体实施方式可以如下:
步骤1、捷联惯性导航***预热准备:捷联惯性导航***开机后,预热30分钟,使陀螺仪和加速度计的工作状态稳定。
步骤2、采集陀螺仪和加速度计数据:捷联惯性导航***预热准备完毕后,保持捷联惯性导航***静止不动,采集5分钟的陀螺仪和加速度计的输出数据。
步骤3、粗对准,计算初始姿态角;
步骤(3-1)、计算计算地理坐标系与载体坐标系间的欧拉角转换矩阵
导航坐标系取为东北天地理坐标系,可知加速度计输出与重力加速度,以及陀螺仪输出与地球自转角速度有如下的关系:
其中,fx1、fy1和fz1以及ωx1、ωy1和ωz1分别为静止位置上捷联惯性导航***在x轴、y轴和z轴输出的比力和角速率;g为重力加速度;ωie为地球自转角速率在导航坐标系中的投影,其在东、北和天向上的投影为Ω=[0 ωiecosL ωiesinL]T,上标T表示向量的转置;L表示当地纬度;为计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵。
步骤(3-2)、计算粗对准过程中的俯仰角θ1、横滚角γ1和航向角具体计算过程如下:
由公式(1)和(2)可计算得到计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵
航向角俯仰角θ1和横滚角γ1的主值计算公式为:
若θ1和γ1的取值范围分别定义为[0,2π]、和[-π,+π],那么航向角俯仰角θ1和横滚角γ1的真实值按如下的方法确定:
由上式所确定的θ1和γ1即为捷联惯性导航***粗对准的航向角、俯仰角和横滚角。
步骤4、对北向陀螺仪常值漂移进行多滤波器联合估计;使用简化的初始标定***误差方程,基于多模型估计的思想,将未知的北向陀螺仪常值漂移量化为多个已知的备选方案,采用多个卡尔曼滤波器,对可观测的北向陀螺仪常值漂移进行估计。北向陀螺仪常值漂移的估计过程如下:
步骤(4-1)、建立简化的初始标定***误差方程:
导航坐标系取为东北天地理坐标系,位置误差和天向通道的速度误差忽略。由于水平通道的加速度计常值漂移,以及天向和东向的陀螺仪常值漂移不可观测,因此,将其从传统的十个状态中去除。
初始标定的状态方程如下:
式中,状态变量x1(t)=[δvE(t) δvN(t) φE(t) φN(t) φU(t)]T,x2(t)=εN(t),上标T表示向量的转置;其中,δv和φ分别为***的速度误差和姿态误差,ε为陀螺仪的常值漂移;下标E、N、U分别表示导航坐标系下的东向、北向和天向;其中,w(t)=[wax(t) way(t) waz(t) wgx(t) wgy(t) wgz(t)]T为***高斯白噪声向量且E[w(t)wT(t)]=Q(t),其中各个元素均为和时间相关的量,包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪声,下标中的第一个值a和g分别代表加速度计和陀螺仪,第二个值x、y和z分别代表载体坐标系的右方、前方和上方。
***状态转移阵中的F5×5和I5×1表达式分别如下:
式中,Ω=[0 ωiecosL ωiesinL]T=[0 ΩNΩU]T为地球自转角速度ωie在东向、北向和天向上的投影,L表示当地纬度;g为当地的重力加速度,I5×1=[0 0 0 1 0]T。
G(t)为***噪声转移矩阵,其表达式为:
式中,为载体坐标系b与计算地理坐标系n'之间的欧拉角转换矩阵,为矩阵的前两行元素组成的矩阵,具体可由公式(3)获得。
初始标定的量测方程如下:
选取两个水平速度误差δvE和δvN为观测量,建立***的量测方程为:
式中,量测向量Z(t)=[δvE(t)δvN(t)]T,量测噪声向量 为高斯白噪声,且E[v(t)vT(t)]=R(t),包括东向和北向速度量测噪声。
对公式(6)和(9)进行离散化,建立初始标定的离散***模型。可得离散化后的***方程为
式中,Φk/k-1和Γk-1分别为状态转移矩阵和***噪声转移阵的离散化形式,Hk为量测阵的离散化形式,过程噪声和量测噪声的方差分别为E[w(k)wT(k)]=Q(k)和E[v(k)vT(k)]=R(k)。
步骤(4-2)、北向陀螺仪常值漂移误差估计:
基于多模型估计的思想,将北向陀螺仪常值漂移误差作为模型不确定参数,将不确定参数量化为多个确定的备选结果,利用多个卡尔曼滤波器分别进行估计,获得北向陀螺仪常值漂移误差。
步骤(a)、参数初始化:根据估计精度和计算复杂度之间的折中,确定备选结果的个数i(i=1,2,…,n),并根据陀螺仪的精度水平,量化得到多个确定的备选结果ε(i),并将量化后的备选结果ε(i)初始化状态如下:
由于各个滤波器的概率相同,初始化各滤波器概率如下:
同时,根据陀螺仪、加速度计噪声水平和量测量的精度,确定过程噪声矩阵Q(k)和量测噪声矩阵R(k),初始化***协方差矩阵P0。
步骤(b)、捷联解算:以步骤(3)获得的初始航向角俯仰角θ1和横滚角γ1为初值,进行捷联解算,获得载体的水平速度输出,即可获得量测量如下:
步骤(c)、时间更新:利用公式(10)获得的状态转移矩阵Φk/k-1和公式(11)获得的初始化状态计算状态一步预测如下:
同时计算一步预测均方误差阵如下:
步骤(d)、量测更新:利用公式(10)获得的量测矩阵Hk和公式(15)获得的一步预测均方误差阵Pk/k-1,可得新息方差如下:
进一步可计算卡尔曼滤波增益如下:
同时可得估计均方误差如下:
利用公式(13)获得的量测量zk和公式(14)获得的状态一步预测可得各滤波器新息如下:
进一步,可求解各滤波器状态估计如下:
步骤(e)、多滤波器融合:利用公式(16)获得的新息方差Uk和公式(19)获得的各滤波器新息Jk(i),各滤波器的新息概率求解如下:
利用公式(12)获得的各滤波器初始概率P0(i),各滤波器概率更新如下所示:
进一步,可得状态估计结果如下:
状态估计中第六个元素即为北向陀螺仪常值漂移误差的估计值。
步骤5、将上述步骤获得的北向陀螺漂移误差作为捷联惯性导航***初始标定的结果,初始标定解算完毕。
以上所述实施例仅表达了本发明的几种实施方式,其描述较为具体和详细,但并不能因此而理解为对本发明专利范围的限制。应当指出的是,对于本领域的普通技术人员来说,在不脱离本发明构思的前提下,还可以做出若干变形和改进,这些都属于本发明的保护范围。因此,本发明专利的保护范围应以所附权利要求为准。
Claims (9)
1.一种捷联惯性导航***的初始标定方法,其特征在于,包括如下步骤:
在捷联惯性导航***保持静止不动的状态下,分别采集陀螺仪和加速度计的输出数据;
根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系,对捷联惯性导航***进行粗对准解算,得到初始姿态角,包括航向角、俯仰角和横滚角;
构建捷联惯性导航***的初始标定***误差的离散***模型,包括状态转移矩阵和量测矩阵;
基于所述离散***模型将未知的北向陀螺仪常值漂移误差量化为多个已知的备选方案,利用多个卡尔曼滤波器,并根据所述初始姿态角对可观测的北向陀螺仪常值漂移误差进行估计,得到北向陀螺仪常值漂移误差的估计值,具体包括:
基于所述离散***模型,采用多模型估计方式将北向陀螺仪常值漂移误差设为离散***模型的不确定参数,并将所述不确定参数量化为多个确定的备选结果;利用多个卡尔曼滤波器,并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移误差分别进行估计,获得北向陀螺仪常值漂移误差的估计值;
将所述北向陀螺仪常值漂移误差的估计值作为捷联惯性导航***初始标定的结果。
2.根据权利要求1所述的捷联惯性导航***的初始标定方法,其特征在于,在捷联惯性导航***保持静止不动的状态下,分别采集陀螺仪和加速度计的输出数据的步骤包括:
在捷联惯性导航***开机后,预热30分钟,直至陀螺仪和加速度计进入稳定工作状态;
在捷联惯性导航***预热进入稳定工作状态后,保持捷联惯性导航***静止不动,采集5分钟的陀螺仪和加速度计的输出数据。
3.根据权利要求1所述的捷联惯性导航***的初始标定方法,其特征在于,根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系,对捷联惯性导航***进行粗对准解算,得到初始姿态角的步骤包括:
根据加速度计输出数据与重力加速度之间的关系以及陀螺仪的输出数据与地球自转角速度之间的关系计算地理坐标系与载体坐标系间的欧拉角转换矩阵;
根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ1、横滚角γ1和航向角
4.根据权利要求1所述的捷联惯性导航***的初始标定方法,其特征在于,所述构建捷联惯性导航***的初始标定***误差的离散***模型的步骤包括:
在取东北天地理坐标系为导航坐标系,以及忽略位置误差和天向通道的速度误差的条件下建立捷联惯性导航***的初始标定的状态方程;
选取捷联惯性导航***的两个水平误差设为观测量,建立捷联惯性导航***的初始标定的量测方程;
对所述初始标定的状态方程和量测方程进行离散化,获得离散化的初始标定的***误差方程。
5.根据权利要求1所述的捷联惯性导航***的初始标定方法,其特征在于,利用多个卡尔曼滤波器,并根据所述初始姿态角和多个确定的备选结果对可观测的北向陀螺仪常值漂移误差分别进行估计,获得北向陀螺仪常值漂移误差的估计值的步骤包括:
对所述多个确定的备选结果进行参数初始化处理,获得备选结果的初始状态及其对应多个卡尔曼滤波器的初始概率;
以所述初始姿态角为初始值进行捷联解算,确定载体的水平速度,获得北向陀螺仪常值漂移误差的量测量;
根据所述离散***模型和确定的备选结果初始状态计算状态一步预测和一步预测均方误差阵;
根据所述离散***模型的量测矩阵和所述一步预测均方误差阵获取新息方差,计算卡尔曼滤波增益和估计均方误差,根据所述北向陀螺仪常值漂移误差的量测量和状态一步预测获取各个卡尔曼滤波器的新息序列,根据所述新息序列分别求解各个卡尔曼滤波器的状态估计值;
根据所述新息方差和各个卡尔曼滤波器的新息序列求解各个卡尔曼滤波器的新息概率,根据所述新息概率对相应的卡尔曼滤波器的初始概率进行更新,根据各个卡尔曼滤波器的状态估计值和更新的各个卡尔曼滤波器的概率获取北向陀螺仪常值漂移误差的估计值。
6.根据权利要求5所述的捷联惯性导航***的初始标定方法,其特征在于,将所述不确定参数量化为多个确定的备选结果的步骤包括:
根据估计精度和计算复杂度确定备选结果的个数,并根据陀螺仪的精度水平,量化得到多个确定的备选结果。
7.根据权利要求1至6任一项所述的捷联惯性导航***的初始标定方法,其特征在于,加速度计输出数据与重力加速度以及陀螺仪输出与地球自转角速度关系如下:
其中,fx1、fy1和fz1以及ωx1、ωy1和ωz1分别为静止位置上的捷联惯性导航***在x轴、y轴和z轴输出的比力和角速率;g为重力加速度;ωie为地球自转角速率在导航坐标系中的投影,其在东、北和天向上的投影为Ω=[0 ωiecosL ωiesinL]T,上标T表示向量的转置;L表示当地纬度;为地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵;
根据所述欧拉角转换矩阵计算粗对准过程中的俯仰角θ1、横滚角γ1和航向角具体计算的步骤包括:
计算地理坐标系n'与载体坐标系b之间的欧拉角转换矩阵步骤包括如下:
C11=C22C33-C23C32
C21=-C12C33+C13C32
C21=C12C23-C13C22
航向角俯仰角θ1和横滚角γ1的主值计算公式为:
若θ1和γ1的取值范围分别定义为[0,2π]、和[-π,+π],航向角俯仰角θ1和横滚角γ1的真实值按如下的方法确定:
式中,为欧拉角转换矩阵,所确定的θ1和γ1为捷联惯性导航***粗对准的航向角、俯仰角和横滚角。
8.根据权利要求1至6任一项所述的捷联惯性导航***的初始标定方法,其特征在于,
初始标定的状态方程如下:
式中,状态变量x1(t)=[δvE(t) δvN(t) φE(t) φN(t) φU(t)]T,x2(t)=εN(t),上标T表示向量的转置;其中,δv和φ分别为***的速度误差和姿态误差,ε为陀螺仪的常值漂移;下标E、N、U分别表示导航坐标系下的东向、北向和天向,w(t)=[wax(t) way(t) waz(t) wgx(t) wgy(t) wgz(t)]T为***高斯白噪声向量且E[w(t)wT(t)]=Q(t),其中各个元素均为和时间相关的量,包括三个方向加速度计的随机偏置噪声和三个方向陀螺仪的随机漂移噪声,下标中的第一个值a和g分别代表加速度计和陀螺仪,第二个值x、y和z分别代表载体坐标系的右方、前方和上方;
***状态转移阵中的F5×5和I5×1表达式分别如下:
式中,Ω=[0 ωiecosL ωiesinL]T=[0 ΩN ΩU]T为地球自转角速度ωie在东向、北向和天向上的投影,L表示当地纬度;g为当地的重力加速度,I5×1=[0 0 0 1 0]T;
G(t)为***噪声转移矩阵,其表达式为:
式中,为载体坐标系b与地理坐标系n'之间的欧拉角转换矩阵,为矩阵的前两行元素组成的矩阵;
初始标定的量测方程如下:
选取两个水平速度误差δvE和δvN为观测量,建立***的量测方程为:
式中,量测向量为Z(t)=[δvE(t) δvN(t)]T,量测噪声向量为为高斯白噪声,且E[v(t)vT(t)]=R(t),包括东向和北向速度量测噪声;
离散***模型的方程为:
xk=Φk/k-1xk-1+Γk-1wk-1
zk=Hkxk+vk
式中,Φk/k-1和Γk-1分别为状态转移矩阵和***噪声转移阵的离散化形式,Hk为量测阵的离散化形式,过程噪声和量测噪声的方差分别为E[w(k)wT(k)]=Q(k)和E[v(k)vT(k)]=R(k)。
9.根据权利要求5所述的捷联惯性导航***的初始标定方法,其特征在于,
量化后的备选结果的初始化状态值如下:
式中,为备选结果的初始化状态值,i=1,2,…,n为确定备选结果的个数,ε(i)为量化得到的多个确定的备选结果;
初始化各滤波器概率如下:
所述北向陀螺仪常值漂移误差的量测量如下:
zk=[δvE δvN]T
式中,δvEδvN为根据初始标定的状态方程两个水平速度误差选取的观测量;
所述状态一步预测如下:
所述一步预测均方误差阵如下:
式中,Φk/k-1和Γk-1分别为离散***模型的状态转移矩阵和***噪声转移阵,为初始化状态值,Qk-1为过程噪声方差矩阵,Pk-1为***协方差矩阵;
所述新息方差如下:
式中,Hk为离散化的量测矩阵,Pk/k-1为一步预测均方误差阵,Rk为根据陀螺仪、加速度计噪声水平和量测量确定的量测噪声矩阵;
所述卡尔曼滤波增益如下:
所述估计均方误差如下:
所述各个卡尔曼滤波器的新息序列如下:
所述各个卡尔曼滤波器的状态估计值如下:
所述各个卡尔曼滤波器的新息概率如下:
所述各个卡尔曼滤波器的概率的更新如下:
式中,Pk(i)'为根据卡尔曼滤波器的初始概率P0(i)对进行Pk(i)更新后的各个卡尔曼滤波器的概率;
所述北向陀螺仪常值漂移误差的估计值如下:
式中,中第六个元素为北向陀螺仪常值漂移误差的估计值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310656741.0A CN103674064B (zh) | 2013-12-06 | 2013-12-06 | 捷联惯性导航***的初始标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310656741.0A CN103674064B (zh) | 2013-12-06 | 2013-12-06 | 捷联惯性导航***的初始标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103674064A CN103674064A (zh) | 2014-03-26 |
CN103674064B true CN103674064B (zh) | 2017-02-08 |
Family
ID=50312332
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310656741.0A Active CN103674064B (zh) | 2013-12-06 | 2013-12-06 | 捷联惯性导航***的初始标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103674064B (zh) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104197938A (zh) * | 2014-09-03 | 2014-12-10 | 厦门大学 | 基于脚部惯性传感器的室内定位方法 |
CN104457786B (zh) * | 2014-10-16 | 2017-04-12 | 哈尔滨工程大学 | 一种多传感器融合技术外场捷联惯性组件快速标定方法 |
CN106153073B (zh) * | 2016-06-21 | 2018-10-12 | 东南大学 | 一种全姿态捷联惯导***的非线性初始对准方法 |
CN109945859B (zh) * | 2019-04-01 | 2022-12-06 | 东南大学 | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 |
CN111006675B (zh) * | 2019-12-27 | 2022-10-18 | 西安理工大学 | 基于高精度重力模型的车载激光惯导***自标定方法 |
CN114485641B (zh) * | 2022-01-24 | 2024-03-26 | 武汉梦芯科技有限公司 | 一种基于惯导卫导方位融合的姿态解算方法及装置 |
CN114543843B (zh) * | 2022-03-14 | 2023-06-20 | 中国船舶重工集团公司第七0七研究所 | 一种谐振陀螺仪通道误差标定校正方法 |
Family Cites Families (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN100541135C (zh) * | 2007-12-18 | 2009-09-16 | 哈尔滨工程大学 | 基于多普勒的光纤陀螺捷联惯导***初始姿态确定方法 |
RU2434202C1 (ru) * | 2010-07-21 | 2011-11-20 | Открытое акционерное общество "Раменское приборостроительное конструкторское бюро" (ОАО "РПКБ") | Комплексная система подготовки и навигации летательного аппарата |
CN102706366B (zh) * | 2012-06-19 | 2015-02-25 | 北京航空航天大学 | 一种基于地球自转角速率约束的sins初始对准方法 |
-
2013
- 2013-12-06 CN CN201310656741.0A patent/CN103674064B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN103674064A (zh) | 2014-03-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103674064B (zh) | 捷联惯性导航***的初始标定方法 | |
CN102706366B (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
CN101514900B (zh) | 一种单轴旋转的捷联惯导***初始对准方法 | |
CN101949703B (zh) | 一种捷联惯性/卫星组合导航滤波方法 | |
CN100516775C (zh) | 一种捷联惯性导航***初始姿态确定方法 | |
CN103900565B (zh) | 一种基于差分gps的惯导***姿态获取方法 | |
CN102538792B (zh) | 一种位置姿态***的滤波方法 | |
CN105091907B (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN103389506B (zh) | 一种用于捷联惯性/北斗卫星组合导航***的自适应滤波方法 | |
CN105371844B (zh) | 一种基于惯性/天文互助的惯性导航***初始化方法 | |
CN108226980A (zh) | 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法 | |
CN103792561B (zh) | 一种基于gnss通道差分的紧组合降维滤波方法 | |
WO2013131471A1 (zh) | 一种惯性测量单元的快速标定方法 | |
CN102519485B (zh) | 一种引入陀螺信息的二位置捷联惯性导航***初始对准方法 | |
CN103575299A (zh) | 利用外观测信息的双轴旋转惯导***对准及误差修正方法 | |
CN104236586B (zh) | 基于量测失准角的动基座传递对准方法 | |
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN109945895B (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
CN101963512A (zh) | 船用旋转式光纤陀螺捷联惯导***初始对准方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
CN103453917A (zh) | 一种双轴旋转式捷联惯导***初始对准与自标校方法 | |
CN103697878B (zh) | 一种单陀螺单加速度计旋转调制寻北方法 | |
CN101706284A (zh) | 提高船用光纤陀螺捷联惯导***定位精度的方法 | |
CN104062672A (zh) | 基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法 | |
CN107015259A (zh) | 采用多普勒测速仪计算伪距/伪距率的紧组合方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CP01 | Change in the name or title of a patent holder |
Address after: 510080 water Donggang 8, Dongfeng East Road, Yuexiu District, Guangzhou, Guangdong. Co-patentee after: Beihang University Patentee after: ELECTRIC POWER RESEARCH INSTITUTE, GUANGDONG POWER GRID CO., LTD. Address before: 510080 water Donggang 8, Dongfeng East Road, Yuexiu District, Guangzhou, Guangdong. Co-patentee before: Beihang University Patentee before: Electrical Power Research Institute of Guangdong Power Grid Corporation |
|
CP01 | Change in the name or title of a patent holder |