3. 坐标系定义与转换：雷达坐标系、车辆坐标系、世界坐标系、欧拉角与旋转矩阵
做雷达标定，说白了就是跟坐标系打交道。我见过不少工程师，算法模型调得飞起，一碰到坐标系转换就翻车。嗯，这其实是个基本功，但也是最容易出bug的地方。
今天咱们就把这件事彻底捋清楚。你想想看，雷达装到车上，它看到的目标位置是相对于雷达自己的。但车辆控制需要的是目标相对于车，或者相对于地面的位置。这中间差了三次坐标变换。
3.1 三大坐标系，各司其职
先搞清楚我们手里有几张“地图”。
3.1.1 雷达坐标系（Sensor Coordinate System）
这是雷达自己的小世界。通常定义：
- X轴：雷达正前方
- Y轴：雷达左侧（符合右手系）
- Z轴：雷达正上方
雷达输出的点云数据，比如距离、方位角、俯仰角，都是在这个坐标系下算的。我个人习惯把雷达坐标系称为“原始坐标系”，因为所有测量数据都从这里出发。
3.1.2 车辆坐标系（Vehicle Coordinate System）
这个坐标系以车辆为中心。标准定义（ISO 8855）是这样的：
- X轴：车辆前进方向
- Y轴：车辆左侧
- Z轴：车辆正上方
- 原点：通常在后轴中心，或者车辆质心
为什么用后轴中心？因为车辆转弯时，后轴中心基本不侧滑，定位更稳定。我在做AEB功能标定时，就吃过亏——一开始把原点定在前保险杠，结果转弯时目标位置跳得厉害。
3.1.3 世界坐标系（World Coordinate System）
这是全局参考系。通常用UTM坐标系或者经纬度+高程。在标定场景里，我们常用的是“标定场坐标系”——一个固定的、已知的参考框架。
世界坐标系的作用很简单：把不同时刻、不同车辆、不同传感器的数据统一到同一个空间里。你想想看，如果两辆车各自用自己的车辆坐标系，那它们永远没法“对话”。
3.2 欧拉角：旋转的三种姿势
坐标系之间怎么转？靠欧拉角。说白了就是绕三个轴各转一次。
常用的顺序是 Z-Y-X（偏航-俯仰-横滚）：
- 偏航角（Yaw, ψ）：绕Z轴旋转。车头左右摆。
- 俯仰角（Pitch, θ）：绕Y轴旋转。车头上下翘。
- 横滚角（Roll, φ）：绕X轴旋转。车身左右侧倾。
3.3 旋转矩阵：数学工具箱
绕单轴的旋转矩阵很简单，记住这三个：
// 绕Z轴旋转（偏航）
Rz(ψ) = | cosψ -sinψ 0 |
| sinψ cosψ 0 |
| 0 0 1 |
// 绕Y轴旋转（俯仰）
Ry(θ) = | cosθ 0 sinθ |
| 0 1 0 |
| -sinθ 0 cosθ |
// 绕X轴旋转（横滚）
Rx(φ) = | 1 0 0 |
| 0 cosφ -sinφ |
| 0 sinφ cosφ |
组合旋转时，顺序很重要。Z-Y-X顺序的完整旋转矩阵是：
R = Rz(ψ) * Ry(θ) * Rx(φ)
注意乘法顺序：先绕X转，再绕Y，最后绕Z。我刚开始做的时候总搞反，结果标定出来的角度全是错的。后来养成了习惯，每次写代码前先画个旋转顺序图。
3.4 从雷达到车辆：外参标定的核心
这一步就是把雷达坐标系下的点，变换到车辆坐标系下。公式很简单：
P_vehicle = R * P_radar + T
其中：
- R：3x3旋转矩阵（雷达相对于车辆的姿态）
- T：3x1平移向量（雷达相对于车辆的位置）
这6个参数（3个旋转角 + 3个平移量），就是雷达的外参。
3.5 从车辆到世界：动态定位
车辆坐标系到世界坐标系的变换，是随时间变化的。因为车在动。通常用GPS/IMU提供的车辆位姿：
P_world = R_vehicle_to_world * P_vehicle + T_vehicle_to_world
这里的R和T是实时更新的。说白了，就是车辆在世界里“漂移”，我们得跟着它算。
3.6 完整变换链路
把上面两步串起来，从雷达到世界的完整变换是：
P_world = R_v2w * (R_r2v * P_radar + T_r2v) + T_v2w
这个公式，就是多传感器融合的数学基础。你想想看，激光雷达、摄像头、毫米波雷达，各自的数据最终都要统一到世界坐标系下，才能做决策。
3.7 知识体系总览
下面这张图，把本章的核心逻辑串起来了。建议你保存下来，写代码时对照着看。
3.8 实战建议
最后，给你几个我在项目中踩过坑后总结的建议：
- 统一单位：雷达输出毫米，车辆坐标系用米，世界坐标系用米。转换前先归一化。我见过有人把毫米当成米用，结果目标飞到了天上。
- 验证闭环：标定完成后，找一个已知位置的目标，把数据从雷达坐标系转到世界坐标系，看看误差。这叫“闭环验证”，能发现大部分问题。
- 记录安装参数：每次装车后，记录雷达的安装位置和角度。万一标定丢了，还能用手动测量的值顶一下。
- 小心符号约定：不同厂家对旋转正方向的定义可能不同。比如“向左转”是正还是负？查清楚再写代码。
坐标系转换，说白了就是“对齐”。把雷达的眼睛、车辆的身体、世界的地图，三者对齐。这一步做扎实了，后面的目标跟踪、融合、决策，才有根基。
可可精品资料库