3、坐标系统与空间变换：世界坐标系、传感器坐标系、刚体变换
坐标系统，说白了就是给空间里的每个点一个“身份证号”。做多传感器融合，最怕的就是各说各话——激光雷达说目标在(1,0,0)，相机说在(0,1,0)，其实指的是同一个点，但坐标系没对齐。我刚开始做融合时，就吃过这个亏，数据对了好久才发现是坐标轴方向定义反了。
这一章，咱们把坐标系统和空间变换彻底讲透。你理解了这些，后面做传感器标定、数据对齐，才能心里有底。
3.1 世界坐标系与传感器坐标系
先搞清楚两个基本概念：
- 世界坐标系：一个固定的参考系，所有传感器数据最终都要统一到这里。比如自动驾驶里，通常把车辆启动时的位置设为原点，x轴指北，y轴指东，z轴朝天。
- 传感器坐标系：每个传感器自己的“小天地”。相机以光心为原点，激光雷达以旋转中心为原点，IMU以自身测量中心为原点。
为什么要区分？因为每个传感器安装位置不同，朝向也不同。你想想看，车顶的激光雷达和保险杠上的毫米波雷达，看到的同一个障碍物，坐标值肯定不一样。
核心原则：所有传感器数据，最终都要变换到同一个世界坐标系下，才能做融合。否则就是鸡同鸭讲。
3.2 刚体变换：旋转矩阵与平移向量
从一个坐标系到另一个坐标系，本质上是刚体变换——物体本身不变，只是我们看它的角度和位置变了。刚体变换由两部分组成：
- 旋转：用旋转矩阵 R 表示，3×3 的正交矩阵，行列式为 +1。
- 平移：用平移向量 t 表示，3×1 的列向量。
公式很简单：p' = R * p + t。其中 p 是原坐标系下的点，p' 是目标坐标系下的点。
我记得有一次做多雷达融合，两个雷达的安装角度差了 2.3 度，我一开始没做旋转补偿，结果融合出来的目标位置总是有 30 厘米的偏差。后来加上旋转矩阵，问题立刻解决。嗯，细节决定成败。
3.2.1 旋转矩阵的性质
旋转矩阵有几个重要性质，写代码时经常用到：
| 性质 | 数学表达 | 实际意义 |
|---|---|---|
| 正交性 | RTR = I | 旋转不改变向量长度 |
| 行列式为 1 | det(R) = 1 | 保证是刚体旋转，不是镜像 |
| 逆等于转置 | R-1 = RT | 反向旋转直接用转置，省计算量 |
小技巧：代码里判断一个矩阵是不是合法的旋转矩阵，就检查 RTR 是否接近单位矩阵，以及行列式是否接近 1。我一般用 np.allclose(R.T @ R, np.eye(3)) 来验证。
3.2.2 平移向量的坑
平移向量看起来简单，但有个容易搞混的地方：平移的方向。公式 p' = R * p + t 里的 t，是从原坐标系原点指向目标坐标系原点的向量，在原坐标系下表示。
我曾经在写标定程序时，把平移向量方向搞反了，结果所有点都往反方向偏了 1 米。排查了半天，才发现是 t 的符号问题。所以，写代码时一定要注释清楚：# t: 从传感器坐标系原点指向世界坐标系原点的向量，在传感器坐标系下表示。
3.3 四元数与欧拉角
旋转矩阵有 9 个参数，但实际自由度只有 3 个。参数多了，容易引入冗余和数值问题。所以工程上更常用四元数和欧拉角。
3.3.1 欧拉角
欧拉角用三个角度表示旋转：绕 x 轴（滚转 roll）、绕 y 轴（俯仰 pitch）、绕 z 轴（偏航 yaw）。直观，但有个大问题——万向锁。
当俯仰角接近 ±90° 时，滚转和偏航会变得无法区分。我做过一个无人机项目，飞控里用欧拉角做姿态解算，飞机垂直爬升时姿态突然跳变，差点炸机。后来换成四元数，问题才解决。
警告：如果你做的是全姿态运动（比如无人机、机器人手臂），千万别只用欧拉角。四元数才是正道。
3.3.2 四元数
四元数用 4 个参数表示旋转：q = w + xi + yj + zk，其中 w 是实部，x、y、z 是虚部。它没有奇点，插值平滑，计算效率高。
四元数转旋转矩阵的公式：
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²)]]看着复杂？别怕，实际开发中直接用库函数就行。比如在 Python 里：
from scipy.spatial.transform import Rotation as R
# 四元数 [x, y, z, w] 转旋转矩阵
q = [0.707, 0, 0, 0.707] # 绕 x 轴旋转 90 度
rot = R.from_quat(q)
matrix = rot.as_matrix()
print(matrix)3.3.3 什么时候用什么？
| 表示方式 | 优点 | 缺点 | 推荐场景 |
|---|---|---|---|
| 旋转矩阵 | 直观，容易组合 | 9 个参数，有冗余 | 理论推导、坐标变换计算 |
| 欧拉角 | 直观，三个角度 | 万向锁，插值困难 | 人机交互、小角度姿态 |
| 四元数 | 无奇点，插值平滑 | 不够直观 | 姿态估计、SLAM、滤波 |
我个人习惯：存储和计算用四元数，展示和调试用欧拉角。这样既避免了万向锁，又方便人理解。
3.4 坐标转换实战
理论说完了，来点实际的。假设你有一个激光雷达，安装在小车前方 0.5 米、高 0.3 米处，俯仰角向下 5 度。现在雷达检测到一个点，在雷达坐标系下是 (2, 1, 0)。怎么转换到世界坐标系？
步骤很简单：
- 定义旋转：根据安装角度，构造旋转矩阵（或四元数）。
- 定义平移：根据安装位置，构造平移向量。
- 执行变换：
p_world = R * p_lidar + t。
代码实现：
import numpy as np
from scipy.spatial.transform import Rotation as R
# 1. 雷达安装参数
# 俯仰角 -5 度（向下），偏航 0，滚转 0
pitch_deg = -5.0
rot = R.from_euler('xyz', [0, pitch_deg, 0], degrees=True)
R_matrix = rot.as_matrix()
# 2. 平移向量（雷达坐标系原点在世界坐标系下的位置）
# 假设世界坐标系原点在小车后轴中心
t = np.array([0.5, 0.0, 0.3]) # x向前，y向左，z向上
# 3. 雷达检测到的点
p_lidar = np.array([2.0, 1.0, 0.0])
# 4. 坐标变换
p_world = R_matrix @ p_lidar + t
print(f"世界坐标系下的点: {p_world}")避坑指南：我曾经在写变换时，把旋转和平移的顺序搞反了。记住，先旋转，后平移。如果先平移再旋转，结果完全不对。
3.5 知识体系总览
下面这张图，把本章的核心逻辑串起来了。你对照着看，思路会更清晰。
嗯，这一章的内容就这些。坐标系统是融合的基石，你把它搞扎实了，后面学传感器标定、时间同步、状态估计，都会轻松很多。记住我强调的那句话：先旋转，后平移；存储用四元数，展示用欧拉角。这是我在无数项目中踩坑踩出来的经验。