---

這一章將來講解速度決策算法，也就是SPEED_HEURISTIC_OPTIMIZER task里面的內容。Apollo 9.0使用動態規劃算法進行速度決策，從類名來看，PathTimeHeuristicOptimizer 是路徑時間啟發式優化器，顧名思義，這里的算法在非凸的ST空間進行了縱向超讓的決策，同時也為后續速度規劃算法提供了一個啟發式的粗解。

1. 動態規劃

動態規劃，英文：Dynamic Programming，簡稱DP。

關于動態規劃，網上其實有很多講解，都很詳細，這里不再贅述。其實如果你刷過動態規劃的算法題，相信你對dp問題一定有很深的理解。如果沒有，也沒有關系。動態規劃一句話概括就是：就是把大問題拆成小問題，記住小問題的答案，避免重復計算，最后拼出大問題的答案。

PathTimeHeuristicOptimizer算法，實際上是將ST空間進行離散，在這離散的圖上尋找“最短”路徑的問題，所以可以使用動態規劃分而治之的思想進行求解。注意這里的“最短”，并不是路程上的最短，而是指包含障礙物代價，加速度代價等一系列代價最小的一條路徑。

動態規劃Task的入口在PathTimeHeuristicOptimizer類里面，下面是整個算法的求解步驟：

• 變道和非變道加載不同的dp參數
• 構建ST柵格圖
• ST柵格圖搜素

path_time_heuristic_optimizer.cc

bool PathTimeHeuristicOptimizer::SearchPathTimeGraph(SpeedData* speed_data) const {// 變道和非變道加載不同的dp參數const auto& dp_st_speed_optimizer_config = reference_line_info_->IsChangeLanePath()? config_.lane_change_speed_config(): config_.default_speed_config();// 構建ST柵格圖GriddedPathTimeGraph st_graph(reference_line_info_->st_graph_data(), dp_st_speed_optimizer_config,reference_line_info_->path_decision()->obstacles().Items(), init_point_);// ST柵格圖搜素if (!st_graph.Search(speed_data).ok()) {AERROR << "failed to search graph with dynamic programming.";return false;}return true;
}

2. 采樣

接下來就是在ST的維度進行采樣：近端稠密一些，遠端稀疏一點，這樣可以節省計算量。同時變道和非變道場景，S方向采樣的稠密點間隔和范圍都不一樣，非變道場景更稠密，范圍更廣一些，這樣可以更好應對cut-in和cut-out場景，讓縱向速度計算更精細。

上圖中，最左下角的點，T肯定是0，而S是速度規劃的起點：

$S_{end}$：速度規劃的終點，是路徑規劃的path長度；
$T_{end}$：很難計算，和道路的限速有關，也和動態障礙物有關。這里需要打破一個思維定式，在路徑規劃里面，必須要把比如60m路徑全部規劃完。在速度規劃里面，不強求在$T_{end}$里面完全規劃完$S_{end}$。比如說路徑規劃出了60m，但是速度規劃可以只規劃20m的距離，因為現實中動態障礙物的軌跡是千奇百怪的。所以，在這里設置$T_{end}=8s$，在這8s的時間內，不強求把$S_{end}$全部規劃完。

關于具體采樣的步驟和內容可以見下面的代碼，可以說加上注釋已經非常容易理解了。唯一需要補充的一點是：cost_table_其實是一個dimension_t_行，dimension_s_列的二維表，也就是T是行維度，S是列維度，將上圖所示的ST離散圖順時針旋轉90°，就可以剛好對應上。但是后面在計算代價和回溯的時候行列S和T又有些混淆，不過沒有關系，對照圖是很好理解的。

Status GriddedPathTimeGraph::InitCostTable() {// Time dimension is homogeneous while Spatial dimension has two resolutions,// dense and sparse with dense resolution coming first in the spatial horizon// Sanity check for numerical stabilityif (unit_t_ < kDoubleEpsilon) {const std::string msg = "unit_t is smaller than the kDoubleEpsilon.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// Sanity check on s dimension settingif (dense_dimension_s_ < 1) {const std::string msg = "dense_dimension_s is at least 1.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}dimension_t_ = static_cast<uint32_t>(std::ceil(total_length_t_ / static_cast<double>(unit_t_))) + 1;double sparse_length_s = total_length_s_ -static_cast<double>(dense_dimension_s_ - 1) * dense_unit_s_;sparse_dimension_s_ = sparse_length_s > std::numeric_limits<double>::epsilon()? static_cast<uint32_t>(std::ceil(sparse_length_s / sparse_unit_s_)): 0;dense_dimension_s_ = sparse_length_s > std::numeric_limits<double>::epsilon()? dense_dimension_s_: static_cast<uint32_t>(std::ceil(total_length_s_ / dense_unit_s_)) + 1;dimension_s_ = dense_dimension_s_ + sparse_dimension_s_;PrintCurves debug;// Sanity Checkif (dimension_t_ < 1 || dimension_s_ < 1) {const std::string msg = "Dp st cost table size incorrect.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// 定義dp代價表，注意這個表：常規ST圖順時針旋轉90°，一共t行，每行s列 , total_cost_默認無窮大cost_table_ = std::vector<std::vector<StGraphPoint>>(dimension_t_, std::vector<StGraphPoint>(dimension_s_, StGraphPoint()));double curr_t = 0.0;for (uint32_t i = 0; i < cost_table_.size(); ++i, curr_t += unit_t_) {          // Tauto& cost_table_i = cost_table_[i];double curr_s = 0.0;// 稠密for (uint32_t j = 0; j < dense_dimension_s_; ++j, curr_s += dense_unit_s_) {  // Scost_table_i[j].Init(i, j, STPoint(curr_s, curr_t));debug.AddPoint("dp_node_points", curr_t, curr_s);}// 稀疏curr_s = static_cast<double>(dense_dimension_s_ - 1) * dense_unit_s_ + sparse_unit_s_;for (uint32_t j = dense_dimension_s_; j < cost_table_i.size(); ++j, curr_s += sparse_unit_s_) {cost_table_i[j].Init(i, j, STPoint(curr_s, curr_t));debug.AddPoint("dp_node_points", curr_t, curr_s);}}// 獲取第一行的sconst auto& cost_table_0 = cost_table_[0];spatial_distance_by_index_ = std::vector<double>(cost_table_0.size(), 0.0);for (uint32_t i = 0; i < cost_table_0.size(); ++i) {spatial_distance_by_index_[i] = cost_table_0[i].point().s();}return Status::OK();
}

3. 代價函數

接下來就是在離散表上，進行“最短”路徑搜索，也就是使用動態規劃算法計算速度規劃起點到每個點的最小代價，然后回溯（反推）獲取最優路徑。

注意：速度動態規劃可以進行剪枝操作，以減少計算量，依據車輛動力學max_acceleration_和max_deceleration_找到下一列可能到達的范圍[next_lowest_row, next_highest_row]，只需要計算當前點到下一列該范圍內的點的代價。

一個點的total_cost由兩部分構成：

1. 點代價：
   a. 障礙物代價：obstacle_cost
   b. 距離終點代價：spatial_potential_cost
   c. 前一個點的：total_cost
2. 邊代價：
   a. 速度代價：Speedcost
   b. 加速度代價：AccelCost
   c. 加加速度代價：JerkCost

3.1 障礙物代價

Apollo 9.0動態規劃中障礙物代價計算做了簡化，只需要計算該點到所有t時刻障礙物的代價，并進行累加。
如下圖所示的$a_{62}$點，該時刻有兩個障礙物，通過公式計算障礙物代價即可：

• 不在范圍內：0
• 在障礙物st邊界框里：∞
• 其他：$cost+=config\_.obstacle\_weight()?config\_.default\_obstacle\_cost()?s\_diff?s\_diff$

這里需要注意的是，跟車和超車需要與前后車保持的安全距離大小不一樣，超車情況下，需要離后車距離更大。

double DpStCost::GetObstacleCost(const StGraphPoint& st_graph_point) {const double s = st_graph_point.point().s();const double t = st_graph_point.point().t();double cost = 0.0;if (FLAGS_use_st_drivable_boundary) {   // false // TODO(Jiancheng): move to configsstatic constexpr double boundary_resolution = 0.1;int index = static_cast<int>(t / boundary_resolution);const double lower_bound = st_drivable_boundary_.st_boundary(index).s_lower();const double upper_bound = st_drivable_boundary_.st_boundary(index).s_upper();if (s > upper_bound || s < lower_bound) {return kInf;}}// 遍歷每個障礙物，計算t時刻障礙物st邊界框的上界和下界，根據無人車的位置(t,s)與邊界框是否重合，計算障礙物代價for (const auto* obstacle : obstacles_) {// Not applying obstacle approaching cost to virtual obstacle like created stop fencesif (obstacle->IsVirtual()) {continue;}// Stop obstacles are assumed to have a safety margin when mapping them out,// so repelling force in dp st is not needed as it is designed to have adc// stop right at the stop distance we design in prior mapping processif (obstacle->LongitudinalDecision().has_stop()) {continue;}auto boundary = obstacle->path_st_boundary();if (boundary.min_s() > FLAGS_speed_lon_decision_horizon) {  // 縱向決策的最遠距離 200continue;}if (t < boundary.min_t() || t > boundary.max_t()) {continue;}if (boundary.IsPointInBoundary(st_graph_point.point())) {return kInf;}// 情況4：需要減速避讓或加速超過的障礙物。計算障礙物在t時刻的上界和下界位置，即上下界的累積距離sdouble s_upper = 0.0;double s_lower = 0.0;// 為了避免其他節點(t,s)再一次計算t時刻的障礙物上下界，利用緩存加速計算。GetBoundarySRange函數可以用來計算t時刻障礙物上界和下界累積距離s，并緩存int boundary_index = boundary_map_[boundary.id()];if (boundary_cost_[boundary_index][st_graph_point.index_t()].first < 0.0) { // 還沒有計算過boundary.GetBoundarySRange(t, &s_upper, &s_lower);boundary_cost_[boundary_index][st_graph_point.index_t()] = std::make_pair(s_upper, s_lower);} else {s_upper = boundary_cost_[boundary_index][st_graph_point.index_t()].first; // 之前計算過，直接取值s_lower = boundary_cost_[boundary_index][st_graph_point.index_t()].second;}// t時刻, 無人車在障礙物后方if (s < s_lower) {const double follow_distance_s = config_.safe_distance(); // 0.2AINFO << "follow_distance_s : " << follow_distance_s;if (s + follow_distance_s < s_lower) {                    // 如果障礙物和無人車在t時刻距離大于安全距離，距離比較遠，cost=0continue;} else {                                                  // 否則距離小于安全距離，計算cost。obstacle_weight：1.0，default_obstacle_cost：1000，auto s_diff = follow_distance_s - s_lower + s;cost += config_.obstacle_weight() * config_.default_obstacle_cost() * s_diff * s_diff;}// t時刻, 無人車在障礙物前方} else if (s > s_upper) {const double overtake_distance_s = StGapEstimator::EstimateSafeOvertakingGap(); // 20if (s > s_upper + overtake_distance_s) {  // or calculated from velocitycontinue;                               // 如果障礙物和無人車在t時刻距離大于安全距離，距離比較遠，cost=0} else {                                  // 否則距離小于安全距離，計算cost。obstacle_weight：1.0，default_obstacle_cost：1000，auto s_diff = overtake_distance_s + s_upper - s;cost += config_.obstacle_weight() * config_.default_obstacle_cost() * s_diff * s_diff;}}}return cost * unit_t_;  // unit_t_ = 1
}

3.2 距離終點代價

距離終點代價其實就是距離路徑規劃終點的一個懲罰，該點距離終點越近，代價越小；距離終點越遠，代價越大。

目的很簡單，就是希望速度規劃能夠就可能將路徑規劃的s全部路徑都覆蓋。

double DpStCost::GetSpatialPotentialCost(const StGraphPoint& point) {return (total_s_ - point.point().s()) * config_.spatial_potential_penalty();  // 距離終點懲罰 100 or 100000(變道)
}

3.3 速度代價

速度代價很簡單，主要考慮兩方面：

• 不要超速，盡量貼近限速；
• 盡量貼近巡航速度，也就是駕駛員設定的速度。

double DpStCost::GetSpeedCost(const STPoint& first, const STPoint& second,const double speed_limit,const double cruise_speed) const {double cost = 0.0;const double speed = (second.s() - first.s()) / unit_t_; //計算時間段[t-1,t]內的平均速度if (speed < 0) { // 倒車？速度代價無窮大return kInf;}const double max_adc_stop_speed = common::VehicleConfigHelper::Instance()->GetConfig().vehicle_param().max_abs_speed_when_stopped();AINFO << "max_adc_stop_speed : "<< max_adc_stop_speed; // 如果速度接近停車，并且在禁停區內 max_stop_speed = 0.2  if (speed < max_adc_stop_speed && InKeepClearRange(second.s())) {// first.s in range  在KeepClear區域低速懲罰 10 * * 1000cost += config_.keep_clear_low_speed_penalty() * unit_t_ * config_.default_speed_cost();}// 計算當前速度和限速的差值比，大于0，超速；小于0，未超速double det_speed = (speed - speed_limit) / speed_limit;if (det_speed > 0) {cost += config_.exceed_speed_penalty() * config_.default_speed_cost() * (det_speed * det_speed) * unit_t_;} else if (det_speed < 0) {cost += config_.low_speed_penalty() * config_.default_speed_cost() * -det_speed * unit_t_;}if (config_.enable_dp_reference_speed()) {double diff_speed = speed - cruise_speed;cost += config_.reference_speed_penalty() * config_.default_speed_cost() * fabs(diff_speed) * unit_t_;  // 10 * 1000 *  Δv * }return cost;
}

3.4 加速度代價

要計算邊的加速度代價，首先需要計算邊的加速度。

以起點為例，計算起點到坐標點的加速度代價，直接用差分來代替速度、加速度、Jerk：

double DpStCost::GetAccelCostByThreePoints(const STPoint& first,const STPoint& second,const STPoint& third) {// 利用3個節點的累積距離s1，s2，s3來計算加速度   double accel = (first.s() + third.s() - 2 * second.s()) / (unit_t_ * unit_t_);return GetAccelCost(accel);
}double DpStCost::GetAccelCostByTwoPoints(const double pre_speed,const STPoint& pre_point,const STPoint& curr_point) {// 利用2個節點的累積距離s1，s2來計算加速度double current_speed = (curr_point.s() - pre_point.s()) / unit_t_;double accel = (current_speed - pre_speed) / unit_t_;return GetAccelCost(accel);
}

邊的加速度代價：
$$\frac{{\left(w_{cost\_dec}?\ddot{s}\right)}^2}{1+e^{(a?max\_dec)}}+\frac{{\left(w_{cost\_acc}?\ddot{s}\right)}^2}{1+e^{?(a?max\_acc)}}$$

double DpStCost::GetAccelCost(const double accel) {double cost = 0.0;// 將給定的加速度 accel 標準化并加上一個偏移量 kShift 來計算得到。這樣做可以確保不同的 accel 值映射到 accel_cost_ 中不同的索引位置。static constexpr double kEpsilon = 0.1; // 表示對加速度的精度要求static constexpr size_t kShift = 100;const size_t accel_key = static_cast<size_t>(accel / kEpsilon + 0.5 + kShift);DCHECK_LT(accel_key, accel_cost_.size());if (accel_key >= accel_cost_.size()) {return kInf;}if (accel_cost_.at(accel_key) < 0.0) {const double accel_sq = accel * accel;double max_acc = config_.max_acceleration();          // 3.0  m/s^2double max_dec = config_.max_deceleration();          // -4.0 m/s^2double accel_penalty = config_.accel_penalty();       // 1.0double decel_penalty = config_.decel_penalty();       // 1.0if (accel > 0.0) {          // 計算加速度正情況下costcost = accel_penalty * accel_sq;} else {                    // 計算加速度負情況下costcost = decel_penalty * accel_sq;}// 總體costcost += accel_sq * decel_penalty * decel_penalty / (1 + std::exp(1.0 * (accel - max_dec))) +accel_sq * accel_penalty * accel_penalty / (1 + std::exp(-1.0 * (accel - max_acc)));accel_cost_.at(accel_key) = cost;} else {cost = accel_cost_.at(accel_key); // 該加速度之前計算過，直接索引}return cost * unit_t_;
}

3.5 jerk代價

加加速度代價的計算更簡單：

double DpStCost::JerkCost(const double jerk) {double cost = 0.0;static constexpr double kEpsilon = 0.1;static constexpr size_t kShift = 200;const size_t jerk_key = static_cast<size_t>(jerk / kEpsilon + 0.5 + kShift);  // 原理同acc的計算if (jerk_key >= jerk_cost_.size()) {return kInf;}if (jerk_cost_.at(jerk_key) < 0.0) {double jerk_sq = jerk * jerk;if (jerk > 0) {cost = config_.positive_jerk_coeff() * jerk_sq * unit_t_;} else {cost = config_.negative_jerk_coeff() * jerk_sq * unit_t_;}jerk_cost_.at(jerk_key) = cost;} else {cost = jerk_cost_.at(jerk_key);}// TODO(All): normalize to unit_t_return cost;
}

4. 回溯

回溯：也就是根據最小代價，從后往前倒推，得到最優的路徑。這是動態規劃算法中常用的一種手段。

因為$T_{end}$是人為給定的一個值，所以可能在$T_{end}$之前，速度規劃就已經到達路徑規劃的終點。所以需要遍歷最上邊和最右邊區域，找到cost最小的作為速度規劃的終點。

由于我們不要求一定要規劃完所有路徑，所以最優的速度規劃可以是紫色那種，也有可能是黃色那種，還有可能是橙色那種。

Status GriddedPathTimeGraph::RetrieveSpeedProfile(SpeedData* const speed_data) {// Step 1 : 計算規劃終點double min_cost = std::numeric_limits<double>::infinity();const StGraphPoint* best_end_point = nullptr;PrintPoints debug("dp_node_edge");for (const auto& points_vec : cost_table_) {for (const auto& pt : points_vec) {debug.AddPoint(pt.point().t(), pt.point().s());}}// for debug plot// debug.PrintToLog();// 尋找最上一行(s=S)和最右一列(t=T)中最小的cost對應的節點，作為規劃終點for (const StGraphPoint& cur_point : cost_table_.back()) {  // 最右一列(t=T)if (!std::isinf(cur_point.total_cost()) &&cur_point.total_cost() < min_cost) {best_end_point = &cur_point;min_cost = cur_point.total_cost();}}for (const auto& row : cost_table_) {                      // 每一個t，也就是每一列const StGraphPoint& cur_point = row.back();              // 每一列的最上一行(s=S)if (!std::isinf(cur_point.total_cost()) &&cur_point.total_cost() < min_cost) {best_end_point = &cur_point;min_cost = cur_point.total_cost();}}if (best_end_point == nullptr) {const std::string msg = "Fail to find the best feasible trajectory.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}// Step 2 : 從規劃終點開始回溯，找到最小cost的規劃路徑std::vector<SpeedPoint> speed_profile;const StGraphPoint* cur_point = best_end_point;PrintPoints debug_res("dp_result");while (cur_point != nullptr) {ADEBUG << "Time: " << cur_point->point().t();ADEBUG << "S: " << cur_point->point().s();ADEBUG << "V: " << cur_point->GetOptimalSpeed();SpeedPoint speed_point;debug_res.AddPoint(cur_point->point().t(), cur_point->point().s());speed_point.set_s(cur_point->point().s());speed_point.set_t(cur_point->point().t());speed_profile.push_back(speed_point);cur_point = cur_point->pre_point();}//  for debug plot//   debug_res.PrintToLog();std::reverse(speed_profile.begin(), speed_profile.end());  // 顛倒容器中元素的順序static constexpr double kEpsilon = std::numeric_limits<double>::epsilon();  // 返回的是 double 類型能夠表示的最小正數if (speed_profile.front().t() > kEpsilon ||speed_profile.front().s() > kEpsilon) {const std::string msg = "Fail to retrieve speed profile.";AERROR << msg;return Status(ErrorCode::PLANNING_ERROR, msg);}AINFO << "front: " << speed_profile.front().t() << " " << speed_profile.front().s();// Step 3 : 計算速度 vfor (size_t i = 0; i + 1 < speed_profile.size(); ++i) {const double v = (speed_profile[i + 1].s() - speed_profile[i].s()) /(speed_profile[i + 1].t() - speed_profile[i].t() + 1e-3);  // 斜率 speed_profile[i].set_v(v);AINFO << "v: " << v;}*speed_data = SpeedData(speed_profile);return Status::OK();
}