devbox
AllinToyou
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

Apollo planning之hybrid A*

作者：AllinToyou | 2024-05-17 04:22:38

赞

踩

hybrid a*

欢迎大家关注我的B站：

偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com)

目录

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的最大和最小边界值），障碍物顶点矢量（顶点位置信息）。该函数如下：

  1.      bool HybridAStar::Plan(double sx, double sy, double sphi, 
  2.                            double ex, double ey, double ephi,
  3.                            const std::vector<double>& XYbounds,
  4.                            const std::vector<std::vector<common::math::Vec2d>>& obstacles_vertices_vec,HybridAStartResult* result)

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

  1. OpenSpaceTrajectoryProvider::OpenSpaceTrajectoryProvider(
  2.     const TaskConfig& config,
  3.     const std::shared_ptr<DependencyInjector>& injector)
  4.     : TrajectoryOptimizer(config, injector) {
  5.   open_space_trajectory_optimizer_.reset(new OpenSpaceTrajectoryOptimizer(
  6.       config.open_space_trajectory_provider_config()
  7.           .open_space_trajectory_optimizer_config()));
  8. }
  9.  
  10. OpenSpaceTrajectoryProvider::~OpenSpaceTrajectoryProvider() {
  11.   if (FLAGS_enable_open_space_planner_thread) {
  12.     Stop();
  13.   }
  14. }
  15.  
  16. void OpenSpaceTrajectoryProvider::Stop() {
  17.   if (FLAGS_enable_open_space_planner_thread) {
  18.     is_generation_thread_stop_.store(true);
  19.     if (thread_init_flag_) {
  20.       task_future_.get();
  21.     }
  22.     trajectory_updated_.store(false);
  23.     trajectory_error_.store(false);
  24.     trajectory_skipped_.store(false);
  25.     optimizer_thread_counter = 0;
  26.   }
  27. }
  28.  
  29. void OpenSpaceTrajectoryProvider::Restart() {
  30.   if (FLAGS_enable_open_space_planner_thread) {
  31.     is_generation_thread_stop_.store(true);
  32.     if (thread_init_flag_) {
  33.       task_future_.get();
  34.     }
  35.     is_generation_thread_stop_.store(false);
  36.     thread_init_flag_ = false;
  37.     trajectory_updated_.store(false);
  38.     trajectory_error_.store(false);
  39.     trajectory_skipped_.store(false);
  40.     optimizer_thread_counter = 0;
  41.   }
  42. }
  43.  
  44. Status OpenSpaceTrajectoryProvider::Process() {
  45.   ADEBUG << "trajectory provider";
  46.   auto trajectory_data =
  47.       frame_->mutable_open_space_info()->mutable_stitched_trajectory_result();
  48.  
  49.   // generate stop trajectory at park_and_go check_stage
  50.   if (injector_->planning_context()
  51.           ->mutable_planning_status()
  52.           ->mutable_park_and_go()
  53.           ->in_check_stage()) {
  54.     ADEBUG << "ParkAndGo Stage Check.";
  55.     GenerateStopTrajectory(trajectory_data);
  56.     return Status::OK();
  57.   }
  58.   // Start thread when getting in Process() for the first time
  59.   if (FLAGS_enable_open_space_planner_thread && !thread_init_flag_) {
  60.     task_future_ = cyber::Async(
  61.         &OpenSpaceTrajectoryProvider::GenerateTrajectoryThread, this);
  62.     thread_init_flag_ = true;
  63.   }
  64.   // Get stitching trajectory from last frame
  65.   const common::VehicleState vehicle_state = frame_->vehicle_state();
  66.   auto* previous_frame = injector_->frame_history()->Latest();
  67.   // Use complete raw trajectory from last frame for stitching purpose
  68.   std::vector<TrajectoryPoint> stitching_trajectory;
  69.   if (!IsVehicleStopDueToFallBack(
  70.           previous_frame->open_space_info().fallback_flag(), vehicle_state)) {
  71.     const auto& previous_planning =
  72.         previous_frame->open_space_info().stitched_trajectory_result();
  73.     const auto& previous_planning_header =
  74.         previous_frame->current_frame_planned_trajectory()
  75.             .header()
  76.             .timestamp_sec();
  77.     const double planning_cycle_time = FLAGS_open_space_planning_period;
  78.     PublishableTrajectory last_frame_complete_trajectory(
  79.         previous_planning_header, previous_planning);
  80.     std::string replan_reason;
  81.     const double start_timestamp = Clock::NowInSeconds();
  82.     stitching_trajectory = TrajectoryStitcher::ComputeStitchingTrajectory(
  83.         vehicle_state, start_timestamp, planning_cycle_time,
  84.         FLAGS_open_space_trajectory_stitching_preserved_length, false,
  85.         &last_frame_complete_trajectory, &replan_reason);
  86.   } else {
  87.     ADEBUG << "Replan due to fallback stop";
  88.     const double planning_cycle_time =
  89.         1.0 / static_cast<double>(FLAGS_planning_loop_rate);
  90.     stitching_trajectory = TrajectoryStitcher::ComputeReinitStitchingTrajectory(
  91.         planning_cycle_time, vehicle_state);
  92.     auto* open_space_status = injector_->planning_context()
  93.                                   ->mutable_planning_status()
  94.                                   ->mutable_open_space();
  95.     open_space_status->set_position_init(false);
  96.   }
  97.   // Get open_space_info from current frame
  98.   const auto& open_space_info = frame_->open_space_info();
  99.  
  100.   if (FLAGS_enable_open_space_planner_thread) {
  101.     ADEBUG << "Open space plan in multi-threads mode";
  102.  
  103.     if (is_generation_thread_stop_) {
  104.       GenerateStopTrajectory(trajectory_data);
  105.       return Status(ErrorCode::OK, "Parking finished");
  106.     }
  107.  
  108.     {
  109.       std::lock_guard<std::mutex> lock(open_space_mutex_);
  110.       thread_data_.stitching_trajectory = stitching_trajectory;
  111.       thread_data_.end_pose = open_space_info.open_space_end_pose();
  112.       thread_data_.rotate_angle = open_space_info.origin_heading();
  113.       thread_data_.translate_origin = open_space_info.origin_point();
  114.       thread_data_.obstacles_edges_num = open_space_info.obstacles_edges_num();
  115.       thread_data_.obstacles_A = open_space_info.obstacles_A();
  116.       thread_data_.obstacles_b = open_space_info.obstacles_b();
  117.       thread_data_.obstacles_vertices_vec =
  118.           open_space_info.obstacles_vertices_vec();
  119.       thread_data_.XYbounds = open_space_info.ROI_xy_boundary();
  120.       data_ready_.store(true);
  121.     }
  122.  
  123.     // Check vehicle state
  124.     if (IsVehicleNearDestination(
  125.             vehicle_state, open_space_info.open_space_end_pose(),
  126.             open_space_info.origin_heading(), open_space_info.origin_point())) {
  127.       GenerateStopTrajectory(trajectory_data);
  128.       is_generation_thread_stop_.store(true);
  129.       return Status(ErrorCode::OK, "Vehicle is near to destination");
  130.     }
  131.  
  132.     // Check if trajectory updated
  133.     if (trajectory_updated_) {
  134.       std::lock_guard<std::mutex> lock(open_space_mutex_);
  135.       LoadResult(trajectory_data);
  136.       if (FLAGS_enable_record_debug) {
  137.         // call merge debug ptr, open_space_trajectory_optimizer_
  138.         auto* ptr_debug = frame_->mutable_open_space_info()->mutable_debug();
  139.         open_space_trajectory_optimizer_->UpdateDebugInfo(
  140.             ptr_debug->mutable_planning_data()->mutable_open_space());
  141.  
  142.         // sync debug instance
  143.         frame_->mutable_open_space_info()->sync_debug_instance();
  144.       }
  145.       data_ready_.store(false);
  146.       trajectory_updated_.store(false);
  147.       return Status::OK();
  148.     }
  149.  
  150.     if (trajectory_error_) {
  151.       ++optimizer_thread_counter;
  152.       std::lock_guard<std::mutex> lock(open_space_mutex_);
  153.       trajectory_error_.store(false);
  154.       // TODO(Jinyun) Use other fallback mechanism when last iteration smoothing
  155.       // result has out of bound pathpoint which is not allowed for next
  156.       // iteration hybrid astar algorithm which requires start position to be
  157.       // strictly in bound
  158.       if (optimizer_thread_counter > 1000) {
  159.         return Status(ErrorCode::PLANNING_ERROR,
  160.                       "open_space_optimizer failed too many times");
  161.       }
  162.     }
  163.  
  164.     if (previous_frame->open_space_info().open_space_provider_success()) {
  165.       ReuseLastFrameResult(previous_frame, trajectory_data);
  166.       if (FLAGS_enable_record_debug) {
  167.         // copy previous debug to current frame
  168.         ReuseLastFrameDebug(previous_frame);
  169.       }
  170.       // reuse last frame debug when use last frame traj
  171.       return Status(ErrorCode::OK,
  172.                     "Waiting for open_space_trajectory_optimizer in "
  173.                     "open_space_trajectory_provider");
  174.     } else {
  175.       GenerateStopTrajectory(trajectory_data);
  176.       return Status(ErrorCode::OK, "Stop due to computation not finished");
  177.     }
  178.   } else {
  179.     const auto& end_pose = open_space_info.open_space_end_pose();
  180.     const auto& rotate_angle = open_space_info.origin_heading();
  181.     const auto& translate_origin = open_space_info.origin_point();
  182.     const auto& obstacles_edges_num = open_space_info.obstacles_edges_num();
  183.     const auto& obstacles_A = open_space_info.obstacles_A();
  184.     const auto& obstacles_b = open_space_info.obstacles_b();
  185.     const auto& obstacles_vertices_vec =
  186.         open_space_info.obstacles_vertices_vec();
  187.     const auto& XYbounds = open_space_info.ROI_xy_boundary();
  188.  
  189.     // Check vehicle state
  190.     if (IsVehicleNearDestination(vehicle_state, end_pose, rotate_angle,
  191.                                  translate_origin)) {
  192.       GenerateStopTrajectory(trajectory_data);
  193.       return Status(ErrorCode::OK, "Vehicle is near to destination");
  194.     }
  195.  
  196.     // Generate Trajectory;
  197.     double time_latency;
  198.     Status status = open_space_trajectory_optimizer_->Plan(
  199.         stitching_trajectory, end_pose, XYbounds, rotate_angle,
  200.         translate_origin, obstacles_edges_num, obstacles_A, obstacles_b,
  201.         obstacles_vertices_vec, &time_latency);
  202.     frame_->mutable_open_space_info()->set_time_latency(time_latency);
  203.  
  204.     // If status is OK, update vehicle trajectory;
  205.     if (status == Status::OK()) {
  206.       LoadResult(trajectory_data);
  207.       return status;
  208.     } else {
  209.       return status;
  210.     }
  211.   }
  212.   return Status(ErrorCode::PLANNING_ERROR);
  213. }
  214.  
  215. void OpenSpaceTrajectoryProvider::GenerateTrajectoryThread() {
  216.   while (!is_generation_thread_stop_) {
  217.     if (!trajectory_updated_ && data_ready_) {
  218.       OpenSpaceTrajectoryThreadData thread_data;
  219.       {
  220.         std::lock_guard<std::mutex> lock(open_space_mutex_);
  221.         thread_data = thread_data_;
  222.       }
  223.       double time_latency;
  224.       Status status = open_space_trajectory_optimizer_->Plan(
  225.           thread_data.stitching_trajectory, thread_data.end_pose,
  226.           thread_data.XYbounds, thread_data.rotate_angle,
  227.           thread_data.translate_origin, thread_data.obstacles_edges_num,
  228.           thread_data.obstacles_A, thread_data.obstacles_b,
  229.           thread_data.obstacles_vertices_vec, &time_latency);
  230.       frame_->mutable_open_space_info()->set_time_latency(time_latency);
  231.       if (status == Status::OK()) {
  232.         std::lock_guard<std::mutex> lock(open_space_mutex_);
  233.         trajectory_updated_.store(true);
  234.       } else {
  235.         if (status.ok()) {
  236.           std::lock_guard<std::mutex> lock(open_space_mutex_);
  237.           trajectory_skipped_.store(true);
  238.         } else {
  239.           std::lock_guard<std::mutex> lock(open_space_mutex_);
  240.           trajectory_error_.store(true);
  241.         }
  242.       }
  243.     }
  244.   }
  245. }
  246.  
  247. bool OpenSpaceTrajectoryProvider::IsVehicleNearDestination(
  248.     const common::VehicleState& vehicle_state,
  249.     const std::vector<double>& end_pose, double rotate_angle,
  250.     const Vec2d& translate_origin) {
  251.   CHECK_EQ(end_pose.size(), 4U);
  252.   Vec2d end_pose_to_world_frame = Vec2d(end_pose[0], end_pose[1]);
  253.  
  254.   end_pose_to_world_frame.SelfRotate(rotate_angle);
  255.   end_pose_to_world_frame += translate_origin;
  256.   double distance_to_vehicle2 =
  257.       std::sqrt((vehicle_state.x() - end_pose_to_world_frame.x()) *
  258.                       (vehicle_state.x() - end_pose_to_world_frame.x()) +
  259.                   (vehicle_state.y() - end_pose_to_world_frame.y()) *
  260.                       (vehicle_state.y() - end_pose_to_world_frame.y()));
  261.   double end_theta_to_world_frame = end_pose[2];
  262.   end_theta_to_world_frame += rotate_angle;
  263.   double distance_to_vehicle1 =
  264.       std::sqrt((vehicle_state.x() - end_pose[0]) *
  265.                     (vehicle_state.x() - end_pose[0]) +
  266.                 (vehicle_state.y() - end_pose[1]) *
  267.                     (vehicle_state.y() - end_pose[1]));
  268.   double distance_to_vehicle =
  269.       std::sqrt((vehicle_state.x() - end_pose_to_world_frame.x()) *
  270.                     (vehicle_state.x() - end_pose_to_world_frame.x()) +
  271.                 (vehicle_state.y() - end_pose_to_world_frame.y()) *
  272.                     (vehicle_state.y() - end_pose_to_world_frame.y()));
  273.   double theta_to_vehicle = std::abs(common::math::AngleDiff(
  274.       vehicle_state.heading(), end_theta_to_world_frame));
  275.   AERROR << "distance_to_vehicle1 is: " << distance_to_vehicle1;
  276.   AERROR << "distance_to_vehicle2 is: " << distance_to_vehicle2;
  277.   AERROR << "theta_to_vehicle" << theta_to_vehicle << "end_theta_to_world_frame"
  278.          << end_theta_to_world_frame << "rotate_angle" << rotate_angle;
  279.   AERROR << "is_near_destination_threshold"
  280.          << config_.open_space_trajectory_provider_config()
  281.                 .open_space_trajectory_optimizer_config()
  282.                 .planner_open_space_config()
  283.                 .is_near_destination_threshold();  // which config file
  284.   AERROR << "is_near_destination_theta_threshold"
  285.          << config_.open_space_trajectory_provider_config()
  286.                 .open_space_trajectory_optimizer_config()
  287.                 .planner_open_space_config()
  288.                 .is_near_destination_theta_threshold();
  289.   distance_to_vehicle = std::min(
  290.                         std::min(distance_to_vehicle, distance_to_vehicle1),
  291.                         distance_to_vehicle2);
  292.   theta_to_vehicle = std::min(theta_to_vehicle,
  293.                      std::abs(vehicle_state.heading()));
  294.   if (distance_to_vehicle < config_.open_space_trajectory_provider_config()
  295.                                 .open_space_trajectory_optimizer_config()
  296.                                 .planner_open_space_config()
  297.                                 .is_near_destination_threshold() &&
  298.      theta_to_vehicle < config_.open_space_trajectory_provider_config()
  299.                              .open_space_trajectory_optimizer_config()
  300.                              .planner_open_space_config()
  301.                              .is_near_destination_theta_threshold()) {
  302.     AERROR << "vehicle reach end_pose";
  303.     frame_->mutable_open_space_info()->set_destination_reached(true);
  304.     return true;
  305.   }
  306.   return false;
  307. }
  308.  
  309. bool OpenSpaceTrajectoryProvider::IsVehicleStopDueToFallBack(
  310.     const bool is_on_fallback, const common::VehicleState& vehicle_state) {
  311.   if (!is_on_fallback) {
  312.     return false;
  313.   }
  314.   static constexpr double kEpsilon = 1.0e-1;
  315.   const double adc_speed = vehicle_state.linear_velocity();
  316.   const double adc_acceleration = vehicle_state.linear_acceleration();
  317.   if (std::abs(adc_speed) < kEpsilon && std::abs(adc_acceleration) < kEpsilon) {
  318.     ADEBUG << "ADC stops due to fallback trajectory";
  319.     return true;
  320.   }
  321.   return false;
  322. }
  323.  
  324. void OpenSpaceTrajectoryProvider::GenerateStopTrajectory(
  325.     DiscretizedTrajectory* const trajectory_data) {
  326.   double relative_time = 0.0;
  327.   // TODO(Jinyun) Move to conf
  328.   static constexpr int stop_trajectory_length = 10;
  329.   static constexpr double relative_stop_time = 0.1;
  330.   static constexpr double vEpsilon = 0.00001;
  331.   double standstill_acceleration =
  332.       frame_->vehicle_state().linear_velocity() >= -vEpsilon
  333.           ? -FLAGS_open_space_standstill_acceleration
  334.           : FLAGS_open_space_standstill_acceleration;
  335.   trajectory_data->clear();
  336.   for (size_t i = 0; i < stop_trajectory_length; i++) {
  337.     TrajectoryPoint point;
  338.     point.mutable_path_point()->set_x(frame_->vehicle_state().x());
  339.     point.mutable_path_point()->set_y(frame_->vehicle_state().y());
  340.     point.mutable_path_point()->set_theta(frame_->vehicle_state().heading());
  341.     point.mutable_path_point()->set_s(0.0);
  342.     point.mutable_path_point()->set_kappa(0.0);
  343.     point.set_relative_time(relative_time);
  344.     point.set_v(0.0);
  345.     point.set_a(standstill_acceleration);
  346.     trajectory_data->emplace_back(point);
  347.     relative_time += relative_stop_time;
  348.   }
  349. }
  350.  
  351. void OpenSpaceTrajectoryProvider::LoadResult(
  352.     DiscretizedTrajectory* const trajectory_data) {
  353.   // Load unstitched two trajectories into frame for debug
  354.   trajectory_data->clear();
  355.   auto optimizer_trajectory_ptr =
  356.       frame_->mutable_open_space_info()->mutable_optimizer_trajectory_data();
  357.   auto stitching_trajectory_ptr =
  358.       frame_->mutable_open_space_info()->mutable_stitching_trajectory_data();
  359.   open_space_trajectory_optimizer_->GetOptimizedTrajectory(
  360.       optimizer_trajectory_ptr);
  361.   open_space_trajectory_optimizer_->GetStitchingTrajectory(
  362.       stitching_trajectory_ptr);
  363.   // Stitch two trajectories and load back to trajectory_data from frame
  364.   size_t optimizer_trajectory_size = optimizer_trajectory_ptr->size();
  365.   double stitching_point_relative_time =
  366.       stitching_trajectory_ptr->back().relative_time();
  367.   double stitching_point_relative_s =
  368.       stitching_trajectory_ptr->back().path_point().s();
  369.   for (size_t i = 0; i < optimizer_trajectory_size; ++i) {
  370.     optimizer_trajectory_ptr->at(i).set_relative_time(
  371.         optimizer_trajectory_ptr->at(i).relative_time() +
  372.         stitching_point_relative_time);
  373.     optimizer_trajectory_ptr->at(i).mutable_path_point()->set_s(
  374.         optimizer_trajectory_ptr->at(i).path_point().s() +
  375.         stitching_point_relative_s);
  376.   }
  377.   *(trajectory_data) = *(optimizer_trajectory_ptr);
  379.   // Last point in stitching trajectory is already in optimized trajectory, so
  380.   // it is deleted
  381.   frame_->mutable_open_space_info()
  382.       ->mutable_stitching_trajectory_data()
  383.       ->pop_back();
  384.   trajectory_data->PrependTrajectoryPoints(
  385.       frame_->open_space_info().stitching_trajectory_data());
  386.   frame_->mutable_open_space_info()->set_open_space_provider_success(true);
  387. }
  388.  
  389. void OpenSpaceTrajectoryProvider::ReuseLastFrameResult(
  390.     const Frame* last_frame, DiscretizedTrajectory* const trajectory_data) {
  391.   *(trajectory_data) =
  392.       last_frame->open_space_info().stitched_trajectory_result();
  393.   frame_->mutable_open_space_info()->set_open_space_provider_success(true);
  394. }
  395.  
  396. void OpenSpaceTrajectoryProvider::ReuseLastFrameDebug(const Frame* last_frame) {
  397.   // reuse last frame's instance
  398.   auto* ptr_debug = frame_->mutable_open_space_info()->mutable_debug_instance();
  399.   ptr_debug->mutable_planning_data()->mutable_open_space()->MergeFrom(
  400.       last_frame->open_space_info()
  401.           .debug_instance()
  402.           .planning_data()
  403.           .open_space());
  404. }
  406. }  // namespace planning
  407. }  // namespace apollo

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

  1.     std::vector<std::vector<common::math::LineSegment2d>>
  2.     obstacles_linesegments_vec;
  3.     for (const auto& obstacle_vertices : obstacles_vertices_vec) {
  4.         size_t vertices_num = obstacle_vertices.size();
  5.         std::vector<common::math::LineSegment2d> obstacle_linesegments;
  6.         for (size_t i = 0; i < vertices_num - 1; ++i) {
  7.         common::math::LineSegment2d line_segment = common::math::LineSegment2d(
  8.             obstacle_vertices[i], obstacle_vertices[i + 1]);
  9.         obstacle_linesegments.emplace_back(line_segment);
  10.         }
  11.         obstacles_linesegments_vec.emplace_back(obstacle_linesegments);
  12.     }
  13.     obstacles_linesegments_vec_ = std::move(obstacles_linesegments_vec);

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

  1.     start_node_.reset(
  2.     new Node3d({sx}, {sy}, {sphi}, XYbounds_, planner_open_space_config_));
  3.     end_node_.reset(
  4.     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 循环搜索。

    1.   if (AnalyticExpansion(current_node)) {
    2.       break;
    3.   }

  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<Node3d> node)

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

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

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

处理细节：

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

3.2 GenerateDpMap 

  1. bool GridSearch::GenerateDpMap(const double ex, const double ey, 
  2.                                const std::vector<double>& XYbounds,
  3.                                const std::vector<std::vector<common::math::LineSegment2d>> &obstacles_linesegments_vec)

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

  1. #include "modules/planning/open_space/coarse_trajectory_generator/grid_search.h"
  2.  
  3. namespace apollo {
  4. namespace planning {
  5.  
  6. GridSearch::GridSearch(const PlannerOpenSpaceConfig& open_space_conf) {
  7.   xy_grid_resolution_ =
  8.       open_space_conf.warm_start_config().grid_a_star_xy_resolution();
  9.   node_radius_ = open_space_conf.warm_start_config().node_radius();
  10. }
  11.  
  12. double GridSearch::EuclidDistance(const double x1, const double y1,
  13.                                   const double x2, const double y2) {
  14.   return std::sqrt((x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2));
  15. }
  16.  
  17. bool GridSearch::CheckConstraints(std::shared_ptr<Node2d> node) {
  18.   const double node_grid_x = node->GetGridX();
  19.   const double node_grid_y = node->GetGridY();
  20.   if (node_grid_x > max_grid_x_ || node_grid_x < 0 ||
  21.       node_grid_y > max_grid_y_ || node_grid_y < 0) {
  22.     return false;
  23.   }
  24.   if (obstacles_linesegments_vec_.empty()) {
  25.     return true;
  26.   }
  27.   for (const auto& obstacle_linesegments : obstacles_linesegments_vec_) {
  28.     for (const common::math::LineSegment2d& linesegment :
  29.          obstacle_linesegments) {
  30.       if (linesegment.DistanceTo({node->GetGridX(), node->GetGridY()}) <
  31.           node_radius_) {
  32.         return false;
  33.       }
  34.     }
  35.   }
  36.   return true;
  37. }
  38.  
  39. std::vector<std::shared_ptr<Node2d>> GridSearch::GenerateNextNodes(
  40.     std::shared_ptr<Node2d> current_node) {
  41.   double current_node_x = current_node->GetGridX();
  42.   double current_node_y = current_node->GetGridY();
  43.   double current_node_path_cost = current_node->GetPathCost();
  44.   double diagonal_distance = std::sqrt(2.0);
  45.   std::vector<std::shared_ptr<Node2d>> next_nodes;
  46.   std::shared_ptr<Node2d> up =
  47.       std::make_shared<Node2d>(current_node_x, current_node_y + 1.0, XYbounds_);
  48.   up->SetPathCost(current_node_path_cost + 1.0);
  49.   std::shared_ptr<Node2d> up_right = std::make_shared<Node2d>(
  50.       current_node_x + 1.0, current_node_y + 1.0, XYbounds_);
  51.   up_right->SetPathCost(current_node_path_cost + diagonal_distance);
  52.   std::shared_ptr<Node2d> right =
  53.       std::make_shared<Node2d>(current_node_x + 1.0, current_node_y, XYbounds_);
  54.   right->SetPathCost(current_node_path_cost + 1.0);
  55.   std::shared_ptr<Node2d> down_right = std::make_shared<Node2d>(
  56.       current_node_x + 1.0, current_node_y - 1.0, XYbounds_);
  57.   down_right->SetPathCost(current_node_path_cost + diagonal_distance);
  58.   std::shared_ptr<Node2d> down =
  59.       std::make_shared<Node2d>(current_node_x, current_node_y - 1.0, XYbounds_);
  60.   down->SetPathCost(current_node_path_cost + 1.0);
  61.   std::shared_ptr<Node2d> down_left = std::make_shared<Node2d>(
  62.       current_node_x - 1.0, current_node_y - 1.0, XYbounds_);
  63.   down_left->SetPathCost(current_node_path_cost + diagonal_distance);
  64.   std::shared_ptr<Node2d> left =
  65.       std::make_shared<Node2d>(current_node_x - 1.0, current_node_y, XYbounds_);
  66.   left->SetPathCost(current_node_path_cost + 1.0);
  67.   std::shared_ptr<Node2d> up_left = std::make_shared<Node2d>(
  68.       current_node_x - 1.0, current_node_y + 1.0, XYbounds_);
  69.   up_left->SetPathCost(current_node_path_cost + diagonal_distance);
  70.  
  71.   next_nodes.emplace_back(up);
  72.   next_nodes.emplace_back(up_right);
  73.   next_nodes.emplace_back(right);
  74.   next_nodes.emplace_back(down_right);
  75.   next_nodes.emplace_back(down);
  76.   next_nodes.emplace_back(down_left);
  77.   next_nodes.emplace_back(left);
  78.   next_nodes.emplace_back(up_left);
  79.   return next_nodes;
  80. }
  81.  
  82. bool GridSearch::GenerateAStarPath(
  83.     const double sx, const double sy, const double ex, const double ey,
  84.     const std::vector<double>& XYbounds,
  85.     const std::vector<std::vector<common::math::LineSegment2d>>&
  86.         obstacles_linesegments_vec,
  87.     GridAStartResult* result) {
  88.   std::priority_queue<std::pair<std::string, double>,
  89.                       std::vector<std::pair<std::string, double>>, cmp>
  90.       open_pq;
  91.   std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;
  92.   std::unordered_map<std::string, std::shared_ptr<Node2d>> close_set;
  93.   XYbounds_ = XYbounds;
  94.   std::shared_ptr<Node2d> start_node =
  95.       std::make_shared<Node2d>(sx, sy, xy_grid_resolution_, XYbounds_);
  96.   std::shared_ptr<Node2d> end_node =
  97.       std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_);
  98.   std::shared_ptr<Node2d> final_node_ = nullptr;
  99.   obstacles_linesegments_vec_ = obstacles_linesegments_vec;
  100.   open_set.emplace(start_node->GetIndex(), start_node);
  101.   open_pq.emplace(start_node->GetIndex(), start_node->GetCost());
  102.  
  103.   // Grid a star begins
  104.   size_t explored_node_num = 0;
  105.   while (!open_pq.empty()) {
  106.     std::string current_id = open_pq.top().first;
  107.     open_pq.pop();
  108.     std::shared_ptr<Node2d> current_node = open_set[current_id];
  109.     // Check destination
  110.     if (*(current_node) == *(end_node)) {
  111.       final_node_ = current_node;
  112.       break;
  113.     }
  114.     close_set.emplace(current_node->GetIndex(), current_node);
  115.     std::vector<std::shared_ptr<Node2d>> next_nodes =
  116.         std::move(GenerateNextNodes(current_node));
  117.     for (auto& next_node : next_nodes) {
  118.       if (!CheckConstraints(next_node)) {
  119.         continue;
  120.       }
  121.       if (close_set.find(next_node->GetIndex()) != close_set.end()) {
  122.         continue;
  123.       }
  124.       if (open_set.find(next_node->GetIndex()) == open_set.end()) {
  125.         ++explored_node_num;
  126.         next_node->SetHeuristic(
  127.             EuclidDistance(next_node->GetGridX(), next_node->GetGridY(),
  128.                            end_node->GetGridX(), end_node->GetGridY()));
  129.         next_node->SetPreNode(current_node);
  130.         open_set.emplace(next_node->GetIndex(), next_node);
  131.         open_pq.emplace(next_node->GetIndex(), next_node->GetCost());
  132.       }
  133.     }
  134.   }
  135.  
  136.   if (final_node_ == nullptr) {
  137.     AERROR << "Grid A searching return null ptr(open_set ran out)";
  138.     return false;
  139.   }
  140.   LoadGridAStarResult(result);
  141.   ADEBUG << "explored node num is " << explored_node_num;
  142.   return true;
  143. }
  144.  
  145. bool GridSearch::GenerateDpMap(
  146.     const double ex, const double ey, const std::vector<double>& XYbounds,
  147.     const std::vector<std::vector<common::math::LineSegment2d>>&
  148.         obstacles_linesegments_vec) {
  149.   std::priority_queue<std::pair<std::string, double>,
  150.                       std::vector<std::pair<std::string, double>>, cmp>
  151.       open_pq;
  152.   std::unordered_map<std::string, std::shared_ptr<Node2d>> open_set;
  153.   dp_map_ = decltype(dp_map_)();
  154.   XYbounds_ = XYbounds;
  155.   // XYbounds with xmin, xmax, ymin, ymax
  156.   max_grid_y_ = std::round((XYbounds_[3] - XYbounds_[2]) / xy_grid_resolution_);
  157.   max_grid_x_ = std::round((XYbounds_[1] - XYbounds_[0]) / xy_grid_resolution_);
  158.   std::shared_ptr<Node2d> end_node =
  159.       std::make_shared<Node2d>(ex, ey, xy_grid_resolution_, XYbounds_);
  160.   obstacles_linesegments_vec_ = obstacles_linesegments_vec;
  161.   open_set.emplace(end_node->GetIndex(), end_node);
  162.   open_pq.emplace(end_node->GetIndex(), end_node->GetCost());
  163.  
  164.   // Grid a star begins
  165.   size_t explored_node_num = 0;
  166.   while (!open_pq.empty()) {
  167.     const std::string current_id = open_pq.top().first;
  168.     open_pq.pop();
  169.     std::shared_ptr<Node2d> current_node = open_set[current_id];
  170.     dp_map_.emplace(current_node->GetIndex(), current_node);
  171.     std::vector<std::shared_ptr<Node2d>> next_nodes =
  172.         std::move(GenerateNextNodes(current_node));
  173.     for (auto& next_node : next_nodes) {
  174.       if (!CheckConstraints(next_node)) {
  175.         continue;
  176.       }
  177.       if (dp_map_.find(next_node->GetIndex()) != dp_map_.end()) {
  178.         continue;
  179.       }
  180.       if (open_set.find(next_node->GetIndex()) == open_set.end()) {
  181.         ++explored_node_num;
  182.         next_node->SetPreNode(current_node);
  183.         open_set.emplace(next_node->GetIndex(), next_node);
  184.         open_pq.emplace(next_node->GetIndex(), next_node->GetCost());
  185.       } else {
  186.         if (open_set[next_node->GetIndex()]->GetCost() > next_node->GetCost()) {
  187.           open_set[next_node->GetIndex()]->SetCost(next_node->GetCost());
  188.           open_set[next_node->GetIndex()]->SetPreNode(current_node);
  189.         }
  190.       }
  191.     }
  192.   }
  193.   ADEBUG << "explored node num is " << explored_node_num;
  194.   return true;
  195. }
  196.  
  197. double GridSearch::CheckDpMap(const double sx, const double sy) {
  198.   std::string index = Node2d::CalcIndex(sx, sy, xy_grid_resolution_, XYbounds_);
  199.   if (dp_map_.find(index) != dp_map_.end()) {
  200.     return dp_map_[index]->GetCost() * xy_grid_resolution_;
  201.   } else {
  202.     return std::numeric_limits<double>::infinity();
  203.   }
  204. }
  205.  
  206. void GridSearch::LoadGridAStarResult(GridAStartResult* result) {
  207.   (*result).path_cost = final_node_->GetPathCost() * xy_grid_resolution_;
  208.   std::shared_ptr<Node2d> current_node = final_node_;
  209.   std::vector<double> grid_a_x;
  210.   std::vector<double> grid_a_y;
  211.   while (current_node->GetPreNode() != nullptr) {
  212.     grid_a_x.push_back(current_node->GetGridX() * xy_grid_resolution_ +
  213.                        XYbounds_[0]);
  214.     grid_a_y.push_back(current_node->GetGridY() * xy_grid_resolution_ +
  215.                        XYbounds_[2]);
  216.     current_node = current_node->GetPreNode();
  217.   }
  218.   std::reverse(grid_a_x.begin(), grid_a_x.end());
  219.   std::reverse(grid_a_y.begin(), grid_a_y.end());
  220.   (*result).x = std::move(grid_a_x);
  221.   (*result).y = std::move(grid_a_y);
  222. }
  223. }  // namespace planning
  224. }  // namespace apollo

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

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

处理细节：

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

3.3 AnalyticExpansion 

bool HybridAStar::AnalyticExpansion(std::shared_ptr<Node3d> current_node)

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

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

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

处理细节：

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

3.4 CalculateNodeCost 

  1. void HybridAStar::CalculateNodeCost(std::shared_ptr<Node3d> current_node,
  2.                                     std::shared_ptr<Node3d> next_node)

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

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

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

处理细节：

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/AllinToyou/article/detail/581956
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号