您现在的位置是:首页 > 传感器

简单的mems陀螺仪姿态算法介绍

2023-08-08 02:08:15

? ? 测试传感器:MPU9250,九轴传感器,其中有三个轴就是陀螺仪的三个方向角速度。

  陀螺仪在每个采样点获得:采样时刻(单位微妙),XYZ三个方向的角速度(单位弧度/秒),记为:wx, wy, wz。陀螺仪静止时,wx, wy, wz也是有读数的,这就是陀螺仪的零漂。

  实验一:将陀螺仪绕X轴旋转时,只有wx有读数;将陀螺仪绕Y轴旋转时,只有wy有读数;将陀螺仪绕Z轴旋转时,只有wz有读数;

  实验二:将陀螺仪绕XY面上的轴旋转,wz读数为零,即与旋转轴垂直的轴上的角速度为零。

  因为陀螺仪采样率很高(1000Hz),通过瞬时读数计算姿态,可以看做:先绕X轴旋转,再绕Y轴旋转,再绕Z轴旋转。

  下面这段代码实现了一个简单的陀螺仪姿态算法,开机并静置几十秒后,拿着陀螺仪旋转,十几分钟内姿态是正确的,之后由于积分累积,误差就越来越大了。
? ? ? ??// 参数说明:

// sampleTS? ?: 采样时刻,单位:微秒