2. 数据表示与坐标系：从世界到传感器，再到那个小立方体

好，我们正式开始第二讲。这一章，说白了就是搞清楚「车在哪、东西在哪、怎么用格子描述空间」。坐标系和体素，是 Occupancy Network 的基石。你想想看，如果连位置都说不清楚，后面的预测、规划全是空中楼阁。

2.1 坐标系：三个世界的来回穿梭

自动驾驶里，我们至少得跟三个坐标系打交道。我个人习惯把它们叫做「上帝视角」、「自我中心」和「眼睛看到的」。

2.1.1 世界坐标系

世界坐标系，就是那个绝对参考系。通常我们选一个固定点作为原点，比如地图上的某个经纬度，或者高精地图的起点。它的 X 轴指向东，Y 轴指向北，Z 轴朝天。所有东西的绝对位置，都在这里定义。

嗯，这里要注意：世界坐标系一旦定下来，整个项目周期就别改了。我在项目中遇到过，有人为了图方便，把世界坐标系原点设在停车场入口，结果换了个停车场，所有地图数据全废了。血的教训。

2.1.2 自车坐标系

自车坐标系，就是以车为中心。原点通常在车辆后轴中心（或者 GPS 天线位置），X 轴朝前，Y 轴朝左，Z 轴朝上。这个坐标系会随着车移动而移动。

为什么需要它？因为决策和规划模块，更关心「障碍物在我前面几米」，而不是「障碍物在经纬度多少」。说白了，自车坐标系是给控制算法看的。

2.1.3 传感器坐标系

传感器坐标系，就是每个传感器自己的小世界。比如激光雷达，原点在激光头，X 轴朝前，Y 轴朝左，Z 轴朝上。相机呢？原点在光心，Z 轴朝前（沿着光轴），X 轴朝右，Y 轴朝下（图像坐标系）。

你想想看，每个传感器看到的数据，都是在自己坐标系下的。要融合它们，就得做坐标变换。我建议你画一张图，把三个坐标系的关系画清楚，比看十页公式都管用。

2.2 体素：三维空间的最小积木

体素，就是 3D 版本的像素。你把三维空间切成一个个小立方体，每个小立方体就是一个体素。每个体素里，要么有东西，要么没东西，要么不确定。

2.2.1 体素的定义

一个体素由三个参数决定：它在网格中的索引 (i, j, k)，以及它的物理尺寸 (dx, dy, dz)。通常我们让 dx = dy = dz，也就是正方体体素。

举个例子：如果体素分辨率是 0.1m，那么索引 (10, 20, 5) 对应的世界坐标就是 (1.0m, 2.0m, 0.5m)。

// 体素到世界坐标的转换（伪代码）
function voxelToWorld(i, j, k, resolution, origin):
    x = origin.x + i * resolution
    y = origin.y + j * resolution
    z = origin.z + k * resolution
    return (x, y, z)

2.2.2 分辨率选择：一个让人头疼的问题

分辨率选多大？这问题我纠结过很久。选小了，细节丰富但计算量爆炸；选大了，计算快但可能漏掉小物体。

分辨率	优点	缺点	适用场景
0.05m (5cm)	细节丰富，能检测到小障碍物	计算量大，内存占用高	近距离感知、泊车
0.1m (10cm)	平衡了精度和效率	可能漏掉细杆	通用场景、城市道路
0.2m (20cm)	计算快，适合大范围	细节丢失严重	高速公路、远距离感知

2.3 占据网格的标签：三态逻辑

每个体素，我们给它贴个标签。不是简单的「有」或「没有」，而是三种状态：Free、Occupied、Unknown。

2.3.1 Free（空闲）

这个体素对应的空间，被传感器「看」过，而且确认没有障碍物。比如激光雷达的射线穿过了这个体素，没有碰到任何东西。那它就是 Free。

注意：Free 不代表「绝对空」，而是「在传感器精度范围内，没检测到东西」。

2.3.2 Occupied（占据）

这个体素里，有障碍物。比如激光雷达的射线在这里终止了，或者相机检测到了物体边缘。Occupied 的体素，就是我们要避开的区域。

2.3.3 Unknown（未知）

这个体素，传感器没看到。可能是被遮挡了，也可能是超出了感知范围。Unknown 区域，规划算法要特别小心，不能贸然开进去。

2.4 实战中的坐标系转换

好了，理论说完了，我们看看实际代码怎么写。下面是一个简单的坐标转换示例，把激光雷达点云从传感器坐标系转到自车坐标系。

import numpy as np

# 传感器到自车的变换矩阵（假设已知）
T_sensor_to_ego = np.array([
    [1, 0, 0, 0.5],   # 激光雷达装在车前方 0.5m
    [0, 1, 0, 0.0],   # 左右居中
    [0, 0, 1, 0.2],   # 高度 0.2m
    [0, 0, 0, 1]
])

# 假设激光雷达看到一个点，在传感器坐标系下是 (1.0, 0.0, 0.0)
point_sensor = np.array([1.0, 0.0, 0.0, 1.0])

# 转换到自车坐标系
point_ego = T_sensor_to_ego @ point_sensor
print(f"自车坐标系下的点: {point_ego[:3]}")
# 输出: [1.5, 0.0, 0.2]

你看，就这么简单。但实际项目中，标定参数会有误差，我建议你每次转换后都做一次可视化检查，看看点云是不是对齐了。

2.5 体素化实战：从点云到占据网格

最后，我们看看怎么把点云变成占据网格。核心思路就三步：

1. 确定范围： 比如自车前方 50m，左右 25m，高度 5m。
2. 计算索引： 每个点云坐标，除以分辨率，取整，得到体素索引。
3. 标记状态： 有点的体素标记为 Occupied，射线穿过的体素标记为 Free，剩下的 Unknown。

def pointcloud_to_occupancy(points, resolution, grid_size):
    """
    points: Nx3 的点云数组
    resolution: 体素分辨率 (m)
    grid_size: (W, H, D) 网格尺寸
    """
    # 初始化占据网格，-1 表示 Unknown
    grid = np.full(grid_size, -1, dtype=np.int8)
    
    # 计算体素索引
    indices = (points / resolution).astype(np.int32)
    
    # 过滤掉超出范围的索引
    mask = (indices[:, 0] >= 0) & (indices[:, 0] < grid_size[0]) & \
           (indices[:, 1] >= 0) & (indices[:, 1] < grid_size[1]) & \
           (indices[:, 2] >= 0) & (indices[:, 2] < grid_size[2])
    indices = indices[mask]
    
    # 标记 Occupied
    grid[indices[:, 0], indices[:, 1], indices[:, 2]] = 1
    
    # 这里省略了射线追踪标记 Free 的步骤（比较复杂）
    # 实际项目中会用 Bresenham 3D 算法
    
    return grid

嗯，这里要提醒一下：射线追踪标记 Free 这一步，其实挺耗时的。我建议你用 CUDA 或者 OpenCL 来加速，CPU 版本跑起来太慢了。

公众号:蓝海资料掘金营，微信deep3321