基于Arduino与BLDC（无刷直流电机）构建的特种救援机器人，其“多传感器融合避障+硬件中断急停”系统是一套将高鲁棒性环境感知与绝对安全底线控制深度融合的架构。该方案通过异构传感器网络解决灾难现场的感知盲区，并利用硬件中断机制确保在任何极端工况下都能强制接管BLDC执行器，实现物理层面的生存保障。

主要特点

1. “远-中-近”三层异构感知与动态融合
   在浓烟、粉尘或光线剧变的灾难现场，单一传感器极易失效。系统构建了多层防御体系：远距使用激光雷达（LiDAR）或毫米波雷达穿透薄雾探测轮廓；中距依赖深度相机或超声波阵列补充检测透明物体；近距则通过红外或力矩传感器作为最后一道防线。在Arduino（或协同的协处理器）上，通过卡尔曼滤波或贝叶斯估计实时计算各传感器的置信度，动态调整权重，有效抑制噪声和误检，构建可靠的局部占据栅格地图。
2. 绝对优先的“安全本能”硬件中断架构
   这是系统的核心安全底线。硬件中断被设计为最高优先级的“底层看门狗”，独立于常规的融合推理逻辑。它直接监控传感器的极近距离数据、IMU的碰撞加速度以及电机的过流状态。一旦触发安全阈值（如距离小于10cm或即将碰撞），系统会绕过所有常规软件规则，瞬间接管控制权，强制BLDC电机执行紧急制动（E-Stop）或极限后退，确保在算法失效或传感器受干扰时，机器人依然具备物理层面的生存能力。
3. BLDC高动态响应与去模糊化执行
   融合控制器输出的控制量需要经过“去模糊化”或决策仲裁处理，才能转化为BLDC电机的实际转速指令。结合BLDC无刷电机的高效率和毫秒级响应特性，系统能够完美跟踪这些平滑的速度曲线。在频繁加减速的避障过程中，BLDC配合FOC（磁场定向控制）驱动器能保持低噪音、无顿挫，并避免机械结构的过度磨损，确保在复杂废墟环境中的机动性。
4. 失效安全（Fail-Safe）与双重保护机制
   系统遵循“故障导向安全”原则。在硬件层面，急停信号可直接接入驱动芯片的使能（EN）引脚，实现物理断电；在软件层面，通过看门狗（Watchdog）机制，一旦主程序死机或通信中断，系统默认进入停止状态。这种软硬件双重保护确保了机器人在任何异常情况下都能可靠停机。

应用场景

1. 地震与坍塌废墟搜索
   在建筑物倒塌形成的狭小空间内，机器人需自主穿行于断壁残垣之间。多传感器融合能帮助机器人避开悬空楼板和碎石，而硬件中断急停则能在机器人意外触碰不稳定结构时瞬间刹停，防止引发二次坍塌。
2. 工业厂区与危险环境巡检
   在化工厂或核电站等高危区域，机器人需长时间自主运行。当遇到突发的人员闯入或设备泄漏时，系统能依靠底层本能瞬间刹停，彻底杜绝碰撞伤人或设备损坏事故，保障人员与资产安全。
3. 人机协作救援与物资运输
   在救援人员与机器人协同作业的场景中，机器人必须具备极高的安全性。带“安全本能”的控制器能确保机器人在靠近救援人员时，即使视觉算法出现短暂延迟，也能依靠底层硬件中断瞬间响应，实现真正的人机共融。
4. 复杂地形越障与侦察
   在野外或灾后非结构化环境中，机器人需克服瓦砾堆、台阶、斜坡等复杂地形。BLDC电机的高扭矩密度配合多传感器融合，能确保机器人在低速爬坡或越障时保持航向稳定，并在遇到未知危险时迅速撤离。

注意事项

1. 传感器选型与环境抗扰
   抗强光干扰： 红外传感器易受日光或白炽灯干扰，建议选用带38kHz调制的型号，或在软件上做多次采样滤波。
   盲区补盲： 激光雷达存在近距离盲区，必须搭配超声波或红外传感器进行近距离补盲，防止剐蹭。
   数据一致性： 多传感器融合时，需确保各传感器的时间戳同步，避免因时钟漂移导致的数据融合错误。
2. 硬件中断的优先级与原子性
   中断服务程序（ISR）： 避障检测的中断服务程序必须极其精简，避免在其中执行耗时操作（如串口打印、复杂计算），以免阻塞主程序。
   变量声明： 关键状态变量（如 obstacleDetected）应声明为 volatile，防止编译器优化导致读取错误。
3. BLDC驱动的制动能力与电源管理
   主动刹车： 普通ESC或简易MOSFET驱动不具备主动刹车功能，断电后电机靠摩擦力滑行停止。若需快速制动，应选择支持“刹车模式”的ESC，或在H桥驱动中实现短接绕组（动态制动）。
   电源隔离： 执行器（高功率域）与控制器（低功率域）需分域供电，并加入大容量缓冲电容，防止电机急停时的电压骤降导致主控板掉电重启。
4. 通信可靠性与超时处理
   指令校验： 串口或无线通信需设计校验机制（如和校验、异或校验），过滤无效或错误的指令。
   超时机制： 若长时间未接收到上位机指令，下位机应自动进入安全状态（如电机停止），防止因通信中断导致机器人失控。
5. 机械结构与安装布局
   传感器布局： 需根据BLDC驱动的最小制动距离合理布置传感器位置，确保检测范围覆盖制动距离。
   急停按钮： 物理急停按钮应设置在易于触及的位置，且采用常闭（NC）型触点串联在电源或使能回路中，确保任何线路断开都能触发停机。

1、多传感器融合安全本能系统（超声波+红外+IMU）
此案例实现“远-中-近”三层防御体系。远距超声波提供主要避障信息，近距红外作为最后防线，IMU监测倾覆风险。所有传感器数据在安全本能层汇聚，一旦任一传感器触发临界阈值，系统立即绕过常规控制逻辑，强制BLDC执行紧急制动。

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

// ===== BLDC电机（差速履带）=====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(8);
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM drvR = BLDCDriver3PWM(5, 6, 8);

// ===== 传感器阵列 =====
// 超声波（远-中距主避障）
#define SONAR_NUM 3
NewPing sonar[SONAR_NUM] = {
    NewPing(2, 3, 200),   // 前
    NewPing(4, 5, 200),   // 左
    NewPing(6, 7, 200)    // 右
};
// 红外（近距补充，防超声波盲区）
#define IR_FRONT 8
// IMU（倾覆检测）
MPU6050 imu;

// ===== 安全边界参数 =====
struct SafetyZone {
    float warning;    // 预警距离(cm)
    float brake;      // 制动距离(cm)
    float critical;   // 临界距离(cm)
};
SafetyZone frontZone = {50, 30, 15};
SafetyZone sideZone = {30, 20, 10};

// ===== 系统安全等级 =====
enum SafetyLevel { LEVEL_SAFE, LEVEL_WARNING, LEVEL_BRAKE, LEVEL_CRITICAL };
SafetyLevel currentLevel = LEVEL_SAFE;
float brakeStrength = 0;  // 制动强度(0~1)

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

    // 电机初始化（速度闭环）
    drvL.init(); drvR.init();
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;

    // 传感器初始化
    pinMode(IR_FRONT, INPUT);
    Wire.begin();
    imu.initialize();
}

void loop() {
    // ===== 1. 多模态感知采集 =====
    // 超声波时分复用测距
    float dist[SONAR_NUM];
    for (int i = 0; i < SONAR_NUM; i++) {
        dist[i] = sonar[i].ping_cm();
        if (dist[i] <= 0 || dist[i] > 150) dist[i] = 150;
        delay(25);  // 避免串扰
    }
    float frontDist = dist[0];

    // 红外近距检测（数字输出）
    bool irObstacle = (digitalRead(IR_FRONT) == LOW);

    // IMU倾覆检测
    int16_t ax, ay, az;
    imu.getAcceleration(&ax, &ay, &az);
    float roll = atan2(ay, az) * 180 / PI;
    float pitch = atan2(-ax, sqrt(ay*ay + az*az)) * 180 / PI;
    bool tiltRisk = (abs(roll) > 25 || abs(pitch) > 25);

    // ===== 2. 分级安全判定（核心）=====
    // 临界级：任何条件触发立即急停
    if (frontDist < frontZone.critical || irObstacle || tiltRisk) {
        currentLevel = LEVEL_CRITICAL;
    } else if (frontDist < frontZone.brake) {
        currentLevel = LEVEL_BRAKE;
    } else if (frontDist < frontZone.warning) {
        currentLevel = LEVEL_WARNING;
    } else {
        currentLevel = LEVEL_SAFE;
    }

    // ===== 3. 分级响应执行 =====
    float baseSpeed = 0.8;  // 正常巡航速度
    switch (currentLevel) {
        case LEVEL_CRITICAL:
            // 紧急制动：立即停车
            motorL.target = 0; motorR.target = 0;
            Serial.println("CRITICAL: Emergency stop!");
            break;
        case LEVEL_BRAKE:
            // 制动级：减速至30%并准备转向
            motorL.target = baseSpeed * 0.3;
            motorR.target = baseSpeed * 0.3;
            break;
        case LEVEL_WARNING:
            // 预警级：减速至60%
            motorL.target = baseSpeed * 0.6;
            motorR.target = baseSpeed * 0.6;
            break;
        default:
            // 安全级：正常行驶
            motorL.target = baseSpeed;
            motorR.target = baseSpeed;
    }

    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();

    delay(20);
}

核心逻辑：安全本能层独立于常规导航逻辑，作为最高优先级监控器运行。多传感器冗余设计确保单一传感器失效时系统仍能维持安全底线——超声波受软材质干扰时，红外可检测近距碰撞；红外受强光干扰时，超声波仍可工作。

2、硬件中断急停 + 分级制动状态机
此案例实现不可屏蔽的硬件急停。急停按钮通过外部中断（FALLING边缘触发）直接触发ISR，设置标志位后由主循环执行“动态制动→切断使能”的两级制动。即使主程序因复杂运算卡死，中断仍能立即响应。

#include <SimpleFOC.h>

// ===== BLDC电机（带使能引脚）=====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(8);
// 驱动器带使能引脚（第4个参数）
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11, 4);
BLDCDriver3PWM drvR = BLDCDriver3PWM(5, 6, 7, 3);

// ===== 急停按钮（硬件中断）=====
#define EMERGENCY_PIN 2
volatile bool emergencyStop = false;

// ===== 急停状态机 =====
enum EStopState {
    ESTOP_IDLE,       // 正常状态
    ESTOP_BRAKING,    // 分级1：动态制动（能耗制动）
    ESTOP_CUTOFF      // 分级2：切断电机使能
};
EStopState estopState = ESTOP_IDLE;
unsigned long brakeStartTime = 0;

// 制动参数
const float BRAKE_CURRENT_LIMIT = 0.5;   // 制动电流限制(A)
const unsigned long BRAKE_DURATION = 500; // 制动持续时间(ms)

// ===== 中断服务程序：极短，只标记 =====
void emergencyISR() {
    emergencyStop = true;
}

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

    // 电机初始化
    drvL.init(); drvR.init();
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;

    // 急停按钮：上拉输入，下降沿触发
    pinMode(EMERGENCY_PIN, INPUT_PULLUP);
    attachInterrupt(digitalPinToInterrupt(EMERGENCY_PIN), emergencyISR, FALLING);

    Serial.println("System ready. Press E-STOP for emergency brake.");
}

void loop() {
    // ===== 急停状态机（最高优先级）=====
    switch (estopState) {
        case ESTOP_IDLE:
            if (emergencyStop) {
                estopState = ESTOP_BRAKING;
                brakeStartTime = millis();
                Serial.println("E-STOP: Braking phase");

                // 分级1：动态制动——施加反向制动电流
                motorL.target = -BRAKE_CURRENT_LIMIT;
                motorR.target = -BRAKE_CURRENT_LIMIT;
            }
            break;

        case ESTOP_BRAKING:
            // 维持制动电流
            motorL.move(motorL.target);
            motorR.move(motorR.target);
            motorL.loopFOC();
            motorR.loopFOC();

            // 制动持续时间到达后进入断电阶段
            if (millis() - brakeStartTime > BRAKE_DURATION) {
                estopState = ESTOP_CUTOFF;
                Serial.println("E-STOP: Cutoff phase");
            }
            break;

        case ESTOP_CUTOFF:
            // 分级2：切断电机使能（硬件层断电）
            motorL.disable();
            motorR.disable();
            Serial.println("E-STOP: Motors disabled");

            // 等待按钮释放后复位
            if (digitalRead(EMERGENCY_PIN) == HIGH) {
                emergencyStop = false;
                estopState = ESTOP_IDLE;
                motorL.enable();
                motorR.enable();
                Serial.println("System reset.");
            }
            break;
    }

    // ===== 正常运动控制（仅在IDLE状态）=====
    if (estopState == ESTOP_IDLE && !emergencyStop) {
        motorL.target = 1.0;
        motorR.target = 1.0;
        motorL.move(motorL.target);
        motorR.move(motorR.target);
    }

    // 非制动状态下继续运行FOC
    if (estopState != ESTOP_BRAKING) {
        motorL.loopFOC();
        motorR.loopFOC();
    }

    delay(10);
}

核心逻辑：硬件中断的响应速度是微秒级，远快于主循环轮询。ISR仅设置volatile bool标志位，不执行任何耗时操作（delay()、Serial.print在ISR中会失效或丢失数据）。分级制动先施加反向电流快速降速（动态制动），再切断使能，避免直接断电导致电机失控滑行。

3、三冗余传感器投票 + 电流环碰撞检测
此案例引入传感器冗余投票和电流环碰撞检测。三个烟雾/温度传感器通过中值投票排除单点故障，BLDC电机的q轴电流变化用于检测碰撞（电流突增表明遇到异常阻力）。两者共同构成特种救援场景下的高可靠性安全系统。

#include <SimpleFOC.h>

// ===== BLDC电机 =====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(8);
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM drvR = BLDCDriver3PWM(5, 6, 8);

// ===== 三冗余温度传感器（模拟输入）=====
#define TEMP_SENSOR_A A0
#define TEMP_SENSOR_B A1
#define TEMP_SENSOR_C A2
float tempFaultCount[3] = {0, 0, 0};

// ===== 碰撞检测（电流环）=====
const float CURRENT_THRESHOLD = 3.0;  // 过流阈值(A)
const float COLLISION_COOLDOWN = 2000; // 碰撞冷却时间(ms)
unsigned long collisionTime = 0;
bool collisionDetected = false;

// ===== 中值投票函数 =====
float medianOfThree(float a, float b, float c) {
    if ((a <= b && b <= c) || (c <= b && b <= a)) return b;
    if ((b <= a && a <= c) || (c <= a && a <= b)) return a;
    return c;
}

// ===== 三冗余温度读取 =====
float getRedundantTemperature() {
    float temps[3];
    temps[0] = analogRead(TEMP_SENSOR_A) * 5.0 / 1023.0 * 100;  // LM35转换
    temps[1] = analogRead(TEMP_SENSOR_B) * 5.0 / 1023.0 * 100;
    temps[2] = analogRead(TEMP_SENSOR_C) * 5.0 / 1023.0 * 100;

    // 一致性检查：与平均值差异过大的传感器标记为故障
    float avg = (temps[0] + temps[1] + temps[2]) / 3;
    for (int i = 0; i < 3; i++) {
        if (abs(temps[i] - avg) > 20) {  // 差异超过20°C
            tempFaultCount[i]++;
            if (tempFaultCount[i] > 2) {
                Serial.print("Temp sensor ");
                Serial.print(i);
                Serial.println(" FAULT!");
            }
        }
    }

    // 中值投票：排除异常值
    return medianOfThree(temps[0], temps[1], temps[2]);
}

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

    // 电机初始化
    drvL.init(); drvR.init();
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

void loop() {
    // ===== 1. 三冗余温度感知 =====
    float safeTemp = getRedundantTemperature();

    // ===== 2. 电流环碰撞检测 =====
    // 读取q轴电流（SimpleFOC内部维护）
    float currentL = abs(motorL.current.q);
    float currentR = abs(motorR.current.q);
    float maxCurrent = max(currentL, currentR);

    if (maxCurrent > CURRENT_THRESHOLD && !collisionDetected) {
        collisionDetected = true;
        collisionTime = millis();
        Serial.println("COLLISION DETECTED! (Current spike)");
    }

    // 碰撞恢复逻辑
    if (collisionDetected) {
        if (millis() - collisionTime > COLLISION_COOLDOWN) {
            // 冷却后检查电流是否恢复
            if (maxCurrent < CURRENT_THRESHOLD * 0.5) {
                collisionDetected = false;
                Serial.println("Collision cleared, resuming.");
            }
        }
    }

    // ===== 3. 热管理决策 =====
    float motorPowerLimit = 1.0;
    if (safeTemp > 80.0) {
        // 过热：限制功率输出
        motorPowerLimit = 0.7;
        Serial.print("THERMAL WARNING: Temp=");
        Serial.print(safeTemp);
        Serial.println("C, power limited to 70%");
    }
    if (safeTemp > 95.0) {
        // 极端过热：强制停机
        motorL.target = 0; motorR.target = 0;
        motorL.move(0); motorR.move(0);
        motorL.loopFOC(); motorR.loopFOC();
        Serial.println("CRITICAL TEMP! Motors stopped.");
        delay(1000);
        return;
    }

    // ===== 4. 正常运动（受碰撞和温度约束）=====
    if (!collisionDetected) {
        float baseSpeed = 0.8 * motorPowerLimit;
        motorL.target = baseSpeed;
        motorR.target = baseSpeed;
    } else {
        // 碰撞后：后退脱离
        motorL.target = -0.3;
        motorR.target = -0.3;
    }

    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();

    delay(20);
}

核心逻辑：三冗余传感器通过中值投票排除单点故障，即使一个传感器失效，系统仍能获得正确的温度读数。电流环碰撞检测利用BLDC的FOC特性——当机器人遇到异常阻力时，q轴电流会突增，这一信号比任何外部传感器都更直接地反映“碰撞”事件。过流保护带延时去抖（Debounce）机制，避免误触发。

要点解读

1. 硬件急停必须独立于软件逻辑，拥有最高优先级

软件急停可能因MCU死机、程序卡死或中断被禁用而失效。工程标准要求急停按钮通过硬件中断触发，ISR仅设置标志位，不执行耗时操作。真正的制动逻辑在主循环中执行。在特种救援场景中，这一设计是“保命符”——即使AI算法崩溃、通信中断，硬件急停仍能物理切断动力。

2. 多传感器冗余是极端环境可靠性的基础

救援现场存在浓烟、高温、粉尘等干扰因素，单一传感器极易失效。多传感器融合通过“异质冗余”实现：超声波受软材质干扰时红外可补充，红外受强光干扰时超声波仍可工作，IMU不受视觉环境限制。只有所有传感器均确认安全才允许运动，任一传感器触发临界阈值即进入安全状态。

3. 分级制动平衡安全性与机械寿命

直接断电（自由停车）可能导致高速电机失控滑行，而立即施加最大制动力矩又可能损伤机械结构。工程推荐三级制动：预警减速（低强度）→ 动态制动（中强度，能耗制动）→ 硬件断电（最终）。动态制动通过反向电流将动能转化为热能，比机械抱闸更平顺，适用于重载救援机器人。

4. BLDC的FOC电流环是碰撞检测的天然传感器

BLDC配合FOC算法时，q轴电流与电机输出转矩成正比。当机器人遇到异常阻力（如碰撞、堵转）时，q轴电流会突增。这一信号比任何外部传感器都更直接地反映“碰撞”事件，响应速度在毫秒级。通过监测q轴电流并设置阈值，可实现无额外传感器的碰撞检测，节省成本和空间。

5. 控制权交接必须平滑，避免安全恢复时的二次冲击

当系统从“紧急制动”状态恢复到“正常控制”状态时，如果直接交还控制权，速度指令的突变可能导致电机猛烈加速，造成机械冲击甚至失控。必须设计速度斜坡（Ramp）机制或平滑交接算法，确保安全警报解除后，电机功率逐步恢复至正常水平。

4、崎岖废墟环境自适应避障+急停（超声波+红外+姿态融合）
场景定位：适用于地震废墟等不规则地形，机器人需躲避静态障碍物（断墙、瓦砾）和动态坍塌风险，同时实时监测自身姿态，避免侧翻，支持手动急停+自动急停双保险。

核心逻辑：以多传感器数据为核心，超声波检测远距离障碍，红外检测近距离盲区，IMU监测机身倾斜角；融合算法加权计算避障方向，同时通过硬件外部中断实现毫秒级急停响应，保障极端工况安全。

#include <SimpleFOC.h>
#include <MPU6050_WE.h>

// ==================== 硬件配置 ====================
// BLDC驱动与编码器
BLDCMotor motorL(3), motorR(3);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(5,6,7,8);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// 传感器引脚
#define ULTRASONIC_TRIG 2  // 超声波触发
#define ULTRASONIC_ECHO 3  // 超声波接收
#define IR_LEFT 4          // 左侧红外
#define IR_RIGHT 5         // 右侧红外
#define EMERGENCY_BUTTON 7 // 急停按钮（外部中断）

// 传感器与IMU
MPU6050_WE imu(Wire);
float pitch, roll;         // 俯仰角/横滚角
float obstacleDistance;    // 障碍距离

// ==================== 急停硬件中断 ====================
volatile bool emergencyStop = false;
void emergencyHandler() {
  emergencyStop = true;
  digitalWrite(13, HIGH);  // 急停LED点亮
}

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

  // 初始化BLDC与FOC
  motorL.linkDriver(&driverL); motorL.linkSensor(&encoderL);
  motorR.linkDriver(&driverR); motorR.linkSensor(&encoderR);
  motorL.init(); motorL.initFOC(); motorL.controller = MotionControlType::velocity;
  motorR.init(); motorR.initFOC(); motorR.controller = MotionControlType::velocity;

  // 初始化传感器
  pinMode(ULTRASONIC_TRIG, OUTPUT); pinMode(ULTRASONIC_ECHO, INPUT);
  pinMode(IR_LEFT, INPUT); pinMode(IR_RIGHT, INPUT);
  pinMode(13, OUTPUT); digitalWrite(13, LOW);
  imu.init(); imu.calcAcceleration(0);

  // 硬件中断初始化（下降沿触发）
  attachInterrupt(digitalPinToInterrupt(EMERGENCY_BUTTON), emergencyHandler, FALLING);
}

void loop() {
  // 1. 急停状态判断
  if (emergencyStop) {
    motorL.move(0); motorR.move(0);
    digitalWrite(13, HIGH);
    while (1);  // 锁定停机
  }

  // 2. 多传感器数据采集
  obstacleDistance = getUltrasonicDistance();  // 超声波测距（需补全代码）
  bool irLeft = digitalRead(IR_LEFT) == LOW;    // 左侧障碍检测
  bool irRight = digitalRead(IR_RIGHT) == LOW;  // 右侧障碍检测
  imu.update();
  imu.getEulerAngles(&pitch, &roll);

  // 3. 融合避障与姿态安全
  bool pitchRisk = pitch > 35 || pitch < -35;  // 俯仰角超限（侧翻风险）
  bool rollRisk = roll > 30 || roll < -30;      // 横滚角超限（侧翻风险）

  // 融合决策
  float vLin = 0.6, vAng = 0;
  if (obstacleDistance < 0.3 || irLeft || irRight || pitchRisk || rollRisk) {
    // 急停触发条件：近距+姿态异常
    if (obstacleDistance < 0.15 || pitchRisk || rollRisk) {
      motorL.move(0); motorR.move(0);  // 自动急停
      delay(100);
    } else {
      // 动态避障
      if (irLeft) vAng = 0.3;
      else if (irRight) vAng = -0.3;
      else vAng = 0.1;  // 无明确方向时小幅右偏
    }
  } else {
    vLin = 0.8;  // 无风险时全速前进
  }

  // 4. BLDC差速控制
  float wheelBase = 0.25;
  motorL.move(vLin - vAng * wheelBase / 2);
  motorR.move(vLin + vAng * wheelBase / 2);

  delay(50);
}

// 超声波测距辅助函数
float getUltrasonicDistance() {
  digitalWrite(ULTRASONIC_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  return pulseIn(ULTRASONIC_ECHO, HIGH) * 0.000034 / 2;  // 单位：米
}

5、低能见度烟雾环境激光雷达+深度相机融合+急停
场景定位：适用于火灾烟雾、粉尘等低能见度救援场景，需精准感知环境三维轮廓，躲避障碍物，同时实时监测有毒气体浓度，浓度超标或路径完全阻塞时触发急停。

核心逻辑：以激光雷达（二维轮廓）和深度相机（三维点云）为核心感知，激光雷达负责远距离障碍扫描，深度相机补充近距离盲区；气体传感器实时监测环境安全，所有数据经卡尔曼滤波融合，生成避障路径；急停机制包含手动按钮+气体超标自动触发+避障死锁自动触发。

#include <SimpleFOC.h>
#include <RPLidar.h>
#include <Wire.h>
#include <Adafruit_NeoPixel.h>

// ==================== 硬件配置 ====================
BLDCMotor motorL(3), motorR(3);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(5,6,7,8);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// 传感器接口
RPLidar lidar;          // 激光雷达（Serial1）
#define CAMERA_INT 2     // 深度相机中断
#define GAS_ANALOG A0    // 气体传感器
#define EMERGENCY_BUTTON 7

// 融合状态
float obstacleAzimuth, obstacleDistance;  // 激光雷达障碍信息
float cameraDepth;                         // 深度相机测距
float gasConcentration;                    // 气体浓度
volatile bool emergencyStop = false;

// ==================== 急停中断处理 ====================
void emergencyHandler() {
  emergencyStop = true;
  // 预留声光报警触发逻辑
}

// ==================== 激光雷达数据处理 ====================
void updateLidar() {
  if (lidar.waitPoint()) {
    float distance = lidar.getCurrentPoint().distance;
    float azimuth = lidar.getCurrentPoint().angle;
    if (distance < 2.0) {  // 有效障碍范围
      obstacleDistance = distance;
      obstacleAzimuth = azimuth;
    }
  }
}

// ==================== 深度相机模拟 ====================
float getCameraDepth() {
  // 实际需连接深度相机（如Intel RealSense），此处模拟数据
  if (digitalRead(CAMERA_INT) == HIGH) {
    cameraDepth = analogRead(A1) * 0.003;  // 模拟深度值（单位：米）
  } else {
    cameraDepth = 999.0;  // 无数据标记
  }
  return cameraDepth;
}

// ==================== 气体浓度采集 ====================
float getGasConcentration() {
  return analogRead(GAS_ANALOG) * 0.5;  // 模拟浓度值（单位：ppm）
}

void setup() {
  Serial.begin(115200);
  Serial1.begin(115200);  // 激光雷达通信
  lidar.init();

  // BLDC初始化
  motorL.linkDriver(&driverL); motorL.linkSensor(&encoderL);
  motorR.linkDriver(&driverR); motorR.linkSensor(&encoderR);
  motorL.init(); motorL.initFOC(); motorL.controller = MotionControlType::velocity;
  motorR.init(); motorR.initFOC(); motorR.controller = MotionControlType::velocity;

  // 传感器与中断初始化
  pinMode(CAMERA_INT, INPUT);
  pinMode(EMERGENCY_BUTTON, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(EMERGENCY_BUTTON), emergencyHandler, FALLING);
}

void loop() {
  if (emergencyStop) {
    motorL.move(0); motorR.move(0);
    while (1);
  }

  // 1. 多传感器数据融合
  updateLidar();
  cameraDepth = getCameraDepth();
  gasConcentration = getGasConcentration();

  // 2. 融合避障逻辑
  float vLin = 0.5, vAng = 0;
  bool obsBlocked = (obstacleDistance < 0.4 && obstacleDistance > 0.1) || (cameraDepth < 0.3 && cameraDepth > 0.05);
  bool gasDanger = gasConcentration > 50;  // 气体超标阈值

  if (gasDanger) {
    // 气体超标自动急停
    motorL.move(0); motorR.move(0);
    delay(500);
    return;
  }

  if (obsBlocked) {
    // 避障死锁自动急停
    if (obstacleDistance < 0.15 && cameraDepth < 0.15) {
      motorL.move(0); motorR.move(0);
      delay(100);
    } else {
      // 动态转向避障
      if (obstacleAzimuth > 0) vAng = -0.4;  // 障碍在右侧，左转向
      else vAng = 0.4;                        // 障碍在左侧，右转向
    }
  } else {
    vLin = 0.7;  // 无风险时正常前进
  }

  // 3. BLDC差速控制
  float wheelBase = 0.3;
  motorL.move(vLin - vAng * wheelBase / 2);
  motorR.move(vLin + vAng * wheelBase / 2);

  delay(50);
}

6、有毒环境气体+温度+压力多维度监测+急停（密闭空间救援）
场景定位：适用于密闭有毒空间（化工厂、地下管道）救援，机器人需同时监测有毒气体浓度、温度、压力等环境参数，参数超标时立即急停，同时通过多传感器融合实现环境安全区域内的自主避障。

核心逻辑：以多维度环境传感器为核心，气体传感器监测有毒成分，温度传感器监测热源风险，压力传感器监测密闭空间坍塌风险；多参数融合判断环境安全，安全状态下基于超声波避障行驶，危险时立即触发自动急停+手动急停；同时实时回传环境数据，辅助救援决策。

#include <SimpleFOC.h>
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_BME280.h>  // 温湿度气压传感器

// ==================== 硬件配置 ====================
BLDCMotor motorL(3), motorR(3);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(5,6,7,8);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// 环境传感器
Adafruit_BME280 bme;          // 温湿度气压
#define GAS_MQ2 A1            // 可燃气体传感器
#define TEMP_TMP36 A2          // 温度传感器
#define PRESSURE_ANALOG A3     // 压力传感器
#define ULTRASONIC_TRIG 2
#define ULTRASONIC_ECHO 3
#define EMERGENCY_BUTTON 7

// 环境阈值
const float GAS_THRESHOLD = 200;  // 可燃气体阈值(ppm)
const float TEMP_THRESHOLD = 60;  // 温度阈值(℃)
const float PRESSURE_THRESHOLD = 1.5;  // 压力阈值(倍大气压)

// 急停与状态
volatile bool emergencyStop = false;
float gasConcentration, temperature, pressure;

// ==================== 急停中断 ====================
void emergencyHandler() {
  emergencyStop = true;
}

// ==================== 环境数据采集 ====================
void updateEnvironmentData() {
  // BME280数据
  temperature = bme.readTemperature();
  pressure = bme.readPressure() / 100000.0;  // 转换为大气压

  // 气体浓度
  gasConcentration = analogRead(GAS_MQ2);

  // 补充温度（TMP36）
  float tmp36 = analogRead(TEMP_TMP36) * 3.3 / 1024;
  temperature = (temperature + tmp36 * 100 - 273.15) / 2;  // 融合平均
}

// ==================== 超声波避障 ====================
float getUltrasonicDistance() {
  digitalWrite(ULTRASONIC_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(ULTRASONIC_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(ULTRASONIC_TRIG, LOW);
  return pulseIn(ULTRASONIC_ECHO, HIGH) * 0.000034 / 2;
}

// ==================== 安全状态判断 ====================
bool isEnvironmentSafe() {
  return gasConcentration < GAS_THRESHOLD &&
         temperature < TEMP_THRESHOLD &&
         pressure < PRESSURE_THRESHOLD;
}

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

  // 初始化BME280
  if (!bme.begin()) {
    Serial.println("BME280初始化失败！");
    while (1);
  }

  // BLDC初始化
  motorL.linkDriver(&driverL); motorL.linkSensor(&encoderL);
  motorR.linkDriver(&driverR); motorR.linkSensor(&encoderR);
  motorL.init(); motorL.initFOC(); motorL.controller = MotionControlType::velocity;
  motorR.init(); motorR.initFOC(); motorR.controller = MotionControlType::velocity;

  // 引脚与中断初始化
  pinMode(ULTRASONIC_TRIG, OUTPUT); pinMode(ULTRASONIC_ECHO, INPUT);
  pinMode(EMERGENCY_BUTTON, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(EMERGENCY_BUTTON), emergencyHandler, FALLING);
}

void loop() {
  if (emergencyStop) {
    motorL.move(0); motorR.move(0);
    while (1);
  }

  // 1. 环境数据与避障数据采集
  updateEnvironmentData();
  float obsDist = getUltrasonicDistance();

  // 2. 安全判断与急停触发
  if (!isEnvironmentSafe()) {
    // 环境危险自动急停
    motorL.move(0); motorR.move(0);
    Serial.println("环境危险：急停触发！");
    Serial.print("气体："); Serial.println(gasConcentration);
    Serial.print("温度："); Serial.println(temperature);
    Serial.print("压力："); Serial.println(pressure);
    delay(1000);
    return;
  }

  // 3. 安全状态下的避障
  float vLin = 0.6, vAng = 0;
  if (obsDist < 0.3) {
    if (obsDist < 0.15) {
      // 近距死锁，急停后转向
      motorL.move(0); motorR.move(0);
      delay(200);
      vAng = random(-0.5, 0.5);  // 随机转向摆脱死锁
    } else {
      // 动态避障
      vAng = 0.35;  // 默认右避障
    }
  } else {
    vLin = 0.7;  // 无障碍时前进
  }

  // 4. BLDC差速控制
  float wheelBase = 0.28;
  motorL.move(vLin - vAng * wheelBase / 2);
  motorR.move(vLin + vAng * wheelBase / 2);

  // 5. 环境数据回传
  Serial.print("气体："); Serial.print(gasConcentration);
  Serial.print(" 温度："); Serial.print(temperature);
  Serial.print(" 压力："); Serial.print(pressure);
  Serial.print(" 障碍距离："); Serial.println(obsDist);

  delay(100);
}

要点解读

1. 硬件中断的“毫秒级急停响应”：筑牢救援安全底线
   急停是特种救援机器人的核心安全屏障，硬件外部中断是实现毫秒级响应的关键。三个案例均采用外部中断绑定急停按钮，配合volatile关键字确保中断状态的原子性，避免中断被优化。区别于软件层面的循环检测，硬件中断直接触发中断服务函数，响应时间＜10ms，可在突发危险时迅速切断BLDC驱动输出，防止电机失控。同时采用输入上拉电阻和按键去抖设计，避免误触发，确保急停指令稳定可靠，这是保障救援人员和被困人员安全的核心底线。

2. 多传感器融合的“互补冗余设计”：解决单一传感器失效痛点
   救援环境复杂多变，单一传感器易出现检测盲区或失效，多传感器融合通过优势互补保障感知可靠性。案例1采用超声波（远距离）+红外（近距离盲区）+IMU（姿态）的融合方案，弥补超声波近距离盲区和红外受光强干扰的缺陷；案例5采用激光雷达（二维全局）+深度相机（三维局部）+气体传感器（环境风险）的融合，兼顾全局路径规划与局部避障；案例3采用气体+温度+压力+超声波的融合，全面覆盖环境安全与运动避障需求。通过传感器特性互补，构建冗余感知体系，避免因单一传感器失效导致机器人失控，大幅提升复杂环境的适应性。

3. 融合决策的“分层优先级逻辑”：兼顾安全与作业连续性
   多传感器数据需通过科学的决策逻辑转化为运动指令，分层优先级设计是平衡安全与作业效率的关键。三个案例均遵循安全优先原则，构建“环境危险→避障死锁→正常运行”的优先级体系：环境参数超标（气体、温度、压力）直接触发急停，优先级最高；避障死锁（障碍物密集）触发临时急停后转向，优先级次之；正常行驶优先级最低。同时采用条件判断与状态机结合的方式，避免优先级冲突，确保机器人在极端情况下优先保障安全，在安全环境下持续推进救援任务，既保障安全又兼顾作业连续性，适配救援场景的动态需求。

4. BLDC闭环控制的“实时动态适配”：支撑复杂工况稳定运行
   救援机器人需频繁应对变负载、颠簸等复杂工况，BLDC电机的实时闭环控制是实现稳定运动的基础。三个案例均采用FOC磁场定向控制，配合编码器实现转速与角度的闭环反馈，确保电机转矩响应迅速、速度控制精准。在避障转向时，能快速响应速度指令变化，保持转向稳定；在颠簸路面时，闭环控制可抑制路面冲击带来的速度波动，维持匀速行驶；同时通过差速运动学解算将决策指令转化为左右轮转速，实现精准的线速度与角速度控制，保障避障轨迹的准确性，满足复杂工况下的运动稳定性需求。

5. 安全逻辑的“双保险机制”：构建全维度安全防护体系
   救援场景风险复杂，单一安全机制易存在漏洞，双保险安全逻辑是杜绝失控的关键。三个案例均构建自动急停+手动急停的双重防护：自动急停基于传感器数据实时触发，当环境危险（气体超标、高温、压力异常）、避障死锁、姿态失稳时自动切断动力；手动急停采用硬件按钮，供救援人员在紧急情况下直接干预，优先级最高，不受软件逻辑影响。同时，急停状态下设置状态锁定，需人工复位后才能恢复运行，避免自动重启引发二次风险。此外，通过状态指示灯实时反馈急停状态，确保人员实时掌握机器人安全状态，形成“感知-判断-执行-反馈”的全维度安全防护闭环。

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