六轴机械臂在自由空间里跑轨迹规划和让它去拧螺丝、擦玻璃、插连接器完全是两码事。前者只要位置到了就行后者位置和力都得管——力大了压碎工件力小了打滑。这就是笛卡尔空间下的力/位混合控制与阻抗控制要解决的问题。我最近在一个6-DOF协作臂项目上把这两套控制策略从仿真到实机都跑了一遍踩了不少坑也积累了一些文档里不会写的经验。这篇内容适合已经做过基础运动学、用过ROS控制框架、想往力控方向深入的开发者也适合正在做机械臂毕业设计、需要把能动能抓升级到能柔顺交互的朋友。下面我按实际调试顺序把原理、实现、参数整定和翻车现场都摊开讲。1. 为什么位置控制到了接触阶段就不够用了1.1 从到点到接触的本质变化位置控制的核心逻辑是给定目标位姿控制器算出关节力矩或速度把末端推过去。在自由空间里这套逻辑没问题因为机械臂末端不受外力约束你让它去哪它就去哪。但一旦末端接触环境情况就变了——环境会给末端一个反作用力这个力会通过机械臂的连杆传回关节形成扰动力矩。如果你还在用纯位置控制控制器会拼命维持位置结果就是力不断累积。举个实际数字假设位置环刚度等效为10000 N/m你让末端往工件表面多走0.5 mm产生的接触力就是5 N。对于拧M3螺丝或者抓鸡蛋这类任务5 N已经足以造成损坏。更麻烦的是这个力是控制器看不见的——位置控制根本不测量力它只知道自己没到位于是继续加大输出。我在第一次做插孔任务时就吃了这个亏。孔轴配合间隙0.1 mm位置控制精度标称0.05 mm理论上够用。但实际装配时轴端倒角一碰到孔口边缘侧向力直接把轴推偏位置环又把它拉回来来回振荡最后把孔口刮花了。问题不在于位置精度不够而在于接触阶段的主导物理量已经从位置变成了力。1.2 力/位混合控制与阻抗控制的分工力/位混合控制Hybrid Force/Position Control的思路很直接把笛卡尔空间的6个自由度拆开一部分方向做位置控制另一部分方向做力控制。比如擦玻璃时垂直于玻璃面的方向做力控制保持恒定压力平行于玻璃面的两个方向做位置控制按轨迹移动。这个拆方向的操作通过一个选择矩阵实现对角线上是0或10对应的方向走力环1对应的方向走位置环。阻抗控制Impedance Control则是另一种哲学不硬拆方向而是把机械臂末端等效成一个弹簧-阻尼-质量系统让末端对外表现为一个可调的阻抗。环境推它它就按设定的刚度退让环境松开它就按设定的刚度回去。你调的不是力也不是位置而是这个虚拟弹簧的软硬。两者的关系可以这样理解力/位混合控制是分而治之适合任务方向明确、约束已知的场景阻抗控制是统一建模适合约束不确定、需要柔顺交互的场景。实际项目中我经常把两者结合——外层用阻抗控制保证安全内层在特定方向叠加力环做精确力跟踪。对比维度力/位混合控制阻抗控制控制目标特定方向力跟踪其余方向位置跟踪末端动态关系质量-阻尼-刚度需要环境模型需要知道约束方向不需要显式环境模型参数整定难度中等方向划分清晰较高三个参数矩阵互相耦合适用场景打磨、擦拭、装配人机协作、未知环境探索力跟踪精度高有积分项时取决于阻抗参数和环境刚度2. 笛卡尔空间力控的数学底子别跳过这一步2.1 雅可比矩阵与力域映射很多教程讲力控直接从控制框图开始结果调参时完全不知道每个量在物理上意味着什么。我建议先把雅可比矩阵的力映射关系搞清楚。机械臂关节力矩 τ 和末端笛卡尔力 F 的关系是τ J^T · F其中 J 是雅可比矩阵6×nn为关节数。这个转置关系是力控的基石。反过来如果你在关节空间施加了力矩 τ末端感受到的等效力就是F (J^T)^(-1) · τ对于6-DOF机械臂J 是6×6方阵可以直接求逆。但要注意当机械臂接近奇异位形时J 的条件数急剧变大求逆会放大误差。我在调试时就遇到过机械臂伸得比较直的时候末端力估计值突然跳变后来发现是雅可比矩阵接近奇异(J^T)^(-1)的数值不稳定。解决办法有两个一是用阻尼最小二乘Damped Least Squares在求逆时加一个阻尼项 λJ^ J^T · (J·J^T λ²·I)^(-1)λ 一般取0.01~0.1根据奇异程度动态调整。二是直接避开奇异位形在轨迹规划阶段就把工作空间限制好。我通常两个都用——规划时避开控制时加阻尼兜底。2.2 笛卡尔空间动力学方程要在笛卡尔空间做力控得把关节空间的动力学方程变换过去。关节空间方程是M(q)·q̈ C(q,q̇)·q̇ G(q) τ τ_ext其中 M 是惯性矩阵C 是科氏力和离心力项G 是重力项τ_ext 是外部接触力映射到关节的力矩。变换到笛卡尔空间后M_x(q)·ẍ C_x(q,q̇)·ẋ G_x(q) F F_ext变换关系是M_x J^(-T) · M · J^(-1) C_x J^(-T) · (C - M·J^(-1)·J̇) · J^(-1) G_x J^(-T) · G这里 J^(-T) 表示先求逆再转置。实际实现时我一般不在线计算完整的 M_x 和 C_x因为计算量大且对模型误差敏感。更常用的做法是在关节空间做动力学补偿重力补偿、摩擦补偿然后在笛卡尔空间只处理力环和阻抗环。这样分层处理鲁棒性更好。提示如果你用的是ROS的ros_control框架joint_effort_controller已经帮你处理了关节空间的力矩接口你只需要在笛卡尔空间算出期望力再通过J^T映射成关节力矩指令发下去即可。2.3 重力补偿力控的前置条件这一点必须单独强调。如果你没做好重力补偿力传感器读到的力里混着机械臂自身重力和负载重力力控根本没法做。重力补偿的基本思路是根据当前关节角度 q计算各连杆重力在末端产生的等效力然后从传感器读数中减掉。对于6-DOF机械臂可以用拉格朗日法或牛顿-欧拉法建立重力项 G(q)。实际工程中我推荐用辨识的方法——让机械臂在多个位姿下静止记录传感器读数和关节角度用最小二乘拟合出各连杆的质量和质心位置。我用的辨识流程大致是在关节空间均匀采样30~50个位姿每个位姿静止2秒记录力传感器六维读数和关节角度。建立回归模型F_gravity Φ(q) · θ其中 θ 是待辨识的重力参数向量各连杆质量和质心。用最小二乘求解 θ。验证在未参与辨识的位姿上测试残差应小于传感器噪声的2倍。实测下来辨识后的重力补偿残差可以做到0.5 N以内对于大多数力控任务够用了。如果残差还是大检查两点一是传感器零点漂移二是关节角度编码器零位是否准确。3. 力/位混合控制的实现细节与选择矩阵设计3.1 选择矩阵怎么定选择矩阵 S 是一个6×6对角矩阵对角线元素为1表示该方向做位置控制为0表示做力控制。比如在笛卡尔坐标系下Z轴方向做力控其余做位置控S 就是S diag(1, 1, 0, 1, 1, 1)但实际任务中约束方向往往不在世界坐标系下而在工件坐标系下。比如拧一个倾斜面上的螺丝力控方向应该是螺丝轴线方向不是世界Z轴。这时候需要把选择矩阵从工件坐标系变换到世界坐标系S_world R · S_task · R^T其中 R 是工件坐标系到世界坐标系的旋转矩阵。这个变换很容易搞错我第一次做的时候就忘了转结果力控方向偏了30度力跟踪效果很差。3.2 力环和位置环的合成力/位混合控制的结构是位置环输出一个笛卡尔位置修正量 Δx_pos力环输出一个笛卡尔力修正量 F_force两者通过选择矩阵合成x_cmd x_des (I - S) · Δx_force F_cmd S · F_des (I - S) · F_force更常见的实现是在速度层面合成。位置环输出期望速度 ẋ_pos力环通过导纳关系输出速度修正 ẋ_forceẋ_cmd S · ẋ_pos (I - S) · ẋ_force其中 ẋ_force 由力误差经过导纳控制器得到ẋ_force (F_des - F_meas) / bb 是导纳系数相当于力到速度的转换增益。b 越大同样的力误差产生的速度越小系统越硬b 越小系统越软。我在实际调试中发现力环的积分项很关键。纯比例控制会有稳态力误差因为环境刚度未知。加积分后稳态误差可以压到0.1 N以内。但积分增益不能太大否则接触瞬间会超调力会冲一下。我的经验值是比例增益先调到系统刚好不振荡然后积分增益取比例增益的1/10~1/5再根据实际力跟踪曲线微调。3.3 接触过渡最容易被忽略的环节从自由空间到接触的过渡阶段是力控最容易出问题的地方。自由空间里力传感器读数为零力环积分项会累积因为力误差一直存在一旦接触积分项突然释放力会猛冲。解决办法是积分限幅接触检测。积分项设置上下限比如±2 N对应的积分量。同时用接触检测逻辑当力传感器读数超过阈值比如0.5 N时才启用积分项接触前积分项清零。我用的接触检测逻辑是这样的# 伪代码 force_threshold 0.5 # N if abs(F_meas) force_threshold: integral_term 0 # 接触前清零 in_contact False else: in_contact True integral_term ki * (F_des - F_meas) * dt integral_term clamp(integral_term, -integral_limit, integral_limit)这个逻辑看起来简单但效果立竿见影。没加之前接触瞬间力冲到8 N加了之后超调控制在1 N以内。4. 阻抗控制的参数整定从能跑到好用4.1 阻抗模型的选择阻抗控制的目标方程是M_d · (ẍ - ẍ_d) B_d · (ẋ - ẋ_d) K_d · (x - x_d) F_ext其中 M_d、B_d、K_d 是期望的惯性、阻尼、刚度矩阵F_ext 是环境对末端的作用力。这个方程的物理意义是把机械臂末端等效成一个质量-阻尼-弹簧系统环境力作用上去末端按这个系统的动态响应。实际实现时通常忽略惯性项M_d 设为零或很小因为惯性项会引入振荡。简化后B_d · (ẋ - ẋ_d) K_d · (x - x_d) F_ext这就是导纳控制的形式测量力输出位置修正。对于大多数协作臂任务导纳控制足够用而且参数更好调。4.2 刚度矩阵 K_d 的整定K_d 决定末端有多硬。K_d 越大末端越不容易被推动K_d 越小末端越柔顺。整定 K_d 的方法我总结了一个实用流程确定任务需要的最大柔顺位移。比如插孔任务允许的侧向偏移是±2 mm期望的最大侧向力是5 N那么 K_d 5 N / 0.002 m 2500 N/m。从低刚度开始试。先设 K_d 500 N/m手动推末端感受柔顺程度然后逐步加大到目标值。各向异性设置。大多数任务不需要各向同性刚度。比如插孔时侧向刚度可以低允许偏移轴向刚度可以高保持插入力。K_d 矩阵的对角线元素可以分别设置。我常用的一个配置是侧向刚度500 N/m轴向刚度2000 N/m扭转刚度50 Nm/rad。这个配置在插孔和抓取任务上都比较稳。4.3 阻尼矩阵 B_d 的整定B_d 决定系统的粘滞感。B_d 太小系统会振荡B_d 太大响应迟钝。有一个经验公式可以参考B_d 2 · ζ · sqrt(K_d · M_d)其中 ζ 是阻尼比一般取0.7~1.0。如果忽略惯性项 M_d这个公式不好直接用。我通常用另一个经验关系B_d α · K_dα 取0.01~0.1单位是秒。α 越小系统越快但容易振荡α 越大系统越稳但响应慢。我一般从 α 0.05 开始试根据实际响应调整。实测中B_d 对接触稳定性的影响比 K_d 还大。有一次我把 K_d 调得很低200 N/m但 B_d 没跟着调结果系统在接触时低频振荡周期大概0.5秒。后来把 B_d 从10 Ns/m加到50 Ns/m振荡就消失了。4.4 阻抗控制与力跟踪的差异这里要澄清一个常见误解阻抗控制不等于力跟踪。阻抗控制控制的是末端的动态关系不是力本身。如果你设定 K_d 1000 N/m期望位置在工件表面下方1 mm那么稳态接触力是1 N——但这是碰巧的结果不是控制器主动跟踪1 N。如果要精确跟踪力需要在阻抗控制外层加一个力环或者直接用力/位混合控制。我在做打磨任务时一开始想用纯阻抗控制保持恒定压力结果发现工件表面高低不平时压力波动很大。后来改成阻抗控制力环积分压力波动从±3 N降到±0.5 N。5. 从仿真到实机的翻车现场与排查链路5.1 仿真里好好的实机上力传感器噪声炸了仿真里力传感器是理想的读数干净。实机上力传感器噪声可能有0.1~0.5 N的峰峰值而且随着温度漂移。我第一次把仿真调好的参数直接上实机力环积分项被噪声喂得乱跑末端一直在抖。排查链路先看原始信号。把力传感器六维数据录下来画时域曲线。如果噪声峰峰值超过0.2 N先做滤波。滤波方案选择。我用的是二阶巴特沃斯低通滤波器截止频率20 Hz。截止频率不能太低否则力环相位滞后大接触时容易振荡也不能太高否则噪声滤不干净。20 Hz是我在1 kHz采样率下的经验值。积分项抗饱和。除了限幅还可以用条件积分——只有当力误差小于某个阈值时才积分大误差时只走比例项。死区设置。力误差小于0.1 N时不动作避免噪声驱动执行器。做完这四步末端抖动基本消失。5.2 接触瞬间的力冲击前面提过积分项的问题但即使积分清零接触瞬间还是可能有冲击。原因是自由空间里末端有一定速度接触时动能瞬间转化为力。解决办法是接近速度控制。在距离接触面还有5~10 mm时把末端速度降到5 mm/s以下。这个慢接近策略可以把接触冲击力降低一个数量级。我在代码里加了一个简单的距离判断# 伪代码 approach_distance 0.01 # 10 mm if distance_to_surface approach_distance: speed 0.05 # 50 mm/s else: speed 0.005 # 5 mm/s距离怎么知道如果工件模型已知可以直接算如果未知可以用力传感器——当力读数超过0.1 N时认为接触但这时候已经碰上了。更好的办法是用视觉或者超声测距但会增加系统复杂度。我的折中方案是先用已知模型做慢接近接触后用阻抗控制吸收残余冲击。5.3 雅可比矩阵奇异导致的力估计跳变前面提过这个问题这里展开讲排查过程。现象机械臂在某个位姿附近时末端力估计值突然从2 N跳到8 N然后又跳回来。关节力矩指令也跟着跳机械臂抖动。排查录雅可比矩阵条件数。在抖动位姿附近条件数从正常的10~20跳到500以上。确认奇异方向。对 J 做SVD分解最小奇异值接近零的方向就是奇异方向。加阻尼。用阻尼最小二乘求逆λ 取0.05。条件数降到50以下跳变消失。规划避奇异。在轨迹规划时用关节空间插值代替笛卡尔空间直线插值避开奇异位形。注意阻尼最小二乘会引入位置误差λ 越大误差越大。λ 0.05 时位置误差大概在0.1 mm量级对于大多数任务可以接受。如果任务精度要求高优先用规划避奇异阻尼作为兜底。5.4 重力补偿残差随位姿变化重力补偿做完后我在工作空间中心区域测试残差0.3 N很好。但机械臂伸到边缘区域时残差变成1.5 N。力控效果明显变差。原因重力补偿参数是在有限位姿下辨识的外推到边缘区域时模型误差放大。另外关节摩擦在低速时非线性明显也会影响残差。改进措施扩大辨识位姿范围。把辨识位姿覆盖整个工作空间而不是集中在中心。加摩擦补偿。用Stribeck摩擦模型补偿关节低速摩擦。摩擦参数也可以用辨识方法得到。在线自适应。如果力控任务时间长可以加一个慢速自适应项在线修正重力补偿残差。我最终用了前两条残差在全工作空间内控制在0.8 N以内。第三条没上因为增加了系统复杂度而且0.8 N对于我的任务够用了。6. 一套可复现的调试流程与参数记录6.1 调试顺序不能乱力控调试最忌讳一上来就调力环参数。正确的顺序是关节空间基础控制。确保位置控制、速度控制正常关节跟随误差小于0.1度。重力补偿。辨识参数验证残差。力传感器标定。零点标定、坐标系标定。力传感器坐标系和末端坐标系不重合时需要做坐标变换。自由空间阻抗控制。不接触环境手动推末端感受柔顺性调 K_d 和 B_d。慢速接触测试。用低速接近观察接触瞬间的力冲击调积分限幅和接触检测阈值。力跟踪测试。设定恒定力目标观察稳态误差和超调。任务级测试。插孔、打磨、抓取等实际任务。每一步都要录数据不要凭感觉。我用的数据记录频率是1 kHz记录量包括关节角度、关节速度、关节力矩、末端位姿、末端速度、力传感器六维、控制指令。6.2 参数记录表调试过程中参数会改很多次不记录的话很快就乱了。我用的记录表格式如下参数初始值调整值调整原因效果K_d 侧向1000 N/m500 N/m插孔时侧向力过大侧向力从8N降到3NB_d 侧向20 Ns/m50 Ns/m接触时低频振荡振荡消失力环 Kp0.50.3接触超调超调从3N降到1N力环 Ki0.050.03噪声驱动积分稳态误差0.2N滤波截止50 Hz20 Hz噪声峰峰值0.3N噪声降到0.1N这个表看起来简单但帮我省了很多重复调试的时间。每次改参数前先查表避免改回去。6.3 一个完整的插孔任务参数配置以轴孔装配为例我最终用的参数配置是接近阶段速度5 mm/s力阈值0.5 N触发接触检测。搜索阶段侧向刚度300 N/m轴向刚度1500 N/m阻尼系数 α 0.06。插入阶段轴向力目标10 N力环 Kp 0.4Ki 0.02积分限幅±3 N。滤波二阶巴特沃斯截止20 Hz。安全限制最大力20 N超过则停止并回退。这套参数在间隙0.1 mm的轴孔上插入成功率95%以上平均插入时间3.2秒。失败的情况主要是孔口有毛刺或者工件定位偏差超过0.5 mm。7. 几个容易踩的坑和我的处理方式7.1 力传感器坐标系与末端坐标系不重合力传感器安装在腕部它的坐标系和末端工具坐标系通常不重合。如果你直接用力传感器读数做力控方向会错。处理方式做一次坐标变换。如果传感器坐标系到末端坐标系的旋转矩阵是 R_se那么末端等效力 F_ee R_se · F_sensor。R_se 可以通过标定得到——在末端安装一个已知质量的负载改变机械臂姿态根据力读数变化反算 R_se。我偷懒的时候直接用CAD模型里的安装关系结果误差有5度力控方向偏了后来还是老老实实标定了一遍。7.2 关节摩擦在低速时的影响协作臂的关节摩擦在低速时非线性很明显尤其是谐波减速器的启动摩擦。这会导致力控在低速时爬行——末端一动一停。处理方式加摩擦前馈补偿。用Stribeck模型τ_friction τ_c · sign(q̇) τ_s · exp(-|q̇|/q̇_s) · sign(q̇) b · q̇其中 τ_c 是库仑摩擦τ_s 是静摩擦q̇_s 是Stribeck速度b 是粘滞摩擦系数。这些参数可以通过关节匀速运动测试辨识。我加了摩擦补偿后低速爬行基本消失力控的平稳性明显提升。7.3 通信延迟对力控稳定性的影响力控对通信延迟很敏感。如果你的控制循环是1 kHz但力传感器数据通过EtherCAT或串口过来有2 ms延迟力环的相位裕度会下降容易振荡。处理方式提高传感器采样率。能用1 kHz就不要用100 Hz。减少通信环节。力传感器直接接控制器不要经过额外的转换模块。预测补偿。用一阶预测器补偿延迟F_pred F_meas dF/dt · τ_delay。但预测器会放大噪声要配合滤波使用。我实测下来2 ms延迟对力控影响不大5 ms以上就要注意了。如果你的系统延迟大先把延迟降下来再调参数。7.4 工件刚度未知时的参数适应阻抗控制的性能取决于环境刚度。如果工件很硬比如金属K_d 可以设大一些如果工件很软比如橡胶K_d 要设小否则力会冲很高。但实际任务中工件刚度往往未知。我的处理方式是先用低刚度试接触根据接触时的力-位移曲线估计环境刚度然后调整 K_d。估计公式是K_env ΔF / Δx在接触过渡阶段记录力和位移做线性拟合得到 K_env。然后设 K_d K_env / 3 ~ K_env / 2。这样既能保证柔顺性又能保证力跟踪精度。这个自适应逻辑我写成了一个简单的状态机接触后前0.5秒做刚度估计然后切换参数。实测效果不错换不同工件时不用手动改参数。8. 从力控到更复杂的交互任务力/位混合控制和阻抗控制是基础往上还可以叠加更多能力。比如力控视觉视觉定位工件力控做精细装配。视觉给粗定位力控做最后0.5 mm的搜索和插入。力控强化学习用强化学习调阻抗参数适应不同工件。这个方向我还在试目前用简单的策略梯度收敛慢但比手动调参省事。多臂协同力控两个臂一起搬一个工件需要协调力分配。这个对通信和同步要求很高我还没实机跑通。如果你刚开始做力控我建议先把单臂的力/位混合控制跑通再考虑叠加其他能力。基础不牢叠加的东西越多越乱。最后分享一个我调试时的小习惯每次改参数前先把当前参数和对应的力曲线截图存下来。调试结束后回看能清楚看到每个参数对系统行为的影响。这个习惯帮我少走了很多弯路也让我对参数整定有了直觉。力控这东西理论看再多不如亲手调一遍。
六轴机械臂力控实战：力/位混合与阻抗控制从仿真到实机
六轴机械臂在自由空间里跑轨迹规划和让它去拧螺丝、擦玻璃、插连接器完全是两码事。前者只要位置到了就行后者位置和力都得管——力大了压碎工件力小了打滑。这就是笛卡尔空间下的力/位混合控制与阻抗控制要解决的问题。我最近在一个6-DOF协作臂项目上把这两套控制策略从仿真到实机都跑了一遍踩了不少坑也积累了一些文档里不会写的经验。这篇内容适合已经做过基础运动学、用过ROS控制框架、想往力控方向深入的开发者也适合正在做机械臂毕业设计、需要把能动能抓升级到能柔顺交互的朋友。下面我按实际调试顺序把原理、实现、参数整定和翻车现场都摊开讲。1. 为什么位置控制到了接触阶段就不够用了1.1 从到点到接触的本质变化位置控制的核心逻辑是给定目标位姿控制器算出关节力矩或速度把末端推过去。在自由空间里这套逻辑没问题因为机械臂末端不受外力约束你让它去哪它就去哪。但一旦末端接触环境情况就变了——环境会给末端一个反作用力这个力会通过机械臂的连杆传回关节形成扰动力矩。如果你还在用纯位置控制控制器会拼命维持位置结果就是力不断累积。举个实际数字假设位置环刚度等效为10000 N/m你让末端往工件表面多走0.5 mm产生的接触力就是5 N。对于拧M3螺丝或者抓鸡蛋这类任务5 N已经足以造成损坏。更麻烦的是这个力是控制器看不见的——位置控制根本不测量力它只知道自己没到位于是继续加大输出。我在第一次做插孔任务时就吃了这个亏。孔轴配合间隙0.1 mm位置控制精度标称0.05 mm理论上够用。但实际装配时轴端倒角一碰到孔口边缘侧向力直接把轴推偏位置环又把它拉回来来回振荡最后把孔口刮花了。问题不在于位置精度不够而在于接触阶段的主导物理量已经从位置变成了力。1.2 力/位混合控制与阻抗控制的分工力/位混合控制Hybrid Force/Position Control的思路很直接把笛卡尔空间的6个自由度拆开一部分方向做位置控制另一部分方向做力控制。比如擦玻璃时垂直于玻璃面的方向做力控制保持恒定压力平行于玻璃面的两个方向做位置控制按轨迹移动。这个拆方向的操作通过一个选择矩阵实现对角线上是0或10对应的方向走力环1对应的方向走位置环。阻抗控制Impedance Control则是另一种哲学不硬拆方向而是把机械臂末端等效成一个弹簧-阻尼-质量系统让末端对外表现为一个可调的阻抗。环境推它它就按设定的刚度退让环境松开它就按设定的刚度回去。你调的不是力也不是位置而是这个虚拟弹簧的软硬。两者的关系可以这样理解力/位混合控制是分而治之适合任务方向明确、约束已知的场景阻抗控制是统一建模适合约束不确定、需要柔顺交互的场景。实际项目中我经常把两者结合——外层用阻抗控制保证安全内层在特定方向叠加力环做精确力跟踪。对比维度力/位混合控制阻抗控制控制目标特定方向力跟踪其余方向位置跟踪末端动态关系质量-阻尼-刚度需要环境模型需要知道约束方向不需要显式环境模型参数整定难度中等方向划分清晰较高三个参数矩阵互相耦合适用场景打磨、擦拭、装配人机协作、未知环境探索力跟踪精度高有积分项时取决于阻抗参数和环境刚度2. 笛卡尔空间力控的数学底子别跳过这一步2.1 雅可比矩阵与力域映射很多教程讲力控直接从控制框图开始结果调参时完全不知道每个量在物理上意味着什么。我建议先把雅可比矩阵的力映射关系搞清楚。机械臂关节力矩 τ 和末端笛卡尔力 F 的关系是τ J^T · F其中 J 是雅可比矩阵6×nn为关节数。这个转置关系是力控的基石。反过来如果你在关节空间施加了力矩 τ末端感受到的等效力就是F (J^T)^(-1) · τ对于6-DOF机械臂J 是6×6方阵可以直接求逆。但要注意当机械臂接近奇异位形时J 的条件数急剧变大求逆会放大误差。我在调试时就遇到过机械臂伸得比较直的时候末端力估计值突然跳变后来发现是雅可比矩阵接近奇异(J^T)^(-1)的数值不稳定。解决办法有两个一是用阻尼最小二乘Damped Least Squares在求逆时加一个阻尼项 λJ^ J^T · (J·J^T λ²·I)^(-1)λ 一般取0.01~0.1根据奇异程度动态调整。二是直接避开奇异位形在轨迹规划阶段就把工作空间限制好。我通常两个都用——规划时避开控制时加阻尼兜底。2.2 笛卡尔空间动力学方程要在笛卡尔空间做力控得把关节空间的动力学方程变换过去。关节空间方程是M(q)·q̈ C(q,q̇)·q̇ G(q) τ τ_ext其中 M 是惯性矩阵C 是科氏力和离心力项G 是重力项τ_ext 是外部接触力映射到关节的力矩。变换到笛卡尔空间后M_x(q)·ẍ C_x(q,q̇)·ẋ G_x(q) F F_ext变换关系是M_x J^(-T) · M · J^(-1) C_x J^(-T) · (C - M·J^(-1)·J̇) · J^(-1) G_x J^(-T) · G这里 J^(-T) 表示先求逆再转置。实际实现时我一般不在线计算完整的 M_x 和 C_x因为计算量大且对模型误差敏感。更常用的做法是在关节空间做动力学补偿重力补偿、摩擦补偿然后在笛卡尔空间只处理力环和阻抗环。这样分层处理鲁棒性更好。提示如果你用的是ROS的ros_control框架joint_effort_controller已经帮你处理了关节空间的力矩接口你只需要在笛卡尔空间算出期望力再通过J^T映射成关节力矩指令发下去即可。2.3 重力补偿力控的前置条件这一点必须单独强调。如果你没做好重力补偿力传感器读到的力里混着机械臂自身重力和负载重力力控根本没法做。重力补偿的基本思路是根据当前关节角度 q计算各连杆重力在末端产生的等效力然后从传感器读数中减掉。对于6-DOF机械臂可以用拉格朗日法或牛顿-欧拉法建立重力项 G(q)。实际工程中我推荐用辨识的方法——让机械臂在多个位姿下静止记录传感器读数和关节角度用最小二乘拟合出各连杆的质量和质心位置。我用的辨识流程大致是在关节空间均匀采样30~50个位姿每个位姿静止2秒记录力传感器六维读数和关节角度。建立回归模型F_gravity Φ(q) · θ其中 θ 是待辨识的重力参数向量各连杆质量和质心。用最小二乘求解 θ。验证在未参与辨识的位姿上测试残差应小于传感器噪声的2倍。实测下来辨识后的重力补偿残差可以做到0.5 N以内对于大多数力控任务够用了。如果残差还是大检查两点一是传感器零点漂移二是关节角度编码器零位是否准确。3. 力/位混合控制的实现细节与选择矩阵设计3.1 选择矩阵怎么定选择矩阵 S 是一个6×6对角矩阵对角线元素为1表示该方向做位置控制为0表示做力控制。比如在笛卡尔坐标系下Z轴方向做力控其余做位置控S 就是S diag(1, 1, 0, 1, 1, 1)但实际任务中约束方向往往不在世界坐标系下而在工件坐标系下。比如拧一个倾斜面上的螺丝力控方向应该是螺丝轴线方向不是世界Z轴。这时候需要把选择矩阵从工件坐标系变换到世界坐标系S_world R · S_task · R^T其中 R 是工件坐标系到世界坐标系的旋转矩阵。这个变换很容易搞错我第一次做的时候就忘了转结果力控方向偏了30度力跟踪效果很差。3.2 力环和位置环的合成力/位混合控制的结构是位置环输出一个笛卡尔位置修正量 Δx_pos力环输出一个笛卡尔力修正量 F_force两者通过选择矩阵合成x_cmd x_des (I - S) · Δx_force F_cmd S · F_des (I - S) · F_force更常见的实现是在速度层面合成。位置环输出期望速度 ẋ_pos力环通过导纳关系输出速度修正 ẋ_forceẋ_cmd S · ẋ_pos (I - S) · ẋ_force其中 ẋ_force 由力误差经过导纳控制器得到ẋ_force (F_des - F_meas) / bb 是导纳系数相当于力到速度的转换增益。b 越大同样的力误差产生的速度越小系统越硬b 越小系统越软。我在实际调试中发现力环的积分项很关键。纯比例控制会有稳态力误差因为环境刚度未知。加积分后稳态误差可以压到0.1 N以内。但积分增益不能太大否则接触瞬间会超调力会冲一下。我的经验值是比例增益先调到系统刚好不振荡然后积分增益取比例增益的1/10~1/5再根据实际力跟踪曲线微调。3.3 接触过渡最容易被忽略的环节从自由空间到接触的过渡阶段是力控最容易出问题的地方。自由空间里力传感器读数为零力环积分项会累积因为力误差一直存在一旦接触积分项突然释放力会猛冲。解决办法是积分限幅接触检测。积分项设置上下限比如±2 N对应的积分量。同时用接触检测逻辑当力传感器读数超过阈值比如0.5 N时才启用积分项接触前积分项清零。我用的接触检测逻辑是这样的# 伪代码 force_threshold 0.5 # N if abs(F_meas) force_threshold: integral_term 0 # 接触前清零 in_contact False else: in_contact True integral_term ki * (F_des - F_meas) * dt integral_term clamp(integral_term, -integral_limit, integral_limit)这个逻辑看起来简单但效果立竿见影。没加之前接触瞬间力冲到8 N加了之后超调控制在1 N以内。4. 阻抗控制的参数整定从能跑到好用4.1 阻抗模型的选择阻抗控制的目标方程是M_d · (ẍ - ẍ_d) B_d · (ẋ - ẋ_d) K_d · (x - x_d) F_ext其中 M_d、B_d、K_d 是期望的惯性、阻尼、刚度矩阵F_ext 是环境对末端的作用力。这个方程的物理意义是把机械臂末端等效成一个质量-阻尼-弹簧系统环境力作用上去末端按这个系统的动态响应。实际实现时通常忽略惯性项M_d 设为零或很小因为惯性项会引入振荡。简化后B_d · (ẋ - ẋ_d) K_d · (x - x_d) F_ext这就是导纳控制的形式测量力输出位置修正。对于大多数协作臂任务导纳控制足够用而且参数更好调。4.2 刚度矩阵 K_d 的整定K_d 决定末端有多硬。K_d 越大末端越不容易被推动K_d 越小末端越柔顺。整定 K_d 的方法我总结了一个实用流程确定任务需要的最大柔顺位移。比如插孔任务允许的侧向偏移是±2 mm期望的最大侧向力是5 N那么 K_d 5 N / 0.002 m 2500 N/m。从低刚度开始试。先设 K_d 500 N/m手动推末端感受柔顺程度然后逐步加大到目标值。各向异性设置。大多数任务不需要各向同性刚度。比如插孔时侧向刚度可以低允许偏移轴向刚度可以高保持插入力。K_d 矩阵的对角线元素可以分别设置。我常用的一个配置是侧向刚度500 N/m轴向刚度2000 N/m扭转刚度50 Nm/rad。这个配置在插孔和抓取任务上都比较稳。4.3 阻尼矩阵 B_d 的整定B_d 决定系统的粘滞感。B_d 太小系统会振荡B_d 太大响应迟钝。有一个经验公式可以参考B_d 2 · ζ · sqrt(K_d · M_d)其中 ζ 是阻尼比一般取0.7~1.0。如果忽略惯性项 M_d这个公式不好直接用。我通常用另一个经验关系B_d α · K_dα 取0.01~0.1单位是秒。α 越小系统越快但容易振荡α 越大系统越稳但响应慢。我一般从 α 0.05 开始试根据实际响应调整。实测中B_d 对接触稳定性的影响比 K_d 还大。有一次我把 K_d 调得很低200 N/m但 B_d 没跟着调结果系统在接触时低频振荡周期大概0.5秒。后来把 B_d 从10 Ns/m加到50 Ns/m振荡就消失了。4.4 阻抗控制与力跟踪的差异这里要澄清一个常见误解阻抗控制不等于力跟踪。阻抗控制控制的是末端的动态关系不是力本身。如果你设定 K_d 1000 N/m期望位置在工件表面下方1 mm那么稳态接触力是1 N——但这是碰巧的结果不是控制器主动跟踪1 N。如果要精确跟踪力需要在阻抗控制外层加一个力环或者直接用力/位混合控制。我在做打磨任务时一开始想用纯阻抗控制保持恒定压力结果发现工件表面高低不平时压力波动很大。后来改成阻抗控制力环积分压力波动从±3 N降到±0.5 N。5. 从仿真到实机的翻车现场与排查链路5.1 仿真里好好的实机上力传感器噪声炸了仿真里力传感器是理想的读数干净。实机上力传感器噪声可能有0.1~0.5 N的峰峰值而且随着温度漂移。我第一次把仿真调好的参数直接上实机力环积分项被噪声喂得乱跑末端一直在抖。排查链路先看原始信号。把力传感器六维数据录下来画时域曲线。如果噪声峰峰值超过0.2 N先做滤波。滤波方案选择。我用的是二阶巴特沃斯低通滤波器截止频率20 Hz。截止频率不能太低否则力环相位滞后大接触时容易振荡也不能太高否则噪声滤不干净。20 Hz是我在1 kHz采样率下的经验值。积分项抗饱和。除了限幅还可以用条件积分——只有当力误差小于某个阈值时才积分大误差时只走比例项。死区设置。力误差小于0.1 N时不动作避免噪声驱动执行器。做完这四步末端抖动基本消失。5.2 接触瞬间的力冲击前面提过积分项的问题但即使积分清零接触瞬间还是可能有冲击。原因是自由空间里末端有一定速度接触时动能瞬间转化为力。解决办法是接近速度控制。在距离接触面还有5~10 mm时把末端速度降到5 mm/s以下。这个慢接近策略可以把接触冲击力降低一个数量级。我在代码里加了一个简单的距离判断# 伪代码 approach_distance 0.01 # 10 mm if distance_to_surface approach_distance: speed 0.05 # 50 mm/s else: speed 0.005 # 5 mm/s距离怎么知道如果工件模型已知可以直接算如果未知可以用力传感器——当力读数超过0.1 N时认为接触但这时候已经碰上了。更好的办法是用视觉或者超声测距但会增加系统复杂度。我的折中方案是先用已知模型做慢接近接触后用阻抗控制吸收残余冲击。5.3 雅可比矩阵奇异导致的力估计跳变前面提过这个问题这里展开讲排查过程。现象机械臂在某个位姿附近时末端力估计值突然从2 N跳到8 N然后又跳回来。关节力矩指令也跟着跳机械臂抖动。排查录雅可比矩阵条件数。在抖动位姿附近条件数从正常的10~20跳到500以上。确认奇异方向。对 J 做SVD分解最小奇异值接近零的方向就是奇异方向。加阻尼。用阻尼最小二乘求逆λ 取0.05。条件数降到50以下跳变消失。规划避奇异。在轨迹规划时用关节空间插值代替笛卡尔空间直线插值避开奇异位形。注意阻尼最小二乘会引入位置误差λ 越大误差越大。λ 0.05 时位置误差大概在0.1 mm量级对于大多数任务可以接受。如果任务精度要求高优先用规划避奇异阻尼作为兜底。5.4 重力补偿残差随位姿变化重力补偿做完后我在工作空间中心区域测试残差0.3 N很好。但机械臂伸到边缘区域时残差变成1.5 N。力控效果明显变差。原因重力补偿参数是在有限位姿下辨识的外推到边缘区域时模型误差放大。另外关节摩擦在低速时非线性明显也会影响残差。改进措施扩大辨识位姿范围。把辨识位姿覆盖整个工作空间而不是集中在中心。加摩擦补偿。用Stribeck摩擦模型补偿关节低速摩擦。摩擦参数也可以用辨识方法得到。在线自适应。如果力控任务时间长可以加一个慢速自适应项在线修正重力补偿残差。我最终用了前两条残差在全工作空间内控制在0.8 N以内。第三条没上因为增加了系统复杂度而且0.8 N对于我的任务够用了。6. 一套可复现的调试流程与参数记录6.1 调试顺序不能乱力控调试最忌讳一上来就调力环参数。正确的顺序是关节空间基础控制。确保位置控制、速度控制正常关节跟随误差小于0.1度。重力补偿。辨识参数验证残差。力传感器标定。零点标定、坐标系标定。力传感器坐标系和末端坐标系不重合时需要做坐标变换。自由空间阻抗控制。不接触环境手动推末端感受柔顺性调 K_d 和 B_d。慢速接触测试。用低速接近观察接触瞬间的力冲击调积分限幅和接触检测阈值。力跟踪测试。设定恒定力目标观察稳态误差和超调。任务级测试。插孔、打磨、抓取等实际任务。每一步都要录数据不要凭感觉。我用的数据记录频率是1 kHz记录量包括关节角度、关节速度、关节力矩、末端位姿、末端速度、力传感器六维、控制指令。6.2 参数记录表调试过程中参数会改很多次不记录的话很快就乱了。我用的记录表格式如下参数初始值调整值调整原因效果K_d 侧向1000 N/m500 N/m插孔时侧向力过大侧向力从8N降到3NB_d 侧向20 Ns/m50 Ns/m接触时低频振荡振荡消失力环 Kp0.50.3接触超调超调从3N降到1N力环 Ki0.050.03噪声驱动积分稳态误差0.2N滤波截止50 Hz20 Hz噪声峰峰值0.3N噪声降到0.1N这个表看起来简单但帮我省了很多重复调试的时间。每次改参数前先查表避免改回去。6.3 一个完整的插孔任务参数配置以轴孔装配为例我最终用的参数配置是接近阶段速度5 mm/s力阈值0.5 N触发接触检测。搜索阶段侧向刚度300 N/m轴向刚度1500 N/m阻尼系数 α 0.06。插入阶段轴向力目标10 N力环 Kp 0.4Ki 0.02积分限幅±3 N。滤波二阶巴特沃斯截止20 Hz。安全限制最大力20 N超过则停止并回退。这套参数在间隙0.1 mm的轴孔上插入成功率95%以上平均插入时间3.2秒。失败的情况主要是孔口有毛刺或者工件定位偏差超过0.5 mm。7. 几个容易踩的坑和我的处理方式7.1 力传感器坐标系与末端坐标系不重合力传感器安装在腕部它的坐标系和末端工具坐标系通常不重合。如果你直接用力传感器读数做力控方向会错。处理方式做一次坐标变换。如果传感器坐标系到末端坐标系的旋转矩阵是 R_se那么末端等效力 F_ee R_se · F_sensor。R_se 可以通过标定得到——在末端安装一个已知质量的负载改变机械臂姿态根据力读数变化反算 R_se。我偷懒的时候直接用CAD模型里的安装关系结果误差有5度力控方向偏了后来还是老老实实标定了一遍。7.2 关节摩擦在低速时的影响协作臂的关节摩擦在低速时非线性很明显尤其是谐波减速器的启动摩擦。这会导致力控在低速时爬行——末端一动一停。处理方式加摩擦前馈补偿。用Stribeck模型τ_friction τ_c · sign(q̇) τ_s · exp(-|q̇|/q̇_s) · sign(q̇) b · q̇其中 τ_c 是库仑摩擦τ_s 是静摩擦q̇_s 是Stribeck速度b 是粘滞摩擦系数。这些参数可以通过关节匀速运动测试辨识。我加了摩擦补偿后低速爬行基本消失力控的平稳性明显提升。7.3 通信延迟对力控稳定性的影响力控对通信延迟很敏感。如果你的控制循环是1 kHz但力传感器数据通过EtherCAT或串口过来有2 ms延迟力环的相位裕度会下降容易振荡。处理方式提高传感器采样率。能用1 kHz就不要用100 Hz。减少通信环节。力传感器直接接控制器不要经过额外的转换模块。预测补偿。用一阶预测器补偿延迟F_pred F_meas dF/dt · τ_delay。但预测器会放大噪声要配合滤波使用。我实测下来2 ms延迟对力控影响不大5 ms以上就要注意了。如果你的系统延迟大先把延迟降下来再调参数。7.4 工件刚度未知时的参数适应阻抗控制的性能取决于环境刚度。如果工件很硬比如金属K_d 可以设大一些如果工件很软比如橡胶K_d 要设小否则力会冲很高。但实际任务中工件刚度往往未知。我的处理方式是先用低刚度试接触根据接触时的力-位移曲线估计环境刚度然后调整 K_d。估计公式是K_env ΔF / Δx在接触过渡阶段记录力和位移做线性拟合得到 K_env。然后设 K_d K_env / 3 ~ K_env / 2。这样既能保证柔顺性又能保证力跟踪精度。这个自适应逻辑我写成了一个简单的状态机接触后前0.5秒做刚度估计然后切换参数。实测效果不错换不同工件时不用手动改参数。8. 从力控到更复杂的交互任务力/位混合控制和阻抗控制是基础往上还可以叠加更多能力。比如力控视觉视觉定位工件力控做精细装配。视觉给粗定位力控做最后0.5 mm的搜索和插入。力控强化学习用强化学习调阻抗参数适应不同工件。这个方向我还在试目前用简单的策略梯度收敛慢但比手动调参省事。多臂协同力控两个臂一起搬一个工件需要协调力分配。这个对通信和同步要求很高我还没实机跑通。如果你刚开始做力控我建议先把单臂的力/位混合控制跑通再考虑叠加其他能力。基础不牢叠加的东西越多越乱。最后分享一个我调试时的小习惯每次改参数前先把当前参数和对应的力曲线截图存下来。调试结束后回看能清楚看到每个参数对系统行为的影响。这个习惯帮我少走了很多弯路也让我对参数整定有了直觉。力控这东西理论看再多不如亲手调一遍。