Q1 MuJoCo 模型里的 49 项传感器定义与 60 维数据：一次关节和 IMU 输出核验

我在前面的 Q1 MuJoCo 实验里已经看到过 nq=29、nv=28 和 nu=22。这次读模型 XML 时，又遇到一组更容易混淆的数字：nsensor=49，但 nsensordata=60。

乍一看，49 个传感器为什么不是 49 个数？我没有直接把这两个字段写进文章就结束，而是顺着 q1_v3_mc.xml 的 <sensor> 区域查了一遍：每个条目占几维，数据在 mjData.sensordata 的哪个地址，22 个关节位置和速度是否真的对应 qpos、qvel。最后又专门测了一个读取时机问题：mj_step() 返回后立即读传感器，和再调用一次 mj_forward() 后读，结果是不是一样。

这篇记录的是固定版 Q1 模型在 MuJoCo 里的定义和仿真输出。我没有实体 Q1，下面的数字也不代表真机传感器精度。

先把实验对象固定下来

这次使用的是启元机器人公开的 Q1 V3.0 q1-urdf 仓库，提交固定为 562b764a18613d459140ed9b255fa30434e346ef，模型文件为 xml/q1_v3_mc.xml。我还记录了这个 XML 的 SHA-256：

d9442f0de18071b100d927e310bc58b04e7be305fea8f126893b2700e8c4bd3b

仓库提交标题和模型文件名都指向 Q1 V3.0，不过同一提交里的 package.xml 仍保留 Q1-V2.0-URDF-0601 这个旧包名。我按提交、模型文件和哈希固定实验对象，没有用包清单里的旧名称推断模型版本。

实验在我的 Apple Silicon Mac 上完成，版本如下：

项目	实际版本
macOS	15.4.1，ARM64
Python	3.10.17
MuJoCo	3.16.0
NumPy	2.2.6
Matplotlib	3.10.9
模型步长	0.001 s

运行脚本没有给 22 个执行器发送输入，只把 data.ctrl 初始化为零，然后推进 1000 步，也就是 1 秒仿真时间。模型仍会在重力和接触动力学下变化，所以后面出现关节角度变化时，不能把它写成某条控制指令的效果。

49 个条目为什么是 60 维

MuJoCo 会把所有传感器的输出连续放进 mjData.sensordata，数组长度由 mjModel.nsensordata 给出。传感器条目数和输出维度本来就是两个概念：一个姿态四元数传感器算一个条目，却需要四个数；一个三轴陀螺仪也是一个条目，却需要三个数。

这个模型的 49 项传感器分成三组：机身与 IMU 相关传感器 5 项、关节位置传感器 22 项、关节速度传感器 22 项。5 项机身传感器占 4+3+3+3+3=16 维，两个关节组各占 22 维，合计正好是 60。

这里有两个不同的索引：sensor_id 表示 XML 里的第几个传感器，sensor_adr 才是它在 sensordata 中的起始位置，sensor_dim 表示从这个位置连续读取多少个数。比如姿态传感器只有一个条目，却从地址 0 开始占 4 个槽位；因此下一个三维 gyro 从地址 4 开始。消费数据时要用地址和维度切片，不能把"第几个传感器"当成数组下标。

图 1：根据固定模型的 nsensor、nsensordata 与传感器分组生成的布局图。

排布图能核对数量，但还不能确认名字、类型和模型参数来自哪一段定义。传感器在 MuJoCo 模型中的序号与 sensordata 地址也不是一回事：四维四元数已经占去前四个槽位，所以接下来我把 XML 原文和编译后地址表分开核对。

我先回到固定提交里的 XML 原文，而不是从最后生成的 CSV 反推传感器名称。<sensor> 区段先写 imu_0 相关的五项，再写 22 个 jointpos，最后写 22 个 jointvel；其中 gyro 和 accelerometer 明确写了 noise="0.001"，关节传感器写的是零噪声。下面这张卡片只展开了原文件中的关键行，省略号代表同组剩余条目。

图 2：根据启元公开 Q1 V3.0 模型 XML 排版，保留原文件行号、字段和值。这是官方源文件的摘录整理图，不是启元发布的宣传配图。

图中的两个合计分别回答了两个问题：传感器列表有 49 行，真正写入 sensordata 的连续槽位有 60 个。布局也能看出顺序不是我在脚本里重新规定的，而是跟 XML 中 <sensor> 的出现顺序一致：前 16 个槽位是机身/IMU 输出，之后是 22 个关节位置，最后是 22 个关节速度。

我用脚本把每个条目的名称、类型、地址、维度、挂载对象和 XML 中的 noise 属性写进 sensor_inventory.csv。这一步很有必要，因为只打印 nsensor 和 nsensordata，后面接 SDK 或写消息转换时仍然不知道某一维到底是什么。

图 3：条目顺序按固定版 XML，地址和维度从 MuJoCo 3.16.0 编译后的 MjModel 读取。它把五项机身/IMU 输出和两个 22 项关节组放在一张表里；这是本次核验整理图，不是启元官方配图。

22 个关节，分别对应哪里

Q1 模型有一个 floating base。它在 qpos 中占 7 个槽位，在 qvel 中占 6 个槽位，因此模型整体是 nq=29、nv=28：

qpos:  floating base 7 维 + 22 个一维 hinge joint = 29
qvel:  floating base 6 维 + 22 个一维 hinge joint = 28

这也是为什么不能拿 qpos 和 qvel 的同一个下标硬套。第一个关节 left_hip_pitch_joint 的位置在 qpos[7]，速度在 qvel[6]。传感器端则更规整：位置传感器从 sensordata[16] 开始连续排到 sensordata[37]，速度传感器从 sensordata[38] 排到 sensordata[59]。

图 4：从编译后的 MjModel 读取的 22 个关节传感器与 qpos/qvel 索引映射。

这张索引表是从编译后的 MjModel 读出来的，不是根据关节名称手写一张静态表。脚本对每个位置传感器取 sensor_objid，再用 jnt_qposadr 找到对应的 qpos 下标；速度传感器则用 jnt_dofadr 找到 qvel 下标。当前固定模型的 22 个关节都是一维 hinge joint，因此每个传感器都占一个槽位。浮动基座的自由关节则不同：位置由三维平移和四元数表示，占 7 个 qpos；速度由线速度和角速度组成，占 6 个 qvel。所以关节位置传感器首地址 16，与第一个关节的 qpos[7] 是两套互不相同的索引。

MuJoCo 的 XML 参考文档对 jointpos 和 jointvel 的说明也很直接：这两类传感器分别把关节位置和速度输出到 sensordata。我还是做了逐项数值核对，而不是只引用文档。1000 步结束后，22 个位置读数和 22 个速度读数都能与对应状态数组对上，前提是先把当前派生量刷新到同一个状态。脚本同时记录传感器序号、数据地址和关节状态下标；这样模型结构一旦变化，就能看出错在传感器排列还是关节映射，而不只是发现"两个数对不上"。

读数不一致，问题出在调用时机

真正让我停下来查的是下面这个现象：如果每一步只调用 mj_step()，然后马上拿 data.sensordata 去和刚更新的 data.qpos、data.qvel 比较，结果并不是零。

我让两份 MjData 使用相同的模型和全零控制输入。一份每步执行：

mujoco.mj_step(model, data)
# 立即读取 data.sensordata、data.qpos、data.qvel

另一份每步执行：

mujoco.mj_step(model, data)
mujoco.mj_forward(model, data)
# 再读取 data.sensordata、data.qpos、data.qvel

在 1000 步、1 秒仿真中，第一种读取方式的 22 个关节最大位置差是 0.018420513 rad，最大速度差是 1.886024685 rad/s。位置误差峰值出现在 left_shoulder_yaw_joint 的第 749 步附近，速度误差峰值出现在第 750 步。这个数字不是模型精度，也不是传感器噪声指标，只是"步进后直接读取"与"当前状态数组"之间的调用时机差异。

图 5：两种读取时机下，22 个关节位置和速度传感器的误差曲线。

追加 mj_forward() 后，22 个位置传感器和 22 个速度传感器的最大绝对误差都变成了 0。两份 MjData 的动力学轨迹也没有被这次刷新改变：整个 1000 步过程中，两个读取路径的最大 qpos 差为 0，最大 qvel 差为 0。

在这个脚本里，mj_forward() 用当前 qpos、qvel 和控制输入重新计算派生量及传感器输出，不推进仿真时间。两条路径的 qpos/qvel 完全相同，说明差异出在读取阶段的刷新时机。代码如果在 mj_step() 后立即打包 sensordata，需要明确消费者拿到的是哪一时刻的状态；数组长度吻合并不能证明采样时刻一致。

5 项机身与 IMU 输出分别是什么

XML 的前 5 项传感器都挂在 imu_0 site 或以它为参考：

名称	类型	维度	单位与本次核对的含义
body-orientation	framequat	4	无量纲，imu_0 相对世界坐标系的姿态四元数（w、x、y、z）
body-angular-velocity	gyro	3	rad/s，site 局部坐标的角速度
body-linear-pos	framepos	3	m，site 在全局坐标中的位置
body-linear-vel	velocimeter	3	m/s，site 局部坐标的线速度
body-linear-acceleration	accelerometer	3	m/s²，site 局部坐标的线加速度，包含重力

MuJoCo 文档对 gyro、velocimeter 和 accelerometer 都强调了 site 局部坐标；framepos 和 framequat 则按空间 frame 返回全局位置或姿态。这个坐标约定不能靠传感器名字猜，所以我又调用 mj_objectVelocity()、mj_objectAcceleration() 和 site 的位置/旋转矩阵，把它们与 sensordata 做了交叉核验。

这两种坐标表达在后处理时不能直接混在一列里。位置和姿态描述 imu_0 相对世界的位置与朝向；gyro、velocimeter、accelerometer 是沿 site 局部轴表达的向量。机器人倾斜后，局部 z 轴相对世界坐标已经转动，若要把不同来源的向量放进同一坐标系，需要依据姿态旋转矩阵转换后再比较。传感器名字里都带有 body-，不代表它们使用同一套坐标轴。表格里的单位也按传感器类型记录：四元数无量纲，角速度用 rad/s，位置用 m，线速度用 m/s，加速度用 m/s²。如果把这些量发布为 ROS 2 消息，坐标系和时间戳也应一起定义，字段名和单位仍不足以说明向量含义。

结果是：body-linear-pos 与 data.site_xpos[imu_0] 完全一致，角速度、线速度、加速度也与 imu_0 的局部参考量一致；四元数只留下约 1.33e-15 的浮点舍入误差。仿真初始时加速度读到约 9.95 m/s² 的 z 分量，是因为 MuJoCo 的加速度传感器定义包含重力，这个数不能直接拿来当成"机器人受到了某个外部冲击"。

图 6：1 秒无控制输入仿真中，执行 mj_forward() 后采样的 5 个机身/IMU 传感器输出。

曲线是 1 秒无控制输入仿真中，执行 mj_forward() 后的 sensordata 采样。值得单独解释的是，约 0.7 s 时加速度曲线出现明显尖峰：CSV 中 site 局部 x 分量为 59.94 m/s²，z 分量为 -96.69 m/s²；同一时刻 imu_0 高度约 0.268 m，姿态四元数的 w/y 分量约为 0.489/-0.872。这时模型已明显倾斜并发生位移，和前面的全零控制、受重力运行是一组条件。我把这个尖峰视为当前模型动态过程中的瞬态，不能从这段仿真里单独归因到传感器、接触参数或某一种硬件效应。

因此这张曲线能用于核对传感器地址、坐标系和状态刷新时机，却不适合作为静止 IMU 的基线。XML 里 gyro 和 accelerometer 的 noise="0.001" 是模型配置字段；这次运行没有做噪声统计，因此不把它解释成真实 IMU 的噪声、偏置或安装误差测量值。

把这次核验放回开发流程

我把复现实验整理成了一个独立的 AtomGit 项目：q1-mujoco-sensor-audit。运行后会得到：

• sensor_inventory.csv：49 个传感器的名称、类型、地址、维度和挂载对象；
• joint_sensor_comparison.csv：采样时刻、传感器读数、qpos/qvel、两种读取时机下的误差；
• body_sensor_timeseries.csv：5 个机身/IMU 传感器的时间序列；
• summary.json：模型提交、XML 哈希、版本、维度、峰值误差和有限值检查；
• figures/：脚本生成的四张实验图表。文章另有两张依据固定版 XML 与编译模型整理的说明图。

在自己的模型目录和项目目录中可以这样运行：

python3.10 -m venv .venv
source .venv/bin/activate
python -m pip install -r requirements-lock.txt

python scripts/audit_q1_sensors.py \
  --model ~/q1-mujoco/official-q1-urdf/xml/q1_v3_mc.xml \
  --steps 1000 \
  --sample-every 10 \
  --output-dir results

脚本会先检查模型提交和 XML 哈希，再检查 49/60 维度；模型换成其他版本、传感器数量变化或关节不再是当前的 22 个一维 hinge joint 时，会停止并提示复核。

我还会把模型提交和 XML 哈希与 CSV、summary.json 放在同一份实验结果中。即使新模型仍然有 49 项传感器，XML 的排列或某个传感器维度改变，也可能让后续地址发生偏移；单看总数无法发现这种变化。版本升级后应重新导出映射表，再更新消费端索引，不能把这一版的 sensordata[16:60] 规则当成跨版本约定。

这次实验的结论

固定提交的 Q1 V3.0 XML 定义了 49 项传感器，编译后占用 60 个连续数据槽位；用 mj_forward() 刷新后，22 个关节位置与 22 个关节速度输出能分别对上相应的 qpos/qvel。直接在 mj_step() 后读取出现的差异，则来自本次代码的状态与传感器刷新顺序。

后续把数据送进 ROS 2 消息或日志时，我会在采样函数里明确派生量的刷新时机。真机 IMU 的噪声、偏置和安装误差要用设备数据另行核对。

参考资料

• 启元机器人 Q1 V3.0 q1-urdf 仓库（固定提交）。
• MuJoCo XML Reference：sensor。
• MuJoCo Python bindings。
• MuJoCo Computation。
  Q1 模型、SDK 与开发资料可从启元机器人开发者社区获取。