MPU6050数据漂移问题讨论

哈喽哈嘿

在处理MPU6050的数据时候，采用的是https://github.com/gabrielbvicari/esp32-mpu6050这个开源库，这个关于MPU6050的库好像是用在一个穿戴设备上的，库里提供了很多函数，但是都是获取原始数据的，并没有直接处理成欧拉角（pitch,roll,yaw）的。
还有一个mpu6050_madgwick_quaternion_update()函数，这个函数是将accl三轴数据和gyro三轴数据解算成四元数，这个函数在开源库里的例程中有使用到，但是并没有利用其数据，经过一些带入数据的测试，输出的四元数始终不正常，一直在波动，也不能成功输出合适的欧拉角数据。再加上例程里没有使用的原因，所以最后决定启用这个函数。
目前使用库里的获取MPU6050中的三轴加速度计和陀螺仪原始数据，自己写了一个处理成欧拉角的函数，处理的时间大概是5ms左右，但是yaw会漂移，大概2分钟会漂移4度

关于减小误差和四元数解算的方法，大家可以一起讨论一下。
代码先放在https://github.com/cuizhongren45/fishbot-mpu6050这里，有兴趣的朋友可以讨论指点一下。!

小鱼

@哈喽哈嘿刚刚下载下来跑一下，我这边几乎两秒钟飘一度

I (149240) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.174
I (149246) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.181
I (149253) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.187
I (149259) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.194
I (149265) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.200
I (149271) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.207
I (149277) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.213
I (149283) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.219
I (149289) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.226
I (149295) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.232
I (149301) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.239
I (149307) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.245
I (149313) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.252
I (149319) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.258
I (149325) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.264
I (149332) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.271
I (149338) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.277
I (149344) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.284
I (149350) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.290
I (149356) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.296
I (149362) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.303
I (149368) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.309
I (149374) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.316
I (149380) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.322
I (149386) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.329
I (149392) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.335
I (149398) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.341
I (149404) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.348
I (149411) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.354
I (149417) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.361
I (149423) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.367
I (149429) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.374
I (149435) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.380
I (149441) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.386
I (149447) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.393
I (149453) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.399
I (149459) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.406
I (149465) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.412
I (149471) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.419
I (149477) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.425
I (149483) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.431
I (149490) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.438
I (149496) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.444
I (149502) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.451
I (149508) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.457
I (149514) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.464
I (149520) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.470
I (149526) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.476
I (149532) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.483
I (149538) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.489
I (149544) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.496
I (149550) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.502
I (149556) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.508
I (149562) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.515
I (149569) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.521
I (149575) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.528
I (149581) LED: led task runing..
I (149582) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.534
I (149591) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.541
I (149597) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.547
I (149603) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.553
I (149609) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.560
I (149615) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.566
I (149621) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.573
I (149627) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.579
I (149633) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.586
I (149639) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.592
I (149645) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.598
I (149652) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.605
I (149658) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.611
I (149664) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.618
I (149670) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.624
I (149676) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.631
I (149682) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.637
I (149688) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.643
I (149694) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.650
I (149700) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.656
I (149706) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.663
I (149712) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.669
I (149718) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.675
I (149724) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.682
I (149731) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.688
I (149737) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.695
I (149743) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.701
I (149749) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.708
I (149755) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.714
I (149761) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.720
I (149767) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.727
I (149773) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.733
I (149779) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.740
I (149785) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.746
I (149791) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.753
I (149797) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.759
I (149804) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.765
I (149810) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.772
I (149816) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.778
I (149822) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.785
I (149828) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.791
I (149834) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.798
I (149840) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.804
I (149846) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.810
I (149852) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.817
I (149858) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.823
I (149864) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.830
I (149870) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.836
I (149876) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.843
I (149882) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.849
I (149888) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.855
I (149895) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.862
I (149901) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.868
I (149907) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.875
I (149913) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.881
I (149919) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.887
I (149925) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.894
I (149931) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.900
I (149937) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.907
I (149943) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.913
I (149949) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.920
I (149955) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.926
I (149961) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.932
I (149967) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.939
I (149974) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.945
I (149980) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.952
I (149986) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.958
I (149992) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.965
I (149998) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.971
I (150004) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.977
I (150010) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.984
I (150016) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.990
I (150022) MPU6050: pitch:-0.405,roll:-0.464,yaw:-145.997
I (150028) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.003
I (150034) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.010
I (150040) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.016
I (150046) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.022
I (150053) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.029
I (150059) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.035
I (150065) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.042
I (150071) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.048
I (150077) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.055
I (150083) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.061
I (150089) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.067
I (150095) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.074
I (150101) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.080
I (150107) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.087
I (150113) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.093
I (150119) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.099
I (150125) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.106
I (150132) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.112
I (150138) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.119
I (150144) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.125
I (150150) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.132
I (150156) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.138
I (150162) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.144
I (150168) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.151
I (150174) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.157
I (150180) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.164
I (150186) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.170
I (150192) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.177
I (150198) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.183
I (150204) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.189
I (150211) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.196
I (150217) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.202
I (150223) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.209
I (150229) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.215
I (150235) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.222
I (150241) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.228
I (150247) MPU6050: pitch:-0.405,roll:-0.464,yaw:-146.234

小鱼

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

 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的驱动，目前器件还没到，希望器件正常可以使用。