CN112378401B - 一种惯导***运动加速度估计方法 - Google Patents

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

Info

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

Claims (1)

1.一种惯性导航***运动加速度估计方法，其特征在于：包含如下设定参数：
定义载体坐标系为b系，当地地理坐标系为n系；t时刻，b系下加速度计输出为
Figure FDA0003683550600000011
b系下运动加速度为
Figure FDA0003683550600000012
b系下陀螺仪输出为
Figure FDA0003683550600000013
γ,θ分别为横滚角和俯仰角，
Figure FDA0003683550600000014
为陀螺常值漂移，T为解算周期；包括如下步骤：
步骤一、以***方向余弦矩阵中与重力加速度有关的元素和陀螺常值漂移为状态量，方向余弦矩阵微分方程为基础建立卡尔曼滤波状态方程；
步骤二、以基于一阶马尔科夫过程的运动加速度模型为基础建立卡尔曼滤波器量测方程；
步骤三、对于步骤一建立的卡尔曼滤波状态方程和步骤二建立的卡尔曼滤波器量测方程，进行卡尔曼滤波一步预测和量测更新；
步骤四、依据步骤三获得的更新方程提取运动加速度；
所述步骤一的具体步骤包括：
(1)设计卡尔曼滤波器状态方程
选取状态量
Figure FDA0003683550600000015
***状态方程为：
Figure FDA0003683550600000016
其中，状态阵
Figure FDA0003683550600000017
噪声驱动阵
Figure FDA0003683550600000018
***噪声阵
Figure FDA0003683550600000019
(2)对***状态方程离散化处理可得
Xk+1＝ΦkXk+GWk
其中，Φk＝I6+F·T为状态阵离散形式；
Figure FDA0003683550600000021
为***噪声阵离散形式；
所述步骤二的具体步骤包括：
(1)构建运动加速度模型
ak+1＝m·ak+ηa
其中，m∈(0,1)为一阶马尔科夫系数；ηa为模型误差；
(2)设计卡尔曼滤波器量测方程
Zk＝HXk+Vk
其中，量测量
Figure FDA0003683550600000022
量测矩阵
Figure FDA0003683550600000023
量测噪声阵
Figure FDA0003683550600000024
所述步骤三的具体步骤包括：
卡尔曼滤波器参数初始值X0＝06×1，P0＝10I6×6；***状态一步预测方程为：
Figure FDA0003683550600000025
Figure FDA0003683550600000026
量测更新方程如下：
Figure FDA0003683550600000027
Figure FDA0003683550600000028
Figure FDA0003683550600000029
其中，
***噪声矩阵
Figure FDA00036835506000000210
根据陀螺性能选取合适值；
量测噪声矩阵
Figure FDA0003683550600000031
根据载体动态大小和加速度计性能选取合适值；
所述步骤四的具体步骤包括：
步骤四中得到的k时刻滤波器状态估计值为
Figure FDA0003683550600000032
则此时载体运动加速度估计值为：
Figure FDA0003683550600000033
其中
Figure FDA0003683550600000034
为***状态估计值
Figure FDA0003683550600000035
的前三个元素。
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 CN112378401A (zh) 2021-02-19
CN112378401B true 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)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN115014391A (zh) * 2022-05-31 2022-09-06 中南大学 一种基于fog校正mems惯导误差的方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101173858A (zh) * 2007-07-03 2008-05-07 北京控制工程研究所 一种月面巡视探测器的三维定姿与局部定位方法
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法

Family Cites Families (7)

* 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
CN101290229A (zh) * 2008-06-13 2008-10-22 哈尔滨工程大学 硅微航姿***惯性/地磁组合方法
CN102692225B (zh) * 2011-03-24 2015-03-11 北京理工大学 一种用于低成本小型无人机的姿态航向参考***
CN105300379B (zh) * 2015-10-13 2017-12-12 上海新纪元机器人有限公司 一种基于加速度的卡尔曼滤波姿态估计方法及***
CN106197376B (zh) * 2016-09-23 2018-08-07 华南农业大学 基于单轴mems惯性传感器的车身倾角测量方法
CN108121890A (zh) * 2016-11-28 2018-06-05 北京雷动云合智能技术有限公司 一种基于线性卡尔曼滤波的航姿信息融合方法
CN108592917B (zh) * 2018-04-25 2021-02-23 珠海全志科技股份有限公司 一种基于失准角的卡尔曼滤波姿态估计方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101173858A (zh) * 2007-07-03 2008-05-07 北京控制工程研究所 一种月面巡视探测器的三维定姿与局部定位方法
CN102809377A (zh) * 2012-08-15 2012-12-05 南京航空航天大学 飞行器惯性/气动模型组合导航方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Output-Based Centralized Longitudinal CACC Systems with Wireless Communication Delay and Actuator Delay;Shi-Yuan Han;《2019 IEEE International Conference on Systems, Man and Cybernetics (SMC)》;20191009;全文 *

Also Published As

Publication number Publication date
CN112378401A (zh) 2021-02-19

Similar Documents

Publication Publication Date Title
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN112378401B (zh) 一种惯导***运动加速度估计方法
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
JP4819802B2 (ja) 複数のナビゲーションコンポーネントの整合システム及び方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN109596144B (zh) Gnss位置辅助sins行进间初始对准方法
CN108007477B (zh) 一种基于正反向滤波的惯性行人定位***误差抑制方法
CN103822633A (zh) 一种基于二阶量测更新的低成本姿态估计方法
CN104613965B (zh) 一种基于双向滤波平滑技术的步进式行人导航方法
CN111380516B (zh) 基于里程计测量信息的惯导/里程计车辆组合导航方法及***
CN108827339B (zh) 一种基于惯性辅助的高效视觉里程计
CN109764870B (zh) 基于变换估计量建模方案的载体初始航向估算方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN117053782A (zh) 一种水陆两栖机器人组合导航方法
CN104406592B (zh) 一种用于水下滑翔器的导航***及姿态角校正和回溯解耦方法
CN114184190A (zh) 一种惯性/里程计组合导航***及方法
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN112033438B (zh) 一种基于速度拟合的晃动基座自对准方法
CN111207734B (zh) 一种基于ekf的无人机组合导航方法
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems
CN114413898B (zh) 多传感器数据融合方法、装置、计算机设备及存储介质
CN117146806A (zh) 一种位姿、安装角的矩阵李群估计方法及***
CN116067369A (zh) 一种基于距离和角度的行人协同导航方法
CN115451946A (zh) 一种MEMS-IMU和Wi-Fi组合的室内行人定位方法
CN113267183A (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