• Apollo planning之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::vectorvectormath::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::vectorvectormath::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) 若有侵权，请联系删除

  • 相关阅读:
    ElasticSearch 报错 FORBIDDEN/12/index read-only / allow delete (api)
    10个打工人必备AI神器，升职加薪靠AI
    2022中国眼博会，山东眼健康展，视力矫正仪器展，护眼产品展
    【一起学Rust · 项目实战】命令行IO项目minigrep——重构优化模块和错误处理
    TCP和UDP C#代码实战
    Stream 流式编程 ( Java 8新特性 )
    德鲁伊数据库连接池的使用
    CentOS 7镜像下载 以及 DVD ISO 和 Minimal ISO 等各版本的区别介绍
    小李移动开发成长记 —— 大话小程序
    Vue----组件
  • 原文地址：https://blog.csdn.net/weixin_65089713/article/details/127449837