3、刚体变换与位姿表示：旋转矩阵、欧拉角、四元数、齐次变换矩阵

各位，这一章咱们聊聊机器人坐标标定的数学基础——刚体变换。说白了，就是描述一个物体在三维空间里怎么转、怎么挪。我刚开始接触这行时，觉得不就是旋转和平移嘛，有啥难的？结果第一次做手眼标定，坐标系一多，直接把自己绕晕了。

后来我总结了一句话：位姿表示是3D视觉的“语法”，语法不通，后面的算法全白搭。今天咱们就把旋转矩阵、欧拉角、四元数、齐次变换矩阵这四兄弟彻底捋清楚。

3.1 旋转矩阵——最直观，但也最“重”

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

举个例子，绕Z轴旋转θ角，旋转矩阵长这样：

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

绕X轴和Y轴的类似，我就不列了。关键是它的性质：

• 正交性：R^T = R^{-1}，逆矩阵等于转置，计算方便
• 组合性：连续旋转就是矩阵乘法，R = R_2 · R_1
• 缺点：9个参数，冗余；插值困难；容易受数值误差影响不再是正交矩阵

3.2 欧拉角——直观但小心“万向锁”

欧拉角用三个角度表示旋转：绕X、Y、Z轴的转角，常用ZYX顺序（偏航-俯仰-滚转）。

比如一个无人机：

• 偏航角（Yaw）：绕Z轴转，决定机头朝向
• 俯仰角（Pitch）：绕Y轴转，决定抬头低头
• 滚转角（Roll）：绕X轴转，决定左右倾斜

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

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

其中α是滚转，β是俯仰，γ是偏航。

欧拉角的优点：直观，3个参数，占用内存小。缺点：万向锁、插值不自然、不同顺序定义混乱。

3.3 四元数——工业界的“真香”选择

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

单位四元数（模长为1）可以表示任意旋转：

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

其中u是旋转轴单位向量，θ是旋转角。

四元数的好处：

• 无万向锁——这是我最看重的
• 插值平滑——球面线性插值（SLERP）让姿态过渡自然
• 紧凑——4个参数，比旋转矩阵少
• 数值稳定——归一化就能修正误差

四元数转旋转矩阵的公式（我经常用这个做接口转换）：

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²) |

其中q = (w, x, y, z)。

3.4 齐次变换矩阵——把旋转和平移装进一个“盒子”

齐次变换矩阵是个4×4矩阵：

T = | R   t |
    | 0   1 |

其中R是3×3旋转矩阵，t是3×1平移向量。它把旋转和平移统一成一个线性变换。

用齐次坐标表示点：p = [x, y, z, 1]^T，变换就是矩阵乘法：

p' = T · p

为什么用齐次形式？因为连续变换可以写成矩阵连乘：

T_total = T_n · T_{n-1} · ... · T_1

这在机器人运动学里太重要了——从基座到末端执行器，就是一连串的齐次变换矩阵相乘。

3.5 四种表示方法的对比

3.6 知识体系总览

下面这张图把四种表示方法的关系和转换路径画清楚了。我建议你把它存下来，做标定遇到姿态问题时先看这张图定位。

3.7 实际项目中的选择建议

说了这么多理论，到底该用哪个？我根据这些年做项目的经验，给你几个实用建议：

1. 做姿态优化（比如手眼标定、BA优化）：用四元数。4个参数，无约束，优化稳定。我习惯用Eigen的Quaterniond类。
2. 做运动学正解（比如机械臂DH参数）：用齐次变换矩阵。连乘方便，一步到位。
3. 做可视化或人机交互：用欧拉角。操作员看得懂，调参直观。但记得加个万向锁检测。
4. 做最终结果输出或接口对接：用旋转矩阵。很多库（OpenCV、PCL）原生支持。

3.8 代码片段：四种表示互转（C++ / Eigen）

下面是我项目里常用的转换工具函数，你直接拿去用：

#include <Eigen/Dense>
#include <Eigen/Geometry>

// 1. 旋转矩阵 → 四元数
Eigen::Quaterniond rotMat2Quat(const Eigen::Matrix3d& R) {
    return Eigen::Quaterniond(R);
}

// 2. 四元数 → 旋转矩阵
Eigen::Matrix3d quat2RotMat(const Eigen::Quaterniond& q) {
    return q.toRotationMatrix();
}

// 3. 旋转矩阵 → 欧拉角 (ZYX顺序)
Eigen::Vector3d rotMat2Euler(const Eigen::Matrix3d& R) {
    return R.eulerAngles(2, 1, 0); // ZYX: 偏航, 俯仰, 滚转
}

// 4. 欧拉角 → 旋转矩阵 (ZYX顺序)
Eigen::Matrix3d euler2RotMat(double yaw, double pitch, double roll) {
    Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
    Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
    return (yawAngle * pitchAngle * rollAngle).toRotationMatrix();
}

// 5. 旋转矩阵 + 平移 → 齐次变换矩阵
Eigen::Matrix4d rotTrans2Hom(const Eigen::Matrix3d& R, const Eigen::Vector3d& t) {
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = t;
    return T;
}

// 6. 齐次变换矩阵 → 旋转矩阵 + 平移
void hom2RotTrans(const Eigen::Matrix4d& T, Eigen::Matrix3d& R, Eigen::Vector3d& t) {
    R = T.block<3,3>(0,0);
    t = T.block<3,1>(0,3);
}