协作型作业机器人核心目标：机器人与操作人员近距离协同作业，不是单纯的自主机器人，也不是遥控小车；以 BLDC 无刷电机作为动力执行单元，Arduino 作为主控，实现跟随人体运动，并且根据人机交互条件自动 / 手动切换作业任务模式，广泛用于搬运、巡检、辅助作业场景。下面从主要特点、应用场景、注意事项三部分做专业解析。

一、主要特点

1. 人机动态跟随（区别普通目标跟随）

普通跟随只做距离闭环；人机协同动态跟随增加速度匹配：机器人不只是追到固定距离，还会跟随人的行走速度做速度同步，人快走机器人提速、人停下机器人降速停止。

• 感知来源：毫米波雷达、ToF、视觉人体检测、超声波组合；获取人与机器人相对距离、相对角度、人体移动速度。
• BLDC 无刷优势：转矩响应快、调速平滑，启停冲击小，适合近距离跟随时的加减速，不会出现猛冲、急刹抖动，保障人机安全。
• Arduino 负责：读取传感器数据，解算人体运动状态，输出速度指令给 BLDC 驱动器。

2. 多模式任务切换机制

具备状态机管理，支持多种作业模式之间平滑切换，典型模式：
1）人机跟随模式：跟随操作人员移动，辅助随行作业；
2）定点作业模式：到达目标点位后，机器人停止跟随，执行巡检、物料投放、检测采集；
3）自主巡航模式：脱离跟随，沿预设路线自主移动；
4）安全待机模式：触发安全条件（距离过近、急停信号）立刻切断动力，进入待机。

切换触发源：人体距离变化、按键指令、串口指令、传感器告警信号。状态机保证模式切换不会出现电机失控、速度跳变。

3. BLDC 动力系统特性

• 无刷电机噪声低、寿命长，车间、仓库近距离人机环境下体验更好；
• 支持闭环速度 / 位置控制，可实现柔性运动，避免硬碰撞伤人；
• Arduino 本身不直接驱动 BLDC，需要外接 FOC 无刷驱动器，主控下发速度指令，驱动器完成电流环、速度环闭环。

4. 安全优先的协作逻辑

协作机器人首要约束：人优先级高于任务。

• 当检测到人进入危险近距离区域，优先降速、停机，不继续执行原有运动指令；
• 设置软限位（距离阈值降速）+ 硬件急停双重防护；
• 跟随过程做限幅：最大速度、最大加速度做硬限制，防止算法异常导致机器人冲撞人员。

5. 人机交互解耦

人和机器人没有物理连接，属于非接触协同；Arduino 做上层逻辑决策，运动控制交给 BLDC 驱动板，实现感知‑决策‑执行分层架构，便于后期修改任务逻辑，不用大幅改动电机底层驱动。

二、应用场景

1. 工厂车间辅助搬运
   工人作业时机器人随行，承载工具、物料；工人走到工位，机器人自动切换定点作业模式，等待装卸物料；工人移动继续跟随。
2. 仓储库房辅助巡检搬运
   巡检人员现场排查，机器人跟随随行，搭载传感器采集环境数据；到达货架位置切换定点采集任务，完成后继续跟随人员移动。
3. 农业田间辅助作业
   农技人员田间巡查，机器人跟随携带采样设备；到作业点切换定点模式完成土壤、图像采集；人员转移继续随行。
4. 室内应急辅助作业
   救援、检修现场，跟随工作人员携带设备；人员停下开展作业，机器人切换待机 / 定点监测；遇到危险靠近自动停机。

限制：该方案基于 Arduino，算力有限，适合中小低速协作机器人；不适合高速重载工业级协作机器人。

三、需要注意的事项

1. 跟随算法层面

1）人体检测容易误识别：环境杂物、墙体、设备会被传感器误判为人，需要多传感器融合校验，不能单靠一路传感器做跟随判断；
2）动态跟随容易出现振荡：人和机器人距离来回拉扯（逼近‑后退‑再逼近），需要对速度环做滤波，限制加速度，不能只做简单 PID 距离闭环；
3）人员遮挡问题：人员被障碍物遮挡后，机器人不能失控乱跑，必须设计目标丢失逻辑：减速、原地待机，而不是继续盲目运动。

2. 任务切换状态机风险

1）模式切换必须做状态锁：切换瞬间禁止速度突变，BLDC 电机需要平滑过渡，禁止直接从高速跟随瞬间切到其他模式；
2）避免状态死锁：传感器异常时防止卡在某一种任务模式，需要看门狗、超时复位逻辑；
3）指令冲突处理：同时收到按键、传感器、串口多路切换指令，需要定义指令优先级，人机安全信号优先级最高。

3. BLDC 电机与驱动硬件问题

1）Arduino 不直接驱动三相无刷，驱动器选型要匹配电机功率；必须做好电流保护；
2）协作场景严禁大冲击运动，FOC 驱动要开启柔性限幅，限制最大扭矩，防止碰撞伤人；
3）电源波动：BLDC 启停会带来电压跌落，造成 Arduino 主控、传感器复位，需要做好电源滤波与独立供电。

4. 人机安全关键点

1）软件降速保护不等于全部保障，必须保留外部硬件急停通路，软件卡死情况下依旧可以切断电机动力；
2）严格限制机器人最大运行速度，人机协同场景不宜高速；
3）要区分 “软告警减速” 和 “紧急停机” 两个阈值，距离近先降速，进入危险距离直接停机。

5. Arduino 平台本身局限性

Arduino 算力有限，复杂人体识别、大量传感器融合会出现延时；传感器数据量大时会造成状态机卡顿。
优化手段：传感器尽量自带预处理输出结果给 Arduino，不要让 Arduino 承担大量原始数据运算；复杂运算可交给 ESP32 等更强控制器。

1、UWB差分定位跟随 + 动态任务分配（人机协同拣选）
适用场景：仓储物流场景中，机器人跟随佩戴UWB标签的拣货员，携带货架/重物；同时动态分配拣货目标点，在货架间实现人机协同作业。
核心逻辑：机器人通过双UWB标签差分定位获取目标的绝对位置与航向角，结合卡尔曼滤波平滑跟踪轨迹。任务分配算法按“优先级权重-距离权重”排序得分，机器人完成当前任务后自动认领最高分任务。

#include <SimpleFOC.h>
#include <DWM1001.h>      // UWB定位库
#include <Kalman.h>       // 卡尔曼滤波

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

// ==================== 任务结构体（动态分配） ====================
struct Task {
    int id;
    float targetX, targetY;
    int priority;         // 1最高，3最低
    bool assigned;
};
Task taskQueue[5] = {
    {1, 5.0, 3.0, 1, false},
    {2, 2.0, 4.0, 2, false},
    {3, 8.0, 6.0, 3, false}
};
int currentTaskIdx = -1;

// ==================== UWB差分定位 ====================
DWM1001 dwm;
float targetX = 0, targetY = 0;    // 目标绝对位置
float robotX = 0, robotY = 0;      // 自身位置
float targetAngle = 0;              // 目标航向角

// 卡尔曼滤波器（平滑UWB跳变）
KalmanFilter kfX(0.01, 0.1), kfY(0.01, 0.1);

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

// ==================== 任务分配算法 ====================
int allocateTask() {
    int bestIdx = -1;
    float bestScore = -999;
    for (int i = 0; i < 5; i++) {
        if (taskQueue[i].assigned) continue;
        float dx = taskQueue[i].targetX - robotX;
        float dy = taskQueue[i].targetY - robotY;
        float dist = sqrt(dx*dx + dy*dy);
        // 得分 = 优先级权重 - 距离权重（每100mm扣0.5分）
        float score = (4 - taskQueue[i].priority) * 1.0 - dist * 0.5;
        if (score > bestScore) {
            bestScore = score;
            bestIdx = i;
        }
    }
    return bestIdx;
}

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机与FOC (略)
    // 初始化UWB模块
    dwm.init(DWM1001_CS, DWM1001_IRQ, DWM1001_RST);
}

// ==================== 主循环 ====================
void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. UWB差分定位更新（卡尔曼滤波平滑）
    dwm.getCoordinates(targetX, targetY);
    // 双标签差分获得航向角
    // targetAngle = atan2(targetY_L2 - targetY_L1, targetX_L2 - targetX_L1);
    float smoothX = kfX.update(targetX);
    float smoothY = kfY.update(targetY);
    
    // 2. 任务分配与切换
    if (currentTaskIdx == -1 || isTaskCompleted()) {
        currentTaskIdx = allocateTask();
        if (currentTaskIdx >= 0) {
            taskQueue[currentTaskIdx].assigned = true;
        }
    }
    
    // 3. 决策切换：有人目标优先跟随 vs 自主任务执行
    float dx = smoothX - robotX;
    float dy = smoothY - robotY;
    float distToHuman = sqrt(dx*dx + dy*dy);
    
    // 若人距离较近（<3m）且任务目标远，优先跟随人
    if (distToHuman < 3.0 && currentTaskIdx >= 0) {
        // 人机协同：跟随目标，同时带任务点作为航向偏置
        float targetDx = taskQueue[currentTaskIdx].targetX - robotX;
        float targetDy = taskQueue[currentTaskIdx].targetY - robotY;
        float followAngle = atan2(dy, dx);
        float taskAngle = atan2(targetDy, targetDx);
        float steerAngle = followAngle * 0.7 + taskAngle * 0.3;  // 加权融合
        float speed = constrain((distToHuman - FOLLOW_DIST) * 0.3, 0.1, MAX_SPEED);
        // 差速驱动
        float wheelBase = 0.25;
        motorL.move(speed - steerAngle * wheelBase / 2);
        motorR.move(speed + steerAngle * wheelBase / 2);
    } else if (currentTaskIdx >= 0) {
        // 自主任务模式：前往任务目标点
        float dxT = taskQueue[currentTaskIdx].targetX - robotX;
        float dyT = taskQueue[currentTaskIdx].targetY - robotY;
        float distT = sqrt(dxT*dxT + dyT*dyT);
        if (distT > 0.2) {
            float angle = atan2(dyT, dxT);
            float speed = constrain(distT * 0.4, 0.1, MAX_SPEED);
            float wheelBase = 0.25;
            motorL.move(speed - angle * wheelBase / 2);
            motorR.move(speed + angle * wheelBase / 2);
        }
    } else {
        motorL.move(0); motorR.move(0);
    }
    
    delay(50);
}

2、时间轴同步编队切换 + 平滑插值（多机协同作业）
适用场景：多台协作机器人在流水线上按时间轴节拍协同运动，需要高精度相位同步；操作员可通过串口指令实时调整整体节拍快慢。
核心逻辑：主机预设时间轴任务序列（T0:初始、T1:展开、T2:合拢……），通过无线广播同步时间戳，所有从机在相同时间点上执行三次样条插值计算目标位姿，实现编队的平滑切换与节奏同步。

#include <SimpleFOC.h>
#include <nRF24L01.h>
#include <RF24.h>

// ==================== BLDC电机（多关节示例） ====================
BLDCMotor joint1(7), joint2(7), joint3(7);
BLDCDriver3PWM drv1(3,5,6,8), drv2(9,10,11,12), drv3(22,23,24,25);

// ==================== 时间轴任务结构 ====================
struct TimeTask {
    float timePoint;       // 时间轴位置（秒）
    float joint1Target;    // 关节目标角度（弧度）
    float joint2Target;
    float joint3Target;
};
TimeTask tasks[] = {
    {0.0, 0.0, 0.0, 0.0},        // 初始编队
    {2.0, 1.2, -0.8, 0.5},       // 展开
    {4.0, -0.5, 1.5, -0.3},      // 交叉
    {6.0, 0.0, 0.0, 0.0}         // 收拢
};
int taskCount = 4;
float timeScale = 1.0;           // 节拍缩放（可通过串口调整）
unsigned long taskStartTime = 0;
float currentTaskIndex = 0;      // 连续插值索引

// ==================== 无线通信同步 ====================
RF24 radio(7, 8);
unsigned long masterClock = 0;

// ==================== 三次样条插值 ====================
float interpBezier(float t, float p0, float p1, float p2, float p3) {
    float u = 1 - t;
    return u*u*u*p0 + 3*u*u*t*p1 + 3*u*t*t*p2 + t*t*t*p3;
}

void updateInterpolation(float elapsed) {
    float scaledTime = elapsed * timeScale;
    
    // 查找当前所在区间
    for (int i = 0; i < taskCount - 1; i++) {
        if (scaledTime >= tasks[i].timePoint && scaledTime < tasks[i+1].timePoint) {
            float t = (scaledTime - tasks[i].timePoint) / (tasks[i+1].timePoint - tasks[i].timePoint);
            // 使用贝塞尔插值平滑过渡（三次样条）
            float j1 = interpBezier(t, tasks[i].joint1Target, 
                                     (tasks[i].joint1Target + tasks[i+1].joint1Target)/2,
                                     (tasks[i].joint1Target + tasks[i+1].joint1Target)/2,
                                     tasks[i+1].joint1Target);
            // 同理 j2、j3
            joint1.move(j1);
            joint2.move(j2);
            joint3.move(j3);
            break;
        }
    }
}

// ==================== 串口接收节拍指令 ====================
void parseSerial() {
    if (Serial.available()) {
        char cmd = Serial.read();
        if (cmd == 'S') timeScale = 0.8;    // 慢速
        else if (cmd == 'F') timeScale = 1.2; // 快速
        else if (cmd == 'R') taskStartTime = millis(); // 重置
    }
}

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机（位置闭环模式）
    joint1.controller = MotionControlType::position;
    joint1.init(); joint1.initFOC();
    // ... joint2、joint3同理
    // 初始化nRF24L01（从机接收同步帧）
    radio.begin();
    radio.openReadingPipe(1, address);
    radio.startListening();
    taskStartTime = millis();
}

void loop() {
    // 1. 接收主控同步帧
    if (radio.available()) {
        radio.read(&masterClock, sizeof(unsigned long));
        taskStartTime = millis() - masterClock;  // 时间轴对齐
    }
    
    // 2. 关节FOC更新
    joint1.loopFOC(); joint2.loopFOC(); joint3.loopFOC();
    
    // 3. 时间轴插值更新
    float elapsed = (millis() - taskStartTime) / 1000.0;
    updateInterpolation(elapsed);
    
    // 4. 解析串口节拍指令
    parseSerial();
    
    delay(10);
}

3、避障优先级的动态跟随与任务切换（FSM状态机管理）
适用场景：工业车间中机器人跟随操作员移动，同时需在遇到行人/货架时主动避让；通过有限状态机管理“巡逻→跟随→避障→任务”多态切换。
核心逻辑：采用FSM管理四种模式——空闲待命、跟随目标、任务执行、紧急避障。避障检测具有硬优先级，触发时无条件挂起跟随/任务，执行后退/绕行后自动恢复。

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

// ==================== BLDC差速电机 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8), driverR(3,5,6,7);
Encoder encoderL(18,19,2048), encoderR(20,21,2048);

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

// ==================== 目标状态（UWB/视觉） ====================
float targetX = 0, targetY = 0;
float robotX = 0, robotY = 0;
bool targetDetected = false;

// ==================== FSM状态枚举 ====================
enum RobotMode {
    MODE_IDLE,
    MODE_FOLLOW,
    MODE_TASK,
    MODE_AVOID
};
RobotMode currentMode = MODE_IDLE;
unsigned long avoidStartTime = 0;

// ==================== 避障参数 ====================
const float AVOID_THRESHOLD = 0.35;   // 35cm触发避障

void setup() {
    Serial.begin(115200);
    // 初始化BLDC电机与FOC (略)
}

// ==================== 主循环 ====================
void loop() {
    motorL.loopFOC(); motorR.loopFOC();
    
    // 1. 多方向超声波测距
    float distF = sonarF.ping_cm() / 100.0;
    float distL = sonarL.ping_cm() / 100.0;
    float distR = sonarR.ping_cm() / 100.0;
    
    // 2. 避障硬优先级检测
    bool obstacleFront = (distF > 0 && distF < AVOID_THRESHOLD);
    bool obstacleLeft = (distL > 0 && distL < AVOID_THRESHOLD);
    bool obstacleRight = (distR > 0 && distR < AVOID_THRESHOLD);
    
    // ==================== FSM状态机 ====================
    switch(currentMode) {
        case MODE_IDLE:
            // 空闲：等待目标出现
            if (targetDetected) {
                currentMode = MODE_FOLLOW;
            } else {
                motorL.move(0); motorR.move(0);
            }
            break;
            
        case MODE_FOLLOW:
            // 避障硬中断
            if (obstacleFront || obstacleLeft || obstacleRight) {
                currentMode = MODE_AVOID;
                avoidStartTime = millis();
                break;
            }
            // 跟随逻辑：计算目标距离与角度
            if (targetDetected) {
                float dx = targetX - robotX;
                float dy = targetY - robotY;
                float dist = sqrt(dx*dx + dy*dy);
                float angle = atan2(dy, dx);
                float speed = constrain((dist - 0.8) * 0.3, 0.1, 0.7);
                float wheelBase = 0.25;
                motorL.move(speed - angle * wheelBase / 2);
                motorR.move(speed + angle * wheelBase / 2);
            } else {
                currentMode = MODE_IDLE;
            }
            break;
            
        case MODE_AVOID:
            // 紧急避障：向空旷方向移动
            if (obstacleLeft && !obstacleRight) {
                motorL.move(-0.3); motorR.move(0.3);  // 右转
            } else if (obstacleRight && !obstacleLeft) {
                motorL.move(0.3); motorR.move(-0.3);  // 左转
            } else {
                motorL.move(-0.3); motorR.move(-0.3); // 后退
            }
            // 避障持续500ms后尝试恢复
            if (millis() - avoidStartTime > 500) {
                currentMode = MODE_FOLLOW;
            }
            break;
            
        case MODE_TASK:
            // 任务模式：前往指定目标点（类似案例一）
            break;
    }
    
    delay(50);
}

要点解读
双UWB差分定位解决“只能测距、无法定向”的核心痛点：单标签UWB只能获得距离信息，无法判断目标的朝向。双标签差分定位通过计算两个标签的连线向量，直接解算出目标航向角，使跟随机器人具备“预判”能力，在转向时能提前切入跟随曲线而非机械追尾，跟随轨迹更顺滑。

任务分配的“动态加权”是实现柔性协同的关键：案例一中的任务分配算法综合考虑了任务优先级与距离成本，机器人动态选择最高得分任务。在多目标场景中，配合“选择性跟随”机制（分配独立网络ID锁定指定操作员），可有效避免多机协同时的“串台”与混乱。

时间轴同步 + 平滑插值是编队切换的“节拍器”：案例二展示了主控通过无线通信广播统一时间轴，从机以相同时间基准执行插值计算，实现编队形态的同步切换。三次样条/贝塞尔插值确保相邻任务点之间的速度/加速度连续，避免编队切换时的机械冲击。

避障逻辑必须拥有硬优先级，独立于跟随/任务：在人机协同场景中，安全高于任务。当超声波/激光雷达检测到障碍物进入安全阈值时（如<35cm），必须无条件中断跟随/任务，执行紧急制动或避让。这一判断应在底层实时响应，不依赖高层调度，案例三的FSM即是此逻辑的实现。

BLDC FOC是实现“丝滑协同”的物理执行保障：人机协同要求机器人起步、停止和转向极其柔顺。FOC控制可使BLDC在低速下平稳输出大扭矩，响应定位更新带来的速度指令变化时不产生冲击。结合卡尔曼滤波平滑UWB定位跳变，形成从“感知平滑→决策平滑→执行平滑”的完整链路，避免因底盘响应生硬导致的编队震荡或追尾。

4、工业装配场景——人机协同动态跟随+精准装配任务切换
适用场景：电子元件精密装配线，操作员手持示教终端（或佩戴定位模块）移动，机器人实时跟随，到达工位后自动切换至“装配模式”，完成元件抓取、对位、压装，离开工位后自动切回跟随。
核心逻辑：
人机跟随：采用UWB/激光雷达融合定位，计算操作员与机器人的相对距离/角度，通过PID算法生成BLDC差速底盘的跟随速度；
任务切换：设定工位触发区域，进入区域后，通信模块（如串口）接收操作员下发的装配指令，切换控制模式；
协同执行：跟随模式下从动电机保持随动，装配模式下关节电机执行预设轨迹，任务完成后自动返回跟随。

#include <SimpleFOC.h>
#include <SoftwareSerial.h>

// 硬件配置：BLDC差速底盘 + 2关节BLDC + UWB通信
BLDCMotor driveMotorL(7), driveMotorR(8);       // 差速驱动电机
BLDCMotor joint1(9), joint2(10);                // 装配关节电机
BLDCDriver3PWM drvL(3,5,6), drvR(11,12,13);
BLDCDriver3PWM drvJoint1(14,15,16), drvJoint2(17,18,19);
Encoder encL(20,21,2048), encR(22,23,2048);
Encoder encJoint1(24,25,2048), encJoint2(26,27,2048);

SoftwareSerial uwbSerial(2,3);  // UWB模块通信（接收操作员位置数据）

// 任务状态机：0=跟随，1=装配
int taskState = 0;
// 跟随参数
float desiredDistance = 0.5;  // 期望跟随距离（m）
float Kp_distance = 2.0, Ki_distance = 0.5, Kd_distance = 0.1;
float distError = 0, distErrorSum = 0, prevDistError = 0;
// 工位触发范围
const float workstationRadius = 0.3;
float operatorPos[2] = {0,0}, robotPos[2] = {0,0};

void setup() {
  Serial.begin(115200);
  uwbSerial.begin(9600);
  
  // 初始化驱动电机（速度控制）
  driveMotorL.linkDriver(&drvL); driveMotorR.linkDriver(&drvR);
  driveMotorL.linkSensor(&encL); driveMotorR.linkSensor(&encR);
  driveMotorL.controller = MotionControlType::velocity;
  driveMotorR.controller = MotionControlType::velocity;
  driveMotorL.init(); driveMotorL.initFOC();
  driveMotorR.init(); driveMotorR.initFOC();
  
  // 初始化关节电机（位置控制）
  joint1.linkDriver(&drvJoint1); joint2.linkDriver(&drvJoint2);
  joint1.linkSensor(&encJoint1); joint2.linkSensor(&encJoint2);
  joint1.controller = MotionControlType::position;
  joint2.controller = MotionControlType::position;
  joint1.init(); joint1.initFOC();
  joint2.init(); joint2.initFOC();
}

void loop() {
  // 1. UWB获取人机位置（简化为串口接收，实际为UWB数据帧）
  if (uwbSerial.available()) {
    // 假设UWB数据格式："P,x,y"（操作员位置），"R,x,y"（机器人位置）
    String data = uwbSerial.readStringUntil('\n');
    parseUwbData(data, operatorPos, robotPos);
  }
  
  // 2. 状态机判断：距离<阈值→进入装配，否则跟随
  float distance = sqrt(pow(operatorPos[0]-robotPos[0],2) + pow(operatorPos[1]-robotPos[1],2));
  if (distance < workstationRadius && taskState == 0) {
    taskState = 1;  // 切换到装配模式
    Serial.println("Task: Assembly mode activated");
  } else if (distance >= workstationRadius + 0.1 && taskState == 1) {
    taskState = 0;  // 切换回跟随模式
    joint1.targetPos = 0;  // 关节回零
    joint2.targetPos = 0;
    Serial.println("Task: Follow mode resumed");
  }
  
  // 3. 状态执行
  if (taskState == 0) {
    // 跟随控制：PID计算目标速度
    distError = desiredDistance - distance;
    distErrorSum += distError;
    float distDiff = distError - prevDistError;
    float linearSpeed = Kp_distance*distError + Ki_distance*distErrorSum + Kd_distance*distDiff;
    linearSpeed = constrain(linearSpeed, 0.1, 0.5);  // 速度限幅
    
    // 差速底盘执行（默认直线跟随，可扩展角度控制）
    driveMotorL.move(linearSpeed);
    driveMotorR.move(linearSpeed);
  } else if (taskState == 1) {
    // 装配任务：执行预设关节轨迹
    executeAssemblyTask();
    // 装配完成后保持静止，等待离开工位
    driveMotorL.move(0);
    driveMotorR.move(0);
  }
  
  driveMotorL.loopFOC();
  driveMotorR.loopFOC();
  joint1.loopFOC();
  joint2.loopFOC();
  
  prevDistError = distError;
  delay(20);  // 50Hz控制周期
}

// 简化UWB数据解析（实际需按模块协议解析）
void parseUwbData(String data, float* destPos) {
  int comma1 = data.indexOf(',');
  int comma2 = data.indexOf(',', comma1+1);
  if (comma1 != -1 && comma2 != -1) {
    destPos[0] = data.substring(comma1+1, comma2).toFloat();
    destPos[1] = data.substring(comma2+1).toFloat();
  }
}

// 预设装配轨迹（简化为关节位置控制）
void executeAssemblyTask() {
  // 关节1转动30°，关节2转动15°，执行压装
  joint1.targetPos = 30 * (PI/180) * 2048;  // 映射到编码器脉冲
  joint2.targetPos = 15 * (PI/180) * 2048;
  // 等待轨迹完成（实际应检测位置偏差，此处简化）
  static unsigned long startTime = 0;
  if (startTime == 0) startTime = millis();
  if (millis() - startTime > 2000) {  // 2秒后复位
    startTime = 0;
    joint1.targetPos = 0;
    joint2.targetPos = 0;
  }
}

5、仓储协作场景——人机动态跟随+多任务按需切换
适用场景：电商仓储拣选，操作员带手持扫码枪移动，机器人跟随操作员，操作员扫描货架后，机器人自动切换任务（如取货、递货、搬运），任务完成后自动找回操作员继续跟随。
核心逻辑：
跟随触发：通过红外/蓝牙融合定位检测操作员，动态调整跟随速度（人快则快，人慢则慢）；
任务切换：接收操作员扫码后发送的任务类型指令（如“取货A123”“递货B456”），切换至对应任务执行逻辑；
任务优先级：急单任务（如易碎品）可打断普通任务，执行完成后自动切回原任务。
参考代码（Arduino ESP32 + SimpleFOC + 蓝牙模块）：

#include <SimpleFOC.h>
#include <BluetoothSerial.h>

BluetoothSerial btSerial;
// BLDC差速底盘
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19,2048), encR(20,21,2048);

// 任务状态：0=跟随，1=取货，2=递货，3=急单
int taskState = 0;
// 蓝牙指令：如"TASK:1,A123"（取货A123）
String taskCommand = "";
// 操作员距离（红外检测，简化为蓝牙信号强度RSSI估算）
int rssiThreshold = -70;  // RSSI越强，距离越近
int currentRssi = 0;

void setup() {
  Serial.begin(115200);
  btSerial.begin("CollaborativeRobot");  // 蓝牙设备名
  
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  // 1. 蓝牙接收指令与RSSI
  if (btSerial.available()) {
    taskCommand = btSerial.readStringUntil('\n');
    Serial.println("Received: " + taskCommand);
    
    // 解析RSSI（假设蓝牙模块返回RSSI数据，此处简化为手动赋值）
    // 实际可通过BT解析指令附带的RSSI
    currentRssi = -65;  // 模拟近距离
  }
  
  // 2. 任务切换逻辑
  if (taskCommand.indexOf("TASK:1") != -1) {
    if (taskState != 1) {
      taskState = 1;
      Serial.println("Task switched: Pick goods");
      // 保存原状态，以便任务完成后切回
    }
  } else if (taskCommand.indexOf("TASK:2") != -1) {
    if (taskState != 2) {
      taskState = 2;
      Serial.println("Task switched: Deliver goods");
    }
  } else if (taskCommand.indexOf("URGENT") != -1) {
    // 急单优先，打断当前任务
    taskState = 3;
    Serial.println("Task switched: Urgent task");
  }
  
  // 3. 状态执行
  switch(taskState) {
    case 0:
      // 跟随模式：基于RSSI调整速度（RSSI越强，距离越近，速度越慢）
      float speed = map(currentRssi, -100, -30, 0.3, 0.05);
      speed = constrain(speed, 0.05, 0.3);
      motorL.move(speed);
      motorR.move(speed);
      Serial.println("Mode: Follow, Speed: " + String(speed));
      break;
      
    case 1:
      // 取货任务：移动到货架A123（简化为直线移动）
      executePickTask(taskCommand.substring(taskCommand.indexOf(',')+1));
      break;
      
    case 2:
      // 递货任务：移动到操作员位置（等待接收）
      executeDeliverTask();
      break;
      
    case 3:
      // 急单任务：高速移动至目标
      motorL.move(0.5);
      motorR.move(0.5);
      // 执行急单逻辑...
      // 完成后切回原任务
      taskState = 0;  // 简化处理，实际需保存原状态
      break;
  }
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(30);
}

// 取货任务执行（简化：移动至货架坐标）
void executePickTask(String shelfId) {
  // 解析货架坐标（假设预设坐标表）
  float shelfX = 1.2, shelfY = 0.8;  // 模拟坐标
  // 此处省略路径规划，简化为直线移动
  motorL.move(0.3);
  motorR.move(0.3);
  
  // 到达后停止，等待操作员确认（简化为延时）
  static unsigned long startTime = 0;
  if (startTime == 0) startTime = millis();
  if (millis() - startTime > 3000) {
    startTime = 0;
    taskState = 0;  // 完成后切回跟随
    Serial.println("Pick task completed, back to follow");
  }
}

// 递货任务执行
void executeDeliverTask() {
  // 移动至操作员位置（基于当前RSSI判断到达）
  if (currentRssi >= -50) {  // 靠近操作员
    motorL.move(0);
    motorR.move(0);
    taskState = 0;  // 交付完成，切回跟随
    Serial.println("Deliver task completed, back to follow");
  } else {
    // 向操作员靠近
    motorL.move(0.2);
    motorR.move(0.2);
  }
}

6、户外巡检场景——人机动态跟随+环境监测任务切换
适用场景：野外电力巡检，操作员徒步巡检，机器人跟随并实时采集环境数据（温湿度、烟雾、气体），操作员发现异常（如设备故障）时，机器人自动切换至“异常核查任务”，完成故障点定位、拍照、数据回传。
核心逻辑：
跟随模式：采用视觉+惯性导航融合，视觉识别操作员轮廓，IMU补偿姿态，生成跟随轨迹；
任务切换：通过WiFi/LoRa通信接收操作员的异常指令，或机器人自主检测到环境阈值超标（如烟雾浓度>阈值），自动触发任务切换；
协同执行：跟随模式下同步采集常规数据，任务模式下聚焦异常核查，完成后自动返回跟随。
参考代码（Arduino ESP32 + SimpleFOC + OV2640摄像头简化逻辑+LoRa模块）：

#include <SimpleFOC.h>
#include <LoRa.h>

// 硬件：BLDC差速底盘 + LoRa通信 + IMU（简化为代码逻辑）
BLDCMotor motorL(7), motorR(8);
BLDCDriver3PWM drvL(3,5,6), drvR(9,10,11);
Encoder encL(18,19,2048), encR(20,21,2048);

LoRaClass LoRa;

// 任务状态：0=跟随+常规监测，1=异常核查
int taskState = 0;
// 环境阈值（模拟）
float smokeThreshold = 300;  // 烟雾浓度阈值
float currentSmoke = 0;
// 操作员识别：视觉简化为布尔标志
bool operatorDetected = true;
// 异常指令（LoRa接收）
String alertCommand = "";

void setup() {
  Serial.begin(115200);
  // LoRa初始化（简化，实际需配置频率、扩频因子）
  // LoRa.begin(868E6);
  
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.linkSensor(&encL); motorR.linkSensor(&encR);
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();
  motorR.init(); motorR.initFOC();
}

void loop() {
  // 1. LoRa接收指令与环境数据模拟
  if (LoRa.available()) {
    alertCommand = LoRa.readStringUntil('\n');
    if (alertCommand.indexOf("ALERT") != -1) {
      taskState = 1;
      Serial.println("Task switched: Abnormality verification");
    }
  }
  
  // 模拟环境传感器数据（实际从传感器读取）
  currentSmoke = random(200, 400);
  
  // 2. 自主触发任务：烟雾超标
  if (currentSmoke > smokeThreshold && taskState == 0) {
    taskState = 1;
    Serial.println("Task switched: Smoke anomaly detected");
  }
  
  // 3. 状态执行
  if (taskState == 0) {
    // 跟随+常规监测：视觉识别操作员则跟随
    if (operatorDetected) {
      float followSpeed = 0.2;  // 基于视觉的跟随速度（简化）
      motorL.move(followSpeed);
      motorR.move(followSpeed);
      // 常规数据采集（串口输出模拟）
      Serial.print("Data: Smoke="); Serial.print(currentSmoke);
      Serial.print(", Temp="); Serial.print(random(20,30));
      Serial.print(", Humidity="); Serial.println(random(40,60));
    } else {
      // 丢失操作员，原地等待
      motorL.move(0);
      motorR.move(0);
    }
  } else if (taskState == 1) {
    // 异常核查任务：定位异常点（简化为移动至预设坐标）
    executeAlertTask();
    // 任务完成后切回跟随
    if (alertTaskCompleted) {
      taskState = 0;
      alertTaskCompleted = false;
      Serial.println("Alert task completed, back to follow");
    }
  }
  
  motorL.loopFOC();
  motorR.loopFOC();
  delay(50);
}

bool alertTaskCompleted = false;

// 异常核查任务执行
void executeAlertTask() {
  // 简化：向异常点移动（预设坐标）
  motorL.move(0.3);
  motorR.move(0.3);
  
  // 到达后执行核查（拍照、数据回传）
  static unsigned long startTime = 0;
  if (startTime == 0) startTime = millis();
  if (millis() - startTime > 5000) {  // 5秒核查
    startTime = 0;
    alertTaskCompleted = true;
    Serial.println("Alert verification completed: Photo captured, data sent");
  }
}

要点解读

1. 人机跟随核心算法：从定位感知到动态速度适配，筑牢协同基础
   人机协同的核心是精准、自适应的动态跟随，需结合场景需求选择定位方式，并通过算法实现速度与轨迹的动态匹配，避免跟丢或碰撞：
   定位技术适配场景：
   工业固定场景：优先选UWB精准定位（厘米级精度），适合工位触发类任务，如案例1的装配场景；
   仓储移动场景：选蓝牙/红外融合，成本低且适配手持终端，如案例2的仓储跟随；
   户外复杂场景：选视觉+IMU融合，抗干扰性强，可识别操作员轮廓，适合户外巡检，如案例3；
   速度自适应算法：避免匀速跟随，需根据人机相对距离动态调整速度（距离远则加速，距离近则减速），常用PID、模糊控制等算法；如案例1用PID控制距离误差，案例5用RSSI映射速度，可保证跟随的柔顺性，避免急停急启；
   跟随轨迹平滑：需引入轨迹规划算法（如贝塞尔曲线、梯形速度曲线），限制加加速度，避免机器人急转弯或急加速，提升人机协作的安全性和舒适性。
2. 任务切换触发机制：多触发源闭环设计，确保切换及时性与可靠性
   任务切换是人机协同的核心决策环节，需设计多触发源、分级优先级的触发机制，避免触发延迟或误触发：
   多触发源冗余：触发方式需覆盖人工指令、环境感知、任务状态三类：
   人工指令：通过蓝牙、LoRa等通信模块下发，如案例2的扫码触发、案例3的人工异常指令；
   环境感知：机器人自主检测环境阈值，如案例3的烟雾超标触发，无需人工干预；
   任务状态：任务完成自动切换，如装配完成后切回跟随；
   优先级与冲突处理：建立任务优先级机制，避免任务冲突导致系统卡顿：急单任务（如危化品泄漏）优先级最高，可打断普通任务；普通任务完成后自动切回默认跟随状态，如案例5中急单可打断取货任务；
   防误触发设计：触发逻辑需加入去抖动和阈值判断，如距离触发需连续满足阈值一定时间（而非单次判断），通信指令需校验格式，避免干扰信号导致误触发，确保切换可靠性。
3. BLDC驱动与控制适配：从底层响应到动态限幅，保障动作精准性
   BLDC是协作机器人的动力核心，需针对跟随与任务切换的动态需求，优化底层控制策略，确保动作响应快、精度高、安全性强：
   控制模式适配场景：
   跟随场景：优先采用速度控制模式，快速响应速度指令变化，满足动态跟随需求，如案例4、5的差速底盘；
   任务执行场景：采用位置/力矩控制模式，确保执行精度，如案例1的装配关节用位置控制，案例3的异常核查用位置定位；
   动态速度限幅：跟随模式下，需根据环境动态调整速度上限：狭窄空间降低速度，开阔空间提高速度；同时设置软限幅，避免速度突变导致机械冲击，提升人机协作的安全性；
   闭环控制优化：采用双闭环FOC控制（电流环+速度/位置环），确保电机转矩平稳、响应迅速；通过编码器实时反馈位置，校正跟踪误差，如案例中均启用loopFOC()实现闭环控制，避免丢步或超调，保障动作精准性。
4. 人机交互与安全协同：通信透明化+安全机制，保障协作零风险
   人机协同的核心是安全与协同，需从通信和安全两方面设计，确保人机信息互通且风险可控：
   轻量化通信机制：协作机器人需实时与操作员交换状态与指令，通信需兼顾实时性与轻量化：工业场景用串口/RS485（稳定），移动场景用蓝牙/WiFi（便捷），户外远距离用LoRa（低功耗远距离），如案例5用蓝牙、案例6用LoRa；通信数据格式需简洁（如指令+参数），避免协议冗余导致延迟；
   双向状态反馈：机器人需向操作员反馈实时状态（当前任务、位置、故障），操作员需向机器人下发指令（任务触发、急停），形成闭环反馈：如案例5中机器人通过串口输出任务状态，案例6通过LoRa接收人工指令；
   三级安全防护：协作机器人需建立“物理+软件+逻辑”三级安全机制：
   物理防护：设置机械限位、急停按钮，防止硬件损坏；
   软件限幅：设置速度、力矩上限，避免超载或碰撞；
   逻辑互锁：任务切换时加入状态锁，防止多任务同时执行导致逻辑冲突；
   碰撞检测：通过距离感知（如超声波、激光）实时检测人机距离，超近距离自动急停，案例中虽未完整实现，但可通过代码扩展超声波检测实现。
5. 工程落地适配：参数整定与场景优化，破解“实验室到现场”难题
   协作机器人从代码到落地，需解决环境适配与参数优化问题，核心是通过闭环调试让系统适应实际场景：
   参数整定顺序：遵循“从内环到外环、从单机到协同”的顺序：
   先整定BLDC底层参数：FOC电流环PID、编码器校准，确保电机运行平稳；
   再整定跟随参数：PID增益、速度限幅，确保跟随稳定无振荡；
   最后整定任务切换参数：触发阈值、优先级，确保切换可靠无卡顿；
   场景化适配优化：不同场景的参数差异极大，需针对性调整：
   工业装配：降低跟随速度、提高位置控制精度，避免碰撞精密零件；
   仓储跟随：提高跟随速度、增加抗干扰能力，适应密集货架环境；
   户外巡检：提升环境感知灵敏度、强化定位抗干扰，适应野外复杂地形；
   故障自恢复机制：现场运行需具备故障自恢复能力：如丢失操作员信号时，自动切换至“原地等待+自主巡检”模式；通信中断时，自动执行预设安全动作，避免失控；同时建立故障日志，方便快速定位问题，降低现场维护成本。

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