CN105352502B - 一种微惯性航姿参考***的姿态获取方法 - Google Patents

一种微惯性航姿参考***的姿态获取方法 Download PDF

Info

Publication number
CN105352502B
CN105352502B CN201510843867.8A CN201510843867A CN105352502B CN 105352502 B CN105352502 B CN 105352502B CN 201510843867 A CN201510843867 A CN 201510843867A CN 105352502 B CN105352502 B CN 105352502B
Authority
CN
China
Prior art keywords
attitude
carrier
angle
gyro
matrix
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
CN201510843867.8A
Other languages
English (en)
Other versions
CN105352502A (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.)
Hubei Sanjiang Aerospace Hongfeng Control Co Ltd
Original Assignee
Hubei Sanjiang Aerospace Hongfeng Control 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 Hubei Sanjiang Aerospace Hongfeng Control Co Ltd filed Critical Hubei Sanjiang Aerospace Hongfeng Control Co Ltd
Priority to CN201510843867.8A priority Critical patent/CN105352502B/zh
Publication of CN105352502A publication Critical patent/CN105352502A/zh
Application granted granted Critical
Publication of CN105352502B publication Critical patent/CN105352502B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • 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)
  • Gyroscopes (AREA)

Abstract

本发明公开了一种微惯性航姿参考***的姿态获取方法，包括下述步骤：(1)根据传感器采集的载体加速度信息和三维磁场强度信息获得载体的姿态角初值；并根据所述姿态角初值获得当前时刻的***姿态矩阵；(2)根据所述***姿态矩阵获得陀螺漂移误差补偿量，并根据陀螺漂移误差调节模型对陀螺输出的数据进行补偿；(3)根据补偿后的陀螺输出的数据对下一个时刻的***姿态矩阵进行更新，并根据更新后的姿态矩阵获得载体的姿态角。本发明解决了由MEMS陀螺精度低、漂移误差大所导致的航姿参考***姿态解算精度较低，不能满足工程化应用的问题；可以抑制MEMS陀螺漂移，提高姿态解算精度；具有较高的工程化应用价值和推广价值。

Description

一种微惯性航姿参考***的姿态获取方法
技术领域
本发明属于导航技术领域，更具体地，涉及一种微惯性航姿参考***的姿态获取方法。
背景技术
在惯性导航***中，姿态是反映载体运动的一个重要参数，通常姿态解算是根据陀螺的数据和载体的速度、位置信息实时计算载体姿态矩阵，实现导航坐标系对地理坐标系的实时跟随以及载体姿态的解算(俯仰角、横滚角和航向角)。而姿态精度对载体的速度、位置计算、识别跟踪、运动轨迹、性能分析等有重要影响。但是由于陀螺和加速度计的精度有限，并且随着时间的增加误差会不断的积累增大，尤其是低成本的MEMS陀螺漂移误差更大，使***的姿态解算精度越来越低，不能够满足工程技术要求。
发明内容
针对现有技术的缺陷，本发明的目的在于提供一种微惯性航姿参考***的姿态获取方法，旨在解决现有技术中由于MEMS陀螺精度低、漂移误差大导致航姿参考***姿态解算精度较低的技术问题。
本发明提供了一种微惯性航姿参考***的姿态获取方法，包括下述步骤：
(1)根据传感器采集的载体加速度信息和三维磁场强度信息获得载体的姿态角初值；并根据所述姿态角初值获得当前时刻的***姿态矩阵；
(2)根据所述***姿态矩阵获得陀螺漂移误差补偿量，并根据陀螺漂移误差调节模型对陀螺输出的数据进行补偿；
(3)根据补偿后的陀螺输出的数据对下一个时刻的***姿态矩阵进行更新，并根据更新后的姿态矩阵获得载体的姿态角。
更进一步地，步骤(1)具体包括：
(1.1)获得载体坐标系下的载体加速度信息和三维磁场强度信息
(1.2)根据所述载体加速度信息计算载体的俯仰角初值和横滚角初值；并根据所述三维磁场强度信息计算载体的航向角初值；
(1.3)根据载体的俯仰角初值、横滚角初值和航向角初值并结合公式
获得载体当前时刻的姿态矩阵
其中，hx hy hz分别为X轴、Y轴、Z轴的磁场强度，ax ay az分别为载体在X轴、Y轴、Z轴的加速度；载体的俯仰角载体的横滚角g为载体在地理坐标系的重力加速度，载体的航向角Hny＝hx*cosγ+hy*sinθ*sinγ-hz*cosθ*sinγ；Hnx＝hy*cosθ+hz*sinγ；导航坐标系为东北天地理坐标系，中n表示导航坐标系，b表示载体坐标系。
更进一步地，，步骤(2)中，陀螺漂移误差调节模型为ω(t)＝ωgyro(t)+ωC(t)；
其中，载体坐标系下陀螺的角速度信息 分别为三个陀螺的输出信息，ωC(t)为陀螺漂移误差补偿量，ω(t)为陀螺补偿以后的输出，
由于陀螺误差会随着时间的增加不断累积，导致获取的姿态信息含有很大的累积误差。加速度计测量信息是俯仰角、横滚角的观测量，磁强计信息是航向角的观测量，本方法利用加速度计测得信息和磁强计测得的信息对陀螺误差进行补偿，消除陀螺的累积误差，提高姿态获取精度。
更进一步地，步骤(3)具体包括：
(3.1)根据补偿后的陀螺输出数据ω(t)获得陀螺角增量反对称矩阵
(3.2)根据公式对下一个时刻的姿态矩阵中四元数进行更新；
(3.3)根据更新后的四元数对下一个时刻的姿态矩阵进行更新；
(3.4)根据更新后的姿态矩阵计算下一个时刻载体的姿态角；
其中，ωx ωy ωz分别为补偿后三个陀螺的输出数据；Δt为***采样周期；qk为下一个时刻的四元数，qk-1为当前时刻的四元数，M为陀螺角增量的反对称矩阵；更新后的***姿态矩阵
q0,q1,q2,q3为四元数；下一个时刻载体的姿态角包括俯仰角θ＝arcsin(T32)、横滚角航向角
在本发明实施例中，载体的姿态信息可以用于***控制和航姿参考。
本发明提供的微惯性航姿参考***的姿态获取方法，以陀螺、加速度计和磁强计为***基本传感器，并利用加速度计和磁强计的信息对陀螺的漂移进行有效补偿，降低陀螺的漂移误差，根据补偿后的陀螺输出数据对姿态矩阵进行更新，从而提高航姿参考***的姿态解算精度，对惯性导航领域的姿态解算有较大贡献，本发明简单巧妙，符合实际需求进步显著且实用性强，能够大规模推广使用。
附图说明
图1是本发明实施例提供的一种微惯性航姿参考***的姿态获取方法实现流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明提供的微惯性航姿参考***的姿态获取方法，解决了由MEMS陀螺精度低、漂移误差大所导致的航姿参考***姿态解算精度较低，不能满足工程化应用的问题。
如图1所示，本发明实施例提供的微惯性航姿参考***的姿态获取方法，包括以下几个步骤：
步骤1：***姿态矩阵初始化处理；
步骤2：读取加速度计数据和磁强计数据，并对陀螺输出数据进行误差补偿；
步骤3：根据补偿后的陀螺输出数据对***姿态矩阵进行更新并根据更新后的姿态矩阵求取***姿态。
作为本发明的一个实施例，步骤1中根据读取的加速度计和磁强计信息计算***初始姿态矩阵，具体通过以下步骤实现：
(1a)选择东北天地理坐标系作为导航坐标系并从加速度计获得载体加速度信息其中ax ay az分别为载体在X轴、Y轴、Z轴的加速度；以及从磁强计获取三维磁场强度信息其中hx hy hz分别为X轴、Y轴、Z轴的磁场强度；其中，东北天地理坐标系的三个方向分别对应载体的XYZ三个坐标轴，在导航领域是约定成熟的定义，不再详细定义。
(1b)根据公式计算载体的俯仰角角和横滚角，其中θ、γ分别为载体的俯仰角和横滚角，g为载体在地理坐标系的重力加速度；
(1c)根据公式计算载体的航向角；且Hny＝hx*cosγ+hy*sinθ*sinγ-hz*cosθ*sinγ，Hnx＝hy*cosθ+hz*sinγ，其中分别为载体的航向角；
(1d)根据公式
求取载体的初始时刻的姿态矩阵，其中为载体初始时刻的姿态矩阵，θ、γ、分别为载体的俯仰角、横滚角和航向角。
作为本发明的另一个实施例，步骤2中读取加速度计数据和磁强计数据，并对陀螺输出数据进行误差补偿，具体通过以下步骤实现：
(2a)在载体坐标系下从陀螺获得载体的角速度信息其中分别为三个陀螺的输出信息；
(2b)建立陀螺漂移误差调节模型ω(t)＝ωgyro(t)+ωC(t)；其中ω(t)为陀螺补偿以后的输出，ωgyro(t)为陀螺补偿前的输出，ωC(t)为陀螺漂移误差补偿量，且
其中
作为本发明的一个实施例，步骤3和步骤4根据补偿后的陀螺输出数据对***姿态矩阵进行更新；根据更新后的姿态矩阵求取***姿态。具体通过以下步骤实现：
(3a)根据补偿后的陀螺输出数据ω(t)求取陀螺角增量反对称矩阵，令ω(t)＝[ωx ωy ωz]T，其中ωx ωy ωz分别为补偿后三个陀螺的输出信息；且陀螺角增量反对称矩阵Δt为***采样周期；
(3b)根据公式对四元数进行更新，其中，qk为k时刻的四元数，qk-1为k-1时刻的四元数，M为陀螺角增量的反对称矩阵；
(3c)根据公式
对***姿态矩阵进行更新，q0,q1,q2,q3为四元数；
(3d)根据θ＝arcsin(T32)；计算载体的姿态角，其中θ、r、分别表示载体的俯仰角、横滚角和航向角。
通过上述公式就可以得到通过加速度计和磁强计补偿以后的陀螺输出数据，利用补偿后的陀螺输出数据对四元数和姿态矩阵进行更新，通过更新后的姿态矩阵根据公式求解航姿参考***的实时姿态信息。
本发明所实现的一种微惯性航姿参考***的姿态获取方法，以陀螺、加速度计和磁强计为***传感器，并用加速度计和磁强计的信息对陀螺的漂移进行有效补偿，降低陀螺的漂移误差，从而提高航姿参考***的姿态解算精度，对惯性导航领域的姿态解算有较大贡献，本发明简单巧妙，符合实际需求进步显著且实用性强，能够大规模推广使用。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种微惯性航姿参考***的姿态获取方法，其特征在于，包括下述步骤：
(1)根据传感器采集的载体加速度信息和三维磁场强度信息获得载体的姿态角初值；并根据所述姿态角初值获得当前时刻的***姿态矩阵；
(2)根据所述***姿态矩阵获得陀螺漂移误差补偿量，并根据陀螺漂移误差调节模型对陀螺输出的数据进行补偿；
(3)根据补偿后的陀螺输出数据对下一个时刻的***姿态矩阵进行更新，并根据更新后的姿态矩阵获得载体的姿态角；
步骤(1)具体包括：
(1.1)获得载体坐标系下的载体加速度信息和三维磁场强度信息
(1.2)根据所述载体加速度信息计算载体的俯仰角初值和横滚角初值；并根据所述三维磁场强度信息计算载体的航向角初值；
(1.3)根据载体的俯仰角初值、横滚角初值和航向角初值并结合公式
获得载体当前时刻的姿态矩阵
其中，hx hy hz分别为X轴、Y轴、Z轴的磁场强度，ax ay az分别为载体在X轴、Y轴、Z轴的加速度；载体的俯仰角载体的横滚角g为载体在地理坐标系的重力加速度，载体的航向角Hny＝hx*cosγ+hy*sinθ*sinγ-hz*cosθ*sinγ；Hnx＝hy*cosθ+hz*sinγ；导航坐标系为东北天地理坐标系，中n表示导航坐标系，b表示载体坐标系。
2.如权利要求1所述的姿态获取方法，其特征在于，步骤(2)中，陀螺漂移误差调节模型为ω(t)＝ωgyro(t)+ωC(t)；
其中，载体坐标系下陀螺的角速度信息 分别为三个陀螺的输出信息，ωC(t)为陀螺漂移误差补偿量，ω(t)为陀螺补偿以后的输出，
3.如权利要求2所述的姿态获取方法，其特征在于，步骤(3)具体包括：
(3.1)根据补偿后的陀螺输出数据ω(t)获得陀螺角增量反对称矩阵
(3.2)根据公式对下一个时刻的姿态矩阵中四元数进行更新；
(3.3)根据更新后的四元数对下一个时刻的姿态矩阵进行更新；
(3.4)根据更新后的姿态矩阵计算下一个时刻载体的姿态角；
其中，ωxωyωz分别为补偿后三个陀螺的输出数据；Δt为***采样周期；qk为下一个时刻的四元数，qk-1为当前时刻的四元数，M为陀螺角增量的反对称矩阵；更新后的***姿态矩阵
q0,q1,q2,q3为四元数；下一个时刻载体的姿态角包括载体的俯仰角θ＝arcsin(T32)、横滚角航向角
CN201510843867.8A 2015-11-26 2015-11-26 一种微惯性航姿参考***的姿态获取方法 Active CN105352502B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201510843867.8A CN105352502B (zh) 2015-11-26 2015-11-26 一种微惯性航姿参考***的姿态获取方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201510843867.8A CN105352502B (zh) 2015-11-26 2015-11-26 一种微惯性航姿参考***的姿态获取方法

Publications (2)

Publication Number Publication Date
CN105352502A CN105352502A (zh) 2016-02-24
CN105352502B true CN105352502B (zh) 2017-12-12

Family

ID=55328512

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201510843867.8A Active CN105352502B (zh) 2015-11-26 2015-11-26 一种微惯性航姿参考***的姿态获取方法

Country Status (1)

Country Link
CN (1) CN105352502B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106092098B (zh) * 2016-08-25 2019-01-04 湖北三江航天红峰控制有限公司 一种基于陀螺和倾角仪的载体航姿测量方法
CN106441301A (zh) * 2016-09-19 2017-02-22 北京机械设备研究所 一种飞行器发射初始参数获取方法及***
CN108534744A (zh) * 2018-01-30 2018-09-14 歌尔科技有限公司 一种姿态角获取方法、装置和手柄
CN110231031A (zh) * 2018-03-05 2019-09-13 高德信息技术有限公司 一种姿态角确定方法、装置及***
CN110530396A (zh) * 2019-07-31 2019-12-03 苏州浪潮智能科技有限公司 一种陀螺仪传感器误差的处理方法及设备
CN110377056B (zh) * 2019-08-19 2022-09-20 深圳市道通智能航空技术股份有限公司 无人机航向角初值选取方法及无人机
CN118243096A (zh) * 2024-05-28 2024-06-25 火丰科技(深圳)有限公司 航向轴陀螺的惯性定向方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1740746B (zh) * 2005-05-23 2010-08-04 清华大学 微小型动态载体姿态测量装置及其测量方法
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿***惯性/地磁组合方法
FI124697B (en) * 2012-04-04 2014-12-15 Jc Inertial Oy Vehicle location
CN102879793B (zh) * 2012-09-28 2014-07-23 北京信息科技大学 超小型gps/ins/磁强计/气压计组合导航***
US8915116B2 (en) * 2013-01-23 2014-12-23 Freescale Semiconductor, Inc. Systems and method for gyroscope calibration
CN103776451B (zh) * 2014-03-04 2016-11-09 哈尔滨工业大学 一种基于mems的高精度三维姿态惯性测量***以及测量方法
CN104121905B (zh) * 2014-07-28 2017-02-22 东南大学 一种基于惯性传感器的航向角获取方法
CN104567931B (zh) * 2015-01-14 2017-04-05 华侨大学 一种室内惯性导航定位的航向漂移误差消除方法

Also Published As

Publication number Publication date
CN105352502A (zh) 2016-02-24

Similar Documents

Publication Publication Date Title
CN105352502B (zh) 一种微惯性航姿参考***的姿态获取方法
CN105910606B (zh) 一种基于角速度差值的方向修正方法
CN104501838B (zh) 捷联惯导***初始对准方法
CN107328410A (zh) 用于定位自动驾驶车辆的方法和汽车电脑
CN104374388B (zh) 一种基于偏振光传感器的航姿测定方法
CN107478223A (zh) 一种基于四元数和卡尔曼滤波的人体姿态解算方法
CN104567931A (zh) 一种室内惯性导航定位的航向漂移误差消除方法
CN103940425B (zh) 一种磁‑惯性组合捷联测量方法
CN103914068A (zh) 一种基于栅格地图的服务机器人自主导航方法
CN104748751A (zh) 姿态矩阵的计算方法及基于姿态矩阵的定位导航方法
CN106979780A (zh) 一种无人车实时姿态测量方法
CN113008227B (zh) 一种基于三轴加速度计测姿的地磁二分量测量方法
CN106814753A (zh) 一种目标位置矫正方法、装置及***
CN105180936A (zh) 一种四轴惯性稳定平台***的伺服回路解耦方法
CN109752000A (zh) 一种mems双轴旋转调制型捷联罗经初始对准方法
CN105841698A (zh) 一种无需调零的auv舵角精确实时测量***
CN110672871B (zh) 一种基于组合惯导信息和光电转塔视频跟踪的运动目标测速测向方法
CN107402007A (zh) 一种提高微型ahrs模块精度的方法和微型ahrs模块
CN110058288A (zh) 无人机ins/gnss组合导航***航向误差修正方法及***
CN104121930B (zh) 一种基于加表耦合的mems陀螺漂移误差的补偿方法
CN105486308A (zh) 估计弹目视线角速率的快收敛Kalman滤波器的设计方法
CN103557866A (zh) 一种基于地磁技术的虚拟陀螺仪及算法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN106092098B (zh) 一种基于陀螺和倾角仪的载体航姿测量方法
CN106979779A (zh) 一种无人车实时姿态测量方法

Legal Events

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