3、坐标系与标定：传感器坐标系、车体坐标系、世界坐标系定义，内参外参标定方法（棋盘格、靶标、手眼标定）
多传感器融合感知的第一性原理，是回答“某个传感器测到的物理量，在真实世界中到底处于什么位置、什么姿态”。如果各传感器各自为政，没有统一的坐标基准，那么激光雷达点云、摄像头像素、毫米波雷达目标、IMU角速度/加速度将无法在空间上对齐，融合算法便无从谈起。本章从坐标系定义出发，系统讲解内参标定、外参标定以及手眼标定的数学原理、操作流程与工程陷阱，帮助读者建立从“像素/点云”到“车体/世界”的完整空间变换链路。
3.1 坐标系定义与空间变换基础
3.1.1 核心原理：从像素到世界的四层变换
多传感器融合的空间变换链路可拆解为四个层级：像素/点云坐标系（原始测量输出）、传感器坐标系（光学/电磁中心）、车体坐标系（车辆运动基准）、世界坐标系（全局定位基准）。每一层之间的变换由外参矩阵（旋转R + 平移t）描述，而像素坐标到传感器坐标的映射则由内参矩阵K描述。
以摄像头为例，其完整投影模型为：
Zc * [u, v, 1]^T = K * [R|t]_BC * [Xb, Yb, Zb, 1]^T其中K包含焦距fx、fy和主点cx、cy，[R|t]_BC为摄像头相对车体的外参。对于激光雷达，其输出本身就是三维点云，因此无需内参，但需要将点云从雷达坐标系变换到车体坐标系。IMU则输出加速度和角速度，其坐标系通常与车体坐标系存在固定安装角，需要通过外参标定补偿。
| 坐标系 | 原点位置 | X轴 | Y轴 | Z轴 | 典型应用 |
|---|---|---|---|---|---|
| 像素系 (u,v) | 图像左上角 | 水平向右 | 垂直向下 | — | 图像处理、特征提取 |
| 传感器系 (C) | 光学中心/雷达天线 | 前向 | 左向 | 上向 | 点云生成、畸变校正 |
| 车体系 (B) | 后轴中心/IMU | 车辆前进方向 | 车辆左侧 | 车辆上方 | 运动控制、路径规划 |
| 世界系 (W) | UTM原点/地图原点 | 东向 | 北向 | 天向 | 全局定位、导航 |
3.1.2 判断方法：坐标系选择与右手定则
工程中常见的坐标系定义遵循右手定则，但不同厂商可能采用不同轴向约定。例如，ROS中摄像头坐标系为Z轴向前、Y轴向下，而车体坐标系为X轴向前、Y轴向左。判断坐标系是否一致的方法是：将两个坐标系的基向量叉乘，结果应指向第三个轴的正方向。若叉乘结果为负，则说明存在镜像或轴向反转，需要额外处理。
3.1.3 案例拆解：IMU与车体坐标系的安装偏差
某L4级自动驾驶测试车将IMU安装在车辆后轴中心前方0.5m、左侧0.2m处，且安装时存在约2°的俯仰角偏差。初始阶段，融合系统输出的车辆姿态与高精定位结果存在系统性偏差，导致车道级定位误差超过30cm。通过外参标定，测得IMU相对车体的平移为(0.5, 0.2, 0.1)m，旋转欧拉角为(0.03rad, 0.035rad, 0.01rad)。补偿后，姿态误差从2.1°降至0.3°，定位精度提升至10cm以内。此案例说明，即使安装位置已知，也必须通过标定获取精确值，因为机械安装公差通常在毫米级到厘米级，而角度偏差即使只有1°，在50m外也会产生近1m的横向误差。
3.2 内参标定：棋盘格与靶标方法
3.2.1 核心原理：内参矩阵与畸变模型
内参标定解决的是“像素坐标如何映射到传感器归一化平面”的问题。对于摄像头，内参矩阵K为3×3上三角矩阵，畸变模型通常采用Brown-Conrady多项式，包含径向畸变(k1, k2, k3)和切向畸变(p1, p2)。标定的本质是求解K和畸变系数，使得所有角点的重投影误差最小化。
棋盘格标定法（Zhang氏标定法）是目前最主流的方法，其核心思想是：拍摄不同姿态的棋盘格图像，提取角点，利用单应性矩阵约束求解内参。靶标法（如圆点靶标）则适用于广角镜头或鱼眼镜头，因为圆形特征在畸变下仍能保持较好的检测稳定性。
3.2.2 操作流程：棋盘格标定的完整步骤
- 准备靶标：打印10×7棋盘格，方格边长30mm，粘贴在平整刚性板上。
- 采集图像：在均匀光照下，从不同角度（俯仰、偏航、滚转）拍摄15-20张图像，确保棋盘格覆盖画面各个区域。
- 角点提取：使用OpenCV的
findChessboardCorners函数，亚像素细化角点坐标。 - 求解内参：调用
calibrateCamera，输入角点世界坐标和像素坐标，输出K、畸变系数、每张图像的外参。 - 评估结果：检查重投影误差，通常应小于0.1像素；若误差过大，剔除异常图像后重新标定。
# Python/OpenCV 内参标定核心代码
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((7*10,3), np.float32)
objp[:,:2] = np.mgrid[0:10,0:7].T.reshape(-1,2) * 30 # 方格边长30mm
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, gray.shape[::-1], None, None)3.2.3 案例拆解：鱼眼摄像头内参标定实战
某泊车系统采用180°鱼眼摄像头，使用标准棋盘格标定时，边缘区域角点检测失败率高达40%。改用圆点靶标（7×9圆点阵列，间距20mm）后，检测成功率提升至95%。标定结果显示，径向畸变k1=-0.35，k2=0.12，切向畸变p1=0.002，p2=-0.001。使用标定后的内参进行畸变校正，图像边缘直线弯曲从15像素降至2像素以内。此案例表明，对于大视场角镜头，靶标类型的选择直接影响标定质量。
3.3 外参标定：传感器到车体、传感器到传感器
3.3.1 核心原理：外参矩阵的物理意义
外参标定求解的是两个坐标系之间的刚体变换T = [R|t]，其中R为3×3旋转矩阵，t为3×1平移向量。对于传感器到车体的外参，通常以车体坐标系为基准，描述传感器在车体上的安装位置和姿态。对于传感器到传感器的外参（如Camera-LiDAR），则描述两个传感器之间的相对位姿。
外参标定的核心挑战在于：如何建立两个坐标系中同一特征点的对应关系。靶标法通过在场景中放置已知尺寸的标定板，同时被多个传感器观测，利用特征点匹配求解外参。自然特征法则利用场景中的角点、边缘等特征，适用于无法放置靶标的场景。
3.3.2 操作流程：Camera-LiDAR联合标定
| 步骤 | 操作内容 | 关键参数/工具 | 注意事项 |
|---|---|---|---|
| 1. 布置靶标 | 放置棋盘格或专用标定板，确保同时被摄像头和雷达观测 | 标定板尺寸≥1m×1m | 避免反光材质，保证雷达反射强度 |
| 2. 数据采集 | 同步采集图像和点云，至少10个不同位姿 | ROS bag / 自定义采集程序 | 确保时间戳对齐，误差<10ms |
| 3. 特征提取 | 图像中提取角点，点云中提取标定板平面 | OpenCV / PCL | 点云平面拟合RANSAC |
| 4. 初始外参估计 | 利用3对以上对应点求解P3P或EPnP | OpenCV solvePnP | 需要粗略初始值 |
| 5. 非线性优化 | 最小化重投影误差，优化R和t | ceres / g2o | 加入鲁棒核函数 |
| 6. 验证 | 将点云投影到图像，检查边缘对齐 | 可视化工具 | 误差应<3像素 |
3.3.3 案例拆解：多传感器外参标定的工程闭环
某Robotaxi平台需要标定前向摄像头、顶部激光雷达和毫米波雷达三者之间的外参。初始使用靶标法，但毫米波雷达点云稀疏，无法稳定检测标定板角点。解决方案是：先标定Camera-LiDAR外参（使用棋盘格），再标定LiDAR-Radar外参（使用金属角反射器），最后通过矩阵乘法间接得到Camera-Radar外参。整个标定过程耗时2小时，最终验证结果：点云投影到图像的平均误差为2.1像素，雷达目标投影误差为0.3m。复盘发现，毫米波雷达的多径效应导致部分反射器位置偏移，通过增加采集帧数（从50帧增至200帧）并取中值滤波，有效抑制了噪声。
3.4 手眼标定：AX = XB问题
3.4.1 核心原理：手眼标定的数学本质
手眼标定源于机器人领域，解决的是“相机（眼）与机械臂末端（手）之间的固定变换”问题。在自动驾驶中，该问题演变为“传感器与运动平台之间的变换”。其数学本质是求解矩阵方程AX = XB，其中A为机械臂末端的运动变换，B为传感器观测到的靶标运动变换，X为待求的手眼变换矩阵。
求解方法分为两步：先求解旋转部分，再求解平移部分。常用的算法包括Tsai-Lenz法、Park-Martin法、Daniilidis四元数法等。工程中通常采集多组运动数据，构建超定方程，通过最小二乘或SVD分解求解。
3.4.2 操作流程：移动平台手眼标定
- 固定靶标：将棋盘格固定在车辆前方，确保车辆运动时靶标始终在视野内。
- 车辆运动：控制车辆进行平移和旋转运动，至少采集3组不同姿态的数据。
- 记录位姿：同时记录车辆的运动学位姿（来自轮速计/IMU）和摄像头观测到的靶标位姿。
- 求解方程：使用OpenCV的
cv2.calibrateHandEye函数，输入两组位姿序列，输出手眼变换矩阵。 - 验证精度：将求解的X代入方程，计算残差，残差应小于0.01。
# OpenCV 手眼标定代码
R_gripper2base, t_gripper2base = get_vehicle_poses() # 车辆运动位姿
R_target2cam, t_target2cam = get_target_poses() # 靶标观测位姿
R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
R_gripper2base, t_gripper2base, R_target2cam, t_target2cam,
method=cv2.CALIB_HAND_EYE_TSAI)3.4.3 案例拆解：AGV手眼标定的精度提升
某仓储AGV使用2D激光雷达进行定位，需要标定雷达与车体中心的外参。初始采用手工测量，误差约5cm，导致AGV在狭窄通道中频繁碰撞。改用基于运动的手眼标定后，控制AGV进行“8”字形运动，采集20组位姿数据。求解结果显示，雷达相对车体的平移为(0.12, 0.05, 0.2)m，偏航角为0.8°。标定后，AGV的重复定位精度从±5cm提升至±1.5cm。复盘发现，运动轨迹的多样性对标定精度影响显著——直线运动无法提供足够的旋转约束，必须包含充分的旋转分量。
3.5 常见误区与总结复盘
3.5.1 误区一：忽略时间同步
外参标定要求各传感器数据在时间上严格对齐。若摄像头帧率为30Hz，激光雷达为10Hz，则时间戳偏差可能达到33ms。在车辆行驶时，33ms内车辆可能移动0.3m（36km/h），导致标定特征点错位。解决方案是使用硬件同步触发或软件插值对齐。
3.5.2 误区二：标定场景过于单一
在室内车库标定的外参，直接用于室外道路场景，可能出现较大误差。原因是不同环境下温度、光照、振动条件不同，导致传感器安装位置微变。建议在多种典型场景下进行验证性标定，并建立外参置信度评估机制。
3.5.3 误区三：忽视标定板平面度
棋盘格粘贴在弯曲表面上，会导致角点世界坐标错误，进而影响内参标定精度。使用前应用直尺或激光水平仪检查标定板平面度，平面度误差应小于0.5mm。