大致流程：首先用 RT-Thread 的 icm20608 軟件包讀取 陀螺儀 (Gyroscope) 和 加速度計(jì) (Accelerometer) 的數(shù)據(jù)，分別計(jì)算出估計(jì)的角度，再用互補(bǔ)濾波器 (Complementary Filter) 融合兩個角度估計(jì)、進(jìn)行校正，其實(shí)核心算法的代碼就 7 行。最后串口把數(shù)據(jù)發(fā)到電腦上，用 Python + OpenGL 可視化。

Github - STM32 IMU 互補(bǔ)濾波器 (RT-Thread)：https://github.com/wuhanstudio/stm32-imu-filter

IMU 傳感器 (Inertial Measurement Unit)

當(dāng)然，如果開發(fā)板靜止不動，繞三個軸的旋轉(zhuǎn)速度都是 0。

由于傳感器的輸出實(shí)際上是來自 ADC 的 16 位數(shù)字信號，我們需要把它的單位轉(zhuǎn)換成重力加速度 g。例如，我們可以選擇測量范圍

，默認(rèn)是

，也就是把傳感器的 16 位輸出

映射到 [-2g, 2g)，于是

也就是下面 icm20608 芯片手冊的 Sensitivity Scale Factor。

于是在代碼里面，將原始的 int16 加速度數(shù)據(jù)除以 16384。

double aSensitivity = 16384;

accel_x = accel_x / aSensitivity;
accel_y = accel_y / aSensitivity;
accel_z = accel_z / aSensitivity;

同樣，我們可以換算出角速度

于是在代碼里面，將原始的 int16 角速度數(shù)據(jù)除以 131。

double gSensitivity = 131;

gyrX = gyro_x / gSensitivity;
gyrY = gyro_y / gSensitivity;
gyrZ = gyro_z / gSensitivity;

這樣我們就把 ADC 輸出的 int16 原始數(shù)據(jù)分布轉(zhuǎn)換成了加速度單位 g，和旋轉(zhuǎn)角速度單位 °/s.

互補(bǔ)濾波器 (Complementary Filter)

我們可以用 互補(bǔ)濾波器 結(jié)合 加速度 和 旋轉(zhuǎn)速度 的測量值，得到更準(zhǔn)確的姿態(tài)預(yù)測。

我們使用下面的圖中的坐標(biāo)系，繞 x 軸旋轉(zhuǎn)的角度為 roll，繞 y 軸的旋轉(zhuǎn)方向?yàn)?pitch，繞 z 軸旋轉(zhuǎn)方向?yàn)?yaw。逆時針旋轉(zhuǎn)為正，順時針旋轉(zhuǎn)為負(fù)。

陀螺儀估計(jì)姿態(tài)

// 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;

加速度計(jì)估計(jì)姿態(tài)

加速度計(jì)不需要積分，我們可以直接對當(dāng)前加速度角度求 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

不管我們的開發(fā)板繞 z 軸旋轉(zhuǎn)多少度，重力加速度始終朝向地面。因此開發(fā)板靜止?fàn)顟B(tài)，我們無法利用重力加速度知道 z 軸的旋轉(zhuǎn)角度 (yaw)，所以上面只計(jì)算 roll 和 pitch，最終 z 軸的旋轉(zhuǎn)角度 yaw 會出現(xiàn)累計(jì)積分誤差。

互補(bǔ)濾波器

我們需要結(jié)合2個測量值是因?yàn)椋盒D(zhuǎn)速度短時間內(nèi)比較準(zhǔn)確，但是由于環(huán)境的噪聲會產(chǎn)生一些隨機(jī)運(yùn)動，時間長了就會漂移，而加速度短時間內(nèi)不一定準(zhǔn)確，但是最終會維持穩(wěn)定。

于是我們就可以取長補(bǔ)短，線性疊加2個測量值的估計(jì)，給出更準(zhǔn)確的估計(jì)。

// complementary filter
gx = gx * 0.96 + ax * 0.04;
gy = gy * 0.96 + ay * 0.04;

短時間內(nèi)，我們相信陀螺儀測量的旋轉(zhuǎn)角速度 (權(quán)值: 0.96)；長時間內(nèi)，環(huán)境噪聲逐漸造成的漂移，由加速度計(jì)慢慢進(jìn)行矯正 (權(quán)值: 0.04)。

總結(jié)

最后總結(jié)一下，其實(shí)核心代碼一共就 7 行。我們先利用加速度求解姿態(tài)，再利用旋轉(zhuǎn)角速度求解姿態(tài)，最后用互補(bǔ)濾波器進(jìn)行一個線性疊加。

// 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