第4章 运动学基础：
《origin》与《axis》的数学原理、旋转矩阵与欧拉角、父子坐标系变换
各位好，我是你们这门课的老朋友。今天咱们聊点硬核的——运动学基础。你可能会问：“URDF 里不就写几个数字吗？至于扯到矩阵？” 嗯，我当年也这么想，直到第一次调试机械臂时，关节转反了 180 度，差点把电机烧了。从那以后，我老老实实把旋转矩阵和欧拉角的关系捋了一遍。
说白了，<origin> 和 <axis> 就是机器人坐标系的“交通规则”。不懂它们，你的机器人模型就像没有地图的司机——寸步难行。
4.1 <origin> 与 <axis> 到底在描述什么？
先看一个最简单的关节定义：
<joint name="shoulder" type="revolute">
<parent link="base_link"/>
<child link="arm_link"/>
<origin xyz="0.1 0.0 0.2" rpy="0 0 1.57"/>
<axis xyz="0 0 1"/>
</joint><origin> 里的 xyz 是子坐标系原点在父坐标系下的平移量，单位米。rpy 是旋转：绕 X 轴转 roll，绕 Y 轴转 pitch，绕 Z 轴转 yaw。我习惯把 rpy 想成“先把坐标系掰成那个姿势，再平移过去”。
<axis> 定义了关节的旋转轴——在 子坐标系 下表示。比如 0 0 1 就是绕子坐标系的 Z 轴转。曾经有个项目，我把轴写成了 0 1 0，结果机械臂像在游泳……排查了半天。
<origin> 是“把子关节放在哪”，<axis> 是“子关节怎么动”。两者都在父与子之间搭桥。
4.2 旋转矩阵与欧拉角——别怕，就三个旋转
欧拉角（RPY）是 URDF 里最常用的旋转表达。你写 rpy="0 0 1.57"，内部其实构建了一个旋转矩阵：
R = Rz(yaw) * Ry(pitch) * Rx(roll)
= | cos(yaw) -sin(yaw) 0 | | cos(pitch) 0 sin(pitch) | | 1 0 0 |
| sin(yaw) cos(yaw) 0 | * | 0 1 0 | * | 0 cos(roll) -sin(roll) |
| 0 0 1 | | -sin(pitch) 0 cos(pitch) | | 0 sin(roll) cos(roll) |注意顺序：先绕 X，再绕 Y，最后绕 Z。我个人记这个顺序的口诀是“XYZ —— 先滚后仰再偏头”。
你可能会问：“为什么不用四元数？” URDF 里其实也支持，但 rpy 更直观。我在调试四足机器人时，用 rpy 一眼就能看出躯干是不是歪了。但要注意——万向锁！当 pitch 接近 ±90° 时，roll 和 yaw 会耦合。我曾经在仿真里遇到狗翻不过来，就是因为万向锁导致姿态插值异常。
rpy="90 0 0"，结果模型直接飞了。
4.3 父子坐标系变换——TF 树的数学本质
每个 joint 定义了一个从父坐标系到子坐标系的变换 T。这个 T 是 4x4 齐次矩阵：
T = | R t |
| 0 1 |其中 R 是旋转矩阵（由 rpy 算出），t 是平移向量（xyz）。把机器人所有关节的 T 连起来，就得到了一条运动学链。比如从 base_link 到 gripper：
T_base_to_gripper = T1 * T2 * T3 * ...注意顺序：左乘。我刚开始总搞反，以为矩阵乘法像数字一样可交换。直到在 Rviz 里看到手臂拧成了麻花……嗯，矩阵乘法不交换，这是铁律。
| 变换 | 含义 | URDF 对应 |
|---|---|---|
| 平移 (xyz) | 子坐标系原点在父系中的位置 | <origin xyz="…"> |
| 旋转 (rpy) | 子坐标系在父系中的姿态 | <origin rpy="…"> |
| 旋转轴 (axis) | 关节运动方向（子系下） | <axis xyz="…"> |
<axis> 时，轴向量一定要归一化！虽然 URDF 解析器会自动归一化，但如果你写 0 0 2，某些旧版本可能会行为异常。另外，固定关节（fixed）不需要 axis，但如果你写了，它会被忽略——别问我怎么知道的。
4.4 一个完整的例子：从 base 到 laser 的变换
假设你的机器人底座上有个激光雷达，你想把它放在前方 10cm，向上 5cm，并且绕 Z 轴转 90 度：
<joint name="laser_joint" type="fixed">
<parent link="base_link"/>
<child link="laser_frame"/>
<origin xyz="0.1 0 0.05" rpy="0 0 1.5708"/>
</joint>这里 rpy 的 1.5708 就是 π/2。你可以在代码里直接用 pi/2 计算，但 URDF 是纯文本，所以得写数字。我个人习惯在 xacro 里用 ${pi/2}，可读性高很多。
变换后的激光坐标系：X 轴指向机器人左侧，Y 轴指向后方，Z 轴朝上。为什么？因为绕 Z 转 90° 会把原来的 X 轴掰到 Y 方向。画个图就明白了——我建议你每次写完 origin，都在 Rviz 里用 TF 显示一下，确认方向对不对。
tf2_echo 验证。三步下来，基本不会错。
好了，这一章的内容就是这些。运动学是机器人建模的骨架，<origin> 和 <axis> 就是骨架上的关节。多练几次，你就能形成肌肉记忆。