3. 位姿描述：位置矢量、姿态矩阵、欧拉角、四元数的数学表示与转换
做机器人控制这么多年，我越来越觉得一个道理：你描述机器人位置和姿态的方式，直接决定了你写代码的效率和调试的体验。说白了，位姿描述就是机器人的「身份证」和「导航仪」。
今天咱们就聊聊这四种最常见的描述方式：位置矢量、姿态矩阵、欧拉角、四元数。我会结合我踩过的坑，把它们的数学表示和转换逻辑讲透。
3.1 位置矢量：最直观的坐标
位置矢量，说白了就是告诉你「机器人的手在哪儿」。用三个坐标值就能搞定：
P = [x, y, z]^T这个很简单，我在调试六轴机器人时，经常用激光跟踪仪测末端位置，直接读三个坐标值。但要注意：坐标系的选择。我记得有一次，我把基坐标系和工具坐标系搞混了，结果机器人撞了夹具……嗯，从那以后我每次都会在代码里显式声明坐标系。
3.2 姿态矩阵：旋转的完整描述
光知道位置还不够，你还得知道机器人末端「朝哪个方向」。姿态矩阵（也叫旋转矩阵）就是干这个的。
它是一个 3×3 的正交矩阵：
R = [r11 r12 r13;
r21 r22 r23;
r31 r32 r33]满足两个条件：R^T R = I 且 det(R) = +1。
我个人习惯用姿态矩阵做底层计算，因为它没有奇点问题。但缺点也很明显——9个参数，冗余度太高。你想想看，明明3个自由度就能描述旋转，却要用9个数，这在实际工程中容易引入数值误差。
3.3 欧拉角：直观但暗藏陷阱
欧拉角是工程师最常用的姿态描述方式。它用三个角度来描述旋转：
- ZYX 顺序（最常用）：先绕 Z 轴转 γ（偏航），再绕 Y 轴转 β（俯仰），最后绕 X 轴转 α（滚转）
- ZYZ 顺序：常用于机器人运动学
数学表示如下（以 ZYX 为例）：
R = Rz(γ) * Ry(β) * Rx(α)其中：
Rz(γ) = [cosγ -sinγ 0;
sinγ cosγ 0;
0 0 1]
Ry(β) = [cosβ 0 sinβ;
0 1 0;
-sinβ 0 cosβ]
Rx(α) = [1 0 0;
0 cosα -sinα;
0 sinα cosα]欧拉角最大的好处是直观。我在示教器上调整姿态时，直接调三个角度值，非常方便。
3.4 四元数：插值的王者
四元数，说白了就是「带约束的复数」。它用四个数来描述旋转：
q = w + xi + yj + zk或者写成向量形式：
q = [w, x, y, z]^T满足归一化条件：w² + x² + y² + z² = 1
四元数最大的优势是：可以做平滑的球面插值（SLERP）。我在做机器人轨迹规划时，几乎都用四元数做姿态插值，因为它不会出现欧拉角的突变问题。
q1 * q2 = [w1*w2 - v1·v2, w1*v2 + w2*v1 + v1×v2]
3.5 四种描述方式的转换
在实际工程中，我们经常需要在四种描述方式之间来回切换。下面是我整理的转换公式：
3.5.1 姿态矩阵 → 欧拉角（ZYX）
β = atan2(-r31, sqrt(r11² + r21²))
α = atan2(r21/cosβ, r11/cosβ)
γ = atan2(r32/cosβ, r33/cosβ)注意：当 cosβ ≈ 0 时，就是万向锁情况，需要特殊处理。
3.5.2 欧拉角 → 四元数
w = cos(α/2)cos(β/2)cos(γ/2) + sin(α/2)sin(β/2)sin(γ/2)
x = sin(α/2)cos(β/2)cos(γ/2) - cos(α/2)sin(β/2)sin(γ/2)
y = cos(α/2)sin(β/2)cos(γ/2) + sin(α/2)cos(β/2)sin(γ/2)
z = cos(α/2)cos(β/2)sin(γ/2) - sin(α/2)sin(β/2)cos(γ/2)3.5.3 四元数 → 姿态矩阵
R = [1-2(y²+z²) 2(xy-wz) 2(xz+wy);
2(xy+wz) 1-2(x²+z²) 2(yz-wx);
2(xz-wy) 2(yz+wx) 1-2(x²+y²)]3.6 知识体系结构图
下面这张图，是我自己总结的位姿描述知识体系。你可以把它当作一个「导航地图」：
3.7 实际工程中的选择建议
说了这么多理论，最后给点实在的建议：
| 应用场景 | 推荐描述方式 | 原因 |
|---|---|---|
| 示教器手动调整 | 欧拉角 | 直观，三个角度值一目了然 |
| 轨迹插值计算 | 四元数 | 平滑无奇点，SLERP插值效果好 |
| 坐标变换计算 | 姿态矩阵 | 矩阵乘法方便，适合链式变换 |
| 传感器数据输出 | 位置矢量 + 四元数 | 紧凑且无歧义 |