在基于Arduino与BLDC（无刷直流电机）构建的多机器人系统中，“RVO + DWA混合避障”代表了从“被动反应式避障”向“预测性协同与局部最优规划”的跨越。从专业视角来看，该机制融合了多智能体博弈论、速度空间采样与底层高动态运动控制，有效解决了复杂动态环境中多机协同的安全与效率问题。以下是关于该技术的详细解析：
一、 主要特点
全局与局部的解耦协同架构
系统采用分层设计，A算法作为全局规划器提供宏观的“理想走廊”（Waypoints），而RVO与DWA作为局部规划器负责微观的“战术闪避”与实时避障。这种解耦设计使得机器人既不盲从全局路径，又能在避开动态障碍后平滑回归全局轨迹，系统模块化程度高且易于调试。
基于速度障碍法（RVO）的预测性防碰撞
针对多机器人协同，系统引入互惠速度障碍法（RVO）。RVO通过计算速度障碍锥，预测未来时刻的碰撞风险，并在当前速度空间中选取无碰撞的最优速度向量。这种机制本质上是前瞻性的，从物理上杜绝了碰撞可能性，且具备去中心化鲁棒性，即使上位机失效，单机仍能依靠本地RVO实现基本的避撞。
基于动态窗口法（DWA）的局部轨迹优化
在RVO保障基础安全的前提下，DWA算法在速度空间内采样多组可行的线速度和角速度，模拟短期轨迹。结合BLDC底盘的运动学约束（如最大加减速、转弯半径），DWA通过评价函数（综合考虑目标方向、安全距离、速度及全局路径贴合度）选出最优解，确保机器人能够平滑、高效地绕过动态障碍物。
底层BLDC的高动态响应与平滑执行
混合避障算法输出的速度和角速度指令是连续且高频变化的。BLDC电机配合FOC（磁场定向控制）驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪RVO和DWA输出的平滑轨迹，避免了传统步进电机在频繁启停和转向时的机械冲击与轨迹偏差。
二、 应用场景
智能仓储多AGV协同调度
在电商仓库或柔性制造车间，多台AGV需同时执行拣货任务。A规划各自取货路径，RVO负责在狭窄通道口、货架转角处实现自主会车和无冲突通行，DWA则应对突然出现的工人或临时堆放的货物，保障物流效率与人员安全。
服务与导览机器人的密集人流穿梭
在商场、医院、酒店等复杂且人流密集的室内环境中，机器人需要频繁应对突然停步的顾客或横穿的宠物。RVO+DWA混合机制能提供极其平滑的避让体验，避免急刹或剧烈转向带来的乘客不适感或物品倾覆风险。
集群表演与科研验证平台
在无人机/小车编队表演或高校科研中，A*规划整体队形变换轨迹，RVO确保个体间在高速飞行/行驶中保持安全间距。该系统是验证局部规划算法与底层电机控制耦合特性的理想平台。
三、 需要注意的事项
主控算力瓶颈与实时性保障
RVO的速度障碍锥计算与DWA的多轨迹推演对浮点运算要求极高。标准的Arduino Uno极易成为算力瓶颈，导致控制周期过长。强烈建议采用ESP32、STM32等高性能MCU，或使用ROS系统配合上位机（如树莓派/Jetson）运行规划器，Arduino仅作为底层BLDC速度执行器。
非完整约束与“死锁”问题
标准的RVO假设机器人是全向移动的，但实际差速驱动的BLDC机器人不能横向移动。这可能导致算出的避让速度在实际中无法执行，或在狭窄空间出现多机“互锁”。对策是在VO计算中引入机器人的运动学模型，并引入“礼让”规则，当检测到死锁时，优先级低的机器人执行“后退-等待”策略。
通信延迟与状态不一致
RVO算法要求每个机器人必须实时共享自己的精确位置、速度和朝向。若通信存在延迟，机器人感知到的对方位置是“过去时”，会导致预测失效。需在通信协议中附带时间戳，接收方根据延迟时间预测对方当前状态，并适当膨胀碰撞检测的边界（Bounding Box）以留出安全余量。
DWA参数调优与轨迹平滑性
DWA算法的性能高度依赖参数配置。模拟时间（Sim Time）设置过短会导致机器人“目光短浅”错过远处障碍，过长则增加计算延迟；评价函数的权重（如安全权重、速度权重）若设置不当，极易导致机器人在接近目标或绕过障碍时出现轨迹大幅偏折、减速停顿甚至原地打转（Twirling）。必须结合实际BLDC底盘的运动学参数进行精细调优。

1、RVO互惠避让 + DWA速度采样（双机相遇避让）
适用场景：两台机器人在狭长走廊中迎面相遇，需要协同避让。RVO确保双方各承担一半避让责任，DWA对选出的避让速度进行运动学约束，保证速度可达。

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

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

// ==================== 机器人状态 ====================
struct RobotState {
    float x, y;          // 位置(m)
    float vx, vy;        // 速度(m/s)
    float radius;        // 机器人半径(m)
};
RobotState self = {0, 0, 0, 0, 0.2};
RobotState neighbor = {2.0, 0.5, -0.3, 0, 0.2};

// ==================== DWA约束参数 ====================
const float MAX_V = 0.8;       // 最大线速度(m/s)
const float MAX_W = 1.2;       // 最大角速度(rad/s)
const float MAX_ACCEL = 1.0;   // 最大加速度(m/s²)
const float DT = 0.1;
const int SIM_STEPS = 6;

void setup() {
    Serial.begin(115200);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
}

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

    // 1. 计算期望速度（向目标前进）
    float goalX = 5.0, goalY = 0;
    float desVx = (goalX - self.x) * 0.3;
    float desVy = (goalY - self.y) * 0.3;
    float desSpeed = sqrt(desVx*desVx + desVy*desVy);
    if (desSpeed > MAX_V) {
        desVx = desVx / desSpeed * MAX_V;
        desVy = desVy / desSpeed * MAX_V;
    }

    // 2. 【核心】RVO速度区域计算
    float safeVx = desVx, safeVy = desVy;
    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;

    if (dist > 0.01 && dist < 2.5) {
        // 相对速度
        float relVx = desVx - neighbor.vx;
        float relVy = desVy - neighbor.vy;
        float dot = (relVx*dx + relVy*dy) / dist;

        if (dot > 0) {
            float relSpeed = sqrt(relVx*relVx + relVy*relVy);
            float ttc = dist / (relSpeed + 0.01);

            if (ttc < 2.0) {
                // 处在RVO碰撞锥内 → 计算避让方向（各承担一半责任）
                float perpX = -dy / dist;
                float perpY = dx / dist;
                float speed = sqrt(desVx*desVx + desVy*desVy);
                safeVx = perpX * speed * 0.7;
                safeVy = perpY * speed * 0.7;
                Serial.println("🔄 RVO避让触发");
            }
        }
    }

    // 3. 【核心】DWA速度采样与运动学约束
    float bestV = 0, bestW = 0, bestScore = -9999;

    for (float v = 0; v <= MAX_V; v += 0.05) {
        for (float w = -MAX_W; w <= MAX_W; w += 0.05) {
            // DWA可行性检查：速度是否可达
            float vx = v * cos(atan2(safeVy, safeVx));
            float vy = v * sin(atan2(safeVy, safeVx));

            // 模拟轨迹碰撞检测
            bool safe = true;
            float sx = self.x, sy = self.y;
            for (int i = 0; i < SIM_STEPS; i++) {
                sx += v * cos(atan2(vy, vx)) * DT;
                sy += v * sin(atan2(vy, vx)) * DT;
                // 简化碰撞检测：确保不进入邻居的安全半径
                float d = sqrt((sx - neighbor.x)*(sx - neighbor.x) + 
                               (sy - neighbor.y)*(sy - neighbor.y));
                if (d < combinedRadius) {
                    safe = false;
                    break;
                }
            }

            if (safe) {
                // 评分：接近目标方向 + 高速优先
                float angleDiff = fabs(atan2(vy, vx) - atan2(safeVy, safeVx));
                float score = 0.6 / (angleDiff + 0.01) + 0.3 * v;
                if (score > bestScore) {
                    bestScore = score;
                    bestV = v;
                    bestW = w;
                }
            }
        }
    }

    // 4. 差速驱动
    float vLin = bestV;
    float vAng = bestW;
    float wheelBase = 0.25;
    motorL.move(vLin - vAng * wheelBase / 2);
    motorR.move(vLin + vAng * wheelBase / 2);

    // 更新位置
    self.x += vLin * cos(atan2(safeVy, safeVx)) * 0.05;
    self.y += vLin * sin(atan2(safeVy, safeVx)) * 0.05;
    self.vx = vLin * cos(atan2(safeVy, safeVx));
    self.vy = vLin * sin(atan2(safeVy, safeVx));

    // 邻居移动（模拟）
    neighbor.x += neighbor.vx * 0.05;
    neighbor.y += neighbor.vy * 0.05;

    delay(50);
}

核心要点：

RVO避让方向计算：相对速度指向对方时，生成垂直于连线方向的避让速度，双方各承担一半避让责任

DWA可行性检验：对RVO选出的速度进行运动学约束，确保在最大加速度范围内可达

速度采样评分：在速度空间采样多组候选速度，评分后选最优

2、多机器人RVO + DWA综合评价（三机交叉冲突）
适用场景：三台及以上机器人在交叉路口相遇，不同机器人按优先级通行，混合算法根据冲突类型动态分配避让责任。

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

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

// ==================== 多机器人状态 ====================
#define NUM_ROBOTS 3
struct RobotState {
    float x, y, vx, vy;
    float radius;
    uint8_t priority;  // 优先级：0最高
};
RobotState robots[NUM_ROBOTS] = {
    {0, 0, 0, 0, 0.2, 0},
    {1.5, 1.0, -0.2, 0, 0.2, 1},
    {2.0, -0.5, -0.1, 0.2, 0.2, 2}
};

// ==================== DWA评价函数权重 ====================
const float W_HEADING = 0.6;     // 航向权重
const float W_DIST = 0.3;        // 距离权重
const float W_SPEED = 0.1;       // 速度权重
const float MAX_V = 0.8;
const float MAX_W = 1.0;

// ==================== RVO速度区域检测 ====================
bool isInRVORegion(float vx, float vy, int selfIdx) {
    for (int i = 0; i < NUM_ROBOTS; i++) {
        if (i == selfIdx) continue;
        float dx = robots[i].x - robots[selfIdx].x;
        float dy = robots[i].y - robots[selfIdx].y;
        float dist = sqrt(dx*dx + dy*dy);
        if (dist > 2.5) continue;

        float relVx = vx - robots[i].vx;
        float relVy = vy - robots[i].vy;
        float dot = (relVx*dx + relVy*dy) / dist;

        if (dot > 0) {
            float relSpeed = sqrt(relVx*relVx + relVy*relVy);
            float ttc = dist / (relSpeed + 0.01);
            if (ttc < 2.0) return true;  // 处在RVO区域
        }
    }
    return false;
}

// ==================== 最优速度选择 ====================
void selectOptimalSpeed(int selfIdx, float goalX, float goalY) {
    float bestV = 0, bestW = 0, bestScore = -9999;

    for (float v = 0.05; v <= MAX_V; v += 0.05) {
        for (float w = -MAX_W; w <= MAX_W; w += 0.05) {
            float vx = v * cos(atan2(goalY - robots[selfIdx].y, 
                                    goalX - robots[selfIdx].x) + w * 0.05);
            float vy = v * sin(atan2(goalY - robots[selfIdx].y, 
                                    goalX - robots[selfIdx].x) + w * 0.05);

            // 如果速度在RVO区域内且优先级不高 → 剔除
            if (isInRVORegion(vx, vy, selfIdx)) {
                // 高优先级机器人可以无视RVO约束
                if (robots[selfIdx].priority > 0) continue;
            }

            // DWA评价函数
            float headingScore = 1.0 - fabs(atan2(vy, vx) - 
                               atan2(goalY - robots[selfIdx].y, 
                                    goalX - robots[selfIdx].x)) / PI;
            float distScore = 1.0 / (sqrt((goalX - robots[selfIdx].x)*(goalX - robots[selfIdx].x) +
                                         (goalY - robots[selfIdx].y)*(goalY - robots[selfIdx].y)) + 0.1);
            float speedScore = v / MAX_V;

            float score = W_HEADING * headingScore + W_DIST * distScore + W_SPEED * speedScore;
            // 高优先级加分
            if (robots[selfIdx].priority == 0) score *= 1.2;

            if (score > bestScore) {
                bestScore = score;
                bestV = v;
                bestW = w;
            }
        }
    }

    // 驱动执行
    float wheelBase = 0.25;
    motorL.move(bestV - bestW * wheelBase / 2);
    motorR.move(bestV + bestW * wheelBase / 2);
}

核心要点：

优先级仲裁：高优先级机器人在RVO检测中可保留更多速度选择，低优先级需避让

综合评价函数：航向权重0.6确保朝向目标，距离权重0.3保证安全间距，速度权重0.1优化效率

冲突类型自适应：相遇冲突时双方共同避让，追赶冲突时仅低优先级机器人减速

3、ARVO自适应膨胀半径 + DWA运动约束
适用场景：环境拥挤度动态变化时，ARVO根据周边机器人密度自适应调整膨胀半径，避免避障失败。

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

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

// ==================== 机器人状态 ====================
struct RobotState {
    float x, y, vx, vy;
    float radius;
    bool stuck;
};
RobotState self = {0, 0, 0, 0, 0.15, false};
RobotState neighbors[4] = {
    {1.2, 0.8, -0.1, 0, 0.15, false},
    {1.5, -0.6, -0.05, 0.1, 0.15, false},
    {0.5, 1.8, 0, -0.05, 0.15, false},
    {0.8, -1.5, 0, 0.05, 0.15, false}
};
int neighborCount = 4;

// ==================== ARVO参数 ====================
float baseRadius = 0.15;
float adaptiveRadius = 0.15;
const float MAX_V = 0.6;
const float MAX_W = 1.0;

// ==================== 计算环境拥挤度 ====================
float computeCrowdedness() {
    float sumDist = 0;
    int validCount = 0;
    for (int i = 0; i < neighborCount; i++) {
        float dx = neighbors[i].x - self.x;
        float dy = neighbors[i].y - self.y;
        float dist = sqrt(dx*dx + dy*dy);
        if (dist < 2.0) {
            sumDist += dist;
            validCount++;
        }
    }
    if (validCount == 0) return 0.3;
    return 1.0 - (sumDist / validCount) / 2.0;  // 0~1，值越大越拥挤
}

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

    // 1. 【核心】ARVO自适应膨胀半径
    float crowdedness = computeCrowdedness();
    // 拥挤时缩小膨胀半径保持机动性，空旷时增大半径保障安全
    adaptiveRadius = baseRadius * (0.8 + 0.4 * (1 - crowdedness));
    adaptiveRadius = constrain(adaptiveRadius, 0.1, 0.25);

    // 2. 目标方向
    float goalX = 4.0, goalY = 0;
    float desVx = (goalX - self.x) * 0.3;
    float desVy = (goalY - self.y) * 0.3;
    float desSpeed = sqrt(desVx*desVx + desVy*desVy);
    if (desSpeed > MAX_V) {
        desVx = desVx / desSpeed * MAX_V;
        desVy = desVy / desSpeed * MAX_V;
    }

    // 3. RVO避让检测（使用自适应半径）
    float safeVx = desVx, safeVy = desVy;
    for (int i = 0; i < neighborCount; i++) {
        float dx = neighbors[i].x - self.x;
        float dy = neighbors[i].y - self.y;
        float dist = sqrt(dx*dx + dy*dy);
        float combinedRadius = adaptiveRadius + neighbors[i].radius;

        if (dist > 0.01 && dist < combinedRadius * 3) {
            float relVx = desVx - neighbors[i].vx;
            float relVy = desVy - neighbors[i].vy;
            float dot = (relVx*dx + relVy*dy) / dist;

            if (dot > 0) {
                float relSpeed = sqrt(relVx*relVx + relVy*relVy);
                float ttc = dist / (relSpeed + 0.01);
                if (ttc < 2.0) {
                    float perpX = -dy / dist;
                    float perpY = dx / dist;
                    float speed = sqrt(desVx*desVx + desVy*desVy);
                    safeVx = perpX * speed * 0.6;
                    safeVy = perpY * speed * 0.6;
                    break;
                }
            }
        }
    }

    // 4. DWA最优速度选择
    float bestV = 0, bestW = 0, bestScore = -9999;
    for (float v = 0; v <= MAX_V; v += 0.05) {
        for (float w = -MAX_W; w <= MAX_W; w += 0.05) {
            float vx = v * cos(atan2(safeVy, safeVx));
            float vy = v * sin(atan2(safeVy, safeVx));

            // 碰撞检测（使用自适应半径）
            bool safe = true;
            for (int i = 0; i < neighborCount; i++) {
                float dx = self.x + vx * 0.05 - neighbors[i].x;
                float dy = self.y + vy * 0.05 - neighbors[i].y;
                float d = sqrt(dx*dx + dy*dy);
                if (d < adaptiveRadius + neighbors[i].radius) {
                    safe = false;
                    break;
                }
            }
            if (!safe) continue;

            float angleScore = 1.0 - fabs(atan2(vy, vx) - atan2(safeVy, safeVx)) / PI;
            float score = 0.7 * angleScore + 0.3 * v / MAX_V;
            if (score > bestScore) {
                bestScore = score;
                bestV = v;
                bestW = w;
            }
        }
    }

    // 5. 驱动执行
    float wheelBase = 0.25;
    motorL.move(bestV - bestW * wheelBase / 2);
    motorR.move(bestV + bestW * wheelBase / 2);

    // 更新位置
    self.x += bestV * cos(atan2(safeVy, safeVx)) * 0.05;
    self.y += bestV * sin(atan2(safeVy, safeVx)) * 0.05;
    self.vx = bestV * cos(atan2(safeVy, safeVx));
    self.vy = bestV * sin(atan2(safeVy, safeVx));

    delay(50);
}

核心要点：
ARVO自适应半径：拥挤时缩小膨胀半径保持机动性，空旷时增大保障安全
环境拥挤度计算：基于周边机器人平均距离量化环境复杂度
半径动态调整：半径可在0.1~0.25m范围内变化，避免避障失败

要点解读
RVO解决多机器人"死锁"的机制是"互惠"假设：RVO与基础VO的核心区别在于假设对方也会避让，双方各承担一半避让责任。相遇冲突仿真显示，RVO-DWA混合算法让两台AGV同时减速并偏转角度，而非只让一方避让，避障效率更高。

DWA的核心价值在于"运动学约束"：RVO理论上选出的避让速度在实际中可能因加速度限制而不可达。DWA通过模拟轨迹采样和可行性检查，确保所选速度在最大加速度、最大速度范围内可达。

混合算法的评价函数决定路径质量：DWA的评价函数通常包含航向权重（朝向目标）、距离权重（保持安全）、速度权重（效率）。多机器人场景中优先级越高或距冲突越近时，距离权重应动态增大。

ARVO解决"膨胀半径固定"的工程难题：固定膨胀半径可能导致拥挤环境失去避让速度（可选速度过少）或空旷环境下安全裕度过剩。ARVO根据环境拥挤度动态调整膨胀半径，在保障安全的同时扩大了可选速度范围。

BLDC FOC是速度指令"精准执行"的硬件保障：混合算法输出的速度指令连续变化。BLDC配合FOC控制可实现毫秒级扭矩响应，确保避让速度被平滑精准执行。差速驱动底盘通过左右轮独立速度控制实现转向，满足DWA的速度采样要求。

1、A全局引导+RVO局部协同避障（多机器人仓储会车）
场景：仓储多AGV在狭窄通道会车，A提供全局路径方向，RVO实时检测机器人间碰撞风险，通过速度仲裁实现协同避让，避免死锁。
硬件配置：ESP32（双核240MHz，支撑多机器人数据计算）、双路BLDC差速底盘（带编码器）、UWB模块（多机器人互定位）、超声波传感器（局部障碍补充）

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

// BLDC电机定义
BLDCMotor motorL(7), motorR(7);

// 多机器人状态（含自身与邻居）
struct Agent {
  float x, y;     // 位置坐标
  float vx, vy;   // 速度向量
  float radius;   // 机器人半径（m）
};

Agent self = {0.0, 0.0, 0.0, 0.0, 0.2};
Agent neighbors[2] = {{3.0, 0.0, -0.5, 0.0, 0.2}, {0.0, 3.0, 0.0, -0.5, 0.2}}; // 2台邻居机器人
const int NEIGHBOR_COUNT = 2;

// A*全局路径点（简化为预设序列）
struct Point { float x, y; };
Point globalPath[] = {{1.0,1.0}, {2.0,2.0}, {3.0,3.0}};
int pathIndex = 0;

// 安全参数
const float SAFE_DIST = 0.8;   // 安全距离（m）
const float REACTION_TIME = 2.0; // 反应时间（s）

void setup() {
  // 初始化BLDC电机（速度控制模式）
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  Serial.begin(9600);
}

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

  // 1. A*全局路径生成期望速度
  Point target = globalPath[pathIndex];
  float desVx = (target.x - self.x) * 0.5;
  float desVy = (target.y - self.y) * 0.5;
  float desV = sqrt(desVx*desVx + desVy*desVy);
  float desW = atan2(desVy, desVx);

  // 2. RVO碰撞风险检测与速度仲裁
  if (!isRVOSafe(desVx, desVy)) {
    Serial.println("&#9888;️ RVO检测到碰撞风险，启动协同避让");
    // 计算互惠避让速度（假设邻居机器人同步避让）
    float avoidVx = desVx * 0.3;
    float avoidVy = desVy + 0.2;
    desV = sqrt(avoidVx*avoidVx + avoidVy*avoidVy);
    desW = atan2(avoidVy, avoidVx);
  }

  // 3. BLDC执行速度指令（差速转向）
  float wheelBase = 0.25;
  motorL.move(desV - desW * wheelBase/2);
  motorR.move(desV + desW * wheelBase/2);

  // 4. 更新自身位姿
  self.x += desVx * 0.05;
  self.y += desVy * 0.05;

  delay(50);
}

// RVO安全检测：计算相对速度与位置，判断碰撞风险
bool isRVOSafe(float vx, float vy) {
  for (int i = 0; i < NEIGHBOR_COUNT; i++) {
    float dx = neighbors[i].x - self.x;
    float dy = neighbors[i].y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < 0.01) continue; // 避免除零

    // 计算相对速度
    float relVx = vx - neighbors[i].vx;
    float relVy = vy - neighbors[i].vy;
    
    // 计算相对速度在位置向量上的投影
    float dotProduct = (relVx*dx + relVy*dy) / dist;
    
    // 碰撞条件：相对速度朝向对方，且距离小于安全距离
    if (dotProduct < 0 && dist < SAFE_DIST) {
      return false;
    }
  }
  return true;
}

5、DWA局部规划+RVO多机器人协同避让（室内密集人流场景）
场景：商场/医院等密集人流环境，多台服务机器人同时运行，DWA负责局部轨迹优化，RVO解决机器人间互碰风险，实现平滑避让。
硬件配置：ESP32（运行DWA与RVO核心算法）、双路BLDC差速底盘（FOC闭环控制）、激光雷达（环境感知）、UWB模块（多机器人互定位）

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

// BLDC电机与传感器定义
BLDCMotor motorL(5), motorR(6);
// 假设激光雷达与UWB已初始化，此处简化传感器数据读取

// DWA参数
const float MAX_V = 0.5;      // 最大线速度（m/s）
const float MAX_W = 1.5;      // 最大角速度（rad/s）
const float ACC_V = 0.3;      // 线加速度（m/s²）
const float ACC_W = 1.0;      // 角加速度（rad/s²）
const float DT = 0.1;         // 模拟步长（s）
const float PREDICT_TIME = 1.0; // 轨迹预测时间（s）
const int SAMPLES_V = 8;      // 线速度采样数
const int SAMPLES_W = 10;     // 角速度采样数

// 评价函数权重
const float ALPHA_HEADING = 0.5;  // 朝向目标权重
const float BETA_DIST = 0.3;      // 障碍物距离权重
const float GAMMA_VELOCITY = 0.2; // 速度权重

// 多机器人RVO参数
const float RVO_SAFE_DIST = 0.6; // 机器人间安全距离（m）
Agent self = {0.0, 0.0, 0.0, 0.0, 0.15};
Agent neighbors[3]; // 最多3台邻居机器人
int neighborCount = 0;

// 全局目标点
float goalX = 5.0, goalY = 5.0;
// 机器人位姿
float robotX = 0, robotY = 0, robotYaw = 0;

void setup() {
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
  Serial.begin(9600);
}

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

  // 1. 读取邻居机器人位置与速度（UWB模块数据，此处简化）
  updateNeighbors();

  // 2. DWA生成局部最优速度
  float bestV, bestW;
  dwaPlan(bestV, bestW);

  // 3. RVO修正速度，避免机器人间碰撞
  if (!rvoAdjust(bestV, bestW)) {
    Serial.println("&#9888;️ RVO修正：机器人间存在碰撞风险，调整速度");
  }

  // 4. BLDC执行速度指令
  float wheelBase = 0.25;
  motorL.move(bestV - bestW * wheelBase/2);
  motorR.move(bestV + bestW * wheelBase/2);

  // 5. 更新位姿
  robotYaw += bestW * DT;
  robotX += bestV * cos(robotYaw) * DT;
  robotY += bestV * sin(robotYaw) * DT;
  self.x = robotX; self.y = robotY;
  self.vx = bestV * cos(robotYaw); self.vy = bestV * sin(robotYaw);

  delay(50);
}

// DWA核心：速度空间采样与轨迹评价
void dwaPlan(float &outV, float &outW) {
  // 动态窗口计算
  float minV = max(0, self.vx - ACC_V * DT);
  float maxV = min(MAX_V, self.vx + ACC_V * DT);
  float minW = max(-MAX_W, self.vy - ACC_W * DT);
  float maxW = min(MAX_W, self.vy + ACC_W * DT);

  // 速度采样与轨迹评价
  float bestScore = -999;
  for (int i = 0; i < SAMPLES_V; i++) {
    float v = minV + i * (maxV - minV) / (SAMPLES_V - 1);
    for (int j = 0; j < SAMPLES_W; j++) {
      float w = minW + j * (maxW - minW) / (SAMPLES_W - 1);
      float score = evaluateTrajectory(v, w);
      if (score > bestScore) {
        bestScore = score;
        outV = v;
        outW = w;
      }
    }
  }
}

// 轨迹评价函数（含目标朝向、障碍物距离、速度）
float evaluateTrajectory(float v, float w) {
  float time = 0, x = robotX, y = robotY, yaw = robotYaw;
  float minObsDist = 999;

  // 轨迹模拟
  while (time < PREDICT_TIME) {
    yaw += w * DT;
    x += v * cos(yaw) * DT;
    y += v * sin(yaw) * DT;
    time += DT;
    // 简化障碍物距离检测（实际需结合激光雷达）
    float obsDist = getLaserDist(x, y);
    if (obsDist < minObsDist) minObsDist = obsDist;
    if (obsDist < 0.2) return -1; // 碰撞，无效轨迹
  }

  // 评价函数计算
  float heading = atan2(goalY - y, goalX - x);
  float headingDiff = abs(heading - yaw);
  float costHeading = ALPHA_HEADING * (1 - headingDiff / PI);
  float costDist = BETA_DIST * minObsDist / 2.0;
  float costVelocity = GAMMA_VELOCITY * v / MAX_V;
  return costHeading + costDist + costVelocity;
}

// RVO速度调整：计算互惠避让速度
bool rvoAdjust(float &v, float &w) {
  for (int i = 0; i < neighborCount; i++) {
    float dx = neighbors[i].x - self.x;
    float dy = neighbors[i].y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < RVO_SAFE_DIST) {
      // 计算相对速度
      float relVx = v * cos(robotYaw) - neighbors[i].vx;
      float relVy = v * sin(robotYaw) - neighbors[i].vy;
      // 计算避让速度偏移
      float avoidVx = relVx * 0.5;
      float avoidVy = relVy * 0.5;
      // 调整线速度与角速度
      v = sqrt(avoidVx*avoidVx + avoidVy*avoidVy);
      w = atan2(avoidVy, avoidVx) - robotYaw;
      return false;
    }
  }
  return true;
}

// 简化的邻居机器人数据更新（实际需UWB读取）
void updateNeighbors() {
  // 此处需根据UWB模块实际数据更新neighbors数组
  // 示例：neighbors[0].x = getUWB_X(0); ...
}

// 简化的激光雷达距离检测（实际需激光雷达数据）
float getLaserDist(float x, float y) {
  // 此处需结合激光雷达数据计算障碍物距离
  return 1.0; // 示例值，实际需替换
}

6、RVO+DWA混合避障+滚动窗口重规划（户外多机器人巡检）
场景：户外园区多台巡检机器人，面对树木、临时路障等静态障碍与移动人员等动态障碍，通过RVO解决机器人间冲突，DWA处理局部动态障碍，滚动窗口实时更新环境地图，实现高效避障。
硬件配置：ESP32（主控）、四轮BLDC底盘（带悬挂与编码器）、激光雷达（360°环境感知）、UWB模块（多机器人互定位）、IMU（姿态补偿）

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

// 硬件与参数定义
BLDCMotor motors[4]; // 四轮BLDC电机
const int WHEEL_BASE = 0.5; // 轮距
const int TRACK_WIDTH = 0.4; // 轴距

// 滚动窗口参数
const int WINDOW_SIZE = 5; // 窗口尺寸（m）
float localMap[WINDOW_SIZE*2][WINDOW_SIZE*2] = {0}; // 局部代价地图
float robotX = 0, robotY = 0, robotYaw = 0; // 机器人位姿

// RVO与DWA参数
const float RVO_SAFE_DIST = 0.7;
Agent neighbors[4];
int neighborCount = 0;
const float MAX_V = 0.4;
const float MAX_W = 1.2;
const float PREDICT_TIME = 1.5;

// 全局目标点
float goalX = 10.0, goalY = 10.0;

void setup() {
  // 初始化4路BLDC电机
  for (int i = 0; i < 4; i++) {
    motors[i].controller = MotionControlType::velocity;
    motors[i].init();
    motors[i].initFOC();
  }
  Serial.begin(9600);
}

void loop() {
  // 1. 更新滚动窗口地图（激光雷达数据）
  updateLocalMap();

  // 2. 读取邻居机器人状态（UWB）
  updateNeighbors();

  // 3. DWA生成局部最优速度
  float bestV, bestW;
  dwaPlan(bestV, bestW);

  // 4. RVO修正，避免机器人间碰撞
  rvoAdjust(bestV, bestW);

  // 5. 滚动窗口重规划：若局部地图障碍密集，调整目标点
  if (isObstacleDense()) {
    adjustLocalGoal(bestV, bestW);
  }

  // 6. BLDC执行速度（四轮差速控制）
  executeFourWheelSpeed(bestV, bestW);

  // 7. 更新位姿
  robotYaw += bestW * 0.1;
  robotX += bestV * cos(robotYaw) * 0.1;
  robotY += bestV * sin(robotYaw) * 0.1;

  delay(50);
}

// 更新局部代价地图（滚动窗口）
void updateLocalMap() {
  // 以机器人为中心，将激光雷达数据映射到局部地图
  // 激光雷达数据读取与坐标转换逻辑（简化）
  float laserData[360]; // 360°激光数据
  readLidar(laserData);
  
  for (int angle = 0; angle < 360; angle++) {
    float rad = angle * PI / 180;
    float dist = laserData[angle];
    if (dist < WINDOW_SIZE) {
      int mapX = (int)((robotX + dist * cos(rad)) * 10) + WINDOW_SIZE;
      int mapY = (int)((robotY + dist * sin(rad)) * 10) + WINDOW_SIZE;
      if (mapX >=0 && mapX < WINDOW_SIZE*2 && mapY >=0 && mapY < WINDOW_SIZE*2) {
        localMap[mapX][mapY] = 1; // 标记为障碍
      }
    }
  }
}

// DWA核心规划
void dwaPlan(float &outV, float &outW) {
  float minV = max(0, bestV - ACC_V * 0.1);
  float maxV = min(MAX_V, bestV + ACC_V * 0.1);
  float minW = max(-MAX_W, bestW - ACC_W * 0.1);
  float maxW = min(MAX_W, bestW + ACC_W * 0.1);

  float bestScore = -999;
  for (int i = 0; i < 8; i++) {
    float v = minV + i * (maxV - minV) / 7;
    for (int j = 0; j < 10; j++) {
      float w = minW + j * (maxW - minW) / 9;
      float score = evaluateTrajectory(v, w);
      if (score > bestScore) {
        bestScore = score;
        outV = v;
        outW = w;
      }
    }
  }
}

// 轨迹评价（结合局部地图障碍）
float evaluateTrajectory(float v, float w) {
  float time = 0, x = robotX, y = robotY, yaw = robotYaw;
  float minObsDist = 999;

  while (time < PREDICT_TIME) {
    yaw += w * 0.1;
    x += v * cos(yaw) * 0.1;
    y += v * sin(yaw) * 0.1;
    time += 0.1;

    // 从局部地图获取障碍物距离
    int mapX = (int)(x * 10) + WINDOW_SIZE;
    int mapY = (int)(y * 10) + WINDOW_SIZE;
    float obsDist = 999;
    if (mapX >=0 && mapX < WINDOW_SIZE*2 && mapY >=0 && mapY < WINDOW_SIZE*2) {
      if (localMap[mapX][mapY] == 1) {
        obsDist = sqrt(pow(x - robotX, 2) + pow(y - robotY, 2));
      }
    }
    if (obsDist < minObsDist) minObsDist = obsDist;
    if (obsDist < 0.3) return -1;
  }

  // 评价函数：目标朝向+障碍物距离+速度
  float heading = atan2(goalY - y, goalX - x);
  float headingDiff = abs(heading - yaw);
  float costHeading = 0.5 * (1 - headingDiff / PI);
  float costDist = 0.3 * minObsDist / 2.0;
  float costVelocity = 0.2 * v / MAX_V;
  return costHeading + costDist + costVelocity;
}

// RVO调整（同案例2，适配四轮底盘）
bool rvoAdjust(float &v, float &w) {
  for (int i = 0; i < neighborCount; i++) {
    float dx = neighbors[i].x - robotX;
    float dy = neighbors[i].y - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    if (dist < RVO_SAFE_DIST) {
      float relVx = v * cos(robotYaw) - neighbors[i].vx;
      float relVy = v * sin(robotYaw) - neighbors[i].vy;
      v = sqrt(relVx*relVx + relVy*relVy) * 0.6;
      w = atan2(relVy, relVx) - robotYaw;
      return false;
    }
  }
  return true;
}

// 四轮差速执行
void executeFourWheelSpeed(float v, float w) {
  float leftV = v - w * TRACK_WIDTH / 2;
  float rightV = v + w * TRACK_WIDTH / 2;
  float frontV = v + w * WHEEL_BASE / 2;
  float rearV = v - w * WHEEL_BASE / 2;

  motors[0].move(leftV);  // 左前
  motors[1].move(rightV); // 右前
  motors[2].move(frontV); // 前侧
  motors[3].move(rearV);  // 后侧
}

// 辅助函数：检测障碍物密集度
bool isObstacleDense() {
  int obstacleCount = 0;
  for (int i = 0; i < WINDOW_SIZE*2; i++) {
    for (int j = 0; j < WINDOW_SIZE*2; j++) {
      if (localMap[i][j] == 1) obstacleCount++;
    }
  }
  return obstacleCount > (WINDOW_SIZE*2 * WINDOW_SIZE*2) * 0.3;
}

// 调整局部目标点（滚动重规划）
void adjustLocalGoal(float &v, float &w) {
  // 选择局部地图中无障碍区域作为临时目标
  float tempGoalX = robotX + 2.0, tempGoalY = robotY;
  if (localMap[(int)(tempGoalX*10)+WINDOW_SIZE][(int)(tempGoalY*10)+WINDOW_SIZE] == 0) {
    goalX = tempGoalX;
    goalY = tempGoalY;
  } else {
    tempGoalY = robotY + 2.0;
    goalX = tempGoalX;
    goalY = tempGoalY;
  }
}

要点解读

1. 算法融合逻辑：全局与局部、单机与多机的协同平衡
   RVO与DWA的混合核心是分层协同，通过明确分工实现优势互补：
   全局-局部分工：A*等全局规划算法提供宏观路径方向，解决“去哪里”的问题；DWA作为局部规划，在滚动窗口内处理动态障碍，解决“怎么避”的问题；RVO则专门解决多机器人间的互碰风险，实现“多机协同”。三者形成“宏观引导-局部避障-多机仲裁”的闭环，避免单一算法的局限性。
   约束互补：DWA严格遵循机器人的运动学与动力学约束（最大速度、加速度），确保轨迹可执行；RVO基于相对速度与位置的几何关系，通过速度调整避免碰撞，两者结合既保证避障的平滑性，又解决多机器人冲突，提升系统整体安全性与效率。
2. 多机器人状态感知：互定位与实时数据同步是协同基础
   多机器人协同避障的前提是精准获取其他机器人的状态信息，需从硬件与软件两方面保障：
   互定位技术：采用UWB模块实现多机器人高精度互定位，获取其他机器人的位置、速度等关键信息，相比蓝牙、WiFi，UWB具备厘米级精度与低延迟特性，能满足实时避障需求；同时可结合IMU进行姿态补偿，提升定位稳定性。
   数据同步机制：通过高频通信（如串口、CAN总线）实现多机器人状态数据实时同步，确保RVO算法计算时，邻居机器人的位置、速度与当前时刻匹配，避免因数据延迟导致的避障失效；同时采用时间戳对齐机制，保证多传感器数据在时间维度上的一致性。
3. BLDC执行精度：轨迹跟踪与动态响应的核心保障
   混合避障算法输出的高频速度、角速度指令，需依赖BLDC的精准执行才能落地，核心在于闭环控制与动态响应：
   闭环速度控制：BLDC电机搭配高精度编码器，实现速度闭环控制，通过PID或FOC（磁场定向控制）算法抑制转矩脉动，确保电机能精准跟踪DWA/RVO输出的速度指令，避免因执行偏差导致的轨迹偏移；例如在避障转向时，左右轮差速需严格匹配规划值，保证转向半径准确。
   动态响应匹配：混合避障算法输出的指令频率高、变化快，BLDC需具备毫秒级的电流响应与转矩输出能力，通过FOC控制实现平滑的转矩调节，避免急加速、急转向导致的机械冲击与打滑；同时加入S曲线加减速处理，对上层规划的速度指令进行平滑，防止阶跃指令引发电机过流。
4. 滚动窗口与实时性：算力约束下的高效重规划
   户外复杂环境要求避障系统具备实时响应能力，滚动窗口机制与算力优化是关键：
   滚动窗口机制：仅维护以机器人为中心的局部代价地图，实时更新环境障碍信息，避免全局地图重算的高算力消耗；窗口大小需根据机器人速度、传感器探测范围动态调整，确保既能覆盖潜在障碍，又不超出嵌入式平台的算力上限，实现对突发障碍的快速响应。
   算法轻量化与算力适配：针对Arduino/ESP32等嵌入式平台的算力瓶颈，对DWA、RVO算法进行简化，如减少速度采样数量、采用查表法替代复杂计算、使用整数运算替代浮点运算；优先选用ESP32、STM32等高性能MCU，或采用“上位机（树莓派）+下位机（Arduino）”架构，上位机运行复杂规划算法，下位机专注BLDC执行，平衡算力与实时性。
5. 安全与鲁棒性：多层级保护与异常处理机制
   户外救援场景对安全性要求极高，需构建多层级防护体系，保障系统稳定运行：
   多层级安全保护：硬件层面设置急停按钮、硬件看门狗，防止系统失控；软件层面对线速度、角速度设置上限，加入S曲线加减速，避免电机过载；对传感器数据进行滑动平均、卡尔曼滤波，消除噪声与伪障碍；同时将障碍物轮廓膨胀，预留安全距离，结合相对速度动态调整安全阈值。
   异常处理与冗余设计：建立传感器失效、通信中断、电机堵转等异常场景的应对机制，如传感器失效时切换至保守避障模式，通信中断时机器人自动进入安全悬停；采用多传感器融合的冗余设计，如激光雷达+超声波+红外互补，避免单一传感器盲区导致避障失效；设置最大避障尝试次数，超限则停机报警，防止机器人陷入死循环。

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