机械臂零力拖动示教：六维力传感器+导纳控制原理与调参实战

做机械臂调试验证的人，多少都遇到过这种情况：想把机械臂拖到一个目标位姿，把手一搭上去，机械臂要么纹丝不动，要么你一松手它猛弹回来，根本没法“顺着人的劲”走。真正舒服的示教手感，应该是人轻轻一推，机械臂就像失重一样顺着外力方向平滑移动，停住就停住，松开就悬浮。这种体验在行业里叫零力拖动示教，而目前工程上最通用、最可靠的实现路径，就是六维力传感器加上导纳控制。这篇内容我按实际做项目的过程来写，从控制原理、硬件选型到调参踩坑，尽量把每一步为什么这么做讲清楚，适合正在做机械臂开发的学生、工程师，还有玩开源机械臂想自己实现拖动的朋友。

1. 为什么零力拖动要选导纳控制

1.1 三种主流拖动示教方案对比

想实现“人推机械臂，机械臂跟着走”，本质上是要让机械臂对末端受到的外力做出位置响应。做这件事业内大体有三条路线，我先把它们的优劣摊开来说。

第一种是关节力矩模式拖动，就是直接读每个关节的力矩传感器或者电机电流，把人手施加在关节上的力矩作为输入，让关节朝力矩方向运动。这种方式在协作臂上很常见，比如一些带关节力矩传感器的协作机器人，拖动时手感天然顺滑。但它的前提是每个关节都要有高精度力矩测量能力，而且整臂的动力学模型、摩擦补偿必须做得足够好，否则你拖小臂还算顺畅，拖到某些特殊姿态就会出现明显的卡顿感或者漂移。对大多数使用普通位置控制型机械臂的人来说，这个前提就不太成立。

第二种是电流环直驱，本质上把伺服电机工作在力矩模式，靠电机电流估算外力。这个做法成本最低，很多开源项目里拿它来“伪拖动”，效果嘛，只能对付轻负载和低精度场景。因为电机电流里混着摩擦、重力、惯量耦合一大堆干扰，你很难分辨哪些电流是外力产生的，哪些是机械臂自身在出力。外力稍微小一点，整个系统就像隔了一层棉花，反应迟钝。

第三种就是标题里说的方案——末端装六维力传感器，把传感器测到的力和力矩作为输入，通过导纳控制器生成一个位置修正量，叠加到机械臂原参考位置上，然后让底层位置环去执行。这条路的优势在于，它不依赖关节力矩测量，也不用精确的整臂动力学模型，只用在末端建立一个“力到位移”的柔顺关系，工程实现非常直接。位置控制型机械臂基本都能用，改造成本低，效果还稳定，所以成为目前拖动示教最常见的落地方式。

1.2 导纳控制的本质：给机械臂装一个“虚拟弹簧阻尼系统”

那导纳控制到底在干什么？用一句话说，它是在机械臂末端虚拟出一个“质量-弹簧-阻尼”系统，让外力作用在这个虚拟系统上产生位移。数学上就一条二阶微分方程：

M(ẍ_d - ẍ) + B(ẋ_d - ẋ) + K(x_d - x) = F_ext

别被这公式吓到，我拆开讲。x_d是机械臂原本期望的位置，x是实际拖动后的位置，F_ext就是六维力传感器测到的人手外力。M是虚拟质量，B是虚拟阻尼，K是虚拟刚度。人推一下机械臂，F_ext变大，为了让等式成立，位移偏差(x_d - x)就会变化，机械臂就“顺着”外力走。

换个生活化的类比，你推一个放在冰面上的大箱子，箱子跟着你的手走，手停了箱子还滑一小段，这是质量在起作用；如果箱子泡在蜂蜜里，你推它会动，但手一停它也马上停，这是阻尼在起作用；如果箱子上拴着一根橡皮筋连在墙面上，你松开手箱子会被拉回原位，这是刚度在起作用。导纳控制就是把这三个物理感觉按需要调出来。拖动示教场景一般不需要回中，所以K常常设成接近零，让机械臂没有“归位感”，只保留适当的虚拟质量和阻尼，保证手感既不轻飘也不笨重。

这里要注意一个方向问题，很多人第一次写这个控制器会懵：F_ext到底该加在哪一边，符号到底取正还是取负？实际工程里不用纠结理论推导，直接记住一个检验标准：你朝传感器正方向推，机械臂必须朝正方向移动，如果推一下反着跑，就把增益符号反过来。

1.3 为什么“外环导纳+内环位置”是工程上最稳的组合

实现导纳控制的时候，主控的架构有两种选择：一种是直接在位置环内部做力位耦合，另一种是外环用导纳控制器算位移修正量，内环继续用机器人厂商提供的位置伺服。

我强烈建议用第二种，原因特别实在。现在市面上的机械臂，底层位置环都是厂商经过大量调试的，响应快、稳定性好、还有各种限位保护。你如果绕过位置环直接下力矩指令，等于把原本厂商帮你调好的东西全部推翻，所有稳定性问题都得自己扛。而外环导纳加内环位置这种模式，导纳控制器只负责算“该往哪挪”，真正执行还是靠成熟的位置环，相当于你只是给一套稳定系统加了一个柔顺外壳，出问题的概率小得多。

而且这种架构与控制频率的配合也很顺。机械臂位置环通常跑500Hz到1kHz，导纳外环跑100Hz到500Hz就够了。外环算出一个参考位置增量，下发给底层位置环去平滑追踪，整个系统不会出现激烈的控制量跳变。就算传感器数据瞬间有个毛刺，内环位置伺服也能把它过滤掉一大部分，不会让机械臂突然抽搐。

2. 硬件选型与系统架构怎么搭

2.1 六维力传感器怎么选：量程、精度与采样率

六维力传感器是整个系统里最关键的硬件，选型错了后面怎么调都白搭。首先是量程，这个要看你机械臂的末端负载和人手施加力的范围。比如一个3到5公斤负载的小型六轴机械臂，人手正常拖动末端时施加的力大致在10到50牛顿之间，偶尔快速推一下峰值可能到80牛。那你选量程就不能只按静态负载来，至少选额定量程100牛以上的传感器，否则很容易过载损伤。力矩方向同理，末端离传感器有一定距离，人手一推会叠加一个不小的力矩，所以宁愿量程留大一点，也不要在手感上过度追求小量程的高精度。

第二是精度和分辨率。拖动示教要求传感器能分辨人手轻微推的那一两牛力，所以分辨率一般要在0.1牛以下。现在国产的坤维、宇立，进口的ATI，在这个级别都没问题。关键点是传感器本身的串扰和温漂，尤其是温漂，传感器上电后内部应变片会随温度缓慢变化，零点会飘。这就是为什么后面一定要做动态零点校准。

第三是采样率和通信方式。六维力传感器常见的输出方式有模拟量、串口、EtherCAT等。模拟量输出延迟最低，但需要额外AD采集卡，而且线缆容易受干扰；串口简单，但一般只有100Hz到500Hz，要看具体型号；EtherCAT适合走实时总线，延迟很低，但要求主站有对应能力。我的建议是采样率至少500Hz，越低越容易在拖动时感觉到“肉”和滞后。通信延迟最好控制在2毫秒以内，否则人手的力已经变了，控制器还在用旧数据算位置，那种粘滞感会非常明显。

2.2 安装位置与坐标系对齐

六维力传感器一般装在机械臂末端法兰和工具之间，这样它测量到的就是人手作用在工具上的力和力矩。这里有个容易踩的坑：传感器测到的力是在传感器自身坐标系下的，而导纳控制算位置修正量是在机械臂基座坐标系或者工具坐标系下做的，两者不一致，必须做坐标变换。

最稳妥的做法是先把传感器的安装姿态标定出来，也就是传感器坐标系相对于机械臂末端坐标系的固定旋转矩阵R_sensor_to_tool。这个矩阵可以通过CAD模型里的安装尺寸算出初始值，然后实际测一次空载拖动，观察静止时传感器三个轴的重力分量方向，再微调标定。标定不准会带来什么后果？最典型的表现是：你在水平方向推，机械臂却往斜上方或斜下方跑，怎么调参数都调不出一个顺滑的直线拖动。

另外，如果你要记录示教轨迹然后回放，那工具中心点的标定也不能省。传感器安装之后，TCP的位置会发生变化，必须重新做一次TCP标定（常见做法是四点法或五点法），让机械臂记录的位置轨迹对应到真正人手握的地方，否则回放时工具端点对不上。

2.3 控制系统的数据链路与时序

从传感器到控制器再到机械臂，整个数据链路的时序设计决定了拖动手感的上限。我实际用过的方案是：六维力传感器通过EtherCAT或者串口接到主控，主控跑一个实时任务，每次循环先读力数据，经过滤波和重力补偿，算导纳控制方程，得到新的参考位置，再通过机械臂的SDK或者总线接口下发位置指令。

这里注意一个时序细节：读力和下发位置要尽量同步在一个控制周期内。如果力数据是10毫秒前的，位置指令是实时的，相当于控制系统里多了一个可变延迟，人手上的感觉就是“不太跟手”。解决方法是给数据打上时间戳，或者在下发指令时对力数据做一次时间对齐。对于多数项目，控制周期做到5到10毫秒，配合适当的滤波，手感已经可以达到商用协作臂的七成水平。如果追求极致手感，必须上实时系统和EtherCAT，控制周期压到1到2毫秒。

3. 核心代码与实现细节拆解

3.1 力信号预处理：零点漂移、滤波与重力补偿

传感器拿到原始数据后，不能直接塞进导纳控制方程，必须先经过三步处理，这也是很多人做出来手感稀烂最容易忽略的地方。

第一步是零点漂移消除。六维力传感器上电后，由于温度变化和应变片预紧状态，零位会缓慢漂移。最简单的方法是在机械臂上电静止后，取前几百个数据点的平均值作为零点，然后每次读数都减去这个零点。但更稳妥的是做一个带遗忘因子的动态零点估计，每隔一段时间自动更新零点。需要注意，这个动态估计只能在机械臂静止且无外力的时候做，如果有外力还去更新零点，那补偿就全错了。

第二步是低通滤波。传感器原始数据里有很多高频噪声，尤其是电机振动、机械结构谐振带来的噪声。我习惯先用一个截止频率5到20Hz的一阶低通滤波器，或者滑动平均窗口，把高频抖掉。但滤波也不能过头，如果截止频率太低，力信号的相位延迟会非常大，机械臂响应就变“肉”。这里推荐一个经验值：截止频率取10Hz左右，对该频率范围内的人手拖动信号几乎没有影响，同时能滤掉大部分振动噪声。

第三步是重力补偿，这是整个预处理里最核心的一步。传感器装好后，即使完全没有外力，只要机械臂姿态变化，传感器上也能测到工具重力造成的力和力矩分量。如果不把它扣掉，机械臂会误以为一直有人在推它，拖动一开始就乱跑。重力补偿的做法是事先辨识出工具的质量m和质心位置r，然后根据当前机械臂姿态计算机器人基坐标系到传感器坐标系的旋转矩阵R，把重力向量g变换到传感器坐标系下，再减去对应的力和力矩：

F_gravity_sensor = R * [0, 0, -m * g]^T T_gravity_sensor = r × F_gravity_sensor

实际处理时，质心r通常也是未知的，可以用标定法求。让机械臂在几个不同姿态下静止，记录传感器读数，用最小二乘解出m和r。这个标定过程我后面会详细说。总之，重力补偿做不好，零力拖动就是纸上谈兵。

3.2 导纳控制器的离散化实现

理论公式是连续的，但控制器是离散的，所以要把二阶微分方程差分化。常用做法是把期望位置当成机械臂当前位置的修正量，针对每个控制周期Ts，用前向欧拉法更新速度，再更新位置。代码写出来大概是这个感觉：

class AdmittanceController: def __init__(self, M, B, K, dt): self.M = M self.B = B self.K = K self.dt = dt self.vel = np.zeros(3) # 速度状态 m/s def update(self, force, pos_ref): # force: 经过补偿后的外力 (fx, fy, fz) # pos_ref: 当前参考位置 (x, y, z) acc = (force - self.B * self.vel - self.K * (pos_ref - self.pos_ref_prev)) / self.M self.vel += acc * self.dt delta_pos = self.vel * self.dt # 更新参考位置 new_pos_ref = pos_ref + delta_pos self.pos_ref_prev = new_pos_ref return new_pos_ref

当然这是最简版本，真正工程里还要加上姿态方向的导纳控制，也就是把力矩信号映射到欧拉角或旋转向量的修正上，原理和位置部分一样。姿态修正有个坑：欧拉角在特定角度会奇异，所以建议用旋转向量（axis-angle）来做姿态插值，或者把力/力矩统一转成六维向量，位置和姿态一起解算。

另外一个重要环节是限幅。不管外力多大，机械臂的位移修正量不能无限增长，否则要么撞到机械限位，要么速度太快触发保护。所以导纳控制器输出前必须要加位置限幅、速度限幅、加速度限幅，建议在代码里加三个饱和度函数，逐级限制。速度限制尤其关键，直接限制每个控制周期位置修正量的变化率，即使外力突然很大，机械臂也只能以最大安全速度移动，不会甩伤人。

3.3 重力补偿的在线辨识与实现

前面提到的重力补偿标定，我实际做过最少需要三个非共面姿态的采样数据才能把m和r解出来。但只做离线标定有个问题，用户换了一个工具，或者工具重心变了，上次标定的参数就失效了。所以在真正的拖动示教产品里，一般会做一个在线辨识功能：机械臂先低速带动工具在几个姿态间运动，同时记录传感器读数和机械臂关节角，用最小二乘或者卡尔曼滤波实时估计m和r。

这个在线辨识过程可以做成自动的，用户按下“开始辨识”按钮，机械臂走一组预设姿态，大概两三秒完成，然后自动更新重力补偿参数。我实际做下来效果很好，辨识出来的质量误差在2%以内，质心位置误差在5毫米以内，补偿后的残余重力干扰能压到1牛以下，这个水平对拖动示教来说已经足够干净了。如果你不想做那么复杂，也可以在固定工具的情况下离线标定一次，然后把参数写死在配置文件里，效果也基本OK。

3.4 安全保护：不能只靠传感器和限位

做拖动示教，人是要和机械臂直接接触的，安全是红线。代码层面的保护至少要有四层。

第一层是力/力矩超限保护，传感器测到的力超过安全阈值立刻停止修正，并让机械臂进入暂停状态，这个阈值设置为人手正常推力的1.5到2倍比较合理，不要太灵敏，否则操作者稍微用力一点就停了，体验很差，但也不能太迟钝，否则力气大一点的人就可能把机械臂推坏。

第二层是位置边界保护，在导纳控制输出的参考位置加入笛卡尔空间的边界框，一旦触发边界，就把边界处的虚拟刚度K调大，产生一个强烈的“推不进去”的感觉，而不是硬生生停下来。这比直接急停手感好很多。

第三层是奇异点保护，机械臂在接近奇异位形时，关节速度会急剧增大，即使末端移动很慢，某个关节也可能瞬间转速爆表，所以在导纳控制器里要实时监测雅可比矩阵的条件数，接近奇异时就限制末端移动方向，只允许沿非奇异方向运动。

第四层是急停逻辑，不管前面几层怎么设计，都必须把外部急停按钮和机器人驱动器的急停回路接好，这玩意平时用不上，关键时刻能救命。软件保护永远不能替代硬件急停。

4. 调参实战：怎么调出舒服的拖动手感

4.1 M、B、K三参数的手感密码

导纳控制器三个核心参数对应三种手感，我直接给一组我常用的初始参考值：虚拟质量M在2到8kg之间，虚拟阻尼B在20到60 N·s/m之间，虚拟刚度K在拖动模式设成0，如果要做边界约束再单独加一个边界虚拟刚度。

调参的顺序我先讲清楚。先把K设为0，然后固定M，从小到大调B。B太小，机械臂会飘，手一推它不收着劲，松手之后还会继续滑很远，感觉很危险；B太大，机械臂像在糖浆里游泳，推起来很“黏”，反应迟钝。找到B的甜点后，再调M。M决定的是启动灵敏度，M越小，越小的力就能让机械臂动起来，适合需要精细示教的场景；M越大，机械臂越“稳重”，快速拖动手感更平滑，不容易抖动，但初始推动需要的力也更大。

我个人的经验口诀是：阻尼管“稳不稳”，质量管“灵不灵”，刚度管“回不回”。实际手感调不好时，先别急着怀疑控制器，把参数表打出来一项项看，多数问题出在阻尼太小或者质量太大上。

4.2 常见问题排查表

我把做这个项目时遇到过的典型问题整理成了一张表，遇到类似情况直接对着查就行。

4.3 仿真先行：低成本验证控制流程

在真机上直接调参，效率低而且风险高，我最推荐的做法是先放到仿真环境里把整个流程跑通。比如用Gazebo或者Webots搭一个机械臂模型，末端加一个虚拟力传感器，在仿真里模拟人手推末端，验证导纳控制器的方向、重力补偿、限位逻辑。这个环节能帮你把80%的bug拦在真机实验之前。

仿真里要注意仿真的力传感器数据是很“干净”的，没有温漂也没有高斯噪声，所以你调出的参数拿到真机上会明显偏“激进”。我的建议是仿真只验证逻辑正确性和参数数量级，真正的手感必须在真机上微调。仿真阶段花一天，真机调参阶段花三天，这个时间分配比较合理。

5. 工程落地中的坑：从能跑到好用的几件麻烦事

5.1 机械谐振污染力信号

有些机械臂的结构刚度不够高，末端装传感器后，在某个速度段会产生机械谐振，传感器测到的力信号会出现明显的高频振荡。这个振动的频率通常在20到50Hz之间，和人的拖动信号频带重叠不多，所以可以通过陷波滤波器把谐振频率附近的信号压下去。为了找谐振频率，可以在机械臂静止时用手敲一下末端，观察传感器数据的频谱峰值，或者直接扫频激励关节，看哪个频率下力信号放大最明显。找到之后在传感器数据处理链路里加一个中心频率对应的陷波器，谐振就能被压制住。这里有一个血泪教训：别试图靠加大阻尼参数来压谐振，那样会把整个系统的响应变迟钝，与谐振无关的所有频段都会被伤到。

5.2 拖动手感“机械感”太强

就算导纳控制做的没问题，有些人拖起来还是会觉得机械臂像“机器人”，不像是在拖一个自然物体。问题往往出在加速度前馈和速度前馈上。基础版的导纳控制只用力来修正位置，但人手在推的时候，大脑本身对运动是有一个预测的，机械臂跟不上这个预测，人就觉得“不跟手”。解决办法是根据速度状态给机械臂一个前馈速度项，让位置内环提前补偿一部分运动趋势。这个前馈量不能给太大，否则机械臂会变得“贼”，稍微碰一下反而会冲出去。我是用了一个速度前馈系数，从0开始逐步加大，直到操作者觉得“顺滑”为止，这一步调好了，手感会明显上一个大台阶。

5.3 末端负载变化导致重力补偿失效

做拖动示教时，末端工具不是固定的，可能一会儿装夹爪，一会儿装吸盘，每次换了工具，重力补偿参数就要重新标定，很烦。解决方案是做一个工具参数自动辨识功能，原理我在前面提过：控制器自动驱动机械臂走几个姿态，用最小二乘在线估计出新工具的质量和质心。这个功能做成之后，更换工具只需要按一个按钮，两三秒就完成标定，项目实用性会提升很多。如果不想做完整辨识，也可以只估质量，不估质心，对拖动示教的手感影响不大，但姿态旋转示教时会有些偏差。

5.4 示教轨迹记录的平滑与回放

零力拖动示教不只是让人能拖，最终目的是把拖动过程记录下来，然后让机械臂自动复现。这个环节有一个隐藏问题：人拖动的轨迹是连续的，但天然带着抖动和停顿，直接拿去回放，机械臂会走出一个“毛毛虫”一样的轨迹，甚至能看到明显的阶梯感。所以记录到的原始轨迹必须做平滑处理，一般做法是采样后先做离线的低通滤波或者样条拟合，再用轨迹规划器重新生成速度和加速度连续的轨迹。我一般会先把原始轨迹降采样到50Hz左右，然后用三次样条或者贝塞尔曲线重新拟合，最后再让底层轨迹规划器做加加速度平滑。这样回放出来的机械臂动作，才会既保留了拖动示教的路径，又看起来像一个老手在操作。

6. 一点建议

六维力传感器加导纳控制的组合，做零力拖动示教，是我目前在普通位置控制型机械臂上试过的最稳定的方案。整套东西从原理到落地其实不复杂，硬骨头全在细节：传感器标定、重力补偿、参数调节、安全保护，每一个环节单拿出来都不难，但串起来就考验工程耐心了。

我自己做完这个项目最大的感触是，参数调优千万别贪，一次只动一个变量，记录下每次改动后操作者的主观感受，比盲调快得多。另一个小技巧是，可以在手边准备一个简单的主观打分表，让不同人来拖同一台机械臂，把他们对“轻飘感”、“迟钝感”、“回弹感”的打分记录下来，这比一个人闷头调参数能更快找到最合适的手感带。总的来说，这套方案值得做，也希望这篇内容能帮你少踩几个坑。