文章目录
TASK系列解析文章
1. [Apollo study notes] - LANE_CHANGE_DECIDER of planning module TASK
2. [Apollo study notes] - PATH_REUSE_DECIDER of planning module TASK
3. [Apollo study notes] - PATH_BORROW_DECIDER of planning module TASK
4. [Apollo study notes] - — PATH_BOUNDS_DECIDER of planning module TASK
5. [Apollo study notes] — PIECEWISE_JERK_PATH_OPTIMIZER of planning module TASK
6. [Apollo study notes] — PATH_ASSESSMENT_DECIDER of planning module TASK
7. [Apollo study notes] — PATH_DECIDER of planning module TASK
8. [Apollo study notes]——RULE_BASED_STOP_DECIDER of planning module TASK
9. [Apollo study notes]——SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER of planning module TASK
10. [Apollo study notes]——SPEED_HEURISTIC_OPTIMIZER of planning module
TASK 11. [Apollo study notes]——Planning Module 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=0∑n−1ws−ref(si−si−ref)2+wds−ref(s˙i−s˙ref)2+pis˙i2+wddss¨i2+i=0∑n−2wdddss′′′i→i+12+wend−s(sn−1−send)2+wend−ds(sn−1˙−send˙)2+wend−dds(sn−1¨−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} x∈Rnmins.t.f(x)gL≤g(x)≤gUxL≤x≤xU,x∈Rn
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,…,sn−1,s˙0,s˙1,…,s˙n−1,s¨0,s¨1,…,s¨n−1,s_slack_upper0,s_slack_lower1,…,s_slack_lowern−1,s_slack_upper0,s_slack_upper1,…,s_slack_uppern−1
采样方式：等间隔的时间采样。除此之外非线性规划中如果打开了软约束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 _ s l a c k _ u pp er to prevent solution failure.
Define objective function
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} my f=i=0∑n−1ws−ref(si−s−refi)2+wv−ref(s˙i−v−ref)2+was¨i2+i=0∑n−2wj(Δts¨i+1−s¨i)2+i=0∑n−1wl a t _ a ccl a t _ a c ci2+wsofts_slack_loweri+wsofts_slack_upperi+wtarget−s(s−starget)2+wtarget−v(v−vtarget)2+wtarget−a(a−atarget)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}} 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)。这部分在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} si−s_soft_loweri+s_slack_loweri≥0si−s_soft_upperi−s_slack_upperi≤0
At the same time, it also needs to meet the basic physical principles, that is, continuity. Compared with quadratic programming, there is less acceleration?:
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+1′si+1=si′+∫0Δts′′(t)dt=si′+si′′∗Δt+21∗si→i+1′′′∗Δt2=si′+21∗si′′∗Δt+21∗si+1′′∗Δt=si+∫0Δts′(t)dt=si+si′∗Δt+21∗si′′∗Δt2+61∗si→i+1′′′∗Δt3=si+si′∗Δt+31∗si′′∗Δt2+61∗si+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=0∑n−1ws(si−si−ref)2+i=0∑n−1wddss¨i2+i=0∑n−2wdddss′′′i→i+12
约束：
• 主车必须在道路边界内，同时不能和障碍物有碰撞 s i ∈ ( s min i , s max i ) {s_i} \in (s_{\min }^i,s_{\max }^i) si∈(smini,smaxi) •According to the current state, the lateral speed/acceleration/jerk of the host vehicle has specific kinematic constraints:
si ′ ∈ ( smini ′ ( t ) , smaxi ′ ( t ) ) , si ′ ′ ∈ ( smini ′ ′ ( t ) , smaxi ′ ′ ( t ) ) , si ′ ′ ′ ∈ ( smini ′ ′ ′ ( t ) , smaxi ′ ′ ′ ( 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))
•Continuity constraints:
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+1′si+1=si′′+∫0Δtsi→i+1′′′dt=si′′+si→i+1′′′∗Δt=si′+∫0Δts′′(t)dt=si′+si′′∗Δt+21∗si→i+1′′′∗Δt2=si′+21∗si′′∗Δt+21∗si+1′′∗Δt=si+∫0Δts′(t)dt=si+si′∗Δt+21∗si′′∗Δt2+61∗si→i+1′′′∗Δt3=si+si′∗Δt+31∗si′′∗Δt2+61∗si+1′′∗Δt2
• Start point constraint : s 0 = sinit 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 ¨ init \ddot s_0=\ddot s_{init}s¨0=s¨initWhat is satisfied is the starting point constraint, which is the state of the actual vehicle planning starting point.
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
Check whether the speedlimit is feasible, and if it is not feasible, output the result of QP; if it is feasible, continue with nonlinear programming. The code only compares the speed limit of the starting point and the initial speed of the starting point.
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
Curvature is about ssThe relational expression of s , so to perform smooth fitting, for the nonlinear programming solver, whether it is the objective function or the constraint function, it needs to satisfy the second-order differentiability: κ ′ = f ′ ′ ( s ) \kappa ' = f ''(s)K′=f′′ (s)
ps:l → κ l \rightarrow \kappal→The smoothing of κ
curvature also uses the quadratic programming method, using the first, second and third derivatives of curvature as the loss function.
Definitions :
minf = ∑ i = 0 n − 1 w κ ( κ i − κ i − ref ) 2 + ∑ i = 0 n − 1 wdd κ κ ̈ i 2 + ∑ i = 0 n − 2 wddd κ κ ′ ′ ′ 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}my f=i=0∑n−1wMr( Mri−Ki−ref)2+i=0∑n−1wdd kK¨i2+i=0∑n−2wddd kK′′′i→i+12
Commitment :
κ i ∈ ( κ min i , κ max i ) {\kappa_i} \in (\kappa_{\min }^i,\kappa_{\max }^i)Ki∈( Mrmini,Kmaxi) κ 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) Ki′∈( Mrmini′(s),Kmaxi′(s)), Mri′′∈( Mrmini′′(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} Ki+1′′Ki+1′Ki+1=Ki′′+∫0Δs _Ki→i+1′′′ds=Ki′′+Ki→i+1′′′∗Δs _=Ki′+∫0Δs _K′′(s)ds=Ki′+Ki′′∗Δs _+21∗Ki→i+1′′′∗Δs _2=Ki′+21∗Ki′′∗Δs _+21∗Ki+1′′∗Δs _=Ki+∫0Δs _K′(s)ds=Ki+Ki′∗Δs _+21∗Ki′′∗Δs _2+61∗Ki→i+1′′′∗Δs _3=Ki+Ki′∗Δs _+31∗Ki′′∗Δs _2+61∗Ki+1′′∗Δs _2
• Start point constraint : κ 0 = κ init \kappa_0=\kappa_{init}K0=Kinit, κ ˙ 0 = κ ˙ init \dot \kappa_0=\dot \kappa_{init}K˙0=K˙init, κ ¨ 0 = κ ¨ init \ddot \kappa_0=\ddot \kappa_{init}K¨0=K¨initWhat is satisfied is the starting point constraint, which is the state of the actual vehicle planning starting point.
Sampling interval : Δ 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
The speed limit function is not directly available. Let’s take a look at how the speed limit function comes from. You can also refer to this blog post [Apollo Study Notes] - SPEED_BOUNDS_PRIORI_DECIDER&&SPEED_BOUNDS_FINAL_DECIDER of the planning module TASK.
The source of the speed limit is shown in the figure below: 
Add all the speed limit functions to get the speed limit function in the figure below. Obviously, this function is both It is discontinuous and not differentiable, so it needs to be smoothed. 
For the smoothing of the speed limit curve, Apollo sampling piecewise polynomial is used for smoothing, and then the sampling quadratic programming method is used to solve it. The objective function of the speed limit curve is as follows:
minf = ∑ i = 0 n − 1 wv ( vi − vi − ref ) 2 + ∑ i = 0 n − 1 wddvv ¨ i 2 + ∑ i = 0 n − 2 wdddvv ′ ′ ′ 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}my f=i=0∑n−1wv(vi−vi−ref)2+i=0∑n−1wdd vv¨i2+i=0∑n−2wfdd vv′′′i→i+12
Promise :
vi ∈ ( 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))
•Continuity constraints:
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+1′vi+1=vi′′+∫0Δs _vi→i+1′′′ds=vi′′+vi→i+1′′′∗Δs _=vi′+∫0Δs _v′′(s)ds=vi′+vi′′∗Δs _+21∗vi→i+1′′′∗Δs _2=vi′+21∗vi′′∗Δs _+21∗vi+1′′∗Δs _=vi+∫0Δs _v′(s)ds=vi+vi′∗Δs _+21∗vi′′∗Δs _2+61∗vi→i+1′′′∗Δs _3=vi+vi′∗Δs _+31∗vi′′∗Δs _2+61∗vi+1′′∗Δs _2
• Start point constraint : v 0 = vinit 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 ¨ init = 0 \ddot v_0=\ddot v_{init}=0v¨0=v¨init=0 satisfies the constraints of the starting point, which is the state of the actual vehicle planning starting point.
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
Due to word limit, the remainder will be placed in another article.
reference
[1] Planning Piecewise Jerk Nonlinear Speed Optimizer Introduction
[2] Planning Speed planning based on nonlinear planning
[3] Apollo Spark Plan study notes - Apollo speed planning algorithm principle and practice
[4] Apollo planning control study notes