CN111076721A - 一种快速收敛的惯性测量单元安装姿态估计方法 - Google Patents

一种快速收敛的惯性测量单元安装姿态估计方法 Download PDF

Info

Publication number
CN111076721A
CN111076721A CN202010062352.5A CN202010062352A CN111076721A CN 111076721 A CN111076721 A CN 111076721A CN 202010062352 A CN202010062352 A CN 202010062352A CN 111076721 A CN111076721 A CN 111076721A
Authority
CN
China
Prior art keywords
kalman filter
measurement unit
matrix
installation attitude
inertial measurement
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.)
Granted
Application number
CN202010062352.5A
Other languages
English (en)
Other versions
CN111076721B (zh
Inventor
冯木榉
翟昆朋
蔺晓龙
高迪
何文涛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhejiang Rongxin Navigation Technology Co Ltd
Original Assignee
Zhejiang Rongxin Navigation Technology Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Zhejiang Rongxin Navigation Technology Co Ltd filed Critical Zhejiang Rongxin Navigation Technology Co Ltd
Priority to CN202010062352.5A priority Critical patent/CN111076721B/zh
Publication of CN111076721A publication Critical patent/CN111076721A/zh
Application granted granted Critical
Publication of CN111076721B publication Critical patent/CN111076721B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种快速收敛的惯性测量单元安装姿态估计方法，涉及惯性导航/卫星导航组合导航技术领域。本发明将车载的低成本消费级惯性测量单元的安装姿态角分解为一个大角度的先验安装姿态角和一个小角度的失准角，采用粒子滤波器估计大角度的先验安装姿态角，同时采用卡尔曼滤波器估计小角度的失准角。通过本发明的实施，能够完成对惯性测量单元安装姿态角的高精度估计，且具有较快的收敛时间；解决了当前已有方法中限制安装姿态角为小角度或收敛时间较长等问题；将本发明应用于所述组合导航系统可以显著提升其导航精度，具有较大的实用价值和可观的经济效益。

Description

一种快速收敛的惯性测量单元安装姿态估计方法
技术领域
本发明属于惯性导航/卫星导航组合导航技术领域，具体涉及一种快速收敛的惯性测量单元安装姿态估计方法。
背景技术
对于采用低成本消费级惯性测量单元的车载惯性导航/卫星导航组合导航系统，在卫星导航信号较弱或中断时，导航解算的误差会迅速增大。采用车辆非完整约束(车辆在正常行驶时侧向和垂直方向速度分量为0)对系统进行辅助，可以在卫星导航信号较弱或中断时实时校正导航解算的误差，从而有效解决该场景下导航解算误差迅速增大的问题。惯性测量单元相对于车辆的安装姿态信息是正确应用车辆非完整约束的必要条件，但在实际应用中，通常无法精确测定该安装姿态角，而必须采用一些估计的方法来获取该安装姿态角的值。现有文献中提到的方法大多都存在局限性，例如，VINANDE等人的论文中提出了一种采用加速度计数据对惯性测量单元的安装姿态进行估计的方法，但该方法应用于低成本消费级惯性测量单元时，具有较大误差，且收敛时间较慢(几分钟)；又如，WU等人的论文中也提出过一种基于卡尔曼滤波器对惯性测量单元的安装姿态进行估计的方法，但该方法限制安装姿态角为小角度，在实际应用中，这样的要求可能无法满足。
因此，本领域的技术人员致力于开发一种快速收敛的惯性测量单元安装姿态估计方法，以达到提高惯性测量单元安装姿态角估计精度的目的。
发明内容
有鉴于现有技术的上述缺陷，本发明所要解决的技术问题是如何解决车载低成本消费级惯性测量单元无法精确估计安装姿态角且收敛时间较长的问题。
为实现上述目的，本发明公开了一种快速收敛的惯性测量单元安装姿态估计方法，涉及惯性导航卫星导航组合导航技术领域，包括以下步骤：
符号约定：ψβα表示β坐标系到α坐标系的欧拉角(姿态角)，
Figure BDA0002374061530000021
表示β坐标系到α坐标系的旋转矩阵；i表示惯性坐标系，e表示地心地固坐标系，b表示惯性测量单元坐标系(原点位于惯性测量单元中心，坐标轴与惯性测量单元的三个敏感轴重合)，b0表示先验惯性测量单元坐标系(与惯性测量单元坐标系相差一个小角度欧拉角的坐标系)，v表示车体坐标系(原点位于车辆后轮中心，x轴指向车辆正前方，y轴指向车辆正右方，z轴指向车辆正下方)，a表示GNSS天线坐标系(原点位于所述GNSS天线相位中心)；
Figure BDA0002374061530000022
表示量x的估计值，
Figure BDA0002374061530000023
表示量x的测量值。
步骤一、设定粒子数N以及终止阈值N0；
步骤二、初始化粒子群
Figure BDA0002374061530000024
及权重序列
Figure BDA0002374061530000025
各粒子的状态向量
Figure BDA0002374061530000026
均匀取自惯性测量单元在其可能的安装空间内的安装姿态角，权重wj取为等权重：wj＝1/N；
步骤三、置循环变量k←1，所述循环变量代表组合导航卡尔曼滤波器的迭代时刻；
步骤四、用每个粒子对应的所述状态向量(先验安装姿态角
Figure BDA0002374061530000027
)作为所述组合导航卡尔曼滤波器的参数，产生N个不同的卡尔曼滤波器的实例，然后同时运行所述N个实例；
步骤五、读取k时刻各实例所述状态向量估计值，根据下面公式计算每个实例对应的GNSS天线速度的估计值
Figure BDA0002374061530000028
Figure BDA0002374061530000029
上式右侧的{·}j[k]表示，花括号内各估计量取第j个所述卡尔曼滤波器实例在k时刻的值，
Figure BDA00023740615300000210
为k时刻陀螺仪的角速度测量值，
Figure BDA00023740615300000211
为由第j个所述卡尔曼滤波器参数
Figure BDA00023740615300000212
计算得到的先验旋转矩阵；
步骤六、读取k时刻所述GNSS天线速度测量值
Figure BDA00023740615300000213
及其精度σv[k]；
步骤七、根据下面公式更新每个粒子的权重：
Figure BDA00023740615300000214
并归一化：
wj←Awj,j＝1,2,…,N；
其中
Figure BDA00023740615300000215
步骤八、根据下面公式计算有效粒子数：
Figure BDA0002374061530000031
如果有效粒子数小于N0则转步骤九，否则置k←k+1，然后转步骤五；
步骤九、置
Figure BDA0002374061530000032
(找出权重最大的粒子的序号)；
步骤十、取第jm个所述卡尔曼滤波器实例在k时刻的所述状态向量的
Figure BDA0002374061530000033
分估计值
Figure BDA0002374061530000034
根据欧拉角ψβα＝[φ,θ,ψ]T到旋转矩阵
Figure BDA0002374061530000035
Figure BDA0002374061530000036
的计算公式：
Figure BDA0002374061530000037
计算对应的旋转矩阵：
Figure BDA0002374061530000038
然后由第jm个粒子的所述状态向量
Figure BDA0002374061530000039
计算对应的旋转矩阵：
Figure BDA00023740615300000310
最后计算v坐标系相对于b坐标系的旋转矩阵估计值：
Figure BDA00023740615300000311
步骤十一、根据旋转矩阵
Figure BDA00023740615300000312
到欧拉角
Figure BDA00023740615300000313
的计算公式：
Figure BDA00023740615300000314
计算v系相对于b系的欧拉角：
Figure BDA00023740615300000315
这个姿态角即为所述惯性测量单元的所述安装姿态角估计值。
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：根据状态转移矩阵、观测矩阵、状态转移噪声矩阵、观测噪声矩阵，按标准卡尔曼滤波流程进行迭代：
预测：
Figure BDA0002374061530000041
误差协方差矩阵的传播：
P-[k]＝Φ[k-1]P[k-1]Φ[k-1]T+Q；
卡尔曼增益矩阵：
K[k]＝P-[k]H[k]T(H[k]P-[k]H[k]T+R[k])-1；
所述状态向量修正：
Figure BDA0002374061530000042
所述误差协方差矩阵的修正：
P[k]＝(I-K[k]H[k])P-[k]。
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：所述卡尔曼滤波器以给定的所述先验安装姿态角
Figure BDA0002374061530000043
作为参数，由所述先验安装姿态角按下面公式计算所述先验旋转矩阵
Figure BDA0002374061530000044
和
Figure BDA0002374061530000045
Figure BDA0002374061530000046
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：取b坐标系原点的位置、速度、姿态误差(均相对于地心地固坐标系)，陀螺仪和加速度计的零偏，失准角
Figure BDA0002374061530000047
a系到b系的杆臂以及v系到b系的杆臂(均投影到v系)组成24维所述状态向量：
Figure BDA0002374061530000048
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：取所述GNSS天线相位中心的位置、速度以及二维零向量n＝[0,0]T组成8维观测向量：
Figure BDA0002374061530000049
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：所述状态向量x的后3个分量，其状态转移模型为：
Figure BDA00023740615300000410
迭代时间间隔为τ，所述卡尔曼滤波器的所述状态转移矩阵为：
Figure BDA0002374061530000051
其中，
Figure BDA0002374061530000052
Figure BDA0002374061530000053
下标k表示式中各测量值和估计值取k时刻的值；
Figure BDA0002374061530000054
表示地球自转角速度，
Figure BDA0002374061530000055
表示加速度计的比力测量值，[·]×表示方括号中矢量对应的反对称矩阵；
Figure BDA0002374061530000056
为引力加速度，
Figure BDA0002374061530000057
表示纬度
Figure BDA0002374061530000058
处的参考椭球表面地心半径。
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：
所述GNSS天线相位中心位置的观测模型为：
Figure BDA0002374061530000059
所述GNSS天线相位中心速度的所述观测模型为：
Figure BDA00023740615300000510
二维零向量n的所述观测模型为：
Figure BDA00023740615300000511
所述卡尔曼滤波器的所述观测矩阵为：
Figure BDA00023740615300000512
其中，
Figure BDA0002374061530000061
Figure BDA0002374061530000062
Figure BDA0002374061530000063
Figure BDA0002374061530000064
下标k表示式中各测量值和估计值取k时刻的值。
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：所述状态向量初始化为全零向量
Figure BDA0002374061530000065
所述误差协方差矩阵的初始化为对角阵：
Figure BDA0002374061530000066
其中，Δx表示量x的初始不确定度。
进一步地，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：设Srg,Sra,Sbgd,Sbad分别为陀螺仪随机噪声PSD、加速度计随机噪声PSD，陀螺仪零偏变化PSD、加速度计零偏变化PSD，所述状态转移噪声矩阵为：
Figure BDA0002374061530000067
设GNSS设备在k时刻输出的位置和速度精度分别为σr[k],σv[k]，运动约束虚拟观测噪声标准差为ε，观测噪声协方差矩阵为：
Figure BDA0002374061530000068
其中kr和kv是两个大于1的经验参数。
与现有技术相比，通过本发明的实施，达到了以下明显的技术效果：
1、本发明能精确估计车载低成本消费级惯性测量单元的安装姿态角，且能快速收敛，姿态角误差小于2.5度，收敛时间小于一分钟。
2、本发明无需限制惯性测量单元的安装姿态，惯性测量单元以任意姿态安装在车体上均可用此方法精确估计出安装姿态角，为车辆非完整约束辅助的应用提供了充分保证。
3、本发明对于提升低成本消费级车载惯性导航/卫星导航组合导航系统的精度具有重要价值。
附图说明
图1是本发明的一个较佳实施例的流程图；
图2是本发明的一个较佳实施例中惯性测量单元的安装示意图；
图3是本发明的一个较佳实施例中迭代至k＝43时的粒子群权重分布图。
其中：1-GNSS天线，2-惯性测量单元，3-车辆。
具体实施方式
以下参考说明书附图介绍本发明的多个优选实施例，使其技术内容更加清楚和便于理解。本发明可以通过许多不同形式的实施例来得以体现，本发明的保护范围并非仅限于文中提到的实施例。
在附图中，结构相同的部件以相同数字标号表示，各处结构或功能相似的组件以相似数字标号表示。附图所示的每一组件的尺寸和厚度是任意示出的，本发明并没有限定每个组件的尺寸和厚度。为了使图示更清晰，附图中有些地方适当夸大了部件的厚度。为保证所附图像的清晰和简洁，部分装置未在附图中画出，但并不影响相关领域普通技术人员对本发明的理解。
实施例1：
如图2所示，GNSS天线1和惯性测量单元2安装于车辆3的顶部水平处，GNSS天线1用于接收GNSS导航信号，惯性测量单元2用于对角速度和加速度进行测量，导航和测量数据发送到处理器进行处理。
如图1所示，本实施例提供了一种快速收敛的惯性测量单元安装姿态估计方法，流程图包括以下步骤：
步骤一、设定粒子数N＝72以及终止阈值N0＝60。
步骤二、初始化粒子群
Figure BDA0002374061530000081
及权重序列
Figure BDA0002374061530000082
各粒子的状态向量可取为
Figure BDA0002374061530000083
权重wj取为等权重：wj＝1/72。
步骤三、置循环变量k←1，该循环变量代表组合导航卡尔曼滤波器的迭代时刻。
步骤四、用每个粒子对应的状态向量(先验安装姿态角
Figure BDA0002374061530000084
)作为组合导航卡尔曼滤波器的参数，产生72个不同的卡尔曼滤波器的实例，各卡尔曼滤波器的迭代时间间隔均为1秒，然后同时运行这72个实例。
步骤五、读取k时刻各实例状态向量估计值，根据下面公式计算每个实例对应的GNSS天线速度的估计值
Figure BDA0002374061530000085
Figure BDA0002374061530000086
上式右侧的{·}j[k]表示，花括号内各估计量取第j个卡尔曼滤波器实例在k时刻的值，
Figure BDA0002374061530000087
为k时刻陀螺仪的角速度测量值，
Figure BDA0002374061530000088
为由第j个卡尔曼滤波器参数
Figure BDA0002374061530000089
计算得到的先验旋转矩阵。
步骤六、读取k时刻GNSS速度测量值
Figure BDA00023740615300000810
及其精度σv[k]。
步骤七、根据下面公式更新每个粒子的权重：
Figure BDA00023740615300000811
并归一化：
wj←Awj,j＝1,2,…,72；
其中
Figure BDA00023740615300000812
步骤八、根据下面公式计算有效粒子数：
Figure BDA00023740615300000813
如果有效粒子数小于N0则转步骤九，否则置k←k+1，然后转步骤五。
步骤九、如图3所示，本实施例在43秒收敛时各粒子的权重。权重最大的粒子的序号为jm＝54。
步骤十、取第jm个卡尔曼滤波器实例在k时刻的状态向量的
Figure BDA00023740615300000814
分量估计值
Figure BDA00023740615300000815
根据欧拉角ψβα＝[φ,θ,ψ]T到旋转矩阵
Figure BDA00023740615300000816
的计算公式：
Figure BDA0002374061530000091
计算对应的旋转矩阵：
Figure BDA0002374061530000092
然后由第jm个粒子的状态向量
Figure BDA0002374061530000093
计算对应的旋转矩阵：
Figure BDA0002374061530000094
最后计算v坐标系相对于b坐标系的旋转矩阵估计值，结果为：
Figure BDA0002374061530000095
步骤十一、根据旋转矩阵
Figure BDA0002374061530000096
到欧拉角
Figure BDA0002374061530000097
的计算公式：
Figure BDA0002374061530000098
计算v系相对于b系的欧拉角：
Figure BDA0002374061530000099
这个姿态角即为惯性测量单元的安装姿态角估计值。
以上详细描述了本发明的较佳具体实施例。应当理解，本领域的普通技术无需创造性劳动就可以根据本发明的构思作出诸多修改和变化。因此，凡本技术领域中技术人员依本发明的构思在现有技术的基础上通过逻辑分析、推理或者有限的实验可以得到的技术方案，皆应在由权利要求书所确定的保护范围内。

Claims (9)

1.一种快速收敛的惯性测量单元安装姿态估计方法，其特征在于，包括以下步骤：
步骤一、设定粒子数N以及终止阈值N0；
步骤二、初始化粒子群
Figure FDA0002374061520000011
及权重序列
Figure FDA0002374061520000012
各粒子的状态向量
Figure FDA0002374061520000013
均匀取自惯性测量单元在其可能的安装空间内的安装姿态角，权重wj取为等权重：wj＝1/N；
步骤三、置循环变量k←1，所述循环变量代表组合导航卡尔曼滤波器的迭代时刻；
步骤四、用每个粒子对应的所述状态向量(先验安装姿态角
Figure FDA0002374061520000014
)作为所述组合导航卡尔曼滤波器的参数，产生N个不同的卡尔曼滤波器的实例，然后同时运行所述N个实例；
步骤五、读取k时刻各实例所述状态向量估计值，根据下面公式计算每个实例对应的GNSS天线速度的估计值
Figure FDA0002374061520000015
Figure FDA0002374061520000016
上式右侧的{·}j[k]表示，花括号内各估计量取第j个所述卡尔曼滤波器实例在k时刻的值，
Figure FDA0002374061520000017
为k时刻陀螺仪的角速度测量值，
Figure FDA0002374061520000018
为由第j个所述卡尔曼滤波器参数
Figure FDA0002374061520000019
计算得到的先验旋转矩阵；
步骤六、读取k时刻所述GNSS天线速度测量值
Figure FDA00023740615200000110
及其精度σv[k]；
步骤七、根据下面公式更新每个粒子的权重：
Figure FDA00023740615200000111
并归一化：
wj←Awj,j＝1,2,…,N；
其中
Figure FDA00023740615200000112
步骤八、根据下面公式计算有效粒子数：
Figure FDA00023740615200000113
如果有效粒子数小于N0则转步骤九，否则置k←k+1，然后转步骤五；
步骤九、置
Figure FDA0002374061520000021
(找出权重最大的粒子的序号)；
步骤十、取第jm个所述卡尔曼滤波器实例在k时刻的所述状态向量的
Figure FDA0002374061520000022
分估计值
Figure FDA0002374061520000023
根据欧拉角ψβα＝[φ,θ,ψ]T到旋转矩阵
Figure FDA0002374061520000024
Figure FDA0002374061520000025
的计算公式：
Figure FDA0002374061520000026
计算对应的旋转矩阵：
Figure FDA0002374061520000027
然后由第jm个粒子的所述状态向量
Figure FDA0002374061520000028
计算对应的旋转矩阵：
Figure FDA0002374061520000029
最后计算v坐标系相对于b坐标系的旋转矩阵估计值：
Figure FDA00023740615200000210
步骤十一、根据旋转矩阵
Figure FDA00023740615200000211
到欧拉角
Figure FDA00023740615200000212
的计算公式：
Figure FDA00023740615200000213
计算v系相对于b系的欧拉角：
Figure FDA00023740615200000214
这个姿态角即为所述惯性测量单元的所述安装姿态角估计值。
2.根据权利要求1所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：根据状态转移矩阵、观测矩阵、状态转移噪声矩阵、观测噪声矩阵，按标准卡尔曼滤波流程进行迭代：
预测：
Figure FDA00023740615200000215
误差协方差矩阵的传播：
P-[k]＝Φ[k-1]P[k-1]Φ[k-1]T+Q；
卡尔曼增益矩阵：
K[k]＝P-[k]H[k]T(H[k]P-[k]H[k]T+R[k])-1；
所述状态向量修正：
Figure FDA0002374061520000031
所述误差协方差矩阵的修正：
P[k]＝(I-K[k]H[k])P-[k]。
3.根据权利要求1所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：所述卡尔曼滤波器以给定的所述先验安装姿态角
Figure FDA0002374061520000032
作为参数，由所述先验安装姿态角按下面公式计算所述先验旋转矩阵
Figure FDA0002374061520000033
和
Figure FDA0002374061520000034
Figure FDA0002374061520000035
4.根据权利要求1所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：取b坐标系原点的位置、速度、姿态误差(均相对于地心地固坐标系)，陀螺仪和加速度计的零偏，失准角
Figure FDA0002374061520000036
a系到b系的杆臂以及v系到b系的杆臂(均投影到v系)组成24维所述状态向量：
Figure FDA0002374061520000037
5.根据权利要求1所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：取所述GNSS天线相位中心的位置、速度以及二维零向量n＝[0,0]T组成8维观测向量：
Figure FDA0002374061520000038
6.根据权利要求2所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：所述状态向量x的后3个分量，其状态转移模型为：
Figure FDA0002374061520000039
迭代时间间隔为τ，所述卡尔曼滤波器的所述状态转移矩阵为：
Figure FDA0002374061520000041
其中，
Figure FDA0002374061520000042
Figure FDA0002374061520000043
下标k表示式中各测量值和估计值取k时刻的值；
Figure FDA0002374061520000044
表示地球自转角速度，
Figure FDA0002374061520000045
表示加速度计的比力测量值，[·]×表示方括号中矢量对应的反对称矩阵；
Figure FDA0002374061520000046
为引力加速度，
Figure FDA0002374061520000047
表示纬度
Figure FDA0002374061520000048
处的参考椭球表面地心半径。
7.根据权利要求2所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：
所述GNSS天线相位中心位置的观测模型为：
Figure FDA0002374061520000049
所述GNSS天线相位中心速度的所述观测模型为：
Figure FDA00023740615200000410
二维零向量n的所述观测模型为：
Figure FDA00023740615200000411
所述卡尔曼滤波器的所述观测矩阵为
Figure FDA00023740615200000412
其中，
Figure FDA0002374061520000051
Figure FDA0002374061520000052
Figure FDA0002374061520000053
Figure FDA0002374061520000054
下标k表示式中各测量值和估计值取k时刻的值。
8.根据权利要求2所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：所述状态向量初始化为全零向量
Figure FDA0002374061520000055
所述误差协方差矩阵的初始化为对角阵：
Figure FDA0002374061520000056
其中，Δx表示量x的初始不确定度。
9.根据权利要求2所述的快速收敛的惯性测量单元安装姿态估计方法，其特征在于，所述步骤四中所述组合导航卡尔曼滤波器，其结构与迭代过程还包括：设Srg,Sra,Sbgd,Sbad分别为陀螺仪随机噪声PSD、加速度计随机噪声PSD，陀螺仪零偏变化PSD、加速度计零偏变化PSD，所述状态转移噪声矩阵为：
Figure FDA0002374061520000057
设GNSS设备在k时刻输出的位置和速度精度分别为σr[k],σv[k]，运动约束虚拟观测噪声标准差为ε，观测噪声协方差矩阵为：
Figure FDA0002374061520000061
其中kr和kv是两个大于1的经验参数。
CN202010062352.5A 2020-01-19 2020-01-19 一种快速收敛的惯性测量单元安装姿态估计方法 Active CN111076721B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010062352.5A CN111076721B (zh) 2020-01-19 2020-01-19 一种快速收敛的惯性测量单元安装姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010062352.5A CN111076721B (zh) 2020-01-19 2020-01-19 一种快速收敛的惯性测量单元安装姿态估计方法

Publications (2)

Publication Number Publication Date
CN111076721A true CN111076721A (zh) 2020-04-28
CN111076721B CN111076721B (zh) 2023-03-28

Family

ID=70323869

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010062352.5A Active CN111076721B (zh) 2020-01-19 2020-01-19 一种快速收敛的惯性测量单元安装姿态估计方法

Country Status (1)

Country Link
CN (1) CN111076721B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111678514A (zh) * 2020-06-09 2020-09-18 电子科技大学 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN114526734A (zh) * 2022-03-01 2022-05-24 长沙金维信息技术有限公司 用于车载组合导航的安装角补偿方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101975585A (zh) * 2010-09-08 2011-02-16 北京航空航天大学 一种基于mrupf的捷联惯导系统大方位失准角初始对准方法
US20150350849A1 (en) * 2014-05-31 2015-12-03 Apple Inc. Location Determination Using Dual Statistical Filters
CN106840211A (zh) * 2017-03-24 2017-06-13 东南大学 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN106932802A (zh) * 2017-03-17 2017-07-07 安科智慧城市技术（中国）有限公司 一种基于扩展卡尔曼粒子滤波的导航方法及系统
CN108981696A (zh) * 2018-08-01 2018-12-11 西北工业大学 一种sins任意失准角无奇异快速传递对准方法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101975585A (zh) * 2010-09-08 2011-02-16 北京航空航天大学 一种基于mrupf的捷联惯导系统大方位失准角初始对准方法
US20150350849A1 (en) * 2014-05-31 2015-12-03 Apple Inc. Location Determination Using Dual Statistical Filters
CN106932802A (zh) * 2017-03-17 2017-07-07 安科智慧城市技术（中国）有限公司 一种基于扩展卡尔曼粒子滤波的导航方法及系统
CN106840211A (zh) * 2017-03-24 2017-06-13 东南大学 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN108981696A (zh) * 2018-08-01 2018-12-11 西北工业大学 一种sins任意失准角无奇异快速传递对准方法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王贇贇: "基于组合导航的汽车姿态数据采集系统设计" *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111678514A (zh) * 2020-06-09 2020-09-18 电子科技大学 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN114526734A (zh) * 2022-03-01 2022-05-24 长沙金维信息技术有限公司 用于车载组合导航的安装角补偿方法
CN114526734B (zh) * 2022-03-01 2022-11-29 长沙金维信息技术有限公司 用于车载组合导航的安装角补偿方法

Also Published As

Publication number Publication date
CN111076721B (zh) 2023-03-28

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN102435763B (zh) 一种基于星敏感器的航天器姿态角速度测量方法
CN111076721B (zh) 一种快速收敛的惯性测量单元安装姿态估计方法
CN110006427B (zh) 一种低动态高振动环境下的bds/ins紧组合导航方法
CN110174121A (zh) 一种基于地磁场自适应修正的航姿系统姿态解算方法
EP4220086A1 (en) Combined navigation system initialization method and apparatus, medium, and electronic device
US20200226768A1 (en) Method for determining a protection radius of a vision-based navigation system
CN111141313B (zh) 一种提高机载局部相对姿态匹配传递对准精度的方法
CN111238467A (zh) 一种仿生偏振光辅助的无人作战飞行器自主导航方法
EP1508776A1 (en) Autonomous velocity estimation and navigation
CN109211231A (zh) 一种基于牛顿迭代法的炮弹姿态估计方法
CN108225375A (zh) 一种基于中值滤波的抗外速度野值的优化粗对准方法
CN109506674B (zh) 一种加速度的校正方法及装置
CN113008272B (zh) 一种用于微小卫星的mems陀螺在轨常值漂移标定方法和系统
Kim et al. Rigid Body Inertia Estimation Using Extended Kalman and Savitzky‐Golay Filters
CN108871319A (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN110375773B (zh) Mems惯导系统姿态初始化方法
CN111982126A (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN114858166B (zh) 基于最大相关熵卡尔曼滤波器的imu姿态解算方法
CN107747944B (zh) 基于融合权重矩阵的机载分布式pos传递对准方法和装置
CN114608588A (zh) 基于脉冲到达时间差的差分x射线脉冲星导航方法
Yang et al. The solution of drone attitudes on Lie groups
Corbetta et al. Attitude estimation of a motorcycle via Unscented Kalman Filter
Li et al. Exploring the Potential of Deep Learning Aided Kalman Filter for GNSS/INS Integration: A Study on 2D Simulation Datasets
CN108507571A (zh) 一种高速运动学下imu姿态捕捉方法及系统

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant