本帖最后由 lingzhiLab 于 2025-6-9 11:30 编辑
STM32F1 本教程使用零知标准板（STM32F103RBT6）通过I2C驱动ICM20948九轴传感器，实现姿态解算，并通过串口将数据实时发送至VOFA+上位机进行3D可视化。代码基于开源库修改优化，适合嵌入式及物联网开发者。在基础驱动上新增滤波参数优化，重点解决yaw值漂移问题，提供完整的参数调优方案和效果对比。
一、硬件准备1.硬件清单零知标准板（主控STM32F103RBT6）
ICM20948九轴传感器模块
USB转串口模块（用于调试和数据传输）
杜邦线若干
2.接线方式 | ICM20948引脚 | 零知开发板引脚 | | VCC | 3.3V | | GND | GND | | SDA | A4 | | ACL | A5 |
硬件连接图
连接实物图
注意：确保I2C引脚正确，避免接反导致芯片损坏。
二、软件环境搭建开发环境 AHRSAlgorithms.cpp（姿态解算库）
ICM20948.cpp（传感器驱动库）
库文件关键功能 Madgwick/Mahony滤波算法
四元数实时输出 getQ()
参数可调：Kp、Ki、beta
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); // yaw
Serial.print(",");
Serial.print(myIMU.pitch, 1); // pitch
Serial.print(",");
Serial.println(myIMU.roll, 1);// roll
delay(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）
界面效果
五、滤波参数优化与动态效果对比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/s²
动态测试
旋转开发板，3D模型同步跟随
快速晃动时波形图显示各轴加速度变化
输出速率调优
ICM20948原始数据输出率约100Hz(10ms/次)
当delt_t=60ms时，姿态解算循环(16.7Hz)与传感器更新周期不同步
导致部分数据帧被重复使用或跳过
完整工程代码百度网盘获取完整工程文件，链接如下: 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
|
楼主
标题置顶
标题高亮
