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

Arduino BLDC 机器人 IMU 角度读取与 PID 互补滤波控制 | 极客日志

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

基于 Arduino 平台实现 BLDC 机器人 IMU 角度读取、互补滤波及 PID 控制，构成了典型的姿态闭环控制系统。该架构是自平衡机器人（如两轮平衡车、倒立摆）或稳定云台的核心技术栈。它通过互补滤波融合 IMU 原始数据以获得精准姿态角，再利用 PID 控制器计算出维持平衡所需的电机驱动力矩，驱动 BLDC 电机执行动作。

1. 主要特点

传感器融合：互补滤波（Complementary Filter）

这是系统的'感知中枢'，解决了单一传感器无法同时满足动态与静态精度需求的矛盾。

频域分割策略：互补滤波本质上是一个频域滤波器。利用低通滤波（LPF）处理加速度计数据，提取低频的重力方向分量（长期稳定，用于修正漂移）；同时利用高通滤波（HPF）处理陀螺仪数据，提取高频的角速度变化分量（动态响应快，短期精度高）。
时域加权实现：在离散的嵌入式系统中，该过程通常简化为加权平均公式：Angle = α * (Angle + Gyro_Rate * dt) + (1-α) * Accel_Angle。其中 α 通常取值在 0.95~0.98 之间，决定了系统对陀螺仪的信任程度。
优势：相较于复杂的卡尔曼滤波，互补滤波计算量极小，仅需几次乘法和加法，非常适合 Arduino 这类资源受限平台，且参数调节直观。

核心算法：PID 控制器（Proportional-Integral-Derivative）

这是系统的'决策大脑'，负责将姿态误差转化为电机控制指令。

比例项（P）：提供与当前倾角误差成正比的恢复力。这是维持平衡的主力项，决定了系统的'刚度'。P 值过小会导致软瘫（无法站稳），过大则会引起高频振荡。
微分项（D）：提供与倾角变化率（即角速度）成正比的阻尼力。它能预测未来的倾角趋势并提前刹车，有效抑制系统的超调和振荡，使运动更加平滑。
积分项（I）：通常在此类平衡系统中设为 0 或极小值。因为平衡环是快速动态环，积分累积容易导致过冲。但在需要消除静差（如精确位置控制）时，需谨慎加入 I 项并配合抗饱和策略。

执行机构：BLDC 电机闭环驱动

这是系统的'肌肉'，负责将控制信号转化为物理动作。

快速响应特性：BLDC 电机相较于有刷电机，具有更高的功率密度和更快的动态响应速度，能够迅速跟随 PID 控制器输出的 PWM 指令，产生所需的恢复力矩。
驱动模式：通常工作在速度环或电流环（力矩环）模式。对于自平衡机器人，速度环更为常用，通过控制轮子的转速来抵消车身的倾角。

2. 应用场景

两轮自平衡机器人（Segway 类）：最经典的倒立摆应用。系统通过 IMU 检测车身的俯仰角（Pitch），PID 控制器计算出左右轮子的目标转速，驱动 BLDC 电机转动以维持车身在垂直位置的动态平衡。
云台稳定系统（Gimbal）：用于相机或传感器的减震稳定。IMU 检测云台的姿态角，PID 控制器驱动 BLDC 电机反向旋转，以抵消载体（如无人机、手持杆）的晃动，从而保持画面或传感器的水平稳定。
倒立摆实验装置：在自动控制原理的教学实验中，该系统用于验证各种控制算法（如 PID、LQR）的有效性，直观展示开环不稳定系统如何通过闭环控制实现稳定。
双足/多足机器人基座平衡：对于腿部机器人，维持躯干的姿态稳定是行走的基础。该技术栈可用于控制机器人躯干的俯仰和横滚角，防止机器人在迈步时倾倒。

3. 注意事项

硬件选型与抗干扰

IMU 选型与安装：推荐使用集成度高、稳定性好的 MPU6050 或 ICM-20689。IMU 模块必须刚性固定在机器人车身的重心附近，且尽量远离电机和大电流导线，防止振动和电磁干扰（EMI）引入噪声。
电源隔离：BLDC 电机启停时的大电流会拉低电源电压，导致 Arduino 复位或 IMU 数据异常。务必使用独立的稳压电路（如 LM2596 或隔离 DC-DC 模块）为传感器和逻辑电路供电，并在电源端并联大容量电容（如 1000μF）。
电机驱动：严禁使用普通航模 ESC（仅支持油门 PWM，无闭环能力）。必须使用支持闭环控制的驱动方案，如 SimpleFOC 库配合专用驱动板（如 B-G431B-ESC1），或带有编码器反馈的伺服驱动器。

算法实现细节

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

机械结构设计

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

图片描述

4. 代码示例

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() {
  
   ax, ay, az, gx, gy, gz;
  mpu.(&ax, &ay, &az, &gx, &gy, &gz);

  
   accelAngle = (ay, az) * RAD_TO_DEG;
   gyroRate = gx / ; 
    gyroAngle = ;
  gyroAngle += gyroRate * dt;
  filteredAngle = alpha * (filteredAngle + gyroAngle * dt) + ( - alpha) * accelAngle;

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

  
  motor.((output, , ));

  
  Serial.();
  Serial.(filteredAngle);
  Serial.();
  Serial.(output);

  (dt * ); 
}

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

  // 加速度计角度
   accelPitch = (-ax, (ay * ay + az * az)) * RAD_TO_DEG;
   accelRoll = (ay, az) * RAD_TO_DEG;

  
    gyroPitch = , gyroRoll = ;
  gyroPitch += gy /  * ;
  gyroRoll += gx /  * ;

  
  pitchAngle = alpha * (pitchAngle + gyroPitch) + ( - alpha) * accelPitch;
  rollAngle = alpha * (rollAngle + gyroRoll) + ( - alpha) * accelRoll;
}

{
     lastTime = ;
    now = ();
   (now - lastTime >= ) { 
    lastTime = now;

    
    ();

    
      iTerm_pitch = , iTerm_roll = ;
     errorPitch = targetPitch - pitchAngle;
     errorRoll = targetRoll - rollAngle;
    iTerm_pitch += errorPitch * ;
    iTerm_roll += errorRoll * ;
     outputPitch = Kp_pitch * errorPitch + Ki_pitch * iTerm_pitch;
     outputRoll = Kp_roll * errorRoll + Ki_roll * iTerm_roll;

    
     throttle = ; 
     m1 = throttle + outputPitch + outputRoll;
     m2 = throttle - outputPitch + outputRoll;
     m3 = throttle - outputPitch - outputRoll;
     m4 = throttle + outputPitch - outputRoll;

    
     ( i = ; i < ; i++) {
       val = i ==  ? m1 : i ==  ? m2 : i ==  ? m3 : m4;
      motors[i].(((val, , , , ), , ));
    }
  }
}

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 = ; 

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

  
   accelAngle = (ay, az) * RAD_TO_DEG;
   gyroRate = gx / ;
    gyroAngle = ;
  gyroAngle += gyroRate * dt;
  filteredAngle = alpha * (filteredAngle + gyroAngle * dt) + ( - alpha) * accelAngle;

  
    integral = ;
   error = targetAngle - filteredAngle;
  integral += error * dt;
   derivative = (error - (lastTime ? (targetAngle - (alpha * (filteredAngle + gyroAngle * dt) + ( - alpha) * accelAngle)) : )) / dt;
   output = Kp * error + Ki * integral + Kd * derivative;

  
  motor.(output);

  
   (now - lastTime >= ) { 
    Serial.();
    Serial.(targetAngle);
    Serial.();
    Serial.(filteredAngle);
    Serial.();
    Serial.(output);
    lastTime = now;
  }
}

5. 要点解读

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。

图片描述

6. 基础 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; // 加速度计角度
   gyroBias = ; 
   lastGyroZ = ; 
};
ComplementaryFilter compFilter;


  {
   input, output, setpoint;
   kp, ki, kd;
   integral = ;
   lastError = ;
   outputLimit = ;
   integralLimit = ;
   antiWindup = ;
};
PIDControl balancePID = {, , , , , };


  {
   accX_offset = , accY_offset = , accZ_offset = ;
   gyroX_offset = , gyroY_offset = , gyroZ_offset = ;
   temperature_offset = ;
   calibrated = ;
};
CalibrationData calib;


  {
   maxAngle = ; 
   maxAngularVel = ; 
   maxCurrent = ; 
   faultCount = ;
   maxFaults = ;
};
SafetyParams safety;

{
  Serial.();
  Serial.();
  Serial.();

  
  Wire.(IMU_SDA, IMU_SCL);
  ();

  
   (!()) {
    Serial.();
     ();
  }

  
  ();

  
  ();

  
  ();
  Serial.();
  Serial.();
  ();
}

{
  Serial.();
  mpu();
  
   deviceID = mpu(MPU6050_ADDRESS, MPU6050_WHO_AM_I);
   (deviceID != ) {
    Serial.();
    Serial.(deviceID, HEX);
     ;
  }
  
  mpu(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, ); 
  mpu(MPU6050_ADDRESS, MPU6050_SMPLRT_DIV, ); 
  mpu(MPU6050_ADDRESS, MPU6050_CONFIG, ); 
  mpu(MPU6050_ADDRESS, MPU6050_GYRO_CONFIG, ); 
  mpu(MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG, ); 
  Serial.();
   ;
}

{
  Serial.();
  Serial.();
   accX_sum = , accY_sum = , accZ_sum = ;
   gyroX_sum = , gyroY_sum = , gyroZ_sum = ;
   samples = ;
   ( i = ; i < samples; i++) {
    
     ax, ay, az, gx, gy, gz;
    (ax, ay, az, gx, gy, gz);
    accX_sum += ax;
    accY_sum += ay;
    accZ_sum += az;
    gyroX_sum += gx;
    gyroY_sum += gy;
    gyroZ_sum += gz;
    ();
     (i %  == ) Serial.();
  }
  Serial.();
  
  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;
  
   accZ_calib = (accZ_sum / samples - calib.accZ_offset);
   ((accZ_calib) > ) { 
     scale =  / accZ_calib; 
    calib.accX_offset *= scale;
    calib.accY_offset *= scale;
  }
  Serial.();
  Serial.();
  Serial.(calib.accX_offset);
  Serial.();
  Serial.(calib.accY_offset);
  Serial.();
  Serial.(calib.accZ_offset);
  Serial.();
  Serial.(calib.gyroX_offset);
  Serial.();
  Serial.(calib.gyroY_offset);
  Serial.();
  Serial.(calib.gyroZ_offset);
  calib.calibrated = ;
}

{
  
  mpu(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, );
  ax = (mpubuffer[] << ) | mpubuffer[];
  ay = (mpubuffer[] << ) | mpubuffer[];
  az = (mpubuffer[] << ) | mpubuffer[];
  gx = (mpubuffer[] << ) | mpubuffer[];
  gy = (mpubuffer[] << ) | mpubuffer[];
  gz = (mpubuffer[] << ) | mpubuffer[];
}

{
     lastTime = ;
    now = ();
   dt = (now - lastTime) / ;
   (dt >= ) { 
    
    (dt);
    
    (dt);
    
    (dt);
    
    ();
    
    ();
    
    motor.();
    lastTime = now;
  }
  
     lastDisplay = ;
   (() - lastDisplay >= ) { 
    ();
    lastDisplay = ();
  }
}

{
  
   ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
  (ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw);
  
   ax = (ax_raw - calib.accX_offset) / ; 
   ay = (ay_raw - calib.accY_offset) / ;
   az = (az_raw - calib.accZ_offset) / ;
   gx = (gx_raw - calib.gyroX_offset) / ; 
   gy = (gy_raw - calib.gyroY_offset) / ;
   gz = (gz_raw - calib.gyroZ_offset) / ;
  
  gx *= PI / ;
  gy *= PI / ;
  gz *= PI / ;
  
   acc_mag = (ax * ax + ay * ay + az * az);
   (acc_mag >  && acc_mag < ) { 
    compFilter.accAngle = (ay, az);
  }
  
  compFilter.lastGyroZ = gz;
  
  compFilter.angleVelocity = gz;
}

{
  
   (!calib.calibrated) ;
  
   gyroAngle = compFilter.angle + compFilter.lastGyroZ * dt;
  
  compFilter.angle = compFilter.alpha * gyroAngle + ( - compFilter.alpha) * compFilter.accAngle;
  
  compFilter.dt = dt;
  
    driftIntegral = ;
   driftError = compFilter.accAngle - compFilter.angle;
  driftIntegral += driftError * dt;
  driftIntegral = (driftIntegral, , );
  
  compFilter.gyroBias = driftIntegral * ;
  compFilter.lastGyroZ -= compFilter.gyroBias;
}

{
  
  
  balancePID.setpoint = ;
  
  balancePID.input = compFilter.angle;
  
   error = balancePID.setpoint - balancePID.input;
  
   pTerm = balancePID.kp * error;
  
  balancePID.integral += error * dt;
  
   (balancePID.antiWindup) {
     (balancePID.integral > balancePID.integralLimit) {
      balancePID.integral = balancePID.integralLimit;
    }   (balancePID.integral < -balancePID.integralLimit) {
      balancePID.integral = -balancePID.integralLimit;
    }
  }
   iTerm = balancePID.ki * balancePID.integral;
  
   dTerm = balancePID.kd * (error - balancePID.lastError) / dt;
  balancePID.lastError = error;
  
  balancePID.output = pTerm + iTerm + dTerm;
  
   (balancePID.output > balancePID.outputLimit) {
    balancePID.output = balancePID.outputLimit;
  }   (balancePID.output < -balancePID.outputLimit) {
    balancePID.output = -balancePID.outputLimit;
  }
  
   ((error) <  && (compFilter.angleVelocity) < ) {
    balancePID.output = ;
  }
}

{
  
   (safety.faultCount >= safety.maxFaults) { 
    motor.();
    ;
  }
  
  motor.();
  
  motor.(balancePID.output);
  
    lastOutput = ;
   outputChange = (balancePID.output - lastOutput);
  lastOutput = balancePID.output;
  
   maxChange =  * compFilter.dt;
   (outputChange > maxChange) {
    balancePID.output = lastOutput + (balancePID.output > lastOutput ? maxChange : -maxChange);
  }
}

{
  
  
   ((compFilter.angle) > safety.maxAngle) {
    safety.faultCount++;
    Serial.();
    Serial.(compFilter.angle *  / PI);
    Serial.();
     (safety.faultCount >= safety.maxFaults) {
      ();
    }
  }
  
   ((compFilter.angleVelocity) > safety.maxAngularVel) {
    safety.faultCount++;
    Serial.();
    Serial.(compFilter.angleVelocity);
    Serial.();
  }
  
    imuErrorCount = ;
   ax, ay, az, gx, gy, gz;
  (ax, ay, az, gx, gy, gz);
   accMag = (ax * ax + ay * ay + az * az) / ;
   (accMag <  || accMag > ) { 
    imuErrorCount++;
     (imuErrorCount > ) {
      safety.faultCount++;
      Serial.();
      Serial.(accMag);
    }
  }  {
    imuErrorCount = ;
  }
  
     lastFaultTime = ;
   (safety.faultCount >  && safety.faultCount < safety.maxFaults) {
     (() - lastFaultTime > ) { 
      safety.faultCount--;
      lastFaultTime = ();
    }
  }
}

{
  
  Serial.();
  motor.();
  balancePID.integral = ;
  balancePID.lastError = ;
  safety.faultCount = ;
  
  ();
  motor.();
}

{
  
  Serial.();
  Serial.(compFilter.angle *  / PI, );
  Serial.();
  Serial.(compFilter.angleVelocity, );
  Serial.();
  Serial.(balancePID.output, );
  Serial.();
  Serial.(safety.faultCount);
}

7. 自适应互补滤波

场景：动态环境下的精确姿态估计。核心逻辑：自适应滤波系数 + 多传感器融合。

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

// Madgwick 滤波
Madgwick madgwick;
#define BETA_DEF 0.1f // 默认 beta 值

// 自适应滤波参数
struct AdaptiveFilter {
  float alpha = 0.98f; // 基础滤波系数
  float alpha_min = 0.90f; // 最小滤波系数
  float alpha_max = 0.995f; // 最大滤波系数
  float beta = BETA_DEF; // Madgwick beta
  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;

// 传感器方差估计
struct VarianceEstimator {
   acc_var[] = {, , }; 
   gyro_var[] = {, , }; 
   mag_var[] = {, , }; 
  
   acc_window[][] = {};
   gyro_window[][] = {};
   window_index = ;
  {
    
    acc_window[][window_index] = ax;
    acc_window[][window_index] = ay;
    acc_window[][window_index] = az;
    gyro_window[][window_index] = gx;
    gyro_window[][window_index] = gy;
    gyro_window[][window_index] = gz;
    window_index = (window_index + ) % ;
    
     ( i = ; i < ; i++) {
       mean_acc = , mean_gyro = ;
       var_acc = , var_gyro = ;
       ( j = ; j < ; j++) {
        mean_acc += acc_window[i][j];
        mean_gyro += gyro_window[i][j];
      }
      mean_acc /= ;
      mean_gyro /= ;
       ( j = ; j < ; j++) {
        var_acc += (acc_window[i][j] - mean_acc) * (acc_window[i][j] - mean_acc);
        var_gyro += (gyro_window[i][j] - mean_gyro) * (gyro_window[i][j] - mean_gyro);
      }
      acc_var[i] = var_acc / ;
      gyro_var[i] = var_gyro / ;
    }
  }
};
VarianceEstimator varianceEst;


  {
:
   kp, ki, kd;
   integral = ;
   lastError = ;
   lastOutput = ;
  
   adaptive_kp = ;
   adaptive_ki = ;
   adaptive_kd = ;
   learning_rate = ;
  
   error_history[] = {};
   error_index = ;
   performance = ;
:
  ( p,  i,  d) : (p), (i), (d) {}
  {
    
    error_history[error_index] = error;
    error_index = (error_index + ) % ;
    
    (error, angular_vel, dt);
    
    integral += error * dt;
    
     integral_limit = ;
     (integral > integral_limit) integral = integral_limit;
     (integral < -integral_limit) integral = -integral_limit;
    
     derivative = ;
     (dt > ) {
      derivative = (error - lastError) / dt;
    }
    lastError = error;
    
     output = (kp * adaptive_kp) * error + (ki * adaptive_ki) * integral + (kd * adaptive_kd) * derivative;
    
    output -= angular_vel * ;
    
     output_limit = ;
     (output > output_limit) output = output_limit;
     (output < -output_limit) output = -output_limit;
    
     max_change =  * dt;
     change = output - lastOutput;
     ((change) > max_change) {
      output = lastOutput + (change >  ? max_change : -max_change);
    }
    lastOutput = output;
     output;
  }
  {
    integral = ;
    lastError = ;
    lastOutput = ;
  }
:
  {
    
     mean_error = ;
     ( i = ; i < ; i++) {
      mean_error += error_history[i];
    }
    mean_error /= ;
     variance = ;
     ( i = ; i < ; i++) {
       diff = error_history[i] - mean_error;
      variance += diff * diff;
    }
    variance /= ;
    
     (variance < ) { 
      adaptive_kp *= ; 
    }   (variance > ) { 
      adaptive_kp *= ; 
    }
    
     ((angular_vel) > ) {
      adaptive_kd = ; 
    }  {
      adaptive_kd = ;
    }
    
    adaptive_kp = (adaptive_kp, , );
    adaptive_ki = (adaptive_ki, , );
    adaptive_kd = (adaptive_kd, , );
  }
};
;


{
  
   acc_mag = (ax * ax + ay * ay + az * az);
   acc_confidence = ;
   ((acc_mag - ) > ) { 
    acc_confidence = ; 
  }
  
   dynamic_alpha = adaptiveFilter.alpha;
   (acc_confidence < ) { 
    dynamic_alpha = ;
  }  { 
    dynamic_alpha = ;
  }
  
  dynamic_alpha = (dynamic_alpha, adaptiveFilter.alpha_min, adaptiveFilter.alpha_max);
  
   acc_roll = (ay, az);
   acc_pitch = (-ax, (ay * ay + az * az));
  
   gyro_roll = adaptiveFilter.roll + gx * dt;
   gyro_pitch = adaptiveFilter.pitch + gy * dt;
   gyro_yaw = adaptiveFilter.yaw + gz * dt;
  
  adaptiveFilter.roll = dynamic_alpha * gyro_roll + ( - dynamic_alpha) * acc_roll;
  adaptiveFilter.pitch = dynamic_alpha * gyro_pitch + ( - dynamic_alpha) * acc_pitch;
  
  adaptiveFilter.yaw = gyro_yaw;
  
  adaptiveFilter.angularVelocity[] = gx;
  adaptiveFilter.angularVelocity[] = gy;
  adaptiveFilter.angularVelocity[] = gz;
  
   roll_innovation = acc_roll - adaptiveFilter.roll;
   pitch_innovation = acc_pitch - adaptiveFilter.pitch;
  adaptiveFilter.innovation = (roll_innovation * roll_innovation + pitch_innovation * pitch_innovation);
  
   (adaptiveFilter.innovation > adaptiveFilter.innovation_threshold) { 
    adaptiveFilter.beta = (BETA_DEF * , );
  }  {
    adaptiveFilter.beta = BETA_DEF;
  }
  
  varianceEst.(ax, ay, az, gx, gy, gz);
}


{
  
  madgwick.beta = adaptiveFilter.beta;
  madgwick.(gx, gy, gz, ax, ay, az, mx, my, mz);
  
  adaptiveFilter.q0 = madgwick.q0;
  adaptiveFilter.q1 = madgwick.q1;
  adaptiveFilter.q2 = madgwick.q2;
  adaptiveFilter.q3 = madgwick.q3;
  
  (adaptiveFilter.q0, adaptiveFilter.q1, adaptiveFilter.q2, adaptiveFilter.q3, adaptiveFilter.roll, adaptiveFilter.pitch, adaptiveFilter.yaw);
}

{
  
  roll = ( * (q0 * q1 + q2 * q3),  -  * (q1 * q1 + q2 * q2));
  pitch = ( * (q0 * q2 - q3 * q1));
  yaw = ( * (q0 * q3 + q1 * q2),  -  * (q2 * q2 + q3 * q3));
}


{
  
   ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw;
  (ax_raw, ay_raw, az_raw, gx_raw, gy_raw, gz_raw);
  
   ax = (ax_raw - calib.accX_offset) / ;
   ay = (ay_raw - calib.accY_offset) / ;
   az = (az_raw - calib.accZ_offset) / ;
   gx = (gx_raw - calib.gyroX_offset) * PI / ( * );
   gy = (gy_raw - calib.gyroY_offset) * PI / ( * );
   gz = (gz_raw - calib.gyroZ_offset) * PI / ( * );
  
    use_madgwick = ;
   (use_madgwick) {
    
    (gx, gy, gz, ax, ay, az, , , , dt);
  }  {
    
    (ax, ay, az, gx, gy, gz, dt);
  }
  
    stationary_count = ;
   motion_level = (gx * gx + gy * gy + gz * gz);
   (motion_level < ) { 
    stationary_count++;
     (stationary_count > ) { 
      use_madgwick = ; 
    }
  }  {
    stationary_count = ;
    use_madgwick = ; 
  }
}

8. 多传感器融合卡尔曼滤波

场景：高精度姿态估计，需要最优估计。核心逻辑：扩展卡尔曼滤波 + 多传感器数据融合。

#include <SimpleFOC.h>
#include <BasicLinearAlgebra.h> // 线性代数库
#include <math.h>

// 使用 BLA 库
using namespace BLA;

// 扩展卡尔曼滤波器
class ExtendedKalmanFilter {
private:
  // 状态：[q0, q1, q2, q3, gx_bias, gy_bias, gz_bias]
  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.();
    P *= ;
    
    Q.();
    (, ) = (, ) = (, ) = (, ) = ; 
    (, ) = (, ) = (, ) = ; 
    
    R.();
    (, ) = (, ) = (, ) = ; 
    (, ) = (, ) = (, ) = ; 
    
    ();
  }
  {
    ->dt = dt;
    
    (gx, gy, gz);
    
    (ax, ay, az, mx, my, mz);
  }
  {
    
     q0 = (), q1 = (), q2 = (), q3 = ();
    
     norm = (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 /= norm;
    q1 /= norm;
    q2 /= norm;
    q3 /= norm;
    
    roll = ( * (q0 * q1 + q2 * q3),  -  * (q1 * q1 + q2 * q2));
    pitch = ( * (q0 * q2 - q3 * q1));
    yaw = ( * (q0 * q3 + q1 * q2),  -  * (q2 * q2 + q3 * q3));
  }
  {
     (axis >=  && axis < ) {
       ( + axis); 
    }
     ;
  }
:
  {
    
    
     wx = gx - ();
     wy = gy - ();
     wz = gz - ();
    
     q0 = (), q1 = (), q2 = (), q3 = ();
     q0_dot =  * (-q1 * wx - q2 * wy - q3 * wz);
     q1_dot =  * (q0 * wx - q3 * wy + q2 * wz);
     q2_dot =  * (q3 * wx + q0 * wy - q1 * wz);
     q3_dot =  * (-q2 * wx + q1 * wy + q0 * wz);
    
    () += q0_dot * dt;
    () += q1_dot * dt;
    () += q2_dot * dt;
    () += q3_dot * dt;
    
    
    ();
    
    P = F * P * ~F + Q;
  }
  {
    
    
    Matrix<MEAS_DIM, > z = {ax, ay, az, mx, my, mz};
    
    Matrix<MEAS_DIM, > z_pred = ();
    
    Matrix<MEAS_DIM, > y = z - z_pred;
    
    Matrix<MEAS_DIM, MEAS_DIM> S = H * P * ~H + R;
    Matrix<STATE_DIM, MEAS_DIM> K = P * ~H * (S);
    
    x = x + K * y;
    
    Matrix<STATE_DIM, STATE_DIM> I;
    I.();
    P = (I - K * H) * P;
    
    ();
  }
  {
    
    Matrix<MEAS_DIM, > z_pred;
     q0 = (), q1 = (), q2 = (), q3 = ();
    
    () =  * (q1 * q3 - q0 * q2);
    () =  * (q0 * q1 + q2 * q3);
    () = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;
    
    () =  * (q0 * q3 + q1 * q2);
    () = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;
    () =  * (q2 * q3 - q0 * q1);
     z_pred;
  }
  {
    
    F.();
     q0 = (), q1 = (), q2 = (), q3 = ();
     wx = , wy = , wz = ; 
    
    (, ) = ;
    (, ) = -wx * dt / ;
    (, ) = -wy * dt / ;
    (, ) = -wz * dt / ;
    (, ) = wx * dt / ;
    (, ) = ;
    (, ) = wz * dt / ;
    (, ) = -wy * dt / ;
    (, ) = wy * dt / ;
    (, ) = -wz * dt / ;
    (, ) = ;
    (, ) = wx * dt / ;
    (, ) = wz * dt / ;
    (, ) = wy * dt / ;
    (, ) = -wx * dt / ;
    (, ) = ;
    
    (, ) = q1 * dt / ;
    (, ) = q2 * dt / ;
    (, ) = q3 * dt / ;
    (, ) = -q0 * dt / ;
    (, ) = q3 * dt / ;
    (, ) = -q2 * dt / ;
    (, ) = -q3 * dt / ;
    (, ) = -q0 * dt / ;
    (, ) = q1 * dt / ;
    (, ) = q2 * dt / ;
    (, ) = -q1 * dt / ;
    (, ) = -q0 * dt / ;
    
    ();
  }
  {
    
    H.();
     q0 = (), q1 = (), q2 = (), q3 = ();
    
    (, ) =  * q2;
    (, ) =  * q3;
    (, ) =  * q0;
    (, ) =  * q1;
    (, ) =  * q1;
    (, ) =  * q0;
    (, ) =  * q3;
    (, ) =  * q2;
    (, ) =  * q0;
    (, ) =  * q1;
    (, ) =  * q2;
    (, ) =  * q3;
    
    (, ) =  * q3;
    (, ) =  * q2;
    (, ) =  * q1;
    (, ) =  * q0;
    (, ) =  * q0;
    (, ) =  * q1;
    (, ) =  * q2;
    (, ) =  * q3;
    (, ) =  * q1;
    (, ) =  * q0;
    (, ) =  * q3;
    (, ) =  * q2;
  }
  {
    
     q0 = (), q1 = (), q2 = (), q3 = ();
     norm = (q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
     (norm > ) {
      () /= norm;
      () /= norm;
      () /= norm;
      () /= norm;
    }  {
      () = ;
      () = () = () = ;
    }
  }
};
ExtendedKalmanFilter ekf;


  {
:
  
    {
     kp, ki, kd;
     integral = ;
     lastError = ;
     output = ;
  } innerPID = {, , };
  
    {
     kp, ki, kd;
     integral = ;
     lastError = ;
     output = ;
  } outerPID = {, , };
  
   feedforward = ;
:
  {
    
     angleError = targetAngle - currentAngle;
    outerPID.integral += angleError * dt;
    outerPID.integral = (outerPID.integral, , );
     angleDeriv = (angleError - outerPID.lastError) / dt;
    outerPID.lastError = angleError;
    
     targetAngularVel = outerPID.kp * angleError + outerPID.ki * outerPID.integral + outerPID.kd * angleDeriv;
    
     velocityError = targetAngularVel - angularVelocity;
    innerPID.integral += velocityError * dt;
    innerPID.integral = (innerPID.integral, , );
     velocityDeriv = (velocityError - innerPID.lastError) / dt;
    innerPID.lastError = velocityError;
    
    innerPID.output = innerPID.kp * velocityError + innerPID.ki * innerPID.integral + innerPID.kd * velocityDeriv;
    
    innerPID.output += feedforward * targetAngularVel;
     innerPID.output;
  }
  {
    innerPID.integral = ;
    innerPID.lastError = ;
    outerPID.integral = ;
    outerPID.lastError = ;
  }
};
CascadePID cascadeController;


{
  
   ax, ay, az, gx, gy, gz, mx, my, mz;
  (ax, ay, az, gx, gy, gz, mx, my, mz);
  
  ekf.(gx, gy, gz, ax, ay, az, mx, my, mz, dt);
  
   roll, pitch, yaw;
  ekf.(roll, pitch, yaw);
   angularVelX = ekf.();
   angularVelY = ekf.();
   angularVelZ = ekf.();
  
   targetAngle = ; 
   currentAngle = pitch; 
   control = cascadeController.(targetAngle, currentAngle, angularVelY, dt);
  
  (control, dt);
  
  (ax, ay, az, gx, gy, gz, mx, my, mz, dt);
}


  {
:
    {
     mean[] = {};
     variance[] = {};
     health = ; 
     errorCount = ;
  };
  SensorStats accStats, gyroStats, magStats;
  
     WINDOW_SIZE = ;
   accHistory[][WINDOW_SIZE] = {};
   gyroHistory[][WINDOW_SIZE] = {};
   magHistory[][WINDOW_SIZE] = {};
   historyIndex = ;
:
  {
    
    accHistory[][historyIndex] = ax;
    accHistory[][historyIndex] = ay;
    accHistory[][historyIndex] = az;
    gyroHistory[][historyIndex] = gx;
    gyroHistory[][historyIndex] = gy;
    gyroHistory[][historyIndex] = gz;
    magHistory[][historyIndex] = mx;
    magHistory[][historyIndex] = my;
    magHistory[][historyIndex] = mz;
    historyIndex = (historyIndex + ) % WINDOW_SIZE;
    
    (accStats, accHistory);
    (gyroStats, gyroHistory);
    (magStats, magHistory);
    
    ();
  }
  {
     (sensorType) {
     :  accStats.health;
     :  gyroStats.health;
     :  magStats.health;
    :  ;
    }
  }
:
  {
    
     ( i = ; i < ; i++) {
       sum = , sumSq = ;
       ( j = ; j < WINDOW_SIZE; j++) {
        sum += history[i][j];
        sumSq += history[i][j] * history[i][j];
      }
      stats.mean[i] = sum / WINDOW_SIZE;
      stats.variance[i] = (sumSq / WINDOW_SIZE) - (stats.mean[i] * stats.mean[i]);
    }
  }
  {
    
     accMag = (accStats.mean[] * accStats.mean[] + accStats.mean[] * accStats.mean[] + accStats.mean[] * accStats.mean[]);
     ((accMag - ) < ) {
      accStats.health = ;
      accStats.errorCount = ;
    }  {
      accStats.errorCount++;
      accStats.health = (,  - accStats.errorCount * );
    }
    
     gyroVar = gyroStats.variance[] + gyroStats.variance[] + gyroStats.variance[];
     (gyroVar < ) {
      gyroStats.health = ;
    }  {
      gyroStats.health =  / ( + gyroVar);
    }
    
     magMag = (magStats.mean[] * magStats.mean[] + magStats.mean[] * magStats.mean[] + magStats.mean[] * magStats.mean[]);
     (magMag >  && magMag < ) {
      magStats.health = ;
    }  {
      magStats.health = ;
    }
  }
};

9. 总结

本文详细介绍了基于 Arduino 平台的 BLDC 机器人姿态控制系统，涵盖了 IMU 数据读取、互补滤波融合及 PID 控制算法。内容包括两轮自平衡机器人、四轴飞行器及云台稳定系统的应用场景，提供了 MPU6050 传感器驱动、滤波器参数整定、PID 调参策略及卡尔曼滤波扩展等核心代码示例。重点讲解了硬件选型、抗干扰措施、实时性保障及安全保护机制，适用于嵌入式控制开发参考。

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