【技术手册】自动驾驶多传感器融合中的坐标系对齐实战

1. 自动驾驶多传感器融合的坐标系挑战第一次接触自动驾驶多传感器融合时我被各种坐标系搞得晕头转向。相机、激光雷达、毫米波雷达各自输出的数据就像说着不同方言的人坐标系对齐就是让它们用普通话交流的关键步骤。在实际项目中我发现90%的感知错误都源于坐标系转换时的细微偏差。以NuScenes数据集为例一辆车通常配备6个相机和1个激光雷达每个传感器都有自己的坐标系。相机的图像是2D的像素坐标系激光雷达点云是3D的雷达坐标系而决策系统需要的是以车辆为中心的Ego坐标系或全局的Global坐标系。这就好比把不同国家的地图拼在一起必须确保每个点的经纬度对应准确。最让我头疼的是传感器安装位置的微小变化。曾有个项目因为相机支架被轻微碰撞导致外参变化整个感知系统精度下降了30%却迟迟找不到原因。后来我们建立了严格的标定流程每次硬件调整后都重新测量外参矩阵。2. 坐标系定义与传感器特性2.1 相机坐标系体系相机的坐标系转换链最长也最复杂。从最底层的像素坐标系(u,v)到图像坐标系(x,y)再到相机坐标系(Xc,Yc,Zc)每一步都涉及不同的数学变换。我习惯用手机相机来类比当你用手指放大照片时实际上是在操作像素坐标系而相机镜头的光学特性决定了图像坐标系到相机坐标系的映射。在NuScenes数据集中相机的内参矩阵K通常包含这些参数K [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]其中fx/fy是焦距cx/cy是主点坐标。实测发现不同相机的这些参数可能相差很大比如广角镜头的fx值通常较小。2.2 激光雷达坐标系特点激光雷达坐标系(Xl,Yl,Zl)相对简单因为直接输出3D点云。但不同品牌的雷达坐标系定义可能不同有的X轴向前有的Z轴向前。我在使用Velodyne和Livox雷达时就遇到过这个问题转换时差点把天和地搞反了。激光雷达与相机的标定特别关键。有次我们使用棋盘格标定时因为没考虑到雷达点云的稀疏性标定误差达到了15cm。后来改用反射率高的特制标定板才把误差控制在3cm内。3. 坐标系转换实战技巧3.1 外参标定的注意事项传感器外参标定是坐标系对齐的基础。对于相机和激光雷达的标定我总结了几点经验标定环境要有足够的特征点墙面最好有适当纹理标定板要同时能被相机和雷达检测到采集数据时要多角度移动标定板温度变化可能影响雷达精度标定前要预热在Python中可以用OpenCV的solvePnP函数计算外参retval, rvec, tvec cv2.solvePnP( object_points, image_points, K, dist_coeffs)3.2 转换链的优化实现坐标系转换往往需要连续矩阵乘法在Python中直接写会很慢。我习惯用numpy的einsum函数来优化# 将点云从Lidar系转到Camera系 points_cam np.einsum(ij,nj-ni, T_lidar2cam, points_lidar)对于实时性要求高的场景还可以把转换矩阵提前计算好。在C项目中我通常会把固定转换写成constexpr矩阵利用编译期优化。4. NuScenes数据集处理实例4.1 多相机数据统一处理NuScenes的6个相机数据需要统一到ego坐标系。通过分析数据集代码我发现关键是要处理好各相机的time_offset。有次因为忽略了10ms的时间差导致运动物体的位置出现了明显偏差。处理多相机数据的典型流程加载各相机的内参和外参对每个像素点应用逆投影变换统一转换到ego坐标系处理时间同步问题4.2 点云与图像融合在MMDetection3D中点云到图像的投影是这样实现的# 将LiDAR点云投影到图像平面 points_img points_cam K.T points_img[:, :2] / points_img[:, [2]]这里要注意深度值zc的处理。我遇到过因为除零错误导致程序崩溃的情况后来加了epsilon保护z points_cam[:, 2] z[abs(z) 1e-5] 1e-5 # 避免除零5. 常见问题与调试方法5.1 坐标系对齐验证技巧验证坐标系对齐是否正确我的土方法是在场景中放置明显标记物分别用相机和雷达检测它的位置转换后看两者是否重合更专业的做法是用重投影误差评估。在标定板上选取特征点计算其在各传感器坐标系中的位置差异。5.2 数值稳定性问题坐标系转换中经常遇到数值不稳定问题。比如旋转矩阵求逆时我习惯用SVD分解代替直接求逆U, s, Vh np.linalg.svd(R) R_inv U np.diag(1/s) Vh对于四元数转换要注意归一化处理。有次因为四元数模长不是1导致旋转后尺度发生了变化。6. 工程实践建议在实际部署时我发现这些做法特别有用为每个转换步骤添加可视化调试工具记录原始数据和转换后的数据以便复现问题对转换矩阵做版本控制记录每次修改在关键转换点添加断言检查矩阵性质比如检查旋转矩阵是否正交assert np.allclose(R R.T, np.eye(3), atol1e-6)坐标系对齐是个需要耐心的工作有时候1mm的误差就可能导致后续感知算法失效。有次排查问题时发现是标定板的打印精度不够A4纸在温湿度变化时会有0.5%的尺寸变化这个教训让我之后都改用金属标定板了。