Baidu Apollo代码解析之s-t图的创建与采样（path_time_graph)

最新推荐文章于 2025-06-01 09:05:03 发布

原创

最新推荐文章于 2025-06-01 09:05:03 发布 · 7.8k 阅读

45 ·

CC 4.0 BY-SA版权

文章标签：

#Apollo #无人驾驶 #运动规划 #轨迹规划 #Lattice Planner

百度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<const Obstacle*>& obstacles,
    const std::vector<PathPoint>& 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<PathPoint>& 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(
      SetPathTim