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有多种方法进行冗余，按下述顺序依次进行计算。

1. 当没有执行到path plan对应的task或者path plan对应的task失败，没有规划路径数据时：从ADC当前位置，沿着参考线平行行驶，fallback path上的航向、曲率、曲率变化率和参考线相等；
   1. 如果ADC在参考线上的投影计算失败，则从ADC当前位置开始，按着ADC的航向方向向前计算fallback path。
2. 当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的频率约是$10Hz$，短时间内环境不会发生剧烈变化，上一帧规划路径仍具有有效性。

• 个人认为第2点这里存在bug，当路径规划成功后，并没有对trajectory_type进行设置，其默认仍然是 ADCTrajectory::UNKOWN, 会导致已经规划成功的路径失效。

1.2 speed plan fallback

speed plan fallback仍然采用最优化的方式进行求解，优化问题的目标函数为$f={\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_中优先级最低的轨迹，即规划时域末端车速最低的轨迹。