Arduino BLDC水下机器人三路IMU三模冗余姿态投票，是采用三套独立IMU传感器并行采集姿态数据，通过表决逻辑（Voting Logic）实现故障检测、隔离与容错输出的高可靠性姿态感知方案。 该方案具备三余度容错与故障隔离、中值投票抗干扰、降级运行与平滑切换、多维度故障评估四大特点，主要应用于深水AUV/ROV、水下桥梁与管线检测、水下应急救援与打捞及科研教学等场景；实际部署时需重点关注I2C地址冲突与总线负载、主控算力与实时性、IMU安装与抗振、水下密封与压力防护、故障阈值与切换平滑性及电源隔离与EMC防护。
一、 技术架构与主要特点
三余度容错与故障隔离：传统单IMU方案存在单点故障风险，一旦传感器失效将导致姿态解算错误甚至机器人失控。三模冗余（TMR）架构通过部署三套物理独立的IMU（如三颗MPU6050/ICM-42688），实现"少数服从多数"的表决逻辑。当某一路IMU出现数据异常（如I2C通信超时、数据跳变、零偏漂移超限）时，系统自动将其隔离，仅采信其余两路的一致数据，确保单路故障不影响整体运行。
中值投票与加权融合：核心表决算法通常采用中值投票（Median Voting）：对三路IMU输出的同一姿态角（Roll/Pitch/Yaw）取中值作为最终输出。中值天然过滤掉偏离值，即使一路IMU输出完全错误，只要另外两路正常，输出依然可靠。进阶方案还会引入多维度故障评估与加权投票：对各路IMU数据的历史一致性、噪声方差、零偏稳定性等维度进行综合评分，动态调整各路权重，而非简单的等权投票，从而提升故障检测精度。
降级运行与平滑切换：当一路IMU被判定故障后，系统从"三模冗余"自动降级为"双模运行"。为防止切换瞬间姿态数据跳变导致BLDC电机控制震荡，需对输出做渐进式修正——将当前采信数据按线性或非线性调整率逐步过渡到目标IMU数据，避免大幅切换影响运动稳定性。
故障计数与健康监控：每路IMU维护独立的故障计数器（faultCount），当连续N个周期数据异常时累计加1，超过阈值后正式标记为"故障"并隔离。这种机制可避免因偶发通信干扰导致的误判，提升系统鲁棒性。
二、 典型应用场景
深水AUV/ROV姿态控制：水下机器人无法接收GPS信号，IMU是唯一的连续姿态感知源。在深水高压、强水流扰动环境下，单IMU可能因电磁干扰或机械冲击产生数据异常。三模冗余投票确保即使单路失效，机器人仍能维持精确姿态控制，避免触底或撞壁。
水下桥梁/管线检测：在桥墩、桩基等狭窄空间中进行精细作业时，机器人需要极高的姿态稳定性和操控精度。三模冗余IMU提供可靠的姿态反馈，配合BLDC推进器的FOC闭环控制，实现六自由度（浮潜、俯仰、横滚、偏航、悬停、定高）的精准操控。
水下应急救援与打捞：AUV在执行水下搜救任务时，若姿态系统失效可能导致设备丢失。三模冗余架构将单点故障概率降低数个数量级，配合水下智能救援系统（如气囊上浮机制），大幅提升设备回收率。
教学与科研验证平台：成本远低于商用惯导系统，适合高校用于"容错控制""多传感器冗余设计"等课程的教学实训，也可用于RoboMaster等机器人竞赛中的高可靠性姿态系统验证。
三、 关键注意事项
I2C地址冲突与总线负载：三颗同型号IMU（如MPU6050）默认I2C地址相同，需通过AD0引脚配置不同地址（0x68/0x69），或使用I2C多路复用器（如TCA9548A）进行通道隔离。三颗IMU同时挂载在同一I2C总线上会增加总线电容，建议将通信速率控制在400kHz以内，并在总线上加4.7kΩ上拉电阻。
主控算力与实时性保障：三路IMU数据采集、姿态解算（互补滤波/EKF）与表决逻辑对算力要求较高。标准Arduino Uno（16MHz）难以胜任，建议采用ESP32（双核240MHz）或STM32F4/F7。控制回路必须使用硬件定时器中断或非阻塞定时（millis()），严禁使用delay()函数，确保控制频率≥100Hz。
IMU安装与抗振设计：三颗IMU必须刚性固定在机器人重心附近，并加硅胶减震垫以隔离BLDC推进器的高频振动。振动噪声会严重干扰陀螺仪和加速度计数据，导致表决逻辑误判。同时，三颗IMU的安装方向需严格一致（或通过软件旋转矩阵校正），否则姿态输出将出现系统性偏差。
水下密封与压力防护：水下环境对电子设备的密封性要求极高。IMU模块需封装在耐压舱内，舱体材料（如铝合金/亚克力）和密封圈需根据工作深度选择。深水环境下，压力变化可能影响IMU的零偏稳定性，需在出厂前进行压力标定补偿。
故障阈值与切换平滑性：
故障判定阈值：需根据实际工况反复调试。阈值过松会导致故障IMU无法及时隔离，过严则可能将正常波动误判为故障。建议设置角度偏差阈值（如>5°）和通信超时阈值（如>50ms）双重判定。
切换平滑：故障隔离后的数据切换必须做缓变处理，避免姿态角突变导致BLDC推进器产生冲击扭矩。建议采用指数移动平均（EMA）进行过渡。
电源隔离与EMC防护：BLDC推进器启停时电流冲击极大，严禁与Arduino及IMU共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在推进器驱动端并联大容量低ESR电容吸收反电动势。IMU信号线需使用屏蔽双绞线，与动力线间距≥50mm。
陀螺仪零偏校准：系统上电后，必须在静止状态下对三颗IMU分别进行零偏校准（至少采集1000组数据取均值），否则积分漂移会迅速导致姿态角计算错误。建议每次下水前重新校准，因为温度变化会影响零偏值。
安全保护机制：
三路全故障保护：当三颗IMU全部被判定故障时，立即触发安全模式——BLDC推进器减速至零并锁定姿态，同时上报故障码。
姿态超限保护：当姿态角超过安全阈值（如俯仰>45°）时，自动关闭推进器并触发上浮机制。
通讯超时保护：与上位机的通讯中断超过设定时间（如5秒），自动进入安全悬停模式。

1、三路IMU并行采集与中值投票（基础冗余架构）
适用场景：低成本水下机器人原型，三路MPU6050通过I2C总线接入，采用硬投票/中值滤波实现基础故障隔离。

核心逻辑：将三路IMU配置为不同的数字低通滤波器带宽（DLPF），制造传感器间的差异性以增强鲁棒性。每个周期并行采集三路数据，对三组姿态角进行排序后取中值作为投票结果。若某一路与中值偏差超过阈值（FAULT_THRESHOLD），则判定该路为故障并标记隔离。

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

// ==================== 三路IMU定义 ====================
MPU6050 imuA, imuB, imuC;

// ==================== 姿态数据结构 ====================
struct ImuData {
    float pitch, roll, yaw;
    float gx, gy, gz;
    bool valid;
};

ImuData imuReadings[3];
bool imuFaultFlags[3] = {false, false, false};

// ==================== 投票参数 ====================
const float FAULT_THRESHOLD = 15.0;   // 角度偏差阈值(度)
const int SAMPLE_COUNT = 5;           // 中值滤波窗口

void setup() {
    Serial.begin(115200);
    Wire.begin();

    // 初始化三路IMU，配置差异化滤波器带宽
    imuA.initialize();
    imuB.initialize();
    imuC.initialize();
    
    // 差异化配置：增强系统鲁棒性，避免共因失效[citation:1]
    imuA.setDLPFMode(MPU6050_DLPF_BW_94);   // 快速响应
    imuB.setDLPFMode(MPU6050_DLPF_BW_44);   // 中等
    imuC.setDLPFMode(MPU6050_DLPF_BW_21);   // 高抗噪
    
    Serial.println("Triple IMU Voting System Ready");
}

// ==================== 单路姿态读取 ====================
bool readIMU(MPU6050& imu, ImuData& data) {
    int16_t ax, ay, az, gx, gy, gz;
    imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    
    if (imu.getDeviceID() == 0x34) {  // 简单校验
        // 加速度计计算姿态角（静态参考）
        data.pitch = atan2(ay, az) * 180.0 / PI;
        data.roll = atan2(-ax, sqrt(ay*ay + az*az)) * 180.0 / PI;
        // 磁力计提供偏航，此处简化
        data.yaw = 0;
        data.gx = gx / 131.0;  // 转为度/秒
        data.gy = gy / 131.0;
        data.gz = gz / 131.0;
        data.valid = true;
        return true;
    }
    data.valid = false;
    return false;
}

// ==================== 中值滤波投票 ====================
float medianVote(float arr[], int n) {
    // 简单冒泡排序取中值
    for (int i = 0; i < n-1; i++) {
        for (int j = 0; j < n-i-1; j++) {
            if (arr[j] > arr[j+1]) {
                float temp = arr[j];
                arr[j] = arr[j+1];
                arr[j+1] = temp;
            }
        }
    }
    return arr[n/2];
}

// ==================== 三路投票核心函数 ====================
float votePitch() {
    float values[3];
    int validCount = 0;
    
    for (int i = 0; i < 3; i++) {
        if (imuReadings[i].valid && !imuFaultFlags[i]) {
            values[validCount++] = imuReadings[i].pitch;
        }
    }
    
    if (validCount < 2) {
        Serial.println("WARN: Insufficient valid IMUs!");
        return imuReadings[0].pitch;  // 退化为单路
    }
    
    // 取中值作为投票结果
    float voted = medianVote(values, validCount);
    
    // 故障检测：检查每路与投票结果的偏差
    for (int i = 0; i < 3; i++) {
        if (imuReadings[i].valid && !imuFaultFlags[i]) {
            if (abs(imuReadings[i].pitch - voted) > FAULT_THRESHOLD) {
                imuFaultFlags[i] = true;
                Serial.print("IMU "); Serial.print(i); Serial.println(" fault detected!");
            }
        }
    }
    
    return voted;
}

void loop() {
    // 1. 并行采集三路IMU
    readIMU(imuA, imuReadings[0]);
    readIMU(imuB, imuReadings[1]);
    readIMU(imuC, imuReadings[2]);
    
    // 2. 中值投票融合
    float fusedPitch = votePitch();
    
    // 3. 输出融合姿态
    Serial.print("Fused Pitch: "); Serial.println(fusedPitch);
    
    delay(50);
}

2、硬投票+软权重融合（带冗余管理算法）
适用场景：参考航空/航海三余度系统设计，使用分级故障判断与动态权重分配机制，在排除故障传感器后重新归一化权重进行加权平均。

核心逻辑：首先进行故障初判断——计算三路IMU姿态值的加权均值，将各值与均值的偏差与门限t1比较，超出则标记为待定。随后进行故障再判断——结合历史数据与运动模型验证，确认故障后将该路权重置零。剩余传感器权重重新归一化后加权融合。

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

// ==================== 三路IMU ====================
MPU6050 imu[3];  // 数组形式管理
float imuPitch[3], imuRoll[3], imuYaw[3];
bool imuHealthy[3] = {true, true, true};

// ==================== 权重与故障判断参数 ====================
float weights[3] = {0.4, 0.35, 0.25};  // 初始权重（可根据置信度调整）
const float T1_THRESHOLD = 10.0;        // 初判断门限(度)
const float T2_THRESHOLD = 15.0;        // 再判断门限(度)
const int CONSISTENCY_WINDOW = 5;       // 一致性检查窗口

// 历史数据用于再判断
float pitchHistory[3][10];
int historyIdx = 0;

void setup() {
    Serial.begin(115200);
    Wire.begin();
    
    for (int i = 0; i < 3; i++) {
        imu[i].initialize();
        // 差异化配置
        imu[i].setDLPFMode(i == 0 ? MPU6050_DLPF_BW_94 : 
                           i == 1 ? MPU6050_DLPF_BW_44 : MPU6050_DLPF_BW_21);
    }
}

// ==================== 故障初判断 ====================
bool initialFaultCheck(float values[], int n) {
    // 计算加权均值
    float weightedSum = 0, totalW = 0;
    for (int i = 0; i < n; i++) {
        if (imuHealthy[i]) {
            weightedSum += values[i] * weights[i];
            totalW += weights[i];
        }
    }
    float mean = (totalW > 0) ? weightedSum / totalW : values[0];
    
    // 检查各路与均值的偏差
    for (int i = 0; i < n; i++) {
        if (imuHealthy[i]) {
            if (abs(values[i] - mean) > T1_THRESHOLD) {
                // 偏差过大，进入再判断
                return false;
            }
        }
    }
    return true;  // 所有路均健康
}

// ==================== 故障再判断（结合历史一致性）====================
bool recheckFault(int idx, float currentVal) {
    // 检查该路最近N次输出是否与系统输出一致
    // 简化实现：检查历史均值与当前值偏差
    float histSum = 0;
    for (int i = 0; i < CONSISTENCY_WINDOW; i++) {
        histSum += pitchHistory[idx][(historyIdx - 1 - i + 10) % 10];
    }
    float histMean = histSum / CONSISTENCY_WINDOW;
    
    if (abs(currentVal - histMean) > T2_THRESHOLD) {
        return true;  // 确认故障
    }
    return false;
}

// ==================== 权重归一化融合 ====================
float weightedVoteFusion() {
    float fused = 0, totalWeight = 0;
    
    for (int i = 0; i < 3; i++) {
        if (imuHealthy[i]) {
            fused += imuPitch[i] * weights[i];
            totalWeight += weights[i];
        }
    }
    
    if (totalWeight > 0) {
        // 归一化
        return fused / totalWeight;
    }
    return imuPitch[0];  // 退化
}

void loop() {
    // 1. 读取三路IMU
    for (int i = 0; i < 3; i++) {
        int16_t ax, ay, az, gx, gy, gz;
        imu[i].getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        imuPitch[i] = atan2(ay, az) * 180.0 / PI;
        // 保存历史
        pitchHistory[i][historyIdx] = imuPitch[i];
    }
    historyIdx = (historyIdx + 1) % 10;
    
    // 2. 故障初判断
    bool allHealthy = initialFaultCheck(imuPitch, 3);
    
    if (!allHealthy) {
        // 3. 再判断：找出故障路
        for (int i = 0; i < 3; i++) {
            if (imuHealthy[i] && recheckFault(i, imuPitch[i])) {
                imuHealthy[i] = false;
                weights[i] = 0;
                Serial.print("IMU "); Serial.print(i); Serial.println(" confirmed faulty.");
            }
        }
        // 权重重新归一化[citation:2][citation:7]
        float newTotal = weights[0] + weights[1] + weights[2];
        if (newTotal > 0) {
            for (int i = 0; i < 3; i++) {
                weights[i] = weights[i] / newTotal;
            }
        }
    }
    
    // 4. 加权投票融合
    float fused = weightedVoteFusion();
    Serial.print("Fused: "); Serial.println(fused);
    delay(50);
}

3、UKF故障检测与隔离（无迹卡尔曼滤波融合）
适用场景：高精度水下机器人应用，三路IMU存在非线性噪声和软故障。使用无迹卡尔曼滤波（UKF） 对三路传感器数据进行状态估计，同时实现故障检测与隔离（FDI）。

核心逻辑：构建UKF系统状态（四元数+陀螺偏置），以三路IMU的角速度和加速度作为测量输入。UKF的残差序列反映传感器异常——当某路IMU的残差持续超过阈值（卡方检验），标记该路为故障并隔离。剩余健康IMU继续参与状态更新。

#include <SimpleFOC.h>
#include <MPU6050.h>
#include <BasicLinearAlgebra.h>  // 矩阵运算库

// ==================== 三路IMU ====================
MPU6050 imu[3];

// ==================== UKF状态变量 ====================
// 状态向量: [q0, q1, q2, q3, bx, by, bz]  (四元数 + 陀螺偏置)
const int STATE_DIM = 7;
float x[STATE_DIM] = {1, 0, 0, 0, 0, 0, 0};  // 初始姿态
float P[STATE_DIM][STATE_DIM];  // 协方差矩阵
float Q[STATE_DIM][STATE_DIM];  // 过程噪声
float R[3][3];                  // 测量噪声

float imuMeasurements[3][3];    // 三路IMU测量值: [roll, pitch, yaw]
float innovation[3];             // 新息(残差)
float innovationCov[3][3];
bool imuFault[3] = {false, false, false};

// ==================== UKF参数 ====================
const float UKF_ALPHA = 0.001;   // 比例因子
const float UKF_BETA = 2.0;      // 高斯分布参数
const float UKF_KAPPA = 0.0;     // 次级比例参数
const float FAULT_THRESHOLD = 3.0; // 卡方检验阈值

// ==================== UKF初始化 ====================
void initUKF() {
    // 初始化协方差矩阵
    for (int i = 0; i < STATE_DIM; i++) {
        for (int j = 0; j < STATE_DIM; j++) {
            P[i][j] = (i == j) ? 0.1 : 0;
        }
    }
    // 过程噪声
    for (int i = 0; i < STATE_DIM; i++) {
        Q[i][i] = 0.001;
    }
    // 测量噪声
    for (int i = 0; i < 3; i++) {
        R[i][i] = 0.01;
    }
}

// ==================== 四元数姿态更新 ====================
void quaternionUpdate(float gx, float gy, float gz, float dt) {
    // 简单四元数积分（实际UKF中由状态方程处理）
    // 此处简化实现
    float q0 = x[0], q1 = x[1], q2 = x[2], q3 = x[3];
    float q0_dot = (-q1*gx - q2*gy - q3*gz) * 0.5;
    float q1_dot = ( q0*gx - q3*gy + q2*gz) * 0.5;
    float q2_dot = ( q3*gx + q0*gy - q1*gz) * 0.5;
    float q3_dot = (-q2*gx + q1*gy + q0*gz) * 0.5;
    
    x[0] += q0_dot * dt;
    x[1] += q1_dot * dt;
    x[2] += q2_dot * dt;
    x[3] += q3_dot * dt;
    
    // 归一化
    float norm = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2] + x[3]*x[3]);
    for (int i = 0; i < 4; i++) x[i] /= norm;
}

// ==================== 故障检测（卡方检验） ====================
bool checkFault(int imuIdx) {
    // 计算马氏距离: d = z' * S_inv * z
    // 若d > 阈值，判定为故障
    // 简化版：检查残差是否超过阈值
    float innovationNorm = 0;
    for (int i = 0; i < 3; i++) {
        innovationNorm += innovation[i] * innovation[i];
    }
    return innovationNorm > FAULT_THRESHOLD;
}

void loop() {
    // 1. 读取三路IMU（每路取前2秒静态校准值）
    for (int i = 0; i < 3; i++) {
        if (imuFault[i]) continue;
        int16_t ax, ay, az, gx, gy, gz;
        imu[i].getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        // 近似姿态角
        imuMeasurements[i][0] = atan2(ay, az) * 180.0 / PI;   // roll
        imuMeasurements[i][1] = atan2(-ax, sqrt(ay*ay + az*az)) * 180.0 / PI; // pitch
        imuMeasurements[i][2] = 0;  // yaw from magnetometer
    }
    
    // 2. 选择健康IMU的主测量值（取第一路健康IMU）
    int primaryIdx = 0;
    for (int i = 0; i < 3; i++) {
        if (!imuFault[i]) { primaryIdx = i; break; }
    }
    
    // 3. UKF预测与更新（简化演示）
    float dt = 0.05;
    // 使用主IMU的陀螺仪更新状态
    int16_t gx0, gy0, gz0;
    imu[primaryIdx].getRotation(&gx0, &gy0, &gz0);
    quaternionUpdate(gx0/131.0, gy0/131.0, gz0/131.0, dt);
    
    // 4. 计算各IMU与UKF状态的残差
    for (int i = 0; i < 3; i++) {
        if (imuFault[i]) continue;
        float estimatedPitch = asin(-2*(x[1]*x[3] - x[0]*x[2])) * 180.0 / PI;
        innovation[i] = imuMeasurements[i][1] - estimatedPitch;
    }
    
    // 5. 故障检测与隔离[citation:8]
    for (int i = 0; i < 3; i++) {
        if (!imuFault[i] && checkFault(i)) {
            imuFault[i] = true;
            Serial.print("IMU "); Serial.print(i); Serial.println(" isolated by UKF FDI.");
        }
    }
    
    // 6. 输出融合姿态
    float fusedPitch = asin(-2*(x[1]*x[3] - x[0]*x[2])) * 180.0 / PI;
    Serial.print("Fused Pitch: "); Serial.println(fusedPitch);
    delay(50);
}

要点解读

1. 三模冗余是水下高可靠性系统的“硬性要求”：水下环境信号屏蔽、传感器失效风险高，单路IMU无法满足任务可靠性要求。航空标准中，三冗余INS的故障率可低于10⁻⁹，是满足关键任务设计的必要条件。三路IMU通过差异化硬件配置（如不同DLPF带宽）增强鲁棒性，避免共因失效。

2. 投票机制的核心是“故障检测与隔离”（FDI）：三模冗余不是简单的“取平均”，而是先检测、隔离故障，再融合健康数据。典型流程为“初判断→再判断→权重归一化”：初判断用偏差阈值筛选可疑传感器；再判断结合历史一致性或运动模型验证；确认故障后排除并归一化剩余权重。

3. 硬投票（中值）适用于突发大故障，加权投票适用于软故障：硬投票（中值滤波/多数表决）鲁棒性强，但信息利用率低；加权投票（动态权重）精度更高，但依赖可靠的权重估计策略。权重分配可基于传感器方差——方差越小权重越大，或基于残差检测——偏差越大权重越小。

4. UKF是应对非线性水下运动与软故障的进阶方案：水下机器人运动呈高度非线性（流体阻力、洋流干扰），标准卡尔曼滤波假设线性不成立。无迹卡尔曼滤波（UKF）通过sigma点传递非线性，精度优于EKF。UKF的残差序列可用于检测传感器软故障，通过卡方检验判断测量是否异常，实现FDI功能。

5. 工程约束：I2C总线冲突与算力分配：三路IMU通过同一I2C总线接入可能产生冲突。解决方案包括：不同I2C地址（MPU6050地址可由AD0引脚配置，最多支持2路）、I2C多路复用器（TCA9548A支持8路）、或SPI接口IMU（如ICM-20948）避免总线竞争。UKF的矩阵运算（维度7~15）在Arduino Uno上可能超时，建议使用ESP32（双核+FPU）或Arduino Due运行UKF，或降级为互补滤波融合。

4、基础三路IMU冗余投票 + BLDC推进姿态闭环控制
适用场景：水下机器人直线航行与基础姿态调整，需通过三路IMU投票输出稳定横滚角（Roll），控制BLDC推进器维持机体平衡，防止水下侧翻，适用于浅水稳定环境的常规巡检。

核心逻辑
三路独立采样：三路IMU独立采集加速度计与陀螺仪数据，避免单节点失效牵连整体；
多源融合计算：每路IMU通过互补滤波融合加速度计与陀螺仪，求解横滚角；
多数投票仲裁：对三路姿态角排序，剔除极端值，取中间值作为可靠输出；
故障自检隔离：实时监测单路数据漂移，异常则标记为“无效”，并在冗余体系中剔除；
闭环姿态控制：基于投票后的横滚角，通过BLDC推进器纠正机体倾斜，维持平衡。

#include <Wire.h>
#include <MPU6050.h> // 简易IMU驱动库
#include <SimpleFOC.h>

// 硬件配置
BLDCMotor motorLeft(9), motorRight(10);  // 两侧推进BLDC
BLDCDriver3PWM drvL(3,5,6), drvR(11,12,13);

// 三路IMU（MPU6050）I2C地址
const uint8_t imuAddr[3] = {0x68, 0x69, 0x6A}; // 需通过焊盘调整硬件地址

// IMU实例
MPU6050 imu[3];

// 三路姿态数据存储
float roll[3] = {0};      // 横滚角
bool imuValid[3] = {true, true, true}; // 传感器有效性标记

// 互补滤波参数
const float alpha = 0.98; // 陀螺仪权重，加速度计占0.02
float compRoll[3] = {0};  // 互补滤波后的横滚角

// FSM状态（冗余状态管理）
enum RedundantState {
  STATE_NORMAL = 0,   // 三路正常
  STATE_ONE_FAIL = 1, // 单路失效
  STATE_TWO_FAIL = 2  // 两路失效（极端，需紧急处理）
};
RedundantState redundantState = STATE_NORMAL;

// 初始化函数
void setup() {
  Serial.begin(115200);
  // 初始化BLDC推进器
  motorLeft.linkDriver(&drvL); motorRight.linkDriver(&drvR);
  motorLeft.linkSensor(&dummyEnc); motorRight.linkSensor(&dummyEnc); // 简化编码器
  motorLeft.controller = MotionControlType::velocity;
  motorRight.controller = MotionControlType::velocity;
  motorLeft.init(); motorLeft.initFOC();
  motorRight.init(); motorRight.initFOC();

  // 初始化三路IMU
  for (int i = 0; i < 3; i++) {
    Wire.begin();
    imu[i].initialize(imuAddr[i]);
    if (!imu[i].testConnection()) {
      Serial.print("IMU "); Serial.print(i); Serial.println(" 连接失败，标记为无效");
      imuValid[i] = false;
    } else {
      imu[i].setFullScaleGyroRange(MPU6050_GYRO_FS_250); // 250°/s量程适配水下
      imu[i].setFullScaleAccelRange(MPU6050_ACCEL_FS_2); // 2g量程
    }
  }
}

void loop() {
  // 1. 三路IMU数据采集与互补滤波
  for (int i = 0; i < 3; i++) {
    if (!imuValid[i]) continue;

    // 读取加速度计和陀螺仪
    int16_t accX = imu[i].getAccelerationX();
    int16_t accY = imu[i].getAccelerationY();
    int16_t accZ = imu[i].getAccelerationZ();
    int16_t gyroX = imu[i].getRotationX();
    int16_t gyroY = imu[i].getRotationY();

    // 计算重力方向的横滚角（加速度计）
    float accRoll = atan2(accX, accZ) * RAD_TO_DEG;

    // 计算陀螺仪横滚角变化率（转换为角速度rad/s）
    float gyroRollRate = gyroY * 0.00745; // 250°/s量程，每LSB对应0.00745°/s

    // 互补滤波融合
    compRoll[i] = alpha * (compRoll[i] + gyroRollRate * 0.01) + (1 - alpha) * accRoll;
    roll[i] = compRoll[i];

    // 自检：若角度漂移超限（±30°无外力下持续超1秒），标记为失效
    if (abs(accRoll) > 60) {
      Serial.print("IMU "); Serial.print(i); Serial.println(" 加速度计漂移超限，标记失效");
      imuValid[i] = false;
    }
  }

  // 2. 多数投票输出可靠姿态
  float votedRoll = 0;
  int validCount = 0;
  for (int i = 0; i < 3; i++) if (imuValid[i]) validCount++;

  if (validCount >= 2) {
    // 收集有效数据
    float validRolls[2];
    int idx = 0;
    for (int i = 0; i < 3; i++) {
      if (imuValid[i]) validRolls[idx++] = roll[i];
    }
    // 多数投票：排序取中间值（2路时直接平均，等效多数）
    sort(validRolls, validRolls + 2);
    votedRoll = validRolls[0] + (validRolls[1] - validRolls[0]) / 2;
  } else if (validCount == 1) {
    // 仅1路有效，直接采用
    for (int i = 0; i < 3; i++) if (imuValid[i]) votedRoll = roll[i];
    redundantState = STATE_ONE_FAIL;
  } else {
    // 三路全失效，启用默认值并进入紧急状态
    votedRoll = 0;
    redundantState = STATE_TWO_FAIL;
    Serial.println("所有IMU失效，进入紧急状态");
    motorLeft.move(0); motorRight.move(0); // 停机
    delay(1000);
    return;
  }

  // 3. BLDC推进器姿态闭环控制
  // 横滚角纠正：左倾则右推进器加速，右倾则左推进器加速
  float motorLSpeed = 0, motorRSpeed = 0;
  float targetRoll = 0; // 目标横滚角（0为水平）
  float error = votedRoll - targetRoll;

  // 简单比例控制（系数需结合实际机器人调整）
  motorLSpeed = -error * 0.5;
  motorRSpeed = error * 0.5;

  // 速度限幅（防止过载）
  motorLSpeed = constrain(motorLSpeed, -0.3, 0.3);
  motorRSpeed = constrain(motorRSpeed, -0.3, 0.3);

  motorLeft.move(motorLSpeed);
  motorRight.move(motorRSpeed);

  // 4. 串口调试输出
  Serial.print("VotedRoll: "); Serial.print(votedRoll);
  Serial.print("\tValid: "); Serial.print(validCount);
  Serial.print("\tState: "); Serial.println(redundantState);

  // 电机闭环控制循环
  motorLeft.loopFOC();
  motorRight.loopFOC();

  delay(10); // 控制周期10ms
}

// 简易编码器模拟（推进器若为开环可省略，此处兼容SimpleFOC接口）
Encoder dummyEnc(0, 0, 1);

5、三路IMU动态权重投票 + 水下避障姿态补偿
适用场景：水下复杂障碍环境（如礁石、管道），机器人需边避障边维持姿态稳定，避障时推进器会频繁加减速，易引发传感器干扰，通过动态权重投票提高对推进干扰的抗性，保障避障动作的连贯性。

核心逻辑
动态权重调节：根据传感器数据稳定性（波动幅度）动态分配权重，稳定性高的传感器权重更高，故障传感器权重降为0；
故障自适应检测：通过陀螺仪漂移阈值、加速度计线性度、数据波动幅度三重判断传感器状态；
避障姿态补偿：避障时根据推进器电流波动预判推进干扰，提前给IMU数据加入补偿系数，避免投票输出因干扰波动；
状态分级仲裁：三路正常时加权投票，单路失效时加权重构，两路失效时启用保守策略，兼顾可靠性与响应性。

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

// 硬件配置
BLDCMotor motorFront(9), motorRear(10); // 前后推进器
BLDCDriver3PWM drvF(3,5,6), drvR(11,12,13);

// 三路IMU配置
const uint8_t imuAddr[3] = {0x68, 0x69, 0x6A};
MPU6050 imu[3];

// 姿态数据结构
struct AttitudeData {
  float roll;     // 横滚角
  float pitch;    // 俯仰角
  float yaw;      // 偏航角
  float gyroDrift; // 陀螺仪漂移速率
  float stability; // 数据稳定性评分（0-1，越高越稳定）
  bool valid;     // 有效性
} attitude[3];

// 冗余参数
float weight[3] = {1, 1, 1}; // 动态权重
float votedAttitude[3] = {0}; // 投票后姿态
const float MAX_DRIFT = 0.5; // 最大允许漂移速率（°/s）
const float VOLATILITY_THRESH = 15; // 波动阈值

// 避障补偿参数
float propInterference = 0; // 推进器干扰系数

void setup() {
  Serial.begin(115200);
  // 初始化BLDC
  motorFront.linkDriver(&drvF); motorRear.linkDriver(&drvR);
  motorFront.linkSensor(&dummyEnc); motorRear.linkSensor(&dummyEnc);
  motorFront.controller = MotionControlType::velocity;
  motorRear.controller = MotionControlType::velocity;
  motorFront.init(); motorFront.initFOC();
  motorRear.init(); motorRear.initFOC();

  // 初始化IMU
  for (int i = 0; i < 3; i++) {
    Wire.begin();
    if (imu[i].testConnection()) {
      imu[i].initialize();
      imu[i].setFullScaleGyroRange(MPU6050_GYRO_FS_250);
      imu[i].setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
      attitude[i].valid = true;
    } else {
      attitude[i].valid = false;
      Serial.print("IMU "); Serial.print(i); Serial.println(" 硬件连接失败");
    }
  }
}

void loop() {
  // 1. 动态权重更新与数据采集
  for (int i = 0; i < 3; i++) {
    if (!attitude[i].valid) {
      weight[i] = 0;
      continue;
    }

    // 读取传感器数据
    int16_t accX = imu[i].getAccelerationX();
    int16_t accY = imu[i].getAccelerationY();
    int16_t accZ = imu[i].getAccelerationZ();
    int16_t gyroX = imu[i].getRotationX();
    int16_t gyroY = imu[i].getRotationY();
    int16_t gyroZ = imu[i].getRotationZ();

    // 互补滤波计算姿态
    float accPitch = atan2(-accY, sqrt(accX*accX + accZ*accZ)) * RAD_TO_DEG;
    float accRoll = atan2(accX, accZ) * RAD_TO_DEG;
    float gyroPitchRate = gyroX * 0.00745;
    float gyroRollRate = gyroY * 0.00745;

    // 更新姿态（简化迭代，实际需基于上一时刻）
    attitude[i].pitch = alpha * (attitude[i].pitch + gyroPitchRate * 0.01) + (1 - alpha) * accPitch;
    attitude[i].roll = alpha * (attitude[i].roll + gyroRollRate * 0.01) + (1 - alpha) * accRoll;
    attitude[i].yaw = 0; // 偏航角依赖磁场，水下暂用陀螺仪积分，此处简化

    // 计算漂移速率（基于陀螺仪零偏变化）
    static float prevGyroY = 0;
    attitude[i].gyroDrift = abs(gyroY - prevGyroY);
    prevGyroY = gyroY;

    // 计算稳定性评分（波动越小，稳定性越高）
    float vol = abs(accRoll - attitude[i].roll) + abs(gyroRollRate);
    attitude[i].stability = constrain(1 - vol / VOLATILITY_THRESH, 0, 1);

    // 故障检测：漂移超限或稳定性<0.3，标记失效
    if (attitude[i].gyroDrift > MAX_DRIFT || attitude[i].stability < 0.3) {
      attitude[i].valid = false;
      weight[i] = 0;
      Serial.print("IMU "); Serial.print(i); Serial.println(" 故障标记");
    } else {
      // 动态权重：稳定性越高，权重越高（归一化处理）
      weight[i] = attitude[i].stability;
    }
  }

  // 2. 加权投票（归一化权重后输出）
  float totalWeight = weight[0] + weight[1] + weight[2];
  if (totalWeight == 0) {
    // 全失效，启用默认保守策略
    Serial.println("所有IMU失效，进入保守模式");
    motorFront.move(0); motorRear.move(0);
    delay(1000);
    return;
  }

  // 加权计算投票姿态
  for (int j = 0; j < 3; j++) {
    votedAttitude[j] = 0;
    for (int i = 0; i < 3; i++) {
      if (attitude[i].valid) {
        if (j == 0) votedAttitude[j] += weight[i] * attitude[i].roll;
        else if (j == 1) votedAttitude[j] += weight[i] * attitude[i].pitch;
        else votedAttitude[j] += weight[i] * attitude[i].yaw;
      }
    }
    votedAttitude[j] /= totalWeight;
  }

  // 3. 避障推进干扰补偿（根据电机电流判断干扰）
  float motorFCurrent = getMotorCurrent(motorFront);
  float motorRCurrent = getMotorCurrent(motorRear);
  propInterference = (abs(motorFCurrent - 0.5) + abs(motorRCurrent - 0.5)) * 0.1;
  // 干扰补偿：干扰越大，姿态修正越平缓，避免波动
  float compFactor = constrain(1 - propInterference, 0.5, 1);
  votedAttitude[0] *= compFactor; // 横滚角补偿

  // 4. 避障姿态控制（以避障左转为例）
  float targetRoll = 0;
  float error = votedAttitude[0] - targetRoll;
  float speedF = 0.3 + error * 0.3;
  float speedR = 0.3 - error * 0.3;

  // 避障时保持航向，同时纠正姿态
  motorFront.move(constrain(speedF, -0.4, 0.4));
  motorRear.move(constrain(speedR, -0.4, 0.4));

  // 5. 串口输出调试
  Serial.print("VotedRoll: "); Serial.print(votedAttitude[0]);
  Serial.print("\tInterference: "); Serial.print(propInterference);
  Serial.print("\tWeights: "); Serial.print(weight[0]); Serial.print(","); Serial.print(weight[1]); Serial.print(","); Serial.println(weight[2]);

  motorFront.loopFOC();
  motorRear.loopFOC();
  delay(10);
}

// 模拟电机电流采集（实际需接电流采样模块，此处简化）
float getMotorCurrent(BLDCMotor &motor) {
  // 实际应通过电流采样芯片获取，此处返回随机模拟值
  return random(0.1, 1.0);
}

// 简易编码器
Encoder dummyEnc(0,0,1);

6、三路IMU投票 + BLDC多自由度协同（俯仰+横滚+偏航）
适用场景：水下六自由度运动的探测机器人，需实时控制横滚、俯仰、偏航三个姿态角，三路IMU分别监测不同轴向姿态，通过多数投票输出全维度可靠姿态，控制多组BLDC推进器实现全姿态调整，适用于水下复杂地形的精确探测。

核心逻辑
全姿态维度投票：针对横滚、俯仰、偏航三个核心姿态角分别进行多数投票，每个轴向独立仲裁，确保多自由度可靠输出；
正交误差校正：对三路IMU安装的正交偏差进行校准，减少安装误差对投票结果的影响；
多推进器协同控制：基于投票后的三维姿态，按“横滚-俯仰-偏航”的优先级分配推进器转速，实现姿态解耦控制；
故障梯度响应：单路失效时仅降低对应轴向精度，两路失效时优先保障偏航稳定（水下偏航是航行核心），防止失控。

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

// 硬件配置：6个BLDC推进器（实际可根据结构简化为4个核心推进器）
BLDCMotor motorRollL(3), motorRollR(4);   // 横滚控制（左右）
BLDCMotor motorPitchU(5), motorPitchD(6); // 俯仰控制（上下）
BLDCMotor motorYawL(7), motorYawR(8);     // 偏航控制（前后差速）

BLDCDriver3PWM drvRL(9,10,11), drvRR(12,13,14);
BLDCDriver3PWM drvPU(15,16,17), drvPD(18,19,20);
BLDCDriver3PWM drvYL(21,22,23), drvYR(24,25,26);

// 三路IMU（分别正交安装，监测不同轴向）
const uint8_t imuAddr[3] = {0x68, 0x69, 0x6A};
MPU6050 imu[3];

// 三路全姿态数据
struct FullAttitude {
  float roll;   // 横滚角
  float pitch;  // 俯仰角
  float yaw;    // 偏航角
  bool valid;   // 有效性
} attitude[3];

// 投票后全姿态
struct VotedAttitude {
  float roll, pitch, yaw;
} votedAtt;

// 校准参数
float imuOffset[3][3] = {
  {0, 0, 0}, // IMU0偏移（横滚校准）
  {0, 0, 0}, // IMU1偏移（俯仰校准）
  {0, 0, 0}  // IMU2偏移（偏航校准）
};

// 互补滤波参数
const float alpha = 0.98;
float compRoll[3] = {0}, compPitch[3] = {0}, compYaw[3] = {0};

void setup() {
  Serial.begin(115200);

  // 初始化6个BLDC推进器
  motorRollL.linkDriver(&drvRL); motorRollR.linkDriver(&drvRR);
  motorPitchU.linkDriver(&drvPU); motorPitchD.linkDriver(&drvPD);
  motorYawL.linkDriver(&drvYL); motorYawR.linkDriver(&drvYR);

  for (auto &m : {&motorRollL, &motorRollR, &motorPitchU, &motorPitchD, &motorYawL, &motorYawR}) {
    m->linkSensor(&dummyEnc);
    m->controller = MotionControlType::velocity;
    m->init();
    m->initFOC();
  }

  // 初始化三路IMU
  for (int i = 0; i < 3; i++) {
    Wire.begin();
    if (imu[i].testConnection()) {
      imu[i].initialize();
      imu[i].setFullScaleGyroRange(MPU6050_GYRO_FS_500); // 500°/s量程，覆盖大姿态变化
      imu[i].setFullScaleAccelRange(MPU6050_ACCEL_FS_4);
      attitude[i].valid = true;
      // 此处可增加离线校准步骤，记录偏移（简化为0，实际需水平放置记录初始值）
    } else {
      attitude[i].valid = false;
    }
  }
}

void loop() {
  // 1. 三路全姿态计算（正交轴向独立计算）
  for (int i = 0; i < 3; i++) {
    if (!attitude[i].valid) continue;

    int16_t accX = imu[i].getAccelerationX();
    int16_t accY = imu[i].getAccelerationY();
    int16_t accZ = imu[i].getAccelerationZ();
    int16_t gyroX = imu[i].getRotationX();
    int16_t gyroY = imu[i].getRotationY();
    int16_t gyroZ = imu[i].getRotationZ();

    // 加速度计计算初始姿态（加偏移校正）
    float accRoll = (atan2(accX, accZ) + imuOffset[i][0]) * RAD_TO_DEG;
    float accPitch = (atan2(-accY, sqrt(accX*accX + accZ*accZ)) + imuOffset[i][1]) * RAD_TO_DEG;
    float accYaw = imuOffset[i][2]; // 加速度计无法解算偏航，用陀螺仪积分

    // 陀螺仪角速度转换
    float gyroRollRate = (gyroY * 0.00745) + imuOffset[i][0] * 0.1;
    float gyroPitchRate = (gyroX * 0.00745) + imuOffset[i][1] * 0.1;
    float gyroYawRate = (gyroZ * 0.00745) + imuOffset[i][2] * 0.1;

    // 互补滤波融合
    compRoll[i] = alpha * (compRoll[i] + gyroRollRate * 0.01) + (1 - alpha) * accRoll;
    compPitch[i] = alpha * (compPitch[i] + gyroPitchRate * 0.01) + (1 - alpha) * accPitch;
    compYaw[i] = alpha * (compYaw[i] + gyroYawRate * 0.01) + (1 - alpha) * compYaw[i]; // 偏航纯陀螺仪积分

    // 存储校正后的姿态
    attitude[i].roll = compRoll[i];
    attitude[i].pitch = compPitch[i];
    attitude[i].yaw = compYaw[i];

    // 故障检测：角度跳变超限标记失效
    if (abs(compRoll[i] - attitude[i].roll) > 90) {
      attitude[i].valid = false;
    }
  }

  // 2. 各轴向多数投票（独立投票，保障多自由度可靠）
  // 横滚角投票
  int rollValid = 0;
  float rollVals[3];
  for (int i = 0; i < 3; i++) {
    if (attitude[i].valid) {
      rollVals[rollValid++] = attitude[i].roll;
    }
  }
  if (rollValid >= 2) {
    sort(rollVals, rollVals + rollValid);
    votedAtt.roll = rollVals[rollValid/2]; // 多数投票，取中间值
  } else if (rollValid == 1) {
    votedAtt.roll = rollVals[0];
  } else {
    votedAtt.roll = 0; // 全失效，默认值
  }

  // 俯仰角投票
  int pitchValid = 0;
  float pitchVals[3];
  for (int i = 0; i < 3; i++) {
    if (attitude[i].valid) {
      pitchVals[pitchValid++] = attitude[i].pitch;
    }
  }
  if (pitchValid >= 2) {
    sort(pitchVals, pitchVals + pitchValid);
    votedAtt.pitch = pitchVals[pitchValid/2];
  } else if (pitchValid == 1) {
    votedAtt.pitch = pitchVals[0];
  } else {
    votedAtt.pitch = 0;
  }

  // 偏航角投票（水下偏航最关键，优先级最高）
  int yawValid = 0;
  float yawVals[3];
  for (int i = 0; i < 3; i++) {
    if (attitude[i].valid) {
      yawVals[yawValid++] = attitude[i].yaw;
    }
  }
  if (yawValid >= 2) {
    sort(yawVals, yawVals + yawValid);
    votedAtt.yaw = yawVals[yawValid/2];
  } else if (yawValid == 1) {
    votedAtt.yaw = yawVals[0];
  } else {
    votedAtt.yaw = 0;
  }

  // 3. 多自由度BLDC推进器协同控制
  // 控制目标：roll=0, pitch=0, yaw=0
  float rollErr = votedAtt.roll;
  float pitchErr = votedAtt.pitch;
  float yawErr = votedAtt.yaw;

  // 横滚控制：左倾则左横滚推进减速，右倾则加速
  float rollLSpeed = -rollErr * 0.3;
  float rollRSpeed = rollErr * 0.3;

  // 俯仰控制：上仰则上俯仰推进减速，下俯则加速
  float pitchUSpeed = -pitchErr * 0.3;
  float pitchDSpeed = pitchErr * 0.3;

  // 偏航控制：左偏航则左偏航推进加速，右偏航则减速（差速转向）
  float yawLSpeed = -yawErr * 0.2;
  float yawRSpeed = yawErr * 0.2;

  // 限幅保护
  float speeds[] = {rollLSpeed, rollRSpeed, pitchUSpeed, pitchDSpeed, yawLSpeed, yawRSpeed};
  for (auto &s : speeds) s = constrain(s, -0.3, 0.3);

  // 执行控制
  motorRollL.move(rollLSpeed); motorRollR.move(rollRSpeed);
  motorPitchU.move(pitchUSpeed); motorPitchD.move(pitchDSpeed);
  motorYawL.move(yawLSpeed); motorYawR.move(yawRSpeed);

  // 4. 串口输出全姿态投票结果
  Serial.print("Voted: R="); Serial.print(votedAtt.roll);
  Serial.print(", P="); Serial.print(votedAtt.pitch);
  Serial.print(", Y="); Serial.println(votedAtt.yaw);

  // 5. 电机闭环控制
  for (auto &m : {&motorRollL, &motorRollR, &motorPitchU, &motorPitchD, &motorYawL, &motorYawR}) {
    m->loopFOC();
  }

  delay(10);
}

// 简易编码器
Encoder dummyEnc(0,0,1);

要点解读

1. 三路IMU的正交安装与硬件隔离：冗余的基础前提
   水下机器人的三路IMU需通过正交安装+硬件隔离消除共因失效，确保三路数据独立可靠，这是冗余投票有效的前提。
   正交安装设计：三路IMU按横滚、俯仰、偏航三个轴向正交安装，避免单一传感器同时监测多个轴向，分散故障风险；同时通过机械隔振（如橡胶垫圈）减少水下振动对传感器的干扰，防止振动导致的数据漂移。
   硬件独立隔离：三路IMU采用独立电源供电+独立I2C地址，防止电源波动、I2C总线故障等单点问题导致多路传感器同时失效；同时IMU与BLDC推进器电源隔离，避免推进器启停产生的电磁干扰耦合到IMU电路，确保传感器数据不受电磁干扰污染。
   安装误差校准：安装时不可避免存在正交偏差，需在代码中预留偏移校准参数，通过离线水平放置、旋转标定记录每路IMU的零偏，减少安装误差对投票结果的影响，避免因安装偏差导致三路数据一致性差、投票失效。
2. 多数投票算法的嵌入式适配：平衡可靠性与实时性
   多数投票是三模冗余的核心，需在Arduino有限算力下实现高效投票，兼顾投票可靠性与水下控制的实时性。
   投票算法轻量化：水下控制要求响应速度极快，无法执行复杂排序，因此采用排序取中间值的简化多数投票，仅对有效数据排序，取中间值作为输出，既避免极端值干扰，又大幅减少计算量，适配Arduino的算力约束，可在10ms控制周期内完成三路数据处理。
   有效性前置检测：投票前必须先判断传感器有效性，通过漂移阈值、波动幅度、数据跳变三重判断标记故障传感器，仅对有效数据进行投票，避免故障传感器的错误数据参与投票，从根本上排除无效数据对结果的干扰，防止因故障传感器数据导致投票结果异常。
   权重与投票结合：针对传感器稳定性差异，引入动态权重机制，稳定性高的传感器赋予更高权重，稳定性差的传感器降低权重，故障传感器权重为0，既保留多数投票的鲁棒性，又充分利用传感器性能差异，提升姿态输出精度，比纯多数投票更适配水下动态环境。
3. 故障自诊断与自适应重构：保障持续可靠输出
   水下环境恶劣，传感器故障突发性强，需通过实时诊断+自适应重构确保单节点失效时系统仍能稳定输出姿态，避免因单故障导致机器人失控。
   多维度故障诊断：构建三重诊断体系，避免单一标准误判：一是陀螺仪漂移检测，监测角速度变化率是否超过阈值，漂移超限说明陀螺仪失效；二是加速度计线性度检测，判断加速度计是否受外力卡滞，数据是否符合重力加速度特征；三是数据波动检测，通过标准差判断数据是否稳定，波动过大说明传感器受干扰或故障，三重诊断确保故障识别的准确性。
   梯度式状态重构：根据故障数量构建三级梯度重构策略：三路正常时采用加权投票，输出全精度姿态；单路失效时剔除故障路，用两路加权重构，精度轻微下降但输出稳定；两路失效时启用保守策略，优先保障偏航角稳定，输出默认姿态，防止失控，确保故障后系统持续可控。
   故障自恢复机制：对标记为故障的传感器，实时监测其数据稳定性，若连续多个控制周期数据恢复正常，自动解除故障标记，重新加入冗余体系，避免短暂干扰导致的传感器误判失效，提升系统容错能力和自恢复能力。
4. 水下环境适配的姿态处理：应对特殊环境干扰
   水下环境与陆地差异极大，必须针对水下干扰特性优化姿态处理算法，消除环境对数据的影响。
   振动与电磁干扰抑制：水下推进器振动、水流冲击会导致IMU数据高频噪声，算法中需加入低通滤波+互补滤波融合，低通滤波滤除高频振动噪声，互补滤波融合加速度计的静态稳定性与陀螺仪的动态响应性，既抗振动又保证姿态响应速度，避免单纯滤波导致的姿态滞后。
   推进器干扰补偿：BLDC推进器启停、调速时会产生振动和电磁辐射，直接影响IMU数据，因此需引入推进器干扰系数，根据推进器电流或转速变化判断干扰强度，干扰强度越大，姿态修正越平缓，避免推进干扰导致投票结果剧烈波动，保障控制连贯性。
   水压与姿态滞后补偿：水下高压可能导致IMU结构轻微形变，产生姿态滞后，可通过前馈补偿算法，结合历史姿态数据预判滞后趋势，提前对姿态输出进行微调，同时在代码中预留水压补偿参数接口，适配不同深度的压力变化，减少水压对姿态的影响。
5. 投票输出与BLDC控制的闭环协同：实现可靠执行
   投票输出的可靠姿态最终需转化为BLDC推进器的精准动作，核心是建立投票-控制-反馈的闭环，确保姿态指令准确执行，同时防止控制与感知脱节。
   控制指令与投票结果的严格匹配：BLDC推进器的控制逻辑必须严格基于投票后的姿态输出，禁止跳过投票环节直接用单一IMU数据控制，避免故障传感器数据导致控制指令错误，引发推进器误动作，例如横滚角投票输出后，再分配推进器转速，确保控制的源头可靠。
   控制周期与投票周期的同步：姿态投票周期与BLDC控制周期必须保持一致，确保控制指令基于最新投票结果，避免因周期不同步导致姿态滞后、控制振荡；同时在控制循环末尾加入投票结果有效性校验，若投票结果无效，立即切断推进器输出，防止失控，保障硬件安全。
   闭环反馈的动态调整：投票后的姿态作为控制目标，同时将BLDC推进器的执行效果反馈给投票系统，例如推进器动作后姿态未按预期纠正，可临时调整投票算法的权重系数或故障判断阈值，形成“投票-控制-反馈-调整”的闭环，逐步优化冗余体系与控制参数的适配性，提升水下控制稳定性。

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