自动驾驶入门用Python手把手实现车辆传感器坐标转换附完整代码在自动驾驶系统的开发中准确理解车辆传感器数据的空间关系是基础中的基础。想象一下当激光雷达检测到前方5米处有一个障碍物时这个5米究竟是以车辆的哪个部位为参考点当多个传感器同时工作时如何将它们的数据统一到同一个坐标系下这些问题的核心就在于坐标系转换。本文将带您用Python从零实现车身坐标系与全局坐标系的相互转换。不同于纯理论推导我们会通过可运行的代码示例让抽象的数学公式变得触手可及。您将学到如何用NumPy构建旋转矩阵处理角度单位时的常见陷阱使用Matplotlib可视化转换结果实际工程中的优化技巧1. 理解自动驾驶中的坐标系1.1 全局坐标系世界的参考框架全局坐标系Global Coordinate System是固定在环境中的绝对参考系。在自动驾驶中它通常采用右手坐标系X轴车辆前进方向Y轴垂直于X轴向左Z轴垂直于地面向上# 全局坐标系示例 import matplotlib.pyplot as plt fig plt.figure(figsize(10, 5)) ax fig.add_subplot(121, projection3d) ax.quiver(0, 0, 0, 5, 0, 0, colorr, labelX-axis) ax.quiver(0, 0, 0, 0, 5, 0, colorg, labelY-axis) ax.quiver(0, 0, 0, 0, 0, 5, colorb, labelZ-axis) ax.set_title(Global Coordinate System) ax.legend()1.2 车身坐标系车辆的视角车身坐标系Body Coordinate System以车辆自身为参考通常定义如下原点车辆后轴中心或质心X轴指向车辆前方Y轴指向车辆左侧Z轴指向上方不同传感器摄像头、雷达等都有自己的局部坐标系需要先转换到车身坐标系再转换到全局坐标系。# 车身坐标系可视化 car_length 4.7 # 假设车辆长度4.7米 car_width 1.8 # 假设车辆宽度1.8米 ax2 fig.add_subplot(122) ax2.add_patch(plt.Rectangle((-car_length/2, -car_width/2), car_length, car_width, fillFalse)) ax2.quiver(0, 0, 1, 0, colorr, scale5, labelVehicle X) ax2.quiver(0, 0, 0, 1, colorg, scale5, labelVehicle Y) ax2.set_xlim(-3, 3) ax2.set_ylim(-2, 2) ax2.set_title(Body Coordinate System) ax2.legend() plt.tight_layout() plt.show()2. 坐标系转换的数学基础2.1 二维旋转矩阵推导在二维空间中旋转矩阵可以将一个点从一个坐标系转换到另一个旋转θ角度的坐标系$$ R \begin{bmatrix} \cosθ -\sinθ \ \sinθ \cosθ \end{bmatrix} $$这个矩阵的推导可以通过多种方式理解向量投影法新坐标系的基向量在原坐标系中的投影复数旋转法利用欧拉公式 $e^{iθ} \cosθ i\sinθ$几何变换法保持向量长度不变的线性变换import numpy as np def rotation_matrix_2d(theta_deg): 生成2D旋转矩阵 theta_rad np.deg2rad(theta_deg) return np.array([ [np.cos(theta_rad), -np.sin(theta_rad)], [np.sin(theta_rad), np.cos(theta_rad)] ])2.2 三维旋转的扩展在三维空间中旋转变得更加复杂需要考虑绕不同轴的旋转def rotation_matrix_3d(roll, pitch, yaw): 生成3D旋转矩阵 (ZYX顺序) # 转换为弧度 roll np.deg2rad(roll) pitch np.deg2rad(pitch) yaw np.deg2rad(yaw) # 绕X轴旋转(roll) Rx np.array([ [1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)] ]) # 绕Y轴旋转(pitch) Ry np.array([ [np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)] ]) # 绕Z轴旋转(yaw) Rz np.array([ [np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1] ]) return Rz Ry Rx # 注意乘法顺序注意三维旋转的顺序很重要常见的ZYX顺序表示先绕Z轴再绕Y轴最后绕X轴旋转。3. 实现车身到全局坐标系的转换3.1 二维情况实现假设车辆在全局坐标系中的位姿为$(x_v, y_v, θ_v)$传感器在车身坐标系中的坐标为$(x_r, y_r)$转换步骤如下旋转用旋转矩阵处理方向平移加上车辆的位置偏移def body_to_global_2d(vehicle_pose, point_body): 将车身坐标系中的点转换到全局坐标系 :param vehicle_pose: [x_v, y_v, theta_v_deg] :param point_body: [x_r, y_r] :return: 全局坐标系中的点[x_g, y_g] x_v, y_v, theta_v_deg vehicle_pose x_r, y_r point_body # 获取旋转矩阵 R rotation_matrix_2d(theta_v_deg) # 旋转 rotated R np.array([x_r, y_r]) # 平移 x_g rotated[0] x_v y_g rotated[1] y_v return np.array([x_g, y_g])3.2 三维情况实现对于三维空间我们需要考虑车辆的全部6个自由度位置和姿态def body_to_global_3d(vehicle_pose, point_body): 3D车身到全局坐标系转换 :param vehicle_pose: [x, y, z, roll, pitch, yaw] (角度制) :param point_body: [x, y, z] :return: 全局坐标系中的点[x, y, z] x_v, y_v, z_v, roll, pitch, yaw vehicle_pose x_r, y_r, z_r point_body # 获取旋转矩阵 R rotation_matrix_3d(roll, pitch, yaw) # 旋转 rotated R np.array([x_r, y_r, z_r]) # 平移 x_g rotated[0] x_v y_g rotated[1] y_v z_g rotated[2] z_v return np.array([x_g, y_g, z_g])3.3 批量转换优化实际应用中我们经常需要同时转换大量点如激光雷达点云可以使用NumPy的广播机制优化def batch_body_to_global_2d(vehicle_pose, points_body): 批量转换二维点 :param vehicle_pose: [x_v, y_v, theta_v_deg] :param points_body: Nx2数组 [[x1,y1], [x2,y2], ...] :return: Nx2全局坐标数组 x_v, y_v, theta_v_deg vehicle_pose R rotation_matrix_2d(theta_v_deg) # 向量化旋转 rotated (R points_body.T).T # 向量化平移 translated rotated np.array([x_v, y_v]) return translated4. 全局到车身坐标系的逆转换4.1 二维逆变换实现从全局坐标转换回车身坐标是上述过程的逆过程平移减去车辆位置旋转应用旋转矩阵的逆转置def global_to_body_2d(vehicle_pose, point_global): 全局坐标系到车身坐标系的转换 :param vehicle_pose: [x_v, y_v, theta_v_deg] :param point_global: [x_g, y_g] :return: 车身坐标系中的点[x_r, y_r] x_v, y_v, theta_v_deg vehicle_pose x_g, y_g point_global # 平移 translated np.array([x_g - x_v, y_g - y_v]) # 旋转使用旋转矩阵的转置 R rotation_matrix_2d(theta_v_deg) R_inv R.T # 旋转矩阵的逆等于其转置 # 旋转 rotated R_inv translated return rotated4.2 三维逆变换实现三维情况类似但需要考虑全部三个旋转角度def global_to_body_3d(vehicle_pose, point_global): 3D全局到车身坐标系转换 :param vehicle_pose: [x, y, z, roll, pitch, yaw] (角度制) :param point_global: [x, y, z] :return: 车身坐标系中的点[x, y, z] x_v, y_v, z_v, roll, pitch, yaw vehicle_pose x_g, y_g, z_g point_global # 平移 translated np.array([x_g - x_v, y_g - y_v, z_g - z_v]) # 旋转逆矩阵 R rotation_matrix_3d(roll, pitch, yaw) R_inv R.T # 正交矩阵的逆等于转置 # 旋转 rotated R_inv translated return rotated5. 实际应用与可视化5.1 传感器数据融合示例假设我们有以下传感器配置传感器车身坐标(x,y)方向(度)前雷达(2.5, 0)0左雷达(0, 0.8)90右雷达(0, -0.8)-90# 定义传感器配置 sensors { front_radar: {position: np.array([2.5, 0]), yaw: 0}, left_radar: {position: np.array([0, 0.8]), yaw: 90}, right_radar: {position: np.array([0, -0.8]), yaw: -90} } # 车辆位姿 (x, y, theta) vehicle_pose np.array([10, 5, 30]) # x10m, y5m, 朝向30度 # 转换每个传感器到全局坐标系 sensor_global {} for name, config in sensors.items(): # 先转换传感器位置 sensor_pos_global body_to_global_2d(vehicle_pose, config[position]) # 计算传感器朝向 (车辆朝向 传感器相对朝向) sensor_yaw_global vehicle_pose[2] config[yaw] sensor_global[name] { position: sensor_pos_global, yaw: sensor_yaw_global }5.2 结果可视化def plot_coordinate_transformation(vehicle_pose, sensor_global): plt.figure(figsize(10, 8)) # 绘制车辆 car_length 4.7 car_width 1.8 R_car rotation_matrix_2d(vehicle_pose[2]) car_corners_body np.array([ [-car_length/2, -car_width/2], [car_length/2, -car_width/2], [car_length/2, car_width/2], [-car_length/2, car_width/2] ]) car_corners_global batch_body_to_global_2d(vehicle_pose, car_corners_body) # 绘制车辆轮廓 plt.fill(car_corners_global[:, 0], car_corners_global[:, 1], lightblue, alpha0.5) # 绘制车辆朝向 plt.quiver(vehicle_pose[0], vehicle_pose[1], np.cos(np.deg2rad(vehicle_pose[2])), np.sin(np.deg2rad(vehicle_pose[2])), colorblue, scale10, labelVehicle Heading) # 绘制传感器 colors {front_radar: red, left_radar: green, right_radar: purple} for name, config in sensor_global.items(): pos config[position] yaw config[yaw] plt.scatter(pos[0], pos[1], colorcolors[name], labelname) plt.quiver(pos[0], pos[1], np.cos(np.deg2rad(yaw)), np.sin(np.deg2rad(yaw)), colorcolors[name], scale10) plt.xlabel(Global X (m)) plt.ylabel(Global Y (m)) plt.title(Vehicle and Sensor Positions in Global Frame) plt.legend() plt.grid(True) plt.axis(equal) plt.show() plot_coordinate_transformation(vehicle_pose, sensor_global)6. 工程实践中的注意事项6.1 角度单位的统一在工程实践中角度单位混乱是常见错误来源确保所有角度要么全是度数要么全是弧度NumPy的三角函数默认使用弧度在接口处明确注释单位# 好的实践明确单位转换 def calculate_angle_diff(angle1_deg, angle2_deg): 计算两个角度之间的最小差值输入为度数 diff (angle1_deg - angle2_deg) % 360 return min(diff, 360 - diff) # 不好的实践混合单位 def problematic_angle_func(angle_deg, angle_rad): 危险混合了度和弧度 return np.sin(angle_deg) np.cos(angle_rad) # 错误6.2 旋转顺序的重要性在三维空间中旋转顺序会影响最终结果。常见的顺序有ZYX航向-俯仰-横滚XYZ自定义序列# 演示不同旋转顺序的结果差异 angles [30, 45, 60] # roll, pitch, yaw R_zyx rotation_matrix_3d(*angles) # 使用ZYX顺序 R_xyz rotation_matrix_3d(angles[2], angles[1], angles[0]) # XYZ顺序 print(ZYX顺序旋转矩阵:\n, R_zyx) print(XYZ顺序旋转矩阵:\n, R_xyz)6.3 性能优化技巧对于实时系统坐标转换需要高效实现预计算不变的旋转矩阵使用NumPy的向量化操作对于固定配置考虑使用查找表# 预计算旋转矩阵示例 class CoordinateTransformer: def __init__(self): self._cache {} def get_rotation_matrix_2d(self, theta_deg): 带缓存的旋转矩阵获取 if theta_deg not in self._cache: theta_rad np.deg2rad(theta_deg) self._cache[theta_deg] np.array([ [np.cos(theta_rad), -np.sin(theta_rad)], [np.sin(theta_rad), np.cos(theta_rad)] ]) return self._cache[theta_deg] def body_to_global(self, vehicle_pose, points_body): 优化后的批量转换 x_v, y_v, theta_v_deg vehicle_pose R self.get_rotation_matrix_2d(theta_v_deg) rotated (R points_body.T).T translated rotated np.array([x_v, y_v]) return translated7. 完整代码示例以下是一个完整的坐标转换类封装了本文介绍的所有功能import numpy as np import matplotlib.pyplot as plt from typing import Union, List class CoordinateTransformer: 自动驾驶坐标系转换工具类 功能 - 2D/3D坐标系转换 - 批量点转换 - 可视化工具 def __init__(self): self._rotation_cache_2d {} self._rotation_cache_3d {} staticmethod def _get_rotation_matrix_2d(theta_deg: float) - np.ndarray: 生成2D旋转矩阵 theta_rad np.deg2rad(theta_deg) return np.array([ [np.cos(theta_rad), -np.sin(theta_rad)], [np.sin(theta_rad), np.cos(theta_rad)] ]) staticmethod def _get_rotation_matrix_3d(roll_deg: float, pitch_deg: float, yaw_deg: float) - np.ndarray: 生成3D旋转矩阵 (ZYX顺序) roll np.deg2rad(roll_deg) pitch np.deg2rad(pitch_deg) yaw np.deg2rad(yaw_deg) Rx np.array([ [1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)] ]) Ry np.array([ [np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)] ]) Rz np.array([ [np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1] ]) return Rz Ry Rx def body_to_global_2d(self, vehicle_pose: Union[List[float], np.ndarray], point_body: Union[List[float], np.ndarray]) - np.ndarray: 2D车身到全局坐标系转换 :param vehicle_pose: [x_v, y_v, theta_v_deg] :param point_body: [x_r, y_r] :return: 全局坐标系中的点[x_g, y_g] x_v, y_v, theta_v_deg vehicle_pose x_r, y_r point_body # 从缓存获取或计算旋转矩阵 if theta_v_deg not in self._rotation_cache_2d: self._rotation_cache_2d[theta_v_deg] self._get_rotation_matrix_2d(theta_v_deg) R self._rotation_cache_2d[theta_v_deg] rotated R np.array([x_r, y_r]) return np.array([rotated[0] x_v, rotated[1] y_v]) def batch_body_to_global_2d(self, vehicle_pose: Union[List[float], np.ndarray], points_body: Union[List[List[float]], np.ndarray]) - np.ndarray: 批量2D车身到全局坐标系转换 :param vehicle_pose: [x_v, y_v, theta_v_deg] :param points_body: Nx2数组 :return: Nx2全局坐标数组 points_body np.asarray(points_body) x_v, y_v, theta_v_deg vehicle_pose if theta_v_deg not in self._rotation_cache_2d: self._rotation_cache_2d[theta_v_deg] self._get_rotation_matrix_2d(theta_v_deg) R self._rotation_cache_2d[theta_v_deg] rotated (R points_body.T).T translated rotated np.array([x_v, y_v]) return translated def plot_transformation(self, vehicle_pose: Union[List[float], np.ndarray], sensors_body: dict None, points_body: Union[List[List[float]], np.ndarray] None): 可视化坐标转换结果 :param vehicle_pose: [x_v, y_v, theta_v_deg] :param sensors_body: 字典 {传感器名: {position: [x,y], yaw: 角度}} :param points_body: 需要转换的点列表 plt.figure(figsize(10, 8)) x_v, y_v, theta_v_deg vehicle_pose # 绘制车辆 car_length 4.7 car_width 1.8 car_corners_body np.array([ [-car_length/2, -car_width/2], [car_length/2, -car_width/2], [car_length/2, car_width/2], [-car_length/2, car_width/2] ]) car_corners_global self.batch_body_to_global_2d(vehicle_pose, car_corners_body) plt.fill(car_corners_global[:, 0], car_corners_global[:, 1], lightblue, alpha0.5) # 绘制车辆朝向 plt.quiver(x_v, y_v, np.cos(np.deg2rad(theta_v_deg)), np.sin(np.deg2rad(theta_v_deg)), colorblue, scale10, labelVehicle Heading) # 绘制传感器 if sensors_body: colors plt.cm.tab10.colors for i, (name, config) in enumerate(sensors_body.items()): pos_global self.body_to_global_2d(vehicle_pose, config[position]) sensor_yaw_global theta_v_deg config.get(yaw, 0) plt.scatter(pos_global[0], pos_global[1], colorcolors[i], labelname) plt.quiver(pos_global[0], pos_global[1], np.cos(np.deg2rad(sensor_yaw_global)), np.sin(np.deg2rad(sensor_yaw_global)), colorcolors[i], scale10) # 绘制其他点 if points_body is not None: points_global self.batch_body_to_global_2d(vehicle_pose, points_body) plt.scatter(points_global[:, 0], points_global[:, 1], colorred, markerx, labelPoints) plt.xlabel(Global X (m)) plt.ylabel(Global Y (m)) plt.title(Coordinate Transformation Visualization) plt.legend() plt.grid(True) plt.axis(equal) plt.show() # 使用示例 if __name__ __main__: transformer CoordinateTransformer() # 定义车辆位姿和传感器 vehicle_pose [10, 5, 30] # x, y, theta_deg sensors { front_radar: {position: [2.5, 0], yaw: 0}, left_radar: {position: [0, 0.8], yaw: 90}, right_radar: {position: [0, -0.8], yaw: -90} } # 可视化 transformer.plot_transformation(vehicle_pose, sensors) # 转换单个点 point_body [1, 0.5] point_global transformer.body_to_global_2d(vehicle_pose, point_body) print(fBody point {point_body} - Global point {point_global}) # 批量转换 points_body np.random.rand(10, 2) * 3 - 1.5 # 生成随机点 points_global transformer.batch_body_to_global_2d(vehicle_pose, points_body) # 可视化带随机点 transformer.plot_transformation(vehicle_pose, sensors, points_body)