坐标系基础：世界坐标系、车体/机器人坐标系、传感器坐标系。欧拉角与四元数
做多传感器融合，第一道坎就是坐标系。
我见过太多项目，算法模型调得漂漂亮亮，一上路就崩。查到最后，往往是坐标系搞错了。说白了，你连「我在哪」、「传感器朝哪」都没说清楚，融合个啥？
这一节，咱们就把坐标系这件事彻底掰扯清楚。
1. 三大坐标系：你总得知道自己在哪
多传感器融合里，我们打交道最多的就是三个坐标系。我习惯把它们叫做「天地车」。
1.1 世界坐标系（World Frame）
这是最大的那个坐标系。它固定在大地上，不随车动。你可以把它理解成「绝对位置」的参考。
- 用途：定义地图、GPS坐标、全局路径规划。
- 常见约定：东北天（ENU）或北东地（NED）。我个人习惯用ENU，X轴指东，Y轴指北，Z轴朝天。
- 注意：不同地图服务（比如高德、百度）的坐标系原点可能不同，融合前一定要对齐。
1.2 车体/机器人坐标系（Body Frame / Vehicle Frame）
这个坐标系固定在车上。原点一般在车辆后轴中心或质心。它描述的是「车本身的状态」。
- 用途：车辆控制、IMU解算、运动学模型。
- 常见约定：前X右Y上Z（ROS标准）。X轴指车头，Y轴指右侧，Z轴指车顶。
- 避坑指南：我曾经遇到一个项目，IMU的坐标系定义和车体坐标系差了90度。结果车一转弯，姿态全乱套。嗯，查了两天才发现。
1.3 传感器坐标系（Sensor Frame）
每个传感器都有自己的小坐标系。相机、激光雷达、毫米波雷达，各有各的朝向。
- 相机：一般Z轴指前，Y轴向下，X轴向右（图像坐标系）。
- 激光雷达：通常X轴指前，Y轴向左，Z轴向上。
- IMU：和车体坐标系类似，但安装角度可能有偏差。
你想想看，这三个坐标系之间怎么转换？
答案就是：外参。
外参就是描述「传感器坐标系」相对于「车体坐标系」的旋转和平移。而「车体坐标系」相对于「世界坐标系」的位姿，就是车辆的定位结果。
核心逻辑：世界坐标系是「绝对参考」，车体坐标系是「载体参考」，传感器坐标系是「观测参考」。所有数据最终都要统一到同一个坐标系下，才能做融合。
2. 旋转的表达：欧拉角 vs 四元数
坐标系之间的旋转怎么描述？
两个主流方法：欧拉角和四元数。我当年刚入行时，觉得欧拉角直观，四元数太抽象。后来被万向锁坑过一次，就老实了。
2.1 欧拉角
欧拉角就是用三个角度来描述旋转：绕X轴转（横滚Roll）、绕Y轴转（俯仰Pitch）、绕Z轴转（偏航Yaw）。
- 优点：直观，好理解。你一说「车头偏了10度」，大家都能懂。
- 缺点：有万向锁问题。当俯仰角接近±90度时，横滚和偏航会耦合，丢失一个自由度。
- 避坑指南：我曾经在无人机项目里用欧拉角做姿态控制。结果飞机一抬头超过80度，控制就乱套了。嗯，后来全改成四元数了。
警告：欧拉角不适合做插值和迭代。如果你要做平滑旋转或卡尔曼滤波，请用四元数。
2.2 四元数
四元数是一个四维复数：q = w + xi + yj + zk。它用四个数表示一个旋转。
- 优点：无万向锁，可插值（球面线性插值SLERP），计算稳定。
- 缺点：不直观。你很难一眼看出「这个四元数对应什么姿态」。
- 实用技巧：调试时，我会把四元数转成欧拉角来看。但代码里，全程用四元数运算。
个人习惯：我一般在传感器驱动层把数据转成四元数。后续的融合、滤波、插值，全部在四元数空间里做。只在最后可视化或日志输出时，才转成欧拉角。
3. 代码示例：欧拉角与四元数互转
说一千道一万，不如看代码。下面是我常用的Python工具函数，基于scipy和numpy。
import numpy as np
from scipy.spatial.transform import Rotation as R
def euler_to_quat(roll, pitch, yaw):
"""
欧拉角转四元数
输入：roll (rad), pitch (rad), yaw (rad)
输出：四元数 [w, x, y, z]
"""
r = R.from_euler('xyz', [roll, pitch, yaw])
quat = r.as_quat() # scipy返回 [x, y, z, w]
return np.array([quat[3], quat[0], quat[1], quat[2]]) # 转为 [w, x, y, z]
def quat_to_euler(quat):
"""
四元数转欧拉角
输入：四元数 [w, x, y, z]
输出：roll (rad), pitch (rad), yaw (rad)
"""
r = R.from_quat([quat[1], quat[2], quat[3], quat[0]]) # 转为 [x, y, z, w]
return r.as_euler('xyz')
# 示例
roll, pitch, yaw = np.radians([10, 20, 30])
quat = euler_to_quat(roll, pitch, yaw)
print(f"四元数: {quat}")
roll_back, pitch_back, yaw_back = quat_to_euler(quat)
print(f"转回欧拉角: {np.degrees([roll_back, pitch_back, yaw_back])}")注意：不同库的四元数顺序可能不同。scipy是 [x, y, z, w]，ROS是 [w, x, y, z]。转换时一定要确认顺序，否则旋转方向会反。
4. 实战中的坐标系管理
在实际项目中，坐标系管理是个脏活累活。我分享几个经验。
4.1 统一命名规范
我习惯用这样的命名：
world：世界坐标系base_link：车体坐标系（ROS标准）lidar_front：前向激光雷达camera_front_center：前向主相机
命名要自解释。别用 frame1、frame2 这种，三个月后你自己都看不懂。
4.2 用TF树管理
ROS的TF（Transform）库是管理坐标系的好工具。它会自动维护一个树状结构，帮你计算任意两个坐标系之间的变换。
你只需要发布每个传感器相对于车体的静态变换（外参），以及车体相对于世界的动态变换（定位）。剩下的，TF帮你搞定。
4.3 验证外参
标定完外参后，一定要验证。我常用的方法：
- 把激光雷达的点云投影到图像上，看边缘是否对齐。
- 让车走一个矩形路径，看IMU和里程计的轨迹是否闭合。
- 用两个传感器观测同一个目标，看位置是否一致。
重要：外参不是标一次就完事的。车辆使用久了，传感器可能松动。我建议每次上线前，都跑一遍快速验证流程。
好了，坐标系的基础就这些。说白了，就是搞清楚「谁相对于谁在哪」。下一节我们会深入外参标定的具体方法，到时候这些概念都会用上。