在基于 Arduino 平台的 SCARA（选择性顺应装配机器人）BLDC 控制系统中，“阻抗控制 + 曲面跟踪”是一种结合了高级柔顺力控与复杂空间轨迹规划的前沿架构。该方案主要用于解决工件存在公差或装夹误差时，机械臂末端需保持恒定接触力并贴合复杂轮廓的难题。以下从专业视角为您详细解析其主要特点、应用场景及注意事项：
一、 主要特点
基于弹簧-阻尼模型的阻抗控制（Impedance Control）
阻抗控制将机器人与环境的交互关系简化为一个“弹簧-阻尼-质量”系统。在 SCARA 机器人的笛卡尔空间中，外环阻抗控制器通过测量末端的位置误差和外部作用力，实时计算系统期望的加速度，从而生成柔顺的运动轨迹。这使得机械臂在接触曲面时，能够像弹簧一样“顺从”外力，而不是刚性对抗，从而实现恒定的法向接触力控制。
基于逆动力学的内环解耦与力矩输出
为了精确执行阻抗控制生成的期望轨迹，系统内环需采用基于逆动力学模型（Inverse Dynamics）的控制策略。通过实时计算并补偿系统自身的惯性、科里奥利力、重力及关节摩擦力，逆动力学控制器将高度耦合的非线性系统解耦为简单的线性系统。其核心输出是精确的关节力矩（转矩）指令，由底层 FOC（磁场定向控制）驱动 BLDC 电机精准执行。
复杂曲面的拟合与自适应参数调节
针对自由曲面的复杂性，系统需结合曲面拟合算法（如 5 次多项式函数插值）来规划平滑的跟踪路径，避免干涉和瓶颈。同时，为克服工件公差带来的影响，常引入模糊控制理论或自适应阻抗控制策略，在线动态调节阻抗参数（如刚度、阻尼系数），以保证对法向接触力的恒定跟踪，降低超调量与振荡次数。
多关节协同与 S 曲线平滑轨迹
SCARA 机器人的曲面跟踪需要多关节的精密配合。系统通过逆运动学（IK）将笛卡尔空间的曲面路径解算为各关节的目标角度，并对所有关节同步执行 S 形速度曲线（S-curve Trajectory）插补。S 曲线通过限制加加速度（Jerk），确保启停过程无冲击，抑制机械谐振，使末端在曲面上的滑动极其平滑。
二、 应用场景
该架构主要适用于对接触力敏感、需适应工件误差的轻型精密加工与交互场景：
精密打磨与去毛刺作业：这是该架构最典型的应用。在粉尘大、环境恶劣的打磨场景中，自适应阻抗控制能保证打磨动力头对自由曲面施加恒定的法向压力，避免因工件装夹误差导致的过切或漏磨，显著提升表面加工质量。
柔顺装配与插拔：在精密电子元器件或齿轮的装配中，阻抗控制允许机械臂在遇到装配阻力时主动退让并调整姿态，实现“零力拖动”或柔顺插入，避免刚性冲击损坏零件。
医疗与康复辅助：在上肢康复训练设备中，阻抗控制可主动控制末端表现的“刚度”和“阻尼”，模拟不同康复阶段所需的辅助力矩，确保患者运动的安全与舒适。
科研与算法验证：作为高校和研究机构验证机器人动力学、力位混合控制、曲面拟合等前沿理论的绝佳低成本实验平台。
三、 需要注意的事项
算力瓶颈与 MCU 选型
阻抗控制与逆动力学模型涉及大量的矩阵运算、雅可比矩阵求逆及三角函数，对处理器的算力要求极高。标准的 Arduino Uno（ATmega328P）完全无法胜任。必须选用高性能 MCU，如 Teensy 4.x（Cortex-M7）、ESP32（双核分工）或 STM32F4/F7，甚至采用上位机（PC/树莓派）进行模型计算，再将转矩指令下发给 Arduino 执行。
动力学模型的精确性与参数辨识
阻抗控制与逆动力学的性能极度依赖于系统模型的准确性。如果质量、质心、惯性张量等参数存在误差，会直接导致力控失准或系统振荡。必须通过精细的系统辨识实验获取准确参数，或在控制回路外增加鲁棒/自适应控制器来补偿模型的不确定性。
底层 FOC 驱动与传感器精度
必须选择支持直接电流/转矩控制模式的 BLDC 驱动器，并采用 FOC 算法以消除换相阶跃，实现极低的转矩脉动。此外，高精度绝对式磁编码器（如 AS5600、MA732）应安装在减速器输出轴（关节侧）以补偿背隙；若缺乏昂贵的六维力传感器，需通过高精度电流采样结合动力学模型进行“无传感器力估计”。
S 曲线参数匹配与多轴同步
S 曲线的最大速度、加速度和加加速度（Jerk）必须通过实验标定，以匹配机械系统的动态特性。过高的 Jerk 会丧失平滑优势，过低则延长作业时间。在多关节协同中，必须确保各轴共享同一时间基准（如定时器中断触发），避免某轴“抢跑”导致机械应力集中或末端轨迹畸变。
安全机制与异常处理
由于涉及物理接触与力控，系统必须具备极高的安全性。软件上需设置力矩/速度软限幅、碰撞检测阈值（如监测 Iq 电流突增）及编码器丢脉冲检测；硬件上必须配备直连驱动使能引脚的急停按钮，在发生失控或过流时立即切断电机动力，防止设备损坏或人员受伤。

1、基于末端力传感器的位置型阻抗控制（基础曲面跟踪）
适用场景：在已知轮廓的曲面上进行打磨或抛光，通过六维力传感器感知接触力，用阻抗控制将力偏差转化为位置修正量。

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

// ==================== 关节定义 ====================
BLDCMotor motor1(7), motor2(7);  // SCARA两个旋转关节
// 需补充Encoder和Driver初始化...

// ==================== 运动学参数 ====================
const float L1 = 10.0, L2 = 10.0;  // 连杆长度(cm)
const float END_EFFECTOR_OFFSET = 2.0;  // 末端偏移

// ==================== 阻抗控制器参数 ====================
// 目标阻抗：M_d * (xdd - xd_dd) + B_d * (xd - xd_d) + K_d * (x - xd) = F_d - F
const float M_d = 1.0;   // 惯性矩阵
const float B_d = 10.0;  // 阻尼矩阵
const float K_d = 50.0;  // 刚度矩阵

// 力控PID（外环）
double targetForce = 2.0;        // 目标接触力(N)
double currentForce = 0;
double forceError = 0;
double posCompensation = 0;
double Kp_f = 0.5, Ki_f = 0.02, Kd_f = 0.01;
PID forcePID(&currentForce, &posCompensation, &targetForce, Kp_f, Ki_f, Kd_f, DIRECT);

// ==================== 全局变量 ====================
float currentX = 5.0, currentY = 8.0;  // 当前末端位置
float desiredX = 5.0, desiredY = 8.0;  // 期望末端位置

void setup() {
  Serial.begin(115200);
  
  // 电机初始化
  motor1.controller = MotionControlType::angle;
  motor2.controller = MotionControlType::angle;
  motor1.init(); motor2.init();
  motor1.initFOC(); motor2.initFOC();
  
  // PID初始化
  forcePID.SetMode(AUTOMATIC);
  forcePID.SetOutputLimits(-0.5, 0.5);
  
  // 力传感器初始化...
}

void loop() {
  motor1.loopFOC(); motor2.loopFOC();
  
  // 1. 读取末端力（假设通过I2C读取）
  currentForce = readForceSensor();
  
  // 2. 力误差计算
  forceError = targetForce - currentForce;
  
  // 3. 阻抗控制：力偏差→位置修正
  // 简化为PID映射（完整阻抗需二阶微分方程）
  forcePID.Compute();
  
  // 4. 基础轨迹（沿曲面轮廓前进）
  static float t = 0;
  t += 0.01;
  // 示例：沿正弦曲面跟踪
  float baseX = 5.0 + 3.0 * sin(t);
  float baseY = 8.0 + 1.0 * cos(t);
  
  // 【核心】位置补偿叠加到Y方向（曲面法向）
  desiredX = baseX;
  desiredY = baseY + posCompensation * 0.3;
  
  // 5. 逆运动学
  float theta1, theta2;
  inverseKinematics(desiredX, desiredY, theta1, theta2);
  
  // 6. BLDC执行
  motor1.move(theta1);
  motor2.move(theta2);
  
  // 调试输出
  Serial.print("Force:"); Serial.print(currentForce);
  Serial.print(" Comp:"); Serial.println(posCompensation);
  
  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));
}

代码要点：本案例实现了阻抗控制的基础框架——力偏差通过PID映射为位置修正量，叠加到基础轨迹上。这种"位置型阻抗控制"适合Arduino平台，因为内环是位置控制，外环是力控制，结构清晰。

2、基于电流环的无力传感器阻抗控制（经济型曲面跟踪）
适用场景：成本敏感的应用，通过检测BLDC电机的电流变化间接感知接触力，无需额外力传感器。适合在抛光、打磨等振动环境下使用，因为力传感器容易受损。

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

// ==================== 关节定义 ====================
BLDCMotor motor1(7), motor2(7);
// 需补充Encoder和Driver初始化...

// ==================== 运动学参数 ====================
const float L1 = 10.0, L2 = 10.0;
const float JACOBIAN_SCALE = 0.08;  // 雅可比缩放

// ==================== 阻抗参数 ====================
float K_impedance = 30.0;   // 阻抗刚度
float D_impedance = 8.0;    // 阻抗阻尼
float targetForce = 1.5;    // 目标接触力(N)

// ==================== 外力估计相关 ====================
float estimatedForce = 0;
float lastTorque = 0;

void setup() {
  Serial.begin(115200);
  
  // 电机初始化为力矩控制模式
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
  motor1.init(); motor2.init();
  motor1.initFOC(); motor2.initFOC();
}

void loop() {
  motor1.loopFOC(); motor2.loopFOC();
  
  // 1. 【核心】从FOC电流环估算接触力
  // motor.current_q 正比于电磁力矩
  float torque1 = motor1.current_q * motor1.phase_resistance;
  float torque2 = motor2.current_q * motor2.phase_resistance;
  
  // 雅可比转置将关节力矩映射为末端力
  float theta1 = motor1.shaft_angle;
  float theta2 = motor2.shaft_angle;
  float J11 = -L1*sin(theta1) - L2*sin(theta1+theta2);
  float J12 = -L2*sin(theta1+theta2);
  float J21 = L1*cos(theta1) + L2*cos(theta1+theta2);
  float J22 = L2*cos(theta1+theta2);
  
  // 简化：只估计Y方向力
  float F_est = (torque1 + torque2) * JACOBIAN_SCALE;
  estimatedForce = 0.7 * estimatedForce + 0.3 * F_est;  // 低通滤波
  
  // 2. 阻抗控制：力误差→速度修正
  float forceError = targetForce - estimatedForce;
  
  // 基于位置的阻抗控制：速度修正 = 力误差 * 增益
  float velCorrection = forceError * 0.02;
  
  // 3. 基础轨迹（沿曲面切线方向）
  static float t = 0;
  t += 0.02;
  float baseX = 8.0 + 2.0 * sin(t);
  float baseY = 6.0 + 0.8 * cos(t);
  
  // 计算期望位置（加入阻抗修正）
  float targetX = baseX;
  float targetY = baseY + velCorrection * 0.5;
  
  // 4. 逆运动学→关节角度
  float theta1_cmd, theta2_cmd;
  inverseKinematics(targetX, targetY, theta1_cmd, theta2_cmd);
  
  // 5. 切换到位置模式执行
  motor1.controller = MotionControlType::angle;
  motor2.controller = MotionControlType::angle;
  motor1.move(theta1_cmd);
  motor2.move(theta2_cmd);
  
  // 6. 返回力矩模式准备下一轮
  motor1.controller = MotionControlType::torque;
  motor2.controller = MotionControlType::torque;
  
  Serial.print("F_est:"); Serial.print(estimatedForce);
  Serial.print(" Comp:"); Serial.println(velCorrection);
  
  delay(10);
}

代码要点：本案例利用BLDC电机的电流环作为"电子测力计"。通过FOC的motor.current_q获取力矩电流分量，结合雅可比转置估算末端接触力。这种"无力传感器"方案在抛光打磨场景中特别有优势，因为振动和热传导会影响力传感器可靠性。

3、力位混合控制的曲面自适应跟踪（带法向速度反馈）
适用场景：更复杂的曲面跟踪任务，采用Raibert-Craig力位混合控制框架，将任务空间正交分解为切向（位置控制）和法向（力控制），并引入法向速度反馈抑制振荡。

#include <SimpleFOC.h>
#include <PID_v1.h>

// ==================== 关节定义 ====================
BLDCMotor motor1(7), motor2(7);
// 需补充Encoder和Driver初始化...

// ==================== 运动学参数 ====================
const float L1 = 10.0, L2 = 10.0;

// ==================== 力位混合控制参数 ====================
// 正交分解：切向用位置控制，法向用力控制
float targetForce = 2.0;        // 目标法向力(N)
float currentForce = 0;

// 位置环PID（切向）
double posError = 0, posOutput = 0;
double Kp_pos = 2.0, Ki_pos = 0.0, Kd_pos = 0.5;
PID posPID(&posError, &posOutput, 0, Kp_pos, Ki_pos, Kd_pos, DIRECT);

// 力环PID（法向）
double forceError = 0, forceOutput = 0;
double Kp_f = 1.5, Ki_f = 0.05, Kd_f = 0.01;
PID forcePID(&forceError, &forceOutput, 0, Kp_f, Ki_f, Kd_f, DIRECT);

// ==================== 曲面法向估计 ====================
float normalAngle = 0;  // 曲面法向角(rad)

void setup() {
  Serial.begin(115200);
  
  motor1.controller = MotionControlType::angle;
  motor2.controller = MotionControlType::angle;
  motor1.init(); motor2.init();
  motor1.initFOC(); motor2.initFOC();
  
  posPID.SetMode(AUTOMATIC);
  posPID.SetOutputLimits(-0.5, 0.5);
  forcePID.SetMode(AUTOMATIC);
  forcePID.SetOutputLimits(-0.3, 0.3);
}

void loop() {
  motor1.loopFOC(); motor2.loopFOC();
  
  // 1. 读取末端力
  currentForce = readForceSensor();
  
  // 2. 估计曲面法向（通过力方向或轮廓几何）
  // 简化：假设曲面轮廓已知，法向由轨迹导数给出
  static float t = 0;
  t += 0.01;
  float dx = -3.0 * sin(t);
  float dy = -1.0 * cos(t);
  normalAngle = atan2(dx, dy) + PI/2;
  
  // 3. 【核心】任务空间正交分解
  // 切向单位向量（沿轨迹方向）
  float tanX = cos(normalAngle + PI/2);
  float tanY = sin(normalAngle + PI/2);
  
  // 法向单位向量（指向曲面内部）
  float normX = cos(normalAngle);
  float normY = sin(normalAngle);
  
  // 4. 基础轨迹（切向位置控制）
  float desiredTangential = t * 0.5;  // 沿切线前进
  float currentPos = getCurrentPosition();
  posError = desiredTangential - currentPos;
  posPID.Compute();
  
  // 5. 力控制（法向）
  forceError = targetForce - currentForce;
  forcePID.Compute();
  
  // 6. 合成末端速度指令
  float vx = posOutput * tanX + forceOutput * normX;
  float vy = posOutput * tanY + forceOutput * normY;
  
  // 【核心】法向速度反馈：抑制振荡
  // 通过监测法向速度分量，增加阻尼
  static float lastNormVel = 0;
  float normVel = vx * normX + vy * normY;
  float damping = (normVel - lastNormVel) * 0.05;
  lastNormVel = normVel;
  
  // 法向速度反馈修正
  vx -= damping * normX * 1.5;
  vy -= damping * normY * 1.5;
  
  // 7. 位置更新
  currentX += vx * 0.01;
  currentY += vy * 0.01;
  
  // 8. 逆运动学
  float theta1, theta2;
  inverseKinematics(currentX, currentY, theta1, theta2);
  
  motor1.move(theta1);
  motor2.move(theta2);
  
  // 调试输出
  Serial.print("F:"); Serial.print(currentForce);
  Serial.print(" N_vel:"); Serial.println(normVel);
  
  delay(10);
}

代码要点：本案例采用力位混合控制框架。将末端任务空间正交分解为切向（位置控制）和法向（力控制），两轴独立控制。特别引入了法向速度反馈——这是抑制曲面跟踪中力振荡的关键技术。实验表明，配置相关力振荡会随操作空间位置和力方向剧烈变化，法向速度反馈可大幅降低振荡。

要点解读
阻抗控制的本质是"力→位置"的转换逻辑：阻抗控制通过"质量-阻尼-弹簧"二阶模型描述末端与环境的相互作用，将力偏差映射为位置/速度修正量。根据内环控制方式分为基于位置的阻抗控制（适合已有位置控制器的工业机器人）和基于力矩的阻抗控制（响应更快但需精确动力学模型）。对于Arduino+BLDC方案，基于位置的阻抗控制更易实现。

无力传感器估计是经济型方案的关键技术：通过FOC的电流环监测motor.current_q（力矩电流分量），结合雅可比转置可估算末端接触力。这在抛光打磨等振动环境中尤为有价值，因为力传感器易受损。核心公式为：τ_e = τ_d + τ_f - nIKt，需辨识关节摩擦模型提高估计精度。

任务空间正交分解是曲面跟踪的理论基础：Raibert-Craig混合控制将任务空间分解为切向（位置控制）和法向（力控制）两个正交子空间。切向负责沿曲面轮廓运动，法向负责维持恒定接触力。这种分解使位置控制和力控制互不干扰，是实现精确曲面跟踪的经典框架。

法向速度反馈是抑制力振荡的有效手段：研究表明，SCARA机器人在曲面跟踪时，配置相关动力学特性会导致显著的力振荡。引入法向速度反馈（在力环中增加速度项）可有效阻尼振荡，改善跟踪性能。实现方式为在力控制器输出中叠加法向速度的负反馈。

控制周期与关节刚度直接影响跟踪精度：SCARA机器人的力控曲面跟踪对控制周期有严格要求。推荐控制频率≥100Hz（10ms周期），力环和位置环的更新频率需一致。此外，关节刚度会影响力控带宽——刚度越高，力响应越快，但越容易产生振荡。需根据实际工况调节阻抗参数。

4、平面曲面恒力跟踪
适用场景：手机玻璃抛光、平板电脑外壳打磨。SCARA末端在XY平面内沿曲面轨迹运动，Z轴阻抗控制保持恒定接触力。

/* ===== Arduino BLDC SCARA机器人 — 平面曲面恒力跟踪 =====
 * 硬件：4轴SCARA(J1,J2,Z,R) + 末端力传感器
 * 核心：XY平面轨迹跟踪(位控) + Z轴阻抗控制(恒力)
 *       曲面轨迹预编程(正弦波/圆弧)
 */
#include <SimpleFOC.h>

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 = 8.0;   // 目标接触力(N)
float forceActual = 0;

// 阻抗参数(Z轴)
const float M_IMP = 0.3;      // 虚拟质量(kg)
const float B_IMP = 8.0;      // 虚拟阻尼(Ns/m)
const float K_IMP = 150.0;    // 虚拟刚度(N/m)

// 阻抗状态
float deltaZ = 0, deltaVz = 0;  // Z轴位置修正

// 曲面轨迹参数
float trajTime = 0;
const float TRAJ_SPEED = 20;    // 跟踪速度(mm/s)
const float AMPLITUDE = 5;      // 曲面起伏幅度(mm)
const float PERIOD = 40;        // 曲面周期(mm)

void setup() {
  Serial.begin(115200);
  
  // 初始化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;
  
  // 2. 阻抗控制(Z轴)
  float forceError = forceActual - forceTarget;
  float accel = (forceError - B_IMP * deltaVz - K_IMP * deltaZ) / M_IMP;
  deltaVz += accel * dt;
  deltaZ += deltaVz * dt;
  
  // 3. 曲面轨迹生成(XY平面)
  trajTime += TRAJ_SPEED * dt;
  
  float nominalX = 100 + trajTime;                          // X方向匀速
  float nominalY = 50 + AMPLITUDE * sin(2*PI*trajTime/PERIOD);  // Y方向正弦起伏
  float nominalZ = 30 + deltaZ;                             // Z轴含阻抗修正
  
  // 4. SCARA运动学逆解
  float j1Angle, j2Angle;
  scaraInverseKinematics(nominalX, nominalY, &j1Angle, &j2Angle);
  
  // 5. 关节控制
  // J1,J2: 位置控制(刚度控制)
  float torque1 = 80.0 * (j1Angle - j1.shaft_angle) - 5.0 * j1.shaft_velocity;
  float torque2 = 80.0 * (j2Angle - j2.shaft_angle) - 5.0 * j2.shaft_velocity;
  
  // Z轴: 阻抗力控
  float torqueZ = (forceError * 0.03 + deltaZ * 0.1);
  
  // R轴: 保持
  float torqueR = -0.5 * jR.shaft_velocity;
  
  j1.move(constrain(torque1, -4.0, 4.0));
  j2.move(constrain(torque2, -4.0, 4.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 > 50) {
    Serial.print("Force:"); Serial.print(forceActual);
    Serial.print(" dZ:"); Serial.println(deltaZ);
    lastPrint = millis();
  }
  
  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));
}

关键设计点：
XY平面位置控制 + Z轴阻抗控制
预编程曲面轨迹（正弦波）
阻抗参数决定柔顺特性
实时力反馈调节Z轴位置

5、三维曲面自适应跟踪
适用场景：汽车内饰曲面打磨、家具曲面抛光。SCARA末端在三维空间沿复杂曲面运动，阻抗控制自适应贴合曲面。

/* ===== Arduino BLDC SCARA机器人 — 三维曲面自适应跟踪 =====
 * 硬件：4轴SCARA + 末端力传感器 + 激光位移传感器(测距)
 * 核心：三维曲面轨迹 + 法向阻抗控制 + 切向速度自适应
 *       激光传感器预扫描曲面轮廓
 */
#include <SimpleFOC.h>
#include <Math.h>

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

// 力传感器
float fx, fy, fz;

// 激光测距(用于预扫描曲面)
float surfaceProfile[200];  // 曲面轮廓数据
int profilePoints = 0;

// 阻抗参数
const float M_3D = 0.4;
const float B_3D = 10.0;
const float K_3D = 200.0;

// 曲面法向估计
float normalX, normalY, normalZ;

// 跟踪状态
float trackProgress = 0;
const float TRACK_LENGTH = 100;  // 跟踪总长度(mm)

void scanSurface() {
  // 预扫描曲面轮廓
  for(int i=0; i<200; i++) {
    float x = i * 0.5;  // 每0.5mm一个点
    // 读取激光测距
    float laserDist = analogRead(A1) * 0.1;
    surfaceProfile[i] = laserDist;
    profilePoints++;
    
    // 移动到下一个扫描点
    float j1Angle, j2Angle;
    scaraInverseKinematics(x, 50, &j1Angle, &j2Angle);
    j1.move(50*(j1Angle-j1.shaft_angle)-3*j1.shaft_velocity);
    j2.move(50*(j2Angle-j2.shaft_angle)-3*j2.shaft_velocity);
    delay(10);
  }
}

void estimateSurfaceNormal(int idx) {
  // 根据曲面轮廓估计法向
  if(idx > 0 && idx < profilePoints-1) {
    float dzdx = (surfaceProfile[idx+1] - surfaceProfile[idx-1]) / 1.0;
    normalX = -dzdx;
    normalY = 0;
    normalZ = 1.0;
    
    // 归一化
    float norm = sqrt(normalX*normalX + normalY*normalY + normalZ*normalZ);
    normalX /= norm;
    normalY /= norm;
    normalZ /= norm;
  }
}

void setup() {
  Serial.begin(115200);
  
  BLDCMotor* motors[] = {&j1, &j2, &jZ, &jR};
  for(auto m : motors) {
    m->controller = MotionControlType::torque;
    m->init(); m->initFOC();
  }
  
  // 预扫描曲面
  scanSurface();
  Serial.println("Surface scanned");
}

void loop() {
  float dt = 0.01;
  
  // 1. 读取力传感器
  readForce3D(&fx, &fy, &fz);
  
  // 2. 计算跟踪位置
  trackProgress += 0.5;  // 0.5mm/步
  int idx = (int)trackProgress;
  if(idx >= profilePoints) {
    trackProgress = 0;
    idx = 0;
  }
  
  // 3. 估计曲面法向
  estimateSurfaceNormal(idx);
  
  // 4. 阻抗控制(法向)
  float forceNormal = fx*normalX + fy*normalY + fz*normalZ;
  float forceError = forceNormal - forceTarget;
  
  static float deltaN = 0, deltaVn = 0;
  float accel = (forceError - B_3D * deltaVn - K_3D * deltaN) / M_3D;
  deltaVn += accel * dt;
  deltaN += deltaVn * dt;
  
  // 5. 计算目标位置(曲面轮廓 + 法向修正)
  float targetX = trackProgress;
  float targetY = 50;
  float targetZ = surfaceProfile[idx] + deltaN * normalZ;
  
  // 6. 运动学逆解和控制
  float j1Angle, j2Angle;
  scaraInverseKinematics(targetX, targetY, &j1Angle, &j2Angle);
  
  float torque1 = 80*(j1Angle-j1.shaft_angle) - 5*j1.shaft_velocity;
  float torque2 = 80*(j2Angle-j2.shaft_angle) - 5*j2.shaft_velocity;
  float torqueZ = forceError * 0.02 + deltaN * 0.05;
  float torqueR = -0.3 * jR.shaft_velocity;
  
  j1.move(constrain(torque1, -4.0, 4.0));
  j2.move(constrain(torque2, -4.0, 4.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();
  delay(10);
}

关键设计点：
激光预扫描获取曲面轮廓
法向估计用于阻抗控制方向
切向匀速跟踪，法向阻抗修正
适用于已知但复杂的曲面

6、未知曲面探索跟踪
适用场景：文物修复、艺术品表面处理。SCARA末端在未知曲面上探索，通过力传感器实时感知曲面形状并自适应跟踪。

/* ===== Arduino BLDC SCARA机器人 — 未知曲面探索跟踪 =====
 * 硬件：4轴SCARA + 末端六维力传感器
 * 核心：无先验知识 → 力传感器感知曲面 → 实时建图 → 自适应跟踪
 *       探索与跟踪同时进行
 */
#include <SimpleFOC.h>

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

// 力传感器
float fx, fy, fz;

// 曲面点云(实时构建)
struct SurfacePoint {
  float x, y, z;
  float nx, ny, nz;  // 法向
};
SurfacePoint pointCloud[500];
int cloudCount = 0;

// 探索参数
float exploreX = 0, exploreY = 0;
const float EXPLORE_STEP = 1.0;  // 探索步长(mm)
const float FORCE_EXPLORE = 5.0; // 探索接触力(N)

// 阻抗参数(探索模式)
const float M_EXPLORE = 0.2;
const float B_EXPLORE = 5.0;
const float K_EXPLORE = 100.0;

void addPointToCloud(float x, float y, float z, float nx, float ny, float nz) {
  if(cloudCount < 500) {
    pointCloud[cloudCount++] = {x, y, z, nx, ny, nz};
  }
}

void estimateLocalNormal(int idx, float* nx, float* ny, float* nz) {
  // 从点云中估计局部法向
  if(idx < 3) {
    *nx = 0; *ny = 0; *nz = 1;
    return;
  }
  
  // 使用最近3个点拟合平面
  SurfacePoint p0 = pointCloud[idx];
  SurfacePoint p1 = pointCloud[idx-1];
  SurfacePoint p2 = pointCloud[idx-2];
  
  float v1x = p1.x - p0.x, v1y = p1.y - p0.y, v1z = p1.z - p0.z;
  float v2x = p2.x - p0.x, v2y = p2.y - p0.y, v2z = p2.z - p0.z;
  
  // 叉积
  *nx = v1y*v2z - v1z*v2y;
  *ny = v1z*v2x - v1x*v2z;
  *nz = v1x*v2y - v1y*v2x;
  
  float norm = sqrt((*nx)*(*nx) + (*ny)*(*ny) + (*nz)*(*nz));
  if(norm > 0.001) {
    *nx /= norm; *ny /= norm; *nz /= norm;
  }
}

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. 读取力传感器
  readForce3D(&fx, &fy, &fz);
  
  // 2. 当前末端位置
  float curX = j1.shaft_angle * 100;  // 简化映射
  float curY = j2.shaft_angle * 100;
  float curZ = jZ.shaft_angle * 10;
  
  // 3. 探索运动(蛇形扫描)
  exploreX += EXPLORE_STEP;
  if(exploreX > 100) {
    exploreX = 0;
    exploreY += 5;  // Y方向步进
  }
  
  // 4. 阻抗控制(保持接触)
  float forceMag = sqrt(fx*fx + fy*fy + fz*fz);
  float forceError = forceMag - FORCE_EXPLORE;
  
  static float deltaExplore = 0, deltaVexp = 0;
  float accel = (forceError - B_EXPLORE * deltaVexp - K_EXPLORE * deltaExplore) / M_EXPLORE;
  deltaVexp += accel * dt;
  deltaExplore += deltaVexp * dt;
  
  // 5. 记录曲面点
  float nx, ny, nz;
  estimateLocalNormal(cloudCount, &nx, &ny, &nz);
  addPointToCloud(exploreX, exploreY, curZ + deltaExplore, nx, ny, nz);
  
  // 6. 运动控制
  float targetZ = curZ + deltaExplore;
  float j1Angle, j2Angle;
  scaraInverseKinematics(exploreX, exploreY, &j1Angle, &j2Angle);
  
  float torque1 = 60*(j1Angle-j1.shaft_angle) - 3*j1.shaft_velocity;
  float torque2 = 60*(j2Angle-j2.shaft_angle) - 3*j2.shaft_velocity;
  float torqueZ = forceError * 0.03;
  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(cloudCount % 50 == 0) {
    Serial.print("Scanned points: "); Serial.println(cloudCount);
  }
  
  delay(10);
}

关键设计点：
无先验知识，实时探索曲面形状
力传感器感知接触力，保持恒力探索
实时构建曲面点云
局部法向估计用于后续跟踪

要点解读
① 阻抗控制是曲面跟踪的核心技术
阻抗控制使机器人末端呈现弹簧-阻尼特性，能够自适应贴合曲面：
F = Ma + Bv + K*x
力误差 → 位置修正
刚度K决定贴合紧密程度
阻尼B决定稳定性
案例一、二、三都基于这个原理。
② 曲面跟踪需要“法向”和“切向”分离控制
案例二中的法向估计就是为了实现这种分离控制。
③ 曲面先验知识的多少决定了控制策略
案例三实时探索+点云构建
④ 力传感器精度直接影响跟踪质量
曲面跟踪要求力传感器：
分辨率：<0.1N
响应速度：>100Hz
温漂：<0.01N/°C
建议使用六维力传感器（如SRI M3715C），成本约2000~5000元。
⑤ 实际部署建议：逐层递进
不要一开始就在未知曲面上运行。推荐步骤：
在平面上调试阻抗参数
在已知曲面上验证跟踪精度
在未知曲面上运行探索模式
合并探索数据和跟踪控制

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