互补滤波和卡尔曼滤波的融合姿态解算方法
姿态解算方法(1)_一种互补滤波方法

姿态解算⽅法(1)_⼀种互补滤波⽅法姿态解算⽅法(1)_ ⼀种互补滤波⽅法本⽂内容主要是参考《An efficient orientation filter for inertial and inertial/magnetic sensor arrays 》此篇论⽂,由于其在普通飞控中的⼴泛运⽤,所以这⾥简单记录学习感受以期后续详细思索。
当然也是因为这篇⽐较易懂,正好可以锻炼⼀下我捉襟见肘的表述能⼒。
写到这⾥可能已经开始被骂了。
⼏个较好的博⽂链接如下:(1)(2)这是在博客园的第⼀篇博⽂,想来⾃⼰⼀直攒在电脑⾥的东西,不仅容易被弄没了,也没办法被⼤家纠错批评,故也就不在乎好坏把想到的都写下来。
⾔归正传,⾸先,需要了解下四元数的知识,这在维基(3)和(1)中都有很好的解释了,这⾥为了争取篇幅讲述算法核⼼部分就不再赘述。
(3)⾓加速度的事那么进⼊关于姿态确定的部分。
三轴陀螺仪模块可以测量传感器所在平⾯的⾓加速度,⼀般来说MPU6050模块返回的是三个⾓速度值。
如果初始姿态是已知的,时间的计量⼜是准确的,那么就可以积分出姿态了。
可惜的是⾓速度的测量准确度实在是不能积分出⼀个准确的位置值。
不过这⾥还是说⼀下这个积分过程。
sω=0ωxωyωzS E ˙q=12SEˆq⊗Sωsω为传感器获得的⾓速度值,再次提醒这⾥的值可能是不准确的。
SEˆq是表⽰传感器相对于地⾯坐标系的SO(3)变换四元数的估计,这⾥⽤于表⽰传感器姿态的估计。
⊗是四元数的⼀种运算⽅式,在上⽂的参考⽂献中可以找得到的。
差分的形式可以表⽰为:δS E qω,t=12SEˆqt−1⊗SωtSE qω,t=S E ˆqt−1+δSE qω,tΔtΔt是两次预测之间的时间差,准确的说是传感器上⼀次数据和这⼀次数据获取时间差。
其他的部分和连续的形式对应就不多说了。
基于场⽅向观测的姿态求解⾸先把姿态观测问题看做是⼀个求尽可能满⾜观测结果的姿态的优化问题。
基于卡尔曼滤波和互补滤波的AHRS系统研究

本栏目责任编辑:梁书计算机工程应用技术基于卡尔曼滤波和互补滤波的AHRS 系统研究蔡阳,胡杰❋(长江大学计算机科学学院,湖北荆州434023)摘要:AHRS 航姿参考系统中通常需要融合MEMS 传感器数据来进行姿态解算,由于MEMS 传感器自身的一些缺陷导致在姿态解算中会出现较为严重的误差。
AHRS 中常见对加速度计、陀螺仪和磁力计进行卡尔曼滤波、互补滤波的方法,由于使用单一的滤波算法时会出现误差,导致姿态角解算精度不高。
本文采用卡尔曼滤波融合互补滤波的滤波算法,通过卡尔曼滤波对加速度计和陀螺仪起抑制漂移作用,进而得到最优估计姿态角,减小传感器引起的误差,再由估计值和磁力计经过互补滤波滤除噪声,提高姿态角的解算精度。
仿真实验表明:融合滤波算法可以抑制漂移和滤除噪声,在静态和动态条件下,都有良好表现。
关键词:AHRS;MEMS ;姿态解算;卡尔曼滤波;互补滤波中国分类号:TP301文献标识码:A 文章编号:1009-3044(2021)10-0230-03开放科学(资源服务)标识码(OSID ):Research on AHRS System Based on Kalman Filter and Complementary Filter CAI Yang,HU Jie(School of Computer Science,Yangtze University,Jingzhou 434023,China)Abstract:AHRS heading and attitude reference system usually needs to fuse MEMS sensor data for attitude calculation.Due to some defects of MEMS sensor itself,there will be more serious errors in attitude calculation.Kalman filtering and complementary filtering methods for accelerometers,gyroscopes,and magnetometers are common in AHRS.Due to errors when a single filtering al⁃gorithm is used,the accuracy of the attitude angle calculation is not high.In this paper,the Kalman filter fusion complementary fil⁃ter filter algorithm is used to suppress drift of the accelerometer and gyroscope through Kalman filter,and then obtain the optimal estimated attitude angle,reduce the error caused by the sensor,and then pass the estimated value and the ple⁃mentary filtering filters out noise and improves the accuracy of attitude angle calculation.Simulation experiments show that the fu⁃sion filtering algorithm can suppress drift and filter noise,and it performs well under static and dynamic conditions.Keywords:AHRS;MEMS;attitude calculation;Kalman filter;complementary filter航姿参考系统AHRS(Attitude and Heading Reference Sys⁃tem)由MEMS(Micro-Electro Mechanical System)惯性传感器三轴陀螺仪、三轴加速度计和磁力计的数据融合来进行姿态解算[1]。
基于互补滤波的飞行器姿态解算

姿态解算一、主线姿态表示方式:矩阵表示,轴角表示,欧拉角表示,四元数表示。
惯性测量单元IMU(Inertial Measurement Unit):MPU6050芯片,包含陀螺仪和加速度计,分别测量三轴加速度和三轴角速度。
注意,传感器所测数据是原始数据,包含了噪声,无法直接用于飞行器的姿态解算,因此需要对数据进行滤波。
滤波算法:非线性互补滤波算法,卡尔曼滤波算法,Mahony互补滤波算法。
二、知识点补充加速度计和陀螺仪加速度计:加速度计,可以测量加速度,包括外力加速度和重力加速度,因此,当被测物体在静止或匀速运动(匀速直线运动)的时候,加速度计仅仅测量的是重力加速度,而重力加速度与R坐标系(绝对坐标系)是固连的,通过这种关系,可以得到加速度计所在平面与地面的角度关系也就是横滚角和俯仰角。
把加速度传感器水平静止放在桌子上,它的Z轴输出的是1g的加速度。
因为它Z轴方向被重力向下拉出了一个形变。
可惜的是,加速度传感器不会区分重力加速度与外力加速度。
所以,当系统在三维空间做变速运动时,它的输出就不正确了,或者说它的输出不能表明物体的姿态和运动状态。
陀螺仪:陀螺仪测量角速度。
陀螺仪模型如图1所示,陀螺仪的每个通道检测一个轴的旋转。
图1[引自网络]上图中,Rxz是R在XZ面上的投影,与Z轴的夹角为Axz。
Ryz是R在ZY面上的投影,与Z轴的夹角为Ayz。
陀螺仪就是测量上面定义角度的变化率,换句话说,它会输出一个与上面这些角度变化率线性相关的值。
加速度计工作原理介绍(摘自网络)大多数加速度计可归为两类:数字和模拟。
数字加速度计可通过I2C,SPI或USART方式获取信息,而模拟加速度计的输出是一个在预定范围内的电压值,你需要用ADC(模拟量转数字量)模块将其转换为数字值。
不管使用什么类型的ADC模块,都会得到一个在一定范围内的数值。
例如一个10位ADC模块的输出值范围在0-1023间。
假设我们从10位ADC模块得到了以下的三个轴的数据:===586,630,561AdcRx AdcRy AdcRz每个ADC 模块都有一个参考电压,假设在我们的例子中,它是3.3V 。
一种基于改进卡尔曼滤波的姿态解算算法

一种基于改进卡尔曼滤波的姿态解算算法王晓初; 李宾; 刘玉县; 郭帅良; 范耀华【期刊名称】《《科学技术与工程》》【年(卷),期】2019(019)024【总页数】7页(P416-422)【关键词】高阶互补; 卡尔曼; 融合算法; 姿态解算【作者】王晓初; 李宾; 刘玉县; 郭帅良; 范耀华【作者单位】广东工业大学广东省计算机集成制造重点实验室广州510006; 广东顺德创新设计研究院佛山528300【正文语种】中文【中图分类】V448.2惯性导航是现代科技中的一门尖端技术,在工业和学术界中越来越得到广泛的关注,且开发一种稳定高效的算法变得越来越有意义。
惯性测量单元(internal measurement unit, IMU)的精度决定了其工作效率[1]。
IMU包括陀螺仪、加速度计和地磁计。
加速度计在静态情况下稳定性好,但长时间运动其数据相对不可靠且易受噪声的影响;陀螺仪在动态情况下稳定性好,但积分运算会产生累计误差且容易受到温漂的影响;地磁计易受磁场的干扰[2]。
实际应用中通常使用加速度计和地磁计输出来修正陀螺仪的漂移误差[2,3]。
对姿态估计和控制问题有相当多的解决方案,广泛采用的姿态估计方法是卡尔曼滤波[3]、四元数算法[4]、互补滤波[5]、Mahony[6]和Madgwick[7]算法等。
互补滤波相对简单,运算量也不大,又能很好的滤除噪声和抑制漂移,但是要确定合适的高通和低通滤波器截止频率是不容易的,且互补滤波只在低精度的惯性系统中静态或准静态有较好的效果[8]。
卡尔曼滤波利用线性系统状态方程,通过输入输出观测数据,对系统状态进行最优估计,但观测数据中包含系统的噪声和干燥,会影响估计的准确性[8]。
大多数传统机体姿态控制方法都是基于反馈方案[9],在文献[4]使用四元数的反馈来稳定飞行器的垂直起飞和降落。
也有使用矢量测量来稳定姿态[10],但会遇到一些较大的噪声干扰。
针对上述众多问题,在文献[3]给出的启发下,考虑陀螺的偏差估计,设计一种基于高阶无源线性互补滤波[11]和卡尔曼的融合算法,利用高阶无源线性互补滤波计算的姿态角作为观测值,通过扩展卡尔曼滤波(extend Kalman filter, EKF)器的处理,验证姿态角的精度。
一种融合互补滤波和卡尔曼滤波高精度姿态测量算法

一种融合互补滤波和卡尔曼滤波高精度姿态测量算法李杰;杨雁宇;冯凯强;李小燕;郑涛;杜思远;许廷金【摘要】针对在4级海况下船体大幅度晃动,甚至丢失GPS信号的复杂环境,常规算法会导致姿态测量精度急剧下降的情形,为'动中通'中的航姿系统设计了一套姿态融合算法.在GPS有效时,卡尔曼滤波的观测量引入双天线GPS输出的航向角,解决航向角观测性弱和估计不准的问题,同时引入互补滤波得到的陀螺修正量,提高了水平姿态角的可观性,融合两种算法提高了解算精度.在GPS无效时,通过互补滤波,抑制陀螺漂移,输出高精度水平姿态角,配合天线所接收信号的强度使'动中通'正常工作.为验证算法的有效性,进行了动态实验,实验结果表明:该算法在GPS有效的情况下能保证俯仰滚动角(RMSE标准)精度在0.2°以内,航向角精度在0.5°以内,在GPS无效情况下也可使俯仰和滚动角精度长时间维持在0.3°以内,具有一定的工程应用价值.%Under complex sea environments of degree-4 sea condition, the ship hulls would wobble or even lose GPS signals, thus the conventional algorithm may lead to the sharp decline of attitude measurement accuracy. To solve this problem, an attitude measurement algorithm of the attitude measurement system is designed for "Satcom on the Move". When GPS is valid, the outputs of the dual-antenna GPS observations of course are introduced to the Kalman filter, which resolves the problem of weak observability and inaccurate estimation of azimuth. Meanwhile, the amount of gyro correction is improved by introducing the complementary filtering, which improves the observability of the horizontal attitude angle and combines the two algorithms to improve the accuracy of the calculation. When the GPS is not valid, the gyro drift is suppressed bycomplementary filtering, and high-precision horizontal attitude angle is output. The intensity of the received signal of the antenna is used to make the "Satcom on the Move" work normally. Finally, dynamic experiments are carried out to verify the effectiveness of the proposed algorithm: in the case of visible satellite, the accuracy of the pitch rolling angle is within 0.2° and the yaw angle is less than 0.5° (RMSE); in absence of visible satellite, the pitch and roll angle accuracies can be maintained within 0.3° for a long time. These show that the algorithm has important engineering value.【期刊名称】《中国惯性技术学报》【年(卷),期】2018(026)001【总页数】6页(P51-55,86)【关键词】动中通;姿态测量;卡尔曼滤波;互补滤波【作者】李杰;杨雁宇;冯凯强;李小燕;郑涛;杜思远;许廷金【作者单位】中北大学电子测试技术科技重点实验室,太原 030051;中北大学电子测试技术科技重点实验室,太原 030051;中北大学电子测试技术科技重点实验室,太原 030051;中北大学电子测试技术科技重点实验室,太原 030051;中北大学电子测试技术科技重点实验室,太原 030051;中北大学电子测试技术科技重点实验室,太原030051;中北大学电子测试技术科技重点实验室,太原 030051【正文语种】中文【中图分类】U666.1现阶段船载姿态参考系统内大多为光纤陀螺或者激光陀螺,虽能达到很高的精度,但相对而言,MEMS器件具有体积小、功耗低、价格便宜的优点,尤其在民用领域有着广阔的前景[1]。
基于卡尔曼滤波和互补滤波的改进型姿态解算方法

基于卡尔曼滤波和互补滤波的改进型姿态解算方法李庆党; 张义龙【期刊名称】《《青岛科技大学学报(自然科学版)》》【年(卷),期】2019(040)006【总页数】6页(P85-89,98)【关键词】最优估计; 姿态解算; 卡尔曼滤波; 互补滤波【作者】李庆党; 张义龙【作者单位】青岛科技大学中德科技学院山东青岛 266061; 青岛科技大学自动化与电子工程学院山东青岛 266061【正文语种】中文【中图分类】TP13姿态解算是惯性导航系统及其他姿态监测控制系统的重要组成部分,其解算精度会直接影响惯性导航系统和其他姿态控制系统的控制精度[1]。
随着微机电技术的发展,三轴MEMS陀螺仪、加速度计、磁力计成为最主要的惯性测量器件。
但现有的MEMS器件存在某些缺陷:MEMS陀螺仪会随时间积累误差;MEMS三轴加速度计高频特性差,在运动过程中会受到三轴上附加加速度的影响;MEMS三轴磁力计易受环境磁场的干扰而失效。
因此往往采用互补滤波的方法,结合几种MEMS惯性器件的特点,在短时间内采用陀螺仪得到的角度做为最优值,定时对加速度采样来的加速度值进行取平均值来校正陀螺仪的得到的角度。
但这种方法解算出的姿态角往往存在精度不够、动态效果差的问题。
本工作提出的改进型互补滤波方法采用卡尔曼滤波融合三轴陀螺仪和三轴加速度计的数据,得到真实角度的最优估计,然后与磁力计的数据一起并入互补滤波算法,最终得到姿态角[2]。
因为卡尔曼滤波估计出的角度经过了过程噪声和观测噪声的补偿,并由卡尔曼增益的加权优化,精度优于单靠角速度积分得到的角度。
因此提高了解算精度。
1 常规互补滤波法鉴于加速度计低频特性比较好,因为加速度的角度可以直接算出来,没有累积误差,所以长时间后也比较准。
而陀螺仪长时间后由于积分误差的累加,会造成输出误差比较大,甚至无法使用。
所以用互补滤波法根据他们的特性取长补短进行姿态解算,每过一段时间就让加速度计去校准一下陀螺仪。
一种二阶互补滤波与卡尔曼滤波的姿态解算方法设计

一种二阶互补滤波与卡尔曼滤波的姿态解算方法设计
黄镇;张浩磊;刘梅;朱健
【期刊名称】《电子工艺技术》
【年(卷),期】2018(039)003
【摘要】AHRS系统中通常采用针对MEMS加速度计与磁强计融合姿态角和陀螺仪输出数据进行Kalman滤波的方法,但是这种方法中MEMS加速度计与磁强计基于MARG方法融合要求载体处于静态状态,否则会出现线加速度带来误差.提出一种互补滤波与卡尔曼滤波结合的方法对航姿参考系统进行姿态解算,用自制AHRS 硬件平台进行静态与动态测试.实验表明:这种方法在静态条件下可有效抑制漂移噪声,在动态条件下可提高姿态角解算精度.
【总页数】4页(P168-170,178)
【作者】黄镇;张浩磊;刘梅;朱健
【作者单位】南京电子器件研究所,江苏南京 210016;南京电子器件研究所,江苏南京 210016;南京电子器件研究所,江苏南京 210016;南京电子器件研究所,江苏南京 210016;微波毫米波单片集成和模块电路重点实验室,江苏南京 210016【正文语种】中文
【中图分类】TP273
【相关文献】
1.互补滤波和卡尔曼滤波的融合姿态解算方法 [J], 张栋;焦嵩鸣;刘延泉
2.一种四旋翼姿态解算与控制优化方法设计分析 [J], 江杰;王康;李刚
3.基于四元数二阶互补滤波的四旋翼姿态解算 [J], 伏家杰;周翟和;尹辉;陈燕
4.基于卡尔曼滤波和互补滤波的改进型姿态解算方法 [J], 李庆党; 张义龙
5.融合互补滤波和卡尔曼滤波的姿态解算算法 [J], 刘颖;杨媛;徐金涛
因版权原因,仅展示原文概要,查看原文内容请购买。
互补滤波算法姿态解算

地磁场向量 可不可以采用和加速度计一样的修正方法来修正? 加速度计在静止时测量的是重力加速度(0,0,1)T ,是有大小和方向的; 同理,地磁计同样测量的是地球磁场的大小和方向,只不过这个方向不再是竖直向下 而是与x轴(或者y轴)呈一个角度,与z轴呈一个角度。记作(bx,by,bz)T ,这里我 们让x轴对准北边,所以by=0,即 (bx,0,bz)T 倘若我们知道bx和bz的精确值,那么我们就可以采用和加速度计一样的修正方法来)
23
01
q q q q 2
2
2
2
0
1
2
3
这样,四元数可以用方向余弦、欧拉角表示,同样,欧拉角也可以用 方向余弦和四元数表示。
用方向余弦表示四元数
对于小角度位移,四元数参数可以用表示为
q 0
1 2
1
(1c11c22c33) 2
q 1
1
4q
c c (
)
32
23
0
三、姿态表示方法
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示,是一个3*3阶的矩阵,矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
三、姿态表示方法
C 3.1 方向余弦矩阵 方向余弦矩阵用符号
n
b 表示,是一个3*3阶的矩阵,矩阵的列表示
载体坐标系中的单位矢量在参考坐标系中的投影。
q 2
1
4q
c c (
)
13
31
0
q 3
1
4q
c c (
)
21
12
- 1、下载文档前请自行甄别文档内容的完整性,平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益,请联系客服反馈,我们会尽快为您处理(人工客服工作时间:9:00-18:30)。
传感器与微系统 ( T r a n s d u c e r a n d Mi c r o s y s t e m T e c h n o l o g i e s )
2 0 1 7年 第 3 6卷 第 3期
DOI : 1 0 . 1 3 8 7 3 / J . 1 0 0 0 - 9 7 8 7 ( 2 0 1 7 ) 0 3 - - 0 0 6 2 - - 0 4
B a o d i n g 0 7 1 0 0 3 , C h i n a )
Ab s t r a c t :A i mi n g a t p r o b l e m o f h i g h n o i s e , l o w p r e c i s i o n o f i n e r t i a l m e a s u r e me n t u n i t ( I MU) a n d l o w p r e c i s i o n o f
出了一种互补滤波和卡尔曼滤波相结合的融合算法 。该算法基于姿态角微分方程建 立系统的状态 方程 模 型, 利用互 补滤 波后 的姿态角作为 系统 的观测量 , 再应用扩展 卡尔曼滤波 ( E K F ) 算法融合 了陀螺 仪 、 加 速
度计 和电子罗盘 的测量数据 。为验证该算法有效性 , 用带有传感器 的开发板依次进行静态和动态测试 , 实 验结果表 明: 结合 了互补 滤波 和卡尔曼滤波 的融合算 法 , 在静 态时能够抑 制姿态角漂移 和滤 出噪声 , 在 动 态时能够快速跟踪姿态 的变化 , 提高了姿态角的解算精度 。 关键词 :卡尔曼滤波 ; 互补 滤波 ; 姿态估计 ; 数据融合 ;惯性测量单元
p r o p o s e d . T h e a l g o i r t h m e s t a b l i s h t h e s t a t e e q u a t i o n mo d e l b a s e d o n d i f f e r e n t i a l e q u a t i o n o f a t t i t u d e a n g l e a n d
c l a s s i c a l a t t i t u d e s o l u t i o n a lg o r i t h m, a f u s e d a l g o r i t h m wi t h c o mp l e me n t a  ̄ f i l t e r i n g a n d Ka l ma n f i l t e in r g i s
互 补 滤 波和 卡 尔 曼 滤 波 的 融合 姿 态 解 算 方 法
张 栋, 焦 嵩鸣 ,刘延 泉
( 华 北 电 力 大 学 自动化 系 , 河北 保定 测量单元 ( I MU) 噪声大 、 精度低 的缺点和常规 的姿态解算算 法精度不 高等问题 , 提
w i t h i n e r t i a l s e l f s o Y s t o t e s t i n s t a t i c a n d d y n a mi c c o n d i t i o n . T h e r e s u l t s o f e x p e ime r n t s s h o w t h a t t h e a l g o i r t h m f u s e d w i t h c o mp l e me n t a r y i f l t e in r g a n d Ka l ma n i f l t e i r n g c a n c o n s t r a i n t d r i t f a n d n o i s e o f a t t i t u d e a n g l e i n s t a t i c
d a t a o f g y o, r a c c e l e ome r t e r a n d e l e c t r o n i c c o mp a s s . T o v e i r f y t h a t t h e a l g o r i t h m i s e f f e c t i v e ,u s e d e v e l o p me n t b o a r d
中图分类号 :V2 4 9
文献标识码 :A
文章编 号 :1 0 0 0 - - 9 7 8 7 ( 2 0 1 7 ) O 3 - - 0 0 6 2 - - 0 4
Fu s e d a t t i t u d e e s t i ma t i o n a l g o r i t h m ba s e d o n c o mp l e me n t a r y
il f t e r i n g a n d Ka l ma n i f l t e r i n g
Z HAN G Do n g,J I AO S o n g — mi n g ,L I U Ya n — q u a n
( D e p a r t me n t o f A u t o ma t i o n , No r t h C h i n a E l e c t r i c P o w e r U n i v e r s i t y ,
c h o  ̄e a t t i t u d e a n g l e a f t e r c o mp e n s a t i o n i f l t e i r n g a s t h e o b s e r v a t i o n o f s y s t e m. Us e E KF lg a o r i t h m f u s e d me a s u r e d