4. 坐标系与变换基础

多模态时空标定的本质，是把不同传感器在同一时刻采集到的观测，统一表达到一个公共参考框架中。要做到这一点，必须先建立清晰的坐标系体系，并掌握描述姿态与位置变换的数学工具。本节从坐标系的定义出发，逐步过渡到旋转矩阵、四元数以及李群李代数，为后续外参标定、时间对齐和融合建图打下基础。

一、四类核心坐标系及其物理含义

在无人机多模态感知系统中，通常涉及以下坐标系：

• 世界坐标系（World / Map Frame）：全局惯性参考系，原点可设在起飞点、地图原点或 GNSS 参考点，Z 轴通常指向天或地。它不随无人机运动而改变，是融合结果的最终表达框架。
• 机体坐标系（Body Frame）：固连于无人机机身，原点一般在质心或 IMU 中心，X 轴指向前、Y 轴指向右、Z 轴指向下（前右下，FRD）或前左上（FLU）。飞控姿态解算、速度控制均以机体坐标系为基准。
• 传感器坐标系（Sensor Frame）：每个传感器独立定义，如相机光学坐标系（Z 轴沿光轴向前，X 向右，Y 向下）、LiDAR 坐标系、IMU 坐标系、GNSS 天线坐标系。它们与机体坐标系之间存在固定的安装外参。
• 中间/辅助坐标系：如相机图像坐标系、归一化平面坐标系、雷达扫描平面坐标系，用于描述投影和观测模型。

坐标系之间的变换由旋转和平移组成，记作 T = (R, t)，其中 R 为旋转矩阵，t 为平移向量。若已知点 p_s 在传感器坐标系下的坐标，则其在世界坐标系下的坐标为 p_w = R_ws · p_s + t_ws。

二、旋转矩阵：最直观的姿态描述

旋转矩阵 R 是一个 3×3 正交矩阵，满足 RᵀR = I 且 det(R) = 1。它直接作用于三维向量，实现坐标变换：p_B = R_BW · p_W。旋转矩阵的复合对应矩阵乘法，R_WC = R_WB · R_BC，这与变换链的物理含义完全一致。

旋转矩阵的优点是线性、无奇异、便于求导和优化；缺点是 9 个参数只有 3 个自由度，存在 6 个约束，直接优化容易破坏正交性。因此在实际标定中，常将旋转矩阵作为中间表达，优化时使用更紧凑的参数化。

三、四元数：紧凑且无万向锁的姿态表示

单位四元数 q = [w, x, y, z] 用 4 个参数描述旋转，满足 w² + x² + y² + z² = 1。相比欧拉角，四元数不存在万向锁；相比旋转矩阵，它更紧凑且插值平滑。四元数乘法对应旋转复合，单位化后可用于姿态传播。

# 四元数乘法（Hamilton 约定）
q = q1 * q2
w = w1*w2 - x1*x2 - y1*y2 - z1*z2
x = w1*x2 + x1*w2 + y1*z2 - z1*y2
y = w1*y2 - x1*z2 + y1*w2 + z1*x2
z = w1*z2 + x1*y2 - y1*x2 + z1*w2

# 四元数转旋转矩阵
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²)]]

在 IMU 姿态解算、视觉惯性里程计中，四元数是状态量的常见选择。但四元数仍有单位模长约束，优化时需注意归一化或采用流形优化。

四、李群与李代数：流形上的优化语言

旋转矩阵集合 SO(3) 和刚体变换集合 SE(3) 构成李群，它们既是群又是光滑流形。李代数 so(3) 和 se(3) 是李群在单位元处的切空间，分别对应三维旋转向量和六维变换向量。指数映射 exp: so(3) → SO(3) 将李代数元素映射为旋转矩阵，对数映射则相反。

李群李代数的核心价值在于：它允许我们在切空间（向量空间）中进行无约束优化，再通过指数映射回到流形上。对于标定问题，待优化变量通常是外参 T_BS，使用 se(3) 上的扰动模型可以避免参数冗余和约束破坏。

五、方法流程：从坐标系定义到标定优化

1. 定义坐标系：明确世界、机体、各传感器的原点与轴向，记录安装方式。
2. 建立变换链：写出从传感器到世界的复合变换 T_WS = T_WB · T_BS。
3. 选择参数化：旋转用四元数或李代数，平移用三维向量。
4. 构建残差：根据观测（图像特征、点云、IMU 预积分）建立重投影误差或点面距离误差。
5. 流形优化：在 se(3) 切空间更新，使用高斯-牛顿或 LM 算法迭代。
6. 验证与固化：用重投影误差、点云重合度评估，输出外参文件。

六、案例场景：相机-IMU 外参标定

假设无人机搭载单目相机和 IMU，需要标定相机到 IMU 的外参 T_CI。标定流程如下：

• 采集标定板图像和同步 IMU 数据，保证时间戳对齐。
• 用相机内参和标定板角点，估计相机位姿 T_WC。
• 用 IMU 预积分，估计 IMU 位姿 T_WI。
• 构建手眼标定方程 T_WI · T_IC = T_WC，求解 T_IC。
• 将 T_IC 作为初值，联合优化重投影误差和 IMU 残差。

该案例中，旋转矩阵用于构建方程，四元数用于姿态传播，李代数用于联合优化，三者协同完成标定。

七、常见误区与复盘

误区	表现	正确做法
坐标系轴向定义混乱	相机 Z 轴方向与机体不一致，导致外参符号错误	统一采用 FRD 或 FLU，明确每个传感器轴向
旋转矩阵直接优化	迭代后 R 不再正交，变换失真	使用四元数或李代数参数化，流形更新
忽略平移与旋转耦合	先标旋转再标平移，误差累积	联合优化 SE(3)，同时估计 R 和 t
四元数未归一化	姿态传播后模长漂移，旋转矩阵错误	每次更新后归一化，或使用单位约束优化
李代数扰动方向混淆	左扰动与右扰动混用，雅可比不一致	统一约定扰动模型，保持推导一致