概述

SpeedProfileGenerator类是apollo planning模块下modules\planning\common\speed_profile_generator.cc/.h实现

从类名来看，应该是速度规划生成器类。

从代码来看SpeedProfileGenerator类主要是实现：
1.产生Fallback应急情况下的速度规划，会设计一个优化问题从车辆当前状态出发求解一个加加速度连续的刹停速度规划，但若求解失败，直接产生-4.0m/s^2固定大减速度的速度规划。
2.会在这个类里设置Fallback应急情况下的速度规划优化问题(s,s’,s’‘)权重系数及(s,s’,s’')边界约束
3.产生固定刹车距离低速爬行速度规划？就是如果刹车距离足够的情况下，可以允许低速(<5km/h)爬行一段再匀减速。而且匀减速较为舒适(-0.8m/s^2)

总之，这个类就是生成停车的速度规划？

• speed_data，里面存放了很多SpeedPoint速度点
• FALLBACK的速度规划，代表应急的速度规划，通常都会导致急刹

speed_profile_generator.h

#pragma once

#include <utility>
#include <vector>

#include "modules/common/proto/pnc_point.pb.h"
#include "modules/planning/common/ego_info.h"
#include "modules/planning/common/reference_line_info.h"
#include "modules/planning/common/speed/speed_data.h"
#include "modules/planning/math/curve1d/quintic_polynomial_curve1d.h"

namespace apollo {
namespace planning {

class SpeedProfileGenerator {
 public:
 //禁用默认构造函数，看了下调用该类时都是直接通过::来访问成员函数，不是通过对象
  SpeedProfileGenerator() = delete;

//产生Fallback应急情况下的速度规划？
//参数：自车信息ego_info
//参数:停止距离stop_distance
//根据自车当前的状态，以及设定的停车距离，根据优化求解问题求出加加速度连续的应急速度规划，返回速度规划的结果SpeedData,由多个速度点(s,t,v,a,da)构成
  static SpeedData GenerateFallbackSpeed(const EgoInfo* ego_info,
                                         const double stop_distance = 0.0);

//填充足够的速度点
//输入参数：speed_data，里面存放了很多SpeedPoint速度点，就是要往输入的速度数据指针里填充足够多的速度点
//如果速度规划数据speed_data里面的点不够3.0s，就一直往里塞最后一个位置s处v,a,da全为0的速度点直到足够3s为止
  static void FillEnoughSpeedPoints(SpeedData* const speed_data);

//产生固定刹车距离爬行速度规划？如果刹车距离比我们想要的还要小，可以低速匀速运动一段
//输入参数：刹车距离distance?
//输入参数：最大速度max_speed?
  static SpeedData GenerateFixedDistanceCreepProfile(const double distance,
                                                     const double max_speed);

 private:
 //产生停车的速度规划(以固定-4.0m/s^2)，输入参数是起始速度init_speed和起始加速度init_acc
  static SpeedData GenerateStopProfile(const double init_speed,
                                       const double init_acc);
};

}  // namespace planning
}  // namespace apollo

speed_profile_generator.cc

#include "modules/planning/common/speed_profile_generator.h"

#include <algorithm>
#include <utility>

#include "cyber/common/log.h"
#include "modules/planning/common/frame.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/math/piecewise_jerk/piecewise_jerk_speed_problem.h"

namespace apollo {
namespace planning {

using apollo::common::SpeedPoint;

//产生Fallback应急情况下的速度规划？
//参数：自车信息ego_info
//参数:停止距离stop_distance
//根据自车当前的状态，以及设定的停车距离，根据优化求解问题求出加加速度连续的应急速度规划，返回速度规划的结果SpeedData,由多个速度点(s,t,v,a,da)构成
SpeedData SpeedProfileGenerator::GenerateFallbackSpeed(
    const EgoInfo* ego_info, const double stop_distance) {
  AERROR << "Fallback using piecewise jerk speed!";
  //获取车辆Fallback轨迹起始点也就是当前车辆状态的v,a?
  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();

  // 如果车辆已经停止了，v<=0 且 a<=0
  if (init_v <= 0.0 && init_a <= 0.0) {
    AWARN << "Already stopped! Nothing to do in GenerateFallbackSpeed()";
    //创建一个空的速度数据对象speed_data，里面存放了很多SpeedPoint速度点
    //而速度点里包含s,t,v,a,da信息，da就是jerk
    SpeedData speed_data;
    //在speed_data新增一个全为0的速度点
    speed_data.AppendSpeedPoint(0.0, 0.0, 0.0, 0.0, 0.0);
    //填充FALLBACK的速度规划数据直到3.0s，但是后面填的一直都是最后一个点，即希望一直刹在原地
    FillEnoughSpeedPoints(&speed_data);
    //返回速度规划数据
    return speed_data;
  }

//定义一个3维数组init_s ，里面存放0.0，初始速度init_v，初始加速度init_a
  std::array<double, 3> init_s = {0.0, init_v, init_a};

  // TODO(all): dt is too small;
  //FLAGS_fallback_time_unit取出fallback_time_unit的值 fallback的轨迹的采样时间单位？默认为0.1s
  double delta_t = FLAGS_fallback_time_unit;
  //FLAGS_fallback_total_time FLAGS_去planning_gflags.cc中取出FLAGS_fallback_total_time的值 3.0s
  double total_time = FLAGS_fallback_total_time;
  //节点数 = 3.0s / 0.1s +1 = 31个
  const size_t num_of_knots = static_cast<size_t>(total_time / delta_t) + 1;

//PiecewiseJerkSpeedProblem大概看了一下代码，主要是对{(t0,s0),(t1,s1),...}速度规划做到加加速度平滑，根据规划路径的s,s',s''求解最优的时间t0,t1,t2...，使得纵向速度规划达到加加速度连续？
//那么这里定义了一个这样的速度规划问题(要求做到加加速度连续)，其节点数31，时间间隔0.1s，初始的纵向位置，速度，加速度？
  PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots, delta_t,
                                                   init_s);

//定义终端的参考状态，参数节点数31，停止距离？这是什么用法？double类的vector还能输入参数？
  std::vector<double> end_state_ref(num_of_knots, stop_distance);
  //设置速度规划优化求解问题的终端参考状态，终端参考状态的权重1.0？
  piecewise_jerk_problem.set_x_ref(1.0, std::move(end_state_ref));
//这个scale_factor其实不是特别明白，到涉及这块再具体看看，实际上是再求解最优速度规划问题时，对s,s',s''(s,v,a)项各自比上一个系数，理解是对目标函数里的优化项进行一个规范化或者就是将每一项的惩罚项的大小进行适当放缩使得，某一项不至于太小从而失去意义。
  piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
//设置速度优化求解问题piecewise_jerk_problem里的s(纵向位置)的边界约束,第一个参数0.0是s的下边界？第二个参数std::fmax(stop_distance, 100.0)是s的上边界(不会少于100m)？
  piecewise_jerk_problem.set_x_bounds(0.0, std::fmax(stop_distance, 100.0));
  //设置速度优化求解问题piecewise_jerk_problem里的s'(纵向速度)的边界,第一个参数0.0是s'的下边界？第二个参数是s'的上边界？
  //planning_upper_speed_limit于planning_gflags.cc里默认定义为31.3m/s速度规划时速度的上界？可以在这里设置速度规划的速度上界约束
  piecewise_jerk_problem.set_dx_bounds(
      0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_v));
  //设置速度优化求解问题piecewise_jerk_problem里的s''(纵向加速度)的边界,第一个参数最大减速度是s''的下边界(默认设置为-6.0m/s^2)？第二个参数最大加速度是s''的上边界(默认设置为2.0m/s^2)？
  //最大减速度，加速度在modules/common/data/vehicle_param.pb.txt里定义
  piecewise_jerk_problem.set_ddx_bounds(veh_param.max_deceleration(),
                                        veh_param.max_acceleration());
  //设置速度优化求解问题piecewise_jerk_problem里的s'''(纵向加加速度)的边界,第一个参数是速度规划纵向加加速度的下边界-4m/s^3？第二个参数是速度规划纵向加加速度的上边界2m/s^3
  //longitudinal_jerk_lower_bound在planning_gflags.cc里定义
  //longitudinal_jerk_upper_bound在planning_gflags.cc里定义
  piecewise_jerk_problem.set_dddx_bound(FLAGS_longitudinal_jerk_lower_bound,
                                        FLAGS_longitudinal_jerk_upper_bound);

  // 求解速度规划的优化问题，这个意思是求解不出来就产生-4.0m/s^2停车的速度规划？
  if (!piecewise_jerk_problem.Optimize()) {
    AERROR << "Piecewise jerk fallback speed optimizer failed!";
    //返回停止的速度规划？输入参数为起始速度init_v，起始加速度init_a
    return GenerateStopProfile(init_v, init_a);
  }

  // 提取输出
  //提取输出的s,ds,dds vector
  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();

//打印s,ds,dds vector对应的时间序列？
  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];
  }

//先定义一个空的速度规划数据speed_data
  SpeedData speed_data;
  //增加第一个速度点(s,t,v,a,da)
  speed_data.AppendSpeedPoint(s[0], 0.0, ds[0], dds[0], 0.0);
 //遍历时间，31个速度点，每个递增delta_t  0.1s
  for (size_t i = 1; i < num_of_knots; ++i) {
    // Avoid the very last points when already stopped
    //避免非常后面的速度点，当已经停住了，s可能存在波动若下面这种情况就直接跳出循环了，已经到最后了
    if (s[i] - s[i - 1] <= 0.0 || ds[i] <= 0.0) {
      break;
    }
//针对遍历的这个时间t=i*0.1s ，设置速度点(s,t,v,a,da)
    speed_data.AppendSpeedPoint(s[i], delta_t * static_cast<double>(i), ds[i],
                                dds[i], (dds[i] - dds[i - 1]) / delta_t);
  }
  //因为是停车的速度规划，可能时间不够3s，填充至3.0s，速度加速度为0，位置保持最后一个点一直填充
  FillEnoughSpeedPoints(&speed_data);
  //返回速度规划的数据speed_data
  return speed_data;
}

//填充足够的速度点
//输入参数：speed_data，里面存放了很多SpeedPoint速度点，就是要往输入的速度数据指针里填充足够多的速度点
//如果速度规划数据speed_data里面的点不够3.0s，就一直往里塞最后一个位置s处v,a,da全为0的速度点直到足够3s为止
void SpeedProfileGenerator::FillEnoughSpeedPoints(SpeedData* const speed_data) {
  //输入参数速度数据指针的最后一个速度点last_point 为输入参数speed_data的最后一个速度点
  const SpeedPoint& last_point = speed_data->back();
  //速度数据指针的最后一个速度点last_point的时间t >= 3.0s,就说明速度点足够了？直接返回
  //FLAGS_fallback_total_time FLAGS_去planning_gflags.cc中取出FLAGS_fallback_total_time的值 3.0s
  if (last_point.t() >= FLAGS_fallback_total_time) {
    return;
  }
  //FLAGS_fallback_time_unit取出fallback_time_unit的值 fallback的轨迹的采样时间单位？默认为0.1s
  //遍历时间t=速度数据中最后一个速度点的时间+0.1s到3.0s，然后一直往输入的指针速度数据里塞(s,t,v,a,da)里v,a,da全为0，s一直保持速度规划的的最后一个位置，就表示一旦FALLBACK期望原地刹住,t就是遍历的叠加时间
  for (double t = last_point.t() + FLAGS_fallback_time_unit;
       t < FLAGS_fallback_total_time; t += FLAGS_fallback_time_unit) {
    speed_data->AppendSpeedPoint(last_point.s(), t, 0.0, 0.0, 0.0);
  }
}

//产生停车的速度规划(以-4.0m/s^2)，输入参数是起始速度init_speed和起始加速度init_acc
SpeedData SpeedProfileGenerator::GenerateStopProfile(const double init_speed,
                                                     const double init_acc) {
  //报错信息：降低车速以恒定的减速度用FALLBACK的停止速度规划
  AERROR << "Slowing down the car within a constant deceleration with fallback "
            "stopping profile.";
  //定义一个空的速度规划数据speed_data
  SpeedData speed_data;

//GFLAGS用法，FLAGS_代表去planning.gflags.cc里取出fallback_total_time的值
//Fallback的速度规划的总时间3.0s
  const double max_t = FLAGS_fallback_total_time;
  //GFLAGS用法，FLAGS_代表去planning.gflags.cc里取出fallback_time_unit的值
//Fallback的速度规划的时间间隔0.1s
  const double unit_t = FLAGS_fallback_time_unit;

//定义之前的纵向位置s为0.0
  double pre_s = 0.0;
  //定义之前的速度为输入的初始速度
  double pre_v = init_speed;
  //FALLBACK应急速度规划时，默认配置的应急减速度为-4.0m/s^2
  double acc = FLAGS_slowdown_profile_deceleration;

//增加一个速度点，添加的第一个速度点s=0,t=0,v=init_speed输入的初始速度，a=init_acc输入的初始加速度，da加加速度为0
  speed_data.AppendSpeedPoint(0.0, 0.0, init_speed, init_acc, 0.0);
  //t=0,0.1,0.2,0.3...3.0s，依次添加 速度规划点
  for (double t = unit_t; t < max_t; t += unit_t) {
    //s从0开始叠加按照匀减速运动递推
    double s = 0.0;
    //速度v也是按照匀减速运动推算
    double v = 0.0;
    s = std::fmax(pre_s,
                  pre_s + 0.5 * (pre_v + (pre_v + unit_t * acc)) * unit_t);
    v = std::fmax(0.0, pre_v + unit_t * acc);
    //把按照s,t,v,a,da=0塞入speed_data
    speed_data.AppendSpeedPoint(s, t, v, acc, 0.0);
    //这一次的s,v又变成上一次的s,v
    pre_s = s;
    pre_v = v;
  }
  //不够3.0s的话填充到3.0s，但是上面的for循环已经保证填充到3.0s了，多余？
  FillEnoughSpeedPoints(&speed_data);
  //返回填充后的速度规划数据
  return speed_data;
}

//产生固定刹车距离爬行速度规划？如果刹车距离比我们想要的还要小，可以低速匀速运动一段
//输入参数：刹车距离distance?
//输入参数：最大速度max_speed?
SpeedData SpeedProfileGenerator::GenerateFixedDistanceCreepProfile(
    const double distance, const double max_speed) {
    //设定了一个常数减速度只有-0.8m/s^2   3s达到完全停止？
  static constexpr double kConstDeceleration = -0.8;  // (~3sec to fully stop)
  //定义了一个常数的行进速度5km/h
  static constexpr double kProceedingSpeed = 2.23;    // (5mph proceeding speed)
  //定义行进速度proceeding_speed=最大速度和行进速度常速里的最小值，最大就5km/h
  //就是行进速度最大不超过5km/h
  const double proceeding_speed = std::fmin(max_speed, kProceedingSpeed);
  //刹停距离 x=v*v/(2a)计算匀减速运动的位移
  const double distance_to_start_deceleration =
      proceeding_speed * proceeding_speed / kConstDeceleration / 2;
  //是否为匀减速模式 = 若输入参数刹车距离 < v*v/(2a)计算出来的位移，输入的期望的安全距离小于公式推算的，那么就说明期望的比较严苛，必须采用匀减速模式，否则可以不用匀减速
  bool is_const_deceleration_mode = distance < distance_to_start_deceleration;
//减速度为刚刚设置的-0.8m/s^2的减速度，3s的话基本可以减2.4m/s，理论上5km/h可以在3s完全刹住
  double a = kConstDeceleration;
  //时间t起点为0
  double t = 0.0;
  //起始点的纵坐标0.0
  double s = 0.0;
  //初始设置速度为行进速度
  double v = proceeding_speed;

//定义固定距离爬行减速速度规划时间间隔0.1s
  static constexpr double kDeltaT = 0.1;

//定义一个空的速度规划数据 speed_data
  SpeedData speed_data;
  //如果速度点s比刹车距离要小且速度还大于0，车还没刹停
  while (s < distance && v > 0) {
    //如果确实是匀减速模式
    if (is_const_deceleration_mode) {
    //把相应的s,t,v,a,da=0这个速度点塞入speed_data，同时s,t,v,a根据匀减速递推计算下，反正t每次+0.1s
      speed_data.AppendSpeedPoint(s, t, v, a, 0.0);
      t += kDeltaT;
      double v_new = std::max(0.0, v + a * t);
      s += kDeltaT * (v + v_new) / 2;
      v = v_new;
    } else {
    //如果不是必须匀减速
    //塞入速度点s,t,v,a=0,da=0塞入速度规划speed_data，那可以a也等于0,很缓慢的接近
      speed_data.AppendSpeedPoint(s, t, v, 0.0, 0.0);
      //t每次+0.1s
      t += kDeltaT;
      //以当前速度匀速运动，s按照匀速叠加
      s += kDeltaT * v;
      //如果现在期望的刹车距离 - 已经走过的(那就是剩下的刹车距离) < 匀减速运动推算的，那就开始匀减速运动了
      if (distance - s < distance_to_start_deceleration)
        is_const_deceleration_mode = true;
    }
  }

//返回速度规划的数据
  return speed_data;
}

}  // namespace planning
}  // namespace apollo