数学基础（上）：刚体变换与齐次坐标、旋转矩阵、欧拉角、四元数

各位同学，欢迎来到《激光雷达与相机外参标定实战》的第一章。

说实话，很多做感知的工程师，一听到「数学基础」四个字就想跳过。我当年也一样，觉得写代码才是硬功夫。直到有一次，我在做多传感器融合时，因为旋转矩阵用错了符号，导致整个点云投影到图像上偏移了十几个像素。排查了整整两天，最后发现是欧拉角顺序搞反了。嗯，从那以后，我再也不敢轻视这部分内容了。

这一章，我们不讲虚的。直接上干货，把刚体变换、齐次坐标、旋转矩阵、欧拉角、四元数这几个核心概念，用实战工程师的视角捋一遍。

1. 刚体变换：传感器在空间中的「搬家」

什么叫刚体变换？说白了，就是你把一个物体从位置A挪到位置B，过程中不捏扁、不拉长、不扭曲。激光雷达和相机之间的外参标定，本质上就是在求一个刚体变换矩阵。

刚体变换由两部分组成：旋转和平移。

旋转：描述传感器坐标系之间的朝向关系。比如激光雷达的X轴，对应相机坐标系的哪个方向。
平移：描述两个传感器原点之间的物理偏移。比如激光雷达装在相机上方10厘米，右侧5厘米。

我个人的习惯是，先理解旋转，再理解平移。因为旋转搞错了，平移再准也没用。

核心公式：

点P在坐标系A中的坐标 P_A，变换到坐标系B中的坐标 P_B：

P_B = R * P_A + t

其中R是旋转矩阵，t是平移向量。

2. 齐次坐标：把旋转和平移「打包」

你想想看，上面那个公式里，旋转是矩阵乘法，平移是向量加法。每次变换都要做两步操作，写代码时容易漏掉平移。有没有办法把旋转和平移合并成一个操作？

有，就是齐次坐标。

齐次坐标的核心思想很简单：给三维点(x, y, z)加一个维度，变成(x, y, z, 1)。然后旋转矩阵R和平移向量t可以拼成一个4x4的矩阵：

| R   t |
| 0   1 |

这样，一次矩阵乘法就完成了旋转+平移。我在项目中经常用这个技巧，尤其是在写标定代码时，用齐次矩阵可以避免很多低级错误。

实战小技巧：

在代码中，我习惯用4x4齐次矩阵表示外参。这样无论是点云变换、坐标转换，还是多传感器串联，都只需要做矩阵乘法。干净利落。

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

旋转矩阵是一个3x3的正交矩阵，行列式为+1。它的每一列，代表原坐标系的一个轴在新坐标系中的投影。

举个例子，绕Z轴旋转θ角：

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

旋转矩阵的好处是直观，你一眼就能看出每个轴怎么转。但坏处也很明显——9个元素，其实只有3个自由度。冗余信息太多，而且容易受数值误差影响，导致矩阵不再是正交的。

我记得有一次做标定优化，迭代几次后旋转矩阵的行列式变成了0.98，结果点云全歪了。从那以后，我很少直接用旋转矩阵做优化。

避坑指南：

我曾经在代码里直接用9个参数优化旋转矩阵，结果收敛到非正交矩阵。后来改用四元数，问题迎刃而解。记住：旋转矩阵适合表示，不适合优化。

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

欧拉角用三个角度表示旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。

为什么说它直观？因为你可以想象成：先低头（pitch），再摇头（yaw），最后歪头（roll）。每个角度都有物理意义。

但欧拉角有个致命问题——万向锁。当pitch接近±90°时，roll和yaw会变得无法区分。说白了，你失去了一个自由度。

我在做车载激光雷达标定时，遇到过这个问题。车辆在坡道上，pitch角接近90°，结果欧拉角表示下的旋转矩阵突然跳变。嗯，从那以后，我标定代码里只用四元数做内部计算，欧拉角只用来给人看。

表示方式	优点	缺点
旋转矩阵	直观，无奇异性	冗余，不适合优化
欧拉角	物理意义明确	万向锁，插值困难
四元数	紧凑，无奇异性，适合插值	不够直观

5. 四元数：工程师的「瑞士军刀」

四元数，说白了就是一个有四个分量的超复数：q = w + xi + yj + zk。其中w是实部，(x, y, z)是虚部。

为什么它这么重要？三个原因：

紧凑：4个参数，没有冗余。
无奇异性：不会出现万向锁。
适合插值：球面线性插值（Slerp）可以平滑过渡。

我个人的习惯是，所有内部计算都用四元数。只有在最后输出给用户看时，才转成欧拉角。

四元数转旋转矩阵（Python示例）：

import numpy as np

def quat_to_rot(q):
    w, x, y, z = q
    R = np.array([
        [1 - 2*y*y - 2*z*z, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
        [2*x*y + 2*w*z, 1 - 2*x*x - 2*z*z, 2*y*z - 2*w*x],
        [2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x*x - 2*y*y]
    ])
    return R

6. 知识体系总览

下面这张图，是我自己总结的本章知识体系。你可以把它当作一个「导航图」，随时回来对照。

7. 总结与个人建议

这一章的内容，说白了就是一句话：用四元数做计算，用欧拉角做展示，用齐次矩阵做变换。

我刚开始做标定时，总想着把所有东西都搞懂再动手。后来发现，先跑通一个最简单的标定流程，再回头理解数学，效率反而更高。你想想看，代码跑通了，你自然就知道哪里容易出问题，数学也就有了落脚点。

下一章，我们会进入实战环节——用Python实现一个简单的旋转矩阵和四元数互转工具。到时候，我会把今天讲的这些数学概念，全部写成可运行的代码。

课后练习：

1. 写一个函数，将欧拉角(roll, pitch, yaw)转换为四元数。

2. 验证：绕Z轴旋转90°，再绕X轴旋转90°，与先绕X轴再绕Z轴，结果是否相同？为什么？

3. 用齐次矩阵表示：点(1, 0, 0)先平移(1, 0, 0)，再绕Y轴旋转90°，求最终坐标。

可可精品资料库