Baidu Apollo代码解析之s-t图的创建与采样（path_time_graph)
百度Apollo中使用Frenet坐标系下的s-t图来帮助分析障碍物的状态（静态、动态）对于自车轨迹规划的影响。典型的如Lattice Planner中，在s-t图中绘制出障碍物占据的s-t区间范围，便可以在范围外采样无碰撞的轨迹点（end condition)，然后拟合和评估轨迹，选取最优的输出。
在s-t图中，静态障碍物是矩形块表示，因为直观的，随着时间推移，其s轴位置始终不变；动态障碍物是四边形表示（如果是匀速运动，则是平行四边形），因为随着时间推移，障碍物的s轴位置不断朝着s轴正方向移动，且斜率就是移动的速度。
当感知模块输出检测到的障碍物、预测模块输出预测的n秒内的障碍物可能轨迹后，应该如何创建s-t图呢？path_time_graph类就是解决这个问题的。文件路径：apollo\modules\planning\lattice\behavior\path_time_graph.cc。原理比较简单，就直接贴代码和注释了。
PathTimeGraph::PathTimeGraph(...) {
...
//在构造函数中，给S-T graph添加所有的动静态障碍物
SetupObstacles(obstacles, discretized_ref_points);
}void PathTimeGraph::SetupObstacles(
const std::vector& obstacles,
const std::vector& discretized_ref_points) {
for (const Obstacle* obstacle : obstacles) {
if (obstacle->IsVirtual()) {
continue;
}
if (!obstacle->HasTrajectory()) {
//plot a rectangle on s-t graph, representing the static obstacle
SetStaticObstacle(obstacle, discretized_ref_points);
} else {
//plot a parallelogram on s-t graph, representing a dynamic obstacle
SetDynamicObstacle(obstacle, discretized_ref_points);
}
}
//对静态障碍物的占据范围，按s坐标升序排列
std::sort(static_obs_sl_boundaries_.begin(), static_obs_sl_boundaries_.end(),
[](const SLBoundary& sl0, const SLBoundary& sl1) {
return sl0.start_s() < sl1.start_s();
});
for (auto& path_time_obstacle : path_time_obstacle_map_) {
path_time_obstacles_.push_back(path_time_obstacle.second);
}
} /**
* @brief plot a rectangle on s-t graph, representing the static obstacle
* @param obstacle
* @param discretized_ref_points, used to transform the obstacle's polygon(in x-y) to frenet
*/
void PathTimeGraph::SetStaticObstacle(
const Obstacle* obstacle,
const std::vector& discretized_ref_points) {
const Polygon2d& polygon = obstacle->PerceptionPolygon();
std::string obstacle_id = obstacle->Id();
//get the start and end of s&l direction, polygon is in x-y,
//so reference line is used to transform the polygon to frenet
SLBoundary sl_boundary =
ComputeObstacleBoundary(polygon.GetAllVertices(), discretized_ref_points);
...
//check if the obstacle is in lane, if not in lane, ignore it
...
//plot a static obstacle's occupancy graph on s-t graph, actually a rectangle,
//so it's ok if the 4 corners are confirmed
path_time_obstacle_map_[obstacle_id].set_id(obstacle_id);
path_time_obstacle_map_[obstacle_id].set_bottom_left_point(
SetPathTimePoint(obstacle_id, sl_boundary.start_s(), 0.0));
path_time_obstacle_map_[obstacle_id].set_bottom_right_point(SetPathTimePoint(
obstacle_id, sl_boundary.start_s(), FLAGS_trajectory_time_length));
path_time_obstacle_map_[obstacle_id].set_upper_left_point(
SetPathTimePoint(obstacle_id, sl_boundary.end_s(), 0.0));
path_time_obstacle_map_[obstacle_id].set_upper_right_point(SetPathTimePoint(
obstacle_id, sl_boundary.end_s(), FLAGS_trajectory_time_length));
static_obs_sl_boundaries_.push_back(std::move(sl_boundary));
}
/**
* @brief Compute obstacle's start and end coordinate in both s and l direction
* @param vertices
* @param discretized_ref_points
* @return boundary with start and end coordinate in both s and l direction,
* without boundary points
*/
SLBoundary PathTimeGraph::ComputeObstacleBoundary(
const std::vector& vertices,
const std::vector& discretized_ref_points) const {
double start_s(std::numeric_limits::max());
double end_s(std::numeric_limits::lowest());
double start_l(std::numeric_limits::max());
double end_l(std::numeric_limits::lowest());
for (const auto& point : vertices) {
auto sl_point = PathMatcher::GetPathFrenetCoordinate(discretized_ref_points,
point.x(), point.y());
start_s = std::fmin(start_s, sl_point.first);
end_s = std::fmax(end_s, sl_point.first);
start_l = std::fmin(start_l, sl_point.second);
end_l = std::fmax(end_l, sl_point.second);
}
SLBoundary sl_boundary;
sl_boundary.set_start_s(start_s);
sl_boundary.set_end_s(end_s);
sl_boundary.set_start_l(start_l);
sl_boundary.set_end_l(end_l);
return sl_boundary;
} /**
* @brief plot a parallelogram on s-t graph, representing a dynamic obstacle
* @param obstacle
* @param discretized_ref_points
*/
void PathTimeGraph::SetDynamicObstacle(
const Obstacle* obstacle,
const std::vector& discretized_ref_points) {
double relative_time = time_range_.first;
while (relative_time < time_range_.second) {
TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
Box2d box = obstacle->GetBoundingBox(point);
SLBoundary sl_boundary =
ComputeObstacleBoundary(box.GetAllCorners(), discretized_ref_points);
...
// The obstacle is not shown on the region to be considered.
// the obstacle is not in lane, out of the s-t graph region
if (sl_boundary.start_s() > path_range_.second ||
sl_boundary.end_s() < path_range_.first ||
sl_boundary.start_l() > left_width ||
sl_boundary.end_l() < -right_width) {
//if the obstacle was in the obstacle_map_ before, now it disappear or go to another lane,
//break and return, so plotting parallelogram is done
if (path_time_obstacle_map_.find(obstacle->Id()) !=
path_time_obstacle_map_.end()) {
break;
}
//if the obstacle has never been observed in the obstacle_map_ before,
//time added to see it would appear or not later
relative_time += FLAGS_trajectory_time_resolution;
continue;
}
//if the obstacle is the first time to be observed, add it to the obstacle_map_,
//this executes only once
if (path_time_obstacle_map_.find(obstacle->Id()) ==
path_time_obstacle_map_.end()) {
path_time_obstacle_map_[obstacle->Id()].set_id(obstacle->Id());
path_time_obstacle_map_[obstacle->Id()].set_bottom_left_point(
SetPathTimePoint(obstacle->Id(), sl_boundary.start_s(),
relative_time));
path_time_obstacle_map_[obstacle->Id()].set_upper_left_point(
SetPathTimePoint(obstacle->Id(), sl_boundary.end_s(), relative_time));
}
//plot a parallelogram on s-t graph, representing a dynamic obstacle
path_time_obstacle_map_[obstacle->Id()].set_bottom_right_point(
SetPathTimePoint(obstacle->Id(), sl_boundary.start_s(), relative_time));
path_time_obstacle_map_[obstacle->Id()].set_upper_right_point(
SetPathTimePoint(obstacle->Id(), sl_boundary.end_s(), relative_time));
relative_time += FLAGS_trajectory_time_resolution;
}
} //获取所有时刻、所有障碍物的占据范围（即外层vector）
//内层vector是某个时刻、所有障碍物的占据范围
std::vector>>
PathTimeGraph::GetPathBlockingIntervals(const double t_start,
const double t_end,
const double t_resolution) {
std::vector>> intervals;
for (double t = t_start; t <= t_end; t += t_resolution) {
intervals.push_back(GetPathBlockingIntervals(t));
}
return intervals;
} /**
* @brief Given time t, check obstacles' occupied s region
* @param t
* @return
*/
std::vector> PathTimeGraph::GetPathBlockingIntervals(
const double t) const {
CHECK(time_range_.first <= t && t <= time_range_.second);
std::vector> intervals;
for (const auto& pt_obstacle : path_time_obstacles_) {
if (t > pt_obstacle.max_t() || t < pt_obstacle.min_t()) {
continue;
}
//lerp() executing a linear interpolation, reveals that the constant s velocity is assumed
double s_upper = lerp(pt_obstacle.upper_left_point().s(),
pt_obstacle.upper_left_point().t(),
pt_obstacle.upper_right_point().s(),
pt_obstacle.upper_right_point().t(), t);
double s_lower = lerp(pt_obstacle.bottom_left_point().s(),
pt_obstacle.bottom_left_point().t(),
pt_obstacle.bottom_right_point().s(),
pt_obstacle.bottom_right_point().t(), t);
intervals.emplace_back(s_lower, s_upper);
}
return intervals;
}
/**
* @brief Sampling expanded boundary points around obstacle, below or above the obstacle
* @param obstacle_id
* @param s_dist a very small value, keep the sample points close to obstacle but not overlapped,
* also control sampling below or above the obstacle
* @param t_min_density time sample interval
* @return sample points upper or lower than obstacle
*/
std::vector PathTimeGraph::GetObstacleSurroundingPoints(
const std::string& obstacle_id, const double s_dist,
const double t_min_density) const {
CHECK(t_min_density > 0.0);
std::vector pt_pairs;
//if the obstacle never show up in obstacle_map_, then return null
if (path_time_obstacle_map_.find(obstacle_id) ==
path_time_obstacle_map_.end()) {
return pt_pairs;
}
const auto& pt_obstacle = path_time_obstacle_map_.at(obstacle_id);
...
if (s_dist > 0.0) {
//sampling above the obstacle
s0 = pt_obstacle.upper_left_point().s();
s1 = pt_obstacle.upper_right_point().s();
t0 = pt_obstacle.upper_left_point().t();
t1 = pt_obstacle.upper_right_point().t();
} else {
//sampling below the obstacle
s0 = pt_obstacle.bottom_left_point().s();
s1 = pt_obstacle.bottom_right_point().s();
t0 = pt_obstacle.bottom_left_point().t();
t1 = pt_obstacle.bottom_right_point().t();
}
...
for (size_t i = 0; i <= num_sections; ++i) {
double t = t_interval * static_cast(i) + t0;
// add s_dist to ensure the sample point don't locate in the obstacle
double s = lerp(s0, t0, s1, t1, t) + s_dist;
STPoint ptt;
ptt.set_t(t);
ptt.set_s(s);
pt_pairs.push_back(std::move(ptt));
}
return pt_pairs;
} /**
* @brief get lateral bounds along the static obstacles s range
* @param s_start static obstacles' start s
* @param s_end static obstacles' end s
* @param s_resolution
* @return
*/
std::vector> PathTimeGraph::GetLateralBounds(
const double s_start, const double s_end, const double s_resolution) {
...
std::vector> bounds;
std::vector discretized_path;
double s_range = s_end - s_start;
double s_curr = s_start;
size_t num_bound = static_cast(s_range / s_resolution);
...
// Initialize bounds by reference line width
for (size_t i = 0; i < num_bound; ++i) {
double left_width = FLAGS_default_reference_line_width / 2.0;
double right_width = FLAGS_default_reference_line_width / 2.0;
ptr_reference_line_info_->reference_line().GetLaneWidth(s_curr, &left_width,
&right_width);
double ego_d_lower = init_d_[0] - ego_width / 2.0;
double ego_d_upper = init_d_[0] + ego_width / 2.0;
//bounds = [right bound, left bound]
bounds.emplace_back(
std::min(-right_width, ego_d_lower - FLAGS_bound_buffer),
std::max(left_width, ego_d_upper + FLAGS_bound_buffer));
discretized_path.push_back(s_curr);
s_curr += s_resolution;
}
for (const SLBoundary& static_sl_boundary : static_obs_sl_boundaries_) {
UpdateLateralBoundsByObstacle(static_sl_boundary, discretized_path, s_start,
s_end, &bounds);
}
for (size_t i = 0; i < bounds.size(); ++i) {
//take the ego width into consideration, the vehiche shouldn't collide with lane edge
bounds[i].first += ego_width / 2.0;
bounds[i].second -= ego_width / 2.0;
//invalid if right bound > left bound
if (bounds[i].first >= bounds[i].second) {
bounds[i].first = 0.0;
bounds[i].second = 0.0;
}
}
return bounds;
} /**
* @brief adjust lateral bounds based on obstacles' relative position to lane center line
* @param sl_boundary static obstacles' boundary(start and end of s&l)
* @param discretized_path path's s coordinate
* @param s_start
* @param s_end
* @param bounds lateral bounds, initialized by lane width(roughly)
*/
void PathTimeGraph::UpdateLateralBoundsByObstacle(
const SLBoundary& sl_boundary, const std::vector& discretized_path,
const double s_start, const double s_end,
std::vector>* const bounds) {
if (sl_boundary.start_s() > s_end || sl_boundary.end_s() < s_start) {
return;
}
auto start_iter = std::lower_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
auto end_iter = std::upper_bound(
discretized_path.begin(), discretized_path.end(), sl_boundary.start_s());
size_t start_index = start_iter - discretized_path.begin();
size_t end_index = end_iter - discretized_path.begin();
//obstacle locate at the center of the lane(reference line)
if (sl_boundary.end_l() > -FLAGS_lattice_epsilon &&
sl_boundary.start_l() < FLAGS_lattice_epsilon) {
for (size_t i = start_index; i < end_index; ++i) {
bounds->operator[](i).first = -FLAGS_lattice_epsilon;
bounds->operator[](i).second = FLAGS_lattice_epsilon;
}
return;
}
//obstacle locate at the right(negative) of the lane(reference line)
if (sl_boundary.end_l() < FLAGS_lattice_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
//adjust right bound to be more left, left nudge, but in the lane, don't change lane
bounds->operator[](i).first =
std::max(bounds->operator[](i).first,
sl_boundary.end_l() + FLAGS_nudge_buffer);
}
return;
}
//obstacle locate at the left(positive) of the lane(reference line)
if (sl_boundary.start_l() > -FLAGS_lattice_epsilon) {
for (size_t i = start_index; i < std::min(end_index + 1, bounds->size());
++i) {
//adjust left bound to be more right, right nudge, but in the lane, don't change lane
bounds->operator[](i).second =
std::min(bounds->operator[](i).second,
sl_boundary.start_l() - FLAGS_nudge_buffer);
}
return;
}
}