STM32F1
????????本教程使用零知標準板（STM32F103RBT6）通過I2C驅動ICM20948九軸傳感器，實現姿態解算，并通過串口將數據實時發送至VOFA+上位機進行3D可視化。代碼基于開源庫修改優化，適合嵌入式及物聯網開發者。在基礎驅動上新增濾波參數優化，重點解決yaw值漂移問題，提供完整的參數調優方案和效果對比。
目錄
STM32F1
一、硬件準備
二、軟件環境搭建
三、核心代碼實現
四、VOFA+上位機配置
五、濾波參數優化與動態效果對比
六、效果演示
七、?效果驗證與結論
一、硬件準備
1.硬件清單
零知標準板（主控STM32F103RBT6）
ICM20948九軸傳感器模塊
USB轉串口模塊（用于調試和數據傳輸）
杜邦線若干
2.接線方式
| ICM20948引腳 | 零知開發板引腳 |
|---|---|
| VCC | 3.3V |
| GND | GND |
| SDA | A4 |
| SCL | A5 |
硬件連接圖
連接實物圖?
注意：確保I2C引腳正確，避免接反導致芯片損壞。
二、軟件環境搭建
開發環境
-
零知 IDE + 零知開發板支持包
-
所需庫文件:
? ? ? AHRSAlgorithms.cpp（姿態解算庫）
? ? ? ICM20948.cpp（傳感器驅動庫）
庫文件關鍵功能
-
AHRSAlgorithms.cpp
????????????????Madgwick/Mahony濾波算法
????????????????四元數實時輸出?
getQ()????????????????參數可調：
Kp、Ki、beta
-
ICM20948.cpp
????????????????I2C通信底層驅動
????????????????自動量程配置（加速度計±2/4/8/16g，陀螺儀±250/500/1000/2000dps）
????????????????磁力計初始化?
initAK09916()????????????????校準函數?
calibrateICM20948()
三、核心代碼實現
主程序框架?
(ICM20948_VOFA.ino)
/* ICM20948完整優化代碼 */
#include "AHRSAlgorithms.h"
#include "ICM20948.h"#define AHRS true
#define SerialDebug true int myLed = LED_BUILTIN;
ICM20948 myIMU;void setup() {pinMode(myLed, OUTPUT);digitalWrite(myLed, HIGH);Serial.begin(115200);Wire.begin();// 初始化與自檢if(myIMU.begin()) {Serial.println("ICM20948初始化成功");// 執行兩級校準myIMU.calibrateICM20948(myIMU.gyroBias, myIMU.accelBias);float magBias[3], magScale[3];myIMU.magCalICM20948(magBias, magScale);// 設置優化分辨率myIMU.getAres(); myIMU.getGres();myIMU.getMres();} else {Serial.println("傳感器初始化失敗!");while(1);}
}void loop() {// 數據讀取if (myIMU.readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01) {myIMU.readAccelData(myIMU.accelCount);myIMU.readGyroData(myIMU.gyroCount);myIMU.readMagData(myIMU.magCount);// 單位轉換myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes - myIMU.magBias[0];myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes - myIMU.magBias[1];myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes - myIMU.magBias[2];}// 更新時間基準myIMU.updateTime();// 姿態解算（使用優化參數）MahonyQuaternionUpdate(myIMU.ax, myIMU.ay, myIMU.az,myIMU.gx * DEG_TO_RAD, myIMU.gy * DEG_TO_RAD,myIMU.gz * DEG_TO_RAD,myIMU.my, myIMU.mx, myIMU.mz, // 軸序修正myIMU.deltat);// 轉換為歐拉角const float* q = getQ();myIMU.yaw = atan2(2.0f*(q[1]*q[2] + q[0]*q[3]), q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) * RAD_TO_DEG;myIMU.pitch = -asin(2.0f*(q[1]*q[3] - q[0]*q[2])) * RAD_TO_DEG;myIMU.roll = atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) * RAD_TO_DEG;// 發送到VOFA+Serial.print(myIMU.yaw, 1); // yawSerial.print(",");Serial.print(myIMU.pitch, 1); // pitchSerial.print(",");Serial.println(myIMU.roll, 1);// rolldelay(10); // 100Hz輸出
}?關鍵配置修改
?在?ICM20948.cpp?中調整量程（根據應用需求）：
// 加速度計量程 (AFS_2G/AFS_4G/AFS_8G/AFS_16G)
void ICM20948::getAres()
{switch (Ascale){// Possible accelerometer scales (and their register bit settings) are:// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that// 2-bit value:case AFS_2G:aRes = 2.0f / 32768.0f;break;case AFS_4G:aRes = 4.0f / 32768.0f;break;case AFS_8G:aRes = 8.0f / 32768.0f;break;case AFS_16G:aRes = 16.0f / 32768.0f;break;}
}// 陀螺儀量程 (GFS_250DPS/GFS_500DPS/GFS_1000DPS/GFS_2000DPS)
void ICM20948::getGres()
{switch (Gscale){// Possible gyro scales (and their register bit settings) are:// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that// 2-bit value:case GFS_250DPS:gRes = 250.0f / 32768.0f;break;case GFS_500DPS:gRes = 500.0f / 32768.0f;break;case GFS_1000DPS:gRes = 1000.0f / 32768.0f;break;case GFS_2000DPS:gRes = 2000.0f / 32768.0f;break;}
}四、VOFA+上位機配置
數據協議設置
選擇?FireWater?協議
格式：
q0, q1, q2（逗號分隔+換行符）波特率：115200
控件添加
3D立方體：顯示實時姿態、綁定四元數數據通道、設置模型縮放比例
波形圖：各軸角速度/加速度
儀表盤：顯示偏航角（Yaw）
界面效果
實時顯示傳感器3D姿態及運動波形
五、濾波參數優化與動態效果對比
1.傳感器校準
float gyroBias[3], accelBias[3];
IMU.calibrateICM20948(gyroBias, accelBias); // 上電時執行一次2.問題現象
使用默認參數(Kp=10.0, Ki=0.0)時，VOFA+顯示yaw值持續漂移（約2-5°/s），動態運動時零漂明顯
3.優化方案：
?在AHRSAlgorithms.h中調整Mahony濾波參數：
// 原參數（漂移明顯）
// #define Kp 2.0f * 5.0f
// #define Ki 0.0f// 優化參數（大幅改善漂移）
#define Kp 3.0f // 降低比例增益，減少高頻噪聲響應
#define Ki 0.1f // 降低積分增益，抑制累積誤差效果對比：
| 參數狀態 | Yaw漂移率 | VOFA+動態表現 |
|---|---|---|
| 默認(Kp=10f,Ki=0.0f) | 2-5°/s | 靜止時緩慢旋轉，運動后復位慢 |
| 優化(Kp=3.0f,Ki=0.1f) | <0.5°/s | 靜止穩定，運動后快速收斂 |
4.優化后效果
參數調整原理：
Kp過高：對加速度計噪聲敏感，導致高頻抖動
Ki過高：積分累積誤差引起零漂黃金比例：
Kp/Ki ≈ 20-30?時平衡動態響應與穩定性
六、效果演示
靜態測試
傳感器平放時，VOFA+顯示俯仰角/橫滾角接近0°
Z軸加速度≈9.8 m/s2
動態測試
旋轉開發板，3D模型同步跟隨
快速晃動時波形圖顯示各軸加速度變化
ICM20948九軸傳感器動態測試
輸出速率調優
ICM20948原始數據輸出率約100Hz(10ms/次)
當
delt_t=60ms時，姿態解算循環(16.7Hz)與傳感器更新周期不同步導致部分數據幀被重復使用或跳過
完整工程代碼
百度網盤獲取完整工程文件，鏈接如下:
零知標準板驅動ICM20948項目工程
https://pan.baidu.com/s/11tr8XJvNrNernqwK1zA9Mw?pwd=pbxd?
七、?效果驗證與結論
測試結果
| 指標 | 優化前 | 優化后 |
|---|---|---|
| 靜態yaw漂移 | 2-5°/s | <0.5°/s |
| 動態收斂時間 | >3s | <1s |
| 高溫穩定性 | 漂移增加300% | 漂移增加<50% |
結論：
- 通過調整
Kp/Ki比例可有效抑制yaw漂移- 磁力計軸序修正提升方位角精度
- VOFA+可視化提供直觀參數調優依據
- 三階段校準確保全溫度范圍穩定性
??(●'?'●)
有關本篇博客的任何問題、或者任何想法和建議等，歡迎您在底部評論區留言，一起交流~
零知開源是一個真正屬于國人自己的開源軟硬件平臺，在開發效率上超越了Arduino平臺并且更加容易上手，大大降低了開發難度。
零知開源在軟件方面提供了完整的學習教程和豐富示例代碼，讓不懂程序的工程師也能非常輕而易舉的搭建電路來創作產品，測試產品。快來動手試試吧！
www.lingzhilab.com
