PART 01 控制方法
在法向（即垂直于接触面的方向）上，我们采用力控策略，确保接触力保持恒定；而在切向（与接触面平行的两个方向）上，则使用位置控制，按预设轨迹移动。关键在于，这两个方向上的运动互不干扰，可在同一控制周期内叠加实现。
PART 02 核心原理
核心原理一句话概括：在工具坐标系中分通道进行控制。
以写字为例——想象你手握一支笔，笔尖垂直压在纸面上：
- 笔的Z轴即为法向：你需要控制笔尖的压力（力控），防止它左右滑动。
- 笔的X、Y轴是切向：你可以让笔在纸面上任意平移，压力自行把控。
在任何时刻，末端的“工具坐标系”均可通过正运动学计算得出，它会随机器人的姿态实时变化。我们要做的，就是在这个坐标系上做文章。
PART 03 步骤分解
实时计算当前末端位置与姿态
通过当前各关节角度，运用正运动学推导出末端在空间中的精确位姿。从中分离得到：
- 位置点：p（x, y, z）
- 姿态：可提取三个轴的朝向——Z轴对应法向n，X轴对应切向tx，Y轴对应切向ty
力控通道：生成法向位移
从力传感器获取法向力Fmeas，误差 e = Fdes - Fmeas。通过导纳控制，将力误差转换为沿法向的小位移：Δz = Kp·e（也可加入积分项）。
- 若 e＞0（力不足），Δz＞0，笔向前压。
- 若 e＜0（力过大），Δz＜0，笔向后缩。
- 若误差很小（处于死区内），Δz=0，笔保持不动。
轨迹通道：生成切向位移
外部轨迹规划器（如自动插补或手动摇杆）给出本周期在工具坐标系下期望的切向移动量：沿工具X轴移动Δxcmd，沿工具Y轴移动Δycmd。这些值直接作为位置控制的位移增量（单位：米）。
在工具坐标系内组合位移
将三维位移拼合在一起：
- X分量：Δxcmd（位置控制）
- Y分量：Δycmd（位置控制）
- Z分量：Δz（力控制）
由此得到工具坐标系下的总位移向量 [Δxcmd, Δycmd, Δz]。
转换到基坐标系
工具坐标系随末端旋转，我们需要将位移从“笔的视角”转换到“机器人底座的视角”。利用末端的旋转矩阵（三个轴的朝向）进行转换：
Δpbase = tx·Δxcmd + ty·Δycmd + n·Δz（向量分量相加）
直观理解：工具X方向在基坐标系下指向哪个方向，就将Δxcmd沿该方向叠加；Y、Z同理。三个方向叠加后，即获得基坐标系下的总位移Δpbase。
更新目标位置
目标位置 = 当前位置 + Δpbase，姿态保持原先姿态不变。
逆运动学求解关节角
运用SCARA逆运动学算法，将新的目标位置与姿态解算出六个关节指令，发送给电机执行。
PART 04 控制结果
法向：始终由力误差驱动，持续逼近设定的目标力值。
切向：独立执行预设轨迹，不受力控干扰。
由于位移是在工具坐标系内分通道再合成，两个方向上的运动天然解耦，各走各路。
PART 05 进一步简化理解
简而言之，力控只关注Z轴的进退，轨迹仅在X/Y平面平移。每个周期计算一次位移，累加到当前位置，再通过逆运动学转换为关节运动。
任何复杂的矩阵运算，都隐藏在“用工具坐标系方向向量乘以位移量再求和”这一步中，物理含义非常清晰——无非就是你朝哪个方向推笔、推多少的问题。
