该方案的核心特点是利用多模态传感器融合与预测算法解决动态环境下的目标锁定与防丢失问题，结合BLDC电机配合FOC算法实现的高动态响应与平滑差速控制，使机器人能够在人流密集场景中实现柔顺避障与精准跟随；主要适用于大型商超、交通枢纽、仓储物流等场景；实际部署需重点解决Arduino算力瓶颈、高频控制环实时性、多径效应干扰及人机安全冗余等问题。
一、主要特点
多模态融合感知与目标锁定——从“单一追踪”到“抗遮挡防丢失”
在人流密集的场景中，目标（如顾客、拣货员）随时可能被其他行人或货架短暂遮挡，单一传感器极易失效。
多源异构融合：系统通常采用UWB（超宽带）进行厘米级相对定位，结合激光雷达（LiDAR）进行环境扫描与点云聚类，甚至融合视觉（如YOLO人体骨骼识别）进行身份绑定。通过扩展卡尔曼滤波（EKF）进行紧耦合，确保在目标被遮挡时，系统仍能依靠惯性航位推算维持跟随连续性。
预测算法补偿：引入卡尔曼滤波等预测算法，根据目标的历史运动轨迹预测其短时位置。当目标暂时丢失时，系统能进行平滑过渡而非立即停止，极大降低了“跟丢”或“跟错人”的概率。
360°安全气泡：集成超声波、红外或激光雷达阵列，构建机器人的“安全气泡”，实时探测前后左右的动态障碍物，确保在拥挤环境中也能安全穿行。
BLDC+FOC柔顺驱动——从“刚性执行”到“平滑差速”
人流密集场景要求机器人在频繁启停、急转弯时保持极高的平稳性，避免惊吓行人或碰撞商品。
高动态响应：BLDC电机的电磁时间常数极小，配合FOC（磁场定向控制）算法的高频电流环（通常>1kHz），能够毫秒级响应上层规划发出的加减速指令。通过差速驱动架构，机器人可以实现原地转向、急停和快速倒车等高难度动作。
平滑差速控制：结合FOC算法，BLDC电机可以实现精细的力矩闭环控制。通过Clark和Park变换，将三相交流电流解耦为励磁电流（d轴）和转矩电流（q轴），实现转矩脉动<1%的平稳输出。这使得机器人在低速和高速下都能平稳运行，避免传统有刷电机因碳刷摩擦产生的高频啸叫与电火花，运行噪音可控制在环境底噪之下。
高效稳定运行：相较于有刷电机，BLDC电机效率高（能量转换效率达85%-90%）、发热低、寿命长，能够保证机器人在长时间、频繁启停的跟随任务中稳定运行，不易因过热而降额。
路径优化与动态避障——从“盲目跟随”到“智能绕行”
在狭窄的货架通道或人流交汇处，机器人不仅要跟随目标，还要主动规划最优路径。
局部路径规划：采用动态窗口法（DWA）或人工势场法，结合实时感知到的障碍物信息，在跟随目标的同时动态调整运动轨迹，实现平滑绕行。
自适应速度控制：根据与目标的距离和周围环境的拥挤程度动态调整运动速度。当环境开阔时，机器人以较高速度趋近；当接近人群或狭窄通道时，自动切换到谨慎慢速模式，确保安全性。
里程计与位姿估计：基于BLDC电机的编码器反馈信息，实时计算机器人的位姿（位置X, Y和航向角θ），结合IMU（惯性测量单元）补偿滑移，确保路径跟踪的精准性。
分层控制架构
规划层（多模态感知+路径规划）：负责目标识别、环境感知与动态轨迹生成。
控制层（FOC+差速控制）：负责将轨迹转化为关节力矩指令，并处理力交互与协同同步。
感知层（UWB+LiDAR+IMU+视觉）：负责提供目标位置、环境几何信息与姿态反馈。
二、典型应用场景
大型商超与仓储式会员店
在面积广阔、货架密集、人流量大的购物环境中，解决顾客推车不便的痛点。
智能购物车：机器人自动跟随顾客，顾客可轻松穿梭于货架之间，专注于商品选择。BLDC的静音特性不会打扰顾客购物。
防碰撞与绕行：在狭窄的货架通道内，机器人能灵活掉头和跟随，避免碰撞商品或行人。
智能仓储与物流人机协同
在仓储分拣或装配产线中，作为“跟随式搬运AGV”自动尾随拣货工人。
高效协同：IMU的辅助确保了AGV在频繁启停、转弯或经过减速带时，能精准保持与工人的安全距离，大幅提升物流流转效率。
抗干扰能力：UWB定位抗干扰能力强，不受光照和人群遮挡影响，适合复杂的仓库环境。
机场与大型交通枢纽
在旅客携带大量行李，需要长距离移动的场景中，作为智能行李车提供辅助。
长距离跟随：自动跟随旅客从值机口到登机口，或在免税店购物时提供辅助，减轻旅客负担。
动态适应：结合视觉的语义识别能力与IMU的姿态补偿，机器人能在人群密集、光照剧烈变化或存在坡坎的环境中，提供平稳、不跟丢的交互体验。
科研与教育验证
作为高校机器人学、嵌入式控制课程的实验平台，用于验证多传感器融合、FOC驱动、动态避障等前沿技术。
三、需要注意的事项
Arduino算力瓶颈与架构分工
多模态融合感知、路径规划、FOC算法同时运行对算力要求极高。
平台选型：经典8位Arduino（如Uno）几乎无法胜任。必须选用ESP32-S3（双核240MHz）、STM32H7或Arduino Portenta H7等高性能平台，或采用“上位机+下位机”架构。
主从架构：建议采用“上位机+下位机”架构。上位机（如树莓派、Jetson或PC）负责多模态感知、路径规划和轨迹生成；下位机（如专用FOC驱动板）负责高频电流环控制和电机换相。
控制频率与实时性
人流密集场景的动态跟随要求极高的实时性，尤其是力矩同步与避障响应。
高频控制环：FOC的电流环频率建议≥1kHz，速度环和位置环≥500Hz。路径规划的更新频率应与控制环匹配（如100Hz~500Hz），避免力矩指令滞后。
通信延迟：上位机与下位机之间的通信（如CAN、EtherCAT）延迟必须控制在毫秒级，否则会导致力矩指令滞后，引发机身抖动或碰撞。
多径效应与定位精度
UWB在金属密集或复杂工业环境中容易受多径效应影响，导致测距误差。
多源融合：建议融合IMU（惯性测量单元）与轮式里程计，通过扩展卡尔曼滤波（EKF）进行紧耦合融合，在UWB信号短暂丢失或跳变时依靠惯性航位推算维持定位连续性。
基站部署：UWB基站应避开金属遮挡物，并采用非共线布置（至少3个基站），以优化几何稀释精度（GDOP）。
人机安全与冗余设计
在人流密集场景中，安全是首要考量。
多传感器冗余：必须采用多传感器冗余融合方案。例如，激光雷达负责远距离路径规划，超声波和红外负责近距离紧急避障，同时配备物理急停按钮和声光报警器，确保任何情况下都能优先保障人身安全。
碰撞检测：需在算法中加入实时碰撞检测机制，结合力矩传感器或电流反馈，当检测到异常阻力时立即触发紧急停止。
电机力矩响应与一致性
跟随效果高度依赖电机的力矩响应性能和一致性。
电机选型：选用低齿槽效应、高扭矩密度的BLDC电机，确保低速下的力矩平稳性。
参数一致性：同一机器人上的所有电机参数（极对数、内阻、电感）必须一致，否则会导致各关节力矩输出不均，影响跟随精度。
电磁兼容（EMC）与电源管理
多电机高频PWM驱动会产生严重的电磁干扰，影响传感器精度。
电源隔离：动力电源与逻辑电源必须物理隔离，使用独立DC-DC模块。
滤波设计：电源入口并联大容量电解电容和高频陶瓷电容，吸收电压尖峰。
布线规范：强电与弱电严格分开，传感器信号线使用屏蔽线。

1、双UWB差分定位 + 基础动态跟随
此案例聚焦感知层与执行层的闭环。通过双UWB标签差分定位获取主机器人的位置与航向，驱动从机器人进行顺滑的动态跟随，解决了单标签“只能测距、无法定向”的核心痛点。

#include <SimpleFOC.h>
#include <DW1000.h> // UWB库，需适配具体模块

// 假设已定义主从机器人BLDC差速底盘电机
BLDCMotor motorL = BLDCMotor(7);
BLDCMotor motorR = BLDCMotor(7);
BLDCDriver3PWM drvL = BLDCDriver3PWM(9, 10, 11);
BLDCDriver3PWM drvR = BLDCDriver3PWM(5, 6, 8);

// UWB标签ID定义
const uint8_t TAG_LEADER_1 = 0x01; // 主机器人前置标签
const uint8_t TAG_LEADER_2 = 0x02; // 主机器人后置标签
const uint8_t TAG_FOLLOWER = 0x03; // 从机器人标签

// 双标签差分定位与跟随参数
float leaderPos[2] = {0, 0};    // 主机器人位置 (x, y)
float leaderYaw = 0;            // 主机器人航向角 (rad)
float followerPos[2] = {0, 0};  // 从机器人当前位置
float followOffset[2] = {-0.5, 0.3}; // 期望跟随偏移 (左后方0.5m, 侧方0.3m)
float followError[2] = {0, 0};
float maxSpeed = 1.5; // 最大线速度 (m/s)

void setup() {
  Serial.begin(115200);
  // 初始化UWB模块 (具体初始化代码依库而定)
  DW1000.begin();
  
  // 初始化BLDC电机及FOC
  drvL.init(); drvR.init();
  motorL.linkDriver(&drvL); motorR.linkDriver(&drvR);
  motorL.init(); motorR.init();
  motorL.initFOC(); motorR.initFOC();
  motorL.controller = MotionControlType::velocity;
  motorR.controller = MotionControlType::velocity;
}

void loop() {
  // 1. 双标签差分定位：获取主机器人位置与航向
  float pos1[2], pos2[2];
  if (DW1000.getPosition(TAG_LEADER_1, pos1) && DW1000.getPosition(TAG_LEADER_2, pos2)) {
    // 主机器人位置取两标签中点
    leaderPos[0] = (pos1[0] + pos2[0]) / 2.0;
    leaderPos[1] = (pos1[1] + pos2[1]) / 2.0;
    // 解算航向角 (两标签连线与X轴夹角)
    leaderYaw = atan2(pos2[1] - pos1[1], pos2[0] - pos1[0]);
  }
  
  // 2. 获取从机器人自身位置
  DW1000.getPosition(TAG_FOLLOWER, followerPos);

  // 3. 计算带航向补偿的跟随目标 (纯追踪算法思想)
  // 目标位置 = 主机器人位置 + 航向旋转后的跟随偏移
  float targetPos[2];
  targetPos[0] = leaderPos[0] + followOffset[0] * cos(leaderYaw) - followOffset[1] * sin(leaderYaw);
  targetPos[1] = leaderPos[1] + followOffset[0] * sin(leaderYaw) + followOffset[1] * cos(leaderYaw);

  // 4. 计算跟随误差
  followError[0] = targetPos[0] - followerPos[0];
  followError[1] = targetPos[1] - followerPos[1];
  float distError = sqrt(followError[0]*followError[0] + followError[1]*followError[1]);

  // 5. 速度控制 (比例控制 + 航向修正)
  float speedMag = constrain(distError * 0.8, 0, maxSpeed);
  float targetAngle = atan2(followError[1], followError[0]);
  
  // 差速底盘运动学分解 (v线速度, w角速度)
  float v = speedMag * cos(targetAngle - leaderYaw); // 沿主机器人航向分解
  float w = 0.5 * atan2(sin(targetAngle - leaderYaw), cos(targetAngle - leaderYaw)); // 转向
  
  // 6. 驱动BLDC电机 (速度模式)
  float leftV = (v - w * 0.3); // 0.3为轮距一半
  float rightV = (v + w * 0.3);
  motorL.target = constrain(leftV, -maxSpeed, maxSpeed);
  motorR.target = constrain(rightV, -maxSpeed, maxSpeed);
  motorL.move(motorL.target);
  motorR.move(motorR.target);
  motorL.loopFOC();
  motorR.loopFOC();

  delay(50); // 20Hz控制周期
}

要点：通过双标签的基线向量解算航向，从机器人可以“预判”主机器人转向趋势，实现顺滑曲线跟随而非机械追尾。

2、动态跟随 + 协同任务分配
此案例在跟随基础上引入决策层。通过UWB识别多个目标并进行动态任务分配，当新任务产生时，系统根据距离与负载状态，智能指派最优机器人执行。

#include <SimpleFOC.h>
#include <DW1000.h>

// ... (BLDC电机及UWB初始化同案例一) ...

// 任务结构体
struct Task {
  uint8_t targetID;   // UWB标签ID (代表目标点或工人)
  float pos[2];       // 目标位置 (可由UWB实时获取)
  int priority;       // 任务优先级 (0最高)
  bool assigned;      // 是否已分配
};

// 机器人状态结构体
struct RobotState {
  uint8_t id;
  float pos[2];
  float battery;      // 电量 (0~1)
  bool isBusy;        // 是否忙碌
  Task currentTask;
};

RobotState robot1, robot2; // 双机器人状态
Task taskQueue[5];    // 任务队列 (最多5个)

void setup() {
  // ... (同上) ...
  // 初始化各机器人状态
  robot1.id = 1; robot1.isBusy = false;
  robot2.id = 2; robot2.isBusy = false;
}

void loop() {
  // 1. 更新双机器人位置 (UWB)
  DW1000.getPosition(TAG_ROBOT1, robot1.pos);
  DW1000.getPosition(TAG_ROBOT2, robot2.pos);

  // 2. 扫描任务队列，进行动态分配
  for (int i = 0; i < 5; i++) {
    if (!taskQueue[i].assigned) {
      // 获取目标位置 (假设目标也佩戴UWB标签)
      DW1000.getPosition(taskQueue[i].targetID, taskQueue[i].pos);
      
      // 计算各机器人与目标距离
      float dist1 = calcDistance(robot1.pos, taskQueue[i].pos);
      float dist2 = calcDistance(robot2.pos, taskQueue[i].pos);
      
      // 评分函数：综合考虑距离与电量 (距离近、电量高者优先)
      float score1 = 0.6 * (1 - dist1/10.0) + 0.4 * robot1.battery; // 假设最大距离10m
      float score2 = 0.6 * (1 - dist2/10.0) + 0.4 * robot2.battery;
      
      // 选择得分更高且空闲的机器人分配任务
      if (score1 > score2 && !robot1.isBusy) {
        assignTask(&robot1, &taskQueue[i]);
      } else if (!robot2.isBusy) {
        assignTask(&robot2, &taskQueue[i]);
      }
    }
  }

  // 3. 执行各自任务 (跟随或前往目标)
  if (robot1.isBusy) {
    followTarget(robot1.pos, robot1.currentTask.pos); // 调用案例一的跟随逻辑
  }
  if (robot2.isBusy) {
    followTarget(robot2.pos, robot2.currentTask.pos);
  }

  delay(50);
}

// 任务分配函数
void assignTask(RobotState* robot, Task* task) {
  robot->currentTask = *task;
  robot->isBusy = true;
  task->assigned = true;
  Serial.print("Robot "); Serial.print(robot->id);
  Serial.println(" assigned to task.");
}
要点：任务分配的核心是设计合理的评分函数，综合考虑距离、电量和优先级等维度动态决策，实现“人尽其才”的柔性调度。

案例三：通道约束自适应编队压缩
此案例引入环境感知层，解决在狭窄通道中保持队形的自适应问题。当检测到通道变窄，自动压缩双机器人编队间距并微调航向，确保协同通过。

cpp
#include <SimpleFOC.h>
#include <DW1000.h>

// ... (BLDC电机及UWB初始化同案例一) ...

// 通道与编队参数
#define NORMAL_SPACING 1.0    // 正常编队间距 (m)
#define MIN_SPACING 0.4       // 最小压缩间距 (m)
#define CHANNEL_WIDTH 1.5     // 通道宽度阈值 (m)
#define SAFE_SIDE_DIST 0.2    // 侧方安全距离 (m)

// 红外传感器引脚 (用于检测通道宽度)
#define IR_LEFT_PIN A0
#define IR_RIGHT_PIN A1

float leaderYaw = 0;
float currentSpacing = NORMAL_SPACING;
bool inNarrowChannel = false;

void setup() {
  // ... (同上) ...
  pinMode(IR_LEFT_PIN, INPUT);
  pinMode(IR_RIGHT_PIN, INPUT);
}

void loop() {
  // 1. 检测通道宽度 (红外测距)
  float leftDist = analogRead(IR_LEFT_PIN) * 0.5;  // 简化映射
  float rightDist = analogRead(IR_RIGHT_PIN) * 0.5;
  float actualWidth = leftDist + rightDist;

  // 2. 通道约束自适应逻辑
  if (actualWidth < CHANNEL_WIDTH && actualWidth > 0) {
    inNarrowChannel = true;
    // 编队间距自适应压缩 (线性映射)
    float compressRatio = actualWidth / CHANNEL_WIDTH;
    currentSpacing = NORMAL_SPACING * compressRatio;
    currentSpacing = constrain(currentSpacing, MIN_SPACING, NORMAL_SPACING);
    
    // 微调航向角，使机器人居中通过通道
    float centerBias = (leftDist - rightDist) / actualWidth;
    float yawAdjust = constrain(centerBias * 15.0, -10.0, 10.0) * DEG_TO_RAD;
    leaderYaw += yawAdjust * 0.1; // 平滑调整
    
    // 压缩状态下减速通过
    float speedScale = 0.5 + 0.5 * compressRatio;
    // ... 将speedScale应用到电机的目标速度上 ...
    
    Serial.println("Narrow channel: spacing=" + String(currentSpacing) + ", yawAdjust=" + String(yawAdjust));
  } else {
    inNarrowChannel = false;
    currentSpacing = NORMAL_SPACING;
  }

  // 3. 将压缩后的编队间距应用到跟随算法 (参考案例一)
  // followOffset[0] 应根据 currentSpacing 动态调整
  followOffset[0] = -currentSpacing; // 跟随距离

  // 4. 执行跟随 (同案例一)
  // ... 调用followTarget() ...

  delay(50);
}

要点：通道约束自适应的本质是将环境感知（通道宽度）引入编队参数（间距、航向）的动态调节，使双机在非结构化环境中保持协同能力。

要点解读
人流密度感知是决策层的关键输入：在人流密集场景中，跟随策略需根据环境拥挤程度动态调整。可通过超声波或红外传感器的遮挡频率间接估算人流密度。密度高时降低速度并收缩编队间距，密度低时恢复正常巡航，避免在人群中引起不必要的干扰。

目标锁定需“防误触”机制：人流中目标易被瞬时遮挡，若一丢目标就停车，会严重影响任务连续性。建议采用锁定持续时间判定——目标需连续稳定满足角度阈值一定时间才判定为锁定成功，瞬时遮挡不会触发解锁。同时可配合卡尔曼滤波预测目标位置，在短暂遮挡时维持跟随状态。

避障优先级高于跟随：安全是第一原则。当检测到障碍威胁时（如前方行人突然停步），应无条件中断跟随动作，执行紧急制动或减速避让。可采用双阈值策略：距离小于临界安全距离时急停，位于安全距离与临界距离之间时减速避让，确保机器人在任何情况下都不碰撞。

航向角补偿是消除“锯齿形”跟随的关键：单纯依赖位置误差的比例控制，容易导致机器人左右摆动。引入航向角闭环校正，将目标航向与实际航向的偏差同时纳入控制量中，可使机器人以平滑弧线收敛到期望路径上。典型的控制律为：控制量 = K1 × 横向误差 + K2 × 航向误差。

主控平台选型决定系统上限：视觉SLAM、UWB定位解算、DWA局部规划等多传感器融合算法，对算力要求较高。标准Arduino Uno（16MHz，2KB RAM）几乎无法胜任。建议采用ESP32、Teensy 4.1等高性能平台，或采用“树莓派/Jetson做感知与规划 + Arduino做底层电机驱动”的异构架构，将高频控制环放在Arduino端，上层决策放在上位机。

4、超声波矩阵动态差速跟随——商场/展厅人流机器人跟随
适用场景：商场、展厅等中近距离结构化环境，机器人需跟随特定人员（如佩戴信标或通过人体特征识别），在人流中保持安全距离，同时避开随机行人、货架等障碍物。

核心逻辑：通过多路超声波传感器环形布局构建局部感知场，采用时分复用避免传感器串扰，利用加权质心法解算目标方位；控制层采用双PID串级结构——外环根据距离偏差与角度偏差规划期望线速度和角速度，内环通过FOC精确调节左右BLDC轮毂电机转速，且避障逻辑拥有最高优先级，确保安全。

/* ===== 超声波矩阵动态差速跟随（商场/展厅场景） =====
 * 硬件：Arduino ESP32 + 2×BLDC差速电机 + 5路超声波传感器（环形布局）
 * 核心：加权质心法解算目标方位 + 双PID串级控制 + 硬避障优先级
 */
#include <SimpleFOC.h>
#include <NewPing.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 SONAR_NUM 5
#define MIN_DIST 25          // 安全避障阈值 (cm)
#define TARGET_DIST 50       // 期望跟随距离 (cm)
NewPing sonar[SONAR_NUM] = {
    NewPing(2, 3, 200), NewPing(4, 5, 200), NewPing(6, 7, 200),
    NewPing(8, 9, 200), NewPing(10, 11, 200)
};

// ==================== 全局变量 ====================
float distances[SONAR_NUM];
float targetPosition = 0;    // 目标相对方位 (-1~1，负=左，正=右)

void setup() {
    Serial.begin(115200);
    // 初始化电机与FOC
    motorL.linkSensor(&encoderL); motorR.linkSensor(&encoderR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
}

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

    // 1. 时分复用超声波测距（防止串扰）
    for (int i = 0; i < SONAR_NUM; i++) {
        distances[i] = sonar[i].ping_cm();
        delay(30);  // 等待回波稳定，避免串扰
    }

    // 2. 加权质心法解算目标方位
    float weightedSum = 0, totalWeight = 0;
    for (int i = 0; i < SONAR_NUM; i++) {
        if (distances[i] > 0 && distances[i] < 200) {
            float angle = (i - 2) * 30.0 * PI / 180.0;  // 假设传感器间隔30°
            float weight = 1.0 / distances[i];  // 距离越近权重越大
            weightedSum += angle * weight;
            totalWeight += weight;
        }
    }
    targetPosition = (totalWeight > 0) ? weightedSum / totalWeight : 0;

    // 3. 硬避障逻辑（优先级高于跟随）
    float minDist = 999;
    for (int i = 0; i < SONAR_NUM; i++) {
        if (distances[i] > 0) minDist = min(minDist, distances[i]);
    }
    if (minDist < MIN_DIST) {
        // 紧急避障：向开阔方向后退
        if (targetPosition < -0.3) {
            motorL.move(-0.5); motorR.move(0.5);  // 左转后退
        } else if (targetPosition > 0.3) {
            motorL.move(0.5); motorR.move(-0.5);  // 右转后退
        } else {
            motorL.move(-0.5); motorR.move(-0.5); // 直行后退
        }
        delay(400);
        return;
    }

    // 4. 双PID串级跟随控制
    float distError = minDist - TARGET_DIST;
    float linearSpeed = constrain(distError * 0.03, 0.1, 2.0);   // 距离环→线速度
    float angularSpeed = constrain(targetPosition * 2.0, -1.0, 1.0); // 角度环→角速度

    // 近距离减速柔化（提升跟随舒适性）
    if (minDist < 35) linearSpeed *= 0.5;

    // 5. 差速驱动解算
    float wheelBase = 0.25;
    float leftSpeed = linearSpeed - angularSpeed * wheelBase / 2;
    float rightSpeed = linearSpeed + angularSpeed * wheelBase / 2;
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);

    // 调试输出
    Serial.print("MinDist:");Serial.print(minDist);
    Serial.print(" TargetPos:");Serial.print(targetPosition);
    Serial.print(" L:");Serial.print(leftSpeed);
    Serial.print(" R:");Serial.println(rightSpeed);

    delay(50);
}

5、多模态融合跟随+动态避障——工厂车间人机协同
适用场景：工厂车间、仓储物流等动态人流场景，机器人需跟随佩戴UWB标签的工人，同时应对叉车、移动货架等动态障碍物，实现“人走到哪，物料送到哪”的柔性协同。

核心逻辑：采用“UWB+IMU+轮式里程计”多模态融合定位，通过扩展卡尔曼滤波（EKF）紧耦合提升定位鲁棒性；控制架构为“外环轨迹规划+内环FOC控制”，外环根据目标位置规划线速度与角速度，内环通过BLDC电机FOC控制实现精准执行；引入滚动窗口重规划，当路径出现动态障碍物时，局部规划器基于运动学约束推演最优避障轨迹，避障结束后平滑回归跟随路径。

/* ===== 多模态融合跟随+动态避障（工厂车间场景） =====
 * 硬件：ESP32 + 2×BLDC电机 + UWB模块 + IMU + 轮式里程计
 * 核心：多模态融合定位 + 滚动窗口重规划 + FOC平滑控制
 */
#include <SimpleFOC.h>
#include <Wire.h>  // IMU通信

// ==================== 硬件初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 模拟UWB、IMU、里程计接口（实际需适配硬件驱动）
float uwbX=0, uwbY=0;      // UWB定位目标坐标
float imuYaw=0;             // IMU航向角
float odoX=0, odoY=0;       // 里程计位置

// ==================== 控制参数 ====================
const float TARGET_DIST = 0.8;  // 期望跟随距离（m）
const float MAX_SPEED = 1.2;    // 最大线速度（m/s）
const float MAX_ANG_SPEED = 1.0;// 最大角速度（rad/s）
const float OBSTACLE_SAFE_DIST = 0.5; // 动态避障安全距离（m）

// ==================== 多模态融合（简化EKF，示例逻辑） ====================
void fuseSensors(float &fusedX, float &fusedY, float &fusedYaw) {
    // 简化融合：UWB为主，IMU补偿航向，里程计补偿短时遮挡
    fusedYaw = imuYaw;  // IMU提供航向基准
    fusedX = uwbX;
    fusedY = uwbY;
    // 短时遮挡时，用里程计+航向推算位置
    if (sqrt(pow(uwbX-odoX,2)+pow(uwbY-odoY,2)) > 1.0) {
        fusedX += odoX * 0.3;
        fusedY += odoY * 0.3;
    }
}

// ==================== 滚动窗口重规划（简化DWA） ====================
void replanLocal(float &vLin, float &vAng, float obsX, float obsY) {
    // 计算机器人与障碍物的相对位置
    float dx = obsX - odoX;
    float dy = obsY - odoY;
    float obsDist = sqrt(dx*dx + dy*dy);
    float obsAngle = atan2(dy, dx);

    // 障碍物在安全距离内，减速并转向避让
    if (obsDist < OBSTACLE_SAFE_DIST) {
        vLin = map(obsDist, 0, OBSTACLE_SAFE_DIST, 0, MAX_SPEED);
        vLin = constrain(vLin, 0, MAX_SPEED);
        // 转向避开障碍物
        vAng = (obsAngle > 0 ? -1 : 1) * MAX_ANG_SPEED * 0.5;
    } else {
        // 无障碍物，恢复跟随速度
        vLin = MAX_SPEED;
        vAng = 0;
    }
}

void setup() {
    Serial.begin(115200);
    // 电机初始化
    motorL.linkSensor(&encL); motorR.linkSensor(&encR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 初始化传感器（实际需调用硬件驱动）
    // uwb.init(); imu.init();
}

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

    // 1. 多模态传感器数据融合
    float fusedX, fusedY, fusedYaw;
    fuseSensors(fusedX, fusedY, fusedYaw);

    // 2. 计算跟随误差与期望速度
    float dx = fusedX - odoX;
    float dy = fusedY - odoY;
    float distError = sqrt(dx*dx + dy*dy) - TARGET_DIST;
    float targetAngle = atan2(dy, dx);
    float angleError = targetAngle - fusedYaw;
    // 归一化角度误差
    if (angleError > PI) angleError -= 2*PI;
    if (angleError < -PI) angleError += 2*PI;

    // 3. 外环轨迹规划（PID简化）
    float vLin = constrain(distError * 0.5, 0, MAX_SPEED);
    float vAng = constrain(angleError * 1.2, -MAX_ANG_SPEED, MAX_ANG_SPEED);

    // 4. 动态避障重规划（模拟障碍物检测）
    float obsX = odoX + 1.0, obsY = odoY;  // 模拟前方障碍物
    replanLocal(vLin, vAng, obsX, obsY);

    // 5. 内环差速驱动解算
    float wheelBase = 0.25;
    float leftSpeed = vLin - vAng * wheelBase / 2;
    float rightSpeed = vLin + vAng * wheelBase / 2;
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);

    // 调试输出
    Serial.print("FusedX:");Serial.print(fusedX);
    Serial.print(" DistErr:");Serial.print(distError);
    Serial.print(" VLin:");Serial.print(vLin);
    Serial.print(" VAng:");Serial.print(vAng);
    Serial.println();

    delay(30);
}

6、园区巡逻跟随+路径优化——园区安防场景
适用场景：园区、厂区等开阔人流场景，机器人需跟随巡逻人员移动，同时应对台阶、沟壑、突发行人等复杂地形与障碍物，实现“人走车随”的平稳跟随与路径优化。

核心逻辑：采用“UWB+视觉”多模态抗遮挡跟随策略，当目标被遮挡时切换至惯性航位推算维持跟随；引入A*全局路径规划与局部动态避障结合的分层架构，全局路径优化巡逻路线，局部避障处理动态障碍物；BLDC电机配合S型加减速算法，确保频繁启停、转弯时的平稳性，结合IMU姿态补偿适应复杂地形。

/* ===== 园区巡逻跟随+路径优化 =====
 * 硬件：ESP32 + 2×BLDC电机 + UWB + 视觉模块 + IMU
 * 核心：多模态抗遮挡跟随 + A*全局路径优化 + S型加减速控制
 */
#include <SimpleFOC.h>
#include <AStar.h>  // 假设A*算法库

// ==================== 硬件初始化 ====================
BLDCMotor motorL(7), motorR(7);
BLDCDriver3PWM driverL(9,10,11,8);
BLDCDriver3PWM driverR(3,5,6,7);
Encoder encL(18,19,2048), encR(20,21,2048);

// 模拟传感器接口
float uwbPos[2] = {0,0};    // UWB目标位置
float visionPos[2] = {0,0};  // 视觉目标位置
float imuPitch=0, imuRoll=0; // IMU姿态
float odoX=0, odoY=0, odoYaw=0; // 里程计

// ==================== 路径优化参数 ====================
const float GRID_SIZE = 0.5;  // 栅格地图尺寸（m）
const int MAP_W = 20, MAP_H = 20; // 地图大小
int gridMap[MAP_W][MAP_H];    // 栅格地图（0=空闲，1=障碍）
std::vector<std::pair<int,int>> globalPath; // A*全局路径
int pathIdx = 0;

// ==================== A*全局路径规划 ====================
std::vector<std::pair<int,int>> planAStar(int sx, int sy, int ex, int ey) {
    // A*算法核心逻辑（简化，实际需完整实现）
    // 此处仅展示框架，实际需实现节点搜索、路径回溯
    std::vector<std::pair<int,int>> path;
    // 示例：直线路径（实际需规避障碍）
    for (int x = sx; x <= ex; x++) path.push_back({x, sy});
    for (int y = sy+1; y <= ey; y++) path.push_back({ex, y});
    return path;
}

// ==================== S型加减速控制 ====================
float sCurveAccel(float currentSpeed, float targetSpeed, float dt) {
    float maxAccel = 2.0; // 最大加速度
    float speedDiff = targetSpeed - currentSpeed;
    if (abs(speedDiff) < 0.01) return currentSpeed;
    float accel = (speedDiff > 0 ? maxAccel : -maxAccel);
    return currentSpeed + accel * dt;
}

// ==================== 多模态抗遮挡跟随 ====================
void antiOcclusionFollow(float &targetX, float &targetY, bool &isOccluded) {
    // 检测目标是否被遮挡（UWB与视觉位置偏差过大）
    float distDiff = sqrt(pow(uwbPos[0]-visionPos[0],2) + pow(uwbPos[1]-visionPos[1],2));
    isOccluded = (distDiff > 1.0); // 偏差超过1m判定为遮挡
    if (isOccluded) {
        // 切换至惯性航位推算（基于上次有效目标位置+航向）
        targetX = odoX + cos(odoYaw) * TARGET_DIST;
        targetY = odoY + sin(odoYaw) * TARGET_DIST;
    } else {
        // 优先使用视觉定位（更精准）
        targetX = visionPos[0];
        targetY = visionPos[1];
    }
}

void setup() {
    Serial.begin(115200);
    // 电机初始化
    motorL.linkSensor(&encL); motorR.linkSensor(&encR);
    motorL.linkDriver(&driverL); motorR.linkDriver(&driverR);
    motorL.controller = MotionControlType::velocity;
    motorR.controller = MotionControlType::velocity;
    motorL.init(); motorL.initFOC();
    motorR.init(); motorR.initFOC();
    // 初始化栅格地图（示例）
    memset(gridMap, 0, sizeof(gridMap));
    // 规划初始全局路径
    globalPath = planAStar(0, 0, 10, 10);
}

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

    // 1. 多模态抗遮挡跟随
    float targetX, targetY;
    bool isOccluded;
    antiOcclusionFollow(targetX, targetY, isOccluded);

    // 2. 路径优化：若偏离全局路径，重规划
    int curGridX = (int)(odoX / GRID_SIZE);
    int curGridY = (int)(odoY / GRID_SIZE);
    if (pathIdx >= globalPath.size() || 
        abs(curGridX - globalPath[pathIdx].first) > 2 ||
        abs(curGridY - globalPath[pathIdx].second) > 2) {
        // 重规划全局路径
        globalPath = planAStar(curGridX, curGridY, 10, 10);
        pathIdx = 0;
    }

    // 3. 计算跟随误差与期望速度
    float dx = targetX - odoX;
    float dy = targetY - odoY;
    float distError = sqrt(dx*dx + dy*dy) - TARGET_DIST;
    float targetAngle = atan2(dy, dx);
    float angleError = targetAngle - odoYaw;
    if (angleError > PI) angleError -= 2*PI;
    if (angleError < -PI) angleError += 2*PI;

    // 4. S型加减速的速度规划
    float vLin = sCurveAccel(motorL.targetVelocity, constrain(distError * 0.4, 0, MAX_SPEED), 0.05);
    float vAng = sCurveAccel(motorR.targetVelocity, constrain(angleError * 0.8, -MAX_ANG_SPEED, MAX_ANG_SPEED), 0.05);

    // 5. IMU姿态补偿（复杂地形稳定）
    // 简化：姿态倾斜时降低速度
    if (abs(imuPitch) > 0.3 || abs(imuRoll) > 0.3) {
        vLin *= 0.5;
        vAng *= 0.5;
    }

    // 6. 差速驱动解算
    float wheelBase = 0.25;
    float leftSpeed = vLin - vAng * wheelBase / 2;
    float rightSpeed = vLin + vAng * wheelBase / 2;
    motorL.move(leftSpeed);
    motorR.move(rightSpeed);

    // 7. 更新里程计（模拟）
    odoX += vLin * cos(odoYaw + vAng*0.1) * 0.05;
    odoY += vLin * sin(odoYaw + vAng*0.1) * 0.05;
    odoYaw += vAng * 0.05;

    delay(30);
}

要点解读

1. 多模态感知融合：解决人流遮挡与定位失准的核心
   人流密集场景的核心挑战是目标遮挡与环境干扰，单一传感器无法满足需求，多模态融合是关键：
   传感器互补设计：案例2采用“UWB+IMU+里程计”，案例3采用“UWB+视觉+IMU”，UWB提供绝对定位，视觉识别目标特征，IMU与里程计弥补短时遮挡的定位盲区，形成“绝对定位+相对定位+特征识别”的互补体系；
   抗遮挡切换逻辑：当多传感器数据偏差超过阈值（如案例3中UWB与视觉位置偏差>1m），判定为目标遮挡，自动切换至惯性航位推算，避免跟随中断；信号恢复后平滑切回主传感器，确保跟随连续性；
   数据滤波优化：对传感器数据进行滑动平均或卡尔曼滤波，剔除人流干扰产生的噪声，提升定位精度，避免因传感器跳变导致机器人抖动。

2. 动态避障与路径优化的分层架构：平衡跟随效率与安全
   人流场景中，既要跟随目标，又要规避随机出现的行人、障碍物，需采用全局路径优化+局部动态避障的分层架构：
   全局路径规划：案例6引入A*算法，预先规划最优巡逻/跟随路线，规避静态障碍物（如固定货架、墙体），提升整体移动效率，避免无效绕行；
   局部动态避障：案例5采用滚动窗口重规划，基于机器人运动学约束（最大速度、转弯半径），实时推演避障轨迹，对突发障碍物（如横穿的行人）进行平滑减速绕行，避障结束后通过路径重投影回归全局路径，避免路径振荡；
   优先级分层：明确“避障优先级高于跟随”，案例4中硬避障逻辑无条件挂起跟随任务，案例2中局部避障输出的速度指令可覆盖外环跟随规划，确保安全底线，再兼顾跟随效率。

3. BLDC电机的精准控制：保障跟随平稳性与响应速度
   跟随效果的优劣直接取决于电机的执行精度与响应速度，BLDC电机结合FOC控制是核心支撑：
   FOC闭环控制：采用磁场定向控制（FOC）实现BLDC电机的转矩、速度双闭环控制，案例4-6均通过FOC将期望速度转化为精准的电机驱动指令，毫秒级响应速度修正，避免跟随过程中的“锯齿状”抖动，提升跟随平稳性；
   差速驱动适配：针对两轮差速底盘，将路径规划输出的线速度、角速度转化为左右轮独立转速，案例4-6均通过差速公式解算，确保机器人能精准转向、直线跟随，适配人流中的频繁启停、转弯需求；
   低速稳定性优化：人流场景常需近距离跟随（0.5-1m），BLDC电机配合FOC算法可实现极低速平稳运行，避免传统有刷电机的低速抖动，确保近距离跟随时不发生碰撞，提升安全性。

4. 算力分配与实时性保障：适配Arduino平台的工程关键
   人流密集场景对控制的实时性要求极高（控制周期≤50ms），而Arduino算力有限，需通过架构优化保障实时性：
   硬件选型升级：放弃算力不足的Arduino Uno，选用ESP32、STM32等高性能平台，案例4-6均基于ESP32设计，其双核架构可将传感器融合、路径规划等复杂逻辑放在Core0，电机FOC控制放在Core1，物理隔离保障电机控制的实时性，避免算力阻塞导致控制延迟；
   非阻塞控制设计：严禁使用delay()函数，采用millis()非阻塞定时或硬件定时器中断，确保控制循环周期稳定，避免因逻辑阻塞导致电机指令滞后，引发跟随偏差；
   算法简化与分工：将计算密集型任务（如复杂A算法、多传感器卡尔曼滤波）放在上位机（树莓派、Jetson）执行，Arduino仅负责电机控制与基础传感器数据处理，案例3的A规划可部署在上位机，通过串口下发路径点，减轻下位机算力负担。

5. 安全冗余与容错机制：应对人流场景的突发风险
   人流场景存在大量不可控因素，安全冗余是系统落地的底线，需从硬件、软件双重设计：
   硬件安全冗余：配备物理急停按钮、防撞条，案例4-6均需在硬件层面设计独立急停回路，优先级高于软件控制，触发后直接切断电机动力，避免失控碰撞；同时采用隔离DC-DC模块为控制电路独立供电，并联大容量电容吸收BLDC电机的反电动势，防止电磁干扰导致传感器失真、主控复位；
   软件容错逻辑：设置通信超时保护（如500ms未收到目标信号自动减速停车）、软件限速限幅（对速度、加速度进行constrain()约束，避免急启急停），案例2中目标丢失后切换至惯性跟随，案例3中遮挡时维持安全距离，避免机器人盲目跟随导致危险；
   状态切换平滑性：跟随、避障、待机等状态切换时，采用梯形速度曲线或S型加减速，避免状态突变导致的机械冲击，如案例3的S型加减速确保启停平稳，提升乘坐舒适性与设备寿命。

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