Lattice Planner从学习到放弃（二）：二次规划的应用与调试

前言

一.静态障碍物的引入

二.单车道场景中面向静态障碍物的避让

1.轨迹规划

2.轨迹实车执行效果之失败案例

三、调试过程记录

1.代码缺失与完善

2.参数的调整

总结

---

前言

前情提要：Lattice Planner从学习到放弃
盲目激情的移植apollo的lattice，体会到了痛苦，当时为了先尽快让lattice算法在自己的框架里跑起来，在摘用源码的时候做了很多欠考虑的改动，虽然快速实现了通过lattice planner规划轨迹，然后达到循迹的效果，但是在随后引入障碍物，产生了大量的坑，深深的被教育，也给带头大哥添了不少麻烦。

看懂流程==>理解原理 ==>成功实现  

最终通过planner中的二次规划实现了单车道内低速障碍物的避让规划。
简明扼要：添加静态障碍物→产生单车道内目标轨迹→绕行或停止

最近节奏比较撕裂，下午或晚上才能搞lattice的复现，但这个“重复造轮子”的过程真的获益良多，和大哥们交流也多起来，知道好的轮子是什么样的，才能尝试去改进或完善，一瓶不响半瓶咣当，半瓶正是在下～保持积极与热情，也期待大佬们各种指导....

---

一.静态障碍物的引入

1.障碍物的格式

暂时不使用Apollo的那套，先用自有结构体接收障碍物信息，然后压入Obstacle类中即可。

对于静态或低速障碍物，无非就是id、长宽高、位置以及朝向，动态障碍物暂时没有进行。

2.自车sl的建立和障碍物的建立

自车相对于参考线起点里程和横向偏移量，以及障碍物的ST、SL信息都是要计算的，对于动态障碍物，SL已经不足以满足需求了吧？后续学习EM再拎出来分析。

二.单车道场景中面向静态障碍物的避让

1.轨迹规划原理

轨迹=纵向轨迹+横向轨迹，之前已经分析了纵向速度规划的过程，以及横纵向轨迹是如何combine成一条完整轨迹的。在lattice中，横向轨迹生成有两种方法：撒点采样法、二次规划法。

1.1基于采样点的轨迹规划

撒点法前边也有记录，知道了起始点和采样点的，然后求解对应的五次多项式的系数，便可得到对应的横向轨迹，不重复了。

1.2基于二次规划的轨迹规划

Apollo源码中FLAGS_lateral_optimization默认是开启的，即横向规划默认使用二次规划进行，由于个人鲁莽认为撒点会更直观简洁，所以关了这个标志位，采用的是撒点采样，随后在实车调试时发现耗时很大，尤其是自车接近障碍物时，耗时飙升——轨迹数太多，障碍物碰撞检测遍历轨迹。额，配置比较低的工控机雪上加霜...然后试用一下二次规划呗，wtf，超nice。

       学习了程十三的轨迹规划综述。

Apollo中二次规划求解使用的是OSQP求解器，官网：OSQP官网点这里 ，二次规划求解有很多方法，关于为何选取OSQP，速度快～为何这么快，数学系的大佬们的战场。标准的二次规划形式：

下来要做的就很明确了：

• 搞清楚路径规划的约束；
• 搞清楚优化目标，即如何评价计算结果的优劣
• 转换成osqp求解器需要的形式，调用求解器
• 坐等结果～

考虑车宽以及和障碍物的距离buffer，车辆的中心的横向可移动范围大概就是如下图，Apollo中，前探60m，采样点间隔1m，所以有60个采样点，也可以根据实际情况调整。

1.2.1 车辆横向状态量设计

横向偏移量作为基本量，跑不掉，作为横向速度变化率，当然也跑不掉；作为横向加加速度，是控制乘坐舒适性的重要指标，也不能放掉；对于，把轨迹看作可拉伸的弹力绳，三阶导意味着其可拉伸的程度，可以用差分计算。so，需要的量齐全了。状态量可以得到：

1.2.2 车辆横向轨迹约束的建立

整个约束建立主要考虑到：

1.自车不能与障碍物碰撞或驶出边界，即

2.轨迹应该保持连续，即相邻两个采样点的

3.轨迹应该保持光滑，即导数连续相邻两个采样点的斜率

4.横向加加速度(相对于s的二阶偏导)应在一定范围内，即

公式2、3直接用的泰勒公式进行的2阶和3阶展开，将代入，可得到公式2和3的最终形式：

到这一步，基本上约束的内容已经很清晰了，整理后如下：

即对于每一个采样点，存在两个不等式和两个等式约束，一共60个采样点，那么约束至少应该为60x4=240个约束条件。

根据状态量的形式，约束矩阵也就定了（这里不一定准确，若有不对求指导额..）：

这里使用的话貌似应该是，行列貌似反了...其实转置以后应该长这样：

1.2.3 约束边界的建立

前边已经描述了约束的建立依据，根据障碍物对车道的侵占情况，更新横向位移的范围，最终约束的上下边界分别为：

其中-2,2作为缺省值不充值，凑够矩阵运算数量，最终用于：

1.2.4 目标函数的建立

二次规划的目标为

其中，将权重作为P项对角阵传入，左右边界作为偏差项q用以控制轨迹和参考线的偏离程度，如下：

至此，整个二次规划建立所需要的材料，全部齐全。在二次规划中，对于矩阵P，以目前形式来看一定是正定的额，是否意味着此问题是凸优化问题，且一定有可行解？求大佬指点。。。

1.2.5 约束矩阵的压缩CSC

从上边已经可以看到，建立起来的矩阵基本都非常稀疏，在运算过程中是非常不方便的，常见的稀疏矩阵压缩方法主要有：

• CSR—Compressed sparse row
• CSC—Compressed sparse column

之前在学习apollo时，发现其采用的csc矩阵，当时并不了解，也是扩展后才知道，可见总结：csc_matrix稀疏矩阵理解 至于为何选择csc的原因可能是osqp官方使用的是csc？调用osqp建立workspace，需要以csc矩阵形式传入求解器。

闲话一大堆，下面上代码。

2.Lattice planning二次规划代码实现

lattice planner中通过调用Trajectory1dGenerator实例化后的成员函数，生成横纵向轨迹，我们要看的常规撒点法和二次规划都在其中，

// 5. generate 1d trajectory bundle for longitudinal and lateral respectively.
  Trajectory1dGenerator trajectory1d_generator(
      init_s, init_d, ptr_path_time_graph, ptr_prediction_querier);
  std::vector> lon_trajectory1d_bundle;
  std::vector> lat_trajectory1d_bundle;
  trajectory1d_generator.GenerateTrajectoryBundles(
      planning_target, &lon_trajectory1d_bundle, &lat_trajectory1d_bundle);

进入函数后，很清晰没啥说的：纵向规划+横向规划。

void Trajectory1dGenerator::GenerateTrajectoryBundles(
    const PlanningTarget& planning_target,
    Trajectory1DBundle* ptr_lon_trajectory_bundle,
    Trajectory1DBundle* ptr_lat_trajectory_bundle) {
  //纵向速度规划
  GenerateLongitudinalTrajectoryBundle(planning_target,
                                       ptr_lon_trajectory_bundle);
  //横向轨迹规划
  GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle);
}

在横向规划内，通过宏定义FLAGS_lateral_optimization，决定是否采用二次规划，如下：

void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
    Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
  //是否使用优化轨迹，true，采用五次多项式规划
  if (!FLAGS_lateral_optimization) {
    auto end_conditions = end_condition_sampler_.SampleLatEndConditions();

    // Use the common function to generate trajectory bundles.
    GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
                                  ptr_lat_trajectory_bundle);
  } else {
    double s_min = init_lon_state_[0];
    double s_max = s_min + FLAGS_max_s_lateral_optimization;//FLAGS_max_s_lateral_optimization = 60

    double delta_s = FLAGS_default_delta_s_lateral_optimization;//规划间隔为1m
    //横向边界
    auto lateral_bounds =
        ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);

    // LateralTrajectoryOptimizer lateral_optimizer;
    std::unique_ptr lateral_optimizer(
        new LateralOSQPOptimizer);

    // 采用的是OSQP求解器
    lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);

    auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();

    ptr_lat_trajectory_bundle->push_back(
        std::make_shared(lateral_trajectory));
  }
}

流程很直白，通过GetLateralBounds函数获取包含障碍物信息的横向边界分布，然后传入lateral_optimizer->optimize()中开始osqp短暂愉快的一生，

完整源码可看apollo：

bool LateralOSQPOptimizer::optimize(
    const std::array& d_state, const double delta_s,
    const std::vector>& d_bounds) {
  std::vector P_data;
  std::vector P_indices;
  std::vector P_indptr;
  //建立目标函数中的P矩阵，主要包括权重分配
  CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
  delta_s_ = delta_s; //1m
  const int num_var = static_cast(d_bounds.size());
  const int kNumParam = 3 * static_cast(d_bounds.size());
  const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3; 
  c_float lower_bounds[kNumConstraint];
  c_float upper_bounds[kNumConstraint];

  const int prime_offset = num_var;
  const int pprime_offset = 2 * num_var;//=6？

  std::vector>> columns;
  columns.resize(kNumParam);

  int constraint_index = 0;

  //constraint_index:0~2
  // d_i+1'' - d_i''
  for (int i = 0; i + 1 < num_var; ++i) {
    columns[pprime_offset + i].emplace_back(constraint_index, -1.0);
    columns[pprime_offset + i + 1].emplace_back(constraint_index, 1.0);
    //FLAGS_lateral_third_order_derivative_max=0.1
    lower_bounds[constraint_index] =
        -FLAGS_lateral_third_order_derivative_max * delta_s_;
    upper_bounds[constraint_index] =
        FLAGS_lateral_third_order_derivative_max * delta_s_;
    ++constraint_index;
  }

  //constraint_index:3~5
  // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
  for (int i = 0; i + 1 < num_var; ++i) {
    columns[prime_offset + i].emplace_back(constraint_index, -1.0);
    columns[prime_offset + i + 1].emplace_back(constraint_index, 1.0);
    columns[pprime_offset + i].emplace_back(constraint_index, -0.5 * delta_s_);
    columns[pprime_offset + i + 1].emplace_back(constraint_index,
                                                -0.5 * delta_s_);

    lower_bounds[constraint_index] = 0.0;
    upper_bounds[constraint_index] = 0.0;
    ++constraint_index;
  }

  //constraint_index:6~8
  // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
  for (int i = 0; i + 1 < num_var; ++i) {
    columns[i].emplace_back(constraint_index, -1.0);
    columns[i + 1].emplace_back(constraint_index, 1.0);
    columns[prime_offset + i].emplace_back(constraint_index, -delta_s_);
    columns[pprime_offset + i].emplace_back(constraint_index,
                                            -delta_s_ * delta_s_ / 3.0);
    columns[pprime_offset + i + 1].emplace_back(constraint_index,
                                                -delta_s_ * delta_s_ / 6.0);

    lower_bounds[constraint_index] = 0.0;
    upper_bounds[constraint_index] = 0.0;
    ++constraint_index;
  }

  columns[0].emplace_back(constraint_index, 1.0);
  lower_bounds[constraint_index] = d_state[0];//d
  upper_bounds[constraint_index] = d_state[0];
  ++constraint_index;

  columns[prime_offset].emplace_back(constraint_index, 1.0);
  lower_bounds[constraint_index] = d_state[1];//d'
  upper_bounds[constraint_index] = d_state[1];
  ++constraint_index;

  columns[pprime_offset].emplace_back(constraint_index, 1.0);
  lower_bounds[constraint_index] = d_state[2];//d''
  upper_bounds[constraint_index] = d_state[2];
  ++constraint_index;

  const double LARGE_VALUE = 2.0;
  for (int i = 0; i < kNumParam; ++i) {
    columns[i].emplace_back(constraint_index, 1.0);
    if (i < num_var) {
      lower_bounds[constraint_index] = d_bounds[i].first;
      upper_bounds[constraint_index] = d_bounds[i].second;
    } else {
      lower_bounds[constraint_index] = -LARGE_VALUE;
      upper_bounds[constraint_index] = LARGE_VALUE;
    }
    ++constraint_index;
  }

  CHECK_EQ(constraint_index, kNumConstraint);

  // change affine_constraint to CSC format
  std::vector A_data;
  std::vector A_indices;
  std::vector A_indptr;
  int ind_p = 0;
  for (int j = 0; j < kNumParam; ++j) {
    A_indptr.push_back(ind_p);
    for (const auto& row_data_pair : columns[j]) {
      A_data.push_back(row_data_pair.second);
      A_indices.push_back(row_data_pair.first);
      ++ind_p;
    }
  }
  A_indptr.push_back(ind_p);

  // offset
  double q[kNumParam];
  for (int i = 0; i < kNumParam; ++i) {
    if (i < num_var) {
      q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
             (d_bounds[i].first + d_bounds[i].second);
    } else {
      q[i] = 0.0;
    }
  }

  // Problem settings
  OSQPSettings* settings =
      reinterpret_cast(c_malloc(sizeof(OSQPSettings)));

  // Define Solver settings as default
  osqp_set_default_settings(settings);
  settings->alpha = 1.0;  // Change alpha parameter
  settings->eps_abs = 1.0e-05;
  settings->eps_rel = 1.0e-05;
  settings->max_iter = 5000;
  settings->polish = true;
  settings->verbose = FLAGS_enable_osqp_debug;

  // Populate data
  OSQPData* data = reinterpret_cast(c_malloc(sizeof(OSQPData)));
  data->n = kNumParam;
  data->m = kNumConstraint;
  data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
                       P_indices.data(), P_indptr.data());
  data->q = q;
  data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
                       A_indices.data(), A_indptr.data());
  data->l = lower_bounds;
  data->u = upper_bounds;

  // Workspace
  OSQPWorkspace* work = osqp_setup(data, settings);

  // Solve Problem
  osqp_solve(work);

  // extract primal results
  // prime求导符号
  for (int i = 0; i < num_var; ++i) {
    opt_d_.push_back(work->solution->x[i]);
    opt_d_prime_.push_back(work->solution->x[i + num_var]);
    opt_d_pprime_.push_back(work->solution->x[i + 2 * num_var]);
  }
  opt_d_prime_[num_var - 1] = 0.0;
  opt_d_pprime_[num_var - 1] = 0.0;

  // Cleanup
  osqp_cleanup(work);
  c_free(data->A);
  c_free(data->P);
  c_free(data);
  c_free(settings);

  return true;
}

三、调试过程记录

1.代码缺失与完善

调试过程中发现各种遗漏或者错误，改就是了...

2.实车执行失败案例与分析

记录下调试过程中失败的过程。

2.1 车辆遇到贴近的障碍物：一直想往上撞
计算耗时无法控制，一旦接近障碍物，由于备选轨迹很多，并且每个轨迹都要进行碰撞检测，外加工控机性能有限，导致耗时飙至900ms，已经严重不符100ms的运算周期了，直接导致轨迹拼接不准确，控制和规划无法很好衔接。

但是有一点没想通：即使轨迹拼接出问题，为何会往障碍物上撞，暂时把这个问题搁置了，后续解决。

2.2 使用二次规划，贴近障碍物时无解，轨迹飞掉
（a）权重不合理，导致轨迹规划无解

发现在和障碍物平齐时，自车横向偏移量总是比边界多了0.1m....然后求解失败

画个图，一切明了，就像那高尔夫球进洞，或者《信条》里的逆向子弹，如果轨迹有偏差，子弹是无法退回枪膛的，只会和枪管发生碰撞。

解决方法比较简单，调节权重，提高自车和障碍物距离的权重，使得生成的轨迹适当远离障碍物，从红色变为绿色线。
（b）过早的转弯，导致与障碍物发生碰撞

这里问题根源还没锁定，可能因素有两块：

• 控制在选取预瞄距离后，过早的进行了转向控制
• 规划的轨迹的确过早转向(这样的话就不满足求解约束了额，可能性不大)

不知有前辈怎么解决的。

2.3 在特定的几个点，轨迹直接飞掉，约束失效
车辆到某些位置会出现规划失败的情形，无法产生有效轨迹。为了查原因，关闭了轨迹有效性检测，然后发现轨迹是这个diao样子....飞掉了

查看该处的障碍物SL信息，发现SL完全乱掉了。回想Frenet坐标系在面对U型弯圆心处障碍物投影特点(障碍物会被极大拉伸)，在出问题的点，该位置处参考线为圆弧形，障碍物恰好位于其圆心附近，导致障碍物在frennet投影时出现了右下图所示，存在多个投影点，其SL图当然是要废掉了。

---

总结

目前已经实现了单车道内障碍物的规避，包括绕行和减速以及停车。一套下来，虽然只是简单的功能复现，仅仅冰山一角但已经收益颇丰，实际操作的过程难度和踩坑大幅超过了自己的预期，多亏了身边大哥的指导和帮助，开发工作真的是需要多交流与沟通。

下一阶段目标：

实现多车道的轨迹规划，园区内可以采用伪车道，即单条referenceline进行扩幅，覆盖整条道路宽度即可，另一个是真实的多车道多referenceline，继续加油，一点点进步。