天景科技苑

这个屌丝很懒，什么也没留下！

热门标签

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

作者：天景科技苑 | 2024-08-21 09:37:16

踩

lattice planner不足

前言

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

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

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

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

一.静态障碍物的引入

1.障碍物的格式

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

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

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

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

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

1.轨迹规划原理

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

1.1基于采样点的轨迹规划

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

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

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

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

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

下来要做的就很明确了：

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

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

1.2.1 车辆横向状态量设计

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

$x=[l_{59}, l_{58} ,..., l_{0}, l_{59}', l_{58}' ,..., l_{0}', l_{59}'', l_{58}'' ,..., l_{0}'' ]^{T}$

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

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

1.自车不能与障碍物碰撞或驶出边界，即 $l_{i}\in [l_{i_{left}} , l_{i_{right}}]$

2.轨迹应该保持连续，即相邻两个采样点的 $l_{i+1}= l_{i}+{l_{i}}'\cdot \Delta s+\frac{1}{2}\cdot {{l_{i}}}''\cdot \Delta s^{2}+\frac{1}{6}\cdot {{l_{i}}}'''\cdot \Delta s^{3}$

3.轨迹应该保持光滑，即导数连续相邻两个采样点的斜率 $l_{i+1}'=l_{i}'+l_{i}''\cdot \Delta s+\frac{1}{2}\cdot l_{i}'''\cdot \Delta s^{2}$

4.横向加加速度(相对于s的二阶偏导)应在一定范围内，即 $\small l_{i}'''=\frac{\Delta l_{i}''}{\Delta s}=\frac{l_{i+1}''-l_{i}''}{\Delta s}\in [-0.1,0.1]$

公式2、3直接用的泰勒公式进行的2阶和3阶展开，将 $l_{i}'''=\frac{l_{i+1}''-l_{i}''}{\Delta s}$ 代入，可得到公式2和3的最终形式：

$\left\{\begin{matrix} l_{i+1}= l_{i}+{l_{i}}'\cdot \Delta s+\frac{1}{3}\cdot {{l_{i}}}''\cdot \Delta s^{2}+\frac{1}{6}\cdot {{l_{i+1}}}''\cdot \Delta s^{2} \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \! \\ l_{i+1}'= l_{i}'+\frac{1}{2}\cdot {l_{i}}''\cdot \Delta s+\frac{1}{2}\cdot {{l_{i+1}}}''\cdot \Delta s \end{matrix}\right.$

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

$\left\{\begin{matrix} l_{i_{left}}\leq l_{i}\leq l_{i_{right}} \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \\ l_{i+1}- l_{i}-l_{i}'\cdot \Delta s-\frac{1}{3}\cdot l_{i}''\cdot \Delta s^{2}-\frac{1}{6}\cdot {{l_{i+1}}}''\cdot \Delta s^{2} =0 \\ l_{i+1}'- l_{i}'-\frac{1}{2}\cdot {l_{i}}''\cdot \Delta s-\frac{1}{2}\cdot {{l_{i+1}}}''\cdot \Delta s=0 \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \,\\ -0.1\Delta s\leq l_{i+1}''- l_{i}''\leq 0.1\Delta s \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \, \end{matrix}\right.$

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

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

这里使用的话貌似应该是 $A^{^{T}}\cdot x$ ，行列貌似反了...其实转置以后应该长这样：

1.2.3 约束边界的建立

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

$bound_{up}=[0.1, 0.1, ...,0.1,0,0,...,0,0,0,...,0,1.45,1.45,...,1.45,2,2,...,2]^{T}$

$bound_{low}=[-0.1, -0.1, ...,-0.1,0,0,...,0,0,0,...,0,-1.45,-1.45,...,-1.45,-2,-2,...,-2]^{T}$

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

$bound_{low}\leq A^{T}\cdot x \leq bound_{up}$

1.2.4 目标函数的建立

二次规划的目标为

$minimize \frac{1}{2}x^{T}\cdot P\cdot x+q^{T}\cdot x$

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

$P=\begin{bmatrix} 2\cdot W_{LateralOffset}+2\cdot W_{ObstaclDistance} & & & & & & & & \\ &... & & & & & & & \\ & &2\cdot W_{LateralOffset}+2\cdot W_{ObstaclDistance} & & & & & & \\ & & &1000 & & & & & \\ & & & &... & & & & \\ & & & & &1000 & & & \\ & & & & & &2000 & & \\ & & & & & & &... & \\ & & & & & & & &2000 \end{bmatrix}$

$q=[-2W_{LateralOffset}(l_-bound[0].first+l_-bound[0].second),...,-2W_{LateralOffset}(l_-bound[59].first+l_-bound[59].second)]$

至此，整个二次规划建立所需要的材料，全部齐全。在二次规划中，对于矩阵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<std::shared_ptr<Curve1d>> lon_trajectory1d_bundle;
  std::vector<std::shared_ptr<Curve1d>> 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<LateralQPOptimizer> 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<PiecewiseJerkTrajectory1d>(lateral_trajectory));
  }
}

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

完整源码可看apollo：


bool LateralOSQPOptimizer::optimize(
    const std::array<double, 3>& d_state, const double delta_s,
    const std::vector<std::pair<double, double>>& d_bounds) {
  std::vector<c_float> P_data;
  std::vector<c_int> P_indices;
  std::vector<c_int> P_indptr;
  //建立目标函数中的P矩阵，主要包括权重分配
  CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
  delta_s_ = delta_s; //1m
  const int num_var = static_cast<int>(d_bounds.size());
  const int kNumParam = 3 * static_cast<int>(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<std::vector<std::pair<c_int, c_float>>> 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<c_float> A_data;
  std::vector<c_int> A_indices;
  std::vector<c_int> 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<OSQPSettings*>(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<OSQPData*>(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，继续加油，一点点进步。

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/天景科技苑/article/detail/1011202