2.6 力位混合控制：接触任务怎么解

2.6 力位混合控制：接触任务怎么解

本节摘要：位置控制在"碰到东西"的瞬间会失守——位置目标的另一面是无穷大力。本节讲清刚度与阻抗的含义，给出阻抗控制的最小实现，并说明装配、打磨、协作拖动三类任务分别怎么配。

一个装销钉的失败实验

把位置控制调到极致的机械臂去插销钉：销对准孔位下压，孔口有一点点偏斜，位置环的答复是"指令说往下 2 毫米，就必须往下 2 毫米"——输出力矩随误差增大直到电机饱和或工件崩坏。这不是参数没调好，而是逻辑死结：位置控制接触物体后，任何位置误差都对应一个力，而刚性位置环追求的恰恰是消灭位置误差，也就是追求无穷大力。

接触任务的本质是控制交互力，而不是控制位置。沿接触面方向保位置、法向控力，这就是力位混合控制；更常用的简化形态是阻抗控制——不去精确跟踪力，而是把末端调校成一个带刚度、阻尼、质量的"虚拟弹簧质量块"，让它跟环境接触时自然表现出柔性。

刚度、阻尼与虚拟质量

阻抗控制的目标动力学写成：

M_d·ẍ + D_d·ẋ + K_d·x = F_ext
​

x 是末端相对参考轨迹的偏差，M_d、D_d、K_d 是虚拟质量、阻尼、刚度三个设计参数。环境把它推开多少（F_ext），它就柔顺地退让多少。三个旋钮的工程含义：

装配插销的常用配置：法向刚度 200 N/m 上下（软，容忍对准误差），切向刚度 2000 N/m（硬，保证沿孔口滑动搜索）；打磨任务法向恒力 5–15 N，用"力误差负反馈到法向速度"的实现更直接。

图：位置控制与阻抗控制接触行为对比

最小实现：从力矩层下手

实现阻抗控制有一条幸运的捷径：它完全不需要力传感器。期望轨迹与实际轨迹的偏差本身就是"虚拟弹簧被拉伸的量"，乘以刚度就是需要输出的虚拟力矩：

def impedance_step(q, v, q_des, v_des, K, D, M_diag, dt):
    """关节空间阻抗控制一步：K、D 为关节刚度/阻尼对角阵"""
    e  = q_des - q            # 位置偏差 = 虚拟弹簧伸长量
    de = v_des - v            # 速度偏差
    tau = M_diag * ( (K * e + D * de) / M_diag )  # 简化：直接输出
    tau = K * e + D * de      # 等效于调校过的"伺服弹簧"
    return tau                 # 交给电流环执行
​

细看会发现它长得像 PD 控制——确实如此：阻抗控制的本质是重新解释 PD 的参数。Kp 不再是"控制增益"而是"物理刚度"，Kd 不再是"阻尼系数"而是"物理阻尼"。视角一换，参数的手感就完全不同了：你在设定机器人的"软硬"，而不是在消灭误差。这也是协作机器人"拖动示教"的原理——把刚度调到接近零、阻尼适度，人手一掰，机器人顺从地跟着走。

当然，有力传感器（六维腕力传感器）时可以做纯力控制：实测力与期望力之差反馈到速度或力矩，精度更高，但传感器成本、标定与滤波是新的负担。选择路径通常是：无传感器阻抗够用就不上力传感器；恒力要求高（打磨、抛光）再上。

三类任务的配置单

• 精密装配（插销、装轴承）：低刚度法向 + 高刚度切向；配合螺旋搜索策略——先垂直软接触，再切向画小圆寻找孔口，力突变即下插。
• 打磨抛光：法向恒力控制（力环），切向位置控制（路径环），两个方向用第 2.4 节同样的思想分别整定。
• 协作拖动示教：零刚度（或极低刚度）+ 关节电流前馈补偿重力与摩擦，人手感是"无阻力"，拖到哪、记录到哪。

⚠️ 常见坑：阻抗参数与环境刚度不匹配。机器人很软（K_d 小）而环境很硬时，接触稳定；机器人很硬而环境也很硬（比如钢对钢），等效刚度极高，极易振荡——阻尼要按接触后的等效系统重整，不能沿用自由空间的参数。

本节要点

• 位置控制的逻辑死结：接触后消灭位置误差等于追求无穷大力，接触任务必须换目标。
• 阻抗控制把末端调成虚拟弹簧-阻尼-质量块，K_d、D_d、M_d 是"软硬手感"的三个旋钮。
• 阻抗控制可无传感器实现——它就是换了物理解释的 PD；纯力控制需力传感器，用于恒力任务。
• 装配、打磨、拖动示教是三种典型配置：软法向硬切向、法向恒力、零刚度。

阻抗与导纳：一对镜像的实现

同一个"柔顺"目标，实现路径有两条且互为镜像。阻抗控制读位置偏差、输出力矩——机器人主动表现得像弹簧（前述方案）；它要求底层力矩接口透明（减速器背隙小、力矩估计准）。导纳控制读力传感器测到的外力、修改位置目标——外力大就后退一点，机器人"被推着走"；它对底层的要求是位置环高品质，力传感器成为必配。选择凭硬件底子：力矩控制底子好（准直驱、输出端编码器）用阻抗；位置控制底子好、已有力传感器用导纳。两条路调好的手感可以非常接近，工程上按"哪边硬件更可信"来选。

接触检测：软着陆前的最后一问

阻抗控制让接触变得安全，但机器人还得先知道"碰没碰到"。无力传感器的接触检测思路朴素而有效：监视位置环的残差——指令在走而编码器不动（或速度骤降而指令未减），即是接触或卡滞的指纹。给残差设阈值，超限即切换到低刚度模式或停机。这套"软件限位器"的响应速度取决于控制周期与检测窗口的长度，典型配置下几十毫秒内可完成判定，足以把撞击力压到无害量级。它与第 5.4 节的功率力限是同一思想在不同层的实现。

问题：装配任务里"找孔"为什么这么难

插销入孔是力控的试金石。难点不在压入（那段只要低刚度），而在寻找孔口：位置误差大于间隙时，销会顶在孔缘上。成熟的策略是把搜索做成显式状态机：软接触下压、检测到顶住、切向画小螺旋搜索、力突变（滑入）即切换为低刚度下压。每个状态的刚度与力阈值都要调：下压刚度过大销会弹跳，搜索速度过快会"跳过"孔口。这类任务的调试本质是把人类装配手感里"边压边晃"的直觉显式参数化——参数表就是手感的数字版。