3 · URDF中的坐标定义

link与joint的坐标系关系，原点偏移与旋转（xyz/rpy）的物理意义

做机器人这么久，我越来越觉得URDF里的坐标定义就像机器人的“骨架说明书”。你想想看，一个机械臂能不能动得顺，很大程度上取决于每个link和joint的坐标系有没有摆对。今天咱们就掰开揉碎聊聊——link坐标系、joint坐标系，还有那堆xyz/rpy到底在说什么。

            💡 一句话先记住： URDF里每个link都有一个自己的坐标系，joint则描述了父link坐标系到子link坐标系的偏移和旋转。说白了，joint就是坐标变换的“桥梁”。
        

3.1 link坐标系 —— 每个刚体的“身份证”

每个link在URDF里都会定义一个坐标系。这个坐标系的原点通常放在link的物理中心或者关节轴线上，具体放哪取决于你的设计习惯。我个人习惯把原点放在关节轴与link底面的交点，这样后续算惯性矩阵时比较直观。

举个例子，一个简单的基座link：

<link name="base_link">
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <box size="0.4 0.3 0.1"/>
    </geometry>
  </visual>
</link>

这里的 origin 是相对于link坐标系的偏移。如果我把xyz写成 0 0 0.05，那视觉模型就会往上挪半厘米。嗯，这里要注意：link坐标系本身的位置是由父joint决定的，而visual origin只是视觉网格的偏移。

3.2 joint坐标系 —— 连接两个世界的变换

joint才是坐标变换的核心。每个joint都有一个 <origin> 标签，它定义了从父link坐标系到子link坐标系的变换。这个变换包含平移（xyz）和旋转（rpy）。

我记得有一次调试一个六轴机器人，末端执行器总是偏了2厘米。查了半天，发现是joint的origin写反了符号。你想想看，如果父link的坐标系原点在关节转轴左侧，而子link的origin却写成了向右偏移，那整个运动学链就全拧了。

🔧 我的小技巧： 写URDF时，先把所有joint的origin画在纸上。用箭头标出父→子的方向，再填xyz/rpy。这招帮我避过不少坑。

3.3 xyz/rpy 的物理意义 —— 别被数字骗了

xyz的单位是米，rpy的单位是弧度。但很多人容易忽略的是：rpy的顺序是绕固定轴（父坐标系）的Z-Y-X旋转。也就是说，先绕Z轴转yaw，再绕Y轴转pitch，最后绕X轴转roll。

为什么会这样？因为ROS里约定用这种“ extrinsic ZYX ”顺序。如果你用别的顺序，比如绕自身轴旋转，那结果就完全不一样了。我曾经在项目里把rpy写成了 0 0 1.57，以为只是绕Z轴转90度，结果模型在Rviz里翻了个跟头——因为实际是绕Y轴转了1.57 rad，跟预期差了十万八千里。

参数	含义	典型值举例
xyz	父→子坐标系原点的平移向量 (m)	0 0 0.3 (沿Z轴向上0.3米)
rpy	绕固定轴Z-Y-X的旋转 (rad)	0 0 1.5708 (绕Z轴转90°)

3.4 一个完整的joint示例

<joint name="shoulder_pan_joint" type="revolute">
  <parent link="base_link"/>
  <child link="shoulder_link"/>
  <origin xyz="0 0 0.1" rpy="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit lower="-3.14" upper="3.14" effort="100" velocity="1.0"/>
</joint>

这个joint把 shoulder_link 放在 base_link 上方0.1米处，且没有旋转。轴是Z轴，所以它是一个绕Z旋转的关节。注意：origin的旋转会影响关节轴的方向。如果我把rpy写成 1.57 0 0，那关节轴就会跟着旋转，变成绕X轴方向。

⚠️ 避坑指南： 我曾经在写一个移动机械臂时，把基座到车体的joint origin写成了 xyz="0 0 0.2" rpy="0 0 0"，但车体本身的坐标系原点在轮子中心。结果一运动，机械臂底座悬空了。后来才意识到：origin的参考系是父link的坐标系，不是世界坐标系。

上图展示了一个典型的父link→joint→子link结构。橙色虚线代表joint的origin变换，它把父link坐标系下的点映射到子link坐标系。实际写URDF时，你只需要填好xyz和rpy，ROS解析器会自动帮你构建出这棵坐标树。

3.5 原点偏移与旋转的“物理感”

我经常跟团队里的人说：把xyz想象成“父link坐标系里的一根棍子”，棍子的末端就是子link坐标系的原点。而rpy就是这根棍子在末端怎么拧。比如 xyz="0 0 0.3" rpy="0 0 1.57"，相当于先沿父link的Z轴走0.3米，然后绕Z轴转90度。子link的X轴就会指向父link的Y轴方向。

如果你觉得抽象，可以在Rviz里打开“显示所有link坐标系”，然后手动改URDF里的数值，看看坐标系怎么动。我刚开始学的时候就是这么干的——改一个数字，刷新一下模型，坐标系箭头就跟着变，特别直观。

📌 记住这个原则： joint的origin是从父到子的变换。子link的坐标系原点永远在父link坐标系下表达。所以写 origin xyz="0 0 0.2" 意思是子link的原点位于父link的Z正方向0.2米处。

3.6 常见错误与我的经验

rpy顺序搞错：ROS用ZYX固定轴顺序，如果你用XYZ顺序，模型会歪。我踩过这个坑，后来在代码里加了个注释提醒自己。
xyz符号反了：尤其是当父link坐标系方向跟直觉不一致时。比如基座坐标系Z轴向上，但有些人的基座Z轴朝前……这时候joint的origin就要仔细算。
忘记考虑joint轴方向：origin的旋转会影响关节轴。如果你给revolute关节加了rpy，轴向量也会跟着转。我曾经把轴设成 0 1 0，但origin里转了90度，结果关节绕着X轴转了，排查了半天。

好了，这一章的内容就是这些。URDF里的坐标定义说难不难，但细节特别多。你只要抓住“joint origin是父→子的变换”这个核心，再配合Rviz可视化，基本不会出大问题。