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

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]); // Roll
  angleRPY[1] = (gyroCoef * (angleRPY[1] + gyro[1] * interval)) + (accCoef * angleAcc[1]); // Pitch
  angleRPY[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而非angle
  roll = 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）
}