STM32F1

本教程使用零知標(biāo)準(zhǔn)板（STM32F103RBT6）通過I2C驅(qū)動(dòng)ICM20948九軸傳感器，實(shí)現(xiàn)姿態(tài)解算，并通過串口將數(shù)據(jù)實(shí)時(shí)發(fā)送至VOFA+上位機(jī)進(jìn)行3D可視化。代碼基于開源庫修改優(yōu)化，適合嵌入式及物聯(lián)網(wǎng)開發(fā)者。在基礎(chǔ)驅(qū)動(dòng)上新增 本教程使用零知標(biāo)準(zhǔn)板（STM32F103RBT6）通過I2C驅(qū)動(dòng)ICM20948九軸傳感器，實(shí)現(xiàn)姿態(tài)解算，并通過串口將數(shù)據(jù)實(shí)時(shí)發(fā)送至VOFA+上位機(jī)進(jìn)行3D可視化。代碼基于開源庫修改優(yōu)化，適合嵌入式及物聯(lián)網(wǎng)開發(fā)者。在基礎(chǔ)驅(qū)動(dòng)上新增濾波參數(shù)優(yōu)化，重點(diǎn)解決yaw值漂移問題，提供完整的參數(shù)調(diào)優(yōu)方案和效果對比。，重點(diǎn)解決yaw值漂移問題，提供完整的參數(shù)調(diào)優(yōu)方案和效果對比。

一、硬件準(zhǔn)備

1.硬件清單

零知標(biāo)準(zhǔn)板（主控STM32F103RBT6）

ICM20948九軸傳感器模塊

USB轉(zhuǎn)串口模塊（用于調(diào)試和數(shù)據(jù)傳輸）

杜邦線若干

2.接線方式

硬件連接圖

連接實(shí)物圖

注意：確保I2C引腳正確，避免接反導(dǎo)致芯片損壞。

二、軟件環(huán)境搭建

開發(fā)環(huán)境

零知 IDE + 零知開發(fā)板支持包

所需庫文件:

AHRSAlgorithms.cpp（姿態(tài)解算庫）

ICM20948.cpp（傳感器驅(qū)動(dòng)庫）

庫文件關(guān)鍵功能

AHRSAlgorithms.cpp

Madgwick/Mahony濾波算法

四元數(shù)實(shí)時(shí)輸出getQ()

參數(shù)可調(diào)：Kp、Ki、beta

ICM20948.cpp

I2C通信底層驅(qū)動(dòng)

自動(dòng)量程配置（加速度計(jì)±2/4/8/16g，陀螺儀±250/500/1000/2000dps）

磁力計(jì)初始化initAK09916()

校準(zhǔn)函數(shù)calibrateICM20948()

三、核心代碼實(shí)現(xiàn)

主程序框架

((ICM20948_VOFA.ino))

/* ICM20948完整優(yōu)化代碼 */
#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初始化成功");
    
    // 執(zhí)行兩級(jí)校準(zhǔn)
    myIMU.calibrateICM20948(myIMU.gyroBias, myIMU.accelBias);
    float magBias[3], magScale[3];
    myIMU.magCalICM20948(magBias, magScale);
    
    // 設(shè)置優(yōu)化分辨率
    myIMU.getAres(); 
    myIMU.getGres();
    myIMU.getMres();
  } else {
    Serial.println("傳感器初始化失敗!");
    while(1);
  }
}

void loop() {
  // 數(shù)據(jù)讀取
  if (myIMU.readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01) {
    myIMU.readAccelData(myIMU.accelCount);
    myIMU.readGyroData(myIMU.gyroCount);
    myIMU.readMagData(myIMU.magCount);
    
    // 單位轉(zhuǎn)換
    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];
  }
  
  // 更新時(shí)間基準(zhǔn)
  myIMU.updateTime();
  
  // 姿態(tài)解算（使用優(yōu)化參數(shù)）
  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
  );
  
  // 轉(zhuǎn)換為歐拉角
  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;
  
  // 發(fā)送到VOFA+
  Serial.print(myIMU.yaw, 1);   // yaw
  Serial.print(",");
  Serial.print(myIMU.pitch, 1); // pitch
  Serial.print(",");
  Serial.println(myIMU.roll, 1);// roll

  delay(10); // 100Hz輸出
}

關(guān)鍵配置修改 關(guān)鍵配置修改

在ICM20948.cpp中調(diào)整量程（根據(jù)應(yīng)用需求）：

// 加速度計(jì)量程 (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+上位機(jī)配置

數(shù)據(jù)協(xié)議設(shè)置

選擇FireWater協(xié)議

格式：q0, q1, q2（逗號(hào)分隔+換行符）

波特率：115200

控件添加

3D立方體：顯示實(shí)時(shí)姿態(tài)、綁定四元數(shù)數(shù)據(jù)通道、設(shè)置模型縮放比例

波形圖：各軸角速度/加速度

儀表盤：顯示偏航角（Yaw）

界面效果

實(shí)時(shí)顯示傳感器3D姿態(tài)及運(yùn)動(dòng)波形

五、濾波參數(shù)優(yōu)化與動(dòng)態(tài)效果對比

1.傳感器校準(zhǔn)

float gyroBias[3], accelBias[3];
IMU.calibrateICM20948(gyroBias, accelBias); // 上電時(shí)執(zhí)行一次

2.問題現(xiàn)象

使用默認(rèn)參數(shù)(Kp=10.0, Ki=0.0)時(shí)，VOFA+顯示yaw值持續(xù)漂移（約2-5°/s），動(dòng)態(tài)運(yùn)動(dòng)時(shí)零漂明顯

3.優(yōu)化方案：

在AHRSAlgorithms.h中調(diào)整Mahony濾波參數(shù)：

// 原參數(shù)（漂移明顯）
// #define Kp 2.0f * 5.0f
// #define Ki 0.0f

// 優(yōu)化參數(shù)（大幅改善漂移）
#define Kp 3.0f    // 降低比例增益，減少高頻噪聲響應(yīng)
#define Ki 0.1f   // 降低積分增益，抑制累積誤差

效果對比：

4.優(yōu)化后效果

參數(shù)調(diào)整原理：

Kp過高：對加速度計(jì)噪聲敏感，導(dǎo)致高頻抖動(dòng)

Ki過高：積分累積誤差引起零漂

黃金比例：Kp/Ki ≈ 20-30時(shí)平衡動(dòng)態(tài)響應(yīng)與穩(wěn)定性

六、效果演示

靜態(tài)測試

傳感器平放時(shí)，VOFA+顯示俯仰角/橫滾角接近0°

Z軸加速度≈9.8 m/s2

動(dòng)態(tài)測試

旋轉(zhuǎn)開發(fā)板，3D模型同步跟隨

快速晃動(dòng)時(shí)波形圖顯示各軸加速度變化

演示視頻： https://live.csdn.net/v/480172?spm=1001.2014.3001.5501

輸出速率調(diào)優(yōu)

ICM20948原始數(shù)據(jù)輸出率約100Hz(10ms/次)

當(dāng)delt_t=60ms時(shí)，姿態(tài)解算循環(huán)(16.7Hz)與傳感器更新周期不同步

導(dǎo)致部分?jǐn)?shù)據(jù)幀被重復(fù)使用或跳過

完整工程代碼

百度網(wǎng)盤獲取完整工程文件，鏈接如下:

https://pan.baidu.com/s/11tr8XJvNrNernqwK1zA9Mw?pwd=pbxd

七、效果驗(yàn)證與結(jié)論

測試結(jié)果

結(jié)論：

通過調(diào)整Kp/Ki比例可有效抑制yaw漂移

磁力計(jì)軸序修正提升方位角精度

VOFA+可視化提供直觀參數(shù)調(diào)優(yōu)依據(jù)

三階段校準(zhǔn)確保全溫度范圍穩(wěn)定性

?(●'?'●)

零知開源是一個(gè)真正屬于國人自己的開源軟硬件平臺(tái)，在開發(fā)效率上超越了Arduino平臺(tái)并且更加容易上手，大大降低了開發(fā)難度。
零知開源在軟件方面提供了完整的學(xué)習(xí)教程和豐富示例代碼，讓不懂程序的工程師也能非常輕而易舉的搭建電路來創(chuàng)作產(chǎn)品，測試產(chǎn)品。快來動(dòng)手試試吧！
www.lingzhilab.com


審核編輯 黃宇