第2章 坐标系与位姿描述：齐次变换矩阵、旋转矩阵、刚体运动学基础
各位搞机器人运动控制的朋友，咱们今天聊聊坐标系和位姿描述。说实话，这章内容是整个轨迹规划的基石。你想想看，机器人要动，首先得知道自己在哪、要去哪、怎么去。这些问题的答案，全藏在坐标系和位姿描述里。
我刚开始做机器人项目时，觉得这玩意儿不就是数学嘛，套公式就行。结果第一次调试六轴机器人，末端执行器死活对不准目标点。折腾了两天，最后发现是旋转矩阵用错了顺序。嗯，从那以后我再也不敢小看这章内容了。
2.1 为什么需要坐标系？
机器人运动控制，说白了就是描述物体在空间中的位置和朝向。位置好办，三个坐标(x,y,z)搞定。但朝向呢？光说“朝东偏上30度”可不行，得用数学语言精确描述。
我个人习惯把坐标系分成三类：
- 世界坐标系：固定在地面，所有运动都参考它
- 工具坐标系：固定在机器人末端，焊枪、抓手都挂在这
- 工件坐标系：固定在工件上，加工路径相对它定义
这三者之间的转换，就是咱们今天要啃的硬骨头。
2.2 齐次变换矩阵：把位置和朝向打包
齐次变换矩阵是个4×4的矩阵，它把旋转和位移打包在一起。结构长这样：
| R11 R12 R13 tx |
| R21 R22 R23 ty |
| R31 R32 R33 tz |
| 0 0 0 1 |左上角3×3是旋转矩阵R，右上角3×1是位移向量t。最后一行固定是[0 0 0 1]，这是齐次坐标的规矩。
核心公式：
新坐标 = 变换矩阵 × 原坐标
说白了就是：
[P_new] = [R t; 0 1] × [P_old]
我在项目中遇到过一个问题：两个机器人协同搬运，需要知道彼此的相对位姿。用齐次变换矩阵，一个矩阵乘法就搞定了。要是没有这个工具，你得分别算旋转和位移，容易出错。
2.3 旋转矩阵的三种表达方式
旋转矩阵有9个元素，但自由度只有3个。所以实际工程中，我们不会直接存9个数，而是用更紧凑的方式表达。常用的有三种：
| 表达方式 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| RPY角 | 3个 | 直观，容易理解 | 有万向锁问题 |
| 欧拉角 | 3个 | 计算简单 | 顺序依赖，有奇异性 |
| 四元数 | 4个 | 无奇异性，适合插值 | 不够直观 |
2.3.1 RPY角（滚转-俯仰-偏航）
RPY角是工程中最常用的方式。想象一架飞机：
- 滚转(Roll)：绕X轴旋转，像飞机侧翻
- 俯仰(Pitch)：绕Y轴旋转，像飞机抬头低头
- 偏航(Yaw)：绕Z轴旋转，像飞机转向
对应的旋转矩阵是三个基本旋转矩阵的乘积：
R = Rz(Yaw) × Ry(Pitch) × Rx(Roll)注意顺序！我当年就是搞反了顺序，导致机器人末端画了个奇怪的弧线。你想想看，先绕X轴转30度，再绕Y轴转45度，和反过来操作，结果完全不同。
避坑指南：我曾经在写轨迹规划代码时，RPY角的顺序搞错了。结果机器人走直线时，末端姿态一直在抖动。排查了两天才发现，是旋转矩阵乘法的顺序反了。记住：RPY的乘法顺序是Z→Y→X，从右往左读。
2.3.2 欧拉角
欧拉角和RPY类似，但旋转顺序不同。常见的欧拉角顺序有：
- ZYX：先绕Z，再绕Y，最后绕X
- ZYZ：先绕Z，再绕Y，最后绕Z
- XYZ：先绕X，再绕Y，最后绕Z
不同的顺序对应不同的应用场景。我个人习惯用ZYX顺序，因为和RPY的转换最直接。
欧拉角有个大问题——万向锁。当第二个旋转角达到±90度时，第一个和第三个旋转轴会重合，丢失一个自由度。这在机器人运动规划中是致命的。
2.3.3 四元数
四元数是我最推荐的旋转表达方式。它没有奇异性，而且做插值时非常平滑。四元数长这样：
q = w + xi + yj + zk其中w是实部，(x,y,z)是虚部。单位四元数满足w² + x² + y² + z² = 1。
从旋转轴和角度构造四元数：
w = cos(θ/2)
x = ax × sin(θ/2)
y = ay × sin(θ/2)
z = az × sin(θ/2)其中(ax, ay, az)是单位旋转轴，θ是旋转角度。
实用技巧：在做轨迹插值时，千万别直接用欧拉角做线性插值。我试过，结果末端姿态会乱跳。用四元数的球面线性插值(SLERP)，平滑得很。代码实现也就十几行，值得你花时间封装一下。
2.4 刚体运动学基础
刚体运动学，说白了就是研究物体怎么动，但不考虑力。核心概念就两个：
- 位姿：位置+朝向，用齐次变换矩阵表示
- 速度：线速度+角速度，用旋量表示
刚体运动的基本公式：
P_new = R × P_old + t其中R是旋转矩阵，t是位移向量。这个公式描述了刚体上任意一点在旋转和平移后的新位置。
我做过一个项目，需要在机器人末端装一个视觉相机。相机坐标系和工具坐标系之间有固定的偏移。用齐次变换矩阵，一次乘法就把相机看到的点转换到机器人基坐标系下了。要是没有这个工具，你得写一堆三角函数，还容易算错。
2.5 知识体系总览
下面这张图把本章的核心内容串起来了。你看着它，就能明白坐标系、旋转矩阵、齐次变换矩阵之间的关系。
2.6 实际工程中的选择建议
说了这么多理论，到底该用哪种？我根据多年经验，给你几个建议：
- 人机交互界面：用RPY角，操作员看得懂
- 轨迹插值计算：用四元数，平滑无奇异性
- 底层运动学计算：用齐次变换矩阵，统一处理
- 传感器数据融合：用四元数，避免万向锁
我的习惯：在代码里，我通常把四元数作为内部存储格式，只在需要显示或人工输入时转成RPY角。这样既保证了计算稳定性，又方便调试。你可以在工具类里封装好转换函数，一劳永逸。
好了，坐标系和位姿描述就聊到这儿。这些内容看着简单，但真用起来坑不少。建议你动手写几个小例子，比如把RPY转成四元数，再用四元数做插值，跑一遍看看效果。实践出真知嘛。