第1章 坐标系与空间变换：多传感器融合的数学基石
各位同学，我是你们这门课的主讲。今天咱们聊点实在的——坐标系与空间变换。
说实话，我刚入行那会儿，觉得坐标系这东西太简单了，不就是x、y、z三个轴嘛。直到有一次，我负责的L4级自动驾驶项目在路测时，激光雷达检测到的障碍物位置和摄像头看到的完全对不上。排查了整整两天，最后发现是IMU安装时偏了0.5度，导致整个坐标变换链全错了。嗯，从那以后，我再也不敢小看坐标系了。
1.1 为什么坐标系这么重要？
你想想看，一辆自动驾驶车上，至少装着：
- 1个GPS/IMU组合导航（告诉你车在哪）
- 1个前向激光雷达（看前方障碍物）
- 4个角雷达（看侧后方）
- 6-8个摄像头（看车道线、红绿灯、行人）
每个传感器都有自己的"小世界"——也就是它的坐标系。激光雷达说"前方5米有个障碍物"，摄像头说"图像右下角有个物体"。这两个信息怎么对应起来？
核心思想：所有传感器数据，最终都要统一到同一个坐标系下，才能做融合。这就是坐标系变换存在的意义。
1.2 四大坐标系，一个都不能少
我个人习惯把自动驾驶中的坐标系分成四层：
| 坐标系 | 原点 | 用途 |
|---|---|---|
| 世界坐标系 | 地球上的某个固定点（如GPS原点） | 全局定位、路径规划 |
| 车辆坐标系 | 车辆质心或后轴中心 | 车辆运动控制、车身状态 |
| 传感器坐标系 | 传感器安装位置 | 原始数据采集 |
| 图像/像素坐标系 | 图像左上角或中心 | 视觉感知结果 |
举个例子：我在做多传感器标定时，经常遇到一个问题——激光雷达装在车顶，摄像头装在前保险杠。它们看到的同一个物体，坐标值完全不同。这时候就需要把激光雷达的点云从"激光雷达坐标系"变换到"车辆坐标系"，再把摄像头检测结果从"像素坐标系"变换到"车辆坐标系"，最后才能做数据关联。
1.3 刚体变换：旋转与平移
说白了，坐标系变换就是两件事：旋转和平移。
旋转：坐标系绕某个轴转了角度。比如车辆转弯时，车辆坐标系相对于世界坐标系就在旋转。
平移：坐标系的原点移动了位置。比如传感器装在车顶，它的原点就比车辆坐标系原点高了一截。
旋转可以用旋转矩阵R表示，平移用平移向量t表示。一个3D空间中的点P，从坐标系A变换到坐标系B，公式就是：
P_B = R * P_A + t这里R是3x3的旋转矩阵，t是3x1的平移向量。
我的经验：旋转矩阵有9个元素，但实际自由度只有3个（绕x、y、z轴的旋转角）。所以实际工程中，我们常用欧拉角或四元数来表示旋转，避免冗余。我个人更推荐四元数，因为它没有万向锁问题。
1.4 齐次坐标与变换矩阵
刚才的公式P_B = R * P_A + t，每次都要写两个部分，有点麻烦。有没有更优雅的方式？
有——齐次坐标。
我们把3D点P = (x, y, z)扩展成4D齐次坐标P_h = (x, y, z, 1)。然后旋转和平移就可以合并成一个4x4的变换矩阵T：
P_B_h = T * P_A_h
其中 T = [R t]
[0 1]这个T矩阵，就是多传感器融合中最核心的数学工具。你想想看，激光雷达到车辆坐标系的变换是一个T1，车辆坐标系到世界坐标系的变换是T2，那么激光雷达的点云直接到世界坐标系，就是T2 * T1。
避坑指南：我曾经在项目中犯过一个低级错误——把变换矩阵的乘法顺序搞反了。记住：从传感器到世界，变换矩阵是从右往左乘的。先乘传感器到车辆的T1，再乘车辆到世界的T2。
1.5 知识体系总览
下面这张图，是我自己总结的坐标系变换知识体系。你可以把它当作本章的"地图"：
1.6 实战：用Python实现坐标变换
光说不练假把式。下面是我写的一个简单的坐标变换工具函数，你可以在仿真脚本中直接复用：
import numpy as np
def build_transform_matrix(rotation_matrix, translation_vector):
"""
构建4x4齐次变换矩阵
参数:
rotation_matrix: 3x3旋转矩阵
translation_vector: 3x1平移向量
返回:
4x4变换矩阵 T
"""
T = np.eye(4)
T[:3, :3] = rotation_matrix
T[:3, 3] = translation_vector.flatten()
return T
def transform_point(point_3d, T):
"""
将3D点从源坐标系变换到目标坐标系
参数:
point_3d: 3x1或(3,)的3D点坐标
T: 4x4变换矩阵
返回:
变换后的3D点坐标 (3,)
"""
# 转为齐次坐标
point_h = np.append(point_3d, 1.0)
# 变换
transformed_h = T @ point_h
# 转回3D坐标
return transformed_h[:3] / transformed_h[3]
# 示例：将激光雷达坐标系下的点变换到车辆坐标系
# 假设激光雷达安装在车顶，向前平移2米，向上平移1.5米
R_lidar2vehicle = np.eye(3) # 假设安装角度无旋转
t_lidar2vehicle = np.array([2.0, 0.0, 1.5])
T_lidar2vehicle = build_transform_matrix(R_lidar2vehicle, t_lidar2vehicle)
# 激光雷达检测到前方5米处有一个障碍物
point_lidar = np.array([5.0, 0.0, 0.0])
point_vehicle = transform_point(point_lidar, T_lidar2vehicle)
print(f"激光雷达坐标系下的点: {point_lidar}")
print(f"车辆坐标系下的点: {point_vehicle}")
# 输出: [7.0, 0.0, 1.5] — 向前7米，高1.5米我的建议：在实际项目中，不要每次都手动构建变换矩阵。我习惯把所有的变换矩阵预先计算好，存成一个字典。比如：
transforms = {
'lidar2vehicle': T_lidar2vehicle,
'vehicle2world': T_vehicle2world,
'camera2vehicle': T_camera2vehicle,
'lidar2world': T_vehicle2world @ T_lidar2vehicle
}这样在仿真循环中直接查表，效率高很多。
1.7 常见坑点与避坑指南
警告：以下是我踩过的坑，你千万别再踩了。
- 旋转矩阵的正交性检查：我曾经从标定文件里读到一个旋转矩阵，直接拿来用，结果变换出来的点全歪了。后来发现那个矩阵不是正交矩阵（R * R^T ≠ I）。记住：真正的旋转矩阵必须是正交的，行列式为+1。
- 左手系 vs 右手系：不同传感器可能使用不同的坐标系约定。比如ROS用的是右手系（x向前，y向左，z向上），但有些相机坐标系是z向前。我在做仿真时，每次都要先确认坐标系的手性。
- 平移向量的单位：激光雷达用毫米，GPS用米，IMU用厘米...这种单位不一致的问题，我至少遇到过3次。建议在仿真脚本一开始就统一单位。
好了，这一章的内容就到这里。坐标系变换是后面所有章节的基础，你花时间把它吃透，后面的传感器标定、数据融合、仿真场景构建都会轻松很多。
可可精品资料库