Apollo planning之hybrid A*

目录

1 hybrid A*的介绍

2 代码解读

3 算法细节

3.1 ValidityCheck

3.2 GenerateDpMap 

3.3 AnalyticExpansion 

3.4 CalculateNodeCost 

---

 

1 hybrid A*的介绍

hybrid_a_star的目标是在开放空间中产生粗轨迹。Hybrid_a_star包含 node3d、grid_search、reeds_shepp_path和hybrid_a_star。hybrid_a_star是产生粗轨迹并调用grid_search和reeds_shepp_path的最重要组件。注意首字母大小写

2 代码解读

输入：当前点（计划起点），目标点（计划终点），ROI_xy_boundary（x和y的最大和最小边界值），障碍物顶点矢量（顶点位置信息）。该函数如下：

     bool HybridAStar::Plan(double sx, double sy, double sphi, 
                           double ex, double ey, double ephi,
                           const std::vector& XYbounds,
                           const std::vector>& obstacles_vertices_vec,HybridAStartResult* result)

输入的 HybridAStar::Plan() 由open_space_trajectory_provider调用，请参考如下代码

OpenSpaceTrajectoryProvider::OpenSpaceTrajectoryProvider(
    const TaskConfig& config,
    const std::shared_ptr& injector)
    : TrajectoryOptimizer(config, injector) {
  open_space_trajectory_optimizer_.reset(new OpenSpaceTrajectoryOptimizer(
      config.open_space_trajectory_provider_config()
          .open_space_trajectory_optimizer_config()));
}

OpenSpaceTrajectoryProvider::~OpenSpaceTrajectoryProvider() {
  if (FLAGS_enable_open_space_planner_thread) {
    Stop();
  }
}

void OpenSpaceTrajectoryProvider::Stop() {
  if (FLAGS_enable_open_space_planner_thread) {
    is_generation_thread_stop_.store(true);
    if (thread_init_flag_) {
      task_future_.get();
    }
    trajectory_updated_.store(false);
    trajectory_error_.store(false);
    trajectory_skipped_.store(false);
    optimizer_thread_counter = 0;
  }
}

void OpenSpaceTrajectoryProvider::Restart() {
  if (FLAGS_enable_open_space_planner_thread) {
    is_generation_thread_stop_.store(true);
    if (thread_init_flag_) {
      task_future_.get();
    }
    is_generation_thread_stop_.store(false);
    thread_init_flag_ = false;
    trajectory_updated_.store(false);
    trajectory_error_.store(false);
    trajectory_skipped_.store(false);
    optimizer_thread_counter = 0;
  }
}

Status OpenSpaceTrajectoryProvider::Process() {
  ADEBUG << "trajectory provider";
  auto trajectory_data =
      frame_->mutable_open_space_info()->mutable_stitched_trajectory_result();

  // generate stop trajectory at park_and_go check_stage
  if (injector_->planning_context()
          ->mutable_planning_status()
          ->mutable_park_and_go()
          ->in_check_stage()) {
    ADEBUG << "ParkAndGo Stage Check.";
    GenerateStopTrajectory(trajectory_data);
    return Status::OK();
  }
  // Start thread when getting in Process() for the first time
  if (FLAGS_enable_open_space_planner_thread && !thread_init_flag_) {
    task_future_ = cyber::Async(
        &OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this);
    thread_init_flag_ = true;
  }
  // Get stitching trajectory from last frame
  const common::VehicleState vehicle_state = frame_->vehicle_state();
  auto* previous_frame = injector_->frame_history()->Latest();
  // Use complete raw trajectory from last frame for stitching purpose
  std::vector stitching_trajectory;
  if (!IsVehicleStopDueToFallBack(
          previous_frame->open_space_info().fallback_flag(), vehicle_state)) {
    const auto& previous_planning =
        previous_frame->open_space_info().stitched_trajectory_result();
    const auto& previous_planning_header =
        previous_frame->current_frame_planned_trajectory()
            .header()
            .timestamp_sec();
    const double planning_cycle_time = FLAGS_open_space_planning_period;
    PublishableTrajectory last_frame_complete_trajectory(
        previous_planning_header, previous_planning);
    std::string replan_reason;
    const double start_timestamp = Clock::NowInSeconds();
    stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory(
        vehicle_state, start_timestamp, planning_cycle_time,
        FLAGS_open_space_trajectory_stitching_preserved_length, false,
        &last_frame_complete_trajectory, &replan_reason);
  } else {
    ADEBUG << "Replan due to fallback stop";
    const double planning_cycle_time =
        1.0 / static_cast(FLAGS_planning_loop_rate);
    stitching_trajectory = TrajectoryStitcher::ComputeReinitStitchingTrajectory(
        planning_cycle_time, vehicle_state);
    auto* open_space_status = injector_->planning_context()
                                  ->mutable_planning_status()
                                  ->mutable_open_space();
    open_space_status->set_position_init(false);
  }
  // Get open_space_info from current frame
  const auto& open_space_info = frame_->open_space_info();

  if (FLAGS_enable_open_space_planner_thread) {
    ADEBUG << "Open space plan in multi-threads mode";

    if (is_generation_thread_stop_) {
      GenerateStopTrajectory(trajectory_data);
      return Status(ErrorCode::OK, "Parking finished");
    }

    {
      std::lock_guard lock(open_space_mutex_);
      thread_data_.stitching_trajectory = stitching_trajectory;
      thread_data_.end_pose = open_space_info.open_space_end_pose();
      thread_data_.rotate_angle = open_space_info.origin_heading();
      thread_data_.translate_origin = open_space_info.origin_point();
      thread_data_.obstacles_edges_num = open_space_info.obstacles_edges_num();
      thread_data_.obstacles_A = open_space_info.obstacles_A();
      thread_data_.obstacles_b = open_space_info.obstacles_b();
      thread_data_.obstacles_vertices_vec =
          open_space_info.obstacles_vertices_vec();
      thread_data_.XYbounds = open_space_info.ROI_xy_boundary();
      data_ready_.store(true);
    }

    // Check vehicle state
    if (IsVehicleNearDestination(
            vehicle_state, open_space_info.open_space_end_pose(),
            open_space_info.origin_heading(), open_space_info.origin_point())) {
      GenerateStopTrajectory(trajectory_data);
      is_generation_thread_stop_.store(true);
      return Status(ErrorCode::OK, "Vehicle is near to destination");
    }

    // Check if trajectory updated
    if (trajectory_updated_) {
      std::lock_guard lock(open_space_mutex_);
      LoadResult(trajectory_data);
      if (FLAGS_enable_record_debug) {
        // call merge debug ptr, open_space_trajectory_optimizer_
        auto* ptr_debug = frame_->mutable_open_space_info()->mutable_debug();
        open_space_trajectory_optimizer_->UpdateDebugInfo(
            ptr_debug->mutable_planning_data()->mutable_open_space());

        // sync debug instance
        frame_->mutable_open_space_info()->sync_debug_instance();
      }
      data_ready_.store(false);
      trajectory_updated_.store(false);
      return Status::OK();
    }

    if (trajectory_error_) {
      ++optimizer_thread_counter;
      std::lock_guard lock(open_space_mutex_);
      trajectory_error_.store(false);
      // TODO(Jinyun) Use other fallback mechanism when last iteration smoothing
      // result has out of bound pathpoint which is not allowed for next
      // iteration hybrid astar algorithm which requires start position to be
      // strictly in bound
      if (optimizer_thread_counter > 1000) {
        return Status(ErrorCode::PLANNING_ERROR,
                      "open_space_optimizer failed too many times");
      }
    }

    if (previous_frame->open_space_info().open_space_provider_success()) {
      ReuseLastFrameResult(previous_frame, trajectory_data);
      if (FLAGS_enable_record_debug) {
        // copy previous debug to current frame
        ReuseLastFrameDebug(previous_frame);
      }
      // reuse last frame debug when use last frame traj
      return Status(ErrorCode::OK,
                    "Waiting for open_space_trajectory_optimizer in "
                    "open_space_trajectory_provider");
    } else {
      GenerateStopTrajectory(trajectory_data);
      return Status(ErrorCode::OK, "Stop due to computation not finished");
    }
  } else {
    const auto& end_pose = open_space_info.open_space_end_pose();
    const auto& rotate_angle = open_space_info.origin_heading();
    const auto& translate_origin = open_space_info.origin_point();
    const auto& obstacles_edges_num = open_space_info.obstacles_edges_num();
    const auto& obstacles_A = open_space_info.obstacles_A();
    const auto& obstacles_b = open_space_info.obstacles_b();
    const auto& obstacles_vertices_vec =
        open_space_info.obstacles_vertices_vec();
    const auto& XYbounds = open_space_info.ROI_xy_boundary();

    // Check vehicle state
    if (IsVehicleNearDestination(vehicle_state, end_pose, rotate_angle,
                                 translate_origin)) {
      GenerateStopTrajectory(trajectory_data);
      return Status(ErrorCode::OK, "Vehicle is near to destination");
    }

    // Generate Trajectory;
    double time_latency;
    Status status = open_space_trajectory_optimizer_->Plan(
        stitching_trajectory, end_pose, XYbounds, rotate_angle,
        translate_origin, obstacles_edges_num, obstacles_A, obstacles_b,
        obstacles_vertices_vec, &time_latency);
    frame_->mutable_open_space_info()->set_time_latency(time_latency);

    // If status is OK, update vehicle trajectory;
    if (status == Status::OK()) {
      LoadResult(trajectory_data);
      return status;
    } else {
      return status;
    }
  }
  return Status(ErrorCode::PLANNING_ERROR);
}

void OpenSpaceTrajectoryProvider::GenerateTrajectoryThread() {
  while (!is_generation_thread_stop_) {
    if (!trajectory_updated_ && data_ready_) {
      OpenSpaceTrajectoryThreadData thread_data;
      {
        std::lock_guard lock(open_space_mutex_);
        thread_data = thread_data_;
      }
      double time_latency;
      Status status = open_space_trajectory_optimizer_->Plan(
          thread_data.stitching_trajectory, thread_data.end_pose,
          thread_data.XYbounds, thread_data.rotate_angle,
          thread_data.translate_origin, thread_data.obstacles_edges_num,
          thread_data.obstacles_A, thread_data.obstacles_b,
          thread_data.obstacles_vertices_vec, &time_latency);
      frame_->mutable_open_space_info()->set_time_latency(time_latency);
      if (status == Status::OK()) {
        std::lock_guard lock(open_space_mutex_);
        trajectory_updated_.store(true);
      } else {
        if (status.ok()) {
          std::lock_guard lock(open_space_mutex_);
          trajectory_skipped_.store(true);
        } else {
          std::lock_guard lock(open_space_mutex_);
          trajectory_error_.store(true);
        }
      }
    }
  }
}

bool OpenSpaceTrajectoryProvider::IsVehicleNearDestination(
    const common::VehicleState& vehicle_state,
    const std::vector& end_pose, double rotate_angle,
    const Vec2d& translate_origin) {
  CHECK_EQ(end_pose.size(), 4U);
  Vec2d end_pose_to_world_frame = Vec2d(end_pose[0], end_pose[1]);

  end_pose_to_world_frame.SelfRotate(rotate_angle);
  end_pose_to_world_frame += translate_origin;
  double distance_to_vehicle2 =
      std::sqrt((vehicle_state.x() - end_pose_to_world_frame.x()) *
                      (vehicle_state.x() - end_pose_to_world_frame.x()) +
                  (vehicle_state.y() - end_pose_to_world_frame.y()) *
                      (vehicle_state.y() - end_pose_to_world_frame.y()));
  double end_theta_to_world_frame = end_pose[2];
  end_theta_to_world_frame += rotate_angle;
  double distance_to_vehicle1 =
      std::sqrt((vehicle_state.x() - end_pose[0]) *
                    (vehicle_state.x() - end_pose[0]) +
                (vehicle_state.y() - end_pose[1]) *
                    (vehicle_state.y() - end_pose[1]));
  double distance_to_vehicle =
      std::sqrt((vehicle_state.x() - end_pose_to_world_frame.x()) *
                    (vehicle_state.x() - end_pose_to_world_frame.x()) +
                (vehicle_state.y() - end_pose_to_world_frame.y()) *
                    (vehicle_state.y() - end_pose_to_world_frame.y()));
  double theta_to_vehicle = std::abs(common::math::AngleDiff(
      vehicle_state.heading(), end_theta_to_world_frame));
  AERROR << "distance_to_vehicle1 is: " << distance_to_vehicle1;
  AERROR << "distance_to_vehicle2 is: " << distance_to_vehicle2;
  AERROR << "theta_to_vehicle" << theta_to_vehicle << "end_theta_to_world_frame"
         << end_theta_to_world_frame << "rotate_angle" << rotate_angle;
  AERROR << "is_near_destination_threshold"
         << config_.open_space_trajectory_provider_config()
                .open_space_trajectory_optimizer_config()
                .planner_open_space_config()
                .is_near_destination_threshold();  // which config file
  AERROR << "is_near_destination_theta_threshold"
         << config_.open_space_trajectory_provider_config()
                .open_space_trajectory_optimizer_config()
                .planner_open_space_config()
                .is_near_destination_theta_threshold();
  distance_to_vehicle = std::min(
                        std::min(distance_to_vehicle, distance_to_vehicle1),
                        distance_to_vehicle2);
  theta_to_vehicle = std::min(theta_to_vehicle,
                     std::abs(vehicle_state.heading()));
  if (distance_to_vehicle < config_.open_space_trajectory_provider_config()
                                .open_space_trajectory_optimizer_config()
                                .planner_open_space_config()
                                .is_near_destination_threshold() &&
     theta_to_vehicle < config_.open_space_trajectory_provider_config()
                             .open_space_trajectory_optimizer_config()
                             .planner_open_space_config()
                             .is_near_destination_theta_threshold()) {
    AERROR << "vehicle reach end_pose";
    frame_->mutable_open_space_info()->set_destination_reached(true);
    return true;
  }
  return false;
}

bool OpenSpaceTrajectoryProvider::IsVehicleStopDueToFallBack(
    const bool is_on_fallback, const common::VehicleState& vehicle_state) {
  if (!is_on_fallback) {
    return false;
  }
  static constexpr double kEpsilon = 1.0e-1;
  const double adc_speed = vehicle_state.linear_velocity();
  const double adc_acceleration = vehicle_state.linear_acceleration();
  if (std::abs(adc_speed) < kEpsilon && std::abs(adc_acceleration) < kEpsilon) {
    ADEBUG << "ADC stops due to fallback trajectory";
    return true;
  }
  return false;
}

void OpenSpaceTrajectoryProvider::GenerateStopTrajectory(
    DiscretizedTrajectory* const trajectory_data) {
  double relative_time = 0.0;
  // TODO(Jinyun) Move to conf
  static constexpr int stop_trajectory_length = 10;
  static constexpr double relative_stop_time = 0.1;
  static constexpr double vEpsilon = 0.00001;
  double standstill_acceleration =
      frame_->vehicle_state().linear_velocity() >= -vEpsilon
          ? -FLAGS_open_space_standstill_acceleration
          : FLAGS_open_space_standstill_acceleration;
  trajectory_data->clear();
  for (size_t i = 0; i < stop_trajectory_length; i++) {
    TrajectoryPoint point;
    point.mutable_path_point()->set_x(frame_->vehicle_state().x());
    point.mutable_path_point()->set_y(frame_->vehicle_state().y());
    point.mutable_path_point()->set_theta(frame_->vehicle_state().heading());
    point.mutable_path_point()->set_s(0.0);
    point.mutable_path_point()->set_kappa(0.0);
    point.set_relative_time(relative_time);
    point.set_v(0.0);
    point.set_a(standstill_acceleration);
    trajectory_data->emplace_back(point);
    relative_time += relative_stop_time;
  }
}

void OpenSpaceTrajectoryProvider::LoadResult(
    DiscretizedTrajectory* const trajectory_data) {
  // Load unstitched two trajectories into frame for debug
  trajectory_data->clear();
  auto optimizer_trajectory_ptr =
      frame_->mutable_open_space_info()->mutable_optimizer_trajectory_data();
  auto stitching_trajectory_ptr =
      frame_->mutable_open_space_info()->mutable_stitching_trajectory_data();
  open_space_trajectory_optimizer_->GetOptimizedTrajectory(
      optimizer_trajectory_ptr);
  open_space_trajectory_optimizer_->GetStitchingTrajectory(
      stitching_trajectory_ptr);
  // Stitch two trajectories and load back to trajectory_data from frame
  size_t optimizer_trajectory_size = optimizer_trajectory_ptr->size();
  double stitching_point_relative_time =
      stitching_trajectory_ptr->back().relative_time();
  double stitching_point_relative_s =
      stitching_trajectory_ptr->back().path_point().s();
  for (size_t i = 0; i < optimizer_trajectory_size; ++i) {
    optimizer_trajectory_ptr->at(i).set_relative_time(
        optimizer_trajectory_ptr->at(i).relative_time() +
        stitching_point_relative_time);
    optimizer_trajectory_ptr->at(i).mutable_path_point()->set_s(
        optimizer_trajectory_ptr->at(i).path_point().s() +
        stitching_point_relative_s);
  }
  *(trajectory_data) = *(optimizer_trajectory_ptr);

  // Last point in stitching trajectory is already in optimized trajectory, so
  // it is deleted
  frame_->mutable_open_space_info()
      ->mutable_stitching_trajectory_data()
      ->pop_back();
  trajectory_data->PrependTrajectoryPoints(
      frame_->open_space_info().stitching_trajectory_data());
  frame_->mutable_open_space_info()->set_open_space_provider_success(true);
}

void OpenSpaceTrajectoryProvider::ReuseLastFrameResult(
    const Frame* last_frame, DiscretizedTrajectory* const trajectory_data) {
  *(trajectory_data) =
      last_frame->open_space_info().stitched_trajectory_result();
  frame_->mutable_open_space_info()->set_open_space_provider_success(true);
}

void OpenSpaceTrajectoryProvider::ReuseLastFrameDebug(const Frame* last_frame) {
  // reuse last frame's instance
  auto* ptr_debug = frame_->mutable_open_space_info()->mutable_debug_instance();
  ptr_debug->mutable_planning_data()->mutable_open_space()->MergeFrom(
      last_frame->open_space_info()
          .debug_instance()
          .planning_data()
          .open_space());
}

}  // namespace planning
}  // namespace apollo

构造obstacles_linesegment_vector。主要方法是按顺序从单个障碍点形成线段;然后，每个障碍线段都存储在将用于生成DP地图obstacles_linesegment_vector中。

    std::vector>
    obstacles_linesegments_vec;
    for (const auto& obstacle_vertices : obstacles_vertices_vec) {
        size_t vertices_num = obstacle_vertices.size();
        std::vector obstacle_linesegments;
        for (size_t i = 0; i < vertices_num - 1; ++i) {
        common::math::LineSegment2d line_segment = common::math::LineSegment2d(
            obstacle_vertices[i], obstacle_vertices[i + 1]);
        obstacle_linesegments.emplace_back(line_segment);
        }
        obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
    }
    obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);

构造与 Node3d 相同的计划点。计划的起点和终点以 Node3d 的形式构造，该节点将保存为打开集，并由有效性Check（）函数进行检查。

    start_node_.reset(
    new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));
    end_node_.reset(
    new Node3d({ex}, {ey}, {ephi}, XYbounds_, planner_open_space_config_));

进入主 while 循环以获取一组节点。

1. 当open_pq_为空时，将生成退出轨迹。open_pq_是 std：:p riority_queue 类型，其中第一个元素表示打开集中节点的顺序，第二个元素表示按降序存储的节点的成本。

2. 使用分析扩展（）函数可以基于从当前点到目标端点reeds_shepp_path来确定是否存在无碰撞轨迹。如果存在，请退出 while 循环搜索。

     if (AnalyticExpansion(current_node)) {
         break;
     }

3. 将当前点存储在闭合集中，并根据自行车运动学模型展开下一个节点。节点数是一个参数。通过Next_node_generator（）函数生成下一个节点，并使用有效性Check（）函数检测此节点。

按节点生成粗轨迹。函数用于生成粗轨迹。

bool HybridAStar::GetResult(HybridAStartResult* result)

 输出：可选输出是部分轨迹信息，包括 x、y、phi、v、a、转向、s。

3 算法细节

3.1 ValidityCheck

bool HybridAStar::ValidityCheck(std::shared_ptr node)

检测方法基于边界范围判断和边界重叠判断。

参数：输入参数是与 node3d 相同的节点。

简介：该功能用于检查碰撞。 

处理细节：

• 边界范围判断。如果节点的 x 和 y 超出边界的相应 x 和 y 的范围，则返回 false，则返回无效。
• 边界重叠判断。如果车辆的边界框与任何线段重叠，则返回 false。通过线条和框是否相交来判断重叠。

3.2 GenerateDpMap 

bool GridSearch::GenerateDpMap(const double ex, const double ey, 
                               const std::vector& XYbounds,
                               const std::vector> &obstacles_linesegments_vec) 

 该函数用于通过动态编程生成dp映射，如下

#include "modules/planning/open_space/coarse_trajectory_generator/grid_search.h"

namespace apollo {
namespace planning {

GridSearch::GridSearch(const PlannerOpenSpaceConfig& open_space_conf) {
  xy_grid_resolution_ =
      open_space_conf.warm_start_config().grid_a_star_xy_resolution();
  node_radius_ = open_space_conf.warm_start_config().node_radius();
}

double GridSearch::EuclidDistance(const double x1, const double y1,
                                  const double x2, const double y2) {
  return std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
}

bool GridSearch::CheckConstraints(std::shared_ptr node) {
  const double node_grid_x = node->GetGridX();
  const double node_grid_y = node->GetGridY();
  if (node_grid_x > max_grid_x_ || node_grid_x < 0 ||
      node_grid_y > max_grid_y_ || node_grid_y < 0) {
    return false;
  }
  if (obstacles_linesegments_vec_.empty()) {
    return true;
  }
  for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
    for (const common::math::LineSegment2d& linesegment :
         obstacle_linesegments) {
      if (linesegment.DistanceTo({node->GetGridX(), node->GetGridY()}) <
          node_radius_) {
        return false;
      }
    }
  }
  return true;
}

std::vector> GridSearch::GenerateNextNodes(
    std::shared_ptr current_node) {
  double current_node_x = current_node->GetGridX();
  double current_node_y = current_node->GetGridY();
  double current_node_path_cost = current_node->GetPathCost();
  double diagonal_distance = std::sqrt(2.0);
  std::vector> next_nodes;
  std::shared_ptr up =
      std::make_shared(current_node_x, current_node_y + 1.0, XYbounds_);
  up->SetPathCost(current_node_path_cost + 1.0);
  std::shared_ptr up_right = std::make_shared(
      current_node_x + 1.0, current_node_y + 1.0, XYbounds_);
  up_right->SetPathCost(current_node_path_cost + diagonal_distance);
  std::shared_ptr right =
      std::make_shared(current_node_x + 1.0, current_node_y, XYbounds_);
  right->SetPathCost(current_node_path_cost + 1.0);
  std::shared_ptr down_right = std::make_shared(
      current_node_x + 1.0, current_node_y - 1.0, XYbounds_);
  down_right->SetPathCost(current_node_path_cost + diagonal_distance);
  std::shared_ptr down =
      std::make_shared(current_node_x, current_node_y - 1.0, XYbounds_);
  down->SetPathCost(current_node_path_cost + 1.0);
  std::shared_ptr down_left = std::make_shared(
      current_node_x - 1.0, current_node_y - 1.0, XYbounds_);
  down_left->SetPathCost(current_node_path_cost + diagonal_distance);
  std::shared_ptr left =
      std::make_shared(current_node_x - 1.0, current_node_y, XYbounds_);
  left->SetPathCost(current_node_path_cost + 1.0);
  std::shared_ptr up_left = std::make_shared(
      current_node_x - 1.0, current_node_y + 1.0, XYbounds_);
  up_left->SetPathCost(current_node_path_cost + diagonal_distance);

  next_nodes.emplace_back(up);
  next_nodes.emplace_back(up_right);
  next_nodes.emplace_back(right);
  next_nodes.emplace_back(down_right);
  next_nodes.emplace_back(down);
  next_nodes.emplace_back(down_left);
  next_nodes.emplace_back(left);
  next_nodes.emplace_back(up_left);
  return next_nodes;
}

bool GridSearch::GenerateAStarPath(
    const double sx, const double sy, const double ex, const double ey,
    const std::vector& XYbounds,
    const std::vector>&
        obstacles_linesegments_vec,
    GridAStartResult* result) {
  std::priority_queue,
                      std::vector>, cmp>
      open_pq;
  std::unordered_map> open_set;
  std::unordered_map> close_set;
  XYbounds_ = XYbounds;
  std::shared_ptr start_node =
      std::make_shared(sx, sy, xy_grid_resolution_, XYbounds_);
  std::shared_ptr end_node =
      std::make_shared(ex, ey, xy_grid_resolution_, XYbounds_);
  std::shared_ptr final_node_ = nullptr;
  obstacles_linesegments_vec_ = obstacles_linesegments_vec;
  open_set.emplace(start_node->GetIndex(), start_node);
  open_pq.emplace(start_node->GetIndex(), start_node->GetCost());

  // Grid a star begins
  size_t explored_node_num = 0;
  while (!open_pq.empty()) {
    std::string current_id = open_pq.top().first;
    open_pq.pop();
    std::shared_ptr current_node = open_set[current_id];
    // Check destination
    if (*(current_node) == *(end_node)) {
      final_node_ = current_node;
      break;
    }
    close_set.emplace(current_node->GetIndex(), current_node);
    std::vector> next_nodes =
        std::move(GenerateNextNodes(current_node));
    for (auto& next_node : next_nodes) {
      if (!CheckConstraints(next_node)) {
        continue;
      }
      if (close_set.find(next_node->GetIndex()) != close_set.end()) {
        continue;
      }
      if (open_set.find(next_node->GetIndex()) == open_set.end()) {
        ++explored_node_num;
        next_node->SetHeuristic(
            EuclidDistance(next_node->GetGridX(), next_node->GetGridY(),
                           end_node->GetGridX(), end_node->GetGridY()));
        next_node->SetPreNode(current_node);
        open_set.emplace(next_node->GetIndex(), next_node);
        open_pq.emplace(next_node->GetIndex(), next_node->GetCost());
      }
    }
  }

  if (final_node_ == nullptr) {
    AERROR << "Grid A searching return null ptr(open_set ran out)";
    return false;
  }
  LoadGridAStarResult(result);
  ADEBUG << "explored node num is " << explored_node_num;
  return true;
}

bool GridSearch::GenerateDpMap(
    const double ex, const double ey, const std::vector& XYbounds,
    const std::vector>&
        obstacles_linesegments_vec) {
  std::priority_queue,
                      std::vector>, cmp>
      open_pq;
  std::unordered_map> open_set;
  dp_map_ = decltype(dp_map_)();
  XYbounds_ = XYbounds;
  // XYbounds with xmin, xmax, ymin, ymax
  max_grid_y_ = std::round((XYbounds_[3] - XYbounds_[2]) / xy_grid_resolution_);
  max_grid_x_ = std::round((XYbounds_[1] - XYbounds_[0]) / xy_grid_resolution_);
  std::shared_ptr end_node =
      std::make_shared(ex, ey, xy_grid_resolution_, XYbounds_);
  obstacles_linesegments_vec_ = obstacles_linesegments_vec;
  open_set.emplace(end_node->GetIndex(), end_node);
  open_pq.emplace(end_node->GetIndex(), end_node->GetCost());

  // Grid a star begins
  size_t explored_node_num = 0;
  while (!open_pq.empty()) {
    const std::string current_id = open_pq.top().first;
    open_pq.pop();
    std::shared_ptr current_node = open_set[current_id];
    dp_map_.emplace(current_node->GetIndex(), current_node);
    std::vector> next_nodes =
        std::move(GenerateNextNodes(current_node));
    for (auto& next_node : next_nodes) {
      if (!CheckConstraints(next_node)) {
        continue;
      }
      if (dp_map_.find(next_node->GetIndex()) != dp_map_.end()) {
        continue;
      }
      if (open_set.find(next_node->GetIndex()) == open_set.end()) {
        ++explored_node_num;
        next_node->SetPreNode(current_node);
        open_set.emplace(next_node->GetIndex(), next_node);
        open_pq.emplace(next_node->GetIndex(), next_node->GetCost());
      } else {
        if (open_set[next_node->GetIndex()]->GetCost() > next_node->GetCost()) {
          open_set[next_node->GetIndex()]->SetCost(next_node->GetCost());
          open_set[next_node->GetIndex()]->SetPreNode(current_node);
        }
      }
    }
  }
  ADEBUG << "explored node num is " << explored_node_num;
  return true;
}

double GridSearch::CheckDpMap(const double sx, const double sy) {
  std::string index = Node2d::CalcIndex(sx, sy, xy_grid_resolution_, XYbounds_);
  if (dp_map_.find(index) != dp_map_.end()) {
    return dp_map_[index]->GetCost() * xy_grid_resolution_;
  } else {
    return std::numeric_limits::infinity();
  }
}

void GridSearch::LoadGridAStarResult(GridAStartResult* result) {
  (*result).path_cost = final_node_->GetPathCost() * xy_grid_resolution_;
  std::shared_ptr current_node = final_node_;
  std::vector grid_a_x;
  std::vector grid_a_y;
  while (current_node->GetPreNode() != nullptr) {
    grid_a_x.push_back(current_node->GetGridX() * xy_grid_resolution_ +
                       XYbounds_[0]);
    grid_a_y.push_back(current_node->GetGridY() * xy_grid_resolution_ +
                       XYbounds_[2]);
    current_node = current_node->GetPreNode();
  }
  std::reverse(grid_a_x.begin(), grid_a_x.end());
  std::reverse(grid_a_y.begin(), grid_a_y.end());
  (*result).x = std::move(grid_a_x);
  (*result).y = std::move(grid_a_y);
}
}  // namespace planning
}  // namespace apollo

参数：ex和ey是目标点的位置，XYbounds_是x和y的边界，obstacles_linesegments_是由边界点组成的线段。

简介：该函数用于生成dp图

处理细节：

• 根据网格分辨率对XYbounds_进行网格化，然后获取最大网格。
• Dp映射存储节点的成本。

3.3 AnalyticExpansion 

bool HybridAStar::AnalyticExpansion(std::shared_ptr current_node)

该函数用于检查分析曲线是否可以从当前配置连接到最终配置而不会发生冲突。如果是这样，搜索结束。

参数：当前节点是规划的起点。

简介：基于芦苇谢普法的函数，是一种由弧和线组成的几何算法。芦苇谢普用于搜索加速。

处理细节：

• 通过最短RSP（）函数生成芦苇舍普斯路径。长度是最佳和最短的。
• 检查路径是否由调用有效性检查（）函数的RSPCheck（）函数无冲突。
• 将整个簧片 shepp 路径作为节点加载，并将节点添加到关闭集。

3.4 CalculateNodeCost 

void HybridAStar::CalculateNodeCost(std::shared_ptr current_node,
                                    std::shared_ptr next_node)

该函数用于计算节点的成本。

参数：当前节点（车辆位置）和下一个节点。

简介：计算成本包括基于全息的考虑障碍物的轨迹成本和启发式成本。

处理细节：

• 轨迹成本包括当前节点的轨迹成本和从当前节点到下一个节点的轨迹成本。
• 轨迹成本由采样距离、它们之间的换挡和转向变化率决定。
• 启发式成本是从 dp 映射中获取的。

本文参考GENERATE COARSE TRAJECTORY IN THE OPEN SPACE — Apollo Auto 0.0.1 文档 (daobook.github.io) 若有侵权，请联系删除