关于MPU6050姿态解算的一阶互补滤波方法（从原理到代码实现）

关于MPU6050一阶互补滤波方法（从原理到代码实现）

1.写在前面
最近知道自己不用考研后便花了很多时间来准备机械创新设计大赛，在设计的多功能防台风窗中需要到MPU6050对窗户的姿态进行检测，用来反馈到步进电机控制电机转动（别问为什么不用编码器反馈来控制电机转动，问就是穷，当然也存在本项目用的电源电压低，丢步严重的问题。）在MPU6050传感器中一般读到的原始数据为三轴加速度和三轴角速度，那么如何利用这六个数据进行姿态解算便成为了一个首要的问题。可以利用官方的dmp来解算姿态，但是代码移植是个问题，我用的是恩智浦的RT1064，手头只有MSP430和STM32的dmp库，在采用卡尔曼滤波算法发现收敛太慢的情况下，我决定采用经典的一阶互补滤波算法和简单的自适应一阶互补滤波算法来试试看，也从数学原理到代码彻底的把一阶互补滤波原理捋一遍。
2.什么是一阶互补滤波以及为什么要用一阶互补滤波
假设我们需要测定一个物理量，这个物理量可以通过两个传感器测得，而且其中一个传感器的测量噪声主要集中在高频段，另一个传感器的噪声主要集中在低频段，那么我们可以构造出两个滤波器，其中一个是低通滤波器，另一个是高通滤波器。然后让这两个传感器测得数据分别通过这两个滤波器再进行加权求和：
Y=αx1+(1-α)x2 (1)
式(1)中Y即为输出的预测值，α为加权系数，如果这个时候α可变，并且时时刻刻保持估算出的Y的方差最小那么α还有一个名字——卡尔曼增益，此时转化为最佳线性滤波——卡尔曼滤波，只是在卡尔曼滤波算法中x1或者x2有一个是实测的，另一个是通过其他传感器再结合数学模型推测的结果，这和我们MPU6050姿态解算很像，比方说我们要解算俯仰角，那么我们需要知到x轴方向的加速度，绕y轴的角速度，这样我们可以通过：
pitch1=asin(ax/g); (2)
pitch2 =pitch(0)+ ∫Wydt; (3)
来算出两个俯仰角。其中ax为x轴方向加速度，g为重力加速度，ax除以重力加速度再求反正弦函数即为从加速度计中推出的俯仰角1（pitch1）。
pitch(0)为初始俯仰角（由第一次测得的pitch1得出），Wy为y轴角速度，角速度对时间积分再加上初始俯仰角即为从陀螺仪推出的俯仰角2（pitch2）。
我们来看看这两个角度是否满足一个噪声集中在高频段，一个噪声集中在低频段：

图中黄线代表加速度测得角度，绿线代表真实的角度，可以看出黄线基本跟随绿线，说明没有漂移误差，但是存在较为严重的毛刺信号，说明存在高频噪声；
图中蓝线代表陀螺仪的积分，可以看出它几乎没有尖锐的毛刺信号，但是随着积分时间的累加，陀螺仪解算出的角度存在极大的漂移，这个漂移可以看做是一个低频噪声，比如漂移了一个常数值，那么就存在一个很大的频率为0的噪声分量，我们利用matlab的快速傅里叶变换来看看是不是这个情况：


clear;
clc;
x=load('mpu6050.txt');
x1=[];
for n=0:1131j=6*n+1;x1(n+1)=x(j);
end
k=0:0.01:11.31;
figure
plot(k,x1);
fs=100;
T=1/fs;
L=1132;
t=(0:L-1)*T;
Y=fft(x1,L);
P2=abs(Y/L);
P1=P2(1:L/2+1);
f=fs*(0:(L/2))/L;
plot(f,P1);

没标横纵坐标单位和图例嗯，坏习惯要改，这里横坐标为频率，单位为Hz，纵坐标为对应频率所占的幅值大小。
从图中可以看出在第低频段存在严重的噪声，在获得测试数据时我是以5Hz为频率对传感器进行摆动，当然不可能精确到5Hz,所以我们要获得的数据频率应集中在3-5Hz的频段中，但是由于积分漂移，0hz（常量）-2Hz的干扰1信号占了绝大部分，所以我们应该使用高通滤波器把这些低频信号给滤除。
同理可得，我们也可以用快速傅里叶变换来分析加速度计解算出的角度信号的幅频特性，这里就不过多展开了，后面有机会讲卡尔曼滤波算法时再写。
得到这两个俯仰角信号的特性后，我们知道它满足互补滤波算法对信号的要求，那么如何设计互补滤波算法呢？

具体的实现流程为：

3.一阶互补滤波的数学推导
一阶低通惯性滤波：LPF=1/(τ*s+1) （4）
这是一个低通滤波器，它的截止频率为1/τ，假如我们要让该滤波器的截止频率为3，那么τ为1/3，我们在matlab中用bode图看一看是不是这样的：## 标题

num=[1];
den=[1/3 1];
sys=tf(num,den);
bode(sys);

得到：

看完了低通滤波，我们还差一个高通滤波，简单：HPF=1-LPF就是高通滤波器：
一阶高通滤波：HPF=1-1/(τ*s+1) （5）
同样设τ为1/3，来看看bode图：

num=[1];
den=[1/3 1];
sys=1-tf(num,den);
bode(sys);

现在再次贴出具体设计方案：

我们看上图左边部分，首先忽略掉陀螺仪，B(s)为输出，加速度计输出角度A(s)为输入，得到闭环传递函数为:
B(s)/A(s)=1/(TAs+1); (6)
同理，以陀螺仪输出角速度G(s)为输入，B(s)为输出，得到闭环传递函数为：
B(s)/G(s)=TA/(TAs+1); (7)
这里有的同志们就要问了，这不是一阶高通滤波的表达式啊，没关系，因为我们一阶高通滤波器输入的是角度，而这里陀螺仪输出的G(s)为角速度啊，角速度积分就是角度，将(7)式变形为：
B(s)=TAs/(TAs+1)*G(s)/s;
这样来看就是一个一阶高通滤波器了！
到此为止1我们已经知道如何在数学上得到一个一阶互补滤波器了，美汁汁！下面我们来看看代码上如何实现，上来先贴出代码：

/*一阶补偿*/
float Angle_offset(float acx1,float gyroy2)
{float hk;hk=-100.0f/(8192-100)*(acx1-100)+100;//MPU_Get_Raw_data(&aacx,&aacy,&aacz,&gyrox,&gyroy,&gyroz);Angle_ax = asin((acx1-hk) /8192);   //去除零点偏移,计算得到角度（弧度）Angle_ax = Angle_ax*180/3.14f;     //弧度转换为度gyroy=-1*(gyroy2+3)/16.4f; //if(abs(gyroy)<1.2)Angle1=0.95f*Angle_ax+0.05f*(Angle1 + gyroy*0.01);/*elseAngle1=0.9f*Angle_ax+0.1f*(Angle1 + gyroy*0.01);    */if(Angle1>=90)Angle1=90;else if(Angle1<=-90)Angle1=-90;pitIsrCnt1=true;return Angle1;
}

这个函数的输入参数1为x轴加速度，参数2为y轴角速度，其余代码不多解释，我们直奔主题，看最关键的一句：Angle1=0.95fAngle_ax+0.05f(Angle1 + gyroy*0.01);为什么要这样写，怎么从数学方面来进行推导？0.95这个系数怎么得来？（我用的是试凑法，0.95为系数试出来刚好收敛速度快且静态误差较小。）下面我们来简单谈一谈。
数学到算法的推导：
拉式域方程为：
变换到时域并且离散化为：

将其变形可得：

好了，到这一步我们可以看出来算法与该公式的关系了，前面提到的0.95这个系数其实是T/(T+TA),那么如果我们知道一阶互补滤波每一次迭代时间T，然后又能确定我们需要设定的截止频率1/TA是多少，我们就可以定量计算出这个系数，当然不同的系统肯定会标定出不同的系数。
在单片机算力不足或者主频不高的情况下，一阶互补滤波可能是比较好的一个选择，当然，如果算力足够，还是推荐使用卡尔曼滤波，后面有空再写一写基于卡尔曼滤波的姿态解算以及自适应的一阶互补滤波，这篇帖子当做学习心得，后面可以留着复习，以此为记。
以上。

关于MPU6050姿态解算的一阶互补滤波方法（从原理到代码实现）相关推荐

姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）
互补滤波原理: 在四轴入门理论知识那节我们说,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系.陀 ...
基于STM32的四旋翼无人机项目（二）：MPU6050姿态解算（含上位机3D姿态显示教学）
前言:本文为手把手教学飞控核心知识点之一的姿态解算--MPU6050 姿态解算(飞控专栏第2篇).项目中飞行器使用 MPU6050 传感器对飞行器的姿态进行解算(四元数方法),搭配设计的卡尔曼滤波器与 ...
【毕业设计】MPU6050姿态解算姿态估计 - 物联网单片机 stm32
文章目录 1 简介 2 MPU6050 3 工作原理 4 单片机与MPU6050通信 4.1 mpu6050 数据格式 4.2 倾角计算方法 5 实现代码 6 最后 1 简介 Hi,大家好,这里是丹成 ...
c语言姿态解算程序,mpu6050姿态解算原理_mpu6050姿态解算程序
描述关于MPU6050姿态解算原理 mpu6050常用作提供飞控运行时的姿态测量和计算,在在姿态结算中有几个重要的概念,欧拉角.四元数等. 欧拉角:用来表征三维空间中运动物体绕着坐标轴旋转的情况.即 ...
stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
Arduino 与 MPU6050 姿态解算+ PROCESSING
2019独角兽企业重金招聘Python工程师标准>>> 买的MPU6050自带姿态解算大大减轻了上层处理器所做的工作. 通过熟悉了一下processing之后做了一个小例子更是感觉这 ...
MPU6050姿态解算1-DMP方式
MPU6050的姿态解算方法有多种,包括硬件方式的DMP解算,软件方式的欧拉角与旋转矩阵解算,软件方式的轴角法与四元数解算.本篇先介绍最易操作的DMP方式. MPU6050基本功能 3轴陀螺仪陀螺仪 ...
mpu6050姿态解算与卡尔曼滤波（2）卡尔曼滤波
卡尔曼滤波,是最优估计理论中十分重要的一个部分. 要全面地理解卡尔曼滤波,你需要一点统计学的知识,以及一点矩阵理论.通常最优估计理论的教材是从最小二乘估计讲起,接着讲到最小方差估计,极大似然估计以及维 ...
【51单片机快速入门指南】4.3.2： MPU6050：一阶互补滤波、二阶互补滤波和卡尔曼滤波获取欧拉角
目录源码 MPU6050_Filter.c MPU6050_Filter.h 使用方法测试程序一阶互补滤波效果二阶互补滤波效果卡尔曼滤波效果总结普中51-单核-A2 STC89C5 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...

关于MPU6050姿态解算的一阶互补滤波方法（从原理到代码实现）

关于MPU6050一阶互补滤波方法（从原理到代码实现）

关于MPU6050姿态解算的一阶互补滤波方法（从原理到代码实现）相关推荐

最新文章

热门文章