第二章 运动学基础（一）：空间描述与变换、齐次坐标、旋转矩阵与欧拉角
各位同学，大家好。今天我们正式开始啃运动学这块硬骨头。说实话，我刚入行那会儿，觉得运动学就是一堆矩阵乘法，枯燥得很。直到有一次，我调试一个七轴机械臂，死活算不对末端位姿，折腾了两天，最后发现是旋转顺序搞反了。从那以后，我再也不敢小看这些“基础”了。
这一章，我们聚焦在“怎么描述一个刚体在空间中的位置和姿态”。你想想看，人形机器人要走路、要抓东西，首先得知道自己手脚在哪儿、朝哪个方向。这就是空间描述与变换要解决的问题。
2.1 空间描述：位置与姿态
一个刚体在三维空间里，有6个自由度——3个位置自由度，3个姿态自由度。位置好说，用坐标(x, y, z)就搞定了。但姿态呢？怎么描述一个物体“朝哪边看”？
我个人习惯把姿态想象成“把物体坐标系旋转到与世界坐标系对齐”。这个旋转关系，就是我们要重点搞清楚的。
核心概念：一个坐标系{B}相对于坐标系{A}的描述，包含两部分：
- 位置矢量：APB —— B的原点在A中的坐标
- 旋转矩阵：ARB —— B的坐标轴在A中的方向
举个例子。假设你的手机平放在桌上，屏幕朝上。手机坐标系的原点在手机中心，Z轴垂直屏幕向上。那么手机相对于桌面的位置就是中心点的坐标，姿态就是Z轴朝上、X轴朝右、Y轴朝前。
2.2 旋转矩阵：姿态的数学表达
旋转矩阵是个3x3的方阵，每一列代表被描述坐标系的坐标轴在参考坐标系中的方向向量。它有两个重要性质：
- 正交性：矩阵的逆等于它的转置，即 RT = R-1
- 行列式为+1：保证是右手系，不会把物体镜像翻转
我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，由于浮点数误差，矩阵慢慢不再正交了。结果就是物体越转越变形。解决办法是定期做正交化处理——用SVD分解或者Gram-Schmidt方法。
实用技巧：检查一个矩阵是不是合法的旋转矩阵，快速方法就是算 R * RT，看结果是不是接近单位矩阵。如果对角线元素偏离1超过1e-6，就该做修正了。
2.3 欧拉角：更直观的姿态表示
旋转矩阵虽然数学上漂亮，但人看着不直观。你想想看，给你一个3x3的矩阵，你能一眼看出它对应什么姿态吗？反正我不能。
欧拉角就友好多了。它用三个角度来描述旋转：绕X轴转（滚转Roll）、绕Y轴转（俯仰Pitch）、绕Z轴转（偏航Yaw）。
但这里有个大坑——旋转顺序。同样的三个角度，按不同顺序旋转，结果完全不同。常见的顺序有：
| 顺序 | 应用场景 | 说明 |
|---|---|---|
| ZYX | 航空、人形机器人 | 先偏航，再俯仰，最后滚转 |
| ZYZ | 工业机械臂 | 常用于腕部关节 |
| XYZ | 计算机图形学 | 固定轴顺序 |
警告：欧拉角有“万向锁”问题。当第二个角度为±90°时，第一个和第三个角度会失去一个自由度。我曾经在仿真中遇到机器人手臂突然“卡住”，就是因为俯仰角到了90°，滚转和偏航无法独立控制。解决方案是用四元数代替欧拉角，这个我们后面会讲。
2.4 齐次坐标：把旋转和平移统一起来
单独用旋转矩阵描述姿态，用位置矢量描述位置，做变换时得分开算：先旋转，再平移。公式长这样：
P' = R * P + T这式子本身没问题，但当你需要做一连串变换时——比如从脚到膝盖、到髋关节、到躯干、到肩膀、到手——每次都要先乘旋转矩阵再加平移向量，代码写起来又长又容易出错。
齐次坐标就是来解决这个问题的。它把3D点扩展成4D：
P_homogeneous = [x, y, z, 1]^T变换矩阵变成4x4：
T = | R T |
| 0 1 |这样，一次矩阵乘法就搞定了旋转和平移：
P' = T * P而且，多个变换可以连乘成一个矩阵：
T_total = T1 * T2 * T3 * ...嗯，这里要注意：矩阵乘法不满足交换律，所以变换顺序很重要。先旋转再平移，和先平移再旋转，结果完全不同。
2.5 知识体系总览
下面这张图，是我自己总结的本章知识结构。你可以把它当作一个“地图”，学完后再回来看，会更有感觉。
2.6 实战：从旋转矩阵到欧拉角的转换
说了这么多理论，我们来点实际的。假设你有一个旋转矩阵，想把它转成ZYX顺序的欧拉角，怎么做？
设旋转矩阵为：
R = | r11 r12 r13 |
| r21 r22 r23 |
| r31 r32 r33 |ZYX顺序的欧拉角 (γ, β, α) 对应 (偏航, 俯仰, 滚转)，计算公式如下：
β = atan2(-r31, sqrt(r11² + r21²))
α = atan2(r21/cosβ, r11/cosβ)
γ = atan2(r32/cosβ, r33/cosβ)注意，当 β = ±90° 时，cosβ = 0，公式就失效了。这就是万向锁。实际代码中要加个判断：
if abs(r31) > 0.9999: # 接近 ±90°
# 万向锁情况，只计算 α + γ 的和
α = 0
γ = atan2(r12, r22)
else:
β = asin(-r31)
α = atan2(r21/cosβ, r11/cosβ)
γ = atan2(r32/cosβ, r33/cosβ)我的经验：在ROS中，用 tf::Matrix3x3 或 Eigen::Matrix3d 处理旋转矩阵，它们都内置了 getRPY() 方法。但底层原理就是我上面写的这些。理解原理，你才能知道什么时候该用、什么时候不该用。
2.7 本章小结
这一章我们讲了三个核心工具：
- 旋转矩阵：数学上完美，适合计算，但不直观
- 欧拉角：直观好理解，但有万向锁问题
- 齐次坐标：把旋转和平移统一成4x4矩阵，方便链式变换
说实话，这些内容看起来简单，但真正用好需要大量练习。我建议你写个小程序，随机生成一个旋转矩阵，然后转成欧拉角，再转回来，看看能不能恢复原样。这个练习能帮你发现很多细节问题。
下一章我们会深入讲四元数——它比欧拉角更稳定，比旋转矩阵更紧凑，是机器人领域的“瑞士军刀”。