基于六维力传感器与导纳控制的机械臂零力拖动示教

搞机器人这几年，有一件事始终特别磨人：示教。传统的示教器一个个点位去对、去记、去改，干过的都懂，那叫一个痛苦。后来我转向了拖动示教方案，用六维力传感器配合导纳控制，让机械臂真正“听话”——人直接握着末端来回拖，机械臂像被一只无形的手带着走，松手即停，轨迹还能一键复现。今天这篇文章就把我从硬件选型到算法实现、再到调试踩坑的全过程拆开讲清楚，聚焦“零力拖动示教”这条主线，适合正在做ROS机械臂开发、毕设里需要机械臂拖动示教、或者想搞清楚导纳控制到底怎么落地的朋友。你不需要有深厚的控制理论基础，但如果你懂一点机器人的运动学和坐标变换，读起来会更加顺手。

1. 零力拖动示教的实现思路与方案选型

1.1 零力拖动到底在解决什么问题

示教的核心诉求很简单：让机械臂以最快的速度学会一条轨迹。传统示教用示教器手动输入各关节角度或末端位姿，再逐点确认、模拟运行、修改，效率低不说，复杂曲面轨迹根本没法靠逐点录。拖动示教则完全不同——人直接握着机器人末端，像教小孩写字一样把动作“带”一遍，机械臂在运动过程中以固定频率记录位姿和时间戳，完成后再由机器人自己复现。整个过程不需要任何代码，不需要碰示教器，几秒钟就能录完一条复杂的空间曲线。

但问题也来了：一个几十公斤的机械臂，凭什么让你一个普通人轻轻一推就能动起来？关节自身有摩擦力、减速器有惯性、电机有扭矩波动，直接推根本推不动，就算推得动，稍微松手就反弹回原位置。所以“零力”指的不是真实物理上的零重力，而是通过算法把机械臂自身的重力、摩擦力、惯性影响全部补偿掉，让人手感受到的净力趋近于零。人给出多少力，机械臂就按这个力的大小和方向产生多少运动，松手后没有外力了，机械臂也不动，这才是真正意义上的零力拖动示教。

1.2 为什么选导纳控制而不是直接力控

讲到拖动，好多人第一反应是“力控”，直觉上觉得给机械臂一个力应该直接跟上。理论上确实有这个思路，但工程上必须区分两个概念：阻抗控制和导纳控制。阻抗控制是把机器人整成一条“弹簧-阻尼-质量”系统，力的输入对应位置偏移，并调节这个“虚拟弹簧”的刚度来改变手感。导纳控制是阻抗控制的“反问题”：外侧是力传感器，内侧是位置环，传感器收到外力后解算出期望的位置修正量，再交给底层位置控制器去执行。对当前这个场景来说，绝大多数工业机械臂和开源机械臂自带的底层控制器都是位置/速度环，导纳控制几乎不用改动硬件和控制器的底层结构，只需在位置环外层叠加一个力-位置换算环节，所以工程可行性远高于直接扭矩控制的阻抗方案。

从另一个角度讲，在六维力传感器数据里加入导纳模型，等于给系统增加了一层“虚拟柔顺”。这样即便机械臂自身刚性十足，人在拖动时感受到的依然是柔顺、平滑、带一点阻尼的手感，而不是死硬或发飘。我试过把阻尼调小、追求“过度灵活”的手感，结果操作者稍微发力过猛，机械臂就像脱缰野马一样冲出去，非常危险。导纳控制在工程上最有价值的地方就是它能像“调音”一样把系统的柔顺度调到恰到好处，既轻松又可控。

1.3 传感器方案对比：末端六维力、关节力矩、电流估算

做拖动示教，力从哪来是首先要定的。工程上最常见的有三种方案：末端六维力传感器、关节力矩传感器、电机电流估算。电流估算成本最低，直接读驱动器电流换算力矩，不需要额外硬件，但精度很差，摩擦力、温度、电流噪声都会让力信号变得乱七八糟，做精细的零力拖动基本不靠谱。关节力矩传感器精度高，能感知每个关节的受力情况，但价格昂贵，而且需要对每个关节重新设计结构，适合高端协作臂，不适合大多数自研项目。

终端项目最稳妥的选择就是末端六维力传感器。它装在机械臂末端法兰和工具之间，能同时测出三维力(Fx,Fy,Fz)和三维力矩(Mx,My,Mz)，精度高、响应快，而且安装和替换都方便，一个传感器就能覆盖所有工具。像国内坤维、宇立，国外ATI都是这个赛道的常见选择。用末端六维力传感器的另一个好处是：重力补偿和惯性补偿可以在末端坐标系里统一建模，算法思路清晰，调试的时候可观测性也更强。这个方案唯一的代价是增加了末端质量和成本，但对于拖动示教这种低频、柔顺控制为主的场景，性价比完全能接受。

2. 六维力传感器数据采集与预处理

2.1 硬件安装与坐标系标定

传感器买到手不是装上就能用的，第一步要做的是把坐标系关系理清楚。六维力传感器一般有两个坐标系：一个是传感器自身坐标系，通常在传感器顶面中心，安装时要注意它的X、Y、Z轴方向是否和机械臂末端坐标系一致。如果安装方向不对，力信号换算到世界坐标系就会完全错乱。我的习惯是：安装后先做一次坐标系核对，让机械臂分别沿基座X、Y、Z三个方向缓慢移动，观察传感器读数的正负号和方向是否对应，不对就把传感器旋转90度重新装，或者直接在软件里做一次坐标变换矩阵修正。

工具端也要处理。传感器下面可能装了夹爪、吸盘、画笔或者其他工具，这些工具的重量、重心位置会直接影响力传感器的读数。我在项目里专门建了一个“工具参数”配置文件，记录工具的质量、重心在传感器坐标系下的位置。没有这个参数，重力补偿完全没法做，因为传感器测到的不只是人手施加的力，还有整个工具的重力在传感器坐标系下的分量。如果换了工具但没有更新重心参数，拖动时机械臂会表现出明显的“偏坠”，一边重一边轻，手感会非常怪。

2.2 力信号滤波与零点漂移处理

六维力传感器的原始数据如果直接拿来用，通常会有明显的高频噪声，尤其是人手快速操作时，传感器本身的应变片信号很容易受到机械振动和环境噪声干扰。最常见的做法是滑动均值滤波或一阶低通滤波。滑动均值滤波窗口大小建议在20到50之间，值太小滤不干净，值太大会引入相位延迟，导致拖动时有“黏滞感”。我自己更偏向用一阶低通滤波，截止频率设在20Hz到50Hz之间，既能滤掉高频抖动，又不会影响人手拖动的有效频率。要注意的是，滤波器和导纳控制器的频率要匹配，不能传感器滤波用高频、控制周期却跑得很慢，那样控制环路的稳定性会出问题。

零点漂移是另一个经常被忽略的坑。传感器在静止时读数应该为零，但实际使用中，温度变化、应变片疲劳、信号放大器温漂，都会让零点慢慢偏移。每次上电后，我建议先让机械臂保持零位静止3到5秒，读取这段时间的均值作为本次运行的零点偏置，然后在后续计算中实时减去。如果项目运行时间很长，还可以在拖动过程中定期判断是否处于静止状态，静止状态下重新刷新零点。这里有个容易踩的细节：机械臂在零位静止时，传感器也会因为工具重力产生读数，所以校零时记录的其实是“传感器零漂”加上“工具重力在当前位置的投影”，这两者必须在后面的重力补偿里统一处理，否则零点偏移会引入一个常量误差，导致机械臂往一边飘。

2.3 重力补偿：拖动示教成败的关键一步

零力拖动手感好不好，八成功力都在重力补偿上。原理说穿了并不复杂：传感器测到的力包含三部分——人手施加的外力、工具/末端附带的重力、机械臂运动时的惯性力。拖动速度通常比较慢，惯性力可以忽略，所以只要把工具重力在传感器坐标系下的投影算出来并在实时数据里减掉，剩下的就是纯正的人手外力。

实现方法分两种。第一种：已知工具质量和重心位置。假设工具质量是m，重力加速度矢量在世界坐标系下是g=[0,0,-9.8]，工具重心在传感器坐标系下的位置是r，当前机械臂末端的旋转矩阵是R，那么工具重力在传感器坐标系下的分量就是 R^T * m * g，重力对传感器产生的力矩分量是 r × (R^T * m * g)。把这些代入原始读数做差即可。第二种：未知工具参数时，可以用“任意姿态标定法”。让机械臂分别运动到多个不同姿态，记录对应的传感器力/力矩读数，再用最小二乘法把工具质量和重心位置拟合出来。这个我强烈推荐，因为很多情况下工具重心位置不好直接测量，拟合既省事又准确。我在项目里就是在五个以上的姿态下各采集几百个数据点，然后离线做一次拟合，效果非常理想。

重力补偿做完之后怎么验证？拿到传感器实时数据，先把滤波、零漂、重力补偿全走一遍，让机械臂在几个不同姿态下保持静止，观察补偿后的力读数是否接近零。如果静止时力读数还有明显的偏置或者随姿态变化，说明重力补偿参数有问题，坚决不能进入下一步。这一步不过关，后面调导纳参数再努力都是白搭。

3. 导纳控制核心算法实现与参数整定

3.1 导纳控制律解析：质量-弹簧-阻尼模型

导纳控制的物理模型很经典：把机器人末端想象成由质量(m)、阻尼(b)、刚度(k)构成的虚拟系统，外部力F作为输入，系统输出的是一个位置偏移量Δp。数学形式写成：

M·Δp̈ + B·Δṗ + K·Δp = F_filt

这里M是虚拟质量，B是虚拟阻尼，K是虚拟刚度。Δp是期望位置相对于当前参考位置的修正量，F_filt是经过滤波和补偿后的外力向量。如果K设为0，虚拟弹簧不存在，外力持续作用时，位置修正量会一直累积，这正是零力拖动所需要的特性——人推着走，机械臂就一直走，松开手，速度衰减到零，机械臂停住。如果K设为大于0，机械臂就表现出“偏移后想要回弹”的特性，这在一些需要弹性柔顺的场景有用，但拖动示教没这个需求。

这个公式看起来很简单，实际编程时要注意力向量和位置增量向量的维度匹配。平动拖动一般只用了(Fx,Fy,Fz)三个力分量，旋转拖动还需要用到(Mx,My,Mz)三个力矩分量。很多项目的拖动示教只做位置拖，也就是只控制末端位置跟随人手，姿态保持不动，这种情况下力矩通道直接关闭，只解算前三行，实现会简化不少。但如果需要拖动末端工具改变姿态（例如拖动喷枪改变喷涂方向），那六个维度都要参与解算。

3.2 从力到位移：控制环路怎么闭环

整个导纳控制环路在代码层面大致是这么几步：

1. 读取六维力传感器原始数据。
2. 力信号滤波，去除高频噪声。
3. 减去零漂。
4. 做重力补偿，得到人手施加的净外力。
5. 判定是否超过启动阈值，判断是否进入拖动状态。
6. 将净外力输入导纳控制律，解算出速度修正量（而不是直接解算位置增量），然后对速度做积分得到位置修正量。
7. 将位置修正量叠加到当前末端参考位姿上，得到新的期望位姿。
8. 对该期望位姿做逆运动学解算，得到关节角度目标。
9. 把关节目标发给底层位置控制器执行。

这里第6步有个非常关键的设计选择：导纳控制统的解算输出到底是位置增量还是速度增量。如果直接输出位置增量，人手稍微抖动一下，位置就会出现一个不小的阶跃，导致机械臂猛地窜动。更平滑的做法是：根据外力解算期望速度，再把速度积分成位置偏移。外力大，速度就大，拖动手感就更“跟手”；外力小，速度就小，拖动手感就偏“沉”。松手后外力归零，速度也衰减为零，位置不再变化，机械臂稳稳停住，这正好满足零力拖动的要求。

第8步的逆运动学也有讲究。对于6自由度机械臂，通常在笛卡尔空间做逆解，给出末端位姿矩阵，求出各关节角。如果直接在关节空间里做位置控制，还需要考虑奇异点问题。我在实际项目里用的是一个比较稳妥的思路：每次控制循环内做一次逆解，并限制关节角变化量在一个小范围内，避免逆解结果跳变导致机械臂抖动。

3.3 参数整定实操：先调谁后调谁

导纳控制器的参数主要有M、B、K，以及启动阈值、最大速度限幅、位置修正量限幅。我的建议整定顺序是：

第一步，把K固定为0，M设为一个相对较大的值，B先设一个中间值，一切以“稳定”为前提。第二步，单独验证重力补偿效果，确认静止状态下补偿后的净力噪声范围，一般在±1N以内才算正常。第三步，设定启动阈值，略高于噪声范围，比如1.5到2N，防止手还没碰传感器机械臂自己就开始飘。第四步，调B。B的值越大，拖动时阻力越大，手感越“黏”；B越小，机械臂越“松”。我是从B=50 N·s/m起步逐步往下调，每次调整后用手握住末端，缓慢沿X方向拖动，感受阻力是否合适。第五步，调M，M主要影响系统对力变化的响应速度和振动特性，M偏小会让响应过于灵敏，容易发生抖动和机构啸叫，M偏大会让启停都有明显延迟。第六步，加上速度限幅和位置限幅，默认速度上限30cm/s比较稳妥，等手感调整合适后再逐步放宽。

参数整定这事没法一口吃成胖子，我前前后后调了三轮才找到比较顺手的组合。中间有一次为了追求“特别轻”的手感，把B调得很小，结果机械臂在拖到一半时开始出现持续的微小振荡，伴随机身嗡嗡响，停下操作等了好几秒才恢复。所以我的忠告是：手感“轻”不一定好，稳定、可控、不振荡才是第一位。

4. 拖动示教与轨迹复现的完整实操流程

4.1 整体系统架构与数据流

一个能用的拖动示教系统，从底层往上大致分为四层：机械臂本体及底层驱动器、实时控制器/工控机、上位机算法层、人机交互层。在实际项目里，我用的是ROS框架，在同一台工控机上同时跑机械臂驱动节点、六维力传感器数据采集节点和导纳控制节点。传感器数据通过串口或以太网传到工控机，导纳控制节点订阅力数据，解算出期望位姿，再通过ROS的动作/话题接口发送给机械臂驱动节点。

这里有一个很重要的工程点：控制周期。导纳控制算法的计算频率至少要达到100Hz以上，最好到200Hz，才能保证拖动的手感平滑。如果传感器数据采集节点和导纳控制节点之间的通信延迟过大，或者算法计算耗时过长，控制周期会变得极不稳定，人手推动时就能感到明显的“一卡一卡”。我前期试过用Python写导纳控制主循环，由于Python解释执行效率的问题，加在一起只能跑到60Hz左右，手感明显不好，后来把核心解算部分用C++实现，定制循环200Hz，拖动体验才算合格。

4.2 拖动示教核心流程分解

整套示教流程我拆成四个状态：初始化、校准、拖动记录、轨迹保存。初始化阶段做的事情比较多：连接传感器、读取参数、检查机械臂当前位姿是否在安全范围内。校准阶段就是前文提到的零点刷新和重力补偿验证。在这两个状态完成后，系统提示“可以开始拖动”，才真正进入拖动记录状态。

拖动记录状态是核心。程序里维护一个环形缓冲区，用来存储时间戳和对应的末端位姿。当判断到净外力超过启动阈值时，开始记录；当净外力持续低于停止阈值超过0.5秒时，判定拖动结束，停止记录。这里不建议用一个简单的阈值判断开始和结束，因为人手在拖动过程中难免会瞬时松一点力。我在代码里用的是“滞回比较”的思路：启动阈值比如2N，停止阈值比如1N，一旦进入拖动状态，必须等外力降到1N以下持续0.3秒才认为停止。这样可以避免因为人手微抖造成轨迹被切断。

记录的过程中时机也很重要。每一条轨迹点的记录间隔要尽量均匀，如果只记录位置而不记录时间戳，复现时大概率会出现轨迹点之间速度忽快忽慢的情况。我习惯以20ms为周期同步记录时间和末端位姿，复现时严格按照时间戳插值，保证运动速度曲线和拖动时基本一致。

4.3 轨迹复现与平滑处理

轨迹记录完成后，原始数据通常没法直接拿来跑。人手拖动时难免会有抖动，直接给底层控制器回放会让机械臂的轨迹看起来毛毛躁躁。我常用的处理流程是：先用低通滤波或Savizky-Golay滤波对位置序列做平滑，再用三次样条插值按固定的插值周期重采样。这样做的好处是轨迹点的时间间隔均匀，速度和加速度曲线连续，后续发给机械臂执行时不会因为不连续的期望位置而产生冲击。

插值完成后的轨迹，还要做速度规划。因为录制的轨迹隐含了原始拖动速度信息，如果拖动时快时慢，复现时也应该保持这个节奏，否则有些工艺环节（比如涂胶、焊接）会因为速度变化导致质量下降。但如果原始轨迹速度很慢，复现时又希望加快时间，可以在时间轴上做等比压缩，同时在每个轨迹点之间重新进行速度规划。我在项目里做了一个可调参数：复现速度倍率，默认1.0倍，也可以设为1.5或2.0，方便实际使用。

4.4 一个可直接改的导纳控制代码示例

下面给一个核心解算流程的伪代码风格示例，主循环每个控制周期跑一次。实际工程里可以根据所用机械臂的SDK替换接口。

import numpy as np class AdmittanceController: def __init__(self, mass=3.0, damping=30.0, stiffness=0.0, dt=0.005): self.M = mass self.B = damping self.K = stiffness self.dt = dt self.vel = np.zeros(3) # 速度积分状态 self.delta_pos = np.zeros(3) # 位置修正量 def reset(self): self.vel = np.zeros(3) self.delta_pos = np.zeros(3) def update(self, f_ext): # f_ext: 经过滤波、零漂、重力补偿后的三维外力 [Fx, Fy, Fz] acc = (f_ext - self.B * self.vel - self.K * self.delta_pos) / self.M self.vel += acc * self.dt # 限制最大修正速度，防止失控 max_vel = 0.3 self.vel = np.clip(self.vel, -max_vel, max_vel) self.delta_pos += self.vel * self.dt return self.delta_pos

实际使用时，你还需要把delta_pos转换到机械臂的基坐标系下，再叠加到当前的期望末端位置上，最后做逆运动学得到关节角。坐标变换这里特别容易出错，因为力传感器测到的是传感器坐标系下的力，但位置修正量要作用在世界坐标系/基坐标系下，中间隔着传感器安装矩阵和末端姿态矩阵。建议写代码时把R_world_to_sensor这个旋转矩阵单独抽出来，每一步先验证矩阵方向对不对，再继续往下写。我在这个环节翻过车，折腾了两天才发现是传感器的Y轴装反了，导致拖动机器人在X方向运动时，机械臂实际往Y方向走，非常吓人。

5. 常见问题与调试心得

5.1 典型故障与排查对照表

我把实际调试中遇到的高频问题整理成了一个表格，方便遇到类似现象时快速定位思路。

这张表解决了我调试阶段至少70%的烦恼。其实多数问题出在“力信号不干净”和“参数没匹配”两大块，真正算法本身的问题反而不多。

5.2 调试经验与避坑提醒

第一，传感器数据必须先可视化再谈算法。写代码之前，先把六维力数据的实时曲线用rqt_plot或者matplotlib画出来，亲眼看一看到底有没有噪声、噪声多大、趋势是否符合物理直觉。我记得第一次装上传感器，看到Z方向重力的读数随机械臂运动而变化，那一刻我才真正理解了重力补偿为什么要做。

第二，参数整定前先把安全措施做足。我见过太多人在初始调试时没做速度和位置限幅，机械臂一失控就撞上旁边的设备。强烈建议调试前把工作空间限位、关节角限位、速度限幅全部设置到安全值，同时把导纳控制的最大修正速度设得足够小，等系统验证稳定后再逐步放宽。

第三，重力补偿要关注温度变化。这个坑比较隐蔽。传感器在使用过程中会发热，零点会跟着漂移，如果长时间连续运行，需要用定期静止校零或动态零点补偿来应对。有一次我在项目演示前没有重新校零，结果机械臂在演示过程中越飘越明显，当时只能紧急中断，非常尴尬。

第四，示教轨迹的复现需要看工艺需求。同样是拖动示教，有的场景只需要末端走位，有的场景还需要过程速度一致，有的场景需要多次重复示教的轨迹完全一致。每一次需求的差异都会影响记录数据和后续处理的方式。比如要求轨迹尽量一致时，重复性误差主要来自原始记录和插值，我建议记录多个样本，取平均后再执行，稳定性会提升很多。

第五，也是我体会最深的一点：手感这东西，参数给不了，得靠手试。导纳控制器的参数整定不是纯粹的技术题，更像是调音师调音。不同的人对“轻”和“稳”的偏好不一样，甚至同一个人的操作习惯，在做精密对位和快速拖甩时对阻力的期望都不同。一个合格的零力拖动系统，至少要提供几组预置手感参数给用户切换，并在实际使用中随时可调。

5.3 下一步还能怎么玩

拖动示教做稳定之后，后面能扩展的方向其实很多。可以在拖动示教的基础上做位姿记录和轨迹编辑，甚至实现多段轨迹拼接；可以把六维力传感器的力矩通道打开，让操作者不仅能拖着走，还能压着末端改变姿态；可以加入视觉引导，把拖动示教录制的轨迹和相机识别到的对象坐标关联起来，实现更加自动化的机器人作业。我在完成基础零力拖动后，又在这套系统上加了力控装配的测试模块，用同一套导纳控制框架做插孔和打磨，效果都挺不错。复用性远比我最初预想的高。

最后说一句真心话：零力拖动示教看着炫，本质是把复杂的机器人编程变成了一种直觉操作。只要能迈过力传感器数据处理和导纳参数整定这两道坎，后面的路会越走越顺。希望这篇文章能让你少走一些弯路，更早体会到“手把手教机器人干活”的爽感。