3. 坐标系与位姿描述：刚体位姿描述、旋转矩阵、齐次变换矩阵、欧拉角与四元数

各位同学，欢迎来到第三章。说实话，这一章是整个运动控制轨迹规划的“地基”。你想想看，机器人要动，首先得知道自己在哪、胳膊往哪伸、末端抓手的姿态对不对。这些问题的核心，就是坐标系与位姿描述。

我刚开始做机器人项目那会儿，觉得坐标系不就是个数学概念嘛，随便搞搞就行。结果有一次调试六轴机械臂，因为旋转顺序搞反了，末端直接撞上了工作台。嗯，从那以后，我再也不敢小看这章内容了。

3.1 刚体位姿描述：位置 + 姿态

刚体，说白了就是一个不会变形的物体。比如机械臂的连杆、末端执行器，都是刚体。描述刚体在空间中的状态，需要两个信息：位置和姿态。

• 位置：刚体上某个参考点（通常是质心或坐标系原点）在参考坐标系中的坐标，用 (x, y, z) 表示。
• 姿态：刚体相对于参考坐标系的朝向，也就是它“转”到了哪个方向。

我习惯把位姿想象成“你站在哪 + 你面朝哪”。位置好理解，姿态才是容易出坑的地方。

3.2 旋转矩阵：姿态的数学表达

旋转矩阵是描述姿态最直接的方式。它是一个 3×3 的正交矩阵，列向量表示刚体坐标系各轴在参考坐标系中的方向。

举个例子，假设刚体坐标系绕 Z 轴旋转 θ 角度，旋转矩阵长这样：

R_z(θ) = | cosθ  -sinθ  0 |
         | sinθ   cosθ  0 |
         | 0      0     1 |

同理，绕 X 轴和 Y 轴的旋转矩阵：

R_x(θ) = | 1   0      0    |
         | 0  cosθ  -sinθ |
         | 0  sinθ   cosθ |

R_y(θ) = | cosθ  0  sinθ |
         | 0     1  0    |
         | -sinθ 0  cosθ |

旋转矩阵有几个重要性质：

• 正交性：R^T = R^-1，且 det(R) = 1
• 组合旋转：多个旋转矩阵相乘，顺序很重要（先乘的离刚体近）
• 逆矩阵就是反向旋转

3.3 齐次变换矩阵：位置 + 姿态的统一

旋转矩阵只处理姿态，位置还得单独加个向量。为了统一，我们引入齐次变换矩阵。它是一个 4×4 矩阵：

T = | R   p |
    | 0   1 |

其中 R 是 3×3 旋转矩阵，p 是 3×1 位置向量。最后一行是 [0 0 0 1]。

齐次变换矩阵的好处：

• 一次矩阵乘法，同时完成旋转和平移
• 可以串联多个变换：T_总 = T₁ * T₂ * T₃ ...
• 逆矩阵可以直接求，用于反向变换

举个例子，从基坐标系到末端坐标系的变换：

T_base_to_end = | R  p |
                | 0  1 |

# 假设末端在基坐标系中的位置是 (0.5, 0.3, 0.2)，绕Z轴旋转30°
# 那么 T 就是：
# | cos30°  -sin30°  0  0.5 |
# | sin30°   cos30°  0  0.3 |
# | 0        0       1  0.2 |
# | 0        0       0  1   |

3.4 欧拉角：直观但有限制

旋转矩阵有9个元素，但真正独立的只有3个。欧拉角就是用3个角度来描述姿态，比如常见的 ZYX 顺序：先绕 Z 轴转 γ，再绕 Y 轴转 β，最后绕 X 轴转 α。

欧拉角的优点：直观，容易理解。缺点也很明显：

• 万向锁问题：当 β = ±90° 时，α 和 γ 的旋转轴重合，丢失一个自由度
• 多值性：同一个姿态对应多组欧拉角
• 插值困难：直接对角度线性插值，路径可能不自然

3.5 四元数：平滑旋转的利器

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

四元数的核心操作：

• 单位四元数：w² + x² + y² + z² = 1，表示纯旋转
• 旋转向量转四元数：绕单位向量 u 旋转 θ 角，q = (cos(θ/2), u*sin(θ/2))
• 四元数乘法：q_总 = q₁ * q₂，表示先转 q₂ 再转 q₁
• 球面线性插值（Slerp）：在两个四元数之间平滑过渡

四元数和旋转矩阵可以互相转换。我个人习惯在轨迹规划中用四元数做插值，在运动学计算中转回旋转矩阵。

3.6 三种姿态表示方法的对比

3.7 实战建议：如何选择

说了这么多，到底用哪个？我的经验是这样的：

• 做运动学正解/逆解：用齐次变换矩阵，4×4 矩阵乘法一气呵成
• 做轨迹规划：用四元数做姿态插值，避免欧拉角的跳变
• 调试和显示：用欧拉角，方便人眼观察和理解
• 传感器融合：用四元数，避免万向锁，适合滤波算法

好了，这一章的内容就到这里。坐标系和位姿描述是运动控制的基石，理解透了，后面的轨迹规划才能走得稳。如果你在项目中遇到位姿相关的问题，欢迎随时交流。