“Arduino BLDC之多目标选择跟随——超市导购机器人”代表了现代服务机器人在复杂动态环境中，将多源异构感知、智能决策算法与底层高性能驱动深度融合的前沿应用。该系统旨在解决超市等人流密集、环境动态变化且目标特征高度相似的场景下，机器人如何精准锁定特定顾客并提供平滑伴随服务的问题。以下从专业视角详细解析其主要特点、应用场景及需要注意的关键事项：
一、 主要特点

1. 多目标感知与智能决策（选择跟随）
   在超市环境中，机器人视野内往往存在多个相似特征（如穿着相似服装的顾客）。系统通常采用“全局感知+局部锁定”的策略：
   多模态融合识别： 结合AI视觉（如人体骨骼关键点、Re-ID行人重识别技术）与UWB（超宽带）/蓝牙信标，在多个潜在目标中进行特征比对与置信度评估。
   目标选择逻辑： 通过多目标跟踪算法（如SORT或DeepSORT），结合顾客距离、停留时间、交互指令（如语音唤醒）等维度，赋予各目标权重，从而在算法层面“选择”并锁定唯一的服务对象，过滤无关干扰。
2. 基于BLDC的高动态与平滑执行
   导购机器人对乘坐体验（或伴随体验）要求极高，传统的直流有刷电机难以胜任。BLDC（无刷直流）电机配合FOC（磁场定向控制）驱动器，具备以下优势：
   低速大扭矩与高平顺性： 能够完美执行上层规划器输出的连续差速指令，在频繁启停、转弯时保持极低的转矩脉动，避免机械顿挫感。
   高动态响应： 当目标突然加速或改变方向时，BLDC能在毫秒级响应速度指令的突变，确保机器人能够“跟得上、转得稳”。
3. 分层式运动学解算与防碰撞控制
   差速运动学解算： 上层算法根据目标在画面中的水平偏移量（ X_{offset}X offset ）和相对距离（ DistanceDistance ），通过PID或模糊控制计算出期望的线速度（ vv ）和角速度（ ww ），再通过差速运动学模型分配给左右BLDC电机。
   主动安全避障： 跟随逻辑中嵌套了局部避障算法（如DWA或VFH）。当跟随路径上出现货架、购物车或儿童时，机器人会优先执行绕行或减速，遵循“安全优先于跟随”的原则。
   二、 典型应用场景
4. 大型商超与仓储式卖场的VIP导购
   在宜家、山姆会员店等大型商超中，机器人作为“智能导购车”或“随行购物篮”，自动跟随被选中的顾客。它能根据顾客的步伐节奏保持1-2米的安全跟随距离，并在顾客驻足观看商品时自动悬停，提供路线指引或商品信息查询服务。
5. 医院与交通枢纽的伴随服务
   在环境复杂的医院、机场或高铁站，机器人可跟随行动不便的患者或携带大量行李的旅客。通过多目标选择算法，机器人能在密集的人流中准确锁定佩戴特定手环或发出语音指令的特定服务对象，避免跟错人。
6. 机器人控制算法的科研与验证
   作为验证“多目标跟踪（MOT）+ 运动控制”耦合特性的绝佳平台。该场景被广泛用于研究复杂动态环境下的目标丢失重捕获策略、行人重识别（Re-ID）算法，以及BLDC底盘在非线性负载下的自适应控制。
   三、 需要注意的关键事项
7. 算力瓶颈与异构架构设计
   多目标选择涉及复杂的视觉AI推理（如YOLO、Re-ID）和多传感器数据融合，标准的Arduino（如Uno/Nano）完全无法胜任。
   对策： 必须采用“上位机+下位机”架构。上位机（如树莓派、Jetson Nano或RK3588核心板）负责视觉识别、目标选择与全局路径规划；Arduino（或ESP32/STM32）作为下位机，仅负责接收上位机的速度指令，并执行BLDC的FOC底层控制与传感器数据采集。
8. 目标丢失与异常状态处理机制
   在超市环境中，目标极易被货架遮挡或进入盲区。系统必须具备完善的容错机制：
   短时丢失（<2秒）： 依靠IMU和轮式里程计进行惯性航迹推测（Dead Reckoning），维持原速度和方向短暂跟随。
   长时丢失： 触发“重捕获逻辑”，如原地缓慢旋转扫描、沿最后已知方向短距寻找；若超时仍未找到，则安全停机并语音提示用户，严禁机器人在场内盲目乱跑或误跟其他顾客。
9. 严格的电源管理与电磁兼容（EMC）
   BLDC电机在频繁启停和差速转向时，会产生巨大的瞬态电流和高频电磁干扰。
   对策： 严禁将Arduino及上位机的逻辑电源与电机动力电源共用。必须使用隔离型DC-DC模块，并在电机驱动板输入端并联大容量低ESR电容以吸收反电动势。视觉传感器和IMU的信号线需做好屏蔽，防止电机PWM噪声导致画面撕裂或姿态解算发散。
10. 机械重心设计与防侧翻保护
    超市导购机器人通常需要搭载较大的显示屏或储物篮。若负载过高或重心偏上，在BLDC电机执行大角度差速转弯时极易发生侧翻。
    对策： 电池等重物应尽量布置在底盘最底部，轮距设计应尽可能宽。同时，底层控制必须结合IMU的横滚角（Roll）数据进行动态限速，当检测到倾角过大时，强制限制BLDC的最大转速和角速度。

1、颜色标签+超声波的多目标选择跟随（入门级视觉跟随）
适用场景：超市入口或促销区，顾客佩戴不同颜色标签（如红/蓝手环），机器人根据预设选择逻辑锁定特定颜色目标，实现简单的“跟我走”导购服务。

/* ===== 颜色标签选择跟随 + 超声波紧急避障 =====
 * 硬件：Arduino + BLDC差速电机 + Pixy2/OpenMV颜色识别 + 超声波传感器
 * 核心：通过颜色标签区分不同顾客，按预设优先级选择跟随目标
 * 参考：基于颜色直方图的视觉定位 + 超声波避障跟随方案
 */
#include <SimpleFOC.h>
#include <NewPing.h>
#include <PID_v1.h>

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

// ==================== 超声波避障传感器 ====================
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarF(TRIG_F, ECHO_F, 150);

// ==================== 视觉目标结构体 ====================
struct VisualTarget {
    bool detected;
    float x;          // 图像X坐标（0~320）
    float area;       // 目标面积（用于距离估计）
    int label;        // 标签ID（1=红色, 2=蓝色, 3=绿色）
};
VisualTarget targets[3];  // 最多3个目标

// 优先级设定：红色优先级最高，蓝色次之，绿色最后
const int PRIORITY_MAP[3] = {1, 2, 3};  // 标签ID优先级顺序

// ==================== PID控制器 ====================
double setpointAngle = 160;   // 期望目标居图像中央(x=160)
double angleError = 0;
double turnOutput = 0;
double Kp = 0.4, Ki = 0.02, Kd = 0.01;
PID anglePID(&angleError, &turnOutput, &setpointAngle, Kp, Ki, Kd, DIRECT);

int selectedTarget = 0;       // 当前选中的目标ID
bool targetLocked = false;

void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);    // OpenMV/Pixy2 通信串口
    
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    anglePID.SetMode(AUTOMATIC);
    anglePID.SetOutputLimits(-0.6, 0.6);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. 获取视觉目标数据 ====================
    receiveVisionData();
    
    // ==================== 2. 【核心】多目标选择逻辑 ====================
    selectedTarget = selectTargetByPriority();
    
    // ==================== 3. 超声波紧急避障 ====================
    float frontDist = sonarF.ping_cm();
    if (frontDist > 0 && frontDist < 20) {
        // 紧急制动避障
        motorL.move(0); motorR.move(0);
        delay(200);
        motorL.move(-0.3); motorR.move(-0.3);
        delay(300);
        motorL.move(0); motorR.move(0);
        return;
    }
    
    // ==================== 4. 目标锁定跟随 ====================
    if (selectedTarget > 0 && targets[selectedTarget-1].detected) {
        targetLocked = true;
        VisualTarget target = targets[selectedTarget-1];
        
        // 角度偏差：图像中心(160) 与目标X坐标的差
        angleError = target.x;
        anglePID.Compute();
        
        // 距离控制：目标面积越大越近
        float distFactor = constrain(target.area / 500.0, 0.0, 1.0);
        float baseSpeed = 0.2 + 0.6 * (1.0 - distFactor);
        baseSpeed = constrain(baseSpeed, 0.1, 0.8);
        
        // 近距离减速
        if (target.area > 600) baseSpeed = 0.1;
        
        // 差速驱动
        float wheelBase = 0.25;
        motorL.move(baseSpeed - turnOutput * wheelBase / 2);
        motorR.move(baseSpeed + turnOutput * wheelBase / 2);
        
        Serial.print("跟随目标: "); Serial.print(selectedTarget);
        Serial.print(" 偏差: "); Serial.println(angleError);
    } else {
        // 无目标：原地旋转搜索
        targetLocked = false;
        motorL.move(0.2);
        motorR.move(-0.2);
        Serial.println("🔍 搜索目标中...");
    }
    
    delay(50);
}

// ==================== 接收视觉数据 ====================
void receiveVisionData() {
    if (Serial1.available()) {
        String data = Serial1.readStringUntil('\n');
        // 格式: "id,x,area" 如 "1,150,350"
        int c1 = data.indexOf(',');
        int c2 = data.indexOf(',', c1+1);
        if (c1 > 0 && c2 > 0) {
            int id = data.substring(0, c1).toInt();
            int idx = id - 1;
            if (idx >= 0 && idx < 3) {
                targets[idx].detected = true;
                targets[idx].x = data.substring(c1+1, c2).toFloat();
                targets[idx].area = data.substring(c2+1).toFloat();
                targets[idx].label = id;
            }
        }
    }
    
    // 超时重置（视觉数据200ms未更新视为丢失）
    static unsigned long lastTime = 0;
    if (millis() - lastTime > 200) {
        for (int i = 0; i < 3; i++) targets[i].detected = false;
    }
    if (Serial1.available()) lastTime = millis();
}

// ==================== 多目标优先级选择 ====================
int selectTargetByPriority() {
    // 按优先级顺序检查：优先级高的标签优先选择
    for (int i = 0; i < 3; i++) {
        int labelId = PRIORITY_MAP[i];
        if (labelId <= 3 && targets[labelId-1].detected) {
            return labelId;
        }
    }
    return 0;  // 无目标
}

核心要点：
颜色标签区分多目标：不同顾客佩戴不同颜色标签，机器人按优先级选择跟随目标，实现简单的“选择跟随”
视觉数据解析：通过串口接收OpenMV/Pixy2发送的“ID,X,面积”格式数据，解析多目标信息
紧急避障：超声波作为硬安全边界，前方距离<20cm时无条件中断跟随

2、多模态目标切换跟随 + 动态权重分配（中级交互式跟随）
适用场景：超市导购机器人通过语音+视觉多模态交互实现目标切换。顾客说“跟我走”或“换一个人跟”时，机器人动态调整跟随目标，实现灵活的导购服务。

/* ===== 多模态目标切换跟随 + 动态权重分配 =====
 * 硬件：ESP32 + BLDC差速电机 + OpenMV视觉 + 语音识别模块 + 超声波
 * 核心：语音指令触发目标切换，动态权重平衡多目标选择
 * 参考：虹曦导购机器人“指向性识别”功能 + 动态权重混合控制
 */
#include <SimpleFOC.h>
#include <NewPing.h>
#include <PID_v1.h>

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

// ==================== 传感器 ====================
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarF(TRIG_F, ECHO_F, 150);

// ==================== 视觉目标 ====================
struct TargetInfo {
    bool detected;
    float x, y;          // 图像坐标
    float area;
    int id;
    float confidence;     // 识别置信度
};
TargetInfo targets[5];
int targetCount = 0;

// ==================== 语音指令 ====================
String voiceCommand = "";
bool commandReceived = false;

// ==================== 跟随控制状态 ====================
int currentTargetId = 0;
bool isFollowing = false;

// ==================== 动态权重参数 ====================
float w_heading = 0.5;   // 目标追踪权重
float w_safety = 0.3;    // 安全权重
float w_smooth = 0.2;    // 平滑权重

void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);  // 视觉模块
    Serial2.begin(9600);    // 语音模块
    
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. 多模态感知 ====================
    // 1.1 视觉目标检测
    receiveVisionTargets();
    
    // 1.2 语音指令接收
    if (Serial2.available()) {
        voiceCommand = Serial2.readStringUntil('\n');
        voiceCommand.trim();
        commandReceived = true;
        Serial.print("语音指令: "); Serial.println(voiceCommand);
    }
    
    // 1.3 超声波避障
    float frontDist = sonarF.ping_cm();
    if (frontDist > 0 && frontDist < 20) {
        emergencyStop();
        return;
    }
    
    // ==================== 2. 【核心】语音指令触发目标切换 ====================
    if (commandReceived) {
        handleVoiceCommand();
        commandReceived = false;
    }
    
    // ==================== 3. 【核心】动态权重调整 ====================
    // 前方有障碍时提高安全权重
    if (frontDist > 0 && frontDist < 50) {
        w_safety = 0.6;
        w_heading = 0.25;
        w_smooth = 0.15;
    } else {
        w_safety = 0.25;
        w_heading = 0.5;
        w_smooth = 0.25;
    }
    
    // ==================== 4. 目标选择与跟随 ====================
    if (currentTargetId > 0) {
        // 查找当前目标是否存在
        int targetIdx = -1;
        for (int i = 0; i < targetCount; i++) {
            if (targets[i].id == currentTargetId && targets[i].detected) {
                targetIdx = i;
                break;
            }
        }
        
        if (targetIdx >= 0) {
            isFollowing = true;
            followTarget(targets[targetIdx]);
        } else {
            // 目标丢失，搜索模式
            isFollowing = false;
            motorL.move(0.2);
            motorR.move(-0.2);
            Serial.println("目标丢失，搜索中...");
        }
    } else {
        // 无目标：待机
        motorL.move(0);
        motorR.move(0);
    }
    
    delay(50);
}

// ==================== 语音指令处理 ====================
void handleVoiceCommand() {
    if (voiceCommand.indexOf("跟我走") >= 0) {
        // 选择最近的目标作为跟随对象
        int bestIdx = -1;
        float maxArea = 0;
        for (int i = 0; i < targetCount; i++) {
            if (targets[i].detected && targets[i].area > maxArea) {
                maxArea = targets[i].area;
                bestIdx = i;
            }
        }
        if (bestIdx >= 0) {
            currentTargetId = targets[bestIdx].id;
            Serial.print("锁定目标: "); Serial.println(currentTargetId);
        }
    } else if (voiceCommand.indexOf("换一个人") >= 0) {
        // 切换到下一个目标
        switchToNextTarget();
    } else if (voiceCommand.indexOf("停止") >= 0) {
        currentTargetId = 0;
        isFollowing = false;
        motorL.move(0); motorR.move(0);
        Serial.println("跟随停止");
    }
}

// ==================== 切换目标 ====================
void switchToNextTarget() {
    // 从当前目标切换到下一个检测到的目标
    int currentIdx = -1;
    for (int i = 0; i < targetCount; i++) {
        if (targets[i].id == currentTargetId) {
            currentIdx = i;
            break;
        }
    }
    
    // 寻找下一个有效目标
    for (int i = 1; i <= targetCount; i++) {
        int nextIdx = (currentIdx + i) % targetCount;
        if (targets[nextIdx].detected && targets[nextIdx].id != currentTargetId) {
            currentTargetId = targets[nextIdx].id;
            Serial.print("切换至目标: "); Serial.println(currentTargetId);
            return;
        }
    }
}

// ==================== 目标跟随 ====================
void followTarget(TargetInfo target) {
    // 计算偏差
    float angleError = target.x - 160;  // 图像中心X
    float areaError = 300 - target.area; // 期望面积
    
    // 加权控制
    float turnCorrection = angleError * 0.005 * w_heading;
    float speedFactor = constrain(areaError / 500.0, -0.5, 0.5) * w_heading;
    
    float baseSpeed = 0.3 + 0.4 * speedFactor;
    baseSpeed = constrain(baseSpeed, 0.05, 0.6);
    
    float wheelBase = 0.25;
    motorL.move(baseSpeed - turnCorrection * wheelBase / 2);
    motorR.move(baseSpeed + turnCorrection * wheelBase / 2);
}

// ==================== 紧急停止 ====================
void emergencyStop() {
    motorL.move(0);
    motorR.move(0);
    delay(200);
    motorL.move(-0.3);
    motorR.move(-0.3);
    delay(300);
    motorL.move(0);
    motorR.move(0);
}

核心要点：
语音指令触发目标切换：支持“跟我走”“换一个人”“停止”等自然语言指令，实现人机交互式目标选择
动态权重分配：根据前方障碍距离动态调整“追踪”“安全”“平滑”三项权重，障碍近时提高安全权重
多目标遍历切换：“换一个人”指令顺序遍历当前检测到的所有目标

3、UWB标签优先选择 + 视觉多目标识别（高精度多目标跟随）
适用场景：超市VIP顾客佩戴UWB标签或员工标签，机器人优先识别并锁定VIP目标，同时视觉系统辅助识别其他顾客，实现“VIP优先+普通跟随”的混合服务模式。

/* ===== UWB优先选择 + 视觉多目标识别混合跟随 =====
 * 硬件：ESP32 + BLDC差速电机 + UWB模块 + OpenMV视觉 + 超声波
 * 核心：UWB提供高精度距离/方位，视觉识别多目标，按优先级融合选择
 * 参考：多基站UWB三角定位 + 视觉-无线电磁融合跟踪方案
 */
#include <SimpleFOC.h>
#include <DW1000.h>
#include <NewPing.h>
#include <math.h>

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

// ==================== UWB配置 ====================
#define TAG_ADDRESS 0xDECA0AD000000001
// 基站坐标（超市部署）
struct Anchor { float x, y; uint64_t addr; };
Anchor anchors[3] = {
    {0.0, 0.0, 0xDECA0AD000000002},
    {8.0, 0.0, 0xDECA0AD000000003},
    {4.0, 6.0, 0xDECA0AD000000004}
};

// ==================== UWB标签目标 ====================
struct UWB_Target {
    float x, y;          // 坐标(m)
    int id;
    float distance;      // 到机器人的距离
    bool active;
};
UWB_Target uwbTargets[5];
int uwbTargetCount = 0;

// ==================== 视觉目标 ====================
struct VisionTarget {
    float x, y;          // 图像坐标
    int id;
    float confidence;
    bool active;
};
VisionTarget visionTargets[5];
int visionTargetCount = 0;

// ==================== 优先级映射 ====================
// UWB标签ID 对应 优先级（数值越小优先级越高）
const int UWB_PRIORITY[5] = {1, 2, 3, 4, 5};  // ID1为VIP最高优先级

// ==================== 超声波避障 ====================
#define TRIG_F 2
#define ECHO_F 3
NewPing sonarF(TRIG_F, ECHO_F, 150);

// ==================== 控制状态 ====================
float robotX = 0, robotY = 0;
int selectedTarget = 0;

void setup() {
    Serial.begin(115200);
    Serial1.begin(115200);  // 视觉模块
    
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    
    // UWB初始化
    DW1000.begin(TAG_ADDRESS);
    DW1000.newConfiguration();
    DW1000.setNetworkId(10);
    DW1000.enableMode(DW1000.MODE_TDOA);
}

void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // ==================== 1. 多源定位感知 ====================
    // 1.1 UWB定位（获取所有标签坐标）
    updateUWBTargets();
    
    // 1.2 视觉目标识别
    updateVisionTargets();
    
    // 1.3 超声波避障
    float frontDist = sonarF.ping_cm();
    if (frontDist > 0 && frontDist < 20) {
        motorL.move(0); motorR.move(0);
        delay(200);
        motorL.move(-0.3); motorR.move(-0.3);
        delay(300);
        motorL.move(0); motorR.move(0);
        return;
    }
    
    // ==================== 2. 【核心】多目标融合选择 ====================
    // 先检查UWB目标，按优先级选择最高者
    int uwbSelected = selectUWBTargetByPriority();
    
    // 若无UWB目标，从视觉目标中选择
    int visionSelected = selectVisionTarget();
    
    // 融合决策：UWB优先
    if (uwbSelected > 0) {
        selectedTarget = uwbSelected;
    } else if (visionSelected > 0) {
        selectedTarget = visionSelected;
    } else {
        selectedTarget = 0;
    }
    
    // ==================== 3. 目标跟随 ====================
    if (selectedTarget > 0) {
        float tx = 0, ty = 0;
        // 查找目标坐标
        bool found = false;
        for (int i = 0; i < uwbTargetCount; i++) {
            if (uwbTargets[i].id == selectedTarget && uwbTargets[i].active) {
                tx = uwbTargets[i].x; ty = uwbTargets[i].y;
                found = true;
                break;
            }
        }
        if (!found) {
            // 从视觉目标查找
            for (int i = 0; i < visionTargetCount; i++) {
                if (visionTargets[i].id == selectedTarget && visionTargets[i].active) {
                    // 视觉目标需转换为实际坐标（简化）
                    tx = robotX + (visionTargets[i].x - 160) / 100.0;
                    ty = robotY + (visionTargets[i].y - 120) / 100.0;
                    found = true;
                    break;
                }
            }
        }
        
        if (found) {
            followCoordinate(tx, ty);
        }
    } else {
        // 无目标，待机搜索
        motorL.move(0);
        motorR.move(0);
        Serial.println("🔍 搜索目标...");
    }
    
    delay(50);
}

// ==================== UWB目标更新 ====================
void updateUWBTargets() {
    // 通过UWB获取所有标签的距离
    for (int i = 0; i < uwbTargetCount; i++) {
        // 实际通过DW1000库获取测距
        // float d0 = DW1000.getRange(anchors[0].addr);
        // 三边定位解算坐标...
        // 此处用模拟数据
    }
}

// ==================== 视觉目标更新 ====================
void updateVisionTargets() {
    if (Serial1.available()) {
        String data = Serial1.readStringUntil('\n');
        // 解析视觉目标数据
        // 格式: "count|id,x,y|id,x,y|..."
        // 简化解析...
    }
}

// ==================== 【核心】UWB目标优先级选择 ====================
int selectUWBTargetByPriority() {
    // 按优先级顺序检查
    for (int p = 1; p <= 5; p++) {
        for (int i = 0; i < uwbTargetCount; i++) {
            if (uwbTargets[i].active && UWB_PRIORITY[i] == p) {
                return uwbTargets[i].id;
            }
        }
    }
    return 0;
}

// ==================== 视觉目标选择 ====================
int selectVisionTarget() {
    // 从视觉目标中选择置信度最高的
    int bestIdx = -1;
    float maxConf = 0;
    for (int i = 0; i < visionTargetCount; i++) {
        if (visionTargets[i].active && visionTargets[i].confidence > maxConf) {
            maxConf = visionTargets[i].confidence;
            bestIdx = i;
        }
    }
    if (bestIdx >= 0) {
        return visionTargets[bestIdx].id;
    }
    return 0;
}

// ==================== 坐标跟随 ====================
void followCoordinate(float tx, float ty) {
    float dx = tx - robotX;
    float dy = ty - robotY;
    float dist = sqrt(dx*dx + dy*dy);
    
    if (dist > 0.2) {
        float targetAngle = atan2(dy, dx);
        float angleError = targetAngle - 0;  // 假设机器人航向为0
        
        // 归一化
        if (angleError > PI) angleError -= 2*PI;
        if (angleError < -PI) angleError += 2*PI;
        
        float baseSpeed = constrain(dist * 0.6, 0.1, 0.8);
        float turnSpeed = constrain(angleError * 1.5, -0.6, 0.6);
        
        float wheelBase = 0.25;
        motorL.move(baseSpeed - turnSpeed * wheelBase / 2);
        motorR.move(baseSpeed + turnSpeed * wheelBase / 2);
        
        // 更新自身坐标（实际需里程计/IMU融合）
        robotX += baseSpeed * cos(0) * 0.05;
        robotY += baseSpeed * sin(0) * 0.05;
    } else {
        motorL.move(0);
        motorR.move(0);
    }
}

核心要点：
UWB优先+视觉辅助：UWB提供高精度全局定位，视觉提供目标身份识别，两者融合实现可靠的多目标选择
优先级映射：VIP顾客佩戴特定UWB标签，系统按预设优先级自动选择目标
传感器融合：UWB克服视觉遮挡问题，视觉弥补UWB无法识别身份的缺陷，学术方案验证了类似“视觉+无线电磁”融合跟踪的可行性

要点解读

1. 多目标选择的核心是“优先级决策机制”
   超市导购机器人面临多人场景时，必须有一套清晰的优先级规则：VIP顾客优先、特定颜色标签优先、语音指令触发切换、或距离最近者优先。案例一采用颜色标签优先级，案例二采用语音指令切换，案例三采用UWB-ID优先级映射，分别对应不同应用场景。

2. 视觉跟随的精度取决于传感器选型与标定
   低成本方案（Pixy2/OpenMV+颜色标签）适合入门级场景，但光照变化影响大。工业级方案（RGB-D+UWB融合）可在货架遮挡环境下达到毫米级跟踪精度。工程实践中需注意摄像头标定、颜色阈值自适应调整等细节。

3. 多模态交互是超市导购场景的“刚需”
   顾客不能只靠机器人识别，还需通过语音、手势等方式主动“告诉”机器人自己的需求。行业首个商业落地的“虹曦”导购机器人就集成了“指向性识别”功能，支持“跟着我”“换一个人”等自然语音指令。

4. 避障安全是跟随系统不可妥协的“硬约束”
   超市货架密集、人流量大，前方障碍距离<20cm时必须触发无条件急停，无论当前跟随哪个目标。这与“安全本能”架构的优先级原则一致——安全高于任务。

5. 硬件算力决定目标识别与跟随的实现方案
   Arduino Uno不足以同时运行YOLO等深度学习模型和人脸识别，此类算力需由OpenMV、树莓派或旭日X3派等协处理器承载。建议采用主从架构：上位机（树莓派/Jetson）负责视觉推理与多目标跟踪，下位机（Arduino/ESP32）负责BLDC实时控制与紧急避障，两者通过串口高频交互。

4、超声波矩阵+PID的多目标动态跟随系统
场景定位
适用于超市通道、货架密集区，机器人通过环形超声波阵列实现360°多目标检测（如前方顾客、侧方货架、后方推车），结合PID算法动态调整BLDC电机速度与转向，保持与目标的安全跟随距离，同时规避静态障碍物。

#include <NewPing.h>
#include <PID_v1.h>

// 硬件引脚定义
#define NUM_SONARS 6  // 环形超声波阵列
#define MOTOR_LEFT_PWM 9
#define MOTOR_RIGHT_PWM 10

// 超声波传感器（环形布局：前/左前/左后/右后/右前/后）
NewPing sonar[NUM_SONARS] = {
  {2, 3, 200},  // 前向
  {4, 5, 200},  // 左前
  {6, 7, 200},  // 左后
  {8, 9, 200},  // 右后
  {10, 11, 200},// 右前
  {12, 13, 200} // 后向
};

// PID控制参数（距离环：维持目标跟随距离）
double targetDist = 80.0;  // 目标跟随距离（cm）
double currentDist, output;
PID distPID(&currentDist, &output, &targetDist, 0.8, 0.2, 0.05, DIRECT);

// 电机控制变量
int baseSpeed = 120;  // 基础速度（PWM值）
int leftSpeed, rightSpeed;

void setup() {
  Serial.begin(115200);
  pinMode(MOTOR_LEFT_PWM, OUTPUT);
  pinMode(MOTOR_RIGHT_PWM, OUTPUT);
  distPID.SetMode(AUTOMATIC);
  distPID.SetOutputLimits(-50, 50);  // 输出限幅，避免速度突变
}

void loop() {
  // 1. 采集多目标距离数据
  float distArray[NUM_SONARS];
  for (int i = 0; i < NUM_SONARS; i++) {
    distArray[i] = sonar[i].ping_cm();
  }

  // 2. 中值滤波：消除异常数据（如超声波盲区跳变）
  currentDist = medianFilter(distArray, NUM_SONARS);

  // 3. PID计算：输出速度调整量
  distPID.Compute();

  // 4. 差速控制：根据输出调整左右电机速度（实现转向跟随）
  leftSpeed = constrain(baseSpeed + output, 80, 200);
  rightSpeed = constrain(baseSpeed - output, 80, 200);

  // 5. 避障逻辑：若侧方/后方距离过近，减速避让
  if (distArray[1] < 30 || distArray[2] < 30) {  // 左侧障碍
    leftSpeed = constrain(leftSpeed * 0.6, 80, 200);
  }
  if (distArray[4] < 30 || distArray[3] < 30) {  // 右侧障碍
    rightSpeed = constrain(rightSpeed * 0.6, 80, 200);
  }

  // 6. 电机输出
  analogWrite(MOTOR_LEFT_PWM, leftSpeed);
  analogWrite(MOTOR_RIGHT_PWM, rightSpeed);

  // 7. 串口调试
  Serial.print("CurrentDist: "); Serial.print(currentDist);
  Serial.print(" | Output: "); Serial.print(output);
  Serial.print(" | LeftSpeed: "); Serial.print(leftSpeed);
  Serial.println(" | RightSpeed: " + String(rightSpeed));

  delay(30);  // 控制频率约33Hz，适配超声波响应速度
}

// 中值滤波函数：去除异常数据
float medianFilter(float arr[], int n) {
  sort(arr, arr + n);
  if (n % 2 == 0) {
    return (arr[n/2 - 1] + arr[n/2]) / 2;
  } else {
    return arr[n/2];
  }
}

5、UWB多基站坐标跟随+多目标切换系统
场景定位
适用于大型超市、仓储式会员店，机器人通过多基站UWB定位实现全局坐标下的多目标跟随（如绑定顾客的UWB标签、指定商品的坐标点），支持多目标切换，结合BLDC FOC控制实现精准坐标跟踪，适应长距离、大范围的跟随需求。

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

// BLDC电机配置（双轮差速底盘）
BLDCMotor motorL = BLDCMotor(7);  // 左轮电机（极对数7）
BLDCMotor motorR = BLDCMotor(7);  // 右轮电机
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 7);

// UWB多基站坐标（单位：米，需提前标定）
struct Anchor {
  float x, y;
  uint64_t addr;
};
Anchor anchors[4] = {
  {0.0, 0.0, 0xDECA0AD000000001},   // 基站1（入口）
  {10.0, 0.0, 0xDECA0AD000000002}, // 基站2（食品区）
  {10.0, 8.0, 0xDECA0AD000000003}, // 基站3（日用品区）
  {0.0, 8.0, 0xDECA0AD000000004}   // 基站4（收银区）
};

// 目标坐标（多目标切换变量）
float targetX = 5.0, targetY = 4.0;  // 默认目标（顾客标签坐标）
float robotX = 0.0, robotY = 0.0;    // 机器人实时坐标
float lookAheadDist = 1.0;            // 预瞄距离（Pure Pursuit核心参数）

// 目标切换标志（可通过RFID/串口触发）
bool switchTarget = false;
int targetID = 1;  // 目标ID：1=顾客，2=食品区，3=日用品区

void setup() {
  Serial.begin(115200);
  // 初始化BLDC电机（FOC闭环控制）
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.init();
  motorL.initFOC();

  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.init();
  motorR.initFOC();

  // 初始化UWB模块（简化版，实际需调用DW1000库）
  initUWB();
}

void loop() {
  // 1. FOC电机闭环控制（内环速度控制）
  motorL.loopFOC();
  motorR.loopFOC();

  // 2. UWB定位解算（获取机器人实时坐标，实际需通过UWB库回调）
  updateRobotPosition();  // 核心函数：通过UWB基站数据解算robotX/robotY

  // 3. 多目标切换逻辑（示例：检测到RFID商品标签，切换目标）
  if (switchTarget) {
    switch (targetID) {
      case 2: targetX = 8.0; targetY = 2.0; break;  // 食品区坐标
      case 3: targetX = 8.0; targetY = 6.0; break;  // 日用品区坐标
    }
    switchTarget = false;
  }

  // 4. Pure Pursuit算法：计算转向曲率与速度
  float dx = targetX - robotX;
  float dy = targetY - robotY;
  float distance = sqrt(dx*dx + dy*dy);

  if (distance > 0.3) {  // 距离目标大于30cm，继续跟随
    // 计算预瞄点（目标前方lookAheadDist处）
    float alpha = atan2(dy, dx);
    float preX = targetX - lookAheadDist * cos(alpha);
    float preY = targetY - lookAheadDist * sin(alpha);

    // 计算转向曲率（基于当前位置与预瞄点的几何关系）
    float crossTrackError = (preX - robotX) * sin(alpha) - (preY - robotY) * cos(alpha);
    float curvature = 2 * crossTrackError / (lookAheadDist * lookAheadDist);

    // 计算线速度与角速度
    float linearSpeed = constrain(distance * 0.5, 0, 2.0);  // 距离越远，速度越快
    float angularSpeed = curvature * linearSpeed;

    // 差速控制：转化为左右电机速度
    motorL.move(linearSpeed - angularSpeed * 0.15);
    motorR.move(linearSpeed + angularSpeed * 0.15);
  } else {
    // 到达目标点，停车
    motorL.move(0);
    motorR.move(0);
  }

  // 5. 串口调试
  Serial.print("Robot: ("); Serial.print(robotX); Serial.print(", "); Serial.print(robotY);
  Serial.print(") | Target: ("); Serial.print(targetX); Serial.print(", "); Serial.print(targetY);
  Serial.print(") | Distance: "); Serial.println(distance);

  delay(50);  // 控制频率20Hz，适配UWB定位更新率
}

// 简化的UWB初始化函数（实际需调用DW1000库）
void initUWB() {
  // 配置UWB模块：网络ID、信道、发射功率
  // 实际代码：DW1000.begin(0xDECA0AD000000001); 等
}

// 简化的坐标更新函数（实际需通过UWB库获取基站距离并解算）
void updateRobotPosition() {
  // 示例：通过三边定位解算坐标（实际需最小二乘法提高精度）
  static float d0, d1, d2;
  d0 = getUWBRange(anchors[0].addr);  // 到基站1的距离
  d1 = getUWBRange(anchors[1].addr);  // 到基站2的距离
  d2 = getUWBRange(anchors[2].addr);  // 到基站3的距离

  // 三边定位解算（简化版，实际需处理非视距误差）
  if (d0 > 0 && d1 > 0 && d2 > 0) {
    // 此处为简化解算逻辑，实际需调用最小二乘法函数
    robotX = (d0*d0 - d1*d1 + anchors[1].x*anchors[1].x) / (2*anchors[1].x);
    robotY = sqrt(d0*d0 - robotX*robotX);
  }
}

6、视觉+IMU融合的多目标识别与跟随系统
场景定位
适用于高端超市、购物中心，机器人通过视觉识别（识别人体、商品标识、二维码）实现多目标锁定，结合IMU姿态矫正，动态调整BLDC电机速度，实现精准跟随与主动导购（如识别顾客手势切换目标、识别商品二维码提供导购信息）。

#include <SimpleFOC.h>
#include <Wire.h>

// BLDC电机配置
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
BLDCDriver3PWM driverR = BLDCDriver3PWM(3, 5, 6, 7);

// IMU配置（MPU6050）
MPU6050 imu;
float gyroYaw = 0.0;  // 偏航角（姿态矫正核心参数）

// 视觉目标参数（OpenMV识别结果，通过串口传输给Arduino）
int targetX = 0, targetY = 0;  // 目标在图像中的坐标
int targetType = 0;            // 目标类型：0=无目标，1=顾客，2=商品
bool targetLost = false;        // 目标丢失标志

// PID控制参数（双环：位置环+角度环）
PID posPID(&posError, &posOutput, &posSetpoint, 2.0, 0.5, 0.1, DIRECT);  // 位置环
PID anglePID(&angleError, &angleOutput, &angleSetpoint, 1.5, 0.3, 0.05, DIRECT);  // 角度环

// 跟随参数
float targetDist = 100.0;  // 目标跟随距离（cm，通过视觉估算）
float currentDist = 0.0;
float posError = 0.0, posOutput = 0.0, posSetpoint = targetDist;
float angleError = 0.0, angleOutput = 0.0, angleSetpoint = 0.0;

void setup() {
  Serial.begin(115200);
  Wire.begin();
  // 初始化IMU
  imu.initialize();
  // 初始化BLDC电机
  driverL.voltage_power_supply = 12;
  driverL.init();
  motorL.linkDriver(&driverL);
  motorL.init();
  motorL.initFOC();

  driverR.voltage_power_supply = 12;
  driverR.init();
  motorR.linkDriver(&driverR);
  motorR.init();
  motorR.initFOC();

  // 初始化PID
  posPID.SetMode(AUTOMATIC);
  posPID.SetOutputLimits(-30, 30);
  anglePID.SetMode(AUTOMATIC);
  anglePID.SetOutputLimits(-20, 20);
}

void loop() {
  // 1. FOC电机闭环控制
  motorL.loopFOC();
  motorR.loopFOC();

  // 2. 读取IMU姿态数据（补偿视觉识别的姿态偏差）
  updateIMU();  // 核心函数：通过MPU6050获取偏航角gyroYaw

  // 3. 读取视觉目标数据（OpenMV通过串口发送：目标类型、坐标、距离）
  readVisualTarget();

  // 4. 目标丢失处理：若目标丢失，进入3秒扫描模式
  if (targetLost) {
    static unsigned long lostTime = 0;
    if (millis() - lostTime < 3000) {
      // 扫描模式：左右转向寻找目标
      motorL.move(1.0);
      motorR.move(-1.0);
    } else {
      // 超时未找到目标，停车
      motorL.move(0);
      motorR.move(0);
    }
    return;
  }

  // 5. 计算位置误差与角度误差
  currentDist = targetDist - getVisualDistance();  // 视觉估算距离（需OpenMV提供）
  posError = currentDist;
  angleError = getVisualAngle() - gyroYaw;  // 视觉目标角度与IMU姿态的偏差

  // 6. 双PID控制计算
  posPID.Compute();
  anglePID.Compute();

  // 7. 差速控制：位置环输出调整速度，角度环输出调整转向
  float baseSpeed = 1.5;  // 基础线速度（rad/s）
  float leftSpeed = baseSpeed + posOutput - angleOutput;
  float rightSpeed = baseSpeed + posOutput + angleOutput;

  // 限幅处理，避免速度突变
  leftSpeed = constrain(leftSpeed, 0.5, 3.0);
  rightSpeed = constrain(rightSpeed, 0.5, 3.0);

  // 8. 电机输出
  motorL.move(leftSpeed);
  motorR.move(rightSpeed);

  // 9. 串口调试
  Serial.print("TargetType: "); Serial.print(targetType);
  Serial.print(" | Dist: "); Serial.print(currentDist);
  Serial.print(" | AngleError: "); Serial.print(angleError);
  Serial.print(" | LeftSpeed: "); Serial.print(leftSpeed);
  Serial.println(" | RightSpeed: " + String(rightSpeed));

  delay(20);  // 控制频率50Hz，适配视觉识别帧率
}

// 更新IMU偏航角（核心姿态矫正）
void updateIMU() {
  int16_t aa, ab, ac;
  int16_t gx, gy, gz;
  imu.getMotion6(&aa, &ab, &ac, &gx, &gy, &gz);
  // 计算偏航角（简化版，实际需融合加速度计与陀螺仪数据）
  gyroYaw += (float)gy * 0.01;  // 角速度积分，0.01为采样周期
  gyroYaw = constrain(gyroYaw, -M_PI, M_PI);  // 角度限幅
}

// 读取视觉目标数据（OpenMV通过串口发送，格式：Type,X,Y,Dist,Angle）
void readVisualTarget() {
  if (Serial.available() >= 10) {
    String data = Serial.readStringUntil('\n');
    int comma1 = data.indexOf(',');
    int comma2 = data.indexOf(',', comma1+1);
    int comma3 = data.indexOf(',', comma2+1);
    int comma4 = data.indexOf(',', comma3+1);

    targetType = data.substring(0, comma1).toInt();
    targetX = data.substring(comma1+1, comma2).toInt();
    targetY = data.substring(comma2+1, comma3).toInt();
    float dist = data.substring(comma3+1, comma4).toFloat();
    float angle = data.substring(comma4+1).toFloat();

    if (targetType == 0) {
      targetLost = true;
    } else {
      targetLost = false;
      targetDist = dist;  // 更新目标距离
      // 计算目标角度（相对于机器人正前方）
      angleSetpoint = angle;
    }
  }
}

// 简化的视觉距离估算函数（实际由OpenMV计算）
float getVisualDistance() {
  // 基于目标在图像中的像素大小估算距离（需提前标定）
  return targetDist;  // 此处返回OpenMV传输的距离值
}

// 简化的视觉角度计算函数
float getVisualAngle() {
  // 基于目标在图像中的X坐标估算角度（需提前标定）
  return (targetX - 320) * 0.01;  // 320为图像中心，0.01为角度转换系数
}

要点解读

1. 多传感器融合：提升多目标识别的鲁棒性与抗干扰能力
   超市环境存在光照变化、人群遮挡、货架干扰等复杂因素，单一传感器无法满足多目标跟随需求，融合设计是核心：
   感知互补：超声波覆盖中远距离（30-200cm），规避视觉盲区；视觉识别目标类型（顾客/商品），弥补超声波无法识别属性的缺陷；IMU提供姿态数据，补偿视觉因机器人晃动导致的识别偏差；UWB提供全局坐标，解决视觉在遮挡场景下的跟随失效问题。
   数据融合：通过中值滤波、卡尔曼滤波等算法，融合多传感器数据，消除异常值（如超声波的盲区跳变、视觉的光照干扰），提升目标检测的准确性。
   场景适配：针对超市不同区域（通道用超声波、货架区用视觉、开阔区用UWB）动态切换主导传感器，兼顾成本与性能，避免单一传感器的局限性。

2. BLDC驱动的精准控制：保障跟随的稳定性与机动性
   超市导购机器人需频繁启停、转向，BLDC的精准控制是跟随平稳性与机动性的关键，核心体现在两点：
   闭环控制架构：采用“外环目标控制+内环FOC控制”的双环架构：外环基于目标距离/坐标计算期望速度与转向角，内环通过FOC算法实现BLDC的电流、速度闭环，确保电机输出严格跟踪期望值，避免因负载突变（如载重变化）导致的速度波动。
   差速转向的精准性：通过编码器反馈的轮速数据，结合PID算法动态调整左右电机PWM，实现差速转向的精准控制，转弯半径可小于0.5米，适应超市狭窄通道的灵活转向，同时通过S型加减速算法限制加加速度，避免急刹、急转导致的货物晃动或机器人侧翻。

3. 多目标切换与优先级调度：适配超市动态导购需求
   超市场景中，机器人需在跟随顾客、导航至商品区、避障等任务间动态切换，多目标调度逻辑决定服务效率：
   目标优先级定义：以“安全”为最高优先级，其次是“核心跟随目标（绑定顾客）”，最后是“次要导购目标（商品导航）”。例如，当顾客与货架同时进入检测范围时，优先保持对顾客的跟随；若顾客手势示意前往某商品区，再切换目标至商品坐标。
   目标锁定与防丢失：通过身份绑定机制（如UWB标签绑定、视觉人脸绑定）确保机器人仅跟随指定目标，避免跟错人；引入卡尔曼滤波预测目标短时运动轨迹，当目标被货架短暂遮挡时，基于历史轨迹继续跟随，而非立即停车，保障跟随的连续性。
   平滑切换机制：目标切换时，通过渐变式调整PID参数与速度，避免速度突变导致的机器人抖动，提升用户体验（如从跟随顾客切换至商品导航时，速度从1m/s平滑过渡至0.5m/s，避免急加速）。

4. 安全与应急机制：保障超市复杂环境下的运行安全
   超市人流密集、环境动态多变，安全是机器人部署的核心前提，需构建多层级安全体系：
   传感器冗余与故障切换：关键传感器（如超声波、视觉）采用双备份设计，当主传感器失效时，自动切换至备用传感器；例如，视觉因强光失效时，自动切换至超声波+IMU的跟随模式，避免目标丢失。
   硬件级应急保护：设计硬件急停回路，串联急停按钮、碰撞开关与电机驱动器的使能端，当软件失效或发生碰撞时，直接切断电机动力，响应时间小于10ms，优先保障人身与财产安全。
   软件限幅与丢失保护：对PWM输出、速度、转向角进行限幅，避免电机失控；当目标丢失超过3秒，触发原地扫描模式，超时仍未找回目标则自动停机，防止机器人盲目移动导致碰撞。

5. 工程化适配：从代码到超市场景的落地优化
   代码的工程化适配决定机器人能否在实际超市中稳定运行，核心优化方向包括：
   参数现场标定：所有核心参数（如目标距离、速度、PID参数）需预留现场标定接口，根据超市实际环境（通道宽度、货架布局、地面摩擦力）动态调整，避免“固定参数适配所有场景”的僵化设计。
   算力与实时性平衡：采用“上位机+下位机”架构，上位机（ESP32/NVIDIA Jetson）负责视觉识别、SLAM等高算力任务，下位机（Arduino）专注BLDC电机控制与传感器数据采集，确保控制循环的实时性，避免因算力不足导致控制延迟。
   抗干扰与稳定性设计：电源隔离，UWB、视觉模块采用独立LDO供电，与电机驱动电源隔离，避免BLDC的电磁干扰导致传感器数据跳变；通信保障，采用带CRC校验的通信协议，设计断线重连机制，确保上位机与下位机的指令传输可靠，避免通信丢包导致控制失效。

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