大家好，我已经把CSDN上的博客迁移到了知乎上，欢迎大家在知乎关注我的专栏慢慢悠悠小马车（https://zhuanlan.zhihu.com/duangduangduang）。希望大家可以多多交流，互相学习。
上文添加优化问题的约束后，本文接着讲如何添加目标函数，即QpSplineStGraph::AddKernel()函数。这部分的代码比较抽象，但是底层逻辑与QP Path Optimizer是相同的，类和函数也是相同的，大家可以参考Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 3 来理解下面的代码。
当然，对于speed的优化，Apollo使用了不同的目标函数，分为7部分，在代码注释中说明。
Status QpSplineStGraph::AddKernel(
const std::vector<const STBoundary*>& boundaries,
const SpeedLimit& speed_limit) {
Spline1dKernel* spline_kernel = spline_solver_->mutable_spline_kernel();
if (qp_st_speed_config_.qp_spline_config().accel_kernel_weight() > 0) {
//1、优化二阶导，即减小加速度
spline_kernel->AddSecondOrderDerivativeMatrix(
qp_st_speed_config_.qp_spline_config().accel_kernel_weight());
}
if (qp_st_speed_config_.qp_spline_config().jerk_kernel_weight() > 0) {
//2、优化三阶导，即减小jerk
spline_kernel->AddThirdOrderDerivativeMatrix(
qp_st_speed_config_.qp_spline_config().jerk_kernel_weight());
}
//3、优化目标：cruise对应的参考速度，即以最短时间、最快速度行驶
if (!AddCruiseReferenceLineKernel(
qp_st_speed_config_.qp_spline_config().cruise_weight()).ok()) {
return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::AddKernel");
}
//4、优化目标：follow决策的参考速度
if (!AddFollowReferenceLineKernel(
boundaries, qp_st_speed_config_.qp_spline_config().follow_weight()).ok()) {
return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::AddKernel");
}
//5、优化目标：yield决策的参考速度
if (!AddYieldReferenceLineKernel(
boundaries, qp_st_speed_config_.qp_spline_config().yield_weight()).ok()) {
return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::AddKernel");
}
//6、优化目标：EM的DP输出的参考速度
if (!AddDpStReferenceKernel(
qp_st_speed_config_.qp_spline_config().dp_st_reference_weight())) {
return Status(ErrorCode::PLANNING_ERROR, "QpSplineStGraph::AddKernel");
}
// init point jerk continuous kernel 看不懂？？
(*spline_kernel->mutable_kernel_matrix())(2, 2) += 2.0 * 4.0 *
qp_st_speed_config_.qp_spline_config().init_jerk_kernel_weight();
(*spline_kernel->mutable_offset())(2, 0) += -4.0 * init_point_.a() *
qp_st_speed_config_.qp_spline_config().init_jerk_kernel_weight();
//7、正则项
spline_kernel->AddRegularization(
qp_st_speed_config_.qp_spline_config().regularization_weight());
return Status::OK();
}
我将目标函数分为3类讨论。每一类中的不同的Kernel，套路是一样的。
1. 对n阶导的优化
指第1、2条。先调用Spline1dKernel::AddNthDerivativekernelMatrix()，再调用SplineSegKernel::NthDerivativeKernel()，与Baidu Apollo代码解析之EM Planner中的QP Path Optimizer 3 中的介绍一致，不再赘述。
2. 对不同来源的参考速度的优化
指第3~6条。对于某种场景（如cruise）、某种决策（如follow、yield）、某历史数据（如DP规划的speed），使现在的规划尽量向其靠近，最小化两者之间的差别。内层都是调用了Spline1dKernel::AddReferenceLineKernelMatrix()。这里与地图中的reference line 没有太大关系，而是指提供的先验数据本身便是一种reference，只是先验数据的获取方式不同、预处理的步骤不同。
第3条：
Status QpSplineStGraph::AddCruiseReferenceLineKernel(const double weight) {
...
double dist_ref = total_path_length_;
for (uint32_t i = 0; i < t_evaluated_.size(); ++i) {
cruise_.push_back(dist_ref);
}
...
if (t_evaluated_.size() > 0) {
//cruise_中的元素都是total_path_length_
//即希望以最短时间到达total_path_length_
spline_kernel->AddReferenceLineKernelMatrix(t_evaluated_, cruise_,
weight * total_time_ / static_cast<double>(t_evaluated_.size()));
}
return Status::OK();
}
第4条：
Status QpSplineStGraph::AddFollowReferenceLineKernel(
const std::vector<const STBoundary*>& boundaries, const double weight) {
...
for (size_t i = 0; i < t_evaluated_.size(); ++i) {
const double curr_t = t_evaluated_[i];
...
for (const auto* boundary : boundaries) {
...
if (boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
success = true;
//s_min表示自车与前方最近障碍物可保持的follow距离
s_min = std::min(s_min, s_upper - boundary->characteristic_length() -
qp_st_speed_config_.qp_spline_config().follow_drag_distance());
}
}
if (success && s_min < cruise_[i]) {
filtered_evaluate_t.push_back(curr_t);
ref_s.push_back(s_min);
...
}
}
//优化目标是尽可能按照follow决策对应的<filtered_evaluate_t, ref_s>
//这种状态行驶，相当于给了参考速度
if (!ref_s.empty()) {
spline_kernel->AddReferenceLineKernelMatrix(filtered_evaluate_t, ref_s,
weight * total_time_ / static_cast<double>(t_evaluated_.size()));
}
...
}
第5条：
Status QpSplineStGraph::AddYieldReferenceLineKernel(
const std::vector<const STBoundary*>& boundaries, const double weight) {
...
for (size_t i = 0; i < t_evaluated_.size(); ++i) {
const double curr_t = t_evaluated_[i];
...
for (const auto* boundary : boundaries) {
...
if (boundary->GetUnblockSRange(curr_t, &s_upper, &s_lower)) {
success = true;
//s_min表示自车与前方最近障碍物可保持的yield距离
s_min = std::min(s_min, s_upper - boundary->characteristic_length() -
qp_st_speed_config_.qp_spline_config().yield_drag_distance());
}
}
if (success && s_min < cruise_[i]) {
filtered_evaluate_t.push_back(curr_t);
ref_s.push_back(s_min);
}
}
//优化目标是尽可能按照yield决策对应的<filtered_evaluate_t, ref_s>
//这种状态行驶，相当于给了参考速度
if (!ref_s.empty()) {
spline_kernel->AddReferenceLineKernelMatrix(filtered_evaluate_t, ref_s,
weight * total_time_ / static_cast<double>(t_evaluated_.size()));
}
...
}
第6条：
bool QpSplineStGraph::AddDpStReferenceKernel(const double weight) const {
std::vector<double> t_pos;
std::vector<double> s_pos;
for (auto point : reference_dp_speed_points_) {
t_pos.push_back(point.t());
s_pos.push_back(point.s());
}
auto* spline_kernel = spline_solver_->mutable_spline_kernel();
if (!t_pos.empty()) {
spline_kernel->AddReferenceLineKernelMatrix(
t_pos, s_pos, weight * total_time_ / static_cast<double>(t_pos.size()));
}
return true;
}
共同点：
//优化目标是尽可能按照对应的<x_coord, ref_x> 这种状态行驶
//相当于给了参考目标，如速度规划时的<t,s>，即希望达到的参考速度
bool Spline1dKernel::AddReferenceLineKernelMatrix(
const std::vector<double>& x_coord, const std::vector<double>& ref_x,
const double weight) { ... }
3. 正则项
指第7条。
void Spline1dKernel::AddRegularization(const double regularized_param) {
Eigen::MatrixXd id_matrix =
Eigen::MatrixXd::Identity(kernel_matrix_.rows(), kernel_matrix_.cols());
kernel_matrix_ += 2.0 * id_matrix * regularized_param;
}
![上下位机：嵌入式工程中的默契（代码解析）[图]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)