Apollo 速度决策
1. 介绍
本 Task 的作用是根据 DP 的结果对每个障碍物做决策. 从而为调整速度边界提供依据. (目前业内此 Task 基本被单障碍物决策模块所取代)
附赠自动驾驶最全的学习资料和量产经验以及100T的资源分享:链接
2. 代码解析
2.1. 总体流程
SpeedDecider::Execute
: 调用MakeObjectDecision
对障碍物做纵向决策.
-
输入: 速度曲线
speed_profile
, 障碍物信息(从path_decision
中提取). -
遍历所有的障碍物:
-
当障碍物的 Boundary 异常的时候, 比如 Boundary 为空(说明预测轨迹与自车 Path 没有交点), 或者 Boundary 的 T 或者 S 越界的时候, 直接做 Ignore 决策.
-
如果上游已经有模块做了纵向决策了, 则做 Ignore 决策. 此步骤和步骤 a 都是调用的
AppendIgnoreDecision
, 目的是确保横向决策和纵向决策都是 Ignore. -
如果是虚拟障碍物并且其中心点不在自车所在车道上, 则不做决策.
-
调用
CheckStopForPedestrian
判断是否应该对行人做 Stop 决策. -
调用
GetSTLocation
判断速度曲线是在障碍物 ST Boundary 的上方还是下方. -
根据速度曲线与 ST Boundary 的位置关系赋予障碍物决策.
-
如果速度曲线在 ST Boundary 下方, 应该做让行决策(Stop 和 Follow 都可以看作特殊场景下的 Yield). 对于一些特殊情况, 比如 Keep Clear 区域, 应该做 Stop 决策; 自车前方障碍物, 应该做 Follow 决策; 如果跟车太近, 应该做 Stop 决策.
-
如果速度曲线在 ST Boudary 上方, 应该做超车决策(Ignore 可以看作特殊场景下的 Overtake).
-
如果速度曲线穿 ST Boudary 而过(一般情况下不会发生, 需要排查异常原因). 此时如果障碍物 Blocking 自车道, 则直接做 Stop 决策.
-
-
最后调用
AppendIgnoreDecision
为每一个没有还没有做决策的障碍物加上 Ignore 决策. 这样做的目的是确保每一个障碍物都是有决策的. 正常情况下, 上游已经对障碍物都做了决策, 如果有遗漏的, 需要排查遗漏的原因.
-
-
输出: 障碍物的决策信息.
2.2. 代码细节
CheckStopForPedestrian
: 判断是否应该对行人做 Stop 决策.
-
输入: 行人的感知信息
-
由于本函数只处理行人相关的决策, 所以需要先判断感知类型是否是行人.
-
如果行人在自车车屁股后面(
obstacle_sl_boundary.end_s() < adc_sl_boundary_.start_s()
), 则无需对其停车. -
根据行人的静止时间判断是否应该做 Stop 决策.
-
读取行人静止的时间, 这个信息很重要, 可以用来区分行人是长时间静止的, 还是由于与自车交互被逼停的.
-
kMaxStopSpeed
是判断行人是静止还是运动的速度阈值, 如果行人的速度大于这个阈值则认为其开始运动, 将其从stop_time_map
删除. 如果行人小于这个阈值则认为其还是静止的. 对于静止时间较长的行人(大于 4 秒), 则认为他在等自车通行, 无需做 Stop 决策, 将result
设置成false
. -
在返回
result
前, 需要将步骤 b 的得到行人时间戳(timestamp)更新到全局状态变量mutable_speed_decider_status
中, 供下一帧计算行人静止的时间.
-
-
输出: 是否应该对该行人做 Stop 决策.
GetSTLocation
: 判断速度曲线是在障碍物 ST Boundary 的上方还是下方.
-
输入: 速度曲线
speed_profile
, 障碍物 ST Boundary(boundary
). -
遍历速度曲线上的每一个点, 得到当前遍历的点
curr_st
和下一个遍历的点next_st
. 这两个点可以组成一个线段speed_line
, 如果这个线段和障碍物的st_boundary
有 Overlap, 则st_location
是 Cross, 即速度曲线穿过了st_boundary
. -
如图用两个向量叉乘(紫色带箭头虚直线表示, 结果存在
side
中)的正负可以判断速度曲线是在 ST Boudnary 的上方还是下方. (Crossing 状态已经在步骤 2 被排除). -
输出:
STLocation
, 有三种情况: 速度曲线在 Boundary 的上方或者下方, 或者速度曲线穿过了 Boudnary.
图 2.1 判断速度曲线与 ST Boundary 的相对位置关系