CN110132271B - 一种自适应卡尔曼滤波姿态估计算法 - Google Patents

一种自适应卡尔曼滤波姿态估计算法 Download PDF

Info

Publication number
CN110132271B
CN110132271B CN201910000422.1A CN201910000422A CN110132271B CN 110132271 B CN110132271 B CN 110132271B CN 201910000422 A CN201910000422 A CN 201910000422A CN 110132271 B CN110132271 B CN 110132271B
Authority
CN
China
Prior art keywords
accelerometer
state
measuring
updating
follows
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
CN201910000422.1A
Other languages
English (en)
Other versions
CN110132271A (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 CN201910000422.1A priority Critical patent/CN110132271B/zh
Publication of CN110132271A publication Critical patent/CN110132271A/zh
Application granted granted Critical
Publication of CN110132271B publication Critical patent/CN110132271B/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/20—Instruments for performing navigational calculations
    • 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/02—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses
    • G01P15/08—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
    • G01P3/42—Devices characterised by the use of electric or magnetic means
    • G01P3/44—Devices characterised by the use of electric or magnetic means for measuring angular speed
    • 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/02—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses
    • G01P15/08—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values
    • G01P2015/0862—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values being provided with particular means being integrated into a MEMS accelerometer structure for providing particular additional functionalities to those of a spring mass system
    • G01P2015/0865—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values being provided with particular means being integrated into a MEMS accelerometer structure for providing particular additional functionalities to those of a spring mass system using integrated signal processing circuitry

Abstract

本发明公开了一种自适应卡尔曼滤波姿态估计算法，具体包括以下步骤：⑴建立卡尔曼滤波方程；⑵载体运动状态检测；⑶卡尔曼滤波一步预测；⑷陀螺仪量测更新；⑸加速度计量测更新；⑹加速度计量测噪声动态调节。本发明综合利用MEMS‑IMU输出的加速度和角速度信息，进行载体运动状态检测，根据检测结果动态调节加速度计量测噪声阵，使滤波器处于最优状态。即使系统有运动加速度时，依然保持姿态的最优估计，保证系统在不同运动状态下均具有较高的姿态测量精度，有效的提高了系统姿态测量精度。

Description

一种自适应卡尔曼滤波姿态估计算法
技术领域
本发明涉及以微机电惯性测量单元(MEMS-IMU)为核心器件的姿态测量系统，尤其是一种自适应卡尔曼滤波姿态估计算法。
背景技术
本算法是基于MEMS-IMU的姿态测量系统，利用MEMS加速度计和MEMS陀螺仪输出进行数据融合，得到姿态信息的最优估计。在载体处于静止或者匀速运动状态时，加速度计可以准确的测量重力加速度，与陀螺仪输出进行融合可以获得较高的姿态测量精度。当载体有运动加速度时，加速度计输出包含两部分：重力加速度和载体运动加速度，且不能将两种加速度分离开来，导致无法以重力加速度为参考进行姿态解算。即可以认为运动加速度为干扰加速度，使得系统姿态误差增大。因此，有必要对如何有效的避免运动加速度的干扰，使得载体有运动加速度时，依然具有较高的姿态测量精度进行研究。
发明内容
本发明的目的在于提高系统对运动加速度干扰的适应能力，提供一种的自适应卡尔曼滤波姿态估计算法，在载体处于不同动态条件下，滤波器可以自动调整相关参数，实现姿态信息的最优估计。
本发明解决其技术问题是采取以下技术方案实现的：
一种自适应卡尔曼滤波姿态估计算法，其特征在于：具体包括以下步骤：
⑴建立卡尔曼滤波方程；
⑵载体运动状态检测；
⑶卡尔曼滤波一步预测；
⑷陀螺仪量测更新；
⑸加速度计量测更新；
⑹加速度计量测噪声动态调节。
而且，所述步骤⑴中离散系统卡尔曼滤波器基本方程为：
Figure BDA0001933379670000011
其中，状态量X＝[θ γ ωx ωy ωz]T；
量测量Z＝[ax ay az gx gy gz]T；
状态转移矩阵
Figure BDA0001933379670000021
其中
Figure BDA0001933379670000022
量测矩阵
Figure BDA0001933379670000023
系统噪声矩阵E{WkWk T}
Figure BDA0001933379670000024
量测噪声矩阵E{VkVk T}
Figure BDA0001933379670000025
而且，所述步骤⑵，设MEMS陀螺仪输出角速度为Gi＝[gx,i gy,i gz,i]T,i＝1,2,k，MEMS加速度计输出加速度为A＝[ax,i ay,i az,i]T,i＝1,2,k；
系统合角速度为
Figure BDA0001933379670000026
合加速度为
Figure BDA0001933379670000027
设系统当前状态为S，S＝1表示系统处于动态；S＝0表示系统处于静态或者匀速运动状态，系统运动状态判断规则如下：
若
Figure BDA0001933379670000028
且
Figure BDA0001933379670000029
则判断系统处于静态或者匀速运动状态S＝0；若
Figure BDA00019333796700000210
或
Figure BDA00019333796700000211
则系统处于动态S＝1；
Figure BDA00019333796700000212
T为MEMS-IMU采样时间。
而且，所述步骤⑶，卡尔曼滤波器参数初始值X0＝05×1，P0＝05×5，系统状态一步预测方程为：
Figure BDA00019333796700000213
而且，所述步骤⑷，利用陀螺仪输出G＝[gx gy gz]T进行量测更新，更新方程如下：
Figure BDA0001933379670000031
Figure BDA0001933379670000032
Figure BDA0001933379670000033
其中，
Figure BDA0001933379670000034
而且，所述步骤⑸，利用加速度计输出A＝[ax ay az]T进行量测更新，更新方程如下：
Figure BDA0001933379670000035
Figure BDA0001933379670000036
Figure BDA0001933379670000037
其中，
Figure BDA0001933379670000038
而且，所述步骤⑹，加速度计量测噪声计算公式如下：
Figure BDA0001933379670000039
若S＝0，则α1＝0.5，α1＝0.5；
若S＝1，则α1＝0.1，α1＝2.0；
其中，
Figure BDA00019333796700000310
yk为加速度计量测滤波新息，Sk为yk的协方差；
Figure BDA00019333796700000311
是以加速度计量测新息为输入的卡方分布函数，即滤波器根据加速度计量测的新息动态调节量测噪声阵R2的大小，进而调节滤波器增益Kk+1,a的大小，实现滤波器参数的自适应调整。
本发明的优点和积极效果是：
本发明综合利用MEMS-IMU输出的加速度和角速度信息，进行载体运动状态检测，根据检测结果动态调节加速度计量测噪声阵，使滤波器处于最优状态。即使系统有运动加速度时，依然保持姿态的最优估计，保证系统在不同运动状态下均具有较高的姿态测量精度，有效的提高了系统姿态测量精度。
附图说明
图1为本发明姿态股计算法流程图。
具体实施方式
下面结合附图并通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。
一种自适应卡尔曼滤波姿态估计算法，包括以下步骤：
⑴建立卡尔曼滤波方程
已知离散系统卡尔曼滤波器基本方程为：
Figure BDA0001933379670000041
其中，状态量X＝[θ γ ωx ωy ωz]T；
量测量Z＝[ax ay az gx gy gz]T。
状态转移矩阵
Figure BDA0001933379670000042
其中
Figure BDA0001933379670000043
量测矩阵
Figure BDA0001933379670000044
系统噪声矩阵E{WkWk T}
Figure BDA0001933379670000045
量测噪声矩阵E{VkVk T}
Figure BDA0001933379670000046
⑵载体运动状态检测
设MEMS陀螺仪输出角速度为Gi＝[gx,i gy,i gz,i]T,i＝1,2,k，MEMS加速度计输出加速度为A＝[ax,i ay,i az,i]T,i＝1,2,k；系统合角速度为
Figure BDA0001933379670000047
合加速度为
Figure BDA0001933379670000048
设系统当前状态为S，S＝1表示系统处于动态；S＝0表示系统处于静态或者匀速运动状态。
系统运动状态判断规则如下：
若
Figure BDA0001933379670000051
且
Figure BDA0001933379670000052
则判断系统处于静态或者匀速运动状态S＝0；若
Figure BDA0001933379670000053
或
Figure BDA0001933379670000054
则系统处于动态S＝1；
Figure BDA0001933379670000055
T为MEMS-IMU采样时间。
⑶卡尔曼滤波一步预测卡尔曼滤波器参数初始值X0＝05×1，P0＝05×5，系统状态一步预测方程为：
Figure BDA0001933379670000056
⑷陀螺仪量测更新利用陀螺仪输出G＝[gx gy gz]T进行量测更新，更新方程如下：
Figure BDA0001933379670000057
Figure BDA0001933379670000058
Figure BDA0001933379670000059
其中，
Figure BDA00019333796700000510
⑸加速度计量测更新
利用加速度计输出A＝[ax ay az]T进行量测更新，更新方程如下：
Figure BDA00019333796700000511
Figure BDA00019333796700000512
Figure BDA00019333796700000513
其中，
Figure BDA00019333796700000514
⑹加速度计量测噪声动态调节加速度计量测噪声计算公式如下：
Figure BDA00019333796700000515
若S＝0，则α1＝0.5，α1＝0.5；
若S＝1，则α1＝0.1，α1＝2.0；
其中，
Figure BDA0001933379670000061
yk为加速度计量测滤波新息，Sk为yk的协方差；
Figure BDA0001933379670000062
是以加速度计量测新息为输入的卡方分布函数，即滤波器根据加速度计量测的新息动态调节量测噪声阵R2的大小，进而调节滤波器增益Kk+1,a的大小，实现滤波器参数的自适应调整。
尽管为说明目的公开了本发明的实施例和附图，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神和范围内，各种替换、变化和修改都是可能的，因此，本发明的范围不局限于实施例和附图所公开的内容。

Claims (2)

1.一种自适应卡尔曼滤波姿态估计算法，其特征在于：具体包括以下步骤：
⑴建立卡尔曼滤波方程；
⑵载体运动状态检测；
设MEMS陀螺仪输出角速度为Gi＝[gx,i gy,i gz,i]T,i＝1,2,…k，MEMS加速度计输出加速度为A＝[ax,i ay,i az,i]T,i＝1,2,…k；
系统合角速度为
Figure FDA0003498402920000011
合加速度为
Figure FDA0003498402920000012
设系统当前状态为S，S＝1表示系统处于动态；S＝0表示系统处于静态或者匀速运动状态，系统运动状态判断规则如下：
若
Figure FDA0003498402920000013
且
Figure FDA0003498402920000014
则判断系统处于静态或者匀速运动状态S＝0；若
Figure FDA0003498402920000015
或
Figure FDA0003498402920000016
则系统处于动态S＝1；
Figure FDA0003498402920000017
T为MEMS-IMU采样时间；
⑶卡尔曼滤波一步预测；
卡尔曼滤波器参数初始值X0＝05×1，P0＝05×5，系统状态一步预测方程为：
Figure FDA0003498402920000018
⑷陀螺仪量测更新；
利用陀螺仪输出G＝[gx gy gz]T进行量测更新，更新方程如下：
Figure FDA0003498402920000019
Figure FDA00034984029200000110
Figure FDA00034984029200000111
其中，H1＝[03×2 I3×3]，
Figure FDA00034984029200000112
r3＝1，
Figure FDA00034984029200000113
⑸加速度计量测更新；
利用加速度计输出A＝[ax ay az]T进行量测更新，更新方程如下：
Figure FDA0003498402920000021
Figure FDA0003498402920000022
Figure FDA0003498402920000023
其中，
Figure FDA0003498402920000024
⑹加速度计量测噪声动态调节，加速度计量测噪声计算公式如下：
Figure FDA0003498402920000025
若S＝0表示系统处于静态或者匀速运动状态，则α1＝0.5；
若S＝1表示系统处于动态，则α1＝0.1；
其中，
Figure FDA0003498402920000026
yk为加速度计量测滤波新息，Sk为yk的协方差；
Figure FDA0003498402920000027
是以加速度计量测新息为输入的卡方分布函数，即滤波器根据加速度计量测的新息动态调节量测噪声阵R2的大小，进而调节滤波器增益Kk+1,a的大小，实现滤波器参数的自适应调整。
2.根据权利要求1所述的自适应卡尔曼滤波姿态估计算法，其特征在于：所述步骤⑴中离散系统卡尔曼滤波器基本方程为：
Figure FDA0003498402920000028
其中，状态量X＝[θ γ ωx ωy ωz]T；
量测量Z＝[ax ay az gx gy gz]T；
状态转移矩阵
Figure FDA0003498402920000029
其中
Figure FDA00034984029200000210
量测矩阵
Figure FDA00034984029200000211
系统噪声矩阵
Figure FDA0003498402920000031
量测噪声矩阵
Figure FDA0003498402920000032
CN201910000422.1A 2019-01-02 2019-01-02 一种自适应卡尔曼滤波姿态估计算法 Active CN110132271B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910000422.1A CN110132271B (zh) 2019-01-02 2019-01-02 一种自适应卡尔曼滤波姿态估计算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910000422.1A CN110132271B (zh) 2019-01-02 2019-01-02 一种自适应卡尔曼滤波姿态估计算法

Publications (2)

Publication Number Publication Date
CN110132271A CN110132271A (zh) 2019-08-16
CN110132271B true CN110132271B (zh) 2022-04-12

Family

ID=67568464

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910000422.1A Active CN110132271B (zh) 2019-01-02 2019-01-02 一种自适应卡尔曼滤波姿态估计算法

Country Status (1)

Country Link
CN (1) CN110132271B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110954103B (zh) * 2019-12-18 2022-02-08 无锡北微传感科技有限公司 基于mems传感器的车体动态姿态估计的方法及系统
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN104316055A (zh) * 2014-09-19 2015-01-28 南京航空航天大学 一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法
CN104596514A (zh) * 2015-01-12 2015-05-06 东南大学 加速度计和陀螺仪的实时降噪系统和方法
CN105043385A (zh) * 2015-06-05 2015-11-11 北京信息科技大学 一种行人自主导航定位的自适应卡尔曼滤波方法
CN107014376A (zh) * 2017-03-01 2017-08-04 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN108827310A (zh) * 2018-07-12 2018-11-16 哈尔滨工程大学 一种船用星敏感器辅助陀螺仪在线标定方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备

Family Cites Families (1)

* 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

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102538781A (zh) * 2011-12-14 2012-07-04 浙江大学 基于机器视觉和惯导融合的移动机器人运动姿态估计方法
CN104316055A (zh) * 2014-09-19 2015-01-28 南京航空航天大学 一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法
CN104596514A (zh) * 2015-01-12 2015-05-06 东南大学 加速度计和陀螺仪的实时降噪系统和方法
CN105043385A (zh) * 2015-06-05 2015-11-11 北京信息科技大学 一种行人自主导航定位的自适应卡尔曼滤波方法
CN107014376A (zh) * 2017-03-01 2017-08-04 华南农业大学 一种适用于农业机械精准作业的姿态倾角估计方法
CN107621264A (zh) * 2017-09-30 2018-01-23 华南理工大学 车载微惯性/卫星组合导航系统的自适应卡尔曼滤波方法
CN108827310A (zh) * 2018-07-12 2018-11-16 哈尔滨工程大学 一种船用星敏感器辅助陀螺仪在线标定方法
CN109163721A (zh) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 姿态测量方法及终端设备

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Energy-Aware Adaptive Attitude Estimation Under External Acceleration for Pedestrian Navigation;Aida Makni，等;《IEEE/ASME TRANSACTIONS ON MECHATRONICS》;20160630;第21卷(第3期);全文 *
基于压力传感器辅助的行人室内定位零速修正方法;赵小明，等;《中国惯性技术学报》;20180228;第26卷(第01期);全文 *

Also Published As

Publication number Publication date
CN110132271A (zh) 2019-08-16

Similar Documents

Publication Publication Date Title
JP6355696B2 (ja) 方位データを改善するためのシステムおよび方法
CN102822626B (zh) 校准在移动装置上的传感器测量
US10209078B2 (en) Local perturbation rejection using time shifting
US8786549B2 (en) Gyro mouse de-drift and hand jitter reduction
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
US20100114487A1 (en) Environmental characteristic determination
CN106813679B (zh) 运动物体的姿态估计的方法及装置
US10197396B2 (en) Always on compass calibration system and methods
AU2015305864B2 (en) Earthmoving machine comprising weighted state estimator
CN110132271B (zh) 一种自适应卡尔曼滤波姿态估计算法
JP4447791B2 (ja) ジャイロメータと加速度計を具備する航空機の姿勢決定装置
EP3732549B1 (en) Method for predicting a motion of an object and method for generating a virtual reality view
CN110595434B (zh) 基于mems传感器的四元数融合姿态估计方法
US10883831B2 (en) Performance of inertial sensing systems using dynamic stability compensation
CN110440756B (zh) 一种惯导系统姿态估计方法
CN113771857B (zh) 一种用于车辆控制的纵向车速估计方法和系统
CN110954103B (zh) 基于mems传感器的车体动态姿态估计的方法及系统
CN114964214B (zh) 一种航姿参考系统的扩展卡尔曼滤波姿态解算方法
CN109674480A (zh) 一种基于改进互补滤波的人体运动姿态解算方法
CN112033405B (zh) 一种室内环境磁异常实时修正与导航方法及装置
CN110389526B (zh) 基于Super-Twisting算法的MEMS陀螺自适应滑模控制方法
CN113608576A (zh) 电子装置控制方法、其电子装置和软件产品
US20240147180A1 (en) Sensor data prediction
CN116595487A (zh) 一种基于动静态切换自适应imu姿态融合方法及系统
EP4309377A1 (en) Sensor data prediction

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