CN103822633B - 一种基于二阶量测更新的低成本姿态估计方法 - Google Patents
一种基于二阶量测更新的低成本姿态估计方法 Download PDFInfo
- Publication number
- CN103822633B CN103822633B CN201410047878.0A CN201410047878A CN103822633B CN 103822633 B CN103822633 B CN 103822633B CN 201410047878 A CN201410047878 A CN 201410047878A CN 103822633 B CN103822633 B CN 103822633B
- Authority
- CN
- China
- Prior art keywords
- accelerometer
- formula
- state
- attitude
- calculate
- 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.)
- Expired - Fee Related
Links
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,所述初始姿态包括横摇、纵摇和航向;
(2)在k≥1时刻,利用微机械陀螺输出的载体三轴的角速率数据,计算出姿态角的变化值Δφ,由式计算出滤波开始前的粗略姿态角;
(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据;
(4)设滤波的状态变量为x=[qe bg ba]T∈R9×1;式中:qe表示四元数误差矢量部分,bg表示陀螺漂移,ba表示加速度计零偏;假设四元数误差 根据四元数相关性质建立状态方程:
式中: 下标g代表陀螺、a代表加速度计、m代表磁力计,yg表示陀螺实际输出。传感器噪声va、vg以及vm为零均值高斯白噪声;
取观测量为 由于qe为小量,再忽略二阶小项,建立量测方程:
式中:表示由估计四元数组成的姿态矩阵,ya和ym分别为加速度计和磁强计输出,定义 和 g为重力加速度,α为磁倾角;
(5)利用步骤(4)中建立的状态方程进行滤波的时间更新,由下式
分别计算出一步预测量和状态误差协方差阵
式中:Φk为系统状态转移矩阵,Qk为系统噪声协方差阵;
(6)在滤波的量测更新阶段采用二阶量测更新,先进行加速度计量测更新;取此阶段观测量根据建立的量测方程有其更新过程如下:
先计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:
式中:下标a表示加速度计量测更新阶段,Ra为加速度计噪声协方差阵
此时的9个状态都会更新,利用更新用代表已更新状态,取即前三个变量值,利用式进行校正,然后进行四元数规范化:
加速度计更新结束后,将的qe部分设置为零;
(7)利用步骤(6)中得到的进行磁力计量测更新;取此阶段观测量为 则得 其更新过程如下:
取此时状态误差协方差阵为
由此计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵: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)中的粗略姿态角,得到精确的姿态角。
此外,在所述步骤(7)磁力计量测更新中修正了标准量测更新算法,从而使磁强计量测更新只影响方位角,其原理如下:
磁力计量测更新前的转换矩阵为更新后的转换矩阵为由矩阵定理可知,当满足下面条件,qe只影响的航向角:
根据四元数和姿态矩阵的关系,可得
由上述条件可推出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);
(2)当k≥1时刻,利用微机械陀螺输出载体三轴角速率数据yg,计算出姿态角的变化值Δφ,由式计算出滤波开始前的粗略姿态角得到
(3)采集微机械加速度计和磁力计输出的载体三轴的线加速度和磁力数据;
(4)设滤波的状态变量为x=[qe bg ba]T∈R9×1。式中:qe表示四元数误差矢量部分,bg表示陀螺漂移,ba表示加速度计零偏。本方法中假设四元数误差 根据四元数相关性质建立状态方程:
式中: 下标g代表陀螺、a代表加速度计、m代表磁力计,yg表示陀螺实际输出。传感器噪声va、vg以及vm为零均值高斯白噪声。
取观测量为 由于qe为小量,再忽略二阶小项,建立量测方程:
式中:表示由估计四元数组成的姿态矩阵,ya和ym分别为加速度计和磁强计输出,定义 和 g为重力加速度,α为磁倾角;
(5)利用步骤(4)中建立的状态方程进行滤波的时间更新,由下式
分别计算出一步预测量和状态误差协方差阵
式中:Φk为系统状态转移矩阵,Qk为系统噪声协方差阵;
(6)结合图2,在滤波的量测更新阶段采用二阶量测更新,先进行加速度计量测更新。该步骤中,取此阶段观测量根据建立的量测方程有其更新过程如下:
先计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:
式中:下标a表示加速度计量测更新阶段,Ra为加速度计噪声协方差阵
此时的9个状态都会更新,利用更新用代表已更新状态,取即前三个变量值,利用式进行校正,然后进行四元数规范化:
加速度计更新结束后,将的qe部分设置为零;
(7)利用步骤(6)中得到的进行磁力计量测更新。本方法修正了标准量测更新算法从而使磁力计量测更新只影响方位角。由此,计算出四元数的误差量。该步骤中,取此阶段观测量为 则可得 其更新过程如下:
取此时状态误差协方差阵为
由此计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵: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,所述初始姿态包括横摇、纵摇和航向;
(2)在k≥1时刻,利用微机械陀螺输出的载体三轴的角速率数据,计算出姿态角的变化值Δφ,由式计算出滤波开始前的粗略姿态角;
(3)采集微机械加速度计和磁力计输出的载体三轴线的加速度和磁力数据;
(4)设滤波的状态变量为x=[qe bg ba]T∈R9×1;式中:qe表示四元数误差矢量部分,bg表示陀螺漂移,ba表示加速度计零偏;假设四元数误差根据四元数相关性质建立状态方程:
式中:下标g代表陀螺、a代表加速度计、m代表磁力计,yg表示陀螺实际输出,传感器噪声va、vg以及vm为零均值高斯白噪声;
取观测量为由于qe为小量,再忽略二阶小项,建立量测方程:
式中:表示由估计四元数组成的姿态矩阵,ya和ym分别为加速度计和磁强计输出,定义和g为重力加速度,α为磁倾角;
(5)利用步骤(4)中建立的状态方程进行滤波的时间更新,由下式
分别计算出一步预测量和状态误差协方差阵
式中:Φk为系统状态转移矩阵,Qk为系统噪声协方差阵;
(6)在滤波的量测更新阶段采用二阶量测更新,先进行加速度计量测更新;取此阶段观测量根据建立的量测方程有其更新过程如下:
先计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:式中:下标a表示加速度计量测更新阶段,Ra为加速度计噪声协方差阵
此时的9个状态都会更新,利用更新用代表已更新状态,取即前三个变量值,利用式进行校正,然后进行四元数规范化:
加速度计更新结束后,将的qe部分设置为零;
(7)利用步骤(6)中得到的进行磁力计量测更新;取此阶段观测量为则得其更新过程如下:
取此时状态误差协方差阵为
由此计算滤波增益:
然后计算状态估计量:
再计算状态误差协方差阵:
式中:下标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 CN103822633A (zh) | 2014-05-28 |
CN103822633B true 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) |
Families Citing this family (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105203098B (zh) * | 2015-10-13 | 2018-10-02 | 上海华测导航技术股份有限公司 | 基于九轴mems传感器的农业机械全姿态角更新方法 |
CN106931965B (zh) * | 2015-12-31 | 2021-04-13 | 中国移动通信集团吉林有限公司 | 一种确定终端姿态的方法及装置 |
CN105509740A (zh) * | 2015-12-31 | 2016-04-20 | 广州中海达卫星导航技术股份有限公司 | 一种农业机械车辆姿态测量方法及测量模块 |
CN105698792A (zh) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | 一种基于自适应鲁邦融合算法的动态mems惯性姿态测量系统 |
CN105606096B (zh) * | 2016-01-28 | 2018-03-30 | 北京航空航天大学 | 一种载体运动状态信息辅助的姿态和航向计算方法和系统 |
CN105716610B (zh) * | 2016-01-28 | 2018-09-04 | 北京航空航天大学 | 一种地磁场模型辅助的载体姿态和航向计算方法和系统 |
CN105865452B (zh) * | 2016-04-29 | 2018-10-02 | 浙江国自机器人技术有限公司 | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 |
CN106176149A (zh) * | 2016-09-08 | 2016-12-07 | 电子科技大学 | 一种基于多传感融合的外骨骼步态分析系统及方法 |
CN106885568B (zh) * | 2017-02-21 | 2020-09-01 | 北京京东尚科信息技术有限公司 | 无人机数据处理方法和装置 |
CN108196701B (zh) * | 2018-01-03 | 2020-06-02 | 海信视像科技股份有限公司 | 确定姿态的方法、装置及vr设备 |
CN108871319B (zh) * | 2018-04-26 | 2022-05-17 | 李志� | 一种基于地球重力场与地磁场序贯修正的姿态解算方法 |
CN109631895B (zh) * | 2019-01-04 | 2023-03-31 | 京东方科技集团股份有限公司 | 一种物体的位姿估计方法和装置 |
CN110048693B (zh) * | 2019-04-10 | 2021-10-12 | 南京航空航天大学 | 基于四元数分布的并行高斯粒子滤波数据处理方法 |
CN110095118A (zh) * | 2019-06-03 | 2019-08-06 | 北京理工大学 | 一种车身姿态角的实时测量方法及系统 |
CN110763224A (zh) * | 2019-11-13 | 2020-02-07 | 内蒙古工业大学 | 一种自动导引运输车导航方法及导航系统 |
CN112254723B (zh) * | 2020-10-13 | 2022-08-16 | 天津津航计算技术研究所 | 基于自适应ekf算法的小型无人机marg航姿估计方法 |
CN114877858B (zh) * | 2022-05-06 | 2023-04-14 | 西安电子科技大学 | 一种高动态和磁干扰环境下的姿态估计算法 |
CN116182871B (zh) * | 2023-04-26 | 2023-07-07 | 河海大学 | 一种基于二阶混合滤波的海缆探测机器人姿态估计方法 |
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算法在姿态估计中的作用》;赵琳等;《中国惯性技术学报》;20091031;第17卷(第5期);557-561 * |
Also Published As
Publication number | Publication date |
---|---|
CN103822633A (zh) | 2014-05-28 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103822633B (zh) | 一种基于二阶量测更新的低成本姿态估计方法 | |
CN110398257B (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
CN107525503B (zh) | 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法 | |
Wu et al. | Velocity/position integration formula part I: Application to in-flight coarse alignment | |
CN101726295B (zh) | 考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法 | |
CN103630137B (zh) | 一种用于导航系统的姿态及航向角的校正方法 | |
CN104736963B (zh) | 测绘系统和方法 | |
CN103389506B (zh) | 一种用于捷联惯性/北斗卫星组合导航系统的自适应滤波方法 | |
CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
CN105606094B (zh) | 一种基于mems/gps组合系统的信息条件匹配滤波估计方法 | |
CN109459019A (zh) | 一种基于级联自适应鲁棒联邦滤波的车载导航计算方法 | |
CN104698485B (zh) | 基于bd、gps及mems的组合导航系统及导航方法 | |
CN105698822B (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
CN109163735B (zh) | 一种晃动基座正向-正向回溯初始对准方法 | |
CN102519485B (zh) | 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法 | |
CN103900613B (zh) | 一种基于磁力计n阶距检测的mems系统误差估计方法 | |
CN106153069B (zh) | 自主导航系统中的姿态修正装置和方法 | |
CN106017452B (zh) | 双陀螺抗扰动寻北方法 | |
CN111399023B (zh) | 基于李群非线性状态误差的惯性基组合导航滤波方法 | |
CN102168978B (zh) | 一种船用惯性导航系统摇摆基座开环对准方法 | |
CN103697878B (zh) | 一种单陀螺单加速度计旋转调制寻北方法 | |
CN102680000A (zh) | 应用零速/航向修正的光纤捷联惯组在线标定方法 | |
CN109764870B (zh) | 基于变换估计量建模方案的载体初始航向估算方法 | |
CN109945859A (zh) | 一种自适应h∞滤波的运动学约束捷联惯性导航方法 | |
CN105157724A (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 |