感知

感知是自主系统如何感知和解读物理世界的方式。本章涵盖传感器模态、标定、传感器融合、3D目标检测、深度估计、占用网络、车道检测以及语义建图——这是每一个机器人、无人机和自动驾驶汽车所依赖的感知基础。

• 对于人类而言，感知世界毫不费力：你看到一辆车驶近，听到它的引擎声，感觉到脚下的地面，然后瞬间在脑海中建立起周围环境的模型。自主系统也必须做同样的事，但它使用的是电子传感器和算法，而不是眼睛和耳朵。

• 根本挑战在于：传感器给你的是原始数值（像素强度、点云、信号反射），系统必须将这些数值转化为有结构的理解：“前方12米处有一个行人，正以1.5米/秒的速度向左移动。”这就是感知问题。

• 后续的所有环节（预测、规划、控制）都依赖于感知。一个拥有完美规划器但感知很差的自动驾驶汽车仍然会撞车。感知就是瓶颈。

传感器模态

• 自主系统使用多种传感器类型，每种都有不同的优势和失效模式。没有哪一种传感器是单独够用的。

• 摄像头 以高分辨率捕获密集的彩色信息。一张图像包含数百万个像素，每个像素记录RGB值（参见第8章）。摄像头价格低廉、重量轻，并提供丰富的纹理和色彩信息，这对于读取标志、检测交通信号灯和识别物体至关重要。

• 摄像头类型包括 单目（单个镜头，无原生深度）、立体（两个镜头之间存在基线，可通过视差获得深度，如第8章所述）和 鱼眼（超宽视场角，180°以上，伴有严重径向畸变，用于环视停车系统）。

• 摄像头的主要弱点是投影过程中会丢失深度信息。通过针孔相机模型（回顾第8章的内参矩阵 \(K\)），3D场景被映射到2D图像平面：

\[\begin{bmatrix} u \\ v \\ 1 \end{bmatrix} = \frac{1}{Z} K \begin{bmatrix} X \\ Y \\ Z \end{bmatrix}\]

• 除以 \(Z\) 会丢弃绝对深度。不同大小、不同距离的两个物体可能产生完全相同的投影。从单张图像恢复深度是不适定问题，这就是为什么需要立体相机或学习的单目深度模型。

• 摄像头在恶劣条件下也会遇到困难：阳光直射引起眩光，黑暗降低信号，雨雾会散射光线。

• 激光雷达 发射激光脉冲并测量每个脉冲返回的时间。由于光速已知（\(c \approx 3 \times 10^8\) m/s），每个反射点的距离为：

\[d = \frac{c \cdot \Delta t}{2}\]

• 因子2考虑了往返行程。通过在整个场景中扫描激光，激光雷达构建一个点云：一组3D坐标 \((x, y, z)\)，通常还带有强度（反射率）值。

• 旋转式激光雷达（例如Velodyne）将激光阵列旋转360°以生成完整的环视视图。典型设备每秒产生超过30万个点，通道数为64–128。结果是一个稀疏但几何精度高的场景3D表示。

• 固态激光雷达 没有活动部件，使用光学相控阵或MEMS反射镜。这使得它们更便宜、更紧凑、更可靠，但通常视场角较窄（120° vs 360°）。

• 激光雷达提供精确的深度，但数据稀疏（远少于摄像头的“像素”），没有颜色信息，并且价格昂贵。在暴雨、大雪或沙尘中，激光雷达的性能也会下降，因为颗粒会散射激光脉冲。

• 雷达 的工作原理与激光雷达相同，基于飞行时间，但使用无线电波（毫米波，车载通常为77 GHz）。无线电波穿透雨、雾、灰尘和雪的能力远优于光，使雷达成为对天气最鲁棒的传感器。

• 雷达还可以通过多普勒效应直接测量速度。当物体向传感器移动时，反射波被压缩（频率升高）；当物体远离时，反射波被拉伸（频率降低）。速度计算公式为：

\[v = \frac{\Delta f \cdot c}{2 f_0}\]

• 其中 \(\Delta f\) 是频移，\(f_0\) 是发射频率。这提供了瞬时径向速度，无需任何跟踪或帧间计算。

• 代价是分辨率：雷达的角度分辨率比摄像头或激光雷达粗糙得多，难以区分邻近物体或检测细节。它的优势在于能够在任何天气下远距离（200米以上）检测车辆。

• 超声波传感器 发射高频声脉冲（40–70 kHz）并测量回声返回时间。它们工作在非常短的距离（0.2–5米），主要用于泊车辅助。其物理原理与激光雷达相同，只是用声音代替光，所以 \(d = \frac{v_{\text{声速}} \cdot \Delta t}{2}\)，其中 \(v_{\text{声速}} \approx 343\) m/s。

• IMU 包含加速度计和陀螺仪，分别测量线性加速度和角速度。IMU提供高频运动数据（通常200–1000 Hz），填补了较慢传感器更新之间的间隙。它们不直接感知环境，而是跟踪机器人自身的运动，因此对于航位推算和状态估计至关重要。

• IMU存在漂移问题：小的测量误差随时间累积，导致估计的位置偏离真实位置。这就是为什么IMU几乎总是与其他传感器（摄像头、GPS、激光雷达）融合使用，而不是单独使用。

• GNSS（全球导航卫星系统，包括GPS）通过三角测量来自多颗卫星的信号提供地球表面的绝对位置。标准GPS的精度为2–5米，不足以进行车道级驾驶。RTK-GPS（实时动态差分）使用固定的基准站来校正误差，实现厘米级精度，但需要清晰的天空视野和基准站基础设施。

传感器标定

• 在传感器协同工作之前，必须进行标定：每个传感器的测量值必须关联到一个公共坐标系中。

• 内参标定 确定传感器的内部参数。对于摄像头，这包括焦距、主点和畸变系数（如第8章所述）。对于激光雷达，这意味着激光束之间的精确角度偏移。一种常见方法是张正友棋盘格标定法，通过从多个角度观察已知平面图案来求解内参矩阵。

• 外参标定 确定两个传感器之间的刚体变换（旋转 \(R\) 和平移 \(\mathbf{t}\)）。如果摄像头和激光雷达安装在同一车辆上，外参标定会找到将点从激光雷达坐标系映射到摄像头坐标系的 \(4 \times 4\) 变换矩阵：

\[\mathbf{p}_{\text{cam}} = \begin{bmatrix} R & \mathbf{t} \\ \mathbf{0}^T & 1 \end{bmatrix} \mathbf{p}_{\text{lidar}}\]

• 这是一个齐次坐标下的仿射变换，正是我们在第2章（线性变换）中研究过的类型。这个矩阵出错意味着激光雷达点会投影到错误的像素上，整个融合流程就会崩溃。

• 时间标定 同步传感器时钟。以30 Hz采集的摄像头和以10 Hz采集的激光雷达会在不同时间戳产生数据。如果汽车以30 m/s（高速公路速度）行驶，10 ms的时间误差对应30 cm的空间误差。硬件触发（共享时钟脉冲）或软件同步（时间戳之间的插值）是必不可少的。

传感器融合

• 没有哪种传感器能覆盖所有条件。摄像头看到颜色和纹理但丢失深度。激光雷达精确测量深度但稀疏且色盲。雷达在任何天气下都能工作但分辨率差。传感器融合 结合了各自的优势并弥补了各自的弱点。

• 早期融合（或数据级融合）在进行任何处理之前就融合原始传感器数据。例如，将激光雷达点投影到摄像头图像上，创建一个RGBD表示（每个像素的颜色+深度），或者用摄像头投影到的像素颜色为每个激光雷达点着色。这种方法保留了最多的信息，但需要精确的标定，并且对未对准敏感。

• 晚期融合（或决策级融合）让每个传感器通过自己的检测流程独立处理，然后合并最终输出（边界框、类别标签、置信度分数）。每个传感器投票，融合模块调解分歧。这种方法更简单、更模块化，但每个流程无法利用其他传感器的原始数据。

• 中级融合 在中间特征表示上进行操作。每个传感器的原始数据被编码到学习到的特征空间（使用CNN或Transformer），然后将特征结合起来。这是现代系统中的主流方法，因为它让网络学会从每种模态中提取什么信息。

• BEVFusion 是一种代表性的中级融合架构。它将摄像头特征和激光雷达特征都投影到一个公共的鸟瞰图表示中，即场景的俯视网格。摄像头特征通过预测的深度分布“提升”到3D，然后溅射到BEV网格上。激光雷达特征已经是3D的，直接被体素化到同一个网格上。融合后的BEV特征随后由检测头处理。

• BEV表示之所以强大，是因为它提供了一个统一的、具有度量尺度的坐标系，空间推理（距离、大小、重叠）变得直观。在摄像头图像中，近处的自行车和远处的卡车可能占据相同数量的像素。在BEV中，它们的真实大小和位置一目了然。

3D目标检测

• 感知的核心任务是检测3D中的物体：它们在哪里，有多大，是什么，面朝哪个方向？每个检测结果是一个3D边界框，包含位置 \((x, y, z)\)、尺寸 \((l, w, h)\)、朝向角 \(\theta\)、类别标签和置信度分数。

• 基于激光雷达的检测 直接在点云上操作。挑战在于点云是无序的、不规则的、密度变化的（近处物体有数千个点，远处物体只有少数几个）。回顾第8章，PointNet通过共享MLP和置换不变的聚合（最大池化）来处理这一问题。

• PointPillars 将点云转换为结构化表示：将地面平面离散化为一个垂直列（“柱子”）的网格。每个柱子内的所有点由一个小的PointNet编码成一个固定大小的特征向量。结果是一个2D伪图像，可以由标准的2D CNN主干处理，然后接一个检测头（如第8章的SSD架构）。这种方法快速且有效。

• CenterPoint 将物体检测为点而不是框。它在BEV中预测一个物体中心的热图，然后在每个峰值处回归框的属性（尺寸、高度、朝向、速度）。这是CenterNet（第8章）的3D模拟：无锚点，训练期间无需NMS，并且可以通过跨帧关联中心点自然地扩展到跟踪。

• 仅使用摄像头的3D检测 必须从2D图像推断深度，这本质上是更困难的。现代方法如BEVDet和BEVFormer使用Transformer架构将2D图像特征“提升”到3D。BEVFormer使用空间交叉注意力：BEV查询关注投影到每个摄像头图像上的特定3D参考点，从相关位置提取特征。

• 基于激光雷达和基于摄像头的3D检测之间的精度差距正在迅速缩小，这得益于更好的深度估计、更大的模型以及时序融合（利用多帧累积深度线索，类似于立体匹配跨时间的工作方式）。

深度估计

• 深度估计是为每个像素或每个点分配一个距离值的问题。

• 立体匹配 使用两个基线距离为 \(b\) 的摄像头。同一个3D点在两幅图像中的水平位置略有不同（即视差 \(d\)）。深度计算公式为（来自第8章）：

\[Z = \frac{f \cdot b}{d}\]

• 其中 \(f\) 是焦距。挑战在于找到两幅图像之间的正确对应关系，尤其是在纹理稀疏区域、遮挡区域和重复图案区域。现代立体匹配网络（例如RAFT-Stereo）使用带有迭代优化的相关体。

• 单目深度估计 从单张图像预测深度。由于这是不适定问题（无限多个3D场景可以产生相同的图像），网络必须学习统计先验：“地板是平的”、“物体距离越远看起来越小”、“纹理梯度指示后退的表面”。

• Depth Anything（在第8章中介绍）通过在大量未标注数据集上进行自监督训练，然后在标注数据上微调，实现了强大的单目深度估计。关键见解在于，尺度不变损失处理了固有的模糊性：模型预测的是相对深度（顺序）而不是绝对米数。

• 激光雷达-摄像头深度融合 将稀疏的激光雷达深度测量值投影到摄像头图像上作为监督信号。网络学习“填补”稀疏点之间的空隙，产生结合了激光雷达精度和摄像头分辨率的稠密深度图。

占用网络

• 传统的感知输出是一个边界框列表，每个检测到的物体对应一个框。但现实世界包含许多不适合用框来整齐表示的东西：形状奇怪的碎片、施工障碍物、悬垂的树枝、部分坍塌的墙壁。

• 占用网络 将场景表示为一个稠密的3D体素网格。每个体素（一个小的空间立方体，例如0.2m × 0.2m × 0.2m）被分类为空闲、占用或未知，并可选择赋予一个语义标签（道路、人行道、车辆、植被等）。

• 这是从以物体为中心的感知（“检测汽车”）向以场景为中心的感知（“3D空间的哪些部分被占用？”）的转变。其优势在于通用性：系统不需要预定义的目标类别列表就能避免与任意障碍物碰撞。

• 在架构上，占用网络接收传感器输入（摄像头、激光雷达或两者），将其编码为3D特征体，然后预测每个体素的标签。3D特征体通常通过将2D特征提升到3D（类似于BEV的构建，但扩展到垂直方向）来构建，并使用3D卷积或稀疏卷积进行处理。

• TPVFormer（三视角视图）通过将3D体分解为三个正交平面（俯视、前视、侧视）来避免完整3D注意力的立方成本。每个平面使用2D注意力，它们的特征在每个体素处组合。这让人联想到SVD如何将矩阵分解为更简单的因子（第2章）：将一个困难的3D问题分解为可管理的2D部分。

• 输出的体素网格直接告诉规划器哪些空间区域是安全的、可以占据，哪些是不安全的，使其成为感知和规划之间的自然接口。

车道检测与道路拓扑

• 对于在结构化道路上行驶的车辆，理解车道几何至关重要。系统必须知道车道在哪里、如何弯曲、在哪里合并和分叉，以及车辆当前在哪条车道上。

• 经典方法对检测到的车道标记拟合参数曲线。一个常用模型是三次多项式：

\[x(y) = a_0 + a_1 y + a_2 y^2 + a_3 y^3\]

• 其中 \(y\) 是前方的纵向距离，\(x\) 是横向偏移。这是一个多项式近似（回顾第3章的泰勒级数），因为道路是平滑曲线，可以用低次多项式很好地捕获。系数通过对检测到的车道点进行最小二乘回归来估计。

• 现代方法使用神经网络直接检测车道。LaneNet 将每条车道视为一个实例，并使用一个嵌入分支来对属于同一车道的像素进行分组，然后进行曲线拟合。GANet 使用基于图的方法，将车道拓扑表示为有向图，其中节点是车道点，边编码连接性（哪些车道合并、分叉或在交叉口连接）。

• 道路拓扑 超越了单条车道曲线，捕捉完整的结构：车道之间如何连接，哪些车道允许左转，高速公路入口匝道在哪里汇入。这被建模为一个有向图，其中交叉口是节点，车道段是带有属性（限速、车道类型、转向限制）的边。

• 图结构对于路径规划至关重要：规划器不仅需要知道“车道在哪里”，还需要知道“哪条车道序列能通往目的地”。

语义建图

• 感知并不止于在单帧中检测物体。随着时间的推移，自主系统会构建一个语义地图：一个持久、结构化的环境表示，能够累积多次观测的信息。

• 最简单的语义地图是一个2D网格（占据栅格），其中每个单元存储被占据的概率。当机器人移动并用传感器扫描时，它使用贝叶斯更新来更新这些概率：

\[P(\text{occupied} \mid z_{1:t}) = \frac{P(z_t \mid \text{occupied}) \cdot P(\text{occupied} \mid z_{1:t-1})}{P(z_t)}\]

• 这就是贝叶斯定理的实际应用（来自第5章）：每个新的测量值 \(z_t\) 更新每个单元的先验置信度。对数几率表示常用于避免乘许多小概率带来的数值问题：

\[l_t = l_{t-1} + \log \frac{P(z_t \mid \text{occupied})}{P(z_t \mid \text{free})}\]

• 加上对数几率相当于乘以概率（回忆 \(\log(ab) = \log a + \log b\)），并且运行总和自然地随时间累积证据。

• 更丰富的地图为每个单元分配语义标签（道路、人行道、建筑物、植被），并且可以扩展到3D。这些与占用网络密切相关，但强调持久性和时间聚合，而不是单帧预测。

• SLAM（同时定位与建图）在第8章中介绍，是在建图的同时跟踪机器人自身位置的算法。视觉-惯性SLAM融合摄像头和IMU数据；激光雷达SLAM使用点云配准。感知流程将检测结果和深度估计送入SLAM系统，后者维护全局地图。

• 现代方法越来越多地使用神经隐式表示（如第8章的NeRF）来构建密集、逼真的地图，可以在任意3D点进行查询。这些神经地图将整个场景的压缩表示存储在网络权重中，能够实现新颖视图合成和详细的空间查询等任务。

编程任务（使用CoLab或notebook）

1. 使用投影矩阵将3D激光雷达点投影到2D摄像头图像上。可视化哪些点落在图像边界内。

   import jax.numpy as jnp
   import matplotlib.pyplot as plt
   
   # 模拟的3D激光雷达点 (x=前向, y=左向, z=向上)
   rng = jax.random.PRNGKey(0)
   points_3d = jax.random.uniform(rng, (200, 3), minval=jnp.array([5, -10, -2]),
                                   maxval=jnp.array([50, 10, 3]))
   
   # 摄像头内参矩阵 (焦距500, 图像中心320x240)
   K = jnp.array([[500, 0, 320],
                  [0, 500, 240],
                  [0,   0,   1.0]])
   
   # 外参: 激光雷达到摄像头 (单位旋转, 小平移)
   R = jnp.eye(3)
   t = jnp.array([0.0, 0.0, -0.5])
   
   # 投影: p_cam = K @ (R @ p_lidar + t)
   p_cam = (R @ points_3d.T).T + t
   p_img = (K @ p_cam.T).T
   p_img = p_img[:, :2] / p_img[:, 2:3]  # 除以 Z
   
   # 滤除摄像头前方的点以及图像内的点
   mask = (p_cam[:, 2] > 0) & (p_img[:, 0] > 0) & (p_img[:, 0] < 640) & \
          (p_img[:, 1] > 0) & (p_img[:, 1] < 480)
   depth = p_cam[mask, 2]
   
   plt.figure(figsize=(8, 5))
   plt.scatter(p_img[mask, 0], p_img[mask, 1], c=depth, cmap="viridis", s=5)
   plt.colorbar(label="深度 (米)")
   plt.xlim(0, 640); plt.ylim(480, 0)
   plt.title("投影到摄像头图像上的激光雷达点")
   plt.xlabel("u (像素)"); plt.ylabel("v (像素)")
   plt.show()
   

2. 构建一个简单的2D占据栅格，使用贝叶斯对数几率更新。模拟一个距离传感器扫描环境，观察地图逐渐形成。

   import jax
   import jax.numpy as jnp
   import matplotlib.pyplot as plt
   
   # 网格设置: 50x50 个单元，每个 0.2 米
   grid_size = 50
   log_odds = jnp.zeros((grid_size, grid_size))
   
   # 传感器模型: 对数几率更新值
   l_occ = 0.85   # 击中表示占据的置信度
   l_free = -0.4  # 穿过表示空闲的置信度
   
   # 模拟障碍物: 网格坐标中的一面墙，从 (5,20) 到 (5,30)
   wall_y = jnp.arange(20, 30)
   
   # 机器人在 (25, 25)，向外扫描
   robot = jnp.array([25, 25])
   
   for angle_deg in range(0, 360, 5):
       angle = jnp.radians(angle_deg)
       direction = jnp.array([jnp.cos(angle), jnp.sin(angle)])
   
       for step in range(1, 25):
           cell = (robot + direction * step).astype(int)
           r, c = int(cell[0]), int(cell[1])
           if r < 0 or r >= grid_size or c < 0 or c >= grid_size:
               break
   
           # 检查这个单元是否是墙
           is_wall = (r == 5) and (c >= 20) and (c < 30)
           if is_wall:
               log_odds = log_odds.at[r, c].add(l_occ)
               break
           else:
               log_odds = log_odds.at[r, c].add(l_free)
   
   # 将对数几率转换为概率
   prob = 1.0 / (1.0 + jnp.exp(-log_odds))
   
   plt.figure(figsize=(6, 6))
   plt.imshow(prob.T, origin="lower", cmap="RdYlGn_r", vmin=0, vmax=1)
   plt.colorbar(label="P(占据)")
   plt.plot(25, 25, "b*", markersize=10, label="机器人")
   plt.legend()
   plt.title("来自贝叶斯更新的2D占据栅格")
   plt.show()
   

3. 使用视差从立体图像对计算深度。模拟两个相机视角下的3D点，计算视差，并恢复深度。

   import jax
   import jax.numpy as jnp
   
   # 相机参数
   f = 500.0     # 焦距（像素）
   b = 0.12      # 基线（米），12厘米
   
   # 已知深度的3D点
   depths_true = jnp.array([5.0, 10.0, 20.0, 50.0, 100.0])
   
   # 视差 = f * b / Z
   disparities = f * b / depths_true
   
   # 从视差恢复深度
   depths_recovered = f * b / disparities
   
   for z, d, z_r in zip(depths_true, disparities, depths_recovered):
       print(f"真实深度: {z:6.1f}米  视差: {d:6.2f}像素  恢复深度: {z_r:6.1f}米")
   
   # 注意：视差与深度成反比
   # 近处物体视差大，远处物体视差小
   # 这就是立体视觉在短距离最准确的原因