Arduino BLDC多机器人协同避障（UWB+超声波融合）系统，是以Arduino/ESP32为主控、BLDC无刷电机为底盘驱动，通过UWB提供全局厘米级绝对定位、超声波阵列提供局部实时障碍物检测，经卡尔曼滤波融合实现多机器人间互避与全局协同导航的多智能体系统。 该方案具备UWB全局定位与超声波局部避障的互补融合、BLDC高动态响应与精确执行、分布式协同避障算法、低延迟通信与状态同步四大特点，主要应用于智能仓储AGV协同、灾难救援集群探索、安防巡逻编队及科研教学平台等场景；实际部署时需重点关注UWB基站部署与视距保障、通信延迟与丢包、Arduino算力瓶颈、电源隔离与EMC防护、传感器融合冲突处理及安全容错机制。
一、 技术架构与主要特点
UWB全局定位与超声波局部避障的互补融合：这是该方案的核心感知架构，两种传感器各取所长、互为补充：
UWB（超宽带）定位层：UWB模块（如DW1001/DW3000系列）通过TDOA（到达时间差）或TOF（飞行时间）测距原理，配合至少3_{4个基站（Anchor）实现室内厘米级（典型精度10}30cm）的绝对定位。每台机器人佩戴UWB标签，基站网络实时解算出所有机器人的全局坐标（X, Y）和航向角（θ），为协同避障提供"谁在哪里"的全局态势感知。
超声波局部感知层：每台机器人搭载3_{5路超声波传感器（如HC-SR04，环形布局覆盖前/左/右/后方），有效测距范围约2}400cm，刷新率约10~20Hz。超声波负责检测UWB无法感知的静态障碍物（墙壁、货架、桌腿等）和UWB定位盲区中的动态障碍。
融合策略：通过扩展卡尔曼滤波（EKF）将UWB的全局绝对坐标（低频、高精度、无累积误差）与编码器里程计（高频、有累积漂移）进行紧耦合，同时用超声波数据构建局部障碍物栅格。当UWB信号短暂丢失时，里程计+超声波惯性导航可维持短时定位；当超声波检测到障碍物时，触发局部避障策略（如人工势场法、DWA动态窗口法），同时通过UWB获取邻居机器人位置，避免"避了障碍物却撞上队友"。
BLDC高动态响应与精确执行：BLDC电机配合FOC（磁场定向控制）驱动，具备毫秒级电流响应和极低转矩脉动，能够精确执行协同避障算法输出的速度/转向指令。双向控制支持原地转向（Zero-turning）和差速转向，在狭窄空间内灵活规避。再生制动功能可将减速时的动能转化为电能回馈，提供强大的电磁制动效果，确保高速运动中紧急制动时不越位。
分布式协同避障算法：多机器人协同避障的核心算法通常采用分层架构：
全局层：A算法在已知地图上规划全局最优路径，输出离散航路点。
协同层：速度障碍法（VO/RVO）或人工势场法（APF），每台机器人根据UWB获取的邻居机器人位置和速度，计算"碰撞锥"（Collision Cone），在速度空间中排除会导致碰撞的速度向量，选择安全速度。当检测到死锁（如两台机器人面对面僵持）时，引入"礼让"规则——优先级低的机器人执行"后退-等待"策略。
局部层：DWA（动态窗口法）在运动学约束下生成候选轨迹，评分函数综合考虑目标方向、障碍物距离和速度平滑度，输出最终的速度指令。
低延迟通信与状态同步：多机器人协同高度依赖实时的位姿共享。系统通常利用ESP32的ESP-NOW等低延迟点对点通信协议，实现机器人之间的毫秒级数据传输（延迟<10ms），确保各节点能实时获取邻居位置和速度，避免因通信延迟导致避障预测失效或队形散乱。
二、 典型应用场景
智能仓储AGV协同搬运：多台AGV在仓库货架间执行货物搬运任务，UWB提供全局定位确保AGV在通道中精确行驶，超声波检测临时堆放的物料或突然出现的工人。VO协同避障算法确保多台AGV在交叉路口不会碰撞，同时通过全局路径规划（如A）优化整体运输效率，减少拥堵。
灾难救援与危险区域探索：在地震废墟或核辐射区域，机器人集群通过UWB实现精确定位和协同覆盖，超声波检测废墟中的障碍物。结合热成像或气体传感器快速覆盖大面积未知区域，单个机器人失效时其余设备能自动补位，确保救援覆盖无死角。
安防巡逻编队：多台巡逻机器人在园区或厂区形成编队巡逻，UWB确保编队保持预设队形，超声波检测行人、车辆等动态障碍物。当遇到动态障碍物或目标突然变向时，协同避障机制确保编队在变换队形、规避障碍的过程中，跟随者不会与领航者或其他队友发生碰撞。
科研与教育平台：作为多智能体系统、传感器融合、协同控制算法的低成本实验平台，广泛用于验证TDOA定位算法、VO/RVO协同避障策略、EKF多源融合等理论，适合高校机器人课程和各类机器人竞赛。
三、 关键注意事项
UWB基站部署与视距保障：
基站数量与布局：至少需要3个基站呈非共线（如四边形）布置以包围工作区域，4个基站可提升定位精度和冗余度。基站坐标的标定误差会直接放大定位与航向解算的偏差，必须精确测量并校准。
视距（LoS）要求：UWB信号需要视距传播，机器人与基站之间不能有严重的物理遮挡（如金属墙体、大型设备）。在遮挡严重的环境中，定位精度会急剧下降甚至丢失。
多标签时隙规划：多台机器人同场作业时，需合理规划UWB通信时隙（TDMA），避免多标签间的射频碰撞与相互干扰。
通信延迟与丢包：
分布式协同避障对通信延迟极为敏感。无线通信模块（如ESP-NOW、nRF24L01）的传输延迟和丢包率需根据实际环境评估。数据包结构应包含帧头、地址、指令、数据、校验和（如CRC16）及帧尾，若接收方校验失败应丢弃数据包并请求重传。
通信超时保护机制必须完善：若在设定时间内未接收到邻居节点的"心跳包"，系统应自动进入安全模式（如减速或停止），避免因信息过期导致误判碰撞。
Arduino算力瓶颈：
UWB非线性方程组解算、EKF融合、VO/RVO协同避障算法对算力要求极高。标准Arduino Uno/Nano（16MHz、2KB SRAM）极易因内存溢出或浮点运算过载而崩溃。
强烈建议采用ESP32（双核240MHz）、Teensy 4.1或STM32等高算力主控，或采用"上位机（负责算法解算与调度）+ Arduino（负责底层BLDC控制）“的分布式架构。控制回路必须使用硬件定时器中断或非阻塞定时（millis()），严禁使用delay()函数，确保控制频率≥50Hz。
电源隔离与EMC防护：
电源隔离：BLDC电机启停时电流冲击极大（堵转可达额定3_{5倍），严禁与Arduino及UWB模块共用电源。必须采用隔离DC-DC模块为控制电路独立供电，并在电机驱动端并联大容量低ESR电容（1000}4700μF）吸收反电动势。
EMC防护：UWB模块对电源噪声和射频干扰极其敏感，而BLDC电机的高频PWM噪声会严重干扰UWB定位和机器人间通信。动力线与信号线必须分开走线（间距≥5cm），通信天线远离电机和驱动器，线缆使用屏蔽线并单点接地。必要时在电源端加装π型滤波电路或共模电感。
传感器融合冲突处理：
当不同传感器对同一障碍的判断冲突时（如超声波判定为中距、红外判定为近距），需设定优先级——近距传感器优先级高于远距传感器，避免误触发紧急制动或漏避障。
超声波测距受温度影响显著（声速公式：v = 331.4 + 0.6T），在温差较大的环境中需加入温度传感器进行声速补偿。
UWB定位数据可能出现跳变（如多径效应导致距离突变），需通过卡尔曼滤波平滑处理，并设置物理极限校验——当距离突变超出物理极限时，丢弃异常数据并触发减速保护。
安全机制与容错设计：
看门狗与超时保护：必须启用硬件看门狗定时器。设置通信超时机制，若在设定时间内未接收到邻居节点的"心跳包”，系统应自动进入安全模式。
UWB信号丢失保护：当检测到UWB信号完全丢失或数据异常时，机器人应自动触发减速或急停机制，切换至纯超声波+里程计的惯性导航模式。
紧急制动层：独立于常规控制回路，当超声波检测到极近距离障碍（如<15cm）时直接切断PWM输出，响应时间<10ms。
死锁检测与逃逸：引入随机游走或"后退-等待"策略，当检测到机器人长期停滞（死锁）时注入随机速度扰动，帮助脱离局部极小值。

1、双机器人UWB差分跟随 + 超声波VFF避障（VFF框架）
场景：两台机器人在仓库或开阔环境中协同作业，从机通过UWB差分定位获取主机的相对位置并保持期望跟随距离，同时利用超声波进行近场避障，避免碰撞静态障碍物或货架。

核心逻辑：虚拟力场（VFF） 框架驱动从机行为——跟随主机产生的引力将机器人拉向目标位置，超声波检测到的障碍物产生斥力将机器人推开，合力决定最终运动方向。双UWB标签差分定位可获取主机的绝对坐标与航向角，克服单标签只能测距无法测向的局限。

#include <SimpleFOC.h>
#include <DW1000Ranging.h>   // UWB定位库
#include <NewPing.h>          // 超声波库

// ===== BLDC差速电机 =====
BLDCMotor motorL(7), motorR(7);
const float WHEEL_BASE = 0.25;

// ===== UWB定位变量 =====
float targetPos[2] = {0, 0};    // 主机位置（来自UWB）
float currentPos[2] = {0, 0};   // 从机自身位置

const float DESIRED_DIST = 1.2;   // 期望跟随距离(m)
const float MAX_SPEED = 0.8;

// ===== VFF力场参数 =====
const float REPULSE_RANGE = 1.0;   // 斥力作用范围(m)
const float REPULSE_GAIN = 5.0;

// ===== 超声波传感器（前方） =====
NewPing sonarF(TRIG_F, ECHO_F, 200);

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

    // 1. UWB获取相对位置（实际从DW1000模块读取）
    // uwb.getPosition(targetPos);  // 主机坐标
    // uwb.getPosition(currentPos); // 从机自身坐标

    // 2. 计算跟随误差
    float dx = targetPos[0] - currentPos[0];
    float dy = targetPos[1] - currentPos[1];
    float distErr = sqrt(dx*dx + dy*dy);

    // 3. VFF力场合成
    float fx = 0, fy = 0;

    // 3.1 引力：期望距离偏差驱动
    if (distErr > DESIRED_DIST) {
        float scale = (distErr - DESIRED_DIST) * 0.02;
        fx += dx / distErr * scale;
        fy += dy / distErr * scale;
    } else if (distErr < DESIRED_DIST * 0.7) {
        // 距离过近 → 产生反向力（后退）
        float scale = (DESIRED_DIST - distErr) * 0.01;
        fx -= dx / distErr * scale;
        fy -= dy / distErr * scale;
    }

    // 3.2 超声波斥力：前方障碍物产生斥力
    float frontDist = sonarF.ping_cm() / 100.0;
    if (frontDist > 0 && frontDist < REPULSE_RANGE) {
        float repForce = REPULSE_GAIN * (1.0 - frontDist / REPULSE_RANGE);
        fx -= repForce;  // 沿机器人自身前进方向的反向
        // 注：完整实现应使用机器人的朝向将斥力转换到世界坐标系
    }

    // 4. 合成速度指令 → BLDC差速驱动
    float vLin = constrain(sqrt(fx*fx + fy*fy), 0, MAX_SPEED);
    float vAng = constrain(atan2(fy, fx) * 1.2, -0.8, 0.8);
    motorL.move(vLin - vAng * WHEEL_BASE / 2);
    motorR.move(vLin + vAng * WHEEL_BASE / 2);

    delay(30);  // ~33Hz控制频率
}

2、RVO互惠避障 + A集中式混合架构
场景：多台机器人在狭长走廊或工厂通道中相向而行（会车场景），需在高速行驶中保持互不碰撞。全局路径由A规划，局部动态避障由RVO（互惠速度障碍） 处理。

核心逻辑：采用分层协同架构——集中式协调器运行A*为每台机器人规划全局路径并下发期望速度；机器人局部执行RVO避障，在高频循环中根据邻居状态修正速度。RVO的核心思想是：当两台机器人可能相撞时，各自承担一半的回避责任，避免“你让我我不让你”的振荡。

#include <SimpleFOC.h>
#include <esp_now.h>   // 机器人间通信（本例用ESP-NOW示意）

// ===== BLDC差速电机（定义略） =====
BLDCMotor motorL(7), motorR(7);

// ===== 机器人状态结构体 =====
struct RobotState {
    float x, y;       // 位置
    float vx, vy;     // 速度（世界坐标系）
    uint8_t id;
};

RobotState self = {0, 0, 0.4, 0, 1};
RobotState neighbor = {3.0, 0, -0.4, 0, 2};  // 从通信获取

// ===== RVO参数 =====
const float TIME_HORIZON = 2.0;   // 速度障碍时间窗口(s)
const float MAX_SPEED = 0.8;
const float MIN_DIST = 0.6;        // 最小安全距离(m)

// ===== A*期望速度（来自集中式协调器） =====
float astarVx = 0.6, astarVy = 0;  // A*下发的期望速度

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

    // 1. 获取邻居状态（ESP-NOW/无线通信接收）
    // esp_now_receive(&neighbor);

    // 2. 计算与邻居的相对状态
    float dx = neighbor.x - self.x;
    float dy = neighbor.y - self.y;
    float dist = sqrt(dx*dx + dy*dy);

    // 3. RVO修正（仅在邻居进入影响范围时触发）
    float finalVx = astarVx, finalVy = astarVy;

    if (dist < MIN_DIST * 4.0 && dist > 0.01) {
        float dvx = self.vx - neighbor.vx;
        float dvy = self.vy - neighbor.vy;
        
        // 预测未来位置（时间窗口内）
        float futureDx = dx + dvx * TIME_HORIZON;
        float futureDy = dy + dvy * TIME_HORIZON;
        float futureDist = sqrt(futureDx*futureDx + futureDy*futureDy);

        // 若预测会进入安全距离 → RVO修正
        if (futureDist < MIN_DIST * 1.5) {
            // 计算回避方向（垂直于相对位置方向）
            float perpX = -dy / dist;
            float perpY = dx / dist;
            float avoidStrength = 0.8 * (1.0 - futureDist / (MIN_DIST * 1.5));
            
            // RVO核心：修正速度 = A*速度 + 回避力（各自承担一半责任）
            finalVx = astarVx + avoidStrength * perpX;
            finalVy = astarVy + avoidStrength * perpY;
        }
    }

    // 4. 限幅后驱动BLDC（差速底盘）
    float speed = constrain(sqrt(finalVx*finalVx + finalVy*finalVy), 0, MAX_SPEED);
    float angle = atan2(finalVy, finalVx);
    motorL.move(speed - angle * WHEEL_BASE / 2);
    motorR.move(speed + angle * WHEEL_BASE / 2);

    // 5. 更新自身状态（里程计推算）
    self.x += self.vx * 0.05;
    self.y += self.vy * 0.05;

    delay(50);  // 20Hz控制循环
}

补充说明：此架构中A*运行在协调器（如ESP32/树莓派）上，通过无线将期望速度下发给各机器人；Arduino端仅执行高频的RVO修正和BLDC驱动，保证实时性。

3、多机协同逃逸 + 模糊逻辑 + 信息素共享
场景：三台及以上机器人在未知环境中同时遭遇障碍物（如突然出现的移动物体），需协同决策逃逸方向，避免各自为政导致拥堵或碰撞。

核心逻辑：每台机器人通过Zigbee或ESP-NOW广播自身探测到的障碍物信息（前方/左/右距离），同时接收其他机器人的感知数据。系统采用模糊逻辑综合判断——根据自身传感器读数、共享的障碍物信息和“信息素地图”（记录各方向的安全度），动态计算最优逃逸方向。这避免了单机感知盲区导致的错误决策。

#include <SimpleFOC.h>
#include <Zigbee.h>    // 多机通信

// ===== BLDC差速电机（定义略） =====
BLDCMotor motorL(7), motorR(7);

// ===== 超声波三方向 =====
#define TRIG_F 2, ECHO_F 3
#define TRIG_L 4, ECHO_L 5
#define TRIG_R 6, ECHO_R 7
NewPing sonarF(TRIG_F, ECHO_F, 200);
NewPing sonarL(TRIG_L, ECHO_L, 200);
NewPing sonarR(TRIG_R, ECHO_R, 200);

// ===== 共享障碍物数据（来自其他机器人） =====
float sharedObstacle[5];  // 其他机器人报告的最小障碍距离

// ===== 信息素地图（各方向优先级） =====
float pheromone[3] = {1.0, 1.0, 1.0};  // 左、右、前

// ===== 模糊规则参数 =====
float frontDanger = 0, leftDanger = 0, rightDanger = 0;

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

    // 1. 读取自身超声波
    float dF = sonarF.ping_cm();
    float dL = sonarL.ping_cm();
    float dR = sonarR.ping_cm();

    // 2. 广播自身状态 + 接收共享障碍信息
    // zigbee.sendStatus(dF, dL, dR);
    // zigbee.receive(sharedObstacle);

    // 3. 模糊化：计算各方向危险度（距离越小越危险）
    frontDanger = (dF < 30) ? (100 - dF) : 0;
    leftDanger = (dL < 30) ? (100 - dL) : 0;
    rightDanger = (dR < 30) ? (100 - dR) : 0;

    // 加入共享障碍信息：其他机器人发现某方向有障碍 → 增加该方向危险度
    // 简化：取所有共享数据的最小值作为全局危险参考
    float minShared = *min_element(sharedObstacle, sharedObstacle + 5);

    // 4. 更新信息素：安全区域信息素增强，危险区域减弱
    if (dF > 60) pheromone[2] += 0.1;  // 前方开阔
    else pheromone[2] -= 0.1;
    if (dL > 50) pheromone[0] += 0.05;
    else pheromone[0] -= 0.05;
    if (dR > 50) pheromone[1] += 0.05;
    else pheromone[1] -= 0.05;
    
    // 信息素限幅
    for (int i=0; i<3; i++) pheromone[i] = constrain(pheromone[i], 0, 2);

    // 5. 模糊决策：综合危险度 + 信息素，选择最优逃逸方向
    // 优先级 = 安全度（100-危险度）+ 信息素
    float leftPriority = (100 - leftDanger) + pheromone[0] * 20;
    float rightPriority = (100 - rightDanger) + pheromone[1] * 20;
    float frontPriority = (100 - frontDanger) + pheromone[2] * 20;

    int escapeDir = 0;  // -1=左转, 0=直行, 1=右转
    if (frontPriority < 60) {  // 前方危险
        if (leftPriority > rightPriority) escapeDir = -1;
        else escapeDir = 1;
    }

    // 6. BLDC差速驱动执行逃逸
    float baseSpeed = (frontPriority > 60) ? 0.6 : 0.3;
    motorL.move(baseSpeed - escapeDir * 0.2);
    motorR.move(baseSpeed + escapeDir * 0.2);

    delay(50);
}

要点解读
UWB解决“全局定位”，超声波负责“近场安全兜底”：UWB提供厘米级（±10cm）的中远距离定位，抗多径干扰且穿透非金属材质能力强，适合开阔区域的多机相对定位；但近距离（<30cm）存在信号衰减和NLOS误差，此时超声波以约0.1-3m的有效测距范围补位，构成“远-UWB、近-超声”的安全冗余架构。

传感器分工，但必须融合后才能“平滑”：UWB原始数据存在跳变（受环境影响），超声波数据也有噪声。工程上需在控制前端加入扩展卡尔曼滤波（EKF）或互补滤波，将UWB坐标、IMU姿态、超声波测距深度融合，输出平滑轨迹，避免机器人因数据抖动出现“抽风式”运动。

避障优先级必须高于跟随，且“硬中断”级响应：在多机器人协同场景中，安全永远是第一位的。控制架构应设计为：当超声波探测到障碍物进入安全阈值时，无条件挂起跟随/路径跟踪任务，执行紧急制动或绕行，这一判断不应依赖复杂的决策层，而应在底层实时响应。参考案例三中的“超时降级”策略，确保丢包时机器人按最后一帧指令短时维持运动。

多机协同避障需要“信息共享”避免各自为政：单机感知能力有限（盲区、遮挡），多台机器人通过Zigbee/ESP-NOW/CAN共享障碍物信息和位姿，能构建更完整的局部地图。案例三中的信息素机制，实质是一种去中心化的隐式协调——各方向的信息素代表该方向的历史“安全评价”，帮助机器人避开其他机器人刚发现的障碍区域，是一种轻量级的协同记忆。

BLDC FOC是实现“柔顺避障”的物理基础：采用FOC矢量控制的BLDC电机可实现低速大扭矩、零转速平稳运行，使机器人启动、急停、转向都“顺滑”无顿挫。在近距离跟随（1-2m）和避障急停场景中，传统有刷电机的低速抖动和换向冲击会严重影响控制精度和用户体验，而FOC驱动的BLDC能实现毫秒级力矩响应和平滑换向，确保协同避障动作的可靠性。

4、地震废墟搜救——弹性链式编队协同避障（UWB定位+超声波避障）
适用场景：地震废墟中，多台BLDC机器人组成链式编队深入探测，UWB提供高精度相对定位，超声波检测局部障碍，编队通过弹性间距调整与自适应旋转避开坍塌物，同时通过链式通信中继回传数据。

核心逻辑：
UWB定位：领航者通过UWB获取与从机的相对位置，从机通过UWB实时跟踪领航者位姿；
超声波避障：每台机器人配备前向超声波传感器，检测近距离障碍，触发局部避障；
弹性链式控制：从机与领航者保持弹性间距（弹簧-阻尼模型），遇障碍时自动调整间距，避免碰撞；
自适应旋转：领航者通过超声波评估周围开阔度，自动选择旋转方向（顺时针/逆时针）绕开障碍。

/* ===== 废墟搜救：弹性链式编队+UWB+超声波融合避障 =====
 * 核心：UWB定位保持编队，超声波局部避障，自适应旋转绕障
 * 适配：1领航+2从机，链式弹性结构
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19), encR(20,21);
#define TRIG_F 2  // 前向超声波
#define ECHO_F 3
NewPing sonarF(TRIG_F, ECHO_F, 200);
// UWB模块引脚（简化，实际需接UWB驱动库）
#define UWB_RX 4
#define UWB_TX 5

// 编队参数
const float SPRING_K = 2.0;      // 弹簧劲度系数
const float NATURAL_LENGTH = 0.9; // 期望间距（m）
const float REPULSE_RANGE = 1.2;  // 超声波斥力作用范围（m）
float leader_x = 0, leader_y = 0; // 领航者坐标
float follower_x = 0.5, follower_y = -0.8; // 从机坐标（UWB获取）

// 核心函数
void initBLDC() {
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

// 模拟UWB获取从机坐标（实际需替换为UWB数据解析）
void updateFollowerPos() {
  // 此处为简化，实际从UWB模块读取数据
  follower_x = leader_x + 0.5;
  follower_y = leader_y - 0.8;
}

// 弹性编队控制
void elasticFormationControl() {
  updateFollowerPos();
  float dx = leader_x - follower_x;
  float dy = leader_y - follower_y;
  float dist = sqrt(dx*dx + dy*dy);
  // 弹簧力+阻尼力
  float spring_force = SPRING_K * (dist - NATURAL_LENGTH);
  float damping_force = 0.6 * (leader_x - follower_x - dx); // 阻尼项
  float target_speed = spring_force + damping_force;
  motorL.move(target_speed);
  motorR.move(target_speed);
}

// 超声波避障+自适应旋转
void adaptiveObstacleAvoid() {
  float obstacle_dist = sonarF.ping_cm() / 100.0; // 转换为米
  if (obstacle_dist < REPULSE_RANGE) {
    // 评估顺时针/逆时针哪个方向更开阔
    float cw_score = 0, ccw_score = 0;
    // 简化：假设左侧障碍少则顺时针，右侧少则逆时针
    if (obstacle_dist < 0.5) {
      // 近距离障碍，优先向开阔侧旋转
      cw_score = 1.0; ccw_score = 0.5;
    }
    float direction = (cw_score > ccw_score) ? 1.0 : -1.0; // 1=顺时针
    // 施加旋转力
    motorL.move(-1.0 * direction);
    motorR.move(1.0 * direction);
    delay(500);
  } else {
    elasticFormationControl();
  }
}

void setup() {
  Serial.begin(115200);
  initBLDC();
  pinMode(TRIG_F, OUTPUT);
  pinMode(ECHO_F, INPUT);
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  adaptiveObstacleAvoid();
  delay(50);
}

5、工业仓储AGV——多机器人协同搬运避障（UWB定位+超声波融合）
适用场景：仓库中多台AGV协同搬运大型货物，UWB实现厘米级定位，超声波检测货架、人员等动态障碍，通过一致性算法保持菱形编队，遇狭窄通道时自动切换队形并调整旋转方向。

核心逻辑：
UWB高精度定位：多台AGV通过UWB实时共享位置，确保编队几何精度；
超声波动态避障：融合超声波与UWB数据，区分静态货架与动态人员，针对性避障；
一致性编队控制：采用去中心化一致性算法，任一AGV故障不影响整体编队；
队形动态切换：狭窄通道时从菱形切换为纵队，旋转方向根据通道宽度自适应调整。

/* ===== 工业仓储：多AGV协同搬运+UWB+超声波融合避障 =====
 * 核心：一致性算法保持编队，UWB定位，超声波动态避障
 * 适配：3台AGV，菱形→纵队队形切换
 */
#include <SimpleFOC.h>
#include <ModbusRTU.h>
#include <SoftwareSerial.h>

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19), encR(20,21);
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarF(TRIG_F, ECHO_F, 200);
SoftwareSerial rs485Serial(2,3);
ModbusRTU modbus;

// 节点参数
#define NODE_ID 2  // 从机ID
const int NUM_NODES = 3;
struct AgentState {
  float x, y, heading;
} self, neighbors[NUM_NODES];

// 编队参数
float formation_offset[3][2] = {{0,0}, {0,0.8}, {0,1.6}}; // 纵队偏移
float consensus_gain = 0.1;

// 核心函数
void initBLDC() {
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

// 一致性算法：计算编队中心
void consensusCenter() {
  float sum_x = self.x, sum_y = self.y;
  for (int i=0; i<NUM_NODES; i++) {
    if (neighbors[i].x != 0 || neighbors[i].y != 0) {
      sum_x += neighbors[i].x;
      sum_y += neighbors[i].y;
    }
  }
  float center_x = sum_x / NUM_NODES;
  float center_y = sum_y / NUM_NODES;
  // 跟踪目标位置
  float target_x = center_x + formation_offset[NODE_ID-1][0];
  float target_y = center_y + formation_offset[NODE_ID-1][1];
  // 计算速度偏差
  float dx = target_x - self.x;
  float dy = target_y - self.y;
  float speed = sqrt(dx*dx + dy*dy) * consensus_gain;
  motorL.move(speed);
  motorR.move(speed);
}

// 超声波避障
void ultrasonicAvoid() {
  float dist = sonarF.ping_cm() / 100.0;
  if (dist < 0.8) { // 障碍距离小于0.8m，减速避让
    motorL.move(0.3);
    motorR.move(0.3);
  }
}

void setup() {
  Serial.begin(115200);
  rs485Serial.begin(9600);
  modbus.begin(rs485Serial);
  initBLDC();
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  // 读取邻节点UWB数据（通过RS485）
  // 此处简化，实际需解析UWB定位数据
  consensusCenter();
  ultrasonicAvoid();
  delay(50);
}

3、园区安防巡逻——多机器人环绕式避障（UWB定位+超声波融合）
适用场景：园区内多台巡逻机器人组成环绕编队，UWB提供高精度定位，超声波检测行人、车辆等动态障碍，编队通过自适应旋转调整巡逻方向，避开障碍并保持覆盖范围。

核心逻辑：
UWB定位与环绕编队：机器人通过UWB保持环绕队形，中心节点为领航者，从机围绕领航者旋转；
超声波多方向检测：机器人配备多路超声波（前、左、右），检测不同方向障碍；
自适应旋转避障：根据多方向超声波数据，评估障碍分布，自动调整旋转方向，避开密集障碍；
环绕弹性控制：从机与领航者保持弹性距离，遇障碍时自动收缩或展开队形。

/* ===== 园区巡逻：环绕编队+UWB+超声波融合避障 =====
 * 核心：UWB定位保持环绕队形，多方向超声波避障，自适应旋转
 * 适配：1领航+2从机，环绕弹性编队
 */
#include <SimpleFOC.h>
#include <NewPing.h>

// 硬件定义
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19), encR(20,21);
// 多方向超声波
#define TRIG_F 2 #define ECHO_F 3
#define TRIG_L 4 #define ECHO_L 5
#define TRIG_R 6 #define ECHO_R 7
NewPing sonarF(TRIG_F, ECHO_F, 200);
NewPing sonarL(TRIG_L, ECHO_L, 200);
NewPing sonarR(TRIG_R, ECHO_R, 200);

// 环绕编队参数
float leader_x = 0, leader_y = 0, leader_theta = 0; // 领航者位姿（UWB获取）
float self_angle = 120; // 从机环绕角度（度）
const float RADIUS = 1.0; // 环绕半径（m）
const float SPRING_K = 1.5;

// 核心函数
void initBLDC() {
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

// 计算环绕目标位置
void calculateTargetPos() {
  float rad = self_angle * PI / 180;
  float target_x = leader_x + RADIUS * cos(rad + leader_theta);
  float target_y = leader_y + RADIUS * sin(rad + leader_theta);
  // 弹性跟踪目标
  float dx = target_x - self_x;
  float dy = target_y - self_y;
  float dist = sqrt(dx*dx + dy*dy);
  float speed = SPRING_K * dist;
  motorL.move(speed);
  motorR.move(speed);
}

// 多方向超声波避障+自适应旋转
void multiDirectionAvoid() {
  float distF = sonarF.ping_cm() / 100.0;
  float distL = sonarL.ping_cm() / 100.0;
  float distR = sonarR.ping_cm() / 100.0;
  
  // 评估障碍分布，选择旋转方向
  if (distF < 0.6 && distL > distR) {
    // 前方障碍，左侧更开阔，顺时针旋转
    motorL.move(-0.8);
    motorR.move(0.8);
    self_angle += 5; // 调整环绕角度
  } else if (distF < 0.6 && distR > distL) {
    // 前方障碍，右侧更开阔，逆时针旋转
    motorL.move(0.8);
    motorR.move(-0.8);
    self_angle -= 5;
  } else if (distL < 0.4) {
    // 左侧障碍，向右避让
    motorL.move(0.6);
    motorR.move(-0.6);
  } else if (distR < 0.4) {
    // 右侧障碍，向左避让
    motorL.move(-0.6);
    motorR.move(0.6);
  } else {
    calculateTargetPos();
  }
}

void setup() {
  Serial.begin(115200);
  initBLDC();
  pinMode(TRIG_F, OUTPUT); pinMode(ECHO_F, INPUT);
  pinMode(TRIG_L, OUTPUT); pinMode(ECHO_L, INPUT);
  pinMode(TRIG_R, OUTPUT); pinMode(ECHO_R, INPUT);
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  multiDirectionAvoid();
  delay(50);
}

要点解读

1. UWB与超声波的“互补融合逻辑”：解决单传感器局限
   UWB与超声波的融合并非简单叠加，而是基于场景互补的精准分工：
   UWB核心作用：提供厘米级高精度相对定位，解决多机器人编队的“位置同步”问题，确保编队几何精度，不受环境遮挡影响；
   超声波核心作用：弥补UWB无法检测近距离障碍的短板，实现近距离（0-2m）障碍检测，响应速度快（毫秒级），可实时触发避障动作；
   融合策略：UWB负责全局编队定位，超声波负责局部避障，两者数据通过时间戳对齐与加权滤波，避免单一传感器失效（如UWB受遮挡、超声波受软材质干扰）导致的避障失效，提升系统鲁棒性。
2. 协同避障的“分级决策机制”：平衡安全与效率
   多机器人协同避障需兼顾安全与通行效率，核心是分级响应与优先级控制：
   优先级排序：避障优先级高于编队保持，当超声波检测到近距离障碍时，立即触发避障动作，暂停编队跟踪，避免碰撞；障碍消除后，自动恢复编队；
   分级避障策略：根据障碍距离划分三级响应——预警区（>0.8m）减速，减速区（0.4-0.8m）调整方向，危险区（<0.4m）紧急制动，避免过度避障导致编队频繁抖动；
   多机协同规则：从机避障时需参考领航者位姿，避免与领航者或其他从机发生二次碰撞，例如从机向左避障时，需同步调整旋转方向，确保与领航者的运动方向一致。
3. BLDC驱动的“动态响应匹配”：保障避障与编队流畅性
   BLDC电机的高动态响应是多机器人协同的核心支撑，需重点匹配避障与编队控制需求：
   毫秒级响应特性：BLDC采用FOC磁场定向控制，转矩响应时间≤10ms，可快速跟踪避障时的加减速指令，避免因响应滞后导致编队脱节或避障不及时；
   速度闭环控制：通过编码器实现速度闭环，确保机器人在避障时的加减速平稳，避免速度波动导致的编队抖动，同时抑制打滑对定位的影响；
   柔性启停设计：避障动作采用线性加减速曲线，而非突变启停，减少机械冲击，提升运动平顺性，尤其适用于狭窄空间内的频繁转向与避障。
4. 通信与算力的“实时性保障”：多机协同的基础前提
   多机器人协同避障对通信与算力的实时性要求极高，需解决两大核心问题：
   通信低延迟与可靠性：采用RS485或LoRa等低延迟通信方式，确保UWB定位数据与避障指令的实时传输，通信周期≤50ms，避免因延迟导致编队位姿不同步；同时加入CRC校验与重传机制，应对复杂环境下的通信丢包；
   算力分配与非阻塞编程：UWB数据解析、融合算法、BLDC控制需占用大量算力，标准Arduino难以胜任，建议采用ESP32、STM32等32位MCU；采用非阻塞编程（如millis()定时器），避免delay()阻塞主循环，确保控制频率≥50Hz，保证避障决策的实时性。
5. 安全冗余与容错设计：极端场景的生存底线
   应急救援、工业仓储等场景对安全性要求极高，需构建多层级安全冗余：
   硬件级安全冗余：保留物理急停按钮，配备机械防撞条，电源采用隔离设计，避免BLDC电机的电磁干扰影响传感器与通信模块；
   软件级容错机制：当UWB通信丢失或传感器失效时，机器人自动切换为“故障降级模式”，停止编队跟踪，仅保留局部避障功能，避免失控；同时设置通信超时保护，超过一定时间未收到领航者数据时，自动执行安全停机；
   避障算法容错：对超声波数据进行滤波处理，避免偶发噪点触发误避障；同时设置转向切换迟滞区间，防止在障碍临界距离时频繁切换旋转方向，导致机器人抖动。

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