当前位置： 首页 > news >正文

第二章、IMU（Inertial Measurement Unit 惯性测量单元）

news 2025/5/20 11:02:04

0 前言

该部分作为开发过程中的学习记录，可能有错误及不足欢迎小伙伴们一起讨论～

注：该部分代码是基于Stackorce的舵机控制板来看的，板载三轴加速度计、陀螺仪。

1 理论部分

IMU核心组件

  • 加速度计
    • 功能：测量物体在三个正交轴（X、Y、Z）上的 线加速度。
    • 原理：通过检测质量块在加速度作用下的位移或压电效应生成电信号。
  • 陀螺仪
    • 功能：测量物体绕三个轴的 角速度（旋转速率）。
    • 原理：利用科里奥利力（MEMS陀螺仪）或光学干涉（光纤陀螺仪）来感知旋转。

扩展组件（非标准）

  • 磁力计
    • 定位：通常不属于传统IMU，但常与IMU结合使用（如AHRS系统）。
    • 功能：测量磁场强度以提供绝对航向（类似电子罗盘），辅助校正陀螺仪漂移。

经典应用场景

  • 基础IMU：仅含加速度计+陀螺仪（如智能手机姿态检测）。
  • 高级系统：IMU+磁力计+GPS构成组合导航系统（如无人机、自动驾驶）。

IMU分类

  • 6轴IMU
    • 组成：3轴加速度计 + 3轴陀螺仪。
    • 功能：测量线加速度（XYZ）和角速度（XYZ），无法直接输出姿态角（需积分计算）。
    • 应用：基础运动追踪（如无人机姿态稳定）。
  • 9轴IMU
    • 组成：6DoF IMU + 3轴磁力计。
    • 功能：增加磁场测量，可计算航向角，减少积分漂移误差。
    • 应用：增强现实（AR）、机器人定位。
  • 10轴IMU
    • 组成：9DoF IMU + 气压计。
    • 功能：通过气压计测量高度变化，辅助垂直方向定位。
    • 应用：无人机定高、室内导航。

如何选取IMU

  • 精度需求：消费场景选MEMS，高精度选光纤/激光陀螺。
  • 环境适应性：高温/震动场景需工业级或战术级IMU。
  • 成本限制：MEMS适合低成本项目，战略级IMU价格可达数百万美元。
  • 系统复杂度：独立IMU需自主开发算法，组合系统可直接输出姿态/位置。

2 Stackorce的IMU代码部分

// 头文件保护宏，防止重复包含
#ifndef _SF_IMU_h
#define _SF_IMU_h// 包含必要的库文件
#include <Arduino.h>   // Arduino核心库
#include <math.h>      // 数学函数库（如三角函数）
#include <Wire.h>      // I2C通信库// 定义π的值，用于角度计算
#define PI 3.1415926535897932384626433832795// MPU6050寄存器地址定义
#define MPU6050_ADDR 0x68          // MPU6050的I2C地址（AD0引脚接地时）
#define MPU6050_SMPLRT_DIV 0x19    // 采样率分频寄存器
#define MPU6050_CONFIG 0x1a        // 配置寄存器（设置低通滤波器）
#define MPU6050_GYRO_CONFIG 0x1b   // 陀螺仪配置寄存器（量程设置）
#define MPU6050_ACCEL_CONFIG 0x1c  // 加速度计配置寄存器（量程设置）
#define MPU6050_WHO_AM_I 0x75      // 设备ID寄存器（用于验证连接）
#define MPU6050_PWR_MGMT_1 0x6b    // 电源管理寄存器（控制休眠模式等）
#define MPU6050_TEMP_H 0x41        // 温度数据高字节寄存器
#define MPU6050_TEMP_L 0x42        // 温度数据低字节寄存器// IMU传感器处理类
class SF_IMU{public:// 构造函数，传入I2C对象引用（支持不同I2C端口）SF_IMU(TwoWire &i2c);// 公共成员变量（通常建议封装为私有并通过方法访问）float gyroOffset[3];  // 陀螺仪三轴偏移校准值[X,Y,Z]float temp;           // 温度传感器读数（摄氏度）float acc[3];        // 加速度计三轴原始数据[X,Y,Z]（单位：g）float gyro[3];        // 陀螺仪三轴原始数据[X,Y,Z]（单位：度/秒）float angleGyro[3];   // 纯陀螺仪积分得到的三轴角度（存在漂移）float angleAcc[2];     // 加速度计计算的俯仰和横滚角（Yaw不可测）float angleRPY[3];   // 最终融合后的姿态角（Roll,Pitch,Yaw）float accCoef;        // 互补滤波器中的加速度计系数（低通）float gyroCoef;       // 互补滤波器中的陀螺仪系数（高通）// 公共方法void init();                     // 初始化传感器配置void calGyroOffsets();           // 校准陀螺仪零偏void update();                   // 更新传感器数据并进行姿态解算void setGyroOffsets(float x, float y, float z); // 手动设置陀螺仪偏移// 以下方法被注释，可能计划后续实现// float* getAcc();// float* getGyro();// float getTemp();// float* getAngleAcc();// float* getAngleGyro();// float* getAngle();private:TwoWire *_i2c;           // I2C通信对象指针// 时间相关变量float interval;          // 两次update()调用的时间间隔（秒）float preInterval;      // 前次更新时间戳// 私有方法void writeToIMU(uint8_t addr, uint8_t data); // 写寄存器uint8_t readFromIMU(uint8_t addr);           // 读单字节寄存器
};#endif // 结束头文件保护

#include "SF_IMU.h"// 构造函数，初始化I2C接口和滤波器系数
SF_IMU::SF_IMU(TwoWire &i2c): _i2c(&i2c) {  // 初始化I2C对象指针// 设置互补滤波器系数（加速度计权重2%，陀螺仪98%）accCoef = 0.02f;   // 加速度计数据信任度（用于低频修正）gyroCoef = 0.98f;  // 陀螺仪数据信任度（用于高频追踪）
}// 初始化MPU6050传感器
void SF_IMU::init() {// 寄存器配置（所有值均为16进制）：writeToIMU(MPU6050_SMPLRT_DIV, 0x00);   // 采样率=1kHz（陀螺仪输出率8kHz/(1+0)=8kHz）writeToIMU(MPU6050_CONFIG, 0x00);       // 禁用低通滤波器（带宽=260Hz）writeToIMU(MPU6050_GYRO_CONFIG, 0x08);  // 陀螺仪量程±500°/s（灵敏度65.5 LSB/°/s）writeToIMU(MPU6050_ACCEL_CONFIG, 0x00); // 加速度计量程±2g（灵敏度16384 LSB/g）writeToIMU(MPU6050_PWR_MGMT_1, 0x01);   // 退出睡眠模式，使用X轴陀螺作为时钟源this->update();  // 首次读取数据以初始化角度值（可能存在风险，建议延迟后执行）// 初始化角度值angleGyro[0] = 0;  // 陀螺仪X轴积分角度归零angleGyro[1] = 0;  // 陀螺仪Y轴积分角度归零angleRPY[0] = angleAcc[0];  // 初始横滚角使用加速度计计算值angleRPY[1] = angleAcc[1];  // 初始俯仰角使用加速度计计算值preInterval = millis();  // 记录初始时间戳calGyroOffsets();        // 执行陀螺仪零偏校准
}// 校准陀螺仪零偏（需保持设备静止）
void SF_IMU::calGyroOffsets() {float x = 0, y = 0, z = 0;int16_t rx, ry, rz;delay(1000);  // 等待设备稳定Serial.println();Serial.println("========================================");Serial.println("Calculating gyro offsets");Serial.print("DO NOT MOVE MPU6050");// 采集3000个样本求平均（约3秒，假设主循环延迟1ms）for (int i = 0; i < 3000; i++) {if (i % 1000 == 0) Serial.print(".");  // 进度指示// 读取陀螺仪原始数据（寄存器0x43开始，连续6字节）_i2c->beginTransmission(MPU6050_ADDR);_i2c->write(0x43);  // 陀螺仪数据起始地址_i2c->endTransmission(false);_i2c->requestFrom((int)MPU6050_ADDR, 6);// 组合高低字节数据（注意MPU6050数据为大端格式）rx = _i2c->read() << 8 | _i2c->read();  // X轴ry = _i2c->read() << 8 | _i2c->read();  // Y轴rz = _i2c->read() << 8 | _i2c->read();  // Z轴// 转换为°/s并累加（65.5=±500°/s量程时的灵敏度）x += ((float)rx) / 65.5;y += ((float)ry) / 65.5;z += ((float)rz) / 65.5;}// 计算平均偏移量（零偏）gyroOffset[0] = x / 3000;  // X轴偏移gyroOffset[1] = y / 3000;  // Y轴偏移gyroOffset[2] = z / 3000;  // Z轴偏移// 输出校准结果Serial.println("Done!");Serial.printf("%f,%f,%f\n", gyroOffset[0], gyroOffset[1], gyroOffset[2]);Serial.print("========================================");delay(1000);  // 校准完成等待
}// 主更新函数（需周期性调用）
void SF_IMU::update() {// 请求14字节数据（加速度6 + 温度2 + 陀螺仪6）_i2c->beginTransmission(MPU6050_ADDR);_i2c->write(0x3B);  // 加速度计数据起始地址_i2c->endTransmission(false);_i2c->requestFrom((int)MPU6050_ADDR, 14);// 读取原始数据（注意顺序：AccX/Y/Z → Temp → GyroX/Y/Z）int16_t rawAccX = _i2c->read() << 8 | _i2c->read();int16_t rawAccY = _i2c->read() << 8 | _i2c->read();int16_t rawAccZ = _i2c->read() << 8 | _i2c->read();int16_t rawTemp = _i2c->read() << 8 | _i2c->read();int16_t rawGyroX = _i2c->read() << 8 | _i2c->read();int16_t rawGyroY = _i2c->read() << 8 | _i2c->read();int16_t rawGyroZ = _i2c->read() << 8 | _i2c->read();/* 温度转换（MPU6050温度传感器公式）：* 公式来源：MPU6050 Datasheet 第30页* Temperature in degrees C = (TEMP_OUT Register Value)/340 + 36.53* 代码等效公式：(raw + 12412)/340 ≈ raw/340 + 36.53*/temp = (rawTemp + 12412.0) / 340.0;  // 转换为摄氏度// 加速度转换（±2g量程，16384 LSB/g）acc[0] = ((float)rawAccX) / 16384.0;  // X轴加速度（g）acc[1] = ((float)rawAccY) / 16384.0;  // Y轴加速度（g）acc[2] = ((float)rawAccZ) / 16384.0;  // Z轴加速度（g）/* 加速度计姿态计算（仅横滚Roll和俯仰Pitch）：* 修正公式：atan2(accY, sqrt(accZ^2 + accX^2)) → 此处简化处理* 添加abs(acc[0])为防止分母接近零时的震荡*/angleAcc[0] = atan2(acc[1], acc[2] + abs(acc[0])) * 360 / (2 * M_PI); // Roll计算angleAcc[1] = atan2(acc[0], acc[2] + abs(acc[1])) * 360 / (-2 * M_PI); // Pitch计算（符号调整）// 陀螺仪数据转换（扣除零偏）gyro[0] = ((float)rawGyroX) / 65.5 - gyroOffset[0]; // X轴角速度（°/s）gyro[1] = ((float)rawGyroY) / 65.5 - gyroOffset[1]; // Y轴角速度（°/s）gyro[2] = ((float)rawGyroZ) / 65.5 - gyroOffset[2]; // Z轴角速度（°/s）// 计算时间间隔（秒）interval = (millis() - preInterval) * 0.001;// 纯陀螺仪积分角度（累积误差会随时间漂移）angleGyro[0] += gyro[0] * interval;  // X轴积分角度angleGyro[1] += gyro[1] * interval;  // Y轴积分角度angleGyro[2] += gyro[2] * interval;  // Z轴积分角度（Yaw）/* 互补滤波器融合角度：* 融合公式：当前角度 = 陀螺预测角度 * 系数 + 加速度计角度 * 系数* 注意：此处实现可能存在问题，正确公式应为：* angle = gyroCoef * (previous_angle + gyro * dt) + accCoef * acc_angle*/angleRPY[0] = (gyroCoef * (angleRPY[0] + gyro[0] * interval)) + (accCoef * angleAcc[0]); // RollangleRPY[1] = (gyroCoef * (angleRPY[1] + gyro[1] * interval)) + (accCoef * angleAcc[1]); // PitchangleRPY[2] = angleGyro[2];  // Yaw仅用陀螺仪（需磁力计补偿）// 更新时间戳preInterval = millis();
}// 辅助函数：向指定寄存器写入数据
void SF_IMU::writeToIMU(uint8_t addr, uint8_t data) {_i2c->beginTransmission(MPU6050_ADDR);_i2c->write(addr);   // 寄存器地址_i2c->write(data);   // 写入值_i2c->endTransmission();
}// 辅助函数：从指定寄存器读取单字节
uint8_t SF_IMU::readFromIMU(uint8_t addr) {_i2c->beginTransmission(MPU6050_ADDR);_i2c->write(addr);  // 设置目标寄存器_i2c->endTransmission();_i2c->requestFrom((uint8_t)MPU6050_ADDR, (uint8_t)1);return _i2c->read();  // 返回读取值
}

官方文档参考：https://docs.arduino.cc/language-reference/

标准Arduino代码由setup()和loop()两个函数组成。

void setup() {// put your setup code here, to run once:// 在这里加入你的setup代码，它只会运行一次：
}void loop() {// put your main code here, to run repeatedly://  在这里加入你的loop代码,它会不断重复运行：
}

Arduino控制器通电或复位后，即会开始执行setup() 函数中的程序，该部分只会执行一次。通常我们会在setup() 函数中完成Arduino的初始化设置，如配置I/O口状态，初始化串口等操作。在setup() 函数中的程序执行完后，Arduino会接着执行loop() 函数中的程序。而loop()函数是一个死循环，其中的程序会不断的重复运行。

因此可以看到读取IMU的数据的代码如下所示：

#include <Arduino.h>   // Arduino核心库，包含基础函数和常量定义
#include <Wire.h>      // I2C通信库，用于与MPU6050传感器通信
#include "SF_IMU.h"    // 自定义的IMU传感器处理库头文件SF_IMU mpu6050 = SF_IMU(Wire);  // 创建IMU对象，使用Wire作为I2C接口
float roll, pitch, yaw;          // 存储融合后的姿态角（横滚、俯仰、偏航）
float gyroX, gyroY, gyroZ;       // 存储校准后的陀螺仪三轴角速度（°/s）void setup() {Serial.begin(115200);          // 初始化串口通信，波特率115200（用于数据输出）Wire.begin(1, 2, 400000UL);    // 初始化I2C总线：// - SDA引脚 = GPIO1// - SCL引脚 = GPIO2// - 频率 = 400kHz（UL表示无符号长整型）mpu6050.init();               // 调用IMU初始化函数，配置传感器参数
}void loop() {// 主循环（重复执行）mpu6050.update();  // 更新传感器数据（包含I2C通信、数据解析、姿态解算）// [!] 注意：此处存在变量名错误！// 根据SF_IMU.h定义，应为angleRPY而非angleroll = mpu6050.angle[0];   // 获取融合后的横滚角（应改为angleRPY[0]）pitch = mpu6050.angle[1];  // 获取融合后的俯仰角（应改为angleRPY[1]）yaw = mpu6050.angle[2];    // 获取融合后的偏航角（应改为angleRPY[2]）gyroX = mpu6050.gyro[0];  // 获取校准后的X轴角速度（°/s）gyroY = mpu6050.gyro[1];  // 获取校准后的Y轴角速度（°/s）gyroZ = mpu6050.gyro[2];  // 获取校准后的Z轴角速度（°/s）// 格式化输出所有数据到串口Serial.printf("%f,%f,%f,%f,%f,%f\n", roll, pitch, yaw, gyroX, gyroY, gyroZ);delay(100);  // 延时100ms（控制数据输出频率≈10Hz）
}
http://www.xdnf.cn/news/538075.html

相关文章：

  • 包装可靠性测试【二】
  • C++寻位映射的奇幻密码：哈希
  • AtomicReference 和 volatile 的比较
  • C++--综合应用-演讲比赛项目
  • 让数据驱动增长更简单！ ClkLog用户行为分析系统正式入驻GitCode
  • 【随手记】 Event Bus vs. Event Loop
  • 01、java方法
  • 【Python训练营打卡】day30 @浙大疏锦行
  • 盲盒APP开发——解锁盲盒经济无限可能
  • mapbox-gl强制请求需要accessToken的问题
  • Chromium 回调设计实战：BindOnce 与 BindRepeating 的最佳实践
  • 【css】【面试提问】css经典问题总结
  • Golang中的runtime.LockOSThread 和 runtime.UnlockOSThread
  • 嵌入式自学第二十四天
  • 整数的个数
  • Ollama 如何在显存资源有限的情况下合理分配给不同的服务？
  • 理解前端工程化
  • 新书速览|鸿蒙HarmonyOS NEXT开发之路 卷2：从入门到应用篇
  • java集成mqtt
  • 停等协议（Stop-and-Wait Protocol）
  • AI人工智能写作平台：AnKo助力内容创作变革！
  • 铅铋环境下应力腐蚀的疲劳试验装置
  • 什么业务需要用到waf
  • 20. 自动化测试框架开发之Excel配置文件的IO开发
  • 【monai 教程】transform之CropPad详解
  • 磁流体 磁性流体 磁液
  • 封装一个基于 WangEditor 的富文本编辑器组件（Vue 3 + TypeScript 实战）
  • UEFI Spec 学习笔记---33 - Human Interface Infrastructure Overview---33.2.6 Strings
  • Oracle 中 open_cursors 参数详解：原理、配置与性能测试
  • 一键无损批量压缩图片 保留高清细节 开源免费！支持 10 + 格式转换