柔性接触的力位混合控制原理解析

PART 01

控制方法

法向 （垂直于接触面的方向）：用力控制，保持恒定接触力。

切向 （与接触面平行的两个方向）：用位置控制，按照用户指令或轨迹移动。

两个方向的运动互不干扰，在同一个控制周期内叠加。

PART 02

核心原理

原理：在工具坐标系里分通道控制

以写字为例，想象拿着一个笔，笔尖垂直于纸面：

笔的Z轴 = 法向：你控制笔尖压力（力控），不能左右滑动。

笔的X、Y轴 = 切向：你可以让笔在纸面上平移，压力自理。

任何时刻，末端的“工具坐标系”都能从正运动学算出来，它随着姿势实时变化。我们就在这个坐标系里做文章。

PART 03

步骤分解

1. 实时算出现有末端位置和姿态

通过当前关节角度，正运动学推出末端在空间中的准确位姿。从中分离出：

• 位置点 ： p （x, y, z）
• 姿态 ：可以提取出三个轴的朝向
• Z轴 → 法向 n
• X轴 → 切向 t x
• Y轴 → 切向 t y

1. 力控通道：生成法向位移 从力传感器得到法向力 F_meas误差： e=Fdes−Fmeas通过导纳控制，把力误差变换为一个沿法向的小位移：Δz=Kp⋅e(可再加上积分项)如果 e>0（力不够）， Δz>0，笔向前压。如果 e<0（力太大）， Δz<0，笔后退。如果误差很小（在死区内）， Δz=0，笔不动。
2. 轨迹通道：生成切向位移 外部轨迹规划器（比如自动插补或人工摇杆）给出本周期在工具坐标系下期望的切向移动量：沿工具X轴移动： Δx_cmd沿工具Y轴移动： Δy_cmd这些值直接当作位置控制的位移增量（单位：米）。
3. 在工具坐标系内组合（末端） 把一个三维位移拼起来：X分量： Δx_cmd （位置控制）Y分量： Δy_cmd（位置控制）Z分量： Δz（力控制）这就得到了 工具坐标系下的总位移向量 [Δxcmd,Δycmd,Δz]。
4. 转换到基座标系 工具坐标系是跟着末端旋转的，我们需要把位移从“笔的视角”转成“机器人底座的视角”。用末端的旋转矩阵（三个轴的朝向）做转换：Δxbase=tx⋅Δxcmd+ty⋅Δy_cmd+n⋅Δz(向量分量相加)直观理解：工具X方向在基座标下是哪个方向，就把 Δxcmd 沿那个方向加；Y、Z同理。三个方向叠加，得到基座标系下的总位移 Δpbase。
5. 更新目标位置 目标位置 = 当前位置 + Δp_base，姿态保持之前的姿态不变。
6. 逆运动学求关节角

用SCARA逆运动学算法，把新的目标位置和姿态解出六个关节指令，发送给电机。

PART 04

控制结果

法向：始终被力误差驱动，逼近设定力。

切向：独立地执行预设轨迹，不受力控干扰。

因为位移是在工具坐标系内分通道再合成的，两个方向的运动天然解耦。

PART 05

进一步简化理解

力控只关心Z轴进退 。

轨迹只在X/Y平面平移 。

每周期算一次位移，加到当前位置，通过IK变成关节运动。

任何复杂的矩阵运算，都隐藏在“用工具坐标系方向向量乘以位移量再相加”这一步里，物理含义非常清晰。