“Arduino BLDC 之基于多模态路径识别与 FOC 拟人化表达的导览机器人架构”可以理解为：机器人通过视觉、激光、IMU、编码器、UWB/二维码等多种感知手段识别路径与环境，在上位机或高性能 MCU 上完成定位、导航和交互决策；底层由 Arduino/ESP32/STM32 等运行 BLDC FOC 闭环，把导航指令、姿态调整和表情关节动作转化为平滑、低冲击、类人的运动与交互表现。
一、主要特点

1. 多模态路径识别提高鲁棒性
   导览环境通常存在人群流动、光照变化、玻璃反射、走廊重复结构等问题。单靠二维码、单靠激光或单靠视觉都可能在特定场景下失效。多模态方案通常把激光 SLAM、视觉 QR 码、IMU、编码器、UWB 或深度相机融合起来：激光负责几何建图与避障，视觉 QR 码提供绝对位姿参考，IMU/编码器提供高频里程计，UWB 在室内补充全局定位。
2. 全局定位与局部路径分层协同
   上层负责“我在哪、要去哪、走哪条路”，通常采用全局地图、拓扑节点或 SLAM 位姿；下层负责“当前这一秒如何安全走到下一个子目标”，通常采用 DWA、PID 轨迹跟踪或局部避障。这样即使上层定位短暂波动，底层仍可保持安全行驶。
3. FOC 拟人化表达把“导航动作”变成“交互动作”
   FOC 的优势不只是让电机转得更稳，它能把力矩、速度、位置解耦出来，用于模拟更自然的动作。例如：
   接近游客时减速柔和、停顿时轻微前倾，模拟“礼貌靠近”；
   讲解时头部/屏幕云台缓慢转动，避免生硬跳变；
   遇到障碍时小幅后退、侧身或点头，传递“我在让行”；
   任务完成时通过云台、灯光、声音和底盘姿态组合表达“确认”。
4. 运动层与表达层统一由 BLDC 执行
   底盘 BLDC 负责移动，云台 BLDC 负责头部/屏幕指向，关节 BLDC 负责耳朵、手臂、尾翼等拟人化部件。由于都采用 FOC 闭环，运动、姿态和表情可以在同一时间轴上协调，避免“底盘很稳、头部很抖”的割裂感。
5. 适合分层异构架构
   典型实现不是把所有算法压在 8 位 Arduino 上，而是采用：上位机/ESP32-S3/STM32 负责多模态感知、SLAM、路径规划和交互状态机；底层 MCU 负责 BLDC FOC 电流环、速度环、位置环和急停保护。FOC 电流环通常需要在 kHz 级稳定运行，不能受上层重任务阻塞。
6. 安全优先级高于拟人化表现
   拟人化动作必须服从安全约束。当游客突然靠近、路径被遮挡、定位置信度下降或电机异常时，应优先减速、停车、保持姿态或退回安全点，而不是继续完成“点头、转身、靠近讲解”等表演性动作。
   二、应用场景
7. 博物馆/科技馆导览
   环境结构化但人流密集，需要稳定定位和安静运动。FOC 可降低低速噪声，云台 BLDC 可平滑指向展品，多模态定位可在展柜、立柱和玻璃附近保持位姿稳定。
8. 商场/展厅接待
   需要主动靠近、让行、引导路线。机器人可通过视觉或 UWB 识别游客位置，用 BLDC 云台完成注视和指引动作，用底盘平滑加减速避免惊吓顾客。
9. 校园/企业园区讲解
   半室外环境光照变化大，GPS/视觉/激光可组合使用。机器人可在固定讲解点停靠，通过头部姿态、屏幕转向和语音同步完成导览表达。
10. 医院/政务大厅引导
    走廊长、人流多、环境动态。激光 SLAM 负责建图和避障，QR 码或 UWB 用于关键位置校正，FOC 底盘保证在狭窄通道内低速可控。
11. 教育科研与创客平台
    适合验证多模态定位、路径跟踪、FOC 拟人化动作、状态机交互和分层实时控制。可以先做“二维码导航 + 云台跟随”，再扩展到“SLAM + 多模态融合 + 情绪/姿态表达”。
    三、需要注意的事项
12. 多模态定位必须做时间对齐和置信度管理
    激光、相机、IMU、编码器、UWB 更新频率不同，直接混用会导致位姿抖动。应统一时间基准，并根据传感器置信度动态调整权重：QR 码可见时校正全局位姿，激光特征稀疏时降低匹配权重，IMU/编码器在短时遮挡下维持航位推算。
13. QR 码、UWB 和激光 SLAM 各有盲区
    QR 码依赖视场和光照，UWB 可能受 NLOS 影响，激光 SLAM 在长走廊和重复结构中容易漂移。工程上不应把任一传感器当作唯一真值，而应设计冗余：关键路口布设 QR 码，开阔区依赖 SLAM，遮挡区依赖里程计/IMU，必要时加入 UWB。
14. FOC 拟人化动作需要参数化，不能只靠硬编码角度
    更稳妥的做法是把“靠近、让行、讲解、等待、告警”等交互状态映射为速度、加速度、jerk、云台角度、底盘偏移和停顿时间的组合。这样同一套动作模型可以适配不同身高游客、不同通道宽度和不同讲解节奏。
15. 云台和底盘动作要解耦但时间同步
    底盘转向时，云台如果直接跟随会显得生硬；如果完全独立，又可能失去指向性。建议采用主从结构：底盘执行路径跟踪，云台执行平滑注视或讲解姿态，两者通过统一时间戳同步，避免动作打架。
16. BLDC 执行层必须限流、限速、限加速度
    导览机器人与人近距离接触，急停、急转和突然抬头都可能造成安全隐患。接近游客、狭窄通道、楼梯口和玻璃展柜时，应自动降低速度上限、角速度上限和云台加速度。
17. 8 位 Arduino 不适合承担完整多模态 SLAM
    若使用 ATmega328P，建议只负责电机闭环、传感器采样和安全保护；SLAM、视觉识别、QR 解码、UWB 融合和交互决策应交给 ESP32-S3、STM32 或上位机。FOC 电流环必须保持高频稳定，不能被串口打印、图像识别或网络通信阻塞。
18. 交互动作要有“可中断”设计
    游客突然伸手、儿童靠近、路径被阻断时，拟人化动作必须能被安全中断。状态机应支持高优先级事件抢占，例如：讲解中检测到近距离障碍 → 立即停止云台转动 → 底盘减速 → 进入让行或后退状态。
19. 标定和热管理不能忽视
    云台零位、底盘轮径、轮距、IMU 安装角、编码器方向都需要标定；长时间讲解时 BLDC 驱动、电池和主控可能发热，应加入温度监控和降额策略。否则拟人化动作初期很自然，运行一段时间后会出现抖动、延迟或停机。
    整体来看，这个架构的关键是：多模态路径识别解决“在哪、往哪走、如何避障”，FOC 拟人化表达解决“如何走得像人、如何与人自然互动”，BLDC 闭环解决“动作要稳、要柔、要可重复”，分层架构解决“感知决策不阻塞电机控制”。做原型时，建议先实现“二维码/激光定位 + BLDC 路径跟踪 + 云台平滑指向”，再加入“交互状态机 → 拟人化动作库 → 多模态定位融合 → 安全中断机制”。

1、基于磁条与红外融合的路径识别 + FOC 平滑差速循迹
适用场景：博物馆/展厅地面预埋磁条或铺设黑色导引线，机器人需在固定路线上平稳巡游，并在检测到展品标签时减速。

#include <SimpleFOC.h>

// 左轮 BLDC 配置
BLDCMotor motorL = BLDCMotor(11);
BLDCDriver3PWM driverL = BLDCDriver3PWM(9, 10, 11, 8);
Encoder encL = Encoder(2, 3, 2048);

// 右轮 BLDC 配置
BLDCMotor motorR = BLDCMotor(11);
BLDCDriver3PWM driverR = BLDCDriver3PWM(5, 6, 7, 4);
Encoder encR = Encoder(18, 19, 2048);

// 多模态输入：红外循迹阵列（模拟量） + 霍尔磁条检测
const int irLeftPin = A0;
const int irRightPin = A1;
const int hallPin = 12; // 磁条标记检测

float targetSpeedL = 0, targetSpeedR = 0;
float baseSpeed = 2.0; // rad/s

void doL_A(){encL.handleA();} void doL_B(){encL.handleB();}
void doR_A(){encR.handleA();} void doR_B(){encR.handleB();}

void setup() {
  Serial.begin(115200);
  
  // 初始化左电机
  encL.init(); encL.enableInterrupts(doL_A, doL_B);
  motorL.linkSensor(&encL);
  driverL.voltage_power_supply = 12; driverL.init();
  motorL.linkDriver(&driverL);
  motorL.controller = MotionControlType::velocity;
  motorL.init(); motorL.initFOC();

  // 初始化右电机
  encR.init(); encR.enableInterrupts(doR_A, doR_B);
  motorR.linkSensor(&encR);
  driverR.voltage_power_supply = 12; driverR.init();
  motorR.linkDriver(&driverR);
  motorR.controller = MotionControlType::velocity;
  motorR.init(); motorR.initFOC();

  pinMode(hallPin, INPUT_PULLUP);
}

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

  // --- 多模态路径融合 ---
  int irL = analogRead(irLeftPin);
  int irR = analogRead(irRightPin);
  bool onMagnet = (digitalRead(hallPin) == LOW);

  // 基础偏差计算（红外差速）
  float error = (irL - irR) * 0.002; // 归一化偏差
  // 磁条标记触发减速（拟人化“驻足”）
  float speedScale = onMagnet ? 0.3 : 1.0; 

  // 差速分配
  targetSpeedL = (baseSpeed - error) * speedScale;
  targetSpeedR = (baseSpeed + error) * speedScale;

  // 限制幅度
  targetSpeedL = constrain(targetSpeedL, -3.0, 3.0);
  targetSpeedR = constrain(targetSpeedR, -3.0, 3.0);

  motorL.move(targetSpeedL);
  motorR.move(targetSpeedR);
  
  delay(10);
}

解读：该案例利用 FOC 的速度环实现两轮独立平滑调速，避免传统直流电机在循迹纠偏时的顿挫感。磁条检测作为“路径点”信号，触发减速以实现拟人化的驻足讲解预备动作 。

2、超声波触发 + 头部 BLDC 拟人化“注视”反馈
适用场景：当参观者靠近展品或机器人时，机器人停止行进，通过头部云台 BLDC 电机执行平滑的“转头注视”或“点头”动作，表达关注。

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

// 头部偏航电机（BLDC + FOC 位置控制）
BLDCMotor headMotor = BLDCMotor(7);
BLDCDriver3PWM headDriver = BLDCDriver3PWM(3, 5, 6, 2);
MagneticSensorSPI headSensor = MagneticSensorSPI(10, 14); // AS5048A 示例

// 超声波（触发交互）
NewPing sonar(7, 8, 200); // trig, echo

float targetHeadAngle = 0;
bool interacting = false;

void setup() {
  Serial.begin(115200);
  
  headSensor.init();
  headMotor.linkSensor(&headSensor);
  headDriver.voltage_power_supply = 12;
  headDriver.init();
  headMotor.linkDriver(&headDriver);
  
  // 位置控制模式，用于精确的拟人化头部角度
  headMotor.controller = MotionControlType::angle;
  headMotor.P_angle.P = 15; // 角度环 P
  headMotor.velocity_limit = 5; // 限制转速，显得温柔
  
  headMotor.init();
  headMotor.initFOC();
}

void loop() {
  headMotor.loopFOC();
  
  int dist = sonar.ping_cm();
  
  // 多模态触发：距离 < 50cm 进入交互
  if (dist > 0 && dist < 50) {
    if (!interacting) {
      interacting = true;
      // 拟人化：缓慢转头“看向”参观者方向（假设右侧）
      targetHeadAngle = 0.6; // rad
    }
  } else {
    if (interacting) {
      interacting = false;
      // 恢复正视前方
      targetHeadAngle = 0.0;
    }
  }
  
  // FOC 位置环执行，平滑移动
  headMotor.move(targetHeadAngle);
  
  // 如果行进底盘存在，此处应暂停底盘速度（略）
  delay(20);
}

解读：BLDC 在 FOC 位置模式下的优势在于转矩平稳。通过限制 velocity_limit，头部转动不会像舵机那样生硬，而是带有“迟疑”或“温和”的质感，这是拟人化表达的核心——动作的加速度曲线比最终位置更重要 。

3、多模态状态机——循迹/互动/情绪反馈融合框架
适用场景：导览机器人需要在“自主巡游”与“停留互动”之间切换，并根据触摸或语音反馈执行不同的 FOC 动作（如开心点头、困惑歪头）。

#include <SimpleFOC.h>

// 底盘双电机（略去初始化细节，参考案例一）
extern BLDCMotor motorL, motorR;
extern float targetSpeedL, targetSpeedR;

// 状态定义
enum RobotState { PATROL, INTERACT, HAPPY, CONFUSED };
RobotState state = PATROL;

// 输入传感器
const int touchPin = A2;
const int sonarPin = A3; // 模拟读距离（简化）

unsigned long stateTimer = 0;
float baseSpeed = 2.0;

void enterState(RobotState newState) {
  state = newState;
  stateTimer = millis();
}

void setup() {
  // 电机初始化同案例一
  // ...
  pinMode(touchPin, INPUT);
}

void loop() {
  motorL.loopFOC(); motorR.loopFOC();
  
  int touchVal = analogRead(touchPin);
  int dist = analogRead(sonarPin); // 映射为 cm 略
  
  switch(state) {
    case PATROL:
      targetSpeedL = baseSpeed; targetSpeedR = baseSpeed;
      // 条件：有人靠近且触摸
      if (dist > 0 && dist < 60 && touchVal > 500) {
        enterState(INTERACT);
        targetSpeedL = 0; targetSpeedR = 0;
      }
      break;
      
    case INTERACT:
      // 停止移动，等待情绪判定（实际接语音/表情识别）
      targetSpeedL = 0; targetSpeedR = 0;
      if (touchVal > 800) { // 重触/长按 -> 开心
        enterState(HAPPY);
      } else if (millis() - stateTimer > 5000) { // 超时无进一步交互
        enterState(PATROL);
      }
      break;
      
    case HAPPY:
      // 拟人化：原地小幅左右扭动（差速振荡）
      targetSpeedL = 0.8 * sin(millis() * 0.005);
      targetSpeedR = -0.8 * sin(millis() * 0.005);
      if (millis() - stateTimer > 2000) enterState(INTERACT);
      break;
      
    case CONFUSED:
      // 拟人化：单轮微动，类似“歪头”
      targetSpeedL = 0.3; targetSpeedR = 0;
      if (millis() - stateTimer > 1500) enterState(PATROL);
      break;
  }
  
  motorL.move(targetSpeedL);
  motorR.move(targetSpeedR);
  delay(10);
}

解读：该框架将多模态感知（距离+触摸）映射为状态转移，而 FOC 电机作为执行器，在 HAPPY 或 CONFUSED 状态下输出特定的差速波形，实现无需额外云台的低成本拟人化表达。状态机保证了行为的确定性与可调试性 。

要点解读

1. FOC 是“拟人化”动作质感的基础，而非单纯动力源
   传统导览机器人多使用直流有刷电机或步进电机，动作往往带有顿挫或噪声。BLDC 配合 FOC 的平滑转矩输出，使得“缓慢转头”“轻柔启停”成为可能。在导览场景中，动作的加速度与 jerk（加加速度）决定了机器人看起来是“机械”还是“有生命感”。案例二中的 velocity_limit 限制正是为了柔化动作 。

2. 多模态路径识别应分层：底层确定性 + 上层语义化
   在博物馆等结构化环境中，磁导航或红外循迹（案例一）提供了厘米级的确定性横向纠偏，不受光照和人群遮挡影响；而视觉标签或 RFID 则提供语义信息（“这是哪个展区”）。两者融合的策略是：底层 PID 保证不走丢，上层事件触发交互状态 。

3. 状态机是导览机器人行为管理的核心骨架
   导览机器人不能同时“快速行进”和“深情注视参观者”。案例三的状态机将 PATROL（巡游）与 INTERACT/HAPPY（互动/情绪）互斥管理，确保 FOC 电机在每一时刻只接收一组明确的目标值。没有状态机的多模态系统容易产生指令冲突（如一边前进一边大幅摇头），导致动作怪异或电机过流。

4. 导览场景的“路径识别”更依赖拓扑节点而非连续轨迹
   与自动驾驶不同，导览机器人的核心任务是“从 A 展品到达 B 展品并停留”。因此路径识别的重点在于节点检测（磁条标记、二维码、AprilTag）的可靠性，而非连续曲线跟踪的极致精度。案例一中的霍尔传感器触发减速，就是拓扑节点意识的一种低成本实现 。

5. Arduino 生态的算力边界决定了架构分工
   SimpleFOC 在 Arduino UNO/ESP32 上运行良好，但多模态融合（视觉、语音情绪识别）通常需要树莓派或 K210 等上位机。合理的架构是：Arduino/ESP32 负责硬实时 FOC 环（10-20kHz）与传感器轮询，上位机负责视觉路径规划与情绪推理，二者通过串口/ROS 通信。不要在 Arduino 主循环中运行阻塞式视觉算法，否则 FOC 环的定时抖动会直接破坏动作的平滑性 。

4、多模态路径识别置信度投票 + FOC 拟人化"注视/犹豫/点头"表达
适用硬件：ESP32 + SimpleFOC 差速底盘（2×BLDC）+ 头部云台（2×BLDC，偏航/俯仰）+ 5 路红外循迹 + TCS34725 色标 + HuskyLens（串口）+ SU-03T 语音模块。核心思路：五种模态各自输出"分支 + 置信度"，加权投票决定走哪条路；总置信度不足时不做选择，而是用表达代替决策——减速、偏头、小幅摆动（犹豫），这在导览场景里比"原地报错"自然得多。

/*
 * 案例4：多模态识别投票 + 表达驱动的头部云台
 * 分支约定：-1 = 左, 0 = 直行, +1 = 右
 */
#include <SimpleFOC.h>
#include <Wire.h>
#include "Adafruit_TCS34725.h"

// ---- 底盘 ----
BLDCMotor wheelL(7), wheelR(7);
BLDCDriver3PWM drvL(9,10,11,8), drvR(22,23,24,25);
Encoder encL(2,3,2048), encR(32,33,2048);
void onL(){ encL.handleA(); } void onR(){ encR.handleA(); }

// ---- 头部云台（表达执行器）----
BLDCMotor headYaw(7), headPitch(7);
BLDCDriver3PWM drvY(12,13,14,15), drvP(16,17,18,19);
Encoder encY(4,5,2048), encP(6,7,2048);
void onY(){ encY.handleA(); } void onP(){ encP.handleA(); }

Adafruit_TCS34725 tcs(TCS34725_INTEGRATIONTIME_50MS, TCS34725_GAIN_4X);
const int irPin[5] = {34, 35, 36, 39, 32};      // 红外循迹阵列
const int IR_TH = 1800;                          // 黑线判定阈值

// ---- 候选：分支 + 置信度 ----
struct Cand { int br; float conf; };

Cand irCand() {
  float w = 0, sum = 0;
  for (int i = 0; i < 5; i++) {
    int v = analogRead(irPin[i]);
    if (v > IR_TH) { w += (i - 2); sum += 1; }   // 压线通道重心
  }
  if (sum == 0) return {0, 0.0f};                // 丢线：无置信
  float pos = w / sum;                            // -2..+2
  int br = (pos < -0.6f) ? -1 : (pos > 0.6f) ? 1 : 0;
  return {br, min(1.0f, 0.4f + 0.3f * sum)};     // 压线通道越多越可信
}

Cand colorCand() {
  uint16_t r, g, b, c; tcs.getRawData(&r, &g, &b, &c);
  if (c < 800) return {0, 0.0f};
  int br = 0; float conf = 0;
  if (r > g * 1.4f && r > b * 1.4f)      { br = -1; conf = 0.85f; }  // 红=左
  else if (g > r * 1.4f && g > b * 1.4f) { br =  1; conf = 0.85f; }  // 绿=右
  else if (b > r * 1.3f && b > g * 1.3f) { br =  0; conf = 0.7f;  }  // 蓝=直
  return {br, conf};
}

Cand visionCand() {                 // HuskyLens 识别箭头/展项标签
  int id = huskyReadId();           // 0=无, 1=左箭头, 2=直行, 3=右箭头
  if (id == 0) return {0, 0.0f};
  return {id - 2, 0.9f};
}

Cand voiceCand() {                  // SU-03T 语音指令
  int cmd = voiceReadCmd();         // 0=无, 1=去A展项, 2=去B, 3=返回
  if (cmd == 0) return {0, 0.0f};
  return {0, 0.8f};                 // 语音只定目标点，不直接定分支
}

int fuse(Cand *c, int n, float &total) {
  float score[3] = {0, 0, 0};
  total = 0;
  for (int i = 0; i < n; i++) {
    if (c[i].conf <= 0.01f) continue;
    score[c[i].br + 1] += c[i].conf;   // 置信度加权投票
    total += c[i].conf;
  }
  int best = 0;
  for (int k = 1; k < 3; k++) if (score[k] > score[best]) best = k;
  return best - 1;
}

// ---- 表达：状态 -> 云台姿态（微幅、慢节律）----
enum Exp { E_IDLE, E_GREET, E_HESITATE, E_NOD, E_APOLOGY };
Exp expNow = E_IDLE;
unsigned long expT0 = 0;

void setExp(Exp e) { if (e != expNow) { expNow = e; expT0 = millis(); } }

void headTarget(float &yaw, float &pitch) {
  float t = (millis() - expT0) / 1000.0f;
  switch (expNow) {
    case E_IDLE:                       // 待机：缓慢扫视 + 呼吸
      yaw   = 0.12f * sin(2 * PI * 0.10f * t);
      pitch = 0.010f * sin(2 * PI * 0.23f * t);
      break;
    case E_GREET:                      // 问候：两次点头 + 转向访客
      yaw   = 0.18f;
      pitch = -0.12f * fabs(sin(2 * PI * t / 1.2f));
      break;
    case E_HESITATE:                   // 犹豫：小幅左右摆 + 微侧头
      yaw   = 0.09f * sin(2 * PI * 1.2f * t);
      pitch = 0.05f;
      break;
    case E_NOD:                        // 应允：单次点头
      yaw   = 0.0f;
      pitch = -0.10f * sin(PI * min(t / 0.9f, 1.0f));
      break;
    case E_APOLOGY:                    // 致歉：低头
      yaw   = 0.0f;
      pitch = 0.13f;
      break;
  }
  pitch += 0.006f * sin(2 * PI * 0.23f * t);   // 呼吸微动始终叠加
}

void setup() {
  encL.init(); encL.enableInterrupts(onL);
  encR.init(); encR.enableInterrupts(onR);
  wheelL.linkSensor(&encL); wheelR.linkSensor(&encR);
  wheelL.controller = wheelR.controller = MotionControlType::velocity;
  wheelL.PID_velocity.P = 0.2f; wheelL.PID_velocity.I = 2.0f;
  wheelR.PID_velocity.P = 0.2f; wheelR.PID_velocity.I = 2.0f;
  wheelL.voltage_limit = wheelR.voltage_limit = 12.0f;
  wheelL.init(); wheelR.init();
  drvL.voltage_power_supply = drvR.voltage_power_supply = 12.0f;
  drvL.init(); drvR.init();
  wheelL.linkDriver(&drvL); wheelR.linkDriver(&drvR);

  encY.init(); encY.enableInterrupts(onY);
  encP.init(); encP.enableInterrupts(onP);
  headYaw.linkSensor(&encY); headPitch.linkSensor(&encP);
  headYaw.controller = headPitch.controller = MotionControlType::angle;
  headYaw.PID_angle.P = 15; headPitch.PID_angle.P = 15;
  headYaw.PID_angle.D = 0.3f; headPitch.PID_angle.D = 0.3f;
  headYaw.voltage_limit = headPitch.voltage_limit = 8.0f;
  headYaw.current_limit = headPitch.current_limit = 0.8f;  // 表达力矩软限
  headYaw.init(); headPitch.init();
  drvY.voltage_power_supply = drvP.voltage_power_supply = 8.0f;
  drvY.init(); drvP.init();
  headYaw.linkDriver(&drvY); headPitch.linkDriver(&drvP);

  Wire.begin(); tcs.begin();
  Serial.begin(115200);
}

void loop() {
  // ---- 1. 多模态采集与投票 ----
  Cand cs[4] = {irCand(), colorCand(), visionCand(), voiceCand()};
  float total; int br = fuse(cs, 4, total);

  // ---- 2. 决策：置信度不足 -> 表达代替决策 ----
  float v = 1.2f; float turn = 0;
  if (total < 0.6f) {
    v = 0.15f;                       // 几乎停住
    setExp(E_HESITATE);
  } else {
    if (br < 0)      { turn = -0.5f; setExp(E_NOD); }
    else if (br > 0) { turn =  0.5f; setExp(E_NOD); }
    else             { turn =  0.0f; setExp(E_IDLE); }
  }

  // 语音/视觉指定目标点时的问候
  if (cs[3].conf > 0.5f || cs[2].conf > 0.5f) setExp(E_GREET);

  // ---- 3. 表达输出：云台位置环（微幅慢节律）----
  float yawT, pitchT; headTarget(yawT, pitchT);
  headYaw.move(yawT); headPitch.move(pitchT);

  // ---- 4. 运动输出：差速速度环 ----
  wheelL.loopFOC(); wheelR.loopFOC();
  wheelL.move(v + turn); wheelR.move(v - turn);

  Serial.print("br="); Serial.print(br);
  Serial.print(" conf="); Serial.print(total, 2);
  Serial.print(" exp="); Serial.println(expNow);
  delay(20);
}

工程提示：云台位置环与底盘速度环都应移入 1kHz 定时器中断；E_HESITATE 的价值在于"用可读的行为表达不确定性"，而不是把"识别失败"变成急停——导览机器人被围观时，观众更接受"它在想"，而不是"它坏了"。

5、FreeRTOS 三任务 —— 识别/表达节拍/执行分离 + 关键帧平滑混合
适用硬件：ESP32 双核 + 4 个 BLDC（底盘 2 + 云台 2）+ 多模态传感器。
核心思路：表达不是"随手摆两下"，而是一套关键帧序列 + 缓动插值；表达任务以 25Hz 独立推进节拍，识别任务 50Hz 出决策，执行任务 1kHz 纯跟踪。安全事件（近距离障碍）可一键抢占表达队列，任何表达都能被瞬间打断。

/*
 * 案例5、：识别-表达-执行 三任务 + 关键帧表达队列 + 安全抢占
 */
#include <SimpleFOC.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"

BLDCMotor wheelL(7), wheelR(7), headYaw(7), headPitch(7);
// 驱动/编码器初始化同案例一（略）
SemaphoreHandle_t mux;

// ---- 表达关键帧：姿态 + 时长 + 缓动 ----
struct Key { float yaw, pitch; float dur; uint8_t ease; }; // ease:0线性 1平滑
const Key SEQ_GREET[] = {
  { 0.20f, -0.05f, 0.35f, 1},   // 转向访客
  { 0.20f, -0.16f, 0.30f, 1},   // 点头下
  { 0.20f, -0.04f, 0.30f, 1},   // 点头上
  { 0.20f, -0.16f, 0.25f, 1},   // 再点一次
  { 0.12f,  0.00f, 0.40f, 1},
};
const Key SEQ_HESITATE[] = {
  {-0.10f, 0.04f, 0.45f, 1},
  { 0.10f, 0.04f, 0.55f, 1},
  {-0.06f, 0.04f, 0.45f, 1},
  { 0.00f, 0.02f, 0.40f, 1},
};
const Key SEQ_APOLOGY[] = {
  { 0.00f, 0.14f, 0.50f, 1},    // 低头
  { 0.00f, 0.10f, 0.80f, 1},    // 停住
};
struct Seq { const Key *k; int n; };
const Seq SEQS[] = { {SEQ_GREET,5}, {SEQ_HESITATE,4}, {SEQ_APOLOGY,2} };
enum SeqId { SQ_GREET = 0, SQ_HESITATE = 1, SQ_APOLOGY = 2, SQ_NONE = -1 };

volatile int  g_seqReq = SQ_NONE;    // 识别任务请求的表达
volatile bool g_safety  = false;     // 安全抢占标志
volatile float g_yawT = 0, g_pitchT = 0;   // 表达输出（给执行任务）
volatile float g_speed = 0, g_turn = 0;    // 运动输出

// ---- 识别任务：50Hz，投票 + 触发表达 ----
void taskPerceive(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    // 多模态采集投票（同案例一，略）
    float total; int br = 0; // = fuse(...)
    float dUS = readFrontUS();

    xSemaphoreTake(mux, portMAX_DELAY);
    if (dUS < 0.45f) {                       // 安全事件：最高优先级
      g_safety = true; g_speed = 0; g_turn = 0;
      g_seqReq = SQ_APOLOGY;
    } else if (total < 0.6f) {               // 识别不确定
      g_safety = false; g_speed = 0.15f;
      g_seqReq = SQ_HESITATE;
    } else {
      g_safety = false;
      g_speed = 1.2f;
      g_turn = (br < 0) ? -0.5f : (br > 0) ? 0.5f : 0.0f;
      if (br == 0 && total > 0.9f) g_seqReq = SQ_GREET;  // 识别确凿 -> 致意
    }
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(20));   // 50Hz
  }
}

// ---- 表达任务：25Hz，关键帧推进 + 平滑插值 ----
float smoothstep(float x) { return x * x * (3 - 2 * x); }

void taskExpression(void*) {
  TickType_t last = xTaskGetTickCount();
  int cur = SQ_NONE, idx = 0;
  float tSeg = 0, fromY = 0, fromP = 0;
  unsigned long t0 = millis(), breath0 = millis();

  for (;;) {
    // 抢占：安全事件优先，切表达立即生效（不等待当前动作完成）
    int req;
    xSemaphoreTake(mux, portMAX_DELAY); req = g_seqReq; xSemaphoreGive(mux);

    if (req != SQ_NONE && (req != cur || g_safety)) {
      cur = req; idx = 0; tSeg = 0;
      fromY = g_yawT; fromP = g_pitchT;
      xSemaphoreTake(mux, portMAX_DELAY); g_seqReq = SQ_NONE; xSemaphoreGive(mux);
    }

    float yawT = 0, pitchT = 0;
    if (cur != SQ_NONE && idx < SEQS[cur].n) {
      const Key &k = SEQS[cur].k[idx];
      tSeg += 0.04f;                                // 25Hz 步进
      float a = min(tSeg / k.dur, 1.0f);
      float w = (k.ease == 1) ? smoothstep(a) : a;   // 缓动，避免阶跃抖动
      yawT   = fromY + (k.yaw   - fromY) * w;
      pitchT = fromP + (k.pitch - fromP) * w;
      if (a >= 1.0f) { fromY = k.yaw; fromP = k.pitch; idx++; tSeg = 0; }
    } else {
      cur = SQ_NONE;
      // 空闲：缓慢扫视 + 呼吸
      float t = (millis() - t0) / 1000.0f;
      yawT   = 0.10f * sin(2 * PI * 0.10f * t);
      pitchT = 0.008f * sin(2 * PI * 0.23f * t);
    }
    // 呼吸微动始终叠加（拟人化的"活着"感来自最低频的那一层）
    float tb = (millis() - breath0) / 1000.0f;
    pitchT += 0.006f * sin(2 * PI * 0.23f * tb);

    xSemaphoreTake(mux, portMAX_DELAY);
    g_yawT = yawT; g_pitchT = pitchT;
    xSemaphoreGive(mux);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(40));      // 25Hz
  }
}

// ---- 执行任务：1kHz，纯跟踪 ----
void taskExec(void*) {
  TickType_t last = xTaskGetTickCount();
  for (;;) {
    float y, p, sp, tn;
    xSemaphoreTake(mux, portMAX_DELAY);
    y = g_yawT; p = g_pitchT; sp = g_speed; tn = g_turn;
    xSemaphoreGive(mux);

    headYaw.loopFOC();  headPitch.loopFOC();
    headYaw.move(y);    headPitch.move(p);
    wheelL.loopFOC();   wheelR.loopFOC();
    wheelL.move(sp + tn); wheelR.move(sp - tn);
    vTaskDelayUntil(&last, pdMS_TO_TICKS(1));
  }
}

void setup() {
  // 四电机初始化同案例一（略）
  mux = xSemaphoreCreateMutex();
  xTaskCreatePinnedToCore(taskPerceive,   "perc", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExpression, "expr", 4096, NULL, 2, NULL, 0);
  xTaskCreatePinnedToCore(taskExec,       "exec", 4096, NULL, 3, NULL, 1);
}
void loop() { vTaskDelay(portMAX_DELAY); }

工程提示：表达任务里 g_seqReq “取走即清"的写法保证同一请求只执行一次；安全抢占用 g_safety 强制重入而不是排队——安全事件永远不排队，直接覆盖当前表达。关键帧插值用 smoothstep 而非线性，是因为线性的角速度在段界处突变，看起来"机械”。

6、讲解点演绎 —— 停顿/注视/柔顺手势 + 观众打断处理
适用硬件：ESP32 + 底盘 2×BLDC + 云台 2×BLDC + 单臂 2×BLDC（肩/肘，力矩模式）+ 前向超声/毫米波 + 语音播报。
核心思路：导览最核心的拟人化不是"会摆动作"，而是"有节拍的讲解"——每句讲解配一个"注视 + 手势 + 停顿"的组合节拍；手臂用力矩模式 + 低电流限幅实现柔顺（观众碰到会自然让开，而不是硬顶），并用"电流偏差"检测外力自动退让。观众靠近则打断讲解、转身致意，之后回到断点继续。

/*
 * 案例6：讲解节拍演绎 + 柔顺手势 + 打断恢复
 * 状态：CRUISE -> ARRIVE -> EXPLAIN -> PAUSE(被打断) -> RESUME
 */
#include <SimpleFOC.h>

BLDCMotor wheelL(7), wheelR(7), headYaw(7), headPitch(7);
BLDCMotor armShoulder(7), armElbow(7);      // 手臂：力矩模式（柔顺）
// 驱动/编码器初始化同案例一（略）

enum St { CRUISE, ARRIVE, EXPLAIN, PAUSE_I, RESUME };
St st = CRUISE;

// ---- 讲解节拍表：每拍 = 注视 + 手势 + 停留 ----
struct Beat {
  float gazeYaw, gazePitch;     // 头部注视方向
  float shTau, elTau;           // 手臂力矩（举起/前伸）
  float hold;                   // 停留时长(s)：停顿是表达的一部分
  uint8_t voice;                // 语音片段号
};
const Beat BEATS[] = {
  { 0.20f, -0.02f, 0.55f, 0.15f, 1.2f, 1},   // 转向展品 + 抬臂示意
  { 0.20f,  0.03f, 0.40f, 0.10f, 1.5f, 2},   // 注视展品讲解
  { 0.05f, -0.06f, 0.25f, 0.05f, 0.8f, 3},   // 回望观众（点头）
  { 0.20f,  0.00f, 0.35f, 0.08f, 1.0f, 4},   // 再次示意
};
const int N_BEATS = 4;
int beatIdx = 0; float beatT = 0;
bool beatStarted = false;

// ---- 柔顺手臂：阻抗控制 + 外力退让 ----
const float KP_ARM = 3.0f, KD_ARM = 0.25f;
const float TAU_LIM = 0.9f;                  // 力矩硬限（安全底线）
float armYield = 1.0f;                       // 1=正常，<1=让步

void armCompliance(float qdSh, float qdEl) {
  float qSh = armShoulder.shaft_angle, vSh = armShoulder.shaft_velocity;
  float qEl = armElbow.shaft_angle,    vEl = armElbow.shaft_velocity;
  float tauSh = KP_ARM * armYield * (qdSh - qSh) - KD_ARM * vSh;
  float tauEl = KP_ARM * armYield * (qdEl - qEl) - KD_ARM * vEl;

  // 外力检测：实际电流显著偏离指令 -> 有人推/碰 -> 让步
  if (fabs(armShoulder.current) > 0.55f || fabs(armElbow.current) > 0.55f)
    armYield = max(0.15f, armYield - 0.05f);
  else
    armYield = min(1.0f, armYield + 0.01f);   // 缓慢恢复，避免抖动

  armShoulder.move(constrain(tauSh, -TAU_LIM, TAU_LIM));
  armElbow.move(constrain(tauEl, -TAU_LIM, TAU_LIM));
}

// 节拍手势力矩 -> 关节角（用位置近似表达姿势，实际可换成角度映射）
float shAngleFor(float tau) { return 0.9f * tau / TAU_LIM; }
float elAngleFor(float tau) { return 0.7f * tau / TAU_LIM; }

void setup() {
  // 电机初始化同案例一，其中手臂用 torque 模式：
  // armShoulder.controller = armElbow.controller = MotionControlType::torque;
  // armShoulder.current_limit = armElbow.current_limit = 1.0f;
  Serial.begin(115200);
}

void loop() {
  float dUS = readFrontUS();          // 前向距离（观众/障碍）
  bool personNear = (dUS < 0.8f);     // 有人靠近（可换成视觉人数）

  switch (st) {
    case CRUISE:                       // 巡航到讲解点
      wheelL.move(1.5f); wheelR.move(1.5f);
      headYaw.move(0.06f * sin(millis() / 4000.0f));
      headPitch.move(0.0f);
      armCompliance(0.0f, 0.0f);       // 手臂自然下垂（零位）
      if (arrivedAtPOI()) { st = ARRIVE; }
      break;

    case ARRIVE:                       // 到位：停稳 + 面向观众致意
      wheelL.move(0); wheelR.move(0);
      headYaw.move(0.20f); headPitch.move(-0.10f);
      armCompliance(0.30f, 0.10f);
      delay(1200);                     // 致意停顿
      st = EXPLAIN; beatIdx = 0; beatT = 0; beatStarted = false;
      break;

    case EXPLAIN: {                    // 讲解演绎：按节拍推进
      if (personNear) { st = PAUSE_I; break; }     // 被打断
      const Beat &b = BEATS[beatIdx];
      if (!beatStarted) { beatStarted = true; beatT = 0; playVoice(b.voice); }
      beatT += 0.02f;

      // 注视：平滑趋近目标（避免头部突跳）
      static float yawC = 0, pitchC = 0;
      yawC   += (b.gazeYaw   - yawC)   * 0.06f;
      pitchC += (b.gazePitch - pitchC) * 0.06f;
      headYaw.move(yawC); headPitch.move(pitchC);

      // 手势：力矩缓升（拟人化的"抬手"不是瞬间到位）
      armCompliance(shAngleFor(b.shTau), elAngleFor(b.elTau));

      if (beatT > b.hold) {          // 本拍结束
        beatIdx++; beatStarted = false;
        if (beatIdx >= N_BEATS) {    // 讲解完成：点头收尾
          headPitch.move(-0.12f); delay(500);
          st = RESUME;
        }
      }
      break;
    }

    case PAUSE_I:                      // 被打断：转身致意，礼貌让步
      wheelL.move(0); wheelR.move(0);
      headYaw.move(0.28f); headPitch.move(0.02f);
      armCompliance(0.10f, 0.02f);     // 手臂收回，避免挡人
      if (personNear) { /* 等待/简短应答 */ }
      else { delay(600); st = EXPLAIN; }   // 人离开 -> 回到当前节拍继续
      break;

    case RESUME:                        // 恢复巡航
      headYaw.move(0.0f); headPitch.move(0.0f);
      armCompliance(0.0f, 0.0f);
      if (fabs(headPitch.shaft_angle) < 0.05f) st = CRUISE;
      break;
  }

  // 执行环（工程上移入 1kHz 中断）
  wheelL.loopFOC(); wheelR.loopFOC();
  headYaw.loopFOC(); headPitch.loopFOC();
  armShoulder.loopFOC(); armElbow.loopFOC();

  Serial.print("st="); Serial.print(st);
  Serial.print(" d="); Serial.println(dUS, 2);
  delay(20);
}

工程提示：armYield 的"快降慢升"是不对称的——让步要快（有人碰的瞬间就软），恢复要慢（避免力矩反复抽动）。手臂用 current_limit + TAU_LIM 双限幅，即使控制逻辑出错也不会夹伤观众；这一点在导览机器人上是硬性安全要求，不是优化项。

要点解读
一、多模态融合的关键是"置信度投票 + 失效降级"，而不是把所有传感器当成一份数据拼起来
五种模态（红外、色标、视觉、语音、超声）各有各的量程、刷新率和失效模式：红外在强光下丢线，色标在脏污地面误判，视觉被遮挡就无输出，语音在噪声里靠不住。案例一的做法是让每个模态输出"分支 + 置信度"，再按置信度加权投票——低置信的源自动退场（conf <= 0.01 直接跳过），总置信不足时不硬选，而是降级为"减速 + 犹豫表达"。融合的价值不在于算得多复杂，而在于知道"此刻该信谁、谁也不信时怎么办"。

二、拟人化的物理载体是"微幅、慢节律、平滑的力矩/角度调制"，不是夸张的大动作
观众感知到的"像人"，主要来自三层叠加：最低频的呼吸（0.2Hz 级微动，案例一 pitch += 0.006·sin）、中频的注视与头部追踪（缓慢趋近而非瞬移）、高频的动作节拍（点头、抬手）。三层都必须平滑——案例二用 smoothstep 缓动而不是线性插值，正是因为线性的角速度在段界突变，看起来"机械"。幅度也要克制：头部 ±0.1~0.2 rad、手臂力矩 0.1~0.5 量级，就足够传达意图；动作越夸张，越容易掉进"恐怖谷"。

三、表达层必须独立成任务，并且能被安全事件"瞬间覆盖"而不是排队
讲解到一半有人冲到 0.5m 内，不能等这句话讲完再反应。案例二的做法是表达任务独立 25Hz 推进，安全标志 g_safety 强制重入、g_seqReq 取走即清——安全事件永远插队，普通表达请求才排队。同时表达绝不能进 1kHz 执行环：关键帧插值、语音触发、节拍计时都是"毫秒到百毫秒"级的事，塞进 FOC 环只会让电机抖。分层之后，表达再花哨也不会影响底盘稳定。

四、拟人化表达需要"语法"：停顿、注视、节拍，比动作本身更传神
案例三的每一条讲解都是"注视展品 → 抬手示意 → 停留 1.2s → 回望观众"的节拍结构，hold 字段就是刻意的停顿。真实讲解员的节奏是"说—停—看—再说"，机器人若一路匀速讲完、动作不停，观众只会觉得是台会动的音箱。停顿是留给观众的反应时间，注视是建立"它在跟我说话"的连接，多通道同步（头、臂、语音、灯光）比单通道加大幅度有效得多。这也是"表达要有剧本"的原因——节拍表（BEATS[]）本质上就是一份可调参的表演脚本。

五、柔顺与可打断是导览机器人的安全底线，也是"拟人"的一部分
导览机器人工作在人群里，手臂抬起就可能被碰到、被孩子抓住。案例三用力矩模式 + 电流限幅 + 外力检测让步（armYield 快降慢升）实现"被碰就让"，而不是刚性顶回去——这既是安全要求，也恰好符合人对"有生命的物体"的预期：被碰会退，被挡会让。同理，任何表达（问候、讲解、手势）都必须能在任意时刻被安全事件打断，并进入一个明确的低风险姿态。能优雅地被打断，比能流畅地表演更难，也更像人。

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