CN110440756B - 一种惯导***姿态估计方法 - Google Patents

一种惯导***姿态估计方法 Download PDF

Info

Publication number
CN110440756B
CN110440756B CN201910571246.7A CN201910571246A CN110440756B CN 110440756 B CN110440756 B CN 110440756B CN 201910571246 A CN201910571246 A CN 201910571246A CN 110440756 B CN110440756 B CN 110440756B
Authority
CN
China
Prior art keywords
measurement
equation
attitude
accelerometer
quaternion
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
CN201910571246.7A
Other languages
English (en)
Other versions
CN110440756A (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 CN201910571246.7A priority Critical patent/CN110440756B/zh
Publication of CN110440756A publication Critical patent/CN110440756A/zh
Application granted granted Critical
Publication of CN110440756B publication Critical patent/CN110440756B/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
    • G01C9/00—Measuring inclination, e.g. by clinometers, by levels

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

本发明涉及一种惯导***姿态估计方法，其技术特点在于：包括以下步骤：步骤1、以姿态四元数为状态量，四元数姿态更新微分方程为基础建立卡尔曼滤波器状态方程；步骤2、以加速度计测量值为输入建立四元数伪卡尔曼波量测方程；步骤3、根据步骤1建立的卡尔曼滤波状态方程和步骤2建立的四元数伪卡尔曼波量测方程进行卡尔曼滤波量测更新，计算量测更新残差值，并根据加速度计量测更新的残差值对量测噪声方差进行实时估计，通过运动加速度的大小动态调节量测噪声方差阵的大小，进而保证***在动态条件下的姿态测量精度。本发明有效的提高了动态条件下水平姿态估计精度。

Description

一种惯导***姿态估计方法
技术领域
本发明属于IMU姿态测量技术领域，尤其是一种惯导***姿态估计方法。
背景技术
基于IMU的姿态测量***，利用加速度计和陀螺仪输出对水平姿态进行最优估计。当载体无运动加速度时，加速度计可以准确的敏感当地重力矢量，与陀螺仪输出进行融合得到较高精度的水平姿态。然而，当载体存在运动加速度时，加速度计输出包括当地重力加速度和运动加速度，此时若以重力矢量为参考计算水平姿态会导致较大的误差。即载体运动加速度干扰了加速度计对于重力矢量的测量，使得水平姿态估计不准确。因此，如何有效的减弱载体运动加速度的干扰，使得动态条件下***依然具有较高的姿态测量精度，具有一定的研究意义。
发明内容
本发明的目的在于克服现有技术的不足，提出一种惯导***姿态估计方法，能够有效减弱运动加速度的干扰，实现姿态信息的最优估计，提高***动态条件下的水平姿态估计精度。
本发明解决其现实问题是采取以下技术方案实现的：
一种惯导***姿态估计方法，包括以下步骤：
步骤1、以姿态四元数为状态量，四元数姿态更新微分方程为基础建立卡尔曼滤波器状态方程；
步骤2、以加速度计测量值为输入建立四元数伪卡尔曼滤波量测方程；
步骤3、根据步骤1建立的卡尔曼滤波状态方程和步骤2建立的四元数伪卡尔曼波量测方程进行卡尔曼滤波量测更新，计算量测更新残差值，并根据加速度计量测更新的残差值对量测噪声方差进行实时估计，通过运动加速度的大小动态调节量测噪声方差阵的大小，进而保证***在动态条件下的姿态测量精度。
而且，所述步骤1的具体步骤包括：
(1)根据已知的加速度计输出为ya＝[0 ax ay az]T，陀螺仪输出为yg＝[0 ωx ωyωz]T，重力矢量为G＝[0 0 0 -g]T，建立卡尔曼滤波状态方程，已知状态方程基本形式为：
Xk+1＝ΦkXk+Wk
其中，状态量X＝[q0 q1 q2 q3 εx εy εz]T，q为载体姿态四元数，ε为陀螺常值漂移；状态转移矩阵
Figure BDA0002110950150000021
其中T为滤波周期；
Figure BDA0002110950150000022
其中，
Figure BDA0002110950150000023
Figure BDA0002110950150000024
Fεq＝03×4，Fεε＝03×3
(2)***噪声矩阵
Figure BDA0002110950150000025
其中，δg,k为陀螺仪输出噪声方差。
而且，所述步骤2的具体步骤包括：
(1)由已知量测方程基本形式为：
Zk＝HkXk+Vk
载体无运动加速度时，加速度计输出为ya和重力矢量为G具有如下关系：
Figure BDA0002110950150000031
两侧都乘以Qk，得
Figure BDA0002110950150000032
进一步变换可得到
04×1＝HkQk+Vk
其中，
Figure BDA0002110950150000033
量测量Z＝04×1；
(2)量测噪声矩阵
Figure BDA0002110950150000034
δa,k为加速度计输出噪声方差，ap,k为载体运动加速度方差。
而且，所述步骤3的具体步骤包括：
(1)卡尔曼滤波一步预测和量测更新：
卡尔曼滤波器参数初始值X0＝[1 0 0 0 0 0 0]T，P0＝10I7×7，***状态一步预测方程为：
Figure BDA0002110950150000035
Figure BDA0002110950150000036
利用加速度计输出ya＝[0 ax ay az]T进行量测更新，更新方程如下：
Figure BDA0002110950150000037
Figure BDA0002110950150000038
Figure BDA0002110950150000039
(2)量测噪声方差自适应调整:
加速度计输出量测更新残差为rk＝-HkXk+1,k，利用一定时间段内，残差值的均值对量测噪声方差阵进行动态调整：
首先对残差序列进行中值滤波，记录最近个数为N(取N＝100)的残差队列rk-N+1，rk-N+2，……，rk；
对队列进行排序后得到由小到大排列新的队列r′vk-N+1，r′vk-N+2，……，rk′；其中，r′vk-N+1≤r′vk-N+2≤…≤r′vk
剔除最大和最小的数后对剩余数据取均值，则量测更新残差值为：
Figure BDA0002110950150000041
则量测噪声方差阵Rk的无偏估计值可以表示为:
Figure BDA0002110950150000042
即滤波器根据加速度计量测更新残差
Figure BDA0002110950150000043
的大小，动态调节量测噪声方差阵Rk的大小，实现不同动态条件下滤波器参数的自适应调整。
本发明的优点和有益效果：
1、本发明通过设计基于加速度计量测更新残差的估计算法，实时调节量测方差阵的大小，在运动加速度大小未知的条件下，很好地解决了动态条件下惯导***水平姿态估计问题，有效的提高了动态条件下水平姿态估计精度，具有较高的工程应用价值。
2、本发明可用于以惯性测量单元(Inertial Measurement Unit，IMU)为核心器件的姿态测量***。当载体具有运动加速度时，滤波器可以在运动加速度大小未知的条件下，根据加速度计量测更新残差动态调节量测方差阵的数值，有效减弱运动加速度的干扰，实现姿态信息的最优估计。
3、本发明针对运动加速度大小不可预测的问题，设计了基于加速度计输出的伪量测模型，无需对加速度计输出的合加速度值进行阈值判断，根据加速度计量测更新的残差值对量测噪声方差进行实时估计，即根据运动加速度的大小动态调节量测噪声方差阵的大小，保证***在动态条件下的姿态测量精度。
附图说明
图1是本发明的处理流程图。
具体实施方式
以下结合附图对本发明实施例作进一步详述：
一种惯导***姿态估计方法，如图1所示，包括以下步骤：
步骤1、以姿态四元数为状态量，四元数姿态更新微分方程为基础建立卡尔曼滤波器状态方程；
所述步骤1的具体步骤包括：
(1)根据已知的加速度计输出为ya＝[0 ax ay az]T，陀螺仪输出为yg＝[0 ωx ωyωz]T，重力矢量为G＝[0 0 0 -g]T，建立卡尔曼滤波状态方程，已知状态方程基本形式为：
Xk+1＝ΦkXk+Wk
其中，状态量X＝[q0 q1 q2 q3 εx εy εz]T，q为载体姿态四元数，ε为陀螺常值漂移；状态转移矩阵
Figure BDA0002110950150000051
其中T为滤波周期，F为***连续方程状态矩阵，Wk为***噪声；
Figure BDA0002110950150000052
其中，
Figure BDA0002110950150000053
Figure BDA0002110950150000061
Fεq＝03×4，Fεε＝03×3
(2)***噪声矩阵
Figure BDA0002110950150000062
其中，I3×3为单位阵，δg,k为陀螺仪输出噪声方差，取决于陀螺仪自身特性，可通过测量获得。
步骤2、以加速度计测量值为输入建立四元数伪卡尔曼波量测方程；
所述步骤2的具体步骤包括：
(1)已知量测方程基本形式为：
Zk＝HkXk+Vk
载体无运动加速度时，加速度计输出为ya和重力矢量为G具有如下关系：
Figure BDA0002110950150000063
两侧都乘以Qk，得
Figure BDA0002110950150000064
进一步变换可得到
04×1＝HkQk+Vk
其中，量测矩阵
Figure BDA0002110950150000065
Qk为姿态四元数，Vk为量测噪声，g为当地重力加速度值；
量测量Z＝04×1；
(2)量测噪声矩阵
Figure BDA0002110950150000066
δa,k为加速度计输出噪声方差，ap,k为载体运动加速度方差；
步骤3、根据步骤1建立的卡尔曼滤波状态方程和步骤2建立的四元数伪卡尔曼波量测方程进行卡尔曼滤波量测更新，计算量测更新残差值，并根据加速度计量测更新的残差值对量测噪声方差进行实时估计，通过运动加速度的大小动态调节量测噪声方差阵的大小，进而保证***在动态条件下的姿态测量精度。
所述步骤3的具体步骤包括：
(1)卡尔曼滤波一步预测和量测更新：
卡尔曼滤波器参数初始值X0＝[1 0 0 0 0 0 0]T，P0＝10I7×7，***状态一步预测方程为：
Figure BDA0002110950150000071
Figure BDA0002110950150000072
利用加速度计输出ya＝[0 ax ay az]T进行量测更新，更新方程如下：
Figure BDA0002110950150000073
Figure BDA0002110950150000074
Figure BDA0002110950150000075
(2)量测噪声方差自适应调整：
加速度计输出量测更新残差为rk＝-HkXk+1,k，利用一定时间段内，残差值的均值对量测噪声方差阵进行动态调整：
首先对残差序列进行中值滤波，记录最近个数为N(取N＝100)的残差队列rk-N+1，rk-N+2，……，rk；对队列进行排序后得到由小到大排列新的队列r′vk-N+1，r′vk-N+2，……，rk′；其中，r′vk-N+1≤r′vk-N+2≤…≤r′vk。剔除最大和最小的数后对剩余数据取均值。
Figure BDA0002110950150000081
则量测噪声方差阵Rk的无偏估计值可以表示为
Figure BDA0002110950150000082
即滤波器根据加速度计量测更新残差
Figure BDA0002110950150000083
的大小，动态调节量测噪声方差阵Rk的大小，实现不同动态条件下滤波器参数的自适应调整。
需要强调的是，本发明所述实施例是说明性的，而不是限定性的，因此本发明包括并不限于具体实施方式中所述实施例，凡是由本领域技术人员根据本发明的技术方案得出的其他实施方式，同样属于本发明保护的范围。

Claims (1)

1.一种惯导***姿态估计方法，其特征在于：包括以下步骤：
步骤1、以姿态四元数为状态量，四元数姿态更新微分方程为基础建立卡尔曼滤波器状态方程；
步骤2、以加速度计测量值为输入建立四元数伪卡尔曼滤波量测方程；
步骤3、根据步骤1建立的卡尔曼滤波器状态方程和步骤2建立的四元数伪卡尔曼滤波量测方程进行卡尔曼滤波量测更新，计算量测更新残差值，并根据加速度计量测更新的残差值对量测噪声方差进行实时估计，通过运动加速度的大小动态调节量测噪声方差阵的大小，进而保证***在动态条件下的姿态测量精度；
所述步骤1的具体步骤包括：
(1)根据已知的加速度计输出为ya＝[0 ax ay az]T，陀螺仪输出为yg＝[0 ωx ωy ωz]T，重力矢量为G＝[0 0 0 -g]T，建立卡尔曼滤波器状态方程，已知状态方程基本形式为：
Xk+1＝ΦkXk+Wk
其中，状态量X＝[q0 q1 q2 q3 εx εy εz]T，q为载体姿态四元数，ε为陀螺常值漂移；状态转移矩阵
Figure FDA0003342651170000011
其中T为滤波周期，F为***连续方程状态矩阵，Wk为***噪声；
Figure FDA0003342651170000012
其中，
Figure FDA0003342651170000021
Figure FDA0003342651170000022
Fεq＝03×4，Fεε＝03×3
(2)***噪声矩阵
Figure FDA0003342651170000023
其中，I3×3为3×3的单位阵，δg,k为陀螺仪输出噪声方差；
所述步骤2的具体步骤包括：
(1)由已知量测方程基本形式为：
Zk＝HkXk+Vk
载体无运动加速度时，加速度计输出为ya和重力矢量为G具有如下关系：
Figure FDA0003342651170000024
两侧都乘以Qk，得
Figure FDA0003342651170000025
进一步变换可得到：
04×1＝HkQk+Vk
其中，量测矩阵
Figure FDA0003342651170000026
Qk为姿态四元数，Vk为量测噪声，g为当地重力加速度值；
量测量Z＝04×1；
(2)量测噪声矩阵
Figure FDA0003342651170000027
δa,k为加速度计输出噪声方差，ap,k为载体运动加速度方差；
所述步骤3的具体步骤包括：
(1)卡尔曼滤波一步预测和量测更新：
卡尔曼滤波器参数初始值X0＝[1 0 0 0 0 0 0]T，P0＝10I7×7，***状态一步预测方程为：
Figure FDA0003342651170000031
Figure FDA0003342651170000032
利用加速度计输出ya＝[0 ax ay az]T进行量测更新，更新方程如下：
Figure FDA0003342651170000033
Figure FDA0003342651170000034
Figure FDA0003342651170000035
(2)量测噪声方差自适应调整:
加速度计输出量测更新残差为rk＝-HkXk+1,k，利用一定时间段内，残差值的均值对量测噪声方差阵进行动态调整：
首先对残差序列进行中值滤波，记录最近个数为N的残差队列rk-N+1，rk-N+2，……，rk；
对队列进行排序后得到由小到大排列新的队列r′k-N+1，r′k-N+2，……，r′k；其中，取N＝100，r′k-N+1≤r′k-N+2≤…≤r′k
剔除最大和最小的数后对剩余数据取均值，则量测更新残差值为：
Figure FDA0003342651170000036
则量测噪声方差阵Rk的无偏估计值可以表示为:
Figure FDA0003342651170000037
即滤波器根据加速度计量测更新残差
Figure FDA0003342651170000041
的大小，动态调节量测噪声方差阵Rk的大小，实现不同动态条件下滤波器参数的自适应调整。
CN201910571246.7A 2019-06-28 2019-06-28 一种惯导***姿态估计方法 Active CN110440756B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910571246.7A CN110440756B (zh) 2019-06-28 2019-06-28 一种惯导***姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910571246.7A CN110440756B (zh) 2019-06-28 2019-06-28 一种惯导***姿态估计方法

Publications (2)

Publication Number Publication Date
CN110440756A CN110440756A (zh) 2019-11-12
CN110440756B true CN110440756B (zh) 2021-12-31

Family

ID=68428386

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910571246.7A Active CN110440756B (zh) 2019-06-28 2019-06-28 一种惯导***姿态估计方法

Country Status (1)

Country Link
CN (1) CN110440756B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112729348B (zh) * 2021-01-10 2023-11-28 河南理工大学 一种用于imu***的姿态自适应校正方法
CN113175926B (zh) * 2021-04-21 2022-06-21 哈尔滨工程大学 一种基于运动状态监测的自适应水平姿态测量方法
CN114264304B (zh) * 2021-12-23 2023-07-14 湖南航天机电设备与特种材料研究所 复杂动态环境高精度水平姿态测量方法与***

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (zh) * 2009-06-03 2009-10-28 哈尔滨工程大学 舰船局部捷联惯导***初始姿态快速测量方法
CN102486377A (zh) * 2009-11-17 2012-06-06 哈尔滨工程大学 一种光纤陀螺捷联惯导***初始航向的姿态获取方法
CN104154914A (zh) * 2014-07-25 2014-11-19 辽宁工程技术大学 一种空间稳定型捷联惯导***初始姿态测量方法
CN105698792A (zh) * 2016-01-26 2016-06-22 上海实汇机电科技有限公司 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量***

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US8626441B2 (en) * 2008-06-17 2014-01-07 Agco Corporation Methods and apparatus for using position/attitude information to enhance a vehicle guidance system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101566477A (zh) * 2009-06-03 2009-10-28 哈尔滨工程大学 舰船局部捷联惯导***初始姿态快速测量方法
CN102486377A (zh) * 2009-11-17 2012-06-06 哈尔滨工程大学 一种光纤陀螺捷联惯导***初始航向的姿态获取方法
CN104154914A (zh) * 2014-07-25 2014-11-19 辽宁工程技术大学 一种空间稳定型捷联惯导***初始姿态测量方法
CN105698792A (zh) * 2016-01-26 2016-06-22 上海实汇机电科技有限公司 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量***

Also Published As

Publication number Publication date
CN110440756A (zh) 2019-11-12

Similar Documents

Publication Publication Date Title
CN110440756B (zh) 一种惯导***姿态估计方法
CN109459019B (zh) 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法
CN112013836B (zh) 一种基于改进自适应卡尔曼滤波的航姿参考***算法
CN109974714B (zh) 一种Sage-Husa自适应无迹卡尔曼滤波姿态数据融合方法
CN110887481B (zh) 基于mems惯性传感器的载体动态姿态估计方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN107014376B (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN110146075B (zh) 一种增益补偿自适应滤波的sins/dvl组合定位方法
JP6083279B2 (ja) 移動状況情報算出方法及び移動状況情報算出装置
CN110887480B (zh) 基于mems传感器的飞行姿态估计方法及***
CN109945859B (zh) 一种自适应h∞滤波的运动学约束捷联惯性导航方法
CN110132271B (zh) 一种自适应卡尔曼滤波姿态估计算法
CN106403952A (zh) 一种动中通低成本组合姿态测量方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN113119980A (zh) 一种用于电动车的道路坡度估计方法、***和设备
CN112070170A (zh) 一种动态残差阈值自适应四元数粒子滤波姿态解算数据融合方法
CN114111843B (zh) 一种捷联惯导***最优动基座初始对准方法
CN110595434A (zh) 基于mems传感器的四元数融合姿态估计方法
CN114935345A (zh) 一种基于模式识别的车载惯导安装角误差补偿方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN112874529B (zh) 基于事件触发状态估计的车辆质心侧偏角估计方法及***
CN109506674B (zh) 一种加速度的校正方法及装置
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN110375773B (zh) Mems惯导***姿态初始化方法
CN110849364B (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