本次博文主要介绍apollo 5.0版本内使用的轨迹规划算法—-public road，该算法的核心思想是PV解耦，即Path-Velocity的解耦，其主要包含两个过程：1.路径规划，2.速度规划。
路径规划其实已经发展很多年，从早期的机器人到现在的无人驾驶，主要的方法包括 采样法，图搜索法，数值优化法等，具体可以查阅相关文献阅读。本篇文章主要讲述apollo轨迹规划模块里面的路径规划，有时间再更新之后的速度规划。
与之前EM规划和Lattice规划不同，当前5.0版本使用的路径规划，更加的灵活方便，原因主要是采用了数值优化的思想，通过边界约束等，保证了密集障碍物场景的灵活性。 而之前的lattice等算法由于采样的局限，导致在复杂环境下可能存在无解的情况。言归正传，apollo在内部的路径规划里主要包括以下几个步骤。 （由于场景的差异性，task与stage也有所不同，因此本文只讲述lane follow scenario）.
Status LaneFollowStage::PlanOnReferenceLine(
const TrajectoryPoint& planning_start_point, Frame* frame,
ReferenceLineInfo* reference_line_info) {
if (!reference_line_info->IsChangeLanePath()) {
reference_line_info->AddCost(kStraightForwardLineCost);
}我们通过 lane_follow_stage.cc文件中 Status LaneFollowStage::PlanOnReferenceLine 函数可以看到具体工作细节
首先，会根据
for (auto* optimizer : task_list_) {
const double start_timestamp = Clock::NowInSeconds();
ret = optimizer->Execute(frame, reference_line_info);
if (!ret.ok()) {
AERROR << "Failed to run tasks[" << optimizer->Name()
<< "], Error message: " << ret.error_message();
break;
}
}当前先验信息判断是否当前参考线是可换道的车道，如果不是那么增加cost。 随后，开始了task的process过程，不同的stage有不同的task，具体可通过 conf/scenario文件夹下的pb.txt，例如：
scenario_type: LANE_FOLLOW
stage_type: LANE_FOLLOW_DEFAULT_STAGE
stage_config: {
stage_type: LANE_FOLLOW_DEFAULT_STAGE
enabled: true
task_type: LANE_CHANGE_DECIDER
task_type: PATH_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: DP_ST_SPEED_OPTIMIZER
task_type: SPEED_DECIDER
task_type: SPEED_BOUNDS_FINAL_DECIDER
task_type: PIECEWISE_JERK_SPEED_OPTIMIZER
task_type: DECIDER_RSS上述task中，根据名称可以看出，path都是与路径相关，从rule_based之后则是与速度规划相关。
按照任务顺序：
1.lane change decider： 该任务主要是用来处理refer_line_info，内部有个状态机，根据换道成功时间与换道失败时间以及当前位置与目标位置来切换状态，以此来处理refer_line_info的changelane信息。
2.lane borrow：该任务相对复杂，主要涉及了一些rules，判断是否可以进行借道超车，rules包括：距离信号交叉口的距离，与静态障碍物的距离，是否是单行道，是否所在车道左右车道线是虚线等规则，以此来判断是否可以借道超车；
3.path bound decider：根据上一任务的结果，来生成相应的道路边界信息，包括三部分:原车道（左右横向位移0.5m），左超车道，右超车道（具体横向位移根据HDMap及周围障碍物在sl坐标系下的位置所决定）；path bound的结果将会作为下一步结果的边界约束，具体原理可查看apollo公开课。
根据上述边界可以构建optimization method。
由此开始了第四个任务：piecewise jerk path optimazation。原理可请查看 该篇博文
公开课路径规划原理
本篇文章主要讲述内部的代码推理，主要涉及osqp优化求解问题。
bool res_opt =
OptimizePath(init_frenet_state.second, end_state,
path_boundary.delta_s(), path_boundary.boundary(),
ddl_bounds, w, &opt_l, &opt_dl, &opt_ddl, max_iter);上述是优化问题的函数接口，
bool PiecewiseJerkPathOptimizer::OptimizePath(
std::vector<double>* dx, std::vector<double>* ddx, const int max_iter) {
//初始化状态，将问题转化为 qp问题
PiecewiseJerkPathProblem piecewise_jerk_problem(lat_boundaries.size(),
delta_s, init_state);
piecewise_jerk_problem.set_end_state_ref({1000.0, 0.0, 0.0}, end_state);
if (end_state[0] != 0) {
std::vector<double> x_ref(lat_boundaries.size(), end_state[0]);
piecewise_jerk_problem.set_x_ref(10.0, x_ref);
}
// 初始化设置各项权重
piecewise_jerk_problem.set_weight_x(w[0]);
piecewise_jerk_problem.set_weight_dx(w[1]);
piecewise_jerk_problem.set_weight_ddx(w[2]);
piecewise_jerk_problem.set_weight_dddx(w[3]);
piecewise_jerk_problem.set_scale_factor({1.0, 10.0, 100.0});
auto start_time = std::chrono::system_clock::now();
//初始化各项变量的边界
piecewise_jerk_problem.set_x_bounds(lat_boundaries);
piecewise_jerk_problem.set_dx_bounds(-FLAGS_lateral_derivative_bound_default,
FLAGS_lateral_derivative_bound_default);
piecewise_jerk_problem.set_ddx_bounds(ddl_bounds);
piecewise_jerk_problem.set_dddx_bound(FLAGS_lateral_jerk_bound); bool success = piecewise_jerk_problem.Optimize(max_iter);
上面是qp问题求解的入口，调用osqp标准求解库，
//首先进行osqp配置
OSQPSettings* settings = SolverDefaultSettings();
settings->max_iter = max_iter;
OSQPWorkspace* osqp_work = osqp_setup(data, settings);
osqp_solve(osqp_work);
PiecewiseJerkProblem::FormulateProblem()//该函数是重点，主要包含了目标函数与响应约束的建立，以及目标函数的offset补偿。
// x(i)^2 * (w_x + w_x_ref)
for (int i = 0; i < n - 1; ++i) {
columns[i].emplace_back(
i, (weight_x_ + weight_x_ref_) / (scale_factor_[0] * scale_factor_[0]));
++value_index;
}
// x(n-1)^2 * (w_x + w_x_ref + w_end_x)
columns[n - 1].emplace_back(
n - 1, (weight_x_ + weight_x_ref_ + weight_end_state_[0]) /
(scale_factor_[0] * scale_factor_[0]));
++value_index;
// x(i)'^2 * w_dx
for (int i = 0; i < n - 1; ++i) {
columns[n + i].emplace_back(
n + i, weight_dx_ / (scale_factor_[1] * scale_factor_[1]));
++value_index;
}
// x(n-1)'^2 * (w_dx + w_end_dx)
columns[2 * n - 1].emplace_back(2 * n - 1,
(weight_dx_ + weight_end_state_[1]) /
(scale_factor_[1] * scale_factor_[1]));
++value_index;
auto delta_s_square = delta_s_ * delta_s_;
// x(i)''^2 * (w_ddx + 2 * w_dddx / delta_s^2)
columns[2 * n].emplace_back(2 * n,
(weight_ddx_ + weight_dddx_ / delta_s_square) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
for (int i = 1; i < n - 1; ++i) {
columns[2 * n + i].emplace_back(
2 * n + i, (weight_ddx_ + 2.0 * weight_dddx_ / delta_s_square) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
}
columns[3 * n - 1].emplace_back(
3 * n - 1,
(weight_ddx_ + weight_dddx_ / delta_s_square + weight_end_state_[2]) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
// -2 * w_dddx / delta_s^2 * x(i)'' * x(i + 1)''
for (int i = 0; i < n - 1; ++i) {
columns[2 * n + i].emplace_back(2 * n + i + 1,
(-2.0 * weight_dddx_ / delta_s_square) /
(scale_factor_[2] * scale_factor_[2]));
++value_index;
}
上述设置了目标函数，主要包括：
l^2+l'^2+l''^2，以及l'''^2，其中l'''通过l''前后两帧之差与delta_s之比替代。osqp内部的核函数矩阵形式是csc格式，需要具体了解该形式的同学，可以百度搜索csc矩阵或通过osqp官方demo学习。
当设计完目标函数矩阵后，开始设置相应的约束,
void PiecewiseJerkProblem::CalculateAffineConstraint(
std::vector<c_float>* A_data, std::vector<c_int>* A_indices,
std::vector<c_int>* A_indptr, std::vector<c_float>* lower_bounds,
std::vector<c_float>* upper_bounds) {
// 3N params bounds on x, x', x''
// 3(N-1) constraints on x, x', x''
// 3 constraints on x_init_
//说的很详细，主要包含 变量边界约束，三个运动学公式约束以及初始状态约束，边界约束主要是横向最大位移、最大速度、最大加速度等，运动学公式主要是 x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s等等，可参考我给的博文首先是变量约束，通过赋值变量上下边界，完成约束设置。
// set x, x', x'' bounds
for (int i = 0; i < num_of_variables; ++i) {
if (i < n) {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
x_bounds_[i].first * scale_factor_[0];
upper_bounds->at(constraint_index) =
x_bounds_[i].second * scale_factor_[0];
} else if (i < 2 * n) {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
dx_bounds_[i - n].first * scale_factor_[1];
upper_bounds->at(constraint_index) =
dx_bounds_[i - n].second * scale_factor_[1];
} else {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
ddx_bounds_[i - 2 * n].first * scale_factor_[2];
upper_bounds->at(constraint_index) =
ddx_bounds_[i - 2 * n].second * scale_factor_[2];
}
++constraint_index;
}
随后是运动学约束，可能这里各位同学会搞混，按照代码的约束，由于csc就是这样的写法，各位同学可以画一个矩阵，这些约束都是按照constrain index作为序列标号，variables[i]在这里只是调用了第i个变量，后面的-1.0代表该变量的相关系数。以这个for循环为例， 约束形式应该是： 0*variable[0]+0*variable[1]+….-variables[2*n+i]+variables[2*n+i+1] = 0(上下界都是0，因此等于0)正好与之前运动学约束对应。其他的同理。
// x(i->i+1)''' = (x(i+1)'' - x(i)'') / delta_s 以constrain index作为序列
for (int i = 0; i + 1 < n; ++i)
{
variables[2 * n + i].emplace_back(constraint_index, -1.0);
variables[2 * n + i + 1].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
dddx_bound_.first * delta_s_ * scale_factor_[2];
upper_bounds->at(constraint_index) =
dddx_bound_.second * delta_s_ * scale_factor_[2];
++constraint_index;
}
// x(i+1)' - x(i)' - 0.5 * delta_s * x(i)'' - 0.5 * delta_s * x(i+1)'' = 0
for (int i = 0; i + 1 < n; ++i) {
variables[n + i].emplace_back(constraint_index, -1.0 * scale_factor_[2]);
variables[n + i + 1].emplace_back(constraint_index, 1.0 * scale_factor_[2]);
variables[2 * n + i].emplace_back(constraint_index,
-0.5 * delta_s_ * scale_factor_[1]);
variables[2 * n + i + 1].emplace_back(constraint_index,
-0.5 * delta_s_ * scale_factor_[1]);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}
// x(i+1) - x(i) - delta_s * x(i)'
// - 1/3 * delta_s^2 * x(i)'' - 1/6 * delta_s^2 * x(i+1)''
auto delta_s_sq_ = delta_s_ * delta_s_;
for (int i = 0; i + 1 < n; ++i) {
variables[i].emplace_back(constraint_index,
-1.0 * scale_factor_[1] * scale_factor_[2]);
variables[i + 1].emplace_back(constraint_index,
1.0 * scale_factor_[1] * scale_factor_[2]);
variables[n + i].emplace_back(
constraint_index, -delta_s_ * scale_factor_[0] * scale_factor_[2]);
variables[2 * n + i].emplace_back(
constraint_index,
-delta_s_sq_ / 3.0 * scale_factor_[0] * scale_factor_[1]);
variables[2 * n + i + 1].emplace_back(
constraint_index,
-delta_s_sq_ / 6.0 * scale_factor_[0] * scale_factor_[1]);
lower_bounds->at(constraint_index) = 0.0;
upper_bounds->at(constraint_index) = 0.0;
++constraint_index;
}最后是初始状态约束，即最终轨迹的初始状态要与当前车辆状态保持一致。
// constrain on x_init
variables[0].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
upper_bounds->at(constraint_index) = x_init_[0] * scale_factor_[0];
++constraint_index;
variables[n].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
upper_bounds->at(constraint_index) = x_init_[1] * scale_factor_[1];
++constraint_index;
variables[2 * n].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
upper_bounds->at(constraint_index) = x_init_[2] * scale_factor_[2];
++constraint_index;
最后，我们将进行offset 补偿，这里主要指的是 最后的参考线要考虑referline这一因素，即初始解。保证尽可能不要有太大偏差，这样有可能给车辆带来不稳定因素，这里主要是给目标函数进行补偿，目标函数的 ref一项。
其实目标函数在横向位移上有两项： l^2+(l-ref)^2,因此可以看到为什么在目标函数里，l^2的系数乘以2，在这里将第二项进行了拆解，于是有了offset。 即 -2ref*i，这个就对应了。各位细品。至于为什么不考虑ref^2，因为它是个非负实数，并不包含任何变量，因此不影响梯度下降，从而不影响整个函数的求解。因此在此处省略。
void PiecewiseJerkPathProblem::CalculateOffset(std::vector<c_float>* q) {
CHECK_NOTNULL(q);
const int n = static_cast<int>(num_of_knots_);
const int kNumParam = 3 * n;
q->resize(kNumParam, 0.0);
if (has_x_ref_) {
for (int i = 0; i < n; ++i) {
q->at(i) += -2.0 * weight_x_ref_ * x_ref_[i] / scale_factor_[0];
}
}最终，pieccewise jerk完成了求解，后面的To jerk函数在这里就不过多介绍了，主要原理就是利用积分，最终得到整条横向位移（沿着s轴），然后通过frenet转化，从而得到笛卡尔坐标系下的具体路径。 由此完成了轨迹规划中的路径规划。
关于路径规划部分的代码已单独上传到git,链接如下：
path_planning