Planning 基于非线性规划的速度规划

c0d3n4m · 发表于 2022-1-29 11:50

在上一篇文章基于二次规划的速度规划算法中，我们讲了如何利用二次规划（QP）算法将动态规划产生的st曲线平滑，生成最终的st曲线。在二次规划的目标函数中，我们看到了曲率对速度的惩罚。在曲率大的地方通过减小速度从而较小向心加速度，从而提高驾驶过程的舒适性。但是曲率是s的函数，而我们优化的变量正是s，所以在二次规划之前通过动态规划的粗轨迹提前计算出每个时间ti时的曲率，进而得到每个时间ti的曲率惩罚权重pi。这种方法如果粗轨迹和最终平滑后的轨迹接近，那么曲率的惩罚位置较为正确，如果平滑结果和粗轨迹相差较大，那么曲率惩罚就不会作用到曲率大的位置，产生相反的效果。
所以apollo在考虑向心加速度的约束，设计了一种基于非线性规划（NLP）的速度规划算法，并应用IPOPT进行求解。在参考线平滑的文章中介绍了IPOPT的求解方法。
按照IPOPT的求解过程，我们首先定义优化变量。
定义优化变量

优化变量的定义和二次规划的速度规划相似，包含了每个固定间隔时间点的位置、速度、加速度。除此之外非线性规划中如果打开了软约束FLAGS_use_soft_bound_in_nonlinear_speed_opt，还会有几个关于s软约束的松弛变量,总共5n个优化变量

$s_0,s_1,\dots,s_{n-1}, \dot s_0,\dot s_1,\dots,\dot s_{n-1}, \ddot s_0,\ddot s_1,\dots,\ddot s_{n-1}, s\_lower_0,s\_lower_1,\dots,s\_lower_{n-1}, s\_upper_0,s\_upper_1,\dots,s\_upper_{n-1}$
定义目标函数

接下来我们给出速度规划的目标函数：

$cost = \sum_{i = 0}^{n-1}w_{ref}(s_i-s\_ref_i)^2+ w_v(\dot s_i-v\_ref)^2+w_a\ddot s_i^2+ w_j(\frac{\ddot s_{i+1}-\ddot s_i}{ \Delta t})^2+\\ w_{lat\_acc}lat\_acc_i^2+\\ w_{soft}s\_lower_i+w_{soft}s\_upper_i\\$
其中s_lower_i和s_upper_i分别是s边界约束的松弛变量
s_ref_i和v_ref分别为位置和速度的参考值，位置参考值来自于动态规划求得的粗st曲线，速度参考值为planning中定义的巡航速度，由用户设置FLAGS_default_cruise_speed
lat_acc为向心加速度，其通过速度平方乘以曲率计算

$acc\_lat_i=\dot s_i^2*kappa(s)$
问题就落到需要得到曲率与s的关系式。
曲率曲线平滑

在路径规划中我们也用的是二次规划（http://piecewise_jerk_path_optimizer.cc），得到的是s关于l的jerk恒定的分段多项式曲线。但是这个曲线s关于l是连续可导的，但是s关于曲率k是不可导的。ipopt中我们的目标函数是要一个二阶可导函数，所以在ipopt求解之前，apollo还对曲率关于s的曲线用二次规划做了分段多项式曲线的拟合，产生二阶可导的s关于曲率k的关系式，这里依然用PiecewiseJerkPathPr
oblem去做平滑拟合：
Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothPathCurvature(
const PathData& path_data) {
  // using piecewise_jerk_path to fit a curve of path kappa profile
  // TODO(Jinyun): move smooth configs to gflags
  const auto& cartesian_path = path_data.discretized_path();
  const double delta_s = 0.5;
  std::vector<double> path_curvature;
  for (double path_s = cartesian_path.front().s();
   path_s < cartesian_path.back().s() + delta_s; path_s += delta_s) {
const auto& path_point = cartesian_path.Evaluate(path_s);
path_curvature.push_back(path_point.kappa());
  }
  const auto& path_init_point = cartesian_path.front();
  std::array<double, 3> init_state = {path_init_point.kappa(),
                                    path_init_point.dkappa(),
                                    path_init_point.ddkappa()};
  PiecewiseJerkPathProblem piecewise_jerk_problem(path_curvature.size(),
                                                delta_s, init_state);
  piecewise_jerk_problem.set_x_bounds(-1.0, 1.0);
  piecewise_jerk_problem.set_dx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_ddx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_dddx_bound(-10.0, 10.0);

  piecewise_jerk_problem.set_weight_x(0.0);
  piecewise_jerk_problem.set_weight_dx(10.0);
  piecewise_jerk_problem.set_weight_ddx(10.0);
  piecewise_jerk_problem.set_weight_dddx(10.0);

  piecewise_jerk_problem.set_x_ref(10.0, std::move(path_curvature));

  if (!piecewise_jerk_problem.Optimize(1000)) {
const std::string msg = &#34;Smoothing path curvature failed&#34;;
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> opt_x;
  std::vector<double> opt_dx;
  std::vector<double> opt_ddx;

  opt_x = piecewise_jerk_problem.opt_x();
  opt_dx = piecewise_jerk_problem.opt_dx();
  opt_ddx = piecewise_jerk_problem.opt_ddx();
...
}
在速度曲线类文章中我们介绍了恒定加加速度曲线类，这里我们借用这个类保存曲率与s的曲线，这样可以利用类中的Evaluate()函数计算曲率的导数来计算目标函数梯度
PiecewiseJerkTrajectory1d smoothed_path_curvature(
   opt_x.front(), opt_dx.front(), opt_ddx.front());

  for (size_t i = 1; i < opt_ddx.size(); ++i) {
double j = (opt_ddx - opt_ddx[i - 1]) / delta_s;
smoothed_path_curvature.AppendSegment(j, delta_s);
  }
smoothed_path_curvature_ = smoothed_path_curvature;
参考曲线平滑

apollo中还对动产规划产生的参考曲线做了平滑，作为NLP问题的迭代初始解和s的参考值，然而QP的速度规划中是直接用动态规划的解插值得到s的参考值，不知道这一步直接用动态规划解插值是否也可以？
从OptimizeByQP()中设置的权重来看，单纯是为了平滑
//QP的速度规划中是直接用动态规划的解插值
modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc：
SpeedPoint sp;
reference_speed_data.EvaluateByTime(curr_t, &sp);
const double path_s = sp.s();
x_ref.emplace_back(path_s);

Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP(
SpeedData* const speed_data, std::vector<double>* distance,
std::vector<double>* velocity, std::vector<double>* acceleration) {
  std::array<double, 3> init_states = {s_init_, s_dot_init_, s_ddot_init_};
  PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots_, delta_t_,
                                                init_states);
  piecewise_jerk_problem.set_dx_bounds(
   0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_states[1]));
  piecewise_jerk_problem.set_ddx_bounds(s_ddot_min_, s_ddot_max_);
  piecewise_jerk_problem.set_dddx_bound(s_dddot_min_, s_dddot_max_);
  piecewise_jerk_problem.set_x_bounds(s_bounds_);

  // TODO(Jinyun): parameter tunnings
  const auto& config =
   config_.piecewise_jerk_nonlinear_speed_optimizer_config();
  piecewise_jerk_problem.set_weight_x(0.0);
  piecewise_jerk_problem.set_weight_dx(0.0);
  piecewise_jerk_problem.set_weight_ddx(config.acc_weight());
  piecewise_jerk_problem.set_weight_dddx(config.jerk_weight());

  std::vector<double> x_ref;
  for (int i = 0; i < num_of_knots_; ++i) {
const double curr_t = i * delta_t_;
// get path_s
SpeedPoint sp;
speed_data->EvaluateByTime(curr_t, &sp);
x_ref.emplace_back(sp.s());
  }
  piecewise_jerk_problem.set_x_ref(config.ref_s_weight(), std::move(x_ref));

  // Solve the problem
  if (!piecewise_jerk_problem.Optimize()) {
const std::string msg =
      &#34;Speed Optimization by Quadratic Programming failed. &#34;
      &#34;st boundary is infeasible.&#34;;
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  *distance = piecewise_jerk_problem.opt_x();
  *velocity = piecewise_jerk_problem.opt_dx();
  *acceleration = piecewise_jerk_problem.opt_ddx();
  return Status::OK();
}
约束定义

约束包含了车辆必须一直往前走，所以有

$s_{i+1}>s_i$
速度、jerk的约束(似乎少了对于加速度的约束？)：

$\frac{\ddot s_{i+1}-\ddot s_i}{\Delta t}\leq jerk_{max}\\ \dot s_i \leq speed\_limit(s_i)$
限速曲线平滑

在速度约束里，关于速度的约束也是和s有关的，速度边界来自于地图和speed_decider中，所以是不可导的。apollo这里的处理和曲率一样，先对速度约束进行分段多项式拟合产生PiecewiseJerkTrajectory1d曲线，再进行计算：
Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothSpeedLimit() {
  // using piecewise_jerk_path to fit a curve of speed_ref
  // TODO(Hongyi): move smooth configs to gflags
  double delta_s = 2.0;
  std::vector<double> speed_ref;
  for (int i = 0; i < 100; ++i) {
double path_s = i * delta_s;
double limit = speed_limit_.GetSpeedLimitByS(path_s);
speed_ref.emplace_back(limit);
  }
  std::array<double, 3> init_state = {speed_ref[0], 0.0, 0.0};
  PiecewiseJerkPathProblem piecewise_jerk_problem(speed_ref.size(), delta_s,
                                                init_state);
  piecewise_jerk_problem.set_x_bounds(0.0, 50.0);
  piecewise_jerk_problem.set_dx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_ddx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_dddx_bound(-10.0, 10.0);

  piecewise_jerk_problem.set_weight_x(0.0);
  piecewise_jerk_problem.set_weight_dx(10.0);
  piecewise_jerk_problem.set_weight_ddx(10.0);
  piecewise_jerk_problem.set_weight_dddx(10.0);

  piecewise_jerk_problem.set_x_ref(10.0, std::move(speed_ref));

  if (!piecewise_jerk_problem.Optimize(4000)) {
const std::string msg = &#34;Smoothing speed limit failed&#34;;
AERROR << msg;
return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> opt_x;
  std::vector<double> opt_dx;
  std::vector<double> opt_ddx;

  opt_x = piecewise_jerk_problem.opt_x();
  opt_dx = piecewise_jerk_problem.opt_dx();
  opt_ddx = piecewise_jerk_problem.opt_ddx();
  PiecewiseJerkTrajectory1d smoothed_speed_limit(opt_x.front(), opt_dx.front(),
                                             opt_ddx.front());

  for (size_t i = 1; i < opt_ddx.size(); ++i) {
double j = (opt_ddx - opt_ddx[i - 1]) / delta_s;
smoothed_speed_limit.AppendSegment(j, delta_s);
  }

  smoothed_speed_limit_ = smoothed_speed_limit;

  return Status::OK();
}
对于位置的软约束：

$s_i-s\_lower_i+slack\_lower_i \geq 0\\ s_i-s\_upper_i-slack\_upper_i \leq 0\\$
这里简单说一下软约束的用处，通过加入松弛变量允许超出约束边界，但是超出约束的话会在目标函数中对松弛变量作出惩罚，主要避免约束过紧导致求不出来解。
约束边界主要来自于边界的类型：
Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds(
const PathData& path_data, const SpeedData& speed_data) {

  // Set s boundary
  if (FLAGS_use_soft_bound_in_nonlinear_speed_opt) {
s_bounds_.clear();
s_soft_bounds_.clear();
// TODO(Jinyun): move to confs
for (int i = 0; i < num_of_knots_; ++i) {
   double curr_t = i * delta_t_;
   double s_lower_bound = 0.0;
   double s_upper_bound = total_length_;
   double s_soft_lower_bound = 0.0;
   double s_soft_upper_bound = total_length_;
   for (const STBoundary* boundary : st_graph_data.st_boundaries()) {
      double s_lower = 0.0;
      double s_upper = 0.0;
      if (!boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
      continue;
      }
      SpeedPoint sp;
      switch (boundary->boundary_type()) {
      case STBoundary::BoundaryType::STOP:
      case STBoundary::BoundaryType::YIELD:
         s_upper_bound = std::fmin(s_upper_bound, s_upper);
         s_soft_upper_bound = std::fmin(s_soft_upper_bound, s_upper);
         break;
      case STBoundary::BoundaryType::FOLLOW:
         s_upper_bound =
            std::fmin(s_upper_bound, s_upper - FLAGS_follow_min_distance);
//这里用的是粗st曲线对应时间的速度定义边界，如果采用QP优化后的speed_data是不是更好呢？
         if (!speed_data.EvaluateByTime(curr_t, &sp)) {
            const std::string msg =
               &#34;rough speed profile estimation for soft follow fence failed&#34;;
            AERROR << msg;
            return Status(ErrorCode::PLANNING_ERROR, msg);
         }
         s_soft_upper_bound =
            std::fmin(s_soft_upper_bound,
                        s_upper - FLAGS_follow_min_distance -
                           std::min(7.0, FLAGS_follow_time_buffer * sp.v()));
         break;
      case STBoundary::BoundaryType::OVERTAKE:
         s_lower_bound = std::fmax(s_lower_bound, s_lower);
         s_soft_lower_bound = std::fmax(s_soft_lower_bound, s_lower + 10.0);
         break;
      default:
         break;
      }
   }
...
}
还有各个散点之间微分关系的约束，同二次规划一样：

$\dddot s_{i->i+1}=\frac{(\ddot s_{i+1}-\ddot s_i)}{\Delta t} \\ \dot s_{i+1}=\dot s_i+\frac{\Delta t \ddot s_{i}}{2}+\frac{\Delta t^2 \dddot s_{i->i+1}}{2} \\ s_{i+1}=s_i+\Delta t \dot s_i+\frac{\Delta t^2\ddot s_i}{3}+\frac{\Delta t^3\ddot s_{i+1}}{6}$
目标函数的梯度

目标函数对于s_i的偏导数如下，其中kappa函数为分段恒jerk多项式函数，所以它的导数dkappa也可以求得

$\frac{\delta cost}{ds_i}=2(w_{ref}s_i-s\_ref_i)+ 2w_{lat\_acc}\dot s_i^4\cdot kappa(s_i)\cdot dkappa(s_i)$
目标函数对于s&#39;_i的偏导数：

$\frac{\delta cost}{d\dot s_i}=2w_{v}(\dot s_i-v\_ref_i)+ 4w_{lat\_acc}\dot s_i^3\cdot kappa(s_i)^2$
目标函数对于s&#39;&#39;_i的偏导数：

$\frac{\delta cost}{d\ddot s_i}=-\frac{2w_{j}(\ddot s_{i+1}-\ddot s_i)}{\Delta t^2}+2w_a\ddot s_i$
对于松弛变量的偏导数：

$\frac{\delta cost}{ds\_lower_i}=w_{soft}\\ \frac{\delta cost}{ds\_upper_i}=w_{soft}$
约束的雅克比矩阵

对于速度约束：

$g_1=\dot s_i - speed\_limit(s_i)\\ \frac{\delta g_1}{d\dot s_i}=1\\ \frac{\delta g_1}{d s_i}=-speed\_limit(s_i)$
对于位置软约束：

$g_2=s_i-s\_lower_i+slack\_lower_i \\ \frac{\delta g_2}{d s_i}=1\\ \frac{\delta g_2}{d slack\_lower_i}=1$

$g_3=s_i-s\_upper_i+slack\_upper_i \\ \frac{\delta g_2}{d s_i}=1\\ \frac{\delta g_2}{d slack\_upper_i}=-1$
对于jerk约束：

$g_3=\frac{\ddot s_{i+1}-\ddot s_i}{\Delta t}\\ \frac{\delta g_3}{d \ddot s_i}=\frac{-1}{\Delta t}\\ \frac{\delta g_3}{d \ddot s_{i+1}}=\frac{1}{\Delta t}\\$
微分关系的等式约束

$g_4= s_{i+1}-s_i-\Delta t \dot s_i-\frac{\Delta t^2\ddot s_i}{3}-\frac{\Delta t^3\ddot s_{i+1}}{6}\\ \frac{\delta g_4}{d s_i}=-1\\ \frac{\delta g_4}{d s_{i+1}}=1\\ \frac{\delta g_4}{d dot s_{i}}=-\Delta t\\ \frac{\delta g_4}{d \ddot s_{i}}=-\frac{\Delta t^2}{3}\\ \frac{\delta g_4}{d \ddot s_{i+1}}=-\frac{\Delta t^3}{6}\\$

$g_5= \dot s_{i+1}-\dot s_i-\Delta t \ddot s_{i}-\frac{\Delta t (\ddot s_{i+1}-\ddot s_{i})}{2}\\ \frac{\delta g_5}{d \dot s_i}=-1\\ \frac{\delta g_5}{d \dot s_{i+1}}=1\\ \frac{\delta g_5}{d \ddot s_{i}}=- \frac{\Delta t}{2}\\ \frac{\delta g_5}{d \ddot s_{i+1}}=-\frac{\Delta t}{2}\\$
对于位置约束

$g_6=s_{i+1}-s_i\\ \frac{\delta g_6}{ds_{i+1}}=1\\ \frac{\delta g_6}{ds_i}=-1$
设定迭代初值

迭代初值采用的是动态规划计算的粗st曲线的位置、速度、加速度：
  if (config.use_warm_start()) {
const auto& warm_start_distance = *distance;
const auto& warm_start_velocity = *velocity;
const auto& warm_start_acceleration = *acceleration;
if (warm_start_distance.empty() || warm_start_velocity.empty() ||
      warm_start_acceleration.empty() ||
      warm_start_distance.size() != warm_start_velocity.size() ||
      warm_start_velocity.size() != warm_start_acceleration.size()) {
   const std::string msg =
      &#34;Piecewise jerk speed nonlinear optimizer warm start invalid!&#34;;
   AERROR << msg;
   return Status(ErrorCode::PLANNING_ERROR, msg);
}
std::vector<std::vector<double>> warm_start;
std::size_t size = warm_start_distance.size();
for (std::size_t i = 0; i < size; ++i) {
   warm_start.emplace_back(std::initializer_list<double>(
      {warm_start_distance, warm_start_velocity,
         warm_start_acceleration}));
}
ptr_interface->set_warm_start(warm_start);
  }
仿真验证与结果分析

使用Dreamview进行仿真，PNC_Monitor来观测仿真结果。由于NLP的速度规划相对于QP的速度规划主要优化在于对向心加速度的约束更为精准，所以这里在sunnyvale中选取一个掉头路段进行测试：

分别用QP算法和NLP算法的速度规划对以上路段进行仿真，设置初始速度为3m/s。为了看出对曲率优化的效果，把位置s相对于轨迹曲率kappa的曲线也展式出来了。结果如下，其中SPEED_HEURISTIC_OPTIMIZER为动态规划产生的速度曲线：

QP算法速度优化结果

NLP算法速度优化结果

从二次规划算法的优化结果中，二次规划算法的减速过程并没有在弯内减速，而是在弯前减速，然后在弯中加速。这是由于动态规划的速度规划目标函数中没有考虑曲率的权重，所以其速度由于小于巡航速度就有了加速的过程。二次规划算法获得动态规划的粗st曲线后是由动态规划的时间t对应的曲率，所以如果二次规划产生减速曲线后就导致路径曲率约束的前移。
从NLP算法仿真结果中，在弯前由于低于巡航速度进行了一定的加速，弯中进行了减速，弯后重新加速，符合一般驾驶的预期。NLP是直接将路径s-关于曲率的关系直接加到目标函数的设计中，而不是像二次规划算法目标函数是t关于曲率的关系。
动态规划和二次规划算法的目标函数大家可以参考我之前写的文章

Doris232 · 发表于 2022-1-29 11:58

楼主有空可以说说这块的仿真验证怎么做吗？目前就是把Apollo里面record好的包拿来跑，请问如何就单独的planning进行仿真呢？

mypro334 · 发表于 2022-1-29 12:00

dreamview有一个sim control，可以做planning仿真

		自动登录	找回密码
密码			立即注册

Planning 基于非线性规划的速度规划

本帖子中包含更多资源