MPU6050数据漂移问题讨论

小鱼

@小鱼另外好像没有看到有初始化自校准的部分代码，是否有加上去试试

 mpu6050_self_test(self_test);
    ESP_LOGI(mpu6050_get_tag(), "Device performing self-test.");

    if (self_test[0] < 1.0f && self_test[1] < 1.0f && self_test[2] < 1.0f &&
        self_test[3] < 1.0f && self_test[4] < 1.0f && self_test[5] < 1.0f) {
        mpu6050_reset();
        mpu6050_calibrate(accel_bias, gyro_bias);
        ESP_LOGI(mpu6050_get_tag(), "Device being calibrated.");
        mpu6050_init();
        ESP_LOGI(mpu6050_get_tag(), "Device initialized.");
        xTaskCreate(step_counter, "StepCounter", 8192, NULL, 1, NULL);
    }

哈喽哈嘿

@小鱼 2秒钟1度也太不可靠了，我再试试是不是代码哪里写错了，再看看其他开源库的写法

小鱼

@哈喽哈嘿我也一起找找测试下，看看有没有做的比较好的，数据读取应该没问题。

哈喽哈嘿

@小鱼确实self_test应该加进去的，关于calibrate它执行的时间比较长，一边要读取数据，另外一边还要写入数据，我就把写入数据那一部分省去了，使得初始化快了很多，效果也没变化

小鱼

@哈喽哈嘿刚测试了下，calibration时间确实太长了，一直没有初始化完成。

哈喽哈嘿

@小鱼那个里面有个packet_count，我的读出来是65535/12，就是满的，我觉得就是循环取平均，我后面改成100了

小鱼

@哈喽哈嘿对的，AHRS相关的开源库可以再试试，卡尔曼估计比较消耗计算。

这里有提供代码可以参考：

哈喽哈嘿

此回复已被删除！

哈喽哈嘿

@小鱼 https://github.com/cuizhongren45/fishbot-mpu6050
这里更新了代码。目前yaw的漂移大幅度减小，mpu6050正常工作前有3000次的取平均计算偏移，上电大概5s后进入正常工作状态，静态误差大幅度减小，5min漂移一度，甚至更加稳定

小鱼

@哈喽哈嘿点赞，刚测试了下最新的代码，运动一会后停下来貌似飘得就厉害了，但刚启动的时候的确要稳定很多。

看了下代码，0.02这个系数是如何确定的，它对整体的计算有何影响。

 angleX = (0.98 * (angleX + gyroX * interval)) + (0.02 * angleAccX);
        angleY = (0.98 * (angleY + gyroY * interval)) + (0.02 * angleAccY);
        angleZ = angleGyroZ;

目前数据会出现大于2*pi的值

I (539616) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.895
I (539622) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.904
I (539628) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.913
I (539634) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.921
I (539640) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.930
I (539646) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.939
I (539652) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.948
I (539658) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.956
I (539664) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.965
I (539670) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.974
I (539676) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.982
I (539682) MPU6050: pitch:-0.159,roll:-0.049,yaw:-390.991

哈喽哈嘿

@小鱼这里我用的是github上MPU6050_tockn这个开源库里的计算公式，上面说采用的是互补滤波器来计算角度，以陀螺仪为主，权重0.98，加速度计权重0.02，当短时间内变化剧烈的时候，可能就不太准确了，不过应该还有优化的空间，采用滤波或者是变化的权重值，应该都可以增加yaw鲁棒性，至于会出现大于2PI的值，yaw是累计增加的，固定在-180~180的话写个判断就能解决。

小鱼

@哈喽哈嘿感谢讲解，把角度范围解决后可以先合一个版本，你看看可以优化的地方持续优化下，这是个有挑战和有深度的问题，我们能尽力我们最大努力做好即可。

哈喽哈嘿

@小鱼我晚上刚看到你前面发的AHRS，我准备再试试AHRS这个库先

小鱼

@哈喽哈嘿也可以，这两天我在整电机和SDK，最快也要下周才能搞PCB，你先整着。

哈喽哈嘿

@小鱼又测试了一下AHRS，效果始终不太好，我转动MPU6050一个直角，yaw的变化只有六七十，感觉误差更大。角度范围问题已经解决了，代码还在那个地址上。

小鱼

@哈喽哈嘿可以，那目前先固定参数吧，完善下代码结构，提一个新的PR到v1.0分支，后续再优化（你可以换成mpu9050试试）

哈喽哈嘿

@小鱼好的，已经pr上去了

哈喽哈嘿

之前准备使用MPU9250来解决yaw角漂移的问题，就是用磁力计纠正yaw的漂移，但是经过一番代码的移植和调试，器件读数始终不正常，上淘宝一看，评论区里早已开火，好多人都说器件的问题，器件读出的ID和手册上对于不上，更别说读取数据了。
无奈，只得继续优化之前MPU6050的驱动，经过测试，之前的互补滤波在转动速度较快的时候，偏差较大，后又测试AHRS算法，其在转动较快的环境下，其鲁棒性相比于互补滤波好一些。关于零点漂移的问题仍然存在，不过经过AHRS和互补滤波的处理，零飘已经大大减小。最后融合两者，以0.3和0.7的权重算作最终的yaw值。这里添加一个测试视频的链接：https://www.bilibili.com/video/BV16v4y1c7nv?spm_id_from=444.41.list.card_archive.click&vd_source=b741c1764645507298e463258c43bbb6
后续也会继续更新9250的驱动，目前器件还没到，希望器件正常可以使用。

小鱼

@哈喽哈嘿赞赞赞，互补滤波被你玩明白了，卡尔曼滤波效果不知道如何，有时间也可以测测看

哈喽哈嘿

@小鱼确实，我之前看论文里有说到用卡尔曼滤波对acc和gyro滤波，可能让数据更好一些，不过卡尔曼的原理我还不太清楚，最近准备先补补理论知识，再用卡尔曼看看效果怎么样