机械臂精密装配实践：导纳控制实现、六维力标定与真机调参记录

搞工业装配或者精密操作的朋友，十有八九都经历过这种崩溃瞬间：在仿真环境或者上位机视觉里看，定位精度到了 0.2 mm，信心满满地下发关节轨迹去插销子，结果“哐”的一声，机器人本体直接报过载报警停机，工件当场被顶歪甚至报废。

原因很简单：工业机械臂本质上是高刚度机构，微米级的定位偏差在刚性接触下，都会被放大成数十牛甚至数百牛的瞬态冲击力。光靠视觉加纯位置控制（Position Control）做精密装配，容错率几乎为零。

今天聊聊我们在真机上做精密装配（比如间隙 0.05 mm 以内的 Peg-in-Hole 轴孔装配）的常用打法：六维力传感器配置、导纳控制回路搭建，以及几套踩过坑才总结出来的工程搜索策略。

一、 控制方案选型：为什么多数场景选导纳（Admittance）？

在做力控时，常聊的概念是阻抗（Impedance）与导纳（Admittance）：

• 阻抗控制：输入是位移偏差，输出是关节力矩（$\tau = K \Delta q + D \Delta \dot{q}$）。这需要机器人底层开放力矩控制接口（如 Franka Panda、优傲等部分带力矩反馈的本体）。
• 导纳控制：外环采集末端六维力传感器的力误差，经过虚拟质量-阻尼-刚度系统计算出期望位移偏移量 $\Delta X$，然后叠加到目标位姿，下发给机械臂的位置/速度伺服接口。

对于大部分工业臂（ABB、KUKA、发那科、或者国产主流协作臂），底层的关节力矩环通常不开源或周期延迟较大。工程落地最稳妥、兼容性最好的方案，往往是外环导纳控制 + 内环高频位置插补。

导纳方程的经典形式： $$M_d (\ddot{x}_c - \ddot{x}_d) + D_d (\dot{x}_c - \dot{x}d) + K_d (x_c - x_d) = F{ext}$$

在稳态装配场景中，为了防止系统超调引起高频抖动，我们通常将虚拟质量项 $M_d$ 设为 0 或者极小，降阶为一阶阻尼系统（Damper Model），让力误差直接映射为末端线速度与角速度。

二、 最小可用的导纳控制核心实现

下面是一段简化的一阶离散导纳控制器核心逻辑。输入为末端传感器测得的外力，输出为发送给机器人的位移补偿量 $\Delta x$。

#include <Eigen/Dense>
#include <iostream>

class AdmittanceController {
public:
    AdmittanceController(double loop_dt) : dt_(loop_dt) {
        // 设置阻尼系数 D 与 刚度系数 K (此处针对末端 6 自由度: x, y, z, roll, pitch, yaw)
        damping_.diagonal() << 200.0, 200.0, 300.0, 15.0, 15.0, 15.0;
        stiffness_.diagonal() << 0.0, 0.0, 100.0, 0.0, 0.0, 0.0; // 平面方向设为纯阻尼，无回弹刚度
        deadband_ << 1.0, 1.0, 1.5, 0.05, 0.05, 0.05; // 传感器死区过滤
    }

    Eigen::Matrix<double, 6, 1> update(
        const Eigen::Matrix<double, 6, 1>& f_measured,
        const Eigen::Matrix<double, 6, 1>& f_desired) 
    {
        Eigen::Matrix<double, 6, 1> f_err = f_measured - f_desired;

        // 死区过滤，抑制小幅传感器白噪声
        for (int i = 0; i < 6; ++i) {
            if (std::abs(f_err(i)) < deadband_(i)) {
                f_err(i) = 0.0;
            } else {
                f_err(i) -= std::copysign(deadband_(i), f_err(i));
            }
        }

        // 一阶导纳方程: D * v + K * delta_x = F_err
        // 推导期望修正速度: v = D.inverse() * (F_err - K * delta_x)
        Eigen::Matrix<double, 6, 1> v_cmd = 
            damping_.inverse() * (f_err - stiffness_ * delta_x_accumulated_);

        // 速度限幅，工程保命机制
        clampVelocity(v_cmd);

        // 积分得到周期位移偏差
        Eigen::Matrix<double, 6, 1> dx = v_cmd * dt_;
        delta_x_accumulated_ += dx;

        return dx;
    }

    void reset() {
        delta_x_accumulated_.setZero();
    }

private:
    void clampVelocity(Eigen::Matrix<double, 6, 1>& v) {
        const double max_trans_vel = 0.03; // 最大平移速度 30 mm/s
        const double max_rot_vel = 0.1;    // 最大旋转速度 0.1 rad/s
        for (int i = 0; i < 3; ++i) {
            v(i) = std::max(std::min(v(i), max_trans_vel), -max_trans_vel);
        }
        for (int i = 3; i < 6; ++i) {
            v(i) = std::max(std::min(v(i), max_rot_vel), -max_rot_vel);
        }
    }

    double dt_;
    Eigen::DiagonalMatrix<double, 6> damping_;
    Eigen::DiagonalMatrix<double, 6> stiffness_;
    Eigen::Matrix<double, 6, 1> deadband_;
    Eigen::Matrix<double, 6, 1> delta_x_accumulated_{Eigen::Matrix<double, 6, 1>::Zero()};
};

三、 精密装配的标准三部曲

有了底层的顺应能力，上层的装配策略才能跑起来。一个标准的轴孔装配，在工业现场通常拆为三步：

[粗定位至上方 2mm] 
       ↓
[阶段 1: 接触探寻 (Touch Probe)] ---> 达到接触力阈值 (如 5N)，判定接触孔口端面
       ↓
[阶段 2: 引导对中 (Spiral Search)] ---> 平面阿基米德螺线搜索 + 轴向恒力下压
       ↓
[阶段 3: 顺应插配 (Insertion)] ---> 深度下降 + 姿态自适应去过约束力矩

1. 接触探寻（Contact Detection） 机械臂以较低速度（如 5~10 mm/s）沿轴向（Z 轴）匀速下行，实时监控 $F_z$。一旦 $F_z$ 越过设定的接触阈值（例如 3~5 N），说明轴端面已与孔口端面发生碰撞，立即刹车并切换至搜索态。

2. 微动对中：阿基米德螺旋线搜索（Spiral Search） 单纯依靠视觉无法弥补几微米的偏差，此时在 XY 平面上施加阿基米德螺旋线运动： $$r(t) = a \cdot \theta(t)$$ $x(t) = r(t) \cos(\theta(t)), \quad y(t) = r(t) \sin(\theta(t))$ 同时保持 Z 方向施加微小的恒力压迫（如 5 N）。当轴端滑落到孔口倒角瞬间，$F_z$ 会出现微小的断崖式下跌，且 Z 方向位置发生微小突降，此时触发搜索成功，锁死当前 XY 坐标。

3. 轴线纠偏与插配（Alignment & Push） 很多人在下插时常犯的错是只管往下按，结果卡死（Jamming）。下插过程中，孔壁会给轴施加侧向力 $F_x, F_y$ 和倾覆力矩 $M_x, M_y$。 此时需要开启姿态顺应：当检测到 $M_x$ 时，让末端绕 X 轴顺着力矩方向微调角度，使得力矩趋近于零。让轴“顺着”孔的真实内径滑下去，而不是和硬结构硬刚。

四、 老唐的真机踩坑清单

在实验室仿真里看曲线都很平滑，但在真机联调时，以下几点如果没处理好，几乎必炸机：

1. 工具端重力补偿（Gravity Compensation）必须做准

六维力传感器安装在机械臂法兰与末端夹爪之间。当机械臂姿态倾斜时，夹爪和夹持工件的重力分量在传感器坐标系下是持续变化的。

• 踩坑现象：机械臂倾斜 30 度，算法误以为外表面受力了 10 N，机械臂顺应乱漂。
• 解法：在装配开始前，做多姿态静态标定，利用最小二乘法解算出工具的质量 $m$ 和质心偏移量 $[c_x, c_y, c_z]$，实时扣除重力分量： $$F_{sensor}^{raw} = F_{ext} + R_{base}^{sensor} \cdot G_{tool}$$

2. 通信延迟与时钟抖动（Jitter）

• 力传感器如果走 Modbus-RTU 串口，通信周期往往在 20~50 ms 且抖动很大。用这个做力控闭环，系统大概率发散震颤。
• 建议基线：六维力传感器尽量走 EtherCAT、CANopen 或高速 UDP，闭环控制周期稳定在 2 ms ~ 5 ms（200 Hz ~ 500 Hz）之间，工控机必须打 PREEMPT_RT 实时补丁。

3. 接触瞬间的刚度切换

空气中运动与刚性接触的边界很突兀。纯阻尼模型在刚接触硬金属时，由于冲击力上升沿太陡，计算出的修正速度很容易超限。

• 解法：传感器原始数据必须做低阶低通滤波（如一阶 RC 滤波，截止频率设在 20~40 Hz），并设置位移/速度增量饱和（Saturation Clamp），宁可动作慢一点，坚决杜绝震荡。

4. 软保护：三道防线

装配任务里，条件分支必须要包含应急回滚逻辑：

• 第一道防线：力偏差超限警告，立即停止步进位移。
• 第二道防线：力持续超限时间超过 200 ms，立即执行原路反向抽回 2 mm。
• 第三道防线：硬件极限急停（E-Stop），绑定到机器人底层的安全控制器。

五、 总结

精密装配是具身智能和机器人从“能展示”迈向“能进厂”的分水岭。不管上层是用大模型规划还是用示教强化学习策略，最底层的力控顺应能力始终是托底的安全网。

把底层的导纳方程写严密，把重力补偿与坐标系变换推清楚，先在 100 Hz 以上的闭环下实现稳定的对中与插拔，再把上层的视觉检测或装配逻辑套进来，整个系统的鲁棒性才会真正立得住。