Apollo_路径优化

牛仔很忙^

已于 2023-08-30 11:31:24 修改

阅读量2.3k

点赞数 1

分类专栏： Apollo 决策规划文章标签：自动驾驶人工智能机器学习

于 2022-04-07 12:25:55 首次发布

本文链接：https://blog.csdn.net/nudt_zrs/article/details/123286791

版权

决策规划同时被 2 个专栏收录

17 篇文章 50 订阅

订阅专栏

Apollo

10 篇文章 41 订阅

订阅专栏

与之前EM规划和Lattice规划不同，Apollo 5.0版本使用的路径规划，更加的灵活方便，原因主要是采用了数值优化的思想，通过边界约束等，保证了密集障碍物场景的灵活性。而之前的lattice等算法由于采样的局限，导致在复杂环境下可能存在无解的情况。

我们的规划任务实际上分为路径规划和速度规划。您提到的更改是在路径规划方面，以使用piecewise_jerk_path_optimizer（即QP）处理静态障碍。在速度规划上，我们仍然使用 DP + QP(piecewise_jerk_speed_optimizer) 框架。
在piecewise_jerk_path_optimizer中，不考虑与上一帧路径的距离，我们只考虑当前状态，或者说路径应该始终与当前状态一致，这足以让路径至少与当前状态平滑连接，我们不真的不需要与未来的道路保持一致。

DP非常耗时。其次，有时我们可能会因为分辨率和采样点的位置而无法找到路径。我们目前的解决方案是：

根据地图、交通规则和障碍物找到路径边界。里面的区域是“可驾驶的”。
然后使用分段jerk算法来寻找路径。它解决了DP解决方案中的上述两个主要问题。

Frenet坐标系、参考线是基础，叠加交规决策、路径边界决策、速度边界决策；

主要关注lane follow场景，配置文件为modules\planning\conf\scenario\lane_follow_config.pb.txt

scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
  stage_type: LANE_FOLLOW_DEFAULT_STAGE
  enabled: true
  task_type: LANE_CHANGE_DECIDER
  task_type: PATH_REUSE_DECIDER
  task_type: PATH_LANE_BORROW_DECIDER
  task_type: PATH_BOUNDS_DECIDER
  task_type: PIECEWISE_JERK_PATH_OPTIMIZER
  task_type: PATH_ASSESSMENT_DECIDER
  task_type: PATH_DECIDER
  task_type: RULE_BASED_STOP_DECIDER
  task_type: ST_BOUNDS_DECIDER
  task_type: SPEED_BOUNDS_PRIORI_DECIDER
  task_type: SPEED_HEURISTIC_OPTIMIZER
  task_type: SPEED_DECIDER
  task_type: SPEED_BOUNDS_FINAL_DECIDER
  # task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  task_type: RSS_DECIDER

......

一种包含15/16个任务，本文主要关注第五个PIECEWISE_JERK_PATH_OPTIMIZER

一、算法思想：

分段常加加速度路径优化 Piecewise jerk Path optimization

二、算法理论

离散s，优化变量为l，l'，l''，建模为QP问题

优化变量： $l_{0},l_{1},l_{2},...l_{n-1};{l_{0}}',{l_{1}}',{l_{2}}',...',{l_{n-1}}';{l_{0}}'',{l_{1}}'',{l_{2}}'',...{l_{n-1}}''$

目标函数：

$\tilde{f}(l(s)) = \omega_{l} \ast \sum_{i=0}^{n-1} l_{i}^2 + \omega_{l'} \ast \sum_{i=0}^{n-1} l_i'^2 + \omega_{l''} \ast \sum_{i=0}^{n-1} l_i''^2 + \omega_{l'''} \ast \sum_{i=0}^{n-2} (\frac{l_{i+1}''-l_{i}''}{ \Delta s})^2 + \omega_{obs} \ast \sum_{i=0}^{n-1} (l_i - 0.5 \ast (l_{min}^i + l_{max}^i))^2$

最后一项：凸空间中央

约束条件：

（1）边界约束 $l\left ( s \right ) \in l_{B}\left ( s \right ), \forall s\in \left [ 0,s_{max} \right ]$

（2）连续性约束

${l}''_{i+1} = {l}''_{i} + \int_{0}^{\Delta s} {l}'''_{i\rightarrow i+1} ds = {l}''_{i} + {l}'''_{i\rightarrow i+1} \ast \Delta s$

${l}'_{i+1} = {l}'_{i} + \int_{0}^{\Delta s} {l}''\left ( s \right ) ds = {l}'_{i} + {l}''_{i} \ast \Delta s + \frac{1}{2} \ast {l}'''_{i\rightarrow i+1}\ast \Delta s^2$

${l}_{i+1} = {l}_{i} + \int_{0}^{\Delta s} {l}'\left ( s \right ) ds = {l}_{i} + {l}'_{i} \ast \Delta s + \frac{1}{2} \ast {l}''_{i} \ast \Delta s^2+ \frac{1}{6} \ast {l}'''_{i\rightarrow i+1}\ast \Delta s^3$