Arduino BLDC 机器人 IMU 角度读取与 PID 互补滤波控制

2. 四轴飞行器姿态控制（简化版）

多旋翼的控制逻辑类似，但涉及多个自由度的混合。这里演示了俯仰和滚转的独立 PID 控制。

#include <Wire.h>
#include <MPU6050.h>
#include <Servo.h>

MPU6050 mpu;
Servo motors[4]; // 4 个电机

// PID 参数（俯仰/滚转/偏航）
float Kp_pitch = 1.2, Ki_pitch = 0.05, Kd_pitch = 0.8;
float Kp_roll = 1.2, Ki_roll = 0.05, Kd_roll = 0.8;
float targetPitch = 0, targetRoll = 0;

// 互补滤波
float alpha = 0.95;
float pitchAngle = 0, rollAngle = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  // 初始化电机
  for (int i = 0; i < 4; i++) {
    motors[i].attach(5 + i); // 引脚 5-8
    motors[i].write(90);     // 初始中立位置
  }
}

void computeAngles() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 加速度计角度
  float accelPitch = atan2(-ax, sqrt(ay * ay + az * az)) * RAD_TO_DEG;
  float accelRoll = atan2(ay, az) * RAD_TO_DEG;

  // 陀螺仪积分
  static float gyroPitch = 0, gyroRoll = 0;
  gyroPitch += gy / 131.0 * 0.01;
  gyroRoll += gx / 131.0 * 0.01;

  // 互补滤波
  pitchAngle = alpha * (pitchAngle + gyroPitch) + (1 - alpha) * accelPitch;
  rollAngle = alpha * (rollAngle + gyroRoll) + (1 - alpha) * accelRoll;
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = millis();
  if (now - lastTime >= 10) { // 100Hz 控制频率
    lastTime = now;
    // 1. 计算姿态角
    computeAngles();

    // 2. PID 控制（简化版：仅俯仰和滚转）
    static float iTerm_pitch = 0, iTerm_roll = 0;
    float errorPitch = targetPitch - pitchAngle;
    float errorRoll = targetRoll - rollAngle;
    iTerm_pitch += errorPitch * 0.01;
    iTerm_roll += errorRoll * 0.01;
    float outputPitch = Kp_pitch * errorPitch + Ki_pitch * iTerm_pitch;
    float outputRoll = Kp_roll * errorRoll + Ki_roll * iTerm_roll;

    // 3. 电机混合（十字型布局）
    int throttle = 1100; // 基础油门（PWM 值）
    int m1 = throttle + outputPitch + outputRoll;
    int m2 = throttle - outputPitch + outputRoll;
    int m3 = throttle - outputPitch - outputRoll;
    int m4 = throttle + outputPitch - outputRoll;

    // 4. 限制输出并写入电机
    for (int i = 0; i < 4; i++) {
      int val = (i == 0 ? m1 : i == 1 ? m2 : i == 2 ? m3 : m4);
      motors[i].write(constrain(map(val, 1000, 2000, 0, 180), 0, 180));
    }
  }
}

3. 云台稳定系统（单轴）

云台更强调平滑性，因此 PID 参数通常比平衡车更柔和，且对噪声更敏感。

#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>

MPU6050 mpu;
BLDCMotor motor(7);
Encoder encoder(2, 3);

// PID 参数
float Kp = 2.0, Ki = 0.1, Kd = 0.05;
float targetAngle = 90.0; // 目标角度（度）

// 互补滤波
float alpha = 0.92;
float filteredAngle = 0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  mpu.initialize();
  motor.init();
  encoder.init();
  motor.linkEncoder(&encoder);
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = millis();
  float dt = (now - lastTime) / 1000.0;
  if (dt > 0.1) dt = 0.1; // 限制最大 dt

  // 1. 读取 IMU
  uint16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 2. 互补滤波（假设绕 X 轴旋转）
  float accelAngle = atan2(ay, az) * RAD_TO_DEG;
  float gyroRate = gx / 131.0;
  static float gyroAngle = 0;
  gyroAngle += gyroRate * dt;
  filteredAngle = alpha * (filteredAngle + gyroAngle * dt) + (1 - alpha) * accelAngle;

  // 3. PID 控制
  static float integral = 0;
  float error = targetAngle - filteredAngle;
  integral += error * dt;
  float derivative = (error - (lastTime ? (targetAngle - filteredAngle) : 0)) / dt;
  float output = Kp * error + Ki * integral + Kd * derivative;

  // 4. 电机控制
  motor.move(output);

  // 调试输出
  if (now - lastTime >= 50) { // 20Hz 打印
    Serial.print("Target: ");
    Serial.print(targetAngle);
    Serial.print(" Actual: ");
    Serial.print(filteredAngle);
    Serial.print(" Output: ");
    Serial.println(output);
    lastTime = now;
  }
}

高级控制策略

4. 基础 IMU 平衡控制（含校准与安全）

实际工程中，零漂校准和安全保护至关重要。下面的代码展示了如何集成校准流程、故障检测以及更稳健的 FOC 控制。

#include <SimpleFOC.h>
#include <MPU6050_tockn.h>
#include <Wire.h>
#include <PID_v1.h>

// IMU 传感器
MPU6050 mpu6050(Wire);
#define IMU_SDA 21
#define IMU_SCL 22

// BLDC 电机
BLDCMotor motor(7);
Encoder encoder(2, 3, 2048);

void doA() { encoder.handleA(); }
void doB() { encoder.handleB(); }

// 互补滤波参数
struct ComplementaryFilter {
  float angle = 0.0;
  float angleVelocity = 0.0;
  float alpha = 0.98;
  float dt = 0.01;
  float accAngle = 0.0;
  float gyroBias = 0.0;
  float lastGyroZ = 0.0;
};
ComplementaryFilter compFilter;

// PID 控制器参数
struct PIDControl {
  double input, output, setpoint;
  double kp, ki, kd;
  double integral = 0;
  double lastError = 0;
  double outputLimit = 12.0;
  double integralLimit = 10.0;
  bool antiWindup = true;
};
PIDControl balancePID = {0, 0, 0, 25.0, 0.5, 0.2};

// 安全参数
struct SafetyParams {
  float maxAngle = 0.5; // 最大允许角度 30°
  float maxAngularVel = 3.0;
  float maxCurrent = 5.0;
  int faultCount = 0;
  int maxFaults = 10;
};
SafetyParams safety;

void setup() {
  Serial.begin(115200);
  Serial.println("===== IMU 角度读取 + PID 控制 + 互补滤波 =====");
  Serial.println("基于 MPU6050 的自平衡控制系统");

  // 1. 初始化 I2C
  Wire.begin(IMU_SDA, IMU_SCL);
  delay(100);

  // 2. 初始化 IMU
  if (!initIMU()) {
    Serial.println("IMU 初始化失败!");
    while (1);
  }

  // 3. 校准 IMU
  calibrateIMU();

  // 4. 初始化电机
  initMotor();

  // 5. 初始化滤波器
  initFilter();
  Serial.println("系统初始化完成");
  Serial.println("等待 3 秒后开始平衡...");
  delay(3000);
}

bool initIMU() {
  Serial.println("初始化 MPU6050...");
  mpu6050.begin();
  uint8_t deviceID = mpu6050.readByte(MPU6050_ADDRESS, MPU6050_WHO_AM_I);
  if (deviceID != 0x68) {
    Serial.print("错误的设备 ID: 0x");
    Serial.println(deviceID, HEX);
    return false;
  }
  // 配置 IMU
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, 0x00);
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_SMPLRT_DIV, 0x07);
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_CONFIG, 0x00);
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_GYRO_CONFIG, 0x18);
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG, 0x10);
  Serial.println("MPU6050 初始化成功");
  return true;
}

void calibrateIMU() {
  Serial.println("开始 IMU 校准...");
  Serial.println("请保持设备静止 10 秒");
  float accX_sum = 0, accY_sum = 0, accZ_sum = 0;
  float gyroX_sum = 0, gyroY_sum = 0, gyroZ_sum = 0;
  int samples = 1000;
  for (int i = 0; i < samples; i++) {
    int16_t ax, ay, az, gx, gy, gz;
    readRawIMU(ax, ay, az, gx, gy, gz);
    accX_sum += ax; accY_sum += ay; accZ_sum += az;
    gyroX_sum += gx; gyroY_sum += gy; gyroZ_sum += gz;
    delay(10);
    if (i % 100 == 0) Serial.print(".");
  }
  Serial.println();
  // 计算偏移
  // ... (省略部分计算逻辑以保持简洁，实际需完整实现)
  Serial.println("校准完成");
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0;
  if (dt >= 0.01) { // 100Hz 控制
    processIMUData(dt);
    complementaryFilter(dt);
    pidControl(dt);
    applyMotorControl();
    safetyCheck();
    motor.loopFOC();
    lastTime = now;
  }
  // 状态显示
  static unsigned long lastDisplay = 0;
  if (millis() - lastDisplay >= 50) {
    displayStatus();
    lastDisplay = millis();
  }
}

// 辅助函数略... (processIMUData, complementaryFilter 等)

5. 自适应互补滤波与卡尔曼滤波进阶

当环境干扰较大或对精度要求极高时，简单的互补滤波可能不够用。此时可以考虑自适应调整系数，甚至引入扩展卡尔曼滤波 (EKF)。

自适应互补滤波

根据传感器的置信度动态调整 α 值。例如，当检测到剧烈震动导致加速度计数据不可信时，自动降低其权重。

扩展卡尔曼滤波 (EKF)

EKF 能处理非线性系统，并提供最优估计。虽然计算量大，但在高性能 MCU 上已可行。它通过预测和更新两个步骤，结合模型预测和传感器测量，不断修正状态估计。

关键要点总结

1. IMU 数据读取与处理

   • 关键点：加速度计数据需进行单位转换（如 atan2 计算角度）。
   • 优化建议：添加校准程序消除零偏（如开机时采集样本取平均）。
   • 使用 DMP（数字运动处理器）硬件解算可减轻 MCU 负担。

2. 互补滤波实现

   • 核心公式：angle = α * (angle + gyro * dt) + (1-α) * accel_angle
   • 参数选择：α接近 1 时信任陀螺仪（动态响应快但易漂移），α接近 0 时信任加速度计（无漂移但噪声大）。
   • 代码技巧：使用静态变量保持状态，限制积分项增长防止饱和。

3. PID 控制实现细节

   • 常见问题：微分项噪声过大 → 使用 derivative = (error - previousError)/dt 而非直接微分陀螺仪数据。
   • 积分项漂移 → 添加积分限幅（抗饱和）。
   • 调试技巧：先调 P 参数，再调 D，最后调 I。通过串口打印 error 和 output 观察响应曲线。

4. 实时性保障措施

   • 固定采样周期：使用 millis() 或硬件定时器而非 delay() 实现定时控制。
   • 资源优化：避免在循环中使用浮点除法，低端 MCU 建议控制频率≤100Hz。

工程提示：以上案例旨在拓展思路，仅供参考。实际编程时，务必根据您的硬件配置、使用场景和具体需求进行调整，并多次实际测试。涉及硬件操作的代码，使用前请确认引脚和电平的参数正确性和安全性。确保电源隔离良好，防止电机启停时的电流波动影响传感器稳定性。