【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=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_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=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−1wlat_acclat_acci2+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
同时还需满足基本的物理学原理，即连续性，和二次规划相比，少了加速度?：
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)• 根据当前状态，主车的横向速度/加速度/加加速度有特定运动学限制：
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+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
• 起点约束： 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=0∑n−1wκ(κi−κi−ref)2+i=0∑n−1wddκκ¨i2+i=0∑n−2wdddκκ′′′i→i+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κi→i+1′′′ds=κi′′+κi→i+1′′′∗Δs=κi′+∫0Δsκ′′(s)ds=κi′+κi′′∗Δs+21∗κi→i+1′′′∗Δs2=κi′+21∗κi′′∗Δs+21∗κi+1′′∗Δs=κi+∫0Δsκ′(s)ds=κi+κi′∗Δs+21∗κi′′∗Δs2+61∗κi→i+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=0∑n−1wv(vi−vi−ref)2+i=0∑n−1wddvv¨i2+i=0∑n−2wdddvv′′′i→i+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+1′vi+1=vi′′+∫0Δsvi→i+1′′′ds=vi′′+vi→i+1′′′∗Δs=vi′+∫0Δsv′′(s)ds=vi′+vi′′∗Δs+21∗vi→i+1′′′∗Δs2=vi′+21∗vi′′∗Δs+21∗vi+1′′∗Δs=vi+∫0Δsv′(s)ds=vi+vi′∗Δs+21∗vi′′∗Δs2+61∗vi→i+1′′′∗Δs3=vi+vi′∗Δs+31∗vi′′∗Δs2+61∗vi+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规划控制学习笔记