大家好,我已经把CSDN上的博客迁移到了知乎上,欢迎大家在知乎关注我的专栏慢慢悠悠小马车(https://zhuanlan.zhihu.com/duangduangduang)。希望大家可以多多交流,互相学习。
SpeedOptimizer类继承自Task类,实现了继承来的Execute()函数,又在Execute()中调用了其纯虚成员函数Process()。而DpStSpeedOptimizer类又继承了SpeedOptimizer类,并实现了Process()函数,在Process()中调用了成员函数SearchStGraph(),到这里,才算进入了使用动态规划算法进行速度规划的正题。总之,Apollo的这个层次设计是真的优美!以上,可见apollo\modules\planning\tasks\task.h 及 apollo\modules\planning\tasks\optimizers\speed_optimizer.h 及 apollo\modules\planning\tasks\optimizers\dp_st_speed\dp_st_speed_optimizer.h 文件。
bool DpStSpeedOptimizer::SearchStGraph(SpeedData* speed_data) const {
DpStGraph st_graph(reference_line_info_->st_graph_data(), dp_st_speed_config_,
reference_line_info_->path_decision()->obstacles().Items(),
init_point_, adc_sl_boundary_);
if (!st_graph.Search(speed_data).ok()) {
AERROR << "failed to search graph with dynamic programming.";
return false;
}
return true;
}
在DpStSpeedOptimizer::SearchStGraph()中,首先构造了DpStGraph类实例,该类包含了所有可能的(s,t)节点的代价表,应用动态规划算法查找出代价最小的路径,然后计算速度,就得到了速度规划的结果。
Status DpStGraph::Search(SpeedData* const speed_data) {
constexpr double kBounadryEpsilon = 1e-2;
for (const auto& boundary : st_graph_data_.st_boundaries()) {
//KEEP_CLEAR的范围不影响速度规划
if (boundary->boundary_type() == STBoundary::BoundaryType::KEEP_CLEAR) {
continue;
}
//若boundary包含原点或非常接近原点(即自车现在的位置),自车应该stop,不再前进
//故s,v,a都是0,即随着t推移,自车不动
if (boundary->IsPointInBoundary({0.0, 0.0}) ||
(std::fabs(boundary->min_t()) < kBounadryEpsilon &&
std::fabs(boundary->min_s()) < kBounadryEpsilon)) {
std::vector<SpeedPoint> speed_profile;
double t = 0.0;
for (int i = 0; i <= dp_st_speed_config_.matrix_dimension_t(); ++i, t += unit_t_) {
SpeedPoint speed_point;
speed_point.set_s(0.0);
speed_point.set_t(t);
speed_point.set_v(0.0);
speed_point.set_a(0.0);
speed_profile.emplace_back(speed_point);
}
*speed_data = SpeedData(speed_profile);
return Status::OK();
}
}
if (!InitCostTable().ok()) { ... }
if (!CalculateTotalCost().ok()) { ... }
if (!RetrieveSpeedProfile(speed_data).ok()) { ... }
return Status::OK();
}
在对自车当前位置(原点)判断后,速度规划主要分为3步:构建代价表InitCostTable(),更新代价表中每个节点的代价CalculateTotalCost(),查找代价最小的规划结束点并逆向构建路径、求取速度RetrieveSpeedProfile()。
// cost_table_[t][s]
// row: s, col: t
//StGraphPoint的total_cost_被初始化为+inf
std::vector<std::vector<StGraphPoint>> cost_table_;
DpStGraph::InitCostTable()没什么好说的,主要小心cost_table_外层是t,内层是s,行数是s,列数是t。
DpStGraph::CalculateTotalCost()的目的是对cost_table_中的每个节点更新其cost,在此,我们先说如何更新单个节点的cost,请看DpStGraph::CalculateCostAt()函数,动态规划的核心都在这个函数里。
void DpStGraph::CalculateCostAt(const std::shared_ptr<StGraphMessage>& msg) {
//动态规划
...
const auto& cost_init = cost_table_[0][0];
if (c == 0) {
//设置起始点的cost为0,为什么不再刚进入函数的时候检查?
DCHECK_EQ(r, 0) << "Incorrect. Row should be 0 with col = 0. row: " << r;
cost_cr.SetTotalCost(0.0);
return;
}
...
if (c == 1) {
//c=1需要单独处理,因为c=0的1列只有1个节点,即初始节点
const double acc = (r * unit_s_ / unit_t_ - init_point_.v()) / unit_t_;
//如果加速度超出范围,不计算cost
if (acc < dp_st_speed_config_.max_deceleration() ||
acc > dp_st_speed_config_.max_acceleration()) {
return;
}
//如果从起点到当前点有障碍物,不计算cost
if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
cost_init)) {
return;
}
cost_cr.SetTotalCost(
//点内cost
cost_cr.obstacle_cost() + cost_init.total_cost() +
//点间cost,从起点到[c=1,r]的各项cost,即[t0,s0]->[t1,sn]的cost
CalculateEdgeCostForSecondCol(r, speed_limit, soft_speed_limit));
cost_cr.SetPrePoint(cost_init);
return;
}
constexpr double kSpeedRangeBuffer = 0.20;
//单时间步长最大可能前进的s
const uint32_t max_s_diff =
static_cast<uint32_t>(FLAGS_planning_upper_speed_limit *
(1 + kSpeedRangeBuffer) * unit_t_ / unit_s_);
//缩小行范围到[r_low,r],因为要更新[c,r]的cost,就要考察
//[c前一列即前一时刻,r前几行]分别到[c,r]的代价,取最小值,松弛操作,动态规划
const uint32_t r_low = (max_s_diff < r ? r - max_s_diff : 0);
const auto& pre_col = cost_table_[c - 1];
if (c == 2) {
for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
const double acc =
(r * unit_s_ - 2 * r_pre * unit_s_) / (unit_t_ * unit_t_);
if (acc < dp_st_speed_config_.max_deceleration() ||
acc > dp_st_speed_config_.max_acceleration()) {
continue;
}
if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
pre_col[r_pre])) {
continue;
}
const double cost =
//第二项是考察的s方向前面某点的cost
cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
CalculateEdgeCostForThirdCol(r, r_pre, speed_limit, soft_speed_limit);
//在未赋有效cost值之前,cost_cr的total_cost是+inf
if (cost < cost_cr.total_cost()) {
cost_cr.SetTotalCost(cost);
cost_cr.SetPrePoint(pre_col[r_pre]);
}
}
return;
}
//固定col即t,考察row即s
for (uint32_t r_pre = r_low; r_pre <= r; ++r_pre) {
//[pre_col,r_pre]不可达,自然不必考虑从该点到[c,r]点
if (std::isinf(pre_col[r_pre].total_cost()) ||
pre_col[r_pre].pre_point() == nullptr) {
continue;
}
const double curr_a = (cost_cr.index_s() * unit_s_ +
pre_col[r_pre].pre_point()->index_s() * unit_s_ -
2 * pre_col[r_pre].index_s() * unit_s_) /
(unit_t_ * unit_t_);
if (curr_a > vehicle_param_.max_acceleration() ||
curr_a < vehicle_param_.max_deceleration()) {
continue;
}
if (CheckOverlapOnDpStGraph(st_graph_data_.st_boundaries(), cost_cr,
pre_col[r_pre])) {
continue;
}
//接下来的2个if判断是为了确认triple_pre_point有效,以计算jerk cost
//因为jerk cost的计算方式不同,对col分3类讨论(思路相同),对应DpStCost类
//中3种计算jerk cost的函数。jerk cost是速度规划有效性的保障,若此处不计算,
//在后续的轨迹validaty check中排除无效轨迹也行,这种预先限制的方法更好
//accel的cost计算和范围检查也是同理,此处没有对jerk范围进行检查
uint32_t r_prepre = pre_col[r_pre].pre_point()->index_s();
const StGraphPoint& prepre_graph_point = cost_table_[c - 2][r_prepre];
if (std::isinf(prepre_graph_point.total_cost())) {
continue;
}
if (!prepre_graph_point.pre_point()) {
continue;
}
const STPoint& triple_pre_point = prepre_graph_point.pre_point()->point();
const STPoint& prepre_point = prepre_graph_point.point();
const STPoint& pre_point = pre_col[r_pre].point();
const STPoint& curr_point = cost_cr.point();
//triple_pre_point和prepre_point只是计算jerk cost需要
double cost = cost_cr.obstacle_cost() + pre_col[r_pre].total_cost() +
CalculateEdgeCost(triple_pre_point, prepre_point, pre_point,
curr_point, speed_limit, soft_speed_limit);
if (cost < cost_cr.total_cost()) {
cost_cr.SetTotalCost(cost);
cost_cr.SetPrePoint(pre_col[r_pre]);
}
}
}
DpStGraph::CalculateTotalCost()中是对每一行、每一列的遍历,巧的地方在于使用了GetRowRange()来计算由当前节点可到达的s(行)范围。
Status DpStGraph::CalculateTotalCost() {
// col and row are for STGraph
// t corresponding to col
// s corresponding to row
size_t next_highest_row = 0;
size_t next_lowest_row = 0;
for (size_t c = 0; c < cost_table_.size(); ++c) {
size_t highest_row = 0;
auto lowest_row = cost_table_.back().size() - 1; //行数-1
int count = static_cast<int>(next_highest_row) -
static_cast<int>(next_lowest_row) + 1;
//count始终>0
if (count > 0) {
std::vector<std::future<void>> results;
for (size_t r = next_lowest_row; r <= next_highest_row; ++r) {
auto msg = std::make_shared<StGraphMessage>(c, r);
...
CalculateCostAt(msg);
}
...
}
for (size_t r = next_lowest_row; r <= next_highest_row; ++r) {
const auto& cost_cr = cost_table_[c][r];
if (cost_cr.total_cost() < std::numeric_limits<double>::infinity()) {
size_t h_r = 0;
size_t l_r = 0;
GetRowRange(cost_cr, &h_r, &l_r);
//由第c列、第 next_lowest_row~next_highest_row 行的节点可到达的最远的s
highest_row = std::max(highest_row, h_r);
//可到达的最近的s
lowest_row = std::min(lowest_row, static_cast<size_t>(l_r));
}
}
next_highest_row = highest_row;
next_lowest_row = lowest_row;
}
return Status::OK();
}
DpStGraph::GetRowRange()中我有个疑惑的地方,就是对s的计算为什么不采用这个公式,而是使用了?
不过,考虑到 max_acceleration>0,max_deceleration<0,从计算结果来说,前者计算的 [lower_bound, upper_bound] 是后者计算结果的子集,倒是不影响程序正常逻辑。
//计算从point出发可能到达的s范围
void DpStGraph::GetRowRange(const StGraphPoint& point, size_t* next_highest_row,
size_t* next_lowest_row) {
...
const auto max_s_size = cost_table_.back().size() - 1; //行数-1
const double speed_coeff = unit_t_ * unit_t_;
//为什么不是v0*t+0.5am*t^2?
const double delta_s_upper_bound =
v0 * unit_t_ + vehicle_param_.max_acceleration() * speed_coeff;
*next_highest_row =
point.index_s() + static_cast<int>(delta_s_upper_bound / unit_s_);
if (*next_highest_row >= max_s_size) {
*next_highest_row = max_s_size;
}
const double delta_s_lower_bound = std::fmax(
0.0, v0 * unit_t_ + vehicle_param_.max_deceleration() * speed_coeff);
*next_lowest_row =
point.index_s() + static_cast<int>(delta_s_lower_bound / unit_s_);
if (*next_lowest_row > max_s_size) {
*next_lowest_row = max_s_size;
} else if (*next_lowest_row < 0) {
*next_lowest_row = 0;
}
}
CalculateCostAt()和GetRowRange()中都有基于当前点,判断可到达s(行)范围的操作。不同的是,GetRowRange()中是向s增大的方向查找,因为要逐行的扩展节点、更新其cost。而CalculateCostAt()中是向s减小的方向查找,即查找哪些行可以到达该点,因为该函数用来计算某节点的cost,就要基于之前经过的节点的cost。
DpStGraph::RetrieveSpeedProfile()以及过程中的其他类与函数,代码都很简洁清晰,就不在此赘述了。只以DpStCost::GetAccelCost()函数为例,提一下动态规划中常用的计算结果缓存技巧。具体请参考apollo\modules\planning\tasks\optimizers\dp_st_speed\dp_st_cost.h
//boundary_cost_也使用了缓存机制
std::vector<std::vector<std::pair<double, double>>> boundary_cost_;
//2个array用来缓存数据,避免多次重复计算
std::array<double, 200> accel_cost_;
std::array<double, 400> jerk_cost_;
double DpStCost::GetAccelCost(const double accel) {
double cost = 0.0;
constexpr double kEpsilon = 0.1;
constexpr size_t kShift = 100;
//将accel的数值和在accel_cost_中的index联系起来
//根据0<=10*accel+0.5+100<=200,得出程序默认accel正常在[-10,10]范围内,符合实际
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;
}
//accel_cost_初始化为-1,<0即从没有计算过
if (accel_cost_.at(accel_key) < 0.0) {
const double accel_sq = accel * accel;
...
if (accel > 0.0) {
cost = accel_penalty * accel_sq;
} else {
cost = decel_penalty * accel_sq;
}
cost += 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 {
//针对这个accel的cost早就被计算过,并保存在accel_cost_内
cost = accel_cost_.at(accel_key);
}
return cost * unit_t_;
}
DpStCost::JerkCost()的流程同理。这里提一个有趣的点。
double DpStCost::JerkCost(const double jerk) {
double cost = 0.0;
constexpr double kEpsilon = 0.1;
constexpr size_t kShift = 200;
//同上面accel cost的处理,认为jerk正常情况下在[-20,20]范围内
const size_t jerk_key = static_cast<size_t>(jerk / kEpsilon + 0.5 + kShift);
if (jerk_key >= jerk_cost_.size()) {
return kInf;
}
...
}