战斗包子
rule based预测模块算法

rule based预测模块算法

2025年10月29日 3.1k 字 大概 13 分钟

本文由AI根据1月版本代码梳理

自动驾驶预测(Prediction)模块 - 框架与算法详解

1. 模块核心概述

该预测模块的核心使命是：为感知系统提供的每个障碍物，生成其在未来一段时间（例如 5 秒）内一条或多条可能的运动轨迹及其概率。

1.1. 核心设计哲学

该框架采用了**“策略模式 (Strategy Pattern)”与“分层降级 (Hierarchical Fallback)”**相结合的经典设计：

  1. 策略模式：

    • 系统（由 PredictorManager 代表）维护一个“算法工具箱”（std.map<PredictorType, Predictor*> predictors_）。
    • 对于不同类型、不同状态的障碍物（如“车道上的车”、“路口的人”），管理器会动态选择（或配置）不同的预测器（“策略”）来处理它。

  2. 分层降级 (Hierarchical Fallback)：

    • 这是本模块的灵魂。系统不会盲目信任任何单一算法。
    • L1 (首选/高清地图)： 优先尝试最高精度的预测器（如 LaneSequencePredictor），它利用高清地图车道信息进行精确预测。
    • L2 (备选/运动学)： 如果 L1 失败（例如，障碍物不在地图车道上，LaneSequencePredictor 返回 false），系统立即降级到 L2 运动学模型（如 CTRV - 恒定转向率模型）。
    • L3 (兜底/运动学)： 如果 L2 也失败（例如，历史数据不足以计算转向率），系统再次降级到 L3（如 CV - 恒定速度模型），做直线外推。
    • L4 (安全约束)： 无论使用哪种模型，所有轨迹都会经过一层硬安全约束检查（如 CheckCrossWithCurb），确保轨迹不会“飞檐走壁”。

2. 核心框架与组件

下面是这个预测模块的组件架构，它清晰地展示了“管理者-工具箱-工人”的层次结构。

classDiagram
    direction LR
    class PredictorManager {
        << (1) 管理者 >>
        -predictors_ : map
        +Run()
        +PredictObstacle()
        +RunVehiclePredictor()
        +RunEmptyPredictor()
    }
    class Predictor {
        << (2) 抽象基类 >>
        +virtual Predict()
        +GenerateTrajectory()
        +SetEqualProbability()
    }
    class SequencePredictor {
        << (3) 抽象工具箱 >>
        +FilterLaneSequences()
        +GetLongitudinalPolynomial()
        +GetLateralPolynomial()
    }
    class LaneSequencePredictor {
        << (4) L1具体实现 >>
        +Predict()
        +DrawLaneSequenceTrajectoryPoints()
    }

    PredictorManager ..> Predictor : 持有(holds)
    Predictor <|-- SequencePredictor : 继承(inherits)
    SequencePredictor <|-- LaneSequencePredictor : 继承(inherits)

2.1. PredictorManager (管理者)

这是模块的**“总调度器”和“安全网”**。

  • 职责 1 - 管理： 在构造时 RegisterPredictors()，将所有具体的预测器（如 LaneSequencePredictor）实例化并存入 predictors_ 中。
  • 职责 2 - 调度： Run() 是主入口。它负责启用多线程（PredictObstaclesInParallel），并将障碍物分发给线程池（PredictionThreadPool）。
  • 职责 3 - 决策（L0/L1）： PredictObstacle() 是核心调度函数。
    • L0 决策： 它首先检查障碍物是否 IsStill() 或 ToIgnore()。如果是，则调用 RunEmptyPredictor()，流程结束。
    • L1 决策： 如果是动态障碍物，它调用 RunVehiclePredictor()。此函数从 predictors_ 中取出 L1 预测器（即 LaneSequencePredictor），并执行 predictor->Predict(...)。
  • 职责 4 - 降级（L2/L3）： PredictObstacle() 最关键的职责。
    • 在 L1 预测器执行完毕后，它会检查 flag_free_move_predict（该标志位在 L1 失败或预测对象为行人/自行车时为 true）。
    • 如果需要降级，它会亲自执行 L2(CTRV) 或 L3(CV) 预测，并调用 CheckCrossWithCurb() 进行安全校验。

2.2. Predictor (抽象基类)

这是所有预测器的“蓝图”。

  • 职责 1 - 契约： 定义纯虚函数 Predict()，强制所有子类都必须实现这个核心功能。
  • 职责 2 - 工具： 提供所有子类都可能用到的公共辅助函数，如 GenerateTrajectory()（将 std::vector 点集转为 Trajectory 消息）和 SetEqualProbability()（为多条轨迹分配平均概率）。

2.3. SequencePredictor (抽象工具箱)

这是专门为“序列”（尤其是车道序列）预测器提供高级算法的“数学工具箱”。

  • 职责 1 - 序列过滤： 提供 FilterLaneSequences()。这是极其重要的启发式（Heuristic）剪枝算法，用于在多条可能的车道序列中，过滤掉不合理的、低概率的选项。
  • 职责 2 - 轨迹数学： 提供 GetLongitudinalPolynomial() 和 GetLateralPolynomial()。这些函数用于求解多项式，以生成在 S-L（Frenet）坐标系下，从当前状态平滑过渡到目标状态（如车道中心）的轨迹。

2.4. LaneSequencePredictor (L1 具体实现)

这是**“主力预测器”**，负责处理在高清地图车道上的障碍物。

  • 职责 1 - L1 实现： 它是 SequencePredictor 的子类，真正实现了 Predict() 函数。
  • 职责 2 - 流程编排： 它的 Predict() 函数是一个清晰的执行流：
    1. 检查 has_lane_graph()（L1 的准入条件）。
    2. 调用父类的 FilterLaneSequences() 进行剪枝。
    3. 从过滤后的序列中，决策出最佳序列（max_prob_index）。
    4. 调用自己的 DrawLaneSequenceTrajectoryPoints() 生成轨迹点。
    5. 调用基类的 GenerateTrajectory() 打包轨迹。

3. 核心预测流程 - 算法详解

以下是预测模块处理一个动态汽车的完整算法流程。

graph TD
    A[开始: 获得障碍物] --> B{L0: 静态或忽略?}
    B -- 是 --> C[RunEmptyPredictor]
    B -- 否 --> D[RunVehiclePredictor]
    D --> E[L1 LaneSequencePredictor.Predict]
    E --> F{L1 预测成功? 有车道图且返回True}
    F -- 是 --> G[添加 L1 轨迹]
    F -- 否 L1失败 --> H[L2/L3 降级逻辑]
    H --> I{L2: 历史点 >= 5?}
    I -- 是 --> J[执行 L2  CTRV  运动学外推]
    I -- 否 --> K{L3: 历史点 > 1?}
    K -- 是 --> L[执行 L3  CV  运动学外推]
    K -- 否 --> M[无法预测, 无轨迹]
    
    J --> N[L4: 安全约束检查]
    L --> N[L4: 安全约束检查]
    N --> O{轨迹是否穿过马路牙子?}
    O -- 是 --> P[截断 Truncate 轨迹]
    O -- 否 --> Q[保留完整轨迹]
    
    P --> R[添加 L2/L3 轨迹]
    Q --> R
    
    C --> Z[结束]
    G --> Z
    M --> Z
    R --> Z

阶段 0：分发与 L0 分诊 (PredictorManager)

  1. PredictorManager::Run() 启动，PredictObstacle() 获得一个障碍物 obstacle。
  2. [L0 过滤] if (obstacle->IsStill()) 检查。
  3. 假设： 障碍物是动态的。IsStill() 为 false。
  4. RunVehiclePredictor() 被调用。

阶段 1：L1 预测 - 高清地图车道 (LaneSequencePredictor)

  1. RunVehiclePredictor() 从 predictors_ 中获取 LANE_SEQUENCE_PREDICTOR（即 LaneSequencePredictor 实例）。
  2. LaneSequencePredictor::Predict() 被调用。
  3. [L1 准入] if (!feature->has_lane_graph()) 检查。
    • 如果为 true（无车道图）： Predict() 立刻 return false。L1 预测失败。流程跳转到**[阶段 2]**。
    • 假设： L1 准入通过。
  4. [L1 算法 - 步骤 1：序列过滤] (详见 4.1 节)
    • 调用 SequencePredictor::FilterLaneSequences()。
    • 对 LaneGraph 中所有可能的序列，基于概率、冗余、启发式场景规则进行“剪枝”。
    • 输出： enable_lane_sequence 布尔向量，标记哪些序列是合理的。
  5. [L1 算法 - 步骤 2：决策最佳序列]
    • 遍历所有 enable_lane_sequence == true 的序列。
    • 使用两个标准寻找 max_prob_index：
      1. 序列概率 sequence.probability()（概率越大越好）。
      2. 航向角差异 std::fabs(common::math::AngleDiff(feature->theta(), ...))（角度差越小越好）。
    • （注：根据代码，角度差（标准2）的优先级更高，它会覆盖掉概率（标准1）的选择）。
  6. [L1 算法 - 步骤 3：生成轨迹点] (详见 4.2 节)
    • 调用 LaneSequencePredictor::DrawLaneSequenceTrajectoryPoints()。
    • 这是一个**“混合”轨迹生成器，它拼接**了两种轨迹：
      • 初始段： 3阶多项式 平滑轨迹（backup_points），确保起点与障碍物当前姿态（位置、速度）完美匹配。
      • 后半段： S-L 坐标系 轨迹（origin_points），确保轨迹“吸附”回车道中心线。
    • 最后，对拼接后的轨迹进行按时间步重采样，生成最终点集 points。
  7. [L1 结果]
    • GenerateTrajectory(points) 被调用，生成 Trajectory 消息。
    • obstacle->add_predicted_trajectory(...) 将轨迹存入障碍物。
    • LaneSequencePredictor::Predict() 返回 true。L1 预测成功。
    • 流程回到 PredictorManager::PredictObstacle()。

阶段 2：L2/L3 降级 - 运动学 (PredictorManager)

  1. PredictorManager::PredictObstacle() 检查 L1 的结果。
  2. flag_free_move_predict = !obstacle->IsStill() && (obstacle->latest_feature().predicted_trajectory().empty() || ...)
  3. 假设： L1 预测失败（return false），predicted_trajectory().empty() 为 true。
  4. flag_free_move_predict 被设为 true。
  5. [L2 降级 - CTRV 模型]
    • if (x_v.size() >= 5) 检查（历史点是否足够多）。
    • 假设： 历史点足够。
    • 算法：
      • a. 计算转向率 yaw_rate：通过历史的 velocity_heading() 或 theta() 变化，计算出平均角速度 θ˙\dot{\theta}。
      • b. 循环推演：在 for 循环中，按 period 递推：
        • heading = latest_heading + yaw_rate * i * period
        • velocity_x = speed * std::cos(heading)
        • x = last_x + velocity_x * period
      • 结果： 生成一条恒定转向率的轨迹 trajectory。
  6. [L3 降级 - CV 模型]
    • else if (obstacle->history_size() > 1)（L2 失败，但 L3 历史点足够）。
    • 假设： L2 失败。
    • 算法：
      • PredictWithLinearSeg() 被调用。
      • velocity_x = speed * std::cos(heading)
      • x = obstacle->latest_feature().position().x() + i * velocity_x * period
      • 结果： 生成一条恒定速度的直线轨迹 trajectory。
  7. [L4 安全约束]
    • flag_cross_with_curb = CheckCrossWithCurb(trajectory, ...) 被调用。
    • 算法：
      • 遍历 L2/L3 生成的轨迹 trajectory 的每一条线段。
      • 遍历地图中周围的马路牙子（Curb）/实线的每一条线段。
      • 进行线段相交测试 trajectory_seg.GetIntersect(lane_line_Seg, ...)。
    • 结果：
      • 如果相交，flag_cross_with_curb = true，并返回碰撞点索引 cross_trajectory_index。
      • trajectory.mutable_trajectory_point()->erase(...)：截断轨迹，只保留碰撞前的部分。
  8. [L2/L3 结果]
    • 降级生成的（可能被截断的）轨迹被 add_trajectory()。
    • 流程结束。

阶段 3：输出

PredictorManager::Run() 结束，prediction_obstacles_ 中包含了所有障碍物的轨迹（L1 或 L2/L3 生成的）。

4. 关键算法深度解析

4.1 深度解析 1：FilterLaneSequences (启发式序列剪枝)

  • 目标： LaneGraph 提供了所有“拓扑上”可能的路径，但很多路径在“物理上”或“意图上”是不合理的。此函数负责剪枝。
  • 算法步骤：
    1. 概率剪枝：
      • 找到“概率最高”的序列 all.first。
      • 任何序列的 probability < FLAGS_lane_sequence_threshold_cruise 都会被禁用，除非它是那条“概率最高”的。
    2. 冗余剪枝：
      • 找到“概率最高”的换道序列 change.first。
      • 如果有多条“左换道”序列，只保留概率最高的那条，禁用其他所有“左换道”序列。
    3. 场景剪枝（长尾补丁）：
      • 这是针对路口交叉/对向来车（diff_velocity_heading > M_PI * 2 / 3）场景的启发式规则。
      • 它检查障碍物是否在转弯道上，并检查其横向速度 raw_velocity().y()。
      • 规则： 如果一个障碍物在“右转”道上，但其横向速度（raw_velocity().y()）显示它在“向左”运动（即并没有真的在右转），则禁用这条“右转”序列。
      • 反之： 如果它在转弯道上，且横向速度匹配转弯意图，则强制选用该序列，并禁用所有其他序列（如直行）。

4.2 深度解析 2：DrawLaneSequenceTrajectoryPoints (混合轨迹生成)

  • 目标： 生成一条平滑、连续、物理可信、且最终“吸附”到车道中心的轨迹。

  • 算法步骤：

        graph TD
      subgraph 阶段 1：S-L 原始轨迹推演
          A1[按恒定加速度, 在 S-L 坐标系下推演]
          A2[L(横向) 按 `approach_rate` 指数衰减]
          A3[将 (S, L) 查地图转为 (X, Y)]
          A4[结果: origin_points (起点不平滑)]
      end
      
      subgraph 阶段 2：3阶多项式初始段
          B1[在 origin_points 上找到"锚点" check_s_point]
          B2[求解3阶多项式, 边界条件:]
          B3["t=0: (pos, vel) = 障碍物当前状态"]
          B4["t=T: (pos, vel) = 锚点状态"]
          B5[结果: backup_points (起点平滑)]
      end
      
      subgraph 阶段 3：拼接与重采样
          C1[拼接: backup_points + origin_points(后半段)]
          C2[计算拼接后轨迹的总弧长 S_total]
          C3[按时间步 t 在弧长 S 上进行插值]
          C4[结果: points (最终轨迹)]
      end

      A4 --> B1;
  B5 --> C1;</pre>

    

      

    1. 阶段 1：(S-L) 原始轨迹推演

        

      • 在 for 循环中，沿车道中心线（S方向）做恒定加速度运动 (lane_s += ...)。
        • 

      • 同时，向车道中心线（L方向）做指数衰减运动 (lane_l *= approach_rate)，确保“吸附”回车道。
        • 

      • 结果： origin_points。一条完全贴合车道、但**在起点处与障碍物当前姿态有“跳变”**的轨迹。
        • 

      

      2. 

    2. 阶段 2：(Cubic) 初始段平滑

        

      • 为了解决“跳变”问题，它在 origin_points 上找到一个“锚点” check_s_point。
        • 

      • 调用 ComputePolynomial<3>(...) 求解一个 3 阶多项式，该多项式可以完美地连接障碍物的当前状态（位置 x,yx, y 和速度 vx,vyv_x, v_y）与未来“锚点”的目标状态。
        • 

      • 结果： backup_points。一条从障碍物当前位置平滑“飞”向车道锚点的轨迹。
        • 

      

      3. 

    3. 阶段 3：(Stitch) 拼接与重采样

        

      • 拼接： backup_points.insert(backup_points.end(), origin_points.begin() + check_s_size, ...)，将**阶段2的“平滑段”和阶段1的“S-L后半段”**拼接在一起。
        • 

      • 重采样： 拼接后的轨迹 backup_points 形状是对的，但时间不对（轨迹点不是等时间间隔的）。
        • 

      • 最后一个 for 循环（tmp_s 和 points_s）执行了**“按时间步重采样”。它计算拼接后轨迹的总“弧长” (points_s)，然后按固定的时间步**（tmp_s）在弧长上进行线性插值，生成最终等时间间隔的 points。
        • 

      

      4.

5. 总结

该预测模块是一个健壮的、具有工业级强度的系统。它以高清地图（L1）为主要预测依据，保证了预测的“意图性”和“准确性”；同时，它又以纯运动学（L2/L3）和硬安全约束（L4）作为强大的“降级”和“兜底”手段，保证了系统在 L1 失败（如地图数据缺失、障碍物行为异常）时的健壮性。

本文作者：战斗包子
本文链接：https://paipai121.github.io/2025/10/29/工作/rule-based预测模块算法/
版权声明：本文采用 CC BY-NC-SA 3.0 CN 协议进行许可