用Python和Nuscenes数据集，手把手教你搞懂自动驾驶的6大坐标系转换

用Python和Nuscenes数据集实战自动驾驶6大坐标系转换第一次接触自动驾驶感知系统时最让人头疼的莫过于各种坐标系之间的转换关系。记得去年参与一个多传感器融合项目时团队花了整整两周时间调试坐标系对齐问题——雷达检测到的行人位置总是比摄像头看到的偏移两米。后来发现是某个外参矩阵的旋转顺序写反了。本文将用Nuscenes数据集和Python代码带您亲手实现六大坐标系的相互转换避开那些年我们踩过的坑。1. 环境准备与数据加载1.1 安装必要工具包在开始前需要配置以下环境建议使用Python 3.8pip install nuscenes-devkit matplotlib numpy opencv-python pyquaternion注意pyquaternion库用于处理三维旋转比手动计算旋转矩阵更可靠1.2 加载Nuscenes数据集样本我们从数据集中选取一个包含相机和激光雷达数据的样本from nuscenes.nuscenes import NuScenes nusc NuScenes(versionv1.0-mini, dataroot/path/to/dataset, verboseTrue) sample nusc.sample[10] # 选取第10个样本 # 获取前视摄像头(CAM_FRONT)数据 cam_data nusc.get(sample_data, sample[data][CAM_FRONT]) # 获取激光雷达数据 lidar_data nusc.get(sample_data, sample[data][LIDAR_TOP])2. 坐标系定义与可视化2.1 六大坐标系精解在自动驾驶系统中这些坐标系构成了感知的骨架坐标系类型维度原点位置轴方向典型用途像素坐标系2D图像左上角u向右, v向下图像标注图像坐标系2D图像中心x向右, y向下镜头畸变校正相机坐标系3D相机光心X右, Y下, Z前三维重建激光雷达系3D雷达中心X右, Y前, Z上点云处理车体坐标系3D车辆中心X前, Y左, Z上传感器融合世界坐标系3D地图原点依地图定义全局定位2.2 使用Matplotlib可视化坐标系让我们绘制出各坐标系的关系示意图import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D fig plt.figure(figsize(12, 8)) ax fig.add_subplot(111, projection3d) # 绘制坐标系箭头 def draw_axis(ax, origin, directions, color, label): for i, (dir, c) in enumerate(zip(directions, [r, g, b])): ax.quiver(*origin, *dir, colorc, arrow_length_ratio0.1) ax.scatter(*origin, ccolor, labellabel) # 示例坐标系实际应用中需替换为真实外参 draw_axis(ax, [0,0,0], [[1,0,0], [0,1,0], [0,0,1]], k, World) draw_axis(ax, [2,2,2], [[0.7,0.7,0], [-0.7,0.7,0], [0,0,1]], b, Camera) draw_axis(ax, [3,0,1], [[0.8,0,0.6], [0,1,0], [-0.6,0,0.8]], r, Lidar) ax.set_xlim([0, 5]); ax.set_ylim([0, 5]); ax.set_zlim([0, 5]) ax.set_xlabel(X); ax.set_ylabel(Y); ax.set_zlabel(Z) ax.legend(); plt.tight_layout(); plt.show()3. 核心转换实战3.1 相机内参从3D到2D的桥梁相机内参矩阵K将相机坐标系下的3D点投影到2D图像平面import numpy as np # 从Nuscenes获取相机内参 cam_calib nusc.get(calibrated_sensor, cam_data[calibrated_sensor_token]) K np.array(cam_calib[camera_intrinsic]) # 3x3内参矩阵 def project_3d_to_2d(points_3d, K): 将相机坐标系下的3D点投影到像素坐标系 points_2d K points_3d.T points_2d points_2d / points_2d[2, :] # 齐次坐标归一化 return points_2d[:2].T # 测试投影 test_point np.array([10, 0, 50, 1]) # 相机前方50米右侧10米 uv project_3d_to_2d(test_point.reshape(1,-1), K) print(f投影像素坐标: {uv.squeeze()})3.2 外参矩阵坐标系间的空间关系外参矩阵描述传感器之间的相对位姿# 获取相机到车体的变换矩阵 cam2ego np.eye(4) cam2ego[:3, :3] np.array(cam_calib[rotation]) cam2ego[:3, 3] np.array(cam_calib[translation]) # 获取激光雷达到车体的变换 lidar_calib nusc.get(calibrated_sensor, lidar_data[calibrated_sensor_token]) lidar2ego np.eye(4) lidar2ego[:3, :3] np.array(lidar_calib[rotation]) lidar2ego[:3, 3] np.array(lidar_calib[translation]) # 计算相机到激光雷达的变换 cam2lidar np.linalg.inv(lidar2ego) cam2ego3.3 完整转换链路示例实现世界坐标系到像素坐标系的完整转换def world_to_pixel(point_world, cam_data, nusc): 世界坐标 - 像素坐标 # 获取各变换矩阵 calib nusc.get(calibrated_sensor, cam_data[calibrated_sensor_token]) K np.array(calib[camera_intrinsic]) ego2global nusc.get(ego_pose, cam_data[ego_pose_token]) # 构建变换矩阵 world2ego np.eye(4) world2ego[:3, :3] Quaternion(ego2global[rotation]).rotation_matrix world2ego[:3, 3] np.array(ego2global[translation]) ego2cam np.linalg.inv(np.eye(4)) ego2cam[:3, :3] np.array(calib[rotation]) ego2cam[:3, 3] np.array(calib[translation]) # 坐标变换 point_cam ego2cam np.linalg.inv(world2ego) point_world uv K point_cam[:3] uv / uv[2] return uv[:2] # 使用示例 point_world np.array([20, 5, 0, 1]) # 齐次坐标 uv world_to_pixel(point_world, cam_data, nusc)4. 典型问题排查指南4.1 常见错误排查表现象可能原因检查方法投影位置偏移外参平移量错误验证传感器安装位置参数物体变形旋转顺序错误检查欧拉角转旋转矩阵的顺序深度异常坐标系Z轴方向不一致确认各坐标系Z轴正方向定义尺度不符单位不统一检查平移量单位(m/mm)4.2 调试技巧可视化中间结果在每个转换步骤后输出并可视化坐标值print(fCamera坐标系下的点: {point_cam})使用已知几何体验证# 创建立方体点云验证投影 cube_points np.array([[0,0,0], [1,0,0], [0,1,0], [0,0,1], [1,1,0], [1,0,1], [0,1,1], [1,1,1]])检查矩阵可逆性assert np.linalg.det(rotation_matrix) ≈ 1.0, 旋转矩阵行列式应为1在真实项目中坐标系对齐问题往往需要结合标定板数据反复验证。最近我们在处理一个雨天场景时发现激光雷达和相机的坐标转换在50米外会出现厘米级偏差后来发现是温度变化导致雷达支架轻微变形所致。这类问题没有银弹扎实的基础加细致的调试才是王道。