[go: up one dir, main page]
More Web Proxy on the site http://driver.im/

CN112378401A - 一种惯导系统运动加速度估计方法 - Google Patents

一种惯导系统运动加速度估计方法 Download PDF

Info

Publication number
CN112378401A
CN112378401A CN202010883340.9A CN202010883340A CN112378401A CN 112378401 A CN112378401 A CN 112378401A CN 202010883340 A CN202010883340 A CN 202010883340A CN 112378401 A CN112378401 A CN 112378401A
Authority
CN
China
Prior art keywords
equation
motion acceleration
state
inertial navigation
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.)
Granted
Application number
CN202010883340.9A
Other languages
English (en)
Other versions
CN112378401B (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 CN202010883340.9A priority Critical patent/CN112378401B/zh
Publication of CN112378401A publication Critical patent/CN112378401A/zh
Application granted granted Critical
Publication of CN112378401B publication Critical patent/CN112378401B/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
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/14—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of gyroscopes

Landscapes

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

Abstract

本发明涉及一种惯性导航系统运动加速度估计方法，包括：步骤一、以系统方向余弦矩阵中与重力加速度有关的元素和陀螺常值漂移为状态量，方向余弦矩阵微分方程为基础建立卡尔曼滤波状态方程；步骤二、以基于一阶马尔科夫过程的运动加速度模型为基础建立卡尔曼滤波器量测方程；步骤三、对于步骤一建立的卡尔曼滤波状态方程和步骤二建立的卡尔曼滤波器量测方程，进行卡尔曼滤波一步预测和量测更新；步骤四、依据步骤三获得的更新方程提取运动加速度。采用本方法，当无外部参考信息且惯导系统不能自对准时，实现了重力加速度与运动加速度解耦，从而实现了对运动加速度的有效估计。

Description

一种惯导系统运动加速度估计方法
技术领域
本发明属于惯性导航系统技术领域，具体涉及一种惯导系统运动加速度估计方法。
背景技术
载体运动时，惯导系统加速度计同时敏感重力加速度和运动加速度。基于惯导系统的载体运动加速度测量，通常利用惯导系统自对准能力或者在外部参考信息辅助，计算得到重力加速度在载体系投影，进而将运动加速度从加速度计输出中分离出来。当无外部参考信息且惯导系统不能自对准时，如何从加速度计输出中有效的提取运动加速度，具有一定的研究意义。
发明内容
本发明的目的在于克服现有技术的不足之处，提供一种当无外部参考信息且惯导系统不能自对准时，可实现对运动加速度的有效估计的惯导系统运动加速度估计方法。
本发明的上述目的通过如下技术方案来实现：
一种惯性导航系统运动加速度估计方法，其特征在于：包括如下步骤：
步骤一、以系统方向余弦矩阵中与重力加速度有关的元素和陀螺常值漂移为状态量，方向余弦矩阵微分方程为基础建立卡尔曼滤波状态方程；
步骤二、以基于一阶马尔科夫过程的运动加速度模型为基础建立卡尔曼滤波器量测方程；
步骤三、对于步骤一建立的卡尔曼滤波状态方程和步骤二建立的卡尔曼滤波器量测方程，进行卡尔曼滤波一步预测和量测更新；
步骤四、依据步骤三获得的更新方程提取运动加速度。
进一步的：包含如下设定参数：
定义载体坐标系为b系，当地地理坐标系为n系；t时刻，b系下加速度计输出为
Figure RE-GDA0002895974390000011
b系下运动加速度为
Figure RE-GDA0002895974390000012
b系下陀螺仪输出为
Figure RE-GDA0002895974390000013
γ,θ分别为横滚角和俯仰角，
Figure RE-GDA0002895974390000014
为陀螺常值漂移，T为解算周期。
进一步的：所述步骤一的具体步骤包括：
(1)设计卡尔曼滤波器状态方程
选取状态量
Figure RE-GDA0002895974390000015
系统状态方程为：
Figure RE-GDA0002895974390000021
其中，状态阵
Figure RE-GDA0002895974390000022
噪声驱动阵
Figure RE-GDA0002895974390000023
系统噪声阵
Figure RE-GDA0002895974390000024
(2)对系统状态方程离散化处理可得
Xk+1＝ΦkXk+GWk
其中，Φk＝I6+F·T为状态阵离散形式；
Figure RE-GDA0002895974390000025
为系统噪声阵离散形式。
进一步的：所述步骤二的具体步骤包括：
(1)构建运动加速度模型
ak+1＝m·ak+ηa
其中，m∈(0,1)为一阶马尔科夫系数；ηa为模型误差；
(2)设计卡尔曼滤波器量测方程
Zk＝HXk+Vk
其中，量测量
Figure RE-GDA0002895974390000026
量测矩阵
Figure RE-GDA0002895974390000027
量测噪声阵
Figure RE-GDA0002895974390000031
进一步的：所述步骤三的具体步骤包括：
卡尔曼滤波器参数初始值X0＝06×1，P0＝10I6×6；系统状态一步预测方程为：
Figure RE-GDA0002895974390000032
Figure RE-GDA0002895974390000033
量测更新方程如下：
Figure RE-GDA0002895974390000034
Figure RE-GDA0002895974390000035
Figure RE-GDA0002895974390000036
其中，
系统噪声矩阵
Figure RE-GDA0002895974390000037
根据陀螺性能选取合适值；
量测噪声矩阵
Figure RE-GDA0002895974390000038
根据载体动态大小和加速度计性能选取合适值。
进一步的：所述步骤四的具体步骤包括：
步骤四中得到的k时刻滤波器状态估计值为
Figure RE-GDA0002895974390000039
则此时载体运动加速度估计值为:
Figure RE-GDA00028959743900000310
其中
Figure RE-GDA00028959743900000311
为系统状态估计值
Figure RE-GDA00028959743900000312
的前三个元素。
本发明具有的优点和积极效果为：
本发明可用于以惯性测量单元(Inertial Measurement Unit，IMU)为核心器件的运动加速度测量系统。载体运动时，惯导系统加速度计同时敏感重力加速度和运动加速度。当无外部参考信息且惯导系统不能自对准时，通过设计基于运动加速度模型的卡尔曼滤波器，对载体运动加速度进行估计，实现了重力加速度与运动加速度解耦，从而实现了对运动加速度的有效估计。
附图说明
图1为本发明的处理流程图。
具体实施方式
下面结合图并通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。
一种惯性导航系统运动加速度估计方法，请参见图1，其发明点为：参数设定为：
定义载体坐标系为b系，当地地理坐标系为n系；t时刻，b系下加速度计输出为
Figure RE-GDA0002895974390000041
b系下运动加速度为
Figure RE-GDA0002895974390000042
b系下陀螺仪输出为
Figure RE-GDA0002895974390000043
γ,θ分别为横滚角和俯仰角，
Figure RE-GDA0002895974390000044
为陀螺常值漂移，T为解算周期。包括如下步骤：
步骤一、以系统方向余弦矩阵中与重力加速度有关的元素和陀螺常值漂移为状态量，方向余弦矩阵微分方程为基础建立卡尔曼滤波状态方程。具体步骤为：
(1)设计卡尔曼滤波器状态方程
选取状态量
Figure RE-GDA0002895974390000045
系统状态方程为：
Figure RE-GDA0002895974390000046
其中，状态阵
Figure RE-GDA0002895974390000047
噪声驱动阵
Figure RE-GDA0002895974390000048
系统噪声阵
Figure RE-GDA0002895974390000049
(2)对系统状态方程离散化处理可得
Xk+1＝ΦkXk+GWk
其中，Φk＝I6+F·T为状态阵离散形式；
Figure RE-GDA0002895974390000051
为系统噪声阵离散形式。
步骤二、以基于一阶马尔科夫过程的运动加速度模型为基础建立卡尔曼滤波器量测方程。具体步骤为：
(1)构建运动加速度模型
ak+1＝m·ak+ηa
其中，m∈(0,1)为一阶马尔科夫系数；ηa为模型误差；
(2)设计卡尔曼滤波器量测方程
Zk＝HXk+Vk
其中，量测量
Figure RE-GDA0002895974390000052
量测矩阵
Figure RE-GDA0002895974390000053
量测噪声阵
Figure RE-GDA0002895974390000054
步骤三、对于步骤一建立的卡尔曼滤波状态方程和步骤二建立的卡尔曼滤波器量测方程，进行卡尔曼滤波一步预测和量测更新。具体步骤为：
卡尔曼滤波器参数初始值X0＝06×1，P0＝10I6×6；系统状态一步预测方程为：
Figure RE-GDA0002895974390000055
Figure RE-GDA0002895974390000056
量测更新方程如下：
Figure RE-GDA0002895974390000057
Figure RE-GDA0002895974390000058
Figure RE-GDA0002895974390000059
其中，
系统噪声矩阵
Figure RE-GDA0002895974390000061
根据陀螺性能选取合适值；
量测噪声矩阵
Figure RE-GDA0002895974390000062
根据载体动态大小和加速度计性能选取合适值。
步骤四、依据步骤三获得的更新方程提取运动加速度。具体步骤为：
步骤四中得到的k时刻滤波器状态估计值为
Figure RE-GDA0002895974390000063
则此时载体运动加速度估计值为
Figure RE-GDA0002895974390000064
其中
Figure RE-GDA0002895974390000065
为系统状态估计值
Figure RE-GDA0002895974390000066
的前三个元素。
尽管为说明目的公开了本发明的实施例和图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和图所公开的内容。

Claims (6)

1.一种惯性导航系统运动加速度估计方法，其特征在于：包括如下步骤：
步骤一、以系统方向余弦矩阵中与重力加速度有关的元素和陀螺常值漂移为状态量，方向余弦矩阵微分方程为基础建立卡尔曼滤波状态方程；
步骤二、以基于一阶马尔科夫过程的运动加速度模型为基础建立卡尔曼滤波器量测方程；
步骤三、对于步骤一建立的卡尔曼滤波状态方程和步骤二建立的卡尔曼滤波器量测方程，进行卡尔曼滤波一步预测和量测更新；
步骤四、依据步骤三获得的更新方程提取运动加速度。
2.根据权利要求1所述的惯性导航系统运动加速度估计方法，其特征在于：包含如下设定参数：
定义载体坐标系为b系，当地地理坐标系为n系；t时刻，b系下加速度计输出为
Figure RE-FDA0002895974380000011
b系下运动加速度为
Figure RE-FDA0002895974380000012
b系下陀螺仪输出为
Figure RE-FDA0002895974380000013
γ,θ分别为横滚角和俯仰角，
Figure RE-FDA0002895974380000014
为陀螺常值漂移，T为解算周期。
3.根据权利要求2所述的惯性导航系统运动加速度估计方法，其特征在于：所述步骤一的具体步骤包括：
(1)设计卡尔曼滤波器状态方程
选取状态量
Figure RE-FDA0002895974380000015
系统状态方程为：
Figure RE-FDA0002895974380000016
其中，状态阵
Figure RE-FDA0002895974380000017
噪声驱动阵
Figure RE-FDA0002895974380000021
系统噪声阵
Figure RE-FDA0002895974380000022
(2)对系统状态方程离散化处理可得
Xk+1＝ΦkXk+GWk
其中，Φk＝I6+F·T为状态阵离散形式；
Figure RE-FDA0002895974380000023
为系统噪声阵离散形式。
4.根据权利要求3所述的惯性导航系统运动加速度估计方法，其特征在于：所述步骤二的具体步骤包括：
(1)构建运动加速度模型
ak+1＝m·ak+ηa
其中，m∈(0,1)为一阶马尔科夫系数；ηa为模型误差；
(2)设计卡尔曼滤波器量测方程
Zk＝HXk+Vk
其中，量测量
Figure RE-FDA0002895974380000024
量测矩阵
Figure RE-FDA0002895974380000025
量测噪声阵
Figure RE-FDA0002895974380000026
5.根据权利要求4所述的惯性导航系统运动加速度估计方法，其特征在于：所述步骤三的具体步骤包括：
卡尔曼滤波器参数初始值X0＝06×1，P0＝10I6×6；系统状态一步预测方程为：
Figure RE-FDA0002895974380000027
Figure RE-FDA0002895974380000028
量测更新方程如下：
Figure RE-FDA0002895974380000031
Figure RE-FDA0002895974380000032
Figure RE-FDA0002895974380000033
其中，
系统噪声矩阵
Figure RE-FDA0002895974380000034
根据陀螺性能选取合适值；
量测噪声矩阵
Figure RE-FDA0002895974380000035
根据载体动态大小和加速度计性能选取合适值。
6.根据权利要求5所述的惯性导航系统运动加速度估计方法，其特征在于：所述步骤四的具体步骤包括：
步骤四中得到的k时刻滤波器状态估计值为
Figure RE-FDA0002895974380000036
则此时载体运动加速度估计值为：
Figure RE-FDA0002895974380000037
其中
Figure RE-FDA0002895974380000038
为系统状态估计值
Figure RE-FDA0002895974380000039
的前三个元素。
CN202010883340.9A 2020-08-28 2020-08-28 一种惯导系统运动加速度估计方法 Active CN112378401B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010883340.9A CN112378401B (zh) 2020-08-28 2020-08-28 一种惯导系统运动加速度估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010883340.9A CN112378401B (zh) 2020-08-28 2020-08-28 一种惯导系统运动加速度估计方法

Publications (2)

Publication Number Publication Date
CN112378401A true CN112378401A (zh) 2021-02-19
CN112378401B CN112378401B (zh) 2022-10-28

Family

ID=74586661

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010883340.9A Active CN112378401B (zh) 2020-08-28 2020-08-28 一种惯导系统运动加速度估计方法

Country Status (1)

Country Link
CN (1) CN112378401B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926A (zh) * 2021-04-21 2021-07-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN115014391A (zh) * 2022-05-31 2022-09-06 中南大学 一种基于fog校正mems惯导误差的方法
CN116338816A (zh) * 2023-03-29 2023-06-27 中国船舶集团有限公司第七〇七研究所 一种基于马尔科夫估计的重力梯度解耦方法
CN116972833A (zh) * 2023-05-19 2023-10-31 哈尔滨工程大学 一种基于自适应增量式卡尔曼滤波的水平姿态测量系统及方法

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
CN101173858A (zh) * 2007-07-03 2008-05-07 北京控制工程研究所 一种月面巡视探测器的三维定姿与局部定位方法
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿系统惯性/地磁组合方法
CN102692225A (zh) * 2011-03-24 2012-09-26 北京理工大学 一种用于低成本小型无人机的姿态航向参考系统
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法
CN105300379A (zh) * 2015-10-13 2016-02-03 上海新纪元机器人有限公司 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN106197376A (zh) * 2016-09-23 2016-12-07 华南农业大学 基于单轴mems惯性传感器的车身倾角测量方法
CN108121890A (zh) * 2016-11-28 2018-06-05 北京雷动云合智能技术有限公司 一种基于线性卡尔曼滤波的航姿信息融合方法
CN108592917A (zh) * 2018-04-25 2018-09-28 珠海全志科技股份有限公司 一种基于失准角的卡尔曼滤波姿态估计方法

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20050240347A1 (en) * 2004-04-23 2005-10-27 Yun-Chun Yang Method and apparatus for adaptive filter based attitude updating
CN101173858A (zh) * 2007-07-03 2008-05-07 北京控制工程研究所 一种月面巡视探测器的三维定姿与局部定位方法
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿系统惯性/地磁组合方法
CN102692225A (zh) * 2011-03-24 2012-09-26 北京理工大学 一种用于低成本小型无人机的姿态航向参考系统
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法
CN105300379A (zh) * 2015-10-13 2016-02-03 上海新纪元机器人有限公司 一种基于加速度的卡尔曼滤波姿态估计方法及系统
CN106197376A (zh) * 2016-09-23 2016-12-07 华南农业大学 基于单轴mems惯性传感器的车身倾角测量方法
CN108121890A (zh) * 2016-11-28 2018-06-05 北京雷动云合智能技术有限公司 一种基于线性卡尔曼滤波的航姿信息融合方法
CN108592917A (zh) * 2018-04-25 2018-09-28 珠海全志科技股份有限公司 一种基于失准角的卡尔曼滤波姿态估计方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
SHI-YUAN HAN: "Output-Based Centralized Longitudinal CACC Systems with Wireless Communication Delay and Actuator Delay", 《2019 IEEE INTERNATIONAL CONFERENCE ON SYSTEMS, MAN AND CYBERNETICS (SMC)》 *
何昊晨: "一种实时移除 ＭＥＭＳ陀螺仪噪声的精准航姿技术", 《重庆大学学报》 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926A (zh) * 2021-04-21 2021-07-27 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN115014391A (zh) * 2022-05-31 2022-09-06 中南大学 一种基于fog校正mems惯导误差的方法
CN116338816A (zh) * 2023-03-29 2023-06-27 中国船舶集团有限公司第七〇七研究所 一种基于马尔科夫估计的重力梯度解耦方法
CN116972833A (zh) * 2023-05-19 2023-10-31 哈尔滨工程大学 一种基于自适应增量式卡尔曼滤波的水平姿态测量系统及方法
CN116972833B (zh) * 2023-05-19 2024-09-24 哈尔滨工程大学 一种基于自适应增量式卡尔曼滤波的水平姿态测量系统及方法

Also Published As

Publication number Publication date
CN112378401B (zh) 2022-10-28

Similar Documents

Publication Publication Date Title
CN112378401B (zh) 一种惯导系统运动加速度估计方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN112097763B (zh) 一种基于mems imu/磁力计/dvl组合的水下运载体组合导航方法
CN100516775C (zh) 一种捷联惯性导航系统初始姿态确定方法
CN105509739B (zh) 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN109883426B (zh) 基于因子图的动态分配与校正多源信息融合方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN110058288B (zh) 无人机ins/gnss组合导航系统航向误差修正方法及系统
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN106949889A (zh) 针对行人导航的低成本mems/gps组合导航系统及方法
CN104635251A (zh) 一种ins/gps组合定位定姿新方法
CN109945895B (zh) 基于渐消平滑变结构滤波的惯性导航初始对准方法
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN111399023B (zh) 基于李群非线性状态误差的惯性基组合导航滤波方法
CN115112119B (zh) 一种基于lstm神经网络辅助的车载导航方法
CN114413898B (zh) 多传感器数据融合方法、装置、计算机设备及存储介质
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
CN116608852A (zh) 一种用于农机惯导设备的陀螺仪温度漂移补偿方法
CN110926465A (zh) 一种mems/gps松组合导航方法
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN114915913A (zh) 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN112033440B (zh) 一种基于梯度下降优化的摇摆基座下无纬度初始对准方法
CN112033438B (zh) 一种基于速度拟合的晃动基座自对准方法
CN114184190A (zh) 一种惯性/里程计组合导航系统及方法
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems

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