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

  
   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. 要点解读

互补滤波的核心机制
- 基本公式：角度 = α × (上次角度 + 陀螺积分) + (1-α) × 加速度计角度
- 系数选择：α=0.98 常用，表示 98% 信任陀螺仪，2% 信任加速度计
- 陀螺零漂补偿：通过比较加速度计角度和陀螺积分角度，自动估计和补偿零漂
- 自适应调整：根据运动状态动态调整α，高速运动时增加陀螺权重
- 有效性检查：检查加速度计模值是否接近 1g，异常时降低其权重
IMU 校准的关键步骤
- 静态校准：设备静止时采集 1000 个样本，计算各轴零偏
- 温度补偿：记录温度变化对零偏的影响（高级应用）
- 尺度校准：通过重力加速度校准加速度计量程
- 正交校准：补偿各轴不正交误差（需要专业设备）
- 在线校准：运行时持续监测和修正零漂
PID 控制的参数整定策略
- Ziegler-Nichols 法：先设 Ki=Kd=0，增加 Kp 直到等幅振荡，然后计算参数
- 试凑法：先调 Kp 到临界振荡，再调 Kd 抑制超调，最后调 Ki 消除静差
- 级联控制：内环控制角速度（快速响应），外环控制角度（精确跟踪）
- 抗饱和处理：积分项限幅，防止深度饱和
- 自适应 PID：根据误差统计自动调整参数
多传感器融合的层次
- 松耦合：各传感器独立处理，结果加权平均（互补滤波）
- 紧耦合：原始数据直接融合（卡尔曼滤波）
- 传感器优先级：加速度计长期准但动态差，陀螺仪短期准但有漂移
- 健康监测：实时评估各传感器可信度，自动降权故障传感器
- 冗余设计：多个同类型传感器互相验证
实时系统的优化技巧
- 定时器中断：使用硬件定时器保证精确的采样周期
- 数据缓冲：双缓冲或环形缓冲处理传感器数据
- 计算优化：使用查表法替代复杂三角函数
- 优先级调度：控制循环最高优先级，显示和通信低优先级
- 内存管理：静态分配避免动态内存碎片
- 看门狗：硬件看门狗防止程序跑飞

注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和 Arduino 版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。

在这里插入图片描述

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

基于 Arduino 平台实现 BLDC 机器人 IMU 角度读取 + PID 控制 + 互补滤波

1. 主要特点

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

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

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

2. 应用场景

3. 注意事项

硬件选型与抗干扰

算法实现细节

机械结构设计

4. 代码示例

1. 两轮自平衡机器人（IMU 角度读取 + PID 控制）

2. 四轴飞行器姿态控制（简化版）

3. 云台稳定系统（单轴）

5. 要点解读

IMU 数据读取与处理

互补滤波实现

PID 控制实现细节

电机控制接口适配

实时性保障措施

6. 基础 IMU 平衡控制

7. 自适应互补滤波

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

9. 要点解读

更多推荐文章

相关免费在线工具

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

基于 Arduino 平台实现 BLDC 机器人 IMU 角度读取 + PID 控制 + 互补滤波

1. 主要特点

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

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

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

2. 应用场景

3. 注意事项

硬件选型与抗干扰

算法实现细节

机械结构设计

4. 代码示例

1. 两轮自平衡机器人（IMU 角度读取 + PID 控制）

2. 四轴飞行器姿态控制（简化版）

3. 云台稳定系统（单轴）

5. 要点解读

IMU 数据读取与处理

互补滤波实现

PID 控制实现细节

电机控制接口适配

实时性保障措施

6. 基础 IMU 平衡控制

7. 自适应互补滤波

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

9. 要点解读

微信扫一扫，关注极客日志

更多推荐文章

相关免费在线工具