📚 课程目录第3章坐标系与运动学

3. 坐标系与运动学

各位同学，欢迎来到第三章。这一章，我们聊聊传感器融合里最基础、也最绕不开的话题——坐标变换与空间描述。

说实话，我刚开始做多传感器融合的时候，觉得这玩意儿不就是矩阵乘来乘去嘛，有啥难的？结果第一次把IMU和视觉数据对齐，发现姿态怎么都对不上，折腾了两天才找到原因——欧拉角的万向锁问题。嗯，从那以后，我再也不敢小看坐标变换了。

你想想看，一个IMU装在无人机中心，一个摄像头装在机头，一个激光雷达装在机腹。它们各自看世界，坐标系都不一样。你要把数据融合到一起，就得先搞清楚：怎么描述一个物体在空间里的位置和姿态？怎么在不同坐标系之间来回切换？

这就是本章要解决的核心问题。

核心要点：刚体运动 = 位置（平移）+ 姿态（旋转）。描述姿态有三种主流方式：旋转矩阵、欧拉角、四元数。齐次变换矩阵把平移和旋转统一成一个4×4矩阵，方便连续变换。

3.1 刚体运动：位置 + 姿态

先问个问题：什么叫刚体？说白了，就是一个物体在运动过程中，内部任意两点之间的距离保持不变。比如你手里的手机、无人机机身、机器人手臂——这些都可以近似看作刚体。

刚体在三维空间里的运动，可以拆成两部分：

平移（位置）：刚体上某一点（通常取质心或原点）在空间中的坐标变化。用三维向量 t = [x, y, z]^T 表示。
旋转（姿态）：刚体绕自身坐标轴的转动。描述起来就比平移复杂多了。

我记得有个学生问我：老师，为什么不能直接用三个角度描述姿态，非要搞那么多方法？我的回答是：因为每种方法都有它的脾气。下面我们一个一个来看。

3.2 旋转矩阵：最直观，但最啰嗦

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

假设有一个点 p 在坐标系 {A} 中的坐标为 p_A，在坐标系 {B} 中的坐标为 p_B。如果 {B} 相对于 {A} 的旋转矩阵是 R_AB，那么：

p_A = R_AB * p_B

旋转矩阵的每一列，其实就是 {B} 坐标系的基向量在 {A} 坐标系下的投影。这个理解很重要，我在做视觉-惯性标定时，就是靠这个物理意义来检查标定结果是否合理。

旋转矩阵有9个元素，但自由度只有3。说白了，它冗余了。好处是直观、容易组合（矩阵乘法），坏处是计算量大，而且数值误差会让它不再是正交矩阵——需要不断重新正交化。

我的习惯：在需要多次连续旋转、或者要跟平移统一处理时，我用旋转矩阵。比如机器人运动学正解，用旋转矩阵链式乘法非常自然。但在做姿态估计、滤波时，我更喜欢四元数。

3.3 欧拉角：直观，但小心万向锁

欧拉角用三个角度来描述旋转：绕X轴（滚转roll）、绕Y轴（俯仰pitch）、绕Z轴（偏航yaw）。顺序很重要！常见的顺序有ZYX、ZYZ等。

举个例子，无人机姿态通常用roll-pitch-yaw（ZYX顺序）：

偏航角 ψ：绕Z轴旋转
俯仰角 θ：绕Y轴旋转
滚转角 φ：绕X轴旋转

欧拉角最大的优点是直观。你跟飞手说“偏航30度”，他马上能理解。但缺点也很致命——万向锁。

⚠️ 我曾经踩过的坑：在做无人机姿态控制时，俯仰角接近±90度时，偏航和滚转变得不可区分，控制直接炸了。这就是万向锁——中间那个轴（俯仰）转到±90度时，内外两个轴共面，丢失了一个自由度。

所以，如果你用欧拉角做姿态估计或控制，一定要限制俯仰角范围，或者干脆换四元数。

3.4 四元数：无奇点，适合插值

四元数是个好东西。它用四个数表示旋转：q = [w, x, y, z]^T，其中w是实部，[x, y, z]是虚部，且满足 w² + x² + y² + z² = 1（单位四元数）。

为什么用四元数？三个理由：

无万向锁：这是最大的优势。任何姿态都能用四元数唯一表示。
插值平滑：球面线性插值（SLERP）可以在两个姿态之间生成平滑过渡。做动画、做轨迹规划时特别好用。
计算效率高：比旋转矩阵少一个维度，组合旋转用四元数乘法也比矩阵乘法快。

四元数旋转公式（点 p 绕单位四元数 q 旋转）：

p' = q * p * q_conj

其中 p 是纯四元数 [0, x, y, z]^T，q_conj 是 q 的共轭。

我个人习惯在卡尔曼滤波的状态向量里用四元数表示姿态。但要注意——四元数必须归一化，否则滤波会发散。我见过有人忘了这步，结果姿态越跑越偏。

3.5 齐次变换矩阵：把平移和旋转装进一个矩阵

好了，现在我们有旋转（用矩阵或四元数），有平移（用向量）。能不能把它们统一起来？

能。齐次变换矩阵就是一个4×4的矩阵：

T = [ R   t ]
    [ 0   1 ]

其中 R 是3×3旋转矩阵，t 是3×1平移向量，最后一行是 [0, 0, 0, 1]。

用齐次坐标，一个点 p 的齐次表示为 [x, y, z, 1]^T。变换就是简单的矩阵乘法：

p_A = T_AB * p_B

连续变换可以链式相乘：

T_AC = T_AB * T_BC

这个太方便了。我在做多传感器外参标定时，就是把每个传感器相对于车体的变换矩阵写出来，然后任意两个传感器之间的变换就是矩阵乘法的结果。

实用技巧：齐次变换矩阵的逆也很好求——不需要真的求4×4矩阵的逆。如果 T = [R t; 0 1]，那么 T^{-1} = [R^T -R^T * t; 0 1]。这个在代码里直接写，比调用通用求逆函数快得多。

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

特性	旋转矩阵	欧拉角	四元数
参数数量	9（冗余）	3（最小）	4（约束1个）
直观性	中等	高	低
万向锁	无	有	无
组合旋转	矩阵乘法	复杂	四元数乘法
插值	困难	困难	SLERP平滑
数值稳定性	需重新正交化	一般	需归一化
常用场景	机器人运动学	人机交互、飞行控制	姿态估计、滤波、动画

3.7 实战：坐标系变换的代码骨架

下面给出一段C++风格的伪代码，展示如何用齐次变换矩阵做坐标系变换。实际项目中，我建议用Eigen或Sophus库，但理解底层逻辑更重要。

// 定义旋转矩阵（绕Z轴旋转30度）
double theta = 30.0 * M_PI / 180.0;
Matrix3d R;
R << cos(theta), -sin(theta), 0,
      sin(theta),  cos(theta), 0,
      0,           0,          1;

// 定义平移向量
Vector3d t(1.0, 2.0, 0.5);

// 构建齐次变换矩阵
Matrix4d T = Matrix4d::Identity();
T.block<3,3>(0,0) = R;
T.block<3,1>(0,3) = t;

// 点p在坐标系{B}中的坐标
Vector4d p_B(0.5, -0.3, 1.2, 1.0);

// 变换到坐标系{A}
Vector4d p_A = T * p_B;

// 求逆变换（快速方法）
Matrix4d T_inv = Matrix4d::Identity();
T_inv.block<3,3>(0,0) = R.transpose();
T_inv.block<3,1>(0,3) = -R.transpose() * t;

避坑指南：写代码时，一定要搞清楚变换的方向。T_AB 表示从 {B} 到 {A} 的变换，还是从 {A} 到 {B}？我建议统一约定：T_AB 表示将 {B} 中的点变换到 {A} 中。这样链式乘法 T_AC = T_AB * T_BC 就非常自然。

3.8 本章小结

这一章我们聊了刚体运动的两种成分——平移和旋转。旋转有三种主流描述方式：

旋转矩阵：直观但冗余，适合链式变换
欧拉角：人机友好，但要小心万向锁
四元数：无奇点、适合插值和滤波，我的首选

齐次变换矩阵把平移和旋转统一成4×4矩阵，是传感器融合中坐标对齐的利器。记住：坐标系变换不是数学游戏，而是让不同传感器的数据在同一个参考系下对话。