MPU6050加速度、角速度的解算以及互补滤波使用

MPU6050加速度的解算
MPU6050角速度的解算
互补滤波使用（数据融合）
- 数字低通滤波器
- 数字高通滤波器

MPU6050加速度的解算

先根据两张图建立各个角度的关系

RollRollRoll(横滚角)	以xxx轴为旋转轴的旋转角度
YawYawYaw(偏航角)	以zzz轴为旋转轴的旋转角度
PitchPitchPitch(俯仰角)	以yyy轴为旋转轴的旋转角度

我们先了解加速度的单位及量程方便进行解算
在MPU初始化函数中找到初始化加速度传感器的函数

//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Accel_Fsr(u8 fsr)
{return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}

通过寄存器找到数据手册中的说明

比如我们设置量程为±2g\pm2g±2g

mpu是通过ADC采集电压的值来输出加速度值，

而数据寄存器是16位且最高一位为符号位，

即ADC读取的值位±32768\pm32768±32768，对应±2g\pm2g±2g,

1g1g1g对应的ADC值为+32768/2+32768/2+32768/2,即16384LSB/gLSB/gLSB/g为数据分辨率

同样可得±4g\pm4g±4g,数据分辨率为+32768/4=8192LSB/g+32768/4=8192LSB/g+32768/4=8192LSB/g

通过上面的说明我们可以把mpu读取到的原始数据解算为加速度
即：Gx=aacx/16384Gx=aacx/16384Gx=aacx/16384单位为m/s2m/s^2m/s2

我们选择PitchPitchPitch解算

水平状态时只有zzz轴有加速度且加速度为1g1g1g

绕yyy轴旋转后：

x、x^、x、为绕yyy轴旋转后的zzz轴加速度方向
z、z^、z、为绕yyy轴旋转后的zzz轴加速度方向
由图（4）（4）（4）可得ρ=arctanGxGz\rho=arctan\frac{Gx}{Gz}ρ=arctanGzGx,

GxGz=aacxaacz\frac{Gx}{Gz}=\frac{aacx}{aacz}GzGx=aaczaacx（分辨率计算后抵消）

ρ\rhoρ的结果为弧度制，我们将其转化为角度制

Pitch=ρ∗2π360Pitch=\frac{\rho*2π}{360}Pitch=360ρ∗2π,

最终的公式为：
Pitch=arctanaacxaacz∗2π360Pitch=arctan\frac{aacx}{aacz}*\frac{2π}{360}Pitch=arctanaaczaacx∗3602π,

2π360=57.2974\frac{2π}{360}=57.29743602π=57.2974

Pitch=arctanaacxaacz∗57.2974Pitch=arctan\frac{aacx}{aacz}*57.2974Pitch=arctanaaczaacx∗57.2974,
代码如下：

mpu_dmp_get_data(&pitch,&roll,&yaw);     //得到姿态角即欧拉角
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);   //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据
Acc_Pitch=atan((float)aacx/(float)aacz)*57.2974;
printf("%f    %f\r\n",pitch,Acc_Pitch);

我们与DMP处理后的PitchPitchPitch进行比较

数据相差一个负号

将公式变为： Pitch=−arctanaacxaacz∗57.2974Pitch=-arctan\frac{aacx}{aacz}*57.2974Pitch=−arctanaaczaacx∗57.2974即可

代码也对应的进行修改：

mpu_dmp_get_data(&pitch,&roll,&yaw);     //得到姿态角即欧拉角
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);   //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据
Acc_Pitch=-atan((float)aacx/(float)aacz)*57.2974;
printf("%f    %f\r\n",pitch,Acc_Pitch);

输出数据波形:
红色为加速度解算
蓝色为DMPDMPDMP解算

加速度解算后的 PitchPitchPitch相对于DMPDMPDMP的输出值包含大量高频噪声,但是灵敏度大大提高，基本可以实时输出角度值。
（个人感觉还是DMPDMPDMP比较稳定）

同理可得

Roll=arctanaacyaacz∗57.2974Roll=arctan\frac{aacy}{aacz}*57.2974Roll=arctanaaczaacy∗57.2974

Yaw=arctanaacxaacy∗57.2974Yaw=arctan\frac{aacx}{aacy}*57.2974Yaw=arctanaacyaacx∗57.2974
由于yawyawyaw角为绕zzz轴旋转的角，陀螺仪平放时，accz近似为1g1g1g
而yyy和zzz上少有加速度分量，所以解算的yawyawyaw误差较大。

yawyawyaw的解算值与DMPDMPDMP输出波形的比较，通过输出曲线图可以看到解算后误差较大，且有大量高频噪声，一般不采用。

MPU6050角速度的解算

先看mpu角速度初始化的函数

//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}

根据寄存器在数据手册中找到相应的说明，

这里的LSB/(°/s)LSB/(°/s)LSB/(°/s)仍然是数据分辨率，
还是ADC读数数据
数据寄存器为16位，最高一位为符号位
即ADC输出范围为±32768\pm32768±32768

比如我们选中测量范围为±2000°/s\pm2000°/s±2000°/s,

ADC读取的原始值为300

数据分辨率就是32768/2000=16.384LSB/(°/s)32768/2000=16.384 LSB/(°/s)32768/2000=16.384LSB/(°/s)

那么转换为角速度就是30016.384=18.3105(°/s)\frac{300}{16.384}=18.3105(°/s)16.384300=18.3105(°/s)

公式为：角速度=角速度原始值数据分辨率角速度=\frac{角速度原始值}{数据分辨率}角速度=数据分辨率角速度原始值单位：(°/s)(°/s)(°/s)

角速度已经推算出来了，

角度就可以通过对角速度的积分来计算（选取PitchPitchPitch计算）

Pitch=∫gyrox角度分辨率∗dtPitch=\int \frac{gyrox}{角度分辨率}*dtPitch=∫角度分辨率gyrox∗dt

dt为我们读取mpu数据的间隔时间，

因为我们读取的数据是离散的，所以直接进行累加就可以计算角度

Pitch=∑1ngyrox角度分辨率∗ΔtPitch=\sum_1^n\frac{gyrox}{角度分辨率}*\Delta tPitch=∑1n角度分辨率gyrox∗Δt

另外两个角的推算方法相同。

代码如下：

        mpu_dmp_get_data(&pitch,&roll,&yaw);     //得到姿态角即欧拉角MPU_Get_Accelerometer(&aacx,&aacy,&aacz);    //得到加速度传感器数据MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据     Acc_Pitch=atan((float)aacx/(float)aacy)*57.297;Gyro_Pitch+=-(gyrox/16.4)*0.01;             //读取间隔为10ms转换为0.01s计算，读取数值与实际值相反加负号printf("%f    %f\r\n",pitch,Gyro_Pitch);

与DMPDMPDMP的输出波形相比，角速度积分的数据漂移比较明显，但数据的平滑性较好，可以有效抑制高频干扰。

互补滤波使用（数据融合）

对比项	加速度	陀螺仪(角速度)
高频振动噪声	十分敏感	基本无感
高频低频姿态漂移	基本无感	飘移严重
实时性	实时性强，基本不会随时间飘移	随时间飘移严重

加速度数据，通过受力分能够显示角度变化趋势，在长期变化来看是可以利用的，实时性强。

角速度数据，加速度积分得到角度，但是由于传感器误差，积分作用会造成累计误差，所以角速度数据在短期是可以使用的，长期来看误差会很大是不可使用的。

数字低通滤波器

可以让长期变化通过(低频)，滤波消除短期变化(高频)，用于对加速度计的滤波。

数字高通滤波器

可以让短时信号通过(低频)，过滤出变化缓慢的信号(高频)，消除漂移。

我们截取陀螺仪解算数据的高频部分和加速度解算数据的低频部分进行融合，形成一个类似与模电的带通滤波器。

简单点就是陀螺仪低频不彳亍，我只要你的高频(短期)信号；
加速度的高频太抖了不彳亍，我就拿你的变化做个修正就OKOKOK了；
代码部分：

        mpu_dmp_get_data(&pitch,&roll,&yaw);     //得到姿态角即欧拉角MPU_Get_Accelerometer(&aacx,&aacy,&aacz);    //得到加速度传感器数据MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);    //得到陀螺仪数据     Acc_Pitch=atan((float)aacx/(float)aacz)*57.297;Gyro_Pitch=(gyrox/16.4)*0.01;             //读取间隔为10ms转换为0.01s计算，读取数值与实际值相反加负号Merge_Pitch=(Last_data+Gyro_Pitch)*(1-a)+a*Acc_Pitch;Last_data=Merge_Pitch;printf("%f    %f\r\n",pitch,-Merge_Pitch);

MergePitch=(Lastdata+GyroPitch)∗(1−a)+a∗AccPitchMergePitch=(Lastdata+GyroPitch)*(1-a)+a*AccPitchMergePitch=(Lastdata+GyroPitch)∗(1−a)+a∗AccPitch

融合结果=(上次融合值+角速度一个采样周期的积分值)∗(1−a)+a∗加速度解算值融合结果=(上次融合值+角速度一个采样周期的积分值)*(1-a)+a*加速度解算值融合结果=(上次融合值+角速度一个采样周期的积分值)∗(1−a)+a∗加速度解算值
从代码中看出，角速度长时间容易飘移，我们就去一个周期的变化(高频)
加速度太抖，我们就拿你当成数据的修正(抑制高频)，对数据进行更新。

蓝色为DMPDMPDMP处理数据
红色为互补滤波融合后的数据
对比DMPDMPDMP，互补滤波的平滑性更好，同时也具备灵敏度与实时性。

MPU6050加速度、角速度的解算以及互补滤波使用相关推荐

stm32 MPU6050 姿态解算 Mahony互补滤波算法
文章目录 0.介绍 1,理论分析 1.1 MPU6050 1.2 Mahony算法原理 2,代码实现 1.1 MPU6050初始化及数据读取 1.2 Mahony算法c语言实现 1.3 将代码移植到你 ...
MPU6050介绍及姿态解算
1.介绍:MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件,相较于多组件方案,免除了组合陀螺仪与加速器时之轴间差的问题,减少了安装空间. (1)绕X轴旋转角度为r ...
四旋翼无人机飞控系统设计(姿态解算)
姿态解算姿态传感器读出加速度和角速度,而对一个系统的自动控制往往需要更加上层和贴近应用的的一个属性:角度.所以需要通过加速度和角速度进行数据融合转化得到姿态角度. 以MPU6050为例,姿态 ...
Arduino 与 MPU6050 姿态解算+ PROCESSING
2019独角兽企业重金招聘Python工程师标准>>> 买的MPU6050自带姿态解算大大减轻了上层处理器所做的工作. 通过熟悉了一下processing之后做了一个小例子更是感觉这 ...
ESP-12F驱动MPU6050使用DMP库姿态解算
一.准备工作主芯片采用是安信可科技的ESF-12F模组(内置ESP8266芯片),使用GPIO模拟I2C驱动MPU6050,i2c驱动部分可直接参考官方例程中的i2c_master.c文件,(附件提 ...
TLE5012B磁编码器解算角度和角速度
1. 写本文是为了记录调试该模块所遇到的问题以及解决办法,同时是为了方便日后回顾. 2. SPI通信和SSC通信 TLE5012B编码器的通信方式为SSC通信,兼容SPI通信,网上已经有很多兼容配置方 ...
详解几种飞控的姿态解算算法
姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...
bvp解算器是什么_几种飞控的姿态解算算法
姿态解算是飞控的一个基础.重要部分,估计出来的姿态会发布给姿态控制器,控制飞行平稳,是飞行稳定的最重要保障.有关姿态解算的基础知识,这里笔者不会细细描述,有关这方面的资料,网上已经有很多.主要是先掌握 ...
【灵动MM32-姿态角解算】移植MPU6050-DMP库实现姿态角PRY解算
MM32使用DMP库处理MPU6050数据.姿态解算实现途径: ①MM32F3277G9P单片机 ②MPU6050模块 ③智能车逐飞MM32开源库 ④正点原子dmp开源库开源库链接: 逐飞科技MM ...

MPU6050加速度、角速度的解算以及互补滤波使用

MPU6050加速度、角速度的解算以及互补滤波使用

MPU6050加速度的解算

MPU6050角速度的解算

互补滤波使用（数据融合）

数字低通滤波器

数字高通滤波器

MPU6050加速度、角速度的解算以及互补滤波使用相关推荐

最新文章

热门文章