1.12 Speed Decider

Posted on Edited on

前面通过DP获得了一个大致的速度折线，在时间上没有和障碍物发生碰撞。

接下来判断障碍物相对于该速度曲线的位置，

在曲线“上方”，需要判断是否需要避让来保持安全距离

在曲线“下方”，需要判断超车处理

遍历obs，依次判断以下条件：

  1. 忽略掉空间或者时间上不相关的obs
1
2
3
4
5
6
if (boundary.IsEmpty() || boundary.max_s() < 0.0 ||
    boundary.max_t() < 0.0 ||
    boundary.min_t() >= speed_profile.back().t()) {
  AppendIgnoreDecision(mutable_obstacle);
  continue;
}
  1. 忽略掉已经有纵向决策的obs
1
2
3
4
if (obstacle->HasLongitudinalDecision()) {
  AppendIgnoreDecision(mutable_obstacle);
  continue;
}
  1. 忽略掉不相关的虚拟的obs
1
2
3
4
5
6
7
// for Virtual obstacle, skip if center point NOT "on lane"
if (obstacle->IsVirtual()) {
  const auto& obstacle_box = obstacle->PerceptionBoundingBox();
  if (!reference_line_->IsOnLane(obstacle_box.center())) {
    continue;
  }
}
  1. 遇到人行横道，需要停下来观察一下
1
2
3
4
5
6
7
8
9
10
// always STOP for pedestrian
if (CheckStopForPedestrian(*mutable_obstacle)) {
  ObjectDecisionType stop_decision;
  if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                         -FLAGS_min_stop_distance_obstacle)) {
    mutable_obstacle->AddLongitudinalDecision("dp_st_graph/pedestrian",
                                              stop_decision);
  }
  continue;
}

  1. 剩下的常规处理，先判断自车boundary是在上方还是下方

    1. 自车在boundary的下方，依次检查stop、follow、yield
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    16
    17
    18
    19
    20
    21
    22
    23
    24
    25
    26
    27
    28
    29
    30
    31
    32
    33
    case BELOW:
            if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
              ObjectDecisionType stop_decision;
              if (CreateStopDecision(*mutable_obstacle, &stop_decision, 0.0)) {
                mutable_obstacle->AddLongitudinalDecision("dp_st_graph/keep_clear",
                                                          stop_decision);
              }
            } else if (CheckIsFollow(*obstacle, boundary)) {
              // stop for low_speed decelerating
              if (IsFollowTooClose(*mutable_obstacle)) {
                ObjectDecisionType stop_decision;
                if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                                       -FLAGS_min_stop_distance_obstacle)) {
                  mutable_obstacle->AddLongitudinalDecision("dp_st_graph/too_close",
                                                            stop_decision);
                }
              } else {  // high speed or low speed accelerating
                // FOLLOW decision
                ObjectDecisionType follow_decision;
                if (CreateFollowDecision(*mutable_obstacle, &follow_decision)) {
                  mutable_obstacle->AddLongitudinalDecision("dp_st_graph",
                                                            follow_decision);
                }
              }
            } else {
              // YIELD decision
              ObjectDecisionType yield_decision;
              if (CreateYieldDecision(*mutable_obstacle, &yield_decision)) {
                mutable_obstacle->AddLongitudinalDecision("dp_st_graph",
                                                          yield_decision);
              }
            }
            break;
    1. 自车在boundary的上方，检查overtake
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    case ABOVE:
            if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
              ObjectDecisionType ignore;
              ignore.mutable_ignore();
              mutable_obstacle->AddLongitudinalDecision("dp_st_graph", ignore);
            } else {
              // OVERTAKE decision
              ObjectDecisionType overtake_decision;
              if (CreateOvertakeDecision(*mutable_obstacle, &overtake_decision)) {
                mutable_obstacle->AddLongitudinalDecision("dp_st_graph/overtake",
                                                          overtake_decision);
              }
            }
            break;
    1. 有交叉，需要停车处理
    1
    2
    3
    4
    5
    6
    7
    8
    9
    10
    11
    12
    13
    14
    15
    case CROSS:
            if (mutable_obstacle->IsBlockingObstacle()) {
              ObjectDecisionType stop_decision;
              if (CreateStopDecision(*mutable_obstacle, &stop_decision,
                                     -FLAGS_min_stop_distance_obstacle)) {
                mutable_obstacle->AddLongitudinalDecision("dp_st_graph/cross",
                                                          stop_decision);
              }
              const std::string msg = absl::StrCat(
                  "Failed to find a solution for crossing obstacle: ",
                  mutable_obstacle->Id());
              AERROR << msg;
              return Status(ErrorCode::PLANNING_ERROR, msg);
            }
            break;