📚 课程目录第3章位姿估计基础

3、位姿估计基础：刚体变换、旋转表示与坐标系转换

位姿估计，说白了就是回答两个问题：「物体在哪儿」和「它朝向哪边」。在机器人抓取里，这两个问题直接决定了机械臂能不能准确地把手伸过去、抓起来。

我个人习惯把位姿估计拆成三块来理解：刚体变换描述位置和姿态的整体变化，旋转表示是描述「转了多少」的数学工具，坐标系转换则是把这些变换在不同参考系之间倒来倒去。这三块是后面所有抓取策略的基础，绕不开。

  核心观点：位姿估计的本质，就是找到从「传感器坐标系」到「物体坐标系」的刚体变换矩阵。抓取规划的所有后续步骤，都建立在这个变换的准确性之上。

3.1 刚体变换：位置 + 姿态 = 完整位姿

刚体变换，就是物体在三维空间里平移加旋转后的总效果。它不改变物体的形状和大小——这也是「刚体」这个名字的由来。

数学上，一个刚体变换可以写成：

T = [R  t]
    [0  1]

其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量。这个 4×4 的齐次变换矩阵，是机器人学里最常用的工具之一。

我记得刚入行时，有个项目里机械臂抓取精度总差 2-3mm。查了半天，发现是变换矩阵里平移向量的单位搞混了——传感器输出是毫米，控制器用的是米。嗯，这种低级错误，犯过一次就再也不会忘了。

变换类型	数学表示	自由度	典型应用
纯平移	t = [x, y, z]^T	3	物体位置移动
纯旋转	R ∈ SO(3)	3	物体朝向变化
刚体变换	T ∈ SE(3)	6	完整位姿描述

小技巧：在实际代码里，我习惯用齐次坐标统一处理。把三维点 [x, y, z] 写成 [x, y, z, 1]^T，这样平移和旋转就能用一个矩阵乘法搞定，不用分开算。

3.2 旋转表示：三种主流方式，各有各的坑

旋转表示是位姿估计里最容易出问题的地方。你想想看，三维空间里的旋转只有 3 个自由度，但表示方式却有十几种。我常用的就三种：欧拉角、旋转矩阵和四元数。

3.2.1 欧拉角：直观但容易「万向锁」

欧拉角用三个角度表示旋转，比如绕 X、Y、Z 轴依次转多少度。直观，好理解，调试时经常用。

但有个大坑——万向锁。当中间轴旋转到 ±90° 时，会丢失一个自由度，导致姿态突变。我曾经在调一个六轴机械臂的抓取姿态时，欧拉角突然跳变，机械臂差点撞到旁边的夹具。从那以后，我在内部计算时再也不用欧拉角了。

// 欧拉角转旋转矩阵（ZYX顺序）
// 注意：顺序不同，结果完全不同
Eigen::Matrix3d eulerToMatrix(double roll, double pitch, double yaw) {
    Eigen::Matrix3d R;
    R = Eigen::AngleAxisd(yaw,   Eigen::Vector3d::UnitZ())
      * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
      * Eigen::AngleAxisd(roll,  Eigen::Vector3d::UnitX());
    return R;
}

避坑指南：欧拉角只适合人机交互和可视化调试。在内部计算、插值、优化时，千万别用欧拉角。我曾经因为欧拉角插值导致抓取轨迹出现「绕远路」的诡异现象，排查了两天才发现是角度跳变问题。

3.2.2 旋转矩阵：稳定但冗余

旋转矩阵是 3×3 的正交矩阵，行列式为 +1。它没有奇异性，计算稳定，是很多算法库的底层表示。

缺点是冗余——9 个参数描述 3 个自由度，而且必须满足正交约束。每次更新后还得重新正交化，否则误差会累积。

// 旋转矩阵正交化（SVD方法）
Eigen::Matrix3d orthogonalize(const Eigen::Matrix3d& R) {
    Eigen::JacobiSVD svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
    return svd.matrixU() * svd.matrixV().transpose();
}

3.2.3 四元数：我的首选

四元数用 4 个参数表示旋转，没有奇异性，插值平滑，计算效率高。我个人强烈推荐在所有内部计算中使用四元数。

四元数 q = [w, x, y, z]，其中 w 是实部，(x, y, z) 是虚部。单位四元数满足 w² + x² + y² + z² = 1。

// 四元数插值（SLERP）
Eigen::Quaterniond slerp(const Eigen::Quaterniond& q1, 
                          const Eigen::Quaterniond& q2, 
                          double t) {
    return q1.slerp(t, q2);
}

表示方式	参数数量	奇异性	插值	计算效率	推荐场景
欧拉角	3	有（万向锁）	差	高	人机交互、调试
旋转矩阵	9	无	一般	中	底层计算、库接口
四元数	4	无	优秀	高	内部计算、优化、滤波

3.3 坐标系转换：把一切对齐到同一个参考系

在抓取场景里，你至少会遇到三个坐标系：相机坐标系、机械臂基坐标系和物体坐标系。位姿估计的核心任务之一，就是把物体在相机坐标系下的位姿，转换到机械臂坐标系下。

坐标系转换的公式很简单：

P_base = T_base_camera * P_camera

其中 T_base_camera 是相机到机械臂基座的变换矩阵，通常通过手眼标定得到。

这里有个容易忽略的点：变换的链式法则。比如你要从物体坐标系到世界坐标系，中间可能经过好几层变换：

T_world_object = T_world_camera * T_camera_object

我建议在代码里维护一个变换树，把所有坐标系的关系组织成树状结构。每次需要转换时，沿着树找到路径，连乘即可。

工程实践：在 ROS 里，tf2 库就是干这个的。如果你自己写代码，建议用 std::map<std::pair<string, string>, Eigen::Isometry3d> 来维护变换关系，清晰又高效。

3.4 知识结构总览

下面这张图把本章的核心知识点串起来了。你可以把它当作位姿估计的「思维导图」——每次遇到位姿相关的问题，先看看自己卡在哪一层。

3.5 本章小结

位姿估计的基础，说白了就是三件事：

刚体变换：用 4×4 齐次矩阵统一描述位置和姿态的变化
旋转表示：欧拉角用于调试，旋转矩阵用于接口，四元数用于内部计算——各司其职
坐标系转换：通过变换树和链式法则，把不同传感器和机械臂的坐标系对齐

我个人建议，刚开始接触位姿估计的同学，先把四元数和齐次变换矩阵练熟。这两个工具用好了，后面学手眼标定、抓取位姿规划都会顺畅很多。