机械臂零力拖动示教从入门到实战：六维力传感器与导纳控制完整指南

机械臂装上六维力传感器，满怀期待地准备做零力拖动示教，结果一松手机械臂直接砸下去，或者轻轻一推就疯狂震荡，这种情况我见过太多次了。很多刚接触这个方向的朋友以为零力拖动就是"读力、发指令"这么简单，真正上手才发现处处是坑。这篇文章从六维力传感器的选型标定、导纳控制的核心逻辑、代码落地到调参流程，把整个链路完整梳理一遍，希望能帮你少走弯路。

1. 零力拖动为什么"处处是坑"：从重力、摩擦到惯量

先把问题定义清楚。零力拖动示教的本质是让操作者握住机械臂末端，以很小的力就能把机械臂拉到任意期望位置，同时机械臂要表现出"顺从"的柔顺特性，一松手就稳稳停住，不漂移、不反弹。市面上很多协作机械臂自带拖动示教功能，但对于自研机械臂、实验室平台或者改装的老旧设备，自己实现一套基于六维力传感器的导纳控制依然是主流方案。

1.1 一个反直觉的结论：零力拖动不是把力控到零

很多人第一次做这个项目时，会陷入一个误区：以为零力拖动就是让机械臂控制器把外力抵消掉，让力传感器读数趋近于零。听起来很合理，但实际做起来会发现两个致命的逻辑问题。

第一个问题是，机械臂本身的物理属性决定了它不可能"凭空消失"。一台几十公斤的机械臂，在重力、关节摩擦和惯性共同作用下，你想要让末端悬停在某个位置，关节电机本来就需要输出很大的力矩来维持。当你试图把这个维持力矩"抵消"到零，机械臂会直接因为重力作用崩下去，这是物理定律决定的，不是控制器的问题。

第二个问题是，力传感器的读数里包含的不仅是操作者施加的力，还包含机械臂自身结构产生的惯性力和重力分量。当机械臂运动时，连杆的加速度会反映到传感器读数上，形成一个"虚假外力"。如果控制器把这个虚假外力也当成操作者意图去响应，系统就会陷入正反馈震荡——越动越晃，越晃越大力。

理解这个问题，零力拖动的本质就很清楚了：它不是力的闭环，而是位置的柔顺整形。我们要做的不是让力为零，而是让「操作者施加的力」经过一个虚拟质量-弹簧-阻尼系统的整形，映射成末端的期望位移或速度，再交给机械臂的位置/速度控制器去执行。

1.2 机械臂本体的干扰源：重力补偿是第一道门槛

在所有干扰源里，重力是最大的敌人。一台常见的六轴协作机械臂，负载加上末端执行器可能有几公斤重，在某个姿态下，重力在传感器上产生的力分量可能高达几十牛顿。相比之下，操作者拖动时施加的力通常只有几牛顿。如果重力补偿做不好，你施加的拖动力完全被淹没在重力分量里，机械臂根本不会按你的意图运动。

重力补偿的关键不只是算一个简单的"mg"，而是要对机械臂各连杆和负载的质心位置做准确建模。因为机械臂姿态变化时，每一根连杆的重力在传感器坐标系下的投影都在变，这个变化是非线性的。以六维力传感器安装在腕部为例，重力补偿的数学模型大致是：

f_sensor = f_operator + f_load_gravity τ_sensor = τ_operator + p_com × f_load_gravity

其中f_load_gravity是末端负载（包括法兰、工具、被抓物体）的重力在传感器坐标系下的矢量，p_com是负载质心在传感器坐标系下的位置。实际工程中，f_load_gravity可以通过机械臂当前姿态矩阵和负载的质量、质心参数实时计算出来。

很多初学者的做法是拿一个水平仪把机械臂调到某个固定姿态，然后测一组力数据做"零点"，这种做法只对固定姿态有效，机械臂一换姿态，补偿就失效了。正确做法是建立负载的质量和质心参数辨识流程，在机械臂运动到多个不同姿态时记录力和力矩数据，用最小二乘法拟合出负载质量和质心位置。这个过程很像给机械臂做一次"称重体检"。

1.3 摩擦力和惯量的影响：为什么拖起来总有"顿挫感"

重力补偿做好之后，第二道坎是摩擦力。机械臂关节里的减速器、轴承、密封圈都会产生摩擦力，这个力方向总是与运动方向相反。低速拖动时，静摩擦和库仑摩擦占了主导，你会明显感觉到一种"黏滞感"——推一下动一下，一停就卡住。

摩擦力很难通过力传感器前馈完全补偿，因为摩擦模型本身就不精确，而且温度、润滑状态都会影响摩擦系数。比较务实的做法是在导纳控制器里加入一个适当的速度阻尼项，配合很小的加速度前馈，让机械臂在低速段表现得"丝滑"一些。另外，机械臂运动时各连杆的惯性耦合也会让末端表现出"沉重感"，这需要对机械臂的惯量矩阵做补偿，但在零力拖动场景下，导纳控制器的虚拟惯量选择得当，可以在一定程度上掩盖真实惯量的影响。

2. 六维力传感器的选型、标定与读数的"脏数据"问题

导纳控制的效果上限，很大程度上取决于六维力传感器数据的质量。这里说的"质量"不只是精度，更重要的是信噪比、更新率和噪声水平。传感器数据不给力，后端的导纳控制器再花哨也白搭。

2.1 选型时容易被忽略的三个参数

六维力传感器的核心参数，不同厂家宣传的侧重点不太一样，但实际落地时我优先关注这三个：

• 量程和过载保护：量程不能只看额定负载，要看冲击载荷。拖动示教过程中，操作者可能会突然用力或者不小心磕碰，传感器的瞬间受力可能是正常工况的几倍。选购时至少要留出1.5到2倍的过载余量，而且要确认传感器本身有机械限位保护，否则一次意外就能让几万块的传感器报废。

• 串扰和精度等级：六维力传感器内部是应变片桥路，理想情况下六个通道相互独立，实际会有串扰。便宜的传感器串扰可能达到2%到5%满量程，高端产品能做到0.1%以下。导纳控制对力的准确性要求不像精密测量那么苛刻，串扰在1%以内一般够用，不必盲目追求实验室级别的高精度。

• 采样率和实时性：力控系统至少要100Hz以上的控制频率才能"跟手"，如果传感器采样率只有几十赫兹，控制环路延时会非常明显。现在的以太网或CAN接口传感器普遍能到500Hz到1000Hz，基本够用。需要注意的是，传感器驱动要有稳定的实时通信保障，否则数据掉包会直接导致控制抖动。

2.2 标定不只是"去皮"，还要处理偏心载荷

六维力传感器的标定包含两个层面：一个是由传感器厂家完成的出厂标定，建立了应变信号到六维力/力矩的映射关系；另一个是你自己需要做的"负载标定"，也就是把法兰、末端工具等附加结构的重力特性标定出来。

实际调试中，很多人只做了"去皮"——在某个姿态下把传感器读数清零，然后就直接开始调导纳控制。这样做在机械臂位置固定时勉强能用，一旦机械臂改变姿态，重力在传感器坐标系的投影方向变了，零点就飘了。

正确的负载标定流程是这样的：

1. 机械臂处于水平零位，记录传感器读数作为初始基准；
2. 让机械臂依次运动到至少5到10个不同的姿态，每个姿态停留，记录力传感器读数和对应的机械臂末端齐次变换矩阵；
3. 利用最小二乘法求解负载质量和质心位置参数，目标是让所有姿态下补偿后的残差最小；
4. 验证标定结果——让机械臂走几个标定时没用过的姿态，检查补偿后传感器读数是否接近零。

标定之后还涉及一个动态补偿的问题。末端负载在快速加减速时，传感器会读数惯性力，虽然导纳控制器本身对这部分有一定的容忍度，但如果加速过猛，惯性力造成的假拖动方向会让人感觉"怪怪的"。工程上的折中方案是控制末端拖动的期望加速度上限，让机械臂的响应尽量平滑。

2.3 滤波处理：如何平衡"平滑"和"灵敏"

六维力传感器的原始数据噪声一般都不小，尤其是在电磁环境复杂的车间里，读数上叠加的噪声可能有正负0.5N甚至更多。导纳控制本质上是一个积分过程，噪声会导致末端位置持续漂移，所以滤波是必须的。

常用的滤波手段有三种：滑动窗口均值滤波、低通滤波（一阶RC或二阶巴特沃斯）、以及基于模型的状态估计。我个人的经验是，滑动窗口均值滤波最简单，但会引入与窗口长度成正比的时间延迟；一阶低通滤波参数好调，但相位滞后会影响"跟手"感；二阶巴特沃斯滤波在平坦度和截止特性上更均衡，适合做中等性能的系统。

滤波截止频率的选择很考验经验。截止频率设得太低，噪声是被滤掉了，但操作者给的力信号也被延迟，拖动感觉像"隔着手套摸东西"；设得太高，噪声穿透到控制环，机械臂末端会高频微抖。对于典型的协作机械臂拖动示教场景，2到10Hz的低通截止频率是一个合理区间，具体数值要根据机械臂的刚度和重量来调。另外，滤波一定要放在重力补偿之后，否则补偿量的高频抖动会被滤波器放大到位置指令里。

3. 导纳控制的核心逻辑：它不是力闭环，而是"受力—位移"整形器

导纳控制被讨论的频率很高，但真正理解它的人其实不多。从控制理论的角度看，导纳控制的本质是定义了外力和期望运动之间的动态关系——把机械臂末端等效成一个由质量、阻尼、弹簧组成的虚拟二阶系统。操作者施加的力是输入，末端位移修正是输出。

3.1 质量问题：为什么导纳模型里有"虚拟质量"

导纳控制的标准方程是：

Md × (d²x_e/dt²) + Dd × (dx_e/dt) + Kd × x_e = F_ext

其中x_e是末端期望位移修正量，F_ext是操作者施加的外力，Md、Dd、Kd分别是虚拟质量、虚拟阻尼和虚拟刚度。这个方程描述的关系是：外力作用在虚拟系统上，产生了多少位移修正。

虚拟质量Md的作用是抑制高频力的影响。如果Md取得太小，操作者施加一个冲击力时，加速度修正量会瞬间变得非常大，机械臂末端会猛冲一下，非常危险。如果Md取得太大，机械臂反应又会变得迟钝，拖起来感觉"笨重"。严格来说，Md应该尽可能匹配机械臂末端的真实等效惯量，这样外力做功的效率最高，拖动最自然。实际工程里常见做法是先测一次机械臂末端的等效质量，然后以这个值为基础上下调整。

3.2 为什么刚度不能设为零：零力拖动不是"零刚度"

虚拟刚度Kd是导纳控制器里最微妙的一个参数。从直觉上看，零力拖动应该把Kd设成0，让外力完全决定位置，不受任何"弹簧回拉"效果的影响。但仔细想就会发现，一旦Kd=0，系统的稳态特性就变成了：只要有外力就持续产生位移，外力消失时位移停在当前位置。

这听起来似乎正是我们想要的，但在工程上根本做不到。首先，绝对零刚度意味着系统没有位置恢复性，任何微小的偏置力（比如重力补偿残差、传感器零漂）都会让末端持续漂移，根本稳稳不住。更关键的是，Kd=0时系统变成一个纯积分结构，外力的微小波动会被不断积分放大，末端位置会出现肉眼可见的低频晃动。

所以实际导纳控制里，Kd设的是一个很小的非零值，它的作用不是"回拉"，而是给位置修正量提供一个微弱的"锚定"。在稳定状态下，位置修正量满足x_e = F_ext / Kd，对于小阻尼、低刚度配置，这个修正量对于几牛顿的外力来说只有几毫米，操作者几乎感知不到，不会觉得"有弹簧拉着"。但正是这微小的刚度，让系统拥有了抵抗漂移的能力。

3.3 阻尼比的物理意义：从"发抖"到"跟手"的密钥

虚拟阻尼Dd决定了系统的能量耗散快慢，它的选取比前两个参数更需要手感调试。用控制理论的话说，导纳控制器的特征方程为：

s² + (Dd/Md)s + (Kd/Md) = 0

阻尼比ζ = Dd / (2√(Md×Kd))是一个无量纲参数，它直接决定了机械臂对外力的响应模式。

• ζ 远小于1：系统欠阻尼，操作者给一个阶跃力，末端会围绕目标位置来回振荡好几次，拖动的感觉就是"机械臂在发抖"，非常廉价。
• ζ 约等于1：临界阻尼，末端快速平滑地到达目标位置，不回弹、不振荡，这是拖动示教最理想的状态。
• ζ 远大于1：过阻尼，末端运动变得缓慢，"拖不动"的感觉明显，操作者需要用更大的力气才能推动。

调参的一个实用技巧是：先把Kd固定在一个较小的值，根据期望的拖动响应速度推算Md，然后按照Dd = 2ζ√(Md×Kd)计算Dd的初值，ζ从1.0往上调。我实测下来，ζ取1到1.5之间时，机械臂的柔顺感和启停响应最接近人手对"顺滑"的直觉感知。

4. 代码落地：从力读数到末端期望位移的完整链路

理论讲了一堆，最终要落到代码里。我以ROS环境 + C++为主讲一下完整链路，因为这是机器人领域最常用的组合。控制框架上，机械臂底层最好是位置控制模式，频率越高越好，力控频率建议至少500Hz以上，1kHz会舒服很多。

4.1 控制框架的选型：为什么建议用位置接口做力控

有些机械臂支持力矩接口（比如直接给关节力矩指令），但大多数商用机械臂的力矩接口要么带宽不够，要么有保护机制限制，直接用起来很别扭。位置接口反而是最通用、最稳定的方式——你给控制器发一个末端位姿或关节位置的增量指令，底层伺服自己去处理动力学问题。

用位置接口做力控的核心思路是：把导纳控制器的输出当作位置的修正量，叠加到当前的期望位置上，形成新的期望位置指令。位置控制器的跟踪带宽如果是几十赫兹，导纳控制器的带宽就不能超过这个范围，否则会发生"指令追不上"的情况。这也是为什么前文强调滤波截止频率不能太高的原因——整个控制链路的带宽是层层受限的。

4.2 力信号处理和控制器的核心代码

下面是一段典型的零力拖动控制主循环伪代码，实际工程里还要加通信超时保护、安全监视等逻辑：

// 控制主循环，频率1kHz void controlLoop() { // 1. 读取六维力传感器数据 Force6D f_raw = ft_sensor.read(); // 2. 重力补偿：利用当前机械臂末端位姿计算负载重力分量 Eigen::Matrix4d T_base_tool = robot.getToolPose(); Force6D f_compensated = f_raw - computeGravityCompensation(T_base_tool); // 3. 滤波：低通滤波器，截止频率5Hz Force6D f_filtered = lowpassFilter(f_compensated, cutoff_freq); // 4. 静区处理：小于阈值(如1.5N)的力视为0，防止零漂和噪声导致漂移 if (f_filtered.force.norm() < DEADZONE_FORCE) { f_filtered.setZero(); } // 5. 导纳控制律：受力 -> 期望位移增量 static double dt = 0.001; static Eigen::Vector3d vel = Eigen::Vector3d::Zero(); static Eigen::Vector3d pos_offset = Eigen::Vector3d::Zero(); Eigen::Vector3d acc = (f_filtered.force - Dd * vel - Kd * pos_offset) / Md; vel += acc * dt; pos_offset += vel * dt; // 6. 限制修正速度，防止冲击 clamp(vel, MAX_TRANSLATION_VEL); clamp(pos_offset, MAX_DRAG_RANGE); // 7. 计算期望末端位姿并发送给机械臂 Eigen::Matrix4d T_target = T_base_tool; T_target.translation() += pos_offset; robot.setToolPose(T_target); }

这段代码在力和位移的映射上只展示了平移部分，完整实现还要处理旋转。旋转方向的导纳控制和平移方向本质相同，但力的映射变成了力矩到位姿增量的转换，通常使用旋转向量（axis-angle）来表达，避免欧拉角的万向锁问题。姿态部分的虚拟惯性、虚拟阻尼参数一般和平移部分独立调节，因为末端对于旋转方向的"手感"往往比平移动更敏感。

4.3 静区、限幅和安全监控：看不见但保命的细节

代码里的静区处理非常关键，但它常常被初学者忽略。传感器的零漂和噪声会让经过滤波的力信号也存在零点附近的微小波动，如果没有静区，机械臂会在静止状态下慢慢"溜走"——有时候是每分钟几毫米，看起来不明显，但会给拖动手感带来一种"没停稳"的印象。静区阈值取1到2N比较合适，低于这个值的读数一律置零。代价是操作者需要用大于静区阈值的力才能让机械臂动起来，这在实际使用中几乎感知不到。

限幅是对导纳控制器输出做速度限制和位移限制。速度限制防止操作者用力过猛导致机械臂高速撞击；位移限制本质上是一个工作空间限制，防止机械臂被"拉"到奇异点或者超出关节行程。我在实际项目中至少会设置三层安全保护：软件限幅、控制器缓冲区的紧急停止指令、以及急停硬件回路。任何一层触发，系统都要立即停止。

4.4 基于ROS的模块划分

如果项目基于ROS，我建议把整个系统拆成三个独立节点：

• ft_sensor_node：负责读传感器、做重力补偿、发布滤波后的力/力矩数据。这个节点只做数据处理，不参与控制决策，保证传感器的采集频率稳定。
• admittance_control_node：订阅处理后数据，运行导纳控制律，输出末端期望位姿增量。核心控制逻辑全在这个节点里，方便单独调试参数。
• robot_driver_node：与机械臂底层通信，执行期望位姿指令，同时反馈当前位姿状态。

模块化的好处是，调导纳参数的时候不用反复编译整套代码，用动态调参工具实时修改Md、Dd、Kd，效率高很多。我自己常用的流程是拉起三个终端，一个跑传感器节点，一个跑控制节点，另一个开可视化工具观察末端位姿轨迹和力曲线，一边看曲线一边调参数。

5. 调参流程：如何从"发抖"调到"跟手"，以及示教轨迹的记录与回放

参数调到什么程度算"好"？主观标准是"跟手"——你想让机械臂往哪走，它就往哪走，速度适中，没有反弹和延迟，松开手它稳稳停在当前位置。客观标准可以用阶跃响应来看：给一个恒定的拖动力，观察末端位移曲线，应该平滑上升并稳定，没有超调和振荡。

5.1 一套可复现的调参顺序

调参的顺序和策略比我刚做这个项目时想象得更重要。我总结了一套从"平庸"调到"顺手"的固定顺序：

1. 先把基础控制频率跑稳。确认力传感器、控制节点、机械臂驱动的通信都在1kHz左右稳定运行，没有周期抖动。
2. 调大虚拟质量到真实惯量水平。可以先不滤波或者滤波很浅，用手推机械臂，感受"起步"是否自然。
3. 从高刚度、高阻尼的"僵硬"状态开始。把Kd设到相对较大的值，Dd也设大一些，确认机械臂不会自己飘，然后逐步降低Kd。
4. 降低刚度到"柔顺但有韧性"的状态。这个阶段操作者应该感觉"要用力才能拖得动，但拖起来很稳"，此时加入力传感器滤波，把截止频率从10Hz往下调。
5. 最后调阻尼。固定Kd和Md，逐步降低Dd，观察末端是否出现抖动；如果出现持续振荡，马上升高Dd，找到"临界不抖"的位置，再往回留20%到30%的余量。

拉力的维度也可以参考，但最终要以实际手感为准。控制理论和手感出现冲突时，信手感。

5.2 调参中常见的异常现象和根因

整理几个高频问题的排查思路，很多都是实战中踩过的坑：

松手回弹问题值得单独说一句。导纳控制器在恒力作用下会有稳态位置修正量x_e = F_ext/Kd，当操作者突然松手，外力消失，位置会以指数形式回到原始参考点，这就是回弹的来源。要消除回弹，常用的做法是记录手离开瞬间的末端位姿，把这个位姿更新为导纳控制器的新的参考点。换句话说，每次松手都"清零"参考点，下次再拖就以当前位置为基准。

5.3 从"拖得动"到"示教产线可用"：录制、回放与轨迹平滑

零力拖动的最终目的是示教，也就是说，操作者拖出来的轨迹要被系统记录下来，然后机械臂可以自动复现。这个环节看起来只是加一个路径录制功能，实际工程里还要处理不少细节。

轨迹录制的核心是时间同步。力传感器数据、机械臂位姿数据和控制器指令各自有不同的时间戳，回放时必须要用统一的时间轴对齐，否则轨迹会发生错位。录制时我会同时记录末端位置和姿态，姿态用四元数存储，避免欧拉角的表示奇异。录制频率不需要跟控制频率一样高，能到100Hz到200Hz就够了，因为回放前还会做轨迹压缩和插值。

回放阶段有两个常见问题。一是回放速度不均匀——因为操作者拖动时速度有快有慢，如果直接按采集时间戳回放，机械臂运动会像"卡带"一样忽快忽慢。我的做法是录制时对轨迹做等时间间隔重采样，回放时再进行轨迹平滑，具体可以用样条插值或者简单的低通滤波。二是回放轨迹必须经过机械臂逆运动学的校验，确保所有路径点在关节空间和笛卡尔空间都可达，不会碰到奇异点。这些工作在Gazebo仿真环境里可以提前验证——把实际录制的轨迹放到仿真的机械臂模型上跑一遍，发现问题改参数，比在实物上反复试错安全得多。

6. 写在最后：零力拖动的"魂"不在算法，在工程细节

这套方案做下来最深刻的体会是：零力拖动本身的技术难度并不算高，导纳控制的方法在五十年前就被提出并研究透了，但把它做成一个"让操作者觉得自然顺手"的工程系统，需要对每个细节都足够较真——重力补偿的残差、传感器的噪声、滤波器引入的延迟、控制频率的稳定性、安全保护的回退逻辑，每一项都能单独毁掉整个手感。

如果让我给正准备做这个方向的朋友一个建议，那就是不要一上来就追求复杂的算法。先把重力补偿做到位，让机械臂在任何姿态下悬停都不漂，再谈力反馈的跟手性。基础不牢，导纳参数再精细也只是在劣质地基上盖楼。另外，有条件一定先在仿真环境里把整套逻辑跑通，再上实物，这能省下大量的调试时间。

个人实际测试中还有一个提升监控效率的做法：把力传感器原始信号、滤波后信号和导纳控制器输出信号同时录下来，拖动的时候回放这几条曲线，可以非常直观地看出信号链路里每一级带来的延迟和畸变。参数问题是出在传感器端、滤波端还是控制端，一眼就能定位，比盲调参数效率高得多。