在机器人学中，精确的姿态估计是确保机器人在复杂动态环境中稳定运行和精准控制的基础。基于Arduino平台结合BLDC（无刷直流电机）的机器人姿态估计系统，通常是一个融合了多源传感器、先进滤波算法与高动态执行器的机电一体化方案。以下是对其主要特点、应用场景及注意事项的详细解析：
一、 主要特点
多源传感器融合与互补滤波
单一传感器难以满足动态环境下的姿态估计需求。系统通常采用IMU（惯性测量单元，如MPU6050），结合三轴加速度计与陀螺仪。由于纯加速度计易受线性加速度污染，而陀螺仪存在积分漂移，系统需通过互补滤波、卡尔曼滤波（KF/EKF）或DMP（数字运动处理器）进行数据融合，实时获取高精度的俯仰（Pitch）、横滚（Roll）和偏航（Yaw）角。
分层式智能控制架构
系统采用上层感知规划与底层闭环控制相结合的架构。上位机或主控负责环境感知与任务调度，而下位机（如Arduino）通过PID等控制算法，结合IMU提供的姿态补偿数据，动态调节BLDC电机的转速或转矩，实现平滑的跟随与转向。
BLDC高动态力矩执行
采用BLDC电机作为执行器，配合FOC（磁场定向控制）算法，具备毫秒级的扭矩响应能力和低速大扭矩特性。当系统计算出期望的姿态或速度时，BLDC电机能迅速、平滑地执行指令，确保在目标突然启停或急转弯时“如影随形”。
强抗扰能力与动态稳定性
通过IMU的姿态补偿机制，系统能够有效抵消底盘在加速、刹车、路面不平或受到外部碰撞时的重心偏移，通常在100-500ms内即可抵抗扰动并恢复稳定，确保在复杂环境下的运动不失控。
二、 典型应用场景
两轮/单轮自平衡移动平台
在倒立摆或平衡车模型中，Arduino通过高频读取IMU姿态，利用PD控制算法输出差速指令，驱动BLDC电机产生恢复力矩，维持机器人在动态环境中的直立与平衡。
智能仓储与物流跟随AGV
在复杂地形或频繁启停的仓库中，结合UWB差分定位或视觉与IMU融合，机器人能够自动尾随工人进行物料配送。IMU辅助确保了在视觉丢失或信号短暂遮挡时，依靠航迹推测维持跟随的连续性。
手持云台与自稳平台
在相机稳定器或无人机辅助稳定中，互补滤波解算出的姿态角直接作为BLDC电机的控制目标，通过反向运动补偿抵消手持抖动或载体晃动，保持镜头水平。
仿生、攀爬与特种作业机器人
在垂直墙面作业的攀爬机器人或四足机器人中，实时监测机身倾角。若倾角超过安全阈值，系统会立即增强吸附力、调整步态或停止运动以防坠落，实现姿态安全监控。
三、 需要注意的关键事项
主控算力与实时性保障
高频IMU读取、传感器融合及多轴PID计算对算力要求较高。标准的Arduino Uno可能面临算力瓶颈，推荐使用ESP32（双核）、Teensy 4.0等高算力板卡。控制回路必须使用硬件定时器中断或millis()非阻塞定时，严禁使用delay()函数，以确保采样周期（dt）的精确性。
IMU安装与物理校准
IMU必须刚性固定在机器人车身的重心附近，避免柔性连接引入相位滞后。在电机附近安装时需加硅胶减震垫以隔离高频振动。此外，上电时必须进行陀螺仪零偏校准和加速度计水平面校准，并根据振动情况微调互补滤波系数（α值）。
严格的电源隔离与电磁兼容（EMC）
BLDC电机启停时电流极大，会产生高频PWM噪声和反电动势尖峰，极易干扰IMU和主控。严禁将电机与Arduino及传感器共用电源，必须采用隔离DC-DC模块独立供电。动力线与信号线必须分开走线，IMU需使用屏蔽线并远离电机。
BLDC驱动匹配与安全保护机制
对于精确姿态伺服，严禁使用仅支持油门PWM的普通航模ESC，必须使用支持闭环控制的驱动方案（如SimpleFOC库配合专用驱动板）或带编码器反馈的伺服驱动器。同时，必须加入软件保护机制，如积分限幅、PWM输出限幅，以及倾角超限（如>30°）自动切断电机动力的“防飞车/防跳楼”安全逻辑。

1、自平衡机器人——互补滤波姿态解算与直立环控制
适用场景：两轮自平衡机器人、倒立摆教学平台，需实时解算俯仰角（Pitch）并驱动BLDC轮毂电机维持动态平衡。
核心逻辑：利用MPU6050读取加速度计与陀螺仪数据，通过互补滤波融合得到平滑无漂移的实时俯仰角。采用PD控制器根据角度偏差计算恢复力矩，驱动左右BLDC轮毂电机产生差速补偿。该架构广泛用于全国大学生智能车竞赛“直立组”。

#include <SimpleFOC.h>
#include <MPU6050.h>

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// ==================== IMU姿态传感器 ====================
MPU6050 mpu;

// ==================== 互补滤波参数 ====================
const float GYRO_WEIGHT = 0.98f;   // 陀螺仪权重（0.95~0.99）
const float ACCEL_WEIGHT = 0.02f;  // 加速度计权重
float pitchAngle = 0;              // 融合后的俯仰角（弧度）
float gyroOffset = 0;              // 陀螺仪零偏
unsigned long lastTime = 0;
float loopPeriod = 0.001f;

// ==================== 直立环PD参数 ====================
const float PITCH_KP = 15.0f;      // 比例增益
const float PITCH_KD = 0.8f;       // 微分增益
float lastPitchRate = 0;

void setup() {
    Serial.begin(115200);
    
    // 初始化IMU
    Wire.begin();
    mpu.initialize();
    mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
    mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250DPS);
    
    // 陀螺仪零偏校准（上电后静止2秒）
    delay(2000);
    gyroOffset = mpu.getRotationX();
    
    // 初始化BLDC电机与FOC
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    
    lastTime = micros();
}

// ==================== 互补滤波姿态解算 ====================
void attitudeComplementaryFilter() {
    // 读取加速度计与陀螺仪原始数据
    float accX = mpu.getAccelerationX();
    float accY = mpu.getAccelerationY();
    float gyroX = mpu.getRotationX() - gyroOffset;
    
    // 加速度计计算俯仰角（基于重力分量）
    // 公式：θ = arctan(ax / sqrt(ay² + az²))
    float accAngle = atan2(accX, accY);
    
    // 互补滤波核心融合公式
    // 陀螺仪积分提供动态响应，加速度计校正长期漂移
    pitchAngle += gyroX * loopPeriod;               // 陀螺仪积分
    pitchAngle = pitchAngle * GYRO_WEIGHT + accAngle * ACCEL_WEIGHT;
    
    // 角度限幅（防止极端姿态导致控制失控）
    if (pitchAngle > 0.785f) pitchAngle = 0.785f;  // ±45°
    if (pitchAngle < -0.785f) pitchAngle = -0.785f;
}

// ==================== 直立环PD控制 ====================
float computeStandingControl() {
    float pitchError = 0 - pitchAngle;  // 目标俯仰角为0（垂直）
    
    // 比例项：产生恢复力矩
    float proportional = pitchError * PITCH_KP;
    
    // 微分项：抑制超调与振荡
    float derivative = (pitchError - lastPitchRate) * PITCH_KD;
    lastPitchRate = pitchError;
    
    return proportional + derivative;
}

void loop() {
    // 高精度时间测量
    unsigned long currentTime = micros();
    loopPeriod = (currentTime - lastTime) / 1000000.0f;
    lastTime = currentTime;
    
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 1. 互补滤波姿态解算
    attitudeComplementaryFilter();
    
    // 2. 直立环计算恢复力矩
    float output = computeStandingControl();
    
    // 3. 差速驱动（左右轮反向输出产生恢复力矩）
    motorL.move(-output);
    motorR.move(output);
    
    // 4. 调试数据输出
    Serial.print("Pitch: "); Serial.print(pitchAngle * 180 / PI);
    Serial.print(" Output: "); Serial.println(output);
    
    delay(10);  // 100Hz控制频率
}

2、两轮差速巡检机器人——卡尔曼滤波融合IMU与里程计消除航向漂移
适用场景：自主导航巡检机器人、仓储AGV，需精准跟踪航向角（偏航角Yaw）实现路径规划。单一IMU存在积分漂移，单一里程计存在累积误差，需融合两者获取低漂移航向角。
核心逻辑：构建三状态卡尔曼滤波器（航向角θ、角速度ω、里程计速度偏差b），将IMU陀螺仪数据作为状态预测输入，将里程计航向与IMU加速度计/磁力计航向作为测量更新，动态分配传感器信任权重，输出最优航向估计。

#include <SimpleFOC.h>
#include <MPU6050.h>
#include <Kalman.h>  // 简易卡尔曼滤波库

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 7);
Encoder encoderL(18, 19, 2048), encoderR(20, 21, 2048);

// ==================== IMU传感器 ====================
MPU6050 mpu;

// ==================== 里程计与轮距参数 ====================
const float WHEEL_RADIUS = 0.05f;    // 轮半径 (m)
const float WHEEL_DISTANCE = 0.25f;  // 两轮间距 (m)
float leftVelocity = 0, rightVelocity = 0;

// ==================== 卡尔曼滤波器（三状态） ====================
// 状态: [theta, omega, bias] = [航向角, 角速度, 里程计偏差]
float dt = 0.01f;
float theta = 0, omega = 0, bias = 0;
// 过程噪声与测量噪声
float Q_omega = 0.001f, Q_bias = 0.0005f;
float R_imu = 0.02f, R_odom = 0.01f;
// 协方差矩阵 (简化为3x3对角阵)
float P00 = 1, P11 = 1, P22 = 1;

void setup() {
    Serial.begin(115200);
    
    // 初始化IMU
    Wire.begin();
    mpu.initialize();
    
    // 初始化BLDC电机与FOC
    motorL.linkSensor(&encoderL);
    motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL);
    motorR.linkDriver(&driverR);
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

// ==================== 卡尔曼滤波预测 ====================
void kalmanPredict(float gyroRate) {
    // 状态预测: theta += omega*dt, omega += gyroRate, bias保持不变
    theta += omega * dt;
    omega += gyroRate * dt;
    
    // 协方差预测
    P00 += dt * (2 * P00 + dt * P11);
    P11 += Q_omega * dt;
    P22 += Q_bias * dt;
}

// ==================== 卡尔曼滤波更新 ====================
void kalmanUpdate(float measuredTheta, float measuredOmega) {
    // 测量1: IMU航向角测量更新
    float K0 = P00 / (P00 + R_imu);
    theta += K0 * (measuredTheta - theta);
    P00 = (1 - K0) * P00;
    
    // 测量2: 里程计角速度测量更新
    float K1 = P11 / (P11 + R_odom);
    omega += K1 * (measuredOmega - omega);
    P11 = (1 - K1) * P11;
}

// ==================== 读取里程计计算航向 ====================
float getOdomTheta() {
    // 通过编码器读取左右轮速度
    leftVelocity = motorL.shaft_velocity;
    rightVelocity = motorR.shaft_velocity;
    
    // 差速模型: 角速度 = (右轮速度 - 左轮速度) / 轮距
    float angularVelocity = (rightVelocity - leftVelocity) / WHEEL_DISTANCE;
    
    // 积分得到航向角
    static float odomTheta = 0;
    odomTheta += angularVelocity * dt;
    return odomTheta;
}

// ==================== 读取IMU磁力计航向 ====================
float getImuTheta() {
    // 磁力计读数计算航向角（磁北为参考）
    // 此处简化，实际需磁力计校准与倾斜补偿
    return mpu.getRotationZ() * dt;  // 简化：仅用陀螺仪积分
}

void loop() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 1. 读取传感器
    float gyroZ = mpu.getRotationZ();  // 陀螺仪角速度 (rad/s)
    float imuTheta = getImuTheta();    // IMU航向角（含漂移）
    float odomTheta = getOdomTheta();  // 里程计航向角（含累积误差）
    
    // 2. 卡尔曼滤波预测与更新
    kalmanPredict(gyroZ);
    kalmanUpdate(imuTheta, odomTheta);
    
    // 3. 输出融合后的航向角用于导航
    Serial.print("Fused Theta: "); Serial.print(theta * 180 / PI);
    Serial.print(" IMU: "); Serial.print(imuTheta * 180 / PI);
    Serial.print(" Odom: "); Serial.println(odomTheta * 180 / PI);
    
    delay(10);
}

3、视觉SLAM自主跟随机器人——IMU与视觉紧耦合姿态估计
适用场景：园区巡逻跟随机器人、商场服务机器人，需在动态环境中维持全局位姿估计，视觉SLAM提供绝对定位，IMU填补帧间运动空白并消除漂移。
核心逻辑：采用上位机（Jetson Nano/树莓派）+下位机（Arduino/ESP32）分层架构。上位机运行ORB-SLAM3获取全局位姿，下位机通过MPU9250 IMU高频采集并发送姿态数据。当视觉SLAM跟踪丢失时，上位机调用下位机IMU数据进行短时位姿推断，实现紧耦合融合。

#include <SimpleFOC.h>
#include <MPU9250.h>
#include <Wire.h>

// ==================== BLDC四轮独立驱动 ====================
BLDCMotor motors[4] = {
    BLDCMotor(7), BLDCMotor(8), BLDCMotor(9), BLDCMotor(10)
};
BLDCDriver3PWM drivers[4] = {
    BLDCDriver3PWM(3, 5, 6, 11),
    BLDCDriver3PWM(22, 23, 24, 25),
    BLDCDriver3PWM(26, 27, 28, 29),
    BLDCDriver3PWM(30, 31, 32, 33)
};
Encoder encoders[4] = {
    Encoder(18, 19, 2048), Encoder(20, 21, 2048),
    Encoder(34, 35, 2048), Encoder(36, 37, 2048)
};

// ==================== IMU (MPU9250) ====================
MPU9250 imu;

// ==================== 姿态数据结构 ====================
struct ImuData {
    float roll, pitch, yaw;     // 欧拉角 (度)
    float gx, gy, gz;           // 角速度 (rad/s)
    float ax, ay, az;           // 加速度 (m/s²)
    unsigned long timestamp;    // 微秒时间戳
};

ImuData imuData;

// ==================== 互补滤波融合（Roll/Pitch） ====================
float roll = 0, pitch = 0;
const float COMPL_FILTER_ALPHA = 0.96f;
unsigned long lastImuTime = 0;

void setup() {
    Serial.begin(115200);
    
    // 初始化IMU (MPU9250)
    Wire.begin();
    imu.begin();
    imu.setAccelRange(MPU9250::ACCEL_RANGE_2G);
    imu.setGyroRange(MPU9250::GYRO_RANGE_250DPS);
    imu.setFilterBandwidth(MPU9250::BANDWIDTH_184HZ);
    lastImuTime = micros();
    
    // 初始化BLDC电机与FOC
    for (int i = 0; i < 4; i++) {
        motors[i].linkSensor(&encoders[i]);
        motors[i].linkDriver(&drivers[i]);
        motors[i].init();
        motors[i].initFOC();
        motors[i].controller = MotionControlType::velocity;
    }
}

// ==================== IMU数据采集与姿态解算 ====================
void updateImuAttitude() {
    // 读取MPU9250原始数据
    imu.readSensor();
    float ax = imu.getAccelX_mss();
    float ay = imu.getAccelY_mss();
    float az = imu.getAccelZ_mss();
    float gx = imu.getGyroX_rads();
    float gy = imu.getGyroY_rads();
    float gz = imu.getGyroZ_rads();
    
    // 计算时间间隔
    unsigned long now = micros();
    float dt = (now - lastImuTime) / 1000000.0f;
    lastImuTime = now;
    
    // 加速度计计算姿态角（静态参考）
    float accelRoll = atan2(ay, az) * 180 / PI;
    float accelPitch = atan2(-ax, sqrt(ay*ay + az*az)) * 180 / PI;
    
    // 互补滤波融合
    roll = roll * COMPL_FILTER_ALPHA + accelRoll * (1 - COMPL_FILTER_ALPHA);
    pitch = pitch * COMPL_FILTER_ALPHA + accelPitch * (1 - COMPL_FILTER_ALPHA);
    // Yaw依赖磁力计或视觉，此处简化
    float yaw = 0; // 由上位机视觉SLAM提供
    
    // 填充数据结构供上位机读取
    imuData.roll = roll;
    imuData.pitch = pitch;
    imuData.yaw = yaw;
    imuData.gx = gx;
    imuData.gy = gy;
    imuData.gz = gz;
    imuData.ax = ax;
    imuData.ay = ay;
    imuData.az = az;
    imuData.timestamp = now;
}

void loop() {
    // 更新电机FOC
    for (int i = 0; i < 4; i++) {
        motors[i].loopFOC();
    }
    
    // 1. IMU姿态解算（高频，100Hz）
    updateImuAttitude();
    
    // 2. 向上位机发送姿态数据（通过串口）
    // 格式: IMU,roll,pitch,yaw,gx,gy,gz,ax,ay,az
    Serial.print("IMU,");
    Serial.print(imuData.roll); Serial.print(",");
    Serial.print(imuData.pitch); Serial.print(",");
    Serial.print(imuData.yaw); Serial.print(",");
    Serial.print(imuData.gx); Serial.print(",");
    Serial.print(imuData.gy); Serial.print(",");
    Serial.print(imuData.gz); Serial.print(",");
    Serial.print(imuData.ax); Serial.print(",");
    Serial.print(imuData.ay); Serial.print(",");
    Serial.println(imuData.az);
    
    // 3. 接收上位机指令（速度/转向命令）
    if (Serial.available()) {
        String cmd = Serial.readStringUntil('\n');
        // 解析速度指令并驱动电机 (略)
    }
    
    delay(10);  // 100Hz控制频率
}

要点解读
互补滤波是Arduino上姿态估计的“黄金基准”：对于资源受限的Arduino平台，互补滤波是首选方案。其计算量极小（仅几次乘加），50行代码即可实现，适合对实时性要求极高的自平衡机器人控制回路。核心公式Angle = α*(Angle + Gyro*dt) + (1-α)*Accel_Angle，其中α通常在0.95~0.99之间，决定了动态响应与抗漂移的权衡。注意：互补滤波仅适用于Roll/Pitch估计，Yaw依赖磁力计或视觉，因加速度计无法提供偏航参考。

卡尔曼滤波是“去噪”与“融合”的数学最优解：相比互补滤波的固定系数，卡尔曼滤波通过协方差矩阵动态计算增益。当传感器受剧烈干扰时，滤波器会自动降低对该传感器的信任度，转而依赖模型预测。关键在于Q（过程噪声）和R（测量噪声）的物理标定：Q小说明信任模型、输出平滑；R大说明传感器噪声大、系统反应迟钝。实际工程中，Q和R需通过实验反复调整。

算力分层：Arduino做实时控制，上位机做重计算：完整的卡尔曼滤波涉及矩阵求逆，在Arduino Uno（8位、2KB RAM）上难以流畅运行。推荐上位机（Jetson/树莓派）+下位机（ESP32/Arduino Due）分层架构：上位机负责视觉SLAM、复杂卡尔曼滤波或粒子滤波；下位机（ESP32利用双核）专责高频FOC控制与互补滤波。ESP32双核物理隔离确保AI推理不阻塞电机控制。

多传感器融合是“冗余设计”的保障：单一传感器必有短板——IMU陀螺仪有积分漂移，加速度计受运动加速度干扰，里程计有累积误差。卡尔曼滤波可融合IMU+里程计+磁力计+视觉等多源数据。当某一传感器失效（如IMU受磁干扰或轮子打滑）时，系统仍能维持短时姿态估计。例如，视觉SLAM跟踪丢失时，可由下位机IMU数据进行短时位姿推断。

电源隔离与精确计时是工程落地的“生死线”：BLDC电机启停瞬间电流极大，会拉低电压导致IMU读数跳变或MCU复位。必须采用隔离DC-DC为传感器独立供电，动力线与信号线物理隔离。同时，姿态解算要求采样时间间隔精确——使用micros()而非delay()测量dt，否则积分误差会迅速发散。控制频率建议≥200Hz（周期≤5ms），以抑制系统振荡。

4、工业物流机器人——振动环境下的IMU-编码器互补滤波姿态估计（稳定载运场景）
适用场景：工厂车间、物流仓库等场景，物流机器人需在地面不平、设备振动的动态环境中载运货物，BLDC电机需根据姿态（俯仰、横滚）调整驱动扭矩，避免货物倾斜、侧翻；IMU易受地面振动干扰，导致姿态数据波动，需通过编码器运动反馈补偿IMU误差，实现振动环境下的稳定姿态估计。
核心逻辑：融合IMU的角速度/加速度数据与编码器的速度/位移数据，采用互补滤波算法——高频振动用编码器运动趋势补偿，低频姿态变化用IMU感知，抵消振动干扰；同时结合BLDC电机的速度反馈，修正姿态估计误差，保障物流机器人在载运过程中姿态稳定可控，精准驱动BLDC电机调整运动状态。

// 核心库引入：IMU+BLDC控制+编码器+数学运算
#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>

// 传感器与硬件引脚定义
#define MPU_ADDR 0x68
#define ENC_A_LEFT 2
#define ENC_B_LEFT 3
#define ENC_A_RIGHT 4
#define ENC_B_RIGHT 5
#define BLDC_LEFT_PWM 9
#define BLDC_LEFT_IN1 10
#define BLDC_LEFT_IN2 11
#define BLDC_RIGHT_PWM 6
#define BLDC_RIGHT_IN1 7
#define BLDC_RIGHT_IN2 8

// 姿态估计与动态补偿参数
const float ENC_CPR = 1200.0;        // 编码器每转脉冲数
const float WHEEL_RADIUS = 50.0;     // 轮径(mm)
const float WHEEL_BASE = 250.0;      // 轮距(mm)
const float VIBRATION_COMP_K = 0.35; // 振动补偿系数，适配工业振动环境
const float COMP_FILTER_TAU = 0.02;  // 互补滤波时间常数，保障实时性
const float MAX_ANGLE_RATE = 5.0;    // 角速度限幅，过滤振动尖峰
const float IMU_OFFSET_SAMPLE = 100; // 传感器静态偏移采样次数

// 姿态融合状态结构体
struct LogisticsAttitudeState {
  // IMU原始数据
  float accel_x, accel_y, accel_z;
  float gyro_x, gyro_y, gyro_z;
  // 编码器运动数据
  float left_velocity, right_velocity;
  float linear_v, angular_w;
  // 融合姿态数据
  float pitch_angle, roll_angle;
  float pitch_rate, roll_rate;
  // 误差与补偿
  float gyro_offset_x, gyro_offset_y;
  float vibration_compensation;
} attState;

// 硬件对象实例化
MPU6050 mpu;
BLDCMotor motorLeft = BLDCMotor(11);
BLDCMotor motorRight = BLDCMotor(11);
BLDCDriver2PWM driverLeft = BLDCDriver2PWM(BLDC_LEFT_PWM, BLDC_LEFT_IN1, BLDC_LEFT_IN2);
BLDCDriver2PWM driverRight = BLDCDriver2PWM(BLDC_RIGHT_PWM, BLDC_RIGHT_IN1, BLDC_RIGHT_IN2);
Encoder encLeft(ENC_A_LEFT, ENC_B_LEFT);
Encoder encRight(ENC_A_RIGHT, ENC_B_RIGHT);

// 传感器静态偏移校准：工业振动环境下，降低静态误差
void sensorOffsetCalibration() {
  mpu.getMotion6(&attState.accel_x, &attState.accel_y, &attState.accel_z,
                 &attState.gyro_x, &attState.gyro_y, &attState.gyro_z);
  delay(20);
  for (int i = 0; i < IMU_OFFSET_SAMPLE; i++) {
    mpu.getMotion6(&attState.accel_x, &attState.accel_y, &attState.accel_z,
                   &attState.gyro_x, &attState.gyro_y, &attState.gyro_z);
    attState.gyro_offset_x += attState.gyro_x;
    attState.gyro_offset_y += attState.gyro_y;
    delay(20);
  }
  attState.gyro_offset_x /= IMU_OFFSET_SAMPLE;
  attState.gyro_offset_y /= IMU_OFFSET_SAMPLE;
  Serial.println("工业振动环境：传感器偏移校准完成");
}

// 编码器运动数据解析：实时计算机器人线速度与角速度，补偿IMU振动误差
void encodeMotionUpdate() {
  static long encLeftCount = 0, encRightCount = 0;
  static unsigned long lastTime = 0;
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;

  if (dt > 0.001) {
    long encLeftNew = encLeft.getCount();
    long encRightNew = encRight.getCount();
    float leftDelta = encLeftNew - encLeftCount;
    float rightDelta = encRightNew - encRightCount;

    attState.left_velocity = (leftDelta / ENC_CPR) * (2.0 * M_PI * WHEEL_RADIUS) / dt;
    attState.right_velocity = (rightDelta / ENC_CPR) * (2.0 * M_PI * WHEEL_RADIUS) / dt;

    attState.linear_v = (attState.left_velocity + attState.right_velocity) / 2.0;
    attState.angular_w = (attState.right_velocity - attState.left_velocity) / WHEEL_BASE;

    encLeftCount = encLeftNew;
    encRightCount = encRightNew;
    lastTime = currentTime;
  }
}

// 振动补偿+互补滤波姿态估计：融合IMU与编码器数据，抑制工业振动干扰
void vibrationComplementaryFilter() {
  // 1. 提取IMU角速度并补偿静态偏移
  float raw_gyro_pitch = (attState.gyro_x - attState.gyro_offset_x) * (M_PI / 180.0);
  float raw_gyro_roll = (attState.gyro_y - attState.gyro_offset_y) * (M_PI / 180.0);

  // 2. 振动补偿：基于编码器角速度，判断并抑制振动噪声
  if (fabs(attState.angular_w) > MAX_ANGLE_RATE) {
    attState.vibration_compensation = attState.angular_w * VIBRATION_COMP_K;
    raw_gyro_pitch -= attState.vibration_compensation * 0.5;
    raw_gyro_roll -= attState.vibration_compensation * 0.5;
  } else {
    attState.vibration_compensation = 0.0;
  }

  // 3. 互补滤波：高频用IMU角速度，低频用加速度计姿态，融合编码器运动趋势
  float gyro_weight = dt / (COMP_FILTER_TAU + dt);
  float accel_weight = COMP_FILTER_TAU / (COMP_FILTER_TAU + dt);

  // 加速度计计算静态姿态（低频可靠）
  float accel_pitch = atan2(attState.accel_y, sqrt(pow(attState.accel_x, 2) + pow(attState.accel_z, 2)));
  float accel_roll = atan2(-attState.accel_x, sqrt(pow(attState.accel_y, 2) + pow(attState.accel_z, 2)));

  // 编码器辅助修正：运动时修正加速度计因惯性产生的误差
  if (fabs(attState.linear_v) > 0.1) {
    accel_pitch += (attState.linear_v * 0.0001) * sin(attState.angular_w);
    accel_roll -= (attState.linear_v * 0.0001) * cos(attState.angular_w);
  }

  // 互补滤波融合
  attState.pitch_angle = gyro_weight * (attState.pitch_angle + raw_gyro_pitch * dt) + accel_weight * accel_pitch;
  attState.roll_angle = gyro_weight * (attState.roll_angle + raw_gyro_roll * dt) + accel_weight * accel_roll;
  attState.pitch_rate = raw_gyro_pitch;
  attState.roll_rate = raw_gyro_roll;
}

// 初始化函数：完成传感器、电机、编码器初始化
void setup() {
  Serial.begin(115200);
  Wire.begin();
  // IMU初始化
  mpu.initialize();
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2G);
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_250DPS);
  sensorOffsetCalibration();

  // BLDC电机初始化
  driverLeft.init();
  driverRight.init();
  motorLeft.linkDriver(&driverLeft);
  motorRight.linkDriver(&driverRight);
  motorLeft.init();
  motorRight.init();
  motorLeft.initFOC();
  motorRight.initFOC();
  motorLeft.controller = MotionControlType::velocity;
  motorRight.controller = MotionControlType::velocity;

  // 编码器初始化
  encLeft.init();
  encRight.init();
  motorLeft.linkSensor(&encLeft);
  motorRight.linkSensor(&encRight);

  // 姿态状态初始化
  attState.pitch_angle = 0.0;
  attState.roll_angle = 0.0;
  attState.pitch_rate = 0.0;
  attState.roll_rate = 0.0;

  Serial.println("工业物流机器人：振动环境姿态估计系统初始化完成");
}

void loop() {
  // 1. 读取IMU数据
  mpu.getMotion6(&attState.accel_x, &attState.accel_y, &attState.accel_z,
                 &attState.gyro_x, &attState.gyro_y, &attState.gyro_z);

  // 2. 解析编码器运动数据
  encodeMotionUpdate();

  // 3. 振动补偿+互补滤波姿态估计
  unsigned long currentTime = millis();
  float dt = (currentTime - lastTime) / 1000.0;
  lastTime = currentTime;
  vibrationComplementaryFilter();

  // 4. 根据姿态控制BLDC电机：姿态倾斜时，调整左右轮扭矩平衡
  float pitch_correct = attState.pitch_angle * 5.0;
  float roll_correct = attState.roll_angle * 10.0;
  float left_target_v = attState.linear_v + roll_correct;
  float right_target_v = attState.linear_v - roll_correct;

  float left_rpm = (left_target_v * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
  float right_rpm = (right_target_v * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);

  motorLeft.target = left_rpm;
  motorRight.target = right_rpm;
  motorLeft.move(50);
  motorRight.move(50);

  // 5. 输出姿态数据（调试）
  Serial.print("姿态：俯仰=" + String(attState.pitch_angle * 180 / M_PI,1) + "°" +
               " 横滚=" + String(attState.roll_angle * 180 / M_PI,1) + "°" +
               " 振动补偿=" + String(attState.vibration_compensation,1) +
               " 线速度=" + String(attState.linear_v,1) + "mm/s");
  Serial.println();

  delay(10);
}

代码逻辑说明：
振动补偿机制：通过编码器运动数据判断振动强度，当角速度超过阈值时，用振动补偿系数抵消IMU的振动噪声，避免姿态数据因设备振动失真；
互补滤波融合：利用IMU高频角速度、加速度计低频姿态、编码器运动趋势的优势，动态调整权重，既保障实时性，又保障姿态精度，适配工业振动环境下的动态干扰；
BLDC闭环联动：将姿态数据直接转化为电机速度修正量，通过调整左右轮转速抵消姿态倾斜，实现物流机器人在振动环境下的稳定载运，避免货物倾斜、侧翻。

5、特种巡检机器人——复杂地形下的卡尔曼滤波姿态估计（高动态冲击场景）
适用场景：电力杆塔、化工设备、户外管道等巡检场景，机器人需在崎岖地形、斜坡、台阶等环境中运行，遭受地形冲击、机械振动与电磁干扰；IMU数据易受动态冲击影响，导致姿态误差急剧累积，需通过卡尔曼滤波融合多源数据，实现复杂地形下的精确姿态估计，保障BLDC电机的稳定驱动与巡检任务的精准执行。
核心逻辑：以卡尔曼滤波为核心，融合IMU的角速度/加速度数据、编码器的运动反馈与BLDC电机的扭矩反馈，建立动态噪声模型——将地形冲击、机械振动视为过程噪声，电机负载变化、传感器漂移视为观测噪声，实时估计姿态最优值，解决动态冲击下的姿态漂移问题；同时根据地形冲击强度自适应调整滤波参数，保障实时性与精度的平衡。

// 核心库引入
#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>
#include <Eigen.h> // 简化矩阵运算，卡尔曼滤波核心

// 硬件引脚定义
#define MPU_ADDR 0x68
#define ENC_A_LEFT 2
#define ENC_B_LEFT 3
#define ENC_A_RIGHT 4
#define ENC_B_RIGHT 5
#define BLDC_LEFT_PWM 9
#define BLDC_LEFT_IN1 10
#define BLDC_LEFT_IN2 11
#define BLDC_RIGHT_PWM 6
#define BLDC_RIGHT_IN1 7
#define BLDC_RIGHT_IN2 8

// 卡尔曼滤波与动态补偿参数
const float ENC_CPR = 1200.0;
const float WHEEL_RADIUS = 50.0;
const float WHEEL_BASE = 250.0;
const float PROCESS_NOISE_VAR = 0.01; // 过程噪声（地形冲击、振动）
const float OBSERVATION_NOISE_VAR = 0.05; // 观测噪声（传感器漂移、电机负载）
const float IMPACT_ADAPT_FACTOR = 0.8; // 冲击自适应系数，冲击时增大过程噪声权重
const float MAX_IMPACT_RATE = 10.0;   // 最大冲击角速度阈值

// 卡尔曼滤波姿态状态结构体
struct KalmanAttitudeState {
  // 状态向量（俯仰角、横滚角、角速度）
  float state_pitch, state_roll, state_pitch_rate, state_roll_rate;
  // 协方差矩阵（简化为标量，适配嵌入式场景）
  float covariance_pitch, covariance_roll;
  // 过程噪声与观测噪声
  float process_noise, observation_noise;
  // 辅助数据
  float accel_pitch, accel_roll;
  float enc_angular_rate;
  float motor_load_feedback;
  bool impact_detected;
} kalmanState;

// 硬件对象实例化
MPU6050 mpu;
BLDCMotor motorLeft = BLDCMotor(11);
BLDCMotor motorRight = BLDCMotor(11);
BLDCDriver2PWM driverLeft = BLDCDriver2PWM(BLDC_LEFT_PWM, BLDC_LEFT_IN1, BLDC_LEFT_IN2);
BLDCDriver2PWM driverRight = BLDCDriver2PWM(BLDC_RIGHT_PWM, BLDC_RIGHT_IN1, BLDC_RIGHT_IN2);
Encoder encLeft(ENC_A_LEFT, ENC_B_LEFT);
Encoder encRight(ENC_A_RIGHT, ENC_B_RIGHT);

// 卡尔曼滤波核心预测步骤：根据角速度预测姿态，考虑动态冲击噪声
void kalmanPredict() {
  float dt = 0.01; // 滤波周期（可按实际调整）

  // 1. 状态预测：基于角速度更新姿态
  float raw_gyro_pitch = kalmanState.state_pitch_rate * dt;
  float raw_gyro_roll = kalmanState.state_roll_rate * dt;
  kalmanState.state_pitch += raw_gyro_pitch;
  kalmanState.state_roll += raw_gyro_roll;

  // 2. 协方差预测：叠加过程噪声，冲击时自适应增大噪声
  if (kalmanState.impact_detected) {
    kalmanState.process_noise = PROCESS_NOISE_VAR * IMPACT_ADAPT_FACTOR;
  } else {
    kalmanState.process_noise = PROCESS_NOISE_VAR;
  }
  kalmanState.covariance_pitch += (kalmanState.covariance_pitch + kalmanState.process_noise);
  kalmanState.covariance_roll += (kalmanState.covariance_roll + kalmanState.process_noise);
}

// 卡尔曼滤波核心更新步骤：融合加速度计、编码器、电机反馈，修正姿态预测
void kalmanUpdate() {
  // 1. 获取加速度计观测值（俯仰、横滚）
  float accel_pitch = atan2(kalmanState.accel_pitch, sqrt(pow(mpu.getAccelerationY(),2)+pow(mpu.getAccelerationZ(),2)));
  float accel_roll = atan2(-mpu.getAccelerationX(), sqrt(pow(mpu.getAccelerationY(),2)+pow(mpu.getAccelerationZ(),2)));

  // 2. 融合编码器与电机反馈：修正冲击导致的加速度计误差
  if (fabs(kalmanState.enc_angular_rate) > MAX_IMPACT_RATE) {
    accel_pitch += kalmanState.enc_angular_rate * 0.0002;
    accel_roll -= kalmanState.enc_angular_rate * 0.0002;
  }
  // 电机负载反馈：负载变化时，修正传感器因机械振动的误差
  accel_pitch += kalmanState.motor_load_feedback * 0.001;

  // 3. 计算卡尔曼增益：基于协方差与观测噪声
  float gain_pitch = kalmanState.covariance_pitch / (kalmanState.covariance_pitch + kalmanState.observation_noise);
  float gain_roll = kalmanState.covariance_roll / (kalmanState.covariance_roll + kalmanState.observation_noise);

  // 4. 更新状态与协方差：用观测值修正预测姿态
  kalmanState.state_pitch += gain_pitch * (accel_pitch - kalmanState.state_pitch);
  kalmanState.state_roll += gain_roll * (accel_roll - kalmanState.state_roll);
  kalmanState.covariance_pitch *= (1 - gain_pitch);
  kalmanState.covariance_roll *= (1 - gain_roll);
}

// 动态冲击检测：结合编码器与电机反馈，判断地形冲击强度
void impactDetection() {
  float left_enc_rate = (encLeft.getVelocity() * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
  float right_enc_rate = (encRight.getVelocity() * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
  kalmanState.enc_angular_rate = (right_enc_rate - left_enc_rate) / WHEEL_BASE;

  // 电机负载反馈：负载突变视为冲击
  kalmanState.motor_load_feedback = motorLeft.controller_pid.output + motorRight.controller_pid.output;

  if (fabs(kalmanState.enc_angular_rate) > MAX_IMPACT_RATE || fabs(kalmanState.motor_load_feedback) > 50) {
    kalmanState.impact_detected = true;
  } else {
    kalmanState.impact_detected = false;
  }
}

// 多源数据融合更新：整合IMU、编码器、电机数据，为卡尔曼滤波提供输入
void multiSensorDataUpdate() {
  int16_t ax, ay, az, gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 转换为物理量
  kalmanState.accel_pitch = ay / 16384.0; // 加速度计灵敏度
  kalmanState.state_pitch_rate = gx / 131.0 * (M_PI / 180.0); // 角速度
  kalmanState.state_roll_rate = gy / 131.0 * (M_PI / 180.0);

  impactDetection();
}

// 初始化函数
void setup() {
  Serial.begin(115200);
  Wire.begin();

  // IMU初始化
  mpu.initialize();
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_4G); // 巡检冲击大，提高量程
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_500DPS);

  // BLDC电机初始化
  driverLeft.init();
  driverRight.init();
  motorLeft.linkDriver(&driverLeft);
  motorRight.linkDriver(&driverRight);
  motorLeft.init();
  motorRight.init();
  motorLeft.initFOC();
  motorRight.initFOC();
  motorLeft.controller = MotionControlType::velocity;
  motorRight.controller = MotionControlType::velocity;

  // 编码器初始化
  encLeft.init();
  encRight.init();
  motorLeft.linkSensor(&encLeft);
  motorRight.linkSensor(&encRight);

  // 卡尔曼滤波初始化
  kalmanState.state_pitch = 0.0;
  kalmanState.state_roll = 0.0;
  kalmanState.state_pitch_rate = 0.0;
  kalmanState.state_roll_rate = 0.0;
  kalmanState.covariance_pitch = 0.1;
  kalmanState.covariance_roll = 0.1;
  kalmanState.process_noise = PROCESS_NOISE_VAR;
  kalmanState.observation_noise = OBSERVATION_NOISE_VAR;
  kalmanState.impact_detected = false;

  Serial.println("特种巡检机器人：卡尔曼滤波姿态估计系统初始化完成");
}

void loop() {
  // 1. 更新多源传感器数据
  multiSensorDataUpdate();

  // 2. 卡尔曼滤波预测与更新
  kalmanPredict();
  kalmanUpdate();

  // 3. 根据姿态控制BLDC电机：复杂地形下，姿态倾斜时动态调整转速
  float pitch_correct = kalmanState.state_pitch * 8.0;
  float roll_correct = kalmanState.state_roll * 12.0;
  float base_v = 100.0; // 基础速度
  float left_v = base_v + roll_correct - pitch_correct * 0.5;
  float right_v = base_v - roll_correct + pitch_correct * 0.5;

  float left_rpm = (left_v * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
  float right_rpm = (right_v * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);

  motorLeft.target = left_rpm;
  motorRight.target = right_rpm;
  motorLeft.move(50);
  motorRight.move(50);

  // 4. 输出姿态数据与冲击状态
  Serial.print("姿态：俯仰=" + String(kalmanState.state_pitch * 180 / M_PI,1) + "°" +
               " 横滚=" + String(kalmanState.state_roll * 180 / M_PI,1) + "°" +
               " 冲击检测=" + String(kalmanState.impact_detected ? "是" : "否"));
  Serial.println();

  delay(10);
}

代码逻辑说明：
卡尔曼滤波动态建模：将地形冲击、机械振动纳入过程噪声，通过预测-更新循环修正姿态，避免传统算法在动态冲击下的姿态漂移；同时根据冲击强度自适应调整噪声参数，平衡精度与实时性；
多源数据融合：融合IMU、编码器、BLDC电机负载反馈，用编码器运动趋势、电机负载变化修正加速度计的动态误差，提升姿态估计的抗冲击能力，适配巡检场景的复杂地形；
自适应调整机制：检测到冲击时，自动增大过程噪声权重，让滤波算法快速响应冲击后的姿态变化，避免滤波滞后导致的姿态失真，保障电机控制的实时性与稳定性。

6、应急救援机器人——强干扰环境下的多传感器融合姿态估计（极限动态场景）
适用场景：地震废墟、洪涝、泥石流等应急救援场景，机器人面临强电磁干扰（如废墟下的电缆）、突发障碍（石块、漂浮物）、剧烈运动（颠簸、攀爬），传感器数据严重失真——IMU受电磁干扰漂移，编码器易因剧烈颠簸信号抖动，需通过多传感器融合与强鲁棒滤波算法，实现极限环境下的精确姿态估计，支撑BLDC电机的稳定驱动与救援任务执行。
核心逻辑：融合IMU、编码器、磁力计（可选）、BLDC电机扭矩/速度反馈，采用“粗滤波-精融合-误差修正”的三级架构——先用阈值过滤强干扰的传感器数据，再用加权融合算法整合多源数据，最后用电机反馈修正姿态误差；同时针对应急救援的极限动态，加入故障检测与容错机制，当某一传感器失效时，自动切换至备份融合模式，保障姿态估计不中断。

// 核心库引入
#include <Wire.h>
#include <MPU6050.h>
#include <SimpleFOC.h>
// 若配备磁力计，添加HMC5883L库，此处为简化演示

// 硬件引脚定义
#define MPU_ADDR 0x68
#define ENC_A_LEFT 2
#define ENC_B_LEFT 3
#define ENC_A_RIGHT 4
#define ENC_B_RIGHT 5
#define BLDC_LEFT_PWM 9
#define BLDC_LEFT_IN1 10
#define BLDC_LEFT_IN2 11
#define BLDC_RIGHT_PWM 6
#define BLDC_RIGHT_IN1 7
#define BLDC_RIGHT_IN2 8

// 强干扰与极限动态参数
const float ENC_CPR = 1200.0;
const float WHEEL_RADIUS = 55.0;
const float WHEEL_BASE = 280.0;
const float MAX_ACCEL_NOISE = 5.0;    // 最大加速度噪声阈值（强电磁干扰过滤）
const float MAX_GYRO_NOISE = 10.0;    // 最大角速度噪声阈值（剧烈颠簸过滤）
const float ENC_NOISE_THRESH = 0.5;    // 编码器噪声阈值（信号抖动过滤）
const float MOTOR_FEEDBACK_WEIGHT = 0.4; // 电机反馈融合权重（强干扰下提升）
const float BACKUP_FUSION_WEIGHT = 0.7; // 传感器失效时备份融合权重

// 极限环境下的姿态融合状态结构体
struct RescueAttitudeState {
  // 传感器原始数据（含干扰过滤标志）
  float accel_x, accel_y, accel_z;
  float gyro_x, gyro_y, gyro_z;
  float enc_left_v, enc_right_v;
  bool accel_valid, gyro_valid, enc_valid;
  // 多传感器融合姿态
  float fused_pitch, fused_roll, fused_yaw;
  float pitch_rate, roll_rate, yaw_rate;
  // 电机反馈数据
  float left_motor_output, right_motor_output;
  float motor_load_diff;
  // 故障与容错状态
  bool sensor_fault;
  int fault_sensor; // 0=无故障，1=IMU，2=编码器
} rescueState;

// 硬件对象实例化
MPU6050 mpu;
BLDCMotor motorLeft = BLDCMotor(11);
BLDCMotor motorRight = BLDCMotor(11);
BLDCDriver2PWM driverLeft = BLDCDriver2PWM(BLDC_LEFT_PWM, BLDC_LEFT_IN1, BLDC_LEFT_IN2);
BLDCDriver2PWM driverRight = BLDCDriver2PWM(BLDC_RIGHT_PWM, BLDC_RIGHT_IN1, BLDC_RIGHT_IN2);
Encoder encLeft(ENC_A_LEFT, ENC_B_LEFT);
Encoder encRight(ENC_A_RIGHT, ENC_B_RIGHT);

// 强干扰数据过滤：过滤强电磁干扰与剧烈颠簸导致的传感器噪声
void sensorNoiseFilter() {
  // IMU加速度计噪声过滤：超过阈值判定为干扰，标记无效
  float accel_magnitude = sqrt(pow(rescueState.accel_x,2)+pow(rescueState.accel_y,2)+pow(rescueState.accel_z,2));
  if (accel_magnitude > MAX_ACCEL_NOISE) {
    rescueState.accel_valid = false;
  } else {
    rescueState.accel_valid = true;
  }

  // IMU角速度噪声过滤：超过阈值判定为干扰，标记无效
  float gyro_magnitude = sqrt(pow(rescueState.gyro_x,2)+pow(rescueState.gyro_y,2)+pow(rescueState.gyro_z,2));
  if (gyro_magnitude > MAX_GYRO_NOISE) {
    rescueState.gyro_valid = false;
  } else {
    rescueState.gyro_valid = true;
  }

  // 编码器噪声过滤：速度突变判定为抖动，标记无效
  static float last_left_v = 0, last_right_v = 0;
  if (fabs(rescueState.enc_left_v - last_left_v) > ENC_NOISE_THRESH ||
      fabs(rescueState.enc_right_v - last_right_v) > ENC_NOISE_THRESH) {
    rescueState.enc_valid = false;
  } else {
    rescueState.enc_valid = true;
  }
  last_left_v = rescueState.enc_left_v;
  last_right_v = rescueState.enc_right_v;
}

// 故障检测与容错：检测传感器故障，自动切换备份融合模式
void sensorFaultDetection() {
  rescueState.sensor_fault = false;
  rescueState.fault_sensor = 0;

  if (!rescueState.accel_valid && !rescueState.gyro_valid) {
    rescueState.sensor_fault = true;
    rescueState.fault_sensor = 1; // IMU完全失效
    Serial.println("应急场景：IMU传感器失效，切换备份融合模式");
  } else if (!rescueState.enc_valid) {
    rescueState.sensor_fault = true;
    rescueState.fault_sensor = 2; // 编码器失效
    Serial.println("应急场景：编码器失效，切换备份融合模式");
  }
}

// 多传感器加权融合：融合有效传感器数据，强干扰下提升电机反馈权重
void multiSensorFusion() {
  float dt = 0.01;
  float base_pitch = 0.0, base_roll = 0.0, base_pitch_rate = 0.0, base_roll_rate = 0.0;
  float accel_weight = 0.0, gyro_weight = 0.0, enc_weight = 0.0, motor_weight = 0.0;

  // 正常模式：所有传感器有效，权重均等
  if (!rescueState.sensor_fault) {
    accel_weight = 0.25;
    gyro_weight = 0.3;
    enc_weight = 0.2;
    motor_weight = MOTOR_FEEDBACK_WEIGHT;

    // 加速度计计算静态姿态
    if (rescueState.accel_valid) {
      base_pitch = atan2(rescueState.accel_y, sqrt(pow(rescueState.accel_x,2)+pow(rescueState.accel_z,2)));
      base_roll = atan2(-rescueState.accel_x, sqrt(pow(rescueState.accel_y,2)+pow(rescueState.accel_z,2)));
    }

    // 角速度融合
    if (rescueState.gyro_valid) {
      base_pitch_rate = rescueState.gyro_x * (M_PI / 180.0);
      base_roll_rate = rescueState.gyro_y * (M_PI / 180.0);
    }

    // 编码器辅助修正运动误差
    if (rescueState.enc_valid) {
      float linear_v = (rescueState.enc_left_v + rescueState.enc_right_v) / 2.0;
      base_pitch += (linear_v * 0.0001) * sin(rescueState.enc_right_v - rescueState.enc_left_v);
      base_roll -= (linear_v * 0.0001) * cos(rescueState.enc_right_v - rescueState.enc_left_v);
    }

    // 电机反馈修正：负载变化导致的姿态误差
    rescueState.motor_load_diff = rescueState.left_motor_output - rescueState.right_motor_output;
    base_pitch += rescueState.motor_load_diff * 0.002;
    base_roll += rescueState.motor_load_diff * 0.001;

  } 
  // 备份模式：IMU失效，用编码器+电机反馈+预设姿态
  else if (rescueState.fault_sensor == 1) {
    accel_weight = 0.0;
    gyro_weight = 0.0;
    enc_weight = BACKUP_FUSION_WEIGHT;
    motor_weight = 1.0 - enc_weight;

    // 编码器估算运动姿态（基于线速度差）
    float angular_rate = (rescueState.enc_right_v - rescueState.enc_left_v) / WHEEL_BASE;
    base_pitch = rescueState.fused_pitch + angular_rate * dt;
    base_roll = rescueState.fused_roll + angular_rate * dt * 0.5;

    // 电机反馈维持基础姿态
    rescueState.motor_load_diff = rescueState.left_motor_output - rescueState.right_motor_output;
    base_pitch += rescueState.motor_load_diff * 0.003;
    base_roll += rescueState.motor_load_diff * 0.002;
  }
  // 备份模式：编码器失效，用IMU+电机反馈
  else if (rescueState.fault_sensor == 2) {
    accel_weight = 0.3;
    gyro_weight = 0.4;
    enc_weight = 0.0;
    motor_weight = 0.3;

    if (rescueState.accel_valid) {
      base_pitch = atan2(rescueState.accel_y, sqrt(pow(rescueState.accel_x,2)+pow(rescueState.accel_z,2)));
      base_roll = atan2(-rescueState.accel_x, sqrt(pow(rescueState.accel_y,2)+pow(rescueState.accel_z,2)));
    }
    if (rescueState.gyro_valid) {
      base_pitch_rate = rescueState.gyro_x * (M_PI / 180.0);
      base_roll_rate = rescueState.gyro_y * (M_PI / 180.0);
    }
    rescueState.motor_load_diff = rescueState.left_motor_output - rescueState.right_motor_output;
    base_pitch += rescueState.motor_load_diff * 0.002;
    base_roll += rescueState.motor_load_diff * 0.001;
  }

  // 融合姿态更新
  rescueState.fused_pitch += (base_pitch - rescueState.fused_pitch) * (accel_weight + enc_weight + motor_weight);
  rescueState.fused_roll += (base_roll - rescueState.fused_roll) * (accel_weight + enc_weight + motor_weight);
  rescueState.pitch_rate = base_pitch_rate;
  rescueState.roll_rate = base_roll_rate;
}

// 初始化函数
void setup() {
  Serial.begin(115200);
  Wire.begin();

  // IMU初始化
  mpu.initialize();
  mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_8G); // 救援极限动态，最大量程
  mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_1000DPS);

  // BLDC电机初始化
  driverLeft.init();
  driverRight.init();
  motorLeft.linkDriver(&driverLeft);
  motorRight.linkDriver(&driverRight);
  motorLeft.init();
  motorRight.init();
  motorLeft.initFOC();
  motorRight.initFOC();
  motorLeft.controller = MotionControlType::velocity;
  motorRight.controller = MotionControlType::velocity;

  // 编码器初始化
  encLeft.init();
  encRight.init();
  motorLeft.linkSensor(&encLeft);
  motorRight.linkSensor(&encRight);

  // 姿态融合初始化
  rescueState.fused_pitch = 0.0;
  rescueState.fused_roll = 0.0;
  rescueState.fused_yaw = 0.0;
  rescueState.accel_valid = false;
  rescueState.gyro_valid = false;
  rescueState.enc_valid = false;
  rescueState.sensor_fault = false;
  rescueState.fault_sensor = 0;

  Serial.println("应急救援机器人：强干扰环境姿态估计系统初始化完成");
}

void loop() {
  // 1. 读取传感器原始数据
  mpu.getMotion6(&rescueState.accel_x, &rescueState.accel_y, &rescueState.accel_z,
                 &rescueState.gyro_x, &rescueState.gyro_y, &rescueState.gyro_z);

  // 2. 读取编码器数据
  rescueState.enc_left_v = (encLeft.getVelocity() * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
  rescueState.enc_right_v = (encRight.getVelocity() * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);

  // 3. 读取电机反馈
  rescueState.left_motor_output = motorLeft.controller_pid.output;
  rescueState.right_motor_output = motorRight.controller_pid.output;

  // 4. 强干扰数据过滤
  sensorNoiseFilter();

  // 5. 故障检测与容错
  sensorFaultDetection();

  // 6. 多传感器加权融合
  multiSensorFusion();

  // 7. 根据融合姿态控制BLDC电机：极限环境下保障姿态稳定
  float pitch_correct = rescueState.fused_pitch * 10.0;
  float roll_correct = rescueState.fused_roll * 15.0;
  float left_target_v = 120.0 + roll_correct - pitch_correct * 0.5;
  float right_target_v = 120.0 - roll_correct + pitch_correct * 0.5;

  float left_rpm = (left_target_v * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
  float right_rpm = (right_target_v * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);

  motorLeft.target = left_rpm;
  motorRight.target = right_rpm;
  motorLeft.move(50);
  motorRight.move(50);

  // 8. 输出姿态与故障状态
  String faultStr = rescueState.sensor_fault ? "传感器故障（" + String(rescueState.fault_sensor) + "）" : "正常";
  Serial.print("融合姿态：俯仰=" + String(rescueState.fused_pitch * 180 / M_PI,1) + "°" +
               " 横滚=" + String(rescueState.fused_roll * 180 / M_PI,1) + "°" +
               " 状态=" + faultStr);
  Serial.println();

  delay(10);
}

代码逻辑说明：
强干扰数据过滤：通过阈值过滤强电磁干扰、剧烈颠簸导致的传感器噪声，标记无效数据，避免噪声数据污染融合结果；
故障容错与备份融合：当某一传感器失效时，自动切换至备份融合模式——IMU失效时用编码器+电机反馈，编码器失效时用IMU+电机反馈，保障极限环境下姿态估计不中断；
动态权重调整：强干扰场景下提升电机反馈的融合权重，利用电机负载变化反馈机械姿态变化，补充传感器失效或干扰导致的数据缺失，实现应急救援极限动态下的稳定姿态估计。

要点解读

1. 多传感器融合：动态环境姿态估计的核心基石，解决单一传感器局限性
   动态环境中，单一传感器无法应对复杂的干扰与运动特性，多传感器融合通过优势互补，实现精度与鲁棒性的双重提升：
   传感器互补特性：IMU（MPU6050）感知高频角速度与加速度，但易受振动、冲击漂移；编码器感知精确的轮速/位移，可辅助修正IMU运动误差；BLDC电机反馈反映负载与运动状态，能补充传感器无法感知的机械姿态变化——三者结合，既覆盖高频动态响应，又保障低频姿态稳定性。
   融合架构适配场景：工业物流场景采用互补滤波，保障实时性与振动抑制；巡检场景采用卡尔曼滤波，适配复杂地形的动态冲击；应急救援场景采用三级融合+容错架构，应对极限干扰与传感器失效——根据场景的动态强度与干扰类型，选择匹配的融合算法，最大化姿态估计效能。
   核心价值：多传感器融合打破单一传感器的性能边界，从根本上解决动态环境下姿态数据漂移、失真、滞后的问题，为BLDC电机控制提供可靠的感知基础，是姿态估计系统稳定运行的核心。
2. 动态误差补偿：针对性抑制环境与机械干扰，保障姿态精度
   动态环境中的振动、冲击、电磁干扰会导致传感器数据误差，动态误差补偿通过建模与自适应调整，从源头抑制误差，确保姿态精度：
   误差来源与补偿机制：
   振动补偿：工业物流场景中，地面振动导致IMU数据波动，通过编码器运动趋势识别振动强度，自适应补偿IMU角速度，抵消振动干扰；
   冲击补偿：巡检场景中，地形冲击导致IMU数据突变，通过卡尔曼滤波的过程噪声自适应调整，快速响应冲击后的姿态变化，避免误差累积；
   干扰过滤：应急救援场景中，强电磁干扰导致传感器数据失真，通过阈值过滤噪声数据，避免无效数据参与融合，保障数据质量。
   动态建模适配性：针对不同类型的动态干扰（振动、冲击、电磁），采用差异化的补偿策略，而非单一的滤波，确保补偿机制与干扰特性匹配，大幅提升姿态估计的抗干扰能力，适配救援、巡检等极限动态场景。
3. 滤波算法选型：适配场景动态特性，平衡实时性与精度
   滤波算法是姿态估计的核心，不同场景的动态强度、干扰特性不同，需针对性选型，平衡嵌入式平台的计算能力、实时性与姿态精度：
   互补滤波：低成本、高实时性场景适配：工业物流场景振动干扰强，但对实时性要求高，互补滤波通过简单的加权融合，结合IMU高频响应与加速度计低频稳定性，配合编码器运动反馈，算法计算量小、响应速度快，能在Arduino嵌入式平台上实现高频姿态更新（100Hz以上），保障物流机器人的运动控制实时性。
   卡尔曼滤波：高动态、复杂干扰场景适配：巡检场景存在地形冲击与机械振动，卡尔曼滤波通过建立动态噪声模型，融合多源数据，实时估计姿态最优值，既能抑制动态冲击导致的误差，又能保持姿态稳定性；同时可通过自适应调整噪声参数，平衡精度与计算量，满足嵌入式平台的算力约束。
   多源融合滤波：极限干扰、传感器失效场景适配：应急救援场景面临强干扰与传感器失效风险，多源融合滤波采用三级架构（过滤-融合-容错），结合阈值过滤、加权融合与备份模式，在极限条件下保障姿态估计不中断，同时通过动态权重调整提升抗干扰能力，适配应急救援的高可靠性需求。
4. BLDC闭环联动：姿态估计与电机控制深度融合，保障运动稳定性
   姿态估计的最终目的是支撑BLDC电机的稳定控制，二者的闭环联动是保障机器人运动稳定性的关键，形成“姿态感知-决策-控制-反馈”的闭环：
   联动逻辑闭环：姿态估计实时输出俯仰、横滚角度，作为电机控制的输入参数——当姿态倾斜时，根据倾斜角度动态调整左右BLDC电机的转速或扭矩，平衡姿态；同时电机的负载反馈与运动状态，反向修正姿态估计的误差，形成闭环优化，避免姿态估计与电机控制脱节。
   动态控制适配：工业物流场景中，姿态倾斜时，通过调整左右轮转速抵消货物倾斜趋势；巡检场景中，遭遇冲击时，姿态估计数据实时修正电机扭矩，保障机器人在复杂地形的通过性；应急救援场景中，传感器失效时，电机反馈作为姿态估计的补充，维持基本运动控制，确保机器人不失控。
   核心价值：姿态估计与BLDC闭环联动，将姿态数据转化为实际的运动控制动作，既发挥姿态估计的感知价值，又通过电机控制将感知转化为稳定运动，是动态环境下机器人实现自主运动的核心保障。
5. 系统鲁棒性与可靠性：应急场景的核心底线，保障极限环境持续运行
   应急救援等极限场景对系统可靠性要求极高，一旦姿态估计失效，机器人可能失控、延误救援，因此鲁棒性与可靠性是系统的底线：
   多维度鲁棒设计：
   硬件层面：预留传感器冗余接口（如磁力计接口，可补充偏航角），采用抗干扰硬件设计，提升传感器的抗环境干扰能力；
   软件层面：加入传感器故障检测、容错切换机制，当传感器失效时，自动切换备份融合模式，保障姿态估计不中断；
   数据层面：采用阈值过滤、数据校验机制，过滤无效数据，避免噪声数据污染融合结果，提升数据可靠性。
   可靠性适配应急场景：应急救援中，机器人可能面临强电磁干扰、剧烈颠簸、传感器失效等极端情况，通过鲁棒性设计，确保姿态估计系统在恶劣条件下仍能稳定工作，即使局部传感器失效，也能维持基本的姿态感知与电机控制，保障救援任务不中断，为救援机器人的可靠运行提供核心支撑。

请注意：以上案例仅作为思路拓展的参考示例，不保证完全正确、适配所有场景或可直接编译运行。由于硬件平台、实际使用场景、Arduino 版本的差异，均可能影响代码的适配性与使用方法的选择。在实际编程开发时，请务必根据自身硬件配置、使用场景及具体功能需求进行针对性调整，并通过多次实测验证效果；同时需确保硬件接线正确，充分了解所用传感器、执行器等设备的技术规范与核心特性。对于涉及硬件操作的代码，使用前务必核对引脚定义、电平参数等关键信息的准确性与安全性，避免因参数错误导致硬件损坏或运行异常。