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

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，避免在循环中使用浮点除法消耗资源。

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