坐标系与刚体变换基础
各位同学，欢迎来到这门实战课程的第一章。
说实话，做激光雷达标定这么多年，我见过太多人一上来就调参、跑算法，结果标定出来一塌糊涂。为什么？说白了，就是坐标系和变换没搞明白。你想想看，连雷达点云在哪个坐标系下、车体朝哪边转都不知道，标定结果能对吗？
这一章，我们就先把地基打牢。我会带着你把世界坐标系、车体坐标系、雷达坐标系、欧拉角、四元数、齐次变换矩阵这些概念，一个一个啃下来。嗯，都是干货，别走神。
核心观点：标定的本质，就是求解不同坐标系之间的刚体变换关系。坐标系搞不懂，标定就是空中楼阁。
1. 坐标系：标定的“语言”
坐标系这东西，说白了就是给空间中的点一个“地址”。没有坐标系，你没法描述“那个障碍物在哪儿”。
在自动驾驶里，我们主要跟三个坐标系打交道：
- 世界坐标系：固定在大地上的坐标系。通常用经纬度+海拔，或者UTM坐标。它是全局的参考系，所有车、所有传感器最终都要统一到这个世界坐标系下。
- 车体坐标系：固定在车辆上的坐标系。一般原点在车辆后轴中心或者质心，X轴朝前，Y轴朝左，Z轴朝上。这个坐标系会随着车辆移动而移动。
- 雷达坐标系：固定在激光雷达上的坐标系。原点在雷达的激光发射中心，Z轴一般朝上，X轴朝前（或者朝侧面，看雷达安装方式）。
我刚开始做标定时，就犯过一个低级错误：把雷达坐标系和车体坐标系的朝向搞反了。结果点云投影到车体上，所有障碍物都跑到右边去了。排查了半天，才发现是坐标系定义没对齐。从那以后，我每次做项目第一件事，就是先把三个坐标系的定义写在白板上。
我的习惯：在代码里用枚举或者常量明确定义每个坐标系的朝向，比如 COORD_VEHICLE_X_FORWARD、COORD_LIDAR_Z_UP。别偷懒，后面能省很多事。
2. 欧拉角与四元数：旋转的两种“方言”
坐标系之间怎么变换？无非是平移加旋转。平移好办，就是三个方向的偏移量。旋转呢？有两种主流表达方式：欧拉角和四元数。
2.1 欧拉角
欧拉角用三个角度来描述旋转：绕X轴转的叫滚转角（Roll），绕Y轴转的叫俯仰角（Pitch），绕Z轴转的叫偏航角（Yaw）。
听起来很直观对吧？但有个大坑——万向锁。当俯仰角接近±90度时，滚转和偏航会变得无法区分，旋转自由度丢失。我在做某个园区无人车项目时，就遇到过这个问题。车辆爬坡时俯仰角接近90度，欧拉角直接炸了，姿态解算完全乱掉。
避坑指南：如果你用欧拉角做插值或者滤波，一定要小心万向锁。我建议在标定内部计算时，尽量用四元数，只在最后输出给用户看时，才转成欧拉角。
2.2 四元数
四元数是个四维向量 (w, x, y, z)，它没有万向锁问题，而且插值平滑。你想想看，一个旋转用四个数表示，比三个数还稳定，是不是很神奇？
四元数的核心公式：
// 四元数乘法（表示旋转叠加）
q_result = q1 * q2
// 用四元数旋转一个点 p
p_rotated = q * p * q_conjugate
// 四元数转旋转矩阵（常用）
R = [
[1-2(y²+z²), 2(xy-wz), 2(xz+wy)],
[2(xy+wz), 1-2(x²+z²), 2(yz-wx)],
[2(xz-wy), 2(yz+wx), 1-2(x²+y²)]
]我个人习惯在代码里用 Eigen 或者 Sophus 库处理四元数，手写容易出错。有一次我手写四元数乘法，符号写反了，结果标定出来的外参旋转了90度，点云全飞到天上去了。嗯，从那以后我再也不敢手撸四元数了。
3. 齐次变换矩阵：把平移和旋转打包
有了旋转，有了平移，怎么把它们组合起来？齐次变换矩阵就是干这个的。
一个4x4的齐次矩阵长这样：
T = [ R t ]
[ 0 1 ]
其中 R 是 3x3 旋转矩阵，t 是 3x1 平移向量。用齐次矩阵的好处是：你可以把多次变换连乘起来，一次搞定。比如从雷达坐标系到车体坐标系，再到世界坐标系：
T_world_lidar = T_world_vehicle * T_vehicle_lidar这个连乘的顺序，我见过太多人搞反了。记住：从右往左读。先做 T_vehicle_lidar（从雷达到车体），再做 T_world_vehicle（从车体到世界）。
我的经验：在代码里，我习惯把变换矩阵的命名写成 T_source_target，比如 T_lidar_vehicle 表示从雷达坐标系变换到车体坐标系。这样连乘时不容易搞混。
4. 知识体系总览
下面这张图，是我自己总结的坐标系与刚体变换知识结构。你可以把它当作这一章的“地图”。
5. 实战：从雷达点到世界点
光说不练假把式。我们来看一个实际例子：
假设雷达检测到一个点，在雷达坐标系下的坐标是 (1.0, 0.5, 0.0)。已知雷达相对于车体的外参是：平移 (0.3, 0.0, 0.8) 米，旋转用四元数表示 (0.999, 0.0, 0.0, 0.044)（约5度偏航角）。
代码实现：
# Python + numpy 示例
import numpy as np
# 雷达坐标系下的点
p_lidar = np.array([1.0, 0.5, 0.0, 1.0]) # 齐次坐标
# 外参：雷达 -> 车体
t = np.array([0.3, 0.0, 0.8])
# 四元数转旋转矩阵（这里用简化示例）
R = np.array([
[0.996, -0.088, 0.0],
[0.088, 0.996, 0.0],
[0.0, 0.0, 1.0]
])
T_lidar_vehicle = np.eye(4)
T_lidar_vehicle[:3, :3] = R
T_lidar_vehicle[:3, 3] = t
# 变换到车体坐标系
p_vehicle = T_lidar_vehicle @ p_lidar
print(f"车体坐标系下的点: {p_vehicle[:3]}")
# 输出: [1.286, 0.588, 0.8]你看，一个简单的矩阵乘法，就把雷达点变换到了车体坐标系。标定的本质，就是把这个 T_lidar_vehicle 矩阵求出来。
记住：标定的输出，就是一个4x4的齐次变换矩阵。后面的所有章节，都是围绕如何精确求解这个矩阵展开的。
好了，这一章的内容就到这里。坐标系和刚体变换是标定的“语言”，不熟练的话后面会很吃力。我建议你动手写几行代码，把上面的例子跑一遍，感受一下矩阵乘法的物理意义。
可可精品资料库