[人工智能] Planning-Apollo Planning失败时Fallback机制

开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> 人工智能 -> Planning-Apollo Planning失败时Fallback机制 -> 正文阅读

[人工智能]Planning-Apollo Planning失败时Fallback机制

在Apollo的planning模块中，当决策规划计算失败时，会触发FallBack机制。不同的情况引发的失败，FallBack机制不同。

1 `Task`失败

在Lane Follow Scenario只有一个lane follow stage，在此stage会依次处理各个task，当任意一个stage计算失败时，会触发fallback trajectory。

1.1 `path plan fallback`

path plan fallback有多种方法进行冗余，按下述顺序依次进行计算。

当没有执行到path plan对应的task或者path plan对应的task失败，没有规划路径数据时：从ADC当前位置，沿着参考线平行行驶，fallback path上的航向、曲率、曲率变化率和参考线相等；
1. 如果ADC在参考线上的投影计算失败，则从ADC当前位置开始，按着ADC的航向方向向前计算fallback path。
当reference_line_info->trajectory_type() != ADCTrajectory::PATH_FALLBACK时：从上一帧规划路径中截取路径作为fallback路径；
1. 当没有上一帧路径或者上一帧路径Trim失败时，使用self lane path作为fallback path。

// apollo/modules/planning/scenarios/lane_follow/lane_follow_stage.cc
void LaneFollowStage::PlanFallbackTrajectory(
    const TrajectoryPoint& planning_start_point, Frame* frame,
    ReferenceLineInfo* reference_line_info) {
  // path and speed fall back
  if (reference_line_info->path_data().Empty()) {
    AERROR << "Path fallback due to algorithm failure";
    GenerateFallbackPathProfile(reference_line_info,
                                reference_line_info->mutable_path_data());
    reference_line_info->AddCost(kPathOptimizationFallbackCost); // default: cost = 2e4
    reference_line_info->set_trajectory_type(ADCTrajectory::PATH_FALLBACK);
  }
  // 这里存在bug，当路径规划成功后，并没有对 trajectory_type进行设置，其默认仍然是 ADCTrajectory::UNKOWN,会导致已经规划成功的路径失效
  if (reference_line_info->trajectory_type() != ADCTrajectory::PATH_FALLBACK) {
    if (!RetrieveLastFramePathProfile(
            reference_line_info, frame,
            reference_line_info->mutable_path_data())) {
      const auto& candidate_path_data =
          reference_line_info->GetCandidatePathData();
      // 如果从上一帧规划路径中截取失败，选取self类型的lane作为规划路径
      for (const auto& path_data : candidate_path_data) {
        if (path_data.path_label().find("self") != std::string::npos) {
          *reference_line_info->mutable_path_data() = path_data;
          AERROR << "Use current frame self lane path as fallback ";
          break;
        }
      }
    }
  }

  AERROR << "Speed fallback due to algorithm failure";
  *reference_line_info->mutable_speed_data() =
      SpeedProfileGenerator::GenerateFallbackSpeed(injector_->ego_info());

  if (reference_line_info->trajectory_type() != ADCTrajectory::PATH_FALLBACK) {
    reference_line_info->AddCost(kSpeedOptimizationFallbackCost);  // default: cost = 2e4
    reference_line_info->set_trajectory_type(ADCTrajectory::SPEED_FALLBACK);
  }
}

个人认为第1点应该首先使用上一帧的规划路径，因为planning的频率约是 $10 H z$ ，短时间内环境不会发生剧烈变化，上一帧规划路径仍具有有效性。
个人认为第2点这里存在bug，当路径规划成功后，并没有对trajectory_type进行设置，其默认仍然是 ADCTrajectory::UNKOWN, 会导致已经规划成功的路径失效。

1.2 `speed plan fallback`

speed plan fallback仍然采用最优化的方式进行求解，优化问题的目标函数为 $\sum_{i=0}^{n-1}{(s_i - 0)}$ ，约束和速度规划的约束一样。当adc已经停车时，则不会进行最优化计算；当最优化问题计算失败时，会按照配置的加速度进行刹车。

// apollo/modules/planning/common/speed_profile_generator.cc
SpeedData SpeedProfileGenerator::GenerateFallbackSpeed(
    const EgoInfo* ego_info, const double stop_distance) {
  AERROR << "Fallback using piecewise jerk speed!";
  const double init_v = ego_info->start_point().v();
  const double init_a = ego_info->start_point().a();
  AWARN << "init_v = " << init_v << ", init_a = " << init_a;
  const auto& veh_param =
      common::VehicleConfigHelper::GetConfig().vehicle_param();

  // if already stopped
  if (init_v <= 0.0 && init_a <= 0.0) {
    AWARN << "Already stopped! Nothing to do in GenerateFallbackSpeed()";
    SpeedData speed_data;
    speed_data.AppendSpeedPoint(0.0, 0.0, 0.0, 0.0, 0.0);
    // 按照最后一个点的速度信息进行填充
    FillEnoughSpeedPoints(&speed_data);
    return speed_data;
  }

  std::array<double, 3> init_s = {0.0, init_v, init_a};

  // TODO(all): dt is too small;
  double delta_t = FLAGS_fallback_time_unit;      // default: 0.1
  // 更小的规划时域是为了减少求解时间？？？上面百度的注释也说明了，此时的求解颗粒度可以大一些，减少时间？？
  double total_time = FLAGS_fallback_total_time;  // dafault: 3.0
  const size_t num_of_knots = static_cast<size_t>(total_time / delta_t) + 1;

  PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t,
                                                   init_s);

  std::vector<double> end_state_ref(num_of_knots, stop_distance);
  // 每个s的决策变量的目标值是停车距离，表示希望车辆尽快停下来
  // 没有对 dx, ddx, dddx, ref_v的weight进行设置，从代码中可知，会采用默认权重0，
  // 因此优化为的cost func = sum(s_i),即会在车辆最大能力范围尽快停下来
  piecewise_jerk_problem.set_x_ref(1.0, std::move(end_state_ref));

  piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
  // s的最大范围应该符合车辆动力学，即车辆按照最大a和da进行刹车时，车辆在此范围内可以停下来，否则会造成优化问题无解
  // 对于乘用车来说100m的刹车距离是足够了
  piecewise_jerk_problem.set_x_bounds(0.0, std::fmax(stop_distance, 100.0));
  piecewise_jerk_problem.set_dx_bounds(
      0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_v));  // default: 31.3 m/s
  // 这里使用车辆的最大加减速能力作为硬约束，而不应该使用舒适性加减速约束
  // 对于商用车，并且是高速满载，使用最大加速度是否安全有待考虑，或者可以在control层，根据车速做加速度控制，保证安全
  piecewise_jerk_problem.set_ddx_bounds(veh_param.max_deceleration(),
                                        veh_param.max_acceleration());
  piecewise_jerk_problem.set_dddx_bound(FLAGS_longitudinal_jerk_lower_bound,  // default: -4.0
                                        FLAGS_longitudinal_jerk_upper_bound); // default: 2.0

  // Solve the problem
  if (!piecewise_jerk_problem.Optimize()) {
    AERROR << "Piecewise jerk fallback speed optimizer failed!";
    // 按照配置参数的恒定加速度停车
    return GenerateStopProfile(init_v, init_a);
  }

  // Extract output
  const std::vector<double>& s = piecewise_jerk_problem.opt_x();
  const std::vector<double>& ds = piecewise_jerk_problem.opt_dx();
  const std::vector<double>& dds = piecewise_jerk_problem.opt_ddx();

  for (size_t i = 0; i < num_of_knots; ++i) {
    ADEBUG << "For[" << delta_t * static_cast<double>(i) << "], s = " << s[i]
           << ", v = " << ds[i] << ", a = " << dds[i];
  }

  SpeedData speed_data;
  speed_data.AppendSpeedPoint(s[0], 0.0, ds[0], dds[0], 0.0);
  for (size_t i = 1; i < num_of_knots; ++i) {
    // Avoid the very last points when already stopped
    if (s[i] - s[i - 1] <= 0.0 || ds[i] <= 0.0) {
      break;
    }
    speed_data.AppendSpeedPoint(s[i], delta_t * static_cast<double>(i), ds[i],
                                dds[i], (dds[i] - dds[i - 1]) / delta_t);
  }
  FillEnoughSpeedPoints(&speed_data);
  return speed_data;
}

2 `context`构建失败

在进行规划时，需要先构建规划的context，当构建失败时，包括ADC状态更新失败、参考线更新失败、Frame初始化失败等，会生成停车轨迹；或者通过配置参数planning会publish空轨迹，同时下发esop的标志位。

从control来看，如果planning下发esop，control会按照配置参数进行soft estop brake。如果planning下发StopTrajectory，control根据当前状态计算得到的控制量是很难确定的，可能会产生不安全的刹车行为，例如高速行驶中突然以最小加速度刹车。

// apollo/modules/planning/on_lane_planning.cc
// TODO(all): fix this! this will cause unexpected behavior from controller
void OnLanePlanning::GenerateStopTrajectory(ADCTrajectory* ptr_trajectory_pb) {
  ptr_trajectory_pb->clear_trajectory_point();

  const auto& vehicle_state = injector_->vehicle_state()->vehicle_state();
  const double max_t = FLAGS_fallback_total_time;  // default: 3.0
  const double unit_t = FLAGS_fallback_time_unit;  // default: 0.1

  TrajectoryPoint tp;
  auto* path_point = tp.mutable_path_point();
  path_point->set_x(vehicle_state.x());
  path_point->set_y(vehicle_state.y());
  path_point->set_theta(vehicle_state.heading());
  path_point->set_s(0.0);
  tp.set_v(0.0);
  tp.set_a(0.0);
  for (double t = 0.0; t < max_t; t += unit_t) {
    tp.set_relative_time(t);
    auto next_point = ptr_trajectory_pb->add_trajectory_point();
    next_point->CopyFrom(tp);
  }
}

3 scenarios

在apollo中planning会处理不同的scenarios，上述1 Task失败是对lane follow scenario的处理。其他scenario略。

4 lattice规划

当纵向采样和横向采样生成的所有轨迹都发生碰撞时，会生成减速停车的轨迹。

// apollo/modules/planning/lattice/trajectory_generation/backup_trajectory_generation.cc
BackupTrajectoryGenerator::BackupTrajectoryGenerator(
    const State& init_s, const State& init_d, const double init_relative_time,
    const std::shared_ptr<CollisionChecker>& ptr_collision_checker,
    const Trajectory1dGenerator* trajectory1d_generator)
    : init_relative_time_(init_relative_time),
      ptr_collision_checker_(ptr_collision_checker),
      ptr_trajectory1d_generator_(trajectory1d_generator) {
  GenerateTrajectory1dPairs(init_s, init_d);
}

void BackupTrajectoryGenerator::GenerateTrajectory1dPairs(const State& init_s,
                                                          const State& init_d) {
  std::vector<std::shared_ptr<Curve1d>> lon_trajectories;
  std::array<double, 5> dds_condidates = {-0.1, -1.0, -2.0, -3.0, -4.0};
  for (const auto dds : dds_condidates) {
    lon_trajectories.emplace_back(
        new ConstantDecelerationTrajectory1d(init_s[0], init_s[1], dds));
  }

  std::vector<std::shared_ptr<Curve1d>> lat_trajectories;
  ptr_trajectory1d_generator_->GenerateLateralTrajectoryBundle(
      &lat_trajectories);

  for (auto& lon : lon_trajectories) {
    for (auto& lat : lat_trajectories) {
      trajectory_pair_pqueue_.emplace(lon, lat);
    }
  }
}

DiscretizedTrajectory BackupTrajectoryGenerator::GenerateTrajectory(
    const std::vector<PathPoint>& discretized_ref_points) {
  while (trajectory_pair_pqueue_.size() > 1) {
    auto top_pair = trajectory_pair_pqueue_.top();
    trajectory_pair_pqueue_.pop();
    DiscretizedTrajectory trajectory =
        TrajectoryCombiner::Combine(discretized_ref_points, *top_pair.first,
                                    *top_pair.second, init_relative_time_);
    if (!ptr_collision_checker_->InCollision(trajectory)) {
      return trajectory;
    }
  }
  auto top_pair = trajectory_pair_pqueue_.top();
  return TrajectoryCombiner::Combine(discretized_ref_points, *top_pair.first,
                                     *top_pair.second, init_relative_time_);
}

纵向采样：分别以 $? 0.1, ? 1.0, ? 2.0, ? 3.0, ? 4.0$ 的加速度生成匀加速的纵向采样；
横向采样： $d$ 的采样为 $0.0, ? 0.5, 0.5$ ， $s$ 的采样为 $10.0, 20.0, 40.0, 80.0$ ；
优先级：从优先队列的比较方式可以看出，优先级最高的是规划时域末端速度最小的曲线。

 // apollo/modules/planning/lattice/trajectory_generation/backup_trajectory_generation.h
 struct CostComparator
      : public std::binary_function<const Trajectory1dPair&,
                                    const Trajectory1dPair&, bool> {
    bool operator()(const Trajectory1dPair& left,
                    const Trajectory1dPair& right) const {
      auto lon_left = left.first;
      auto lon_right = right.first;
      auto s_dot_left = lon_left->Evaluate(1, FLAGS_trajectory_time_length);
      auto s_dot_right = lon_right->Evaluate(1, FLAGS_trajectory_time_length);
      if (s_dot_left < s_dot_right) {
        return true;
      }
      return false;
    }
  };

  std::priority_queue<Trajectory1dPair, std::vector<Trajectory1dPair>,
                      CostComparator>
      trajectory_pair_pqueue_

因此，BackupTrajectory是可以以最小减速度行驶的无碰撞轨迹，如果所有轨迹都有碰撞，则是trajectory_pair_pqueue_中优先级最低的轨迹，即规划时域末端车速最低的轨迹。