基于Arduino与BLDC（无刷直流电机）的“UWB多基站三角定位与选择性跟随系统”，是一种专为复杂、多目标动态环境设计的先进机器人导航方案。该系统利用超宽带（UWB）技术的厘米级测距精度，结合多基站几何解算，驱动BLDC电机实现对特定目标的稳定、抗干扰跟随。以下是该系统的详细专业解析：
一、 主要特点
厘米级高精度与强抗干扰定位
系统采用UWB技术，通过测量纳秒级脉冲信号的飞行时间（ToF）或到达时间差（TDOA）进行测距，定位精度可达厘米级（通常<10cm）。UWB极宽的频带使其能够有效区分直达信号与反射信号，在充满金属货架、墙体等易产生多径效应的复杂工业环境中，依然能保持极高的定位稳定性，远优于蓝牙或Wi-Fi。
多基站协同的三角定位解算
系统依赖预先部署的固定基站（Anchor）与机器人上的标签（Tag）协同工作。通过至少3个非共线布置的基站，系统利用三边测量法（Trilateration）或多边测量法解算出目标在全局坐标系中的精确二维或三维位置。这种架构无需改造地面环境（如铺设磁条或二维码），具备极高的部署灵活性。
选择性跟随与相对坐标转换
系统具备“选择性”识别能力，通过为不同的UWB标签分配独立的网络ID，机器人能够在多个目标中精准锁定并跟随指定的目标（如特定的拣货员或载具）。在控制层面，系统会将全局坐标系下的目标位置实时转换为机器人本体坐标系下的相对距离和方位角，从而为底层的运动控制提供直接输入。
BLDC高动态响应与平滑跟随
底层采用BLDC电机作为执行器，配合FOC（磁场定向控制）或高性能闭环驱动，具备毫秒级的扭矩响应能力。当上层算法根据UWB数据计算出期望的线速度和角速度时，BLDC电机能够迅速、平滑地执行差速转向或加减速指令，确保在目标突然启停或急转弯时，机器人能够“如影随形”且不发生剧烈晃动。
二、 典型应用场景
智能仓储与物流拣选（人机协同AGV）
在大型仓库中，机器人自动识别并跟随佩戴UWB标签的拣货员。机器人作为移动载具，承载拣选篮或重物，跟随工人在货架间穿梭。选择性跟随机制确保了机器人只响应其绑定的操作员，避免了多机协同时的“串台”与混乱。
工厂车间物料配送与工具跟随
在制造车间，机器人可根据工位呼叫或跟随特定操作员，将物料、重型工具或零部件精准配送至指定位置。UWB的高抗干扰能力使其能在复杂的电磁与金属环境中稳定运行，大幅减轻工人的搬运负担。
医疗康复与助行辅助
在医院或康复中心，作为智能助行器或输液架跟随患者移动。UWB的低延迟与高精度确保了设备在患者改变步速或转向时能及时响应，提供安全的支撑与跟随体验。
多机器人协同与科研验证
作为室内高精度定位、多智能体协同以及传感器融合算法的科研验证平台。在机器人竞赛或实验室中，用于验证新型TDOA定位算法、卡尔曼滤波平滑策略以及动态避障跟随逻辑。
三、 需要注意的关键事项
基站几何布局与视距（LoS）保障
基站的部署直接决定定位精度。至少需要3个基站呈非共线（如四边形）布置以包围工作区域，且必须保证机器人与基站之间无严重的物理遮挡（视距要求）。基站坐标需经过高精度物理测量并输入系统，任何标定误差都会直接放大定位偏差。
Arduino算力瓶颈与架构分工
UWB的原始测距数据解算（如求解非线性方程组）及后续的滤波算法对算力要求极高，标准Arduino Uno/Nano极易因内存溢出或浮点运算过载而崩溃。强烈建议采用ESP32、Teensy 4.1或树莓派等高算力主控，或采用“上位机解算坐标 + Arduino底层控制”的分布式架构。
数据滤波与多传感器融合
尽管UWB抗干扰能力强，但在极端环境或人体遮挡时仍会出现测距跳变。系统必须引入卡尔曼滤波（KF）或扩展卡尔曼滤波（EKF）对原始坐标进行平滑处理。同时，建议融合IMU（惯性测量单元）和轮式里程计，在UWB信号短暂丢失时通过航位推算（Dead Reckoning）维持跟随的连续性。
严格的电源隔离与电磁兼容（EMC）
UWB模块对电源噪声和射频干扰极其敏感，而BLDC电机在启停时会产生巨大的电流冲击和高频PWM噪声。必须为UWB模块和Arduino主控提供独立的LDO或DC-DC电源，严禁与电机共用；信号线需远离动力线，并在电源端加装π型滤波电路，防止电机干扰导致定位数据乱跳。
安全机制与容错设计
必须设计完善的安全策略。例如，当检测到与目标的距离过近或UWB信号完全丢失时，机器人应自动触发减速或急停机制。此外，在多机器人同场竞技或作业时，需合理规划UWB通信时隙，避免多标签间的射频碰撞与相互干扰。
需要我提供一份基于ESP32+DWM1000的UWB三角定位解算与BLDC选择性跟随控制代码框架吗？可以结合你的基站布局坐标和底盘参数来定制。

1、恒力装配 + 位置补偿
适用场景：电子元件插装、轴承压入。SCARA末端安装力传感器，通过BLDC关节力矩控制保持恒定接触力，同时根据力误差自适应调整Z轴位置。

/* ===== Arduino BLDC SCARA机器人 — 恒力装配 + 位置补偿 =====
 * 硬件：4轴SCARA(J1,J2,Z,R) + 末端拉压力传感器
 *       其中J1,J2使用BLDC，Z轴使用BLDC+丝杠，R轴使用BLDC
 * 核心：力闭环(内环) + Z轴位置自适应补偿(外环)
 */
#include <SimpleFOC.h>
#include <Wire.h>

// 4个BLDC关节
BLDCMotor j1(5), j2(6), jZ(7), jR(8);
BLDCDriver3PWM drv1, drv2, drvZ, drvR;
Encoder enc1(2,3,2048), enc2(4,5,2048), encZ(9,10,2048), encR(11,12,2048);

// 末端力传感器
const int FORCE_PIN = A0;
float forceTarget = 10.0;   // 目标接触力(N)
float forceActual = 0;

// 力控PID(内环)
float Kp_force = 0.06, Ki_force = 0.001, Kd_force = 0.005;
float forceIntegral = 0, lastForceError = 0;

// Z轴位置自适应补偿(外环)
float zOffset = 0;
float Kp_z = 0.008;
float maxZOffset = 3.0;  // 最大补偿量(mm)

// 装配轨迹参数
float assemblyDepth = 15.0;  // 装配深度(mm)
float insertSpeed = 5.0;     // 插入速度(mm/s)

void setup() {
  Serial.begin(115200);
  
  // 初始化4个BLDC(力矩模式)
  BLDCMotor* motors[] = {&j1, &j2, &jZ, &jR};
  for(auto m : motors) {
    m->controller = MotionControlType::torque;
    m->init(); m->initFOC();
  }
  
  pinMode(FORCE_PIN, INPUT);
}

void loop() {
  float dt = 0.01;  // 100Hz控制周期
  
  // 1. 读取力传感器
  forceActual = analogRead(FORCE_PIN) * 50.0 / 4095.0;  // 12位ADC
  
  // 2. 力误差计算
  float forceError = forceTarget - forceActual;
  
  // 3. 力控PID(内环) → 关节力矩指令
  forceIntegral += forceError * dt;
  forceIntegral = constrain(forceIntegral, -5, 5);
  float derivative = (forceError - lastForceError) / dt;
  float torqueCmd = Kp_force * forceError + Ki_force * forceIntegral + Kd_force * derivative;
  lastForceError = forceError;
  
  // 4. Z轴位置自适应补偿(外环)
  zOffset += Kp_z * forceError * dt;
  zOffset = constrain(zOffset, -maxZOffset, maxZOffset);
  
  // 5. SCARA运动学逆解 + 位置补偿
  float targetX = 100;  // 装配位置X
  float targetY = 50;   // 装配位置Y
  float targetZ = assemblyDepth + zOffset;  // Z轴含补偿
  
  float j1Angle, j2Angle;
  scaraInverseKinematics(targetX, targetY, &j1Angle, &j2Angle);
  
  // 6. 执行：位置环(外环) + 力矩环(内环)
  // J1,J2位置控制(刚度控制)
  float torque1 = 50.0 * (j1Angle - j1.shaft_angle) - 2.0 * j1.shaft_velocity;
  float torque2 = 50.0 * (j2Angle - j2.shaft_angle) - 2.0 * j2.shaft_velocity;
  
  // Z轴力控
  float torqueZ = torqueCmd * 0.01;  // 力到力矩转换
  
  // R轴保持
  float torqueR = -0.5 * jR.shaft_velocity;
  
  j1.move(constrain(torque1, -3.0, 3.0));
  j2.move(constrain(torque2, -3.0, 3.0));
  jZ.move(constrain(torqueZ, -2.0, 2.0));
  jR.move(constrain(torqueR, -1.0, 1.0));
  
  j1.loopFOC(); j2.loopFOC();
  jZ.loopFOC(); jR.loopFOC();
  
  // 7. 安全保护
  if(forceActual > forceTarget * 1.5) {
    j1.move(0); j2.move(0); jZ.move(0); jR.move(0);
    Serial.println("Overforce! Emergency stop.");
  }
  
  delay(10);
}

void scaraInverseKinematics(float x, float y, float* j1, float* j2) {
  float L1 = 200, L2 = 180;
  float d = sqrt(x*x + y*y);
  *j2 = acos((L1*L1 + L2*L2 - d*d) / (2*L1*L2));
  *j1 = atan2(y, x) - atan2(L2*sin(*j2), L1 + L2*cos(*j2));
}

关键设计点：
双环控制：外环位置补偿消除力误差，内环力矩控制快速响应
Z轴位置补偿量限制在±3mm，防止过度下压损坏工件
力超限1.5倍时紧急停机
SCARA运动学逆解实现XY平面定位

2、阻抗控制 + 曲面跟踪
适用场景：手机壳打磨、弧面抛光。SCARA末端呈现弹簧-阻尼特性，自适应贴合曲面，保持恒定接触力。

/* ===== Arduino BLDC SCARA机器人 — 阻抗控制 + 曲面跟踪 =====
 * 硬件：4轴SCARA + 末端力传感器 + 激光位移传感器(可选)
 * 核心：末端阻抗控制 → 柔顺贴合曲面 → 恒力跟踪
 */
#include <SimpleFOC.h>

BLDCMotor j1(5), j2(6), jZ(7), jR(8);

// 阻抗参数
const float M_DES = 0.3;      // 虚拟质量(kg)
const float B_DES = 8.0;      // 虚拟阻尼(Ns/m)
const float K_DES = 150.0;    // 虚拟刚度(N/m)

// 力传感器
float forceMeasured = 0;
float forceDesired = 8.0;     // 期望接触力(N)

// 阻抗状态
float deltaX = 0, deltaV = 0;  // 位置修正量和速度

// 轨迹参数
float trajX = 100, trajY = 50;
float trajSpeed = 20;  // mm/s

void setup() {
  Serial.begin(115200);
  
  BLDCMotor* motors[] = {&j1, &j2, &jZ, &jR};
  for(auto m : motors) {
    m->controller = MotionControlType::torque;
    m->init(); m->initFOC();
  }
}

void loop() {
  float dt = 0.01;
  
  // 1. 读取力传感器
  forceMeasured = analogRead(A0) * 50.0 / 4095.0;
  
  // 2. 阻抗控制模型(二阶弹簧-阻尼系统)
  float forceError = forceMeasured - forceDesired;
  
  // 加速度 = (力误差 - 阻尼×速度 - 刚度×位置) / 质量
  float accel = (forceError - B_DES * deltaV - K_DES * deltaX) / M_DES;
  
  deltaV += accel * dt;
  deltaX += deltaV * dt;
  
  // 3. 轨迹生成(沿曲面移动)
  static float t = 0;
  t += trajSpeed * dt;
  
  // 曲面轨迹：正弦波形
  float nominalX = trajX + t;
  float nominalY = trajY + 10 * sin(t * 0.05);
  float nominalZ = 20 + deltaX;  // Z轴含阻抗修正
  
  // 4. SCARA运动学逆解
  float j1Angle, j2Angle;
  scaraInverseKinematics(nominalX, nominalY, &j1Angle, &j2Angle);
  
  // 5. 关节控制(力矩模式)
  float torque1 = K_DES * (j1Angle - j1.shaft_angle) - B_DES * j1.shaft_velocity;
  float torque2 = K_DES * (j2Angle - j2.shaft_angle) - B_DES * j2.shaft_velocity;
  float torqueZ = forceError * 0.02;  // Z轴力控
  float torqueR = -0.3 * jR.shaft_velocity;
  
  j1.move(constrain(torque1, -3.0, 3.0));
  j2.move(constrain(torque2, -3.0, 3.0));
  jZ.move(constrain(torqueZ, -2.0, 2.0));
  jR.move(constrain(torqueR, -1.0, 1.0));
  
  j1.loopFOC(); j2.loopFOC();
  jZ.loopFOC(); jR.loopFOC();
  
  // 6. 监控
  static unsigned long lastPrint = 0;
  if(millis() - lastPrint > 100) {
    Serial.print("Force:"); Serial.print(forceMeasured);
    Serial.print(" DeltaX:"); Serial.println(deltaX);
    lastPrint = millis();
  }
  
  delay(10);
}

关键设计点：
阻抗控制使末端呈现弹簧-阻尼特性，自适应贴合曲面
虚拟质量/阻尼/刚度参数决定柔顺特性
轨迹沿曲面移动，Z轴阻抗修正保持恒力
适用于不规则曲面加工

3、力/位混合控制 + 磨损自适应
适用场景：精密装配、去毛刺。在约束方向用力控制，在自由方向用位置控制，同时根据累计工作量自适应补偿工具磨损。

/* ===== Arduino BLDC SCARA机器人 — 力/位混合 + 磨损自适应 =====
 * 硬件：4轴SCARA + 末端力传感器 + 主轴编码器
 * 核心：法向力控 + 切向位控 + 工具磨损补偿
 */
#include <SimpleFOC.h>

BLDCMotor j1(5), j2(6), jZ(7), jR(8);

// 选择矩阵(1=力控方向, 0=位控方向)
// SCARA中：Z轴力控，XY位控，R轴位控
const float SELECTION[4] = {0, 0, 1, 0};  // J1,J2位控, Z力控, R位控

// 力控参数(Z轴)
float fn_target = 12.0;
float fn_actual = 0;
float Kp_fn = 0.05, Ki_fn = 0.001;

// 位控参数(XY)
float Kp_xy = 0.8, Kd_xy = 0.1;

// 磨损补偿
float wearAmount = 0;
float wearRate = 0.0002;  // 每秒钟磨损量(mm/s)
unsigned long workTime = 0;
float compensatedZ = 0;

void setup() {
  Serial.begin(115200);
  
  BLDCMotor* motors[] = {&j1, &j2, &jZ, &jR};
  for(auto m : motors) {
    m->controller = MotionControlType::torque;
    m->init(); m->initFOC();
  }
}

void loop() {
  float dt = 0.01;
  
  // 1. 读取力传感器
  fn_actual = analogRead(A0) * 50.0 / 4095.0;
  
  // 2. 磨损量估计
  workTime += 1;
  wearAmount = workTime * wearRate / 100.0;
  wearAmount = constrain(wearAmount, 0, 2.0);
  
  // 3. 力/位混合控制
  // Z轴(力控方向)
  float fn_error = fn_target - fn_actual;
  static float fnIntegral = 0;
  fnIntegral += fn_error * dt;
  float forceCorrection = Kp_fn * fn_error + Ki_fn * fnIntegral;
  
  // XY轴(位控方向) - 圆形轨迹
  static float t = 0;
  t += 0.02;
  float targetX = 100 + 20 * cos(t);
  float targetY = 50 + 20 * sin(t);
  
  float j1Angle, j2Angle;
  scaraInverseKinematics(targetX, targetY, &j1Angle, &j2Angle);
  
  // 4. 混合控制输出(选择矩阵)
  float controlZ = forceCorrection * SELECTION[2];
  float controlJ1 = (j1Angle - j1.shaft_angle) * Kp_xy * (1 - SELECTION[0]);
  float controlJ2 = (j2Angle - j2.shaft_angle) * Kp_xy * (1 - SELECTION[1]);
  
  // 5. 磨损补偿叠加到Z轴
  compensatedZ = controlZ + wearAmount * 0.3;
  
  // 6. 关节力矩输出
  float torque1 = controlJ1 * 50.0 - 2.0 * j1.shaft_velocity;
  float torque2 = controlJ2 * 50.0 - 2.0 * j2.shaft_velocity;
  float torqueZ = compensatedZ * 0.02;
  float torqueR = -0.3 * jR.shaft_velocity;
  
  j1.move(constrain(torque1, -3.0, 3.0));
  j2.move(constrain(torque2, -3.0, 3.0));
  jZ.move(constrain(torqueZ, -2.0, 2.0));
  jR.move(constrain(torqueR, -1.0, 1.0));
  
  j1.loopFOC(); j2.loopFOC();
  jZ.loopFOC(); jR.loopFOC();
  
  // 7. 磨损报警
  if(wearAmount > 1.5) {
    Serial.println("Warning: Tool wear exceeded 1.5mm, replace tool!");
  }
  
  delay(10);
}

关键设计点：
选择矩阵区分力控方向(Z)和位控方向(XY)
磨损量基于累计工作时间估算，自动补偿到Z轴
圆形轨迹测试力/位混合控制性能
磨损超限报警提示更换工具
五点核心要点解读
① SCARA机器人的力控核心是Z轴
SCARA的XY轴刚性大，适合位置控制；Z轴（升降轴）刚性相对较小，适合力控。案例一、二、三都将力控集中在Z轴，XY轴保持位置控制。
② 力控与位控的切换取决于任务阶段
一个典型的装配任务包含多个阶段：
接近阶段：位控(快速移动)
接触阶段：力控(柔顺接触)
装配阶段：力/位混合(插入+力保)
保持阶段：力控(恒力保持)
退出阶段：位控(快速退回)
每个阶段需要不同的控制策略。
③ 阻抗控制的参数整定是关键
案例二中的M_DES/B_DES/K_DES需要根据工件材质和工艺要求调试。
④ 工具磨损补偿是工业级应用的必备功能
砂轮、磨头、刀具在使用过程中会逐渐磨损，如果不补偿：
接触力逐渐下降
加工质量不稳定
需要频繁人工调整
案例三中的磨损补偿基于累计工作时间，虽然简单但实用。更精确的方法是基于主轴功率或力传感器反馈。
⑤ SCARA的运动学逆解是力控的基础
力控需要在任务空间（笛卡尔坐标）中进行，而电机控制需要在关节空间中进行。运动学逆解的质量直接影响力控精度：
奇异性规避：避免在奇异点附近力控失效
解算精度：浮点数精度至少0.01mm
实时性：逆解计算时间<1ms

4、基于末端力传感器的阻抗控制（力/位混合）
适用场景：SCARA机器人执行装配、打磨等需要与工件接触的任务，通过感知接触力主动调整位置，避免刚性碰撞。

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

// 假设使用六维力传感器（通过I2C/SPI读取）
// 本示例简化为单轴力传感器
#define FORCE_SENSOR_PIN A0

// 定义两个关节电机
BLDCMotor motor1(7), motor2(7);
Encoder encoder1(2, 3), encoder2(4, 5);
BLDCDriver3PWM driver1(9,10,11), driver2(5,6,7);

// SCARA运动学参数
const float L1 = 10.0, L2 = 10.0; // 连杆长度(cm)

// 阻抗控制参数
float K_impedance = 2.0;    // 阻抗刚度
float D_impedance = 0.5;    // 阻抗阻尼
float target_force = 0.5;   // 期望接触力(N)

void setup() {
  Serial.begin(115200);
  
  // 初始化电机1
  motor1.linkSensor(&encoder1);
  motor1.linkDriver(&driver1);
  motor1.controller = MotionControlType::angle;
  motor1.P_angle.P = 5.0;
  motor1.init();
  motor1.initFOC();
  
  // 初始化电机2（同上）
  motor2.linkSensor(&encoder2);
  motor2.linkDriver(&driver2);
  motor2.controller = MotionControlType::angle;
  motor2.P_angle.P = 5.0;
  motor2.init();
  motor2.initFOC();
}

void loop() {
  // 1. 读取末端接触力
  int raw = analogRead(FORCE_SENSOR_PIN);
  float force = (raw / 1023.0) * 5.0; // 转换为力值(N)
  
  // 2. 计算力偏差
  float force_error = target_force - force;
  
  // 3. 阻抗控制：力偏差转化为位置修正量
  float pos_correction = K_impedance * force_error + 
                          D_impedance * (force_error - last_force_error);
  last_force_error = force_error;
  
  // 4. 计算目标末端位置（以当前末端位置为基准）
  float target_X = current_X + pos_correction * 0.1;
  float target_Y = current_Y;
  
  // 5. 逆运动学求解关节角度
  float theta1, theta2;
  inverseKinematics(target_X, target_Y, theta1, theta2);
  
  // 6. FOC控制
  motor1.move(theta1);
  motor2.move(theta2);
  motor1.loopFOC();
  motor2.loopFOC();
  
  delay(10);
}

// 逆运动学函数
void inverseKinematics(float x, float y, float &t1, float &t2) {
  float r = sqrt(x*x + y*y);
  t2 = acos((L1*L1 + L2*L2 - r*r) / (2*L1*L2));
  t1 = atan2(y, x) - atan2(L2*sin(t2), L1 + L2*cos(t2));
}

代码要点：本案例实现“阻抗控制”的基本框架。末端力偏差通过阻抗方程(力 = 刚度×位置偏差 + 阻尼×速度)映射为位置修正量，再通过逆运动学转换为关节角度指令，实现末端柔顺特性。

5、基于电流环的力矩补偿（扭矩跟随）
适用场景：SCARA机器人进行精细装配或抛光的恒力控制，通过电机电流间接感知负载力矩，无需额外力传感器。

#include <SimpleFOC.h>

BLDCMotor motor1(7), motor2(7);
// 编码器和驱动器初始化略...

// 力矩控制参数
float K_torque = 0.5;        // 力矩补偿增益
float current_torque1 = 0;   // 电机1当前力矩(从电流推算)
float target_torque = 0.2;   // 期望补偿力矩(N·m)

void setup() {
  // ... 初始化电机(需启用电流采样) ...
  
  // 配置为力矩控制模式
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
}

void loop() {
  // 1. 从FOC获取当前力矩(电流)
  current_torque1 = motor1.current_q * motor1.phase_resistance; // 简化计算
  
  // 2. 力矩偏差
  float torque_error = target_torque - current_torque1;
  
  // 3. 力矩补偿：根据偏差调整关节2的目标力矩
  float compensation = K_torque * torque_error;
  float target_torque2 = 0.3 + compensation; // 基准力矩+补偿
  
  // 4. 执行力矩控制
  motor1.move(target_torque);  // 关节1保持恒力矩
  motor2.move(target_torque2); // 关节2带力矩补偿
  
  motor1.loopFOC();
  motor2.loopFOC();
  
  delay(10);
}

代码要点：本案例利用BLDC的电流环作为力矩感知手段。FOC算法中motor.current_q代表力矩电流分量，通过监测其变化可间接判断末端负载，实现“无传感器的力控”。这与学术研究中“通过嵌入式电流环作为低层力矩调节器”的思路一致。

6、带力觉保护的轨迹自适应（碰触即停/避让）
适用场景：SCARA机器人在人机协作场景中，末端触碰障碍物或人体时自动停止或后退，保障安全。

#include <SimpleFOC.h>

BLDCMotor motor1(7), motor2(7);
// 编码器和驱动器初始化略...

// 安全阈值
const float FORCE_THRESHOLD = 1.0;   // 力阈值(N)
const float COLLISION_DELTA = -0.5;  // 碰撞后退角度(rad)

void setup() {
  // ... 电机初始化 ...
  
  // 设置为位置控制模式
  motor1.controller = MotionControlType::angle;
  motor2.controller = MotionControlType::angle;
}

void loop() {
  // 1. 读取末端力(来自力传感器或电流估算)
  float current_force = readEndEffectorForce();
  
  // 2. 判断是否发生碰撞
  if (current_force > FORCE_THRESHOLD) {
    // 触发安全响应：后退并暂停
    Serial.println("Collision detected! Stopping...");
    
    // 记录当前角度
    float current_angle1 = motor1.shaft_angle;
    float current_angle2 = motor2.shaft_angle;
    
    // 后退一个安全角度
    motor1.move(current_angle1 + COLLISION_DELTA);
    motor2.move(current_angle2 + COLLISION_DELTA);
    
    // 等待到达目标
    while (!motor1.isReached() || !motor2.isReached()) {
      motor1.loopFOC();
      motor2.loopFOC();
      delay(5);
    }
    
    // 保持停止状态2秒
    delay(2000);
    
    // 恢复轨迹(可从预设路径点继续)
    resumeTrajectory();
  } else {
    // 正常轨迹跟踪
    float target_angle1, target_angle2;
    getNextTrajectoryPoint(target_angle1, target_angle2);
    
    motor1.move(target_angle1);
    motor2.move(target_angle2);
    motor1.loopFOC();
    motor2.loopFOC();
  }
  
  delay(10);
}

代码要点：本案例将力感知融入安全监控逻辑。当末端力超过阈值时，触发“事件中断”——暂停轨迹跟踪并执行后退动作，实现碰撞保护。这是工业协作机器人常用的“力觉安全”机制。

五点要点解读
力控的核心是“电流环”与“传感器”双轨制：实现末端力控有两种路径——一是通过末端力传感器直接测量()，二是利用FOC的电流环间接估算力矩。后者成本更低但精度有限，前者更精确但需要额外硬件。

阻抗控制 vs. 导纳控制：阻抗控制将力偏差映射为位置修正(案例一的核心思想)，适用于刚性环境中的柔顺作业；导纳控制则相反，将位置偏差映射为力输出，适用于拖动示教等场景。选择取决于应用目标。

运动学解算是力控的“桥梁”：末端力/位置修正需要通过逆运动学转换为关节角度指令。SCARA机器人的逆运动学相对简单(闭式解)，适合实时计算。正逆解算法的准确性直接影响力控精度。

多轴协调与优先级管理：在SCARA的多关节系统中，力控策略需考虑各轴的优先级。例如案例5中将补偿力矩分配给特定关节，避免“力争夺”现象。SimpleFOC库支持多轴同步控制。

安全机制是力控系统的“保险丝”：案例6展示了力控的防御性编程思路。在协作机器人应用中，力控不仅用于性能提升，更用于安全保护。建议在软件层面(力阈值监控)和硬件层面(急停电路)设置双重安全冗余。