自动驾驶规划模块学习笔记-piecewise分段曲线

jerk值是常量

构造函数

假定p0, v0, a0是起点时刻的位置，速度和加速度，p1, v1, a1是终点时刻的位置，速度和加速度，param是曲线的长度参数（可以是时间，也可以是fernet坐标系的s），jerk为固定值。可以通过p0,  v0, a0, param, jerk这5个参数来构造一段曲线。

ConstantJerkTrajectory1d::ConstantJerkTrajectory1d(const double p0,
                                                   const double v0,
                                                   const double a0,
                                                   const double j,
                                                   const double param)
    : p0_(p0), v0_(v0), a0_(a0), param_(param), jerk_(j) {
  CHECK_GT(param, FLAGS_numerical_epsilon);
  // 计算终点时刻的状态
  p1_ = Evaluate(0, param_);
  v1_ = Evaluate(1, param_);
  a1_ = Evaluate(2, param_);
}

终点状态计算

其实本质上是一段特殊的三次多项式， 其三阶导（加加速度）是一个常量

double ConstantJerkTrajectory1d::Evaluate(const std::uint32_t order,
                                          const double param) const {
  switch (order) {
    case 0: {
      return p0_ + v0_ * param + 0.5 * a0_ * param * param +
             jerk_ * param * param * param / 6.0;
    }
    case 1: {
      return v0_ + a0_ * param + 0.5 * jerk_ * param * param;
    }
    case 2: {
      return a0_ + jerk_ * param;
    }
    case 3: {
      return jerk_;
    }
    default:
      return 0.0;
  }
}

分段曲线 

构造函数

相当于曲线初始化，赋值0时刻的p0, v0, a0

PiecewiseJerkTrajectory1d::PiecewiseJerkTrajectory1d(const double p,
                                                     const double v,
                                                     const double a) {
  last_p_ = p;
  last_v_ = v;
  last_a_ = a;
  param_.push_back(0.0);
}

增加一段曲线

在曲线的末尾增加一段，相当于以（last_p, last_v, last_a）为起点，生成一段长度为param，且jerk值固定的曲线，拼接在当前曲线末尾。同时更新param，last_p, last_v, last_a

segments_是vector，对应每段jerk固定的三次多项式曲线

param_是vector, 对应的是每段曲线末尾的累计长度和，如果有三段曲线，长度分别为p1, p2, p3, 则param_里会存放四个数{0, p1, p1+p2, p1+p2+p3}

void PiecewiseJerkTrajectory1d::AppendSegment(const double jerk,
                                              const double param) {
  CHECK_GT(param, FLAGS_numerical_epsilon);

  param_.push_back(param_.back() + param);

  segments_.emplace_back(last_p_, last_v_, last_a_, jerk, param);

  last_p_ = segments_.back().end_position();

  last_v_ = segments_.back().end_velocity();

  last_a_ = segments_.back().end_acceleration();
}

计算param对应的值

这里用到了两个std标准函数, lower_bound找到第一个大于等于目标值的位置（upper_bound是找到第一个大于目标值的位置，如果没有则取最后一个元素）。distance计算左开右闭区间内的元素个数。

double PiecewiseJerkTrajectory1d::Evaluate(const std::uint32_t order,
                                           const double param) const {
  // 找到第一个大于等于param的位置
  auto it_lower = std::lower_bound(param_.begin(), param_.end(), param);
  // 如果param在第一段曲线
  if (it_lower == param_.begin()) {
    return segments_[0].Evaluate(order, param);
  }
  // 如果param在最后一段曲线，对应的param要减去倒数第二段曲线的累计长度
  if (it_lower == param_.end()) {
    auto index = std::max(0, static_cast(param_.size() - 2));
    return segments_.back().Evaluate(order, param - param_[index]);
  }
  // 如果param在中间某段曲线，对应param要减去前一段曲线的累计长度
  // distance计算[)左开右闭的元素个数
  auto index = std::distance(param_.begin(), it_lower);
  return segments_[index - 1].Evaluate(order, param - param_[index - 1]);
}

加速度值是常量

实现加速度值是常量的分段曲线（路径规划中一般用于纵向轨迹生成），因为加速度值是常量，可以理解为是一段二次多项式曲线。该类具体实现如下

构造函数

分别定义四个参数vector，其中s_和t_中的元素是累加值，v_中元素为对应时刻的速度，a_中的元素是固定值。

构造函数：对上诉四个vector第一个元素赋值，即定义初始状态

  // accumulated s
  std::vector s_;
  std::vector v_;
  // accumulated t
  std::vector t_;
  std::vector a_;

// constructor
PiecewiseAccelerationTrajectory1d::PiecewiseAccelerationTrajectory1d(const double start_s, const double start_v) {
  s_.push_back(start_s);
  v_.push_back(start_v);
  a_.push_back(0.0);
  t_.push_back(0.0);
}

增加一段曲线

定义增加曲线的时长t_duration，计算结束状态，并更新对应的参数

void PiecewiseAccelerationTrajectory1d::AppendSegment(const double a, const double t_duration) {
  double s0 = s_.back();
  double v0 = v_.back();
  double t0 = t_.back();

  double v1 = v0 + a * t_duration;

  double delta_s = (v0 + v1) * t_duration * 0.5;
  double s1 = s0 + delta_s;
  double t1 = t0 + t_duration;

  // if t is between t0 and t1, s will be less than s0 and s1？？？
  s1 = std::fmax(s1, s0);
  s_.push_back(s1);
  v_.push_back(v1);
  a_.push_back(a);
  t_.push_back(t1);
}

计算t时刻的状态

线性插值可以得到任一时刻t的状态。实际应用中下面的函数会有一个潜在的问题，当t超过t_.back()，会出现外插值的情况，外插值在实际应用中有可能会带来意想不到的计算误差，应该做个保护比较好。

std::array PiecewiseAccelerationTrajectory1d::Evaluate(const double t) const {
  assert(t_.size() > 1);

  auto it_lower = std::lower_bound(t_.begin(), t_.end(), t);
  if (it_lower == t_.begin()) {
    return {s_.front(), v_.front(), 0.0, 0.0};
  }
  if (it_lower == t_.end()) {
    it_lower -= 1;
  }
  auto index = std::distance(t_.begin(), it_lower);

  double s0 = s_[index - 1];
  double v0 = v_[index - 1];
  double t0 = t_[index - 1];

  double v1 = v_[index];
  double t1 = t_[index];

  double ratio = GetInterpolationRatio(t0, t1, t);
  double v = LinearInterpolate(v0, v1, ratio);
  double s = (v0 + v) * (t - t0) * 0.5 + s0;

  double a = a_[index];
  double j = 0.0;

  return {{s, v, a, j}};
}