搜索
bottom↓
回复: 3

MPU6050 四元数求得欧拉角 为什么会漂移

[复制链接]

出500入8汤圆

发表于 2016-5-24 22:11:16 | 显示全部楼层 |阅读模式
当固定MPU6050 一定角度时,融合后的Anglex,Angley会匀速变大或变小,大约10变化2度,平放时候为0不会发生变化。
void IMUupdate( float gx, float gy, float gz, float ax, float ay, float az )
{
        float        norm;
        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;

        float        q0y_q0y = q0_y * q0_y;
        float        q1y_q1y = q1_y * q1_y;
        float        q2y_q2y = q2_y * q2_y;
        float        q3y_q3y = q3_y * q3_y;
        float        q1y_q2y = q1_y * q2_y;
        float        q0y_q3y = q0_y * q3_y;

        /* Yaw轴四元素的微分方程 */
        q0_y        = q0_y + (-q1_y * gx - q2_y * gy - q3_y * gz) * halfT;
        q1_y        = q1_y + (q0_y * gx + q2_y * gz - q3_y * gy) * halfT;
        q2_y        = q2_y + (q0_y * gy - q1_y * gz + q3_y * gx) * halfT;
        q3_y        = q3_y + (q0_y * gz + q1_y * gy - q2_y * gx) * halfT;
        /* 规范化yaw */
        norm        = sqrt( q0y_q0y + q1y_q1y + q2y_q2y + q3y_q3y );
        q0_y        = q0_y / norm;
        q1_y        = q1_y / norm;
        q2_y        = q2_y / norm;
        q3_y        = q3_y / norm;

/* 规范化加速度计值 */
        norm        = sqrt( ax * ax + ay * ay + az * az );
        ax        = ax / norm;
        ay        = ay / norm;
        az        = az / norm;
/* 估计重力方向和流量/变迁 */
        vx        = 2 * (q1q3 - q0q2);
        vy        = 2 * (q0q1 + q2q3);
        vz        = q0q0 - q1q1 - q2q2 + q3q3;
/* 向量外积再相减得到差分就是误差 */
        ex        = (ay * vz - az * vy);
        ey        = (az * vx - ax * vz);
        ez        = (ax * vy - ay * vx);
/* 对误差进行PI计算 */
        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;
        /* 规范化Pitch、Roll轴四元数 */
        norm        = sqrt( q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 );
        q0        = q0 / norm;
        q1        = q1 / norm;
        q2        = q2 / norm;
        q3        = q3 / norm;

        Anglex        = atan2( 2 * q2q3 + 2 * q0q1, -2 * q1q1 - 2 * q2q2 + 1 ) * 57.2957795f;
        Angley        = asin( -2 * q1q3 + 2 * q0q2 ) * 57.2957795f;
        Anglez        = atan2( 2 * q1y_q2y + 2 * q0y_q3y, -2 * q2y_q2y - 2 * q3y_q3y + 1 ) * 57.2957795f;
}

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x

阿莫论坛20周年了!感谢大家的支持与爱护!!

一只鸟敢站在脆弱的枝条上歇脚,它依仗的不是枝条不会断,而是自己有翅膀,会飞。

出0入0汤圆

发表于 2016-5-24 23:04:39 | 显示全部楼层
你先想想两个问题:
1. 为什么会有Y方向的四元数?

2.下面的这个程序是否有错?
       /* 四元素的微分方程 */
        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;

出500入8汤圆

 楼主| 发表于 2016-5-25 07:03:18 | 显示全部楼层
zouyf12 发表于 2016-5-24 23:04
你先想想两个问题:
1. 为什么会有Y方向的四元数?

我参考的这个帖子:http://www.amobbs.com/thread-5492189-1-1.html   /* 四元素的微分方程 */这一块程序没看出错误来。
y轴先忽略,没作用

出0入0汤圆

发表于 2016-5-26 23:30:39 | 显示全部楼层
1. 以下程序,计算q1时的q0已经不是原来的q0了。
        q0        = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
        q1        = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;  

2.    gx        = gx + Kp * ex + exInt;
        gy        = gy + Kp * ey + eyInt;
        gz        = gz + Kp * ez + ezInt;
Kp 是否有赋值? 有的话加大看看效果。
回帖提示: 反政府言论将被立即封锁ID 在按“提交”前,请自问一下:我这样表达会给举报吗,会给自己惹麻烦吗? 另外:尽量不要使用Mark、顶等没有意义的回复。不得大量使用大字体和彩色字。【本论坛不允许直接上传手机拍摄图片,浪费大家下载带宽和论坛服务器空间,请压缩后(图片小于1兆)才上传。压缩方法可以在微信里面发给自己(不要勾选“原图),然后下载,就能得到压缩后的图片】。另外,手机版只能上传图片,要上传附件需要切换到电脑版(不需要使用电脑,手机上切换到电脑版就行,页面底部)。
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|amobbs.com 阿莫电子技术论坛 ( 粤ICP备2022115958号, 版权所有:东莞阿莫电子贸易商行 创办于2004年 (公安交互式论坛备案:44190002001997 ) )

GMT+8, 2024-3-29 15:06

© Since 2004 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

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