当前位置:   article > 正文

Lattice Planner从学习到放弃(二):二次规划的应用与调试_lattice planner不足

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基于采样点的轨迹规划

撒点法前边也有记录,知道了起始点和采样点的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,把轨迹看作可拉伸的弹力绳,三阶导意味着其可拉伸的程度,可以用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.自车不能与障碍物碰撞或驶出边界,即li[lileft,liright]

2.轨迹应该保持连续,即相邻两个采样点的li+1=li+liΔs+12liΔs2+16liΔs3

3.轨迹应该保持光滑,即导数连续相邻两个采样点的斜率li+1=li+liΔs+12liΔs2

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阶展开,将li=li+1liΔ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也就定了(这里不一定准确,若有不对求指导额..):

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

1.2.3 约束边界的建立

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

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实例化后的成员函数,生成横纵向轨迹,我们要看的常规撒点法和二次规划都在其中,

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

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

  1. void Trajectory1dGenerator::GenerateTrajectoryBundles(
  2. const PlanningTarget& planning_target,
  3. Trajectory1DBundle* ptr_lon_trajectory_bundle,
  4. Trajectory1DBundle* ptr_lat_trajectory_bundle) {
  5. //纵向速度规划
  6. GenerateLongitudinalTrajectoryBundle(planning_target,
  7. ptr_lon_trajectory_bundle);
  8. //横向轨迹规划
  9. GenerateLateralTrajectoryBundle(ptr_lat_trajectory_bundle);
  10. }

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

  1. void Trajectory1dGenerator::GenerateLateralTrajectoryBundle(
  2. Trajectory1DBundle* ptr_lat_trajectory_bundle) const {
  3. //是否使用优化轨迹,true,采用五次多项式规划
  4. if (!FLAGS_lateral_optimization) {
  5. auto end_conditions = end_condition_sampler_.SampleLatEndConditions();
  6. // Use the common function to generate trajectory bundles.
  7. GenerateTrajectory1DBundle<5>(init_lat_state_, end_conditions,
  8. ptr_lat_trajectory_bundle);
  9. } else {
  10. double s_min = init_lon_state_[0];
  11. double s_max = s_min + FLAGS_max_s_lateral_optimization;//FLAGS_max_s_lateral_optimization = 60
  12. double delta_s = FLAGS_default_delta_s_lateral_optimization;//规划间隔为1m
  13. //横向边界
  14. auto lateral_bounds =
  15. ptr_path_time_graph_->GetLateralBounds(s_min, s_max, delta_s);
  16. // LateralTrajectoryOptimizer lateral_optimizer;
  17. std::unique_ptr<LateralQPOptimizer> lateral_optimizer(
  18. new LateralOSQPOptimizer);
  19. // 采用的是OSQP求解器
  20. lateral_optimizer->optimize(init_lat_state_, delta_s, lateral_bounds);
  21. auto lateral_trajectory = lateral_optimizer->GetOptimalTrajectory();
  22. ptr_lat_trajectory_bundle->push_back(
  23. std::make_shared<PiecewiseJerkTrajectory1d>(lateral_trajectory));
  24. }
  25. }

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

完整源码可看apollo:

  1. bool LateralOSQPOptimizer::optimize(
  2. const std::array<double, 3>& d_state, const double delta_s,
  3. const std::vector<std::pair<double, double>>& d_bounds) {
  4. std::vector<c_float> P_data;
  5. std::vector<c_int> P_indices;
  6. std::vector<c_int> P_indptr;
  7. //建立目标函数中的P矩阵,主要包括权重分配
  8. CalculateKernel(d_bounds, &P_data, &P_indices, &P_indptr);
  9. delta_s_ = delta_s; //1m
  10. const int num_var = static_cast<int>(d_bounds.size());
  11. const int kNumParam = 3 * static_cast<int>(d_bounds.size());
  12. const int kNumConstraint = kNumParam + 3 * (num_var - 1) + 3;
  13. c_float lower_bounds[kNumConstraint];
  14. c_float upper_bounds[kNumConstraint];
  15. const int prime_offset = num_var;
  16. const int pprime_offset = 2 * num_var;//=6?
  17. std::vector<std::vector<std::pair<c_int, c_float>>> columns;
  18. columns.resize(kNumParam);
  19. int constraint_index = 0;
  20. //constraint_index:0~2
  21. // d_i+1'' - d_i''
  22. for (int i = 0; i + 1 < num_var; ++i) {
  23. columns[pprime_offset + i].emplace_back(constraint_index, -1.0);
  24. columns[pprime_offset + i + 1].emplace_back(constraint_index, 1.0);
  25. //FLAGS_lateral_third_order_derivative_max=0.1
  26. lower_bounds[constraint_index] =
  27. -FLAGS_lateral_third_order_derivative_max * delta_s_;
  28. upper_bounds[constraint_index] =
  29. FLAGS_lateral_third_order_derivative_max * delta_s_;
  30. ++constraint_index;
  31. }
  32. //constraint_index:3~5
  33. // d_i+1' - d_i' - 0.5 * ds * (d_i'' + d_i+1'')
  34. for (int i = 0; i + 1 < num_var; ++i) {
  35. columns[prime_offset + i].emplace_back(constraint_index, -1.0);
  36. columns[prime_offset + i + 1].emplace_back(constraint_index, 1.0);
  37. columns[pprime_offset + i].emplace_back(constraint_index, -0.5 * delta_s_);
  38. columns[pprime_offset + i + 1].emplace_back(constraint_index,
  39. -0.5 * delta_s_);
  40. lower_bounds[constraint_index] = 0.0;
  41. upper_bounds[constraint_index] = 0.0;
  42. ++constraint_index;
  43. }
  44. //constraint_index:6~8
  45. // d_i+1 - d_i - d_i' * ds - 1/3 * d_i'' * ds^2 - 1/6 * d_i+1'' * ds^2
  46. for (int i = 0; i + 1 < num_var; ++i) {
  47. columns[i].emplace_back(constraint_index, -1.0);
  48. columns[i + 1].emplace_back(constraint_index, 1.0);
  49. columns[prime_offset + i].emplace_back(constraint_index, -delta_s_);
  50. columns[pprime_offset + i].emplace_back(constraint_index,
  51. -delta_s_ * delta_s_ / 3.0);
  52. columns[pprime_offset + i + 1].emplace_back(constraint_index,
  53. -delta_s_ * delta_s_ / 6.0);
  54. lower_bounds[constraint_index] = 0.0;
  55. upper_bounds[constraint_index] = 0.0;
  56. ++constraint_index;
  57. }
  58. columns[0].emplace_back(constraint_index, 1.0);
  59. lower_bounds[constraint_index] = d_state[0];//d
  60. upper_bounds[constraint_index] = d_state[0];
  61. ++constraint_index;
  62. columns[prime_offset].emplace_back(constraint_index, 1.0);
  63. lower_bounds[constraint_index] = d_state[1];//d'
  64. upper_bounds[constraint_index] = d_state[1];
  65. ++constraint_index;
  66. columns[pprime_offset].emplace_back(constraint_index, 1.0);
  67. lower_bounds[constraint_index] = d_state[2];//d''
  68. upper_bounds[constraint_index] = d_state[2];
  69. ++constraint_index;
  70. const double LARGE_VALUE = 2.0;
  71. for (int i = 0; i < kNumParam; ++i) {
  72. columns[i].emplace_back(constraint_index, 1.0);
  73. if (i < num_var) {
  74. lower_bounds[constraint_index] = d_bounds[i].first;
  75. upper_bounds[constraint_index] = d_bounds[i].second;
  76. } else {
  77. lower_bounds[constraint_index] = -LARGE_VALUE;
  78. upper_bounds[constraint_index] = LARGE_VALUE;
  79. }
  80. ++constraint_index;
  81. }
  82. CHECK_EQ(constraint_index, kNumConstraint);
  83. // change affine_constraint to CSC format
  84. std::vector<c_float> A_data;
  85. std::vector<c_int> A_indices;
  86. std::vector<c_int> A_indptr;
  87. int ind_p = 0;
  88. for (int j = 0; j < kNumParam; ++j) {
  89. A_indptr.push_back(ind_p);
  90. for (const auto& row_data_pair : columns[j]) {
  91. A_data.push_back(row_data_pair.second);
  92. A_indices.push_back(row_data_pair.first);
  93. ++ind_p;
  94. }
  95. }
  96. A_indptr.push_back(ind_p);
  97. // offset
  98. double q[kNumParam];
  99. for (int i = 0; i < kNumParam; ++i) {
  100. if (i < num_var) {
  101. q[i] = -2.0 * FLAGS_weight_lateral_obstacle_distance *
  102. (d_bounds[i].first + d_bounds[i].second);
  103. } else {
  104. q[i] = 0.0;
  105. }
  106. }
  107. // Problem settings
  108. OSQPSettings* settings =
  109. reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
  110. // Define Solver settings as default
  111. osqp_set_default_settings(settings);
  112. settings->alpha = 1.0; // Change alpha parameter
  113. settings->eps_abs = 1.0e-05;
  114. settings->eps_rel = 1.0e-05;
  115. settings->max_iter = 5000;
  116. settings->polish = true;
  117. settings->verbose = FLAGS_enable_osqp_debug;
  118. // Populate data
  119. OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
  120. data->n = kNumParam;
  121. data->m = kNumConstraint;
  122. data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
  123. P_indices.data(), P_indptr.data());
  124. data->q = q;
  125. data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
  126. A_indices.data(), A_indptr.data());
  127. data->l = lower_bounds;
  128. data->u = upper_bounds;
  129. // Workspace
  130. OSQPWorkspace* work = osqp_setup(data, settings);
  131. // Solve Problem
  132. osqp_solve(work);
  133. // extract primal results
  134. // prime求导符号
  135. for (int i = 0; i < num_var; ++i) {
  136. opt_d_.push_back(work->solution->x[i]);
  137. opt_d_prime_.push_back(work->solution->x[i + num_var]);
  138. opt_d_pprime_.push_back(work->solution->x[i + 2 * num_var]);
  139. }
  140. opt_d_prime_[num_var - 1] = 0.0;
  141. opt_d_pprime_[num_var - 1] = 0.0;
  142. // Cleanup
  143. osqp_cleanup(work);
  144. c_free(data->A);
  145. c_free(data->P);
  146. c_free(data);
  147. c_free(settings);
  148. return true;
  149. }

三、调试过程记录

1.代码缺失与完善

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

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

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

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

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

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

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

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

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

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

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

不知有前辈怎么解决的。

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

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






总结

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

下一阶段目标:

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

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

闽ICP备14008679号