发 帖  
张飞软硬开源基于STM32 BLDC直流无刷电机驱动器开发视频套件, 👉戳此立抢👈
[经验] 谈一谈 MPU6050 姿态融合
2015-6-15 22:03:04  42269 MPU6050
分享
本帖最后由 tyw0403 于 2015-6-15 22:29 编辑

   

首先要明确,MPU6050 是一款姿态传感器,使用它就是为了得到待测物体(如四轴、平衡小车) x、y、z 轴的倾角(俯仰角 Pitch、滚转角 Roll、偏航角 Yaw) 。我们通过 I2C 读取到 MPU6050 的六个数据(三轴加速度 AD 值、三轴角速度 AD 值)经过姿态融合后就可以得到 Pitch、Roll、Yaw 角。

本帖主要介绍三种姿态融合算法:四元数法 、一阶互补算法和卡尔曼滤波算法。




一、四元数法

关于四元数的一些概念和计算就不写上来了,我也不懂。我能告诉你的是:通过下面的算法,可以把六个数据转化成四元数(q0、q1、q2、q3),然后四元数转化成欧拉角(P、R、Y 角)。



虽然 MPU6050 自带的 DMP库可以直接输出四元数,减轻 STM32 的运算负担, 这里在此没有使用,因为我是用 STM32 的硬件 I2C 读取 MPU6050 数据的(http://bbs.elecfans.com/forum.ph ... 4&page=1#pid3625735),DMP库需要对 I2C 函数进行修改,如 DMP 库中的 I2C 写:i2c_write(st.hw->addr, st.reg->pwr_mgmt_1, 1, &(data[0]));有4个输入变量,而 STM32 硬件 I2C 的 I2C 写为:void MPU6050_I2C_ByteWrite(u8 slaveAddr, u8 pBuffer, u8 writeAddr),只有 3 个输入量(这之间的差异好像是由于 MPU6050 的 DMP 库是针对 MSP430 单片机写的),所以必须进行修改,但是改固件库是一件很痛苦的事,你们应该都懂。当然,如果你用模拟 I2C 的话,是容易实现的,网上的 DMP 移植几乎都是基于模拟 I2C 的。


复制代码


#include<math.h>

#include "stm32f10x.h"

//---------------------------------------------------------------------------------------------------

// 变量定义


#define Kp 100.0f                        // 比例增益支配率收敛到加速度计/磁强计

#define Ki 0.002f                // 积分增益支配率的陀螺仪偏见的衔接

#define halfT 0.001f                // 采样周期的一半


float q0 = 1, q1 = 0, q2 = 0, q3 = 0;          // 四元数的元素,代表估计方向

float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差


float Yaw,Pitch,Roll;  //偏航角,俯仰角,翻滚角



void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

        float norm;

        float vx, vy, vz;

        float ex, ey, ez;  


        // 测量正常化

        norm = sqrt(ax*ax + ay*ay + az*az);      

        ax = ax / norm;                   //单位化

        ay = ay / norm;

        az = az / norm;      


        // 估计方向的重力

        vx = 2*(q1*q3 - q0*q2);

        vy = 2*(q0*q1 + q2*q3);

        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;


        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和

        ex = (ay*vz - az*vy);

        ey = (az*vx - ax*vz);

        ez = (ax*vy - ay*vx);


        // 积分误差比例积分增益

        exInt = exInt + ex*Ki;

        eyInt = eyInt + ey*Ki;

        ezInt = ezInt + ez*Ki;


        // 调整后的陀螺仪测量

        gx = gx + Kp*ex + exInt;

        gy = gy + Kp*ey + eyInt;

        gz = gz + Kp*ez + ezInt;


        // 整合四元数率和正常化

        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;  


        // 正常化四元

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

        q0 = q0 / norm;

        q1 = q1 / norm;

        q2 = q2 / norm;

        q3 = q3 / norm;


        Pitch  = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch ,转换为度数

        Roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // rollv

        //Yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;                //此处没有价值,注掉

}




要注意的的是,四元数算法输出的是三个量 Pitch、Roll 和 Yaw,运算量很大。而像平衡小车这样的例子只需要一个角(Pitch 或 Roll )就可以满足工作要求,个人觉得做平衡小车最好不用四元数法。




二、一阶互补算法

MPU6050 可以输出三轴的加速度和角速度。通过加速度和角速度都可以得到 Pitch 和 Roll 角(加速度不能得到 Yaw 角),就是说有两组 Pitch、Roll 角,到底应该选哪组呢?别急,先分析一下。MPU6050 的加速度计和陀螺仪各有优缺点,三轴的加速度值没有累积误差,且通过算 tan()  可以得到倾角,但是它包含的噪声太多(因为待测物运动时会产生加速度,电机运行时振动会产生加速度等),不能直接使用;陀螺仪对外界振动影响小,精度高,通过对角速度积分可以得到倾角,但是会产生累积误差。所以,不能单独使用 MPU6050 的加速度计或陀螺仪来得到倾角,需要互补。一阶互补算法的思想就是给加速度和陀螺仪不同的权值,把它们结合到一起,进行修正。得到 Pitch 角的程序如下:




复制代码


//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.001;//注意:dt的取值为滤波器采样时间

float angle;


angle_ax=atan(ax/az)*57.3;     //加速度得到的角度

gy=(float)gyo[1]/7510.0;       //陀螺仪得到的角速度

Pitch = yijiehubu(angle_ax,gy);


float yijiehubu(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

     angle = K1 * angle_m + (1-K1) * (angle + gyro_m * dt);

     return angle;

}




互补算法只能得到一个倾角,这在平衡车项目中够用了,而在四轴飞行器设计中还需要 Roll 和 Yaw,就需要两个 互补算法,我是这样写的,注意变量不要搞混:


复制代码


//一阶互补滤波

float K1 =0.1; // 对加速度计取值的权重

float dt=0.001;//注意:dt的取值为滤波器采样时间

float angle_P,angle_R;



float yijiehubu_P(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

     angle_P = K1 * angle_m + (1-K1) * (angle_P + gyro_m * dt);

         return angle_P;

}


float yijiehubu_R(float angle_m, float gyro_m)//采集后计算的角度和角加速度

{

     angle_R = K1 * angle_m + (1-K1) * (angle_R + gyro_m * dt);

         return angle_R;

}

单靠 MPU6050 无法准确得到 Yaw 角,需要和地磁传感器结合使用。






三、卡尔曼滤波

其实卡尔曼滤波和一阶互补有些相似,输入也是一样的。卡尔曼原理以及什么5个公式等等的,我也不太懂,就不写了,感兴趣的话可以上网查。在此给出具体程序,和一阶互补算法一样,每次卡尔曼滤波只能得到一个方向的角度。




复制代码




#include<math.h>

#include "stm32f10x.h"

#include "Kalman_Filter.h"




//卡尔曼滤波参数与函数

float dt=0.001;//注意:dt的取值为kalman滤波器采样时间

float angle, angle_dot;//角度和角速度

float P[2][2] = {{ 1, 0 },

                 { 0, 1 }};

float Pdot[4] ={ 0,0,0,0};

float Q_angle=0.001, Q_gyro=0.005; //角度数据置信度,角速度数据置信度

float R_angle=0.5 ,C_0 = 1;

float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;


//卡尔曼滤波

float Kalman_Filter(float angle_m, float gyro_m)//angleAx 和 gyroGy

{

        angle+=(gyro_m-q_bias) * dt;

        angle_err = angle_m - angle;

        Pdot[0]=Q_angle - P[0][1] - P[1][0];

        Pdot[1]=- P[1][1];

        Pdot[2]=- P[1][1];

        Pdot[3]=Q_gyro;

        P[0][0] += Pdot[0] * dt;

        P[0][1] += Pdot[1] * dt;

        P[1][0] += Pdot[2] * dt;

        P[1][1] += Pdot[3] * dt;

        PCt_0 = C_0 * P[0][0];

        PCt_1 = C_0 * P[1][0];

        E = R_angle + C_0 * PCt_0;

        K_0 = PCt_0 / E;

        K_1 = PCt_1 / E;

        t_0 = PCt_0;

        t_1 = C_0 * P[0][1];

        P[0][0] -= K_0 * t_0;

        P[0][1] -= K_0 * t_1;

        P[1][0] -= K_1 * t_0;

        P[1][1] -= K_1 * t_1;

        angle += K_0 * angle_err; //最优角度

        q_bias += K_1 * angle_err;

        angle_dot = gyro_m-q_bias;//最优角速度


        return angle;

}






作个总结:三种融合算法都能够输出姿态角(Pitch 和 Roll ),一次四元数法可以输出 P、R、Y 三个倾角,计算量比较大。一阶互补和卡尔曼滤波每次只能输出一个轴的姿态角。



注:使用“匿名地面站”上位机软件,给调试过程带来了很大便捷。软件可在附件中下载。

通过上位机可以看到,姿态融合成功!




1.jpg



附件:上位机软件(无需安装,使用简单方便)。

匿名地面站.zip (4.76 MB, 下载次数: 5792)







PS:请问如何添加代码文字?

73
2015-6-15 22:03:04   评论 分享淘帖
171 个讨论
谢谢分享。。。。
2015-6-16 10:00:46 评论

举报

很好的资料,谢谢楼主的分享!!!!
2015-6-16 16:55:11 评论

举报

00000000000正在弄
2015-6-17 20:04:29 评论

举报

Thankyou,LZ!请问你这个地面站怎么通讯工作的?
2015-7-2 14:02:59 评论

举报

cheng920727 发表于 2015-7-2 14:02
Thankyou,LZ!请问你这个地面站怎么通讯工作的?

看看视频教程。
『匿名四轴』--地面站讲解0513--总体介绍
http://www.tudou.com/programs/view/4tyajcXBWLU/


『匿名四轴』--地面站讲解0513--基本收发+高级收码
http://www.tudou.com/programs/view/lEjU0VjP3y4/


『匿名四轴』--地面站讲解0513--波形显示
http://www.tudou.com/programs/view/Y8fqutrQpMo/


『匿名四轴』--地面站讲解0513--二维波形
http://www.tudou.com/programs/view/IPJ3u0L3_ks/


『匿名四轴』--地面站讲解0513--状态显示
http://www.tudou.com/programs/view/QvCWglriJQs/


『匿名四轴』--地面站讲解0513--飞控设置
http://www.tudou.com/programs/view/oWiOjcxSW9g/


『匿名四轴』--地面站讲解0513--地面站
http://www.tudou.com/programs/view/6sIKUdAtuVU/



『匿名四轴』--地面站讲解0513--飞行控制
http://www.tudou.com/programs/view/Vu2MRuIjEiI/


『匿名四轴』--地面站讲解0513--Excel写入
http://www.tudou.com/programs/view/Apta_cbUXxA/


2015-7-2 17:18:59 评论

举报

谢谢楼主的分享, 嘻嘻         
2015-7-5 14:51:34 评论

举报

真是及时雨啊,讲的很不错
2015-7-18 11:19:33 评论

举报


谢谢分享。。。。
2015-7-22 13:48:12 评论

举报


谢谢分享。。。。
2015-7-22 13:48:40 评论

举报

lz很棒。。。。。。。。。。。。。。。。。
2015-7-22 16:30:59 评论

举报

lz写的很棒、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、、
2015-7-22 16:31:53 评论

举报

非常棒 赞一个R5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEAR5F100LEA
谢谢分享,,,,学习了
2015-7-23 14:29:38 评论

举报

很不错的分享!!!!!!
2015-7-24 09:04:53 评论

举报

markmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmarkmark
2015-7-28 14:53:22 评论

举报

很好,顶!!!!!!!
2015-8-2 09:19:09 评论

举报

看看这个 匿名地面站 这个工具挺好啊,以前我自己用c#写的,比这个简单很多,重复造轮子了
2015-8-2 15:08:49 评论

举报

谢谢分享!!!
2015-8-2 15:19:11 评论

举报

51刚学完,想做一个平衡车,什么卡尔曼,互补看的一头雾水,看了这个帖子后明白了很多基本的事情,谢谢分享
2015-8-3 09:42:49 评论

举报

高级模式
您需要登录后才可以回帖 登录 | 注册

发表新帖
课程
    关闭

    站长推荐 上一条 /8 下一条

    快速回复 返回顶部 返回列表