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

算法实现细节

• 滤波器参数整定：互补滤波系数 α 需要根据采样频率 dt 调整。α 越大，系统响应越快但漂移越明显；α 越小，系统越稳但动态响应越迟钝。通常先通过公式 α = τ / (τ + dt) 进行估算（τ 为时间常数，通常取 0.1s）。
• PID 参数整定：建议采用'由小到大'的试凑法。先将 I、D 设为 0，逐步增大 P 直到系统开始振荡，然后加入 D 项抑制振荡。对于平衡车，通常 D 项至关重要，用于提供阻尼。
• 采样频率与定时：控制回路的频率必须稳定且足够高（建议 ≥ 100Hz）。严禁在主循环中使用 delay() 函数阻塞程序。应使用硬件定时器中断（如 Timer1）来触发控制周期，确保 dt 的精确性。

机械结构设计

• 重心位置：对于自平衡机器人，重心越高，系统的不稳定性越强，对控制算法的响应速度要求也越高。建议在调试初期尽量降低重心（如将电池放低），待参数调好后再恢复。
• 传动刚性：电机与轮子之间的连接必须牢固，避免皮带过松或齿轮间隙过大。传动系统的弹性会导致相位滞后，容易引发 PID 控制的振荡。

代码示例

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

要点解读

IMU 数据读取与处理

• 关键点：加速度计数据需进行单位转换（如案例 1 的 atan2(ay, az) 计算角度）。陀螺仪数据需根据量程设置转换（如/131.0 对应±250°/s 量程）。
• 优化建议：添加校准程序消除零偏（如开机时采集 500 个样本取平均）。使用 DMP（数字运动处理器）硬件解算（如 MPU6050 的 DMP 模式）。

互补滤波实现

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

PID 控制实现细节

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

电机控制接口适配

• 不同电机类型处理：有刷电机：直接 PWM 输出（如 analogWrite）。无刷电机：需 FOC 库（如案例 1/3 的 SimpleFOC）。舵机：使用 Servo.h 库（如案例 2）。
• 安全限制：必须使用 constrain() 限制输出范围。紧急停止机制（如案例 1 的 constrain(output, -10, 10)）。

实时性保障措施

• 固定采样周期：使用 millis() 而非 delay() 实现定时控制（如所有案例）。动态调整 dt 但限制最大值（如案例 3 的 if(dt>0.1) dt=0.1）。
• 资源优化：避免在循环中使用 float 除法（可预先计算倒数）。对于低端 MCU（如 Arduino UNO），建议控制频率≤100Hz。

4、基础 IMU 平衡控制

场景：自平衡小车基础控制 核心逻辑：MPU6050 + 互补滤波 + 位置式 PID

#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 CalibrationData {
  float accX_offset = 0, accY_offset = 0, accZ_offset = 0;
  float gyroX_offset = 0, gyroY_offset = 0, gyroZ_offset = 0;
  float temperature_offset = 0;
  bool calibrated = false;
};
CalibrationData calib;

// 安全参数
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); // 采样率 1kHz
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_CONFIG, 0x00); // 禁用 DLPF
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_GYRO_CONFIG, 0x18); // ±2000°/s
  mpu6050.writeByte(MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG, 0x10); // ±8g
  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();
  // 计算偏移
  calib.accX_offset = accX_sum / samples;
  calib.accY_offset = accY_sum / samples;
  calib.accZ_offset = accZ_sum / samples;
  calib.gyroX_offset = gyroX_sum / samples;
  calib.gyroY_offset = gyroY_sum / samples;
  calib.gyroZ_offset = gyroZ_sum / samples;

  // 计算重力加速度
  float accZ_calib = (accZ_sum / samples - calib.accZ_offset);
  if(abs(accZ_calib) > 100) { // 应该在 1g 附近
    float scale = 16384.0 / accZ_calib; // 1g = 16384 LSB
    calib.accX_offset *= scale;
    calib.accY_offset *= scale;
  }
  Serial.println("校准完成");
  Serial.print("加速度计偏移：");
  Serial.print(calib.accX_offset);
  Serial.print(", ");
  Serial.print(calib.accY_offset);
  Serial.print(", ");
  Serial.println(calib.accZ_offset);
  Serial.print("陀螺仪偏移：");
  Serial.print(calib.gyroX_offset);
  Serial.print(", ");
  Serial.print(calib.gyroY_offset);
  Serial.print(", ");
  Serial.println(calib.gyroZ_offset);
  calib.calibrated = true;
}

void readRawIMU(int16_t& ax, int16_t& ay, int16_t& az, int16_t& gx, int16_t& gy, int16_t& gz) {
  // 读取原始 IMU 数据
  mpu6050.readBytes(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, 14);
  ax = (mpu6050.buffer[0] << 8) | mpu6050.buffer[1];
  ay = (mpu6050.buffer[2] << 8) | mpu6050.buffer[3];
  az = (mpu6050.buffer[4] << 8) | mpu6050.buffer[5];
  gx = (mpu6050.buffer[8] << 8) | mpu6050.buffer[9];
  gy = (mpu6050.buffer[10] << 8) | mpu6050.buffer[11];
  gz = (mpu6050.buffer[12] << 8) | mpu6050.buffer[13];
}

void loop() {
  static unsigned long lastTime = 0;
  unsigned long now = micros();
  float dt = (now - lastTime) / 1000000.0;
  if(dt >= 0.01) { // 100Hz 控制
    // 1. 读取并处理 IMU 数据
    processIMUData(dt);
    // 2. 互补滤波
    complementaryFilter(dt);
    // 3. PID 控制计算
    pidControl(dt);
    // 4. 应用电机控制
    applyMotorControl();
    // 5. 安全检查
    safetyCheck();
    // 6. 执行 FOC
    motor.loopFOC();
    lastTime = now;
  }
  // 状态显示
  static unsigned long lastDisplay = 0;
  if(millis() - lastDisplay >= 50) { // 20Hz 显示
    displayStatus();
    lastDisplay = millis();
  }
}

void processIMUData(float dt) {
  // 读取并处理 IMU 数据
  int16_t ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
  readRawIMU(ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw);

  // 应用校准
  float ax = (ax_raw - calib.accX_offset) / 16384.0; // 转换为 g
  float ay = (ay_raw - calib.accY_offset) / 16384.0;
  float az = (az_raw - calib.accZ_offset) / 16384.0;
  float gx = (gx_raw - calib.gyroX_offset) / 131.0; // 转换为°/s
  float gy = (gy_raw - calib.gyroY_offset) / 131.0;
  float gz = (gz_raw - calib.gyroZ_offset) / 131.0;

  // 转换为 rad/s
  gx *= PI / 180.0;
  gy *= PI / 180.0;
  gz *= PI / 180.0;

  // 计算加速度计角度
  float acc_mag = sqrt(ax*ax + ay*ay + az*az);
  if(acc_mag > 0.5 && acc_mag < 1.5) { // 检查有效性
    compFilter.accAngle = atan2(ay, az);
  }
  // 更新陀螺仪数据
  compFilter.lastGyroZ = gz;
  // 记录角速度
  compFilter.angleVelocity = gz;
}

void complementaryFilter(float dt) {
  // 互补滤波
  if(!calib.calibrated) return;

  // 陀螺仪积分
  float gyroAngle = compFilter.angle + compFilter.lastGyroZ * dt;

  // 互补滤波融合
  compFilter.angle = compFilter.alpha * gyroAngle + (1.0 - compFilter.alpha) * compFilter.accAngle;

  // 更新采样时间
  compFilter.dt = dt;

  // 零漂补偿
  static float driftIntegral = 0;
  float driftError = compFilter.accAngle - compFilter.angle;
  driftIntegral += driftError * dt;
  driftIntegral = constrain(driftIntegral, -0.1, 0.1);

  // 应用零漂补偿
  compFilter.gyroBias = driftIntegral * 0.01;
  compFilter.lastGyroZ -= compFilter.gyroBias;
}

void pidControl(float dt) {
  // PID 控制计算
  // 设置点（垂直位置）
  balancePID.setpoint = 0.0;
  // 当前角度
  balancePID.input = compFilter.angle;

  // 计算误差
  double error = balancePID.setpoint - balancePID.input;

  // 比例项
  double pTerm = balancePID.kp * error;

  // 积分项
  balancePID.integral += error * dt;

  // 抗饱和
  if(balancePID.antiWindup) {
    if(balancePID.integral > balancePID.integralLimit) {
      balancePID.integral = balancePID.integralLimit;
    } else if(balancePID.integral < -balancePID.integralLimit) {
      balancePID.integral = -balancePID.integralLimit;
    }
  }
  double iTerm = balancePID.ki * balancePID.integral;

  // 微分项
  double dTerm = balancePID.kd * (error - balancePID.lastError) / dt;
  balancePID.lastError = error;

  // 计算输出
  balancePID.output = pTerm + iTerm + dTerm;

  // 输出限制
  if(balancePID.output > balancePID.outputLimit) {
    balancePID.output = balancePID.outputLimit;
  } else if(balancePID.output < -balancePID.outputLimit) {
    balancePID.output = -balancePID.outputLimit;
  }

  // 死区处理
  if(abs(error) < 0.01 && abs(compFilter.angleVelocity) < 0.1) {
    balancePID.output = 0;
  }
}

void applyMotorControl() {
  // 应用电机控制
  if(safety.faultCount >= safety.maxFaults) { // 故障状态，停止电机
    motor.disable();
    return;
  }

  // 启用电机
  motor.enable();

  // 应用 PID 输出
  motor.move(balancePID.output);

  // 记录控制量
  static float lastOutput = 0;
  float outputChange = abs(balancePID.output - lastOutput);
  lastOutput = balancePID.output;

  // 输出变化率限制
  float maxChange = 10.0 * compFilter.dt;
  if(outputChange > maxChange) {
    balancePID.output = lastOutput + (balancePID.output > lastOutput ? maxChange : -maxChange);
  }
}

void safetyCheck() {
  // 安全检查
  // 检查角度
  if(abs(compFilter.angle) > safety.maxAngle) {
    safety.faultCount++;
    Serial.print("角度超限：");
    Serial.print(compFilter.angle * 180 / PI);
    Serial.println("°");
    if(safety.faultCount >= safety.maxFaults) {
      emergencyStop();
    }
  }

  // 检查角速度
  if(abs(compFilter.angleVelocity) > safety.maxAngularVel) {
    safety.faultCount++;
    Serial.print("角速度超限：");
    Serial.print(compFilter.angleVelocity);
    Serial.println("rad/s");
  }

  // 检查 IMU 数据有效性
  static int imuErrorCount = 0;
  int16_t ax, ay, az, gx, gy, gz;
  readRawIMU(ax, ay, az, gx, gy, gz);
  float accMag = sqrt(ax*ax + ay*ay + az*az) / 16384.0;
  if(accMag < 0.8 || accMag > 1.2) { // 应该在 1g 附近
    imuErrorCount++;
    if(imuErrorCount > 5) {
      safety.faultCount++;
      Serial.print("加速度异常：");
      Serial.println(accMag);
    }
  } else {
    imuErrorCount = 0;
  }

  // 自动恢复
  static unsigned long lastFaultTime = 0;
  if(safety.faultCount > 0 && safety.faultCount < safety.maxFaults) {
    if(millis() - lastFaultTime > 1000) { // 1 秒无新故障
      safety.faultCount--;
      lastFaultTime = millis();
    }
  }
}

void emergencyStop() {
  // 紧急停止
  Serial.println("紧急停止激活!");
  motor.disable();
  balancePID.integral = 0;
  balancePID.lastError = 0;
  safety.faultCount = 0;
  // 等待恢复
  delay(1000);
  motor.enable();
}

void displayStatus() {
  // 显示状态
  Serial.print("角度：");
  Serial.print(compFilter.angle * 180 / PI, 1);
  Serial.print("° 角速度：");
  Serial.print(compFilter.angleVelocity, 2);
  Serial.print("rad/s 输出：");
  Serial.print(balancePID.output, 2);
  Serial.print("V 故障：");
  Serial.println(safety.faultCount);
}