做机械臂调试验证的人多少都遇到过这种情况想把机械臂拖到一个目标位姿把手一搭上去机械臂要么纹丝不动要么你一松手它猛弹回来根本没法“顺着人的劲”走。真正舒服的示教手感应该是人轻轻一推机械臂就像失重一样顺着外力方向平滑移动停住就停住松开就悬浮。这种体验在行业里叫零力拖动示教而目前工程上最通用、最可靠的实现路径就是六维力传感器加上导纳控制。这篇内容我按实际做项目的过程来写从控制原理、硬件选型到调参踩坑尽量把每一步为什么这么做讲清楚适合正在做机械臂开发的学生、工程师还有玩开源机械臂想自己实现拖动的朋友。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 常见问题排查表我把做这个项目时遇到过的典型问题整理成了一张表遇到类似情况直接对着查就行。现象可能原因排查与处理拖动时手感黏滞、反应迟钝滤波截止频率太低或B值偏大检查滤波频率是否在10Hz左右减小B值松手后机械臂继续漂移B值偏小或零点补偿不干净增大B值检查零点是否漂移重新去皮拖动过程中机械臂抖动传感器噪声大或控制周期过长加强滤波缩短控制周期到5ms以内检查通信延迟推某个方向机械臂却往斜向跑坐标系变换错误或重力补偿不准重新标定传感器安装矩阵重新辨识重力参数拖到某一段突然很“硬”接近空间边界或奇异点保护触发检查边界框设置查看雅可比条件数是否异常传感器读数缓慢漂移温度变化导致零点漂移加动态零点更新或控制环境温度4.3 仿真先行低成本验证控制流程在真机上直接调参效率低而且风险高我最推荐的做法是先放到仿真环境里把整个流程跑通。比如用Gazebo或者Webots搭一个机械臂模型末端加一个虚拟力传感器在仿真里模拟人手推末端验证导纳控制器的方向、重力补偿、限位逻辑。这个环节能帮你把80%的bug拦在真机实验之前。仿真里要注意仿真的力传感器数据是很“干净”的没有温漂也没有高斯噪声所以你调出的参数拿到真机上会明显偏“激进”。我的建议是仿真只验证逻辑正确性和参数数量级真正的手感必须在真机上微调。仿真阶段花一天真机调参阶段花三天这个时间分配比较合理。5. 工程落地中的坑从能跑到好用的几件麻烦事5.1 机械谐振污染力信号有些机械臂的结构刚度不够高末端装传感器后在某个速度段会产生机械谐振传感器测到的力信号会出现明显的高频振荡。这个振动的频率通常在20到50Hz之间和人的拖动信号频带重叠不多所以可以通过陷波滤波器把谐振频率附近的信号压下去。为了找谐振频率可以在机械臂静止时用手敲一下末端观察传感器数据的频谱峰值或者直接扫频激励关节看哪个频率下力信号放大最明显。找到之后在传感器数据处理链路里加一个中心频率对应的陷波器谐振就能被压制住。这里有一个血泪教训别试图靠加大阻尼参数来压谐振那样会把整个系统的响应变迟钝与谐振无关的所有频段都会被伤到。5.2 拖动手感“机械感”太强就算导纳控制做的没问题有些人拖起来还是会觉得机械臂像“机器人”不像是在拖一个自然物体。问题往往出在加速度前馈和速度前馈上。基础版的导纳控制只用力来修正位置但人手在推的时候大脑本身对运动是有一个预测的机械臂跟不上这个预测人就觉得“不跟手”。解决办法是根据速度状态给机械臂一个前馈速度项让位置内环提前补偿一部分运动趋势。这个前馈量不能给太大否则机械臂会变得“贼”稍微碰一下反而会冲出去。我是用了一个速度前馈系数从0开始逐步加大直到操作者觉得“顺滑”为止这一步调好了手感会明显上一个大台阶。5.3 末端负载变化导致重力补偿失效做拖动示教时末端工具不是固定的可能一会儿装夹爪一会儿装吸盘每次换了工具重力补偿参数就要重新标定很烦。解决方案是做一个工具参数自动辨识功能原理我在前面提过控制器自动驱动机械臂走几个姿态用最小二乘在线估计出新工具的质量和质心。这个功能做成之后更换工具只需要按一个按钮两三秒就完成标定项目实用性会提升很多。如果不想做完整辨识也可以只估质量不估质心对拖动示教的手感影响不大但姿态旋转示教时会有些偏差。5.4 示教轨迹记录的平滑与回放零力拖动示教不只是让人能拖最终目的是把拖动过程记录下来然后让机械臂自动复现。这个环节有一个隐藏问题人拖动的轨迹是连续的但天然带着抖动和停顿直接拿去回放机械臂会走出一个“毛毛虫”一样的轨迹甚至能看到明显的阶梯感。所以记录到的原始轨迹必须做平滑处理一般做法是采样后先做离线的低通滤波或者样条拟合再用轨迹规划器重新生成速度和加速度连续的轨迹。我一般会先把原始轨迹降采样到50Hz左右然后用三次样条或者贝塞尔曲线重新拟合最后再让底层轨迹规划器做加加速度平滑。这样回放出来的机械臂动作才会既保留了拖动示教的路径又看起来像一个老手在操作。6. 一点建议六维力传感器加导纳控制的组合做零力拖动示教是我目前在普通位置控制型机械臂上试过的最稳定的方案。整套东西从原理到落地其实不复杂硬骨头全在细节传感器标定、重力补偿、参数调节、安全保护每一个环节单拿出来都不难但串起来就考验工程耐心了。我自己做完这个项目最大的感触是参数调优千万别贪一次只动一个变量记录下每次改动后操作者的主观感受比盲调快得多。另一个小技巧是可以在手边准备一个简单的主观打分表让不同人来拖同一台机械臂把他们对“轻飘感”、“迟钝感”、“回弹感”的打分记录下来这比一个人闷头调参数能更快找到最合适的手感带。总的来说这套方案值得做也希望这篇内容能帮你少踩几个坑。