“Arduino BLDC之双机器人RVO（互惠速度障碍）互惠避障与动态规划”代表了多智能体协同控制与底层高性能驱动在复杂动态环境中的深度融合。该系统旨在解决传统避障算法在双机或多机狭路相逢时极易陷入“死锁”或“振荡”的痛点。通过RVO算法实现拟人化的“社交避让”，并结合BLDC（无刷直流）电机的精准执行，系统能够在狭窄通道中实现极其平滑、安全且高效的自主交汇。

一、 主要特点

1. 基于“互惠”哲学的分布式速度仲裁
   传统的速度障碍法（VO）将其他机器人视为不配合的纯障碍物，容易导致迎面而来的双机同时向同一侧避让，从而陷入反复震荡的死锁。RVO（互惠速度障碍法）引入了“社交”假设：假设对方也会承担一半的避让责任。在规划时，每个机器人只需避开由对方产生的速度障碍锥的一半。这种分布式机制使得双机在狭路相逢时能够自然地表现出“右侧通行”等拟人化行为，实现平滑的擦肩而过。
2. 宏观全局规划与微观局部避障的分层架构
   系统采用严密的分层控制设计。A*等全局规划算法作为“宏观引导层”，负责在静态地图上计算出从起点到终点的最优参考路径；而RVO作为“微观局部层”，在滚动窗口内实时接收邻居机器人的状态，预测未来几秒内的碰撞风险，并动态微调全局给出的期望速度，从而实现“宏观最优，微观安全”。
3. 状态高频共享与BLDC的高动态精准执行
   RVO算法的准确性高度依赖数据的实时性。在底层执行端，BLDC电机配合FOC（磁场定向控制）算法，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪RVO输出的高频、连续且微小的速度修正指令，确保机器人在执行复杂协同避障动作时丝滑无顿挫。
   二、 典型应用场景
4. 仓储物流与柔性制造车间
   在电商仓库或工厂通道中，多台AGV/AMR需要频繁在狭窄的货架通道中交汇或相向而行。RVO算法结合BLDC底盘，能够确保机器人在高速运行中实现无碰撞的平滑会车，避免急刹或原地打转，极大提升物流流转效率。
5. 室内服务与导览机器人
   在商场、医院、酒店等人流密集且空间受限的环境中，服务机器人需要频繁应对突然停步的顾客或横穿的其他设备。RVO提供的平滑避让体验，能有效避免剧烈转向带来的乘客不适感或物品倾覆风险。
6. 多智能体协同算法科研与验证
   作为高校与科研机构验证多智能体导航、分布式控制及底层电机耦合特性的理想平台。通过调整RVO的时间视野（Time Horizon）或BLDC的响应带宽，可以直观研究机器人在不同场景下的避障策略与运动平滑度。
   三、 需要注意的关键事项
7. 通信延迟与数据时效性
   RVO算法对通信延迟极其敏感。如果无线通信（如ESP-NOW）延迟过高或丢包，预测的碰撞锥就会失效，导致避障失败甚至发生真实碰撞。必须设计健壮的通信协议，并在软件层加入“通信超时保护”：当超过设定时间未收到邻居数据时，机器人应自动降级为保守的局部避障策略或紧急减速。
8. 算力瓶颈与实时性保障
   在滚动窗口内进行RVO的速度空间求解（通常涉及KD-Tree搜索与线性规划）对浮点运算能力要求较高。标准的Arduino Uno难以胜任高频的RVO解算。强烈建议采用ESP32、STM32等高性能MCU，或使用“上位机+下位机”架构。同时，严禁在主循环中使用阻塞函数，必须保证控制周期的绝对稳定。
9. 运动学约束与轨迹平滑性
   RVO输出的最优速度指令必须严格受限于BLDC底盘的物理运动学约束（如最大线速度、角速度及加速度限制）。在下发指令前，必须进行S曲线加减速平滑处理，防止阶跃指令导致电机过流或轮胎打滑。此外，需精细调优RVO参数，避免在狭窄通道中因过度避让而产生不必要的绕行。
10. 硬件级安全冗余与EMC防护
    多机协同场景下，BLDC电机的高频启停会产生强烈的电磁干扰（EMC），极易干扰无线通信模块。必须做好严格的电源隔离与信号屏蔽。同时，软件层面的RVO不能作为唯一的安全保障，必须保留硬件级急停按钮、过流保护以及物理防撞条，防止因算法死锁或传感器失效导致的严重碰撞事故。

1、双机器人RVO互惠避障——差速底盘相向会车
适用场景：狭窄走廊/通道中两台机器人相向而行，需要互惠避让避免“死锁”。

核心逻辑：每台机器人通过无线通信（ESP-NOW）获取对方的位置和速度向量。主循环中，先按全局路径（如A*给出的方向）计算期望速度，再对每个邻居执行RVO碰撞预测——若预测会发生碰撞，则计算互惠速度修正向量，将期望速度调整到RVO锥之外。RVO的核心假设是双方各承担一半避让责任，因此避障行为比传统VO更自然平滑。

#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>

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

// ==================== 智能体状态结构体 ====================
struct AgentState {
    float x, y;        // 全局坐标 (m)
    float vx, vy;      // 速度向量 (m/s)
    float radius;      // 机器人半径 (m)
    uint8_t id;
};

AgentState self = {0, 0, 0, 0, 0.3, 1};
AgentState neighbor = {0, 0, 0, 0, 0.3, 2};

// ==================== RVO参数 ====================
const float TIME_HORIZON = 2.0;     // 预测时间窗口 (秒)
const float MAX_SPEED = 1.0;        // 最大线速度 (m/s)

void setup() {
    Serial.begin(115200);
    // 初始化电机与FOC (略)
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();

    // 初始化ESP-NOW通信 (略)
    // 注册接收回调函数
    esp_now_register_recv_cb(onDataRecv);
}

// ==================== ESP-NOW接收回调 ====================
void onDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
    memcpy(&neighbor, incomingData, sizeof(neighbor));
    // 时间戳补偿：实际应用中需根据延迟预测邻居当前位置
}

// ==================== RVO核心计算 ====================
struct RVOResult {
    float vx, vy;
    bool collisionRisk;
};

RVOResult computeRVO(AgentState self, AgentState other, float prefVx, float prefVy) {
    RVOResult result = {prefVx, prefVy, false};
    
    // 相对位置与速度
    float dx = other.x - self.x;
    float dy = other.y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    float dvx = self.vx - other.vx;
    float dvy = self.vy - other.vy;
    
    // 若距离大于安全阈值，不触发RVO
    if (dist > 3.0) return result;
    
    // 预测相对位置 (假设双方匀速)
    float futureDx = dx + dvx * TIME_HORIZON;
    float futureDy = dy + dvy * TIME_HORIZON;
    float futureDist = sqrt(futureDx*futureDx + futureDy*futureDy);
    
    // 若预测距离小于两机器人半径之和，触发避让
    float minDist = self.radius + other.radius;
    if (futureDist < minDist * 1.2) {
        result.collisionRisk = true;
        
        // RVO核心：向垂直于相对速度方向施加修正，且双方各承担一半
        float angleToOther = atan2(dy, dx);
        float avoidanceAngle = angleToOther + PI/2; // 垂直方向
        
        // 互惠修正：取期望速度与避让方向的加权平均
        float weight = 0.5; // RVO互惠因子
        float rvoVx = prefVx + weight * cos(avoidanceAngle) * 0.5;
        float rvoVy = prefVy + weight * sin(avoidanceAngle) * 0.5;
        
        // 限制速度幅值
        float speed = sqrt(rvoVx*rvoVx + rvoVy*rvoVy);
        if (speed > MAX_SPEED) {
            rvoVx = rvoVx / speed * MAX_SPEED;
            rvoVy = rvoVy / speed * MAX_SPEED;
        }
        result.vx = rvoVx;
        result.vy = rvoVy;
    }
    return result;
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. 更新自身位置 (来自里程计) ====================
    // self.x, self.y 通过编码器积分获得 (略)
    
    // ==================== 2. 计算期望速度 (来自全局路径规划) ====================
    float prefVx = 0.5;  // 假设向右前进
    float prefVy = 0.0;
    
    // ==================== 3. 执行RVO修正 ====================
    RVOResult rvo = computeRVO(self, neighbor, prefVx, prefVy);
    
    // ==================== 4. 差速驱动 ====================
    float vLin = sqrt(rvo.vx*rvo.vx + rvo.vy*rvo.vy);
    float vAng = atan2(rvo.vy, rvo.vx);
    float wheelBase = 0.25;
    motorL.move(vLin - vAng * wheelBase / 2);
    motorR.move(vLin + vAng * wheelBase / 2);
    
    // ==================== 5. 广播自身状态 ====================
    // esp_now_send(broadcastMac, (uint8_t*)&self, sizeof(self));
    
    delay(50);
}

2、双UWB仓储双机器人——任务协同+动态避碰
适用场景：智能仓储中两台AGV协同补货，需在窄通道内错车且避免“抢料”。

核心逻辑：每台机器人通过双UWB标签实现高精度全局定位（精度10~30cm）和机器人间相对测距，无需中央服务器中转。当UWB测距小于安全阈值（如1.5m）时触发VO/RVO局部避碰。任务协同采用“距离任务点近者优先+电量高者优先”的协商策略，当一台机器人任务受阻时，可将任务ID广播让另一台“窃取”。

#include <SimpleFOC.h>
#include <DW1000.h>  // UWB定位库
#include <esp_now.h>

// ==================== BLDC差速电机 (同案例一) ====================
// ... (电机定义略)

// ==================== UWB定位 ====================
// 每个机器人通过UWB标签与场地锚点通信，解算全局坐标
float selfX_uwb = 0, selfY_uwb = 0;
float otherX_uwb = 0, otherY_uwb = 0;

// ==================== 任务协同变量 ====================
struct Task {
    int id;
    float targetX, targetY;
    bool isActive;
};
Task myTask = {1, 5.0, 3.0, true};
Task otherTask = {2, 2.0, 4.0, true};

// ==================== 任务协商 ====================
void negotiateTasks() {
    float myDist = sqrt(pow(myTask.targetX - selfX_uwb, 2) + 
                        pow(myTask.targetY - selfY_uwb, 2));
    float otherDist = sqrt(pow(otherTask.targetX - otherX_uwb, 2) + 
                           pow(otherTask.targetY - otherY_uwb, 2));
    
    // 距离任务点近的机器人优先执行该任务
    if (otherDist < myDist && myTask.isActive) {
        // 广播任务移交请求
        // esp_now_send(...);
        myTask.isActive = false;
    }
}

void setup() {
    Serial.begin(115200);
    // 初始化电机、FOC、UWB模块、ESP-NOW (略)
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. 更新UWB定位 ====================
    // updateUWBPosition(&selfX_uwb, &selfY_uwb);
    // 接收邻居UWB坐标 (通过ESP-NOW)
    
    // ==================== 2. 任务协商 ====================
    negotiateTasks();
    
    // ==================== 3. UWB测距避碰 ====================
    float dx = otherX_uwb - selfX_uwb;
    float dy = otherY_uwb - selfY_uwb;
    float dist = sqrt(dx*dx + dy*dy);
    const float SAFE_DIST = 1.5;  // 安全距离
    
    float targetVx = 0.3, targetVy = 0.0; // 任务方向
    
    if (dist < SAFE_DIST) {
        // 触发VO避让：向远离对方的方向修正
        float angleToOther = atan2(dy, dx);
        float avoidVx = -cos(angleToOther) * 0.3;
        float avoidVy = -sin(angleToOther) * 0.3;
        targetVx += avoidVx;
        targetVy += avoidVy;
    }
    
    // ==================== 4. 差速驱动 ====================
    float vLin = sqrt(targetVx*targetVx + targetVy*targetVy);
    float vAng = atan2(targetVy, targetVx);
    float wheelBase = 0.3;
    motorL.move(vLin - vAng * wheelBase / 2);
    motorR.move(vLin + vAng * wheelBase / 2);
    
    delay(50);
}

3、VFF局部极小逃逸 + 弹性圆形编队
适用场景：三台以上机器人在U型障碍或对称布局中陷入局部极小（如两两相对“顶牛”），需通过附加旋转力场脱离死锁。

核心逻辑：采用VFF（虚拟力场）控制——目标引力吸引机器人前进，机器人间斥力和障碍物斥力防止碰撞。但VFF在对称障碍中易陷入局部极小（合力为零）。解决方案：检测到机器人速度长期低于阈值（停滞），激活“附加旋转力场”，在合力方向叠加一个垂直于当前方向的旋转分量，引导机器人沿障碍边缘滑出死锁。编队采用“虚拟弹簧”模型，弹性保持队形。

#include <SimpleFOC.h>
#include <math.h>

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
// ... (驱动器和编码器初始化略)

// ==================== 三机器人V形编队 ====================
#define NUM_ROBOTS 3
struct RobotState {
    float x, y, vx, vy;
    bool stuck;
};
RobotState robots[NUM_ROBOTS] = {
    {0, 0, 0, 0, false},
    {-0.5, 0.5, 0, 0, false},
    {0.5, 0.5, 0, 0, false}
};

// ==================== VFF参数 ====================
const float ATTRACT_GAIN = 0.015;
const float REPULSE_ROBOT = 8.0;
const float REPULSE_RANGE = 1.0;
const float STUCK_SPEED = 0.03;      // 停滞速度阈值
const float ROTATION_GAIN = 0.8;     // 旋转力场增益

// 各机器人的编队目标偏移
float formationOffsets[NUM_ROBOTS][2] = {
    {0, 0},
    {-0.5, 0.5},
    {0.5, 0.5}
};

void setup() {
    Serial.begin(115200);
    // 初始化电机与FOC (略)
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    int id = 0;  // 控制机器人0，其他机器人同理
    
    // ==================== 1. VFF力场计算 ====================
    float fx = 0, fy = 0;
    
    // 1.1 目标引力 (指向编队目标点)
    float targetX = 5.0, targetY = 3.0;
    fx += (targetX + formationOffsets[id][0] - robots[id].x) * ATTRACT_GAIN;
    fy += (targetY + formationOffsets[id][1] - robots[id].y) * ATTRACT_GAIN;
    
    // 1.2 机器人间斥力 (编队保持)
    for (int j = 0; j < NUM_ROBOTS; j++) {
        if (j == id) continue;
        float dx = robots[id].x - robots[j].x;
        float dy = robots[id].y - robots[j].y;
        float dist = sqrt(dx*dx + dy*dy);
        if (dist < REPULSE_RANGE && dist > 0.01) {
            float f = REPULSE_ROBOT * (1.0 - dist/REPLUSE_RANGE) / (dist + 0.05);
            fx += f * dx / dist;
            fy += f * dy / dist;
        }
    }
    
    // 1.3 模拟障碍物斥力 (U型障碍)
    // ... (省略)
    
    // ==================== 2. 停滞检测 ====================
    float speed = sqrt(robots[id].vx*robots[id].vx + robots[id].vy*robots[id].vy);
    if (speed < STUCK_SPEED) {
        robots[id].stuck = true;
    } else {
        robots[id].stuck = false;
    }
    
    // ==================== 3. 附加旋转力场逃逸 ====================
    if (robots[id].stuck) {
        // 在合力方向叠加垂直旋转分量
        float forceAngle = atan2(fy, fx);
        float rotX = -sin(forceAngle) * ROTATION_GAIN;
        float rotY = cos(forceAngle) * ROTATION_GAIN;
        fx += rotX;
        fy += rotY;
    }
    
    // ==================== 4. 差速驱动 ====================
    float vLin = constrain(sqrt(fx*fx + fy*fy) * 5, 0, 0.8);
    float vAng = constrain(atan2(fy, fx) * 1.5, -0.6, 0.6);
    float wheelBase = 0.25;
    motorL.move(vLin - vAng * wheelBase / 2);
    motorR.move(vLin + vAng * wheelBase / 2);
    
    // 更新自身位置 (简化积分)
    robots[id].x += fx * 0.05;
    robots[id].y += fy * 0.05;
    robots[id].vx = fx;
    robots[id].vy = fy;
    
    delay(50);
}

要点解读
1、RVO的核心价值：化解“死锁”，实现互惠避让
传统速度障碍法（VO）将其他机器人视为“不配合”的静态障碍，容易导致两机器人同时向同侧躲避，陷入反复震荡的“死锁”。RVO（互惠速度障碍法）假设双方各承担一半避让责任，通过计算互惠速度集合，让多机器人在狭窄通道会车时表现出更拟人化的“靠右通行”行为。实现的关键是每个机器人需通过无线通信高频获取邻居的实时速度向量。

2、通信延迟是协同的头号杀手，需时间戳补偿
多机协同的最大痛点是：机器人感知到的邻居位置永远是“过去时”。工程对策包括：在通信协议中附带时间戳，接收方用延迟时间预测对方当前状态；采用TDMA时分多址确保关键信息高概率送达；通信中断超过阈值时，机器人自动进入“安全悬停”或独立沿墙探索模式。

3、分层规划架构：A*“管宏观”，VO/RVO“管微观”
多智能体协同避障的标准架构是分层设计——全局层（如A*）在静态地图上规划最优路径；局部层（VO/RVO）实时响应动态环境，微调全局路径的期望速度。两层解耦后，全局规划可以降低频率（如10Hz），局部避障保持高频（如100Hz），既保证宏观最优，又确保微观实时安全，8位MCU也能跑起来。

4、局部极小逃逸：VFF的固有缺陷与旋转力场修复
人工势场法（VFF/VFH）天然存在局部极小值陷阱——当目标引力和障碍物斥力在U型障碍中恰好抵消时，机器人会停滞不前。解决方案包括：停滞检测（速度低于阈值持续数秒）+ 附加旋转力场（在合力方向叠加垂直分量），引导机器人沿障碍边缘滑出死锁；或采用随机游走扰动注入。

5、BLDC FOC是协同避障的物理执行保障
VO/RVO/VFF算法会频繁输出微小连续的速度向量变化（如从直行突然变为斜向低速），普通电机难以精准响应这种高频微调。SimpleFOC库的磁场定向控制（FOC）能毫秒级精准响应速度和扭矩指令，使机器人在执行协同避障时“丝滑”无顿挫。紧急制动时，BLDC的再生制动可将制动距离显著缩短——这在多机密集场景中是“生死之分”。

4、双机器人同步跟随与RVO互惠避障（工厂物料搬运场景）
适用场景：工厂车间两条并行的物流通道中，两台机器人需同步跟随同一移动载具（如AGV小车），在跟随过程中，双机需保持固定间距，同时通过RVO算法规避通道内的临时障碍（如人员、固定货架），避免双机路径交叉与相互干扰，确保同步跟随的稳定性。

// 核心库引入（BLDC控制+串口通信+RVO算法+数学运算）
#include <SimpleFOC.h>
#include <WirelessSerial.h> // 无线通信（适配HC-05/HC-06蓝牙模块，或用有线串口替代）
#include <math.h>

// 双机硬件引脚定义（左右机器人的BLDC电机接口）
// 机器人1（主机，承担主导权）
#define ROBOT1_LEFT_PWM 9
#define ROBOT1_LEFT_IN1 10
#define ROBOT1_LEFT_IN2 11
#define ROBOT1_RIGHT_PWM 5
#define ROBOT1_RIGHT_IN1 6
#define ROBOT1_RIGHT_IN2 7
#define ROBOT1_LEFT_ENC_A 2
#define ROBOT1_LEFT_ENC_B 3
#define ROBOT1_RIGHT_ENC_A 4
#define ROBOT1_RIGHT_ENC_B 12
#define ROBOT1_SERIAL_TX 11  // 无线发送引脚
#define ROBOT1_SERIAL_RX 10  // 无线接收引脚

// 机器人2（从机，同步跟随主机）
#define ROBOT2_LEFT_PWM 9
#define ROBOT2_LEFT_IN1 10
#define ROBOT2_LEFT_IN2 11
#define ROBOT2_RIGHT_PWM 5
#define ROBOT2_RIGHT_IN1 6
#define ROBOT2_RIGHT_IN2 7
#define ROBOT2_LEFT_ENC_A 2
#define ROBOT2_LEFT_ENC_B 3
#define ROBOT2_RIGHT_ENC_A 4
#define ROBOT2_RIGHT_ENC_B 12
#define ROBOT2_SERIAL_TX 11
#define ROBOT2_SERIAL_RX 10

// RVO同步跟随核心参数
const float WHEEL_RADIUS = 50.0;       // 轮子半径（mm）
const float ROBOT_WIDTH = 200.0;       // 机器人轮距（mm）
const float ENC_PULSES_PER_REV = 1000.0; // 编码器每转脉冲数
const float TARGET_FOLLOW_DIST = 600.0; // 同步跟随目标间距（mm）
const float FOLLOW_KP = 0.6;           // 跟随距离比例系数
const float FOLLOW_KI = 0.1;           // 跟随距离积分系数
const float MAX_LINEAR_V = 180.0;      // 最大线速度（mm/s）
const float RVO_SAFE_DIST = 300.0;     // RVO避障安全距离（mm）
const float INTER_ROBOT_SAFE_DIST = 400.0; // 双机安全距离（mm）

// 双机状态结构体（通信同步的核心数据）
struct DualRobotState {
  // 当前位置（简化为X-Y平面坐标，单位mm）
  float robot1_x, robot1_y;
  float robot2_x, robot2_y;
  // 当前速度（线速度+角速度，单位mm/s、rad/s）
  float robot1_v, robot1_w;
  float robot2_v, robot2_w;
  // 跟随目标坐标
  float target_x, target_y;
  // 障碍信息（简化为障碍坐标与半径）
  struct Obstacle { float x, y, r; } obstacle;
} dualState;

// 硬件对象实例化（以机器人1为例，机器人2同理）
BLDCMotor robot1LeftMotor = BLDCMotor(11);
BLDCMotor robot1RightMotor = BLDCMotor(11);
BLDCDriver2PWM robot1LeftDriver = BLDCDriver2PWM(ROBOT1_LEFT_PWM, ROBOT1_LEFT_IN1, ROBOT1_LEFT_IN2);
BLDCDriver2PWM robot1RightDriver = BLDCDriver2PWM(ROBOT1_RIGHT_PWM, ROBOT1_RIGHT_IN1, ROBOT1_RIGHT_IN2);
Encoder robot1LeftEnc(ROBOT1_LEFT_ENC_A, ROBOT1_LEFT_ENC_B);
Encoder robot1RightEnc(ROBOT1_RIGHT_ENC_A, ROBOT1_RIGHT_ENC_B);
WirelessSerial robot1Com; // 机器人1通信对象

BLDCMotor robot2LeftMotor = BLDCMotor(11);
BLDCMotor robot2RightMotor = BLDCMotor(11);
BLDCDriver2PWM robot2LeftDriver = BLDCDriver2PWM(ROBOT2_LEFT_PWM, ROBOT2_LEFT_IN1, ROBOT2_LEFT_IN2);
BLDCDriver2PWM robot2RightDriver = BLDCDriver2PWM(ROBOT2_RIGHT_PWM, ROBOT2_RIGHT_IN1, ROBOT2_RIGHT_IN2);
Encoder robot2LeftEnc(ROBOT2_LEFT_ENC_A, ROBOT2_LEFT_ENC_B);
Encoder robot2RightEnc(ROBOT2_RIGHT_ENC_A, ROBOT2_RIGHT_ENC_B);
WirelessSerial robot2Com; // 机器人2通信对象

// 辅助函数：速度转RPM
float linearVToRpm(float linearV) {
  return (linearV * 60.0) / (2.0 * M_PI * WHEEL_RADIUS);
}

// 辅助函数：线速度与角速度转左右轮速度
void motionToWheelVelocity(float v, float w, float &leftV, float &rightV) {
  leftV = v - w * ROBOT_WIDTH / 2.0;
  rightV = v + w * ROBOT_WIDTH / 2.0;
}

// 辅助函数：计算两点间距离
float distanceBetween(float x1, float y1, float x2, float y2) {
  return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

// RVO互惠避障核心算法：计算双机避障后的期望速度
void rvoCalculate(float &selfV, float &selfW, float other_x, float other_y, float other_vx, float other_vy, float safeDist) {
  // 计算双机相对位置与相对速度
  float dx = other_x - dualState.robot1_x;
  float dy = other_y - dualState.robot1_y;
  float dist = distanceBetween(dualState.robot1_x, dualState.robot1_y, other_x, other_y);

  // 若距离小于安全距离，启动RVO避障
  if (dist < safeDist) {
    // 计算单位相对位置向量
    float unit_dx = dx / dist;
    float unit_dy = dy / dist;
    // 计算期望避障偏移（沿相对位置反方向偏移）
    float avoidOffset = (safeDist - dist) / 2.0;
    // 修正自身线速度，向偏移方向微调
    selfV -= avoidOffset * 0.02;
    // 调整角速度，转向避障方向
    selfW += atan2(unit_dy, unit_dx) * 0.3;
    // 速度限幅
    selfV = constrain(selfV, -MAX_LINEAR_V, MAX_LINEAR_V);
    selfW = constrain(selfW, -2.0, 2.0); // 角速度限幅
  }
}

// 同步跟随控制：计算主机期望速度，从机同步跟随
void syncFollowControl() {
  // 计算双机到目标的距离
  float dist1 = distanceBetween(dualState.robot1_x, dualState.robot1_y, dualState.target_x, dualState.target_y);
  float dist2 = distanceBetween(dualState.robot2_x, dualState.robot2_y, dualState.target_x, dualState.target_y);

  // 主机跟随目标速度
  static float integral1 = 0.0, integral2 = 0.0;
  float error1 = dist1 - TARGET_FOLLOW_DIST;
  float error2 = dist2 - TARGET_FOLLOW_DIST * 2.0; // 从机保持更远的间距

  // 主机速度计算（PID控制）
  integral1 += error1 * FOLLOW_KI;
  integral1 = constrain(integral1, -100.0, 100.0);
  float baseV1 = error1 * FOLLOW_KP + integral1;
  dualState.robot1_v = constrain(baseV1, 30.0, MAX_LINEAR_V);
  dualState.robot1_w = (atan2(dualState.target_y - dualState.robot1_y, dualState.target_x - dualState.robot1_x) * 180.0 / M_PI) * 0.05;

  // 从机同步跟随主机速度
  float distInter = distanceBetween(dualState.robot1_x, dualState.robot1_y, dualState.robot2_x, dualState.robot2_y);
  float errorInter = distInter - TARGET_FOLLOW_DIST;
  integral2 += errorInter * FOLLOW_KI;
  integral2 = constrain(integral2, -100.0, 100.0);
  dualState.robot2_v = dualState.robot1_v + errorInter * FOLLOW_KP;
  dualState.robot2_v = constrain(dualState.robot2_v, 30.0, MAX_LINEAR_V);
  dualState.robot2_w = dualState.robot1_w + (atan2(dualState.robot1_y - dualState.robot2_y, dualState.robot1_x - dualState.robot2_x) * 180.0 / M_PI) * 0.03;
}

// 初始化函数
void setup() {
  Serial.begin(115200);
  // 初始化机器人1通信
  robot1Com.begin(9600, ROBOT1_SERIAL_TX, ROBOT1_SERIAL_RX);
  // 初始化机器人2通信
  robot2Com.begin(9600, ROBOT2_SERIAL_TX, ROBOT2_SERIAL_RX);

  // 初始化机器人1电机与编码器
  robot1LeftDriver.init();
  robot1RightDriver.init();
  robot1LeftMotor.linkDriver(&robot1LeftDriver);
  robot1RightMotor.linkDriver(&robot1RightDriver);
  robot1LeftMotor.init();
  robot1RightMotor.init();
  robot1LeftMotor.initFOC();
  robot1RightMotor.initFOC();
  robot1LeftMotor.controller = MotionControlType::velocity;
  robot1RightMotor.controller = MotionControlType::velocity;
  robot1LeftEnc.init();
  robot1RightEnc.init();
  robot1LeftMotor.linkSensor(&robot1LeftEnc);
  robot1RightMotor.linkSensor(&robot1RightEnc);

  // 初始化机器人2电机与编码器（操作同机器人1，略）
  robot2LeftDriver.init();
  robot2RightDriver.init();
  robot2LeftMotor.linkDriver(&robot2LeftDriver);
  robot2RightMotor.linkDriver(&robot2RightDriver);
  robot2LeftMotor.init();
  robot2RightMotor.init();
  robot2LeftMotor.initFOC();
  robot2RightMotor.initFOC();
  robot2LeftMotor.controller = MotionControlType::velocity;
  robot2RightMotor.controller = MotionControlType::velocity;
  robot2LeftEnc.init();
  robot2RightEnc.init();
  robot2LeftMotor.linkSensor(&robot2LeftEnc);
  robot2RightMotor.linkSensor(&robot2RightEnc);

  // 初始化双机状态
  dualState = {0.0, 0.0, 500.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000.0, 1000.0, {0.0, 0.0, 0.0}};

  Serial.println("双机器人同步跟随与RVO避障初始化完成");
}

void loop() {
  // 1. 同步双机状态：机器人1发送自身状态，机器人2发送自身状态，互相接收
  // 简化为机器人1主导，机器人2同步，实际可根据需要扩展双向同步
  robot1Com.println("1," + String(dualState.robot1_x) + "," + String(dualState.robot1_y) + "," + String(dualState.robot1_v) + "," + String(dualState.robot1_w));
  robot2Com.println("2," + String(dualState.robot2_x) + "," + String(dualState.robot2_y) + "," + String(dualState.robot2_v) + "," + String(dualState.robot2_w));

  // 2. 解析对方状态（简化逻辑，实际需完善通信协议解析）
  delay(10); // 通信延迟缓冲

  // 3. 同步跟随控制：计算双机期望速度
  syncFollowControl();

  // 4. RVO互惠避障：双机之间、双机与障碍之间的避障
  rvoCalculate(dualState.robot1_v, dualState.robot1_w, dualState.robot2_x, dualState.robot2_y, 0, 0, INTER_ROBOT_SAFE_DIST);
  rvoCalculate(dualState.robot2_v, dualState.robot2_w, dualState.robot1_x, dualState.robot1_y, 0, 0, INTER_ROBOT_SAFE_DIST);
  // 与障碍的避障（简化，假设障碍静态）
  if (dualState.obstacle.r > 0) {
    rvoCalculate(dualState.robot1_v, dualState.robot1_w, dualState.obstacle.x, dualState.obstacle.y, 0, 0, RVO_SAFE_DIST);
    rvoCalculate(dualState.robot2_v, dualState.robot2_w, dualState.obstacle.x, dualState.obstacle.y, 0, 0, RVO_SAFE_DIST);
  }

  // 5. 速度转左右轮速度，控制电机
  float robot1LeftV, robot1RightV, robot2LeftV, robot2RightV;
  motionToWheelVelocity(dualState.robot1_v, dualState.robot1_w, robot1LeftV, robot1RightV);
  motionToWheelVelocity(dualState.robot2_v, dualState.robot2_w, robot2LeftV, robot2RightV);

  robot1LeftMotor.target = linearVToRpm(robot1LeftV);
  robot1RightMotor.target = linearVToRpm(robot1RightV);
  robot2LeftMotor.target = linearVToRpm(robot2LeftV);
  robot2RightMotor.target = linearVToRpm(robot2RightV);

  robot1LeftMotor.move(50);
  robot1RightMotor.move(50);
  robot2LeftMotor.move(50);
  robot2RightMotor.move(50);

  // 6. 更新位置状态（简化，实际需结合编码器数据实时更新）
  dualState.robot1_x += dualState.robot1_v * cos(dualState.robot1_w) * 0.05;
  dualState.robot1_y += dualState.robot1_v * sin(dualState.robot1_w) * 0.05;
  dualState.robot2_x += dualState.robot2_v * cos(dualState.robot2_w) * 0.05;
  dualState.robot2_y += dualState.robot2_v * sin(dualState.robot2_w) * 0.05;

  // 7. 输出状态
  Serial.print("R1:(" + String(dualState.robot1_x,1) + "," + String(dualState.robot1_y,1) + ")V=" + String(dualState.robot1_v,1));
  Serial.print(" | R2:(" + String(dualState.robot2_x,1) + "," + String(dualState.robot2_y,1) + ")V=" + String(dualState.robot2_v,1));
  Serial.println(" | 双机距离：" + String(distanceBetween(dualState.robot1_x, dualState.robot1_y, dualState.robot2_x, dualState.robot2_y),1));

  delay(50);
}

代码逻辑说明：
核心融合点：通过串口通信实现双机状态实时同步，主机承担主导跟随任务，从机同步跟随主机轨迹；结合RVO算法，在双机间距不足或遇到障碍时，动态调整双机的速度与角速度，实现互惠避障，既避免双机碰撞，又保证跟随效率；
核心逻辑：双机状态同步→同步跟随速度计算→RVO避障调整→速度分配至电机→位置状态更新，形成完整的协同控制闭环；
扩展性：可增加多传感器融合（如激光雷达、超声波），提升障碍检测精度；优化通信协议，实现更高效的双机状态同步，适配更复杂的工厂通道环境。

5、双机器人动态任务分配与RVO+动态规划路径（仓储分拣场景）
适用场景：仓储多库区场景中，两台机器人需协同完成动态生成的分拣任务（如不同货架的货物取放），任务随订单实时更新，双机需通过动态规划实现任务的高效分配，同时结合RVO算法规避路径交叉，确保在动态任务环境下，双机既不碰撞，又能最大化分拣效率，适配仓储物流的实时性需求。

// 核心库引入
#include <SimpleFOC.h>
#include <WirelessSerial.h>
#include <vector>
#include <math.h>

// 硬件引脚定义（同案例1，略，可根据实际调整）
// 此处仅保留核心参数与结构体定义，硬件初始化代码同案例1，后续省略重复部分

// 动态规划与任务核心参数
const float WHEEL_RADIUS = 50.0;
const float ROBOT_WIDTH = 200.0;
const float ENC_PULSES_PER_REV = 1000.0;
const float MAX_LINEAR_V = 150.0;
const float ROBOT_COST = 1.0;          // 机器人行驶单位距离成本
const float TASK_PRIORITY_WEIGHT = 1.5;// 高优先级任务成本权重
const float INTER_ROBOT_SAFE_DIST = 350.0;

// 动态任务结构体
struct DynamicTask {
  int taskId;
  float target_x, target_y;
  int priority; // 任务优先级：1（低）、2（中）、3（高）
  int taskType; // 任务类型：1（取货）、2（放货）
};

// 双机状态与任务状态
struct DualTaskState {
  // 双机状态
  float robot1_x, robot1_y, robot1_v, robot1_w;
  float robot2_x, robot2_y, robot2_v, robot2_w;
  // 动态任务队列
  std::vector<DynamicTask> tasks;
  // 双机分配的任务
  int robot1TaskId = -1;
  int robot2TaskId = -1;
  // 分配成本
  float totalCost = 0.0;
} taskState;

// 辅助函数：计算路径长度（距离）
float calculatePathLength(float x1, float y1, float x2, float y2) {
  return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

// 动态规划任务分配算法：最小化总行驶成本，同时考虑任务优先级
void dynamicTaskAssignment() {
  // 若任务队列为空，直接返回
  if (taskState.tasks.empty()) {
    taskState.robot1TaskId = -1;
    taskState.robot2TaskId = -1;
    taskState.totalCost = 0.0;
    return;
  }

  // 计算双机到每个任务的距离成本
  std::vector<float> robot1Costs, robot2Costs;
  for (auto &task : taskState.tasks) {
    // 计算机器人1到任务的成本（距离*单位成本*优先级权重）
    float dist1 = calculatePathLength(taskState.robot1_x, taskState.robot1_y, task.target_x, task.target_y);
    float cost1 = dist1 * ROBOT_COST * (TASK_PRIORITY_WEIGHT + (task.priority - 1) * 0.5);
    robot1Costs.push_back(cost1);

    // 计算机器人2到任务的成本
    float dist2 = calculatePathLength(taskState.robot2_x, taskState.robot2_y, task.target_x, task.target_y);
    float cost2 = dist2 * ROBOT_COST * (TASK_PRIORITY_WEIGHT + (task.priority - 1) * 0.5);
    robot2Costs.push_back(cost2);
  }

  // 枚举所有可能的任务分配方案，选择总成本最小的方案
  int minCostIdx1 = -1, minCostIdx2 = -1;
  float minTotalCost = INFINITY;
  for (int i = 0; i < taskState.tasks.size(); i++) {
    for (int j = 0; j < taskState.tasks.size(); j++) {
      if (i == j) continue; // 同一任务不可分配给两机
      // 计算总成本
      float currentCost = robot1Costs[i] + robot2Costs[j];
      // 增加双机协同避障成本（距离越近成本越高）
      float interDist = calculatePathLength(taskState.robot1_x, taskState.robot1_y, taskState.robot2_x, taskState.robot2_y);
      float interCost = (INTER_ROBOT_SAFE_DIST - interDist) * 0.1; // 间距越小，成本越高
      currentCost += interCost;

      if (currentCost < minTotalCost) {
        minTotalCost = currentCost;
        minCostIdx1 = i;
        minCostIdx2 = j;
      }
    }
  }

  // 分配任务
  if (minCostIdx1 != -1 && minCostIdx2 != -1) {
    taskState.robot1TaskId = taskState.tasks[minCostIdx1].taskId;
    taskState.robot2TaskId = taskState.tasks[minCostIdx2].taskId;
  } else if (!taskState.tasks.empty() && taskState.tasks.size() == 1) {
    // 仅一个任务，分配给距离更近的机器人
    int closerRobot = (robot1Costs[0] < robot2Costs[0]) ? 1 : 2;
    if (closerRobot == 1) {
      taskState.robot1TaskId = taskState.tasks[0].taskId;
    } else {
      taskState.robot2TaskId = taskState.tasks[0].taskId;
    }
  }

  taskState.totalCost = minTotalCost;
}

// RVO路径避障：双机路径交叉时的动态调整
void rvoPathAdjustment() {
  // 获取双机分配的任务目标位置
  float target1_x, target1_y, target2_x, target2_y;
  bool hasTarget1 = false, hasTarget2 = false;

  for (auto &task : taskState.tasks) {
    if (task.taskId == taskState.robot1TaskId) {
      target1_x = task.target_x;
      target1_y = task.target_y;
      hasTarget1 = true;
    }
    if (task.taskId == taskState.robot2TaskId) {
      target2_x = task.target_x;
      target2_y = task.target_y;
      hasTarget2 = true;
    }
  }

  // 若双机有目标，且目标路径交叉，调整角速度
  if (hasTarget1 && hasTarget2) {
    // 计算双机到目标的方向向量
    float dir1_x = target1_x - taskState.robot1_x;
    float dir1_y = target1_y - taskState.robot1_y;
    float dir2_x = target2_x - taskState.robot2_x;
    float dir2_y = target2_y - taskState.robot2_y;

    // 计算两方向的夹角（判断路径是否交叉）
    float dotProduct = dir1_x * dir2_x + dir1_y * dir2_y;
    float distInter = calculatePathLength(taskState.robot1_x, taskState.robot1_y, taskState.robot2_x, taskState.robot2_y);

    // 若夹角接近180度（对向行驶）且距离较近，启动RVO调整
    if (dotProduct < -0.5 && distInter < INTER_ROBOT_SAFE_DIST) {
      // 调整角速度，使机器人向两侧避让
      taskState.robot1_w += 0.2;
      taskState.robot2_w -= 0.2;
    }
  }
}

// 任务目标跟踪控制：计算机器人到任务目标的速度
void taskTargetTracking(float &robotX, float &robotY, float &robotV, float &robotW, float targetX, float targetY) {
  float dist = calculatePathLength(robotX, robotY, targetX, targetY);
  if (dist > 50.0) { // 距离目标较远，快速靠近
    robotV = MAX_LINEAR_V * 0.8;
  } else if (dist > 10.0) { // 接近目标，减速
    robotV = MAX_LINEAR_V * 0.3;
  } else { // 到达目标，停车
    robotV = 0.0;
  }
  // 计算转向角速度，对准目标
  float angle = atan2(targetY - robotY, targetX - robotX);
  robotW = angle * 0.5; // 角速度比例控制
}

// 初始化函数（硬件初始化同案例1，此处省略重复代码，仅保留核心初始化逻辑）
void setup() {
  Serial.begin(115200);
  // 硬件初始化（同案例1，略）

  // 初始化任务状态
  taskState.robot1_x = 0.0; taskState.robot1_y = 0.0;
  taskState.robot2_x = 500.0; taskState.robot2_y = 0.0;
  taskState.robot1_v = 0.0; taskState.robot1_w = 0.0;
  taskState.robot2_v = 0.0; taskState.robot2_w = 0.0;
  taskState.robot1TaskId = -1; taskState.robot2TaskId = -1;
  taskState.tasks.clear();

  // 添加初始动态任务
  taskState.tasks.push_back({1, 800.0, 400.0, 3, 1});
  taskState.tasks.push_back({2, 600.0, -300.0, 2, 2});

  Serial.println("双机器人动态任务分配与RVO+动态规划初始化完成");
}

void loop() {
  // 1. 动态更新任务（模拟订单实时生成，实际需接入上位机或传感器）
  // 此处简化为每10秒生成一个新任务，实际可根据需求修改触发条件
  if (millis() % 10000 == 0) {
    int newTaskId = taskState.tasks.size() + 1;
    float newX = random(200.0, 1000.0);
    float newY = random(-500.0, 500.0);
    int newPriority = random(1, 3);
    taskState.tasks.push_back({newTaskId, newX, newY, newPriority, random(1, 2)});
    Serial.println("新增任务：ID=" + String(newTaskId) + "，位置(" + String(newX,1) + "," + String(newY,1) + ")");
  }

  // 2. 动态规划任务分配
  dynamicTaskAssignment();

  // 3. 双机目标跟踪与RVO路径调整
  float target1_x, target1_y, target2_x, target2_y;
  bool hasTarget1 = false, hasTarget2 = false;
  for (auto &task : taskState.tasks) {
    if (task.taskId == taskState.robot1TaskId) {
      target1_x = task.target_x;
      target1_y = task.target_y;
      hasTarget1 = true;
    }
    if (task.taskId == taskState.robot2TaskId) {
      target2_x = task.target_x;
      target2_y = task.target_y;
      hasTarget2 = true;
    }
  }

  if (hasTarget1) {
    taskTargetTracking(taskState.robot1_x, taskState.robot1_y, taskState.robot1_v, taskState.robot1_w, target1_x, target1_y);
  } else {
    taskState.robot1_v = 0.0;
    taskState.robot1_w = 0.0;
  }

  if (hasTarget2) {
    taskTargetTracking(taskState.robot2_x, taskState.robot2_y, taskState.robot2_v, taskState.robot2_w, target2_x, target2_y);
  } else {
    taskState.robot2_v = 0.0;
    taskState.robot2_w = 0.0;
  }

  // 4. RVO路径调整，规避双机路径交叉
  rvoPathAdjustment();

  // 5. 控制电机（同案例1，略，需补充具体电机控制代码）
  // 此处省略电机控制代码，实际需根据机器人硬件调用对应的电机控制函数

  // 6. 更新位置状态
  taskState.robot1_x += taskState.robot1_v * cos(taskState.robot1_w) * 0.05;
  taskState.robot1_y += taskState.robot1_v * sin(taskState.robot1_w) * 0.05;
  taskState.robot2_x += taskState.robot2_v * cos(taskState.robot2_w) * 0.05;
  taskState.robot2_y += taskState.robot2_v * sin(taskState.robot2_w) * 0.05;

  // 7. 输出状态
  Serial.print("任务数：" + String(taskState.tasks.size()) + " | ");
  Serial.print("R1任务ID：" + String(taskState.robot1TaskId) + " | ");
  Serial.print("R2任务ID：" + String(taskState.robot2TaskId) + " | ");
  Serial.print("总成本：" + String(taskState.totalCost,1));
  Serial.println();

  delay(50);
}

代码逻辑说明：
核心融合点：引入动态任务队列，通过动态规划算法计算双机到各任务的成本，选择总成本最小的任务分配方案，兼顾任务优先级与双机路径干扰；结合RVO算法，在双机路径交叉时动态调整角速度，避免路径冲突，实现动态任务下的高效协同；
核心逻辑：动态任务生成→动态规划任务分配→双机目标跟踪→RVO路径调整→电机执行→状态更新，形成“任务-分配-执行-避障”的闭环；
扩展性：可接入真实的订单系统或上位机，实现任务的实时动态更新；优化动态规划算法，加入任务完成时间约束，进一步提升任务分配的合理性；结合视觉传感器识别任务目标，提升场景适配性。

6、双机器人狭窄通道RVO协同避障与动态重规划（商场巡检场景）
适用场景：商场狭窄通道（如扶梯旁、货架通道）中，两台机器人需进行联合巡检，通道宽度有限，且存在动态行人干扰，双机需通过RVO算法实时规避行人与相互干扰，同时当通道出现突发障碍（如临时堆放的货物）导致路径中断时，双机需快速启动动态重规划，重新选择协同路径，确保巡检任务不中断，适配商场狭窄、动态的复杂环境。

// 核心库引入
#include <SimpleFOC.h>
#include <WirelessSerial.h>
#include <math.h>

// 狭窄通道与重规划核心参数
const float WHEEL_RADIUS = 45.0;
const float ROBOT_WIDTH = 180.0;
const float ENC_PULSES_PER_REV = 1000.0;
const float MAX_LINEAR_V = 100.0;
const float CHANNEL_WIDTH = 600.0;     // 通道宽度（mm）
const float OBSTACLE_DETECT_DIST = 250.0; // 障碍检测距离
const float RVO_ADJUST_THRESH = 150.0;   // RVO调整阈值
const float PATH_REPLAN_DIST = 300.0;    // 路径重规划触发距离

// 通道与重规划状态结构体
struct ChannelReplanState {
  // 双机状态
  float robot1_x, robot1_y, robot1_v, robot1_w, robot1_target_x, robot1_target_y;
  float robot2_x, robot2_y, robot2_v, robot2_w, robot2_target_x, robot2_target_y;
  // 巡检路径（固定路线+动态调整点）
  std::vector<std::pair<float, float>> patrolPath;
  // 当前巡检点索引
  int robot1PathIdx = 0;
  int robot2PathIdx = 1; // 双机巡检点错位，避免路径完全重叠
  // 动态障碍信息
  struct DynamicObstacle { float x, y, vx, vy, r; };
  std::vector<DynamicObstacle> obstacles;
  // 路径重规划标志
  bool needReplan = false;
  // 重规划路径
  std::vector<std::pair<float, float>> replanPath1, replanPath2;
  int replanIdx1 = 0, replanIdx2 = 0;
} channelState;

// 辅助函数：计算点到通道边界的距离
float distanceToChannelBoundary(float x, float y, float channelCenterY) {
  float topBoundary = channelCenterY + CHANNEL_WIDTH / 2.0;
  float bottomBoundary = channelCenterY - CHANNEL_WIDTH / 2.0;
  if (y > topBoundary) return y - topBoundary;
  else if (y < bottomBoundary) return bottomBoundary - y;
  else return 0.0;
}

// RVO狭窄通道避障：同时规避行人、双机与通道边界
void rvoNarrowChannelAdjustment() {
  float channelCenterY = 0.0; // 通道中心线Y坐标（根据实际场景设定）

  // 双机相互RVO调整
  float distInter = calculatePathLength(channelState.robot1_x, channelState.robot1_y, channelState.robot2_x, channelState.robot2_y);
  if (distInter < RVO_ADJUST_THRESH) {
    float dx = channelState.robot2_x - channelState.robot1_x;
    float dy = channelState.robot2_y - channelState.robot1_y;
    float unit_dx = dx / distInter;
    float unit_dy = dy / distInter;
    // 向两侧避让，远离对方
    channelState.robot1_w -= unit_dy * 0.3;
    channelState.robot2_w += unit_dy * 0.3;
  }

  // 通道边界避障
  float distBoundary1 = distanceToChannelBoundary(channelState.robot1_x, channelState.robot1_y, channelCenterY);
  float distBoundary2 = distanceToChannelBoundary(channelState.robot2_x, channelState.robot2_y, channelCenterY);
  if (distBoundary1 < RVO_ADJUST_THRESH / 2.0) {
    // 靠近上边界，向下调整
    channelState.robot1_w -= 0.2;
  }
  if (distBoundary2 < RVO_ADJUST_THRESH / 2.0) {
    // 靠近下边界，向上调整
    channelState.robot2_w += 0.2;
  }

  // 动态行人避障（简化，假设行人动态障碍信息已存入obstacles）
  for (auto &obstacle : channelState.obstacles) {
    float dist1 = calculatePathLength(channelState.robot1_x, channelState.robot1_y, obstacle.x, obstacle.y);
    float dist2 = calculatePathLength(channelState.robot2_x, channelState.robot2_y, obstacle.x, obstacle.y);

    if (dist1 < OBSTACLE_DETECT_DIST) {
      // 计算避障偏移
      float dx = obstacle.x - channelState.robot1_x;
      float dy = obstacle.y - channelState.robot1_y;
      float unit_dx = dx / dist1;
      float unit_dy = dy / dist1;
      // 向远离障碍的方向调整角速度
      channelState.robot1_w += unit_dy * 0.5;
      // 减速避障
      channelState.robot1_v = constrain(channelState.robot1_v * 0.7, 20.0, MAX_LINEAR_V);
    }

    if (dist2 < OBSTACLE_DETECT_DIST) {
      float dx = obstacle.x - channelState.robot2_x;
      float dy = obstacle.y - channelState.robot2_y;
      float unit_dx = dx / dist2;
      float unit_dy = dy / dist2;
      channelState.robot2_w -= unit_dy * 0.5;
      channelState.robot2_v = constrain(channelState.robot2_v * 0.7, 20.0, MAX_LINEAR_V);
    }
  }
}

// 动态重规划：当路径被障碍阻断时，重新规划双机巡检路径
void dynamicPathReplan() {
  if (!channelState.needReplan) return;

  // 清除原重规划路径
  channelState.replanPath1.clear();
  channelState.replanPath2.clear();

  // 简化的重规划逻辑：沿通道方向避开障碍，选择平行路径
  float obstacleX = channelState.obstacles[0].x;
  float obstacleY = channelState.obstacles[0].y;
  float channelCenterY = 0.0;

  // 机器人1重规划路径：从障碍上方绕过
  channelState.replanPath1.push_back({channelState.robot1_x, channelState.robot1_y});
  if (obstacleY > channelCenterY) {
    channelState.replanPath1.push_back({obstacleX + 200.0, channelCenterY + CHANNEL_WIDTH / 3.0});
    channelState.replanPath1.push_back({1200.0, channelCenterY + CHANNEL_WIDTH / 3.0});
  } else {
    channelState.replanPath1.push_back({obstacleX + 200.0, channelCenterY - CHANNEL_WIDTH / 3.0});
    channelState.replanPath1.push_back({1200.0, channelCenterY - CHANNEL_WIDTH / 3.0});
  }

  // 机器人2重规划路径：从障碍下方绕过，与机器人1路径错位
  channelState.replanPath2.push_back({channelState.robot2_x, channelState.robot2_y});
  if (obstacleY > channelCenterY) {
    channelState.replanPath2.push_back({obstacleX + 200.0, channelCenterY - CHANNEL_WIDTH / 3.0});
    channelState.replanPath2.push_back({1200.0, channelCenterY - CHANNEL_WIDTH / 3.0});
  } else {
    channelState.replanPath2.push_back({obstacleX + 200.0, channelCenterY + CHANNEL_WIDTH / 3.0});
    channelState.replanPath2.push_back({1200.0, channelCenterY + CHANNEL_WIDTH / 3.0});
  }

  // 重置重规划索引
  channelState.replanIdx1 = 0;
  channelState.replanIdx2 = 0;
  channelState.needReplan = false;
}

// 重规划路径跟踪：执行重新规划的路径
void replanPathTracking() {
  if (channelState.replanIdx1 < channelState.replanPath1.size() && channelState.replanIdx2 < channelState.replanPath2.size()) {
    float target1_x = channelState.replanPath1[channelState.replanIdx1].first;
    float target1_y = channelState.replanPath1[channelState.replanIdx1].second;
    float target2_x = channelState.replanPath2[channelState.replanIdx2].first;
    float target2_y = channelState.replanPath2[channelState.replanIdx2].second;

    // 跟踪重规划路径目标
    taskTargetTracking(channelState.robot1_x, channelState.robot1_y, channelState.robot1_v, channelState.robot1_w, target1_x, target1_y);
    taskTargetTracking(channelState.robot2_x, channelState.robot2_y, channelState.robot2_v, channelState.robot2_w, target2_x, target2_y);

    // 当接近当前重规划目标时，切换到下一个目标
    if (calculatePathLength(channelState.robot1_x, channelState.robot1_y, target1_x, target1_y) < 50.0) {
      channelState.replanIdx1++;
    }
    if (calculatePathLength(channelState.robot2_x, channelState.robot2_y, target2_x, target2_y) < 50.0) {
      channelState.replanIdx2++;
    }
  } else {
    // 重规划路径执行完毕，恢复原巡检路径
    channelState.replanIdx1 = -1;
    channelState.replanIdx2 = -1;
    channelState.robot1PathIdx = 0;
    channelState.robot2PathIdx = 1;
  }
}

// 初始化函数（硬件初始化同案例1，略）
void setup() {
  Serial.begin(115200);
  // 硬件初始化（同案例1，略）

  // 初始化通道与重规划状态
  float channelCenterY = 0.0;
  // 初始化巡检路径：沿通道的直线路径，双机错位巡检
  channelState.patrolPath = {{0.0, channelCenterY}, {300.0, channelCenterY}, {600.0, channelCenterY}, {900.0, channelCenterY}, {1200.0, channelCenterY}};
  channelState.robot1PathIdx = 0;
  channelState.robot2PathIdx = 1;
  channelState.robot1_x = 0.0; channelState.robot1_y = channelCenterY;
  channelState.robot2_x = 0.0; channelState.robot2_y = channelCenterY;
  channelState.robot1_v = 0.0; channelState.robot1_w = 0.0;
  channelState.robot2_v = 0.0; channelState.robot2_w = 0.0;
  channelState.needReplan = false;
  channelState.obstacles.clear();

  Serial.println("双机器人狭窄通道RVO协同避障与动态重规划初始化完成");
}

void loop() {
  // 1. 模拟动态行人障碍（实际需接入传感器检测）
  // 每15秒添加一个动态行人，障碍物信息实时更新
  if (millis() % 15000 == 0) {
    float obsX = random(200.0, 1000.0);
    float obsY = random(-CHANNEL_WIDTH/3.0, CHANNEL_WIDTH/3.0);
    float obsVx = random(-20.0, 20.0);
    float obsVy = 0.0; // 简化为沿通道方向移动
    float obsR = 50.0; // 行人半径
    channelState.obstacles.push_back({obsX, obsY, obsVx, obsVy, obsR});
    Serial.println("新增动态行人障碍：位置(" + String(obsX,1) + "," + String(obsY,1) + ")");
  }

  // 2. 判断是否需要路径重规划：障碍阻断原巡检路径
  for (auto &obstacle : channelState.obstacles) {
    // 检查机器人1的原路径是否被障碍阻断
    float distPath1 = calculatePathLength(channelState.robot1_x, channelState.robot1_y, obstacle.x, obstacle.y);
    float distPath2 = calculatePathLength(channelState.robot2_x, channelState.robot2_y, obstacle.x, obstacle.y);
    if (distPath1 < PATH_REPLAN_DIST || distPath2 < PATH_REPLAN_DIST) {
      channelState.needReplan = true;
      break;
    }
  }

  // 3. 若需要重规划，执行动态重规划
  if (channelState.needReplan) {
    dynamicPathReplan();
    replanPathTracking();
  } else {
    // 4. 正常巡检路径跟踪
    float channelCenterY = 0.0;
    if (channelState.robot1PathIdx < channelState.patrolPath.size()) {
      float target1_x = channelState.patrolPath[channelState.robot1PathIdx].first;
      float target1_y = channelState.patrolPath[channelState.robot1PathIdx].second;
      taskTargetTracking(channelState.robot1_x, channelState.robot1_y, channelState.robot1_v, channelState.robot1_w, target1_x, target1_y);
      if (calculatePathLength(channelState.robot1_x, channelState.robot1_y, target1_x, target1_y) < 50.0) {
        channelState.robot1PathIdx++;
      }
    } else {
      channelState.robot1PathIdx = 0; // 巡检路径循环
    }

    if (channelState.robot2PathIdx < channelState.patrolPath.size()) {
      float target2_x = channelState.patrolPath[channelState.robot2PathIdx].first;
      float target2_y = channelState.patrolPath[channelState.robot2PathIdx].second;
      taskTargetTracking(channelState.robot2_x, channelState.robot2_y, channelState.robot2_v, channelState.robot2_w, target2_x, target2_y);
      if (calculatePathLength(channelState.robot2_x, channelState.robot2_y, target2_x, target2_y) < 50.0) {
        channelState.robot2PathIdx++;
      }
    } else {
      channelState.robot2PathIdx = 1; // 双机错位循环
    }
  }

  // 5. 狭窄通道RVO协同避障
  rvoNarrowChannelAdjustment();

  // 6. 控制电机（同案例1，略）

  // 7. 更新位置状态
  channelState.robot1_x += channelState.robot1_v * cos(channelState.robot1_w) * 0.05;
  channelState.robot1_y += channelState.robot1_v * sin(channelState.robot1_w) * 0.05;
  channelState.robot2_x += channelState.robot2_v * cos(channelState.robot2_w) * 0.05;
  channelState.robot2_y += channelState.robot2_v * sin(channelState.robot2_w) * 0.05;

  // 8. 输出状态
  String statusStr = channelState.needReplan ? "重规划中" : "正常巡检";
  Serial.print("状态：" + statusStr + " | R1巡检点：" + String(channelState.robot1PathIdx) + " | R2巡检点：" + String(channelState.robot2PathIdx));
  Serial.print(" | 障碍数：" + String(channelState.obstacles.size()));
  Serial.println();

  delay(50);
}

代码逻辑说明：
核心融合点：针对狭窄通道的特殊约束，优化RVO算法，同时规避双机干扰、通道边界与动态行人；当障碍阻断原巡检路径时，启动动态重规划，生成平行于通道的避障路径，双机错位执行，确保巡检任务不中断，适配商场狭窄、动态的复杂环境；
核心逻辑：动态障碍检测→路径阻断判断→动态重规划→重规划路径跟踪→RVO狭窄通道避障→电机执行→状态更新，形成“障碍感知-路径决策-协同避障-任务执行”的闭环；
扩展性：可接入激光雷达、摄像头等传感器，提升动态障碍检测精度；优化动态重规划算法，采用更智能的路径规划方法（如A*算法），生成更优的避障路径；结合语音、灯光等交互装置，提醒通道中的行人避让，提升场景安全性。

要点解读

1. 双机器人协同的核心：RVO互惠避障与状态同步，解决路径冲突与信息不对称问题
   双机器人协同的核心矛盾是路径冲突与信息不对称，RVO算法与状态同步机制是解决这一矛盾的关键，也是实现高效协同的基础：
   RVO互惠避障的本质：双向感知与动态妥协：RVO算法的核心是让双机都具备“预判对方运动意图”的能力，而非单方面避障。通过实时获取对方的位置、速度与运动方向，计算双方的避障偏移量，让双机同时向不同方向避让，而非互相推挤。案例中通过相对位置与相对速度计算避障调整量，既避免了双机对向行驶的碰撞，又减少了同向行驶的路径干扰，实现了“互惠共赢”的避障效果。
   状态同步的关键：通信协议与实时性保障：双机协同的前提是信息对称，需建立可靠的通信机制，实时同步双机的位置、速度、目标与任务状态。案例中采用串口通信（无线或有线），定义简洁的通信协议，确保数据快速解析；同时控制通信频率与延迟，避免因通信延迟导致的状态不同步，确保双机决策基于相同的实时信息，提升协同的一致性与稳定性。
2. 动态规划的核心：任务与路径的双重优化，解决动态场景下的决策滞后问题
   商场、工厂等场景的需求动态变化，传统固定路径与任务分配无法适配，动态规划是实现“自适应决策”的核心，核心是任务与路径的双重优化：
   任务分配的动态规划：成本与效率的平衡：动态任务场景中，任务分配需兼顾行驶成本、任务优先级与时间约束。案例2通过构建成本模型，考虑距离成本、优先级权重与双机协同成本，采用枚举法选择总成本最小的分配方案，既保证了任务分配的实时性，又最大化了作业效率；后续可引入更复杂的动态规划算法，加入任务完成时间、机器人电量等约束，进一步提升决策的合理性。
   路径规划的动态重规划：实时响应环境变化：当环境出现突发障碍（如行人、货物）导致原路径阻断时，需快速启动重规划。案例3构建了路径阻断检测机制，触发重规划后，生成避障路径并平滑切换，确保任务不中断；核心是平衡重规划的实时性与路径的最优性，通过简化路径规划逻辑，确保在狭窄通道等复杂场景中快速决策，同时保证路径的可行性，避免因重规划延迟导致的停机。
3. BLDC电机控制的核心：速度闭环与动态响应匹配，保障协同动作的精准性
   BLDC电机是双机器人执行协同动作的执行器，其控制精度直接决定协同效果，核心是速度闭环控制与动态响应的精准匹配：
   速度闭环的精准跟踪：双机器人的协同依赖速度的精准控制，需采用速度闭环控制，通过编码器实时反馈轮子转速，与目标速度对比，通过PID算法调整PWM占空比，确保实际速度紧跟目标速度。案例中通过SimpleFOC库实现速度闭环，精准跟踪跟随速度、避障速度与路径跟踪速度，避免因速度偏差导致的跟随失效或避障不及时。
   动态响应与协同需求的匹配：协同场景中，机器人的速度需要频繁切换（跟随、避障、重规划），要求电机具备快速的动态响应能力。通过优化PID参数，在保证稳定性的前提下，提升响应速度，当速度目标突变时，电机能快速调整到目标速度；同时，设置速度限幅，避免因速度突变导致的电机过载或机器人失控，确保双机在频繁的速度切换中保持协同稳定。
4. 复杂场景适配的核心：约束感知与动态调整，解决特殊场景的适配瓶颈
   商场狭窄通道、工厂并行通道等特殊场景存在严格的约束（通道宽度、动态障碍），需通过约束感知与动态调整实现场景适配，解决传统算法的适配瓶颈：
   约束感知：场景特征与风险的量化：不同场景的核心约束不同，需先量化场景特征与风险。案例3针对狭窄通道，量化通道宽度、边界距离、动态障碍检测距离等参数，将这些约束融入RVO算法与路径规划，提前预判风险，避免机器人超出通道边界或与动态障碍碰撞；工厂并行通道场景需感知通道间距与物流方向，调整双机的并行间距与行驶方向，确保高效通过。
   动态调整：算法与场景的实时适配：场景约束动态变化时，算法需实时调整。案例3通过感知通道宽度变化与动态障碍，动态调整RVO的避障阈值与速度；当通道变窄时，自动降低速度、增大避障距离，避免超出通道边界；当动态障碍增多时，降低行驶速度，提升避障响应速度，通过算法与场景的动态适配，确保在复杂约束场景中稳定运行。
5. 系统落地的关键：鲁棒性设计与安全边界，解决实际工程的稳定性与安全问题
   双机器人系统在实际应用中面临环境干扰、硬件差异与突发状况，鲁棒性设计与安全边界是系统落地的关键，核心是可靠性保障与安全兜底：
   鲁棒性设计：多维度容错与抗干扰：实际场景中存在传感器噪声、通信延迟、硬件参数差异等干扰，需通过多维度容错提升鲁棒性。传感器数据采用滤波算法去除噪声；通信失败时，设置状态保持机制，采用默认协同策略，避免失控；硬件参数差异通过参数校准，适配不同机器人的轮径、轮距，确保双机协同的一致性；同时，加入异常状态检测，当电机故障、传感器失效时，及时触发保护机制。
   安全边界设计：安全冗余与紧急处理：安全是双机器人应用的核心，需设置多层安全边界。硬件层面，配备紧急制动装置，当检测到碰撞风险时，快速切断电机电源；软件层面，设置安全距离、速度上限、加速度限制，避免机器人超速或过度避障；当双机距离小于安全阈值时，立即触发停车或分离策略；同时，预留人工干预接口，在系统失控时，可远程紧急制动，通过硬件与软件的双重安全冗余，确保系统在任何情况下都不发生安全事故，满足实际工程的安全要求。

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