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

进阶场景：四轴飞行器姿态控制

如果是四轴飞行器，我们需要同时处理俯仰（Pitch）和滚转（Roll）。这里使用舵机模拟电机输出，逻辑类似但涉及混合矩阵。

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

云台稳定系统（单轴）

云台需要更平滑的控制，通常目标角度是固定的（如水平 90 度）。这里我们增加了 dt 的动态调整和安全限制。

#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 - (alpha * (filteredAngle + gyroAngle * dt) + (1 - alpha) * accelAngle)) : 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;
  }
}

高级方案：自适应与卡尔曼滤波

当环境复杂时，简单的互补滤波可能不够。我们可以引入自适应系数或扩展卡尔曼滤波（EKF）来提高精度。

自适应互补滤波

根据运动状态动态调整 α。静止时用 Madgwick 滤波校准，运动时用互补滤波保证实时性。

#include <SimpleFOC.h>
#include <MPU6050_tockn.h>
#include <MadgwickAHRS.h>

// ... (省略部分头文件定义)

// 自适应滤波参数
struct AdaptiveFilter {
  float alpha = 0.98f;
  float alpha_min = 0.90f;
  float alpha_max = 0.995f;
  float beta = 0.1f;
  float innovation = 0.0f;
  float innovation_threshold = 0.1f;
  float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;
  float roll = 0.0f, pitch = 0.0f, yaw = 0.0f;
  float angularVelocity[3] = {0, 0, 0};
};
AdaptiveFilter adaptiveFilter;

// ... (其他结构体定义)

// 自适应互补滤波函数
void adaptiveComplementaryFilter(float ax, float ay, float az, float gx, float gy, float gz, float dt) {
  // 计算加速度置信度
  float acc_mag = sqrt(ax * ax + ay * ay + az * az);
  float acc_confidence = 1.0f;
  if (abs(acc_mag - 1.0f) > 0.2f) {
    acc_confidence = 0.1f; // 降低置信度
  }

  // 计算动态滤波系数
  float dynamic_alpha = adaptiveFilter.alpha;
  if (acc_confidence < 0.5f) {
    dynamic_alpha = 0.995f; // 增加陀螺权重
  } else {
    dynamic_alpha = 0.98f;
  }
  dynamic_alpha = constrain(dynamic_alpha, adaptiveFilter.alpha_min, adaptiveFilter.alpha_max);

  // 计算角度
  float acc_roll = atan2(ay, az);
  float acc_pitch = atan2(-ax, sqrt(ay * ay + az * az));
  float gyro_roll = adaptiveFilter.roll + gx * dt;
  float gyro_pitch = adaptiveFilter.pitch + gy * dt;

  // 融合
  adaptiveFilter.roll = dynamic_alpha * gyro_roll + (1.0f - dynamic_alpha) * acc_roll;
  adaptiveFilter.pitch = dynamic_alpha * gyro_pitch + (1.0f - dynamic_alpha) * acc_pitch;
  adaptiveFilter.yaw = gyro_yaw; // 简化的航向积分

  // 更新角速度
  adaptiveFilter.angularVelocity[0] = gx;
  adaptiveFilter.angularVelocity[1] = gy;
  adaptiveFilter.angularVelocity[2] = gz;
}

扩展卡尔曼滤波 (EKF)

对于高精度需求，可以使用 EKF 融合多传感器数据（加速度计、磁力计、陀螺仪），并估计零偏。

#include <SimpleFOC.h>
#include <BasicLinearAlgebra.h>
#include <math.h>

using namespace BLA;

class ExtendedKalmanFilter {
private:
  static const int STATE_DIM = 7;
  static const int MEAS_DIM = 6;
  Matrix<STATE_DIM, 1> x;
  Matrix<STATE_DIM, STATE_DIM> P;
  Matrix<STATE_DIM, STATE_DIM> F;
  Matrix<STATE_DIM, STATE_DIM> Q;
  Matrix<MEAS_DIM, STATE_DIM> H;
  Matrix<MEAS_DIM, MEAS_DIM> R;
  float dt = 0.01f;

public:
  ExtendedKalmanFilter() { init(); }

  void init() {
    x = {1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
    P.Identity(); P *= 0.1f;
    Q.Identity(); Q(0, 0) = Q(1, 1) = Q(2, 2) = Q(3, 3) = 1e-6f;
    Q(4, 4) = Q(5, 5) = Q(6, 6) = 1e-8f;
    R.Identity(); R(0, 0) = R(1, 1) = R(2, 2) = 0.1f;
    R(3, 3) = R(4, 4) = R(5, 5) = 0.5f;
    updateJacobians();
  }

  void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz, float dt) {
    this->dt = dt;
    predict(gx, gy, gz);
    updateMeasurement(ax, ay, az, mx, my, mz);
  }

  void getEulerAngles(float &roll, float &pitch, float &yaw) {
    float q0 = x(0), q1 = x(1), q2 = x(2), q3 = x(3);
    float norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
    roll = atan2(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2));
    pitch = asin(2.0f * (q0 * q2 - q3 * q1));
    yaw = atan2(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3));
  }

  // ... (predict, updateMeasurement 等内部方法略，逻辑同上)
};

ExtendedKalmanFilter ekf;

关键要点总结

在实际调试中，有几个细节往往决定了成败：

1. 互补滤波机制：核心公式是 angle = α × (上次角度 + 陀螺积分) + (1-α) × 加速度计角度。α=0.98 表示信任陀螺仪为主，但需配合零漂补偿策略。
2. IMU 校准：静态校准必不可少。设备静止时采集样本计算零偏，必要时加入温度补偿和尺度校准。
3. PID 整定：建议采用'由小到大'的试凑法。先设 I、D 为 0，增大 P 直到临界振荡，再加入 D 抑制超调。对于平衡车，D 项至关重要。
4. 实时性保障：严禁在主循环中使用 delay() 阻塞。应使用硬件定时器中断或 millis() 精确控制采样周期（建议 ≥100Hz）。
5. 安全保护：必须设置输出限幅和紧急停止机制。例如检测到角度超限或 IMU 数据异常时，立即切断电机电源。

实际应用中，代码需要根据具体硬件配置（如电机类型、传感器型号）进行调整。务必确认引脚连接正确，并在通电前检查电源隔离情况，避免大电流干扰导致复位。