Bootstrap

Apollo星火计划学习笔记——Apollo速度规划算法原理与实践

文章目录

前言

Apollo星火计划课程链接如下
星火计划2.0基础课：https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课：https://apollo.baidu.com/community/online-course/12

TASK系列解析文章

1.【Apollo学习笔记】——规划模块TASK之LANE_CHANGE_DECIDER
2.【Apollo学习笔记】——规划模块TASK之PATH_REUSE_DECIDER
3.【Apollo学习笔记】——规划模块TASK之PATH_BORROW_DECIDER
4.【Apollo学习笔记】——规划模块TASK之PATH_BOUNDS_DECIDER
5.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_PATH_OPTIMIZER
6.【Apollo学习笔记】——规划模块TASK之PATH_ASSESSMENT_DECIDER
7.【Apollo学习笔记】——规划模块TASK之PATH_DECIDER
8.【Apollo学习笔记】——规划模块TASK之RULE_BASED_STOP_DECIDER
9.【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
10.【Apollo学习笔记】——规划模块TASK之SPEED_HEURISTIC_OPTIMIZER
11.【Apollo学习笔记】——规划模块TASK之SPEED_DECIDER
12.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER
13.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)
14.【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(二)

1. 速度规划算法总体介绍

    Apollo中对路径规划解耦，分为路径规划与速度规划两部分。并将规划分为决策与优化两个部分。
• 路径规划 —— 静态环境（道路，静止/低速障碍物）
• 速度规划 —— 动态环境（中/高速障碍物）
在这里插入图片描述
## 1.1 速度规划的坐标系
在这里插入图片描述

速度规划的坐标系

    注意：速度规划的 s s s沿着轨迹的方向，路径规划的 s s s沿着参考线的方向。

1.2 不同场景下的ST图

1.2.1 主车向前匀速行驶

在这里插入图片描述

1.2.2 主车先向前匀速行驶，后停车

在这里插入图片描述

1.2.3 主车跟随前车行驶

在这里插入图片描述
ps:蓝色四边形为障碍车在ST图下的投影。长边的斜率代表车速，短边代表障碍车在主车规划出的路径中占据的长度。

1.2.4 主车跟随前车刹停

在这里插入图片描述

1.2.5 障碍车在主车后方跟行

在这里插入图片描述

1.3 速度规划算法整体流程

    路径规划的配置文件在lane_follow_config.pb.txt中

// /home/yuan/apollo-edu/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: 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

_DECIDER结尾的为决策部分 _OPTIMIZER结尾的为优化部分。

1.3.1 Task： SPEED_BOUNDS_PRIORI_DECIDER

产生速度可行驶边界
在这里插入图片描述
    所形成的区域是非凸的，不能用之前凸优化的方法去做，需要用动态规划的方法去做。

1.3.2 Task：SPEED_HEURISTIC_OPTIMIZER

动态规划规划目标

  • 加速度尽可能小
  • 离障碍物纵向距离尽可能远
  • 满足车辆加减速度要求
  • 满足限速要求

产生粗糙速度规划曲线
在这里插入图片描述

1.3.3 Task： SPEED_DECIDER

产生速度决策
在这里插入图片描述    根据粗规划出的速度曲线，依据曲线在障碍物的上方还是下方，采取不同的决策。

1.3.4 Task： SPEED_BOUNDS_FINAL_DECIDER

产生速度规划边界
在这里插入图片描述在这里插入图片描述    在障碍物的上方或下方确定可行使区域。

1.3.5 Task： PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER && PIECEWISE_JERK_SPEED_OPTIMIZER

产生平滑速度规划曲线
在这里插入图片描述在这里插入图片描述    根据ST图的可行驶区域，优化出一条平滑的速度曲线。满足一阶导、二阶导平滑（速度加速度平滑）；满足道路限速；满足车辆动力学约束。
PIECEWISE_JERK_SPEED_OPTIMIZER 基于二次规划的速度规划
PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER 基于非线性规划的速度规划
两者二选一即可

1.3.6 Stage: CombinePathAndSpeedProfile

将SL曲线、ST曲线合成为完整轨迹，之后作为Planning的输出。
在这里插入图片描述

2. 基于动态规划的速度规划

2.1 动态规划

动态规划——通过把原问题分解为相对简单的子问题，再根据子问题的解来求解出原问题解的方法
在这里插入图片描述
状态转移方程
f ( P ) = min ⁡ { f ( R ) + w R → P } f(P) = \min \{ f(R) + {w_{R \to P}}\} f(P)=min{f(R)+wR→P​}

2.2 基于动态规划的速度规划

    基于动态规划的速度规划的流程如下：
1.对路程和时间进行采样
2.搜索出粗略的可行路线
3.选出代价最小的一条
l

2.2.1 对路程和时间进行采样

在这里插入图片描述
    速度规划在ST图进行采样，在 t t t的方向上以固定的间隔进行采样，在 s s s方向上以先密后疏的方式进行采样（离主车越近，所需规划的精度就需更高；离主车越远，牺牲采样精度，提升采样效率）

// 时间采样的一般参数设置
unit_t: 1.0  //采样时间
dense_dimension_s: 101  // 采样密集区域的点数
dense_unit_s: 0.1   //采样密集区域的间隔
sparse_unit_s: 1.0  //采样系数区域的间隔

2.2.2 设计状态转移方程

2.2.2.1 障碍物cost计算

在这里插入图片描述
S_safe_overtake超车的安全距离
S_safe_follow跟车的安全距离
    在设计状态转移方程时，要求不能与障碍物发生碰撞以及和障碍物不发生碰撞。于是可以得到以下方程：在这里插入图片描述

i i i是 t t t方向上的索引， j j j是 s s s方向上的索引

    如果在障碍物距离之内，则cost设为无穷；如果在安全距离之外，则cost设为0；如果在安全距离与障碍物之间，则按按之间的距离计算。

2.2.2.2 距离cost计算

    目的是更快的到达目的地
在这里插入图片描述    距离cost计算方式如下

C s p a t i a l = w s p a t i a l ( s t o t a l − s ( j ) ) {C_{spatial}} = {w_{spatial}}({s_{total}} - s(j)) Cspatial​=wspatial​(stotal​−s(j)) w s p a t i a l {w_{spatial}} wspatial​为损失权值
( s t o t a l − s ( j ) ) ({s_{total}} - s(j)) (stotal​−s(j))当前点到目标点的差值。

2.2.2.3 状态转移cost计算

    状态转移cost计算分为三个部分：
C e d g e = C s p e e d + C a c c + C j e r k {C_{edge}} = {C_{speed}} + {C_{acc}} + {C_{jerk}} Cedge​=Cspeed​+Cacc​+Cjerk​ C s p e e d {C_{speed}} Cspeed​——速度代价
C a c c {C_{acc}} Cacc​——加速度代价
C j e r k {C_{jerk}} Cjerk​——加加速度代价
节点间速度为： v = s ( j + k ) − s ( j ) Δ t v = \frac{{s(j + k) - s(j)}}{{\Delta t}} v=Δts(j+k)−s(j)​
限速比率： v det ⁡ = v − v l i m i t v l i m i t {v_{\det }} = \frac{{v - {v_{limit}}}}{{{v_{limit}}}} vdet​=vlimit​v−vlimit​​

     C s p e e d {C_{speed}} Cspeed​速度代价的计算如下：
在这里插入图片描述    若速度<0，则是倒车的状况，轨迹不可行，代价值设为无穷大；若速度>0,且高于限速，则会有超速的惩罚；若速度<0,且低于限速，则会有低速的惩罚。在Apollo中，超速的惩罚值（1000）远大于低速的惩罚值（10）。
在这里插入图片描述

    加速度的计算如下：
a ( i + 1 , j + k ) = s ( k + j ) − s ( j ) Δ t − s ( j ) − s ( l ) Δ t Δ t a(i + 1,j + k) = \frac{{\frac{{s(k + j) - s(j)}}{{\Delta t}} - \frac{{s(j) - s(l)}}{{\Delta t}}}}{{\Delta t}} a(i+1,j+k)=ΔtΔts(k+j)−s(j)​−Δts(j)−s(l)​​     C a c c {C_{acc}} Cacc​加速度代价的计算如下：在这里插入图片描述    若超过最大加速度或小于最小加速度，则代价值设为无穷大，若在之间，Apollo设计了这样的代价函数进行计算： y = x 2 + x 2 1 + e x + 4 + x 2 1 + e x + 2 y = {x^2} + \frac{{{x^2}}}{{1 + {e^{x + 4}}}} + \frac{{{x^2}}}{{1 + {e^{x + 2}}}} y=x2+1+ex+4x2​+1+ex+2x2​    其函数图像如下：在这里插入图片描述    越靠近0，代价值越小；越靠近目标值，代价值越大，满足舒适性与平滑性。
在这里插入图片描述    加加速度的计算方式如下： j e r k = s 4 − 3 s 3 + 3 s 2 − s 1 Δ t 3 jerk = \frac{{{s_4} - 3{s_3} + 3{s_2} - {s_1}}}{{\Delta {t^3}}} jerk=Δt3s4​−3s3​+3s2​−s1​​在这里插入图片描述
    加加速度超过设定边界，设为无穷；若在之间，则按二次方的方式进行计算。加加速度越小越好。

    最后是总的代价：在这里插入图片描述    迭代范围：在这里插入图片描述    在每次迭代时会将总的代价与当前节点的代价进行比较，取最小的一个，进行更新。
在这里插入图片描述    从 s ( i , j ) s(i,j) s(i,j)到 s ( i + 1 , j + k ) s(i+1,j+k) s(i+1,j+k)可以拓展到速度范围内的节点，按代价值的大小进行更新，最后按最后一列代价值最小的点进行求解，再进行回溯，得到ST曲线。

3. 基于二次规划的速度规划

    动态规划得到的轨迹还比较粗糙，需要用优化的方法对轨迹进行进一步的平滑。基于二次规划的速度规划的方法与路径规划基本一致。

  1. 确定优化变量
  2. 设计目标函数
  3. 设计约束在这里插入图片描述

3.1 确定优化变量

在这里插入图片描述    优化变量 x x x， x x x有三个部分组成：从 s 0 s_0 s0​, s 1 s_1 s1​, s 2 s_2 s2​到 s n − 1 s_{n-1} sn−1​，从 s ˙ 0 \dot s_0 s˙0​, s ˙ 1 \dot s_1 s˙1​, s ˙ 2 \dot s_2 s˙2​到 s ˙ n − 1 \dot s_{n-1} s˙n−1​,从 s ¨ 0 \ddot s_0 s¨0​, s ¨ 1 \ddot s_1 s¨1​, s ¨ 2 \ddot s_2 s¨2​到 s ¨ n − 1 \ddot s_{n-1} s¨n−1​.在这里插入图片描述ps：三阶导的求解方式为： s ′ ′ i + 1 − s ′ ′ i Δ t \frac{{{{s''}_{i + 1}} - {{s''}_i}}}{{\Delta t}} Δts′′i+1​−s′′i​​

3.2 设计目标函数

    对于目标函数的设计，我们需要明确以下目标：

  • 尽可能贴合决策时制定的st曲线： ∣ s i − s i − r e f ∣ ↓ \left| {{s_i} - {s_{i - ref}}} \right| \downarrow ∣si​−si−ref​∣↓
  • 确保舒适的体感，尽可能降低加速度/加加速度： ∣ s ¨ i + 1 ∣ ↓ \left| {{{\ddot s}_{i + 1}}} \right| \downarrow ∣s¨i+1​∣↓， ∣ s ′ ′ ′ i → i + 1 ∣ ↓ \left| {{{s'''}_{i \to i + 1}}} \right| \downarrow ∣s′′′i→i+1​∣↓
  • 尽可能按照巡航速度行驶： ∣ s ˙ i − v r e f ∣ ↓ \left| {{{\dot s}_i} - {v_{ref}}} \right| \downarrow ∣s˙i​−vref​∣↓
  • 在转弯时减速行驶, 曲率越大，速度越小： ∣ p i s ˙ i ∣ ↓ \left| {{p_i}{{\dot s}_i}} \right| \downarrow ∣pi​s˙i​∣↓

    最后会得到以下目标函数：在这里插入图片描述

w s w_s ws​——位置的权重
w v w_v wv​——速度的权重
p i p_i pi​——曲率的权重
w a w_a wa​——加速度的权重
w j w_j wj​——加加速度的权重

3.3 要满足的约束条件

    接下来谈谈约束的设计。
    要满足的约束条件：
• 主车必须在道路边界内，同时不能和障碍物有碰撞 s i ∈ ( s min ⁡ i , s max ⁡ i ) {s_i} \in (s_{\min }^i,s_{\max }^i) si​∈(smini​,smaxi​)• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：在这里插入图片描述•必须满足基本的物理原理：
在这里插入图片描述
•起始点约束：; s 0 = s i n i t s_0=s_{init} s0​=sinit​, s ˙ 0 = s i n i t \dot s_0=s_{init} s˙0​=sinit​, s ¨ 0 = s i n i t \ddot s_0=s_{init} s¨0​=sinit​满足的是起点的约束，即为实际车辆规划起点的状态。

3.4 转化为二次规划问题求解

在这里插入图片描述    代入OSQP求解器进行求解，输出一条平稳、舒适、能安全避开障碍物并且尽快到达目的地的速度分配曲线。

4. 基于非线性规划的速度规划

    为了使得限速更加精细，Apollo提出了一种基于非线性规划的速度规划方法。

4.1 二次规划速度规划算法的问题

在这里插入图片描述在这里插入图片描述    基于二次规划的速度规划中， p i p_i pi​是曲率关于时间 t t t的函数，但实际上路径的曲率是与 s s s相关的。二次规划在原先动态规划出来的粗糙ST曲线上将关于 s s s的曲率惩罚转化为关于 t t t的曲率惩罚，如此，当二次规划曲线与动态规划曲线差别不大，规划出来基本一致；若规划差别大，则会差别很大。就如图所示，规划出来的区间差别较大。限速/曲率的函数是关于 s s s的函数，而 s s s是我们要求的优化量，只能通过动态规划进行转化，如此就会使得二次规划的速度约束不精确。

4.2 确定优化变量

    基于非线性规划的速度规划步骤与之前规划步骤基本一致。
在这里插入图片描述    采样方式：等间隔的时间采样。 s l o w e r s_{lower} slower​与 s u p p e r s_{upper} supper​为松弛变量，防止求解失败。

4.3 定义目标函数

在这里插入图片描述    目标函数与二次规划的目标函数差不多，增加了横向加速度的代价值以及松弛变量 w s o f t s l o w e r w_{soft}s_{lower} wsoft​slower​与 w s o f t s u p p e r w_{soft}s_{upper} wsoft​supper​。
    横向加速度的计算方式：在这里插入图片描述
    曲率是关于 s s s的关系式,所以要进行平滑，对于非线性规划的求解器，无论是目标函数还是约束函数，都需要满足二阶可导： κ ′ = f ′ ′ ( s ) \kappa ' = f''(s) κ′=f′′(s)在这里插入图片描述    曲率的平滑也是用到了二次规划的方法，用曲率的一阶导、二阶导、三阶导作为损失函数.在这里插入图片描述    最后得到一条平滑曲率的曲线。

4.4 定义约束

    接下来是约束条件：

  • 规划的速度要一直往前走： s i ≤ s i + 1 {s_i} \le {s_{i + 1}} si​≤si+1​
  • 加加速度不能超过定义的极限值： j e r k min ⁡ ≤ s ¨ i + 1 − s ¨ i Δ t ≤ j e r k max ⁡ jer{k_{\min }} \le \frac{{{{\ddot s}_{i{\rm{ + 1}}}} - {{\ddot s}_i}}}{{\Delta t}} \le jer{k_{\max }} jerkmin​≤Δts¨i+1​−s¨i​​≤jerkmax​
  • 速度满足路径上的限速： s ˙ i ≤ s p e e d _ l i m i t ( s i ) {\dot s_i} \le speed\_limit({s_i}) s˙i​≤speed_limit(si​)

    限速的函数并非直接可以得到，接下来看看限速函数是怎么来的。
    限速的来源如下图所示：在这里插入图片描述    将所有的限速函数相加，得到下图的限速函数，很明显，该函数既不连续也不可导，所以需要对其进行平滑处理。在这里插入图片描述    对于限速曲线的平滑，Apollo采样分段多项式进行平滑，之后采样二次规划的方式进行求解。限速曲线的目标函数如下：在这里插入图片描述    如此，我们就有了连续且可导的限速曲线。
    再回到约束中,为了避免求解的失败，二次规划中对位置的硬约束，在非线性规划中转为了对位置的软约束。提升求解的精度。在这里插入图片描述    同时还需满足基本的物理学原理在这里插入图片描述

4.5 求解器求解

    最后代入Ipopt中进行非线性规划的求解。
    Ipopt（Interior Point Optimizer）是一个用于大规模非线性优化的开源软件包。它可用于解决如下形式的非线性规划问题：在这里插入图片描述     g L {g^L} gL和 g U {g^U} gU是约束函数的上界和下界， x L {x^L} xL和 x U {x^U} xU是优化变量的上界和下界。

    Ipopt的求解由以下几个函数构成：

1.get_nlp_info()定义问题规模

/** Method to return some info about the nlp */
  bool get_nlp_info(int &n, int &m, int &nnz_jac_g, int &nnz_h_lag,
                    IndexStyleEnum &index_style) override;

• 优化变量数量：n
• 约束函数数量：m
• 雅可比矩阵非0项数量：nnz_jac_g
• 黑塞矩阵非0项数量：nnz_h_lag

2.get_bounds_info()定义约束边界约束

  /** Method to return the bounds for my problem */
  bool get_bounds_info(int n, double *x_l, double *x_u, int m, double *g_l,
                       double *g_u) override;

• 自变量的下边界：x_l
• 自变量的上边界： x_u
• 约束函数下边界：g_l
• 约束函数的上边界：g_u

3.get_starting_point()定义初值

  /** Method to return the starting point for the algorithm */
  bool get_starting_point(int n, bool init_x, double *x, bool init_z,
                          double *z_L, double *z_U, int m, bool init_lambda,
                          double *lambda) override;

• 定义优化变量的初始值x
    对于速度规划问题，如何计算初始解？
    Apollo同样用分段多项式二次规划的求解方式，得到符号约束的速度平滑曲线，作为非线性规划的初值。在这里插入图片描述
4.eval_f()求解目标函数

  /** Method to return the objective value */
  bool eval_f(int n, const double *x, bool new_x, double &obj_value) override;

• 变量值：x
• 目标函数值：obj_val

5.eval_grad_f()求解梯度

  /** Method to return the gradient of the objective */
  bool eval_grad_f(int n, const double *x, bool new_x, double *grad_f) override;

• 变量值：x
• 梯度值：grad_f

梯度的定义：在这里插入图片描述
目标函数：在这里插入图片描述在这里插入图片描述偏导数：在这里插入图片描述
# 5. 速度规划算法实践在这里插入图片描述在这里插入图片描述

6.eval_g()求解约束函数

  /** Method to return the constraint residuals */
  bool eval_g(int n, const double *x, bool new_x, int m, double *g) override;

• 变量值：x
• 约束函数值：g

7.eval_jac_g()求解约束雅可比矩阵

  /** Method to return:
   *   1) The structure of the jacobian (if "values" is nullptr)
   *   2) The values of the jacobian (if "values" is not nullptr)
   */
  bool eval_jac_g(int n, const double *x, bool new_x, int m, int nele_jac,
                  int *iRow, int *jCol, double *values) override;

• 变量值：x
• 雅可比矩阵非0元素数量：nele_jac
• 雅可比矩阵值：values

雅可比矩阵：在这里插入图片描述    求解器通过稀疏矩阵来保存值。在这里插入图片描述在这里插入图片描述

稀疏矩阵的例子

    求解雅可比矩阵需要对约束函数进行求偏导：
对于速度约束有：
微分关系等式约束：在这里插入图片描述在这里插入图片描述在这里插入图片描述在这里插入图片描述

8.eval_h()求解黑塞矩阵

  /** Method to return:
   *   1) The structure of the hessian of the lagrangian (if "values" is
   * nullptr) 2) The values of the hessian of the lagrangian (if "values" is not
   * nullptr)
   */
  bool eval_h(int n, const double *x, bool new_x, double obj_factor, int m,
              const double *lambda, bool new_lambda, int nele_hess, int *iRow,
              int *jCol, double *values) override;

• 变量值：·x·
• 拉格朗日乘数：·lambda·
• 黑塞矩阵值：·values·
• 目标函数因数：·obj_factor·

黑塞矩阵：在这里插入图片描述在这里插入图片描述拉格朗日函数在这里插入图片描述Ipopt的拉格朗日黑塞矩阵：在这里插入图片描述
目标函数的二阶偏导数：
在这里插入图片描述

约束函数的二阶偏导数：
在这里插入图片描述
9. finalize_solution()

  /** @name Solution Methods */
  /** This method is called when the algorithm is complete so the TNLP can
   * store/write the solution */
  void finalize_solution(Ipopt::SolverReturn status, int n, const double *x,
                         const double *z_L, const double *z_U, int m,
                         const double *g, const double *lambda,
                         double obj_value, const Ipopt::IpoptData *ip_data,
                         Ipopt::IpoptCalculatedQuantities *ip_cq) override;

目标函数取得最小值时的优化量：x
目标函数最小值：obj_value

5. 速度规划算法实践

云实验地址——Apollo规划之速度规划仿真调试
1.启动DreamView

bash scripts/bootstrap.sh

在这里插入图片描述
模式选择Mkz Standard Debug,地图选择Apollo Virutal Map,打开Sim Control模式，打开PNC Monitor,等待屏幕中间区域出现Mkz车模型和地图后即表示成功进入仿真模式。在这里插入图片描述
点击左侧Tab栏Module Controller,启动Planning,Prediction, Routing模块， 如果需要录制数据则打开Recorder模块。在这里插入图片描述模块启动完成后，点击左侧Tab栏Profile, 选择Scenario Profiles里的course场景集，右上角选择场景场景开始仿真，点击减速让行场景和加速超车场景，观察PNC Monitor st曲线区别.

PNC Monitor中上方的st图是动态规划生成的st曲线，下方的st图是优化算法生成的st曲线在这里插入图片描述

5.1 加速超车，减速让行场景速度规划实践

减速让行的场景，可以看到规划出的曲线在ST图中位于障碍物的下方。
在这里插入图片描述
加速超车的场景，可以看到规划出的曲线在ST图中位于障碍物的上方。

在这里插入图片描述

5.2 弯道场景速度规划实践

打开Data Recorder,将场景切换为掉头场景，接近弯道时点击Updata Time记录时间，场景运行结束后关闭planning模块
在这里插入图片描述在云实验界面，点击Notebook打开jupyter

jupyter notebook

在这里插入图片描述
创建新的notebook，并输入%matplotlib notebook激活matplotlib

%matplotlib notebook

在jupyter notebook中运行以下命令打开对应时间的非线性规划的中间运行结果/apollo/modules/planning/tools/plot_st_nlp.py为绘图脚本文件的路径，planning.INFO为planning日志文件的路径，23:29:03为update更新的时间

run /apollo/modules/planning/tools/plot_st_nlp.py -f planning.INFO -t 23:29:03

在这里插入图片描述
有可能会出现这种状况在这里插入图片描述
刷新几次，或将日志文件用其他方式打开（例如vim），当里面出现日志内容时，就可以了

在这里插入图片描述在这里插入图片描述

横坐标为 t t t，纵坐标为 s s s的ST图。橙色与绿色的线为边界。蓝色为规划出来的曲线。紫色为动态规划产生的曲线。红色为非线性规划平滑后的曲线
在这里插入图片描述
速度关于时间的曲线
在这里插入图片描述
加速度关于时间的曲线
在这里插入图片描述
横坐标为 s s s,纵坐标为 v v v.可以看到通过弯道的速度还是很大的
在这里插入图片描述
位置与kappa关系的曲线。橙色为规划平滑后的曲线。

通过点击下方-1 + 1来切换上-帧或下一帧速度规划数据 在这里插入图片描述
调高planning_ config.pb.txt文件中的横向加速度权重，重新打开planning模块运行掉头场景，观察修改后速度曲线变化 

default_task_config: {
  task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER
  piecewise_jerk_nonlinear_speed_optimizer_config {
    acc_weight: 2.0
    jerk_weight: 3.0
    lat_acc_weight: 10.0
    s_potential_weight: 0.05
    ref_v_weight: 5.0
    ref_s_weight: 100.0
    soft_s_bound_weight: 1e6
    use_warm_start: true
  }
}

在这里插入图片描述在这里插入图片描述在这里插入图片描述在这里插入图片描述在这里插入图片描述

lat_acc_weight: 1000.0

5.3 通过减速带场景速度规划实践

打开planning模块，切换场景到减速带场景进行仿真，接近减速带时记录下时间，并通过plot st nlp.py脚本观察接近减速带时非线性规划算法的速度规划
在这里插入图片描述

在lane_ follow config.pb.txt文件中， 修改速度优化算法为QP算法，重新打开planning模块， 重新运行减速带场景，并记录接近减速带时的时间

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: 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

运行plot_ st qp.py,观察二次规划算法的速度 规划曲线在减速带区域速度规划和非线性规划算法有何区别。
在这里插入图片描述在这里插入图片描述
在这里插入图片描述在这里插入图片描述

通过减速带的速度不超过5m/s.
在这里插入图片描述
非线性规划

在这里插入图片描述
在这里插入图片描述在这里插入图片描述在这里插入图片描述

超出约束，算法的缺陷
在这里插入图片描述
二次规划

二次规划求解效率高，但不精确；非线性规划求解效率低，但精度高。

悦读

道可道，非常道；名可名，非常名。 无名，天地之始，有名，万物之母。 故常无欲，以观其妙，常有欲，以观其徼。 此两者，同出而异名，同谓之玄，玄之又玄，众妙之门。

最新收录
Tableau招聘信息数据可视化
MySQL触发器
最新毕设-Node.js-贺州市油茶生物多样性管理-54787（免费领项目）可做计算机毕业设计JAVA、PHP、爬虫、APP、小程序、C#、C++、python、数据可视化、全套文案
用QFramework来重构 祖玛游戏
深入了解：与AI高效沟通的诀窍
C语言-指针讲解(3)
无线路由器建立usb共享打印服务器,无线路由器USB网络共享管理设置方法
时间序列模型后续~（从简单线性回归到xgboost + 层次聚类）
高通 camera 调试
一文通透优化算法：从梯度下降、SGD到牛顿法、共轭梯度(23修订版)
;