4. 环境感知与动态障碍物表示

在动态场景中，轨迹重规划的质量并不只取决于优化器本身，更取决于感知系统能否稳定、连续、可预测地描述周围障碍物。若感知输出只是“某一帧有一辆车在某个位置”，规划器只能做瞬时避让；只有当感知系统给出目标身份、运动状态、未来轨迹及其不确定性时，规划器才能做出有时域一致性的决策。本章围绕传感器融合、目标跟踪、轨迹预测与不确定性建模四个环节，说明动态障碍物如何从原始观测变成规划可用的结构化输入。

一、核心原理：从观测到可规划对象

环境感知的目标不是“看见物体”，而是为规划提供带时间戳、带协方差、带语义类别、带预测分布的动态对象。典型链路如下：多传感器原始数据先经过时间同步与空间标定，进入融合层形成统一障碍物列表；跟踪层为每个目标分配稳定 ID 并估计位置、速度、加速度；预测层输出未来 3～8 秒的多模态轨迹；不确定性层将观测噪声、预测误差和交互不确定性量化为协方差或概率分布，最终交给规划器作为动态约束。

二、方法流程：融合、跟踪与预测的关键步骤

1. 传感器融合。常用做法分为前融合与后融合。前融合在点云或特征层拼接，信息损失小但对标定和算力要求高；后融合在目标级融合，工程落地更稳。融合前必须完成时间同步，通常以激光雷达帧为基准，将雷达和相机观测插值到同一时刻。

# 目标级融合伪代码
for each radar_track in radar_tracks:
    matched = associate(radar_track, camera_objects, gate=2.5m)
    if matched:
        fused_state = kalman_update(radar_track.state, matched.measurement)
        fused_cov   = update_covariance(radar_track.cov, matched.cov)
    else:
        fused_state = radar_track.state  # 保留单传感器航迹

2. 目标跟踪。跟踪的核心是“预测—关联—更新”循环。预测用运动模型外推，关联用马氏距离或匈牙利算法把观测分配给已有航迹，更新用卡尔曼滤波修正状态与协方差。工程上还要处理航迹起始、确认、丢失与终止，避免 ID 频繁跳变。

3. 轨迹预测。短时预测可用恒定速度或恒定加速度模型；中长时预测需引入道路结构、历史轨迹和交互意图。多模态预测输出若干条候选轨迹及其概率，例如“直行 0.6、左转 0.3、右转 0.1”。

4. 不确定性建模。不确定性至少包含三类：观测噪声、状态估计误差、预测模型误差。规划器通常使用协方差椭圆或概率占据栅格，将不确定性转化为安全裕度。

三、案例场景：十字路口右转遇行人

自车在十字路口右转，右前方人行横道有一名行人。感知系统输出：行人 ID=17，位置 (12.4, 3.1)，速度 1.2 m/s，方向朝北，预测轨迹两条——继续过街概率 0.7，折返概率 0.3。若规划器只取最大概率轨迹，可能选择紧贴行人后方通过；但折返概率 0.3 对应横向协方差较大，安全裕度不足。正确做法是把两条轨迹都作为动态约束，取并集的概率占据区域，并降低通过速度。

四、误区与复盘

综上，动态障碍物表示是轨迹重规划的前置基础。传感器融合解决“看得全”，目标跟踪解决“跟得稳”，轨迹预测解决“想得远”，不确定性建模解决“信得过”。四者共同构成规划器可消费的动态环境模型，缺一不可。