【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER(一)

文章目录

  • TASK系列解析文章
  • 前言
  • PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER功能介绍
  • PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER相关配置
  • PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER流程
    • 确定优化变量
    • 定义目标函数
    • 定义约束
    • Process
      • SetUpStatesAndBounds
      • OptimizeByQP
      • CheckSpeedLimitFeasibility
      • SmoothPathCurvature
      • SmoothSpeedLimit
      • OptimizeByNLP
  • 参考

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(二)

前言

在Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine函数会依次调用task_list中的TASK,本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限,文章可能有纰漏的地方,还请批评斧正。

modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中,我们可以看到LaneFollow所需要执行的所有task。

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

本文将继续介绍LaneFollow的第14个TASK——PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER

PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER功能介绍

产生平滑速度规划曲线
在这里插入图片描述在这里插入图片描述
根据ST图的可行驶区域,优化出一条平滑的速度曲线。满足一阶导、二阶导平滑(速度加速度平滑);满足道路限速;满足车辆动力学约束。

PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER相关配置

modules/planning/conf/planning_config.pb.txt

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: 1000.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
  }
}

PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER流程

上文我们介绍了基于二次规划的速度规划方法【Apollo学习笔记】——规划模块TASK之PIECEWISE_JERK_SPEED_OPTIMIZER

首先,来看看基于二次规划的速度规划方法存在的问题。
m i n f = ∑ i = 0 n − 1 w s − r e f ( s i − s i − r e f ) 2 + w d s − r e f ( s ˙ i − s ˙ r e f ) 2 + p i s ˙ i 2 + w d d s s ¨ i 2 + ∑ i = 0 n − 2 w d d d s s ′ ′ ′ i → i + 1 2 + w e n d − s ( s n − 1 − s e n d ) 2 + w e n d − d s ( s n − 1 ˙ − s e n d ˙ ) 2 + w e n d − d d s ( s n − 1 ¨ − s e n d ¨ ) 2 \begin{aligned} minf&=\sum_{i=0}^{n-1}w_{s-ref}(s_i-s_{i-ref})^2+w_{ds-ref}(\dot{s}_i-\dot s_{ref})^2+p_i\dot{s}_i^2+w_{dds}\ddot{s}_i^2+\sum_{\color{red}i=0}^{\color{red}n-2}w_{ddds}{s^{'''}}_{i \to i + 1}^2\\ & +w_{end-s}(s_{n-1}-s_{end})^2+w_{end-ds}(\dot{s_{n-1}}-\dot{s_{end}})^2+w_{end-dds}(\ddot{s_{n-1}}-\ddot{s_{end}})^2 \end{aligned} minf=i=0n1wsref(sisiref)2+wdsref(s˙is˙ref)2+pis˙i2+wddss¨i2+i=0n2wdddss′′′ii+12+wends(sn1send)2+wendds(sn1˙send˙)2+wenddds(sn1¨send¨)2
在这里插入图片描述

// modules/planning/tasks/optimizers/piecewise_jerk_speed/piecewise_jerk_speed_optimizer.cc
    // get path_s
    SpeedPoint sp;
    // 依据当前时间估计
    reference_speed_data.EvaluateByTime(curr_t, &sp);
    const double path_s = sp.s();
    x_ref.emplace_back(path_s);
    // get curvature
    PathPoint path_point = path_data.GetPathPointWithPathS(path_s);
    penalty_dx.push_back(std::fabs(path_point.kappa()) *
                         config.kappa_penalty_weight());

基于二次规划的速度规划中, p i p_i pi是曲率关于时间 t t t的函数(从代码中可以看到曲率 κ \kappa κ是依据时间 t t t估计出的点计算的),但实际上路径的曲率是与 s s s相关的。二次规划在原先动态规划出来的粗糙ST曲线上将关于 s s s的曲率惩罚转化为关于 t t t的曲率惩罚,如此,当二次规划曲线与动态规划曲线差别不大,规划出来基本一致;若规划差别大,则会差别很大。就如图所示,规划出来的区间差别较大。限速/曲率的函数是关于 s s s的函数,而 s s s是我们要求的优化量,只能通过动态规划进行转化,如此就会使得二次规划的速度约束不精确。

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

非线性规划(Nonlinear Programming,简称NLP)是指在目标函数或者约束条件中包含非线性函数的优化问题。目标函数或者约束条件都可以是非线性/非凸的,但是需要满足二阶连续可导。以下是非线性规划的标准形式:

min ⁡ x ∈ R n f ( x ) s.t. g L ≤ g ( x ) ≤ g U x L ≤ x ≤ x U , x ∈ R n \begin{aligned} \min_{x\in\mathbb{R}^{n}}&& f(x) \\ \text{s.t.}&& g^{L}\leq g(x)\leq g^{U} \\ &&x^{L}\leq x\leq x^{U}, \\ &&x\in\mathbb{R}^{n} \end{aligned} xRnmins.t.f(x)gLg(x)gUxLxxU,xRn

g L {g^L} gL g U {g^U} gU是约束函数的上界和下界, x L {x^L} xL x U {x^U} xU是优化变量的上界和下界。


确定优化变量

基于非线性规划的速度规划步骤与之前规划步骤基本一致。
x = ( s 0 , s 1 , … , s n − 1 , s ˙ 0 , s ˙ 1 , … , s ˙ n − 1 , s ¨ 0 , s ¨ 1 , … , s ¨ n − 1 , s _ s l a c k _ u p p e r 0 , s _ s l a c k _ l o w e r 1 , … , s _ s l a c k _ l o w e r n − 1 , s _ s l a c k _ u p p e r 0 , s _ s l a c k _ u p p e r 1 , … , s _ s l a c k _ u p p e r n − 1 ) \begin{aligned}x=\begin{pmatrix}s_0,s_1,\ldots,s_{n-1},\\\dot{s}_0,\dot{s}_1,\ldots,\dot{s}_{n-1},\\\ddot{s}_0,\ddot{s}_1,\ldots,\ddot{s}_{n-1},\\s\_slack\_upper_0,s\_slack\_lower_1,\ldots,s\_slack\_lower_{n-1},\\s\_slack\_upper_0,s\_slack\_upper_1,\ldots,s\_slack\_upper_{n-1}\end{pmatrix}\end{aligned} x= s0,s1,,sn1,s˙0,s˙1,,s˙n1,s¨0,s¨1,,s¨n1,s_slack_upper0,s_slack_lower1,,s_slack_lowern1,s_slack_upper0,s_slack_upper1,,s_slack_uppern1

采样方式:等间隔的时间采样。除此之外非线性规划中如果打开了软约束FLAGS_use_soft_bound_in_nonlinear_speed_opt,还会有松弛变量 s _ s l a c k _ l o w e r s\_slack\_lower s_slack_lower s _ s l a c k _ u p p e r s\_slack\_upper s_slack_upper,防止求解失败。

定义目标函数

m i n f = ∑ i = 0 n − 1 w s − r e f ( s i − s − r e f i ) 2 + w v − r e f ( s ˙ i − v − r e f ) 2 + w a s ¨ i 2 + ∑ i = 0 n − 2 w j ( s ¨ i + 1 − s ¨ i Δ t ) 2 + ∑ i = 0 n − 1 w l a t _ a c c l a t _ a c c i 2 + w s o f t s _ s l a c k _ l o w e r i + w s o f t s _ s l a c k _ u p p e r i + w t a r g e t − s ( s − s t a r g e t ) 2 + w t a r g e t − v ( v − v t a r g e t ) 2 + w t a r g e t − a ( a − a t a r g e t ) 2 \begin{aligned}minf=&\sum_{i=0}^{n-1}w_{s-ref}(s_i-s_-ref_i)^2+w_{v-ref}(\dot{s}_i-v_-ref)^2+w_a\ddot{s}_i^2+\sum_{i=0}^{n-2}w_j(\frac{\ddot{s}_{i+1}-\ddot{s}_i}{\Delta t})^2\\&+\sum_{i=0}^{n-1}w_{lat\_acc}lat\_acc_i^2+w_{soft}s\_slack\_lower_i+w_{soft}s\_slack\_upper_i\\&+w_{target-s}(s-s_{target})^2+w_{target-v}(v-v_{target})^2+w_{target-a}(a-a_{target})^2 \end{aligned} minf=i=0n1wsref(sisrefi)2+wvref(s˙ivref)2+was¨i2+i=0n2wj(Δts¨i+1s¨i)2+i=0n1wlat_acclat_acci2+wsofts_slack_loweri+wsofts_slack_upperi+wtargets(sstarget)2+wtargetv(vvtarget)2+wtargeta(aatarget)2
目标函数与二次规划的目标函数差不多,增加了横向加速度的代价值以及松弛变量 w s o f t s _ s l a c k _ l o w e r w_{soft}s\_slack\_lower wsofts_slack_lower w s o f t s _ s l a c k _ u p p e r w_{soft}s\_slack\_upper wsofts_slack_upper

横向加速度的计算方式:
l a t _ a c c i = s i 2 ⋅ κ ( s i ) lat\_acc_i=s^2_i\cdot\kappa(s_i) lat_acci=si2κ(si)

定义约束

接下来是约束条件:
对于变量 x x x的边界约束,需满足:

  • s i ∈ ( s min ⁡ i , s max ⁡ i ) {s_i} \in (s_{\min }^i,s_{\max }^i) si(smini,smaxi)
  • s i ′ ∈ ( s m i n i ′ ( t ) , s m a x i ′ ( t ) ) s_{i}^{\prime}\in\left(s_{min}^{i}{}^{\prime}(t),s_{max}^{i}{}^{\prime}(t)\right) si(smini(t),smaxi(t))
  • s i ′ ′ ∈ ( s m i n i ′ ′ ( t ) , s m a x i ′ ′ ( t ) ) s_{i}^{\prime\prime}\in\left(s_{min}^{i}{}^{\prime\prime}(t),s_{max}^{i}{}^{\prime\prime}(t)\right) si′′(smini′′(t),smaxi′′(t))
  • s _ s l a c k _ l o w e r i ∈ ( s _ s l a c k _ l o w e r min ⁡ i , s _ s l a c k _ l o w e r max ⁡ i ) {s\_slack\_lower_i} \in ({s\_slack\_lower}_{\min }^i,{s\_slack\_lower}_{\max }^i) s_slack_loweri(s_slack_lowermini,s_slack_lowermaxi)
  • s _ s l a c k _ u p p e r i ∈ ( s _ s l a c k _ u p p e r min ⁡ i , s _ s l a c k _ u p p e r max ⁡ i ) {s\_slack\_upper_i} \in ({s\_slack\_upper}_{\min }^i,{s\_slack\_upper}_{\max }^i) s_slack_upperi(s_slack_uppermini,s_slack_uppermaxi)

对于 g ( x ) g(x) g(x)的约束,需满足:

  • 规划的速度要一直往前走 s i ≤ s i + 1 {s_i} \le {s_{i + 1}} sisi+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+1s¨ijerkmax
  • 速度满足路径上的限速 s ˙ i ≤ s p e e d _ l i m i t ( s i ) {\dot s_i} \le speed\_limit({s_i}) s˙ispeed_limit(si)。这部分在SmoothSpeedLimit有具体介绍。

为了避免求解的失败,二次规划中对位置的硬约束,在非线性规划中转为了对位置的软约束。提升求解的精度。
s i − s _ s o f t _ l o w e r i + s _ s l a c k _ l o w e r i ≥ 0 s i − s _ s o f t _ u p p e r i − s _ s l a c k _ u p p e r i ≤ 0 \begin{aligned}s_i-s\_soft\_lower_i+s\_slack\_lower_i\geq0\\s_i-s\_soft\_upper_i-s\_slack\_upper_i\leq0\end{aligned} sis_soft_loweri+s_slack_loweri0sis_soft_upperis_slack_upperi0

同时还需满足基本的物理学原理,即连续性,和二次规划相比,少了加速度?:

s i + 1 ′ = s i ′ + ∫ 0 Δ t s ′ ′ ( t ) d t = s i ′ + s i ′ ′ ∗ Δ t + 1 2 ∗ s i → i + 1 ′ ′ ′ ∗ Δ t 2 = s i ′ + 1 2 ∗ s i ′ ′ ∗ Δ t + 1 2 ∗ s i + 1 ′ ′ ∗ Δ t s i + 1 = s i + ∫ 0 Δ t s ′ ( t ) d t = s i + s i ′ ∗ Δ t + 1 2 ∗ s i ′ ′ ∗ Δ t 2 + 1 6 ∗ s i → i + 1 ′ ′ ′ ∗ Δ t 3 = s i + s i ′ ∗ Δ t + 1 3 ∗ s i ′ ′ ∗ Δ t 2 + 1 6 ∗ s i + 1 ′ ′ ∗ Δ t 2 \begin{aligned} s_{i+1}^{\prime} &=s_i^{\prime}+\int_0^{\Delta t}\boldsymbol{s''}(t)dt=s_i^{\prime}+s_i^{\prime\prime}*\Delta t+\frac12*s_{i\to i+1}^{\prime\prime\prime}*\Delta t^2 \\ &= s_i^{\prime}+\frac12*s_i^{\prime\prime}*\Delta t+\frac12*s_{i+1}^{\prime\prime}*\Delta t\\ s_{i+1} &=s_i+\int_0^{\Delta t}\boldsymbol{s'}(t)dt \\ &=s_i+s_i^{\prime}*\Delta t+\frac12*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i\to i+1}^{\prime\prime\prime}*\Delta t^3\\ &=s_i+s_i^{\prime}*\Delta t+\frac13*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i+1}^{\prime\prime}*\Delta t^2 \end{aligned} si+1si+1=si+0Δts′′(t)dt=si+si′′Δt+21sii+1′′′Δt2=si+21si′′Δt+21si+1′′Δt=si+0Δts(t)dt=si+siΔt+21si′′Δt2+61sii+1′′′Δt3=si+siΔt+31si′′Δt2+61si+1′′Δt2


Process

PiecewiseJerkSpeedNonlinearOptimizer 继承自基类SpeedOptimizer. 因此,当task::Execute()被执行时, PiecewiseJerkSpeedNonlinearOptimizer中的函数Process()将会执行具体流程。

流程大致如下:

  • Input.输入部分包括PathData以及起始的TrajectoryPoint
  • Process.
    • Snaity Check. 这样可以确保speed_data不为空,并且speed Optimizer不会接收到空数据.
    • const auto problem_setups_status = SetUpStatesAndBounds(path_data, *speed_data); 初始化QP问题。若失败,则会清除speed_data中的数据。
    • const auto qp_smooth_status = OptimizeByQP(speed_data, &distance, &velocity, &acceleration); 求解QP问题,并获得distance\velocity\acceleration等数据。 若失败,则会清除speed_data中的数据。这部分用以计算非线性问题的初始解,对动态规划的结果进行二次规划平滑
    • const bool speed_limit_check_status = CheckSpeedLimitFeasibility();
      检查速度限制。接着或执行以下四个步骤:
      1)Smooth Path Curvature 2)SmoothSpeedLimit 3)Optimize By NLP 4)Record speed_constraint
    • 将 s/t/v/a/jerk等信息添加进 speed_data 并且补零防止fallback。
  • Output.输出SpeedData, 包括轨迹的s/t/v/a/jerk。

SetUpStatesAndBounds

SetUpStatesAndBounds主要完成对 s i n i t , s ˙ i n i t , s ¨ i n i t s_{init},\dot s_{init},\ddot s_{init} sinit,s˙init,s¨init的初始化设置;初始化设置 s ˙ , s ¨ , s ′ ′ ′ \dot s,\ddot s, s^{'''} s˙,s¨,s′′′的boundary;根据FLAGS_use_soft_bound_in_nonlinear_speed_opt判断是否启用软约束;若启用,则依据不同类型的boundary,更新s_soft_bounds_和s_bounds_;若不启用,同样依据不同类型的boundary,更新s_bounds_;最后获取speed_limit_和cruise_speed_。

Status PiecewiseJerkSpeedNonlinearOptimizer::SetUpStatesAndBounds(
    const PathData& path_data, const SpeedData& speed_data) {
  // Set st problem dimensions
  const StGraphData& st_graph_data =
      *reference_line_info_->mutable_st_graph_data();
  // TODO(Jinyun): move to confs
  delta_t_ = 0.1;
  total_length_ = st_graph_data.path_length();
  total_time_ = st_graph_data.total_time_by_conf();
  num_of_knots_ = static_cast<int>(total_time_ / delta_t_) + 1;

  // Set initial values
  s_init_ = 0.0;
  s_dot_init_ = st_graph_data.init_point().v();
  s_ddot_init_ = st_graph_data.init_point().a();

  // Set s_dot bounary
  s_dot_max_ = std::fmax(FLAGS_planning_upper_speed_limit,
                         st_graph_data.init_point().v());

  // Set s_ddot boundary
  const auto& veh_param =
      common::VehicleConfigHelper::GetConfig().vehicle_param();
  s_ddot_max_ = veh_param.max_acceleration();
  s_ddot_min_ = -1.0 * std::abs(veh_param.max_deceleration());

  // Set s_dddot boundary
  // TODO(Jinyun): allow the setting of jerk_lower_bound and move jerk config to
  // a better place
  s_dddot_min_ = -std::abs(FLAGS_longitudinal_jerk_lower_bound);
  s_dddot_max_ = FLAGS_longitudinal_jerk_upper_bound;

  // Set s boundary
  // 若启用软约束
  if (FLAGS_use_soft_bound_in_nonlinear_speed_opt) {
    s_bounds_.clear();
    s_soft_bounds_.clear();
    // TODO(Jinyun): move to confs
    // 遍历每一段segment
    for (int i = 0; i < num_of_knots_; ++i) {
      double curr_t = i * delta_t_;
      double s_lower_bound = 0.0;
      double s_upper_bound = total_length_;
      double s_soft_lower_bound = 0.0;
      double s_soft_upper_bound = total_length_;
      // 遍历每一个STBoundary
      for (const STBoundary* boundary : st_graph_data.st_boundaries()) {
        double s_lower = 0.0;
        double s_upper = 0.0;
        // 获取未被阻塞的s的范围,即s_lower和s_upper
        if (!boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
          continue;
        }
        SpeedPoint sp;
        // 根据不同的类型,更新bound
        switch (boundary->boundary_type()) {
          case STBoundary::BoundaryType::STOP:
          case STBoundary::BoundaryType::YIELD:
            s_upper_bound = std::fmin(s_upper_bound, s_upper);
            s_soft_upper_bound = std::fmin(s_soft_upper_bound, s_upper);
            break;
          case STBoundary::BoundaryType::FOLLOW:
            s_upper_bound =
                // FLAGS_follow_min_distance: min follow distance for vehicles/bicycles/moving objects; 3.0
                std::fmin(s_upper_bound, s_upper - FLAGS_follow_min_distance);
            // 依据时间估计出SpeedPoint
            if (!speed_data.EvaluateByTime(curr_t, &sp)) {
              const std::string msg =
                  "rough speed profile estimation for soft follow fence failed";
              AERROR << msg;
              return Status(ErrorCode::PLANNING_ERROR, msg);
            }
            s_soft_upper_bound =
                std::fmin(s_soft_upper_bound,
                          s_upper - FLAGS_follow_min_distance -
                          // FLAGS_follow_time_buffer: time buffer in second to calculate the following distance
                          // 2.5
                              std::min(7.0, FLAGS_follow_time_buffer * sp.v()));
            break;
          case STBoundary::BoundaryType::OVERTAKE:
            s_lower_bound = std::fmax(s_lower_bound, s_lower);
            s_soft_lower_bound = std::fmax(s_soft_lower_bound, s_lower + 10.0);
            break;
          default:
            break;
        }
      }
      if (s_lower_bound > s_upper_bound) {
        const std::string msg =
            "s_lower_bound larger than s_upper_bound on STGraph";
        AERROR << msg;
        return Status(ErrorCode::PLANNING_ERROR, msg);
      }
      s_soft_bounds_.emplace_back(s_soft_lower_bound, s_soft_upper_bound);
      s_bounds_.emplace_back(s_lower_bound, s_upper_bound);
    }
  } else {
    s_bounds_.clear();
    // TODO(Jinyun): move to confs
    for (int i = 0; i < num_of_knots_; ++i) {
      double curr_t = i * delta_t_;
      double s_lower_bound = 0.0;
      double s_upper_bound = total_length_;
      for (const STBoundary* boundary : st_graph_data.st_boundaries()) {
        double s_lower = 0.0;
        double s_upper = 0.0;
        if (!boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
          continue;
        }
        SpeedPoint sp;
        switch (boundary->boundary_type()) {
          case STBoundary::BoundaryType::STOP:
          case STBoundary::BoundaryType::YIELD:
            s_upper_bound = std::fmin(s_upper_bound, s_upper);
            break;
          case STBoundary::BoundaryType::FOLLOW:
            s_upper_bound = std::fmin(s_upper_bound, s_upper - 8.0);
            break;
          case STBoundary::BoundaryType::OVERTAKE:
            s_lower_bound = std::fmax(s_lower_bound, s_lower);
            break;
          default:
            break;
        }
      }
      if (s_lower_bound > s_upper_bound) {
        const std::string msg =
            "s_lower_bound larger than s_upper_bound on STGraph";
        AERROR << msg;
        return Status(ErrorCode::PLANNING_ERROR, msg);
      }
      s_bounds_.emplace_back(s_lower_bound, s_upper_bound);
    }
  }
  // 获取speed_limit_和cruise_speed_
  speed_limit_ = st_graph_data.speed_limit();
  cruise_speed_ = reference_line_info_->GetCruiseSpeed();
  return Status::OK();
}

OptimizeByQP


这部分用以计算非线性问题的初始解,对动态规划的结果进行二次规划平滑。Apollo同样用分段多项式二次规划的求解方式,得到符合约束的速度平滑曲线,作为非线性规划的初值。

目标函数
m i n f = ∑ i = 0 n − 1 w s ( s i − s i − r e f ) 2 + ∑ i = 0 n − 1 w d d s s ¨ i 2 + ∑ i = 0 n − 2 w d d d s s ′ ′ ′ i → i + 1 2 \begin{aligned} minf&=\sum_{i=0}^{n-1}w_s(s_i-s_{i-ref})^2+\sum_{i=0}^{n-1}w_{dds}\ddot s_{i}^2+\sum_{i=0}^{n-2}w_{ddds}{s^{'''}}_{i \to i + 1}^2 \end{aligned} minf=i=0n1ws(sisiref)2+i=0n1wddss¨i2+i=0n2wdddss′′′ii+12

约束
主车必须在道路边界内,同时不能和障碍物有碰撞 s i ∈ ( s min ⁡ i , s max ⁡ i ) {s_i} \in (s_{\min }^i,s_{\max }^i) si(smini,smaxi)根据当前状态,主车的横向速度/加速度/加加速度有特定运动学限制
s i ′ ∈ ( s m i n i ′ ( t ) , s m a x i ′ ( t ) ) , s i ′ ′ ∈ ( s m i n i ′ ′ ( t ) , s m a x i ′ ′ ( t ) ) , s i ′ ′ ′ ∈ ( s m i n i ′ ′ ′ ( t ) , s m a x i ′ ′ ′ ( t ) ) s_{i}^{\prime}\in\left(s_{min}^{i}{}^{\prime}(t),s_{max}^{i}{}^{\prime}(t)\right)\text{,}s_{i}^{\prime\prime}\in\left(s_{min}^{i}{}^{\prime\prime}(t),s_{max}^{i}{}^{\prime\prime}(t)\right)\text{,}s_{i}^{\prime\prime\prime}\in\left(s_{min}^{i}{}^{\prime\prime\prime}(t),s_{max}^{i}{}^{\prime\prime\prime}(t)\right) si(smini(t),smaxi(t)),si′′(smini′′(t),smaxi′′(t)),si′′′(smini′′′(t),smaxi′′′(t))
连续性约束
s i + 1 ′ ′ = s i ′ ′ + ∫ 0 Δ t s i → i + 1 ′ ′ ′ d t = s i ′ ′ + s i → i + 1 ′ ′ ′ ∗ Δ t s i + 1 ′ = s i ′ + ∫ 0 Δ t s ′ ′ ( t ) d t = s i ′ + s i ′ ′ ∗ Δ t + 1 2 ∗ s i → i + 1 ′ ′ ′ ∗ Δ t 2 = s i ′ + 1 2 ∗ s i ′ ′ ∗ Δ t + 1 2 ∗ s i + 1 ′ ′ ∗ Δ t s i + 1 = s i + ∫ 0 Δ t s ′ ( t ) d t = s i + s i ′ ∗ Δ t + 1 2 ∗ s i ′ ′ ∗ Δ t 2 + 1 6 ∗ s i → i + 1 ′ ′ ′ ∗ Δ t 3 = s i + s i ′ ∗ Δ t + 1 3 ∗ s i ′ ′ ∗ Δ t 2 + 1 6 ∗ s i + 1 ′ ′ ∗ Δ t 2 \begin{aligned} s_{i+1}^{\prime\prime} &=s_i''+\int_0^{\Delta t}s_{i\to i+1}^{\prime\prime\prime}dt=s_i''+s_{i\to i+1}^{\prime\prime\prime}*\Delta t \\ s_{i+1}^{\prime} &=s_i^{\prime}+\int_0^{\Delta t}\boldsymbol{s''}(t)dt=s_i^{\prime}+s_i^{\prime\prime}*\Delta t+\frac12*s_{i\to i+1}^{\prime\prime\prime}*\Delta t^2 \\ &= s_i^{\prime}+\frac12*s_i^{\prime\prime}*\Delta t+\frac12*s_{i+1}^{\prime\prime}*\Delta t\\ s_{i+1} &=s_i+\int_0^{\Delta t}\boldsymbol{s'}(t)dt \\ &=s_i+s_i^{\prime}*\Delta t+\frac12*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i\to i+1}^{\prime\prime\prime}*\Delta t^3\\ &=s_i+s_i^{\prime}*\Delta t+\frac13*s_i^{\prime\prime}*\Delta t^2+\frac16*s_{i+1}^{\prime\prime}*\Delta t^2 \end{aligned} si+1′′si+1si+1=si′′+0Δtsii+1′′′dt=si′′+sii+1′′′Δt=si+0Δts′′(t)dt=si+si′′Δt+21sii+1′′′Δt2=si+21si′′Δt+21si+1′′Δt=si+0Δts(t)dt=si+siΔt+21si′′Δt2+61sii+1′′′Δt3=si+siΔt+31si′′Δt2+61si+1′′Δt2

起点约束 s 0 = s i n i t s_0=s_{init} s0=sinit, s ˙ 0 = s ˙ i n i t \dot s_0=\dot s_{init} s˙0=s˙init, s ¨ 0 = s ¨ i n i t \ddot s_0=\ddot s_{init} s¨0=s¨init满足的是起点的约束,即为实际车辆规划起点的状态。

Status PiecewiseJerkSpeedNonlinearOptimizer::OptimizeByQP(
    SpeedData* const speed_data, std::vector<double>* distance,
    std::vector<double>* velocity, std::vector<double>* acceleration) {
  std::array<double, 3> init_states = {s_init_, s_dot_init_, s_ddot_init_};
  PiecewiseJerkSpeedProblem piecewise_jerk_problem(num_of_knots_, delta_t_,
                                                   init_states);
  piecewise_jerk_problem.set_dx_bounds(
      0.0, std::fmax(FLAGS_planning_upper_speed_limit, init_states[1]));
  piecewise_jerk_problem.set_ddx_bounds(s_ddot_min_, s_ddot_max_);
  piecewise_jerk_problem.set_dddx_bound(s_dddot_min_, s_dddot_max_);
  piecewise_jerk_problem.set_x_bounds(s_bounds_);

  // TODO(Jinyun): parameter tunnings
  const auto& config =
      config_.piecewise_jerk_nonlinear_speed_optimizer_config();
  piecewise_jerk_problem.set_weight_x(0.0);
  piecewise_jerk_problem.set_weight_dx(0.0);
  piecewise_jerk_problem.set_weight_ddx(config.acc_weight());
  piecewise_jerk_problem.set_weight_dddx(config.jerk_weight());

  std::vector<double> x_ref;
  for (int i = 0; i < num_of_knots_; ++i) {
    const double curr_t = i * delta_t_;
    // get path_s
    SpeedPoint sp;
    speed_data->EvaluateByTime(curr_t, &sp);
    x_ref.emplace_back(sp.s());
  }
  piecewise_jerk_problem.set_x_ref(config.ref_s_weight(), std::move(x_ref));

  // Solve the problem
  if (!piecewise_jerk_problem.Optimize()) {...


  *distance = piecewise_jerk_problem.opt_x();
  *velocity = piecewise_jerk_problem.opt_dx();
  *acceleration = piecewise_jerk_problem.opt_ddx();
  return Status::OK();
}

CheckSpeedLimitFeasibility

检查speedlimit是否可行,若不可行则输出QP的结果;若可行,则继续进行非线性规划。代码中只对始点的速度限制和起始点的初始速度进行比较。

bool PiecewiseJerkSpeedNonlinearOptimizer::CheckSpeedLimitFeasibility() {
  // a naive check on first point of speed limit
  static constexpr double kEpsilon = 1e-6;
  const double init_speed_limit = speed_limit_.GetSpeedLimitByS(s_init_);
  // 起始点的速度限制和起始点的初始速度进行比较
  if (init_speed_limit + kEpsilon < s_dot_init_) {
    AERROR << "speed limit [" << init_speed_limit
           << "] lower than initial speed[" << s_dot_init_ << "]";
    return false;
  }
  return true;
}

SmoothPathCurvature

曲率是关于 s s s的关系式,所以要进行平滑拟合,对于非线性规划的求解器,无论是目标函数还是约束函数,都需要满足二阶可导: κ ′ = f ′ ′ ( s ) \kappa ' = f''(s) κ=f′′(s)在这里插入图片描述
ps: l → κ l \rightarrow \kappa lκ
曲率的平滑也是用到了二次规划的方法,用曲率的一阶导、二阶导、三阶导作为损失函数.

目标函数
m i n f = ∑ i = 0 n − 1 w κ ( κ i − κ i − r e f ) 2 + ∑ i = 0 n − 1 w d d κ κ ¨ i 2 + ∑ i = 0 n − 2 w d d d κ κ ′ ′ ′ i → i + 1 2 \begin{aligned} minf&=\sum_{i=0}^{n-1}w_{\kappa}(\kappa_i-\kappa_{i-ref})^2+\sum_{i=0}^{n-1}w_{dd\kappa}\ddot \kappa_{i}^2+\sum_{i=0}^{n-2}w_{ddd\kappa}{\kappa^{'''}}_{i \to i + 1}^2 \end{aligned} minf=i=0n1wκ(κiκiref)2+i=0n1wddκκ¨i2+i=0n2wdddκκ′′′ii+12

约束
κ i ∈ ( κ min ⁡ i , κ max ⁡ i ) {\kappa_i} \in (\kappa_{\min }^i,\kappa_{\max }^i) κi(κmini,κmaxi) κ i ′ ∈ ( κ m i n i ′ ( s ) , κ m a x i ′ ( s ) ) , κ i ′ ′ ∈ ( κ m i n i ′ ′ ( s ) , κ m a x i ′ ′ ( s ) ) , κ i ′ ′ ′ ∈ ( κ m i n i ′ ′ ′ ( s ) , κ m a x i ′ ′ ′ ( s ) ) \kappa_{i}^{\prime}\in\left(\kappa_{min}^{i}{}^{\prime}(s),\kappa_{max}^{i}{}^{\prime}(s)\right)\text{,}\kappa_{i}^{\prime\prime}\in\left(\kappa_{min}^{i}{}^{\prime\prime}(s),\kappa_{max}^{i}{}^{\prime\prime}(s)\right)\text{,}\kappa_{i}^{\prime\prime\prime}\in\left(\kappa_{min}^{i}{}^{\prime\prime\prime}(s),\kappa_{max}^{i}{}^{\prime\prime\prime}(s)\right) κi(κmini(s),κmaxi(s)),κi′′(κmini′′(s),κmaxi′′(s)),κi′′′(κmini′′′(s),κmaxi′′′(s))
连续性约束
κ i + 1 ′ ′ = κ i ′ ′ + ∫ 0 Δ s κ i → i + 1 ′ ′ ′ d s = κ i ′ ′ + κ i → i + 1 ′ ′ ′ ∗ Δ s κ i + 1 ′ = κ i ′ + ∫ 0 Δ s κ ′ ′ ( s ) d s = κ i ′ + κ i ′ ′ ∗ Δ s + 1 2 ∗ κ i → i + 1 ′ ′ ′ ∗ Δ s 2 = κ i ′ + 1 2 ∗ κ i ′ ′ ∗ Δ s + 1 2 ∗ κ i + 1 ′ ′ ∗ Δ s κ i + 1 = κ i + ∫ 0 Δ s κ ′ ( s ) d s = κ i + κ i ′ ∗ Δ s + 1 2 ∗ κ i ′ ′ ∗ Δ s 2 + 1 6 ∗ κ i → i + 1 ′ ′ ′ ∗ Δ s 3 = κ i + κ i ′ ∗ Δ s + 1 3 ∗ κ i ′ ′ ∗ Δ s 2 + 1 6 ∗ κ i + 1 ′ ′ ∗ Δ s 2 \begin{aligned} \kappa_{i+1}^{\prime\prime} &=\kappa_i''+\int_0^{\Delta s}\kappa_{i\to i+1}^{\prime\prime\prime}ds=\kappa_i''+\kappa_{i\to i+1}^{\prime\prime\prime}*\Delta s \\ \kappa_{i+1}^{\prime} &=\kappa_i^{\prime}+\int_0^{\Delta s}\boldsymbol{\kappa''}(s)ds=\kappa_i^{\prime}+\kappa_i^{\prime\prime}*\Delta s+\frac12*\kappa_{i\to i+1}^{\prime\prime\prime}*\Delta s^2 \\ &= \kappa_i^{\prime}+\frac12*\kappa_i^{\prime\prime}*\Delta s+\frac12*\kappa_{i+1}^{\prime\prime}*\Delta s\\ \kappa_{i+1} &=\kappa_i+\int_0^{\Delta s}\boldsymbol{\kappa'}(s)ds \\ &=\kappa_i+\kappa_i^{\prime}*\Delta s+\frac12*\kappa_i^{\prime\prime}*\Delta s^2+\frac16*\kappa_{i\to i+1}^{\prime\prime\prime}*\Delta s^3\\ &=\kappa_i+\kappa_i^{\prime}*\Delta s+\frac13*\kappa_i^{\prime\prime}*\Delta s^2+\frac16*\kappa_{i+1}^{\prime\prime}*\Delta s^2 \end{aligned} κi+1′′κi+1κi+1=κi′′+0Δsκii+1′′′ds=κi′′+κii+1′′′Δs=κi+0Δsκ′′(s)ds=κi+κi′′Δs+21κii+1′′′Δs2=κi+21κi′′Δs+21κi+1′′Δs=κi+0Δsκ(s)ds=κi+κiΔs+21κi′′Δs2+61κii+1′′′Δs3=κi+κiΔs+31κi′′Δs2+61κi+1′′Δs2
起点约束 κ 0 = κ i n i t \kappa_0=\kappa_{init} κ0=κinit, κ ˙ 0 = κ ˙ i n i t \dot \kappa_0=\dot \kappa_{init} κ˙0=κ˙init, κ ¨ 0 = κ ¨ i n i t \ddot \kappa_0=\ddot \kappa_{init} κ¨0=κ¨init满足的是起点的约束,即为实际车辆规划起点的状态。

采样间隔 Δ s = 0.5 \Delta s = 0.5 Δs=0.5

Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothPathCurvature(
    const PathData& path_data) {
  // using piecewise_jerk_path to fit a curve of path kappa profile
  // TODO(Jinyun): move smooth configs to gflags
  const auto& cartesian_path = path_data.discretized_path();
  const double delta_s = 0.5;
  std::vector<double> path_curvature;
  for (double path_s = cartesian_path.front().s();
       path_s < cartesian_path.back().s() + delta_s; path_s += delta_s) {
    const auto& path_point = cartesian_path.Evaluate(path_s);
    path_curvature.push_back(path_point.kappa());
  }
  const auto& path_init_point = cartesian_path.front();
  std::array<double, 3> init_state = {path_init_point.kappa(),
                                      path_init_point.dkappa(),
                                      path_init_point.ddkappa()};
  PiecewiseJerkPathProblem piecewise_jerk_problem(path_curvature.size(),
                                                  delta_s, init_state);
  piecewise_jerk_problem.set_x_bounds(-1.0, 1.0);
  piecewise_jerk_problem.set_dx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_ddx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_dddx_bound(-10.0, 10.0);

  piecewise_jerk_problem.set_weight_x(0.0);
  piecewise_jerk_problem.set_weight_dx(10.0);
  piecewise_jerk_problem.set_weight_ddx(10.0);
  piecewise_jerk_problem.set_weight_dddx(10.0);

  piecewise_jerk_problem.set_x_ref(10.0, std::move(path_curvature));

  if (!piecewise_jerk_problem.Optimize(1000)) {
    const std::string msg = "Smoothing path curvature failed";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> opt_x;
  std::vector<double> opt_dx;
  std::vector<double> opt_ddx;

  opt_x = piecewise_jerk_problem.opt_x();
  opt_dx = piecewise_jerk_problem.opt_dx();
  opt_ddx = piecewise_jerk_problem.opt_ddx();

  PiecewiseJerkTrajectory1d smoothed_path_curvature(
      opt_x.front(), opt_dx.front(), opt_ddx.front());

  for (size_t i = 1; i < opt_ddx.size(); ++i) {
    double j = (opt_ddx[i] - opt_ddx[i - 1]) / delta_s;
    smoothed_path_curvature.AppendSegment(j, delta_s);
  }

  smoothed_path_curvature_ = smoothed_path_curvature;

  return Status::OK();
}

SmoothSpeedLimit


限速的函数并非直接可以得到,接下来看看限速函数是怎么来的。也可参考这篇博文【Apollo学习笔记】——规划模块TASK之SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER
限速的来源如下图所示:在这里插入图片描述将所有的限速函数相加,得到下图的限速函数,很明显,该函数既不连续也不可导,所以需要对其进行平滑处理。在这里插入图片描述对于限速曲线的平滑,Apollo采样分段多项式进行平滑,之后采样二次规划的方式进行求解。限速曲线的目标函数如下:
m i n f = ∑ i = 0 n − 1 w v ( v i − v i − r e f ) 2 + ∑ i = 0 n − 1 w d d v v ¨ i 2 + ∑ i = 0 n − 2 w d d d v v ′ ′ ′ i → i + 1 2 \begin{aligned} minf&=\sum_{i=0}^{n-1}w_{v}(v_i-v_{i-ref})^2+\sum_{i=0}^{n-1}w_{ddv}\ddot v_{i}^2+\sum_{i=0}^{n-2}w_{dddv}{v^{'''}}_{i \to i + 1}^2 \end{aligned} minf=i=0n1wv(viviref)2+i=0n1wddvv¨i2+i=0n2wdddvv′′′ii+12

约束
v i ∈ ( v min ⁡ i , v max ⁡ i ) {v_i} \in (v_{\min }^i,v_{\max }^i) vi(vmini,vmaxi) v i ′ ∈ ( v m i n i ′ ( s ) , v m a x i ′ ( s ) ) , v i ′ ′ ∈ ( v m i n i ′ ′ ( s ) , v m a x i ′ ′ ( s ) ) , v i ′ ′ ′ ∈ ( v m i n i ′ ′ ′ ( s ) , v m a x i ′ ′ ′ ( s ) ) v_{i}^{\prime}\in\left(v_{min}^{i}{}^{\prime}(s),v_{max}^{i}{}^{\prime}(s)\right)\text{,}v_{i}^{\prime\prime}\in\left(v_{min}^{i}{}^{\prime\prime}(s),v_{max}^{i}{}^{\prime\prime}(s)\right)\text{,}v_{i}^{\prime\prime\prime}\in\left(v_{min}^{i}{}^{\prime\prime\prime}(s),v_{max}^{i}{}^{\prime\prime\prime}(s)\right) vi(vmini(s),vmaxi(s)),vi′′(vmini′′(s),vmaxi′′(s)),vi′′′(vmini′′′(s),vmaxi′′′(s))
连续性约束
v i + 1 ′ ′ = v i ′ ′ + ∫ 0 Δ s v i → i + 1 ′ ′ ′ d s = v i ′ ′ + v i → i + 1 ′ ′ ′ ∗ Δ s v i + 1 ′ = v i ′ + ∫ 0 Δ s v ′ ′ ( s ) d s = v i ′ + v i ′ ′ ∗ Δ s + 1 2 ∗ v i → i + 1 ′ ′ ′ ∗ Δ s 2 = v i ′ + 1 2 ∗ v i ′ ′ ∗ Δ s + 1 2 ∗ v i + 1 ′ ′ ∗ Δ s v i + 1 = v i + ∫ 0 Δ s v ′ ( s ) d s = v i + v i ′ ∗ Δ s + 1 2 ∗ v i ′ ′ ∗ Δ s 2 + 1 6 ∗ v i → i + 1 ′ ′ ′ ∗ Δ s 3 = v i + v i ′ ∗ Δ s + 1 3 ∗ v i ′ ′ ∗ Δ s 2 + 1 6 ∗ v i + 1 ′ ′ ∗ Δ s 2 \begin{aligned} v_{i+1}^{\prime\prime} &=v_i''+\int_0^{\Delta s}v_{i\to i+1}^{\prime\prime\prime}ds=v_i''+v_{i\to i+1}^{\prime\prime\prime}*\Delta s \\ v_{i+1}^{\prime} &=v_i^{\prime}+\int_0^{\Delta s}\boldsymbol{v''}(s)ds=v_i^{\prime}+v_i^{\prime\prime}*\Delta s+\frac12*v_{i\to i+1}^{\prime\prime\prime}*\Delta s^2 \\ &= v_i^{\prime}+\frac12*v_i^{\prime\prime}*\Delta s+\frac12*v_{i+1}^{\prime\prime}*\Delta s\\ v_{i+1} &=v_i+\int_0^{\Delta s}\boldsymbol{v'}(s)ds \\ &=v_i+v_i^{\prime}*\Delta s+\frac12*v_i^{\prime\prime}*\Delta s^2+\frac16*v_{i\to i+1}^{\prime\prime\prime}*\Delta s^3\\ &=v_i+v_i^{\prime}*\Delta s+\frac13*v_i^{\prime\prime}*\Delta s^2+\frac16*v_{i+1}^{\prime\prime}*\Delta s^2 \end{aligned} vi+1′′vi+1vi+1=vi′′+0Δsvii+1′′′ds=vi′′+vii+1′′′Δs=vi+0Δsv′′(s)ds=vi+vi′′Δs+21vii+1′′′Δs2=vi+21vi′′Δs+21vi+1′′Δs=vi+0Δsv(s)ds=vi+viΔs+21vi′′Δs2+61vii+1′′′Δs3=vi+viΔs+31vi′′Δs2+61vi+1′′Δs2

起点约束 v 0 = v i n i t v_0=v_{init} v0=vinit, v ˙ 0 = v ˙ i n i t = 0 \dot v_0=\dot v_{init}=0 v˙0=v˙init=0, v ¨ 0 = v ¨ i n i t = 0 \ddot v_0=\ddot v_{init}=0 v¨0=v¨init=0满足的是起点的约束,即为实际车辆规划起点的状态。

Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothSpeedLimit() {
  // using piecewise_jerk_path to fit a curve of speed_ref
  // TODO(Hongyi): move smooth configs to gflags
  double delta_s = 2.0;
  std::vector<double> speed_ref;
  // 获取速度限制
  for (int i = 0; i < 100; ++i) {
    double path_s = i * delta_s;
    double limit = speed_limit_.GetSpeedLimitByS(path_s);
    speed_ref.emplace_back(limit);
  }
  std::array<double, 3> init_state = {speed_ref[0], 0.0, 0.0};
  PiecewiseJerkPathProblem piecewise_jerk_problem(speed_ref.size(), delta_s,
                                                  init_state);
  piecewise_jerk_problem.set_x_bounds(0.0, 50.0);
  piecewise_jerk_problem.set_dx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_ddx_bounds(-10.0, 10.0);
  piecewise_jerk_problem.set_dddx_bound(-10.0, 10.0);

  piecewise_jerk_problem.set_weight_x(0.0);
  piecewise_jerk_problem.set_weight_dx(10.0);
  piecewise_jerk_problem.set_weight_ddx(10.0);
  piecewise_jerk_problem.set_weight_dddx(10.0);

  piecewise_jerk_problem.set_x_ref(10.0, std::move(speed_ref));

  if (!piecewise_jerk_problem.Optimize(4000)) {
    const std::string msg = "Smoothing speed limit failed";
    AERROR << msg;
    return Status(ErrorCode::PLANNING_ERROR, msg);
  }

  std::vector<double> opt_x;
  std::vector<double> opt_dx;
  std::vector<double> opt_ddx;

  opt_x = piecewise_jerk_problem.opt_x();
  opt_dx = piecewise_jerk_problem.opt_dx();
  opt_ddx = piecewise_jerk_problem.opt_ddx();
  PiecewiseJerkTrajectory1d smoothed_speed_limit(opt_x.front(), opt_dx.front(),
                                                 opt_ddx.front());

  for (size_t i = 1; i < opt_ddx.size(); ++i) {
    double j = (opt_ddx[i] - opt_ddx[i - 1]) / delta_s;
    smoothed_speed_limit.AppendSegment(j, delta_s);
  }

  smoothed_speed_limit_ = smoothed_speed_limit;

  return Status::OK();
}

OptimizeByNLP

由于字数限制,剩余部分将会放在另一篇文章中。

参考

[1] Planning Piecewise Jerk Nonlinear Speed Optimizer Introduction
[2] Planning 基于非线性规划的速度规划
[3] Apollo星火计划学习笔记——Apollo速度规划算法原理与实践
[4] Apollo规划控制学习笔记

你可能感兴趣的:(Apollo,apollo,决策规划,自动驾驶,算法,人工智能)