大家好，我已經把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=)