大致流程:首先用 RT-Thread 的 icm20608 软件包读取 陀螺仪 (Gyroscope) 和 加速度计 (Accelerometer) 的数据,分别计算出估计的角度,再用互补滤波器 (Complementary Filter) 融合两个角度估计、进行校正,其实核心算法的代码就 7 行。最后串口把数据发到电脑上,用 Python + OpenGL 可视化。
IMU 传感器 (Inertial Measurement Unit)
当然,如果开发板静止不动,绕三个轴的旋转速度都是 0。
由于传感器的输出实际上是来自 ADC 的 16 位数字信号,我们需要把它的单位转换成重力加速度 g。例如,我们可以选择测量范围
,默认是
,也就是把传感器的 16 位输出
映射到 [-2g, 2g),于是
也就是下面 icm20608 芯片手册的 Sensitivity Scale Factor。
于是在代码里面,将原始的 int16 加速度数据除以 16384。
double aSensitivity = 16384;
accel_x = accel_x / aSensitivity;
accel_y = accel_y / aSensitivity;
accel_z = accel_z / aSensitivity;
同样,我们可以换算出角速度
于是在代码里面,将原始的 int16 角速度数据除以 131。
double gSensitivity = 131;
gyrX = gyro_x / gSensitivity;
gyrY = gyro_y / gSensitivity;
gyrZ = gyro_z / gSensitivity;
这样我们就把 ADC 输出的 int16 原始数据分布转换成了加速度单位 g,和旋转角速度单位 °/s.
互补滤波器 (Complementary Filter)
我们可以用 互补滤波器 结合 加速度 和 旋转速度 的测量值,得到更准确的姿态预测。
我们使用下面的图中的坐标系,绕 x 轴旋转的角度为 roll,绕 y 轴的旋转方向为 pitch,绕 z 轴旋转方向为 yaw。逆时针旋转为正,顺时针旋转为负。
陀螺仪估计姿态
// angles based on gyro (deg/s)
gx = gx + gyrX * TIME_STEP_MS / 1000;
gy = gy + gyrY * TIME_STEP_MS / 1000;
gz = gz + gyrZ * TIME_STEP_MS / 1000;
加速度计估计姿态
加速度计不需要积分,我们可以直接对当前加速度角度求 arctan 得到角度:
// angles based on accelerometer
ax = atan2(accelY, accelZ) * 180 / M_PI; // roll
ay = atan2(-accelX, sqrt( pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI; // pitch
不管我们的开发板绕 z 轴旋转多少度,重力加速度始终朝向地面。因此开发板静止状态,我们无法利用重力加速度知道 z 轴的旋转角度 (yaw),所以上面只计算 roll 和 pitch,最终 z 轴的旋转角度 yaw 会出现累计积分误差。
互补滤波器
我们需要结合2个测量值是因为:旋转速度短时间内比较准确,但是由于环境的噪声会产生一些随机运动,时间长了就会漂移,而加速度短时间内不一定准确,但是最终会维持稳定。
于是我们就可以取长补短,线性叠加2个测量值的估计,给出更准确的估计。
// complementary filter
gx = gx * 0.96 + ax * 0.04;
gy = gy * 0.96 + ay * 0.04;
短时间内,我们相信陀螺仪测量的旋转角速度 (权值: 0.96);长时间内,环境噪声逐渐造成的漂移,由加速度计慢慢进行矫正 (权值: 0.04)。
总结
最后总结一下,其实核心代码一共就 7 行。我们先利用加速度求解姿态,再利用旋转角速度求解姿态,最后用互补滤波器进行一个线性叠加。
// angles based on gyro (deg/s)
gx = gx + gyrX * TIME_STEP_MS / 1000;
gy = gy + gyrY * TIME_STEP_MS / 1000;
gz = gz + gyrZ * TIME_STEP_MS / 1000;
// angles based on accelerometer
ax = atan2(accelY, accelZ) * 180 / M_PI; // roll
ay = atan2(-accelX, sqrt( pow(accelY, 2) + pow(accelZ, 2))) * 180 / M_PI; // pitch
// complementary filter
gx = gx * 0.96 + ax * 0.04;
gy = gy * 0.96 + ay * 0.04;
References
https://github.com/mattzzw/Arduino-mpu6050
https://github.com/RT-Thread-pa
本文分享自微信公众号 - RTThread物联网操作系统(RTThread)。
如有侵权,请联系 support@oschina.cn 删除。
本文参与“OSC源创计划”,欢迎正在阅读的你也加入,一起分享。