CN103822633A - 一种基于二阶量测更新的低成本姿态估计方法 - Google Patents
一种基于二阶量测更新的低成本姿态估计方法 Download PDFInfo
- Publication number
- CN103822633A CN103822633A CN201410047878.0A CN201410047878A CN103822633A CN 103822633 A CN103822633 A CN 103822633A CN 201410047878 A CN201410047878 A CN 201410047878A CN 103822633 A CN103822633 A CN 103822633A
- Authority
- CN
- China
- Prior art keywords
- magnetometer
- attitude
- prime
- accelerometer
- formula
- 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
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/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
本发明提供的是一种基于二阶量测更新的低成本姿态估计方法。本发明通过对三轴微机械陀螺、三轴微机械加速度计和三轴磁强计的输出数据,进行滤波处理得到载体的姿态信息。针对在室内或磁干扰较强场所,磁强计输出会使横摇和纵摇误差变大,传统方法难以解决的问题。本方法在滤波的量测更新阶段,创新性地采用二阶量测更新,即先进行加速度计量测更新再进行磁强计量测更新。以此修正标准量测更新算法从而使磁强计更新只影响方位角。利用本方法可以使用低成本的微惯性测量单元和磁强计进行姿态估计,并且估计精度高、实时性好、适应强磁干扰环境。本方法适用于车辆、无人机和船舰等载体的姿态估计。
Description
技术领域
本发明涉及的是一种适用于利用低成本的传感器,对车辆、船舶、无人机载体等进行姿态估计地方法。具体地说是一种基于二阶量测更新的低成本姿态估计方法。
背景技术
在很多工程应用领域中都需要进行姿态估计,例如,机动追踪、无人机和生物应用等方面。如果给定载体初始状态,则可以通过积分陀螺仪计算出估计姿态值。它的估计误差主要由陀螺漂移和算法误差组成。此外,也可以通过加速度计和磁力计的输出计算出姿态值。在这种情况下,由于外部加速度和磁干扰,估计误差可能会变大。因此每种姿态估计算法的基本问题就是如何将陀螺仪、加速度计和磁力计结合起来使用。因此,用微惯性传感器和磁传感器来估计姿态是近年来的流行方法之一。
目前,大多数的姿态估计中观测量都会选取地球磁场,利用磁强计或罗经等设备输出磁力信息。其中磁强计的输出不仅能提供航向信息同时也包括纵摇和横摇的一些信息。但它有一个严重的缺点就是纵摇角和横摇角受磁干扰影响,由于室内存在较强的磁干扰,因此在室内应用该种算法时纵摇误差和横摇误差都有较大的变化。所以较为明智的选择就是利用磁强计输出数据只计算航向角,因此这不适用于传统的四元数间接卡尔曼滤波。
发明内容
本发明的目的在于提供一种通过测量微惯性测量单元和磁强计输出的载体三个轴的角速率、线加速度和磁力数据,利用明改进的四元数卡尔曼滤波处理,得到载体的精确姿态信息的基于二阶量测更新的低成本姿态估计方法。
本发明的目的是这样实现的:
(1)在k=0时刻,即初始时刻由初始对准得到初始姿态φ0,所述初始姿态包括横摇、纵摇和航向;
(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据;
式中: 下标g代表陀螺、a代表加速度计、m代表磁力计,yg表示陀螺实际输出。传感器噪声va、vg以及vm为零均值高斯白噪声;
取观测量为 由于qe为小量,再忽略二阶小项,建立量测方程:
式中:表示由估计四元数组成的姿态矩阵,ya和ym分别为加速度计和磁强计输出,定义 和 g为重力加速度,α为磁倾角;
(5)利用步骤(4)中建立的状态方程进行滤波的时间更新,由下式
式中:Φk为系统状态转移矩阵,Qk为系统噪声协方差阵;
先计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:
式中:下标a表示加速度计量测更新阶段,Ra为加速度计噪声协方差阵
取此时状态误差协方差阵为
由此计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:Pk=(I-Km,kHm,k)Pa,k(I-Km,kHm,k)′+Km,kRmK′m,k;
(8)根据滤波计算出的四元数误差,校正步骤(2)中的粗略姿态角,得到精确的姿态角。
此外,在所述步骤(7)磁力计量测更新中修正了标准量测更新算法,从而使磁强计量测更新只影响方位角,其原理如下:
根据四元数和姿态矩阵的关系,可得
由上述条件可推出qe×r3=0,即当qe与r3平行时,qe只影响航向角。为了使磁力计更新满足qe=βr3,β∈R,需要对Km,k(1:3,1:3)进行约束。由于在加速度计更新结束时将因此磁力计量测更新方程为
为使任意的zm,k都满足qe=βr3,β∈R,Km,k(1:3,1:3)需要具有以下结构Km,k(1:3,1:3)=r3l′,当l使Tr Pk(1:3,1:3)取最小值时,l=r3。因此满足约束的最优滤波增益为
本发明中修正了标准的四元数卡尔曼滤波,使得磁强计输出仅用于航向估计误差补偿。这样做的好处是,使得了这种姿态估计算法可以很好的应用在室内或磁干扰较强的场所。
此外,为了配合这种姿态估计算法,改变了传统的量测更新方式。在传统的滤波方法中不同的观测量同步进行量测更新。但为了适用于本发明的方法采用二阶量测更新,即先进行加速度计量测更新,将得到的状态变量中四元数误差量设为零,然后再进行磁强计量测更新。这样做的目的是使得磁强计的输出仅用于航向估计误差补偿。
本发明与现有的技术相比的优点在于:改变了传统的量测更新方式,解决了在室内或磁干扰较强的场所磁强计输出易受干扰,导致纵摇和横摇误差较大的问题。通过本发明的二阶量测更新方法,磁强计输出只用于航向估计误差补偿,因此避免了磁干扰的影响,提高了滤波在此情况下的精度,实现了对载体姿态信息的准确获取。
对本发明的有益效果说明如下:
采用本发明的基于二阶量测更新的低成本姿态估计方法,假设三个姿态角的初始误差角分别为:0.5°、0.5°、10°,为了模拟实际捷联惯导系统三个姿态角的变化,假设惯导系统在静基座上以基座原点为中心做如下摇摆:
ψ=10°+6°sin(2πt/4+π/4)
θ=8°sin(2πt/5+π/5)
η=10°sin(2πt/6+π/6)
为了证明本方法不受磁干扰影响,将磁强计的输出噪声设为较大值。仿真实验结果:图3为横摇失准角误差曲线,图4为纵摇失准角误差曲线,图5为航向失准角误差曲线。从图中可以看出采用本方法可以在强磁干扰的环境下,依然可以获得很高的姿态估计精度。
利用本发明方法可以使用低成本的微惯性测量单元和磁强计进行姿态估计,并且估计精度高、实时性好、适应强磁干扰环境。本方法适用于车辆、无人机和船舰等载体的姿态估计。
附图说明
图1为本发明流程图。
图2为本发明滤波算法流程图。
图3为具体实施方式的横摇失准角误差实验曲线。
图4为具体实施方式的纵摇失准角误差实验曲线。
图5为具体实施方式的航向失准角误差实验曲线。
具体实施方式
(1)结合图1,在初始时刻根据初始对准的结果得到初始姿态φ0,包括横摇、纵摇和航向,然后计算出初始姿态矩阵C(q0);
(3)采集微机械加速度计和磁力计输出的载体三轴的线加速度和磁力数据;
式中: 下标g代表陀螺、a代表加速度计、m代表磁力计,yg表示陀螺实际输出。传感器噪声va、vg以及vm为零均值高斯白噪声。
取观测量为 由于qe为小量,再忽略二阶小项,建立量测方程:
(5)利用步骤(4)中建立的状态方程进行滤波的时间更新,由下式
式中:Φk为系统状态转移矩阵,Qk为系统噪声协方差阵;
先计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:
式中:下标a表示加速度计量测更新阶段,Ra为加速度计噪声协方差阵
取此时状态误差协方差阵为
由此计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:Pk=(I-Km,kHm,k)Pa,k(I-Km,kHm,k)′+Km,kRmK′m,k;
式中:下标m表示磁力计量测更新阶段,Pa,k(1:3,1:3)表示由Pa,k前三行前三列所组成的矩阵,0mn为m行n列的零矩阵并且
(8)根据滤波计算出的四元数误差,校正步骤(2)中的粗略姿态角,得到精确的姿态角,重复步骤(5)~(7)直至结束。
Claims (1)
1.一种基于二阶量测更新的低成本姿态估计方法,其特征是:
(1)在k=0时刻,即初始时刻由初始对准得到初始姿态φ0,所述初始姿态包括横摇、纵摇和航向;
(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据;
式中: 下标g代表陀螺、a代表加速度计、m代表磁力计,yg表示陀螺实际输出。传感器噪声va、vg以及vm为零均值高斯白噪声;
取观测量为 由于qe为小量,再忽略二阶小项,建立量测方程:
(5)利用步骤(4)中建立的状态方程进行滤波的时间更新,由下式
式中:Φk为系统状态转移矩阵,Qk为系统噪声协方差阵;
先计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:
式中:下标a表示加速度计量测更新阶段,Ra为加速度计噪声协方差阵
取此时状态误差协方差阵为
由此计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:Pk=(I-Km,kHm,k)Pa,k(I-Km,kHm,k)′+Km,kRmK′m,k;
式中:下标m表示磁力计量测更新阶段,Pa,k(1:3,1:3)表示由Pa,k前三行前三列所组成的矩阵,0mn为m行n列的零矩阵并且
(8)根据滤波计算出的四元数误差,校正步骤(2)中的粗略姿态角,得到精确的姿态角。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410047878.0A CN103822633B (zh) | 2014-02-11 | 2014-02-11 | 一种基于二阶量测更新的低成本姿态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410047878.0A CN103822633B (zh) | 2014-02-11 | 2014-02-11 | 一种基于二阶量测更新的低成本姿态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103822633A true CN103822633A (zh) | 2014-05-28 |
CN103822633B CN103822633B (zh) | 2016-12-07 |
Family
ID=50757808
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410047878.0A Expired - Fee Related CN103822633B (zh) | 2014-02-11 | 2014-02-11 | 一种基于二阶量测更新的低成本姿态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103822633B (zh) |
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105203098A (zh) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN105509740A (zh) * | 2015-12-31 | 2016-04-20 | 广州中海达卫星导航技术股份有限公司 | 一种农业机械车辆姿态测量方法及测量模块 |
CN105606096A (zh) * | 2016-01-28 | 2016-05-25 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN105698792A (zh) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统 |
CN105716610A (zh) * | 2016-01-28 | 2016-06-29 | 北京航空航天大学 | 一种地磁场模型辅助的载体姿态和航向计算方法和系统 |
CN105865452A (zh) * | 2016-04-29 | 2016-08-17 | 浙江国自机器人技术有限公司 | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 |
CN106176149A (zh) * | 2016-09-08 | 2016-12-07 | 电子科技大学 | 一种基于多传感融合的外骨骼步态分析系统及方法 |
CN106885568A (zh) * | 2017-02-21 | 2017-06-23 | 北京京东尚科信息技术有限公司 | 无人机数据处理方法和装置 |
CN106931965A (zh) * | 2015-12-31 | 2017-07-07 | 中国移动通信集团吉林有限公司 | 一种确定终端姿态的方法及装置 |
CN108871319A (zh) * | 2018-04-26 | 2018-11-23 | 李志� | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 |
CN109631895A (zh) * | 2019-01-04 | 2019-04-16 | 京东方科技集团股份有限公司 | 一种物体的位姿估计方法和装置 |
WO2019134305A1 (zh) * | 2018-01-03 | 2019-07-11 | 青岛海信电器股份有限公司 | 确定姿态的方法、装置、智能设备、存储介质和程序产品 |
CN110048693A (zh) * | 2019-04-10 | 2019-07-23 | 南京航空航天大学 | 基于四元数分布的并行高斯粒子滤波数据处理方法 |
CN110095118A (zh) * | 2019-06-03 | 2019-08-06 | 北京理工大学 | 一种车身姿态角的实时测量方法及系统 |
CN110763224A (zh) * | 2019-11-13 | 2020-02-07 | 内蒙古工业大学 | 一种自动导引运输车导航方法及导航系统 |
CN112254723A (zh) * | 2020-10-13 | 2021-01-22 | 天津津航计算技术研究所 | 基于自适应ekf算法的小型无人机marg航姿估计方法 |
CN114877858A (zh) * | 2022-05-06 | 2022-08-09 | 西安电子科技大学 | 一种高动态和磁干扰环境下的姿态估计算法 |
CN116182871A (zh) * | 2023-04-26 | 2023-05-30 | 河海大学 | 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 |
Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1908584A (zh) * | 2006-08-23 | 2007-02-07 | 北京航空航天大学 | 一种捷联惯性导航系统初始姿态确定方法 |
CN101726295A (zh) * | 2008-10-24 | 2010-06-09 | 中国科学院自动化研究所 | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 |
CN103090870A (zh) * | 2013-01-21 | 2013-05-08 | 西北工业大学 | 一种基于mems传感器的航天器姿态测量方法 |
-
2014
- 2014-02-11 CN CN201410047878.0A patent/CN103822633B/zh not_active Expired - Fee Related
Patent Citations (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN1908584A (zh) * | 2006-08-23 | 2007-02-07 | 北京航空航天大学 | 一种捷联惯性导航系统初始姿态确定方法 |
CN101726295A (zh) * | 2008-10-24 | 2010-06-09 | 中国科学院自动化研究所 | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 |
CN103090870A (zh) * | 2013-01-21 | 2013-05-08 | 西北工业大学 | 一种基于mems传感器的航天器姿态测量方法 |
Non-Patent Citations (1)
Title |
---|
赵琳等: "《基于修正的Rodrigues参数的EKF算法在姿态估计中的作用》", 《中国惯性技术学报》 * |
Cited By (24)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105203098A (zh) * | 2015-10-13 | 2015-12-30 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN106931965A (zh) * | 2015-12-31 | 2017-07-07 | 中国移动通信集团吉林有限公司 | 一种确定终端姿态的方法及装置 |
CN105509740A (zh) * | 2015-12-31 | 2016-04-20 | 广州中海达卫星导航技术股份有限公司 | 一种农业机械车辆姿态测量方法及测量模块 |
CN106931965B (zh) * | 2015-12-31 | 2021-04-13 | 中国移动通信集团吉林有限公司 | 一种确定终端姿态的方法及装置 |
CN105698792A (zh) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统 |
CN105606096B (zh) * | 2016-01-28 | 2018-03-30 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN105716610A (zh) * | 2016-01-28 | 2016-06-29 | 北京航空航天大学 | 一种地磁场模型辅助的载体姿态和航向计算方法和系统 |
CN105606096A (zh) * | 2016-01-28 | 2016-05-25 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN105865452A (zh) * | 2016-04-29 | 2016-08-17 | 浙江国自机器人技术有限公司 | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 |
CN105865452B (zh) * | 2016-04-29 | 2018-10-02 | 浙江国自机器人技术有限公司 | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 |
CN106176149A (zh) * | 2016-09-08 | 2016-12-07 | 电子科技大学 | 一种基于多传感融合的外骨骼步态分析系统及方法 |
CN106885568A (zh) * | 2017-02-21 | 2017-06-23 | 北京京东尚科信息技术有限公司 | 无人机数据处理方法和装置 |
CN106885568B (zh) * | 2017-02-21 | 2020-09-01 | 北京京东尚科信息技术有限公司 | 无人机数据处理方法和装置 |
WO2019134305A1 (zh) * | 2018-01-03 | 2019-07-11 | 青岛海信电器股份有限公司 | 确定姿态的方法、装置、智能设备、存储介质和程序产品 |
CN108871319A (zh) * | 2018-04-26 | 2018-11-23 | 李志� | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 |
CN109631895A (zh) * | 2019-01-04 | 2019-04-16 | 京东方科技集团股份有限公司 | 一种物体的位姿估计方法和装置 |
CN110048693A (zh) * | 2019-04-10 | 2019-07-23 | 南京航空航天大学 | 基于四元数分布的并行高斯粒子滤波数据处理方法 |
CN110048693B (zh) * | 2019-04-10 | 2021-10-12 | 南京航空航天大学 | 基于四元数分布的并行高斯粒子滤波数据处理方法 |
CN110095118A (zh) * | 2019-06-03 | 2019-08-06 | 北京理工大学 | 一种车身姿态角的实时测量方法及系统 |
CN110763224A (zh) * | 2019-11-13 | 2020-02-07 | 内蒙古工业大学 | 一种自动导引运输车导航方法及导航系统 |
CN112254723A (zh) * | 2020-10-13 | 2021-01-22 | 天津津航计算技术研究所 | 基于自适应ekf算法的小型无人机marg航姿估计方法 |
CN114877858A (zh) * | 2022-05-06 | 2022-08-09 | 西安电子科技大学 | 一种高动态和磁干扰环境下的姿态估计算法 |
CN114877858B (zh) * | 2022-05-06 | 2023-04-14 | 西安电子科技大学 | 一种高动态和磁干扰环境下的姿态估计算法 |
CN116182871A (zh) * | 2023-04-26 | 2023-05-30 | 河海大学 | 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 |
Also Published As
Publication number | Publication date |
---|---|
CN103822633B (zh) | 2016-12-07 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103822633A (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
CN110398257B (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
CN102486377B (zh) | 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法 | |
CN103344260B (zh) | 基于rbckf的捷联惯导系统大方位失准角初始对准方法 | |
CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
CN103575299A (zh) | 利用外观测信息的双轴旋转惯导系统对准及误差修正方法 | |
CN102519485B (zh) | 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法 | |
CN109945895B (zh) | 基于渐消平滑变结构滤波的惯性导航初始对准方法 | |
CN103217699B (zh) | 一种基于偏振信息的组合导航系统递推优化初始对准方法 | |
CN104132662A (zh) | 基于零速修正的闭环卡尔曼滤波惯性定位方法 | |
CN103557864A (zh) | Mems捷联惯导自适应sckf滤波的初始对准方法 | |
Zhao | GPS/IMU integrated system for land vehicle navigation based on MEMS | |
CN103364817B (zh) | 一种基于r-t-s平滑的pos系统双捷联解算后处理方法 | |
CN104374388A (zh) | 一种基于偏振光传感器的航姿测定方法 | |
CN105371844A (zh) | 一种基于惯性/天文互助的惯性导航系统初始化方法 | |
CN103674064B (zh) | 捷联惯性导航系统的初始标定方法 | |
CN106403952A (zh) | 一种动中通低成本组合姿态测量方法 | |
CN105091907A (zh) | Sins/dvl组合中dvl方位安装误差估计方法 | |
CN103900608A (zh) | 一种基于四元数ckf的低精度惯导初始对准方法 | |
CN104062672A (zh) | 基于强跟踪自适应Kalman滤波的SINSGPS组合导航方法 | |
CN114777812B (zh) | 一种水下组合导航系统行进间对准与姿态估计方法 | |
CN105157724A (zh) | 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 | |
CN109764870A (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 | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20161207 Termination date: 20220211 |