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

基于 Arduino 平台实现 BLDC 机器人的 IMU 角度读取、互补滤波及 PID 控制，构成了典型的姿态闭环系统。这套架构是自平衡机器人或稳定云台的核心技术栈，通过融合传感器数据获取精准姿态，再由 PID 计算驱动力矩驱动电机。

核心在于三块：传感器融合、决策大脑和执行机构。互补滤波作为感知中枢，解决了单一传感器无法兼顾动态与静态精度的问题。它利用低通滤波处理加速度计提取重力方向，高通滤波处理陀螺仪捕捉角速度变化，最后加权平均得到稳定角度。PID 控制器则是决策大脑，比例项提供恢复力，微分项抑制振荡，积分项消除静差（平衡系统中通常设为 0）。BLDC 电机负责执行，其快速响应特性适合跟随 PWM 指令产生恢复力矩。

应用场景很广，比如两轮自平衡机器人，通过检测俯仰角维持垂直平衡；云台稳定系统，反向旋转抵消晃动；或是倒立摆实验装置验证控制算法。实际开发中要注意硬件选型与抗干扰。IMU 推荐 MPU6050 或 ICM-20689，需刚性固定在重心附近，远离电机干扰。电源要隔离，防止电机启停拉低电压导致复位。算法实现上，滤波器系数 α 通常在 0.95~0.98 之间，PID 参数建议'由小到大'试凑，采样频率建议≥100Hz，严禁使用 delay() 阻塞主循环。

实战中，我们可以参考以下代码结构，涵盖基础平衡、四轴简化版及云台单轴控制。

1. 两轮自平衡机器人（IMU 角度读取 + PID 控制）

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

MPU6050 mpu;
BLDCMotor motor(7);
Encoder encoder(2, 3); // 编码器引脚

// PID 参数
float Kp = 40.0, Ki = 10.0, Kd = 0.5;
float targetAngle = 0.0; // 目标角度（垂直平衡点）
float previousError = 0, integral = 0;

// 互补滤波参数
float alpha = 0.98; // 加速度计权重
float dt = 0.01;    // 采样时间 (s)
float filteredAngle = 0;

void setup() {
  Serial.begin(9600);
  Wire.begin();
  mpu.initialize();
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250);
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
  
  motor.init();
  encoder.init();
  motor.linkEncoder(&encoder);
}

void loop() {
  // 1. 读取 IMU 数据
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 2. 互补滤波计算角度
  float accelAngle = atan2(ay, az) * RAD_TO_DEG;
  float gyroRate = gx / 131.0; // 转换为°/s
  static float gyroAngle = 0;
  gyroAngle += gyroRate * dt;
  filteredAngle = alpha * (filteredAngle + gyroAngle * dt) + (1 - alpha) * accelAngle;

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

  // 4. 电机控制（限制输出范围）
  motor.move(constrain(output, -10, 10));

  // 调试输出
  Serial.print("Angle: ");
  Serial.print(filteredAngle);
  Serial.print(" Output: ");
  Serial.println(output);

  delay(dt * 1000); // 保持固定采样周期
}

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

#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. 云台稳定系统（单轴）

#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;
  }
}

要点解读

IMU 数据读取与处理 关键点在于单位转换。加速度计数据需用 atan2 计算角度，陀螺仪数据需根据量程设置转换（如 /131.0 对应±250°/s 量程）。优化建议是添加校准程序消除零偏，或使用 DMP（数字运动处理器）硬件解算。

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

PID 控制实现细节 常见问题包括微分项噪声过大，建议使用 (error - previousError)/dt 而非直接微分陀螺仪数据。积分项漂移可通过添加限幅解决。调试技巧是先调 P 参数，再调 D，最后调 I，并通过串口打印观察响应曲线。

实时性保障措施 固定采样周期至关重要。使用 millis() 而非 delay() 实现定时控制，动态调整 dt 但限制最大值。对于低端 MCU（如 Arduino UNO），建议控制频率≤100Hz，避免在循环中使用浮点除法消耗资源。

以上案例旨在拓展思路，实际使用时请根据硬件配置、场景和需求进行调整测试，确保连接安全与参数正确。