3. 坐标系与标定

各位同学，今天我们来聊聊感知融合里最基础、也最容易踩坑的一环——坐标系与标定。

说实话，我见过不少团队，算法模型跑得飞起，一上路就翻车。查到最后，十有八九是坐标系没对齐。你想想看，雷达说「前方5米有障碍」，相机说「左前方3米有车」，到底信谁？说白了，坐标系不统一，融合就是空谈。

3.1 车辆坐标系：一切感知的「锚点」

我个人习惯，先把车辆坐标系定死。这是所有传感器数据的最终归宿。

在自动驾驶领域，我们通常采用ISO 8855标准。车辆坐标系的原点一般放在后轴中心，或者车辆质心。具体怎么选？我建议放在后轴中心，因为车辆运动学模型用这个点最方便。

3.2 相机坐标系：从像素到空间的桥梁

相机坐标系，说白了就是解决「图像上的一个点，对应现实世界哪里」的问题。

相机坐标系的原点在光心，Z轴指向相机前方，X轴向右，Y轴向下。嗯，这里要注意，相机坐标系的Y轴是向下的，和车辆坐标系不一样。很多新手在这里搞混。

相机标定主要做两件事：

• 内参标定： 焦距、主点、畸变系数。我一般用张正友标定法，棋盘格拍20张左右就够了。
• 外参标定： 相机相对于车辆坐标系的旋转和平移。这个才是融合的关键。

// 像素坐标 -> 相机坐标
P_cam = K_inv * [u, v, 1]^T * depth

// 相机坐标 -> 车辆坐标
P_vehicle = R * P_cam + T

3.3 雷达坐标系：距离与速度的真相

雷达坐标系相对简单。原点在雷达中心，X轴指向雷达正前方，Y轴向左，Z轴向上。

雷达输出的数据是极坐标形式：距离、水平角、俯仰角。我们需要先转成直角坐标，再转到车辆坐标系。

3.4 联合标定：让所有传感器「说同一种语言」

联合标定，就是把相机、雷达、激光雷达等传感器的坐标系，统一到车辆坐标系下。

我常用的方法是：

1. 找一个空旷场地，放置标定物（比如角反射器或棋盘格）
2. 让车辆静止，采集各传感器数据
3. 手动或自动匹配特征点
4. 用最小二乘法求解外参

下面这张图，是我自己总结的标定流程框架：

3.5 实战中的标定技巧

说了这么多理论，来点实际的。我在项目里总结了几条铁律：

• 标定场地要够大： 至少20米×20米，太近了标定精度不够
• 多角度采集： 别只拍正面，左右各45度也要拍
• 温度影响： 夏天和冬天，传感器外壳热胀冷缩，外参会有微小变化。我建议每季度重新标定一次

3.6 代码示例：外参标定计算

下面给一段我常用的外参求解代码，用SVD分解求旋转矩阵：

import numpy as np

def solve_extrinsics(points_cam, points_vehicle):
    """
    求解相机到车辆坐标系的外参
    points_cam: 相机坐标系下的点集 (N x 3)
    points_vehicle: 车辆坐标系下的点集 (N x 3)
    返回: R (3x3), T (3x1)
    """
    # 计算质心
    centroid_cam = np.mean(points_cam, axis=0)
    centroid_vehicle = np.mean(points_vehicle, axis=0)
    
    # 去中心化
    points_cam_centered = points_cam - centroid_cam
    points_vehicle_centered = points_vehicle - centroid_vehicle
    
    # SVD分解
    H = points_cam_centered.T @ points_vehicle_centered
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T
    
    # 确保旋转矩阵行列式为1（右手系）
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T
    
    # 计算平移向量
    T = centroid_vehicle - R @ centroid_cam
    
    return R, T