【花雕学编程】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 控制的振荡。
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;voidsetup(){ 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);}voidloop(){// 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;// 转换为°/sstaticfloat 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;voidsetup(){ 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);// 初始中立位置}}voidcomputeAngles(){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;// 陀螺仪积分staticfloat 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;}voidloop(){staticunsignedlong lastTime =0;unsignedlong now =millis();if(now - lastTime >=10){// 100Hz控制频率 lastTime = now;// 1. 计算姿态角computeAngles();// 2. PID控制（简化版：仅俯仰和滚转）staticfloat 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++){ motors[i].write(constrain(map(i==0?m1:i==1?m2:i==2?m3:m4,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;voidsetup(){ Serial.begin(115200); Wire.begin(); mpu.initialize(); motor.init(); encoder.init(); motor.linkEncoder(&encoder);}voidloop(){staticunsignedlong lastTime =0;unsignedlong now =millis();float dt =(now - lastTime)/1000.0;if(dt >0.1) dt =0.1;// 限制最大dt// 1. 读取IMUint16_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;staticfloat gyroAngle =0; gyroAngle += gyroRate * dt; filteredAngle = alpha *(filteredAngle + gyroAngle)+(1- alpha)* accelAngle;// 3. PID控制staticfloat 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);#defineIMU_SDA21#defineIMU_SCL22// BLDC电机 BLDCMotor motor(7); Encoder encoder(2,3,2048);voiddoA(){ encoder.handleA();}voiddoB(){ encoder.handleB();}// 互补滤波参数structComplementaryFilter{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控制器参数structPIDControl{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};// 校准数据structCalibrationData{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;// 安全参数structSafetyParams{float maxAngle =0.5;// 最大允许角度 30°float maxAngularVel =3.0;// 最大角速度float maxCurrent =5.0;// 最大电流int faultCount =0;int maxFaults =10;}; SafetyParams safety;voidsetup(){ Serial.begin(115200); Serial.println("===== IMU角度读取 + PID控制 + 互补滤波 ====="); Serial.println("基于MPU6050的自平衡控制系统");// 1. 初始化I2C Wire.begin(IMU_SDA, IMU_SCL);delay(100);// 2. 初始化IMUif(!initIMU()){ Serial.println("IMU初始化失败!");while(1);}// 3. 校准IMUcalibrateIMU();// 4. 初始化电机initMotor();// 5. 初始化滤波器initFilter(); Serial.println("系统初始化完成"); Serial.println("等待3秒后开始平衡...");delay(3000);}boolinitIMU(){ 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);returnfalse;}// 配置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初始化成功");returntrue;}voidcalibrateIMU(){ 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;}voidreadRawIMU(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];}voidloop(){staticunsignedlong lastTime =0;unsignedlong 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;}// 状态显示staticunsignedlong lastDisplay =0;if(millis()- lastDisplay >=50){// 20Hz显示displayStatus(); lastDisplay =millis();}}voidprocessIMUData(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;// 转换为gfloat 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;// 转换为°/sfloat 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;}voidcomplementaryFilter(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;// 零漂补偿staticfloat 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;}voidpidControl(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;}elseif(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;}elseif(balancePID.output <-balancePID.outputLimit){ balancePID.output =-balancePID.outputLimit;}// 死区处理if(abs(error)<0.01&&abs(compFilter.angleVelocity)<0.1){ balancePID.output =0;}}voidapplyMotorControl(){// 应用电机控制if(safety.faultCount >= safety.maxFaults){// 故障状态，停止电机 motor.disable();return;}// 启用电机 motor.enable();// 应用PID输出 motor.move(balancePID.output);// 记录控制量staticfloat 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);}}voidsafetyCheck(){// 安全检查// 检查角度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数据有效性staticint 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;}// 自动恢复staticunsignedlong lastFaultTime =0;if(safety.faultCount >0&& safety.faultCount < safety.maxFaults){if(millis()- lastFaultTime >1000){// 1秒无新故障 safety.faultCount--; lastFaultTime =millis();}}}voidemergencyStop(){// 紧急停止 Serial.println("紧急停止激活!"); motor.disable(); balancePID.integral =0; balancePID.lastError =0; safety.faultCount =0;// 等待恢复delay(1000); motor.enable();}voiddisplayStatus(){// 显示状态 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);}5、自适应互补滤波
场景：动态环境下的精确姿态估计
核心逻辑：自适应滤波系数 + 多传感器融合
#include<SimpleFOC.h>#include<MPU6050_tockn.h>#include<MadgwickAHRS.h>// Madgwick滤波 Madgwick madgwick;#defineBETA_DEF0.1f// 默认beta值// 自适应滤波参数structAdaptiveFilter{float alpha =0.98f;// 基础滤波系数float alpha_min =0.90f;// 最小滤波系数float alpha_max =0.995f;// 最大滤波系数float beta = BETA_DEF;// Madgwick betafloat 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;// 传感器方差估计structVarianceEstimator{float acc_var[3]={0.01f,0.01f,0.01f};// 加速度方差float gyro_var[3]={0.001f,0.001f,0.001f};// 陀螺方差float mag_var[3]={0.1f,0.1f,0.1f};// 磁力计方差// 在线估计float acc_window[3][10]={0};float gyro_window[3][10]={0};int window_index =0;voidupdate(float ax,float ay,float az,float gx,float gy,float gz){// 更新滑动窗口 acc_window[0][window_index]= ax; acc_window[1][window_index]= ay; acc_window[2][window_index]= az; gyro_window[0][window_index]= gx; gyro_window[1][window_index]= gy; gyro_window[2][window_index]= gz; window_index =(window_index +1)%10;// 计算方差for(int i =0; i <3; i++){float mean_acc =0, mean_gyro =0;float var_acc =0, var_gyro =0;for(int j =0; j <10; j++){ mean_acc += acc_window[i][j]; mean_gyro += gyro_window[i][j];} mean_acc /=10.0f; mean_gyro /=10.0f;for(int j =0; j <10; 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 /9.0f; gyro_var[i]= var_gyro /9.0f;}}}; VarianceEstimator varianceEst;// 自适应PID控制器classAdaptivePID{private:float kp, ki, kd;float integral =0;float lastError =0;float lastOutput =0;// 自适应参数float adaptive_kp =1.0f;float adaptive_ki =1.0f;float adaptive_kd =1.0f;float learning_rate =0.01f;// 性能指标float error_history[10]={0};int error_index =0;float performance =0;public:AdaptivePID(float p,float i,float d):kp(p),ki(i),kd(d){}floatcalculate(float error,float dt,float angular_vel){// 更新误差历史 error_history[error_index]= error; error_index =(error_index +1)%10;// 自适应调整adaptParameters(error, angular_vel, dt);// 计算积分 integral += error * dt;// 抗饱和float integral_limit =5.0f;if(integral > integral_limit) integral = integral_limit;if(integral <-integral_limit) integral =-integral_limit;// 计算微分float derivative =0;if(dt >0){ derivative =(error - lastError)/ dt;} lastError = error;// 计算输出float output =(kp * adaptive_kp)* error +(ki * adaptive_ki)* integral +(kd * adaptive_kd)* derivative;// 考虑角速度 output -= angular_vel *0.5f;// 输出限制float output_limit =12.0f;if(output > output_limit) output = output_limit;if(output <-output_limit) output =-output_limit;// 输出变化率限制float max_change =10.0f* dt;float change = output - lastOutput;if(abs(change)> max_change){ output = lastOutput +(change >0? max_change :-max_change);} lastOutput = output;return output;}voidreset(){ integral =0; lastError =0; lastOutput =0;}private:voidadaptParameters(float error,float angular_vel,float dt){// 计算误差统计float mean_error =0;for(int i =0; i <10; i++){ mean_error += error_history[i];} mean_error /=10.0f;float variance =0;for(int i =0; i <10; i++){float diff = error_history[i]- mean_error; variance += diff * diff;} variance /=10.0f;// 基于误差统计调整if(variance <0.001f){// 误差稳定 adaptive_kp *=0.99f;// 减小增益}elseif(variance >0.01f){// 误差波动大 adaptive_kp *=1.01f;// 增加增益}// 基于角速度调整微分if(abs(angular_vel)>2.0f){ adaptive_kd =1.2f;// 高速时增加微分}else{ adaptive_kd =1.0f;}// 限制范围 adaptive_kp =constrain(adaptive_kp,0.5f,2.0f); adaptive_ki =constrain(adaptive_ki,0.1f,2.0f); adaptive_kd =constrain(adaptive_kd,0.1f,2.0f);}}; AdaptivePID adaptiveBalancePID(25.0f,0.5f,0.2f);// 自适应互补滤波voidadaptiveComplementaryFilter(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;float gyro_yaw = adaptiveFilter.yaw + gz * 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;// 计算新息float roll_innovation = acc_roll - adaptiveFilter.roll;float pitch_innovation = acc_pitch - adaptiveFilter.pitch; adaptiveFilter.innovation =sqrt(roll_innovation*roll_innovation + pitch_innovation*pitch_innovation);// 自适应betaif(adaptiveFilter.innovation > adaptiveFilter.innovation_threshold){// 新息大，增加beta adaptiveFilter.beta =min(BETA_DEF *2.0f,0.5f);}else{ adaptiveFilter.beta = BETA_DEF;}// 更新方差估计 varianceEst.update(ax, ay, az, gx, gy, gz);}// Madgwick滤波实现voidmadgwickFilterUpdate(float gx,float gy,float gz,float ax,float ay,float az,float mx,float my,float mz,float dt){// 更新Madgwick滤波器 madgwick.beta = adaptiveFilter.beta; madgwick.update(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;// 转换为欧拉角quaternionToEuler(adaptiveFilter.q0, adaptiveFilter.q1, adaptiveFilter.q2, adaptiveFilter.q3, adaptiveFilter.roll, adaptiveFilter.pitch, adaptiveFilter.yaw);}voidquaternionToEuler(float q0,float q1,float q2,float q3,float&roll,float&pitch,float&yaw){// 四元数转欧拉角 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));}// 传感器融合主循环voidsensorFusionLoop(float dt){// 读取原始数据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.0f;float ay =(ay_raw - calib.accY_offset)/16384.0f;float az =(az_raw - calib.accZ_offset)/16384.0f;float gx =(gx_raw - calib.gyroX_offset)* PI /(131.0f*180.0f);float gy =(gy_raw - calib.gyroY_offset)* PI /(131.0f*180.0f);float gz =(gz_raw - calib.gyroZ_offset)* PI /(131.0f*180.0f);// 选择滤波算法staticbool use_madgwick =false;if(use_madgwick){// 使用Madgwick滤波madgwickFilterUpdate(gx, gy, gz, ax, ay, az,0,0,0, dt);}else{// 使用自适应互补滤波adaptiveComplementaryFilter(ax, ay, az, gx, gy, gz, dt);}// 切换条件staticint stationary_count =0;float motion_level =sqrt(gx*gx + gy*gy + gz*gz);if(motion_level <0.1f){// 静止 stationary_count++;if(stationary_count >50){// 静止0.5秒 use_madgwick =true;// 静止时用Madgwick}}else{ stationary_count =0; use_madgwick =false;// 运动时用互补滤波}}6、多传感器融合卡尔曼滤波
场景：高精度姿态估计，需要最优估计
核心逻辑：扩展卡尔曼滤波 + 多传感器数据融合
#include<SimpleFOC.h>#include<BasicLinearAlgebra.h>// 线性代数库#include<math.h>// 使用BLA库usingnamespace BLA;// 扩展卡尔曼滤波器classExtendedKalmanFilter{private:// 状态: [q0, q1, q2, q3, gx_bias, gy_bias, gz_bias]staticconstint STATE_DIM =7;staticconstint 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();}voidinit(){// 初始状态 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();}voidupdate(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);}voidgetEulerAngles(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));}floatgetAngularVelocity(int axis){if(axis >=0&& axis <3){returnx(4+ axis);// 返回估计的角速度}return0.0f;}private:voidpredict(float gx,float gy,float gz){// 状态预测// 去除零偏的角速度float wx = gx -x(4);float wy = gy -x(5);float wz = gz -x(6);// 四元数导数float q0 =x(0), q1 =x(1), q2 =x(2), q3 =x(3);float q0_dot =0.5f*(-q1 * wx - q2 * wy - q3 * wz);float q1_dot =0.5f*( q0 * wx - q3 * wy + q2 * wz);float q2_dot =0.5f*( q3 * wx + q0 * wy - q1 * wz);float q3_dot =0.5f*(-q2 * wx + q1 * wy + q0 * wz);// 更新四元数x(0)+= q0_dot * dt;x(1)+= q1_dot * dt;x(2)+= q2_dot * dt;x(3)+= q3_dot * dt;// 零偏保持不变// 更新状态转移雅可比updateJacobians();// 协方差预测: P = F * P * F' + Q P = F * P *~F + Q;}voidupdateMeasurement(float ax,float ay,float az,float mx,float my,float mz){// 测量更新// 构建测量向量 Matrix<MEAS_DIM,1> z ={ax, ay, az, mx, my, mz};// 预测测量 Matrix<MEAS_DIM,1> z_pred =measurementModel();// 计算新息 Matrix<MEAS_DIM,1> y = z - z_pred;// 计算卡尔曼增益: K = P * H' * (H * P * H' + R)^-1 Matrix<MEAS_DIM, MEAS_DIM> S = H * P *~H + R; Matrix<STATE_DIM, MEAS_DIM> K = P *~H *Inverse(S);// 状态更新: x = x + K * y x = x + K * y;// 协方差更新: P = (I - K * H) * P Matrix<STATE_DIM, STATE_DIM> I; I.Identity(); P =(I - K * H)* P;// 四元数归一化normalizeQuaternion();} Matrix<MEAS_DIM,1>measurementModel(){// 测量模型 Matrix<MEAS_DIM,1> z_pred;float q0 =x(0), q1 =x(1), q2 =x(2), q3 =x(3);// 加速度计测量模型 (重力方向)z_pred(0)=2.0f*(q1 * q3 - q0 * q2);z_pred(1)=2.0f*(q0 * q1 + q2 * q3);z_pred(2)= q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3;// 磁力计测量模型 (简化)z_pred(3)=2.0f*(q0 * q3 + q1 * q2);z_pred(4)= q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3;z_pred(5)=2.0f*(q2 * q3 - q0 * q1);return z_pred;}voidupdateJacobians(){// 更新状态转移雅可比矩阵 F.Identity();float q0 =x(0), q1 =x(1), q2 =x(2), q3 =x(3);float wx =0, wy =0, wz =0;// 简化处理// 四元数部分F(0,0)=1.0f;F(0,1)=-wx*dt/2;F(0,2)=-wy*dt/2;F(0,3)=-wz*dt/2;F(1,0)= wx*dt/2;F(1,1)=1.0f;F(1,2)= wz*dt/2;F(1,3)=-wy*dt/2;F(2,0)= wy*dt/2;F(2,1)=-wz*dt/2;F(2,2)=1.0f;F(2,3)= wx*dt/2;F(3,0)= wz*dt/2;F(3,1)= wy*dt/2;F(3,2)=-wx*dt/2;F(3,3)=1.0f;// 零偏部分F(0,4)= q1*dt/2;F(0,5)= q2*dt/2;F(0,6)= q3*dt/2;F(1,4)=-q0*dt/2;F(1,5)= q3*dt/2;F(1,6)=-q2*dt/2;F(2,4)=-q3*dt/2;F(2,5)=-q0*dt/2;F(2,6)= q1*dt/2;F(3,4)= q2*dt/2;F(3,5)=-q1*dt/2;F(3,6)=-q0*dt/2;// 更新测量雅可比updateMeasurementJacobian();}voidupdateMeasurementJacobian(){// 更新测量雅可比矩阵 H.Fill(0.0f);float q0 =x(0), q1 =x(1), q2 =x(2), q3 =x(3);// 加速度计雅可比H(0,0)=-2*q2;H(0,1)=2*q3;H(0,2)=-2*q0;H(0,3)=2*q1;H(1,0)=2*q1;H(1,1)=2*q0;H(1,2)=2*q3;H(1,3)=2*q2;H(2,0)=2*q0;H(2,1)=-2*q1;H(2,2)=-2*q2;H(2,3)=2*q3;// 磁力计雅可比H(3,0)=2*q3;H(3,1)=2*q2;H(3,2)=2*q1;H(3,3)=2*q0;H(4,0)=2*q0;H(4,1)=2*q1;H(4,2)=-2*q2;H(4,3)=-2*q3;H(5,0)=-2*q1;H(5,1)=2*q0;H(5,2)=2*q3;H(5,3)=-2*q2;}voidnormalizeQuaternion(){// 四元数归一化float q0 =x(0), q1 =x(1), q2 =x(2), q3 =x(3);float norm =sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);if(norm >0.0001f){x(0)/= norm;x(1)/= norm;x(2)/= norm;x(3)/= norm;}else{x(0)=1.0f;x(1)=x(2)=x(3)=0.0f;}}}; ExtendedKalmanFilter ekf;// 级联PID控制器classCascadePID{private:// 内环PID (角度)structInnerPID{float kp, ki, kd;float integral =0;float lastError =0;float output =0;} innerPID ={25.0f,0.5f,0.2f};// 外环PID (角速度)structOuterPID{float kp, ki, kd;float integral =0;float lastError =0;float output =0;} outerPID ={2.0f,0.1f,0.05f};// 前馈float feedforward =0.8f;public:floatcalculate(float targetAngle,float currentAngle,float angularVelocity,float dt){// 外环：角度控制float angleError = targetAngle - currentAngle; outerPID.integral += angleError * dt; outerPID.integral =constrain(outerPID.integral,-1.0f,1.0f);float angleDeriv =(angleError - outerPID.lastError)/ dt; outerPID.lastError = angleError;// 外环输出 = 目标角速度float targetAngularVel = outerPID.kp * angleError + outerPID.ki * outerPID.integral + outerPID.kd * angleDeriv;// 内环：角速度控制float velocityError = targetAngularVel - angularVelocity; innerPID.integral += velocityError * dt; innerPID.integral =constrain(innerPID.integral,-5.0f,5.0f);float velocityDeriv =(velocityError - innerPID.lastError)/ dt; innerPID.lastError = velocityError;// 内环输出 innerPID.output = innerPID.kp * velocityError + innerPID.ki * innerPID.integral + innerPID.kd * velocityDeriv;// 前馈补偿 innerPID.output += feedforward * targetAngularVel;return innerPID.output;}voidreset(){ innerPID.integral =0; innerPID.lastError =0; outerPID.integral =0; outerPID.lastError =0;}}; CascadePID cascadeController;// 多传感器融合控制voidmultiSensorControlLoop(float dt){// 1. 读取所有传感器float ax, ay, az, gx, gy, gz, mx, my, mz;readAllSensors(ax, ay, az, gx, gy, gz, mx, my, mz);// 2. 扩展卡尔曼滤波 ekf.update(gx, gy, gz, ax, ay, az, mx, my, mz, dt);// 3. 获取估计状态float roll, pitch, yaw; ekf.getEulerAngles(roll, pitch, yaw);float angularVelX = ekf.getAngularVelocity(0);float angularVelY = ekf.getAngularVelocity(1);float angularVelZ = ekf.getAngularVelocity(2);// 4. 级联PID控制float targetAngle =0.0f;// 垂直位置float currentAngle = pitch;// 俯仰角控制float control = cascadeController.calculate( targetAngle, currentAngle, angularVelY, dt );// 5. 应用控制applyControl(control, dt);// 6. 传感器有效性检查checkSensorHealth(ax, ay, az, gx, gy, gz, mx, my, mz, dt);}// 传感器健康监测classSensorHealthMonitor{private:structSensorStats{float mean[3]={0};float variance[3]={0};float health =1.0f;// 健康度int errorCount =0;}; SensorStats accStats, gyroStats, magStats;// 滑动窗口staticconstint WINDOW_SIZE =20;float accHistory[3][WINDOW_SIZE]={0};float gyroHistory[3][WINDOW_SIZE]={0};float magHistory[3][WINDOW_SIZE]={0};int historyIndex =0;public:voidupdate(float ax,float ay,float az,float gx,float gy,float gz,float mx,float my,float mz){// 更新历史数据 accHistory[0][historyIndex]= ax; accHistory[1][historyIndex]= ay; accHistory[2][historyIndex]= az; gyroHistory[0][historyIndex]= gx; gyroHistory[1][historyIndex]= gy; gyroHistory[2][historyIndex]= gz; magHistory[0][historyIndex]= mx; magHistory[1][historyIndex]= my; magHistory[2][historyIndex]= mz; historyIndex =(historyIndex +1)% WINDOW_SIZE;// 计算统计calculateStats(accStats, accHistory);calculateStats(gyroStats, gyroHistory);calculateStats(magStats, magHistory);// 更新健康度updateHealth();}floatgetHealth(int sensorType){switch(sensorType){case0:return accStats.health;case1:return gyroStats.health;case2:return magStats.health;default:return0.0f;}}private:voidcalculateStats(SensorStats &stats,float history[3][WINDOW_SIZE]){// 计算均值和方差for(int i =0; i <3; i++){float sum =0, sumSq =0;for(int j =0; 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]);}}voidupdateHealth(){// 加速度计健康度float accMag =sqrt(accStats.mean[0]*accStats.mean[0]+ accStats.mean[1]*accStats.mean[1]+ accStats.mean[2]*accStats.mean[2]);if(abs(accMag -1.0f)<0.2f){ accStats.health =1.0f; accStats.errorCount =0;}else{ accStats.errorCount++; accStats.health =max(0.0f,1.0f- accStats.errorCount *0.1f);}// 陀螺仪健康度float gyroVar = gyroStats.variance[0]+ gyroStats.variance[1]+ gyroStats.variance[2];if(gyroVar <0.1f){ gyroStats.health =1.0f;}else{ gyroStats.health =1.0f/(1.0f+ gyroVar);}// 磁力计健康度float magMag =sqrt(magStats.mean[0]*magStats.mean[0]+ magStats.mean[1]*magStats.mean[1]+ magStats.mean[2]*magStats.mean[2]);if(magMag >0.1f&& magMag <2.0f){ magStats.health =1.0f;}else{ magStats.health =0.5f;}}};要点解读
- 互补滤波的核心机制
基本公式：角度 = α × (上次角度 + 陀螺积分) + (1-α) × 加速度计角度
系数选择：α=0.98常用，表示98%信任陀螺仪，2%信任加速度计
陀螺零漂补偿：通过比较加速度计角度和陀螺积分角度，自动估计和补偿零漂
自适应调整：根据运动状态动态调整α，高速运动时增加陀螺权重
有效性检查：检查加速度计模值是否接近1g，异常时降低其权重 - IMU校准的关键步骤
静态校准：设备静止时采集1000个样本，计算各轴零偏
温度补偿：记录温度变化对零偏的影响（高级应用）
尺度校准：通过重力加速度校准加速度计量程
正交校准：补偿各轴不正交误差（需要专业设备）
在线校准：运行时持续监测和修正零漂 - PID控制的参数整定策略
Ziegler-Nichols法：先设Ki=Kd=0，增加Kp直到等幅振荡，然后计算参数
试凑法：先调Kp到临界振荡，再调Kd抑制超调，最后调Ki消除静差
级联控制：内环控制角速度（快速响应），外环控制角度（精确跟踪）
抗饱和处理：积分项限幅，防止深度饱和
自适应PID：根据误差统计自动调整参数 - 多传感器融合的层次
松耦合：各传感器独立处理，结果加权平均（互补滤波）
紧耦合：原始数据直接融合（卡尔曼滤波）
传感器优先级：加速度计长期准但动态差，陀螺仪短期准但有漂移
健康监测：实时评估各传感器可信度，自动降权故障传感器
冗余设计：多个同类型传感器互相验证 - 实时系统的优化技巧
定时器中断：使用硬件定时器保证精确的采样周期
数据缓冲：双缓冲或环形缓冲处理传感器数据
计算优化：使用查表法替代复杂三角函数
优先级调度：控制循环最高优先级，显示和通信低优先级
内存管理：静态分配避免动态内存碎片
看门狗：硬件看门狗防止程序跑飞
注意，以上案例只是为了拓展思路，仅供参考。它们可能有错误、不适用或者无法编译。您的硬件平台、使用场景和Arduino版本可能影响使用方法的选择。实际编程时，您要根据自己的硬件配置、使用场景和具体需求进行调整，并多次实际测试。您还要正确连接硬件，了解所用传感器和设备的规范和特性。涉及硬件操作的代码，您要在使用前确认引脚和电平等参数的正确性和安全性。
