“Arduino BLDC 之自主巡检机器人：多传感器融合与实时控制分离”可以理解为：巡检机器人把“看环境、判断状态、规划路径”这类高算力任务交给上位机或高性能 MCU，把“电机 FOC 闭环、姿态补偿、安全停机”这类高实时任务交给底层 MCU；上位机输出目标速度、姿态角或力矩指令，底层通过 BLDC 电流环/速度环/位置环稳定执行，同时用 IMU、编码器、测距、视觉、气体/温度等传感器做融合，保证巡检过程既看得准，又动得稳。这里的“分离”不是简单拆成两块板子，而是把 感知决策 与 运动执行 按时间尺度、算力需求和安全性边界分开：上层可以容忍几十毫秒到几百毫秒的延迟，但底层 FOC 电流环、姿态环和安全保护通常要求毫秒级甚至更短的确定周期。

一、主要特点

1. 上层负责“看懂环境”，下层负责“稳定执行”
   上层通常运行 SLAM、路径规划、缺陷识别、任务调度等算法，例如视觉里程计、激光 SLAM、仪表识别、热像分析、气体异常判断等。 下层负责接收简化指令，完成 BLDC FOC 电流环、速度环、位置环、差速解算、姿态补偿、触地/打滑检测和紧急停机。
2. 多传感器融合解决单一传感器不可靠问题
   巡检场景中，IMU 可提供高频姿态和角速度，编码器提供轮速和里程，激光/ToF/超声波提供近距离障碍，相机提供语义和纹理信息，气体/温度/湿度传感器提供环境状态。 单一传感器容易受光照、反光、振动、打滑或电磁干扰影响，融合后能提升定位、避障和异常判断的可靠性。
3. 实时控制分离保证关键闭环不被阻塞
   如果视觉推理、地图更新、日志写入或网络通信占用主循环，BLDC 电流环和姿态环会出现抖动。工程上通常把 FOC 电流环、IMU 读取、编码器采样和安全保护放在硬件定时器中断或高优先级任务中；把 SLAM、识别、路径规划放在上位机或低优先级任务中。
4. 通信接口采用“简化指令 + 状态回传”
   上下层之间不应传输原始点云、图像或完整地图，而应传输压缩后的控制量，例如目标线速度、角速度、机身俯仰角、最大加速度、急停标志等。底层回传实际速度、IMU 姿态、电池电压、电机电流、故障码和传感器健康状态。这样既能降低串口/CAN 带宽压力，也能减少通信抖动对控制的影响。
5. BLDC FOC 是柔顺执行和低速稳定性的基础
   巡检机器人常需要在狭窄通道、坡道、碎石地或设备附近低速运行。BLDC 配合 FOC 可降低转矩脉动，提高低速控制精度，减少抖动和噪声；在遇到打滑、磕碰或负载突变时，也能通过电流环更快响应。
6. 安全机制必须独立于高层算法
   即使上层仍在输出路径或速度指令，底层也应保留独立的安全判断：例如 IMU 倾角超限、编码器失联、电流异常、通信超时、电池欠压、碰撞检测等触发时，应立即降级或停机，不能等待上层重新规划。

二、应用场景

1. 变电站/配电房巡检
   机器人需要识别表计、指示灯、开关状态，同时检测局部过热、气体泄漏和异常噪声。上层做视觉和传感器融合判断，下层保证在电缆沟盖板、门槛、碎石地面上稳定移动。
2. 工业园区与管道巡检
   地面可能存在油污、积水、砂石、门槛和金属格栅。BLDC 底盘配合 IMU 和测距传感器可保持姿态稳定；上层视觉或热像模块负责识别泄漏、腐蚀、裂纹等缺陷。
3. 仓储货架巡检
   机器人在狭窄货架通道中运行，需要避免剐蹭立柱和托盘。上层负责定位和任务分配，下层负责近距离避障、差速转向和低速平稳控制。
4. 温室/农田监测
   地面软硬不均、存在坡度和灌溉管道。BLDC 高扭矩特性适合泥泞或松软地面，IMU 和轮速融合可补偿打滑；上层负责作物图像、温湿度和光照数据采集。
5. 隧道、廊道、地下空间巡检
   这类环境常存在弱光、高湿、强电磁干扰或 GPS 缺失。上层可用激光/视觉惯导融合定位，下层依靠 IMU、编码器和近距离传感器维持基本运动安全。
6. 教学科研平台
   适合验证 EKF/Madgwick 姿态融合、轮速里程计、轻量 SLAM、差速底盘控制、FOC 电流环、通信协议设计和故障降级策略。

三、需要注意的事项

1. 先划分时间尺度，再做硬件分工
   FOC 电流环通常需要较高更新频率，姿态环和速度环也需要稳定周期；SLAM、视觉识别和路径规划则可以较慢但算力更高。 如果只用 8 位 Arduino，建议仅承担轻量执行；更稳妥的方案是上位机或 ESP32-S3/STM32 做融合与规划，底层 MCU 做实时控制。
2. 传感器必须做时间戳和坐标标定
   IMU、编码器、相机、激光和测距模块的采样频率不同，若直接按当前时刻融合，容易产生姿态跳变或定位漂移。上层融合时应使用时间戳对齐；机械安装上要标定相机外参、IMU 安装角、轮径和轮距。
3. 通信要有超时、校验和降级策略
   上下层通信一旦丢包或延迟，机器人不能继续按旧指令高速运行。应设置心跳包、CRC 校验、指令有效期和超时停机机制。通信恢复后也应从低速安全状态重新进入任务，而不是立即恢复原速度。
4. 底层安全回路优先级最高
   急停、过流、过温、倾角超限、编码器失联、电池欠压等保护应放在底层 MCU，不依赖上位机判断。即使上层死机或通信中断，底层也必须能把电机安全停下来。
5. BLDC 动力电源与控制电源要隔离
   电机启停、差速转向和堵转时会产生较大电流冲击和高频噪声，可能干扰 IMU、ADC、编码器和通信模块。建议使用隔离 DC-DC、低 ESR 电容、屏蔽线和合理接地，敏感信号线远离动力线。
6. 融合算法要考虑动态干扰
   机器人加速、减速、转弯或越障时，加速度计会混入动态加速度，单纯依赖 IMU 容易误判姿态。可用编码器速度、轮速约束或视觉/激光观测辅助校正；打滑严重时也要降低对里程计的信任权重。
7. 上层指令应带限幅和平滑过渡
   上层下发的速度或角速度不应突变。底层应加入加速度限幅、斜率限制或 S 曲线插值，避免 BLDC 电流冲击、车身抖动和传感器数据跳变。
8. 巡检任务要区分“感知失败”和“运动失败”
   相机被遮挡、激光被污染、气体传感器预热未完成，属于感知降级，可以降低任务等级或原地等待；而电机失步、IMU 异常、通信中断、电池骤降属于运动安全风险，应优先停机或返回安全点。
   整体来看，这条技术链路可以概括为：上层多传感器融合感知与规划 → 生成简化运动指令 → 底层实时状态估计与 BLDC 闭环执行 → 故障检测与安全降级 → 状态回传与任务更新。它的核心价值在于：上层负责“判断该去哪里、发现什么异常”，下层负责“保证每一步都稳定、可停、可恢复”。做原型时，建议先用“上位机 SLAM + 底层差速 BLDC + IMU/编码器融合 + 心跳超时保护”跑通最小闭环，再逐步加入视觉识别、气体检测和复杂路径规划。

1、ESP32双核任务分离——Core 1控电机，Core 0跑融合与规划
此案例展示最直接的架构分离：利用ESP32双核特性，将时间关键的控制任务锁定在Core 1，融合与规划运行在Core 0。搜索结果显示，这种双核任务分离方案已在室外埋地管道巡检机器人中验证。

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

// ==================== 电机与传感器 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);
Encoder encL(18, 19, 2048), encR(20, 21, 2048);
MPU6050 imu;

// ==================== 核间共享变量（volatile + 原子访问） ====================
volatile float sharedTargetV = 0.0f;    // Core 0 写入，Core 1 读取
volatile float sharedTargetW = 0.0f;
volatile float sharedYaw = 0.0f;        // Core 1 写入，Core 0 读取
portMUX_TYPE sharedMux = portMUX_INITIALIZER_UNLOCKED;

// ==================== 任务句柄 ====================
TaskHandle_t taskCore0, taskCore1;

// ==================== Core 0：融合 + 规划（低频，100ms） ====================
void core0Task(void *pv) {
    TickType_t lastWake = xTaskGetTickCount();
    const TickType_t period = pdMS_TO_TICKS(100);  // 10Hz
    
    while (1) {
        // 1. 读取IMU，互补滤波融合航向
        int16_t ax, ay, az, gx, gy, gz;
        imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
        
        static float yaw = 0;
        static unsigned long lastT = 0;
        float dt = (millis() - lastT) / 1000.0f;
        if (dt > 0.01f) {
            yaw += (gz / 131.0f) * dt;  // 陀螺仪积分
            float accelYaw = atan2(ay, ax) * RAD_TO_DEG;
            yaw = 0.98f * yaw + 0.02f * accelYaw;  // 互补滤波
            lastT = millis();
        }
        
        // 2. 原子写入共享航向
        portENTER_CRITICAL(&sharedMux);
        sharedYaw = yaw;
        portEXIT_CRITICAL(&sharedMux);
        
        // 3. 简化规划：基于航向和预设航点计算目标速度
        // （实际应接入A*/DWA，此处简化为“保持航向”）
        float targetYaw = 0.0f;  // 期望航向
        float yawError = targetYaw - yaw;
        
        portENTER_CRITICAL(&sharedMux);
        sharedTargetV = 0.3f;                    // 恒定前进速度
        sharedTargetW = constrain(yawError * 0.01f, -1.0f, 1.0f);
        portEXIT_CRITICAL(&sharedMux);
        
        vTaskDelayUntil(&lastWake, period);
    }
}

// ==================== Core 1：BLDC FOC 控制（高频，1kHz） ====================
void core1Task(void *pv) {
    TickType_t lastWake = xTaskGetTickCount();
    const TickType_t period = pdMS_TO_TICKS(1);  // 1kHz
    
    while (1) {
        // 1. FOC 电流环（最高优先级）
        motorL.loopFOC();
        motorR.loopFOC();
        
        // 2. 原子读取共享速度指令
        float v, w;
        portENTER_CRITICAL(&sharedMux);
        v = sharedTargetV;
        w = sharedTargetW;
        portEXIT_CRITICAL(&sharedMux);
        
        // 3. 差速运动学 → 左右轮速度
        float vL = v - w * 0.125f;  // 半轮距 0.125m
        float vR = v + w * 0.125f;
        
        motorL.move(vL / 0.05f);
        motorR.move(vR / 0.05f);
        
        vTaskDelayUntil(&lastWake, period);
    }
}

void setup() {
    Serial.begin(115200);
    
    // 电机初始化
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.linkSensor(&encL);   motorR.linkSensor(&encR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    // IMU 初始化
    Wire.begin();
    imu.initialize();
    
    // 创建任务：Core 1 优先级更高
    xTaskCreatePinnedToCore(core0Task, "Fusion", 8192, NULL, 1, &taskCore0, 0);
    xTaskCreatePinnedToCore(core1Task, "Control", 8192, NULL, 2, &taskCore1, 1);
}

void loop() {
    vTaskDelay(pdMS_TO_TICKS(1000));  // loop 空闲
}

要点：核间共享变量必须原子化读写（portENTER_CRITICAL）。Core 0 以 10Hz 更新速度指令和航向，Core 1 以 1kHz 执行 FOC 和差速控制。volatile 保证编译器不优化，portMUX 保证读写不撕裂。

2、单核时间片分离——高频FOC中断 + 低频融合主循环
当MCU不具备双核（如Arduino Uno/Nano）时，采用定时器中断执行高频控制，主循环执行融合与规划。这是成本最低的架构分离方案。

#include <SimpleFOC.h>
#include <TimerOne.h>

// ==================== 电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);
Encoder encL(2, 3, 500), encR(18, 19, 500);

// ==================== 全局变量（中断与主循环共享） ====================
volatile float g_targetV = 0.0f;
volatile float g_targetW = 0.0f;

// ==================== 定时器中断：1kHz FOC 控制 ====================
void controlISR() {
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 读取指令（float 在 8 位机上非原子，但单写者场景可接受短暂撕裂）
    float v = g_targetV;
    float w = g_targetW;
    
    float vL = v - w * 0.125f;
    float vR = v + w * 0.125f;
    
    motorL.move(vL / 0.05f);
    motorR.move(vR / 0.05f);
}

// ==================== 主循环：融合与规划（约 20Hz） ====================
void setup() {
    Serial.begin(115200);
    
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.linkSensor(&encL);   motorR.linkSensor(&encR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    // 1kHz 定时器中断（1ms 周期）
    Timer1.initialize(1000);
    Timer1.attachInterrupt(controlISR);
    
    Serial.println("单核时间片分离就绪");
}

void loop() {
    // 低频任务：传感器融合 + 规划（20Hz）
    static unsigned long lastPlan = 0;
    if (millis() - lastPlan < 50) return;
    lastPlan = millis();
    
    // 读取超声波/激光等传感器
    float frontDist = readUltrasonic();
    
    // 简化规划：避障优先
    if (frontDist < 0.3f) {
        g_targetV = 0.0f;
        g_targetW = 0.5f;  // 转向避障
    } else {
        g_targetV = 0.3f;
        g_targetW = 0.0f;
    }
    
    Serial.print("Plan: V="); Serial.print(g_targetV);
    Serial.print(" W="); Serial.println(g_targetW);
}

float readUltrasonic() {
    // 简化：返回模拟距离
    return 0.5f;
}

要点：定时器中断保证 FOC 控制的硬实时性，主循环负责慢速任务。共享变量使用 volatile 防止编译器优化。SimpleFOC 官方强调 loopFOC() 和 move() 必须在主循环中高频调用，中断中执行需确保 FOC 初始化已完成。

3、分层融合 + 安全通道分离（编码器/IMU高频，GPS/规划低频，超声波独立中断）
此案例展示最完整的三层分层融合架构：编码器里程计作为高频内部基准，IMU 互补滤波提供航向，GPS/规划层低频校正，超声波作为独立安全通道。搜索结果显示，这种分层递进融合策略在电力巡检机器人中实现了“室外大范围巡航+近距可靠避障”的兼顾。

#include <SimpleFOC.h>
#include <Wire.h>
#include <TinyGPS++.h>

// ==================== 电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9, 10, 11, 8);
BLDCDriver3PWM driverR(3, 5, 6, 4);
Encoder encL(2, 3, 2048), encR(18, 19, 2048);

// ==================== GPS/规划层 ====================
TinyGPSPlus gps;
HardwareSerial gpsSerial(1);  // ESP32 硬件串口

// ==================== 共享状态 ====================
volatile float g_cmdV = 0.0f;      // 融合层输出
volatile float g_cmdW = 0.0f;
volatile bool  g_emergency = false; // 安全中断置位

// ==================== 安全通道（独立中断） ====================
void emergencyISR() {
    // 超声波触发的中断（或轮询式急停）
    g_emergency = true;
}

// ==================== 高频控制任务（Core 1） ====================
void controlTask(void *pv) {
    TickType_t last = xTaskGetTickCount();
    const TickType_t period = pdMS_TO_TICKS(1);  // 1kHz
    
    while (1) {
        motorL.loopFOC();
        motorR.loopFOC();
        
        // 安全通道优先：任何情况下急停
        if (g_emergency) {
            motorL.move(0);
            motorR.move(0);
        } else {
            float v = g_cmdV;
            float w = g_cmdW;
            motorL.move((v - w * 0.125f) / 0.05f);
            motorR.move((v + w * 0.125f) / 0.05f);
        }
        
        vTaskDelayUntil(&last, period);
    }
}

// ==================== 融合与规划任务（Core 0） ====================
void fusionTask(void *pv) {
    TickType_t last = xTaskGetTickCount();
    const TickType_t period = pdMS_TO_TICKS(50);  // 20Hz
    
    float odomX = 0, odomY = 0, odomYaw = 0;
    float gpsX = 0, gpsY = 0;
    
    while (1) {
        // --- 第一层：编码器里程计（高频） ---
        float vL = encL.getVelocity() * 0.05f;
        float vR = encR.getVelocity() * 0.05f;
        float dt = 0.05f;
        
        float v = (vL + vR) / 2.0f;
        float w = (vR - vL) / 0.25f;
        
        odomYaw += w * dt;
        odomX += v * cos(odomYaw) * dt;
        odomY += v * sin(odomYaw) * dt;
        
        // --- 第二层：GPS 低频校正 ---
        while (gpsSerial.available()) gps.encode(gpsSerial.read());
        
        if (gps.location.isUpdated() && gps.hdop.hdop() < 2.0f) {
            // GPS 有效：将里程计位置“拉回”GPS 附近
            float alpha = 0.1f;  // 校正权重
            odomX = (1 - alpha) * odomX + alpha * gpsX;
            odomY = (1 - alpha) * odomY + alpha * gpsY;
        }
        
        // --- 简化规划：向目标航点前进 ---
        float targetX = 5.0f, targetY = 3.0f;
        float dx = targetX - odomX;
        float dy = targetY - odomY;
        float targetYaw = atan2(dy, dx);
        float yawError = targetYaw - odomYaw;
        
        // 归一化角度误差
        while (yawError > PI) yawError -= 2 * PI;
        while (yawError < -PI) yawError += 2 * PI;
        
        g_cmdV = 0.4f;
        g_cmdW = constrain(yawError * 0.8f, -1.0f, 1.0f);
        
        // --- 安全通道检查（超声波轮询） ---
        float frontDist = readUltrasonic();
        if (frontDist < 0.25f) {
            g_emergency = true;
            Serial.println("EMERGENCY: obstacle < 0.25m");
        } else if (frontDist > 0.4f) {
            g_emergency = false;
        }
        
        vTaskDelayUntil(&last, period);
    }
}

float readUltrasonic() {
    // 简化：返回模拟距离
    return 0.6f;
}

void setup() {
    Serial.begin(115200);
    gpsSerial.begin(9600, SERIAL_8N1, 16, 17);  // RX=16, TX=17
    
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.linkSensor(&encL);   motorR.linkSensor(&encR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    xTaskCreatePinnedToCore(controlTask, "Ctrl", 8192, NULL, 3, NULL, 1);
    xTaskCreatePinnedToCore(fusionTask, "Fusion", 8192, NULL, 1, NULL, 0);
}

void loop() {
    vTaskDelay(pdMS_TO_TICKS(1000));
}

要点：三层频率递进——编码器 100Hz+（高频内部基准）、GPS 1~10Hz（低频全局锚点）、超声波毫秒级（独立安全中断）。安全通道的 g_emergency 标志位拥有最高执行优先级，在控制任务中无条件覆盖所有导航指令。

要点解读

1. 分离的本质是“频率解耦”，而非“功能解耦”
   传感器融合（EKF/互补滤波）的更新周期通常在 10~100ms，路径规划的 A*/DWA 可能需要数十毫秒，而 BLDC 的 FOC 电流环要求 1kHz 甚至更高。如果串行执行，FOC 的实时性被融合与规划任务“拖累”，导致电流环抖动、电机发热、导航精度下降。分离的核心是让不同频率的任务运行在各自的时间尺度上。

2. ESP32 双核是嵌入式巡检机器人的“最低成本分离方案”
   xTaskCreatePinnedToCore 可以将高优先级控制任务锁定在 Core 1，融合与规划运行在 Core 0。这种方案不需要外部协处理器，在单芯片上实现了硬实时与软实时的共存。注意：共享变量必须用 volatile 声明，跨核读写需要 portENTER_CRITICAL 保护，防止 32 位 float 的读写撕裂。

3. 定时器中断是单核 MCU 的“时间片分离”手段
   对于 Arduino Uno/Nano 等单核平台，Timer1.attachInterrupt(controlISR) 可以在 1ms 周期内执行 FOC 和电机控制，主循环以 20~50Hz 运行融合与规划。代价是中断服务程序必须极短——只做 FOC 和速度指令执行，任何 Serial.print 或复杂计算都会破坏实时性。

4. 安全通道必须独立于导航层，且拥有最高执行优先级
   超声波/触觉急停不能等待融合层或规划层处理。案例三中 g_emergency 标志位在控制任务中无条件覆盖所有速度指令。这是分层架构的“底线保护”——即使融合层卡死或规划层输出异常，安全通道仍能保证机器人不撞墙。

5. 异构计算是算力不足时的工程标准
   当 Arduino 需要同时处理 SLAM、深度学习或复杂 EKF 时，单芯片分离仍然不够。工业级方案采用上位机（树莓派/Jetson）+ 下位机（Arduino/ESP32） 架构：上位机负责运算密集型任务（SLAM、视觉、全局规划），下位机专职实时传感器读取和 BLDC 闭环控制，通过串口或 CAN 总线通信。这种异构架构是自主巡检机器人从“能跑”到“可靠”的关键跃迁。

4、SimpleFOC 双核架构 —— FOC 实时控制环与决策环物理分离
适用硬件：ESP32 / Arduino Due + BLDC（SimpleFOC）+ 编码器 + MPU6050 + 超声波。

核心思路：把"电机电流环/速度环"（必须毫秒级）和"避障/导航决策"（可几十毫秒级）拆成两个独立执行体，决策层只写目标速度，控制层只读目标速度。

/*
 * 案例4：SimpleFOC BLDC 巡检机器人 —— 实时控制与决策分离
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include <MPU6050.h>

// ============ 执行层（实时控制，不受决策阻塞） ============
BLDCMotor motor = BLDCMotor(7);            // 7 对极
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 2048);
void onEncoder() { encoder.handleA(); }

volatile float targetSpeed = 0.0f;         // 决策层写，控制层读
volatile bool  emergencyStop = false;      // 硬停止标志

void setupControl() {
  encoder.init();
  encoder.enableInterrupts(onEncoder);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;   // 速度环
  motor.PID_velocity.P = 0.2f;
  motor.PID_velocity.I = 2.0f;
  motor.voltage_limit = 12.0f;
  motor.init();
  driver.voltage_power_supply = 12.0f;
  driver.init();
  motor.linkDriver(&driver);
}

// 高频控制循环：只执行，不决策（建议用定时器中断 1kHz 触发）
void controlLoopISR() {
  motor.loopFOC();                          // 电流环
  if (emergencyStop) motor.move(0);
  else motor.move(targetSpeed);             // 速度环
}

// ============ 决策层（多传感器融合） ============
MPU6050 imu;
const int trigPin = 6, echoPin = 7;
float readUltrasonic() {
  digitalWrite(trigPin, LOW); delayMicroseconds(2);
  digitalWrite(trigPin, HIGH); delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  long d = pulseIn(echoPin, HIGH);
  return d * 0.034 / 2.0;                   // cm
}

void setup() {
  Serial.begin(115200);
  pinMode(trigPin, OUTPUT); pinMode(echoPin, INPUT);
  Wire.begin(); imu.initialize();
  setupControl();
  // 用定时器挂高频控制环，见注释
}

void loop() {
  // ---- 融合 1：超声近距离优先（硬避障）----
  float dist = readUltrasonic();
  if (dist < 20.0f) { emergencyStop = true; targetSpeed = 0; }
  else emergencyStop = false;

  // ---- 融合 2：IMU 姿态/航向纠偏 ----
  imu.update();
  float yaw = imu.getYaw();
  float err = constrain(yaw - 0.0f, -30, 30);   // 目标航向 0°
  targetSpeed = map(err, -30, 30, -3.0f, 3.0f); // 比例转向

  Serial.print("dist="); Serial.print(dist);
  Serial.print(" yaw="); Serial.print(yaw);
  Serial.print(" spd="); Serial.println(targetSpeed);
  delay(20);                                    // 决策周期 20ms
}

实际布线上，把 controlLoopISR() 挂到硬件定时器中断（如 ESP32 的 hw_timer 或 Due 的 Timer），确保控制环不受 loop() 里超声波 pulseIn 阻塞（pulseIn 最长可卡几十毫秒）。

5、FreeRTOS 任务优先级分离 —— 实时控制/传感器/融合各自成任务
适用硬件：ESP32（原生双核 + FreeRTOS）。把巡检任务拆成三个优先级明确的独立 Task，从"代码结构"上强制分离控制与融合。

/*
 * 案例5：ESP32 + BLDC 巡检，FreeRTOS 三任务分离
 *   T1 实时控制  (最高优先级，1kHz)
 *   T2 传感器采集 (中优先级，10ms)
 *   T3 融合决策   (低优先级，50ms)
 */
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor motor = BLDCMotor(7);
BLDCDriver3PWM driver = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encoder = Encoder(2, 3, 2048);
void onEncoder() { encoder.handleA(); }

// 跨任务共享的"信箱"结构（用临界区/信号量保护）
volatile float   g_speed   = 0.0f;   // T1 读，T3 写
volatile float   g_dist    = 999.0f; // T1 读，T2 写
volatile float   g_yaw     = 0.0f;   // T1 读，T3 写
volatile bool    g_estop   = false;  // 任何任务可写
SemaphoreHandle_t speedMux;

// ---- T1：实时控制（绝不在此做传感器逻辑）----
void taskControl(void *pv) {
  TickType_t xLast = xTaskGetTickCount();
  for (;;) {
    motor.loopFOC();
    if (g_estop) { motor.move(0); }
    else {
      xSemaphoreTake(speedMux, portMAX_DELAY);
      motor.move(g_speed);
      xSemaphoreGive(speedMux);
    }
    vTaskDelayUntil(&xLast, pdMS_TO_TICKS(1));  // 1kHz
  }
}

// ---- T2：传感器采集（纯采集，不决策）----
void taskSense(void *pv) {
  TickType_t xLast = xTaskGetTickCount();
  for (;;) {
    g_dist = readUltrasonic();   // 阻塞发生在本任务，不影响 T1
    vTaskDelayUntil(&xLast, pdMS_TO_TICKS(10));
  }
}

// ---- T3：融合决策（优先级最低，不碰电机）----
void taskFusion(void *pv) {
  TickType_t xLast = xTaskGetTickCount();
  for (;;) {
    float d = g_dist;
    float y = g_yaw;
    g_estop = (d < 20.0f);
    float spd = constrain(-y * 0.5f, -3.0f, 3.0f);
    if (!g_estop) {
      xSemaphoreTake(speedMux, portMAX_DELAY);
      g_speed = spd;
      xSemaphoreGive(speedMux);
    }
    vTaskDelayUntil(&xLast, pdMS_TO_TICKS(50));
  }
}

void setup() {
  encoder.init();
  encoder.enableInterrupts(onEncoder);
  motor.linkSensor(&encoder);
  motor.controller = MotionControlType::velocity;
  motor.PID_velocity.P = 0.2f; motor.PID_velocity.I = 2.0f;
  motor.voltage_limit = 12.0f;
  motor.init();
  driver.voltage_power_supply = 12.0f; driver.init();
  motor.linkDriver(&driver);

  speedMux = xSemaphoreCreateMutex();
  // 优先级：控制(3) > 采集(2) > 融合(1)，各跑在独立核心
  xTaskCreatePinnedToCore(taskControl, "ctl", 4096, NULL, 3, NULL, 1);
  xTaskCreatePinnedToCore(taskSense,   "sen", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskFusion,  "fus", 4096, NULL, 1, NULL, 0);
}
void loop() { vTaskDelay(portMAX_DELAY); }  // 主循环空转

6、ESC 型 BLDC + 加权融合避障（无 FOC，兼容常见航模电调）
适用硬件：任意 Arduino + ESC（Servo 库 50Hz PWM）+ 多路超声波/红外 + 罗盘。

强调"加权平均 + 投票"融合：多个传感器各自给"转向建议"，按置信度加权合并成单一目标，再交给低速伺服式 BLDC。

/*
 * 案例6：ESC BLDC 巡检机器人 —— 多传感器加权融合导航
 * 传感器：前(US1)/左(US2)/右(US3) 三个超声波 + 红外探测 + 指南针
 */
#include <Servo.h>

Servo esc;                  // BLDC 电调当作舵机接口，50Hz
const int usPins[3][2] = {{6,7},{8,9},{10,11}}; // trig,echo

float readUS(int idx) {
  digitalWrite(usPins[idx][0], LOW); delayMicroseconds(2);
  digitalWrite(usPins[idx][0], HIGH); delayMicroseconds(10);
  digitalWrite(usPins[idx][0], LOW);
  long d = pulseIn(usPins[idx][1], HIGH);
  return (d > 0) ? d * 0.034 / 2.0 : 60.0f;   // 超时给远值
}

// 每个方向给出"转向权重"：(sensor, 建议航向偏移, 置信度)
// 融合输出 = Σ(建议×置信度) / Σ置信度
float fuseSteer(float front, float left, float right) {
  float steer = 0, conf = 0;
  // 前方越近，越要转向（转向量=前方危险程度的函数）
  float fw = constrain(1.0f - front / 40.0f, 0, 1);   // 前方置信度
  if (fw > 0.1f) { steer += (left - right) * fw; conf += fw; } // 往空旷侧偏

  // 左、右各自被逼近时，反向躲开
  float lw = constrain(1.0f - left / 30.0f, 0, 1);
  if (lw > 0.1f) { steer += (-1.0f) * 15 * lw; conf += lw; }   // 左近 -> 右转
  float rw = constrain(1.0f - right / 30.0f, 0, 1);
  if (rw > 0.1f) { steer += (1.0f) * 15 * rw; conf += rw; }    // 右近 -> 左转

  return (conf > 0) ? steer / conf : 0.0f;   // 归一化
}

void setup() {
  esc.attach(9);
  esc.writeMicroseconds(1500);   // BLDC 中位（停）
  for (int i = 0; i < 3; i++) pinMode(usPins[i][0], OUTPUT);
  Serial.begin(115200);
}

void loop() {
  float f = readUS(0), l = readUS(1), r = readUS(2);
  float steer = fuseSteer(f, l, r);           // 融合 -> 单一转向量

  // 实时控制分离：根据转向量决定 ESC 油门，控制层只管输出
  int throttle = 1500;                         // 中位
  if (f < 25.0f) throttle = 1250;              // 紧急减速
  else throttle = 1500 + (int)steer * 8;       // 转向映射到差速补偿
  esc.writeMicroseconds(constrain(throttle, 1250, 1750));

  Serial.print("steer="); Serial.print(steer);
  Serial.print(" thr="); Serial.println(throttle);
  delay(20);
}

要点解读
一、控制分离的本质是"时间尺度隔离"
实时控制（FOC 电流环 1kHz、速度环数百 Hz、ESC 输出 50Hz）和传感器融合决策（几十 ms）对实时性的要求差一到两个数量级。把它们绑在一个循环里，pulseIn、delay、Serial.print 这类阻塞调用就会拖垮电机控制。案例一二用定时器/独立 Task 把高频执行体和低频决策体物理分开，案例三则把"融合算转向"和"输出油门"显式拆成两步——这是巡检稳定性的根基。

二、融合的关键是"置信度"而非简单叠加
多传感器数据不能无脑相加。案例三展示了"加权平均 + 归一化"：每个传感器既给建议值也给置信度（距离越近置信度越高），融合输出 = Σ(建议×置信度)/Σ置信度，把互相矛盾或噪声源（如超声波超时返回远值）对结果的干扰降到最低。真正工程里还会按传感器类型再乘权重（IMU 姿态 > 超声 > 红外），构成"级联信任"。

三、跨层共享数据必须"同步"
决策层写、控制层读的共享变量（targetSpeed、g_speed），在 FreeRTOS 下要用互斥锁或临界区保护，防止读写撕裂；中断上下文里的共享变量（案例一）最好声明为 volatile 并单字节写。这是"分离之后最容易引入的 bug"——分层清楚了，但忘记锁，控制环读到半个数据会导致电机抖动甚至飞车。

四、实时层必须是"纯执行、无决策"
控制环（motor.loopFOC()/move()）内部绝不能放传感器读取、滤波、打印等逻辑。它应该只消费一个清晰的目标量（目标速度/目标位置），并读取一个紧急停止标志。这样即使决策层被阻塞或跑飞，执行层仍能保持电机安全可控——这也是"应急停止"能可靠生效的前提（案例一里 emergencyStop 独立于决策循环，随时可硬停）。

五、为"故障降级"预留通道，而非死等最优融合
巡检机器人的鲁棒性不在于融合算法多聪明，而在于"传感器失效时还能安全跑"。好的分离架构会做：主传感器超时/数据非法时，自动降级到次级传感器（超声失效用红外），再降级到保守慢速；同时保留独立的硬件急停（案例一的 emergencyStop 直接置零速度）。融合层可以复杂，但控制层永远有一条"不知道发生了什么也能停下来"的最低保障路径。

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