关节空间阻抗控制实战：基于MuJoCo的UR5e柔顺控制仿真

1. 为什么做关节空间阻抗控制

1.1 位控和力控之间的“第三种选择”

机械臂控制大致分三派：位置控制、力/力矩控制、阻抗控制。位置控制在自由空间里非常稳定，但一旦碰到环境——哪怕只是擦到一下桌面、碰上某个工装边角，就可能让关节电机瞬间出现过流报警，严重时还能把减速器憋坏。力/力矩控制倒是不怕接触，但它需要精确的力传感器标定、准确的动力学模型，甚至要在线辨识摩擦项和负载变化，做不好就抖成筛子，工业现场没人敢拿它跑产线。

阻抗控制（Impedance Control）刚好站在中间。它不直接控制接触力，而是把机械臂的关节或末端“装扮”成一个质量-弹簧-阻尼系统：期望位置偏差越大，回拉力矩越大，但过程是柔顺的，不会硬碰硬。这个特性让它成为柔顺装配、人机协作、打磨抛光、医疗机器人以及强化学习仿真环境里非常常见的选择。尤其在MuJoCo物理引擎里，因为有高频、稳定的接触求解，阻抗控制的复现效果非常接近理论模型，非常适合做算法验证。

1.2 关节空间 vs 笛卡尔空间：先分清坐标系再动手

阻抗控制又分两种实现空间：关节空间（Joint Space）和任务空间/笛卡尔空间（Task Space）。本项目的重点是关节空间，原因很直白：它是入门阻抗控制的最佳路径，也是很多机器人控制算法的地基。

关节空间阻抗控制的期望轨迹就是每个关节的目标角度，控制律可以写成非常对称的形式：

tau = Kp(qd - q) - Kd(dq) + ff

这里 qd 是期望关节角，q 是当前关节角，dq 是关节速度，Kp 是关节刚度矩阵，Kd 是关节阻尼矩阵，ff 是前馈力矩（通常包含重力补偿项）。

如果换成笛卡尔空间，事情就没这么简单了。你需要在任务空间定义期望刚度 Kx，再通过雅可比矩阵映射到关节空间：

tau = J^T Kx (xd - x) - J^T Dx (dx) + ff

这个公式看起来也不复杂，但一旦机械臂靠近奇异位形，J 矩阵的病态会导致映射后的等效刚度爆炸，控制力矩瞬间超限，机械臂当场“抽搐”。MuJoCo里的UR5e之类的6轴臂，工作空间边缘的奇异问题非常明显。

所以我给读者的建议是：不要跳过关节空间直接上笛卡尔空间。把关节空间的物理含义吃透了，再去碰任务空间的相关内容，你会发现后面的路顺很多。这也是这篇博文只讲关节空间的原因——先立住基本功。

2. 仿真环境搭建与机械臂模型准备

2.1 MuJoCo安装与基本运行姿势

先说安装。MuJoCo从3.x版本开始，官方直接通过Python库分发，不需要再像旧版本那样去官网注册、下载、配置MJKEY路径，省掉了大量麻烦。国内用户注意，PyPI源如果慢，用国内镜像安装即可，模型文件都是随wheel包内置的，不会缺东西。

我在Windows 11和Ubuntu 22.04上都验证过同一套代码，没有平台差异。唯一要注意的是Python版本，建议3.9以上，太老的版本在渲染模块上偶尔会有兼容性告警，影响不大但没必要给自己添堵。

pip install mujoco

装上之后，跑一个最简单的测试：加载MuJoCo自带的UR5e模型，仿真一步，打印关节角度。

import mujoco # 获取官方内置的UR5e模型路径，这个API在mujoco 3.x里非常方便 xml_path = mujoco.models.get_model_path("ur5e.xml") model = mujoco.MjModel.from_xml_path(xml_path) data = mujoco.MjData(model) # 仿真一步 mujoco.mj_step(model, data) print(data.time)

能打印出 0.002，说明物理引擎已经跑起来了。这里有个容易让新手懵的点：MuJoCo默认仿真步长是2ms，也就是控制频率500Hz。这个频率对阻抗控制来说非常关键，后面我专门讲。

2.2 用内置UR5e模型：检查关节和执行器

自带UR5e模型的好处是不需要自己写XML，关节名称、质量参数、碰撞体、视觉模型都是现成的。加载之后，第一件事是确认关节名称和执行器名称，我踩过“按索引猜关节顺序”的坑——某个版本里索引顺序和你想象的不一致，结果控制命令全写到了错误的关节上，机械臂直接扭成奇怪的姿势。

在项目里，我建议统一用名称映射来取索引：

joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] joint_ids = [model.joint(name).id for name in joint_names] nq = len(joint_ids) # UR5e模型的执行器名称和关节名称一致，均为motor类型 actuator_ids = [model.actuator(name).id for name in joint_names]

UR5e有6个旋转关节，每个都配了一个motor类型的执行器。这个名字容易让人误以为“motor控制器就是速度模式”，实际上在MuJoCo里，motor执行器的ctrl输入直接是广义力/力矩。也就是说，你给data.ctrl赋什么值，关节就收到多少力矩，这是实现阻抗控制的基础。

2.3 为什么不能用position执行器做阻抗控制

这是个非常常见的坑。MuJoCo内置了position类型的执行器，它本身自带一个强PD伺服环，你给它设置ctrl为目标角度，引擎内部的隐式PID会帮你把关节拉过去。从使用体验上看，好像“设一个目标角度”就够了，很多人就顺手拿它当阻抗控制用。

但这里有一个本质问题：如果你用position执行器，MuJoCo内部已经有一个强增益的PD环在工作，你外部再写一个阻抗控制器，相当于两套PD串联在同一个关节上。内环的刚度和阻尼无法从外部精确控制，外环参数就很难调，整体行为既不像纯位置控制，也不像阻抗控制。更麻烦的是，内环的高增益会掩盖你外环的柔顺特性，导致机械臂末端碰到障碍物时依然“很硬”，柔顺效果出不来。

所以做阻抗控制，一定要用motor执行器，让引擎只负责接收力矩，所有柔顺特性都由你写的控制律来定义。这也是整个项目最核心的架构前提。

3. 控制律原理与参数整定经验

3.1 PD + 重力补偿：关节空间阻抗的基本形式

关节空间阻抗控制的最简形式就是PD控制加前馈补偿：

tau = Kp(qd - q) - Kd(dq) + tau_ff

这里的核心逻辑是：把关节角偏差看作弹簧的伸长量，偏差越大，产生的“反抗力矩”越大；关节速度则对应阻尼力，阻尼项用来消耗能量，防止系统震荡。

前馈项 tau_ff 在MuJoCo里可以直接取data.qfrc_bias。它是广义偏置力，包含重力、科氏力和离心力的贡献。在机械臂低速运动场景下，科氏力和离心力占比很小，但重力项绝不能省——UR5e的第2、3关节在水平伸展时，重力矩能到几十牛米。如果不做重力补偿，单靠PD的力矩上限去扛，静态误差会非常大，控制效果看起来就像是“抬不动胳膊”。

import numpy as np # 期望关节位置：让机械臂摆出一个比较舒展的位姿 q_des = np.array([0.0, -0.8, 1.2, -1.5, -0.5, 0.0]) # 刚度矩阵：越靠近底座，虚拟弹簧刚度越大 Kp = np.diag([200.0, 200.0, 100.0, 50.0, 20.0, 10.0]) # 阻尼矩阵：根据临界阻尼的近似关系来设定，后面细说 Kd = np.diag([40.0, 40.0, 20.0, 10.0, 5.0, 3.0]) # 读取当前关节位置和速度 q = data.qpos[joint_ids] dq = data.qvel[joint_ids] # 计算PD项 + 重力/动力学前馈 tau = Kp @ (q_des - q) - Kd @ dq tau_ff = data.qfrc_bias[joint_ids] # 写入控制命令 data.ctrl[actuator_ids] = tau + tau_ff

这个代码片段就是整套阻抗控制的核心。后面所有的调试、测试，都是在反复调整Kp、Kd和仿真工况。

3.2 刚度与阻尼的整定经验

阻抗控制最讲究的就是 Kp 和 Kd 的匹配。Kp 太大，机械臂表现得很“硬”，好处是跟踪误差小，坏处是接触时冲击力大；Kp 太小，机械臂软绵绵的，重力和外力稍微一变就偏离目标。

阻尼 Kd 的作用是防止震荡。只用 Kp 不加 Kd 的系统，本质上是一个无阻尼弹簧振子，在仿真里会因为数值离散和时间延迟持续震荡，看起来就像机械臂在“帕金森”。

整定的一个近似方法是用“临界阻尼”公式：

Kd = 2 * sqrt(Kp * I_eff)

其中 I_eff 是关节的等效惯量，理论值需要通过动力学辨识得到。在MuJoCo仿真里，我们可以直接从data.qfrc_bias对速度的敏感性来估，但对大多数场景并不需要算那么精确。工程做法是：先给一个 Kp，再用 Kd = 2 * sqrt(Kp) 作为初值（假设单位惯量），然后观察响应曲线微调。

我给一组实测可用的参数，UR5e在MuJoCo里表现很稳：

注意，这组参数是针对“展示柔顺性”调的。如果你做的是高精度轨迹跟踪，刚度可以翻倍甚至更高，但那就不是阻抗控制的主场了，直接用位置模式更合适。

3.3 仿真步长与控制频率的关系

MuJoCo物理引擎的默认dt=0.002s，也就是 500Hz。对阻抗控制来说，控制频率越高，控制器的等效相位滞后越小，系统越稳定。

如果你把控制频率降到 50Hz（例如只在每10个物理步里更新一次控制律），PD控制器的离散化延迟会显著增加，等效于给系统注入一个负阻尼。实测下来，UR5e的肩部关节会开始出现可感知的高频抖动，严重时直接发散。

所以我在代码里坚持每个物理步都调用一次控制律，也就是while循环里每个mujoco.mj_step之前都重算ctrl。如果你后面要做更复杂的多线程控制、外部通信、视觉推理，优先级一定是保证控制频率，渲染可以降帧，控制绝不能降频。

4. 完整代码实现与调试记录

4.1 代码结构：一个脚本跑通整条链路

这个项目不搞复杂架构，单文件impedance_control.py就能跑通。核心模块只有三块：

• 初始化：加载模型、获取关节/执行器索引、设定初始位形。
• 控制循环：每个物理步计算阻抗控制力矩、写入data.ctrl、执行mujoco.mj_step。
• 可视化与测试场景：用mujoco.viewer实时观察，并自动注入外力脉冲/目标点阶跃来验证柔顺特性。

整个代码大约150行，不需要ROS，不需要装额外库，只要mujoco和numpy。

4.2 核心控制循环：逐步拆解

下面这段是控制循环的正片，我把每一步都写了注释，方便直接对照修改。

import numpy as np import mujoco import mujoco.viewer xml_path = mujoco.models.get_model_path("ur5e.xml") model = mujoco.MjModel.from_xml_path(xml_path) data = mujoco.MjData(model) joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] joint_ids = [model.joint(name).id for name in joint_names] actuator_ids = [model.actuator(name).id for name in joint_names] nq = len(joint_ids) # 目标位姿：可以改成任意合法关节角 q_des = np.array([0.0, -0.8, 1.2, -1.5, -0.5, 0.0]) # 刚度与阻尼矩阵 Kp = np.diag([200.0, 200.0, 100.0, 50.0, 20.0, 10.0]) Kd = np.diag([40.0, 40.0, 20.0, 10.0, 5.0, 3.0]) # 先把机械臂摆到初始位置，减小启动冲击 for i in range(100): q_init = np.array([0.0, -1.0, 1.5, -1.2, -0.5, 0.0]) data.ctrl[actuator_ids] = 100.0 * (q_init - data.qpos[joint_ids]) - 10.0 * data.qvel[joint_ids] mujoco.mj_step(model, data) with mujoco.viewer.launch_passive(model, data) as viewer: step = 0 while viewer.is_running(): # 1. 读取当前状态 q = data.qpos[joint_ids].copy() dq = data.qvel[joint_ids].copy() # 2. 阻抗控制律：PD + 动力学前馈 tau = Kp @ (q_des - q) - Kd @ dq tau_ff = data.qfrc_bias[joint_ids] data.ctrl[actuator_ids] = tau + tau_ff # 3. 在第2000步时，给末端施加一个向下的外力脉冲，持续200步 if step == 2000: wrist_body_id = model.body("wrist_3_link").id data.xfrc_applied[wrist_body_id, :3] = [0.0, 0.0, -30.0] if step == 2200: wrist_body_id = model.body("wrist_3_link").id data.xfrc_applied[wrist_body_id, :3] = [0.0, 0.0, 0.0] # 4. 推进物理仿真 mujoco.mj_step(model, data) step += 1 # 5. 同步渲染 viewer.sync()

这段代码有两个值得留意的设计。

第一个是启动阶段的“预调整”（100步的初始化循环）。如果直接从默认位置（通常是竖直状态）瞬间切换到目标位置附近，PD控制器产生的大偏差力矩会让整条机械臂“弹”一下。先用一个较弱的PD把它拉到一个中间位姿，再启动阻抗控制，曲线会干净很多。

第二个是data.xfrc_applied的用法。它是对指定body施加一个外力和力矩，单位是N和N·m，这个力是施加在笛卡尔空间上的。用在腕部时，很直观地模拟了“末端撞到东西”或者“末端挂了一个重物”的工况。第2000步施加30N向下力，持续0.4秒后撤掉，刚好能看出控制器如何在外力作用下“让位”、又在撤力后“复位”——这就是柔顺性最直观的验证。

4.3 观察现象：阶跃响应与柔顺让位

跑起来之后，你会看到几个值得注意的现象：

• 机械臂从初始位置快速接近目标位姿，动作干净，没有明显超调。
• 2020步左右（外力刚开始作用时），腕部被压低了一段距离，目标位置与当前关节角之间出现了偏差。这不是“控制失败”，而是阻抗控制的正常行为——它天生允许位置偏差来换取接触力的限制。
• 撤去外力后，机械臂又缓慢回到目标位姿，表现出类似弹簧回中的特性。

如果此时你把data.qpos[joint_ids]实时打出来，可以更定量地看到每个关节的偏差。末端受30N力时，肩部和肘部的偏差大约在0.02-0.08rad之间，腕部关节因为刚度更低，偏差会更明显。

4.4 调参踩过的坑：四条实战记录

第一坑：不加阻尼必炸。我第一次跑的时候直接把Kd设成零，想看看纯刚度下什么表现，结果肩部关节在2秒后开始高频震荡，振幅不断放大直到物理引擎报NaN。这不是MuJoCo的问题，是无阻尼PD系统在离散时间下的经典不稳定，真实机械臂上同样会发生，只不过有摩擦和驱动响应延迟才没那么快爆。

第二坑：控制频率降低到50Hz导致“抽筋”。我试过只在每10个物理步更新一次控制律来模拟低频率控制器，结果在低刚度关节（腕部）出现肉眼可见的抖动。如果你打算把控制器放到外部进程（比如通过socket通信），一定要保证控制频率不要低于300Hz，否则阻抗参数得整体下调，系统会显得迟钝。

第三坑：用位置执行器做阻抗，柔顺效果出不来。我早期偷懒直接用MuJoCo的position actuator，通过设置目标角度来实现“所谓阻抗”，结果机械臂碰到障碍物时刚硬无比，完全体现不出柔顺。后来改成motor执行器、把阻抗力矩写进ctrl之后，效果立竿见影。

第四坑：关节索引搞错，命令写错关节。从XML模型里获取索引一定用model.joint(name).id，不要靠 manual 数。UR5e还好，关节顺序和XML顺序一致，但有些模型文件里执行器顺序和关节顺序不是对应的，按索引写很容易把命令送到错误执行器上，还不好排查。

5. 常见问题与排查速查表

MuJoCo和阻抗控制组合在一起，新手遇到的大多数问题其实是两类：环境类问题（装上跑不起来）和控制类问题（跑起来但行为不对）。这里我把实操中高频的问题整理成一个速查表。

还有一个我反复提醒自己的点：仿真里的摩擦模型和真实机械臂完全不同。MuJoCo默认关节摩擦系数很小，尤其是低速区，所以你在仿真里调好的阻抗参数，迁移到真实机械臂上一定要重新整定。通常真实机械臂需要更大的Kd来克服驱动器响应延迟和摩擦非线性。

写在最后的一个小技巧

如果你只是想在MuJoCo里快速验证“某个任务目标用关节空间阻抗控制做不出什么问题”，建议直接把外力脉冲测试换成“目标点阶跃”。比如每2秒随机切换一次q_des，观察机械臂对阶跃目标的追踪响应。这个测试比外力测试更干净，因为不涉及外力大小标定，也更容易看出每个关节的刚度/阻尼设置是否合理。

另外一个非常实用的功能是MuJoCo viewer的回放。跑完整个仿真后，你可以用viewer打开记录文件重新播放刚才的过程，特别适合评审或者复盘：明明刚才机械臂在抖动，等你想截图的时候发现它又稳了，回放能帮你把之前那段抖动完完整整找出来。用法很简单，viewer界面里找到录制控制按钮，操作路径并不深，自己摸索一下就行。

因为做这个项目，我最大的体会是：阻抗控制本身不复杂，复杂的是把参数调到“既够稳、又有柔顺感”的那个甜点。仿真环境给了我们一个无限试错的舞台，这是做物理样机验证前最好的练兵场。后面如果你要升级到笛卡尔空间阻抗，原理也是在这个关节空间版本上扩展——先把这一版跑通，后面的路就好走了。