刚体变换与齐次坐标：旋转矩阵、平移向量、齐次坐标的引入与优势

大家好，我是你们的老朋友。今天我们来聊聊刚体变换和齐次坐标。说实话，这两个概念是自动驾驶感知里最基础、也最容易踩坑的地方。我记得刚入行那会儿，就因为坐标变换搞反了，导致一辆测试车在园区里画了个大圈圈……嗯，从那以后，我对这部分内容就格外上心。

什么是刚体变换？

说白了，刚体变换就是「物体在空间里挪了个位置，但形状大小一点没变」。你想想看，一辆车从A点开到B点，车身不会扭曲，轮子不会变形——这就是刚体变换。

在数学上，刚体变换由两部分组成：

• 旋转：物体绕某个轴转了个角度
• 平移：物体整体移动了一段距离

我在项目中遇到过一个问题：激光雷达和相机的外参标定，本质上就是在求这两个传感器之间的刚体变换。如果算错了，点云投影到图像上就会「飘」到天上去。

旋转矩阵：不只是3×3的数字

旋转矩阵，听起来挺唬人，其实就是个3×3的矩阵。它描述了一个坐标系相对于另一个坐标系的朝向关系。

举个例子，假设有个点P在坐标系A中的坐标是(x_a, y_a, z_a)，我想知道它在坐标系B中的坐标。如果两个坐标系原点重合，那只需要一个旋转矩阵R就能搞定：

P_b = R * P_a

这里有个关键点：旋转矩阵是正交矩阵，也就是说它的逆等于它的转置。这个性质在实际工程中特别有用——求逆变换时不用算矩阵求逆，直接转置就行，省了不少计算资源。

平移向量：别小看这个3×1

平移向量就简单多了，就是个三维向量，表示两个坐标系原点之间的偏移。比如：

P_b = R * P_a + t

这里的t就是平移向量。看起来很简单对吧？但我在实际调试中吃过亏——平移向量的单位一定要统一。有一次激光雷达用的是毫米，相机标定用的是米，结果投影出来的点全跑偏了。排查了半天才发现是单位问题。

齐次坐标：为什么我们需要它？

好，问题来了。上面那个公式 P_b = R * P_a + t，每次都要写一个加号，有点麻烦。而且如果连续做多次变换，比如从车体坐标系到世界坐标系再到地图坐标系，公式会变得又长又乱：

P_map = R2 * (R1 * P_car + t1) + t2

看着就头疼，对吧？

齐次坐标就是来解决这个问题的。它的思路很简单：给三维坐标多加一个维度，变成四维。这样旋转和平移就能合并成一个4×4的矩阵：

| x' |   | R11 R12 R13 tx |   | x |
| y' | = | R21 R22 R23 ty | * | y |
| z' |   | R31 R32 R33 tz |   | z |
| 1  |   | 0   0   0   1  |   | 1 |

你看，一个矩阵乘法就搞定了旋转和平移，不用再加来加去。连续变换也变成了矩阵连乘：

T_map_car = T_map_world * T_world_car

简洁、优雅、高效。这就是齐次坐标的魅力。

齐次坐标的三大优势

总结一下，齐次坐标为什么在自动驾驶领域这么受欢迎：

1. 统一表达：旋转和平移合并成一个矩阵，公式简洁
2. 便于链式变换：多次变换就是矩阵连乘，计算效率高
3. 支持无穷远点：齐次坐标可以表示方向向量（w=0），这在相机投影中特别有用

知识体系总览

下面这张图是我自己整理的刚体变换与齐次坐标的知识结构，希望能帮你理清思路：

代码示例：从三维到齐次

最后，给你看一段我常用的Python代码片段，演示如何构建和使用齐次变换矩阵：

import numpy as np

def build_transform(R, t):
    """
    从旋转矩阵R和平移向量t构建4×4齐次变换矩阵
    R: 3×3 numpy array
    t: 3×1 numpy array
    """
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t.flatten()
    return T

# 示例：绕Z轴旋转90度，再平移(1, 2, 3)
theta = np.pi / 2
R = np.array([
    [np.cos(theta), -np.sin(theta), 0],
    [np.sin(theta),  np.cos(theta), 0],
    [0,              0,             1]
])
t = np.array([[1], [2], [3]])

T = build_transform(R, t)
print("齐次变换矩阵：")
print(T)

# 变换一个点
p = np.array([0, 1, 0, 1])  # 齐次坐标，w=1
p_transformed = T @ p
print(f"变换后的点：{p_transformed[:3]}")

这段代码我用了好多年，基本没改过。你拿去用的时候，记得检查一下R是不是正交矩阵——np.allclose(R.T @ R, np.eye(3)) 跑一下，能避免很多隐藏问题。

可可精品资料库