机械臂接触力控工程实践：阻抗与导纳控制实现及真机调参踩坑指南

做机械臂这几年，不管是早期的自动化装配、打磨，还是现在给具身智能 / VLA 做真机执行器，大家迟早都会撞上一堵墙：单纯的位置控制在接触任务里极易翻车。

工件稍微有一点公差，机械臂要么因为刚性过大直接把末端或工件顶爆，要么触发驱动器的过流保护停机。解决接触问题的正道，就是力控。

今天老唐系统理一下机械臂阻抗控制（Impedance Control）与导纳控制（Admittance Control）的底层逻辑，并给出一套基于工业/协作臂常见接口的导纳控制落地实现与调参避坑清单。

一、 阻抗还是导纳：先搞清楚你的硬件能给什么接口

很多人刚接触时容易把阻抗控制和导纳控制混为一谈。从物理因果律上讲，这两者是互为倒数的对偶关系：

1. 阻抗控制（Impedance Control）：

   • 输入位移，输出力。即外界改变它的位置，它根据设定的质量-阻尼-刚度模型产生反作用力。
   • 硬件要求：底层必须支持高频的关节力矩控制（Joint Torque Control，通常需要 1kHz 以上，例如 Franka Emika FCI、KUKA iiwa 的 FRI，或者自研带双编码器/力矩传感器的关节）。
   • 特点：响应极快，理论上限最高，但对机器人动力学模型（惯量矩阵、科氏力、重力项）精度极敏感，模型不准会直接发散。

2. 导纳控制（Admittance Control）：

   • 输入外力，输出位移/速度。末端六维力传感器测到外力，控制器通过虚拟弹簧-阻尼模型算出机械臂应该顺应偏移的位移或速度，再下发给机器人的位置环或速度环。
   • 硬件要求：末端安装六维力/力矩传感器（F/T Sensor），机械臂开放实时位置或速度下发接口（例如 UR 的 RTDE、川崎/ABB 的低延迟位置流接口、或者基于 ROS 2 的 ros2_control ForwardCommandController）。
   • 特点：对机器人本体动力学模型依赖较低，可以用在绝大多数工业协作臂上，工程落地门槛低，但带宽受限于机器人的位置内环。

在系统集成和大部分真实工业/具身场景中，因为我们用的大多不是纯力矩机械臂，导纳控制是最实用、性价比最高的选择。

二、 导纳控制核心模型与离散化

在末端笛卡尔空间，我们通常将机械臂等效为一个二阶质量-阻尼-刚度系统（Mass-Damper-Spring）：

$$M_d (\ddot{x}_d - \ddot{x}_c) + D_d (\dot{x}_d - \dot{x}c) + K_d (x_d - x_c) = F{ext}$$

其中：

• $x_d, \dot{x}_d, \ddot{x}_d$：期望的末端位姿、线/角速度、加速度。
• $x_c, \dot{x}_c, \ddot{x}_c$：导纳修正后实际发给机械臂的指令。
• $F_{ext}$：经过去重力与偏置校准后的末端外力。
• $M_d, D_d, K_d$：虚拟惯量、虚拟阻尼、虚拟刚度对角矩阵。

为了在实时循环（比如 500Hz，周期 $\Delta t = 0.002s$）中计算，我们通常将 $M_d$ 设为对角阵或者直接简化为一阶系统（若机器人本体伺服环响应较慢，忽略惯量项 $M_d$，只保留阻尼和刚度能让系统更稳定）。

采用标准欧拉法对二阶系统做离散化更新：

$$\ddot{x}_c[k] = \ddot{x}d[k] - M_d^{-1} \left( F{ext}[k] - D_d (\dot{x}_d[k] - \dot{x}_c[k-1]) - K_d (x_d[k] - x_c[k-1]) \right)$$ $$\dot{x}_c[k] = \dot{x}_c[k-1] + \ddot{x}_c[k] \Delta t$$ $$x_c[k] = x_c[k-1] + \dot{x}_c[k] \Delta t$$

三、 核心工程实现示例 (C++)

下面是一个适合在 ros2_control 的 update() 或独立 500Hz 线程中调用的笛卡尔一维导纳控制器示例（以 Z 轴接触力控为例）：

#include <iostream>
#include <algorithm>

class AdmittanceController1D {
public:
    AdmittanceController1D(double m, double d, double k, double dt)
        : m_(m), d_(d), k_(k), dt_(dt),
          pos_actual_(0.0), vel_actual_(0.0), acc_actual_(0.0) {}

    // f_ext: 剔除自重后的力测量值 (N)
    // pos_des: 上层路径规划给出的期望位置 (m)
    // f_des: 期望保持的接触力 (N), 如恒力按压时为正
    double update(double pos_des, double vel_des, double f_ext, double f_des) {
        // 计算力跟踪误差与位置误差
        // 注意方向定义: 假设向下为正，外部阻力向上
        double force_error = f_ext - f_des;
        double pos_error = pos_des - pos_actual_;
        double vel_error = vel_des - vel_actual_;

        // 动力学方程: M * (a_d - a) + D * (v_d - v) + K * (p_d - p) = f_error
        // 整理得到当前步加速度
        acc_actual_ = (force_error + d_ * vel_error + k_ * pos_error) / m_;

        // 积分得到速度和位置
        vel_actual_ += acc_actual_ * dt_;
        
        // 速度限幅，防止受力突变时剧烈飞车
        vel_actual_ = std::clamp(vel_actual_, -max_vel_, max_vel_);
        
        pos_actual_ += vel_actual_ * dt_;

        return pos_actual_; // 下发给逆运动学求解或直接送入伺服驱动器
    }

    void reset(double init_pos) {
        pos_actual_ = init_pos;
        vel_actual_ = 0.0;
        acc_actual_ = 0.0;
    }

private:
    double m_;  // 虚拟质量
    double d_;  // 虚拟阻尼
    double k_;  // 虚拟刚度
    double dt_; // 控制周期 (s)
    
    double pos_actual_;
    double vel_actual_;
    double acc_actual_;
    
    const double max_vel_ = 0.1; // 100 mm/s 软限速
};

四、 接触力控真机落地踩坑清单

在模拟器里（如 MuJoCo、Isaac Sim）跑力控通常都很丝滑，但一旦部署到真机，十有八九会遇到振荡或掉线。这里总结几个关键坑点：

1. 六维力传感器的重力与零点漂移

这是导致机械臂静止时自己乱飘的首要原因。

• 现象：机械臂改变姿态时，力传感器读数剧烈变化，导致导纳控制器误判外界受力。
• 解法：必须做末端负载动力学校准。设末端工具质量为 $m_t$，质心偏置为 $r_t$。在空载状态下，遍历空间多个姿态（至少 12 个），记录姿态矩阵 $R_b^s$ 与传感器原始力数据，使用最小二乘法精确求解出末端负载的质心坐标与重力向量，并在控制循环中实时剔除： $$F_{ext} = F_{raw} - R_s^b \cdot [0, 0, -m_t g]^T$$

2. 通信抖动与 PREEMPT_RT

• 现象：机械臂刚接触硬物表面，瞬间发生高频剧烈“哒哒哒”敲击，甚至直接过载急停。
• 原因：控制回路出现周期抖动（Jitter）。力控属于高刚度闭环，如果系统非实时，一次 10ms 的延迟可能导致下发位移过大，下一帧传感器读出力激增，又以更大的速度反弹，形成发散振荡（Contact Instability）。
• 解法：
  • 控制机必须打 PREEMPT_RT 补丁。
  • 绑核隔离 CPU（isolcpus），将力控通信线程绑定到独立核心，设置调度策略为 SCHED_FIFO，优先级设为 95 以上。

3. 刚度矩阵 $K_d$ 与接触刚度的失配

• 调试准则：
  • 自由空间运动：调大刚度 $K_d$，此时导纳控制器退化为普通位置跟踪，轨迹更精准。
  • 硬接触场景（金属打金属、装配）：把接触轴的刚度 $K_d$ 调小，甚至设为 0（变为纯质量-阻尼系统，适合做恒力跟踪），主要靠阻尼 $D_d$ 吸收能量。
  • 临界阻尼经验：为了避免系统振荡，通常让虚拟系统处于过阻尼或临界阻尼状态，经验公式为 $D_d \ge 2 \sqrt{M_d K_d}$。

4. 传感器数据的低通滤波延迟

• 六维力传感器由于高频噪声（特别是本体减速器谐波引起的振动），通常需要滤波。
• 千万不要用大窗口的滑动平均滤波，那会引入几十毫秒的相位滞后，直接摧毁控制环路稳定性。建议使用一阶巴特沃斯低通滤波器（Butterworth Filter），截止频率设在 20Hz - 50Hz 之间，在滤除高频噪声和保留动态响应之间取折中。

五、 总结

做力控本质上是在处理能量的传递与耗散。位置控制假设环境刚度为零，而接触力控必须时刻考虑环境刚度的耦合。

在工程落地上，如果你使用的是常见工业臂或协作臂，先老老实实做一套高质量的末端重力补偿，把系统跑在 500Hz + RT 内核上，从一阶导纳模型（阻尼-刚度）开始调参，稳住后再做接触，能帮你少走很多弯路。后续有时间，我们再聊聊如何把大模型/VLA 输出的高层语义动作转换成底层的力控顺应参数。