阿波罗 planning代码-modules\planning\lattice\trajectory_generation\PiecewiseBrakingTrajectoryGenerator类详解

概述

PiecewiseBrakingTrajectoryGenerator类是apollo planning模块下modules\planning\lattice\trajectory_generation\piecewise_braking_trajectory_generator.cc/.h实现

从类名来看，应该是PiecewiseBrakingTrajectoryGenerator分段匀减速刹停轨迹生成器类？

从代码来看PiecewiseBrakingTrajectoryGenerator类主要是实现：

1.根据当前车速，纵向位置，目标车速，需要到达的纵向位置计算出一条纵向分段匀减速刹停轨迹，分成4种情况；
//a.距离不够，直接以当前车速刹停至目标点；
//b.先舒适的减速至目标速度巡航一段时间，再舒适的减速刹停停在目标s上；(初始速度大于目标速度)
//c.先舒适的加速至目标速度，巡航一段时间，再刹停(初始速度小于目标速度)
//d.先舒适的加速至最大速度，再刹停(初始速度小于目标速度,纵向长度没办法实现完整的梯形速度规划)
2. 为了实现1的功能，写了一些辅助函数ComputeStopDistance根据加速度，速度计算刹停距离；ComputeStopDeceleration根据距离，速度反推刹停需要的减速度；

简而言之，就是计算一条刹停纵向轨迹

piecewise_braking_trajectory_generator.h

#pragma once

#include <memory>

#include "modules/planning/common/trajectory1d/piecewise_acceleration_trajectory1d.h"

namespace apollo {
namespace planning {

class PiecewiseBrakingTrajectoryGenerator {
 public:
  PiecewiseBrakingTrajectoryGenerator() = delete;

//PiecewiseBrakingTrajectoryGenerator类的主要函数，Genearate()生成分段刹车轨迹的函数？
//其实就是输入当前状态(纵向位置，车速)，需要停在的纵向位置来计算纵向的轨迹，分成几种情况：
//a.距离不够，直接以当前车速刹停至目标点；
//b.先舒适的减速至目标速度巡航一段时间，再舒适的减速刹停停在目标s上；(初始速度大于目标速度)
//c.先舒适的加速至目标速度，巡航一段时间，再刹停(初始速度小于目标速度)
//d.先舒适的加速至最大速度，再刹停(初始速度小于目标速度,纵向长度没办法实现完整的梯形速度规划)
//输入参数：目标纵向位置s_target,当前车辆的纵向位置s_curr，目标车速v_target，当前车速v_curr，舒适加速度a_comfort？ 舒适的减速度d_comfort？最大时间max_time(指轨迹的时间长度，planning_gflags里trajectory_time_length配置为8.0s)？
  static std::shared_ptr<Curve1d> Generate(
      const double s_target, const double s_curr, const double v_target,
      const double v_curr, const double a_comfort, const double d_comfort,
      const double max_time);

//计算刹停距离，输入v和减速度dec, v^2/2a
  static double ComputeStopDistance(const double v, const double dec);

//根据刹停距离和初始速度反推减速度
//a=v^2/(2x)
  static double ComputeStopDeceleration(const double dist, const double v);
};

}  // namespace planning
}  // namespace apollo

piecewise_braking_trajectory_generator.cc

#include "modules/planning/lattice/trajectory_generation/piecewise_braking_trajectory_generator.h"

#include <cmath>

#include "cyber/common/log.h"

namespace apollo {
namespace planning {

//PiecewiseBrakingTrajectoryGenerator类的主要函数，Genearate()生成分段刹车轨迹的函数？
//其实就是输入当前状态(纵向位置，车速)，需要停在的纵向位置来计算纵向的轨迹，分成几种情况：
//a.距离不够，直接以当前车速刹停至目标点；
//b.先舒适的减速至目标速度巡航一段时间，再舒适的减速刹停停在目标s上；(初始速度大于目标速度)
//c.先舒适的加速至目标速度，巡航一段时间，再刹停(初始速度小于目标速度)
//d.先舒适的加速至最大速度，再刹停(初始速度小于目标速度,纵向长度没办法实现完整的梯形速度规划)
//输入参数：目标纵向位置s_target,当前车辆的纵向位置s_curr，目标车速v_target，当前车速v_curr，舒适加速度a_comfort？ 舒适的减速度d_comfort？最大时间max_time(指轨迹的时间长度，planning_gflags里trajectory_time_length配置为8.0s)？
std::shared_ptr<Curve1d> PiecewiseBrakingTrajectoryGenerator::Generate(
    const double s_target, const double s_curr, const double v_target,
    const double v_curr, const double a_comfort, const double d_comfort,
    const double max_time) {
    //首先创建一个共享指针，分段匀加速1维轨迹类，用当前纵向位置和速度去初始化这个类对象ptr_trajectory 
  std::shared_ptr<PiecewiseAccelerationTrajectory1d> ptr_trajectory =
      std::make_shared<PiecewiseAccelerationTrajectory1d>(s_curr, v_curr);

//计算要走过的纵向距离，用纵向目标位置减去当前的纵向位置
  double s_dist = s_target - s_curr;

//计算舒适的刹停距离？输入当前车速和舒适的减速度作为参数
  double comfort_stop_dist = ComputeStopDistance(v_curr, d_comfort);

  // if cannot stop using comfort deceleration, then brake in the beginning.
  //如果用舒适的减速度无法在规定的纵向距离内完成降速，那么一开始就刹车？
  //如果舒适的减速完成距离>要求的减速完成距离(至少不能超过纵向目标位置)
  if (comfort_stop_dist > s_dist) {
    //这是从要求的减速距离s_dist，以及当前车速反推刹停需要的减速度
    double stop_d = ComputeStopDeceleration(s_dist, v_curr);
    //计算出刹停需要的时间
    double stop_t = v_curr / stop_d;
    //在轨迹里初始s,v之后增加一段以反推的减速度刹停的这一段纵向轨迹
    ptr_trajectory->AppendSegment(-stop_d, stop_t);
//若刹停的的时间小于轨迹需要的时间长度8s，就在该轨迹后面增加一段加速度为0的轨迹填补剩下的时间
    if (ptr_trajectory->ParamLength() < max_time) {
      ptr_trajectory->AppendSegment(0.0,
                                    max_time - ptr_trajectory->ParamLength());
    }
    //返回这个分段匀减速刹车轨迹(该情况下必须从一开始就刹车，而且是以不舒适的减速度刹停)
    return ptr_trajectory;
  }

  // otherwise, the vehicle can stop from current speed with comfort brake.
  //车辆可以以舒服的减速度减速至目标速度的话
  //如果当前车速大于目标车速
  if (v_curr > v_target) {
  //计算巡航时间，也就是减速至目标速度后匀速运动的时间
  //用从当前纵向位置到目标纵向位置的总的距离 - 舒适的减速至目标速度的距离 = 剩下的需要匀速运动完成的距离，剩下的需要匀速运动完成的距离 / 目标速度 = 匀速巡航时间
    double t_cruise = (s_dist - comfort_stop_dist) / v_target;
    //计算舒适的减速至目标速度需要的时间
    double t_rampdown = (v_curr - v_target) / d_comfort;
    
    //这个是计算目标速度再以舒适的减速度刹停需要的时间？
    double t_dec = v_target / d_comfort;

//在分段匀减速轨迹里，在初始的s和速度之后增加一段以舒适的减速度-2m/s^2减少至目标速度的轨迹
    ptr_trajectory->AppendSegment(-d_comfort, t_rampdown);
  //在分段匀减速轨迹里，在减速至目标速度后再增加一段匀速巡航的轨迹
    ptr_trajectory->AppendSegment(0.0, t_cruise);
    //最后增加一段以舒服的减速度刹停的减速段轨迹
    ptr_trajectory->AppendSegment(-d_comfort, t_dec);

//如果这一通操作下来，待设置的分段匀减速刹停轨迹时间还是小于8s，用加速度0去填补剩下的时间，直到轨迹的时间达到8s
    if (ptr_trajectory->ParamLength() < max_time) {
      ptr_trajectory->AppendSegment(0.0,
                                    max_time - ptr_trajectory->ParamLength());
    }
    //返回这个分段匀减速刹停轨迹(该情况下先舒适的减速至目标速度巡航一段时间，再舒适的减速刹停停在目标s上，另外apollo里定义的舒适减速度是-2m/s^2)
    return ptr_trajectory;

  } else {
  //如果当前车速小于目标车速
  //反过来计算以当前车速以舒适的加速度2m/s^2（apollo里定义的，实际上也并不能算舒适）到目标速度的加速时间
    double t_rampup = (v_target - v_curr) / a_comfort;
    //以目标速度舒适的减速至当前车速的时间t_rampdown 
    double t_rampdown = (v_target - v_curr) / d_comfort;
    //这是 车辆先匀加速至目标速度，再匀减速至当前初始车速的距离，加减速的距离总长s_ramp 
    double s_ramp = (v_curr + v_target) * (t_rampup + t_rampdown) * 0.5;
//计算需要走过的纵向距离 - 加减速走过的距离 - 从当前车速出发舒适的刹停距离 
//剩下的距离就是需要保持目标车速匀速行驶的距离
    double s_rest = s_dist - s_ramp - comfort_stop_dist;
    //如果计算出的剩下的保持目标车速匀速行驶的距离是 > 0的话
    if (s_rest > 0) {
    //计算巡航时间 
      double t_cruise = s_rest / v_target;
      //计算从目标速度刹停的时间
      double t_dec = v_target / d_comfort;

      // construct the trajectory
      //构建这条轨迹
      
      //在待设置的分段匀减速刹停轨迹里在初始状态(当前车速，当前纵向位置)后新增一段
      //舒适匀加速到目标速度的轨迹
      ptr_trajectory->AppendSegment(a_comfort, t_rampup);
      //新增一段以目标速度巡航的轨迹
      ptr_trajectory->AppendSegment(0.0, t_cruise);
      //新增一段从目标速度开始刹停的轨迹
      ptr_trajectory->AppendSegment(-d_comfort, t_dec);

      //如果轨迹的时间长度小于8s就在刹停后补全一段速度为0加速度为0的轨迹到8s
      if (ptr_trajectory->ParamLength() < max_time) {
        ptr_trajectory->AppendSegment(0.0,
                                      max_time - ptr_trajectory->ParamLength());
      }
      //返回这个分段匀减速刹停轨迹(该情况下先舒适的加速至目标速度，巡航一段时间，再刹停)
      return ptr_trajectory;
    } else { //如果计算出的剩下的保持目标车速匀速行驶的距离是 <= 0的话，
    //就说明不需要匀速段，直接从当前车速舒适加速至最多目标车速，再舒适的刹停
      double s_rampup_rampdown = s_dist - comfort_stop_dist;
      //计算从当前车速舒适加速的最大速度
      double v_max = std::sqrt(v_curr * v_curr + 2.0 * a_comfort * d_comfort *
                                                     s_rampup_rampdown /
                                                     (a_comfort + d_comfort));
//计算加速的时间
      double t_acc = (v_max - v_curr) / a_comfort;
      //计算舒适减速的时间
      double t_dec = v_max / d_comfort;

      // construct the trajectory
      //构建这段分段匀减速刹停轨迹
      //再当前状态后增加一段 舒适加速至最大速度的轨迹
      ptr_trajectory->AppendSegment(a_comfort, t_acc);
      //再增加一段舒适减速至0的轨迹
      ptr_trajectory->AppendSegment(-d_comfort, t_dec);

//轨迹上不满8s，加速度为0，速度为0补全至8s
      if (ptr_trajectory->ParamLength() < max_time) {
        ptr_trajectory->AppendSegment(0.0,
                                      max_time - ptr_trajectory->ParamLength());
      }
      //返回这个分段匀减速刹停轨迹(该情况下先舒适的加速至最大速度，再刹停)
      return ptr_trajectory;
    }
  }
}

//计算刹停距离，输入v和减速度dec, v^2/2a
double PiecewiseBrakingTrajectoryGenerator::ComputeStopDistance(
    const double v, const double dec) {
  ACHECK(dec > 0.0);
  return v * v / dec * 0.5;
}

//根据刹停距离和初始速度反推减速度
//a=v^2/(2x)
double PiecewiseBrakingTrajectoryGenerator::ComputeStopDeceleration(
    const double dist, const double v) {
  return v * v / dist * 0.5;
}

}  // namespace planning
}  // namespace apollo

附录 apollo中默认定义的舒适加减速度

apollo里的舒适加速度和减速度都在planning_gflags里配置，定义最大加减速度为±4m/s^{2，然后舒适加减速度在最大值基础上乘以一个factor，这个factor同样在planning_gflags.cc里配置为0.5，也就是apollo里默认配置的舒适的加减速度为±2m/s}2。