🤖 roboto_origin_03 Wiki
首页 / 硬件 / URDF 模型结构与关节配置

本文档解析 Atom01(V1.0)与 Roboto Origin(V2.0)人形机器人的 URDF 运动学模型,涵盖连杆拓扑树、关节轴向与限位、碰撞体简化策略,以及两版模型在关节配置上的关键差异。阅读本文档前,建议先熟悉机械结构的总体布局,以便将 URDF 中的抽象连杆名称与实物部件对应起来。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

模型拓扑概览

两个版本的 URDF 均采用相同的树形拓扑结构:以 base_link(骨盆/腰部基座)为根节点,向下分叉出左/右下肢链,向上延伸出 torso_link(胸腔)并进一步展开为左/右上肢链。全模型共包含 24 个连杆(link)23 个旋转关节(revolute joint),所有关节均为单自由度旋转副,未使用移动副或球关节。

graph TD
    A[base_link<br/>腰部基座] --> B[left_thigh_yaw_joint]
    A --> C[right_thigh_yaw_joint]
    A --> D[torso_joint]
    
    B --> E[left_thigh_yaw_link]
    E --> F[left_thigh_roll_joint]
    F --> G[left_thigh_roll_link]
    G --> H[left_thigh_pitch_joint]
    H --> I[left_thigh_pitch_link]
    I --> J[left_knee_joint]
    J --> K[left_knee_link]
    K --> L[left_ankle_pitch_joint]
    L --> M[left_ankle_pitch_link]
    M --> N[left_ankle_roll_joint]
    N --> O[left_ankle_roll_link]
    
    C --> P[right_thigh_yaw_link]
    P --> Q[right_thigh_roll_joint]
    Q --> R[right_thigh_roll_link]
    R --> S[right_thigh_pitch_joint]
    S --> T[right_thigh_pitch_link]
    T --> U[right_knee_joint]
    U --> V[right_knee_link]
    V --> W[right_ankle_pitch_joint]
    W --> X[right_ankle_pitch_link]
    X --> Y[right_ankle_roll_joint]
    Y --> Z[right_ankle_roll_link]
    
    D --> AA[torso_link<br/>胸腔]
    AA --> AB[left_arm_pitch_joint]
    AB --> AC[left_arm_pitch_link]
    AC --> AD[left_arm_roll_joint]
    AD --> AE[left_arm_roll_link]
    AE --> AF[left_arm_yaw_joint]
    AF --> AG[left_arm_yaw_link]
    AG --> AH[left_elbow_pitch_joint]
    AH --> AI[left_elbow_pitch_link]
    AI --> AJ[left_elbow_yaw_joint]
    AJ --> AK[left_elbow_yaw_link]
    
    AA --> AL[right_arm_pitch_joint]
    AL --> AM[right_arm_pitch_link]
    AM --> AN[right_arm_roll_joint]
    AN --> AO[right_arm_roll_link]
    AO --> AP[right_arm_yaw_joint]
    AP --> AQ[right_arm_yaw_link]
    AQ --> AR[right_elbow_pitch_joint]
    AR --> AS[right_elbow_pitch_link]
    AS --> AT[right_elbow_yaw_joint]
    AT --> AU[right_elbow_yaw_link]

Sources: Atom01_urdf.urdf, roboto_origin.urdf

连杆命名规范与可视化网格

连杆命名遵循 {side}_{body_part}_{dof}_link 的层级规则,其中 sideleftrightdof 使用航空航天领域中常用的 yaw/roll/pitch 描述旋转方向。所有连杆均关联独立的 STL 可视化网格文件,存放于同级 meshes/ 目录下。两版本虽然文件名完全一致(各 23 个 STL),但 V2.0 的网格数据随机械结构优化而更新,因此不可直接混用。

身体部位 连杆名称示例 对应 mesh 文件
腰部基座 base_link base_link.STL
髋关节偏航 left_thigh_yaw_link left_thigh_yaw_link.STL
髋关节横滚 left_thigh_roll_link left_thigh_roll_link.STL
髋关节俯仰 left_thigh_pitch_link left_thigh_pitch_link.STL
膝关节 left_knee_link left_knee_link.STL
踝关节俯仰 left_ankle_pitch_link left_ankle_pitch_link.STL
踝关节横滚 left_ankle_roll_link left_ankle_roll_link.STL
胸腔 torso_link torso_link.STL
肩关节俯仰 left_arm_pitch_link left_arm_pitch_link.STL
肩关节横滚 left_arm_roll_link left_arm_roll_link.STL
肩关节偏航 left_arm_yaw_link left_arm_yaw_link.STL
肘关节俯仰 left_elbow_pitch_link left_elbow_pitch_link.STL
肘关节偏航 left_elbow_yaw_link left_elbow_yaw_link.STL

Sources: Atom01_urdf.urdf, roboto_origin.urdf

关节配置详解

每个关节通过 <origin> 定义父连杆到子连杆的相对位姿,通过 <axis> 定义旋转轴单位向量,并通过 <limit> 约束运动范围、力矩与最大转速。以下按身体部位展开说明。

下肢关节链

单腿包含 6 个关节,构成从骨盆到脚掌的串联运动链。髋关节采用 yaw-roll-pitch 三轴正交布局,其中偏航轴与横滚轴并非严格沿世界坐标系的 XYZ 方向,而是根据机械结构做了约 30° 的倾斜补偿(表现为 axis 向量中 X 与 Z 分量均非零)。膝关节与踝关节俯仰轴则严格沿 Y 轴方向,使得腿部主要在矢状面内做前后摆动。

关节名称 父连杆 子连杆 旋转轴 (axis xyz) 力矩上限 (N·m) 速度上限 (rad/s)
left_thigh_yaw_joint base_link left_thigh_yaw_link 0.5 0 0.86603 (V1.0) / -0.5 0 -0.86603 (V2.0) 80 10.47
left_thigh_roll_joint left_thigh_yaw_link left_thigh_roll_link 0.86603 0 -0.5 80 10.47
left_thigh_pitch_joint left_thigh_roll_link left_thigh_pitch_link 0 1 0 80 10.47
left_knee_joint left_thigh_pitch_link left_knee_link 0 1 0 80 10.47
left_ankle_pitch_joint left_knee_link left_ankle_pitch_link 0 1 0 18 3.7692
left_ankle_roll_joint left_ankle_pitch_link left_ankle_roll_link 1 0 0 18 3.7692

右腿关节与左腿呈镜像对称,仅在 Y 方向的位置参数取反(例如 right_thigh_yaw_joint 的 origin y 为 -0.0725)。值得注意的是,V2.0 将大腿偏航关节的旋转轴反向,并相应交换了限位的上下界,这一改动使得 SolidWorks 导出的坐标系与实际控制代码中的正方向定义更加统一,开发者在迁移控制算法时需特别留意符号翻转。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

躯干关节

torso_joint 连接 base_linktorso_link,旋转轴为 0 0 1,即绕世界坐标系 Z 轴的偏航运动。其限位设置为 -3.143.14,理论上可无限旋转(实际受走线约束)。该关节在行走控制中用于上半身与下半身的相对扭动,相当于传统人形机器人中的“腰部旋转”自由度。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

上肢关节链

单臂包含 5 个关节,沿用 pitch-roll-yaw-pitch-yaw 的串联顺序。肩关节俯仰轴沿 Y 方向,控制手臂前后摆动;横滚轴沿 X 方向,控制手臂内外翻转;偏航轴沿 Z 方向,控制手臂内外旋。肘关节俯仰轴再次回到 Y 方向,实现小臂的屈伸;末端肘偏航轴沿 X 方向,用于腕部姿态调整。由于手臂关节所需力矩远小于腿部,其 effort 统一设为 18 N·m,velocity 为 3.7692 rad/s。

关节名称 父连杆 子连杆 旋转轴 (axis xyz) 限位下限 限位上限
left_arm_pitch_joint torso_link left_arm_pitch_link 0 1 0 -1.57 1.57
left_arm_roll_joint left_arm_pitch_link left_arm_roll_link 1 0 0 -0.25 1
left_arm_yaw_joint left_arm_roll_link left_arm_yaw_link 0 0 1 (V1.0) / 0 0 -1 (V2.0) -0.6 (V1.0) / -1.57 (V2.0) 1.3 (V1.0) / 1.57 (V2.0)
left_elbow_pitch_joint left_arm_yaw_link left_elbow_pitch_link 0 1 0 -0.6 3.14 (V1.0) / 1.57 (V2.0)
left_elbow_yaw_joint left_elbow_pitch_link left_elbow_yaw_link 1 0 0 -1.57 1.57

右臂关节同样为镜像对称,且在 V2.0 中与左臂同步调整了偏航轴方向与限位范围。V2.0 将肘关节俯仰上限从 3.14 rad 缩减至 1.57 rad,这是出于机械限位结构的实际约束所做的修正,避免了仿真中出现过驱动(over-extension)的非法姿态。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

V1.0 与 V2.0 关节限位差异速查

以下表格汇总了两版 URDF 中关节限位(limit lower/upper)与旋转轴(axis)发生变化的关节,便于在仿真迁移或控制器适配时快速定位。

关节名称 V1.0 旋转轴 V2.0 旋转轴 V1.0 限位 (rad) V2.0 限位 (rad) 差异说明
left_thigh_yaw_joint 0.5 0 0.86603 -0.5 0 -0.86603 [-0.2, 1] [-1, 0.2] 轴与限位同时反向
right_thigh_yaw_joint 0.5 0 0.86603 -0.5 0 -0.86603 [-1, 0.2] [-0.2, 1] 轴与限位同时反向
left_thigh_pitch_joint 0 1 0 0 1 0 [-1, 1] [-1.57, 1.57] 限位放宽
right_thigh_pitch_joint 0 1 0 0 1 0 [-1, 1] [-1.57, 1.57] 限位放宽
left_knee_joint 0 1 0 0 1 0 [-1.5, 1.5] [-0.2, 2.5] 下限收紧、上限放宽
right_knee_joint 0 1 0 0 1 0 [-1.5, 1.5] [-0.2, 2.5] 下限收紧、上限放宽
left_ankle_pitch_joint 0 1 0 0 1 0 [-1.5, 1.3] [-0.6, 0.6] 限位大幅收紧
right_ankle_pitch_joint 0 1 0 0 1 0 [-1.5, 1.3] [-0.6, 0.6] 限位大幅收紧
left_arm_yaw_joint 0 0 1 0 0 -1 [-0.6, 1.3] [-1.57, 1.57] 轴反向,限位对称化
right_arm_yaw_joint 0 0 1 0 0 -1 [-1.3, 0.6] [-1.57, 1.57] 轴反向,限位对称化
left_elbow_pitch_joint 0 1 0 0 1 0 [-0.6, 3.14] [-0.6, 1.57] 上限收紧
right_elbow_pitch_joint 0 1 0 0 1 0 [-0.6, 3.14] [-0.6, 1.57] 上限收紧

其余关节(如 thigh_rollankle_rollarm_pitcharm_rollelbow_yaw 等)在两版中的轴向量与限位值保持一致,可直接复用。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

碰撞体简化策略

URDF 中每个连杆均包含 <visual><collision> 两组几何描述。视觉层统一使用高精度 STL 网格以保证渲染真实感;碰撞层则采用了大幅简化的策略:大部分连杆的 STL 碰撞 mesh 被注释掉,替换为包围盒(box)或圆柱体(cylinder)。这一做法显著降低了物理引擎(如 PyBullet、MuJoCo、Gazebo)在接触检测时的计算开销,同时避免了不规则 mesh 导致的仿真穿透与抖动。

具体而言,大腿连杆(thigh_pitch_link)与小腿连杆(knee_link)使用尺寸约为 0.08 × 0.1 × 0.22 m 的长方体包围盒;胸腔(torso_link)使用 0.16 × 0.16 × 0.22 m 的长方体;手臂各连杆则采用半径 0.03 m、长度 0.05–0.10 m 的圆柱体近似。脚踝横滚连杆(ankle_roll_link)因结构扁平且与地面接触敏感,保留了原始 STL 作为碰撞体。若你在自定义仿真环境中需要更高精度的接触检测,可取消注释并恢复 mesh 碰撞,但需权衡计算资源。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

坐标系与运动学约定

URDF 严格遵循右手坐标系:X 轴指向机器人前方,Y 轴指向机器人左侧,Z 轴竖直向上。所有关节的 <origin> 均基于父连杆坐标系描述,因此在解析运动学链时需逐层累加变换。例如,left_arm_pitch_joint 的 origin 为 xyz="0 0.12175 0.20618",表示该关节在 torso_link 坐标系中位于 Y 正方向 0.122 m(左侧)、Z 正方向 0.206 m(上方),这对应了左肩关节在胸腔上的实际安装位置。

关节的 rpy="0 0 0" 表明子连杆坐标系与父连杆坐标系无初始旋转,所有姿态差异完全由 <axis> 定义的旋转轴与 <origin> 定义的平移决定。这种零初始旋转的设计降低了正运动学手算的复杂度,也便于将 URDF 直接导入 Pinocchio、RBDL 等解析库进行自动求导。

Sources: Atom01_urdf.urdf, roboto_origin.urdf

后续阅读建议

理解 URDF 的拓扑与关节配置后,下一步建议深入以下主题: