陀螺仪与磁力计的姿态融合——互补滤波(续)
大家好，我是学电子的小白白~
上一篇我们讲了陀螺仪与加速度计的数据融合，本篇文章是互补滤波的续篇，接着讲陀螺仪与磁力计的数据融合。
1）为什么需要磁力计的数据
先回忆一下上一篇内容，加速度计是如何修正陀螺仪的呢？
我们是假定了在大部分时间里，载体是静止或者匀速直线运动的，所以可以认为加速度计的测量值，就是重力加速度。但是，在载体水平时，重力加速度的方向与Z轴相同，是无法反映出偏航角yaw的（因为偏航角是绕Z轴旋转，即使偏航角变化，Z轴方向仍然不变，也就是说加速度计无法感知Z轴的旋转）。
而磁力计可以测出地球的磁场，能够得到水平的北向方位，所以引入磁力计的数据，可以用来修正陀螺仪的偏航角。
2）陀螺仪与磁力计融合方法
我们仍然先回忆一下上一篇的内容，陀螺仪与加速度计是怎样融合的，会有助于本篇方法的理解。
陀螺仪与加速度计融合时，我们先是通过当前时刻的姿态转移矩阵，算出了一个重力加速度的方向vx、vy、vz（这个方向包含了陀螺仪的姿态信息）；再将这个方向与加速度计测量到的重力加速度方向ax、ay、az对比，求出误差ex、ey、ez，再用误差去修正下一时刻的陀螺仪角速度gx、gy、gz。
本篇中，陀螺仪与磁力计融合时，我们也用类似的方法，先通过姿态转移矩阵的变换，利用当前的姿态信息求出一个磁场的方向；再用它与磁力计测到的磁场方向比对，求出误差，最后用这个误差去修正下一时刻的角速度。
但是，需要注意的是，利用加速度计姿态融合时，由于重力加速的方向只有z向有，x、y向都为0，所以我们可以认为它（方向）是已知的；而地球的磁场并不是完全的水平指向北极，在竖直方向上也有一定的大小，所以它不是个已知量；因此，具体实现的时候，磁力计数据的融合与加速度计有点不同，主要在利用姿态信息求磁场方向的这一步。
假定我们需要修正的姿态，是已经被加速度计修正过的，那么此时的姿态与真实的载体姿态就只存在偏航角yaw的偏移。
修正的主要步骤如下：
a) 磁力计测到的地球磁场数值为mx、my、mz，是在未修正偏航角的载体坐标系下的，可以通过当前的姿态转移矩阵转换到地理坐标系下（n系），转换后的磁力计数据为hx、hy、hz，这组磁力计的数据，与真实的磁场数据，只存在偏航角的偏移：
b) 在n系中，假定真实的磁场数据为bx、by、bz，由于z向的磁场数据与偏航角不相关，所以有bz=hz；如果n系的x轴指北，则by=0，bx=√(hx2+hy2)，这就是真实的磁场数据。（如果n系y轴指北，则bx=0，by=√(hx2+hy2)）。
c) 我们得到了真实的磁场数据后（就想当于加速度计融合时，已知的重力加速度），再通过未修正偏航角的姿态转移矩阵变回到载体系，得到一组载体系下的磁场数据wx、wy、wz，这组数据中就包含了偏航角的偏差信息：
d) 然后就可以用wx、wy、wz与磁力计测到的载体系下真实的mx、my、mz叉乘（求外积），求出偏差。
e) 最后，使用这组偏差去修正陀螺仪的角速度，仍然使用与加速度计数据融合相同的方法，PI后修正。
具体的实现框图如下：
融合磁力计数据的主要的代码实现如下：
（这段代码里也包括了加速度计的数据融合，计算误差时，将加速计的误差和磁力计的误差相加后，再求PI去修正陀螺仪的角速度。完整工程文件可以在文末关注公众号找到下载地址）
3）测试验证
使用时，只要周期性地获取陀螺仪、加速度计、磁力计的数据，调用互补滤波函数，更新四元数，计算姿态角即可（与上一篇加速度计数据融合用法一样）。
测试一下互补滤波的效果，使用的硬件平台是MPU6050和HMC5883L，处理器是stm32f103c8t6。
初始化时，需要平放几秒钟让加速度计和陀螺仪自行校准。平放时俯仰角和滚转角都接近于0，偏航角依据与地磁北向的夹角有一定初始值。经过较长时间后，俯仰角、滚转角、偏航角都可以保持正确，不会产生误差累积和漂移；姿态变化后，在回到初始位置，也基本不会有偏差，说明陀螺仪的数据得到了修正；磁力计对偏航角的修正是有效的：
好了，关于互补滤波姿态融合的知识，就讲到这里了。由于篇幅所限，文中的代码只是截取了核心片段进行讲解，完整的工程代码可以免费提供下载交流。
如果觉得有用可以关注作者微 信公.众号“小白白学电子”，有更多内容分享，在公众号可以找到所有代码和资料下载地址。
陀螺仪与磁力计的姿态融合——互补滤波(续)相关推荐
- IMU姿态融合（MPU9250从校正到滤波步骤）
ref:https://blog.csdn.net/u013236946/article/details/72934934 一.动机 MPU9250包括三轴加速度计.三轴陀螺仪和三轴磁力计.因为加速度 ...
- 使用扩展卡尔曼滤波（EKF）进行AHRS九轴姿态融合
AHRS九轴姿态融合 EKF滤波 卡尔曼滤波 在做九轴姿态融合的过程中,这里介绍一种融合算法,基于EKF的九轴姿态融合算法: 首先表明,该算法并非自己想到的,算法原理参考了这篇论文:Dale E. S ...
- 姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
互补滤波原理: 在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系.陀 ...
- 姿态解算知识（三）-陀螺仪加速度计6轴数据融合
这么久的惯导总算是没白看,加上一篇博客的指点,这两天把Mahony的九轴数据融合算法看懂了.可惜第二版硬件还没到,磁力计用不了,没法验证效果~今天先总结下陀螺仪和加速度计的六轴数据融合. 版权声明 原 ...
- STC51单片机15——MPU6050六轴数据融合，互补滤波，时间常数可调，可稳定运行，串口显示角度值
51单片机用模拟IIC的方式读取MPU6050的原始数据,之后经过换算转成三轴加速度和三轴角速度.设定定时器,以固定的频率采集以上得到的数据,并加入互补滤波,去除加速度的噪声以及陀螺仪的零飘.注意,本 ...
- STM32F103之实验7一阶互补滤波求解移动机器人的姿态代码
上一篇博客,采用的是DMP解算姿态,且给出了底层硬件配置及驱动方式,此处摒弃DMP直接由单片机自己实现姿态解算,接下来介绍由三轴陀螺仪和加速度计的值来使用四元数软件解算姿态的方法. 我们先来看看如何用 ...
- 四旋翼姿态解算——互补滤波算法及理论推导
转载请注明出处:http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 文章链接:http://blog.csdn.net/hongbin_xu ...
- 树莓派pico mpu6050 一阶互补滤波四元数法 解算姿态角
micro-python:一阶互补滤波&四元数法 代码 2.系统方案 2.1.组成 本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分 电池为一节14500锂电池, 容 ...
- 浅谈陀螺仪和加速度计的互补滤波
结合加速度计和陀螺仪的平衡解决方案 首先通过ADC采样,读出陀螺仪和编码器的电压值,并将其转化为可用单位: 1.偏移(offset):让陀螺仪静止,加速度计保持水平并静止,这时候读到电压值,就是偏移值 ...
最新文章
- 视频数据处理方法！关于开源软件FFmpeg视频抽帧的学习
- 硬盘满了 mysql启不来_MySQL数据库之磁盘已满造成的mysql启动失败问题分享
- 在webstorm里面添加自定义web browsers的方法
- Autograd：自动求导
- python 虚拟环境 virtualenv virtualenvwrapper的使用方法、命令
- rust加载不进去服务器eac_基于腾讯云的 Rust 和 WebAssembly 函数即服务
- 奇异值分解（Singular Value Decomposition，SVD）
- 【华为云技术分享】云小课 | OBS提供多方面数据安全保障，让存储放心、贴心、省心
- 面试题简答题——数据库相关汇总
- #CSP 201509-1 数列分段（100分）
- 绑定到异步的ObservableCollection
- Django之form组件加cookie，session
- swt/jface第六天 table
- 异步fifo_【好设计论文】一种面向异步FIFO的低开销容错机制研究
- 弹出确认提示框，确定，取消
- linux幻灯片制作软件,Marp(代码制作幻灯片软件)
- 服务器系统漏洞rc4,服务器如何修复旧加密算法漏洞
- APISpace 语音验证码API
- html情侣相册,浪漫的情侣相册名字
- 怎么在线给pdf加盖电子公章