互补滤波法补充

最新推荐文章于 2024-04-17 20:02:11 发布
最新推荐文章于 2024-04-17 20:02:11 发布
阅读量611 收藏 5
点赞数
分类专栏： 无人机

四旋翼姿态解算——互补滤波法补充（融合磁力计）

2017年03月01日 16:18:06 阅读数：6614 标签： 算法数据飞控数学 更多

个人分类： STM32四旋翼无人机

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/hongbin_xu/article/details/59110226

转载请注明出处：http://blog.csdn.net/hongbin_xu 或 http://hongbin96.com/ 
文章链接：http://blog.csdn.net/hongbin_xu/article/details/59110226 或 http://hongbin96.com/119

上一次的博文中（点我打开）介绍了互补滤波法的算法流程和程序实现，但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算出姿态。

由于机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角yaw,并且磁力计也同样无法测得z轴的旋转量。

所以，我们考虑使用加速度计和磁力计同时对陀螺仪进行校正。这次介绍如何融合磁力计的数据进行姿态解算。 
在介绍算法之前，想再提一点自己的感想。很多新手入门姿态解算算法时，最先接触的就是这个Mahony的互补滤波算法，也相对简单一些。有的人觉得数学推导很麻烦，也不愿意花时间研究代码，懵懵懂懂地把程序复制粘贴过去就随便填了几个参数，然后就去看现象。其实大家都知道这种方法是不对的吧，没出问题还好，一旦出了错就不知所措了。很多时候，飞控出了问题，不一定是控制的问题，有时候问题出在姿态解算上（“血与泪”的教训，我当初就是在这个地方被坑了很久）。得到了姿态角后，先看看波形，看看数据怎么样，滤波器有没有用。（不想自己写程序和调试的，请无视这些。） 
这里再推荐一个很实用的工具：匿名上位机。功能很强大，可以自己写程序上传数据，在上位机上看波形，一切OK之后再进行下一步的工作。 
这是匿名上位机的界面的截图，可以到匿名科创的官网上下载： 
匿名上位机

好了，开始介绍算法部分吧！ 
还是按照这张框图得流程来介绍： 
这里写图片描述
首先，假设磁力计测量得到的三轴磁场数据为这里写图片描述 。 
为方便运算，像此前一样，将数据归一化之后再进行运算。得到载体坐标系b下的标准磁场数据这里写图片描述，其中这里写图片描述。为表示简便，将归一化之后的数据还是表示为这里写图片描述。 
将这组载体坐标系b下测得的数据这里写图片描述旋转到导航坐标系n下，得到这里写图片描述。 
坐标系的旋转相当于乘以一个旋转矩阵这里写图片描述 。 
使用以前推导得到的结论，其为四元数表示的旋转矩阵这里写图片描述： 
这里写图片描述 
得到： 
这里写图片描述 
假定理想情况下，让导航坐标系n中x轴指向正北方向，并设这个新的导航坐标系下磁力计数据为这里写图片描述。那么根据高中物理知识分析可以知道，Z轴方向分量（即重力方向分量）相等，水平方向分量相等。 则容易得到： 
这里写图片描述 
若取y轴指向正北方向，则： 
这里写图片描述 
取x轴或y轴朝向正北，按照自己情况定义。我在程序中取的是x轴，所以后面使用x轴朝正北方向的结果来推导。 
将磁力线在导航坐标系n中的理想输出这里写图片描述 再次旋转到载体坐标系b中，得到在b系中的理想输出： 
这里写图片描述 
将理想输出这里写图片描述和原始输出这里写图片描述 作叉积得到误差e(w)，将其作为补偿项送给陀螺仪进行校正： 
这里写图片描述 
表示成矩阵形式： 
这里写图片描述 
这部分只是用来修正YAW轴的，还要加上加速度的校正补偿项（前面的博文中已经推导过），再送给陀螺仪： 
这里写图片描述 
后面的就与前面博文中介绍的步骤一样了，这里不做赘述。

终于可以上代码了：


#define Kp 10.0f                        // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f                          // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f                   // half the sample period采样周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;    // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0;    // scaled integral error
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
  float norm;
  float hx, hy, hz, bx, bz;
  float wx, wy, wz;
  float vx, vy, vz;
  float ex, ey, ez;

  // 先把这些用得到的值算好
  float q0q0 = q0*q0;
  float q0q1 = q0*q1;
  float q0q2 = q0*q2;
  float q0q3 = q0*q3;
  float q1q1 = q1*q1;
  float q1q2 = q1*q2;
  float q1q3 = q1*q3;
  float q2q2 = q2*q2;
  float q2q3 = q2*q3;
  float q3q3 = q3*q3;

    if(ax*ay*az==0)
        return;

    if(mx*my*mz == 0)
        return;

  norm = sqrt(ax*ax + ay*ay + az*az);       //acc数据归一化
  ax = ax / norm;
  ay = ay / norm;
  az = az / norm;

  norm = sqrt(mx*mx + my*my + mz*mz);       //mag数据归一化
  mx = mx / norm;
  my = my / norm;
  mz = mz / norm;

//  mx = 0;
//  my = 0;
//  mz = 0;

hx = 2 * mx * (0.5 - q2q2 - q3q3) + 2 * my * (q1q2 - q0q3) + 2 * mz * (q1q3 + q0q2);  
hy = 2 * mx * (q1q2 + q0q3) + 2 * my * (0.5 - q1q1 - q3q3) + 2 * mz * (q2q3 - q0q1);  
hz = 2 * mx * (q1q3 - q0q2) + 2 * my * (q2q3 + q0q1) + 2 * mz * (0.5 - q1q1 -q2q2);          
bx = sqrt((hx*hx) + (hy*hy));  
bz = hz;

  // estimated direction of gravity and flux (v and w)              估计重力方向和流量/变迁
  vx = 2*(q1q3 - q0q2);                                             //四元素中xyz的表示
  vy = 2*(q0q1 + q2q3);
  vz = q0q0 - q1q1 - q2q2 + q3q3 ;

    wx = 2 * bx * (0.5 - q2q2 - q3q3) + 2 * bz * (q1q3 - q0q2);  
  wy = 2 * bx * (q1q2 - q0q3) + 2 * bz * (q0q1 + q2q3);  
  wz = 2 * bx * (q0q2 + q1q3) + 2 * bz * (0.5 - q1q1 - q2q2); 

  // error is sum of cross product between reference direction of fields and direction measured by sensors
//  ex = (ay*vz - az*vy) ;                                               //向量外积在相减得到差分就是误差
//  ey = (az*vx - ax*vz) ;
//  ez = (ax*vy - ay*vx) ;

    ex = (ay*vz - az*vy) + (my*wz - mz*wy);  
    ey = (az*vx - ax*vz) + (mz*wx - mx*wz);  
    ez = (ax*vy - ay*vx) + (mx*wy - my*wx);

  exInt = exInt + ex * Ki;                                //对误差进行积分
  eyInt = eyInt + ey * Ki;
  ezInt = ezInt + ez * Ki;

  // adjusted gyroscope measurements
  gx = gx + Kp*ex + exInt;                                              //将误差PI后补偿到陀螺仪，即补偿零点漂移
  gy = gy + Kp*ey + eyInt;
  gz = gz + Kp*ez + ezInt;                                          //这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

  // integrate quaternion rate and normalise                           //四元素的微分方程
  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

  // normalise quaternion
  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
  q0 = q0 / norm;
  q1 = q1 / norm;
  q2 = q2 / norm;
  q3 = q3 / norm;

  Q_ANGLE.z = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
  Q_ANGLE.y  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
  Q_ANGLE.x = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}
  • 1
  • 2
  • 3
  • 4
  • 5
  • 6
  • 7
  • 8
  • 9
  • 10
  • 11
  • 12
  • 13
  • 14
  • 15
  • 16
  • 17
  • 18
  • 19
  • 20
  • 21
  • 22
  • 23
  • 24
  • 25
  • 26
  • 27
  • 28
  • 29
  • 30
  • 31
  • 32
  • 33
  • 34
  • 35
  • 36
  • 37
  • 38
  • 39
  • 40
  • 41
  • 42
  • 43
  • 44
  • 45
  • 46
  • 47
  • 48
  • 49
  • 50
  • 51
  • 52
  • 53
  • 54
  • 55
  • 56
  • 57
  • 58
  • 59
  • 60
  • 61
  • 62
  • 63
  • 64
  • 65
  • 66
  • 67
  • 68
  • 69
  • 70
  • 71
  • 72
  • 73
  • 74
  • 75
  • 76
  • 77
  • 78
  • 79
  • 80
  • 81
  • 82
  • 83
  • 84
  • 85
  • 86
  • 87
  • 88
  • 89
  • 90
  • 91
  • 92
  • 93
  • 94
  • 95
  • 96
  • 97

照着前面推导的步骤，再跟着程序走一遍，是不是清晰了许多？

推荐一些比较好的资料的链接： 
陀螺仪、加速度计 、磁力计的介绍 
http://www.360doc.com/content/13/1217/19/235269_337950743.shtml 
磁力计工作原理介绍 
http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html

不会写代码的美年达
关注
  • 0
    点赞
  • 踩
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
互补滤波法
02-24
这是我在网上找的说明互补滤波法的框图，很不错。流程图很清楚地阐释了整个互补滤波的流程。
九轴姿态解算 积分互补滤波
02-01
九轴姿态解算，四元数互补滤波法，磁力计数据单独，测试四轴可以起飞
大神对飞控精准高度估计算法解读
csshuke的专栏
05-21 4800
飞控的高度信息主要由气压计来提供，也可包含GPS、超声波等。跟姿态解算类似，单一的传感器所提供的信息并不能满足实际应用要求，所以同样需要多传感器数据融合。气压计高度问题主要有以下几点，如图所示：1、噪声相对较大，数据不够平滑，不过这个问题不大，可以使用一阶低通滤波来处理；2、数据从上电开始会有漂移，尤其受温度影响，这里图中反应不明显；3、最大的问题在于，受外界气流影响太大，图中50-90s左右，设...
STM32无人机-四轴四元数姿态解算与卡尔曼滤波
迷途
09-19 1389
这部分很难，新手知道四元数的功能是将 6 轴传感器数据转化为三轴姿态角度数据即可。四元数解算程序店家已经封装成一个函数，输入 MPU6050 数值，解算周期 dt，输出三维姿态角信息。事实上所谓 DMP 就是 MPU6050 内部的四元数解算，但是他们内部的解算精度低没有自己 做四元数解算精度高。四元数具体的实现有下面几个步骤： 1）机体重力加速度计转换到地理坐标系后。2）与四元数计算的地理重力加速度比较。3）得到误差校正陀螺仪的输出。4）然后用陀螺仪数据进行四元数更新。5）再转换到欧拉角。
mahony 互补滤波器
最新发布
木牛的博客
04-17 50
陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。通过加速度计得到 Δ q a c c ^ \widehat{\Delta q_{acc}} Δqacc​​ ，然后校正四元数中的横滚（roll）和俯仰（pitch）分量。在 px4 中，磁力计使用 GPS 信息 [ 0 , 0 , m a g ] [0, 0, mag] [0,0,mag] 进行校准，故，公式与加速度计相同。在预测环节得到的四元数 q e b ( k ) q^b_e(k) qeb​(k) ，通过加速度计和磁力计的值进行校正。
无人机底层开发-MPU6050的互补滤波
飘飘仙儿__的博客
11-14 3656
对于MPU6050来讲，加速度计对飞行器的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度则不受加速度的影响，但是随着时间增加，陀螺仪的积分漂移和温度漂移带来的误差比较大，因此需要用互补滤波来弥补这两个传感器的缺点。 互补滤波即是在短时间内采用陀螺仪的角度作为最优，长时间用加速度计采样的角度更加准确，通过这两个的值进行互补。那么滤波在哪里？那就是加速度计要滤掉高频信号，陀螺仪要...
无人机姿态解算_互补滤波（1）
slowdownbabe的博客
05-17 3107
无人机姿态解算_互补滤波
四旋翼姿态解算——互补滤波法补充（融合磁力计）
weixin_30824599的博客
03-01 1763
上一次的博文中（点我打开）介绍了互补滤波法的算法流程和程序实现，但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算出姿态。 由于机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角yaw,并且磁力计也同样无法测得z轴的旋转量。 所以，我们考虑使用加速度计和磁力计同时对陀螺仪进行校正。这次介绍如何融合磁力计的数据进行姿态解算。 在介绍算法之前，想再提一点自己的...
陀螺仪与磁力计的姿态融合——互补滤波(续)
小白白学电子
11-30 3658
上一篇我们讲了陀螺仪与加速度计的数据融合，本篇文章是互补滤波的续篇，接着讲陀螺仪与磁力计的数据融合。
【多源融合】Mahony互补滤波详述
weiziqi_fan的博客
08-18 861
本篇博文利用智能手机采集IMU数据，结合matlab代码介绍了AHRS滤波的原理。
作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准1（磁力计如何校正航向）
萧西朗的博客
05-31 1万+
文档参考：上海丙寅电子有限公司 技术文档导语：磁力计传感器取自于大地磁感应的强度hx，hy，hz；1如何得到罗盘的方位角：怎么才能够从简单的 3 轴数据得到罗盘的方位角   1）当 3 轴磁力计工作时可以读到 XYZ 三轴的磁场强度hx，hy，hz，此时的数值并不能直接用作方位角的计算！                   因为此时的读数可能受到器件版面上其他一些含磁材料的影响，形成圆心坐标的硬铁...
作为无人机方面做嵌入式编写的飞控总结5-磁力计对航向角yaw的校准2（磁力计如何校正航向）
萧西朗的博客
05-31 7124
导：根据我前面的视频又说过这个流程：姿态角来自四元素，四元素取决于陀螺仪，陀螺仪存在温度漂移、零点漂移和时间积分误差，所以使用加计（理想与实际的err值）来pi校准陀螺；    现在，磁力计对航向角yaw的校准，就是同样的道理：使用磁力计（理想与实际的err）来校准陀螺；故：现在的err构成为：    现在重点分析 的计算过程；1.既然磁力计的校准与加速度计的校准具有相似性，同时，此过程就是姿态结...
一个旨在演示IMU滤波器（互补滤波器、卡尔曼滤波器和 Mahony&Madgwick滤波器）的项目_ C++_代码_相关文件
07-12
开放式IMU过滤器 一个旨在演示 IMU 滤波器（互补滤波器、卡尔曼滤波器和 Mahony&Madgwick 滤波器）的项目，其中包含大量参考资料和教程。 参考 鼻子 教程和算法 在嵌入式应用中使用 IMU（加速度计和陀螺仪设备）的指南 IMU 指南算法的 Arduino 代码。使用 5DOF IMU（加速度计和陀螺仪组合） DCM 教程 – 定向运动学简介 IMU 数据融合：互补、卡尔曼和 Mahony 滤波器 IMU 产品 X-IO Technologies 的 x-IMU IMU 砖 - TinkerForge 新的开源 IMU - PIXHAWK IMU 的 mbed 食谱 源代码 X-IO Technologies 的开源 IMU 和 AHRS 算法 IMU（加速度计和陀螺仪）和 MARG 传感器阵列（加速度计、陀螺仪和磁力计）的方向估计算法 mbed IMUfilter 更多详情、使用方法，请下载后阅读README.md文件
基于陀螺仪和加速度计的四元数互补滤波融合算法
11-30
基于陀螺仪和加速度计的四元数互补滤波融合算法，斯坦福虚拟现实课程讲义（英文版）
3阶互补滤波
05-25
讲解了ArduPilot中AP_TECS库中的用于估计高度和垂直方向上的爬升速率的三阶互补滤波的实现 参考的paper
九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）
11-06
九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）九轴传感器姿态解算方法（互补滤波和梯度下降法MATLAB）九轴传感器姿态解算方法（互补滤波和梯度下降法...
互补滤波原理
lhyyds的博客
01-20 1823
互补滤波原理 　　对mpu6050来说，加速度计对四轴飞行器的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度则不受加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。那么要怎么弥补这两个缺点呢？那就要用到互补滤波了。 这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。也就是说，短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了。 那么滤波在哪里？加速度计要滤
姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
热门推荐
蓬莱道人的博客
05-21 3万+
    在四轴入门理论知识那节我们说，加速度计和磁传感器都是极易受外部干扰的传感器，都只能得到2维的角度关系，但是测量值随时间的变化相对较小，结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。接下来我们介绍三种传感器数据的融合。...
AHRS互补滤波（Mahony）算法及开源代码
weixin_42918498的博客
10-30 1万+
AHRS(Attitude and heading ref AHRS(Attitude and heading reference system，也就是航姿参考系统。在互补滤波算法中传感器主要采用了IMU（陀螺仪、加速度计）和磁力计。 AHRS的基本思想是，在载体没有平移运动的情况下，通过加速度感知重力分量，可以计算出载体的俯仰（Pitch）和横滚（Pitch)；磁力计可以感知磁北方向，因此可以计算载体的磁北航向；而陀螺仪测量输出载体的旋转角速度，通过积分可以计算得到横滚、俯仰、航向增量，但由于陀螺输出值含
互补滤波和梯度下降法matlab
06-14
互补滤波和梯度下降法是两种在MATLAB中常用的信号处理和优化算法。 互补滤波是一种数字滤波技术，用于把两个不同频段的信号进行分离。在MATLAB中，可以使用函数“complementaryFilter”来进行互补滤波。该函数的...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值