在基于 Arduino 平台的 BLDC（无刷直流电机）多巡检机器人系统中，采用“集中式协调 + VO（速度障碍法）速度仲裁”是一种兼顾全局路径规划与局部动态避障的高级架构。以下从专业视角为您详细解析其主要特点、应用场景及注意事项：
一、 主要特点
集中式协调与全局规划
系统采用集中式协调器（如 ESP32 或树莓派）作为“大脑”，负责全局的路径分配与冲突消解。协调器利用 A* 等算法，根据机器人的负载、通行能力与冲突代价，为每台机器人分配全局路径。在发生路权冲突时，协调器会提前下发期望速度或等待窗口，从宏观上避免死锁。
VO/RVO 局部速度仲裁
在微观层面，系统引入 VO（速度障碍法）或 RVO（互惠速度障碍法）进行局部避障。VO 算法假设其他智能体是不配合的障碍物，而 RVO 则假设双方均承担一半避让责任，从而计算出互惠的速度集合。这使得多机器人在狭窄通道交汇时，能表现出更自然、流畅的“右侧通行”等拟人化行为，有效化解死锁。
高频状态共享与实时通信
多智能体协同的基石是高频信息共享。每个机器人必须通过无线通信（如 ESP-NOW）实时广播自身的坐标 (x, y) 和速度向量 (vx, vy)。VO/RVO 算法的准确性高度依赖这些数据的实时性，若通信延迟过高，预测的碰撞锥将失效，导致避障失败。
底层 FOC 精准执行
VO/RVO 算法会频繁输出微小且连续变化的速度向量（如从直行突变为斜向低速移动）。普通电机驱动难以精准响应，而基于 Arduino 的 SimpleFOC 库通过磁场定向控制（FOC），能够毫秒级精准响应这些高频微调，确保机器人在执行复杂协同避障动作时丝滑无顿挫。
算力与实时性的平衡策略
标准的 VO/RVO 算法涉及大量三角函数和向量运算。为保证系统实时性，工程上通常将繁重的 A* 全局规划放在低频率循环（如每 500ms 一次），而将高频的 VO 局部避障放在主循环中；同时，仅在检测到附近有邻居时才触发完整的 VO 计算，以平衡 Arduino 的算力瓶颈。
二、 应用场景
该架构主要适用于工业园区、走廊等结构化环境中的多巡检机器人协同作业：
固定任务点巡检：4~6 台巡检机器人沿预设的固定目标序列（如走廊、拐角、闸机）进行巡检。
动态冲突消解：在路权冲突点（如交叉口、狭窄通道），协调器结合 VO 算法提前进行限速或停车仲裁，避免机器人相互碰撞或陷入死锁。
已知地图导航：由于环境地图已知，集中式协调器可以全局统筹，结合局部的 VO 避障，实现多机高效、安全地穿梭于复杂结构化环境中。
三、 需要注意的事项
通信的实时性与抗干扰
多机协同对通信延迟极其敏感。必须确保无线通信模块（如 ESP-NOW）的高频稳定广播。若通信延迟导致预测碰撞锥失效，极易发生真实碰撞。在工业现场还需注意电磁干扰对通信链路的影响。
计算复杂度与算力匹配
Arduino 等微控制器的算力有限，难以长时间承受标准 VO/RVO 的密集向量运算。必须对算法进行适当简化（如简化判定逻辑），或采用分层计算架构（低频全局规划 + 高频局部避障），防止主控芯片过载导致系统卡顿。
底层电机的动态响应能力
速度仲裁算法输出的指令通常是高频变化的微小向量。系统必须配备支持 FOC（磁场定向控制）的 BLDC 驱动方案，以确保底层电机能够毫秒级精准响应这些速度和扭矩的连续变化，否则会出现执行顿挫，导致避障轨迹畸变。
集中式节点的可靠性
由于采用集中式协调，协调器（如 ESP32/树莓派）成为了系统的单点故障源。需为其设计完善的故障冗余机制，例如在协调器宕机或通信中断时，下位机应具备自主降级为纯 VO 避障或安全停机的能力，防止多机失控相撞。
安全距离与参数整定
需合理设置速度障碍时间窗口（VO_TAU）和安全距离（SAFE_DIST）。时间窗口过短会导致避障反应不及，过长则会导致机器人过度保守、频繁减速；安全距离需结合机器人的制动性能和 FOC 响应延迟进行综合整定。

1、集中式路径分配 + VO避碰
适用场景：仓库多AGV巡检。中央服务器分配路径，各机器人通过VO（速度障碍法）避免相互碰撞，同时遵循中央路径。

/* ===== Arduino BLDC 多巡检机器人 — 集中式路径分配 + VO避碰 =====
 * 硬件：2×BLDC差速底盘 × N台 + 中央服务器(PC/树莓派) + Wi-Fi通信
 * 核心：中央分配全局路径 → 各机器人VO局部避碰 → 路径跟踪
 */
#include <SimpleFOC.h>
#include <WiFi.h>
#include <ArduinoJson.h>

BLDCMotor motorL(5), motorR(6);
BLDCDriver3PWM drvL, drvR;
Encoder encL(2,3,2048), encR(4,5,2048);

// 机器人自身信息
uint8_t myId = 1;
float myX = 0, myY = 0, myYaw = 0;
float myVx = 0, myVy = 0;

// 中央分配的路径点队列
struct Waypoint { float x, y; };
std::vector<Waypoint> path;
int pathIndex = 0;

// 邻居机器人信息(来自中央广播)
struct NeighborRobot {
  uint8_t id;
  float x, y, vx, vy;
  float radius;  // 机器人半径
};
NeighborRobot neighbors[10];
int neighborCount = 0;

// VO参数
const float SAFETY_MARGIN = 0.3;   // 安全裕度(m)
const float MAX_SPEED = 0.5;       // 最大速度(m/s)
const float REACTION_TIME = 1.0;   // 反应时间(s)

// Wi-Fi客户端
WiFiClient client;
const char* serverIP = "192.168.1.100";
const int serverPort = 8080;

void setup() {
  Serial.begin(115200);
  WiFi.begin("SSID", "password");
  while(WiFi.status() != WL_CONNECTED) delay(500);
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 从中央服务器接收路径和邻居信息
  receiveFromServer();
  
  // 2. 更新自身位置
  myVx = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0 * cos(myYaw);
  myVy = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0 * sin(myYaw);
  myX += myVx * dt;
  myY += myVy * dt;
  
  // 3. 发送自身状态到服务器
  sendToServer();
  
  // 4. 路径跟踪(期望速度)
  float desVx = 0, desVy = 0;
  if(pathIndex < path.size()) {
    Waypoint wp = path[pathIndex];
    float dx = wp.x - myX;
    float dy = wp.y - myY;
    float dist = sqrt(dx*dx + dy*dy);
    
    if(dist < 0.2) {
      pathIndex++;  // 到达路径点
    } else {
      desVx = MAX_SPEED * dx / dist;
      desVy = MAX_SPEED * dy / dist;
    }
  }
  
  // 5. VO速度仲裁
  float safeVx = desVx, safeVy = desVy;
  if(!isVelocitySafe(desVx, desVy)) {
    // 当前速度不安全 → 寻找安全速度
    findSafeVelocity(&safeVx, &safeVy, desVx, desVy);
  }
  
  // 6. 转换为差速控制
  float vLin = sqrt(safeVx*safeVx + safeVy*safeVy);
  float vAng = atan2(safeVy, safeVx) - myYaw;
  
  motorL.move(vLin * 1000 - vAng * 80);
  motorR.move(vLin * 1000 + vAng * 80);
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

bool isVelocitySafe(float vx, float vy) {
  for(int i=0; i<neighborCount; i++) {
    // 相对速度
    float relVx = vx - neighbors[i].vx;
    float relVy = vy - neighbors[i].vy;
    
    // 相对位置
    float dx = neighbors[i].x - myX;
    float dy = neighbors[i].y - myY;
    float dist = sqrt(dx*dx + dy*dy);
    
    // 碰撞锥检测
    float relSpeed = sqrt(relVx*relVx + relVy*relVy);
    if(relSpeed < 0.01) continue;
    
    float angleRel = atan2(relVy, relVx);
    float anglePos = atan2(dy, dx);
    float angleDiff = abs(angleRel - anglePos);
    
    float coneHalfAngle = asin((neighbors[i].radius + SAFETY_MARGIN) / dist);
    
    if(angleDiff < coneHalfAngle) {
      float ttc = dist * cos(angleDiff) / relSpeed;
      if(ttc < REACTION_TIME && ttc > 0) {
        return false;  // 会导致碰撞
      }
    }
  }
  return true;
}

void findSafeVelocity(float* vx, float* vy, float desVx, float desVy) {
  // 速度空间采样
  float bestVx = 0, bestVy = 0;
  float bestDist = 999;
  
  for(float theta = -PI; theta <= PI; theta += PI/8) {
    for(float speed = 0.1; speed <= MAX_SPEED; speed += 0.1) {
      float svx = speed * cos(theta);
      float svy = speed * sin(theta);
      
      if(isVelocitySafe(svx, svy)) {
        float d = sqrt(pow(svx-desVx,2) + pow(svy-desVy,2));
        if(d < bestDist) {
          bestDist = d;
          bestVx = svx;
          bestVy = svy;
        }
      }
    }
  }
  
  *vx = bestVx;
  *vy = bestVy;
}

关键设计点：
中央服务器分配全局路径
各机器人VO局部避碰
速度空间采样寻找安全速度
Wi-Fi通信交换状态信息

集中式任务调度 + 速度优先级仲裁
适用场景：变电站巡检、隧道巡检。中央服务器根据任务紧急度分配速度优先级，高优先级机器人优先通行。

/* ===== Arduino BLDC 多巡检机器人 — 集中式任务调度 + 速度仲裁 =====
 * 硬件：2×BLDC差速底盘 × N台 + 中央服务器 + Wi-Fi
 * 核心：中央分配任务和优先级 → 各机器人根据优先级调整速度
 *       高优先级机器人保持速度，低优先级减速让行
 */
#include <SimpleFOC.h>
#include <WiFi.h>

BLDCMotor motorL(5), motorR(6);

// 任务信息
struct Mission {
  uint8_t id;
  uint8_t priority;      // 0最高, 10最低
  float targetX, targetY;
  float assignedSpeed;   // 分配的速度
};
Mission myMission;

// 邻居优先级信息
struct PriorityInfo {
  uint8_t id;
  uint8_t priority;
  float x, y, vx, vy;
};
PriorityInfo others[10];
int otherCount = 0;

// 速度仲裁结果
float finalSpeed = 0;
float finalOmega = 0;

void setup() {
  Serial.begin(115200);
  WiFi.begin("SSID", "password");
  while(WiFi.status() != WL_CONNECTED) delay(500);
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 从中央接收任务和邻居信息
  receiveMissionAndPeers();
  
  // 2. 速度优先级仲裁
  arbitrateSpeed();
  
  // 3. 导航控制
  navigateToTarget();
  
  // 4. 发送自身状态
  sendStatus();
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

void arbitrateSpeed() {
  // 基础速度 = 分配速度
  float baseSpeed = myMission.assignedSpeed;
  float speedReduction = 0;
  
  for(int i=0; i<otherCount; i++) {
    // 计算与其他机器人的距离
    float dx = others[i].x - myX;
    float dy = others[i].y - myY;
    float dist = sqrt(dx*dx + dy*dy);
    
    if(dist < 2.0) {  // 2米范围内需要考虑
      // 如果对方优先级更高，我方减速
      if(others[i].priority < myMission.priority) {
        // 优先级更高的机器人 → 让行
        float factor = 1.0 - dist / 2.0;
        speedReduction = max(speedReduction, factor * 0.6);
      } else if(others[i].priority == myMission.priority) {
        // 同级 → 各自减速50%
        speedReduction = max(speedReduction, 0.3 * (1.0 - dist / 2.0));
      }
      // 对方优先级更低 → 我方保持速度
    }
  }
  
  // 应用减速
  finalSpeed = baseSpeed * (1.0 - speedReduction);
  finalSpeed = max(finalSpeed, 0.1);  // 不低于0.1m/s
}

void navigateToTarget() {
  float dx = myMission.targetX - myX;
  float dy = myMission.targetY - myY;
  float dist = sqrt(dx*dx + dy*dy);
  float angle = atan2(dy, dx);
  
  if(dist > 0.2) {
    float vLin = constrain(finalSpeed * 1000, 0, 300);
    float vAng = constrain((angle - myYaw) * 2.0, -1.5, 1.5);
    
    motorL.move(vLin - vAng * 80);
    motorR.move(vLin + vAng * 80);
  } else {
    motorL.move(0);
    motorR.move(0);
    Serial.println("Reached target");
  }
}

关键设计点：
中央服务器分配任务优先级
速度仲裁基于相对优先级和距离
高优先级机器人保持速度，低优先级让行
同级机器人各减速50%

3、集中式地图共享 + VO协同避障
适用场景：大面积巡检（停车场、机场）。各机器人共享局部地图，中央融合为全局地图，VO避障时考虑地图中的静态障碍和动态机器人。

/* ===== Arduino BLDC 多巡检机器人 — 集中式地图共享 + VO协同避障 =====
 * 硬件：2×BLDC差速底盘 × N台 + 激光雷达/超声波 + 中央服务器
 * 核心：各机器人上传局部地图 → 中央融合全局地图 → 下发
 *       VO避障同时考虑静态障碍和动态机器人
 */
#include <SimpleFOC.h>
#include <WiFi.h>
#include <ArduinoJson.h>

BLDCMotor motorL(5), motorR(6);

// 局部地图(10×10栅格，每格0.5m)
const int LOCAL_SIZE = 10;
float localMap[LOCAL_SIZE][LOCAL_SIZE];

// 全局地图(从中央接收)
const int GLOBAL_SIZE = 100;
float globalMap[GLOBAL_SIZE][GLOBAL_SIZE];

// 机器人位置
float myX = 0, myY = 0, myYaw = 0;

// 邻居(来自中央广播)
struct Peer {
  uint8_t id;
  float x, y, vx, vy;
};
Peer peers[20];
int peerCount = 0;

// VO参数
const float ROBOT_RADIUS = 0.3;
const float STATIC_SAFETY = 0.2;

void buildLocalMap() {
  // 使用超声波/激光雷达填充局部地图
  for(int i=0; i<LOCAL_SIZE; i++)
    for(int j=0; j<LOCAL_SIZE; j++)
      localMap[i][j] = 0;
  
  // 前方障碍
  float dF = readCM(TRIG_F, ECHO_F);
  if(dF < 80) {
    int fx = LOCAL_SIZE/2 + round(cos(myYaw) * dF/50);
    int fy = LOCAL_SIZE/2 + round(sin(myYaw) * dF/50);
    if(fx>=0 && fx<LOCAL_SIZE && fy>=0 && fy<LOCAL_SIZE)
      localMap[fx][fy] = 1;
  }
  // 左右同理...
}

bool isVelocitySafeGlobal(float vx, float vy) {
  // 1. 检查静态障碍(全局地图)
  float futureX = myX + vx * 0.5;
  float futureY = myY + vy * 0.5;
  int gx = (int)(futureX / 0.5);
  int gy = (int)(futureY / 0.5);
  if(gx>=0 && gx<GLOBAL_SIZE && gy>=0 && gy<GLOBAL_SIZE) {
    if(globalMap[gx][gy] > 0.5) return false;
  }
  
  // 2. 检查动态机器人(VO)
  for(int i=0; i<peerCount; i++) {
    float relVx = vx - peers[i].vx;
    float relVy = vy - peers[i].vy;
    float dx = peers[i].x - myX;
    float dy = peers[i].y - myY;
    float dist = sqrt(dx*dx + dy*dy);
    
    float relSpeed = sqrt(relVx*relVx + relVy*relVy);
    if(relSpeed < 0.01) continue;
    
    float angleRel = atan2(relVy, relVx);
    float anglePos = atan2(dy, dx);
    float angleDiff = abs(angleRel - anglePos);
    float coneHalfAngle = asin((ROBOT_RADIUS + STATIC_SAFETY) / dist);
    
    if(angleDiff < coneHalfAngle) {
      float ttc = dist * cos(angleDiff) / relSpeed;
      if(ttc < 2.0 && ttc > 0) return false;
    }
  }
  
  return true;
}

void findSafeVelocityGlobal(float* vx, float* vy, float desVx, float desVy) {
  float bestVx = desVx, bestVy = desVy;
  float bestDist = 999;
  
  // 速度空间采样(考虑全局地图和VO)
  for(float theta = -PI; theta <= PI; theta += PI/6) {
    for(float speed = 0.1; speed <= 0.5; speed += 0.1) {
      float svx = speed * cos(theta);
      float svy = speed * sin(theta);
      
      if(isVelocitySafeGlobal(svx, svy)) {
        float d = sqrt(pow(svx-desVx,2) + pow(svy-desVy,2));
        if(d < bestDist) {
          bestDist = d;
          bestVx = svx;
          bestVy = svy;
        }
      }
    }
  }
  
  *vx = bestVx;
  *vy = bestVy;
}

void setup() {
  Serial.begin(115200);
  WiFi.begin("SSID", "password");
  while(WiFi.status() != WL_CONNECTED) delay(500);
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 构建局部地图并上传
  buildLocalMap();
  uploadLocalMap();
  
  // 2. 接收全局地图和邻居信息
  receiveGlobalMap();
  
  // 3. 路径跟踪
  float desVx = 0.3, desVy = 0;  // 期望速度(来自路径规划)
  
  // 4. VO+地图避障
  float safeVx, safeVy;
  findSafeVelocityGlobal(&safeVx, &safeVy, desVx, desVy);
  
  // 5. 执行
  float vLin = sqrt(safeVx*safeVx + safeVy*safeVy);
  float vAng = atan2(safeVy, safeVx) - myYaw;
  
  motorL.move(vLin * 1000 - vAng * 80);
  motorR.move(vLin * 1000 + vAng * 80);
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
各机器人上传局部地图，中央融合为全局地图
VO避障同时考虑静态障碍(全局地图)和动态机器人
速度空间采样寻找安全速度
地图共享提高整体避障效率

要点解读
① 集中式协调的核心是“全局视角”
每个机器人只能看到局部环境，但中央服务器可以看到全局。集中式协调的优势：
避免路径冲突（两车对头）
优化整体效率（不是单个最优）
统一任务调度（优先级管理）
② VO速度仲裁的关键是“相对速度”
VO的核心思想是将动态障碍物映射到速度空间：
碰撞锥 = {v_rel | 相对速度指向对方且碰撞时间<阈值}
案例一和案例三都使用了这种方法。注意：VO假设对方匀速运动，实际中需要不断更新。
③ 通信延迟是集中式系统的主要挑战
Wi-Fi延迟通常在10~100ms，对于高速移动的机器人可能不够。应对策略：
预测补偿：用卡尔曼滤波预测对方位置
降低频率：控制周期20ms，通信周期100ms
本地备份：通信中断时使用本地传感器
④ 实际部署建议：混合架构
纯集中式的风险：中央服务器宕机→全部瘫痪。
推荐混合架构：
集中层：任务分配、路径规划(低频)
分布式层：VO避碰、本地控制(高频)
备份层：通信中断时独立运行
案例三已经体现了这种分层思想（局部地图本地构建，全局地图中央融合）。

4、A全局规划 + 基础VO局部避障（集中式调度）
适用场景：多机器人执行巡检任务时，中央控制器为每个机器人规划A路径，各机器人通过VO检测局部碰撞风险并仲裁速度。

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

// ==================== BLDC差速电机定义 ====================
BLDCMotor motorL(7), motorR(7);
// 需补充Encoder和Driver初始化...

// ==================== 智能体状态结构体 ====================
struct Agent {
    float x, y;          // 当前位置
    float vx, vy;        // 当前速度向量
    float targetX, targetY; // 目标位置（由A*下发）
};

// 当前机器人（自身）
Agent self = {0, 0, 0, 0, 10, 10};

// 其他机器人状态（来自中央协调器广播）
Agent others[2] = {
    {50, 50, 1, 0, 60, 50},
    {100, 0, 0, 1, 100, 20}
};
const int OTHER_COUNT = 2;

// ==================== 安全参数 ====================
const float SAFE_RADIUS = 15.0;      // 安全距离(cm)
const float EMERGENCY_RADIUS = 5.0;  // 紧急停止半径(cm)

// ==================== 速度仲裁核心函数 ====================
struct VelocityCommand {
    float vx, vy;
    bool safe;
};

VelocityCommand velocityArbitration(float desiredVx, float desiredVy) {
    VelocityCommand result = {desiredVx, desiredVy, true};
    
    // 【核心】VO检测：遍历所有其他机器人
    for (int i = 0; i < OTHER_COUNT; i++) {
        float dx = others[i].x - self.x;
        float dy = others[i].y - self.y;
        float dist = sqrt(dx*dx + dy*dy);
        
        if (dist < EMERGENCY_RADIUS) {
            // 紧急情况：立即停止
            result.vx = 0; result.vy = 0;
            result.safe = false;
            Serial.println("⚠️ 紧急停止！");
            return result;
        }
        
        if (dist < SAFE_RADIUS) {
            // 计算相对速度
            float relVx = desiredVx - others[i].vx;
            float relVy = desiredVy - others[i].vy;
            
            // 计算相对速度在连线方向上的投影
            float dot = (relVx*dx + relVy*dy) / dist;
            
            // 如果相对速度指向对方→存在碰撞风险
            if (dot > 0) {
                // 计算碰撞时间
                float ttc = dist / (dot + 0.01);
                if (ttc < 2.0) {  // 2秒内可能碰撞
                    // 速度仲裁：垂直于连线方向避让
                    float perpX = -dy / dist;
                    float perpY = dx / dist;
                    float mag = sqrt(desiredVx*desiredVx + desiredVy*desiredVy);
                    
                    result.vx = perpX * mag * 0.8;
                    result.vy = perpY * mag * 0.8;
                    result.safe = false;
                    Serial.print("🔄 VO触发: 避让方向 (");
                    Serial.print(result.vx); Serial.print(", ");
                    Serial.print(result.vy); Serial.println(")");
                    return result;
                }
            }
        }
    }
    return result;
}

// ==================== Setup ====================
void setup() {
    Serial.begin(115200);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
}

// ==================== Loop ====================
void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 计算期望速度（由A*或上位机下发）
    float desiredVx = (self.targetX - self.x) * 0.05;
    float desiredVy = (self.targetY - self.y) * 0.05;
    
    // 2. 【核心】VO速度仲裁
    VelocityCommand cmd = velocityArbitration(desiredVx, desiredVy);
    
    // 3. 转换为差速指令
    float v = sqrt(cmd.vx*cmd.vx + cmd.vy*cmd.vy);
    float w = atan2(cmd.vy, cmd.vx);
    float wheelBase = 0.3;  // 轮距(m)
    
    float vL = (v - w * wheelBase/2);
    float vR = (v + w * wheelBase/2);
    
    // 4. BLDC执行
    motorL.move(vL);
    motorR.move(vR);
    
    // 5. 更新自身位置（实际需用里程计）
    self.x += cmd.vx * 0.05;
    self.y += cmd.vy * 0.05;
    
    delay(50);  // 20Hz
}

代码要点：本案例通过VO算法预测碰撞风险，当检测到相对速度指向其他机器人时，强制将速度转向垂直于连线的方向，实现分布式避让。中央协调器只负责下发A*目标点，具体速度仲裁由各机器人本地完成。

5、互惠速度障碍（RVO）与平滑差速避障
适用场景：多机器人在狭窄通道中会车，RVO假设对方也会避让，避免“死锁”现象，使避障更平滑。

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

BLDCMotor motorL(7), motorR(7);
// 补充Encoder和Driver初始化...

// ==================== 智能体定义 ====================
struct Agent {
    float x, y, vx, vy;
    float radius;  // 机器人半径
};

Agent self = {0, 0, 0, 0, 5.0};
Agent neighbor = {30, 0, -1, 0, 5.0};

// ==================== RVO避让计算 ====================
struct RVOResult {
    float vx, vy;
    bool conflict;
};

RVOResult computeRVO(float desiredVx, float desiredVy) {
    RVOResult result = {desiredVx, desiredVy, false};
    
    // 计算相对位置和相对速度
    float dx = neighbor.x - self.x;
    float dy = neighbor.y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    float combinedRadius = self.radius + neighbor.radius;
    
    // 如果距离小于安全阈值，触发RVO
    if (dist < combinedRadius * 3.0) {
        float relVx = desiredVx - neighbor.vx;
        float relVy = desiredVy - neighbor.vy;
        
        // 计算相对速度在连线方向的投影
        float dot = (relVx*dx + relVy*dy) / dist;
        
        if (dot > 0 && dist < combinedRadius * 2.0) {
            // 【核心】RVO：各承担一半避让责任
            float tau = dist / (dot + 0.01);
            if (tau < 3.0) {
                // 计算碰撞锥边界
                float theta = atan2(-dy, -dx);
                float halfAngle = asin(combinedRadius / dist);
                
                // 当前速度方向角
                float currentAngle = atan2(relVy, relVx);
                
                // 判断是否在碰撞锥内
                float angleDiff = currentAngle - theta;
                if (fabs(angleDiff) < halfAngle) {
                    // 转向避开碰撞锥（各承担一半角度）
                    float newAngle = theta + (angleDiff > 0 ? halfAngle : -halfAngle);
                    float speed = sqrt(desiredVx*desiredVx + desiredVy*desiredVy);
                    
                    // 执行避让速度（RVO：本方承担一半转向）
                    float evasiveAngle = newAngle + (theta - currentAngle) * 0.5;
                    result.vx = speed * cos(evasiveAngle);
                    result.vy = speed * sin(evasiveAngle);
                    result.conflict = true;
                    Serial.println("🔄 RVO避让");
                }
            }
        }
    }
    return result;
}

// ==================== Setup ====================
void setup() {
    Serial.begin(115200);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
}

// ==================== Loop ====================
void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 期望速度（追踪目标点）
    float desiredVx = (50 - self.x) * 0.05;
    float desiredVy = (50 - self.y) * 0.05;
    
    // 2. RVO计算
    RVOResult cmd = computeRVO(desiredVx, desiredVy);
    
    // 3. 转换为差速指令并驱动
    float v = sqrt(cmd.vx*cmd.vx + cmd.vy*cmd.vy);
    float w = atan2(cmd.vy, cmd.vx);
    float wheelBase = 0.3;
    
    motorL.move(v - w * wheelBase/2);
    motorR.move(v + w * wheelBase/2);
    
    // 更新位置
    self.x += cmd.vx * 0.05;
    self.y += cmd.vy * 0.05;
    
    delay(50);
}

代码要点：RVO与基础VO的核心区别在于“互惠”假设——当双方同时避让时，各承担50%的转向责任。这避免了双方因完全依赖对方避让而导致的“死锁”或反复摆动。

6、编队保持 + VO防碰撞（集中式协调 + 分布式仲裁）
适用场景：多机器人需要保持特定队形（如菱形、V形）行进，同时面临动态障碍物干扰。中央协调器下发队形偏移量，各机器人本地通过VO仲裁避开障碍。

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

BLDCMotor motorL(7), motorR(7);
// 补充Encoder和Driver初始化...

// ==================== 编队参数 ====================
#define NUM_ROBOTS 3
#define FORMATION_SPACING 40.0  // 编队间距(cm)

// 编队模板（相对于领航者的偏移量）
struct FormationOffset {
    float dx, dy;
};

FormationOffset formation[NUM_ROBOTS] = {
    {0, 0},          // 领航者
    {-20, 35},       // 左后
    {20, 35}         // 右后
};

// ==================== 机器人状态 ====================
struct RobotState {
    float x, y;
    float vx, vy;
    int id;
};

RobotState robots[NUM_ROBOTS] = {
    {0, 0, 0, 0, 0},
    {-20, 35, 0, 0, 1},
    {20, 35, 0, 0, 2}
};

// 障碍物信息（来自传感器）
struct Obstacle {
    float x, y;
    float vx, vy;
    float radius;
};

Obstacle obstacle = {100, 50, 0, -1, 10};  // 模拟动态障碍物

// ==================== 全局目标点（由中央协调器下发） ====================
float globalTargetX = 200;
float globalTargetY = 0;

// ==================== VO防碰撞与编队融合 ====================
struct VelocityCmd {
    float vx, vy;
};

VelocityCmd formationWithVO(RobotState self, float leaderVx, float leaderVy, 
                             Obstacle obs, float dt) {
    // 1. 计算编队目标位置（跟随领航者）
    float targetX = robots[0].x + formation[self.id].dx;
    float targetY = robots[0].y + formation[self.id].dy;
    
    // 2. 编队引力速度（趋近目标位置）
    float formationVx = (targetX - self.x) * 0.05;
    float formationVy = (targetY - self.y) * 0.05;
    
    // 3. 跟踪领航者速度（速度一致性）
    float followVx = leaderVx + (targetX - self.x) * 0.03;
    float followVy = leaderVy + (targetY - self.y) * 0.03;
    
    // 4. 合成期望速度（编队为主，速度跟踪为辅）
    float desiredVx = formationVx * 0.6 + followVx * 0.4;
    float desiredVy = formationVy * 0.6 + followVy * 0.4;
    
    // 5. 【核心】VO检测障碍物
    float dx = obs.x - self.x;
    float dy = obs.y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    if (dist < 30.0 && dist > 0) {
        // 计算相对速度
        float relVx = desiredVx - obs.vx;
        float relVy = desiredVy - obs.vy;
        float dot = (relVx*dx + relVy*dy) / dist;
        
        if (dot > 0 && dist < 25.0) {
            // 碰撞风险：垂直方向避让
            float perpX = -dy / dist;
            float perpY = dx / dist;
            float mag = sqrt(desiredVx*desiredVx + desiredVy*desiredVy);
            
            // 根据障碍物位置决定左转还是右转（保持编队方向）
            float side = (obs.x > self.x) ? 1.0 : -1.0;
            desiredVx = perpX * mag * side;
            desiredVy = perpY * mag * side;
            Serial.print("🚧 避障 ID:"); Serial.println(self.id);
        }
    }
    
    return {desiredVx, desiredVy};
}

// ==================== Setup ====================
void setup() {
    Serial.begin(115200);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
}

// ==================== Loop ====================
void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    float dt = 0.05;
    
    // 1. 领航者朝目标前进
    float leaderVx = (globalTargetX - robots[0].x) * 0.02;
    float leaderVy = (globalTargetY - robots[0].y) * 0.02;
    robots[0].vx = leaderVx; robots[0].vy = leaderVy;
    robots[0].x += leaderVx * dt;
    robots[0].y += leaderVy * dt;
    
    // 2. 从机执行编队保持 + VO避障
    for (int i = 1; i < NUM_ROBOTS; i++) {
        VelocityCmd cmd = formationWithVO(robots[i], leaderVx, leaderVy, 
                                           obstacle, dt);
        robots[i].vx = cmd.vx; robots[i].vy = cmd.vy;
        robots[i].x += cmd.vx * dt;
        robots[i].y += cmd.vy * dt;
        
        // 差速驱动
        float v = sqrt(cmd.vx*cmd.vx + cmd.vy*cmd.vy);
        float w = atan2(cmd.vy, cmd.vx);
        float wheelBase = 0.3;
        motorL.move(v - w * wheelBase/2);
        motorR.move(v + w * wheelBase/2);
        
        // 打印调试信息
        Serial.print("Robot"); Serial.print(i);
        Serial.print(": (");
        Serial.print(robots[i].x); Serial.print(", ");
        Serial.print(robots[i].y); Serial.print(") ");
        Serial.print("Target: (");
        Serial.print(robots[0].x + formation[i].dx);
        Serial.print(", ");
        Serial.print(robots[0].y + formation[i].dy);
        Serial.println(")");
    }
    
    // 3. 障碍物移动（模拟）
    obstacle.y -= 1.0 * dt;
    
    delay(50);
}

代码要点：本案例展示了“编队保持 + 避障”的双目标融合。每个机器人需同时满足两个约束：编队引力（保持在编队中的相对位置）和障碍物斥力（VO避让）。通过权重分配（0.6:0.4）实现优先级权衡，既保持队形稳定，又能灵活规避障碍。

要点解读
“集中式协调”与“分布式VO仲裁”的分层架构：中央控制器（如Arduino Mega/树莓派）负责全局A*路径规划和任务分配，各机器人本地执行VO速度仲裁。这种“战略集中、战术分散”架构兼顾了全局最优性与局部实时响应，避免单点性能瓶颈。

VO算法的本质是“速度空间的碰撞预测”：VO通过计算机器人之间的相对位置与相对速度，在速度空间中划定一个“碰撞锥”（Velocity Cone）。如果期望速度落在此锥内，则存在碰撞风险，需强制转向。这一预测机制使机器人在碰撞前主动避让，而非“撞上了再躲”。

RVO解决“死锁”问题的关键在于“互惠假设”：基础VO假设对方不避让，可能导致双方因互相“抢道”而进入死锁。RVO（互惠速度障碍）引入“各承担一半避让责任”的假设，使双方同时转向，协作完成避障。实验表明，RVO可将速度共识误差降低至周期性通信方案的1.25%。

BLDC FOC是实现“平滑仲裁”的执行保障：VO仲裁输出的速度指令往往频繁变化（转向、减速、加速）。BLDC电机的FOC控制可实现毫秒级力矩响应和低速平稳运行，使机器人执行避让动作时流畅自然，避免“锯齿状”路径或机械冲击。

通信延迟与算力限制是工程落地的关键挑战：VO算法依赖各机器人实时共享位置和速度信息，通信丢包或延迟会导致VO预测失效。工程解决方案包括：使用事件触发通信（仅在状态变化显著时广播，减少带宽占用）、采用TDMA时隙分配避免数据碰撞、以及为定位误差留出安全余量（适当膨胀机器人边界）。

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