百度apollo自动驾驶planning代码学习-Apollo\modules\planning\lattice\behavior\FeasibleRegion类代码详解

概述

跳过
apollo/modules/planning/dag/,
apollo/modules/planning/data/,
apollo/modules/planning/images/,
apollo/modules/planning/integration_tests/
这四个目录没有讲解，要么与规划无关，要么涉及时再讲解，规划模块的integration_tests集成测试可以等解析完所有planning下其他所有代码再来解析。

FeasibleRegion类是apollo planning模块下modules\planning\lattice\behavior\feasible_region.cc/.h实现

从类名来看，应该是可行区域类？

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

1.储存车辆的初始状态s,v,a；
2.获取以最大的加速度4m/s^2在指定时间t内能到达的最远的s(supper)，能达到的最
大速度v(vupper);
3.获取以最大的减速度-6m/s^2在指定时间t内能到达的最近的s——slower,能达到的
最小速度v(VLower);
4.获取以最大加速度4m/s^2到达指定s的最小时间TLower;

feasible_region.h

#pragma once

#include <algorithm>

#include "modules/common/proto/pnc_point.pb.h"

namespace apollo {
namespace planning {

class FeasibleRegion {
 public:
 //带参构造函数
//输入参数是个三维数组类 init_s?至少init_s[0]代表s,init_s[1]代表v，第三个参数好像没用到？init_s是初始状态？
  explicit FeasibleRegion(const std::array<double, 3>& init_s);

//获取s的上界？输入参数是时间t
//获取以最大加速度4.0m/s^2指定时间能到达的最远的s?
  double SUpper(const double t) const;

//获取s的下界？输入参数是时间t
//获取以最小加速度-6.0m/s^2在指定时间能到达的最远的s?
  double SLower(const double t) const;

//获取v的上界？输入参数是时间t
//获取以最大加速度4.0m/s^2在指定时间能达到的最大的速度?
//v=v0+at
  double VUpper(const double t) const;

//获取v的下界？输入参数是时间t
//获取以最大减速度-6.0m/s^2在指定时间能达到的最小的速度?
//v=v0+at，但是速度不能小于0
  double VLower(const double t) const;

//获取达到输入的s处需要的最小时间t，那就是以最大加速度过去
  double TLower(const double s) const;

 private:
 //车辆的初始状态，s,v,第3个参数是a?
  std::array<double, 3> init_s_;

  double t_at_zero_speed_;

  double s_at_zero_speed_;
};

}  // namespace planning
}  // namespace apollo

feasible_region.cc

#include "modules/planning/lattice/behavior/feasible_region.h"

#include <cmath>

#include "cyber/common/log.h"
#include "modules/planning/common/planning_gflags.h"

namespace apollo {
namespace planning {

//带参构造函数
//输入参数是个三维数组类 init_s?至少init_s[0]代表s,init_s[1]代表v，第三个参数好像没用到？init_s是初始状态？
FeasibleRegion::FeasibleRegion(const std::array<double, 3>& init_s) {
  //输入参数init_s用来初始化类成员init_s_
  init_s_ = init_s;

//获取初始状态里的速度
  double v = init_s[1];
  //检查v>=0?
  CHECK_GE(v, 0.0);
//获取最大的减速度6.0m/s^2，在planning_gflags.cc里定义
  const double max_deceleration = -FLAGS_longitudinal_acceleration_lower_bound;
 //获取以最大减速度减到0的时间t_at_zero_speed_
  t_at_zero_speed_ = v / max_deceleration;
  //获取以最大减速度减到0时的纵向坐标s_at_zero_speed_
  //s=s0+v^2/2a
  s_at_zero_speed_ = init_s[0] + v * v / (2.0 * max_deceleration);
}

//获取s的上界？输入参数是时间t
//获取以最大加速度4.0m/s^2指定时间能到达的最远的s?
double FeasibleRegion::SUpper(const double t) const {
  //检查t>=0?
  ACHECK(t >= 0.0);
  //以最大加速度4.0m/s^2加速输入的时间所达到的纵向位置s
  //s=s0+v0t+1/2*a*t^2匀加速
  return init_s_[0] + init_s_[1] * t +
         0.5 * FLAGS_longitudinal_acceleration_upper_bound * t * t;
}

//获取s的下界？输入参数是时间t
//获取以最小加速度-6.0m/s^2在指定时间能到达的最远的s?
double FeasibleRegion::SLower(const double t) const {
  //如果输入的t小于减速度到0需要的时间，返回纵向的位置s
  if (t < t_at_zero_speed_) {
    return init_s_[0] + init_s_[1] * t +
           0.5 * FLAGS_longitudinal_acceleration_lower_bound * t * t;
  }
  return s_at_zero_speed_;
}

//获取v的上界？输入参数是时间t
//获取以最大加速度4.0m/s^2在指定时间能达到的最大的速度?
//v=v0+at
double FeasibleRegion::VUpper(const double t) const {
  return init_s_[1] + FLAGS_longitudinal_acceleration_upper_bound * t;
}

//获取v的下界？输入参数是时间t
//获取以最大减速度-6.0m/s^2在指定时间能达到的最小的速度?
//v=v0+at，但是速度不能小于0
double FeasibleRegion::VLower(const double t) const {
  return t < t_at_zero_speed_
             ? init_s_[1] + FLAGS_longitudinal_acceleration_lower_bound * t
             : 0.0;
}

//获取达到输入的s处需要的最小时间t，那就是以最大加速度过去
double FeasibleRegion::TLower(const double s) const {
  //首先检查输入的s是否比初始纵向位置s要大
  ACHECK(s >= init_s_[0]);

//获取从当前纵向位置到输入的s剩余的距离
  double delta_s = s - init_s_[0];
  //获取初始状态的速度v
  double v = init_s_[1];
  //设定加速度为最大加速度，这样才会是最短时间到s
  double a = FLAGS_longitudinal_acceleration_upper_bound;
  //求解需要的时间t并返回
  double t = (std::sqrt(v * v + 2.0 * a * delta_s) - v) / a;
  return t;
}

}  // namespace planning
}  // namespace apollo