3. 姿态表示方法

做抓取规划，第一个绕不开的问题就是：怎么描述一个物体在空间里的朝向？

说白了，就是姿态表示。我见过不少新手，代码跑飞了，查半天发现是姿态表示没选对，或者转换时出了bug。今天咱们就把四种主流方法掰开揉碎讲清楚。

3.1 旋转矩阵（Rotation Matrix）

旋转矩阵是个3x3的方阵，每一列代表旋转后的坐标轴在原坐标系下的投影。它最直观，也最“笨重”。

优点：

• 无奇异性： 任何姿态都能用一个唯一的旋转矩阵表示，不会像欧拉角那样“卡住”。
• 组合方便： 连续旋转就是矩阵乘法，R_总 = R₂ · R₁，代码写起来很直接。
• 适合插值？ 其实不太适合，但做坐标变换时非常稳。

缺点：

• 冗余： 9个元素描述3个自由度，浪费内存和计算。
• 必须正交化： 数值误差会让矩阵慢慢“漂移”，不再是合法旋转矩阵。我早期做手眼标定时就吃过这个亏，迭代几次后矩阵行列式变成0.98，结果全歪了。
• 不直观： 你看着一堆数字，很难想象物体到底转成了什么样子。

3.2 欧拉角（Euler Angles）

欧拉角用三个角度（比如绕X、Y、Z轴的转角）来描述姿态。最常见的是ZYX顺序，也就是先绕Z转yaw，再绕Y转pitch，最后绕X转roll。

优点：

• 直观： 你一说“绕Z转30度”，谁都能听懂。调试时打印欧拉角，一眼就知道机械臂当前姿态对不对。
• 紧凑： 3个数字，存储和传输都省。

缺点：

• 万向锁（Gimbal Lock）： 当pitch接近±90°时，yaw和roll的旋转轴会重合，丢失一个自由度。我记得有一次做抓取仿真，物体突然“卡住”转不动，查了半天发现是欧拉角pitch到了89.9度。
• 顺序依赖： ZYX、XYZ、ZYZ……不同顺序结果完全不同。团队协作时没约定好，代码一对接就崩。
• 插值困难： 直接对三个角度做线性插值，路径会非常诡异，物体在空中乱翻。

3.3 四元数（Quaternion）

四元数是个超复数，q = w + xi + yj + zk，其中w是实部，x、y、z是虚部，且满足 w² + x² + y² + z² = 1。

说实话，我刚接触四元数时也觉得它很“玄学”。但用久了你会发现，它才是工程上的最优解。

优点：

• 无奇异性： 任何姿态都能表示，没有万向锁。
• 紧凑： 4个数字，比旋转矩阵少一半多。
• 插值平滑： 球面线性插值（SLERP）能生成最短路径的旋转，抓取路径规划时特别好用。
• 组合高效： 两个四元数相乘就能合成旋转，计算量比矩阵小。

缺点：

• 不直观： 你看着 (0.707, 0, 0.707, 0)，能想象出物体转了多少吗？反正我不能。
• 双值性： q 和 -q 表示同一个旋转。做优化时如果不约束符号，可能会在两个等价解之间来回跳。
• 必须归一化： 数值误差会让模长偏离1，需要定期归一化。

3.4 轴角表示法（Axis-Angle）

轴角法用一个单位向量 u 表示旋转轴，一个标量 θ 表示旋转角度。形式上就是 (u_x, u_y, u_z, θ)。

优点：

• 最直观的几何意义： 绕哪个轴转多少度，一清二楚。
• 紧凑： 4个数字，和四元数一样。
• 适合优化： 在流形优化中，轴角是李代数 so(3) 的表示，做梯度下降非常自然。

缺点：

• 奇异性： 当 θ = 0 时，旋转轴 u 没有定义。任何方向都可以，导致数值不稳定。
• 插值困难： 直接对 θ 做线性插值，路径不是最短的。
• 组合复杂： 两个轴角组合需要先转成矩阵或四元数，算完再转回来，麻烦。

3.5 四种方法对比总结

特性	旋转矩阵	欧拉角	四元数	轴角
元素个数	9	3	4	4
奇异性	无	有（万向锁）	无	有（θ=0）
直观性	差	好	差	好
插值质量	差	差	好（SLERP）	差
组合效率	中（9次乘法）	低	高（16次乘法）	低
存储开销	大	小	中	中
工程推荐度	★☆☆☆☆	★★☆☆☆	★★★★★	★★★☆☆

3.6 知识体系结构图

下面这张图帮你理清四种姿态表示方法之间的关系，以及在实际抓取流程中怎么选。

3.7 实战中的选择策略

说了这么多，到底怎么选？我总结了一个简单的决策流程：

1. 如果你在写人机交互界面： 用欧拉角。操作员看着yaw/pitch/roll就能理解。
2. 如果你在做姿态存储和传递： 用四元数。紧凑、无奇异、插值方便。
3. 如果你在做底层坐标变换： 用旋转矩阵。乘法直接，配合齐次矩阵用起来很顺手。
4. 如果你在做姿态优化（比如抓取姿态微调）： 用轴角。增量式更新，天然适合梯度下降。