CN112325881B - 一种惯导系统姿态解算方法 - Google Patents

一种惯导系统姿态解算方法 Download PDF

Info

Publication number
CN112325881B
CN112325881B CN202011021793.7A CN202011021793A CN112325881B CN 112325881 B CN112325881 B CN 112325881B CN 202011021793 A CN202011021793 A CN 202011021793A CN 112325881 B CN112325881 B CN 112325881B
Authority
CN
China
Prior art keywords
coordinate system
attitude
carrier
fixed connection
earth
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
CN202011021793.7A
Other languages
English (en)
Other versions
CN112325881A (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.)
707th Research Institute of CSIC
Original Assignee
707th Research Institute of CSIC
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 707th Research Institute of CSIC filed Critical 707th Research Institute of CSIC
Priority to CN202011021793.7A priority Critical patent/CN112325881B/zh
Publication of CN112325881A publication Critical patent/CN112325881A/zh
Application granted granted Critical
Publication of CN112325881B publication Critical patent/CN112325881B/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/18—Stabilised platforms, e.g. by gyroscope
    • 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
    • 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)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种惯导系统姿态解算方法，以零均值加速度假设为理论依据，设计了基于地球固联坐标系的载体姿态更新方案；首先，选取t＝0时刻载体坐标系b为地球固联坐标系k1，即
Figure DDA0002700856820000011
利用陀螺输出更新姿态矩阵，得到给定时间区间内载体坐标系b与地球固联坐标系k1的转换关系
Figure DDA0002700856820000012
进而得到加速度计在地球固联坐标系k1的输出
Figure DDA0002700856820000016
根据零均值加速度假设，可以得到地球固联坐标系k1系与地理坐标系n的方向余弦矩阵
Figure DDA0002700856820000013
结合载体坐标系b与地球固联坐标系k1的转换关系
Figure DDA0002700856820000014
可以得到给定时间区间载体坐标系b与地理坐标系n的方向余弦矩阵
Figure DDA0002700856820000015
在下一个给定时间区间重复上述过程，即可解算得到载体姿态信息。本发明有效降低了载体瞬时加速度对姿态精度的影响，较好的解决了动态条件下系统姿态解算问题。

Description

一种惯导系统姿态解算方法
技术领域
本发明属于捷联式惯导系统技术领域，具体涉及一种惯导系统姿态解算方法。
背景技术
捷联式惯导系统包含三轴加速度计和三轴陀螺仪，利用加速度计和陀螺仪输出可以解算得到系统水平姿态。传统的姿态解算算法通常假设载体运动加速度为零，即加速度计只敏感重力加速度矢量。上述假设在载体处于静态或者匀速运动时成立，可以解算得到准确的姿态信息。然而，实际应用中，载体加速度是实时变化的，即零加速度假设不成立，导致姿态解算精度不高。因此，如何提高运动条件下惯导系统的姿态解算精度，具有一定的研究意义。
发明内容
本发明的目的在于克服现有技术的不足之处，提供一种惯导系统姿态解算方法。
本发明为解决上述技术问题采取的技术方案为：
一种惯导系统姿态解算方法，其特征在于：包括如下设定参数：
定义系统解算周期为Ts，零均值加速度区间长度为M＝nTs(n>0，正整数)；载体坐标系为b系，当地地理坐标系为e系，地球固联坐标系为k1系；t时刻，b系下加速度计输出为
Figure BDA0002700856800000011
b系下陀螺仪输出为
Figure BDA0002700856800000012
e系下重力矢量为Ge＝[0 0 -g]T，姿态四元数Q(t)＝[q0 q1 q2 q3]T；解算步骤如下：
步骤1、选定地球固联坐标系：
定义t＝0时刻载体坐标系为地球固联坐标系，0时刻b系和k1系之间的方向余弦矩阵和姿态四元数分别见表述式(1)和(2)
Figure BDA0002700856800000013
Figure BDA0002700856800000014
步骤2、保存t∈(0,nTs]区间陀螺仪数据和加速度计数据，存储序列分别为：
Figure BDA0002700856800000015
Figure BDA0002700856800000016
步骤3、计算t∈(0,nTs]区间加速度计在k1系输出：
由保存的陀螺仪数据进行陀螺姿态更新，更新方程见表述式(5)和(6)：
Figure BDA0002700856800000021
姿态四元数
Figure BDA0002700856800000022
t∈(0,nTs]可以得到b系与k1系的方向余弦矩阵，并保存
Figure BDA0002700856800000023
Figure BDA0002700856800000024
加速度计输出在k1系的表达式为：
Figure BDA0002700856800000025
步骤4、计算k1系与e系转换关系
在t∈(0,nTs]区间内，加速度计输出在k1系的均值为：
Figure BDA0002700856800000026
根据零均值加速度假设可知：
Figure BDA0002700856800000027
求解上式得到横滚角R和俯仰角P：
Figure BDA0002700856800000028
Figure BDA0002700856800000029
相应的e系与k1系的姿态四元数为：
Figure BDA0002700856800000031
步骤5、计算b系与e系转换关系：
根据步骤3中保存的
Figure BDA0002700856800000032
t∈(0,nTs]和步骤4中
Figure BDA0002700856800000033
得到t＝nTs时刻，载体姿态四元数：
Figure BDA0002700856800000034
至此，得到了t＝nTs时刻载体姿态信息；
步骤6、重新选定地球固联坐标系：
定义t＝Ts时刻载体坐标系为地球固联坐标系，Ts时刻的b系和k1系之间的方向余弦矩阵和姿态四元数分别见表述式(14)和(15)
Figure BDA0002700856800000035
Figure BDA0002700856800000036
步骤7、更新陀螺数据存储序列：
Figure BDA0002700856800000037
步骤8、重复步骤3～5，得到t＝(n+1)Ts时刻载体姿态四元数
Figure BDA0002700856800000038
至此，得到了t＝(n+1)Ts时刻载体姿态信息；
步骤9、重复步骤7和8，即可计算得到后续每个解算周期的载体姿态信息。
本发明具有的优点和积极效果：
本发明以零均值加速度假设为理论依据，设计了基于地球固联坐标系的载体姿态更新方案，将设定时间区间内的加速度计输出转换到地球固联坐标系，周期性的对系统姿态进行修正，有效降低了载体瞬时加速度对姿态精度的影响，较好的解决了动态条件下系统姿态解算问题。
附图说明
图1是本发明的解算流程图。
具体实施方式
下面结合图并通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。
一种惯导系统姿态解算方法，参见图1，该解算方法以零均值加速度假设为理论依据，设计了基于地球固联坐标系的载体姿态更新方案。首先，选取t＝0时刻载体坐标系b为地球固联坐标系k1，即
Figure BDA0002700856800000041
利用陀螺输出更新姿态矩阵，得到给定时间区间内载体坐标系b与地球固联坐标系k1的转换关系
Figure BDA0002700856800000042
进而得到加速度计在地球固联坐标系k1的输出
Figure BDA0002700856800000043
根据零均值加速度假设，可以得到地球固联坐标系k1系与地理坐标系n的方向余弦矩阵
Figure BDA0002700856800000044
结合载体坐标系b与地球固联坐标系k1的转换关系
Figure BDA0002700856800000045
可以得到给定时间区间载体坐标系b与地理坐标系n的方向余弦矩阵
Figure BDA0002700856800000046
在下一个给定时间区间重复上述过程，即可解算得到载体姿态信息。
本惯导系统姿态解算方法的设定参数包括：
定义系统解算周期为Ts，零均值加速度区间长度为M＝nTs(n>0)；载体坐标系为b系，当地地理坐标系为e系，地球固联坐标系为k1系；t时刻，b系下加速度计输出为
Figure BDA0002700856800000047
b系下陀螺仪输出为
Figure BDA0002700856800000048
e系下重力矢量为Ge＝[0 0 -g]T，姿态四元数Q(t)＝[q0 q1 q2 q3]T。
本惯导系统姿态解算方法的具体解算步骤包括：
步骤1、选定地球固联坐标系：
定义t＝0时刻载体坐标系为地球固联坐标系，0时刻的b系和k1系之间的方向余弦矩阵和姿态四元数分别见表述式(1)和(2)
Figure BDA0002700856800000049
Figure BDA00027008568000000410
步骤2、保存t∈(0,nTs]区间陀螺仪数据和加速度计数据，存储序列分别为：
Figure BDA00027008568000000411
Figure BDA00027008568000000412
步骤3、计算t∈(0,nTs]区间加速度计在k1系输出：
由保存的陀螺仪数据进行陀螺姿态更新，更新方程见表述式(5)和(6)：
Figure BDA0002700856800000051
姿态四元数
Figure BDA0002700856800000052
t∈(0,nTs]可以得到b系与k1系的方向余弦矩阵，并保存
Figure BDA0002700856800000053
Figure BDA0002700856800000054
加速度计输出在k1系的表达式为：
Figure BDA0002700856800000055
步骤4、计算k1系与e系转换关系
在t∈(0,nTs]区间内，加速度计输出在k1系的均值为：
Figure BDA0002700856800000056
根据零均值加速度假设可知：
Figure BDA0002700856800000057
求解上式得到横滚角R和俯仰角P：
Figure BDA0002700856800000058
Figure BDA0002700856800000059
相应e系与k1系的姿态四元数为：
Figure BDA0002700856800000061
步骤5、计算b系与e系转换关系：
根据步骤3中保存的
Figure BDA0002700856800000062
t∈(0,nTs]和步骤4中
Figure BDA0002700856800000063
得到t＝nTs时刻，载体姿态四元数：
Figure BDA0002700856800000064
至此，得到了t＝nTs时刻载体姿态信息；
步骤6、重新选定地球固联坐标系：
定义t＝Ts时刻载体坐标系为地球固联坐标系，TS时刻的b系和k1系之间的方向余弦矩阵和姿态四元数分别见表述式(14)和(15)
Figure BDA0002700856800000065
Figure BDA0002700856800000066
步骤7、更新陀螺数据存储序列：
Figure BDA0002700856800000067
步骤8、重复步骤3～5，得到t＝(n+1)Ts时刻载体姿态四元数
Figure BDA0002700856800000068
至此，得到了t＝(n+1)Ts时刻载体姿态信息；
步骤9、重复步骤7和8，即可计算得到后续每个解算周期的载体姿态信息。
尽管为说明目的公开了本发明的实施例和图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和图所公开的内容。

Claims (1)

1.一种惯导系统姿态解算方法，其特征在于：包括如下设定参数：
定义系统解算周期为Ts，零均值加速度区间长度为M＝nTs(n>0，正整数)；载体坐标系为b系，当地地理坐标系为e系，地球固联坐标系为k1系；t时刻，b系下加速度计输出为
Figure FDA0002700856790000011
b系下陀螺仪输出为
Figure FDA0002700856790000012
e系下重力矢量为Ge＝[0 0 -g]T，姿态四元数Q(t)＝[q0 q1 q2 q3]T；解算步骤如下：
步骤1、选定地球固联坐标系：
定义t＝0时刻载体坐标系为地球固联坐标系，0时刻b系和k1系之间的方向余弦矩阵和姿态四元数分别见表述式(1)和(2)
Figure FDA0002700856790000013
Figure FDA0002700856790000014
步骤2、保存t∈(0,nTs]区间陀螺仪数据和加速度计数据，存储序列分别为：
Figure FDA0002700856790000015
Figure FDA0002700856790000016
步骤3、计算t∈(0,nTs]区间加速度计在k1系输出：
由保存的陀螺仪数据进行陀螺姿态更新，更新方程见表述式(5)和(6)：
Figure FDA0002700856790000017
姿态四元数
Figure FDA0002700856790000018
可以得到b系与k1系的方向余弦矩阵，并保存
Figure FDA0002700856790000019
Figure FDA00027008567900000110
加速度计输出在k1系的表达式为：
Figure FDA0002700856790000021
步骤4、计算k1系与e系转换关系
在t∈(0,nTs]区间内，加速度计输出在k1系的均值为：
Figure FDA0002700856790000022
根据零均值加速度假设可知：
Figure FDA0002700856790000023
求解上式得到横滚角R和俯仰角P：
Figure FDA0002700856790000024
Figure FDA0002700856790000025
相应的e系与k1系的姿态四元数为：
Figure FDA0002700856790000026
步骤5、计算b系与e系转换关系：
根据步骤3中保存的
Figure FDA0002700856790000027
和步骤4中
Figure FDA0002700856790000028
得到t＝nTs时刻，载体姿态四元数：
Figure FDA0002700856790000029
至此，得到了t＝nTs时刻载体姿态信息；
步骤6、重新选定地球固联坐标系：
定义t＝Ts时刻载体坐标系为地球固联坐标系，Ts时刻的b系和k1系之间的方向余弦矩阵和姿态四元数分别见表述式(14)和(15)
Figure FDA00027008567900000210
Figure FDA0002700856790000031
步骤7、更新陀螺数据存储序列：
Figure FDA0002700856790000032
步骤8、重复步骤3～5，得到t＝(n+1)Ts时刻载体姿态四元数
Figure FDA0002700856790000033
至此，得到了t＝(n+1)Ts时刻载体姿态信息；
步骤9、重复步骤7和8，即可计算得到后续每个解算周期的载体姿态信息。
CN202011021793.7A 2020-09-25 2020-09-25 一种惯导系统姿态解算方法 Active CN112325881B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011021793.7A CN112325881B (zh) 2020-09-25 2020-09-25 一种惯导系统姿态解算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011021793.7A CN112325881B (zh) 2020-09-25 2020-09-25 一种惯导系统姿态解算方法

Publications (2)

Publication Number Publication Date
CN112325881A CN112325881A (zh) 2021-02-05
CN112325881B true CN112325881B (zh) 2022-04-12

Family

ID=74304057

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011021793.7A Active CN112325881B (zh) 2020-09-25 2020-09-25 一种惯导系统姿态解算方法

Country Status (1)

Country Link
CN (1) CN112325881B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101672649A (zh) * 2009-10-20 2010-03-17 哈尔滨工程大学 一种基于数字低通滤波的船用光纤捷联系统系泊对准方法
CN102706349A (zh) * 2012-06-06 2012-10-03 辽宁工程技术大学 一种基于光纤捷联罗经技术的载体姿态确定方法
CN102706366A (zh) * 2012-06-19 2012-10-03 北京航空航天大学 一种基于地球自转角速率约束的sins初始对准方法
CN102997932A (zh) * 2011-09-15 2013-03-27 北京自动化控制设备研究所 一种消除高精度惯导系统标定中转台抖动影响的方法
CN103900565A (zh) * 2014-03-04 2014-07-02 哈尔滨工程大学 一种基于差分gps的惯导系统姿态获取方法
CN104198765A (zh) * 2014-09-15 2014-12-10 大连楼兰科技股份有限公司 车辆运动加速度检测的坐标系转换方法
CN108534775A (zh) * 2018-06-25 2018-09-14 中国铁道科学研究院集团有限公司 基于捷联式惯性导航系统的空间轨迹重建方法及装置

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104064869B (zh) * 2014-06-13 2016-10-05 北京航天万达高科技有限公司 基于mems惯导的双四元数动中通天线控制方法及系统

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101672649A (zh) * 2009-10-20 2010-03-17 哈尔滨工程大学 一种基于数字低通滤波的船用光纤捷联系统系泊对准方法
CN102997932A (zh) * 2011-09-15 2013-03-27 北京自动化控制设备研究所 一种消除高精度惯导系统标定中转台抖动影响的方法
CN102706349A (zh) * 2012-06-06 2012-10-03 辽宁工程技术大学 一种基于光纤捷联罗经技术的载体姿态确定方法
CN102706366A (zh) * 2012-06-19 2012-10-03 北京航空航天大学 一种基于地球自转角速率约束的sins初始对准方法
CN103900565A (zh) * 2014-03-04 2014-07-02 哈尔滨工程大学 一种基于差分gps的惯导系统姿态获取方法
CN104198765A (zh) * 2014-09-15 2014-12-10 大连楼兰科技股份有限公司 车辆运动加速度检测的坐标系转换方法
CN108534775A (zh) * 2018-06-25 2018-09-14 中国铁道科学研究院集团有限公司 基于捷联式惯性导航系统的空间轨迹重建方法及装置

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A Linear Kalman Filter for MARG Orientation Estimation Using the Algebraic Quaternion Algorithm;Roberto G. Valenti，等;《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》;20160228;第65卷(第02期);全文 *
以地理座标系计算的捷联式惯性导航系统的误差分析;高龙海;《现代防御技术》;19790830(第04期);全文 *

Also Published As

Publication number Publication date
CN112325881A (zh) 2021-02-05

Similar Documents

Publication Publication Date Title
CN110231029B (zh) 一种水下机器人多传感器融合数据处理方法
CN110702143B (zh) 基于李群描述的sins捷联惯性导航系统动基座快速初始对准方法
CN105910606A (zh) 一种基于角速度差值的方向修正方法
CN107741240B (zh) 一种适用于动中通的组合惯导系统自适应初始对准方法
CN109631939B (zh) 一种基于磁强计和加速度计的快速对准方法
CN111366984B (zh) 一种基于重力卫星星间激光测距系统确定引力场模型的方法
CN111722295B (zh) 一种水下捷联式重力测量数据处理方法
CN115855049B (zh) 基于粒子群优化鲁棒滤波的sins/dvl导航方法
CN116481564B (zh) 基于Psi角误差修正模型的极地双惯导协同标定方法
CN113587925A (zh) 一种惯性导航系统及其全姿态导航解算方法与装置
CN112212862A (zh) 一种改进粒子滤波的gps/ins组合导航方法
CN112665570B (zh) 一种基于星敏感器的mems陀螺零偏在轨简化工程计算方法
CN113108781B (zh) 一种应用于无人船行进间的改进粗对准方法
CN111256688A (zh) 一种惯性导航系统的预积分算法
CN112325881B (zh) 一种惯导系统姿态解算方法
CN111060140B (zh) 一种地球椭球模型下的极区惯性导航误差获得方法
CN116499493A (zh) 一种基于低频逆向滤波的惯导快速对准方法
CN111637892A (zh) 一种基于视觉和惯性导航组合的移动机器人定位方法
CN114061574B (zh) 一种基于位置不变约束及零速校正的采煤机定姿定向方法
CN111649738B (zh) 微重力场下的加速度计初始姿态解算方法
CN113532479A (zh) 基于李群乘性卡尔曼滤波的捷联惯导系统运动初始对准方法
CN112697166B (zh) 一种运动状态下的捷联惯导系统自对准方法
CN111189473A (zh) 一种基于磁传感器和加表的航姿系统陀螺误差补偿方法
CN116182900B (zh) 纬度未知条件下动基座大失准角一体化对准方法
CN117570976B (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