启元 Q1 官方模型在 MuJoCo 里调 PD 参数 分享两个坑摘要对机器人我的感觉停留在上春晚能跳舞能格斗但听业内人士说很多都是用遥控器在背后操控的。后来才知道就算是遥控遥控器也只管说“抬手”手能不能真抬到位、抬的时候抖不抖靠的是底下每个关节自己的控制这篇想看的就是这一层。手上没有 Q1 真机我就从启元开发者社区下了官方的 Q1 模型放进 MuJoCo 里先玩起来。想法很简单把机器人骨盆固定在半空只动左肩一个关节让它按 0.5 Hz 的余弦轨迹上下摆然后换两组 PD 参数Kp15/Kd0.1 和 Kp60/Kd0.4看哪组跟得准、各用多大力矩。实际前后跑了四轮。第一轮 Kd 给到了 41 ms 的控制周期下力矩一直正负来回跳接近四成的采样点顶在 ±22 N·m 的上限上误差反而比小参数还大。第二轮把阻尼降下来肩膀的曲线终于正常了可我的检查脚本报出两侧髋关节越限了 0.539 rad差不多 31°。查下来是我为了固定骨盆删掉 freejoint 之后MuJoCo 默认的父子碰撞过滤对骨盆失效了骨盆一直顶着髋部连杆。把这两对碰撞排除掉以后第三轮才拿到能用的数据Kp60 那组平均误差约 0.87°Kp15 那组约 3.3°两组峰值力矩都只有 1.1 N·m 出头。下面按顺序讲模型怎么改、控制怎么写、结果怎么看、两个坑怎么查最后对着官方 SDK 文档列一下真机上对应的接口。一、实验要做的事目标是看 PD 参数对单关节跟踪精度的影响所以其他变量能固定的都固定了。Q1 是一台双足人形机器人如果让它自由站着左臂一动整个身体都会跟着晃就分不清误差是谁带来的。所以我把骨盆钉在离地 0.62 m 的位置下面截图里机器人悬在空中就是这个原因。图 1骨盆固定后的 Q1 初始姿态。整体流程否是下载官方Q1 模型改造模型固定骨盆、加电机基线测试不控制会怎样PD 对照两组参数全身检查通过正式结果 完整复现图 2实验流程。“检查不通过回去改模型”这一步实际走了两次。二、模型和环境Q1 模型在启元开发者社区资料包页的“仿真”分类里下载Q1 URDF v1.0.0AtomGit 上的 primebot/q1-urdf 也是同一套。包里xml/目录下有三个 MuJoCo 文件我用的是q1_v3.xml另外两个full_range放宽了关节范围mc自带了电机有需要可以直接拿来用。环境Windows 11Python 3.11.13MuJoCo 3.16.0NumPy 2.4.6Matplotlib 3.11.2。三、改造模型q1_v3.xml里没有执行器骨盆也是自由的。我没改官方文件而是在脚本里读进来改完再另存为fixed.xml# 步长 1 ms积分器换成 implicitfastET.SubElement(root,option,timestepstr(DT),integratorimplicitfast)iffixed:# 去掉骨盆的 freejoint固定在 0.62 mpelvis.remove(pelvis.find(freejoint))pelvis.set(pos,0 0 0.62)# 排除骨盆和两侧髋部连杆的碰撞原因见第六节contactET.SubElement(root,contact)forsidein(left,right):ET.SubElement(contact,exclude,body1pelvis,body2f{side}_hip_pitch_link)# 每个关节加一个力矩电机上限直接用官方的 actuatorfrcrangeactuatorsET.SubElement(root,actuator)forjointinroot.findall(.//joint):ET.SubElement(actuators,motor,namejoint.get(name)_motor,jointjoint.get(name),gear1,ctrllimitedtrue,ctrlrangejoint.get(actuatorfrcrange))这样左肩俯仰left_shoulder_pitch_joint的力矩上限就是官方给的 ±22 N·m。要注意这种 motor 是理想的给多少出多少跟真电机差得远。四、PD 控制PD 就是一个弹簧加一个阻尼Kp 管“偏了多少就拉回多少”Kp60 意味着偏 1 rad 给 60 N·mKd 管“转得快就往回压一点”防止冲过头。每个仿真步1 ms走一遍下面这个循环目标角度q_ref算角度差和速度差读实际角度和速度τ Kp·Δq Kd·Δq̇限幅±22 N·mMuJoCo步进 1 ms图 3控制循环。代码# 目标轨迹中心 -0.5 rad幅度 0.35 rad周期 2 sdesired[index]-.5.35*np.cos(2*np.pi*.5*t)velocity[index]-.35*np.pi*np.sin(np.pi*t)# 目标角速度# PD没有加重力补偿rawkp*(desired-data.qpos[qa])kd*(velocity-data.qvel[va])# 按官方上限截断data.ctrl[:]np.clip(raw,model.actuator_ctrlrange[:,0],model.actuator_ctrlrange[:,1])轨迹起点我特意设成 −0.15 rad和左肩初始角度一样不然一开始就有个阶跃误差会被开头那一下拉高。两组参数组KpN·m/radKdN·m·s/radlow150.1high600.4其他关节400.2表 1PD 参数每组跑 10 s。Kp 和 Kd 是一起放大 4 倍的想单独看哪个起作用得再分开跑。五、结果指标lowhighRMSE0.0582 rad3.33°0.0152 rad0.87°最大误差0.0777 rad4.45°0.0187 rad1.07°峰值力矩1.17 N·m1.12 N·m限幅比例00表 2第三轮结果。RMSE 可以理解成整段的平均误差。图 4从上到下依次是目标/实际角度、误差、左肩力矩。high 的误差差不多是 low 的四分之一。中间那张误差图有点意思两条线从头到尾都是负的实际角度一直比目标高一截从来没有越过目标下面的力矩也一直是负的像是始终在顶着什么。我猜是没加重力补偿电机一直在扛手臂的重量还没验证。力矩方面两组峰值都在 1.11.2 N·m离 22 N·m 差得远没有一次被限幅。也就是说这条轨迹对肩关节来说负载很轻两组的差别纯粹在精度上。只凭这一组数据推不出“增益越大越好”。下面第一轮的结果就是反例。六、两个坑run-01high (60, 4)力矩高频振荡39.47% 采样被限幅run-02降低 Kd肩部正常髋关节越限 0.539 radrun-03排除骨盆-髋部碰撞run-04从头重跑确认图 5四轮实验。坑一Kd 给大了第一轮 high 用的是 (60, 4)其他关节也是 (60, 4)。跑出来 high 组的力矩正负快速交替39.47% 的采样点卡在 ±22 N·mRMSE 0.0663 rad比 low 组0.0553还差。保持姿态的场景也一样49.51% 被限幅。图 6第一轮最下面的力矩曲线基本糊成一片。第二轮把 Kd 降到 0.4顺手把其他关节也改成了 (40, 0.2)振荡就没了。坑二髋关节越限第二轮肩部曲线看着没问题但检查脚本报了越限左髋hip_pitch最大到 2.009 rad上限是 1.47 rad超了 0.539 rad右侧超了 0.538 rad。这个脚本会遍历每个采样点、每个关节# 每个点、每个关节算低于下限或高于上限的量violationnp.maximum(model.jnt_range[:,0]-states[:,:model.nq],states[:,:model.nq]-model.jnt_range[:,1])# 超过 0.001 rad 直接报错assertviolation.max().001,fJoint range exceeded in{name}这条 assert 是第二轮之后补的之前脚本只打印数值所以第二轮才能顺利跑完出图。这个问题不用跑仿真就能复现初始姿态下调一次mj_forward看接触数自由骨盆的模型是 0固定骨盆的是 6全在骨盆和两侧hip_pitch_link之间。问题出在固定骨盆这一步。MuJoCo 默认会过滤直接相连的父子 body 之间的碰撞但父 body 是 world 时不过滤。骨盆去掉 freejoint 后焊到了 world 上这条过滤就不再作用于它。MuJoCo 文档碰撞过滤解决方法是只 exclude 这两对其他碰撞照常保留检查项run-02run-03全身最大越限0.539 rad0最大接触数60表 3修复前后。七、对应到真机仿真中真机文档每个关节给 PD 力矩低层话题/aima/hal/joint/command可同时给位置、速度、力矩、Kp、Kd1 ms 一次建议 5001000 Hz低于 100 Hz 可能触发硬件看门狗Windows MuJoCoUbuntu 22.04 ROS2 Humble暂不支持 Windows 开发停程序就行可切阻尼或零力矩模式急停退出前要发一帧阻尼指令表 4仿真和真机接口对照。真机用低层控制要先开开发者模式新版文档写了开启后不再保修测试时要悬挂或拉安全绳。仿真里的 Kp/Kd 不能直接拿过去用坑一已经说明参数和控制周期很敏感。还有一处要小心关节能转多少度官方给了三份数彼此对不上。关节SDK 接口文档URDFq1_v3.urdfMuJoCoq1_v3.xml左肩俯仰本文用的−2.6180 rad33 N·m−3.1421.571 rad22 N·m−3.041.47 rad22 N·m左髋俯仰−0.4890.489 rad33 N·m−3.0541.571 rad36 N·m−2.951.47 rad36 N·m左膝−0.4890.489 rad33 N·m02.444 rad36 N·m0.12.34 rad36 N·m表 5同一关节在三份官方资料里的范围和力矩上限。最离谱的是膝盖。人的膝盖只能往一个方向弯URDF 和 MuJoCo 里也都是从 0 往上文档里却写成正负对称的 ±0.489等于说膝盖还能反着弯而且和上面髋关节那一行一模一样很像表格错了行已经在启元社区反馈还没等到回复。本文左肩的轨迹在 −0.85−0.15 rad 之间三份都在范围内所以对实验没影响但真上机器人限位和力矩上限以哪份为准得先问官方。总结这次实验结论本身不复杂在 Q1 左肩这种负载很轻的关节上Kp60、Kd0.4 比 Kp15、Kd0.1 准了将近四倍力矩还没多用。花时间的是前两轮。第一轮 Kd 给到 4力矩一直在 ±22 N·m 之间跳跟踪反而更差可见 1 ms 周期下阻尼并不是越大越稳。第二轮更危险因为它的肩部曲线完全正常如果检查脚本只看目标关节这组数据就会被当成结果用掉而实际上两侧髋关节已经被骨盆顶出了 30° 左右。原因是固定骨盆删掉 freejoint 以后MuJoCo 对父子 body 的默认碰撞过滤不再作用于骨盆这个规则文档里写得很清楚只是我改模型的时候没想到它。之后我在自己的流程里加了两条只要改过官方模型就把全身关节限位和所有接触都查一遍检查脚本发现问题必须直接报错退出不能只打印。这次也有不少没覆盖到的地方骨盆是固定的电机是理想的只测了一个关节、一条轨迹、两组参数误差图里一直挂着的那段偏差也还没搞清来源。接下来我准备补一组带重力补偿的对照看偏差能不能消掉如果以后能摸到真机会按文档先在悬挂状态下试低层关节控制。代码和数据都放在下面的仓库里用同一个命令就能完整重跑。参考资料启元机器人开发者社区模型、SDK、工具链下载primebot/q1-urdfQ1 URDF 与 MuJoCo 模型primebot/q1-dev-guide、primebot/q1-sdk接口说明与 SDK 指南MuJoCo Python 文档MuJoCo 碰撞检测与过滤本文代码、数据和检查脚本Qiuner/q1-mujoco-pd-experiment启元机器人开发者社区https://dev.primebot.com/。 
长河君 · 2026/10/27 18:52:17 · 阅读 1.2k 启元 Q1 官方模型在 MuJoCo 里调 PD 参数 分享两个坑
启元 Q1 官方模型在 MuJoCo 里调 PD 参数 分享两个坑摘要对机器人我的感觉停留在上春晚能跳舞能格斗但听业内人士说很多都是用遥控器在背后操控的。后来才知道就算是遥控遥控器也只管说“抬手”手能不能真抬到位、抬的时候抖不抖靠的是底下每个关节自己的控制这篇想看的就是这一层。手上没有 Q1 真机我就从启元开发者社区下了官方的 Q1 模型放进 MuJoCo 里先玩起来。想法很简单把机器人骨盆固定在半空只动左肩一个关节让它按 0.5 Hz 的余弦轨迹上下摆然后换两组 PD 参数Kp15/Kd0.1 和 Kp60/Kd0.4看哪组跟得准、各用多大力矩。实际前后跑了四轮。第一轮 Kd 给到了 41 ms 的控制周期下力矩一直正负来回跳接近四成的采样点顶在 ±22 N·m 的上限上误差反而比小参数还大。第二轮把阻尼降下来肩膀的曲线终于正常了可我的检查脚本报出两侧髋关节越限了 0.539 rad差不多 31°。查下来是我为了固定骨盆删掉 freejoint 之后MuJoCo 默认的父子碰撞过滤对骨盆失效了骨盆一直顶着髋部连杆。把这两对碰撞排除掉以后第三轮才拿到能用的数据Kp60 那组平均误差约 0.87°Kp15 那组约 3.3°两组峰值力矩都只有 1.1 N·m 出头。下面按顺序讲模型怎么改、控制怎么写、结果怎么看、两个坑怎么查最后对着官方 SDK 文档列一下真机上对应的接口。一、实验要做的事目标是看 PD 参数对单关节跟踪精度的影响所以其他变量能固定的都固定了。Q1 是一台双足人形机器人如果让它自由站着左臂一动整个身体都会跟着晃就分不清误差是谁带来的。所以我把骨盆钉在离地 0.62 m 的位置下面截图里机器人悬在空中就是这个原因。图 1骨盆固定后的 Q1 初始姿态。整体流程否是下载官方Q1 模型改造模型固定骨盆、加电机基线测试不控制会怎样PD 对照两组参数全身检查通过正式结果 完整复现图 2实验流程。“检查不通过回去改模型”这一步实际走了两次。二、模型和环境Q1 模型在启元开发者社区资料包页的“仿真”分类里下载Q1 URDF v1.0.0AtomGit 上的 primebot/q1-urdf 也是同一套。包里xml/目录下有三个 MuJoCo 文件我用的是q1_v3.xml另外两个full_range放宽了关节范围mc自带了电机有需要可以直接拿来用。环境Windows 11Python 3.11.13MuJoCo 3.16.0NumPy 2.4.6Matplotlib 3.11.2。三、改造模型q1_v3.xml里没有执行器骨盆也是自由的。我没改官方文件而是在脚本里读进来改完再另存为fixed.xml# 步长 1 ms积分器换成 implicitfastET.SubElement(root,option,timestepstr(DT),integratorimplicitfast)iffixed:# 去掉骨盆的 freejoint固定在 0.62 mpelvis.remove(pelvis.find(freejoint))pelvis.set(pos,0 0 0.62)# 排除骨盆和两侧髋部连杆的碰撞原因见第六节contactET.SubElement(root,contact)forsidein(left,right):ET.SubElement(contact,exclude,body1pelvis,body2f{side}_hip_pitch_link)# 每个关节加一个力矩电机上限直接用官方的 actuatorfrcrangeactuatorsET.SubElement(root,actuator)forjointinroot.findall(.//joint):ET.SubElement(actuators,motor,namejoint.get(name)_motor,jointjoint.get(name),gear1,ctrllimitedtrue,ctrlrangejoint.get(actuatorfrcrange))这样左肩俯仰left_shoulder_pitch_joint的力矩上限就是官方给的 ±22 N·m。要注意这种 motor 是理想的给多少出多少跟真电机差得远。四、PD 控制PD 就是一个弹簧加一个阻尼Kp 管“偏了多少就拉回多少”Kp60 意味着偏 1 rad 给 60 N·mKd 管“转得快就往回压一点”防止冲过头。每个仿真步1 ms走一遍下面这个循环目标角度q_ref算角度差和速度差读实际角度和速度τ Kp·Δq Kd·Δq̇限幅±22 N·mMuJoCo步进 1 ms图 3控制循环。代码# 目标轨迹中心 -0.5 rad幅度 0.35 rad周期 2 sdesired[index]-.5.35*np.cos(2*np.pi*.5*t)velocity[index]-.35*np.pi*np.sin(np.pi*t)# 目标角速度# PD没有加重力补偿rawkp*(desired-data.qpos[qa])kd*(velocity-data.qvel[va])# 按官方上限截断data.ctrl[:]np.clip(raw,model.actuator_ctrlrange[:,0],model.actuator_ctrlrange[:,1])轨迹起点我特意设成 −0.15 rad和左肩初始角度一样不然一开始就有个阶跃误差会被开头那一下拉高。两组参数组KpN·m/radKdN·m·s/radlow150.1high600.4其他关节400.2表 1PD 参数每组跑 10 s。Kp 和 Kd 是一起放大 4 倍的想单独看哪个起作用得再分开跑。五、结果指标lowhighRMSE0.0582 rad3.33°0.0152 rad0.87°最大误差0.0777 rad4.45°0.0187 rad1.07°峰值力矩1.17 N·m1.12 N·m限幅比例00表 2第三轮结果。RMSE 可以理解成整段的平均误差。图 4从上到下依次是目标/实际角度、误差、左肩力矩。high 的误差差不多是 low 的四分之一。中间那张误差图有点意思两条线从头到尾都是负的实际角度一直比目标高一截从来没有越过目标下面的力矩也一直是负的像是始终在顶着什么。我猜是没加重力补偿电机一直在扛手臂的重量还没验证。力矩方面两组峰值都在 1.11.2 N·m离 22 N·m 差得远没有一次被限幅。也就是说这条轨迹对肩关节来说负载很轻两组的差别纯粹在精度上。只凭这一组数据推不出“增益越大越好”。下面第一轮的结果就是反例。六、两个坑run-01high (60, 4)力矩高频振荡39.47% 采样被限幅run-02降低 Kd肩部正常髋关节越限 0.539 radrun-03排除骨盆-髋部碰撞run-04从头重跑确认图 5四轮实验。坑一Kd 给大了第一轮 high 用的是 (60, 4)其他关节也是 (60, 4)。跑出来 high 组的力矩正负快速交替39.47% 的采样点卡在 ±22 N·mRMSE 0.0663 rad比 low 组0.0553还差。保持姿态的场景也一样49.51% 被限幅。图 6第一轮最下面的力矩曲线基本糊成一片。第二轮把 Kd 降到 0.4顺手把其他关节也改成了 (40, 0.2)振荡就没了。坑二髋关节越限第二轮肩部曲线看着没问题但检查脚本报了越限左髋hip_pitch最大到 2.009 rad上限是 1.47 rad超了 0.539 rad右侧超了 0.538 rad。这个脚本会遍历每个采样点、每个关节# 每个点、每个关节算低于下限或高于上限的量violationnp.maximum(model.jnt_range[:,0]-states[:,:model.nq],states[:,:model.nq]-model.jnt_range[:,1])# 超过 0.001 rad 直接报错assertviolation.max().001,fJoint range exceeded in{name}这条 assert 是第二轮之后补的之前脚本只打印数值所以第二轮才能顺利跑完出图。这个问题不用跑仿真就能复现初始姿态下调一次mj_forward看接触数自由骨盆的模型是 0固定骨盆的是 6全在骨盆和两侧hip_pitch_link之间。问题出在固定骨盆这一步。MuJoCo 默认会过滤直接相连的父子 body 之间的碰撞但父 body 是 world 时不过滤。骨盆去掉 freejoint 后焊到了 world 上这条过滤就不再作用于它。MuJoCo 文档碰撞过滤解决方法是只 exclude 这两对其他碰撞照常保留检查项run-02run-03全身最大越限0.539 rad0最大接触数60表 3修复前后。七、对应到真机仿真中真机文档每个关节给 PD 力矩低层话题/aima/hal/joint/command可同时给位置、速度、力矩、Kp、Kd1 ms 一次建议 5001000 Hz低于 100 Hz 可能触发硬件看门狗Windows MuJoCoUbuntu 22.04 ROS2 Humble暂不支持 Windows 开发停程序就行可切阻尼或零力矩模式急停退出前要发一帧阻尼指令表 4仿真和真机接口对照。真机用低层控制要先开开发者模式新版文档写了开启后不再保修测试时要悬挂或拉安全绳。仿真里的 Kp/Kd 不能直接拿过去用坑一已经说明参数和控制周期很敏感。还有一处要小心关节能转多少度官方给了三份数彼此对不上。关节SDK 接口文档URDFq1_v3.urdfMuJoCoq1_v3.xml左肩俯仰本文用的−2.6180 rad33 N·m−3.1421.571 rad22 N·m−3.041.47 rad22 N·m左髋俯仰−0.4890.489 rad33 N·m−3.0541.571 rad36 N·m−2.951.47 rad36 N·m左膝−0.4890.489 rad33 N·m02.444 rad36 N·m0.12.34 rad36 N·m表 5同一关节在三份官方资料里的范围和力矩上限。最离谱的是膝盖。人的膝盖只能往一个方向弯URDF 和 MuJoCo 里也都是从 0 往上文档里却写成正负对称的 ±0.489等于说膝盖还能反着弯而且和上面髋关节那一行一模一样很像表格错了行已经在启元社区反馈还没等到回复。本文左肩的轨迹在 −0.85−0.15 rad 之间三份都在范围内所以对实验没影响但真上机器人限位和力矩上限以哪份为准得先问官方。总结这次实验结论本身不复杂在 Q1 左肩这种负载很轻的关节上Kp60、Kd0.4 比 Kp15、Kd0.1 准了将近四倍力矩还没多用。花时间的是前两轮。第一轮 Kd 给到 4力矩一直在 ±22 N·m 之间跳跟踪反而更差可见 1 ms 周期下阻尼并不是越大越稳。第二轮更危险因为它的肩部曲线完全正常如果检查脚本只看目标关节这组数据就会被当成结果用掉而实际上两侧髋关节已经被骨盆顶出了 30° 左右。原因是固定骨盆删掉 freejoint 以后MuJoCo 对父子 body 的默认碰撞过滤不再作用于骨盆这个规则文档里写得很清楚只是我改模型的时候没想到它。之后我在自己的流程里加了两条只要改过官方模型就把全身关节限位和所有接触都查一遍检查脚本发现问题必须直接报错退出不能只打印。这次也有不少没覆盖到的地方骨盆是固定的电机是理想的只测了一个关节、一条轨迹、两组参数误差图里一直挂着的那段偏差也还没搞清来源。接下来我准备补一组带重力补偿的对照看偏差能不能消掉如果以后能摸到真机会按文档先在悬挂状态下试低层关节控制。代码和数据都放在下面的仓库里用同一个命令就能完整重跑。参考资料启元机器人开发者社区模型、SDK、工具链下载primebot/q1-urdfQ1 URDF 与 MuJoCo 模型primebot/q1-dev-guide、primebot/q1-sdk接口说明与 SDK 指南MuJoCo Python 文档MuJoCo 碰撞检测与过滤本文代码、数据和检查脚本Qiuner/q1-mujoco-pd-experiment启元机器人开发者社区https://dev.primebot.com/。