基于Arduino与BLDC（无刷直流电机）的“机器人多目标优先级跟随与动态切换系统”，是解决机器人在复杂、多目标动态环境中实现智能资源分配与柔性控制的核心技术方案。该系统通过多源传感器融合与智能调度算法，赋予机器人在多目标场景下动态评估任务紧迫度、平滑切换跟随对象的能力，并依赖BLDC的高动态响应来执行复杂的运动指令。以下是该系统的详细专业解析：
一、 主要特点
多模态感知与多目标识别能力
系统通常采用超声波矩阵、视觉（如RGB-D相机）或UWB等多模态传感器阵列，实现对三维空间内多个目标的实时探测与识别。例如，通过部署多个超声波传感器形成十字形探测阵列，或结合视觉语义识别，系统能够同时锁定并跟踪多个目标对象，为多目标优先级管理提供丰富的数据输入。
动态优先级仲裁与智能调度
系统摒弃了传统的固定优先级或时间片轮转调度，引入了基于模糊逻辑或规则库的动态优先级仲裁机制。系统能够综合评估各目标的属性（如距离、目标接近度、身份标识等），将其转化为连续的“任务紧迫度”。例如，当系统检测到紧急指令或特定高优先级目标时，能瞬间将其优先级提升至最高，并动态分配运动资源。
基于BLDC的平滑过渡与动态差速控制
在目标切换过程中，传统算法的“硬切换”容易导致执行机构剧烈冲击。该系统利用模糊逻辑输出的连续权重值，使得多个BLDC关节或任务之间的协同动作更加平滑。结合BLDC的高动态响应特性，系统能够根据目标位置和方位偏差，实时进行动态差速控制（如目标偏右时左轮加速、右轮减速），实现平滑自然的轨迹规划与跟随。
预测性跟随与自适应环境感知
系统具备预测目标运动趋势的能力，能够提前调整跟随策略。同时，系统能够根据环境和目标特性自动调整跟随参数（如跟随距离、响应速度），并在跟随过程中结合自适应避障融合，确保在复杂动态环境中的通行效率与安全性。
二、 典型应用场景
多任务服务与医疗配送机器人
在医院或酒店等服务场景中，机器人需要同时处理“自主导航”、“语音交互响应”、“物品状态监测”等多个任务。当护士发出紧急语音指令或识别到特定VIP客户时，系统能瞬间将“语音交互”或“特定客户跟随”的优先级提升至最高，暂停其他任务或降低BLDC速度，确保及时响应人工干预。
工业巡检与预测性维护
在工厂中，巡检机器人搭载多种传感器。当系统检测到某台设备的异常信号（如振动幅值较大且温度偏高）时，会动态调整任务调度，指令机器人降低BLDC速度，在该设备前停留更长时间进行详细检测，而非按固定路线匆匆路过。
复杂环境下的自主探索与救援
在未知的废墟或洞穴环境中，机器人需要同时处理“地图构建”、“路径规划”、“传感器数据采集”和“能源管理”等多个任务。模糊动态调度器能根据环境复杂度（如狭窄通道增多）和传感器数据质量，动态分配CPU时间片和电机控制资源，确保在复杂区域降低移动速度以保证数据采集的完整性。
商业服务与展会物流
在餐厅、商场或展览会中，机器人作为智能导购、行李搬运或展品展示助手。系统能够根据预设规则选择最合适的跟随目标（如识别特定颜色的背包或帽子），在人群密集的环境中提供平稳、不跟丢的交互体验，并根据环境自动优化能耗。
三、 需要注意的关键事项
规则库的完备性与“维数灾难”
随着任务数量和输入变量（如距离、速度、电量、温度）的增加，模糊规则的数量呈指数级增长。这极易超出Arduino的Flash存储空间，导致推理时间变长。对策是采用分层模糊控制（先粗调再细调），或将部分规则固化为查表法（Look-Up Table），避免在线进行复杂的数学运算。
主控算力瓶颈与实时性保障
多目标识别、模糊推理及多路BLDC的闭环控制对算力要求极高。标准8位Arduino（如Uno）处理浮点运算较慢，若任务调度周期要求极高（< 10ms），建议采用ESP32、STM32或Teensy等32位高性能平台。控制回路必须使用硬件定时器中断或millis()非阻塞定时，严禁使用delay()函数，以确保视觉数据与电机控制的严格同步。
通信延迟与多MCU协同
在分布式架构中，若使用普通UART或低速I2C，通信延迟会导致调度决策滞后。必须选用实时性高的CAN总线或高速SPI，并严格规定数据帧的优先级。主控需周期性广播同步帧，各从控节点根据时间戳统一更新PWM输出，防止因通信延迟导致的多电机动作不同步（“拉扯”现象）。
安全冗余与故障保护机制
模糊调度属于软件层逻辑，存在失效风险。对于“急停”、“碰撞检测”等最高安全等级的任务，建议采用硬件中断直接切断电机电源，绕过模糊软件调度层，确保绝对的安全响应速度。此外，必须为MCU启用硬件看门狗定时器，防止程序跑飞导致机器人停在原地或乱撞。

1、固定优先级跟随 + 目标丢失切换
适用场景：工厂中AGV按预设优先级跟随特定人员（如主管>组长>工人）。每个目标有固定优先级，高优先级目标出现时自动切换，目标丢失时降级跟随次优目标。

/* ===== Arduino BLDC 多目标优先级跟随 — 固定优先级 + 丢失切换 =====
 * 硬件：2×BLDC差速底盘 + UWB/视觉多目标定位 + 编码器
 * 核心：固定优先级列表 → 最高优先级目标可见则跟随 → 丢失后切换
 */
#include <SimpleFOC.h>
#include <vector>
#include <algorithm>

BLDCMotor motorL(5), motorR(6);
BLDCDriver3PWM drvL, drvR;
Encoder encL(2,3,2048), encR(4,5,2048);

// 目标结构体
struct FollowTarget {
  uint8_t id;             // 目标ID
  uint8_t priority;       // 优先级(0最高)
  float x, y;             // 位置
  float vx, vy;           // 速度
  bool visible;           // 是否可见
  unsigned long lastSeen; // 最后可见时间
  float confidence;       // 置信度(0~1)
};

// 目标列表(按优先级排序)
FollowTarget targets[] = {
  {0, 0, 0, 0, 0, 0, false, 0, 0},  // 主管
  {1, 1, 0, 0, 0, 0, false, 0, 0},  // 组长
  {2, 2, 0, 0, 0, 0, false, 0, 0},  // 工人A
  {3, 3, 0, 0, 0, 0, false, 0, 0}   // 工人B
};
const int TARGET_COUNT = 4;

// 当前跟随目标索引
int currentTargetIdx = -1;
int lastTargetIdx = -1;

// 切换锁定(防止频繁切换)
unsigned long switchLockTime = 0;
const int SWITCH_LOCK_MS = 2000;  // 切换后锁定2秒

// 目标丢失超时
const int LOST_TIMEOUT = 3000;  // 3秒无更新视为丢失

// 机器人位置
float robotX = 0, robotY = 0, robotYaw = 0;

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

void loop() {
  float dt = 0.02;
  
  // 1. 更新所有目标状态(来自UWB/视觉)
  updateAllTargets();
  
  // 2. 选择最佳跟随目标
  selectBestTarget();
  
  // 3. 跟随控制
  if(currentTargetIdx >= 0) {
    followTarget(currentTargetIdx);
  } else {
    // 无目标 → 原地等待
    motorL.move(0);
    motorR.move(0);
    Serial.println("No target available, waiting...");
  }
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

void updateAllTargets() {
  for(int i=0; i<TARGET_COUNT; i++) {
    // 从定位系统读取目标位置(伪代码)
    float tx, ty;
    bool detected = getTargetPosition(targets[i].id, &tx, &ty);
    
    if(detected) {
      // 计算速度
      if(targets[i].visible) {
        float dt_real = 0.1;  // 实际时间差
        targets[i].vx = (tx - targets[i].x) / dt_real;
        targets[i].vy = (ty - targets[i].y) / dt_real;
      }
      
      targets[i].x = tx;
      targets[i].y = ty;
      targets[i].visible = true;
      targets[i].lastSeen = millis();
      targets[i].confidence = min(1.0, targets[i].confidence + 0.1);
    } else {
      // 目标丢失：置信度衰减
      targets[i].confidence *= 0.95;
      
      // 超时判定
      if(millis() - targets[i].lastSeen > LOST_TIMEOUT) {
        targets[i].visible = false;
        targets[i].confidence = 0;
      }
    }
  }
}

void selectBestTarget() {
  // 切换锁定检查
  if(millis() - switchLockTime < SWITCH_LOCK_MS) return;
  
  int bestIdx = -1;
  int bestPriority = 999;
  
  for(int i=0; i<TARGET_COUNT; i++) {
    if(!targets[i].visible) continue;
    if(targets[i].confidence < 0.3) continue;  // 置信度过低
    
    // 选择优先级最高的可见目标
    if(targets[i].priority < bestPriority) {
      bestPriority = targets[i].priority;
      bestIdx = i;
    }
  }
  
  // 如果找到了更好的目标且不同于当前
  if(bestIdx >= 0 && bestIdx != currentTargetIdx) {
    lastTargetIdx = currentTargetIdx;
    currentTargetIdx = bestIdx;
    switchLockTime = millis();
    
    Serial.print("Switched to target ID:");
    Serial.print(targets[currentTargetIdx].id);
    Serial.print(" Priority:");
    Serial.println(targets[currentTargetIdx].priority);
  }
  
  // 如果当前目标丢失，尝试切换到其他目标
  if(currentTargetIdx >= 0 && !targets[currentTargetIdx].visible) {
    // 找下一个优先级最高的
    for(int i=0; i<TARGET_COUNT; i++) {
      if(targets[i].visible && targets[i].confidence > 0.3) {
        currentTargetIdx = i;
        switchLockTime = millis();
        Serial.print("Lost target, switched to ID:");
        Serial.println(targets[currentTargetIdx].id);
        break;
      }
    }
    // 如果都没有，设为-1
    if(currentTargetIdx >= 0 && !targets[currentTargetIdx].visible) {
      currentTargetIdx = -1;
    }
  }
}

void followTarget(int idx) {
  float dx = targets[idx].x - robotX;
  float dy = targets[idx].y - robotY;
  float dist = sqrt(dx*dx + dy*dy);
  float angle = atan2(dy, dx);
  
  // 带速度前馈的跟随
  float vLin = constrain((dist - 1.0) * 50 + targets[idx].vx * 0.3, 0, 250);
  float vAng = constrain((angle - robotYaw) * 2.0, -1.5, 1.5);
  
  motorL.move(vLin - vAng * 80);
  motorR.move(vLin + vAng * 80);
}

关键设计点：
固定优先级列表决定跟随顺序
目标丢失后置信度衰减，超时后标记不可见
切换锁定防止频繁切换
速度前馈提高跟随响应

2、动态优先级评分 + 平滑切换
适用场景：商场导购机器人、医院配送机器人。优先级不仅取决于预设等级，还综合考虑距离、停留时间、任务紧急度等动态因素。

/* ===== Arduino BLDC 多目标跟随 — 动态优先级评分 + 平滑切换 =====
 * 硬件：2×BLDC差速底盘 + UWB/视觉 + 麦克风阵列(语音指令)
 * 核心：多维度评分(预设等级+距离+等待时间+任务紧急度) → 动态排序
 *       切换时平滑过渡，避免急停急转
 */
#include <SimpleFOC.h>
#include <Math.h>

BLDCMotor motorL(5), motorR(6);

// 目标结构体(扩展)
struct SmartTarget {
  uint8_t id;
  uint8_t basePriority;    // 基础优先级(0~10)
  float x, y;
  float distance;          // 距离机器人
  float waitTime;          // 等待时间(秒)
  float urgency;           // 任务紧急度(0~1)
  float score;             // 综合评分
  bool active;
  unsigned long firstSeen; // 首次出现时间
};
SmartTarget targets[10];
int targetCount = 0;

// 评分权重
const float W_BASE = 0.4;      // 基础优先级权重
const float W_DIST = 0.2;      // 距离权重(近的加分)
const float W_WAIT = 0.2;      // 等待时间权重(久的加分)
const float W_URGENCY = 0.2;   // 紧急度权重

// 当前跟随目标
int currentIdx = -1;
int previousIdx = -1;

// 平滑切换参数
float smoothVLin = 0, smoothVAng = 0;
const float SMOOTH_ALPHA = 0.85;  // 平滑系数

void computeScores() {
  for(int i=0; i<targetCount; i++) {
    if(!targets[i].active) continue;
    
    // 距离评分(越近越高)
    float distScore = 1.0 - targets[i].distance / 10.0;
    distScore = constrain(distScore, 0, 1);
    
    // 等待时间评分
    float waitScore = constrain(targets[i].waitTime / 60.0, 0, 1);
    
    // 综合评分
    targets[i].score = 
      W_BASE * (1.0 - targets[i].basePriority / 10.0) +
      W_DIST * distScore +
      W_WAIT * waitScore +
      W_URGENCY * targets[i].urgency;
  }
}

int selectBestSmartTarget() {
  int bestIdx = -1;
  float bestScore = -999;
  
  for(int i=0; i<targetCount; i++) {
    if(!targets[i].active) continue;
    
    // 当前跟随目标有连续性加成
    float continuityBonus = (i == currentIdx) ? 0.3 : 0;
    float finalScore = targets[i].score + continuityBonus;
    
    if(finalScore > bestScore) {
      bestScore = finalScore;
      bestIdx = i;
    }
  }
  return bestIdx;
}

void smoothSwitch(int newIdx) {
  if(newIdx == currentIdx) return;
  
  previousIdx = currentIdx;
  currentIdx = newIdx;
  
  Serial.print("Smooth switching to target ID:");
  Serial.println(targets[currentIdx].id);
}

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

void loop() {
  float dt = 0.02;
  
  // 1. 更新目标列表
  updateSmartTargets();
  
  // 2. 计算综合评分
  computeScores();
  
  // 3. 选择最佳目标
  int bestIdx = selectBestSmartTarget();
  
  // 4. 平滑切换(评分差距超过阈值才切换)
  if(bestIdx >= 0) {
    if(currentIdx < 0) {
      smoothSwitch(bestIdx);
    } else {
      float scoreDiff = targets[bestIdx].score - targets[currentIdx].score;
      if(scoreDiff > 0.15) {  // 评分差距超过0.15才切换
        smoothSwitch(bestIdx);
      }
    }
  }
  
  // 5. 平滑跟随控制
  if(currentIdx >= 0) {
    float dx = targets[currentIdx].x - robotX;
    float dy = targets[currentIdx].y - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    float angle = atan2(dy, dx);
    
    float targetVLin = constrain((dist - 1.0) * 45, 0, 230);
    float targetVAng = constrain((angle - robotYaw) * 1.8, -1.5, 1.5);
    
    // 平滑滤波
    smoothVLin = SMOOTH_ALPHA * smoothVLin + (1-SMOOTH_ALPHA) * targetVLin;
    smoothVAng = SMOOTH_ALPHA * smoothVAng + (1-SMOOTH_ALPHA) * targetVAng;
    
    motorL.move(smoothVLin - smoothVAng * 80);
    motorR.move(smoothVLin + smoothVAng * 80);
  } else {
    smoothVLin *= 0.9;
    smoothVAng *= 0.9;
    motorL.move(smoothVLin);
    motorR.move(smoothVLin);
  }
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
多维评分体系：基础优先级+距离+等待时间+紧急度
连续性加成防止频繁切换
评分差距阈值控制切换灵敏度
平滑滤波使速度变化柔和

3、群组跟随 + 领导者切换
适用场景：编队巡逻、群体表演。机器人跟随一个群组，群组内成员可动态成为领导者，机器人跟随当前领导者。

/* ===== Arduino BLDC 群组跟随 — 领导者动态切换 =====
 * 硬件：2×BLDC差速底盘 + UWB群组定位 + ESP-NOW通信
 * 核心：群组成员通过通信协商领导者 → 机器人跟随领导者
 *       领导者切换时编队重组
 */
#include <SimpleFOC.h>
#include <WiFi.h>
#include <esp_now.h>

BLDCMotor motorL(5), motorR(6);

// 群组成员
struct GroupMember {
  uint8_t id;
  float x, y, yaw;
  bool isLeader;
  uint8_t leaderId;       // 当前认可的领导者
  unsigned long lastSeen;
  uint8_t votes;           // 得票数(选举用)
};
GroupMember group[5];
int memberCount = 0;

// 自身信息
uint8_t myId = 0;
float myX = 0, myY = 0, myYaw = 0;

// 当前领导者
uint8_t leaderId = 0xFF;
unsigned long leaderChangeTime = 0;

// 编队偏移(相对于领导者)
float offsetX = -1.0;  // 左侧1米
float offsetY = -0.5;  // 后方0.5米

// ESP-NOW通信
void onDataRecv(const esp_now_recv_info_t*, const uint8_t* data, int len) {
  if(len != sizeof(GroupMember)) return;
  
  GroupMember received;
  memcpy(&received, data, sizeof(GroupMember));
  
  // 更新群组成员
  bool found = false;
  for(int i=0; i<memberCount; i++) {
    if(group[i].id == received.id) {
      group[i] = received;
      group[i].lastSeen = millis();
      found = true;
      break;
    }
  }
  if(!found && memberCount < 5) {
    group[memberCount++] = received;
    group[memberCount-1].lastSeen = millis();
  }
}

void broadcastSelf() {
  GroupMember me = {myId, myX, myY, myYaw, false, leaderId, millis(), 0};
  esp_now_send(broadcastAddr, (uint8_t*)&me, sizeof(me));
}

// 领导者选举(基于距离和信号强度)
void electLeader() {
  // 清除旧票数
  for(int i=0; i<memberCount; i++) group[i].votes = 0;
  
  // 每个成员投票给距离最近的成员(但不能投给自己)
  for(int i=0; i<memberCount; i++) {
    float nearestDist = 999;
    uint8_t voteFor = group[i].id;
    
    for(int j=0; j<memberCount; j++) {
      if(i == j) continue;
      float dx = group[i].x - group[j].x;
      float dy = group[i].y - group[j].y;
      float dist = sqrt(dx*dx + dy*dy);
      if(dist < nearestDist) {
        nearestDist = dist;
        voteFor = group[j].id;
      }
    }
    group[i].votes = 1;  // 每人一票
  }
  
  // 统计得票
  uint8_t voteCount[5] = {0};
  for(int i=0; i<memberCount; i++) {
    voteCount[group[i].leaderId]++;
  }
  
  // 得票最多的成为领导者
  uint8_t newLeader = group[0].id;
  uint8_t maxVotes = 0;
  for(int i=0; i<memberCount; i++) {
    if(voteCount[group[i].id] > maxVotes) {
      maxVotes = voteCount[group[i].id];
      newLeader = group[i].id;
    }
  }
  
  if(newLeader != leaderId) {
    leaderId = newLeader;
    leaderChangeTime = millis();
    Serial.print("New leader elected: ID ");
    Serial.println(leaderId);
  }
}

void setup() {
  Serial.begin(115200);
  WiFi.mode(WIFI_STA);
  esp_now_init();
  esp_now_register_recv_cb(onDataRecv);
  
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  float dt = 0.02;
  
  // 1. 广播自身信息
  broadcastSelf();
  
  // 2. 更新群组(清理超时成员)
  for(int i=0; i<memberCount; i++) {
    if(millis() - group[i].lastSeen > 5000) {
      group[i] = group[--memberCount];
    }
  }
  
  // 3. 领导者选举(每5秒一次)
  static unsigned long lastElection = 0;
  if(millis() - lastElection > 5000) {
    electLeader();
    lastElection = millis();
  }
  
  // 4. 找到领导者位置
  float leaderX = 0, leaderY = 0, leaderYaw = 0;
  bool leaderFound = false;
  for(int i=0; i<memberCount; i++) {
    if(group[i].id == leaderId) {
      leaderX = group[i].x;
      leaderY = group[i].y;
      leaderYaw = group[i].yaw;
      leaderFound = true;
      break;
    }
  }
  
  // 5. 编队跟随
  if(leaderFound) {
    // 目标位置 = 领导者位置 + 编队偏移(考虑领导者朝向)
    float targetX = leaderX + offsetX * cos(leaderYaw) - offsetY * sin(leaderYaw);
    float targetY = leaderY + offsetX * sin(leaderYaw) + offsetY * cos(leaderYaw);
    
    float dx = targetX - myX;
    float dy = targetY - myY;
    float dist = sqrt(dx*dx + dy*dy);
    float angle = atan2(dy, dx);
    
    float vLin = constrain(dist * 40, 0, 200);
    float vAng = constrain((angle - myYaw) * 2.0, -1.5, 1.5);
    
    motorL.move(vLin - vAng * 80);
    motorR.move(vLin + vAng * 80);
  } else {
    motorL.move(0);
    motorR.move(0);
  }
  
  // 6. 更新自身位置
  float speed = (motorL.shaft_velocity + motorR.shaft_velocity) / 2.0;
  myX += speed * cos(myYaw) * dt;
  myY += speed * sin(myYaw) * dt;
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(20);
}

关键设计点：
群组成员通过无线通信交换信息
领导者通过投票选举产生
编队跟随考虑领导者朝向
超时成员自动清理

要点解读
① 多目标优先级跟随的核心是“评分函数”
评分函数决定了机器人跟随谁。一个好的评分函数应考虑：
预设优先级（案例一的固定优先级）
动态因素（案例二的距离、等待时间、紧急度）
连续性（防止频繁切换）
群组共识（案例三的投票选举）
② 切换策略必须包含“滞回区”和“锁定时间”
没有滞回区的切换会导致：
在两个目标之间来回切换（振荡）
机器人忽左忽右（路径抖动）
案例一的SWITCH_LOCK_MS和案例二的scoreDiff > 0.15都是滞回区的体现。
③ 目标丢失处理是系统鲁棒性的关键
案例一使用了置信度衰减+超时判定双重机制。
④ 群组跟随需要通信协议支持
案例三中ESP-NOW通信的关键点：
广播频率：20~50Hz
数据内容：ID、位置、朝向、领导者ID
冲突处理：CSMA/CA
超时清理：5秒无更新移除
⑤ 实际部署建议：分层决策
顶层：任务分配(哪个目标需要跟随)
中层：目标选择(评分+切换逻辑)
底层：运动控制(PID/导纳)
案例一、二、三分别侧重不同层次，实际系统需要三者结合。

4、UWB多标签优先级跟随（ID绑定 + 卡尔曼滤波）
适用场景：仓储物流场景中，机器人从多个佩戴UWB标签的工人中锁定特定目标（如通过ID绑定），并在目标短暂消失时维持预测跟踪。

#include <DW1000.h>
#include <PID_v1.h>

#define TARGET_TAG_ID  0x1234      // 绑定的目标标签ID
#define SAFE_DISTANCE  120          // 安全跟随距离(cm)
#define SEARCH_TIMEOUT 3000         // 目标丢失搜索超时(ms)

DW1000  uwb;
PID     distPID(&currentDist, &motorOutput, SAFE_DISTANCE, 0.4, 0.05, 0.02, DIRECT);
PID     anglePID(&angleError, &turnOutput, 0, 2.0, 0.8, 0.1, DIRECT);

unsigned long lastSeen = 0;
float targetX = 0, targetY = 0, targetAngle = 0;

void setup() {
    Serial.begin(115200);
    uwb.begin();
    uwb.setAddress(0x5678);
    distPID.SetMode(AUTOMATIC);
    distPID.SetOutputLimits(-255, 255);
    anglePID.SetMode(AUTOMATIC);
    anglePID.SetOutputLimits(-100, 100);
    pinMode(MOTOR_L, OUTPUT);
    pinMode(MOTOR_R, OUTPUT);
}

void loop() {
    // 1. 【核心】扫描所有标签，按ID筛选目标
    float allTags[10][3];  // [id][x,y,rssi]
    int tagCount = uwb.scanTags(allTags);

    int targetIdx = -1;
    for (int i = 0; i < tagCount; i++) {
        if ((int)allTags[i][0] == TARGET_TAG_ID) {
            targetIdx = i;
            break;
        }
    }

    // 2. 目标存在 → 卡尔曼滤波平滑 + 跟随
    if (targetIdx >= 0) {
        lastSeen = millis();
        float rawX = allTags[targetIdx][1];
        float rawY = allTags[targetIdx][2];

        // 简易卡尔曼滤波（一维）
        static float kalmanX = 0, kalmanY = 0;
        float K = 0.15;
        kalmanX = kalmanX + K * (rawX - kalmanX);
        kalmanY = kalmanY + K * (rawY - kalmanY);

        targetX = kalmanX; targetY = kalmanY;
        targetAngle = atan2(targetY, targetX) * 180 / PI;

        float dist = sqrt(targetX*targetX + targetY*targetY);
        currentDist = dist;
        distPID.Compute();

        angleError = targetAngle;
        anglePID.Compute();

        // 差速输出
        int leftSpeed  = constrain(motorOutput - turnOutput, -255, 255);
        int rightSpeed = constrain(motorOutput + turnOutput, -255, 255);
        analogWrite(MOTOR_L, abs(leftSpeed));
        analogWrite(MOTOR_R, abs(rightSpeed));
        setMotorDir(MOTOR_L, leftSpeed >= 0);
        setMotorDir(MOTOR_R, rightSpeed >= 0);
    }
    // 3. 目标丢失 → 原地旋转搜索
    else if (millis() - lastSeen > SEARCH_TIMEOUT) {
        analogWrite(MOTOR_L, 80);
        analogWrite(MOTOR_R, 0);  // 原地右转搜索
    }
    else {
        analogWrite(MOTOR_L, 0);
        analogWrite(MOTOR_R, 0);  // 等待
    }
    delay(50);  // 20Hz更新率
}

代码要点：本案例通过UWB标签ID筛选实现目标锁定，利用卡尔曼滤波平滑定位数据，并在目标丢失时触发“原地搜索”行为。系统在面对多标签干扰时能持续锁定特定ID，体现了“多目标识别→优先级锁定”的核心逻辑。

5、视觉颜色标签 + IMU融合的多目标选择性跟随
适用场景：服务机器人从人群中识别特定颜色标签的用户，并在目标被遮挡时通过IMU航迹推算维持跟随连续性。

#include <OpenMV.h>      // 视觉传感器通信
#include <MPU6050.h>     // IMU姿态
#include <PID_v1.h>

OpenMV camera;
MPU6050 imu;

// PID控制器：X方向（水平偏移）和Y方向（距离）
PID xPID(&xError, &xOutput, &xSetpoint, 2.0, 0.5, 0.1, DIRECT);
PID yPID(&yError, &yOutput, &ySetpoint, 1.5, 0.3, 0.05, DIRECT);

float targetX_offset = 0;   // 目标水平偏移 (-1.0 ~ 1.0)
float targetDistance = 0;   // 目标距离（由视觉估算）
unsigned long lastSeen = 0;
const unsigned long LOST_TIMEOUT = 2000;

void setup() {
    Serial.begin(115200);
    camera.begin();
    imu.begin();
    xPID.SetMode(AUTOMATIC);
    yPID.SetMode(AUTOMATIC);
    // BLDC电机初始化...
}

void loop() {
    // 1. 【核心】从视觉数据中提取多目标，按颜色/特征筛选
    camera.update();
    int targetCount = camera.getTargetCount();
    
    int selectedIdx = -1;
    for (int i = 0; i < targetCount; i++) {
        String tag = camera.getTag(i);
        // 只跟随带有特定颜色标签的目标（如红色）
        if (tag == "RED_TAG") {
            selectedIdx = i;
            break;
        }
    }
    
    // 2. 目标存在 → 更新跟随状态
    if (selectedIdx >= 0) {
        lastSeen = millis();
        targetX_offset = camera.getXOffset(selectedIdx);  // 像素偏移
        targetDistance = camera.getDistance(selectedIdx); // 估算距离
        
        xSetpoint = 0;  // 期望水平居中
        ySetpoint = 80; // 期望跟随距离(cm)
        
        xError = targetX_offset;
        yError = targetDistance - ySetpoint;
        
        xPID.Compute();
        yPID.Compute();
        
        // 差速驱动BLDC
        int leftSpeed = constrain(yOutput - xOutput, -255, 255);
        int rightSpeed = constrain(yOutput + xOutput, -255, 255);
        motorL.move(leftSpeed);
        motorR.move(rightSpeed);
    }
    // 3. 目标丢失 → IMU航迹推算 + 原地等待
    else if (millis() - lastSeen < LOST_TIMEOUT) {
        // 短时丢失：保持最后速度方向，等待目标重现
        // 利用IMU预测目标运动趋势
        float yawRate = imu.getGyroZ();
        // 减速并微调方向
        motorL.move(50 - yawRate * 10);
        motorR.move(50 + yawRate * 10);
    } else {
        // 长时丢失：停止并发出提示
        motorL.move(0);
        motorR.move(0);
        // 触发搜索模式（如原地旋转）
    }
    
    motorL.loopFOC();
    motorR.loopFOC();
    delay(30);
}

代码要点：本案例展示了“视觉为主、IMU为辅”的多目标跟随策略。视觉传感器负责目标识别与定位，IMU在目标短暂遮挡时提供航迹推算，确保跟随连续性。“动态优先级调度”体现为：目标存在时优先视觉锁定，丢失时由IMU接管。

6、动态权重分配的混合控制器（多目标权重竞争）
适用场景：复杂环境中，机器人需要在“轨迹跟踪精度”、“能量效率”、“运动平稳性”等多个控制目标间动态权衡，如AGV在空载/重载、开阔/狭窄通道等场景自适应切换。

#include <SimpleFOC.h>
#include <MPU6050.h>

BLDCMotor motor(11);
Encoder encoder(2,3,2048);
BLDCDriver3PWM driver(9,10,11,8);

MPU6050 imu;

// 多目标权重
float W_track = 0.6;   // 轨迹跟踪权重
float W_energy = 0.2;  // 节能权重
float W_smooth = 0.2;  // 平稳性权重

float target_position = 0;
float stiffness = 10.0; // 刚度系数
float damping = 0.5;   // 阻尼系数

void setup() {
    motor.linkSensor(&encoder);
    motor.linkDriver(&driver);
    motor.controller = MotionControlType::torque; // 力矩模式
    motor.init();
    motor.initFOC();
    imu.begin();
}

void loop() {
    motor.loopFOC();
    
    // 1. 【核心】感知当前状态，动态计算权重
    float accel = imu.getAccelMagnitude();
    float current_consumption = motor.current_q;  // 电流消耗
    
    // 判断工况：急转弯/避障 → 提高跟踪精度权重
    if (imu.getGyroZ() > 2.0) {
        W_track = 0.8;
        W_energy = 0.1;
        W_smooth = 0.1;
    }
    // 高速巡航 → 提高节能和平稳性权重
    else if (motor.shaft_velocity > 50) {
        W_track = 0.3;
        W_energy = 0.4;
        W_smooth = 0.3;
    }
    // 重载/爬坡 → 提高扭矩权重
    else if (current_consumption > 1.0) {
        W_track = 0.5;
        W_energy = 0.1;
        W_smooth = 0.4;
    }
    // 默认工况：均衡分配
    else {
        W_track = 0.5;
        W_energy = 0.25;
        W_smooth = 0.25;
    }
    
    // 2. 多目标加权计算
    float torque_track = stiffness * (target_position - motor.shaft_angle);
    float torque_energy = -current_consumption * 0.1;  // 节能项
    float torque_smooth = damping * (-motor.shaft_velocity); // 阻尼项
    
    // 加权合并
    float torque_cmd = W_track * torque_track 
                     + W_energy * torque_energy 
                     + W_smooth * torque_smooth;
    
    // 安全限幅
    torque_cmd = constrain(torque_cmd, -3.0, 3.0);
    motor.move(torque_cmd);
    
    delay(10);
}

代码要点：本案例展示“动态权重分配的混合控制器”。系统通过IMU（加速度/角速度）和电机电流实时感知工况，动态调整“跟踪精度”、“节能”、“平稳性”三项目标的权重，在急转弯时优先保证轨迹跟踪，在巡航时优先节能和平稳。这种“情境感知+自适应决策”的架构，是多目标优先级系统的典型实现。

要点解读
“多目标识别”是优先级切换的前提：系统需通过视觉或UWB等方式对场景中所有候选目标进行检测和编号，再通过预设规则（如特定颜色标签、ID绑定或距离最近原则）锁定唯一目标作为跟随对象。上层“目标筛选”与底层“BLDC驱动”分离解耦。

“动态权重分配”是实现多目标权衡的数学工具：在复杂环境中，机器人需在“跟踪精度”、“能量效率”、“运动平稳性”甚至“安全性”等多个控制目标间动态平衡。权重因子的设置直接影响系统行为——急转弯时拉高跟踪权重，巡航时拉高节能权重。

目标丢失时的“预测跟随”与“搜索策略”是系统的最后防线：视觉或UWB目标可能被短暂遮挡。系统需设计短时丢失的航迹推算（IMU/里程计预测）和长时丢失的主动搜索（原地旋转或扇形扫描）两档应对策略，防止机器人失控“飞车”。

BLDC FOC是实现“自然跟随”的执行保障：多目标识别与动态权重切换输出的速度/力矩指令，需要通过BLDC电机配合FOC（磁场定向控制）实现高动态响应——低速平滑、扭矩精准、加减速丝滑，避免机械顿挫。

分层式软硬件协同架构是算力保障的工程方案：多目标识别对算力要求极高，典型方案是“上位机（树莓派/Jetson）负责视觉推理与目标选择 + 下位机（Arduino/ESP32）负责BLDC底层驱动与安全监控”，两者通过串口/CAN总线高频交互。底层电机控制周期需≥50Hz，且必须在中断中执行，防止被上层通信阻塞。

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