大致流程:首先用 RT-Thread 的 icm20608 軟件包讀取 陀螺儀 (Gyroscope) 和 加速度計(jì) (Accelerometer) 的數(shù)據(jù),分別計(jì)算出估計(jì)的角度,再用互補(bǔ)濾波器 (Complementary Filter) 融合兩個(gè)角度估計(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)
我們先介紹下從 I2C 總線讀取出傳感器原始數(shù)值后,如何處理得到加速度和旋轉(zhuǎn)角速度。一個(gè)六軸的 IMU 可以測(cè)量 x, y, z 三個(gè)方向的重力加速度,和繞三個(gè)軸的旋轉(zhuǎn)角速度。比如,開發(fā)板如果靜止放置在桌面上,會(huì)測(cè)量到 z 方向的重力加速度。三個(gè)軸的加速度
當(dāng)然,如果開發(fā)板靜止不動(dòng),繞三個(gè)軸的旋轉(zhuǎn)速度都是 0。
三個(gè)軸的旋轉(zhuǎn)角速度
由于傳感器的輸出實(shí)際上是來自 ADC 的 16 位數(shù)字信號(hào),我們需要把它的單位轉(zhuǎn)換成重力加速度 g。例如,我們可以選擇測(cè)量范圍
,默認(rèn)是
,也就是把傳感器的 16 位輸出
映射到 [-2g, 2g),于是
也就是下面 icm20608 芯片手冊(cè)的 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)速度 的測(cè)量值,得到更準(zhǔn)確的姿態(tài)預(yù)測(cè)。
我們使用下面的圖中的坐標(biāo)系,繞 x 軸旋轉(zhuǎn)的角度為 roll,繞 y 軸的旋轉(zhuǎn)方向?yàn)?pitch,繞 z 軸旋轉(zhuǎn)方向?yàn)?yaw。逆時(shí)針旋轉(zhuǎn)為正,順時(shí)針旋轉(zhuǎn)為負(fù)。
陀螺儀估計(jì)姿態(tài)
陀螺儀測(cè)量的是瞬間的旋轉(zhuǎn)角速度,所以位置的估計(jì)其實(shí)就是時(shí)間的積分。例如,每過 100ms 測(cè)量一次旋轉(zhuǎn)速度,旋轉(zhuǎn)速度 x 時(shí)間 = 旋轉(zhuǎ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;
當(dāng)然,由于環(huán)境存在大量噪聲,陀螺儀測(cè)量數(shù)據(jù)會(huì)存在隨機(jī)的波動(dòng),這些噪聲經(jīng)過積分累積,最后會(huì)造成位置的漂移。比如下面這張圖,過了很長時(shí)間后,雖然開發(fā)板是靜止的,但是右邊的陀螺儀估計(jì)的位置,就無法回到原點(diǎn),這就是長時(shí)間的累計(jì)誤差造成的。加速度計(jì)估計(jì)姿態(tài)
加速度計(jì)不需要積分,我們可以直接對(duì)當(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 會(huì)出現(xiàn)累計(jì)積分誤差。
互補(bǔ)濾波器
我們需要結(jié)合2個(gè)測(cè)量值是因?yàn)椋盒D(zhuǎn)速度短時(shí)間內(nèi)比較準(zhǔn)確,但是由于環(huán)境的噪聲會(huì)產(chǎn)生一些隨機(jī)運(yùn)動(dòng),時(shí)間長了就會(huì)漂移,而加速度短時(shí)間內(nèi)不一定準(zhǔn)確,但是最終會(huì)維持穩(wěn)定。
于是我們就可以取長補(bǔ)短,線性疊加2個(gè)測(cè)量值的估計(jì),給出更準(zhǔn)確的估計(jì)。
// complementary filter
gx = gx * 0.96 + ax * 0.04;
gy = gy * 0.96 + ay * 0.04;
短時(shí)間內(nèi),我們相信陀螺儀測(cè)量的旋轉(zhuǎn)角速度 (權(quán)值: 0.96);長時(shí)間內(nèi),環(huán)境噪聲逐漸造成的漂移,由加速度計(jì)慢慢進(jìn)行矯正 (權(quán)值: 0.04)。
總結(jié)
最后總結(jié)一下,其實(shí)核心代碼一共就 7 行。我們先利用加速度求解姿態(tài),再利用旋轉(zhuǎn)角速度求解姿態(tài),最后用互補(bǔ)濾波器進(jìn)行一個(gè)線性疊加。
// 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
點(diǎn)擊閱讀原文查看近期賽事
原文標(biāo)題:RT-Thread 互補(bǔ)濾波器 (STM32 + 6 軸 IMU)
文章出處:【微信公眾號(hào):RTThread物聯(lián)網(wǎng)操作系統(tǒng)】歡迎添加關(guān)注!文章轉(zhuǎn)載請(qǐng)注明出處。
-
RT-Thread
+關(guān)注
關(guān)注
31文章
1239瀏覽量
39437
原文標(biāo)題:RT-Thread 互補(bǔ)濾波器 (STM32 + 6 軸 IMU)
文章出處:【微信號(hào):RTThread,微信公眾號(hào):RTThread物聯(lián)網(wǎng)操作系統(tǒng)】歡迎添加關(guān)注!文章轉(zhuǎn)載請(qǐng)注明出處。
發(fā)布評(píng)論請(qǐng)先 登錄
相關(guān)推薦
評(píng)論