Apollo6.0代码Lattice算法详解——Part4:计算障碍物ST/SL图
Apollo6.0代码Lattice算法详解——Part4:计算障碍物ST/SL图
- 0.前置知识
- 1.涉及主要函数
- 2.函数关系
- 3.部分函数代码详解
-
- 3.1 lattice_planner.cc中代码部分
- 3.2 函数-GetReferencePoint
- 3.3 函数-GetSpeedLimitFromS
- 3.4 函数-InitCorners
- 3.5 函数-Box2d
- 3.6 函数-GetPointAtTime
- 3.7 函数-GetPathFrenetCoordinate
- 3.8 函数-ComputeObstacleBoundary
- 3.9 函数-SetStaticObstacle
- 3.10 函数-SetDynamicObstacle
- 3.11 函数-SetupObstacles
- 4.参考资料
0.前置知识
1.涉及主要函数
1) 函数名: PathTimeGraph( const std::vector
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: ST/SL图类的构造函数
2) 函数名: SetupObstacles( const std::vector
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: 构建障碍物的ST/SL图
3) 函数名: SetStaticObstacle( const Obstacle* obstacle, const std::vector< PathPoint >& discretized_ref_points)
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: 构建静态障碍物的SL图
4) 函数名: ComputeObstacleBoundary( const std::vector< common::math::Vec2d >& vertices, const std::vector< PathPoint >& discretized_ref_points)
函数位置: modules/planning/lattice/behavior/path_time_graph.cc
函数作用: 计算障碍物SL边框
5) 函数名: GetPathFrenetCoordinate( const std::vector& reference_line, const double x, const double y)
函数位置: modules/common/math/path_matcher.cc
函数作用: 计算障碍物当前点在sl坐标系中的sl_point
6) 函数名: SetDynamicObstacle( const Obstacle* obstacle, const std::vector< PathPoint >& discretized_ref_points)
函数位置: modules/common/math/path_matcher.cc
函数作用: 构建动态障碍物的ST图
7) 函数名: GetPointAtTime( const double relative_time)
函数位置: modules/planning/common/obstacle.cc
函数作用: 获得障碍物在当前时刻的TrajectoryPoint
8) 函数名: GetBoundingBox( const common::TrajectoryPoint& point)
函数位置: modules/planning/common/obstacle.cc
函数作用: 获得障碍物在当前时刻的TrajectoryPoint
9) 函数名: Box2d(const Vec2d ¢er, const double heading, const double length, const double width)
函数位置: modules/common/math/box2d.cc
函数作用: Box2d构造函数
10) 函数名: InitCorners()
函数位置: modules/common/math/box2d.cc
函数作用: 计算Box2d四个角点的笛卡尔坐标
11) 函数名: GetSpeedLimitFromS(const double s)
函数位置: modules/planning/reference_line/reference_line.cc
函数作用: 跟据当前s值返回速度限制值
12) 函数名: GetReferencePoint(const double s)
函数位置: modules/planning/reference_line/reference_line.cc
函数作用: 跟据当前s值返回参考线上的ReferencePoint
13) 函数名: InterpolateWithMatchedIndex( const ReferencePoint& p0, const double s0, const ReferencePoint& p1, const double s1, const InterpolatedIndex& index)
函数位置: modules/planning/reference_line/reference_line.cc
函数作用: 线性插值找ReferencePoint
2.函数关系
3.部分函数代码详解
3.1 lattice_planner.cc中代码部分
// 通过预测得到障碍物list
auto ptr_prediction_querier = std::make_shared<PredictionQuerier>(
frame->obstacles(), ptr_reference_line);
// 解析决策,计算障碍物的s-t图
// 4. parse the decision and get the planning target.
// 传入obs,参考线,参考线信息,自车当前点的s值,sl坐标系下规划终点的s值,轨迹的t,自车当前点的l相关值;
// 传入这么多构建一个PathTimeGraph对象,会跟据障碍物类型生成对应的ST图
auto ptr_path_time_graph = std::make_shared<PathTimeGraph>(
ptr_prediction_querier->GetObstacles(), *ptr_reference_line,
reference_line_info, init_s[0],
init_s[0] + FLAGS_speed_lon_decision_horizon, 0.0, // 200m
FLAGS_trajectory_time_length, init_d); // 8s
// 跟据init_s[0]寻找规划时的速度限制
double speed_limit =
reference_line_info->reference_line().GetSpeedLimitFromS(init_s[0]);
// 设定lattice算法的巡航速度
reference_line_info->SetLatticeCruiseSpeed(speed_limit);
// planning_target来自高精地图
PlanningTarget planning_target = reference_line_info->planning_target();
if (planning_target.has_stop_point()) {
ADEBUG << "Planning target stop s: " << planning_target.stop_point().s()
<< "Current ego s: " << init_s[0];
}
ADEBUG << "Decision_Time = "
<< (Clock::NowInSeconds() - current_time) * 1000;
current_time = Clock::NowInSeconds();
3.2 函数-GetReferencePoint
代码部分:
ReferencePoint ReferenceLine::GetReferencePoint(const double s) const {
const auto& accumulated_s = map_path_.accumulated_s();
// 如果s在参考线的后面，就返回参考线的第一个点
if (s < accumulated_s.front() - 1e-2) {
AWARN << "The requested s: " << s << " < 0.";
return reference_points_.front();
}
// 如果s在参考线的前面，就返回参考线的最后一个点
if (s > accumulated_s.back() + 1e-2) {
AWARN << "The requested s: " << s
<< " > reference line length: " << accumulated_s.back();
return reference_points_.back();
}
// 根据当前s值找到参考线上最近的点
auto interpolate_index = map_path_.GetIndexFromS(s);
size_t index = interpolate_index.id;
size_t next_index = index + 1;
if (next_index >= reference_points_.size()) {
next_index = reference_points_.size() - 1;
}
// 下面就是用线性插值找到更精确的点
const auto& p0 = reference_points_[index];
const auto& p1 = reference_points_[next_index];
const double s0 = accumulated_s[index];
const double s1 = accumulated_s[next_index];
return InterpolateWithMatchedIndex(p0, s0, p1, s1, interpolate_index);
}
3.3 函数-GetSpeedLimitFromS
代码部分:
double ReferenceLine::GetSpeedLimitFromS(const double s) const {
// 有速度限制段直接返回速度限制，该段的速度限制貌似是从高精地图获取的
for (const auto& speed_limit : speed_limit_) {
if (s >= speed_limit.start_s && s <= speed_limit.end_s) {
return speed_limit.speed_limit;
}
}
const auto& map_path_point = GetReferencePoint(s);
double speed_limit = FLAGS_planning_upper_speed_limit; // 31.3
bool speed_limit_found = false;
// 循环更新speed_limit的最小值
for (const auto& lane_waypoint : map_path_point.lane_waypoints()) {
if (lane_waypoint.lane == nullptr) {
AWARN << "lane_waypoint.lane is nullptr.";
continue;
}
speed_limit_found = true;
speed_limit =
std::fmin(lane_waypoint.lane->lane().speed_limit(), speed_limit);
}
// 如果速度限制没有找到，则通过判断当前道路的类型，赋值不同道路类型默认的速度限制值
if (!speed_limit_found) {
// use default speed limit based on road_type
speed_limit = FLAGS_default_city_road_speed_limit; // 15.67
hdmap::Road::Type road_type = GetRoadType(s);
if (road_type == hdmap::Road::HIGHWAY) {
speed_limit = FLAGS_default_highway_speed_limit; // 29.06
}
}
return speed_limit;
}
3.4 函数-InitCorners
图解:
代码部分:
void Box2d::InitCorners() {
const double dx1 = cos_heading_ * half_length_;
const double dy1 = sin_heading_ * half_length_;
const double dx2 = sin_heading_ * half_width_;
const double dy2 = -cos_heading_ * half_width_;
corners_.clear();
// 插入障碍物box四个角点的坐标
corners_.emplace_back(center_.x() + dx1 + dx2, center_.y() + dy1 + dy2);
corners_.emplace_back(center_.x() + dx1 - dx2, center_.y() + dy1 - dy2);
corners_.emplace_back(center_.x() - dx1 - dx2, center_.y() - dy1 - dy2);
corners_.emplace_back(center_.x() - dx1 + dx2, center_.y() - dy1 + dy2);
for (auto &corner : corners_) {
max_x_ = std::fmax(corner.x(), max_x_);
min_x_ = std::fmin(corner.x(), min_x_);
max_y_ = std::fmax(corner.y(), max_y_);
min_y_ = std::fmin(corner.y(), min_y_);
}
}
3.5 函数-Box2d
图解:
代码部分:
Box2d::Box2d(const Vec2d ¢er, const double heading, const double length,
const double width)
: center_(center),
length_(length),
width_(width),
half_length_(length / 2.0),
half_width_(width / 2.0),
heading_(heading),
cos_heading_(cos(heading)),
sin_heading_(sin(heading)) {
CHECK_GT(length_, -kMathEpsilon);
CHECK_GT(width_, -kMathEpsilon);
InitCorners();
}
3.6 函数-GetPointAtTime
代码部分:
common::TrajectoryPoint Obstacle::GetPointAtTime(
const double relative_time) const {
const auto& points = trajectory_.trajectory_point();
if (points.size() < 2) { // 认为是静态障碍物
common::TrajectoryPoint point;
point.mutable_path_point()->set_x(perception_obstacle_.position().x());
point.mutable_path_point()->set_y(perception_obstacle_.position().y());
point.mutable_path_point()->set_z(perception_obstacle_.position().z());
point.mutable_path_point()->set_theta(perception_obstacle_.theta());
point.mutable_path_point()->set_s(0.0);
point.mutable_path_point()->set_kappa(0.0);
point.mutable_path_point()->set_dkappa(0.0);
point.mutable_path_point()->set_ddkappa(0.0);
point.set_v(0.0);
point.set_a(0.0);
point.set_relative_time(0.0);
return point;
} else { // 认为是一个运动的障碍物
auto comp = [](const common::TrajectoryPoint p, const double time) {
return p.relative_time() < time;
};
auto it_lower =
std::lower_bound(points.begin(), points.end(), relative_time, comp);
if (it_lower == points.begin()) {
return *points.begin();
} else if (it_lower == points.end()) {
return *points.rbegin();
}
return common::math::InterpolateUsingLinearApproximation(
*(it_lower - 1), *it_lower, relative_time);
}
}
3.7 函数-GetPathFrenetCoordinate
代码部分:
std::pair<double, double> PathMatcher::GetPathFrenetCoordinate(
const std::vector<PathPoint>& reference_line, const double x,
const double y) {
// 找到障碍物x,y最近的参考线上的点
auto matched_path_point = MatchToPath(reference_line, x, y);
// 参考线点的heading
double rtheta = matched_path_point.theta();
// 参考线点的x
double rx = matched_path_point.x();
// 参考线点的y
double ry = matched_path_point.y();
double delta_x = x - rx;
double delta_y = y - ry;
// Tr X Nr
double side = std::cos(rtheta) * delta_y - std::sin(rtheta) * delta_x;
std::pair<double, double> relative_coordinate;
// 参考线点在参考线上的总长,即当前障碍物点在sl坐标系的s值
relative_coordinate.first = matched_path_point.s();
// 当前障碍物点在sl坐标系的l值(带正负)
relative_coordinate.second =
std::copysign(std::hypot(delta_x, delta_y), side);
return relative_coordinate;
}
3.8 函数-ComputeObstacleBoundary
代码部分:
// 计算障碍物边框
SLBoundary PathTimeGraph::ComputeObstacleBoundary(
const std::vector<common::math::Vec2d>& vertices,
const std::vector<PathPoint>& discretized_ref_points) const {
// 初始化操作
double start_s(std::numeric_limits<double>::max());
double end_s(std::numeric_limits<double>::lowest());
double start_l(std::numeric_limits<double>::max());
double end_l(std::numeric_limits<double>::lowest());
for (const auto& point : vertices) {
// 获得障碍物当前点在sl坐标系中的sl_point
auto sl_point = PathMatcher::GetPathFrenetCoordinate(discretized_ref_points,
point.x(), point.y());
// 通过下面的操作，可以使得最后获得sl的start和end
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);
}
/
// The start_s and end_s are longitudinal values.
// start_s <= end_s.
//
// end_s
// ^
// |
// S direction
// |
// start_s
//
// The start_l and end_l are lateral values.
// start_l <= end_l. Left side of the reference line is positive,
// and right side of the reference line is negative.
// end_l <-----L direction---- start_l
/
// 得到障碍物的在SL坐标系中的boundary
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;
}
3.9 函数-SetStaticObstacle
图解:
代码部分:
void PathTimeGraph::SetStaticObstacle(
const Obstacle* obstacle,
const std::vector<PathPoint>& discretized_ref_points) {
// 获得障碍物边框
const Polygon2d& polygon = obstacle->PerceptionPolygon();
// 障碍物ID
std::string obstacle_id = obstacle->Id();
// 得到障碍物的在SL坐标系中的boundary
SLBoundary sl_boundary =
ComputeObstacleBoundary(polygon.GetAllVertices(), discretized_ref_points);
// 宽度 = 参考线宽度的一半
double left_width = FLAGS_default_reference_line_width * 0.5; // 4m
double right_width = FLAGS_default_reference_line_width * 0.5;
//
ptr_reference_line_info_->reference_line().GetLaneWidth(
sl_boundary.start_s(), &left_width, &right_width);
// 忽略一些车道之外和参考线范围之外的障碍物
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) { // 即障碍物所在位置在右侧车道之外
ADEBUG << "Obstacle [" << obstacle_id << "] is out of range.";
return;
}
// 障碍物ID
path_time_obstacle_map_[obstacle_id].set_id(obstacle_id);
// 障碍物SL图的左下角点（start_s,0）
path_time_obstacle_map_[obstacle_id].set_bottom_left_point(
SetPathTimePoint(obstacle_id, sl_boundary.start_s(), 0.0));
// 障碍物SL图的右下角点（start_s,8）
path_time_obstacle_map_[obstacle_id].set_bottom_right_point(SetPathTimePoint(
obstacle_id, sl_boundary.start_s(), FLAGS_trajectory_time_length));
// 障碍物SL图的左上角点（end_s,0）
path_time_obstacle_map_[obstacle_id].set_upper_left_point(
SetPathTimePoint(obstacle_id, sl_boundary.end_s(), 0.0));
// 障碍物SL图的右上角点（end_s,8）
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));
ADEBUG << "ST-Graph mapping static obstacle: " << obstacle_id
<< ", start_s : " << sl_boundary.start_s()
<< ", end_s : " << sl_boundary.end_s()
<< ", start_l : " << sl_boundary.start_l()
<< ", end_l : " << sl_boundary.end_l();
}
3.10 函数-SetDynamicObstacle
图解:
代码部分:
void PathTimeGraph::SetDynamicObstacle(
const Obstacle* obstacle,
const std::vector<PathPoint>& discretized_ref_points) {
double relative_time = time_range_.first; // 0s
// 循环遍历轨迹中的每一个点，每个点画一个ST图，最后把所有ST图组合起来，形成动态障碍物的ST图
while (relative_time < time_range_.second) { // 8s
// 获得障碍物轨迹点
TrajectoryPoint point = obstacle->GetPointAtTime(relative_time);
Box2d box = obstacle->GetBoundingBox(point);
SLBoundary sl_boundary =
ComputeObstacleBoundary(box.GetAllCorners(), discretized_ref_points);
double left_width = FLAGS_default_reference_line_width * 0.5;
double right_width = FLAGS_default_reference_line_width * 0.5;
ptr_reference_line_info_->reference_line().GetLaneWidth(
sl_boundary.start_s(), &left_width, &right_width);
// The obstacle is not shown on the region to be considered.
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) {
// 如果该障碍物的ID在障碍物的map里，说明该障碍物已经处理过了，直接退出当前while循环
if (path_time_obstacle_map_.find(obstacle->Id()) !=
path_time_obstacle_map_.end()) {
break;
}
relative_time += FLAGS_trajectory_time_resolution;
continue;
}
// 如果该障碍物的ID不在障碍物的map里，添加相关信息
if (path_time_obstacle_map_.find(obstacle->Id()) ==
path_time_obstacle_map_.end()) {
// 记录id
path_time_obstacle_map_[obstacle->Id()].set_id(obstacle->Id());
// 这个if里记录的是ST图的左边边框,如果是第一次注册的话,因为第一次注册需要知道是从哪里开始的
// 记录左下
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));
}
// 这里会每次都更新障碍物ST图的右侧边界,等于说每次都更新障碍物在ST图上的终点,即结束
// 右下
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;
}
}
3.11 函数-SetupObstacles
代码部分:
// 实际上是在做障碍物的ST/SL图
void PathTimeGraph::SetupObstacles(
const std::vector<const Obstacle*>& obstacles,
const std::vector<PathPoint>& discretized_ref_points) {
// 遍历所有的障碍物，去生成对应的ST图
for (const Obstacle* obstacle : obstacles) {
// 判断是不是虚拟的obs
if (obstacle->IsVirtual()) {
continue;
}
// 通过判断障碍物是否有轨迹来判断是 静态障碍物 还是 动态障碍物
if (!obstacle->HasTrajectory()) {
SetStaticObstacle(obstacle, discretized_ref_points);
} else {
SetDynamicObstacle(obstacle, discretized_ref_points);
}
}
// 静态障碍物按start_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();
});
// 所有障碍物的ST信息则全部压入path_time_obstacles_中实现存储。
for (auto& path_time_obstacle : path_time_obstacle_map_) {
path_time_obstacles_.push_back(path_time_obstacle.second);
}
}
4.参考资料
1.参考视频：Lattice.
2.参考资料：Lattice Planner从学习到放弃（一）.额不…到实践.