基于Arduino与BLDC（无刷直流电机）构建的机器人系统，其“ARVO（自适应相互速度障碍法）自适应膨胀半径 + DWA（动态窗口法）运动约束”是一套将多智能体协同避障与底层高动态执行深度融合的先进导航架构。该方案通过动态调整安全边界解决复杂环境下的避障死锁问题，并利用BLDC的高动态响应能力精准执行DWA输出的高频速度指令。

主要特点

1. 基于环境复杂度的ARVO自适应膨胀半径
   传统的相互速度障碍法（RVO）在复杂环境中常因固定的膨胀半径导致避障失败或路径过于保守。ARVO算法通过实时评估周边环境的复杂度（如障碍物密度、通道宽度），动态调整机器人的膨胀半径。在开阔区域，系统自动增大膨胀半径以保障安全距离；在狭窄通道或密集人群中，系统则适当缩小膨胀半径，扩大可选速度范围，确保机器人既能安全避障又能顺利通过瓶颈区域。
2. 融合运动学约束的DWA动态窗口规划
   DWA算法在速度空间（v, ω）中，根据BLDC底盘的硬件极限（最大速度、最大加速度）和实时传感器数据（障碍物距离），构建一个动态可行的速度窗口。系统在该窗口内采样多组线速度和角速度，通过运动学模型推演未来短时间内的轨迹，并利用多目标评价函数（包含方位角、障碍物距离、速度平滑性等指标）对轨迹进行打分，最终选择最优速度指令执行。这种机制确保了机器人在避障时始终处于物理可达且安全的状态。
3. BLDC高动态响应与FOC平滑执行
   ARVO与DWA算法输出的速度指令往往是连续且高频变化的。BLDC电机配合FOC（磁场定向控制）驱动器，具备极低的转矩脉动和毫秒级的电流响应能力，能够完美跟踪这些平滑的速度曲线。在频繁启停和转向的避障过程中，BLDC能保持运动平滑，避免因机械顿挫导致传感器数据抖动，从而保证避障决策的准确性。
4. 分层式“感知-规划-执行”闭环协同
   系统形成严密的分层闭环：ARVO算法在上层处理多智能体协同与动态障碍物避让，输出期望速度；DWA算法在中间层结合BLDC的运动学约束进行局部轨迹优化；BLDC底层控制器精准执行该速度。当机器人绕过动态障碍后，又能平滑地回归全局路径，实现了从宏观决策到微观执行的无缝衔接。

应用场景

1. 人机协作仓储与物流AMR
   在电商仓库或柔性制造车间，人员和叉车会随时横穿机器人通道。ARVO算法能根据环境密度动态调整安全边界，确保AMR在高速运行中既能安全避让行人，又不会因过度保守而频繁停车，保障物流效率与人员安全。
2. 密集人群中的服务机器人
   在商场、医院或机场等人群密集且移动轨迹不可预测的场所，机器人需具备极高的避障灵活性。ARVO的自适应膨胀半径能根据人流密度动态调整，在拥挤时缩小安全距离以通过瓶颈，在开阔时增大安全距离以保障舒适感。
3. 多机器人协同编队与避障
   在多台机器人协同作业的场景中，ARVO算法能有效处理机器人之间的相互避让问题，避免传统RVO算法因固定膨胀半径导致的“死锁”或“振荡”现象。结合DWA的运动约束，机器人能在保持编队队形的同时，灵活应对突发障碍物。
4. 嵌入式导航算法验证与科研
   该系统是验证多智能体协同算法、局部路径规划以及运动控制算法的绝佳硬件平台。通过调整ARVO的膨胀半径策略与DWA的评价函数权重，能够直观地观察机器人在不同环境下的避障行为与轨迹质量。

注意事项

1. 膨胀半径的动态调整策略
   ARVO算法的效果高度依赖于膨胀半径的调整策略。需根据具体场景反复调试环境复杂度的评估函数与膨胀半径的上下限，避免在狭窄通道中因半径过大导致路径规划失败，或在开阔区域因半径过小导致安全裕度不足。
2. DWA参数整定与评价函数权重
   DWA算法的性能取决于评价函数中各指标（方位角、障碍物距离、速度）的权重系数。需根据BLDC底盘的实际响应特性与应用场景，反复调试权重系数，以在“追求效率”、“保证安全”与“运动平滑”之间找到最佳平衡点。
3. 传感器数据融合与噪声抑制
   ARVO与DWA算法均依赖实时传感器数据。必须采用卡尔曼滤波或中值滤波等算法，对激光雷达或超声波传感器的数据进行预处理，剔除因环境噪声或动态干扰产生的野值，确保避障决策的可靠性。
4. 计算资源与实时性
   ARVO与DWA算法涉及大量的浮点运算与轨迹推演。在Arduino Uno等资源受限的平台上，建议采用定点数运算或查表法优化代码，或优先选用ESP32、STM32等具备硬件FPU的高性能微控制器，以保证控制周期的实时性。
5. 动力学参数标定
   DWA算法的准确性高度依赖于机器人运动学参数的标定。必须精确测量BLDC底盘的最大线速度、最大角速度、最大加速度和最大减速度。参数偏差会导致规划出的轨迹在实际执行时发生碰撞或偏离。

1、环境复杂度评估与自适应膨胀半径
此案例实现ARVO的核心机制：通过多方向超声波评估周边障碍物密度，动态调整膨胀半径。复杂度越高（障碍物越近、越多），半径越大以保障安全；复杂度低时缩小半径以释放速度空间。

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

// BLDC 差速底盘
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// 八向超声波阵列
#define SONAR_NUM 8
NewPing sonars[SONAR_NUM] = {
    NewPing(22,23,150), NewPing(24,25,150), NewPing(26,27,150), NewPing(28,29,150),
    NewPing(30,31,150), NewPing(32,33,150), NewPing(34,35,150), NewPing(36,37,150)
};

// ARVO 膨胀半径参数
const float R_MIN = 0.15;       // 开阔区域最小半径(m)
const float R_MAX = 0.50;       // 狭窄区域最大半径(m)
float rExpansion = 0.25;        // 当前动态膨胀半径

// 环境复杂度评估
float assessComplexity() {
    float densitySum = 0;
    int validCount = 0;
    
    for (int i = 0; i < SONAR_NUM; i++) {
        float dist = sonars[i].ping_cm() / 100.0;
        if (dist > 0 && dist < 2.0) {
            densitySum += (1.0 - dist / 2.0);  // 距离越近贡献越大
            validCount++;
        }
    }
    // 归一化到0~1
    return validCount > 0 ? constrain(densitySum / SONAR_NUM, 0, 1) : 0;
}

// 自适应膨胀半径更新
void updateExpansionRadius() {
    float complexity = assessComplexity();
    // 复杂度越高，半径越大
    rExpansion = R_MIN + (R_MAX - R_MIN) * complexity;
    rExpansion = constrain(rExpansion, R_MIN, R_MAX);
    
    Serial.print("复杂度: "); Serial.print(complexity);
    Serial.print(" | 膨胀半径: "); Serial.println(rExpansion);
}

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

void loop() {
    updateExpansionRadius();
    // rExpansion 将作为后续速度筛选的安全约束
    motorL.target = 0.5; motorR.target = 0.5;
    motorL.move(motorL.target); motorR.move(motorR.target);
    motorL.loopFOC(); motorR.loopFOC();
    delay(50);
}

核心逻辑：论文指出，ARVO通过判断周边环境复杂度来调整膨胀半径，既保障安全距离，又适当扩大可选速度范围。八向超声波提供全向感知，复杂度评估函数将“距离近、障碍多”映射为高复杂度，触发更大的安全半径。

2、ARVO+DWA融合速度筛选（运动学约束）
此案例将ARVO的安全约束与DWA的运动学约束融合。DWA根据BLDC的加速度极限生成动态窗口，ARVO的膨胀半径作为碰撞检测的安全阈值，两者取交集选出最终速度。

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

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// 四向超声波（简化）
NewPing sonarF(22,23,150), sonarL(24,25,150), sonarR(26,27,150), sonarB(28,29,150);

// 机器人运动状态
float robotYaw = 0;
float currentV = 0, currentW = 0;

// DWA 运动学约束（与BLDC实际能力匹配）
const float MAX_V = 1.0;        // 最大线速度(m/s)
const float MAX_W = 1.5;        // 最大角速度(rad/s)
const float ACC_V = 2.0;        // 最大线加速度(m/s²)
const float ACC_W = 3.0;        // 最大角加速度(rad/s²)
const float DT = 0.1;           // 控制周期(s)

// ARVO 膨胀半径（由案例一提供）
float rExpansion = 0.25;

// ARVO安全约束：预测轨迹是否远离障碍
bool isVelocitySafe(float v, float w, float r) {
    float simYaw = robotYaw;
    float simX = 0, simY = 0;  // 局部坐标系
    
    for (float t = 0; t < 0.5; t += 0.1) {
        simX += v * cos(simYaw) * 0.1;
        simY += v * sin(simYaw) * 0.1;
        simYaw += w * 0.1;
        
        float dF = sonarF.ping_cm() / 100.0;
        // 前方距离需大于膨胀半径（论文核心约束）
        if (dF > 0 && dF < r) return false;
    }
    return true;
}

// DWA动态窗口生成
void computeDynamicWindow(float& vMin, float& vMax, float& wMin, float& wMax) {
    vMin = max(0.0f, currentV - ACC_V * DT);
    vMax = min(MAX_V, currentV + ACC_V * DT);
    wMin = max(-MAX_W, currentW - ACC_W * DT);
    wMax = min(MAX_W, currentW + ACC_W * DT);
}

// ARVO+DWA融合速度选择
void selectVelocity(float& bestV, float& bestW) {
    float vMin, vMax, wMin, wMax;
    computeDynamicWindow(vMin, vMax, wMin, wMax);
    
    bestV = 0; bestW = 0;
    float bestScore = -1e9;
    
    // 在DWA动态窗口内采样
    for (float v = vMin; v <= vMax; v += 0.1) {
        for (float w = wMin; w <= wMax; w += 0.2) {
            // ARVO约束：膨胀半径下的安全性
            if (!isVelocitySafe(v, w, rExpansion)) continue;
            
            // 评价函数：速度偏好 + 朝向目标
            float score = 0.5 * v;  // 简化：鼓励速度
            
            if (score > bestScore) {
                bestScore = score;
                bestV = v; bestW = w;
            }
        }
    }
}

void loop() {
    float bestV, bestW;
    selectVelocity(bestV, bestW);
    
    float wheelBase = 0.25;
    motorL.target = bestV - bestW * wheelBase / 2.0;
    motorR.target = bestV + bestW * wheelBase / 2.0;
    motorL.move(motorL.target); motorR.move(motorR.target);
    motorL.loopFOC(); motorR.loopFOC();
    
    currentV = bestV; currentW = bestW;
    delay(50);
}

核心逻辑：论文明确指出，融合RVO和DWA的目的是“对可选避障速度进行运动学约束”。ARVO提供安全速度集合（膨胀半径约束），DWA提供可达速度集合（加速度约束），交集即为最终可执行的安全速度。

3、多机器人协同的ARVO责任分配
此案例将ARVO扩展到双机器人会车场景。每台机器人根据对方位置和速度，动态调整自身膨胀半径——距离越近、速度越快，半径越大，避让责任越明确，避免“双方同时让行”导致的振荡。

#include <SimpleFOC.h>

BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(9,10,11), drvR(5,6,8);

// 机器人状态（通过ESP-NOW或串口交换）
struct RobotState {
    float x, y, vx, vy;
    float radius;
};
RobotState self = {0, 0, 0, 0, 0.25};
RobotState other = {0, 0, 0, 0, 0.25};
float selfYaw = 0;

// ARVO自适应责任分配
void updateAdaptiveExpansion() {
    float dx = self.x - other.x;
    float dy = self.y - other.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    if (dist < 1.5) {
        // 距离越近 + 自身速度越快 → 承担更大避让责任
        float speedFactor = sqrt(self.vx*self.vx + self.vy*self.vy);
        self.radius = 0.2 + 0.3 * (1.0 - dist/1.5) + 0.1 * speedFactor;
        self.radius = constrain(self.radius, 0.15, 0.5);
    } else {
        self.radius = 0.25;
    }
}

// ARVO避障速度计算
void computeARVOVelocity(float& outVx, float& outVy) {
    float goalX = 5.0, goalY = 0.0;
    float dx = goalX - self.x, dy = goalY - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    // 目标方向基础速度
    float vx = dx / dist * 0.5;
    float vy = dy / dist * 0.5;
    
    // 对方机器人斥力（基于自适应膨胀半径）
    float rdx = self.x - other.x, rdy = self.y - other.y;
    float rdist = sqrt(rdx*rdx + rdy*rdy);
    
    if (rdist < self.radius + other.radius) {
        float repForce = 2.0 * (1.0 - rdist / (self.radius + other.radius));
        vx += rdx / rdist * repForce;
        vy += rdy / rdist * repForce;
    }
    
    // 速度限幅
    float mag = sqrt(vx*vx + vy*vy);
    if (mag > 0.8) { vx = vx/mag*0.8; vy = vy/mag*0.8; }
    outVx = vx; outVy = vy;
}

void loop() {
    // 通信交换状态（简化）
    // Serial.write/read...
    
    updateAdaptiveExpansion();
    
    float vx, vy;
    computeARVOVelocity(vx, vy);
    
    // 转换为差速控制
    float targetYaw = atan2(vy, vx);
    float yawError = targetYaw - selfYaw;
    while (yawError > PI) yawError -= 2*PI;
    while (yawError < -PI) yawError += 2*PI;
    
    float vLin = sqrt(vx*vx + vy*vy);
    float vAng = yawError * 1.5;
    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(30);
}

核心逻辑：多机器人ARVO的关键是“避让责任动态分配”。论文指出，传统固定膨胀半径在多机器人相遇时可能导致避障失败。通过自适应半径，距离更近或速度更快的一方承担更大避让责任，实现“主次分明”的协同避让。

要点解读

1. 自适应膨胀半径解决了“安全与效率”的固有矛盾

传统RVO使用固定膨胀半径：半径太小则安全余量不足，半径太大则在狭窄通道中“无路可走”。ARVO通过评估环境复杂度动态调整半径——开阔区域缩小半径以释放速度空间，狭窄区域增大半径以保障安全。这种“按需分配安全余量”的策略，是ARVO的核心创新。

2. DWA的运动学约束是RVO“理论速度”落地的关键

RVO计算出的避障速度在几何上安全，但可能超出BLDC的加速度极限，导致“规划出来但执行不了”。论文明确指出，融合RVO和DWA的目的是“对可选避障速度进行运动学约束”。DWA的动态窗口根据当前速度和最大加速度筛选可达速度，确保最终选择的避障速度在物理上可实现。

3. 多方向感知是复杂度评估的基础，单一传感器不足

ARVO的膨胀半径调整依赖对周边环境复杂度的准确判断。仅靠前方超声波无法区分“狭窄通道”与“开阔空间中的单个障碍”。需要八向超声波阵列提供全向感知，综合“障碍物距离”和“障碍物数量”两个维度才能做出合理决策。

4. 算力瓶颈决定架构选择，ESP32是底线配置

完整的ARVO+DWA融合涉及环境评估、速度空间采样和轨迹预测，对算力要求较高。标准Arduino Uno（2KB RAM）难以胜任。工程建议使用ESP32（双核240MHz，520KB RAM），或采用“上位机规划+Arduino底层控制”的分层架构。

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

ARVO+DWA输出的避障速度指令连续且高频变化。BLDC配合FOC算法具备毫秒级电流响应能力，能精准跟踪连续变化的速度指令，避免有刷电机在频繁启停和转向时的机械冲击与轨迹偏差。SimpleFOC库的MotionControlType::velocity模式是实现速度闭环的标准方法。

4、ARVO基础自适应膨胀半径程序（核心：环境感知+动态膨胀半径调整）
该程序聚焦ARVO核心逻辑，通过超声波传感器检测周围障碍物距离，动态调整障碍物膨胀半径：障碍物距离近时增大膨胀半径（提升安全余量），距离远时减小半径（释放运行空间），适用于机器人初始避障与局部路径规划的基础控制，是整个系统的第一步。

核心逻辑
1.传感器数据采集：读取前、左、右三组超声波传感器，获取障碍物距离；
2. 膨胀半径计算：根据“目标安全距离”与“障碍物距离”的差值，动态调整膨胀半径（差值越小，半径越大）；
3. 边界约束：设定膨胀半径的最大/最小值，避免半径过小导致碰撞或过大导致空间浪费。

// ARVO基础自适应膨胀半径程序——机器人局部避障核心
#include <NewPing.h> // 超声波传感器库（简化测距逻辑）

// 硬件引脚定义
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
#define LEFT_ENC_A 2
#define LEFT_ENC_B 3
#define RIGHT_ENC_A 4
#define RIGHT_ENC_B 5// 超声波传感器引脚（HC-SR04）
#define ULTRA_FRONT_TRIG 22
#define ULTRA_FRONT_ECHO 23
#define ULTRA_LEFT_TRIG 24
#define ULTRA_LEFT_ECHO 25
#define ULTRA_RIGHT_TRIG 26
#define ULTRA_RIGHT_ECHO 27

// ARVO核心参数
const float MIN_INFLATION_RADIUS = 0.2;  // 最小膨胀半径（m，避免空间浪费）
const float MAX_INFLATION_RADIUS = 0.8;  // 最大膨胀半径（m，最大安全余量）
const float BASE_INFLATION = 0.3;        // 基础膨胀半径（m，平坦环境使用）
const float SAFE_THRESHOLD = 0.5;        // 安全距离阈值（m，低于此值增大半径）
const float SPEED_STEP = 0.05;           // 半径调整步长（m）

// 传感器数据变量
float frontDistance = 0.0, leftDistance = 0.0, rightDistance = 0.0;
float inflationRadius = BASE_INFLATION;bool obstacleDetected = false;

// 编码器计数变量
volatile long leftCount = 0, rightCount = 0;
int leftSpeed = 0, rightSpeed = 0;

void setup() {
  Serial.begin(115200);
  
  // 电机引脚初始化
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT); // 编码器中断初始化
  attachInterrupt(0, leftEncCount, RISING);
  attachInterrupt(1, rightEncCount, RISING); // 超声波传感器初始化（NewPing库）
  NewPing ultraSounds[] = {
    NewPing(ULTRA_FRONT_TRIG, ULTRA_FRONT_ECHO, 200), // 前侧，最大测距2m
    NewPing(ULTRA_LEFT_TRIG, ULTRA_LEFT_ECHO, 200),
    NewPing(ULTRA_RIGHT_TRIG, ULTRA_RIGHT_ECHO, 200)
  };
  
  // 电机初始方向：前进
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH);
  Serial.println("ARVO初始化完成，开始检测障碍物...");
}

// 左电机编码器中断void leftEncCount() { leftCount++; }
// 右电机编码器中断
void rightEncCount() { rightCount++; }

// 超声波测距函数：返回距离（m）
float getUltrasonicDistance(NewPing &ultra) {
  float distanceCM = ultra.ping_cm();
  // 过滤无效数据（超出测距范围或负数）
  if (distanceCM <= 0 || distanceCM > 200) {
    return 2.0; // 返回最大值，代表无障碍物
  }
  return distanceCM / 100.0; // 转换为米
}

// ARVO核心函数：计算自适应膨胀半径
void calculateARVORadius() {
  // 取三个方向的最小障碍物距离，作为当前环境风险参考
  float minDistance = frontDistance;
  if (leftDistance < minDistance) minDistance = leftDistance;
  if (rightDistance < minDistance) minDistance = rightDistance;
  
  // 判定是否检测到障碍物
  obstacleDetected = (minDistance <= SAFE_THRESHOLD);
  
  if (obstacleDetected) {
    // 障碍物距离越近，膨胀半径越大（最大不超过最大值）
    inflationRadius = BASE_INFLATION + (SAFE_THRESHOLD - minDistance) * 2;
  } else {
    // 无障碍物时，回归基础半径
    inflationRadius = BASE_INFLATION;
  }
  
  // 边界约束  inflationRadius = constrain(inflationRadius, MIN_INFLATION_RADIUS, MAX_INFLATION_RADIUS);
}

// 电机基础控制：根据膨胀半径调整基础速度
void motorControlByRadius() {
  // 膨胀半径越大，基础速度越慢（安全优先）
  int baseSpeed = map(inflationRadius, MIN_INFLATION_RADIUS, MAX_INFLATION_RADIUS, 200, 100);
  
  // 检测到障碍物时，进一步降低速度
  if (obstacleDetected) {
    baseSpeed = min(baseSpeed, 120);
  }
  
  // 控制电机匀速前进
  analogWrite(LEFT_MOTOR_PWM, baseSpeed);
  analogWrite(RIGHT_MOTOR_PWM, baseSpeed);
}

// 电机转速计算（编码器反馈）
int calcSpeed(volatile long &count, int sampleTime) {
  int speed = (count * 60) / (12 * sampleTime / 1000);  count = 0;
  return speed;
}

void loop() {
  // 1. 读取超声波传感器数据
  NewPing ultraSounds[] = {
    NewPing(ULTRA_FRONT_TRIG, ULTRA_FRONT_ECHO, 200),
    NewPing(ULTRA_LEFT_TRIG, ULTRA_LEFT_ECHO, 200),
    NewPing(ULTRA_RIGHT_TRIG, ULTRA_RIGHT_ECHO, 200)
  };
  frontDistance = getUltrasonicDistance(ultraSounds[0]);
  leftDistance = getUltrasonicDistance(ultraSounds[1]);
  rightDistance = getUltrasonicDistance(ultraSounds[2]);
  
  // 2. 计算自适应膨胀半径
  calculateARVORadius(); // 3. 根据半径控制电机
  motorControlByRadius();
  
  // 4. 计算电机实际转速（反馈验证）
  leftSpeed = calcSpeed(leftCount, 100);
  rightSpeed = calcSpeed(rightCount, 100); // 5. 串口输出调试信息
  Serial.print("前距(m):");
  Serial.print(frontDistance, 2);
  Serial.print(" 左距(m):");
  Serial.print(leftDistance, 2);
  Serial.print(" 右距(m):");
  Serial.print(rightDistance, 2);
  Serial.print(" 膨胀半径(m):");
  Serial.print(inflationRadius, 2);
  Serial.print(" 障碍物:");
  Serial.println(obstacleDetected ? "是" : "否");
  
  delay(100); // 循环周期100ms，适配超声波测距速度
}

程序适配说明
若使用360°激光测距模块（如YLiDAR），可替换超声波传感器，实现更全面的环境感知，膨胀半径调整更精准；
膨胀半径的步长（SPEED_STEP）可根据机器人运动速度调整，速度越快，步长应越小，避免半径突变导致运动不平稳；
可根据场景需求调整BASE_INFLATION（如狭窄空间可降低基础半径，开阔空间可提高）。

5、DWA基础运动约束程序（核心：速度窗口筛选+最优速度选择）
该程序聚焦DWA核心逻辑，基于运动约束（速度、加速度、角速度限制），在可行的速度窗口内筛选最优速度组合，确保机器人运动平稳、无碰撞，适用于机器人常规导航的速度控制，是ARVO避障与路径执行的衔接。

核心逻辑
速度窗口约束：设定最大线速度、最大角速度，以及加速度限值，生成可行速度窗口；
代价函数优化：基于“目标导向”“碰撞风险”的代价权重，在窗口内筛选使总代价最小的速度；
电机控制输出：将最优速度转换为BLDC电机的PWM占空比，实现精准运动。

// DWA基础运动约束程序——机器人速度控制核心
#include <Servo.h> // 可选，用于转向舵机（简化版用轮式差速转向）

// 硬件引脚定义
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
#define LEFT_ENC_A 2
#define LEFT_ENC_B 3
#define RIGHT_ENC_A 4
#define RIGHT_ENC_B 5

// DWA核心参数
const float MAX_LINEAR_SPEED = 0.5; // 最大线速度（m/s）
const float MAX_ANGULAR_SPEED = 1.0; // 最大角速度（rad/s）
const float MAX_LINEAR_ACC = 0.2;    // 最大线加速度（m/s²）
const float MAX_ANGULAR_ACC = 0.5;   // 最大角加速度（rad/s²）
const float VELOCITY_RESILIENCE = 0.1;// 速度分辨率（m/s，窗口采样间隔）
const float ANGULAR_RESILIENCE = 0.1; // 角速度分辨率（rad/s）

// 目标参数（简化为固定目标，实际可从路径规划获取）
float targetX = 3.0;  // 目标X坐标（m）
float targetY = 2.0;  // 目标Y坐标（m）
float robotX = 0.0;   // 机器人当前X坐标
float robotY = 0.0;   // 机器人当前Y坐标
float robotTheta = 0.0; // 机器人当前朝向（rad）

// 速度状态float currentLinearSpeed = 0.0;
float currentAngularSpeed = 0.0;
bool obstacleDetected = false;
float obstacleDistance = 2.0;

// 编码器计数变量
volatile long leftCount = 0, rightCount = 0;
int wheelRadius = 0.05; // 轮子半径（m）
int wheelBase = 0.2;    // 左右轮间距（m）

void setup() {
  Serial.begin(115200); pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  
  attachInterrupt(0, leftEncCount, RISING);
  attachInterrupt(1, rightEncCount, RISING); digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH);
  Serial.println("DWA初始化完成，开始运动控制...");
}

void leftEncCount() { leftCount++; }
void rightEncCount() { rightCount++; }

// 位姿更新：通过编码器计算机器人坐标与朝向（纯航位推算）
void updatePose(int sampleTime) {
  // 计算左右轮线速度（m/s）
  int leftRPM = (leftCount * 60) / (1000 * sampleTime / 1000);
  int rightRPM = (rightCount * 60) / (1000 * sampleTime / 1000);
  leftCount = 0;
  rightCount = 0;
  float leftV = (leftRPM * 2 * PI * wheelRadius) / 60;
  float rightV = (rightRPM * 2 * PI * wheelRadius) / 60;
  
  // 计算机器人线速度与角速度（差速模型）
  float linearV = (leftV + rightV) / 2;
  float angularV = (rightV - leftV) / wheelBase;
  
  // 积分计算位姿（简化，实际需处理累积误差）
  robotX += linearV * cos(robotTheta) * sampleTime / 1000;
  robotY += linearV * sin(robotTheta) * sampleTime / 1000;
  robotTheta += angularV * sampleTime / 1000;
  
  // 更新当前速度（用于加速度约束）
  currentLinearSpeed = linearV;
  currentAngularSpeed = angularV;
}

// DWA核心函数：筛选最优速度
void DWA_SelectOptimalVelocity() {
  float bestLinearV = 0.0;
  float bestAngularV = 0.0;
  float minCost = 1e9; // 最大代价阈值 // 计算障碍物碰撞代价权重（距离越近，权重越高）
  float obstacleWeight = obstacleDetected ? map(obstacleDistance, 0.2, 1.0, 1.0, 0.3) : 0.3;
  float goalWeight = obstacleDetected ? 0.4 : 0.7; // 目标导向权重
  
  // 生成速度窗口：根据当前速度与加速度限制，确定可行范围
  float vMin = max(0.0, currentLinearSpeed - MAX_LINEAR_ACC);
  float vMax = min(MAX_LINEAR_SPEED, currentLinearSpeed + MAX_LINEAR_ACC);
  float wMin = max(-MAX_ANGULAR_SPEED, currentAngularSpeed - MAX_ANGULAR_ACC);
  float wMax = min(MAX_ANGULAR_SPEED, currentAngularSpeed + MAX_ANGULAR_ACC);
  
  // 遍历速度窗口，筛选最优解  for (float v = vMin; v <= vMax; v += VELOCITY_RESILIENCE) {
    for (float w = wMin; w <= wMax; w += ANGULAR_RESILIENCE) {
      // 1. 计算目标导向代价（与目标的距离，越小越好）
      float goalDx = targetX - robotX;
      float goalDy = targetY - robotY;
      float goalDist = sqrt(goalDx * goalDx + goalDy * goalDy);
      // 预测运动后的目标距离（简化为直线运动）
      float predictX = robotX + v * cos(robotTheta) * 0.1;
      float predictY = robotY + v * sin(robotTheta) * 0.1;
      float predictDist = sqrt((targetX - predictX)*(targetX - predictX) + (targetY - predictY)*(targetY - predictY));
      float goalCost = goalWeight * predictDist;
      
      // 2. 计算障碍物代价（距离越近，代价越高）
      float obstacleCost = obstacleWeight * (obstacleDistance -0.3) / 1.7; // 假设膨胀半径0.3m
      
      // 3. 计算加速度代价（尽量保持匀速，减少波动）
      float accCost = 0.1 * abs(v - currentLinearSpeed) + 0.1 * abs(w - currentAngularSpeed);
      
      // 4. 总代价
      float totalCost = goalCost + obstacleCost + accCost;
      
      // 筛选最小代价
      if (totalCost < minCost) {
        minCost = totalCost;
        bestLinearV = v;
        bestAngularV = w;
      }
    }
  }
  
  // 输出最优速度
Serial.print("最优线速度:");
  Serial.print(bestLinearV, 2);
  Serial.print(" 最优角速度:");
  Serial.println(bestAngularV, 2);
}

// 速度转电机PWM：将最优速度转换为BLDC电机控制信号
void velocityToMotorPWM(float linearV, float angularV) {
  // 差速模型：左右轮速度 = 线性速度 ± 角速度*基线/2
  float leftV = linearV - angularV * wheelBase / 2;
  float rightV = linearV + angularV * wheelBase / 2;
  
  // 速度转换为PWM（简化映射，实际需根据电机特性校准）
  int leftPWM = map(leftV, 0, MAX_LINEAR_SPEED, 0, 255);
  int rightPWM = map(rightV, 0, MAX_LINEAR_SPEED, 0, 255);
  
  // 边界约束（避免负值，后退需额外控制方向）
  leftPWM = constrain(leftPWM, 0, 255);
  rightPWM = constrain(rightPWM, 0, 255);
  
  // 控制电机
  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);
}

// 模拟障碍物检测（实际需接入传感器）
void detectObstacle() {
  // 简化逻辑：目标距离小于2m时，判定为有障碍物（可替换为传感器数据）
  float goalDist = sqrt((targetX - robotX)*(targetX - robotX) + (targetY - robotY)*(targetY - robotY));
  obstacleDetected = (goalDist < 2.0);
  obstacleDistance = goalDist;
}

void loop() {
  // 1. 更新位姿
  updatePose(100);
  
  // 2. 检测障碍物
  detectObstacle();
  
  // 3. DWA筛选最优速度
  DWA_SelectOptimalVelocity();
  
  // 4. 速度转电机PWM并输出
  velocityToMotorPWM(currentLinearSpeed, currentAngularSpeed); // 此处替换为最优速度
  velocityToMotorPWM(g_bestLinearV, g_bestAngularV); // 输出最优速度对应的PWM
  
  // 5. 串口输出当前位姿
  Serial.print("当前坐标:(");
  Serial.print(robotX, 2);
  Serial.print(",");
  Serial.print(robotY, 2);
  Serial.print(") 朝向:");
  Serial.println(robotTheta, 2);
  
  delay(100);
}

程序优化方向
目标导向代价可加入“航向偏差”权重，确保机器人朝向目标，避免绕路；
速度窗口可根据ARVO输出的膨胀半径动态调整（膨胀半径大时，降低最大速度），案例3将实现这一联动；
可采用粒子群优化或遗传算法替代遍历，提升复杂环境下的最优解筛选效率。

6、ARVO+DWA联动动态导航程序（核心：风险感知+动态约束+最优路径执行）
该程序集成前两个案例的核心功能，将ARVO输出的膨胀半径作为DWA的动态约束参数，实现“环境风险感知→动态调整运动约束→筛选最优速度→平稳执行导航”的完整闭环，适用于机器人在未知复杂环境中的自主导航，是ARVO与DWA协同的核心应用。

核心逻辑
ARVO输出动态约束：ARVO计算的膨胀半径，直接影响DWA的最大线速度（半径越大，最大速度越低）；
DWA适配约束筛选速度：基于ARVO提供的动态约束，调整DWA的速度窗口，筛选既安全又能向目标移动的速度；
路径执行与反馈：电机执行最优速度，传感器实时反馈位姿与环境，形成闭环控制。

// ARVO+DWA联动动态导航程序——机器人自主避障导航核心
#include <NewPing.h>

// 硬件引脚定义
#define LEFT_MOTOR_PWM 9
#define LEFT_MOTOR_DIR 10
#define RIGHT_MOTOR_PWM 11
#define RIGHT_MOTOR_DIR 12
#define LEFT_ENC_A 2
#define LEFT_ENC_B 3
#define RIGHT_ENC_A 4
#define RIGHT_ENC_B 5

// 超声波传感器引脚
#define ULTRA_FRONT_TRIG 22
#define ULTRA_FRONT_ECHO 23
#define ULTRA_LEFT_TRIG 24
#define ULTRA_LEFT_ECHO 25
#define ULTRA_RIGHT_TRIG 26
#define ULTRA_RIGHT_ECHO 27

// ARVO+DWA核心参数
const float MIN_INFLATION = 0.2, MAX_INFLATION = 0.8, BASE_INFLATION = 0.3;
const float SAFE_THRESHOLD = 0.5;

// DWA基础参数
const float MAX_LINEAR_BASE = 0.5;   // 基础最大线速度（m/s）
const float MAX_ANGULAR = 1.0;// 最大角速度（rad/s）
const float MAX_LINEAR_ACC = 0.2;     // 最大线加速度（m/s²）
const float MAX_ANGULAR_ACC = 0.5;    // 最大角加速度（rad/s²）
const float VELOCITY_RES = 0.1, ANGULAR_RES = 0.1;

// 目标与位姿
float targetX = 5.0, targetY = 3.0;
float robotX = 0.0, robotY = 0.0, robotTheta = 0.0;
float currentLinearV = 0.0, currentAngularV = 0.0;

// ARVO与DWA状态
float inflationRadius = BASE_INFLATION;
float frontDist = 2.0, leftDist = 2.0, rightDist = 2.0;
bool obstacleDetected = false;
float bestLinearV = 0.0, bestAngularV = 0.0;

// 编码器与运动参数
volatile long leftCount = 0, rightCount = 0;
const float wheelRadius = 0.05, wheelBase = 0.2;

void setup() {
  Serial.begin(115200);
  
  pinMode(LEFT_MOTOR_PWM, OUTPUT);
  pinMode(LEFT_MOTOR_DIR, OUTPUT);
  pinMode(RIGHT_MOTOR_PWM, OUTPUT);
  pinMode(RIGHT_MOTOR_DIR, OUTPUT);
  
  attachInterrupt(0, leftEncCount, RISING);
  attachInterrupt(1, rightEncCount, RISING);
  
  digitalWrite(LEFT_MOTOR_DIR, HIGH);
  digitalWrite(RIGHT_MOTOR_DIR, HIGH);
  Serial.println("ARVO+DWA联动导航初始化完成...");
}

void leftEncCount() { leftCount++; }
void rightEncCount() { rightCount++; }

// 超声波测距
float getUltrasonicDistance(int trig, int echo) {
  NewPing ultra(trig, echo, 200);
  float distCM = ultra.ping_cm();
  return (distCM <= 0 || distCM > 200) ? 2.0 : distCM / 100.0;
}

// ARVO核心：计算自适应膨胀半径
void calculateARVO() {
  frontDist = getUltrasonicDistance(ULTRA_FRONT_TRIG, ULTRA_FRONT_ECHO);
  leftDist = getUltrasonicDistance(ULTRA_LEFT_TRIG, ULTRA_LEFT_ECHO);
  rightDist = getUltrasonicDistance(ULTRA_RIGHT_TRIG, ULTRA_RIGHT_ECHO);
  
  float minDist = min({frontDist, leftDist, rightDist});
  obstacleDetected = (minDist <= SAFE_THRESHOLD);
  
  if (obstacleDetected) {
    inflationRadius = BASE_INFLATION + (SAFE_THRESHOLD - minDist) * 2;
  } else {
    inflationRadius = BASE_INFLATION;
  }
  inflationRadius = constrain(inflationRadius, MIN_INFLATION, MAX_INFLATION);
  
  // 动态调整DWA最大线速度：膨胀半径越大，最大速度越低
  MAX_LINEAR_BASE = map(inflationRadius, MIN_INFLATION, MAX_INFLATION, 0.5, 0.2);
}

// 位姿更新
void updatePose(int sampleTime) {
  int leftRPM = (leftCount * 60) / (1000 * sampleTime / 1000);
  int rightRPM = (rightCount * 60) / (1000 * sampleTime / 1000);
  leftCount = 0; rightCount = 0;
  
  float leftV = (leftRPM * 2 * PI * wheelRadius) / 60;
  float rightV = (rightRPM * 2 * PI * wheelRadius) / 60;
 
  currentLinearV = (leftV + rightV) / 2;
  currentAngularV = (rightV - leftV) / wheelBase;
  
  robotX += currentLinearV * cos(robotTheta) * sampleTime / 1000;
  robotY += currentLinearV * sin(robotTheta) * sampleTime / 1000;
  robotTheta += currentAngularV * sampleTime / 1000;
}

// DWA核心：适配ARVO约束筛选最优速度
void DWA_AdaptiveSelect() {
  float minCost = 1e9;
  
  // 目标导向权重与障碍物权重（由ARVO风险决定）
  float obstacleWeight = obstacleDetected ? map(min(frontDist, leftDist, rightDist), 0.2, 1.0, 1.0, 0.3) : 0.3;
  float goalWeight = obstacleDetected ? 0.4 : 0.7;
  
  // 动态速度窗口（受ARVO约束的最大速度限制）
  float vMin = max(0.0, currentLinearV - MAX_LINEAR_ACC);
  float vMax = min(MAX_LINEAR_BASE, currentLinearV + MAX_LINEAR_ACC);
  float wMin = max(-MAX_ANGULAR, currentAngularV - MAX_ANGULAR_ACC);
  float wMax = min(MAX_ANGULAR, currentAngularV + MAX_ANGULAR_ACC);
  
  // 遍历速度窗口
  for (float v = vMin; v <= vMax; v += VELOCITY_RES) {
    for (float w = wMin; w <= wMax; w += ANGULAR_RES) {
      // 预测运动后位置
      float predictX = robotX + v * cos(robotTheta) * 0.1;
      float predictY = robotY + v * sin(robotTheta) * 0.1;
      float goalDx = targetX - predictX;
      float goalDy = targetY - predictY;
      float goalDist = sqrt(goalDx * goalDx + goalDy * goalDy);
      float goalCost = goalWeight * goalDist;
      
      // 碰撞代价（基于ARVO膨胀半径）
      float minPredictDist = min({frontDist - v*0.1, leftDist, rightDist});
      float obstacleCost = obstacleWeight * max(0.0, inflationRadius - minPredictDist);
      
      // 加速度代价
      float accCost = 0.1 * abs(v - currentLinearV) + 0.1 * abs(w - currentAngularV);
      
 float totalCost = goalCost + obstacleCost + accCost;
      
      if (totalCost < minCost) {
        minCost = totalCost;
        bestLinearV = v;
        bestAngularV = w;
      }
    }
  }
}

// 速度转电机PWM
void velocityToMotorPWM(float linearV, float angularV) {
  float leftV = linearV - angularV * wheelBase / 2;
  float rightV = linearV + angularV * wheelBase / 2;
  
  int leftPWM = map(leftV, 0, MAX_LINEAR_BASE, 0, 255);
  int rightPWM = map(rightV, 0, MAX_LINEAR_BASE, 0, 255);
  leftPWM = constrain(leftPWM, 0, 255);
  rightPWM = constrain(rightPWM, 0, 255);
  
  analogWrite(LEFT_MOTOR_PWM, leftPWM);
  analogWrite(RIGHT_MOTOR_PWM, rightPWM);
}

void loop() {
  // 1. ARVO计算自适应膨胀半径（输出动态约束）
  calculateARVO();
  
  // 2. 更新位姿
  updatePose(100);
  
  // 3. DWA适配ARVO约束，筛选最优速度
  DWA_AdaptiveSelect();
  
  // 4. 执行最优速度
  velocityToMotorPWM(bestLinearV, bestAngularV);
  
  // 5. 输出调试信息
  Serial.print("膨胀半径:");
  Serial.print(inflationRadius, 2);
  Serial.print(" 前距:");
  Serial.print(frontDist, 2);
  Serial.print(" 最优速度:");
  Serial.print(bestLinearV, 2);
  Serial.print("/");
  Serial.println(bestAngularV, 2);
  
  // 6. 停止控制：到达目标后停车
  float goalDist = sqrt((targetX - robotX)*(targetX - robotX) + (targetY - robotY)*(targetY - robotY));
  if (goalDist < 0.2) {
    analogWrite(LEFT_MOTOR_PWM, 0);
    analogWrite(RIGHT_MOTOR_PWM, 0);
    Serial.println("到达目标，停止运行");
  }
  
  delay(100);
}

程序优化方向
可加入“局部路径规划”层，在ARVO生成的膨胀地图中生成参考路径，DWA的goalCost改为“与参考路径的距离”，提升导航效率；
增加回退逻辑，当机器人陷入局部极小值（如被障碍物包围）时，触发回退机制，重新规划路径；
结合MPU6050的姿态数据，修正位姿累积误差，提升导航精度。

要点解读
要点1：膨胀半径与安全距离的动态关联——ARVO的核心逻辑
ARVO的本质是“风险自适应的安全空间设计”，膨胀半径不是固定值，需与环境风险动态绑定，关键要把握三项准则：
风险量化标准：无风险（障碍物距离＞安全阈值）时，采用基础膨胀半径，避免浪费运行空间；风险越高（障碍物越近），半径越大，确保机器人运动时的边缘余量。案例4中以“障碍物距离与安全阈值的差值”量化风险，半径随风险线性增长，逻辑简单且适配多数场景。
边界约束不可缺：需设定最小与最大半径，最小半径保证基本安全（防止卡死），最大半径避免空间压缩过度导致机器人无法前进。案例4中0.2m~0.8m的范围，适配中小型机器人在管廊、车间等场景的运动需求。
多方向风险整合：单一方向的距离无法全面反映环境风险，需整合所有感知方向的最小距离作为风险参考，避免局部障碍物被忽略。
要点2：DWA速度窗口的动态适配——运动约束的关键
DWA的核心是“在可行范围内找最优”，速度窗口的合理性直接影响导航效率与安全性，需适配ARVO输出动态调整：
窗口范围随风险收紧：膨胀半径越大（风险越高），窗口的最大线速度越低、范围越窄，避免机器人因速度过快无法及时避障。案例6中将膨胀半径映射为最大线速度，风险升高时速度上限从0.5m/s降至0.2m/s，从源头限制风险。
动态窗口优于静态：静态窗口无法应对动态环境，需以当前速度为基础，结合加速度限制确定窗口范围，保证运动平稳性。案例5中vMin/vMax基于当前速度加减加速度计算，避免速度突变导致电机过载或运动抖动。
分辨率与计算量平衡：速度分辨率越高，最优解越精准，但计算量越大，需根据Arduino的算力选择合适分辨率（案例中0.1m/s、0.1rad/s为通用值，高端芯片可提升分辨率）。
要点3：代价函数的权重平衡——最优速度的筛选准则
DWA的核心是“多项代价的加权优化”，权重分配需贴合场景需求，避免单一代价主导导致导航逻辑失衡：
权重场景化调整：障碍物近时，提高碰撞代价权重（安全优先）；开阔环境则提高目标导向权重（效率优先）。案例中障碍物场景下，障碍物权重提升至0.4~1.0，目标权重降至0.4，确保安全的前提下尽量向目标移动。
代价项的互补性：需包含目标导向、碰撞风险、运动平稳三类代价，避免机器人追求最短路径导致急停急转，或追求平稳导致绕路。案例5中加速度代价占比10%，有效减少电机输出波动，提升运动连续性。
代价计算的时效性：需基于实时感知数据（障碍物距离、位姿）计算代价，而非用历史数据，确保筛选的速度适配当前环境。
要点4：传感器与控制逻辑的联动——系统稳定的基础
ARVO+DWA系统的稳定性，依赖于传感器感知精度、电机控制精度、控制逻辑的协同，需重点把控：
传感器感知的一致性：多传感器数据需统一坐标系与单位，避免因数据偏差导致风险误判。案例中超声波传感器均统一为米制单位，以“最小距离”作为风险参考，确保膨胀半径计算准确。
电机控制的响应速度：BLDC电机的PWM控制需与速度更新周期匹配，避免控制延迟导致的速度偏差。案例中循环周期设为100ms，与超声波测距周期、编码器采样周期对齐，减少控制滞后。
误差补偿不可少：编码器航位推算存在累积误差，需通过外部传感器（如MPU6050、定位模块）定期校正，避免位姿误差累积导致导航偏离目标。
要点5：参数校准与场景适配——方案落地的前提
任何控制逻辑都需结合具体硬件与场景校准参数，脱离校准的程序无法发挥预期效果，需分两步落地：
硬件参数校准：必须校准轮子半径、基线、编码器线数、电机PWM与速度的映射关系，确保“速度指令-电机转速-实际速度”精准对应。案例中电机PWM的映射函数需根据电机特性，实际测试调整，而非直接套用通用公式。
场景参数适配：不同场景的风险阈值、膨胀半径范围、速度限值不同（如狭窄管廊需降低膨胀半径，开阔车间可提高），需根据实际环境调整参数，或支持外部参数配置（串口、按键），便于现场调试。

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