Arduino BLDC双机器人VFF避障+随机扰动逃逸的核心在于：以VFF构建去中心化的局部动态避障力场，以随机扰动打破对称死锁，以BLDC FOC闭环保证高动态执行精度，在Arduino有限算力下实现双机协同的安全、平滑与鲁棒。
一、系统架构与核心原理
该系统采用"VFF局部避障 + 随机扰动逃逸 + BLDC高动态执行"的分层架构，各层协同完成从环境感知到安全运动的完整闭环：
第一层：VFF虚拟力场层（局部动态避障，核心）
VFF（Virtual Force Field，虚拟力场法）是人工势场法与栅格法的结合，其核心思想是在机器人周围构建一个虚拟力场——目标点产生引力场，障碍物（包括另一台机器人）产生斥力场，机器人的运动方向由合力矢量决定。
在双机器人系统中，每台机器人独立计算自身的虚拟力场：
目标引力： 机器人沿合力方向运动，实现动态避障与路径平滑。
VFF的关键优势在于去中心化——每台机器人仅依赖局部传感器（超声波、红外、激光雷达）和邻近通信（NRF24L01、ESP-NOW）获取环境与邻居状态，独立计算力场，无需中央调度器，避免单点故障。
第二层：随机扰动逃逸层（局部极小值脱困）
纯VFF存在一个经典缺陷——局部极小值问题：当两台机器人对称布局（如面对面相遇），引力与斥力恰好平衡，合力为零，机器人陷入"死锁"或"振荡"状态，无法自行脱困。
随机扰动逃逸机制通过以下方式打破对称：
随机游走模式：当检测到机器人长时间停滞（速度低于阈值且持续N个控制周期），向合力矢量中注入一个随机方向的速度扰动项，打破力的平衡，使机器人偏离死锁点。
虚拟目标点切换：在局部极小值区域设置临时虚拟目标点，引导机器人先向虚拟目标移动脱离困境，再切换回真实目标。
混合全局规划触发：当VFF连续M次无法提供有效路径时，触发全局路径规划算法（如A*）重新规划路径，绕过死锁区域。
第三层：BLDC FOC执行层（高动态运动控制）
VFF输出的速度/方向指令需要BLDC电机精确执行。采用FOC（磁场定向控制）驱动BLDC电机，通过Clarke变换（ABC→αβ）和Park变换（αβ→dq）将三相电流解耦为直轴（d轴，励磁分量）和交轴（q轴，转矩分量），分别施加独立PI调节器，实现：
零速高转矩启动：满足机器人从静止快速加速的需求。
正弦化电流波形：消除传统六步换相的转矩脉动，实现低速平滑运转，减少双机协同中的队形抖动。
高响应带宽：FOC控制周期可达50μs级，确保机器人能快速响应VFF输出的速度指令，减少惯性导致的过冲或振荡。
再生制动：减速时将动能转化为电能回馈母线，提供强大的电磁制动效果，确保双机在狭窄空间内的精准会车。
第四层：分布式通信与状态同步层
双机器人协同高度依赖实时的相对位姿共享。系统通常采用ESP-NOW等低延迟点对点通信协议，实现毫秒级数据传输，确保各机器人在高速运动状态下能实时获取邻居位置和速度，避免因通信延迟导致的预测失效或碰撞。
二、主要特点

1. 去中心化自组织架构
   这是双机器人VFF系统最核心的设计哲学。每台机器人通过局部传感器和邻近通信独立决策，无需中央调度器：
   分布式决策：每台机器人独立计算虚拟力场，避免集中式控制的单点故障风险。
   对等通信：双机之间通过NRF24L01或ESP-NOW交换位置、速度、航向等状态信息，通信延迟控制在10ms以内。
   容错性强：即使一台机器人失效，另一台仍能依靠本地VFF继续执行避障任务，系统不会完全瘫痪。
2. VFF的动态适应性与参数在线调整
   VFF算法的参数并非固定不变，而是根据环境复杂度和机器人状态实时调整：
   障碍物密度自适应：当传感器检测到周围障碍物增多时，自动增大斥力增益系数 k rep，增大安全距离 d safe，使机器人更保守地避障。
   电量/负载自适应：当电池电量低或负载增大时，自动降低最大速度限制，减小引力增益 k att，避免高速运动导致失控。
   双机协同自适应：当两台机器人距离小于安全阈值时，将对方视为动态障碍物，斥力场强度随距离减小而急剧增大，确保不会碰撞。
3. 随机扰动逃逸的多种策略
   针对不同类型的局部极小值，系统采用多种逃逸策略：
   对称死锁（双机面对面）：注入随机横向速度扰动，使其中一台机器人略微偏转，打破对称性。
   U型障碍物陷阱：当VFF连续多次无法提供有效路径时，触发A*全局重规划，绕过U型障碍物。
   振荡陷阱（机器人在两个障碍物之间来回摆动）：引入阻尼项抑制振荡，同时注入小幅随机扰动帮助机器人脱离振荡区域。
4. BLDC FOC的低脉动平滑驱动
   FOC驱动相比传统六步换相在双机协同中具有显著优势：
   转矩平滑：正弦电流驱动消除转矩纹波，双机在狭窄通道会车时不会因力矩脉动产生抖动。
   低速可控：FOC可在极低转速下输出平稳转矩，满足双机在障碍物附近缓慢调整姿态的需求。
   双向控制：支持电机正反转无缝切换，实现原地转向（Zero-turning）和差速转向，极大提升狭窄空间内的运动灵活性。
5. 多模态传感器融合增强鲁棒性
   系统融合多种传感器数据，通过卡尔曼滤波降低噪声对VFF计算的影响：
   IMU（MPU6050/ICM-20948）：提供航向角和角速度，辅助VFF计算运动方向。
   编码器里程计：提供速度和位移反馈，用于VFF的速度闭环控制。
   超声波/红外/激光雷达：提供障碍物距离信息，构建斥力场。
   UWB定位（可选）：提供厘米级绝对位置，用于双机之间的精确相对定位。
   三、应用场景
6. 智能仓储与物流分拣
   多台AGV在仓库货架间执行货物搬运，VFF控制可实时避开临时堆放的物料或其他AGV。双机在狭窄通道会车时，VFF自动调整各自的运动轨迹，通过随机扰动打破死锁，确保高效通行。BLDC的低噪音特性适合仓储环境。
7. 灾难救援与危险区域探索
   在地震废墟或核辐射区域，双机器人通过VFF协同避障，结合热成像或气体传感器快速覆盖大面积未知区域。当一台机器人因障碍物被困时，另一台可通过随机扰动逃逸机制自主脱困，无需人工干预。
8. 安防巡逻编队
   双巡逻机器人在园区内协同巡逻，覆盖更宽的监控视野。当遇到障碍物（如停放的车辆）时，VFF控制双机自适应绕行，保持协同关系。UWB定位确保双机在无GPS环境下维持精确的相对位置。
9. 农业巡检与环境监测
   农田中双机器人沿作物行间移动，VFF控制可避开水坑、石块等障碍物，并通过分布式感知数据共享生成全覆盖路径，提升农药喷洒或土壤采样的效率。
10. 高校科研与算法验证
    作为多智能体协同控制、VFF算法验证和BLDC高动态驱动的教学实验平台，演示去中心化协同、局部极小值逃逸、FOC平滑控制等核心技术。适用于高校机器人学、多智能体系统和自动控制课程。
    四、需要注意的事项
11. 通信延迟与带宽限制
    无线通信的延迟可能导致VFF计算基于过时的邻居位置信息，引发误判：
    TDMA时隙分配：采用时分多址协议为每台机器人分配固定通信时隙，避免数据冲突。
    状态预测补偿：引入卡尔曼滤波或恒速模型对邻居位置进行预测，补偿通信延迟带来的误差。
    通信频率匹配：通信频率应与VFF控制频率匹配（建议不低于10Hz），确保力场计算的实时性。
12. 算力与实时性平衡
    VFF算法、传感器数据处理及BLDC FOC控制对MCU算力提出较高要求：
    经典8位Arduino（Uno/Mega）：难以同时处理VFF+FOC+传感器融合，控制频率建议不低于100Hz。
    推荐平台：ESP32（双核240MHz）或Teensy 4.1，支持一核运行VFF+通信，另一核运行FOC+编码器采集。
    算法简化：在算力受限平台上，可采用固定采样方向（如8方向）替代全向搜索，降低VFF计算量。
13. 局部极小值逃逸的参数整定
    随机扰动逃逸的性能高度依赖参数设置：
    停滞检测阈值：速度低于阈值且持续N个控制周期才触发逃逸，N过小导致频繁误触发，N过大导致脱困响应慢。建议N=50_100（即0.51秒）。
    随机扰动幅度：扰动过大导致机器人偏离目标过远，过小无法打破死锁。建议扰动速度为最大速度的10%~20%。
    全局重规划触发条件：VFF连续M次无法提供有效路径时触发A*，M过小导致频繁全局重规划（计算量大），M过大导致长时间卡死。建议M=20~30。
14. 电磁兼容（EMC）设计
    双BLDC电机同时运行会产生严重的电磁干扰：
    电源隔离：电机供电与主控/传感器供电必须通过独立DC-DC模块分开，严禁共用电源。
    信号屏蔽：编码器、IMU等敏感信号线必须使用屏蔽线，并在GPIO输入引脚上串联小电阻进行硬件滤波。
    PCB布局：强电（电机线、电池线）与弱电（信号线）严格分开走线，最好呈90°垂直交叉，模拟地与数字地分离。
15. 安全冗余机制
    双机器人协同运动对安全性要求极高：
    硬件急停：物理急停按钮直接切断电机电源，确保在软件崩溃时也能紧急停车。
    最小安全距离：设置双机之间的最小安全距离阈值（如300mm），低于阈值时强制减速或停机。
    看门狗定时器：加入硬件看门狗，当程序跑飞时自动重启。
    电池电压监控：防止欠压运行导致主控复位或电机失控。
16. 双机协同的队形保持与碰撞避免
    在双机协同运动中，既要保持一定的队形关系，又要避免碰撞：
    虚拟弹簧-阻尼模型：双机之间等效为弹簧-阻尼连接，当距离偏离期望值时，弹簧力将其拉回，同时允许短暂偏离以避免碰撞。
    互斥速度障碍（RVO）：在VFF基础上引入RVO算法，通过计算速度障碍锥预测未来碰撞风险，在速度空间中选取无碰撞的最优速度向量。
    运动学约束：差速驱动的BLDC机器人有非完整约束（不能横向移动），VFF计算的速度指令必须经过运动学可行性校验，避免输出无法执行的速度向量。
17. 传感器噪声对VFF计算的影响
    传感器噪声会导致斥力场计算出现跳变，引发机器人运动抖动：
    卡尔曼滤波：对传感器数据进行卡尔曼滤波，降低噪声对VFF计算的影响。
    斥力场平滑：对斥力矢量进行低通滤波，避免斥力方向突变导致机器人急转。
    多传感器融合：融合超声波、红外、激光雷达等多种传感器数据，通过加权平均降低单一传感器的噪声影响。
    五、系统协同关系总结
    局部传感器感知（超声波/红外/激光雷达 → 障碍物距离与方位）→ VFF力场计算（目标引力 + 障碍物斥力 + 邻居斥力 → 合力矢量）→ 局部极小值检测（速度停滞/振荡检测 → 触发随机扰动或全局重规划）→ 速度/方向指令输出（合力矢量 → 目标速度 + 目标角速度）→ BLDC FOC闭环执行（编码器反馈 + 电流环力矩保护 → 精确跟踪速度指令）→ 分布式通信同步（ESP-NOW/NRF24L01 → 双机状态实时共享）
    三者形成"VFF局部避障 + 随机扰动逃逸 + BLDC高动态执行"的闭环体系——VFF解决"如何实时避开障碍物和邻居"，随机扰动解决"如何打破死锁脱离局部极小值"，BLDC FOC解决"如何精确执行速度指令"。在Arduino有限算力下，这一架构以较低的计算开销实现了双机器人协同的安全、平滑与鲁棒。

1、双机器人VFF基础力场与BLDC差速执行
此案例聚焦于VFF力场的核心构建与执行。将目标点建模为引力场，障碍物（含另一台机器人）建模为斥力场，通过向量合成输出平移速度与转向指令，再通过BLDC差速底盘精准执行。

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

// BLDC电机定义（差速底盘）
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// 超声波传感器（前方避障）
#define TRIG_PIN 12
#define ECHO_PIN 13
NewPing sonar(TRIG_PIN, ECHO_PIN, 200);

// ===== VFF参数 =====
const float GOAL_X = 4.0, GOAL_Y = 0.0;   // 目标点
const float ATTRACT_GAIN = 0.8;            // 引力增益
const float REPULSE_GAIN = 6.0;            // 斥力增益
const float REPULSE_RANGE = 1.0;           // 斥力作用范围(m)

// ===== 机器人状态（简化：仅维护自身位置估算）=====
float selfX = 0.0, selfY = 0.0;
float otherX = 0.0, otherY = 0.0;  // 邻居位置（通过通信获取）

// VFF力场计算
void computeVFF(float& fx, float& fy) {
    // 1. 目标引力
    fx = (GOAL_X - selfX) * ATTRACT_GAIN;
    fy = (GOAL_Y - selfY) * ATTRACT_GAIN;

    // 2. 邻居机器人斥力（防碰撞）
    float dx = selfX - otherX;
    float dy = selfY - otherY;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < REPULSE_RANGE && dist > 0.01) {
        float f = REPULSE_GAIN * (1.0 - dist/REPULSE_RANGE) / (dist + 0.1);
        fx += dx / dist * f;
        fy += dy / dist * f;
    }

    // 3. 超声波障碍斥力
    int obsDist = sonar.ping_cm();
    if (obsDist > 0 && obsDist < REPULSE_RANGE * 100) {
        float obsF = REPULSE_GAIN * (1.0 - obsDist/(REPULSE_RANGE*100)) * 2.0;
        fx -= obsF;  // 假设障碍在正前方，产生向后斥力
    }
}

void setup() {
    Serial.begin(115200);
    // BLDC初始化（速度闭环模式）
    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. 计算VFF合力
    float fx, fy;
    computeVFF(fx, fy);

    // 2. 将合力转换为差速底盘速度指令
    float vLin = constrain(sqrt(fx*fx + fy*fy) * 5, 0, 1.0);
    float vAng = constrain(atan2(fy, fx) * 1.5, -0.8, 0.8);
    float wheelBase = 0.25;

    motorL.target = vLin - vAng * wheelBase / 2;
    motorR.target = vLin + vAng * wheelBase / 2;

    // 3. 执行FOC
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();

    // 4. 简化位置更新（实际应使用编码器里程计）
    selfX += fx * 0.05;
    selfY += fy * 0.05;

    delay(50);
}

关键逻辑：VFF的核心在于将“目标追踪”与“避障”统一为力场合成问题。目标产生引力，障碍物和邻居产生斥力，合力方向即为机器人运动方向。BLDC配合FOC的毫秒级动态响应，能将连续的VFF速度指令精准执行，避免传统有刷电机的“抖动”问题。

2、优先级互斥逃逸（对称死锁破解）
此案例针对VFF的经典缺陷——对称死锁。当两台机器人在狭窄通道中对称相遇时，双方的斥力互相抵消，合力趋近于零，机器人陷入停滞。解决方案是引入优先级机制：优先级低的机器人主动让行，优先级高的保持原路径。

// ===== 机器人优先级与逃逸状态 =====
int selfPriority = 1;     // 自身优先级（0最高）
int otherPriority = 1;    // 邻居优先级（通过通信获取）
bool escapeActive = false;
unsigned long escapeStartTime = 0;
const float ESCAPE_DURATION = 800;  // 逃逸持续时间(ms)

// 邻居距离阈值
const float COLLISION_RISK_DIST = 0.8;  // 近距离风险阈值(m)

void loop() {
    // 1. 计算VFF基础合力（同案例一）
    float fx, fy;
    computeVFF(fx, fy);

    // 2. 检测是否陷入死锁（合力接近零）
    float forceMag = sqrt(fx*fx + fy*fy);
    float distToOther = sqrt(pow(selfX-otherX, 2) + pow(selfY-otherY, 2));

    // ===== 核心：优先级互斥逃逸 =====
    if (distToOther < COLLISION_RISK_DIST) {
        if (selfPriority > otherPriority) {
            // 自身优先级低 → 主动让行，注入逃逸力
            escapeActive = true;
            escapeStartTime = millis();

            // 逃逸方向：垂直于运动方向 + 远离邻居
            float dx = selfX - otherX;
            float dy = selfY - otherY;
            float perpX = -dy / (distToOther + 0.01);
            float perpY = dx / (distToOther + 0.01);

            // 注入垂直方向的逃逸力
            fx += perpX * 1.5;
            fy += perpY * 1.5;

            Serial.println("ESCAPE: Low priority yielding");
        } else {
            // 自身优先级高 → 保持路径，仅微调避让
            // 不注入逃逸力，让低优先级机器人主动让开
            Serial.println("HOLD: High priority keeping path");
        }
    }

    // 3. 逃逸超时自动退出
    if (escapeActive && millis() - escapeStartTime > ESCAPE_DURATION) {
        escapeActive = false;
    }

    // 4. 执行（同案例一）
    // ...
}

关键逻辑：优先级互斥机制通过简单的规则解决了对称死锁——优先级低的机器人主动注入垂直方向的逃逸力，打破受力平衡；优先级高的机器人保持原路径。这种“主从让行”策略比双方都随机扰动更高效，因为它避免了双方同时让行导致的振荡。

3、随机游走逃逸（非对称扰动策略）
此案例针对无法通过优先级解决的非对称死锁（如障碍物布局导致的局部极小值）。当检测到机器人长期停滞（速度持续低于阈值）时，触发随机游走逃逸：在原有VFF合力基础上叠加随机方向的速度扰动，打破局部极小值陷阱。

// ===== 停滞检测与随机逃逸参数 =====
unsigned long stallStartTime = 0;
bool stallDetected = false;
const float STALL_SPEED_THRESHOLD = 0.05;  // 速度低于此值视为停滞
const unsigned long STALL_TIME_THRESHOLD = 1500;  // 持续停滞1.5秒触发逃逸
const float RANDOM_WALK_FORCE = 2.0;       // 随机游走力强度
const unsigned long RANDOM_WALK_DURATION = 1000;  // 随机游走持续时间(ms)

unsigned long randomWalkStartTime = 0;
bool randomWalkActive = false;

void loop() {
    // 1. 计算VFF基础合力（同案例一）
    float fx, fy;
    computeVFF(fx, fy);

    // 2. 估算当前运动速度（基于合力或编码器）
    float currentSpeed = sqrt(fx*fx + fy*fy) * 5;  // 简化估算

    // ===== 核心：停滞检测与随机游走逃逸 =====
    if (currentSpeed < STALL_SPEED_THRESHOLD) {
        // 速度过低，开始计时
        if (!stallDetected) {
            stallDetected = true;
            stallStartTime = millis();
        }

        // 持续停滞超过阈值 → 触发随机游走
        if (millis() - stallStartTime > STALL_TIME_THRESHOLD && !randomWalkActive) {
            randomWalkActive = true;
            randomWalkStartTime = millis();
            Serial.println("RANDOM WALK ESCAPE: Stuck detected, injecting random force");
        }
    } else {
        // 速度正常，重置停滞检测
        stallDetected = false;
        stallStartTime = 0;
    }

    // 3. 随机游走：在合力基础上叠加随机扰动
    if (randomWalkActive) {
        // 生成随机方向的扰动
        float randomAngle = random(0, 628) / 100.0;  // 0~2π
        fx += cos(randomAngle) * RANDOM_WALK_FORCE;
        fy += sin(randomAngle) * RANDOM_WALK_FORCE;

        // 随机游走超时退出
        if (millis() - randomWalkStartTime > RANDOM_WALK_DURATION) {
            randomWalkActive = false;
            stallDetected = false;
            Serial.println("RANDOM WALK ESCAPE: Resuming normal VFF");
        }
    }

    // 4. 执行（同案例一）
    // ...
}

关键逻辑：随机游走逃逸的核心是“在局部极小值处注入非定向扰动”。当VFF合力趋近于零且持续一段时间后，系统判定陷入局部极小值，在原有合力上叠加一个随机方向的大幅扰动。这种随机性打破了对称性，使机器人有机会“跃出”陷阱。随机游走与贪婪下降的结合是随机局部搜索的经典范式——在搜索空间（b）这类锯齿状地形中，一两次随机步就足以逃离局部极小值。

要点解读

1. VFF的核心优势是“力场合成”的自然性与平滑性

VFF将目标追踪和避障统一为向量合成问题。目标产生引力（指向目标），障碍物和邻居产生斥力（远离障碍），合力方向即为运动方向。这种数学建模方式能自然处理多障碍场景，生成平滑的避障轨迹，而非生硬的“急转弯”或“停车-转向-再前进”的离散动作。BLDC配合FOC的连续速度响应能力，是VFF平滑轨迹精准落地的执行保障。

2. 局部极小值死锁是VFF的固有缺陷，必须显式处理

纯VFF算法在对称障碍物或狭窄通道中容易陷入“死锁”——机器人因受力平衡而停滞。案例二和案例三分别展示了两种逃逸策略：优先级互斥适用于双机器人对称相遇场景，通过“主从让行”打破平衡；随机游走适用于单机器人面对复杂障碍布局的场景，通过注入随机扰动跳出陷阱。两种策略可组合使用：优先尝试优先级让行，若让行后仍停滞，则触发随机游走。

3. BLDC+FOC是VFF指令执行的“硬约束”

VFF输出的是连续的速度向量（Vx, Vy）和角速度（Wz），要求执行机构具备高动态响应能力。传统有刷电机难以精确跟踪连续变化的速度指令，容易产生“抖动”或“抽搐”。BLDC配合FOC算法可实现扭矩和转速的毫秒级精确调节，低转速下转矩平滑，完美契合VFF的连续速度指令。

4. 双机器人协同依赖可靠的通信与状态共享

双机器人VFF避障的前提是双方能实时获取对方的位置。案例一中通过通信获取邻居位置，但无线通信的延迟可能导致VFF计算基于过时信息。工程上需在通信协议中附带时间戳，或引入卡尔曼滤波预测对方当前位置。若通信中断，退化为纯反应式避障（仅依赖超声波检测邻居）。

5. 算力与实时性是Arduino平台的硬约束

VFF力场计算、传感器数据处理和BLDC的FOC控制对算力要求较高。标准Arduino Uno难以同时处理这些任务，强烈建议升级至ESP32或Teensy 4.1。控制回路应使用硬件定时器中断或millis()非阻塞定时（建议控制频率≥20Hz），严禁使用delay()函数，以确保通信与电机控制的严格同步。

4、仓储双机器人货物协同搬运——VFF流场避障 + 随机碰撞扰动逃逸
适用场景：仓储货架间的双机器人协同搬运货物，环境中存在动态障碍物（如工作人员、AGV），机器人需保持协作间距，同时应对随机碰撞扰动（如其他AGV刮蹭、货物晃动），确保搬运任务不中断。

核心逻辑：
双机器人VFF流场构建：每个机器人以自身位置为中心建立局部流场，目标点为“引力场”，障碍物为“斥力场”，两个机器人互为“协作约束场”（避免距离过近），流场叠加后生成机器人运动向量；
协作约束避障：通过蓝牙/WiFi实时共享位置信息，在VFF流场中加入协作项，确保两个机器人的间距大于安全阈值，同时规避第三方障碍物；
随机碰撞扰动逃逸：通过IMU检测碰撞产生的姿态角速度突变，触发扰动响应——立即根据扰动方向生成反方向逃逸向量，叠加至VFF流场，同时通过BLDC电流环快速制动，稳定姿态后恢复协作流场。

/* ===== 仓储双机器人协同搬运：VFF流场避障 + 随机碰撞扰动逃逸 =====
 * 硬件：2台ESP32（Arduino兼容）+ BLDC差速底盘 + IMU（MPU6050）+ 超声波+蓝牙
 * 核心：VFF流场生成→协作约束避障→碰撞扰动检测→逃逸向量叠加
 * 注：代码为单机器人端，双机器人通过蓝牙交互位置数据（需另一台同构代码，仅调整角色）
 */
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <SimpleFOC.h>
#include <SoftwareSerial.h>

// --- 硬件定义 ---
BLDCMotor motorL(3), motorR(4);
BLDCDriver3PWM driverL(5,6,7), driverR(8,9,10);
Adafruit_MPU6050 mpu;
SoftwareSerial btSerial(11,12); // 蓝牙通信（双机器人交互）

// --- VFF流场参数 ---
float robotX = 0, robotY = 0;       // 本机位置
float partnerX = 0, partnerY = 0;    // 协作机器人位置
float targetX = 500, targetY = 500;  // 目标货物位置（mm）
float obstacleX = 0, obstacleY = 0; // 第三方障碍物（超声波检测）
float flowFieldX = 0, flowFieldY = 0;// VFF流场向量（X/Y方向）
float safeDist = 200;                // 协作安全距离（mm）

// --- 扰动参数 ---
float gyroZ = 0;            // Z轴角速度（碰撞扰动）
float disturbanceThreshold = 15; // 扰动触发阈值（°/s）
bool isDisturbance = false; // 扰动标志
float escapeDir = 0;        // 逃逸方向（弧度）
float escapeMag = 0.8;      // 逃逸向量幅度

// --- 传感器数据 ---
float distObstacle = 0;     // 障碍物距离（超声波）
#define TRIG_PIN 13, ECHO_PIN 14

void setup() {
    Serial.begin(115200);
    // BLDC初始化
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.init(); motorR.init();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    // IMU初始化
    Wire.begin();
    if (!mpu.begin()) { Serial.println("MPU6050失败"); while(1); }
    // 蓝牙初始化
    btSerial.begin(9600);
    // 超声波初始化
    pinMode(TRIG_PIN, OUTPUT); pinMode(ECHO_PIN, INPUT);
    // 启动电机
    motorL.move(0); motorR.move(0);
    Serial.println("仓储双机器人协同搬运系统启动");
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    // 1. 传感器数据采集：位置、障碍物、姿态
    updateSensors();
    // 2. 蓝牙通信：获取协作机器人位置
    updatePartnerPosition();
    // 3. VFF流场生成：引力场+斥力场+协作约束场
    generateVFFFlowField();
    // 4. 扰动检测与逃逸：随机碰撞触发逃逸
    handleDisturbance();
    // 5. BLDC执行：流场向量转速度指令
    executeMotion();
    delay(20); // 20ms控制周期，保障实时性
}

// --- 传感器数据采集：位置、障碍物、姿态 ---
void updateSensors() {
    // 超声波检测障碍物
    digitalWrite(TRIG_PIN, LOW); delayMicroseconds(2);
    digitalWrite(TRIG_PIN, HIGH); delayMicroseconds(10);
    digitalWrite(TRIG_PIN, LOW);
    distObstacle = pulseIn(ECHO_PIN, HIGH) * 0.0343; // 距离（cm→mm，*10）
    obstacleX = 0; obstacleY = 0; // 简化：前方障碍物，位置设为正前方（需实际标定）
    if (distObstacle < 300) obstacleY = -distObstacle; // 前方障碍物，Y方向为-距离
    
    // IMU检测姿态扰动
    sensors_event_t a, g;
    mpu.getEvent(&a, &g);
    gyroZ = g.gyro.z;
    // 本机位置（编码器/蓝牙定位，简化为固定递增，实际需结合定位模块）
    robotX += flowFieldX * 0.1; robotY += flowFieldY * 0.1;
}

// --- 蓝牙通信：获取协作机器人位置 ---
void updatePartnerPosition() {
    if (btSerial.available()) {
        String data = btSerial.readStringUntil('\n');
        if (data.indexOf("POS") != -1) { // 数据格式：POS,X,Y
            int comma1 = data.indexOf(',');
            int comma2 = data.indexOf(',', comma1+1);
            partnerX = data.substring(comma1+1, comma2).toFloat();
            partnerY = data.substring(comma2+1).toFloat();
            Serial.print("协作机器人位置："); Serial.print(partnerX); Serial.print(","); Serial.println(partnerY);
        }
    }
    // 发送本机位置（每500ms一次）
    if (millis() % 500 == 0) {
        btSerial.print("POS," + String(robotX) + "," + String(robotY) + "\n");
    }
}

// --- VFF流场生成：引力场+斥力场+协作约束场 ---
void generateVFFFlowField() {
    // 1. 引力场（指向目标）
    float attVecX = targetX - robotX;
    float attVecY = targetY - robotY;
    float attMag = sqrt(attVecX*attVecX + attVecY*attVecY);
    float attUnitX = attVecX / attMag;
    float attUnitY = attVecY / attMag;
    float attStrength = map(attMag, 0, 1000, 1, 0.3); // 距离越远，引力越强
    
    // 2. 第三方障碍物斥力场
    float repVecX = robotX - obstacleX;
    float repVecY = robotY - obstacleY;
    float repMag = sqrt(repVecX*repVecX + repVecY*repVecY);
    float repUnitX = 0, repUnitY = 0;
    float repStrength = 0;
    if (distObstacle < 300 && repMag > 0) { // 障碍物在安全距离内
        repUnitX = repVecX / repMag;
        repUnitY = repVecY / repMag;
        repStrength = map(distObstacle, 0, 300, 2, 0); // 距离越近，斥力越强
    }
    
    // 3. 协作约束场（避免与协作机器人距离过近）
    float coVecX = robotX - partnerX;
    float coVecY = robotY - partnerY;
    float coMag = sqrt(coVecX*coVecX + coVecY*coVecY);
    float coUnitX = 0, coUnitY = 0;
    float coStrength = 0;
    if (coMag < safeDist && coMag > 0) { // 距离小于安全阈值，产生斥力
        coUnitX = coVecX / coMag;
        coUnitY = coVecY / coMag;
        coStrength = map(coMag, 0, safeDist, 1.5, 0);
    } else if (coMag > safeDist*1.5) { // 距离过远，产生引力
        coUnitX = -coVecX / coMag;
        coUnitY = -coVecY / coMag;
        coStrength = map(coMag, safeDist*1.5, 1000, 0, 0.5);
    }
    
    // 4. 流场向量叠加
    flowFieldX = attUnitX * attStrength + repUnitX * repStrength + coUnitX * coStrength;
    flowFieldY = attUnitY * attStrength + repUnitY * repStrength + coUnitY * coStrength;
}

// --- 扰动检测与逃逸：随机碰撞触发逃逸 ---
void handleDisturbance() {
    // 扰动检测：Z轴角速度超过阈值（碰撞扰动）
    if (abs(gyroZ) > disturbanceThreshold) {
        isDisturbance = true;
        // 逃逸方向：与扰动角速度方向相反（简化为Z轴正负对应左右逃逸）
        escapeDir = (gyroZ > 0) ? PI/2 : -PI/2; // 顺时针碰撞→左转，逆时针→右转
        Serial.print("检测到碰撞扰动，角速度："); Serial.print(gyroZ); Serial.println("°/s");
    }
    
    if (isDisturbance) {
        // 叠加逃逸向量至流场
        flowFieldX += cos(escapeDir) * escapeMag;
        flowFieldY += sin(escapeDir) * escapeMag;
        Serial.println("执行逃逸向量，流场调整后：X=" + String(flowFieldX) + ", Y=" + String(flowFieldY));
        // 逃逸持续500ms后恢复
        if (millis() % 500 == 0) {
            isDisturbance = false;
            Serial.println("扰动逃逸结束，恢复VFF流场");
        }
    }
}

// --- BLDC执行：流场向量转速度指令 ---
void executeMotion() {
    // 流场向量转底盘速度（简化：X方向为前进，Y方向为转向）
    float speed = flowFieldX * 0.5; // 前进速度（0~0.5对应电机转速）
    float steer = flowFieldY * 2.0; // 转向差速（Y为正→右转，右轮减速）
    // 差速控制
    float leftSpeed = constrain(speed - steer, -0.5, 0.5);
    float rightSpeed = constrain(speed + steer, -0.5, 0.5);
    // 执行速度
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);
}

配套协作机器人代码（核心差异）：仅需修改蓝牙数据格式中的角色标识（如添加ROBOT=1/2），流场生成时将目标点设为“跟随前机”，其他逻辑与主机器人一致，通过蓝牙实时同步位置实现双机约束。

5、电力设备双巡检机器人——VFF区域覆盖避障 + 随机环境扰动逃逸
适用场景：变电站内的双机器人协同巡检电力设备，需覆盖所有设备区域，避免与设备、彼此发生碰撞，同时应对随机环境扰动（如风吹导致的机器人晃动、飞鸟遮挡传感器），确保巡检覆盖率不降低。

核心逻辑：
区域覆盖VFF流场：以巡检区域网格化目标点为基础，构建“覆盖引力场”，同时加入设备障碍物斥力场与双机器人互斥场，引导机器人遍历所有目标点；
随机环境扰动检测：通过IMU检测姿态晃动（加速度突变）、超声波检测传感器遮挡，识别环境扰动，触发逃逸逻辑——暂停区域覆盖，生成扰动反方向逃逸向量，待姿态稳定后返回未覆盖目标点；
区域覆盖补偿：扰动结束后，自动识别已巡检与未巡检区域，调整VFF流场的引力场目标点，优先覆盖未巡检区域，确保巡检完整性。

/* ===== 电力设备双巡检：VFF区域覆盖避障 + 随机环境扰动逃逸 =====
 * 硬件：2台ESP32 + BLDC底盘 + IMU + 超声波 + 避障雷达（简化为多超声波阵列）
 * 核心：区域覆盖流场→环境扰动检测→逃逸恢复→未覆盖区域补偿
 */
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <SimpleFOC.h>
#include <SoftwareSerial.h>

// --- 硬件定义 ---
BLDCMotor motorL(3), motorR(4);
BLDCDriver3PWM driverL(5,6,7), driverR(8,9,10);
Adafruit_MPU6050 mpu;
SoftwareSerial btSerial(11,12);

// --- VFF区域覆盖参数 ---
float robotX = 0, robotY = 0;
float partnerX = 0, partnerY = 0;
// 巡检目标点网格（示例：3x3网格，坐标单位mm）
float targetGrid[9][2] = {
{100,100}, {300,100}, {500,100},
{100,300}, {300,300}, {500,300},
{100,500}, {300,500}, {500,500}
};
bool targetVisited[9] = {false}; // 目标点巡检标记
int currentTarget = 0;            // 当前目标点索引
float safeDist = 250;             // 双机安全距离

// --- 设备障碍物 ---
float deviceObstacles[4][2] = { // 示例：4个电力设备位置
{200,200}, {400,200}, {200,400}, {400,400}
};
float deviceRadius = 80;          // 设备安全半径

// --- 扰动参数 ---
float accelX = 0, accelY = 0;      // 加速度（风吹晃动检测）
float disturbanceThreshold = 2;    // 晃动阈值（m/s²）
bool isSensorBlocked = false;      // 传感器遮挡标志
float escapeAngle = 0;             // 逃逸角度

// --- 传感器数据 ---
float distFront = 0, distLeft = 0, distRight = 0;
#define TRIG_F 13, ECHO_F 14
#define TRIG_L 15, ECHO_L 16
#define TRIG_R 17, ECHO_R 18

void setup() {
    Serial.begin(115200);
    // BLDC初始化
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.init(); motorR.init();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    // IMU初始化
    Wire.begin();
    if (!mpu.begin()) { Serial.println("MPU6050失败"); while(1); }
    // 蓝牙初始化
    btSerial.begin(9600);
    // 超声波初始化
    pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
    pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
    pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);
    Serial.println("电力双巡检系统启动");
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    // 1. 传感器与状态更新
    updateState();
    // 2. 蓝牙交互：获取协作机器人位置与巡检进度
    updatePartnerInfo();
    // 3. VFF区域覆盖流场生成
    generateCoverageVFF();
    // 4. 环境扰动检测与逃逸
    handleEnvironmentDisturbance();
    // 5. 运动执行
    executeMotion();
    delay(20);
}

// --- 状态更新：位置、传感器、巡检进度 ---
void updateState() {
    // 超声波检测
    distFront = pingDistance(TRIG_F, ECHO_F);
    distLeft = pingDistance(TRIG_L, ECHO_L);
    distRight = pingDistance(TRIG_R, ECHO_R);
    // IMU加速度检测（风吹晃动）
    sensors_event_t a, g;
    mpu.getEvent(&a, &g);
    accelX = a.acceleration.x;
    accelY = a.acceleration.y;
    // 位置更新（简化为流场驱动的增量更新）
    robotX += flowFieldX * 0.1;
    robotY += flowFieldY * 0.1;
    // 检测是否到达当前目标点
    if (distToTarget(currentTarget) < 50) {
        targetVisited[currentTarget] = true;
        currentTarget = findNextUnvisitedTarget(); // 寻找下一个未巡检目标
    }
}

// --- 超声波测距辅助函数 ---
float pingDistance(int trig, int echo) {
    digitalWrite(trig, LOW); delayMicroseconds(2);
    digitalWrite(trig, HIGH); delayMicroseconds(10);
    digitalWrite(trig, LOW);
    return pulseIn(echo, HIGH) * 0.0343 * 10; // mm
}

// --- 寻找下一个未巡检目标点 ---
int findNextUnvisitedTarget() {
    for (int i=0; i<9; i++) {
        if (!targetVisited[i]) return i;
    }
    // 所有目标点已巡检，重置（循环巡检）
    memset(targetVisited, 0, sizeof(targetVisited));
    return 0;
}

// --- 计算到目标点的距离 ---
float distToTarget(int idx) {
    float dx = targetGrid[idx][0] - robotX;
    float dy = targetGrid[idx][1] - robotY;
    return sqrt(dx*dx + dy*dy);
}

// --- 蓝牙交互：同步位置与巡检进度 ---
void updatePartnerInfo() {
    if (btSerial.available()) {
        String data = btSerial.readStringUntil('\n');
        if (data.indexOf("INFO") != -1) {
            // 数据格式：INFO,X,Y,target1,target2,...（巡检标记）
            int comma1 = data.indexOf(',');
            int comma2 = data.indexOf(',', comma1+1);
            partnerX = data.substring(comma1+1, comma2).toFloat();
            partnerY = data.substring(comma2+1, data.indexOf(',', comma2+1)).toFloat();
            // 解析巡检标记（简化：仅同步3个标记，实际可序列化数组）
            // ... 省略标记同步逻辑，实际需按协议解析
        }
    }
    // 发送本机信息
    String sendData = "INFO," + String(robotX) + "," + String(robotY);
    for (int i=0; i<9; i++) sendData += "," + String(targetVisited[i]?1:0);
    btSerial.print(sendData + "\n");
}

// --- VFF区域覆盖流场生成 ---
void generateCoverageVFF() {
    // 1. 目标点引力场（覆盖引力）
    float dx = targetGrid[currentTarget][0] - robotX;
    float dy = targetGrid[currentTarget][1] - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    float attUnitX = dx/dist, attUnitY = dy/dist;
    float attStrength = map(dist, 0, 600, 0.3, 1.0);
    
    // 2. 设备障碍物斥力场
    float repSumX = 0, repSumY = 0;
    for (int i=0; i<4; i++) {
        float ddx = robotX - deviceObstacles[i][0];
        float ddy = robotY - deviceObstacles[i][1];
        float d = sqrt(ddx*ddx + ddy*ddy);
        if (d < deviceRadius + 100) { // 障碍物安全距离
            float repStrength = map(d, 0, deviceRadius + 100, 2.0, 0);
            repSumX += (ddx/d) * repStrength;
            repSumY += (ddy/d) * repStrength;
        }
    }
    
    // 3. 双机器人互斥场
    float codx = robotX - partnerX;
    float cody = robotY - partnerY;
    float cod = sqrt(codx*codx + cody*cody);
    float coSumX = 0, coSumY = 0;
    if (cod < safeDist && cod > 0) {
        float coStrength = map(cod, 0, safeDist, 1.5, 0);
        coSumX += (codx/cod) * coStrength;
        coSumY += (cody/cod) * coStrength;
    }
    
    // 4. 流场叠加
    flowFieldX = attUnitX * attStrength + repSumX + coSumX;
    flowFieldY = attUnitY * attStrength + repSumY + coSumY;
}

// --- 环境扰动检测与逃逸 ---
void handleEnvironmentDisturbance() {
    // 扰动1：风吹晃动（加速度突变）
    if ((abs(accelX) > disturbanceThreshold) || (abs(accelY) > disturbanceThreshold)) {
        escapeAngle = atan2(accelY, accelX) + PI; // 反方向逃逸
        Serial.println("检测到风吹晃动，执行逃逸");
        escapeAction(3000); // 逃逸持续3秒
    }
    // 扰动2：传感器遮挡（超声波数据异常）
    if (distFront < 10 || distLeft < 10 || distRight < 10) {
        isSensorBlocked = true;
        escapeAngle = (distFront < 10) ? PI/2 : (distLeft < 10 ? PI : 0); // 遮挡方向反侧
        Serial.println("传感器被遮挡，执行逃逸");
        escapeAction(2000);
    }
}

// --- 逃逸动作执行 ---
void escapeAction(unsigned long duration) {
    // 叠加逃逸向量至流场
    flowFieldX = cos(escapeAngle) * 1.0;
    flowFieldY = sin(escapeAngle) * 1.0;
    // 逃逸期间暂停目标点跟踪
    unsigned long startTime = millis();
    while (millis() - startTime < duration) {
        motorL.move(flowFieldX * 0.5 - flowFieldY * 2.0);
        motorR.move(flowFieldX * 0.5 + flowFieldY * 2.0);
        motorL.loopFOC(); motorR.loopFOC();
    }
    // 逃逸结束，恢复流场
    Serial.println("环境扰动逃逸结束，恢复区域覆盖流场");
}

// --- 运动执行 ---
void executeMotion() {
    float speed = constrain(flowFieldX * 0.5, -0.5, 0.5);
    float steer = constrain(flowFieldY * 2.0, -0.5, 0.5);
    motorL.move(speed - steer);
    motorR.move(speed + steer);
}

6、应急搜救双机器人——VFF动态避障 + 随机突发扰动逃逸
适用场景：地震废墟等应急搜救场景，双机器人协同进入狭窄废墟通道，环境中存在随机突发扰动（如余震碎石滑落、障碍物坍塌），机器人需快速规避动态障碍，同时应对突发扰动避免被困，保障搜救任务持续。

核心逻辑：
动态VFF流场：结合激光雷达（简化为多超声波阵列）构建动态障碍物实时地图，VFF流场实时更新，实现对移动障碍物（如滑落碎石）的快速规避；
双机器人协同拓扑：采用“主从跟随”拓扑，主机负责路径规划，从机通过VFF流场跟随主机，保持安全距离的同时规避动态障碍，提升通道通过效率；
随机突发扰动逃逸：通过电流传感器检测电机负载突变（如被碎石卡住）、IMU检测姿态剧烈变化（如余震倾斜），触发逃逸逻辑——生成与扰动方向相反的逃逸向量，同时切换至“刚性逃逸模式”（全功率输出），摆脱困境后恢复主从协同。

/* ===== 应急搜救双机器人：VFF动态避障 + 随机突发扰动逃逸 =====
 * 硬件：2台ESP32（主/从机）+ BLDC履带底盘 + IMU + 电流传感器 + 多超声波阵列
 * 核心：主从协同拓扑→动态VFF避障→突发扰动检测→刚性逃逸
 * 注：仅提供主机代码，从机代码简化为跟随主机的VFF流场生成，蓝牙同步主机位置
 */
#include <Wire.h>
#include <Adafruit_MPU6050.h>
#include <SimpleFOC.h>
#include <SoftwareSerial.h>

// --- 硬件定义：履带底盘（差速控制） ---
BLDCMotor motorL(3), motorR(4);
BLDCDriver3PWM driverL(5,6,7), driverR(8,9,10);
Adafruit_MPU6050 mpu;
SoftwareSerial btSerial(11,12);

// --- 角色标识：主机（1）、从机（2） ---
#define ROBOT_ROLE 1

// --- VFF动态避障参数 ---
float robotX = 0, robotY = 0;
float followerX = 0, followerY = 0; // 从机位置
float dynamicObstacles[5][2] = {0}; // 动态障碍物（碎石）
int obstacleCount = 0;              // 动态障碍物数量
float escapeZone = 150;             // 障碍物安全距离

// --- 搜救目标与安全区 ---
float searchTargetX = 1000, searchTargetY = 1000; // 搜救目标点
float safeZoneX = 0, safeZoneY = 0;               // 安全区（余震时返回）

// --- 扰动参数 ---
float motorCurrent = 0;            // 电机电流（卡滞检测）
float tiltAngle = 0;               // 倾斜角度（余震检测）
float currentThreshold = 3.0;      // 卡滞电流阈值（A）
float tiltThreshold = 15;          // 倾斜角度阈值（°）
bool isEmergency = false;          // 应急逃逸标志
float emergencyDir = 0;            // 应急逃逸方向

// --- 传感器数据 ---
float distArray[8] = {0};          // 多超声波阵列（8个方向）
#define TRIG_PORTS {13,14,15,16,17,18,19,20}
#define ECHO_PORTS {21,22,23,24,25,26,27,28}
int trigPorts[] = TRIG_PORTS;
int echoPorts[] = ECHO_PORTS;

void setup() {
    Serial.begin(115200);
    // BLDC初始化
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.init(); motorR.init();
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    // IMU初始化
    Wire.begin();
    if (!mpu.begin()) { Serial.println("MPU6050失败"); while(1); }
    // 蓝牙初始化
    btSerial.begin(9600);
    // 超声波阵列初始化
    for (int i=0; i<8; i++) {
        pinMode(trigPorts[i], OUTPUT);
        pinMode(echoPorts[i], INPUT);
    }
    // 电流传感器引脚（A0）
    pinMode(A0, INPUT);
    Serial.println("应急搜救主机启动");
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    // 1. 多传感器数据融合
    updateDynamicState();
    // 2. 蓝牙交互：同步从机位置与状态
    syncFollowerState();
    // 3. VFF动态流场生成（主从协同+动态避障）
    generateDynamicVFF();
    // 4. 突发扰动检测与刚性逃逸
    handleEmergencyDisturbance();
    // 5. 运动执行
    executeMotion();
    delay(15); // 高实时性要求，15ms周期
}

// --- 动态状态更新：位置、障碍物、电流、姿态 ---
void updateDynamicState() {
    // 多超声波阵列检测动态障碍物
    for (int i=0; i<8; i++) {
        distArray[i] = pingDistance(trigPorts[i], echoPorts[i]);
        // 检测新障碍物（距离<200mm且未记录）
        if (distArray[i] < 200) {
            float angle = i * PI/4; // 8个方向，每45°一个
            dynamicObstacles[obstacleCount][0] = robotX + cos(angle) * distArray[i];
            dynamicObstacles[obstacleCount][1] = robotY + sin(angle) * distArray[i];
            obstacleCount = min(obstacleCount + 1, 5);
        }
    }
    // 位置更新（编码器/惯性定位，简化为流场驱动增量）
    robotX += flowFieldX * 0.1;
    robotY += flowFieldY * 0.1;
    // 电流传感器检测卡滞
    motorCurrent = analogRead(A0) * 0.01;
    // IMU检测倾斜
    sensors_event_t a, g;
    mpu.getEvent(&a, &g);
    tiltAngle = atan2(a.acceleration.x, a.acceleration.z) * 180 / PI;
}

// --- 蓝牙同步从机状态 ---
void syncFollowerState() {
    if (btSerial.available()) {
        String data = btSerial.readStringUntil('\n');
        if (data.indexOf("FOLLOWER_POS") != -1) {
            int comma1 = data.indexOf(',');
            int comma2 = data.indexOf(',', comma1+1);
            followerX = data.substring(comma1+1, comma2).toFloat();
            followerY = data.substring(comma2+1).toFloat();
        } else if (data.indexOf("EMERGENCY") != -1) {
            // 从机触发应急，主机协同逃逸
            isEmergency = true;
            emergencyDir = data.substring(data.indexOf(',') + 1).toFloat();
        }
    }
    // 主机发送状态给从机
    if (millis() % 300 == 0) {
        String sendData = "HOST_POS," + String(robotX) + "," + String(robotY);
        sendData += ",CURRENT," + String(motorCurrent) + ",TILT," + String(tiltAngle);
        btSerial.print(sendData + "\n");
    }
}

// --- VFF动态流场生成（主从协同+动态避障） ---
void generateDynamicVFF() {
    // 1. 搜救目标引力场
    float dx = searchTargetX - robotX;
    float dy = searchTargetY - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    float attUnitX = dx/dist, attUnitY = dy/dist;
    float attStrength = map(dist, 0, 1200, 0.3, 1.0);
    
    // 2. 动态障碍物斥力场
    float repSumX = 0, repSumY = 0;
    for (int i=0; i<obstacleCount; i++) {
        float ddx = robotX - dynamicObstacles[i][0];
        float ddy = robotY - dynamicObstacles[i][1];
        float d = sqrt(ddx*ddx + ddy*ddy);
        if (d < escapeZone && d > 0) {
            float repStrength = map(d, 0, escapeZone, 3.0, 0);
            repSumX += (ddx/d) * repStrength;
            repSumY += (ddy/d) * repStrength;
        }
    }
    
    // 3. 从机跟随约束场（主机为从机提供安全距离约束）
    float codx = robotX - followerX;
    float cody = robotY - followerY;
    float cod = sqrt(codx*codx + cody*cody);
    float coSumX = 0, coSumY = 0;
    if (cod < 300 && cod > 150) { // 保持150~300mm的跟随距离
        float coStrength = map(cod, 150, 300, 0.5, 0);
        coSumX -= (codx/cod) * coStrength; // 主机引导从机，产生拉力
        coSumY -= (cody/cod) * coStrength;
    } else if (cod <= 150) { // 距离过近，产生斥力
        float coStrength = map(cod, 0, 150, 1.5, 0);
        coSumX += (codx/cod) * coStrength;
        coSumY += (cody/cod) * coStrength;
    }
    
    // 4. 流场叠加
    flowFieldX = attUnitX * attStrength + repSumX + coSumX;
    flowFieldY = attUnitY * attStrength + repSumY + coSumY;
}

// --- 突发扰动检测与刚性逃逸 ---
void handleEmergencyDisturbance() {
    if (isEmergency) { // 从机触发应急，协同逃逸
        flowFieldX = cos(emergencyDir) * 1.5;
        flowFieldY = sin(emergencyDir) * 1.5;
        if (millis() % 2000 == 0) isEmergency = false;
        return;
    }
    
    // 扰动1：电机卡滞（电流超阈值）
    if (motorCurrent > currentThreshold) {
        emergencyDir = PI; // 向后逃逸（卡滞时后退）
        rigidEscape();
        return;
    }
    // 扰动2：余震倾斜（角度超阈值）
    if (abs(tiltAngle) > tiltThreshold) {
        // 向倾斜反方向逃逸
        emergencyDir = (tiltAngle > 0) ? PI/2 : -PI/2;
        rigidEscape();
        return;
    }
}

// --- 刚性逃逸模式：全功率输出摆脱困境 ---
void rigidEscape() {
    Serial.print("触发刚性逃逸，方向："); Serial.println(emergencyDir * 180 / PI);
    // 叠加逃逸向量，提升力度
    flowFieldX = cos(emergencyDir) * 2.0;
    flowFieldY = sin(emergencyDir) * 2.0;
    // 全功率输出（0.8为电机最大转速，根据实际调整）
    float speed = 0.8;
    float steer = constrain(flowFieldY * 3.0, -0.3, 0.3);
    motorL.move(speed - steer);
    motorR.move(speed + steer);
    // 逃逸持续3秒，期间持续检测扰动
    unsigned long startTime = millis();
    while (millis() - startTime < 3000) {
        motorL.loopFOC(); motorR.loopFOC();
        // 若扰动消除，提前退出
        if (motorCurrent < currentThreshold && abs(tiltAngle) < tiltThreshold) {
            isEmergency = false;
            break;
        }
    }
    // 逃逸结束，恢复动态流场
    if (!isEmergency) {
        Serial.println("刚性逃逸结束，恢复动态避障流场");
    }
}

// --- 运动执行 ---
void executeMotion() {
    float speed = constrain(flowFieldX * 0.5, -0.5, 0.8);
    float steer = constrain(flowFieldY * 3.0, -0.3, 0.3);
    motorL.move(speed - steer);
    motorR.move(speed + steer);
}

// --- 超声波测距辅助函数 ---
float pingDistance(int trig, int echo) {
    digitalWrite(trig, LOW); delayMicroseconds(2);
    digitalWrite(trig, HIGH); delayMicroseconds(10);
    digitalWrite(trig, LOW);
    return pulseIn(echo, HIGH) * 0.0343 * 10;
}

要点解读

1. VFF流场的动态适配：从静态避障到动态协作的核心驱动
   VFF流场法通过“引力场引导目标、斥力场规避障碍、约束场维持协作”的三层结构，是双机器人避障与扰动响应的核心，关键要点包括：
   流场结构的分层设计：必须明确“目标引力场、障碍物斥力场、机器人约束场”的分层逻辑——引力场保证任务导向（如搬运目标、巡检区域），斥力场保障安全距离（第三方障碍、设备、双机器人间距），约束场维持协作关系（避免碰撞、保持跟随）；三层场叠加时需通过权重分配（如斥力场权重>约束场>引力场），确保安全优先；
   动态参数的实时调整：流场参数（引力强度、斥力强度、约束距离）需随环境动态变化——障碍物越近，斥力越强；距离目标越远，引力越强；双机器人距离过近时，约束场权重提升；通过参数映射公式（如距离与强度的反比例关系）实现实时调整，确保流场始终适配当前环境；
   双机器人约束场的定制化：根据协作模式（协同搬运、区域覆盖、主从跟随）设计约束场规则——搬运时为“互斥+平衡”，避免碰撞同时保持协作力；区域覆盖时为“互斥+覆盖补偿”，避免重复巡检；主从跟随时为“引力+斥力”，保持安全跟随距离，避免从机碰撞主机。
2. 随机扰动的多维度检测：从单一信号到多传感器融合的精准识别
   随机扰动具有突发性、多样性（碰撞、环境晃动、传感器遮挡、卡滞）的特点，单一传感器无法全面识别，需通过多维度融合检测，核心要点包括：
   扰动特征的差异化识别：针对不同扰动类型，设计专属检测信号——碰撞扰动用IMU角速度突变，环境晃动用IMU加速度突变，传感器遮挡用超声波数据异常，卡滞用电机电流突变，覆盖所有典型扰动场景；
   阈值自适应调整：扰动阈值需结合场景动态调整——搜救场景的卡滞电流阈值高于仓储场景，废墟中的倾斜角度阈值高于平坦环境；通过环境感知（如IMU检测地面平整度）自动调整阈值，避免误触发或漏触发；
   多传感器融合决策：采用投票机制或加权融合，综合多传感器数据判断扰动——如同时检测到IMU角速度突变和电流突变，才判定为碰撞卡滞，提升检测准确性；同时设置扰动确认时间窗（如连续100ms检测到异常才触发），避免噪声干扰导致误判。
3. 扰动逃逸的向量控制：从被动制动到主动逃逸的策略优化
   扰动逃逸不是简单的停机制动，而是通过向量控制实现主动脱离扰动区域，快速恢复协作，核心要点包括：
   逃逸向量的精准生成：逃逸方向需与扰动方向相反或侧向规避——碰撞扰动时，根据角速度方向生成反方向向量；卡滞时生成向后逃逸向量；传感器遮挡时生成遮挡反侧向量；通过坐标变换（如扰动方向的反方向角计算）实现向量精准生成；
   逃逸力度的动态控制：逃逸力度需匹配扰动强度——强扰动（如严重卡滞、余震倾斜）采用刚性逃逸（全功率输出），弱扰动（如轻微碰撞）采用柔性逃逸（适度调整流场力度）；同时设置力度上限，避免逃逸过程中二次碰撞；
   逃逸与协作的切换逻辑：逃逸期间需暂停正常协作流场，完全执行逃逸向量；待扰动消除后，通过“逃逸确认-流场恢复-协作重启”的分步逻辑，平滑切换回协作模式——先检测扰动是否消除，再恢复原流场，最后同步双机器人位置，避免切换过程中的顿挫或碰撞。
4. 双机器人的通信与协同：从信息共享到协同决策的核心保障
   双机器人的高效避障与扰动响应依赖实时通信与协同决策，核心要点包括：
   轻量化通信协议设计：采用短帧、低频率、结构化的通信协议——如“POS,X,Y”表示位置，“INFO,X,Y,mark1,mark2”表示状态，“EMERGENCY,angle”表示应急，避免数据传输冗余；同时通过时间戳校验，确保数据实时性，避免延迟导致的碰撞；
   协同状态的全局同步：实时同步双机器人的位置、速度、巡检进度、扰动状态——如区域覆盖场景，同步巡检标记，避免重复巡检；主从跟随场景，主机同步自身位置，从机实时跟随；通过同步信息，确保双机器人的决策基于全局状态，而非局部信息；
   故障协同与容错机制：当一个机器人发生扰动，另一个机器人需协同应对——如主机逃逸时，从机暂停跟随，进入等待模式；从机触发应急时，主机调整流场协同逃逸；同时设计主从切换机制，当主机故障时，从机自动切换为主角色，继续任务，保障系统可靠性。
5. BLDC的实时控制与安全保障：从动力执行到扰动响应的底层支撑
   BLDC是机器人运动的动力核心，其控制精度与响应速度直接决定避障与逃逸的落地效果，核心要点包括：
   多闭环控制的协同切换：正常避障时采用速度环控制，保证运动平稳；扰动逃逸时切换至电流环+速度环双闭环，全功率输出实现刚性逃逸；通过控制模式的动态切换，适配不同场景的控制需求——平稳时追求精度，逃逸时追求力量与速度；
   高实时性控制周期保障：避障与扰动响应对控制周期要求极高，需控制在15-20ms以内；通过Arduino的非阻塞编程（避免delay）、定时器中断、硬件加速（如ESP32的双核并行处理），保障控制周期稳定，避免延迟导致的避障失效或逃逸滞后；
   安全与保护机制设计：设置电机过流、过压、过载保护，避免逃逸时的全功率输出导致电机烧毁；设置硬件急停与软件急停双重保障，应对极端失控场景；同时通过电流传感器实时监测电机状态，结合温度传感器防止电机过热，确保机器人在扰动逃逸时的硬件安全。

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