以前在学校调机械臂力控最痛苦的不是写控制器而是每次改参数都要上真机验证真机还怕撞。后来我养成了习惯任何控制律都先在 MuJoCo 里跑通再去动真机。这篇文章用一个完整的小项目记录如何在 MuJoCo 里实现机械臂的关节空间阻抗控制Impedance Control包括模型构建、控制律推导、Python 代码和调参经验。无论你是做具身智能机械臂抓取还是研究 UR5e、Panda 这类机械臂的柔顺控制这套思路都可以直接迁移。1. 为什么用 MuJoCo 做阻抗控制验证选型与建模1.1 MuJoCo 相比其他仿真器的优势阻抗控制的核心不仅仅是“让机械臂动起来”而是让机械臂对外力表现出可控的柔顺性。要验证这种动态特性仿真器需要满足两个条件一是动力学要够真实尤其是重力、科氏力和摩擦等偏置力不能糊弄二是接触模型要可靠因为阻抗控制最终是要应对接触的。MuJoCo 在这两点上表现很突出。它的 contact model 基于凸优化求解稳定性和速度都很好而且 Python API 从 2.x 之后越来越完善直接在mujoco包里操作model和data就能拿到qfrc_bias这类广义力信息这对实现阻抗控制来说非常方便。Gazebo 在 ROS 生态里用得广但动力学仿真速度慢接触参数标定也经常让人头疼PyBullet 上手快但在力控相关的高级动力学信息上需要自己算的东西更多。不是说其他仿真器不行而是在实现关节空间阻抗控制这个具体场景下MuJoCo 能让我把精力集中在控制律本身而不是花大量时间在仿真器适配和动力学推导上。另外MuJoCo 的时间步长和求解器选择也比较灵活。我习惯把timestep设成 0.002 秒也就是 500Hz 的控制频率。这个频率对阻抗控制来说足够高因为机械臂关节的闭环响应带宽通常不会超过 10Hz太高的频率反而会引入数字噪声。对于接触场景积分器用 Euler 往往比高阶方法更稳定不容易出现高频抖动。1.2 一个适合复现的两自由度机械臂 XML很多人一上来就加载一个巨大的 Panda URDF 或者带 STL 网格的机器人模型我的建议是先别急。社区模型虽然视觉效果好但动力学参数未必干净自由度过高会让调参时多出很多干扰项。我在验证控制律的时候习惯先用一个极简模型。两连杆平面机械臂就够了因为它已经包含了重力耦合、科氏力、关节间惯性耦合等所有关键动力学特性。这里给出我用的planar_arm.xml关节轴沿 y 方向机械臂在 x-z 平面内运动重力会真实作用到关节上所以qfrc_bias的补偿效果可以被很直观地验证。mujoco modelplanar_arm_2dof compiler angleradian/ option timestep0.002 integratorEuler gravity0 0 -9.81/ default joint axis0 1 0 damping0.05 armature0.01 limitedtrue range-3.0 3.0/ geom typecapsule size0.03 density1000/ /default worldbody light pos0 -2 3/ camera nameview pos0.2 -1.2 0.4 xyaxes0 1 0 0 0 1 modefixed/ geom namefloor typeplane pos0 0 -0.5 size1 1 0.01 rgba0.8 0.8 0.8 1/ body namelink0 pos0 0 0.15 joint nameshoulder pos0 0 0/ geom fromto0 0 0 0.25 0 0/ body namelink1 pos0.25 0 0 joint nameelbow pos0 0 0/ geom fromto0 0 0 0.25 0 0/ body nametip pos0.25 0 0 geom nametip_sphere typesphere size0.04 rgba1 0 0 0.8/ /body /body /body /worldbody actuator motor nameshoulder_motor jointshoulder ctrlrange-10 10/ motor nameelbow_motor jointelbow ctrlrange-5 5/ /actuator /mujoco这个模型里有两个关节shoulder和elbow每个关节都由一个motor执行器直接输出力矩。连杆用capsule定义两个关节都带一点阻尼和 armature模拟电机的转子惯量。ctrlrange限制了最大输出力矩这很关键——真实电机有饱和如果控制律算出来的力矩超出范围实际表现会和非线性限幅一样后面调参会遇到。2. 关节空间阻抗控制到底在控制什么控制律拆解2.1 从弹簧-阻尼系统到机械臂关节阻抗控制这个名词听起来高级但核心思想非常简单把机械臂的每个关节当作一个“弹簧-阻尼系统”来控制。假设你用手推一个门门后有一根弹簧和阻尼器你推得越深弹簧给你的反作用力越大你推得越快阻尼器给你的阻力越大。阻抗控制就是想精确地设定这根“虚拟弹簧”的刚度和“虚拟阻尼器”的阻尼。在关节空间里控制律可以写成tau Kp * (q_des - q) - Kd * qd bias(q, qd)其中q_des是期望关节角q是当前关节角qd是当前关节角速度Kp是关节刚度矩阵Kd是关节阻尼矩阵bias是重力、科氏力、离心力等动力学偏置项的补偿。如果期望角速度不为零第二项可以写成Kd * (qd_des - qd)。直观理解就是关节偏离目标位置越远控制力矩越大试图把它拉回去关节运动速度越快控制力矩越想把它“刹住”。当机械臂末端碰到环境时由于位置偏差增大控制力矩增大但增大的幅度由Kp决定。所以Kp代表的是机械臂对外力的“抵抗程度”也就是柔顺性。Kp越小越柔Kp越大越“硬”。2.2 PD重力补偿与通用阻抗控制的异同很多人看到上面的公式会说这不就是 PD 位置控制加个重力补偿吗对也不对。形式上一模一样但设计思想不同。PD 位置控制追求的是轨迹跟踪精度所以增益通常调得很大关节刚得像铁棍。阻抗控制虽然也用了相同的数学结构但Kp、Kd被理解为“期望阻抗参数”是系统对外力动态响应的设计目标而不是单纯为了消除跟踪误差。举个例子在装配场景里你希望机械臂在插入销钉时如果遇到轻微的位姿偏差机械臂能“退让”一点而不是和工件硬碰硬。这时的Kp就不能设太大否则接触力会迅速飙升。同时Kd必须匹配好否则机械臂会像弹簧一样来回振荡。阻抗控制的本质是调节“力”和“位置偏差”之间的动态关系而不是让机械臂精确到达某一点。两者在公式上重合但调参思路完全不同。更进一步通用的关节空间阻抗控制还包含期望惯量整形控制律会变成tau M(q) * a h(q, qd)其中a是参考加速度由期望惯量矩阵、期望阻尼和期望刚度共同决定。这会引入惯性矩阵的实时计算和求逆实现复杂度更高。对于大多数工程验证场景PD重力补偿的阻抗控制已经足够表达核心特性这也是我在这篇文章里重点展开的形式。2.3 qfrc_bias 在 MuJoCo 中的角色实现阻抗控制时最大的麻烦是算重力补偿。在多关节机械臂中每个关节的重力矩都会随着所有关节的角度变化而变化还要考虑科氏力和离心力的耦合。手推动力学方程既容易出错又难以维护。MuJoCo 里有一个现成的量data.qfrc_bias。qfrc_bias是广义坐标下的偏置力包含了重力、科氏力、离心力等所有速度相关和位置相关的非线性项。在求解动力学时MuJoCo 已经把机械臂的模型参数、质量和惯性分布考虑进去了所以直接拿它做动力学前馈一行动力学推导都不用写。于是关节空间阻抗控制律在 MuJoCo 里就变成tau Kp * (q_des - data.qpos[:2]) - Kd * data.qvel[:2] data.qfrc_bias[:2]注意data.qfrc_bias的长度是nv即自由度数量。对于两连杆模型就是 2。这样实现出来的控制器在稳态时只要Kp和Kd不是零关节角就一定能锁在期望位置附近因为重力等偏置已经被补偿掉了。3. Python 控制循环怎么写从初始化到力矩输出3.1 主循环框架与执行顺序MuJoCo 里最容易被忽略的细节是循环里各个函数的调用顺序。我的标准控制循环如下# 1. 计算当前状态下的动力学偏置 mujoco.mj_forward(model, data) # 2. 从 data 中读取关节状态 q data.qpos[:2].copy() qd data.qvel[:2].copy() # 3. 计算阻抗控制力矩 tau Kp * (q_des - q) - Kd * qd data.qfrc_bias[:2] data.ctrl[:2] tau # 4. 推进仿真一个时间步 mujoco.mj_step(model, data)为什么必须mj_forward之后再读qfrc_bias因为mj_step会在内部推进状态并更新data中的各项量如果你在mj_step之后再去读qfrc_bias它已经是下一时刻的偏置力了。用滞后一拍的偏置力来做当前时刻的重力补偿轻则控制精度下降重则出现持续振荡。刚开始我就在这里踩过坑后面专门写一节讲。另外data.ctrl在 MuJoCo 中对应的是执行器的控制输入。对于motor执行器来说它直接就是关节力矩所以写入tau就行。如果用的是position或velocity执行器就还需要额外的转换但阻抗控制通常都是力矩驱动的所以推荐直接用motor。3.2 外力扰动注入用 mj_applyFT 模拟接触验证阻抗控制柔顺性的一个重要手段是给机械臂施加外部力。很多人会用data.xfrc_applied但这个字段的力是定义在 body 局部坐标系下的当机械臂关节转动时局部坐标系的朝向会跟着变很难模拟一个恒定的世界系外负载。更好的方式是mujoco.mj_applyFT。它可以在世界坐标系下把力作用到指定 body 的指定点上并且自动转换为广义力累加到data.qfrc_applied中。代码片段如下body_tip mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, tip) point data.xpos[body_tip].copy() force np.array([0.0, 0.0, -15.0]) # 世界系下竖直向下的力 torque np.zeros(3) mujoco.mj_applyFT(model, data, force, torque, point, body_tip, data.qfrc_applied)这样在仿真到某个时间点后末端就会承受一个竖直向下的恒定力机械臂在阻抗控制下会出现稳态位置偏差。这个偏差的大小和Kp直接相关能非常直观地体现阻抗特性。3.3 数据记录与结果输出为了分析控制效果我会在循环里记录时间、关节角、期望关节角和实际力矩最后用 matplotlib 画出来。数据记录不需要每个仿真步都做每隔一步或者每五步记录一次就行否则数据文件会变得很大。下面的完整代码里用if i % 2 0来控制记录频率最终会生成一张包含两行子图的图片第一行是关节角曲线第二行是控制力矩曲线。4. 调参实验刚度、阻尼对阶跃响应与抗扰能力的影响4.1 三组参数下的阶跃响应对比我用上面这个两连杆模型做了多组实验下面三组比较有代表性。参数组Kp肩/肘Kd肩/肘阶跃响应表现外力扰动下的偏移第一组30 / 2025 / 18响应慢无超调接近过阻尼偏移很大柔顺但跟踪能力弱第二组180 / 1205 / 3响应快振荡明显甚至小幅发散趋势偏移很小但力矩容易饱和接触风险高第三组80 / 5015 / 10轻微超调约 0.5 秒稳定偏移适中柔顺性和跟踪性平衡较好第三组就是我最终采用的参数。第一组看上去稳定但机械臂对目标位置的“锁定能力”太弱稍有一点外力就会被推开这在很多场景下是不可接受的。第二组响应很快但阻尼不足导致关节像弹簧一样来回弹跳如果末端真的碰到硬物接触力会瞬间爆炸。第三组在快速响应和动态稳定性之间取得了一个折中。4.2 为什么阻尼不能随便拍脑袋选工程上很多人只看Kp随便把Kd设一个值就开始跑结果发现机械臂要么抖得厉害要么慢得难受。阻尼的选取和关节处的有效惯量密切相关。对于一个由弹簧-阻尼-质量构成的二阶系统临界阻尼的经验公式是Kd ≈ 2 * sqrt(Kp * I_eff)其中I_eff是关节处的有效惯量。但机械臂的I_eff不是常数它会随着构型变化而变化。肩关节在机械臂完全伸展时有效惯量大在蜷缩时有效惯量小肘关节的惯量相对稳定但也会变动。所以阻抗参数在理想情况下应该做“变增益”让Kp、Kd随构型变化。如果嫌麻烦可以像我现在这样选一组在大多数构型下都能稳定工作的折中参数。在实际调试时我通常先在仿真里把Kd从小到大扫一遍每次增加 30% 左右观察阶跃响应的超调量和稳定时间直到不再振荡。这个过程的效率比在真机上调试高太多了。4.3 外力扰动下的阻抗特性分析我在 t1 秒时向末端施加-15N的竖直向下的力可以看到机械臂关节角度出现明显的稳态偏移。偏移量由Kp和机械臂的雅可比共同决定。如果只看关节空间Kp越大稳态偏移越小但与此同时如果末端被环境卡住接触力也会按照同样的比例放大。这就是阻抗控制的“双刃剑”本质。你不可能同时要求机械臂既像铁板一样刚硬又像海绵一样柔软。Kp决定的是力的上限和刚度的折中Kd决定的是动态过程的耗能能力。理解了这个再看很多真实机械臂的柔顺控制参数就不会觉得奇怪了。5. 我在 MuJoCo 里踩过的坑与迁移到真机时要注意的事5.1 qfrc_bias 更新顺序导致的控制器发散我最初写控制循环时为了少一次mj_forward直接在mj_step之后读qfrc_bias结果机械臂在目标点附近出现缓慢的发散振荡。原因就是mj_step之后data已经推进到了新状态qfrc_bias对应的是新状态的重力和科氏力。用这个偏置去做当前状态的重力补偿相当于控制环路里多了一个时序滞后项。解决方式就是严格执行mj_forward- 读状态 - 算力矩 - 写ctrl-mj_step的顺序。如果你在每个控制周期还需要更新传感器、接触力等信息mj_forward本来也是必须的。5.2 执行器饱和与关节限位MuJoCo 里的motor执行器如果设置了ctrlrange控制力矩超出范围时会直接限幅。这在物理上是合理的但在观察控制效果时很容易让人误判。比如第二组参数响应快但力矩一直顶在 ±10Nm 的边界上这时候系统已经不是线性阻抗控制了而是“尽可能快地追赶”的非线性模式。所以排查异常曲线时一定要把data.ctrl画出来看是否长时间饱和。关节限位也是一个隐蔽问题。如果机械臂在瞬态响应中触发joint_range限位MuJoCo 会施加很强的冲击约束曲线就会变得很怪。我把range设置得很宽就是为了让实验过程尽量不触发限位把注意力放在控制律本身。5.3 从两连杆迁移到六轴机械臂把两连杆的代码迁移到 UR5e 或 Panda 这类六轴机械臂上控制律部分几乎不用改只是q_des、Kp、Kd都变成 6 维向量data.qfrc_bias也取前 6 个元素即可。MuJoCo 里有相关的 URDF 解析工具可以直接把 URDF 转成model不需要手写 XML。但有两件事必须重做。第一Kp、Kd不能直接沿用两连杆的数值要按六个关节的有效惯量重新标定否则肘部关节可能因为有效惯量小而过冲肩部关节可能因为有效惯量大而响应迟钝。第二真实机械臂的关节摩擦、减速器背隙、通信延迟在仿真里都不存在仿真里调好的参数迁移到真机上通常要适当降低增益。我在 MuJoCo 里确认的是控制律的逻辑和稳定性边界而不是“照搬参数就能在真机上跑”。5.4 关于代码版本和 viewerMuJoCo 的 Python API 在 2.x 和 3.x 之间变化很大。老博客里常见的mujoco_py包已经不再推荐现在官方是pip install mujoco直接import mujoco。代码里用到的mujoco.MjModel.from_xml_path、mujoco.mj_forward、mujoco.mj_step在 3.x 下都没问题。如果你用的是 2.x 或更老的接口照样要按自己的版本调整。实时可视化可以用mujoco.viewer.launch_passive但我在服务器上跑实验时经常没有显示设备所以完整代码以数据记录和绘图为主。想看动画的话在循环里加上viewer.sync()即可。6. 完整可运行代码XML 与 Python 脚本6.1 planar_arm.xml 完整代码把下面内容保存为planar_arm.xml和 Python 脚本放在同一个目录下。mujoco modelplanar_arm_2dof compiler angleradian/ option timestep0.002 integratorEuler gravity0 0 -9.81/ default joint axis0 1 0 damping0.05 armature0.01 limitedtrue range-3.0 3.0/ geom typecapsule size0.03 density1000/ /default worldbody light pos0 -2 3/ camera nameview pos0.2 -1.2 0.4 xyaxes0 1 0 0 0 1 modefixed/ geom namefloor typeplane pos0 0 -0.5 size1 1 0.01 rgba0.8 0.8 0.8 1/ body namelink0 pos0 0 0.15 joint nameshoulder pos0 0 0/ geom fromto0 0 0 0.25 0 0/ body namelink1 pos0.25 0 0 joint nameelbow pos0 0 0/ geom fromto0 0 0 0.25 0 0/ body nametip pos0.25 0 0 geom nametip_sphere typesphere size0.04 rgba1 0 0 0.8/ /body /body /body /worldbody actuator motor nameshoulder_motor jointshoulder ctrlrange-10 10/ motor nameelbow_motor jointelbow ctrlrange-5 5/ /actuator /mujoco6.2 impedance_control.py 完整代码import numpy as np import mujoco import matplotlib.pyplot as plt # 阻抗控制参数 Kp np.array([80.0, 50.0]) Kd np.array([15.0, 10.0]) q_des np.array([0.6, -1.0]) # 外部扰动参数 external_force_after 1.0 F_ext np.array([0.0, 0.0, -15.0]) model mujoco.MjModel.from_xml_path(planar_arm.xml) data mujoco.MjData(model) body_tip mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, tip) mujoco.mj_resetData(model, data) data.qpos[:2] [0.0, 0.0] data.qvel[:2] 0.0 mujoco.mj_forward(model, data) t_list [] q_list [] tau_list [] q_des_list [] for i in range(1500): t data.time # 清零外部广义力 data.qfrc_applied[:] 0.0 # 1 秒后施加世界系下的末端下压力 if t external_force_after: point data.xpos[body_tip].copy() torque np.zeros(3) mujoco.mj_applyFT( model, data, F_ext, torque, point, body_tip, data.qfrc_applied ) # 更新当前状态的动力学偏置 mujoco.mj_forward(model, data) q data.qpos[:2].copy() qd data.qvel[:2].copy() # 关节空间阻抗控制律 tau Kp * (q_des - q) - Kd * qd data.qfrc_bias[:2] data.ctrl[:2] tau mujoco.mj_step(model, data) if i % 2 0: t_list.append(t) q_list.append(q.copy()) tau_list.append(tau.copy()) q_des_list.append(q_des.copy()) t_arr np.array(t_list) q_arr np.array(q_list) tau_arr np.array(tau_list) q_des_arr np.array(q_des_list) fig, axes plt.subplots(2, 1, figsize(9, 6), sharexTrue) axes[0].plot(t_arr, q_arr[:, 0], labelshoulder q) axes[0].plot(t_arr, q_arr[:, 1], labelelbow q) axes[0].plot(t_arr, q_des_arr[:, 0], --, labelshoulder q_des) axes[0].plot(t_arr, q_des_arr[:, 1], --, labelelbow q_des) axes[0].set_ylabel(joint angle (rad)) axes[0].legend() axes[1].plot(t_arr, tau_arr[:, 0], labelshoulder tau) axes[1].plot(t_arr, tau_arr[:, 1], labelelbow tau) axes[1].set_ylabel(torque (Nm)) axes[1].set_xlabel(time (s)) axes[1].legend() plt.tight_layout() plt.savefig(impedance_result.png, dpi150) print(Saved impedance_result.png)6.3 运行方式与预期输出运行前安装依赖pip install mujoco numpy matplotlib然后在命令行执行python impedance_control.py如果一切正常目录下会生成一张impedance_result.png。图中可以看到机械臂从初始状态快速收敛到期望关节角在 t1s 之后受到竖直向下的末端外力作用肩关节和肘关节角度都出现了稳态偏移同时控制力矩明显增大。这个“增大”的力矩就是阻抗控制为了让机械臂抵抗外力而额外输出的恢复力矩与Kp成正相关。如果你希望实时看到机械臂动画可以导入mujoco.viewer在mj_step之后调用viewer.sync()并保证模型和循环运行在同一进程内。我自己后来把这套控制律迁移到了六轴 UR5e 模型上控制律没改只换了模型文件和高纬度参数。MuJoCo 里可以放心地把参数范围拉得很开观察每一种取值下的动态表现这是真机调试给不了的自由度。但真机和仿真最大的差别是摩擦和通信延迟所以仿真里调好的参数只是起点不是终点。建议你从两连杆开始把每个参数变化带来的现象都跑一遍再去碰复杂模型。这样对阻抗控制的理解会扎实很多。