CN110823213B - 一种提高sins/dr组合导航***相对航向角精度的方法 - Google Patents

一种提高sins/dr组合导航***相对航向角精度的方法 Download PDF

Info

Publication number
CN110823213B
CN110823213B CN201810921129.4A CN201810921129A CN110823213B CN 110823213 B CN110823213 B CN 110823213B CN 201810921129 A CN201810921129 A CN 201810921129A CN 110823213 B CN110823213 B CN 110823213B
Authority
CN
China
Prior art keywords
matrix
error
sins
state
navigation system
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
Application number
CN201810921129.4A
Other languages
English (en)
Other versions
CN110823213A (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.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
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 Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201810921129.4A priority Critical patent/CN110823213B/zh
Publication of CN110823213A publication Critical patent/CN110823213A/zh
Application granted granted Critical
Publication of CN110823213B publication Critical patent/CN110823213B/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

Landscapes

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

Abstract

本发明属于导航***数据后处理技术，具体为一种提高SINS/DR组合导航***相对航向角精度的方法，首先建立改进的SINS/DR组合导航滤波模型，其速度误差状态量为捷联惯导***速度误差减去航位推算***速度误差，进行导航解算和正向卡尔曼滤波之后，对RTS平滑器进行初始化，从后往前进行逆向平滑计算，通过改进滤波模型，一方面避免了***矩阵中通过加速度计的值计算加速度的误差，另一方面避免由于惯导速度发散而导致的观测矩阵中相关项引起的误差。另外对于实时性要求不高或可以进行离线处理的应用场合，通过平滑后处理来提高SINS/DR组合导航***航向角相对精度，从而提高相对位置测量精度。

Description

一种提高SINS/DR组合导航***相对航向角精度的方法
技术领域
本发明属于导航***数据后处理技术，具体涉及一种导航***相对航向角 精度提高方法。
背景技术
在SINS/DR组合导航***中，通常以里程计进行航位推算所得的位置、速 度作为量测量与捷联惯导***进行组合导航，通过卡尔曼滤波对捷联惯导*** 的姿态误差进行估计，从而保持惯导姿态精度。通过对SINS/DR组合导航*** 各个误差项的可观测度进行分析可知，水平姿态角误差可观测，而航向角误差 角在一般情况下不可观测，无法通过线向机动来提高航向失准角可观测性从而 对航向角误差做出估计。
发明内容
本发明的目的在于提供一种提高SINS/DR组合导航***相对航向角精度的 方法，能够提高相对航向精度，减小航向陀螺漂移的影响，从而提高相对位置 测量精度。
本发明的技术方案如下：
一种提高SINS/DR组合导航***相对航向角精度的方法，该方法包括如下 步骤：
1)建立改进的SINS/DR组合导航滤波模型
改进后的状态转移矩阵如下：
Figure BDA0001764259510000011
其中，M1为速度误差微分方程中速度误差因子矩阵，M2为姿态误差微分 方程中速度误差因子矩阵，gn为地理坐标系下的重力矢量，
Figure BDA0001764259510000021
为地理坐标系 相对于惯性坐标系的转动矢量，
Figure BDA0001764259510000022
为姿态矩阵；
改进后的观测矩阵如下：
H＝[I2×2 02×9]
速度误差状态量为捷联惯导***速度误差减去航位推算***速度误差，即
Figure RE-GDA0001898883200000024
2)进行导航解算和正向卡尔曼滤波
按k＝1,2,...N顺时方向计算并存储各时刻的先验状态估计
Figure BDA0001764259510000024
和后验状态估计
Figure BDA0001764259510000025
先验协方差阵
Figure BDA0001764259510000026
和后验协方差阵
Figure BDA0001764259510000027
3)对RTS平滑器进行初始化
Figure BDA0001764259510000028
Figure BDA0001764259510000029
其中，
Figure BDA00017642595100000210
为平滑器状态量初始值，PN为平滑器协方差矩阵初始值，
Figure BDA00017642595100000211
为卡 尔曼滤波最后N时刻的状态估计值，
Figure BDA00017642595100000212
为卡尔曼滤波最后N时刻的协方差矩阵；
4)RTS平滑计算
从k＝N-1开始，从后往前进行逆向平滑计算，计算公式如下式
Figure BDA00017642595100000213
Figure BDA00017642595100000214
Figure BDA00017642595100000215
Figure BDA00017642595100000216
式中，
Figure BDA00017642595100000217
为求逆矩阵中间变量，Fk为***转移矩阵(由状态矩阵A求得)， Kk为平滑增益，Pk为平滑误差估计协方差阵，
Figure BDA00017642595100000218
为平滑滤波器的状态变量，其 他量为步骤2)中正向滤波保存的值。
本发明的显著效果在于：通过改进滤波模型，一方面避免了***矩阵中通 过加速度计的值计算加速度的误差，另一方面避免由于惯导速度发散而导致的 观测矩阵中相关项引起的误差。另外对于实时性要求不高或可以进行离线处理 的应用场合，通过平滑后处理来提高SINS/DR组合导航***航向角相对精度， 从而提高相对位置测量精度。
附图说明
图1a为原滤波模型进行卡尔曼滤波修正后的姿态角误差示意图；
图1b为改进后滤波模型进行卡尔曼滤波修正后的姿态角误差示意图；
图2a为原滤波模型进行RTS平滑修正后的姿态角误差示意图；
图2b为改进后滤波模型进行RTS平滑修正后的姿态角误差比较示意图。
具体实施方式
下面结合附图及具体实施例对本发明作进一步详细说明。
步骤1)建立改进的SINS/DR组合导航滤波模型
根据SINS/DR组合导航***中速度误差微分方程的特性，对组合导航*** 的***矩阵及观测矩阵进行了改进，改进后的***矩阵设置如式(1)所示：
Figure RE-GDA0001957225340000033
其中，M1为速度误差微分方程中速度误差因子矩阵，M2为姿态误差微分方 程中速度误差因子矩阵，gn为地理坐标系下的重力矢量，
Figure BDA0001764259510000032
为地理坐标系相对 于惯性坐标系的转动矢量，
Figure BDA0001764259510000033
为姿态矩阵。
改进后的观测矩阵如式(2)所示：
H＝[I2×2 02×9] (2)
在改进的滤波模型中，***的速度误差状态量不再是SINS的速度误差
Figure RE-GDA0001898883200000042
而是
Figure RE-GDA0001898883200000043
步骤2)正向滤波
进行正常的导航解算及卡尔曼滤波，在计算过程中，按k＝1,2,...N顺时方向 计算并存储各时刻的先验状态估计
Figure BDA0001764259510000043
和后验状态估计
Figure BDA0001764259510000044
先验协方差阵
Figure BDA0001764259510000045
和 后验协方差阵
Figure BDA0001764259510000046
步骤3)平滑滤波器初始化
正向卡尔曼滤波计算结束后，对RTS平滑器进行初始化，初始化方法如式 (3)：
Figure BDA0001764259510000047
其中，
Figure BDA0001764259510000048
为平滑器状态量初始值，PN为平滑器协方差矩阵初始值，
Figure BDA0001764259510000049
为卡 尔曼滤波最后N时刻的状态估计值，
Figure BDA00017642595100000410
为卡尔曼滤波最后N时刻的协方差矩阵。
步骤4)RTS平滑计算
初始化完成后，从k＝N-1开始，从后往前进行逆向平滑计算，计算公式 如式(4)：
Figure BDA00017642595100000411
式中，
Figure BDA00017642595100000412
为求逆矩阵中间变量，Fk为***转移矩阵(由状态矩阵A求得)， Kk为平滑增益，Pk为平滑误差估计协方差阵，
Figure BDA00017642595100000413
为平滑滤波器的状态变量，其 他量为步骤2)中正向滤波保存的值。
利用改进后的滤波模型进行卡尔曼滤波及平滑的结果与常用滤波模型进行 滤波及平滑后的结果进行对比如图1a、图1b、图2a和图2b所示。
使用改进后的滤波模型其平滑修正后姿态角相对精度优于原滤波模型，在 两种滤波模型下RTS平滑后航向角相对精度优于卡尔曼滤波。在改进后的滤波 模型下经过平滑处理后，2000s内航向角相对误差不超过0.002°，航向角的相对 精度得到了极大地提高。通过提高航向角相对精度，即可提高轨迹测量的相对 位置精度。

Claims (1)

1.一种提高SINS/DR组合导航***相对航向角精度的方法，其特征在于，该方法包括如下步骤：
1)建立改进的SINS/DR组合导航滤波模型
改进后的状态转移矩阵如下：
Figure FDA0003574280080000011
其中，M1为速度误差微分方程中速度误差因子矩阵，M2为姿态误差微分方程中速度误差因子矩阵，gn为地理坐标系下的重力矢量，
Figure FDA0003574280080000012
为地理坐标系相对于惯性坐标系的转动矢量，
Figure FDA0003574280080000013
为姿态矩阵；
改进后的观测矩阵如下：
H＝[I2×2 02×9]
速度误差状态量为捷联惯导***速度误差减去航位推算***速度误差，即
Figure FDA0003574280080000014
2)进行导航解算和正向卡尔曼滤波
按k＝1,2,...N顺时方向计算并存储各时刻的先验状态估计
Figure FDA0003574280080000015
和后验状态估计
Figure FDA0003574280080000016
先验协方差阵
Figure FDA0003574280080000017
和后验协方差阵
Figure FDA0003574280080000018
3)对RTS平滑器进行初始化
Figure FDA0003574280080000019
Figure FDA00035742800800000110
其中，
Figure FDA00035742800800000111
为平滑器状态量初始值，PN为平滑器协方差矩阵初始值，
Figure FDA00035742800800000112
为卡尔曼滤波最后N时刻的状态估计值，
Figure FDA00035742800800000113
为卡尔曼滤波最后N时刻的协方差矩阵；
4)RTS平滑计算
从k＝N-1开始，从后往前进行逆向平滑计算，计算公式如下式
Figure FDA0003574280080000021
Figure FDA0003574280080000022
Figure FDA0003574280080000023
Figure FDA0003574280080000024
式中，
Figure FDA0003574280080000025
为求逆矩阵中间变量，Fk为***转移矩阵，其由状态矩阵A求得，Kk为平滑增益，Pk为平滑误差估计协方差阵，
Figure FDA0003574280080000026
为平滑滤波器的状态变量，其他量为步骤2)中正向滤波保存的值。
CN201810921129.4A 2018-08-14 2018-08-14 一种提高sins/dr组合导航***相对航向角精度的方法 Active CN110823213B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810921129.4A CN110823213B (zh) 2018-08-14 2018-08-14 一种提高sins/dr组合导航***相对航向角精度的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810921129.4A CN110823213B (zh) 2018-08-14 2018-08-14 一种提高sins/dr组合导航***相对航向角精度的方法

Publications (2)

Publication Number Publication Date
CN110823213A CN110823213A (zh) 2020-02-21
CN110823213B true CN110823213B (zh) 2022-07-08

Family

ID=69547080

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810921129.4A Active CN110823213B (zh) 2018-08-14 2018-08-14 一种提高sins/dr组合导航***相对航向角精度的方法

Country Status (1)

Country Link
CN (1) CN110823213B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111521178B (zh) * 2020-04-28 2021-01-15 中国人民解放军国防科技大学 基于管长约束的钻探用定位定向仪孔内定位方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6167347A (en) * 1998-11-04 2000-12-26 Lin; Ching-Fang Vehicle positioning method and system thereof
CN101393025A (zh) * 2008-11-06 2009-03-25 哈尔滨工程大学 Auv组合导航***无迹切换方法
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航***及其导航定位方法
CN102519450A (zh) * 2011-12-12 2012-06-27 东南大学 一种用于水下滑翔器的组合导航装置及方法
CN103217158A (zh) * 2012-12-31 2013-07-24 贾继超 一种提高车载sins/od组合导航精度的方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法
CN104977002A (zh) * 2015-06-12 2015-10-14 同济大学 基于sins/双od的惯性组合导航***及其导航方法
CN105318876A (zh) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 一种惯性里程计组合高精度姿态测量方法
CN106767900A (zh) * 2016-11-23 2017-05-31 东南大学 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5398120B2 (ja) * 2007-03-22 2014-01-29 古野電気株式会社 Ｇｐｓ複合航法装置

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6167347A (en) * 1998-11-04 2000-12-26 Lin; Ching-Fang Vehicle positioning method and system thereof
CN101393025A (zh) * 2008-11-06 2009-03-25 哈尔滨工程大学 Auv组合导航***无迹切换方法
CN102445200A (zh) * 2011-09-30 2012-05-09 南京理工大学 微小型个人组合导航***及其导航定位方法
CN102519450A (zh) * 2011-12-12 2012-06-27 东南大学 一种用于水下滑翔器的组合导航装置及方法
CN103217158A (zh) * 2012-12-31 2013-07-24 贾继超 一种提高车载sins/od组合导航精度的方法
CN103278163A (zh) * 2013-05-24 2013-09-04 哈尔滨工程大学 一种基于非线性模型的sins/dvl组合导航方法
CN103389095A (zh) * 2013-07-24 2013-11-13 哈尔滨工程大学 一种用于捷联惯性/多普勒组合导航***的自适应滤波方法
CN105318876A (zh) * 2014-07-09 2016-02-10 北京自动化控制设备研究所 一种惯性里程计组合高精度姿态测量方法
CN104977002A (zh) * 2015-06-12 2015-10-14 同济大学 基于sins/双od的惯性组合导航***及其导航方法
CN106767900A (zh) * 2016-11-23 2017-05-31 东南大学 一种基于组合导航技术的船用光纤捷联惯导***的在线标定方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
SINS/DR组合导航***可观测性研究;严涛等;《现代防御技术》;20120615(第03期);全文 *

Also Published As

Publication number Publication date
CN110823213A (zh) 2020-02-21

Similar Documents

Publication Publication Date Title
CN105806338B (zh) 基于三向卡尔曼滤波平滑器的gnss/ins组合定位定向算法
CN108614426B (zh) 一种基于干扰观测器的多移动机器人编队鲁棒控制方法
CN109029448B (zh) 单目视觉惯性定位的imu辅助跟踪模型
CN103822633B (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN110455309B (zh) 具备在线时间校准的基于msckf的视觉惯性里程计
CN102538781A (zh) 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN108731700B (zh) 一种视觉惯性里程计中的加权欧拉预积分方法
CN108415435B (zh) 一种农业机械圆曲线自动驾驶控制方法
CN111949929A (zh) 一种多传感器融合的四足机器人运动里程计设计方法
CN108490943B (zh) 一种农业机械自适应曲线自动驾驶控制方法
CN110823213B (zh) 一种提高sins/dr组合导航***相对航向角精度的方法
CN111750865A (zh) 一种用于双功能深海无人潜器导航***的自适应滤波导航方法
CN104406592B (zh) 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法
CN108120452A (zh) Mems陀螺仪动态数据的滤波方法
CN111256688A (zh) 一种惯性导航***的预积分算法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN108981691B (zh) 一种天空偏振光组合导航在线滤波与平滑方法
CN112986977B (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN114323007A (zh) 一种载体运动状态估计方法及装置
CN110873577B (zh) 一种水下快速动基座对准方法及装置
CN113916226A (zh) 一种基于最小方差的组合导航***抗扰滤波方法
CN113091754A (zh) 一种非合作航天器位姿一体化估计和惯性参数确定方法
EP3410074B1 (en) Method and device for improving performance of relative-position sensor, and computer storage medium
CN108279025B (zh) 一种基于重力信息的光纤陀螺罗经快速精对准方法
CN109459769A (zh) 一种自主定位方法与***

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