基于Arduino与BLDC（无刷直流电机）构建的双机器人系统，其“VFF（虚拟力场法）基础力场与BLDC差速执行”是一套将分布式多智能体协同算法与底层高精度运动控制深度融合的架构。该方案通过构建虚拟力场实现去中心化的自主避障与协同，并利用BLDC的高动态响应能力精准执行差速运动指令。
以下是该系统的核心技术拆解：

主要特点

1. 去中心化的VFF分布式决策架构
   系统采用人工势场法（APF）的核心思想，将目标点建模为引力场，障碍物（含另一台机器人）建模为斥力场。每台机器人通过局部传感器（如超声波、激光雷达）和邻近通信（如NRF24L01、CAN总线）独立感知环境并计算合力矢量。这种去中心化架构避免了集中式控制的单点故障风险，实现了真正的分布式自组织协同。
2. 动态力场与局部极小值逃逸
   在双机器人协同中，VFF算法不仅处理静态障碍物，还需处理动态的“互斥”问题。当两台机器人陷入对称布局导致的“死锁”（相互排斥又无法脱离）或局部极小值时，系统会引入随机扰动项或切换至全局路径规划（如A*算法）辅助脱困。同时，算法会根据环境复杂度实时调整引力/斥力系数，优化运动效率。
3. BLDC差速执行与FOC高动态响应
   VFF算法输出的合力矢量会被解算为期望的线速度和角速度，进而通过差速运动学模型转换为左右轮的独立速度指令。BLDC电机配合FOC（磁场定向控制）驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪VFF输出的高频、连续变化的速度指令，确保机器人在复杂力场环境下的运动平滑性与敏捷性。
4. 多模态传感器融合与闭环协同
   系统形成严密的“感知-规划-执行”闭环：局部传感器融合IMU、里程计等数据，通过卡尔曼滤波降低噪声对VFF计算的影响；BLDC底层控制器通过双闭环（速度环+电流环）精准执行差速指令，并利用编码器反馈实时校正里程计，确保双机器人在协同过程中的位姿估计准确，避免因累积误差导致的协同失效。

应用场景

1. 智能仓储与物流分拣
   在电商仓库中，多台AGV（自动导引车）在货架间执行货物搬运。VFF控制可实时避开临时堆放的物料或其他AGV，同时通过全局路径规划优化整体运输效率，减少拥堵。双机器人协同搬运时，VFF力场可确保两者保持安全距离，避免碰撞。
2. 灾难救援与危险区域探索
   在地震废墟或核辐射区域，机器人集群通过VFF协同避障，结合热成像或气体传感器快速覆盖大面积未知区域。当单台机器人因障碍物被困时，另一台可通过力场感知其状态并自动补位或提供辅助，提升救援效率与安全性。
3. 安防巡逻编队
   双机器人以特定编队（如菱形、纵队）在园区内协同巡逻，覆盖更宽的监控视野。当遇到障碍物（如停放的车辆）时，VFF力场驱动编队自适应旋转绕行，保持队形完整性。UWB定位确保各机器人在无GPS环境下维持精确的相对位置。
4. ROS导航算法验证与科研
   该系统常被用作验证ROS（机器人操作系统）多智能体协同算法的硬件平台，特别是在测试分布式避障、编队控制等算法时，能够直观地观察和调优VFF力场参数与BLDC底层执行性能。

注意事项

1. 传感器选型与融合
   激光雷达（LiDAR）： 是构建高精度局部代价地图的核心，提供360°点云数据，但成本较高。
   超声波/红外： 成本低，适合作为近距离防撞的补充，但易受环境干扰，需进行多传感器数据融合以消除盲区。
2. VFF参数调优与局部极小值处理
   VFF算法的效果高度依赖于引力/斥力系数的设定。在实际应用中，需要根据具体场景反复调试各项指标的权重系数，以在“追求效率”和“保证安全”之间找到最佳平衡点。同时，必须设计有效的局部极小值逃逸机制，避免机器人陷入死锁。
3. 计算性能瓶颈
   虽然VFF算法相对简单，但在Arduino（特别是基础款Uno/Nano）上运行复杂的力场计算与多传感器融合仍具挑战。建议优先选用ESP32或Arduino Due等具备更高主频和更大RAM的高性能微控制器，以保证控制周期的实时性。
4. 动力学参数标定
   差速执行的效果高度依赖于机器人运动学参数的准确性。必须精确标定BLDC底盘的最大线速度、最大角速度、最大加速度和最大减速度。参数偏差会导致规划出的轨迹在实际执行时发生碰撞或偏离。
5. 通信延迟与同步
   在双机器人协同中，通信延迟会直接影响VFF力场的计算精度。需确保通信协议（如NRF24L01、CAN）的实时性，并对时间戳进行同步，减少因时钟漂移导致的协同误差。

1、单机器人 VFF 基础力场计算与差速执行
此案例聚焦于 VFF 算法在单台 Arduino BLDC 差速机器人上的最小可行实现。核心逻辑是将目标引力场与障碍斥力场叠加，合成虚拟合力矢量，再将合力方向与大小解算为左右轮速度指令，驱动 BLDC 电机执行。

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

// BLDC 差速底盘电机定义
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM drvR = BLDCDriver3PWM(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;   // 目标点坐标 (m)
const float ATTRACT_GAIN = 0.8;            // 引力增益系数
const float REPULSE_GAIN = 6.0;            // 斥力增益系数
const float REPULSE_RANGE = 1.2;           // 斥力作用范围 (m)

// 机器人位置估算（简化，实际应使用编码器里程计）
float selfX = 0.0, selfY = 0.0;

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;
}

// ===== VFF 力场计算核心 =====
void computeVFF(float& fx, float& fy) {
    // 1. 目标引力：指向目标点
    fx = (GOAL_X - selfX) * ATTRACT_GAIN;
    fy = (GOAL_Y - selfY) * ATTRACT_GAIN;
    
    // 2. 超声波障碍斥力
    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;
        fy -= obsF * 0.2;  // 侧向分量辅助绕行
    }
}

void loop() {
    // 1. 计算 VFF 合力
    float fx, fy;
    computeVFF(fx, fy);
    
    // 2. 合力大小与方向
    float forceMag = sqrt(fx * fx + fy * fy);
    float forceAngle = atan2(fy, fx);
    
    // 3. 转换为差速底盘速度指令
    // 线速度：与合力大小成正比，限制在合理范围
    float vLin = constrain(forceMag * 5.0, 0, 0.8);
    // 角速度：与合力方向偏差相关
    float vAng = constrain(forceAngle * 1.2, -0.6, 0.6);
    
    float wheelBase = 0.25;  // 轮距 (m)
    motorL.target = vLin - vAng * wheelBase / 2.0;
    motorR.target = vLin + vAng * wheelBase / 2.0;
    
    // 4. 执行 FOC
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 5. 简化位置更新（实际应使用编码器）
    selfX += fx * 0.05;
    selfY += fy * 0.05;
    
    delay(50);
}

核心逻辑说明：VFF 将“目标追踪”与“避障”统一为力场合成问题。目标产生引力（指向目标），障碍物产生斥力（远离障碍），合力方向即为期望运动方向。超声波传感器的不准确数据被栅格化处理，每个单元格对机器人施加与其累积值成正比、与距离成反比的斥力。BLDC 配合 FOC 的毫秒级动态响应，能将连续的 VFF 速度指令精准执行，避免传统有刷电机的“抖动”问题。

2、双机器人 VFF 协同力场与主从通信
此案例在单机器人基础上引入双机协同。两台机器人通过串口交换状态信息（位置、目标），每台机器人将对方视为动态障碍物，斥力强度随距离减小而急剧增大，实现协同避障与目标跟踪。

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

// ===== 双机器人状态结构体 =====
struct RobotState {
    float x, y;           // 位置
    float targetX, targetY; // 目标点
    bool hasTarget;       // 是否检测到目标
};

// 自身与邻居状态
RobotState self = {0, 0, 4.0, 0, false};
RobotState neighbor = {0, 0, 0, 0, false};

// ===== 通信参数（串口点对点）=====
#define ROBOT_BAUD_RATE 115200

// ===== VFF 参数 =====
const float ATTRACT_GAIN = 0.8;
const float NEIGHBOR_REPULSE_GAIN = 8.0;   // 邻居斥力增益（比障碍物更高）
const float NEIGHBOR_REPULSE_RANGE = 1.5;  // 邻居斥力范围
const float OBSTACLE_REPULSE_GAIN = 6.0;

// BLDC 电机与传感器（同案例一，略）
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);
NewPing sonar(12, 13, 200);

void setup() {
    Serial.begin(ROBOT_BAUD_RATE);
    // 电机初始化（同案例一，略）
}

// ===== 发送自身状态给邻居 =====
void sendState() {
    Serial.write((uint8_t*)&self, sizeof(RobotState));
}

// ===== 接收邻居状态 =====
void receiveState() {
    if (Serial.available() >= sizeof(RobotState)) {
        Serial.readBytes((uint8_t*)&neighbor, sizeof(RobotState));
    }
}

// ===== 双机器人 VFF 计算 =====
void computeVFF(float& fx, float& fy) {
    // 1. 目标引力
    fx = (self.targetX - self.x) * ATTRACT_GAIN;
    fy = (self.targetY - self.y) * ATTRACT_GAIN;
    
    // 2. 邻居机器人斥力（防碰撞）
    float dx = self.x - neighbor.x;
    float dy = self.y - neighbor.y;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < NEIGHBOR_REPULSE_RANGE && dist > 0.01) {
        // 斥力与距离成反比，近距离急剧增大
        float neighborF = NEIGHBOR_REPULSE_GAIN * (1.0 - dist / NEIGHBOR_REPULSE_RANGE) / (dist + 0.1);
        fx += dx / dist * neighborF;
        fy += dy / dist * neighborF;
    }
    
    // 3. 超声波障碍斥力
    int obsDist = sonar.ping_cm();
    if (obsDist > 0 && obsDist < 120) {
        float obsF = OBSTACLE_REPULSE_GAIN * (1.0 - obsDist / 120.0);
        fx -= obsF;
    }
}

void loop() {
    // 1. 通信：交换状态
    receiveState();
    sendState();
    
    // 2. 目标共享：若自身无目标但邻居有，跟随邻居目标
    if (!self.hasTarget && neighbor.hasTarget) {
        self.targetX = neighbor.targetX;
        self.targetY = neighbor.targetY;
    }
    
    // 3. VFF 计算与执行
    float fx, fy;
    computeVFF(fx, fy);
    
    float vLin = constrain(sqrt(fx*fx + fy*fy) * 5.0, 0, 0.8);
    float vAng = constrain(atan2(fy, fx) * 1.2, -0.6, 0.6);
    
    float wheelBase = 0.25;
    motorL.target = vLin - vAng * wheelBase / 2.0;
    motorR.target = vLin + vAng * wheelBase / 2.0;
    
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();
    
    delay(20);
}

核心逻辑说明：双机器人 VFF 的核心是“去中心化自组织”。每台机器人独立计算 VFF，无需中央调度器。通过串口或 ESP-NOW 交换位置和目标信息，将邻居视为“动态障碍物”，斥力增益显著高于静态障碍物，实现“优先避人（机）、其次避墙”。主从互补策略让一台机器人负责大范围搜索，另一台负责近距离跟踪，通过状态共享弥补各自传感器盲区。

3、BLDC FOC 高动态执行层与防死锁机制
此案例聚焦于 VFF 指令在 BLDC 电机上的高动态执行，并加入防死锁机制。纯 VFF 在两机对称相遇时可能因引力与斥力恰好平衡而陷入“死锁”。通过检测停滞状态并注入随机扰动或优先级逃逸力，打破对称性。

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

// ===== BLDC 电机（带 FOC 电流环配置）=====
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// FOC 电流环 PID 参数（影响转矩响应带宽）
float currentKp = 0.5, currentKi = 100.0;

// ===== 死锁检测参数 =====
const float STALL_SPEED_THRESHOLD = 0.05;   // 速度阈值
const unsigned long STALL_TIME_THRESHOLD = 800; // 持续停滞时间 (ms)
unsigned long stallStartTime = 0;
bool stallDetected = false;

// ===== 随机逃逸参数 =====
const float RANDOM_WALK_FORCE = 1.5;
const unsigned long RANDOM_WALK_DURATION = 600;
unsigned long randomWalkStartTime = 0;
bool randomWalkActive = false;

// ===== VFF 参数与状态（简化）=====
float selfX = 0, selfY = 0;
float neighborX = 2, neighborY = 0;  // 邻居位置（通过通信获取）
float targetX = 4, targetY = 0;

void setup() {
    Serial.begin(115200);
    
    // 电机初始化 + FOC 电流环配置
    drvL.init(); drvR.init();
    motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
    motorL.init(); motorR.init();
    motorL.initFOC(); motorR.initFOC();
    
    // 配置电流环 PID（高带宽提升转矩响应）
    motorL.PID_current_q.P = currentKp;
    motorL.PID_current_q.I = currentKi;
    motorR.PID_current_q.P = currentKp;
    motorR.PID_current_q.I = currentKi;
    
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
}

// ===== VFF 计算（含邻居斥力）=====
void computeVFF(float& fx, float& fy) {
    // 目标引力
    fx = (targetX - selfX) * 0.8;
    fy = (targetY - selfY) * 0.8;
    
    // 邻居斥力
    float dx = selfX - neighborX;
    float dy = selfY - neighborY;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < 1.2 && dist > 0.01) {
        float f = 8.0 * (1.0 - dist/1.2) / (dist + 0.1);
        fx += dx / dist * f;
        fy += dy / dist * f;
    }
}

void loop() {
    // 1. VFF 基础计算
    float fx, fy;
    computeVFF(fx, fy);
    
    float forceMag = sqrt(fx*fx + fy*fy);
    
    // ===== 2. 死锁检测 =====
    if (forceMag < STALL_SPEED_THRESHOLD) {
        if (!stallDetected) {
            stallDetected = true;
            stallStartTime = millis();
        }
        // 持续停滞 → 触发随机逃逸
        if (millis() - stallStartTime > STALL_TIME_THRESHOLD && !randomWalkActive) {
            randomWalkActive = true;
            randomWalkStartTime = millis();
            Serial.println("DEADLOCK! Random escape activated");
        }
    } else {
        stallDetected = false;
        stallStartTime = 0;
    }
    
    // 3. 随机逃逸：注入随机方向扰动
    if (randomWalkActive) {
        float randomAngle = random(0, 628) / 100.0;
        fx += cos(randomAngle) * RANDOM_WALK_FORCE;
        fy += sin(randomAngle) * RANDOM_WALK_FORCE;
        
        if (millis() - randomWalkStartTime > RANDOM_WALK_DURATION) {
            randomWalkActive = false;
            stallDetected = false;
            Serial.println("Escape complete, resuming VFF");
        }
    }
    
    // 4. 差速解算与 BLDC 执行
    float vLin = constrain(sqrt(fx*fx + fy*fy) * 5.0, 0, 0.8);
    float vAng = constrain(atan2(fy, fx) * 1.2, -0.6, 0.6);
    float wheelBase = 0.25;
    
    motorL.target = vLin - vAng * wheelBase / 2.0;
    motorR.target = vLin + vAng * wheelBase / 2.0;
    
    motorL.move(motorL.target);
    motorR.move(motorR.target);
    motorL.loopFOC();
    motorR.loopFOC();
    
    // 5. 简化位置更新
    selfX += fx * 0.03;
    selfY += fy * 0.03;
    
    delay(20);
}

核心逻辑说明：FOC（磁场定向控制）通过 Clarke 变换和 Park 变换将三相电流解耦为直轴（励磁）和交轴（转矩）分量，分别施加独立 PI 调节器，实现正弦化电流波形，消除六步换相的转矩脉动。VFF 输出的速度指令需要 BLDC 的快速响应能力——FOC 控制周期可达 50μs 级，确保机器人能快速响应速度指令，减少惯性导致的过冲。随机逃逸机制通过注入随机方向扰动打破对称死锁，是纯 VFF 在双机器人场景中的必要补充。

要点解读

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

VFF 将目标追踪和避障统一为向量合成问题。目标产生引力（指向目标），障碍物和邻居产生斥力（远离障碍），合力方向即为运动方向。这种数学建模方式能自然处理多障碍场景，生成平滑的避障轨迹，而非生硬的“急转弯”或“停车-转向-再前进”的离散动作。VFF 最初由 Borenstein 和 Koren 提出，将 certainty grids 用于障碍物表示、potential fields 用于导航，特别适合超声波传感器的不准确数据。

2. BLDC+FOC 是 VFF 连续速度指令精准执行的保障

VFF 输出的是连续的速度向量（Vx, Vy）和角速度（Wz），要求执行机构具备高动态响应能力。FOC 通过正弦化电流驱动消除转矩纹波，双机在狭窄通道会车时不会因力矩脉动导致队形抖动。FOC 控制周期可达 50μs 级，确保机器人能快速响应 VFF 输出的速度指令，减少惯性导致的过冲或振荡。SimpleFOC 库的 MotionControlType::velocity 模式是实现速度闭环的标准方法。

3. 双机器人协同依赖去中心化架构与实时通信

双机器人 VFF 的核心设计哲学是“去中心化自组织”。每台机器人通过局部传感器和邻近通信独立决策，无需中央调度器。对等通信（ESP-NOW 或串口）交换位置、速度、航向等状态信息，通信延迟需控制在 10ms 以内。即使一台机器人失效，另一台仍能依靠本地 VFF 继续执行避障任务，系统不会完全瘫痪。主从互补策略让双机通过状态共享弥补各自传感器盲区。

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

纯 VFF 在两机对称相遇时，引力与斥力恰好平衡，合力为零，机器人陷入“死锁”或“振荡”状态。解决方案包括：随机游走模式（注入随机方向扰动打破对称）、虚拟目标点切换（设置临时目标引导脱离困境）、混合全局规划触发（VFF 连续失效时触发 A* 重规划）。案例三的停滞检测机制通过监测速度持续低于阈值来触发逃逸，是工程上最简单有效的实现方式。

5. 超声波传感器的数据融合是 VFF 工程落地的关键

超声波传感器存在波束角宽、镜面反射和数据跳变等问题，单一读数不能直接用于力场计算。VFF 采用栅格法表示障碍物，每个单元格有一个累积值 Cf 表示该处存在障碍物的可信度。当超声波检测到障碍物时，只更新传感器轴线上对应距离的栅格累积值，通过多次扫描逐步积累可信度。这种方法有效抑制了超声波数据的噪声，同时实现了多传感器数据的融合。

4、双机器人VFF基础避障与目标跟踪（I2C通信协同）
适用场景：两辆差速机器人协同执行目标趋近任务，需实时避开静态障碍物和彼此，同时保持对全局目标点的跟踪，适用于仓储分拣、人机协作跟随等场景。

核心逻辑：每台机器人独立运行VFF算法，目标点产生引力，静态障碍物和其他机器人产生斥力，合力决定运动方向；机器人间通过I2C总线交换位置信息，实现“互斥”避碰，无需中心调度。

#include <SimpleFOC.h>
#include <Wire.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);

// ==================== 机器人状态 ====================
#define MY_ID 1
#define I2C_ADDR 0x20 + MY_ID  // 0x21 或 0x22
struct RobotState {
    float x, y;
    uint8_t priority;  // 优先级：越高越有"路权"
};
RobotState self = {0, 0, 2};    // 自身位置与优先级
RobotState other = {0, 0, 1};   // 邻居状态
// ==================== VFF参数 ====================
const float GOAL_X = 4.0, GOAL_Y = 2.0;
const float ATTRACT_GAIN = 0.02;
const float REPULSE_GAIN = 6.0;
const float REPULSE_RANGE = 1.2;   // 斥力作用范围(m)
// ==================== I2C通信 ====================
void receiveEvent(int howMany) {
    if (Wire.available() >= 12) {
        uint8_t buf[12];
        for(int i=0; i<12; i++) buf[i] = Wire.read();
        other.x = *(float*)(buf);
        other.y = *(float*)(buf + 4);
        other.priority = buf[8];
    }
}
void requestEvent() {
    uint8_t buf[12];
    *(float*)(buf) = self.x;
    *(float*)(buf + 4) = self.y;
    buf[8] = self.priority;
    Wire.write(buf, 12);
}

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

    // I2C从机模式

    Wire.begin(I2C_ADDR);
    Wire.onReceive(receiveEvent);
    Wire.onRequest(requestEvent);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();

    // ==================== 1. VFF力场计算 ====================
    // 目标引力

    float fx = (GOAL_X - self.x) * ATTRACT_GAIN;
    float fy = (GOAL_Y - self.y) * ATTRACT_GAIN;

    // 邻居斥力（互斥避碰核心）

    float dx = self.x - other.x;
    float dy = self.y - other.y;
    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.01);
        fx += f * dx / dist;
        fy += f * dy / dist;
    }

    // 静态障碍物斥力（超声波/红外模拟）
    // ... (从传感器读取障碍物距离并加入斥力)

    // ==================== 2. 优先级柔化处理 ====================
    // 当距离极近时，低优先级主动让行

    if (dist < 0.6) {
        if (self.priority < other.priority) {
            // 低优先级：施加垂直于运动方向的逃逸力
            float perpX = -dy / (dist + 0.01);
            float perpY = dx / (dist + 0.01);
            fx += perpX * 0.5;
            fy += perpY * 0.5;
        } else {
            // 高优先级：小幅减速，让低优先级先过
            fx *= 0.85;
            fy *= 0.85;
        }
    }

    // ==================== 3. 差速驱动 ====================
    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;
    // ...（补充电机控制代码，将vLin、vAng转化为左右轮速度）
}

5、双机器人VFF互斥避障+优先级逃逸（窄通道错车）
适用场景：两台AGV在仓库窄通道相向而行，需互惠避让避免“死锁”，同时保持对各自目标的跟踪，适用于窄通道物流搬运、产线协同等场景。

核心逻辑：每台机器人独立运行VFF人工势场，目标点产生引力，对方机器人和静态障碍物产生斥力；当距离过近时启动互斥逃逸，低优先级机器人主动偏航让行，高优先级小幅减速配合，优先级通过无线通信交换，实现分布式决策。

// 核心逻辑参考案例1的VFF力场计算，补充优先级通信与逃逸策略
// 1. 优先级通信（以ESP-NOW为例，实现双机状态同步）
#include <ESP8266WiFi.h>
#include <ESPNow.h>

// 定义从机和主机的MAC地址
uint8_t masterMac[6] = {0x12, 0x34, 0x56, 0x78, 0x9A, 0xBC};
uint8_t slaveMac[6] = {0xDE, 0xF0, 0x12, 0x34, 0x56, 0x78};

struct RobotStatus {
  float x, y;
  uint8_t priority;
};
RobotStatus selfStatus, otherStatus;

// 初始化ESP-NOW通信
void initEspNow() {
  WiFi.mode(WIFI_STA);
  if (ESPNow.begin() == ESPNOW_OK) {
    // 主机注册从机，从机注册主机
    ESPNow.addPeer(masterMac, 6);
    ESPNow.addPeer(slaveMac, 6);
  }
}

// 接收回调函数
void onRecv(uint8_t *mac, uint8_t *data, int len) {
  RobotStatus *recvStatus = (RobotStatus *)data;
  otherStatus = *recvStatus;
}

// 2. VFF互斥避障+优先级逃逸核心逻辑
void loop() {
  // 1. 接收对方机器人状态
  ESPNow.setCallback(onRecv);

  // 2. VFF力场计算（同案例1，补充目标引力、障碍斥力、对方斥力）
  // ...（省略重复的VFF基础计算，重点补充优先级逃逸）

  // 3. 优先级逃逸策略
  float dx = selfStatus.x - otherStatus.x;
  float dy = selfStatus.y - otherStatus.y;
  float dist = sqrt(dx*dx + dy*dy);
  if (dist < 0.5) { // 进入互斥临界距离
    if (selfStatus.priority < otherStatus.priority) {
      // 低优先级：主动偏航，施加垂直于连线的逃逸力
      float escapeAngle = atan2(dy, dx) + M_PI/2;
      fx += cos(escapeAngle) * 0.8;
      fy += sin(escapeAngle) * 0.8;
    } else {
      // 高优先级：减速配合，降低引力输出
      fx *= 0.7;
      fy *= 0.7;
    }
  }

  // 4. BLDC差速执行（同案例1的差速驱动逻辑）
  // ...（省略电机控制代码）
}

6、双机器人UWB定位+虚拟弹簧协同编队（主从跟随）
适用场景：主AGV负责长货架搬运，从AGV跟随并辅助支撑，需保持恒定编队间距与避碰，适用于仓储协同搬运、农业编队作业等场景。

核心逻辑：主机器人按路径行驶，从机器人通过双UWB差分定位获取主机的相对位置，采用虚拟弹簧模型保持期望间距，间距过大时加速追赶，间距过近时减速后退；BLDC FOC确保速度指令精准响应，实现“软连接”跟随。

#include <SimpleFOC.h>
#include <Wire.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);
// ==================== 双UWB定位参数 ====================
#define ROBOT_ID 2  // 1=主机, 2=从机
float targetPos[2] = {0, 0};   // 主机位置（通过UWB获取）
float currentPos[2] = {0, 0};  // 从机自身位置
float deltaPos[2] = {0, 0};    // 相对位置
// ==================== 虚拟弹簧跟随参数 ====================
const float DESIRED_DIST_X = -0.8;   // 期望相对X偏移（主车后0.8m）
const float DESIRED_DIST_Y = 0.0;    // 期望相对Y偏移（同车道）
const float SPRING_K = 1.5;          // 弹簧刚度
const float DAMPING_K = 0.3;         // 阻尼系数
const float MAX_SPEED = 1.2;         // 最大跟随速度
// ==================== 超声波近场避障 ====================
#define TRIG_F 2
#define ECHO_F 3

NewPing sonarFront(TRIG_F, ECHO_F, 200);
const float SAFE_DIST = 0.4;  // 前向安全距离(m)
void setup() {

    Serial.begin(115200);
    
    // 初始化BLDC电机

    motorL.linkSensor(&encoderL);

    motorR.linkSensor(&encoderR);

    motorL.linkDriver(&driverL);

    motorR.linkDriver(&driverR);

    motorL.init(); motorL.initFOC();

    motorR.init(); motorR.initFOC();

    motorL.controller = MotionControlType::velocity;

    motorR.controller = MotionControlType::velocity;
    
    // 初始化UWB模块（DW1000）

    // uwb_init(ROBOT_ID);
}
// ==================== 主机位置获取（UWB差分定位）====================
void updateUWB() {

    // 实际通过UWB模块读取，此处模拟

    // uwb_get_position(targetPos);

    // uwb_get_self_position(currentPos);

    deltaPos[0] = targetPos[0] - currentPos[0];

    deltaPos[1] = targetPos[1] - currentPos[1];
}
void loop() {

    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. UWB定位更新

    updateUWB();
    
    // 2. 虚拟弹簧力计算

    float dx = deltaPos[0] - DESIRED_DIST_X;

    float dy = deltaPos[1] - DESIRED_DIST_Y;
    float fx = SPRING_K * dx + DAMPING_K * (dx - lastDx) / 0.05;

    float fy = SPRING_K * dy + DAMPING_K * (dy - lastDy) / 0.05;
    lastDx = dx; lastDy = dy;
    
    // 3. 前向超声波避障（硬优先级）

    float frontDist = sonarFront.ping_cm() / 100.0;

    if (frontDist > 0 && frontDist < SAFE_DIST) {

        motorL.move(-0.4); motorR.move(-0.4);

        delay(300);

        return;

    }
    
    // 4. 差速驱动

    float vLin = constrain(sqrt(fx*fx + fy*fy) * 1.2, 0, MAX_SPEED);

    float vAng = constrain(atan2(fy, fx) * 1.2, -0.6, 0.6);
    float wheelBase = 0.3;

    motorL.move(vLin - vAng * wheelBase / 2);

    motorR.move(vLin + vAng * wheelBase / 2);
    
    delay(50);
}

要点解读

1. VFF力场的“引力-斥力”协同设计：实现动态平衡
   VFF基础力场的核心是通过引力与斥力的向量合成，实现避障与目标跟踪的动态平衡。引力由目标点产生，驱动机器人趋近目标；斥力由障碍物和其他机器人产生，实现避碰。案例4中，通过I2C同步机器人位置，将对方机器人纳入斥力计算，避免相互碰撞；案例2引入优先级机制，对斥力进行柔化处理，解决窄通道死锁问题；案例3采用虚拟弹簧模型，本质是引力与阻尼力的协同，实现主从机器人的柔性跟随，而非刚性约束，避免因机械误差导致内应力损坏。

2. BLDC差速执行的“高精度闭环”：保障力场指令落地
   BLDC差速执行是VFF力场指令落地的关键，需依托高精度闭环控制，匹配力场输出的连续速度指令。三个案例均采用FOC磁场定向控制，实现毫秒级扭矩响应和低转矩脉动，确保机器人能平滑执行加速、减速和转向动作。差速驱动的运动学解算需精准，通过公式将VFF输出的线速度和角速度转化为左右轮转速，且需结合编码器反馈实现闭环，避免因轮子打滑导致轨迹偏差，保障力场指令的精准执行。

3. 双机协同的“通信与同步”：解决信息时滞难题
   双机器人协同的核心挑战是通信延迟与状态同步，信息时滞会导致VFF计算基于过时数据，引发误判或振荡。案例4采用I2C短距离通信，实现低延迟状态同步，适合近距离协同；案例5采用ESP-NOW无线通信，通过时间戳同步和状态预测补偿延迟，解决远距离协同的时滞问题；案例6通过UWB差分定位获取主机位置，结合卡尔曼滤波预测主机运动状态，避免因通信延迟导致的跟随滞后。通信协议需兼顾低延迟与可靠性，且需配套时间戳机制和状态预测算法，确保双机信息同步。

4. 局部极小值与死锁的“逃逸机制”：提升系统鲁棒性
   纯VFF算法易陷入局部极小值，导致机器人停滞或振荡，需设计逃逸机制提升鲁棒性。案例5通过优先级柔化处理，在窄通道互斥场景中，低优先级机器人主动偏航，高优先级减速配合，避免相互僵持；案例4预留静态障碍物斥力接口，可扩展随机扰动或虚拟目标点策略，当检测到长期停滞时，注入随机速度扰动，重构力场梯度，引导机器人脱离局部极值区域。逃逸机制需结合场景设计，避免过度扰动导致轨迹失控，同时需与底层运动学约束结合，确保逃逸动作平滑可行。

5. 算力与实时性的“平衡策略”：适配Arduino硬件特性
   VFF算法、传感器处理与BLDC闭环控制对算力要求较高，需结合Arduino硬件特性平衡算力与实时性。三个案例均建议采用ESP32、STM32等高性能MCU，替代标准Arduino Uno，以满足浮点运算和高频控制需求；软件层面需避免阻塞函数，采用非阻塞定时，保证控制周期稳定；同时简化VFF计算，减少不必要的迭代，将非关键逻辑（如日志记录）移至协处理器。此外，底层采用FOC硬件闭环，减轻CPU负担，确保电机控制的实时性，实现算力资源向核心算法倾斜，保障系统整体响应速度。

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