以专业视角来看，基于 Arduino 生态（通常以 ESP32 或 STM32 等高性能 MCU 为核心）的园区物资配送机器人多目标动态任务分配系统，是一个集成了多机通信、智能调度算法、高精度定位与底层高动态运动控制的复杂机器人工程。以下是该系统的主要特点、应用场景及注意事项的详细解析：
一、 主要特点

1. 智能动态任务分配与调度
   系统摒弃了固定的指派模式，引入了动态任务分配算法。调度中心（或分布式节点）会综合考量任务优先级、截止时间、机器人当前状态（如位置、剩余电量、当前负载）等维度，实时计算并指派最优的配送任务。例如，当距离最近的机器人正在执行高优先级任务时，系统会自动将新的配送指令分配给次优的闲置机器人，实现全局效率最大化。
2. 多机协同与接力配送机制
   针对大型园区的长距离运输，系统支持“货物接力”模式。多台自主移动机器人（AMR）通过协作，在预设的交接点（如固定货架或暂存台）将载荷传递给下一台机器人，形成接力链。这要求机器人具备通信协商（如通过 Wi-Fi/LoRa Mesh 网络）和任务交接的能力，以覆盖整个园区的广阔空间。
3. 多源融合定位与高精度停靠
   园区环境复杂，系统通常采用多传感器融合方案（如 SLAM 激光雷达 + 轮式编码器 + IMU）进行绝对定位修正，确保机器人在复杂的室外或半室外环境中不迷路。特别是在交接点，机器人必须能实现厘米级的精准停靠，以保证物理或逻辑载荷转移的可靠性。
4. 高动态响应与柔顺控制
   底层采用 BLDC（无刷直流电机）配合 FOC（磁场定向控制）驱动器。FOC 算法能够精准控制电机的扭矩和速度，具备极低的转矩脉动和毫秒级电流响应。这不仅使得机器人启动和停止极其平滑，有效保护了所承载的快递包裹或物资，还能在狭窄的站点通道内灵活穿梭。
   二、 应用场景
5. 大型园区/企业末端物流配送
   在大学校园或大型企业园区内，中央仓库的机器人将包裹运出，到达宿舍区或办公楼附近的固定交接点，再由该区域的“最后一公里”机器人接力送至楼下收件柜或前台。
6. 医院内部物资与药品配送
   药房的机器人将药品送往住院部交接点，由病区内的机器人接手，送至各护士站。此场景要求运行安静、平稳，符合医院环境要求，且需具备极高的调度可靠性。
7. 快递分拣中心内部转运
   将从主分拣线下来的包裹，由入口区的机器人运送到指定的装车月台区。途中可能需要经过中继站与其他机器人接力，以覆盖庞大的厂房空间，实现中低速、中轻载的点对点运输。
8. 图书馆书籍归还与上架
   读者在自助机归还书籍后，机器人将其运往编目区，处理完毕后再由另一台机器人接力运送至相应书库进行上架，实现自动化流转。
   三、 需要注意的事项
9. 算力瓶颈与分布式架构设计
   多目标动态分配逻辑、多传感器融合以及全局路径规划对算力要求极高。标准 Arduino Uno/Nano 极易因内存溢出或浮点运算过载而崩溃。强烈建议采用 ESP32、STM32 等高算力主控，或采用“上位机（云端或本地服务器）解算与调度 + Arduino 底层 BLDC 控制”的分布式架构。
10. 通信可靠性与心跳机制
    接力任务高度依赖稳定、低延迟的通信网络。信号盲区、干扰或丢包可能导致任务失败或机器人“迷路”。必须设计心跳保活机制和断线重连策略，确保机器人本身作为执行节点能与调度服务器保持实时同步。
11. 严格的电源隔离与电磁兼容（EMC）
    BLDC 电机在启停时会产生巨大的电流冲击和高频 PWM 噪声，极易导致主控板电压跌落复位或传感器数据乱跳。必须为 BLDC 驱动和 Arduino 主控提供独立的电源（严禁共用），并在电源端加装滤波电路，防止电机干扰导致系统失控。
12. 完善的安全机制与容错设计
    必须设计完善的安全策略。当检测到传感器数据异常或通信完全丢失时，机器人应自动触发减速或急停机制。此外，在多机器人同场作业时，需合理规划通信时隙，避免多标签间的射频碰撞与相互干扰；对于重载或重心偏移的物资，系统需具备自适应负载补偿能力，防止跑偏。

1、基于分布式拍卖的动态任务分配（多机抢单）
适用场景：园区多台配送机器人独立运作，新订单到来时通过无线通信进行"拍卖"，由距离最近、电量最充裕的机器人"竞标"获得任务执行权。
核心逻辑：每台机器人维护自身状态（位置、电量、当前任务负载），通过ESP-NOW广播任务竞标请求。各节点根据自身条件计算"竞标成本"（综合考虑距离、电量、负载），成本最低者胜出并执行任务。出价最高的机器人认领任务并更新目标点。

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

// ==================== 机器人状态 ====================
struct RobotState {
    uint8_t id;
    float x, y;              // 当前位置（UWB/里程计）
    float battery;           // 剩余电量 0~1
    uint8_t taskCount;       // 当前任务数
    float speed;             // 当前速度
};

RobotState self = {1, 0, 0, 0.95, 0, 0};
RobotState peers[5];         // 最多5个邻居
int peerCount = 0;

// ==================== 任务结构 ====================
struct DeliveryTask {
    uint16_t taskId;
    float targetX, targetY;
    float reward;            // 任务价值（距离权重）
    uint8_t assignedTo;      // 0=未分配
};

DeliveryTask activeTask = {0, 0, 0, 0, 0};

// ==================== 拍卖参数 ====================
const float COST_DIST_WEIGHT = 2.0;   // 距离成本权重
const float COST_BATTERY_WEIGHT = 1.5; // 低电量惩罚权重
const float MAX_BID_DIST = 20.0;       // 最大竞标距离(m)

// ESP-NOW回调
void onDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
    // 接收邻居状态或拍卖请求
}

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机与FOC
    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;

    // 初始化ESP-NOW（略）
    WiFi.mode(WIFI_STA);
    esp_now_init();
    esp_now_register_recv_cb(onDataRecv);
}

// ==================== 竞标成本计算 ====================
float calculateBid(DeliveryTask task, RobotState robot) {
    float dx = task.targetX - robot.x;
    float dy = task.targetY - robot.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    // 距离成本：越远成本越高
    float distCost = dist * COST_DIST_WEIGHT;
    // 电量成本：电量越低，成本越高（惩罚电量不足）
    float batteryCost = (1.0 - robot.battery) * COST_BATTERY_WEIGHT;
    // 负载成本：当前任务越多，成本越高
    float loadCost = robot.taskCount * 0.5;
    
    return distCost + batteryCost + loadCost;
}

// ==================== 竞标与任务认领 ====================
bool participateInAuction(DeliveryTask task) {
    float myBid = calculateBid(task, self);
    
    // 超出竞标范围则放弃
    float dx = task.targetX - self.x;
    float dy = task.targetY - self.y;
    if (sqrt(dx*dx + dy*dy) > MAX_BID_DIST) return false;
    
    // 广播自己的出价（实际通过ESP-NOW）
    // 收集各邻居的出价，选最低者获胜
    // 此处简化为：自己出价低于阈值则认领
    if (myBid < 10.0) {
        activeTask = task;
        activeTask.assignedTo = self.id;
        return true;
    }
    return false;
}

// ==================== 执行配送任务 ====================
void executeTask() {
    if (activeTask.assignedTo != self.id) return;
    
    float dx = activeTask.targetX - self.x;
    float dy = activeTask.targetY - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    if (dist < 0.2) {
        // 到达目标，任务完成
        activeTask.assignedTo = 0;
        self.taskCount--;
        return;
    }
    
    float speed = constrain(dist * 0.6, 0.1, 0.8);
    float angle = atan2(dy, dx);
    float wheelBase = 0.25;
    motorL.move(speed - angle * wheelBase / 2);
    motorR.move(speed + angle * wheelBase / 2);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 模拟新任务到达（实际通过WiFi/串口接收）
    static unsigned long lastTaskTime = 0;
    if (millis() - lastTaskTime > 10000 && activeTask.assignedTo == 0) {
        DeliveryTask newTask = {1, random(5, 15), random(5, 15), 0, 0};
        if (participateInAuction(newTask)) {
            lastTaskTime = millis();
        }
    }
    
    // 执行任务
    executeTask();
    
    // 更新位置（里程计积分）
    // self.x, self.y 通过编码器更新
    delay(50);
}

2、Voronoi区域划分 + BFS路径分配（集群覆盖遍历）
适用场景：多台机器人协同完成园区大面积巡视/盘点任务，通过Voronoi图将区域划分为多块，各机器人负责自己区域内的BFS路径规划与遍历。
核心逻辑：中央节点（或通过分布式协商）基于各机器人当前位置生成Voronoi图，将目标点按所属区域分配给对应机器人。各机器人在各自区域内通过BFS规划最优遍历路径。BLDC执行层确保路径跟随精度。

#include <SimpleFOC.h>
#include <esp_now.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 MAX_ROBOTS 4
#define MAP_SIZE 20   // 20x20栅格

struct RobotPos {
    float x, y;
    uint8_t id;
};
RobotPos robotPositions[MAX_ROBOTS];
int robotCount = 0;

// ==================== Voronoi划分 + BFS ====================
int8_t voronoiMap[MAP_SIZE][MAP_SIZE];  // 每个栅格所属机器人ID (-1=未分配)
uint8_t visited[MAP_SIZE][MAP_SIZE];    // 已遍历标记
uint8_t taskQueue[100][2];              // BFS任务队列
int queueHead = 0, queueTail = 0;

// 邻居状态接收
void onDataRecv(const uint8_t *mac, const uint8_t *incomingData, int len) {
    // 接收其他机器人位置
}

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机（略）
    // 初始化ESP-NOW（略）
    
    robotPositions[0] = {0, 0, 1};  // 自身位置
    robotCount = 1;
}

// ==================== Voronoi区域划分 ====================
void computeVoronoi() {
    // 初始化地图
    for(int i=0; i<MAP_SIZE; i++)
        for(int j=0; j<MAP_SIZE; j++)
            voronoiMap[i][j] = -1;
    
    // 对每个栅格，分配到最近的机器人
    for(int i=0; i<MAP_SIZE; i++) {
        for(int j=0; j<MAP_SIZE; j++) {
            float minDist = 999;
            int owner = -1;
            for(int k=0; k<robotCount; k++) {
                float dx = i - robotPositions[k].x;
                float dy = j - robotPositions[k].y;
                float d = dx*dx + dy*dy;
                if(d < minDist) {
                    minDist = d;
                    owner = k;
                }
            }
            voronoiMap[i][j] = owner;
        }
    }
}

// ==================== BFS路径规划（区域内） ====================
void bfsPlanPath(int startX, int startY, int targetX, int targetY) {
    // BFS标准实现，仅搜索属于自己区域的栅格
    int8_t myId = 0;  // 自身ID
    int dirs[4][2] = {{0,1}, {1,0}, {0,-1}, {-1,0}};
    bool localVisited[MAP_SIZE][MAP_SIZE] = {false};
    int queue[100][2];
    int head = 0, tail = 0;
    
    queue[tail][0] = startX; queue[tail][1] = startY;
    localVisited[startX][startY] = true;
    tail++;
    
    while(head < tail) {
        int x = queue[head][0], y = queue[head][1];
        head++;
        
        if(x == targetX && y == targetY) {
            // 找到路径，转换为电机指令
            return;
        }
        
        for(int d=0; d<4; d++) {
            int nx = x + dirs[d][0];
            int ny = y + dirs[d][1];
            if(nx>=0 && nx<MAP_SIZE && ny>=0 && ny<MAP_SIZE) {
                if(!localVisited[nx][ny] && voronoiMap[nx][ny] == myId) {
                    localVisited[nx][ny] = true;
                    queue[tail][0] = nx; queue[tail][1] = ny;
                    tail++;
                }
            }
        }
    }
}

// ==================== 遍历执行 ====================
void executeCoverage() {
    // 简化：按栅格逐格遍历
    static int gridX = 0, gridY = 0;
    if(voronoiMap[gridX][gridY] == 0 && !visited[gridX][gridY]) {
        // 移动到该栅格
        float dx = gridX - robotPositions[0].x;
        float dy = gridY - robotPositions[0].y;
        float dist = sqrt(dx*dx + dy*dy);
        if(dist < 0.2) {
            visited[gridX][gridY] = 1;
            gridX++;
            if(gridX >= MAP_SIZE) { gridX = 0; gridY++; }
        } else {
            float speed = constrain(dist * 0.5, 0.1, 0.6);
            float angle = atan2(dy, dx);
            float wheelBase = 0.25;
            motorL.move(speed - angle * wheelBase / 2);
            motorR.move(speed + angle * wheelBase / 2);
        }
    }
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 更新其他机器人位置（通过ESP-NOW）
    // 2. 重新计算Voronoi划分
    computeVoronoi();
    // 3. 执行区域遍历
    executeCoverage();
    
    delay(50);
}

3、集中式任务调度 + P-IDM速度优先级仲裁（多机协同配送）
适用场景：园区中央调度系统统一分配配送订单，多台机器人在共享通道上运行时，通过P-IDM速度仲裁实现自组织通行，避免碰撞与拥堵。
核心逻辑：中央调度器通过MQTT向各机器人下发任务目标点。各机器人在执行配送过程中，通过ESP-NOW广播自身位置与优先级，运行P-IDM（Priority-IDM）速度模型——高优先级机器人保持更近跟车距离和更高速度，低优先级自动减速让行，实现无需集中调度的走廊通行秩序。

#include <SimpleFOC.h>
#include <esp_now.h>
#include <WiFi.h>
#include <PubSubClient.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);

// ==================== P-IDM参数 ====================
const float MAX_SPEED = 2.0;
const float MIN_SPEED = 0.1;
const float SAFE_DIST = 1.5;      // 期望安全间距(m)
const float TIME_HEADWAY = 1.2;   // 时距(s)

// ==================== 机器人与任务 ====================
struct RobotState {
    float x, y, speed;
    uint8_t priority;    // 0=最低，9=最高
    float heading;
};
RobotState self = {0, 0, 0, 5, 0};
RobotState neighbors[5];
int neighborCount = 0;

struct TaskPoint {
    float x, y;
    bool active;
};
TaskPoint currentGoal = {10, 5, false};

// ==================== P-IDM速度计算 ====================
float computePIDMSpeed(RobotState self, RobotState front) {
    float dx = front.x - self.x;
    float dy = front.y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    // 计算相对速度
    float dv = self.speed - front.speed;
    
    // 期望间距：基础安全距离 + 时距×速度
    float desiredGap = SAFE_DIST + TIME_HEADWAY * self.speed;
    
    // 根据优先级调整（高优先级容忍更小间距）
    float priorityFactor = 1.0 + (self.priority - 3) * 0.1;
    desiredGap = desiredGap / priorityFactor;
    
    // IDM加速公式（简化）
    float sStar = desiredGap + self.speed * dv / (2 * sqrt(1.0));
    float acceleration = 0.5 * (1 - pow(self.speed / MAX_SPEED, 4) 
                                 - pow(sStar / dist, 2));
    
    float targetSpeed = self.speed + acceleration * 0.05;
    return constrain(targetSpeed, MIN_SPEED, MAX_SPEED);
}

// ==================== 到达目标 ====================
void moveToGoal() {
    if(!currentGoal.active) {
        motorL.move(0); motorR.move(0);
        return;
    }
    
    float dx = currentGoal.x - self.x;
    float dy = currentGoal.y - self.y;
    float dist = sqrt(dx*dx + dy*dy);
    
    if(dist < 0.3) {
        currentGoal.active = false;
        motorL.move(0); motorR.move(0);
        return;
    }
    
    // 目标方向
    float targetAngle = atan2(dy, dx);
    
    // P-IDM速度（找前方最近机器人作为跟随目标）
    RobotState front = {0, 0, 0, 0, 0};
    float minDist = 999;
    for(int i=0; i<neighborCount; i++) {
        float d = sqrt(pow(neighbors[i].x - self.x, 2) + pow(neighbors[i].y - self.y, 2));
        if(d < minDist && d > 0.1) {
            minDist = d;
            front = neighbors[i];
        }
    }
    
    float speed = MAX_SPEED;
    if(minDist < 8.0 && minDist > 0.1) {
        speed = computePIDMSpeed(self, front);
    }
    
    // 差速驱动
    float vLin = constrain(speed, 0.1, MAX_SPEED);
    float vAng = constrain(atan2(dy, dx) * 1.2, -0.6, 0.6);
    float wheelBase = 0.25;
    motorL.move(vLin - vAng * wheelBase / 2);
    motorR.move(vLin + vAng * wheelBase / 2);
}

// ==================== MQTT回调 ====================
void mqttCallback(char* topic, byte* payload, unsigned int length) {
    // 接收中央调度器下发的任务
    // 格式: "goal,x,y"
    if(strcmp(topic, "delivery/task") == 0) {
        float x, y;
        sscanf((char*)payload, "%f,%f", &x, &y);
        currentGoal = {x, y, true};
    }
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 广播自身状态（ESP-NOW）
    // 接收邻居状态
    
    // 执行任务
    moveToGoal();
    
    delay(50);
}

要点解读
分布式拍卖算法是实现"无中心调度"任务分配的轻量级方案：仓储补货场景中，机器人通过无线通信（ESP-NOW）广播任务竞标请求，各节点根据距离、剩余电量、当前任务负载综合计算竞标成本。成本最低者胜出并执行任务，无需中央调度器，天然具备冗余性。此方案尤其适合园区场景中WiFi覆盖不稳定的边缘区域。

Voronoi区域划分将大范围遍历问题分解为互不重叠的子问题：在多机协同盘点或巡检场景，中央节点基于各机器人当前位置生成Voronoi图，将目标点按所属区域分配给各机器人，确保每个货架/区域被且仅被一台机器人覆盖。学术研究表明，Voronoi划分结合BFS连通性奖惩机制可有效提升区域划分的连通性与任务均衡性。

P-IDM速度仲裁实现"无需协商"的走廊通行秩序：IDM（智能驾驶模型）原本用于自适应巡航控制。P-IDM将任务优先级融入速度规划参数——高优先级机器人保持更近的跟车距离和更高速度，低优先级机器人自动减速让行。集中调度器下发任务后，各机器人仅需广播自身状态，即可在窄通道中自然形成有序通行秩序，无需实时避碰协商。

算力分层是工程落地的"黄金架构"：动态区域划分、路径规划及UWB数据解析计算量巨大，标准Arduino Uno无法胜任。标准架构为上位机（树莓派/ESP32，运行协同算法与任务调度）+下位机（Arduino，专职BLDC FOC控制）。上位机通过MQTT下发目标点，下位机执行速度闭环。ES32双核架构下，Core 0专职1kHz FOC控制，Core 1运行规划与通信，确保"边算边走"不卡顿。

BLDC FOC是速度指令"精准执行"的物理保障：任务仲裁输出的速度指令是连续变化的（如P-IDM输出从0.3m/s降至0.05m/s）。普通有刷电机在低速下抖动严重，难以精准响应。BLDC配合FOC控制可实现毫秒级扭矩响应和低速平稳运行，确保每次速度仲裁结果被平滑执行，减少因速度阶跃导致的货架倾倒和定位超调。

4、园区多优先级任务动态排序程序（适配急件/常规件配送）
适用场景：园区物资配送的核心基础——多优先级任务动态管控，适用于写字楼文件急送、车间零部件紧急补料、生活区物资常规配送等场景，需根据物资紧急程度、配送时限动态调整任务优先级，为核心调度系统提供排序基础，避免配送资源错配，确保紧急物资优先配送。
核心逻辑：通过串口或无线模块接收物资任务信息（任务ID、目的地坐标、优先级等级、预计送达时限、物资重量），结合实时时间，建立任务队列；采用“多维优先级算法”对任务动态排序，优先级维度包含物资紧急等级（急件1级/常规件2级）、剩余配送时间裕度、任务权重；排序完成后，将优先级最高的任务下发至机器人执行，同时联动BLDC电机驱动模块设定行驶速度，保障急件任务的快速响应，适配园区动态变化的任务需求。

// 园区多优先级任务动态排序程序（Arduino核心主控，适配Mega2560/ESP32）
#include <Arduino.h>
#include <Queue.h>  // 引入队列库，管理任务队列

// 核心配置参数（按园区实际需求调整）
#define MAX_TASKS 20        // 最大同时处理任务数
#define EMERGENCY_PRIORITY 1  // 急件优先级等级
#define NORMAL_PRIORITY 2      // 常规件优先级等级
#define TIME_LIMIT_COEF 1000   // 时限换算系数（ms对应1秒）

// 任务结构体定义
struct DeliveryTask {
  String taskId;        // 任务唯一标识
  float targetX;        // 配送目标坐标X（园区地图坐标）
  float targetY;        // 配送目标坐标Y
  int priorityLevel;    // 优先级等级（1=急件，2=常规）
  unsigned long deadline;  // 配送截止时间（Unix毫秒时间戳）
  float cargoWeight;    // 物资重量（kg）
  bool isAssigned;      // 是否已分配给机器人
  unsigned long createTime; // 任务创建时间
};

// 全局任务队列与优先级排序相关变量
Queue<DeliveryTask> taskQueue;  // 待处理任务队列
DeliveryTask sortedTasks[MAX_TASKS]; // 排序后的任务数组
int currentSortedCount = 0;        // 已排序任务数量
unsigned long currentTimeMillis = 0; // 实时时间戳

// BLDC电机控制接口（预留，后续联动速度）
void setBLDCSpeed(int speedLevel); // 设置电机速度等级，预留实现

// 多维度优先级评分函数（核心算法）
int calculateTaskScore(DeliveryTask task) {
  unsigned long currentTime = millis();
  // 计算剩余配送时间裕度（剩余时间越短，得分越高）
  unsigned long timeRemaining = task.deadline - currentTime;
  // 避免负数（超时任务，时间裕度为0，得分由优先级兜底）
  if (timeRemaining < 0) timeRemaining = 0;
  
  // 多维度加权评分：优先级权重40%，时间裕度权重30%，任务权重30%
  int priorityScore = (EMERGENCY_PRIORITY - task.priorityLevel + 1) * 40;
  int timeScore = (timeRemaining / TIME_LIMIT_COEF) > 0 ? (100 - (timeRemaining / TIME_LIMIT_COEF)) : 0;
  timeScore = constrain(timeScore, 0, 100); // 限制得分范围
  int weightScore = task.cargoWeight > 50 ? 10 : (task.cargoWeight > 20 ? 5 : 0); // 重量对应权重
  
  return priorityScore + timeScore + weightScore;
}

// 任务动态排序核心函数（冒泡排序，适配Arduino算力）
void dynamicTaskSorting() {
  currentSortedCount = taskQueue.size();
  // 将队列任务转为数组，方便排序
  for (int i = 0; i < currentSortedCount; i++) {
    sortedTasks[i] = taskQueue.peek();
    taskQueue.pop();
  }
  
  // 冒泡排序：按得分降序排列（得分越高，优先级越高）
  for (int i = 0; i < currentSortedCount - 1; i++) {
    for (int j = 0; j < currentSortedCount - i - 1; j++) {
      int scoreA = calculateTaskScore(sortedTasks[j]);
      int scoreB = calculateTaskScore(sortedTasks[j+1]);
      if (scoreA < scoreB) {
        DeliveryTask temp = sortedTasks[j];
        sortedTasks[j] = sortedTasks[j+1];
        sortedTasks[j+1] = temp;
      }
    }
  }
  
  // 排序后重新压入队列，保留排序结果
  for (int i = 0; i < currentSortedCount; i++) {
    taskQueue.push(sortedTasks[i]);
  }
}

// 任务接收处理函数（解析串口传入的任务信息）
void processNewTask(String taskData) {
  // 任务数据格式："ID,X,Y,Priority,Deadline,Weight"，示例："T001,120.5,85.3,1,1700000000000,15.2"
  int delimiterCount = 0;
  int startIndex = 0;
  String tempField = "";
  DeliveryTask newTask;
  
  for (int i = 0; i < taskData.length(); i++) {
    if (taskData[i] == ',') {
      delimiterCount++;
      switch (delimiterCount) {
        case 1: newTask.taskId = tempField; break;
        case 2: newTask.targetX = tempField.toFloat(); break;
        case 3: newTask.targetY = tempField.toFloat(); break;
        case 4: newTask.priorityLevel = tempField.toInt(); break;
        case 5: newTask.deadline = tempField.toInt(); break;
        case 6: newTask.cargoWeight = tempField.toFloat(); break;
      }
      tempField = "";
    } else {
      tempField += taskData[i];
    }
  }
  // 处理最后一个字段
  newTask.cargoWeight = tempField.toFloat();
  
  // 初始化任务参数
  newTask.isAssigned = false;
  newTask.createTime = millis();
  
  // 加入任务队列
  if (taskQueue.size() < MAX_TASKS) {
    taskQueue.push(newTask);
    Serial.print("新任务加入队列：");
    Serial.print(newTask.taskId);
    Serial.println(" 优先级：" + String(newTask.priorityLevel));
    // 触发动态排序
    dynamicTaskSorting();
  } else {
    Serial.println("任务队列已满，无法加入新任务！");
  }
}

// 获取最高优先级任务（供调度系统调用）
DeliveryTask getTopPriorityTask() {
  if (taskQueue.size() == 0) {
    DeliveryTask emptyTask;
    emptyTask.taskId = "NULL";
    return emptyTask;
  }
  return taskQueue.peek();
}

// 标记任务已分配（从队列移除已分配任务）
void markTaskAssigned() {
  if (taskQueue.size() > 0) {
    taskQueue.pop();
  }
}

// BLDC电机速度联动函数（根据任务优先级设置速度）
void linkBLDCWithTaskPriority(DeliveryTask task) {
  if (task.priorityLevel == EMERGENCY_PRIORITY) {
    setBLDCSpeed(5); // 急件：高速模式，适配园区快速行驶
    Serial.println("急件任务：BLDC电机切换至高速模式");
  } else {
    setBLDCSpeed(3); // 常规件：中速模式，保障平稳配送
    Serial.println("常规任务：BLDC电机切换至中速模式");
  }
}

// BLDC电机速度设置（预留实现，需对接实际电机驱动）
void setBLDCSpeed(int speedLevel) {
  // 此处需对接BLDC电机控制库，如SimpleFOC，设置PWM占空比对应速度等级
  // 示例：speedLevel 1-5，对应PWM占空比20%-100%
  int pwmValue = map(speedLevel, 1, 5, 200, 1000);
  // analogWrite(PWM_PIN, pwmValue);  // 实际需根据电机驱动引脚调整
}

void setup() {
  Serial.begin(115200);
  Serial.println("园区多优先级任务动态排序系统启动");
  // 初始化BLDC电机（预留接口，需对接电机初始化代码）
  // initBLDCMotor();
}

void loop() {
  currentTimeMillis = millis();
  // 检测串口是否有新任务
  if (Serial.available() > 0) {
    String taskData = Serial.readStringUntil('\n');
    taskData.trim();
    if (!taskData.isEmpty()) {
      processNewTask(taskData);
    }
  }
  
  // 动态刷新排序（每1秒刷新一次，适配任务时间裕度变化）
  if (currentTimeMillis % 1000 == 0) {
    dynamicTaskSorting();
    Serial.println("任务队列实时排序完成，当前队列长度：" + String(taskQueue.size()));
  }
  
  // 若有可分配任务，联动电机速度
  DeliveryTask topTask = getTopPriorityTask();
  if (!topTask.taskId.equals("NULL") && !topTask.isAssigned) {
    linkBLDCWithTaskPriority(topTask);
  }
  
  delay(100);
}

代码逻辑说明：
多维度优先级算法：结合物资紧急等级、剩余配送时间、物资重量三个核心维度，通过加权评分实现任务优先级的动态计算，避免单一维度导致的优先级偏差，适配园区任务优先级动态变化的核心需求。
动态排序机制：采用队列管理任务，每秒触发一次排序，确保任务队列始终按实时优先级排列，同时预留串口任务接收接口，适配园区任务的动态下达场景，保障调度系统的基础排序能力。
电机速度联动：根据任务优先级等级，直接设定BLDC电机速度等级，急件任务匹配高速模式，常规任务匹配中速模式，实现任务优先级与电机驱动的初步联动，为后续精准配送奠定基础。

5、多机器人资源匹配动态调度程序（电量/载重/位置适配）
适用场景：园区多机器人协同配送的核心调度环节，适用于园区内有多个配送机器人并行作业的场景，需动态统筹机器人的电量、载重能力、实时位置，匹配最适配的任务，避免机器人电量不足时承接任务、超载配送，提升多机器人资源利用率，保障配送任务高效完成。

核心逻辑：通过无线通信模块接收多台机器人的实时状态（当前位置坐标、剩余电量、最大载重、当前负载），结合待配送任务的目的地坐标、物资重量、优先级；采用“多资源匹配算法”，计算每个机器人承接任务的适配得分，得分最高的机器人自动承接该任务；同时在任务执行过程中，实时监测机器人电量，若电量低于阈值，自动触发回收指令，联动BLDC电机驱动调整行驶速度，确保机器人安全返回充电，实现多机器人的动态资源匹配与协同调度。

// 多机器人资源匹配动态调度程序（Arduino主控，适配多机器人协同）
#include <Arduino.h>
#include <vector>
#include <cmath>

// 核心配置参数（按园区实际机器人数量与需求调整）
#define MAX_ROBOTS 5          // 最大支持机器人数量
#define LOW_BATTERY_THRESHOLD 20 // 低电量阈值（%，低于此值触发回收）
#define MAX_LOAD_CAPACITY 100  // 机器人最大载重（kg）
#define DISTANCE_COEF 1000     // 距离换算系数（将坐标距离转为实际距离值）

// 机器人状态结构体
struct RobotState {
  String robotId;      // 机器人唯一标识
  float currentX;      // 实时位置坐标X
  float currentY;      // 实时位置坐标Y
  int batteryLevel;    // 剩余电量百分比（0-100）
  float currentLoad;   // 当前负载重量（kg）
  bool isBusy;         // 是否正在执行任务
  unsigned long lastUpdateTime; // 状态更新时间
};

// 配送任务结构体（复用案例1的任务定义，保持一致）
struct DeliveryTask {
  String taskId;
  float targetX;
  float targetY;
  int priorityLevel;
  unsigned long deadline;
  float cargoWeight;
  bool isAssigned;
  unsigned long createTime;
};

// 全局变量
std::vector<RobotState> robotStates;  // 机器人状态数组
DeliveryTask pendingTasks[MAX_TASKS]; // 待分配任务数组
int pendingTaskCount = 0;            // 待分配任务数量
bool needChargingRecovery = false;    // 是否需要触发充电回收

// BLDC电机控制函数（预留，对接电机速度调整）
void adjustRobotSpeed(String robotId, int speedLevel);
void initBLDCForRobot(String robotId); // 初始化机器人BLDC电机

// 计算机器人与任务目标的距离（欧氏距离，适配园区坐标定位）
float calculateDistance(float x1, float y1, float x2, float y2) {
  return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2)) * DISTANCE_COEF;
}

// 计算机器人承接任务的适配得分（多资源匹配核心算法）
int calculateRobotTaskFitness(RobotState robot, DeliveryTask task) {
  // 判定基础适配条件：未忙碌+电量充足+载重满足+非低电量
  if (robot.isBusy || robot.batteryLevel < 30 || 
      robot.currentLoad + task.cargoWeight > MAX_LOAD_CAPACITY) {
    return -1; // 不满足基础条件，得分为负，不参与匹配
  }
  
  // 多维度适配得分计算：电量得分25%，距离得分25%，载重利用率得分25%，任务优先级匹配得分25%
  int batteryScore = robot.batteryLevel; // 电量越高，得分越高
  float taskDistance = calculateDistance(robot.currentX, robot.currentY, task.targetX, task.targetY);
  // 距离得分：距离越近，得分越高（按园区实际距离调整满分阈值，示例500m为满分）
  int distanceScore = taskDistance < 500 ? map(taskDistance, 0, 500, 100, 0) : 0;
  // 载重利用率得分：剩余载重与任务重量比越匹配，得分越高
  float remainingCapacity = MAX_LOAD_CAPACITY - robot.currentLoad;
  float loadRatio = task.cargoWeight / remainingCapacity;
  int loadScore = (loadRatio <= 1) ? map(loadRatio, 0, 1, 100, 50) : 0;
  // 任务优先级匹配得分：急件优先匹配电量充足、距离近的机器人
  int priorityScore = task.priorityLevel == 1 ? (robot.batteryLevel > 50 ? 100 : 50) : 80;
  
  return batteryScore + distanceScore + loadScore + priorityScore;
}

// 多机器人与任务匹配核心函数
void matchRobotsWithTasks() {
  // 遍历所有待分配任务
  for (int t = 0; t < pendingTaskCount; t++) {
    DeliveryTask *task = &pendingTasks[t];
    if (task->isAssigned) continue; // 已分配任务跳过
    
    int bestRobotIdx = -1;
    int maxFitnessScore = -1;
    
    // 遍历所有机器人，寻找适配得分最高的
    for (int r = 0; r < robotStates.size(); r++) {
      RobotState *robot = &robotStates[r];
      int fitnessScore = calculateRobotTaskFitness(*robot, *task);
      if (fitnessScore > maxFitnessScore) {
        maxFitnessScore = fitnessScore;
        bestRobotIdx = r;
      }
    }
    
    // 若有匹配的机器人，执行任务分配
    if (bestRobotIdx != -1) {
      RobotState *assignedRobot = &robotStates[bestRobotIdx];
      assignedRobot->isBusy = true;
      assignedRobot->currentLoad += task->cargoWeight;
      task->isAssigned = true;
      
      // 联动BLDC电机设置行驶速度（按任务优先级）
      if (task->priorityLevel == 1) {
        adjustRobotSpeed(assignedRobot->robotId, 5); // 急件高速
      } else {
        adjustRobotSpeed(assignedRobot->robotId, 3); // 常规中速
      }
      
      Serial.print("任务分配成功：任务ID ");
      Serial.print(task->taskId);
      Serial.print(" 分配给机器人：");
      Serial.print(assignedRobot->robotId);
      Serial.println(" 适配得分：" + String(maxFitnessScore));
    }
  }
  
  // 检查机器人低电量状态，触发回收指令
  checkRobotBatteryAndRecovery();
}

// 低电量回收检查与触发
void checkRobotBatteryAndRecovery() {
  for (int i = 0; i < robotStates.size(); i++) {
    RobotState *robot = &robotStates[i];
    if (robot->batteryLevel < LOW_BATTERY_THRESHOLD && robot->isBusy) {
      // 低电量且正在执行任务，触发回收
      robot->isBusy = false;
      // 联动BLDC电机调整为回收速度（低速模式）
      adjustRobotSpeed(robot->robotId, 1);
      Serial.print("机器人低电量回收：");
      Serial.print(robot->robotId);
      Serial.println(" 当前电量：" + String(robot->batteryLevel) + "%");
      needChargingRecovery = true;
    }
  }
}

// 接收机器人状态更新（串口或无线接收）
void processRobotStateUpdate(String robotData) {
  // 机器人状态格式："RobotID,X,Y,Battery,Load,Busy"，示例："R001,50.2,68.7,75,12.5,false"
  int delimiterCount = 0;
  String tempField = "";
  String robotId = "";
  float x = 0, y = 0;
  int battery = 0;
  float load = 0;
  bool busy = false;
  
  for (int i = 0; i < robotData.length(); i++) {
    if (robotData[i] == ',') {
      delimiterCount++;
      switch (delimiterCount) {
        case 1: robotId = tempField; break;
        case 2: x = tempField.toFloat(); break;
        case 3: y = tempField.toFloat(); break;
        case 4: battery = tempField.toInt(); break;
        case 5: load = tempField.toFloat(); break;
        case 6: busy = (tempField == "true"); break;
      }
      tempField = "";
    } else {
      tempField += robotData[i];
    }
  }
  
  // 更新机器人状态
  bool found = false;
  for (int i = 0; i < robotStates.size(); i++) {
    if (robotStates[i].robotId == robotId) {
      robotStates[i].currentX = x;
      robotStates[i].currentY = y;
      robotStates[i].batteryLevel = battery;
      robotStates[i].currentLoad = load;
      robotStates[i].isBusy = busy;
      robotStates[i].lastUpdateTime = millis();
      found = true;
      break;
    }
  }
  
  // 新增机器人
  if (!found) {
    RobotState newRobot;
    newRobot.robotId = robotId;
    newRobot.currentX = x;
    newRobot.currentY = y;
    newRobot.batteryLevel = battery;
    newRobot.currentLoad = load;
    newRobot.isBusy = busy;
    newRobot.lastUpdateTime = millis();
    robotStates.push_back(newRobot);
    Serial.println("新增机器人：" + robotId);
    initBLDCForRobot(robotId); // 初始化新增机器人的BLDC电机
  }
}

// 预留的BLDC电机速度调整函数
void adjustRobotSpeed(String robotId, int speedLevel) {
  // 此处需对接对应机器人的BLDC电机控制，按机器人ID设置专属电机速度
  // 示例：通过串口向机器人下发速度指令，或直接控制本地BLDC驱动
  int pwmValue = map(speedLevel, 1, 5, 200, 1000);
  Serial.print("向机器人 ");
  Serial.print(robotId);
  Serial.println(" 下发BLDC速度指令：PWM=" + String(pwmValue));
  // 实际控制代码：digitalWrite(MOTOR_PWM_PIN, pwmValue);
}

// 预留的机器人BLDC电机初始化函数
void initBLDCForRobot(String robotId) {
  // 此处需初始化对应机器人的BLDC电机，配置编码器、PWM等参数
  Serial.println("初始化机器人 " + robotId + " 的BLDC电机");
}

void setup() {
  Serial.begin(115200);
  Serial.println("多机器人资源匹配动态调度系统启动");
  // 初始化示例：预置2台机器人状态
  RobotState robot1 = {"R001", 30.5, 45.2, 80, 5.0, false, millis()};
  RobotState robot2 = {"R002", 70.8, 60.3, 75, 8.0, false, millis()};
  robotStates.push_back(robot1);
  robotStates.push_back(robot2);
}

void loop() {
  // 接收机器人状态更新
  if (Serial.available() > 0) {
    String robotData = Serial.readStringUntil('\n');
    robotData.trim();
    if (!robotData.isEmpty() && robotData.indexOf("RobotID") == -1) { // 避免标题行
      processRobotStateUpdate(robotData);
    }
  }
  
  // 每2秒执行一次资源匹配与调度
  if (millis() % 2000 == 0) {
    matchRobotsWithTasks();
    Serial.println("多机器人资源匹配调度完成");
  }
  
  // 低电量回收状态处理
  if (needChargingRecovery) {
    // 可在此添加充电座引导、路径规划等逻辑，联动BLDC电机
    needChargingRecovery = false;
  }
  
  delay(100);
}

代码逻辑说明：
多资源匹配算法：从电量、距离、载重利用率、任务优先级四个核心维度构建适配得分模型，精准匹配机器人与任务，避免资源错配，确保机器人在载重、电量充足的前提下承接最适配的任务，提升资源利用率。
动态调度机制：每2秒触发一次资源匹配，实时接收机器人状态更新，确保任务分配与机器人实时状态同步，适配园区机器人位置、电量、负载的动态变化，实现动态调度闭环。
低电量应急处理：实时监测机器人电量，低于阈值时自动触发回收指令，联动BLDC电机切换为低速回收模式，避免机器人在作业过程中因电量不足瘫痪，保障机器人的安全运行与持续作业能力。

6、园区动态路径冲突规避与任务重分配程序（多机器人协同路径优化）
适用场景：园区多机器人并行配送时的核心协同环节，适用于园区内道路狭窄、交叉口密集、人流量大的场景，多机器人同时执行任务时易出现路径冲突、路口拥堵，需实时监测路径冲突，动态调整机器人行驶路径，同时在冲突导致任务延误时触发任务重分配，保障配送效率，适配园区复杂环境。
核心逻辑：通过无线通信获取多机器人的实时位置与规划路径，建立路径数据库；采用“路径冲突检测算法”对比机器人规划路径，识别交叉口、狭窄路段的冲突点；针对冲突点动态调整机器人行驶优先级与避让路径，引导机器人错峰通过；若冲突导致某机器人任务延误超时，自动触发任务重分配，重新匹配其他空闲机器人承接任务，同时联动BLDC电机调整速度与转向，实现路径冲突的动态规避与任务重分配。

// 园区动态路径冲突规避与任务重分配程序（Arduino核心调度）
#include <Arduino.h>
#include <vector>
#include <cmath>

// 核心配置参数（按园区道路实际布局调整）
#define CONFLICT_THRESHOLD_DISTANCE 5 // 路径冲突判定距离阈值（米）
#define MAX_CONFLICT_WAIT_TIME 30000  // 冲突等待超时时间（ms，30秒）
#define PATH_REPLAN_INTERVAL 5000     // 路径重规划周期（ms）

// 机器人路径规划结构体
struct RobotPath {
  String robotId;
  std::vector<std::pair<float, float>> pathNodes; // 路径节点列表（坐标对）
  int currentNodeIdx;        // 当前所在路径节点索引
  unsigned long nodeArriveTime; // 到达当前节点的时间
  String targetTaskId;       // 关联的任务ID
};

// 路径冲突判定结构体
struct PathConflict {
  String robotAIdx;
  String robotBIdx;
  std::pair<float, float> conflictPoint; // 冲突点坐标
  unsigned long conflictStartTime;      // 冲突发生时间
  bool isResolved;                      // 冲突是否已解决
};

// 全局变量
std::vector<RobotPath> robotPaths;      // 所有机器人的规划路径
std::vector<PathConflict> conflicts;     // 路径冲突列表
std::vector<String> reassignedTasks;    // 需重分配的任务ID

// BLDC电机控制函数（预留，联动转向与速度调整）
void setRobotDirection(String robotId, int direction); // 设置机器人转向：1左，2直行，3右
void adjustRobotSpeedByPath(String robotId, float distanceToNextNode);

// 计算两点距离（米，适配园区坐标与实际距离的换算）
float calculateActualDistance(float x1, float y1, float x2, float y2) {
  // 园区坐标与实际距离的换算，示例：1坐标单位=1米
  return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
}

// 路径冲突检测核心算法
void detectPathConflicts() {
  conflicts.clear();
  for (int i = 0; i < robotPaths.size(); i++) {
    for (int j = i + 1; j < robotPaths.size(); j++) {
      RobotPath *robotA = &robotPaths[i];
      RobotPath *robotB = &robotPaths[j];
      if (robotA->targetTaskId == robotB->targetTaskId) continue; // 同任务不检测
      
      // 遍历机器人A的后续路径节点与机器人B的后续路径节点
      for (int aNode = robotA->currentNodeIdx; aNode < robotA->pathNodes.size() - 1; aNode++) {
        for (int bNode = robotB->currentNodeIdx; bNode < robotB->pathNodes.size() - 1; bNode++) {
          // 获取当前路段的起点和终点
          std::pair<float, float> startA = robotA->pathNodes[aNode];
          std::pair<float, float> endA = robotA->pathNodes[aNode + 1];
          std::pair<float, float> startB = robotB->pathNodes[bNode];
          std::pair<float, float> endB = robotB->pathNodes[bNode + 1];
          
          // 检测两路段是否相交，且相交点距离在冲突阈值内
          std::pair<float, float> intersectionPoint = findPathIntersection(startA, endA, startB, endB);
          if (intersectionPoint.first != -1000 && // 存在交点
              calculateActualDistance(robotA->pathNodes[robotA->currentNodeIdx].first, 
                                      robotA->pathNodes[robotA->currentNodeIdx].second,
                                      intersectionPoint.first, intersectionPoint.second) < CONFLICT_THRESHOLD_DISTANCE * 2) {
            // 判定为路径冲突
            PathConflict conflict;
            conflict.robotAIdx = robotA->robotId;
            conflict.robotBIdx = robotB->robotId;
            conflict.conflictPoint = intersectionPoint;
            conflict.conflictStartTime = millis();
            conflict.isResolved = false;
            conflicts.push_back(conflict);
            Serial.print("检测到路径冲突：机器人");
            Serial.print(robotA->robotId);
            Serial.print("与机器人");
            Serial.print(robotB->robotId);
            Serial.println("在点(" + String(intersectionPoint.first) + "," + String(intersectionPoint.second) + ")冲突");
          }
        }
      }
    }
  }
}

// 计算两条线段的交点（核心几何算法，无交点返回(-1000,-1000)）
std::pair<float, float> findPathIntersection(std::pair<float, float> startA, std::pair<float, float> endA,
                                              std::pair<float, float> startB, std::pair<float, float> endB) {
  float x1 = startA.first, y1 = startA.second;
  float x2 = endA.first, y2 = endA.second;
  float x3 = startB.first, y3 = startB.second;
  float x4 = endB.first, y4 = endB.second;
  
  float denom = (y4 - y3) * (x2 - x1) - (x4 - x3) * (y2 - y1);
  if (denom == 0) return {-1000, -1000}; // 平行线，无交点
  
  float ua = ((x4 - x3) * (y1 - y3) - (y4 - y3) * (x1 - x3)) / denom;
  float ub = ((x2 - x1) * (y1 - y3) - (y2 - y1) * (x1 - x3)) / denom;
  
  // 交点在线段范围内才判定为有效交点
  if (ua >= 0 && ua <= 1 && ub >= 0 && ub <= 1) {
    float x = x1 + ua * (x2 - x1);
    float y = y1 + ua * (y2 - y1);
    return {x, y};
  } else {
    return {-1000, -1000};
  }
}

// 路径冲突规避核心策略
void resolvePathConflicts() {
  for (int i = 0; i < conflicts.size(); i++) {
    PathConflict *conflict = &conflicts[i];
    if (conflict->isResolved) continue;
    
    // 寻找冲突关联的两个机器人
    RobotPath *robotA = nullptr;
    RobotPath *robotB = nullptr;
    for (int j = 0; j < robotPaths.size(); j++) {
      if (robotPaths[j].robotId == conflict->robotAIdx) robotA = &robotPaths[j];
      if (robotPaths[j].robotId == conflict->robotBIdx) robotB = &robotPaths[j];
    }
    if (!robotA || !robotB) continue;
    
    // 冲突规避策略：按任务优先级+到达冲突点时间设定优先级，低优先级机器人避让
    int priorityA = 0, priorityB = 0;
    unsigned long arriveTimeA = conflict->conflictStartTime + (calculateActualDistance(
      robotA->pathNodes[robotA->currentNodeIdx].first, robotA->pathNodes[robotA->currentNodeIdx].second,
      conflict->conflictPoint.first, conflict->conflictPoint.second) / 0.5) * 1000; // 假设速度0.5m/s，计算到达时间
    unsigned long arriveTimeB = conflict->conflictStartTime + (calculateActualDistance(
      robotB->pathNodes[robotB->currentNodeIdx].first, robotB->pathNodes[robotB->currentNodeIdx].second,
      conflict->conflictPoint.first, conflict->conflictPoint.second) / 0.5) * 1000);
    
    // 任务优先级影响（需对接案例1的优先级，此处简化）
    // 此处假设任务优先级存储在全局任务数组，实际需关联任务ID查询
    // 简化逻辑：到达时间早的优先通行，若时间相近，高优先级任务优先
    if (arriveTimeA < arriveTimeB) priorityA = 1;
    else if (arriveTimeB < arriveTimeA) priorityB = 1;
    else {
      // 时间相近，高优先级任务优先
      // 实际需查询任务优先级，此处简化为假设robotA任务优先级更高则priorityA=1
      priorityA = 1;
    }
    
    // 低优先级机器人执行避让：调整行驶方向或停车等待
    if (priorityA == 1) {
      // robotB避让：调整转向或停车等待
      setRobotDirection(robotB->robotId, 0); // 0表示停车等待
      adjustRobotSpeedByPath(robotB->robotId, 0); // 速度降为0
      Serial.println("机器人" + robotB->robotId + "避让等待，机器人" + robotA->robotId + "优先通行");
      conflict->isResolved = true;
    } else {
      setRobotDirection(robotA->robotId, 0);
      adjustRobotSpeedByPath(robotA->robotId, 0);
      Serial.println("机器人" + robotA->robotId + "避让等待，机器人" + robotB->robotId + "优先通行");
      conflict->isResolved = true;
    }
    
    // 若等待超时，触发任务重分配
    if (millis() - conflict->conflictStartTime > MAX_CONFLICT_WAIT_TIME) {
      // 选择等待的机器人任务标记为重分配
      String waitRobotId = priorityA == 1 ? robotB->robotId : robotA->robotId;
      String taskId = waitRobotId == robotA->robotId ? robotA->targetTaskId : robotB->targetTaskId;
      reassignedTasks.push_back(taskId);
      Serial.println("冲突等待超时，任务" + taskId + "需重分配");
      conflict->isResolved = true;
    }
  }
}

// 任务重分配核心函数（复用案例2的资源匹配逻辑）
void reassignReplannedTasks() {
  if (reassignedTasks.empty()) return;
  
  // 此处需复用案例2的匹配逻辑，实际需关联全局待分配任务队列
  // 简化逻辑：将需重分配的任务加入待分配队列，触发案例2的匹配函数
  for (String taskId : reassignedTasks) {
    Serial.println("任务重分配：任务" + taskId + "进入待分配队列");
    // 实际需将taskId加入案例2的pendingTasks数组，设置isAssigned=false
  }
  reassignedTasks.clear();
}

// 预留的机器人转向控制函数
void setRobotDirection(String robotId, int direction) {
  // direction：0停车，1左转，2直行，3右转
  // 实际需通过串口向机器人下发转向指令，或控制BLDC电机的转向控制引脚
  Serial.print("机器人" + robotId + "转向指令：");
  if (direction == 0) Serial.println("停车等待");
  else if (direction == 1) Serial.println("左转避让");
  else if (direction == 2) Serial.println("直行通行");
  else if (direction == 3) Serial.println("右转避让");
}

// 预留的路径速度调整函数
void adjustRobotSpeedByPath(String robotId, float distanceToNextNode) {
  // 根据距离下一节点的距离调整速度，距离越近速度越低，避免碰撞
  int speedLevel = map(distanceToNextNode, 0, 20, 1, 5); // 距离0-20米，速度1-5级
  speedLevel = constrain(speedLevel, 1, 5);
  // 实际需调用案例2的adjustRobotSpeed函数
  Serial.print("机器人" + robotId + "根据路径距离调整速度至等级：" + String(speedLevel));
}

// 接收机器人路径更新（串口或无线）
void processRobotPathUpdate(String pathData) {
  // 路径数据格式："RobotID,NodeX1,NodeY1,NodeX2,NodeY2,...,CurrentNodeIdx,TaskId"
  // 简化逻辑：解析核心信息，更新机器人路径
  String robotId = pathData.substring(0, pathData.indexOf(','));
  pathData = pathData.substring(pathData.indexOf(',') + 1);
  
  // 寻找现有机器人路径，更新
  for (int i = 0; i < robotPaths.size(); i++) {
    if (robotPaths[i].robotId == robotId) {
      // 解析路径节点、当前节点索引、任务ID
      // 实际需完整解析，此处简化
      robotPaths[i].targetTaskId = "T001"; // 实际需解析
      robotPaths[i].currentNodeIdx = 0;    // 实际需解析
      Serial.println("更新机器人" + robotId + "的路径信息");
      return;
    }
  }
  // 新增机器人路径
  RobotPath newPath;
  newPath.robotId = robotId;
  newPath.targetTaskId = "T001";
  newPath.currentNodeIdx = 0;
  robotPaths.push_back(newPath);
  Serial.println("新增机器人" + robotId + "的路径信息");
}

void setup() {
  Serial.begin(115200);
  Serial.println("园区动态路径冲突规避与任务重分配系统启动");
}

void loop() {
  // 每5秒检测一次路径冲突
  if (millis() % PATH_REPLAN_INTERVAL == 0) {
    detectPathConflicts();
    resolvePathConflicts();
    reassignReplannedTasks();
    Serial.println("完成一轮路径冲突检测与规避处理");
  }
  
  // 接收机器人路径更新
  if (Serial.available() > 0) {
    String pathData = Serial.readStringUntil('\n');
    pathData.trim();
    if (!pathData.isEmpty()) {
      processRobotPathUpdate(pathData);
    }
  }
  
  delay(100);
}

代码逻辑说明：
路径冲突精准检测：采用线段相交算法，精准识别机器人规划路径的冲突点，通过距离阈值判定有效冲突，避免误判，适配园区交叉口、狭窄路段的冲突场景，为规避策略提供精准依据。
动态规避策略：结合任务优先级与机器人到达冲突点的时间，设定通行优先级，低优先级机器人采取停车等待、调整转向等规避措施，同时联动BLDC电机调整速度与方向，实现动态避让，减少路径冲突带来的延误。
任务重分配闭环：当路径冲突导致机器人等待超时，自动将延误任务标记为重分配，调用资源匹配逻辑重新分配给空闲机器人，形成“冲突检测-规避-超时重分配”的闭环，保障配送任务按时完成，适配园区复杂环境的任务保障需求。

要点解读

1. 多目标优先级动态排序：以核心需求驱动任务管控，保障紧急配送优先级
   多目标优先级排序是园区物资配送的基础核心，通过构建多维度的优先级评价体系，动态适配园区任务优先级的动态变化，确保紧急任务优先响应、常规任务平稳推进，从源头解决任务排序混乱导致的配送延误问题，适配写字楼急件、车间补料等紧急场景的核心需求。
   多维度评价体系：融合物资紧急等级、配送时限裕度、物资重量三个核心维度，采用加权评分算法替代单一优先级排序，避免单一维度偏差导致的紧急任务被延误，确保急件任务始终处于高优先级队列，适配园区任务的多样性需求。
   动态排序机制：采用秒级刷新的动态排序逻辑，结合实时时间自动调整任务优先级，例如临近截止时间的任务自动提升优先级，避免因时间推移导致任务优先级失效，保障任务队列始终适配实时配送需求，解决静态排序无法适配动态场景的痛点。
   电机联动响应：将任务优先级与BLDC电机速度直接绑定，急件任务自动匹配高速驱动模式，常规任务匹配中速模式，实现任务优先级与执行效率的闭环联动，确保紧急任务在驱动层面获得优先保障，提升紧急配送的响应速度。
2. 资源适配动态匹配：统筹多机器人核心资源，实现精准高效分配
   资源匹配是多机器人协同的核心环节，通过整合机器人的电量、载重、位置核心资源，构建多维度适配算法，实现任务与机器人的精准匹配，解决资源闲置、超载配送、电量不足等核心问题，最大化提升多机器人的资源利用率，适配园区多机器人并行作业的场景需求。
   多资源适配模型：从电量充足度、距离适配性、载重匹配度、任务优先级四个维度构建适配得分模型，确保机器人在电量满足、载重允许的前提下，承接距离最近、优先级适配的任务，避免资源错配导致的效率损耗，例如低电量机器人不承接远距离任务，满载机器人不承接额外任务。
   动态资源统筹：采用定时触发的匹配机制，实时接收机器人状态更新，同步更新资源池信息，确保任务分配与机器人的实时位置、电量、负载状态同步，避免静态分配导致的资源与任务脱节，保障资源分配始终适配机器人的动态状态。
   低电量应急闭环：建立低电量监测与回收触发机制，当机器人电量低于阈值时，自动停止承接新任务并触发回收，联动BLDC电机切换为低速回收模式，引导机器人返回充电，形成“资源监测-风险预警-应急处置”的闭环，保障机器人持续作业能力，避免作业途中因电量不足瘫痪。
3. 路径冲突动态规避：破解多机器人协同瓶颈，保障高效通行
   路径冲突是多机器人协同配送的核心痛点，通过实时检测路径冲突、动态制定规避策略、联动电机调整行驶状态，解决多机器人在交叉口、狭窄路段的拥堵问题，减少路径冲突导致的等待时间，提升多机器人通行效率，适配园区复杂道路环境与高密度作业场景。
   精准冲突检测：采用线段相交算法精准识别路径冲突点，结合距离阈值判定有效冲突，避免因微小路径偏差导致的误判，确保冲突检测的准确性，为后续规避策略提供精准依据，适配园区多样化道路布局。
   智能规避策略：结合任务优先级与机器人到达冲突点的时间，制定优先级通行规则，低优先级机器人采取停车等待或调整路径的规避措施，同时联动BLDC电机调整速度与转向，实现高效避让，既保障高优先级任务优先通行，又减少冲突带来的整体延误。
   超时重分配机制：针对冲突等待超时的场景，自动触发任务重分配，将延误任务重新匹配给空闲机器人，形成“冲突规避-超时处置-任务重分配”的闭环，避免单一冲突导致任务延误无法挽回，保障任务按时完成，适配园区复杂环境下的任务保障需求。
4. BLDC电机与调度联动：动力精准适配，支撑动态配送执行
   BLDC电机是机器人配送的执行核心，将电机驱动与调度逻辑深度联动，实现电机速度、转向与任务需求、路径状态的精准匹配，确保机器人在不同配送场景下的行驶稳定性、响应及时性，支撑动态配送的高效执行，适配园区多样化行驶场景。
   任务优先级与速度联动：根据任务优先级动态调整电机速度，急件任务匹配高速模式，常规任务匹配中速模式，保障不同优先级任务的执行效率，同时避免高速行驶带来的安全隐患，适配园区快慢分流的行驶需求。
   路径状态与转向联动：在路径冲突规避时，联动BLDC电机调整转向或停车等待，低优先级机器人通过停车或转向避让，高优先级机器人维持通行，实现路径状态与电机执行的实时协同，保障避让措施的及时落地，减少冲突风险。
   资源状态与动力联动：结合机器人电量、负载状态调整电机输出，低电量机器人降低速度以节省电量，重载机器人保持平稳速度避免载具晃动，确保电机动力与资源状态相匹配，既保障配送安全，又提升能源利用效率，适配园区节能与安全需求。
5. 场景化适配与扩展性：贴合园区实际需求，支撑系统迭代升级
   方案的落地核心在于适配园区多样化场景，同时具备灵活的扩展性，以满足园区业务扩展、硬件迭代、功能升级的需求，既适配当前园区的物资配送场景，又为未来智慧园区升级预留空间，保障系统的长期适用性与生命力。
   场景化参数适配：所有核心参数（优先级权重、距离阈值、电量阈值、路径规划周期）均采用可配置化设计，可根据园区不同场景（写字楼、生产车间、生活区）的道路布局、配送需求灵活调整，例如生产车间重载机器人可调整载重阈值，生活区可降低速度以保障行人安全，实现场景化精准适配。
   模块化扩展设计：代码采用模块化架构，任务排序、资源匹配、路径规避、电机控制四大模块相互解耦，可独立升级或替换，例如可替换更高效的优先级算法，可对接UWB高精度定位提升路径规划精度，可扩展多机器人类型适配特殊物资配送，支撑系统的功能迭代。
   多接口兼容适配：预留串口、无线通信接口，可对接园区各类硬件设备（GPS/UWB定位模块、无线通信模块、传感器），同时兼容主流BLDC电机控制库，适配不同型号的Arduino开发板与电机驱动，降低硬件选型限制，便于快速集成到不同机器人硬件平台，提升方案的适用性与落地效率。

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