刚体运动学基础：坐标系与位姿表示

各位同学，欢迎来到第二章。这一章的内容，说白了就是机器人学的“语言”。你想想看，要让机器人动起来，你总得告诉它“往哪动”、“怎么转”吧？这些信息怎么描述？靠的就是坐标系、旋转矩阵、四元数这些东西。

我个人习惯把这一章叫做“地基”。地基打不牢，后面做运动规划、动力学控制，甚至训练强化学习策略，都会出各种奇怪的bug。我当年刚入行时，就因为搞混了欧拉角的旋转顺序，让一个六轴机械臂在调试现场差点撞了人……嗯，从那以后，我再也不敢轻视这些基础概念了。

2.1 坐标系与位姿的基本概念

我们先从最直观的说起。你在房间里，想告诉朋友“杯子在桌子上”。这句话里隐含了两个信息：杯子相对于桌子的位置，以及杯子的朝向（杯口朝上还是朝下）。在机器人学里，我们把“位置+朝向”统称为位姿。

描述位姿，必须先有一个参考系。这个参考系就是坐标系。通常我们用右手定则建立笛卡尔坐标系：大拇指指向X轴，食指指向Y轴，中指指向Z轴。

• 世界坐标系：固定在地面上的全局参考系，所有运动都以此为基准。
• 机体坐标系：固定在机器人本体上的坐标系，随机器人移动而移动。
• 工具坐标系：固定在机器人末端执行器（比如夹爪、焊枪）上的坐标系。

我在做移动机器人项目时，经常需要在这三个坐标系之间来回转换。比如，激光雷达检测到前方有障碍物，数据是在雷达坐标系下的，但控制指令需要转换到机体坐标系。这一步搞错了，机器人就会撞墙。

2.2 旋转矩阵：最直观的旋转描述

旋转矩阵是一个3×3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的旋转关系。

假设有一个点P，在世界坐标系下的坐标为p，在机体坐标系下的坐标为p'。那么：

p = R * p'

其中R就是旋转矩阵。它的每一列，其实就是机体坐标系的三个轴在世界坐标系下的方向向量。

绕X、Y、Z轴旋转的基本旋转矩阵长这样：

R_x(θ) = [1      0       0    ]
         [0   cosθ   -sinθ   ]
         [0   sinθ    cosθ   ]

R_y(θ) = [cosθ    0    sinθ  ]
         [0       1      0    ]
         [-sinθ   0    cosθ  ]

R_z(θ) = [cosθ  -sinθ    0   ]
         [sinθ   cosθ    0   ]
         [0       0      1   ]

旋转矩阵有个好处：多个旋转可以连乘。比如先绕X轴转α，再绕Y轴转β，再绕Z轴转γ，总的旋转矩阵就是：

R = R_z(γ) * R_y(β) * R_x(α)

注意顺序！矩阵乘法不满足交换律，先转后转结果完全不同。我见过太多新手在这里栽跟头了。

2.3 欧拉角：直观但容易踩坑

欧拉角用三个角度来描述旋转，比如(roll, pitch, yaw)，分别对应绕X、Y、Z轴的旋转。直观吧？但坑也在这里。

欧拉角有两大问题：

1. 万向锁：当pitch角达到±90°时，roll和yaw的旋转轴会重合，丢失一个自由度。这时候你无论怎么转roll或yaw，效果都一样。
2. 旋转顺序歧义：同样的三个角度，按XYZ顺序和按ZYX顺序，结果完全不同。行业内常用的顺序有ZYX、XYZ、ZYZ等，必须约定清楚。

欧拉角到旋转矩阵的转换公式（以ZYX顺序为例）：

R = R_z(yaw) * R_y(pitch) * R_x(roll)

展开后：

R = [cos(y)cos(p)  cos(y)sin(p)sin(r)-sin(y)cos(r)  cos(y)sin(p)cos(r)+sin(y)sin(r)]
    [sin(y)cos(p)  sin(y)sin(p)sin(r)+cos(y)cos(r)  sin(y)sin(p)cos(r)-cos(y)sin(r)]
    [-sin(p)       cos(p)sin(r)                       cos(p)cos(r)                  ]

其中r=roll, p=pitch, y=yaw。

2.4 四元数：高效且无奇点

四元数是一个超复数，形式为 q = w + xi + yj + zk，其中w是实部，(x,y,z)是虚部。它用四个参数表示旋转，没有万向锁问题，而且插值平滑。

四元数表示绕单位向量 u = (u_x, u_y, u_z) 旋转角度θ：

q = [cos(θ/2), u_x*sin(θ/2), u_y*sin(θ/2), u_z*sin(θ/2)]

四元数的乘法：

q1 * q2 = [w1*w2 - x1*x2 - y1*y2 - z1*z2,
           w1*x2 + x1*w2 + y1*z2 - z1*y2,
           w1*y2 - x1*z2 + y1*w2 + z1*x2,
           w1*z2 + x1*y2 - y1*x2 + z1*w2]

用四元数旋转一个向量 v：

v' = q * v * q_conjugate

其中q_conjugate = [w, -x, -y, -z]。

2.5 齐次变换矩阵：统一平移与旋转

旋转矩阵只能描述旋转，不能描述平移。齐次变换矩阵把两者统一成一个4×4矩阵：

T = [R   t]
    [0   1]

其中R是3×3旋转矩阵，t是3×1平移向量。最后一行是[0, 0, 0, 1]。

用齐次坐标表示点P：

P_homogeneous = [x, y, z, 1]^T

变换后的点：

P' = T * P

多个变换可以连乘：

T_total = T1 * T2 * T3

这其实就是机器人运动学中的链式法则。每个关节的变换矩阵连乘，就得到了末端执行器相对于基座的位姿。

2.6 知识体系总览

下面这张图总结了本章的核心知识结构，我建议你把它存下来，后面学完整个课程再回来看，会有更深的理解。

2.7 各表示方法的对比

为了方便你选择，我把四种姿态表示方法整理成了表格：

好了，这一章的内容就到这里。这些概念看起来有点抽象，但相信我，后面做运动学建模、轨迹规划、甚至训练强化学习策略时，你会无数次用到它们。建议你动手写几行代码，把旋转矩阵、欧拉角、四元数之间的转换跑一遍，感受一下它们之间的差异。