Apollo 速度决策

Apollo 速度决策

1. 介绍

本 Task 的作用是根据 DP 的结果对每个障碍物做决策. 从而为调整速度边界提供依据. (目前业内此 Task 基本被单障碍物决策模块所取代)

附赠自动驾驶最全的学习资料和量产经验以及100T的资源分享：链接

2. 代码解析

2.1. 总体流程

SpeedDecider::Execute: 调用MakeObjectDecision对障碍物做纵向决策.

1. 输入: 速度曲线speed_profile, 障碍物信息(从path_decision中提取).

2. 遍历所有的障碍物:

   1. 当障碍物的 Boundary 异常的时候, 比如 Boundary 为空(说明预测轨迹与自车 Path 没有交点), 或者 Boundary 的 T 或者 S 越界的时候, 直接做 Ignore 决策.

   2. 如果上游已经有模块做了纵向决策了, 则做 Ignore 决策. 此步骤和步骤 a 都是调用的AppendIgnoreDecision, 目的是确保横向决策和纵向决策都是 Ignore.

   3. 如果是虚拟障碍物并且其中心点不在自车所在车道上, 则不做决策.

   4. 调用CheckStopForPedestrian判断是否应该对行人做 Stop 决策.

   5. 调用GetSTLocation判断速度曲线是在障碍物 ST Boundary 的上方还是下方.

   6. 根据速度曲线与 ST Boundary 的位置关系赋予障碍物决策.

      1. 如果速度曲线在 ST Boundary 下方, 应该做让行决策(Stop 和 Follow 都可以看作特殊场景下的 Yield). 对于一些特殊情况, 比如 Keep Clear 区域, 应该做 Stop 决策; 自车前方障碍物, 应该做 Follow 决策; 如果跟车太近, 应该做 Stop 决策.

      2. 如果速度曲线在 ST Boudary 上方, 应该做超车决策(Ignore 可以看作特殊场景下的 Overtake).

      3. 如果速度曲线穿 ST Boudary 而过(一般情况下不会发生, 需要排查异常原因). 此时如果障碍物 Blocking 自车道, 则直接做 Stop 决策.

   7. 最后调用AppendIgnoreDecision为每一个没有还没有做决策的障碍物加上 Ignore 决策. 这样做的目的是确保每一个障碍物都是有决策的. 正常情况下, 上游已经对障碍物都做了决策, 如果有遗漏的, 需要排查遗漏的原因.

3. 输出: 障碍物的决策信息.

2.2. 代码细节

CheckStopForPedestrian: 判断是否应该对行人做 Stop 决策.

1. 输入: 行人的感知信息

2. 由于本函数只处理行人相关的决策, 所以需要先判断感知类型是否是行人.

3. 如果行人在自车车屁股后面(obstacle_sl_boundary.end_s() < adc_sl_boundary_.start_s()), 则无需对其停车.

4. 根据行人的静止时间判断是否应该做 Stop 决策.

   1. 读取行人静止的时间, 这个信息很重要, 可以用来区分行人是长时间静止的, 还是由于与自车交互被逼停的.

   2. kMaxStopSpeed是判断行人是静止还是运动的速度阈值, 如果行人的速度大于这个阈值则认为其开始运动, 将其从stop_time_map删除. 如果行人小于这个阈值则认为其还是静止的. 对于静止时间较长的行人(大于 4 秒), 则认为他在等自车通行, 无需做 Stop 决策, 将result设置成false.

   3. 在返回result前, 需要将步骤 b 的得到行人时间戳(timestamp)更新到全局状态变量mutable_speed_decider_status中, 供下一帧计算行人静止的时间.

5. 输出: 是否应该对该行人做 Stop 决策.

GetSTLocation: 判断速度曲线是在障碍物 ST Boundary 的上方还是下方.

1. 输入: 速度曲线speed_profile, 障碍物 ST Boundary(boundary).

2. 遍历速度曲线上的每一个点, 得到当前遍历的点curr_st和下一个遍历的点next_st. 这两个点可以组成一个线段speed_line, 如果这个线段和障碍物的st_boundary有 Overlap, 则st_location是 Cross, 即速度曲线穿过了st_boundary.

3. 如图用两个向量叉乘(紫色带箭头虚直线表示, 结果存在side中)的正负可以判断速度曲线是在 ST Boudnary 的上方还是下方. (Crossing 状态已经在步骤 2 被排除).

4. 输出: STLocation, 有三种情况: 速度曲线在 Boundary 的上方或者下方, 或者速度曲线穿过了 Boudnary.

图 2.1 判断速度曲线与 ST Boundary 的相对位置关系