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

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_STAGEenabled: truetask_type: LANE_CHANGE_DECIDERtask_type: PATH_REUSE_DECIDERtask_type: PATH_LANE_BORROW_DECIDERtask_type: PATH_BOUNDS_DECIDERtask_type: PIECEWISE_JERK_PATH_OPTIMIZERtask_type: PATH_ASSESSMENT_DECIDERtask_type: PATH_DECIDERtask_type: RULE_BASED_STOP_DECIDERtask_type: SPEED_BOUNDS_PRIORI_DECIDERtask_type: SPEED_HEURISTIC_OPTIMIZERtask_type: SPEED_DECIDERtask_type: SPEED_BOUNDS_FINAL_DECIDERtask_type: PIECEWISE_JERK_SPEED_OPTIMIZER# task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZERtask_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_OPTIMIZERpiecewise_jerk_nonlinear_speed_optimizer_config {acc_weight: 2.0jerk_weight: 3.0lat_acc_weight: 1000.0s_potential_weight: 0.05ref_v_weight: 5.0ref_s_weight: 100.0soft_s_bound_weight: 1e6use_warm_start: true}
}

PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER流程

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

首先，来看看基于二次规划的速度规划方法存在的问题。
$$\begin{array}{rl}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 _{i=0}^{n-2}{w}_{ddds}{s^{{}^{\prime \prime \prime }}}_{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{array}$$

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

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

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

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

$$\begin{array}{rlr}\underset{x\in {\mathbb{R}}^n}{\min }& & f(x)\\ \text{s.t.}& & g^L\le g(x)\le g^U\\ & & x^L\le x\le x^U,\\ & & x\in {\mathbb{R}}^n\end{array}$$

$g^L$和$g^U$是约束函数的上界和下界，$x^L$和$x^U$是优化变量的上界和下界。

---

确定优化变量

基于非线性规划的速度规划步骤与之前规划步骤基本一致。
$$\begin{array}{r}x=\left(\begin{array}{c}{s}_0,s_1,\dots ,s_{n-1},\\ {\dot{s}}_0,{\dot{s}}_1,\dots ,{\dot{s}}_{n-1},\\ {\ddot{s}}_0,{\ddot{s}}_1,\dots ,{\ddot{s}}_{n-1},\\ s\_slack\_uppe{r}_0,s\_slack\_lowe{r}_1,\dots ,s\_slack\_lowe{r}_{n-1},\\ s\_slack\_uppe{r}_0,s\_slack\_uppe{r}_1,\dots ,s\_slack\_uppe{r}_{n-1}\end{array}\right)\end{array}$$

采样方式：等间隔的时间采样。除此之外非线性规划中如果打开了软约束FLAGS_use_soft_bound_in_nonlinear_speed_opt，还会有松弛变量$s\_slack\_lower$与$s\_slack\_upper$，防止求解失败。

定义目标函数

$$\begin{array}{rl}minf=& \sum _{i=0}^{n-1}{w}_{s-ref}(s_i-s_{-}re{f}_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\_ac{c}_i^2+w_{soft}s\_slack\_lowe{r}_i+w_{soft}s\_slack\_uppe{r}_i\\ & +w_{target-s}(s-s_{target}{)}^2+w_{target-v}(v-v_{target}{)}^2+w_{target-a}(a-a_{target}{)}^2\end{array}$$
目标函数与二次规划的目标函数差不多，增加了横向加速度的代价值以及松弛变量$w_{soft}s\_slack\_lower$与$w_{soft}s\_slack\_upper$。

横向加速度的计算方式：
$$lat\_ac{c}_i=s_i^2\cdot \kappa (s_i)$$

定义约束

接下来是约束条件：
对于变量$x$的边界约束，需满足：

• $s_i\in (s_{\min }^i,s_{\max }^i)$
• $s_i^{\prime }\in \left(s_{min}^i{}^{\prime }(t),s_{max}^i{}^{\prime }(t)\right)$
• $s_i^{\prime \prime }\in \left(s_{min}^i{}^{\prime \prime }(t),s_{max}^i{}^{\prime \prime }(t)\right)$
• $s\_slack\_lowe{r}_i\in ({s\_slack\_lower}_{\min }^i,{s\_slack\_lower}_{\max }^i)$
• $s\_slack\_uppe{r}_i\in ({s\_slack\_upper}_{\min }^i,{s\_slack\_upper}_{\max }^i)$

对于$g(x)$的约束，需满足：

• 规划的速度要一直往前走：$s_i\le s_{i+1}$
• 加加速度不能超过定义的极限值：$jer{k}_{\min }\le \frac{{\ddot{s}}_{i+1}-{\ddot{s}}_i}{\Delta t}\le jer{k}_{\max }$
• 速度满足路径上的限速：${\dot{s}}_i\le speed\_limit(s_i)$。这部分在SmoothSpeedLimit有具体介绍。

为了避免求解的失败，二次规划中对位置的硬约束，在非线性规划中转为了对位置的软约束。提升求解的精度。
$$\begin{array}{r}{s}_i-s\_soft\_lowe{r}_i+s\_slack\_lowe{r}_i\ge 0\\ s_i-s\_soft\_uppe{r}_i-s\_slack\_uppe{r}_i\le 0\end{array}$$

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

$$\begin{array}{rl}{s}_{i+1}^{\prime }& =s_i^{\prime }+{\int }_0^{\Delta t}{\mathbf{s}}^{\prime \prime }(t)dt=s_i^{\prime }+s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t^2\\ & =s_i^{\prime }+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i+1}^{\prime \prime }\ast \Delta t\\ s_{i+1}& =s_i+{\int }_0^{\Delta t}{\mathbf{s}}^{\prime }(t)dt\\ & =s_i+s_i^{\prime }\ast \Delta t+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t^3\\ & =s_i+s_i^{\prime }\ast \Delta t+\frac{1}{3}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i+1}^{\prime \prime }\ast \Delta t^2\end{array}$$

---

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_{init},{\dot{s}}_{init},{\ddot{s}}_{init}$的初始化设置；初始化设置$\dot{s},\ddot{s},s^{{}^{\prime \prime \prime }}$的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 dimensionsconst StGraphData& st_graph_data =*reference_line_info_->mutable_st_graph_data();// TODO(Jinyun): move to confsdelta_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 valuess_init_ = 0.0;s_dot_init_ = st_graph_data.init_point().v();s_ddot_init_ = st_graph_data.init_point().a();// Set s_dot bounarys_dot_max_ = std::fmax(FLAGS_planning_upper_speed_limit,st_graph_data.init_point().v());// Set s_ddot boundaryconst 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 places_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// 遍历每一段segmentfor (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_;// 遍历每一个STBoundaryfor (const STBoundary* boundary : st_graph_data.st_boundaries()) {double s_lower = 0.0;double s_upper = 0.0;// 获取未被阻塞的s的范围，即s_lower和s_upperif (!boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {continue;}SpeedPoint sp;// 根据不同的类型，更新boundswitch (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.0std::fmin(s_upper_bound, s_upper - FLAGS_follow_min_distance);// 依据时间估计出SpeedPointif (!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.5std::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 confsfor (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同样用分段多项式二次规划的求解方式，得到符合约束的速度平滑曲线，作为非线性规划的初值。

目标函数：
$$\begin{array}{rl}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^{{}^{\prime \prime \prime }}}_{i\to i+1}^2\end{array}$$

约束：
• 主车必须在道路边界内，同时不能和障碍物有碰撞$$s_i\in (s_{\min }^i,s_{\max }^i)$$• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：
$$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)$$
• 连续性约束：
$$\begin{array}{rl}{s}_{i+1}^{\prime \prime }& =s_i^{\prime \prime }+{\int }_0^{\Delta t}{s}_{i\to i+1}^{\prime \prime \prime }dt=s_i^{\prime \prime }+s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t\\ s_{i+1}^{\prime }& =s_i^{\prime }+{\int }_0^{\Delta t}{\mathbf{s}}^{\prime \prime }(t)dt=s_i^{\prime }+s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t^2\\ & =s_i^{\prime }+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t+\frac{1}{2}\ast s_{i+1}^{\prime \prime }\ast \Delta t\\ s_{i+1}& =s_i+{\int }_0^{\Delta t}{\mathbf{s}}^{\prime }(t)dt\\ & =s_i+s_i^{\prime }\ast \Delta t+\frac{1}{2}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i\to i+1}^{\prime \prime \prime }\ast \Delta t^3\\ & =s_i+s_i^{\prime }\ast \Delta t+\frac{1}{3}\ast s_i^{\prime \prime }\ast \Delta t^2+\frac{1}{6}\ast s_{i+1}^{\prime \prime }\ast \Delta t^2\end{array}$$

• 起点约束：$s_0=s_{init}$,${\dot{s}}_0={\dot{s}}_{init}$,${\ddot{s}}_0={\ddot{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 tunningsconst 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_sSpeedPoint 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 problemif (!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 limitstatic 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$的关系式,所以要进行平滑拟合，对于非线性规划的求解器，无论是目标函数还是约束函数，都需要满足二阶可导：$${\kappa }^{\prime }=f^{\prime \prime }(s)$$
ps: $l\to \kappa$
曲率的平滑也是用到了二次规划的方法，用曲率的一阶导、二阶导、三阶导作为损失函数.

目标函数：
$$\begin{array}{rl}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 }^{{}^{\prime \prime \prime }}}_{i\to i+1}^2\end{array}$$

约束：
$${\kappa }_i\in ({\kappa }_{\min }^i,{\kappa }_{\max }^i)$$$${\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)$$
• 连续性约束：
$$\begin{array}{rl}{\kappa }_{i+1}^{\prime \prime }& ={\kappa }_i^{\prime \prime }+{\int }_0^{\Delta s}{\kappa }_{i\to i+1}^{\prime \prime \prime }ds={\kappa }_i^{\prime \prime }+{\kappa }_{i\to i+1}^{\prime \prime \prime }\ast \Delta s\\ {\kappa }_{i+1}^{\prime }& ={\kappa }_i^{\prime }+{\int }_0^{\Delta s}{\mathbf{\kappa }}^{\prime \prime }(s)ds={\kappa }_i^{\prime }+{\kappa }_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast {\kappa }_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^2\\ & ={\kappa }_i^{\prime }+\frac{1}{2}\ast {\kappa }_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast {\kappa }_{i+1}^{\prime \prime }\ast \Delta s\\ {\kappa }_{i+1}& ={\kappa }_i+{\int }_0^{\Delta s}{\mathbf{\kappa }}^{\prime }(s)ds\\ & ={\kappa }_i+{\kappa }_i^{\prime }\ast \Delta s+\frac{1}{2}\ast {\kappa }_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast {\kappa }_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^3\\ & ={\kappa }_i+{\kappa }_i^{\prime }\ast \Delta s+\frac{1}{3}\ast {\kappa }_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast {\kappa }_{i+1}^{\prime \prime }\ast \Delta s^2\end{array}$$
• 起点约束：${\kappa }_0={\kappa }_{init}$,${\dot{\kappa }}_0={\dot{\kappa }}_{init}$,${\ddot{\kappa }}_0={\ddot{\kappa }}_{init}$满足的是起点的约束，即为实际车辆规划起点的状态。

采样间隔：$\Delta 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 gflagsconst 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采样分段多项式进行平滑，之后采样二次规划的方式进行求解。限速曲线的目标函数如下：
$$\begin{array}{rl}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^{{}^{\prime \prime \prime }}}_{i\to i+1}^2\end{array}$$

约束：
$$v_i\in (v_{\min }^i,v_{\max }^i)$$$$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)$$
• 连续性约束：
$$\begin{array}{rl}{v}_{i+1}^{\prime \prime }& =v_i^{\prime \prime }+{\int }_0^{\Delta s}{v}_{i\to i+1}^{\prime \prime \prime }ds=v_i^{\prime \prime }+v_{i\to i+1}^{\prime \prime \prime }\ast \Delta s\\ v_{i+1}^{\prime }& =v_i^{\prime }+{\int }_0^{\Delta s}{\mathbf{v}}^{\prime \prime }(s)ds=v_i^{\prime }+v_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast v_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^2\\ & =v_i^{\prime }+\frac{1}{2}\ast v_i^{\prime \prime }\ast \Delta s+\frac{1}{2}\ast v_{i+1}^{\prime \prime }\ast \Delta s\\ v_{i+1}& =v_i+{\int }_0^{\Delta s}{\mathbf{v}}^{\prime }(s)ds\\ & =v_i+v_i^{\prime }\ast \Delta s+\frac{1}{2}\ast v_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast v_{i\to i+1}^{\prime \prime \prime }\ast \Delta s^3\\ & =v_i+v_i^{\prime }\ast \Delta s+\frac{1}{3}\ast v_i^{\prime \prime }\ast \Delta s^2+\frac{1}{6}\ast v_{i+1}^{\prime \prime }\ast \Delta s^2\end{array}$$

• 起点约束：$v_0=v_{init}$,${\dot{v}}_0={\dot{v}}_{init}=0$,${\ddot{v}}_0={\ddot{v}}_{init}=0$满足的是起点的约束，即为实际车辆规划起点的状态。

Status PiecewiseJerkSpeedNonlinearOptimizer::SmoothSpeedLimit() {// using piecewise_jerk_path to fit a curve of speed_ref// TODO(Hongyi): move smooth configs to gflagsdouble 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规划控制学习笔记