21ic电子技术开发论坛 › 开发板专区 › Arduino › 零知开源——STM32F103RBT6驱动 ICM20948 九轴传感器及 ...
返回列表 发新帖我要提问

[Arduino资料] 零知开源——STM32F103RBT6驱动 ICM20948 九轴传感器及 vofa + 上位机可视化教程

[复制链接]
 楼主| lingzhiLab 发表于 2025-6-9 11:25 | 显示全部楼层 |阅读模式
STM32F1, ICM20948, 姿态解算, 滤波参数优化, FA
本帖最后由 lingzhiLab 于 2025-6-9 11:30 编辑

STM32F1
        本教程使用零知标准板（STM32F103RBT6）通过I2C驱动ICM20948九轴传感器，实现姿态解算，并通过串口将数据实时发送至VOFA+上位机进行3D可视化。代码基于开源库修改优化，适合嵌入式及物联网开发者。在基础驱动上新增滤波参数优化，重点解决yaw值漂移问题，提供完整的参数调优方案和效果对比。

一、硬件准备1.硬件清单
零知标准板（主控STM32F103RBT6）
ICM20948九轴传感器模块
USB转串口模块（用于调试和数据传输）
杜邦线若干

2.接线方式
ICM20948引脚 零知开发板引脚
VCC3.3V
GNDGND
SDAA4
ACLA5
硬件连接图
85676de0ad90464881dc74f48ea14ab3.png

连接实物图
ea9d023ee02240f5ac27e14114517a26.jpeg


注意：确保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)
复制
  1. /* ICM20948完整优化代码 */
    

  2. #include "AHRSAlgorithms.h"
    

  3. #include "ICM20948.h"
    


  4. 

  5. #define AHRS true
    

  6. #define SerialDebug true 
    


  7. 

  8. int myLed = LED_BUILTIN;  
    

  9. ICM20948 myIMU;
    


  10. 

  11. void setup() {
    

  12.   pinMode(myLed, OUTPUT);
    

  13.   digitalWrite(myLed, HIGH);
    

  14.   
    

  15.   Serial.begin(115200);
    

  16.   Wire.begin();
    

  17.   
    

  18.   // 初始化与自检
    

  19.   if(myIMU.begin()) {
    

  20.     Serial.println("ICM20948初始化成功");
    

  21.     
    

  22.     // 执行两级校准
    

  23.     myIMU.calibrateICM20948(myIMU.gyroBias, myIMU.accelBias);
    

  24.     float magBias[3], magScale[3];
    

  25.     myIMU.magCalICM20948(magBias, magScale);
    

  26.     
    

  27.     // 设置优化分辨率
    

  28.     myIMU.getAres(); 
    

  29.     myIMU.getGres();
    

  30.     myIMU.getMres();
    

  31.   } else {
    

  32.     Serial.println("传感器初始化失败!");
    

  33.     while(1);
    

  34.   }
    

  35. }
    


  36. 

  37. void loop() {
    

  38.   // 数据读取
    

  39.   if (myIMU.readByte(ICM20948_ADDRESS, INT_STATUS_1) & 0x01) {
    

  40.     myIMU.readAccelData(myIMU.accelCount);
    

  41.     myIMU.readGyroData(myIMU.gyroCount);
    

  42.     myIMU.readMagData(myIMU.magCount);
    

  43.     
    

  44.     // 单位转换
    

  45.     myIMU.ax = (float)myIMU.accelCount[0] * myIMU.aRes;
    

  46.     myIMU.ay = (float)myIMU.accelCount[1] * myIMU.aRes;
    

  47.     myIMU.az = (float)myIMU.accelCount[2] * myIMU.aRes;
    

  48.     myIMU.gx = (float)myIMU.gyroCount[0] * myIMU.gRes;
    

  49.     myIMU.gy = (float)myIMU.gyroCount[1] * myIMU.gRes;
    

  50.     myIMU.gz = (float)myIMU.gyroCount[2] * myIMU.gRes;
    

  51.     myIMU.mx = (float)myIMU.magCount[0] * myIMU.mRes - myIMU.magBias[0];
    

  52.     myIMU.my = (float)myIMU.magCount[1] * myIMU.mRes - myIMU.magBias[1];
    

  53.     myIMU.mz = (float)myIMU.magCount[2] * myIMU.mRes - myIMU.magBias[2];
    

  54.   }
    

  55.   
    

  56.   // 更新时间基准
    

  57.   myIMU.updateTime();
    

  58.   
    

  59.   // 姿态解算（使用优化参数）
    

  60.   MahonyQuaternionUpdate(
    

  61.     myIMU.ax, myIMU.ay, myIMU.az,
    

  62.     myIMU.gx * DEG_TO_RAD, 
    

  63.     myIMU.gy * DEG_TO_RAD,
    

  64.     myIMU.gz * DEG_TO_RAD,
    

  65.     myIMU.my, myIMU.mx, myIMU.mz, // 轴序修正
    

  66.     myIMU.deltat
    

  67.   );
    

  68.   
    

  69.   // 转换为欧拉角
    

  70.   const float* q = getQ();
    

  71.   myIMU.yaw   = atan2(2.0f*(q[1]*q[2] + q[0]*q[3]), 
    

  72.                    q[0]*q[0] + q[1]*q[1] - q[2]*q[2] - q[3]*q[3]) * RAD_TO_DEG;
    

  73.   myIMU.pitch = -asin(2.0f*(q[1]*q[3] - q[0]*q[2])) * RAD_TO_DEG;
    

  74.   myIMU.roll  = atan2(2.0f*(q[0]*q[1] + q[2]*q[3]),
    

  75.                    q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]) * RAD_TO_DEG;
    

  76.   
    

  77.   // 发送到VOFA+
    

  78.   Serial.print(myIMU.yaw, 1);   // yaw
    

  79.   Serial.print(",");
    

  80.   Serial.print(myIMU.pitch, 1); // pitch
    

  81.   Serial.print(",");
    

  82.   Serial.println(myIMU.roll, 1);// roll
    


  83. 

  84.   delay(10); // 100Hz输出
    

  85. }

关键配置修改 在 ICM20948.cpp 中调整量程（根据应用需求）：
复制

  1. 

  2. // 加速度计量程 (AFS_2G/AFS_4G/AFS_8G/AFS_16G)
    

  3. void ICM20948::getAres()
    

  4. {
    

  5.   switch (Ascale)
    

  6.   {
    

  7.     // Possible accelerometer scales (and their register bit settings) are:
    

  8.     // 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs  (11).
    

  9.     // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
    

  10.     // 2-bit value:
    

  11.     case AFS_2G:
    

  12.       aRes = 2.0f / 32768.0f;
    

  13.       break;
    

  14.     case AFS_4G:
    

  15.       aRes = 4.0f / 32768.0f;
    

  16.       break;
    

  17.     case AFS_8G:
    

  18.       aRes = 8.0f / 32768.0f;
    

  19.       break;
    

  20.     case AFS_16G:
    

  21.       aRes = 16.0f / 32768.0f;
    

  22.       break;
    

  23.   }
    

  24. }
    


  25. 

  26. // 陀螺仪量程 (GFS_250DPS/GFS_500DPS/GFS_1000DPS/GFS_2000DPS)
    

  27. void ICM20948::getGres()
    

  28. {
    

  29.   switch (Gscale)
    

  30.   {
    

  31.     // Possible gyro scales (and their register bit settings) are:
    

  32.     // 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
    

  33.     // Here's a bit of an algorith to calculate DPS/(ADC tick) based on that
    

  34.     // 2-bit value:
    

  35.     case GFS_250DPS:
    

  36.       gRes = 250.0f / 32768.0f;
    

  37.       break;
    

  38.     case GFS_500DPS:
    

  39.       gRes = 500.0f / 32768.0f;
    

  40.       break;
    

  41.     case GFS_1000DPS:
    

  42.       gRes = 1000.0f / 32768.0f;
    

  43.       break;
    

  44.     case GFS_2000DPS:
    

  45.       gRes = 2000.0f / 32768.0f;
    

  46.       break;
    

  47.   }
    

  48. }

四、VOFA+上位机配置数据协议设置
            选择 FireWater 协议
            格式：q0, q1, q2（逗号分隔+换行符）
            波特率：115200

控件添加
           3D立方体：显示实时姿态、绑定四元数数据通道、设置模型缩放比例
           波形图：各轴角速度/加速度
           仪表盘：显示偏航角（Yaw）


界面效果
d83a1a155f52455fb4d22c98ab79dedb.png

        实时显示传感器3D姿态及运动波形

五、滤波参数优化与动态效果对比1.传感器校准

复制
  1. float gyroBias[3], accelBias[3];
    

  2. IMU.calibrateICM20948(gyroBias, accelBias); // 上电时执行一次


2.问题现象使用默认参数(Kp=10.0, Ki=0.0)时，VOFA+显示yaw值持续漂移（约2-5°/s），动态运动时零漂明显

26f0315322714c82a5b697cbed36d2b3.gif



3.优化方案： 在AHRSAlgorithms.h中调整Mahony滤波参数：

复制
  1. // 原参数（漂移明显）
    

  2. // #define Kp 2.0f * 5.0f
    

  3. // #define Ki 0.0f
    


  4. 

  5. // 优化参数（大幅改善漂移）
    

  6. #define Kp 3.0f    // 降低比例增益，减少高频噪声响应
    

  7. #define Ki 0.1f   // 降低积分增益，抑制累积误差


效果对比：
              参数状态    Yaw漂移率              VOFA+动态表现   
      默认(Kp=10f,Ki=0.0f)          2-5°/s   静止时快速旋转，运动后复位慢
     优化(Kp=3.0f,Ki=0.1f)           <0.5°/s      静止稳定，运动后快速收敛  



4.优化后效果
ea095afa46eb4591831b044adbc69f25.gif

参数调整原理：
    Kp过高：对加速度计噪声敏感，导致高频抖动
    Ki过高：积分累积误差引起零漂
          黄金比例：Kp/Ki ≈ 20-30 时平衡动态响应与稳定性

六、效果演示静态测试
        传感器平放时，VOFA+显示俯仰角/横滚角接近0°
        Z轴加速度≈9.8 m/s²

21ecda44a0ea475f888913e577730945.png


动态测试
     旋转开发板，3D模型同步跟随
     快速晃动时波形图显示各轴加速度变化

https://www.bilibili.com/video/BV1jeTYz9E8D/?share_source=copy_web&vd_source=75d3b293c1933aa8dc6757ac429e12da

输出速率调优
   ICM20948原始数据输出率约100Hz(10ms/次)
   当delt_t=60ms时，姿态解算循环(16.7Hz)与传感器更新周期不同步
   导致部分数据帧被重复使用或跳过

7cf9f44e2abc4c6c9c828fae3d398cb1.png



完整工程代码百度网盘获取完整工程文件，链接如下:    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


回复 赏

举报

相关帖子
返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 注册

本版积分规则

lingzhiLab

22

主题

33

帖子

0

粉丝
快速回复 在线客服 返回列表 返回顶部