[아폴로 연구 노트]——계획 모듈 TASK의 SPEED_DECIDER

TASK 시리즈 분석 기사

1. [Apollo 스터디 노트] - 계획 모듈 TASK의 LANE_CHANGE_DECIDER
2. [Apollo 스터디 노트] - 계획 모듈 TASK의 PATH_REUSE_DECIDER
3. [Apollo 스터디 노트] - 계획 모듈 TASK PATH_BORROW_DECIDER
4. [Apollo 스터디 노트] -
— 계획 모듈 TASK 5 의 PATH_BOUNDS_DECIDER. [Apollo 연구 노트] — 계획 모듈 TASK
6 의 PIECEWISE_JERK_PATH_OPTIMIZER . [Apollo 연구 노트] — 계획 모듈 TASK
7 의 PATH_ASSESSMENT_DECIDER . [Apollo 연구 노트] — 계획 모듈 TASK
8 의 PATH_DECIDER . [Apollo 연구
Notes]——계획 모듈 TASK 9 의 RULE_BASED_STOP_DECIDER. [Apollo 연구 노트]——계획 모듈 TASK 10 의 SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER. [Apollo 연구 노트]——계획 모듈 TASK 11 의 SPEED_HEURISTIC_OPTIMIZER. [Apollo 연구 노트]——계획 모듈 TA SK SPEED_DECIDER 12.


[Apollo 연구 노트]——계획 모듈 TASK의 PIECEWISE_JERK_SPEED_OPTIMIZER
13. [Apollo 연구 노트]——계획 모듈 TASK의 PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER (1)
14. [Apollo 연구 노트]——계획 모듈 TA의 PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER SK (2)

머리말

Apollo Spark 프로젝트 연구 노트 - Apollo 경로 계획 알고리즘의 원리 및 실습 과 [ Apollo 연구 노트] - 계획 모듈에서 설명하는 내용은... Stage::Process 함수가 PlanOnReferenceLinetask_list에서 TASK를 차례로 호출하는 것입니다. 예제에서는 TASK 부분이 정확히 무엇을 하는지 차례로 소개하므로 LaneFollow를 계속 사용할 것입니다. 개인 능력의 한계로 인해 기사에 부족한 부분이 있을 수 있으니 비판과 정정 부탁드립니다.

modules/planning/conf/scenario/lane_follow_config.pb.txt구성 파일 에서 LaneFollow가 실행해야 하는 모든 작업을 볼 수 있습니다.

stage_config: {
    
    
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

이번 글에서는 계속해서 LaneFollow의 11번째 TASK를 소개하겠습니다——SPEED_DECIDER

SPEED_DECIDER 함수 소개

속도 결정은
    대략적으로 계획된 속도 곡선을 기반으로 생성되며, 곡선이 장애물 위에 있는지 아래에 있는지에 따라 다른 결정이 내려집니다.

경로 결정자의 생각에 따라 경로 계획자가 경로 계획을 완료하고 무인 차량에 대한 최적의 주행 경로를 얻을 수 있을 때 경로 결정자는 정적 장애물, 특히 아래에 있지 않은 장애물에 대한 레이블을 업데이트해야 합니다. 특수 도로 상황 장애물, 시간 정보 부족으로 인해 경로 결정자 PathDecider는 동적 장애물에 대한 레이블 업데이트를 수행할 수 없습니다.

속도 플래너는 미래 시간 8초 또는 미래 전진 거리 150m 계획을 완료한 뒤, 미래의 매 순간(누적 거리 s) 기준선 상의 무인 차량의 위치를 ​​획득한 후 협력한다. 미리 계획된 경로를 사용하면 동적 장애물의 레이블 업데이트를 완료할 수 있으며, 여기서는 가장 가까운 첫 번째 장애물 경계 상자의 레이블 업데이트만 수행되고 모든 예측 시간은 업데이트되지 않습니다.

SPEED_DECIDER 관련 구성

SPEED_DECIDER 프로세스

SpeedDecider는 Task 클래스에서 직접 상속되며, 먼저 생성자가 관련 구성을 로드한 다음 Execute는 특정 실행을 위한 함수 항목입니다. Execute는 MakeObjectDecision 함수를 호출하고, SpeedDecider 전체의 주요 로직도 MakeObjectDecision에 있습니다.

• 입력 : common::Status SpeedDecider::Execute(Frame* frame, ReferenceLineInfo* reference_line_info) { 프레임 및 reference_line_info

그럼 MakeObjectDecision 함수를 직접 살펴보겠습니다.
주요 프로세스는 아래 그림과 같습니다.

MakeObject결정

SpeedDecider는 경로 결정, DP 알고리즘에 의해 생성된 속도 곡선 및 장애물 STBoundary의 위치 관계를 기반으로 무시, 중지, 따르기, 양보, 추월의 종단 결정을 생성합니다.

Status SpeedDecider::MakeObjectDecision(
    const SpeedData& speed_profile, PathDecision* const path_decision) const {
    
    
  // 检查动态规划的结果
  if (speed_profile.size() < 2) {
    
    
    const std::string msg = "dp_st_graph failed to get speed profile.";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }
  // 遍历障碍物
  for (const auto* obstacle : path_decision->obstacles().Items()) {
    
    
    auto* mutable_obstacle = path_decision->Find(obstacle->Id());
    const auto& boundary = mutable_obstacle->path_st_boundary();
    // 若障碍物的STBoundary在ST Graph的范围内，忽略
    if (boundary.IsEmpty() || boundary.max_s() < 0.0 ||
        boundary.max_t() < 0.0 ||
        boundary.min_t() >= speed_profile.back().t()) {
    
    
      AppendIgnoreDecision(mutable_obstacle);
      continue;
    }
    // 若障碍物已经有纵向决策的障碍物，忽略
    if (obstacle->HasLongitudinalDecision()) {
    
    
      AppendIgnoreDecision(mutable_obstacle);
      continue;
    }

    // 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;
      }
    }

    // always STOP for pedestrian
    // 遇到行人障碍物，添加stop决策
    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;
    }
    // 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation
    auto location = GetSTLocation(path_decision, speed_profile, boundary);
    // 检查KEEP_CLEAR区域是否阻塞
    if (!FLAGS_use_st_drivable_boundary) {
    
    
      if (boundary.boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
        if (CheckKeepClearBlocked(path_decision, *obstacle)) {
    
    
          location = BELOW;
        }
      }
    }

    switch (location) {
    
    
      case BELOW:
        // 若障碍物类型为KEEP_CLEAR，创建停止决策
        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);
          }
        // 检查ADC是否处于跟车的状态
        } 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;
      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;
      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;
      default:
        AERROR << "Unknown position:" << location;
    }
    AppendIgnoreDecision(mutable_obstacle);
  }

  return Status::OK();
}

핵심 부분은 경로 결정, DP에서 얻은 속도 곡선 및 장애물 경계를 기반으로 STlocation을 가져오는 코드입니다.

    // 根据路径决策、DP得到的速度曲线、障碍物边界获取STlocation
    auto location = GetSTLocation(path_decision, speed_profile, boundary);

GetSTLocation

다음으로 GetSTLocation 함수의 구체적인 구현을 살펴보겠습니다.
GetSTLocation은 ST 다이어그램의 속도 곡선에 있는 장애물과 지점 간의 위치 관계를 기반으로 결정을 결정합니다.

SpeedDecider::STLocation SpeedDecider::GetSTLocation(
    const PathDecision* const path_decision, const SpeedData& speed_profile,
    const STBoundary& st_boundary) const {
    
    
  // 若boundary为空，设置为BELOW；一般情况下这个障碍物直接被跳过，不考虑
  if (st_boundary.IsEmpty()) {
    
    
    return BELOW;
  }

  STLocation st_location = BELOW;
  bool st_position_set = false;
  const double start_t = st_boundary.min_t();
  const double end_t = st_boundary.max_t();
  // 遍历速度曲线中的每一个点
  for (size_t i = 0; i + 1 < speed_profile.size(); ++i) {
    
    
    const STPoint curr_st(speed_profile[i].s(), speed_profile[i].t());
    const STPoint next_st(speed_profile[i + 1].s(), speed_profile[i + 1].t());
    // 跳过和障碍物不在一个ST范围内的
    if (curr_st.t() < start_t && next_st.t() < start_t) {
    
    
      continue;
    }
    if (curr_st.t() > end_t) {
    
    
      break;
    }

    if (!FLAGS_use_st_drivable_boundary) {
    
    
      common::math::LineSegment2d speed_line(curr_st, next_st);
      // 检查是否碰撞
      if (st_boundary.HasOverlap(speed_line)) {
    
    
        ADEBUG << "speed profile cross st_boundaries.";
        st_location = CROSS;

        if (!FLAGS_use_st_drivable_boundary) {
    
    
          // 检查类型是否是KEEP_CLEAR
          if (st_boundary.boundary_type() ==
              STBoundary::BoundaryType::KEEP_CLEAR) {
    
    
            // 若CheckKeepClearCrossable为false，则设置为BELOW
            if (!CheckKeepClearCrossable(path_decision, speed_profile,
                                         st_boundary)) {
    
    
              st_location = BELOW;
            }
          }
        }
        break;
      }
    }

    // note: st_position can be calculated by checking two st points once
    //       but we need iterate all st points to make sure there is no CROSS
    if (!st_position_set) {
    
    
      if (start_t < next_st.t() && curr_st.t() < end_t) {
    
    
        STPoint bd_point_front = st_boundary.upper_points().front();
        double side = common::math::CrossProd(bd_point_front, curr_st, next_st);
        st_location = side < 0.0 ? ABOVE : BELOW;
        st_position_set = true;
      }
    }
  }
  return st_location;
}

수업 기능 확인

클래스 기능을 확인하세요. 이 유형의 기능은 주로 다양한 장애물을 판단하는 데 사용됩니다.

CheckKeepClear교차 가능

bool SpeedDecider::CheckKeepClearCrossable(
    const PathDecision* const path_decision, const SpeedData& speed_profile,
    const STBoundary& keep_clear_st_boundary) const {
    
    
  bool keep_clear_crossable = true;
  // 计算最后一点的速度
  const auto& last_speed_point = speed_profile.back();
  double last_speed_point_v = 0.0;
  if (last_speed_point.has_v()) {
    
    
    last_speed_point_v = last_speed_point.v();
  } else {
    
    
    const size_t len = speed_profile.size();
    if (len > 1) {
    
    
      const auto& last_2nd_speed_point = speed_profile[len - 2];
      last_speed_point_v = (last_speed_point.s() - last_2nd_speed_point.s()) /
                           (last_speed_point.t() - last_2nd_speed_point.t());
    }
  }
  static constexpr double kKeepClearSlowSpeed = 2.5;  // m/s
  ADEBUG << "last_speed_point_s[" << last_speed_point.s()
         << "] st_boundary.max_s[" << keep_clear_st_boundary.max_s()
         << "] last_speed_point_v[" << last_speed_point_v << "]";
  // 若最后一点的s小于KeepClear区域的最大值，且最后一点的速度小于阈值速度，
  // 则keep_clear_crossable = false
  if (last_speed_point.s() <= keep_clear_st_boundary.max_s() &&
      last_speed_point_v < kKeepClearSlowSpeed) {
    
    
    keep_clear_crossable = false;
  }
  return keep_clear_crossable;
}

보행자의 경우정지 확인

bool SpeedDecider::CheckStopForPedestrian(const Obstacle& obstacle) const {
    
    
  const auto& perception_obstacle = obstacle.Perception();
  // 检查类型是否是行人
  if (perception_obstacle.type() != PerceptionObstacle::PEDESTRIAN) {
    
    
    return false;
  }

  const auto& obstacle_sl_boundary = obstacle.PerceptionSLBoundary();
  // 不考虑在车之后的行人
  if (obstacle_sl_boundary.end_s() < adc_sl_boundary_.start_s()) {
    
    
    return false;
  }

  // read pedestrian stop time from PlanningContext
  auto* mutable_speed_decider_status = injector_->planning_context()
                                           ->mutable_planning_status()
                                           ->mutable_speed_decider();
   // 用hash表存储停止时间                                        
  std::unordered_map<std::string, double> stop_time_map;
  for (const auto& pedestrian_stop_time :
       mutable_speed_decider_status->pedestrian_stop_time()) {
    
    
    stop_time_map[pedestrian_stop_time.obstacle_id()] =
        pedestrian_stop_time.stop_timestamp_sec();
  }

  const std::string& obstacle_id = obstacle.Id();

  // update stop timestamp on static pedestrian for watch timer
  // check on stop timer for static pedestrians
  static constexpr double kSDistanceStartTimer = 10.0;
  static constexpr double kMaxStopSpeed = 0.3;
  static constexpr double kPedestrianStopTimeout = 4.0;

  bool result = true;
  if (obstacle.path_st_boundary().min_s() < kSDistanceStartTimer) {
    
    
    const auto obstacle_speed = std::hypot(perception_obstacle.velocity().x(),
                                           perception_obstacle.velocity().y());
    // 如果行人速度超过最大停车速度，则删除行人
    if (obstacle_speed > kMaxStopSpeed) {
    
    
      stop_time_map.erase(obstacle_id);
    } else {
    
    
      if (stop_time_map.count(obstacle_id) == 0) {
    
    
        // add timestamp
        stop_time_map[obstacle_id] = Clock::NowInSeconds();
        ADEBUG << "add timestamp: obstacle_id[" << obstacle_id << "] timestamp["
               << Clock::NowInSeconds() << "]";
      } else {
    
    
        // check timeout
        double stop_timer = Clock::NowInSeconds() - stop_time_map[obstacle_id];
        ADEBUG << "stop_timer: obstacle_id[" << obstacle_id << "] stop_timer["
               << stop_timer << "]";
        // 检查其是否已经等待了足够长的时间
        if (stop_timer >= kPedestrianStopTimeout) {
    
    
          result = false;
        }
      }
    }
  }

  // write pedestrian stop time to PlanningContext
  mutable_speed_decider_status->mutable_pedestrian_stop_time()->Clear();
  for (const auto& stop_time : stop_time_map) {
    
    
    auto pedestrian_stop_time =
        mutable_speed_decider_status->add_pedestrian_stop_time();
    pedestrian_stop_time->set_obstacle_id(stop_time.first);
    pedestrian_stop_time->set_stop_timestamp_sec(stop_time.second);
  }
  return result;
}

CheckIs팔로우

bool SpeedDecider::CheckIsFollow(const Obstacle& obstacle,
                                 const STBoundary& boundary) const {
    
    
  const double obstacle_l_distance =
      std::min(std::fabs(obstacle.PerceptionSLBoundary().start_l()),
               std::fabs(obstacle.PerceptionSLBoundary().end_l()));
  // FLAGS_follow_min_obs_lateral_distance: obstacle min lateral distance to follow; 2.5
  if (obstacle_l_distance > FLAGS_follow_min_obs_lateral_distance) {
    
    
    return false;
  }

  // move towards adc
  if (boundary.bottom_left_point().s() > boundary.bottom_right_point().s()) {
    
    
    return false;
  }

  static constexpr double kFollowTimeEpsilon = 1e-3;
  static constexpr double kFollowCutOffTime = 0.5;
  if (boundary.min_t() > kFollowCutOffTime ||
      boundary.max_t() < kFollowTimeEpsilon) {
    
    
    return false;
  }

  // cross lane but be moving to different direction
  // FLAGS_follow_min_time_sec: 
  // " min follow time in st region before considering a valid follow,"
  // " this is to differentiate a moving obstacle cross adc's"
  // " current lane and move to a different direction"
  // 2.0
  if (boundary.max_t() - boundary.min_t() < FLAGS_follow_min_time_sec) {
    
    
    return false;
  }

  return true;
}

확인유지지우기차단됨

bool SpeedDecider::CheckKeepClearBlocked(
    const PathDecision* const path_decision,
    const Obstacle& keep_clear_obstacle) const {
    
    
  bool keep_clear_blocked = false;

  // check if overlap with other stop wall
  for (const auto* obstacle : path_decision->obstacles().Items()) {
    
    
    if (obstacle->Id() == keep_clear_obstacle.Id()) {
    
    
      continue;
    }
    const double obstacle_start_s = obstacle->PerceptionSLBoundary().start_s();
    const double adc_length =
        VehicleConfigHelper::GetConfig().vehicle_param().length();
    const double distance =
        obstacle_start_s - keep_clear_obstacle.PerceptionSLBoundary().end_s();

    if (obstacle->IsBlockingObstacle() && distance > 0 &&
        distance < (adc_length / 2)) {
    
    
      keep_clear_blocked = true;
      break;
    }
  }
  return keep_clear_blocked;
}

클래스 함수 생성

다양한 유형의 의사결정을 수립합니다.