3. 坐标系与变换基础
多传感器标定的本质，是求解不同坐标系之间的空间变换关系。无论是激光雷达与相机的联合标定，还是IMU与轮式里程计的外参标定，最终都要落到“把一个传感器坐标系下的点，变换到另一个传感器坐标系或世界坐标系下”这一数学操作上。因此，坐标系定义与刚体变换理论是整个标定流程的基石。本章将系统梳理机器人领域常见的坐标系体系，深入讲解旋转矩阵、欧拉角、四元数三种旋转表达方式的数学定义、适用场景与转换方法，并结合实际标定工程中的坐标变换案例，帮助学员建立从物理世界到数学表达的完整映射。
3.1 三种核心坐标系：定义、作用与层级关系
3.1.1 核心原理：坐标系的物理意义与数学表达
在机器人系统中，坐标系是描述物体位置和姿态的基准框架。我们通常定义三类核心坐标系：世界坐标系（World Frame, {W}）、机器人坐标系（Body/Base Frame, {B}）和传感器坐标系（Sensor Frame, {S}）。世界坐标系是固定在地面或环境中的全局参考系，通常以机器人启动位置、场地原点或GPS经纬度基准为原点，其X-Y平面与水平面平行，Z轴垂直向上。机器人坐标系固连在机器人本体上，原点一般位于机器人底盘的几何中心或IMU安装位置，随机器人运动而运动。传感器坐标系则固连在每一个传感器上，例如相机的光心、激光雷达的扫描中心、IMU的加速度计测量轴交点。
三者之间的层级关系为：世界坐标系是全局参考，机器人坐标系是中间枢纽，传感器坐标系是数据源头。任何传感器测量到的数据，首先在自身坐标系{S}中表达，然后通过外参（Extrinsic）变换到机器人坐标系{B}，再通过机器人的位姿（Pose）变换到世界坐标系{W}。这一链式变换关系是标定工作的核心主线。
| 坐标系 | 原点位置 | 轴方向约定 | 是否运动 | 典型用途 |
|---|---|---|---|---|
| 世界坐标系 {W} | 固定参考点 | Z轴垂直向上，X/Y水平 | 固定 | 全局路径规划、地图构建 |
| 机器人坐标系 {B} | 机器人中心/IMU处 | X向前，Y向左，Z向上（ROS REP-103） | 随机器人移动 | 运动控制、局部感知融合 |
| 传感器坐标系 {S} | 传感器测量中心 | 相机Z向前，雷达X向前 | 随传感器安装 | 原始数据采集与预处理 |
3.1.2 判断方法：如何确定坐标系的轴向与原点
在实际工程中，确定坐标系需要遵循明确规范。对于机器人坐标系，ROS社区推荐使用REP-103标准：X轴指向机器人前方，Y轴指向左方，Z轴垂直向上，且遵循右手定则。对于相机坐标系，通常以光心为原点，Z轴指向相机前方（拍摄方向），X轴向右，Y轴向下（注意与图像坐标系区分）。对于激光雷达坐标系，一般以扫描中心为原点，X轴指向雷达前方，Y轴指向左方，Z轴向上。判断轴向时，可以借助传感器输出的已知方向数据（如重力方向、地磁方向）进行初步验证。
3.1.3 案例拆解：AGV小车多传感器坐标系定义
假设我们有一台AGV（自动导引车），配备一个前向2D激光雷达、一个前视单目相机和一个位于车体中心的IMU。我们定义世界坐标系{W}为AGV启动时车体中心在地面的投影点，X轴指向启动时的前方。机器人坐标系{B}原点在IMU安装位置，X轴指向车体正前方，Y轴向左，Z轴向上。激光雷达坐标系{L}原点在雷达扫描中心，X轴指向雷达正前方（与车体X轴平行），Z轴向上。相机坐标系{C}原点在光心，Z轴指向车体前方略向下倾斜（安装俯仰角约5°），X轴向右。
标定任务就是求解T_BL（雷达相对车体的位姿）和T_BC（相机相对车体的位姿）。通过标定板或特征点匹配，我们最终得到：T_BL = (x=0.15m, y=0.0m, z=0.35m, roll=0°, pitch=0°, yaw=0°)，T_BC = (x=0.25m, y=0.0m, z=0.55m, roll=0°, pitch=-5°, yaw=0°)。这些外参将用于后续将雷达点云投影到图像上，实现数据融合。
3.1.4 常见误区与总结复盘
- 误区一：混淆机器人坐标系与IMU坐标系。IMU坐标系通常以自身测量轴为基准，可能与车体坐标系存在安装偏角，需要额外标定。
- 误区二：忽略坐标系轴向的右手定则。左手系与右手系的混用会导致旋转方向判断错误。
- 误区三：将世界坐标系固定于移动物体上。世界坐标系必须固定于惯性参考系或地面，否则位姿估计失去意义。
复盘：坐标系定义是整个标定工程的第一步，建议在项目启动时输出一份《坐标系定义说明书》，明确各坐标系原点、轴向、单位，并经团队评审确认。
3.2 刚体变换：旋转与平移的数学表达
3.2.1 核心原理：刚体变换的构成与性质
刚体变换描述了一个物体在三维空间中不发生形变的运动，包含旋转（Rotation）和平移（Translation）两部分。在齐次坐标下，刚体变换可以表示为4×4矩阵T：
T = [ R t ]
[ 0 1 ]其中R是3×3旋转矩阵，t是3×1平移向量。刚体变换具有以下性质：保持点间距离不变、保持向量夹角不变、保持手性（不产生镜像）。在标定中，我们通常用T_AB表示“从B坐标系到A坐标系的变换”，即把B坐标系下的点变换到A坐标系下：p_A = T_AB * p_B。
3.2.2 旋转矩阵：正交性与约束
旋转矩阵R是一个3×3的正交矩阵，满足R^T * R = I且det(R) = 1。这意味着R虽然有9个元素，但只有3个自由度（9个元素 + 6个正交约束 = 3个独立变量）。旋转矩阵的优点是可直接用于向量旋转计算，且复合旋转只需矩阵乘法；缺点是参数冗余，不适合直接用于非线性优化（需要施加正交约束）。
3.2.3 欧拉角：直观但存在万向锁
欧拉角用三个角度表示旋转，常见顺序为ZYX（偏航角yaw、俯仰角pitch、横滚角roll）。其旋转矩阵为R = Rz(yaw) * Ry(pitch) * Rx(roll)。欧拉角非常直观，适合人机交互显示，但存在著名的万向锁（Gimbal Lock）问题：当pitch = ±90°时，yaw和roll的旋转轴重合，丢失一个自由度，导致姿态解算奇异。因此，欧拉角不适合用于全局姿态估计或插值。
3.2.4 四元数：无奇异性的优雅表达
四元数是由一个实部和三个虚部组成的超复数：q = w + xi + yj + zk，通常表示为q = (w, x, y, z)。单位四元数（||q|| = 1）可以表示三维旋转，其与旋转轴n和旋转角θ的关系为：q = (cos(θ/2), n_x * sin(θ/2), n_y * sin(θ/2), n_z * sin(θ/2))。四元数没有奇异性，且旋转复合运算只需四元数乘法，计算效率高，非常适合用于姿态插值（如SLAM中的平滑插值）和惯性导航解算。
| 表达方式 | 参数数量 | 奇异性 | 计算效率 | 适合场景 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 无 | 中等（矩阵乘法） | 坐标变换计算 |
| 欧拉角 | 3 | 有（万向锁） | 高（三角函数） | 显示、简单控制 |
| 四元数 | 4 | 无 | 高（四元数乘法） | 姿态估计、融合、插值 |
3.3 三种旋转表达方式的相互转换
3.3.1 转换方法与公式
在标定算法中，经常需要在三种表达方式之间切换。以下是核心转换公式：
- 旋转矩阵 → 欧拉角（ZYX顺序）：
pitch = asin(-R[2][0])yaw = atan2(R[1][0], R[0][0])roll = atan2(R[2][1], R[2][2]) - 旋转矩阵 → 四元数：
w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2x = (R[2][1] - R[1][2]) / (4w)y = (R[0][2] - R[2][0]) / (4w)z = (R[1][0] - R[0][1]) / (4w) - 四元数 → 旋转矩阵：使用标准公式，涉及
1-2(y²+z²)等项。 - 欧拉角 → 四元数：分别计算各轴四元数后相乘：
q = qz(yaw) * qy(pitch) * qx(roll)。
pitch = ±90°时，atan2的参数可能退化为0/0，导致角度不稳定。此时应使用四元数或旋转矩阵进行姿态表示，避免使用欧拉角。
3.3.2 实战场景：手眼标定中的转换应用
在机器人手眼标定（Hand-Eye Calibration）中，我们需要求解相机与机械臂末端之间的变换关系T_CE。通常通过机械臂运动到多个位姿，记录末端位姿T_BE和相机观测到的标定板位姿T_CW，构建AX=XB方程。求解过程中，旋转部分通常用四元数或旋转矩阵表示，而最终输出给机器人控制器的外参，往往需要转换为欧拉角格式（因为多数机械臂API接受欧拉角或轴角）。
例如，我们求解得到旋转矩阵：
R = [ 0.998 -0.052 0.030 ]
[ 0.051 0.998 0.021 ]
[ -0.031 -0.019 0.999 ]转换为ZYX欧拉角：pitch = asin(-(-0.031)) ≈ 1.78°，yaw = atan2(0.051, 0.998) ≈ 2.93°，roll = atan2(-0.019, 0.999) ≈ -1.09°。同时转换为四元数：q ≈ (0.9997, -0.0155, 0.0089, 0.0256)。两种表达方式在后续的位姿插值和滤波中各有用途。
3.3.3 常见误区与工程建议
- 误区一：忽略旋转顺序。ZYX与XYZ顺序得到的欧拉角完全不同，转换时必须明确约定。
- 误区二：四元数未归一化。直接使用未归一化的四元数进行旋转计算会导致尺度错误，每次更新后应重新归一化。
- 误区三：在优化问题中直接使用欧拉角。欧拉角的万向锁会导致雅可比矩阵奇异，建议在优化中使用四元数或旋转向量（李代数）。
3.4 链式变换与标定中的坐标传递
3.4.1 核心原理：变换的复合与求逆
多传感器标定的核心操作是变换的复合。假设已知雷达相对机器人坐标系的变换T_BL，机器人相对世界坐标系的变换T_WB，则雷达点云变换到世界坐标系为：T_WL = T_WB * T_BL。变换的逆T_LB = inv(T_BL)表示从机器人坐标系到雷达坐标系的变换，其旋转部分为R^T，平移部分为-R^T * t。
3.4.2 操作流程：外参标定中的数据传递链
- 数据采集：同步采集各传感器数据（如相机图像、雷达点云、IMU角速度/加速度）。
- 特征提取：在各自坐标系中提取特征点（如标定板角点、反射靶标中心）。
- 对应匹配：建立不同传感器观测到的同一特征点的对应关系。
- 求解外参：利用最小二乘或非线性优化求解
T_BS，使得重投影误差最小。 - 验证与精化：在未参与标定的数据上验证外参精度，必要时进行联合优化。
3.4.3 案例拆解：相机与激光雷达联合标定
某无人驾驶车辆需要将64线激光雷达点云投影到前视相机图像上。已知相机内参矩阵K和畸变系数，初始外参猜测为T_BC = (x=0.5m, y=0.0m, z=1.2m, yaw=0°, pitch=-3°, roll=0°)。我们使用标定板（棋盘格）放置在车辆前方5米处，同时采集雷达点云和相机图像。在雷达点云中提取标定板平面上的点，拟合平面得到法向量n_L；在图像中检测棋盘格角点，通过PnP求解得到标定板在相机坐标系下的位姿T_CB。
通过构建重投影误差函数E = Σ || p_img - K * T_BC * p_lidar ||²，使用Levenberg-Marquardt算法优化T_BC。优化后得到精确外参：T_BC = (x=0.498m, y=0.012m, z=1.198m, yaw=0.2°, pitch=-2.8°, roll=0.1°)。验证时，将雷达点云投影到图像上，平均重投影误差从初始的15像素降低到1.2像素，满足融合要求。
3.4.4 总结复盘与注意事项
链式变换中，变换顺序不可交换，T_WB * T_BL ≠ T_BL * T_WB。在代码实现中，建议使用齐次矩阵乘法库（如Eigen、Sophus）避免手写错误。此外，标定结果的精度受初始值影响较大，建议先用线性方法（如DLT）求解初始值，再进行非线性优化。
本章系统梳理了坐标系定义、刚体变换的三种旋转表达方式及其转换关系，并通过链式变换展示了多传感器标定中的坐标传递逻辑。掌握这些基础，是后续学习具体标定方法（如手眼标定、多激光雷达标定）的前提。在实际工程中，建议学员反复练习三种旋转表达的互转，并熟练使用Eigen或Sophus库进行变换计算，做到“心中有坐标系，手中有变换矩阵”。