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

示例二：四轴飞行器姿态控制（简化版）

涉及多轴混合输出，需注意电机布局与 PID 分配。

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

MPU6050 mpu;
Servo motors[4];

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);
    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) {
    lastTime = now;
    computeAngles();
    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;
    int throttle = 1100;
    int m1 = throttle + outputPitch + outputRoll;
    int m2 = throttle - outputPitch + outputRoll;
    int m3 = throttle - outputPitch - outputRoll;
    int m4 = throttle + outputPitch - outputRoll;
    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));
    }
  }
}

示例三：云台稳定系统

单轴控制，重点在于目标角度设定与平滑度。

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

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

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;

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

  float accelAngle = atan2(ay, az) * RAD_TO_DEG;
  float gyroRate = gx / 131.0;
  static float gyroAngle = 0;
  gyroAngle += gyroRate * dt;
  filteredAngle = alpha * (filteredAngle + gyroAngle) + (1 - alpha) * accelAngle;

  static float integral = 0;
  float error = targetAngle - filteredAngle;
  integral += error * dt;
  float derivative = (error - 0) / dt;
  float output = Kp * error + Ki * integral + Kd * derivative;

  motor.move(output);
  if(now - lastTime >= 50) {
    Serial.print("Target: "); Serial.print(targetAngle);
    Serial.print(" Actual: "); Serial.print(filteredAngle);
    Serial.print(" Output: "); Serial.println(output);
    lastTime = now;
  }
}

高级策略与调试建议

对于高精度需求，可引入自适应互补滤波或扩展卡尔曼滤波（EKF）。自适应滤波能根据运动状态动态调整权重，EKF 则能融合多传感器数据获得最优估计。

1. IMU 校准：开机静止采集样本计算零偏。
2. PID 调参：先 P 后 D 再 I，观察串口波形。
3. 实时性：使用 millis() 或定时器，避免阻塞。
4. 安全机制：增加故障计数与紧急停止逻辑。

实际编程时，请根据硬件配置调整引脚与参数，确保连接安全。