我的编程空间,编程开发者的网络收藏夹
学习永远不晚

【Apollo学习笔记】——规划模块TASK之PATH_DECIDER

短信预约 -IT技能 免费直播动态提醒
省份

北京

  • 北京
  • 上海
  • 天津
  • 重庆
  • 河北
  • 山东
  • 辽宁
  • 黑龙江
  • 吉林
  • 甘肃
  • 青海
  • 河南
  • 江苏
  • 湖北
  • 湖南
  • 江西
  • 浙江
  • 广东
  • 云南
  • 福建
  • 海南
  • 山西
  • 四川
  • 陕西
  • 贵州
  • 安徽
  • 广西
  • 内蒙
  • 西藏
  • 新疆
  • 宁夏
  • 兵团
手机号立即预约

请填写图片验证码后获取短信验证码

看不清楚,换张图片

免费获取短信验证码

【Apollo学习笔记】——规划模块TASK之PATH_DECIDER

Apollo星火计划学习笔记——Apollo路径规划算法原理与实践与【Apollo学习笔记】——Planning模块讲到……Stage::Process的PlanOnReferenceLine函数会依次调用task_list中的TASK,本文将会继续以LaneFollow为例依次介绍其中的TASK部分究竟做了哪些工作。由于个人能力所限,文章可能有纰漏的地方,还请批评斧正。

modules/planning/conf/scenario/lane_follow_config.pb.txt配置文件中,我们可以看到LaneFollow所需要执行的所有task。

stage_config: {  stage_type: LANE_FOLLOW_DEFAULT_STAGE  enabled: true  task_type: LANE_CHANGE_DECIDER  task_type: PATH_REUSE_DECIDER  task_type: PATH_LANE_BORROW_DECIDER  task_type: PATH_BOUNDS_DECIDER  task_type: PIECEWISE_JERK_PATH_OPTIMIZER  task_type: PATH_ASSESSMENT_DECIDER  task_type: PATH_DECIDER  task_type: RULE_BASED_STOP_DECIDER  task_type: SPEED_BOUNDS_PRIORI_DECIDER  task_type: SPEED_HEURISTIC_OPTIMIZER  task_type: SPEED_DECIDER  task_type: SPEED_BOUNDS_FINAL_DECIDER  task_type: PIECEWISE_JERK_SPEED_OPTIMIZER  # task_type: PIECEWISE_JERK_NONLINEAR_SPEED_OPTIMIZER  task_type: RSS_DECIDER

本文将继续介绍LaneFollow的第7个TASK——PATH_DECIDER

根据选出的路径给出对障碍物的决策

在这里插入图片描述若是绕行的路径,则产生绕行的决策;若前方有障碍物阻塞,则产生停止的决策。

modules/planning/conf/planning_config.pb.txt

default_task_config: {  task_type: PATH_DECIDER  path_decider_config{    static_obstacle_buffer: 0.3  }}

modules/planning/proto/task_config.proto

//// PathDeciderConfigmessage PathDeciderConfig {  // buffer for static obstacles (meter)  optional double static_obstacle_buffer = 1 [default = 0.3];}

输入:

Status PathDecider::Process(const ReferenceLineInfo *reference_line_info,const PathData &path_data,PathDecision *const path_decision) {

输出:
路径决策的信息都保存到了path_decision中。

路径决策代码流程及框架

在这里插入图片描述
Process函数主要功能是调用了MakeObjectDecision函数。而在MakeObjectDecision函数中调用了MakeStaticObstacleDecision函数。

路径决策的主要功能都在MakeStaticObstacleDecision中。这部分代码还是比较清晰的。

Status PathDecider::Process(const ReferenceLineInfo *reference_line_info,const PathData &path_data,PathDecision *const path_decision) {  // skip path_decider if reused path  if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {    return Status::OK();  }  std::string blocking_obstacle_id;  if (reference_line_info->GetBlockingObstacle() != nullptr) {    blocking_obstacle_id = reference_line_info->GetBlockingObstacle()->Id();  }  // 调用MakeObjectDecision函数  if (!MakeObjectDecision(path_data, blocking_obstacle_id, path_decision)) {    const std::string msg = "Failed to make decision based on tunnel";    AERROR << msg;    return Status(ErrorCode::PLANNING_ERROR, msg);  }  return Status::OK();}bool PathDecider::MakeObjectDecision(const PathData &path_data,         const std::string &blocking_obstacle_id,         PathDecision *const path_decision) {  // path decider的主要功能在MakeStaticObstacleDecision中  if (!MakeStaticObstacleDecision(path_data, blocking_obstacle_id,      path_decision)) {    AERROR << "Failed to make decisions for static obstacles";    return false;  }  return true;}

MakeStaticObstacleDecision

获取frenet坐标系下的坐标

  ... ...  // 1.获取frenet坐标下的path路径  const auto &frenet_path = path_data.frenet_frame_path();  if (frenet_path.empty()) {    AERROR << "Path is empty.";    return false;  }  ... ...

根据障碍物做决策

  ... ...  // 2.遍历每个障碍物,做决策  for (const auto *obstacle : path_decision->obstacles().Items()) {    const std::string &obstacle_id = obstacle->Id();    const std::string obstacle_type_name =        PerceptionObstacle_Type_Name(obstacle->Perception().type());    ADEBUG << "obstacle_id[<< " << obstacle_id << "] type["           << obstacle_type_name << "]";    ... ...

如果障碍物不是静态或virtual,则跳过

    // 2.1 如果障碍物不是静态的或者是virtual的,就跳过    if (!obstacle->IsStatic() || obstacle->IsVirtual()) {    // (stop fence,各种fence)      continue;    }

如果障碍物有了ignore/stop决策,则跳过

    // 2.2 如果障碍物已经有 ignore/stop 决策,就跳过    if (obstacle->HasLongitudinalDecision() &&        obstacle->LongitudinalDecision().has_ignore() &&        obstacle->HasLateralDecision() &&        obstacle->LateralDecision().has_ignore()) {      continue;    }    if (obstacle->HasLongitudinalDecision() &&        obstacle->LongitudinalDecision().has_stop()) {      // STOP decision      continue;    }

如果障碍物挡住了路径,加stop决策

    // 2.3 如果障碍物挡住了路径,加stop决策    if (obstacle->Id() == blocking_obstacle_id &&        !injector_->planning_context()             ->planning_status()             .path_decider()             .is_in_path_lane_borrow_scenario()) {      // Add stop decision      ADEBUG << "Blocking obstacle = " << blocking_obstacle_id;      ObjectDecisionType object_decision;      *object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);      path_decision->AddLongitudinalDecision("PathDecider/blocking_obstacle",                 obstacle->Id(), object_decision);      continue;    }

如果是clear-zone,跳过

    // 2.4 如果是clear-zone,跳过    if (obstacle->reference_line_st_boundary().boundary_type() ==        STBoundary::BoundaryType::KEEP_CLEAR) {      continue;    }

如果障碍物不在路径上,跳过

    // 2.5 如果障碍物不在路径上,跳过    ObjectDecisionType object_decision;    object_decision.mutable_ignore();    const auto &sl_boundary = obstacle->PerceptionSLBoundary();    if (sl_boundary.end_s() < frenet_path.front().s() ||        sl_boundary.start_s() > frenet_path.back().s()) {      path_decision->AddLongitudinalDecision("PathDecider/not-in-s",                 obstacle->Id(), object_decision);      path_decision->AddLateralDecision("PathDecider/not-in-s", obstacle->Id(),            object_decision);      continue;    }

nudge判断

  • 如果距离静态障碍物距离太远,则忽略。
  • 如果静态障碍物距离车道中心太近,则停止。
  • 如果横向方向很近,则避开。
    // 2.6 nudge判断,如果距离静态障碍物距离太远,则忽略。    //               如果静态障碍物距离车道中心太近,则停止。    //               如果横向方向很近,则避开。    if (curr_l - lateral_radius > sl_boundary.end_l() ||        curr_l + lateral_radius < sl_boundary.start_l()) {      // 1. IGNORE if laterally too far away.      path_decision->AddLateralDecision("PathDecider/not-in-l", obstacle->Id(),            object_decision);    } else if (sl_boundary.end_l() >= curr_l - min_nudge_l &&               sl_boundary.start_l() <= curr_l + min_nudge_l) {      // 2. STOP if laterally too overlapping.      *object_decision.mutable_stop() = GenerateObjectStopDecision(*obstacle);      if (path_decision->MergeWithMainStop(              object_decision.stop(), obstacle->Id(),              reference_line_info_->reference_line(),              reference_line_info_->AdcSlBoundary())) {        path_decision->AddLongitudinalDecision("PathDecider/nearest-stop",                   obstacle->Id(), object_decision);      } else {        ObjectDecisionType object_decision;        object_decision.mutable_ignore();        path_decision->AddLongitudinalDecision("PathDecider/not-nearest-stop",                   obstacle->Id(), object_decision);      }    } else {      // 3. NUDGE if laterally very close.      if (sl_boundary.end_l() < curr_l - min_nudge_l) {  // &&        // sl_boundary.end_l() > curr_l - min_nudge_l - 0.3) {        // LEFT_NUDGE        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();        object_nudge_ptr->set_type(ObjectNudge::LEFT_NUDGE);        object_nudge_ptr->set_distance_l(            config_.path_decider_config().static_obstacle_buffer());        path_decision->AddLateralDecision("PathDecider/left-nudge",              obstacle->Id(), object_decision);      } else if (sl_boundary.start_l() > curr_l + min_nudge_l) {  // &&        // sl_boundary.start_l() < curr_l + min_nudge_l + 0.3) {        // RIGHT_NUDGE        ObjectNudge *object_nudge_ptr = object_decision.mutable_nudge();        object_nudge_ptr->set_type(ObjectNudge::RIGHT_NUDGE);        object_nudge_ptr->set_distance_l(            -config_.path_decider_config().static_obstacle_buffer());        path_decision->AddLateralDecision("PathDecider/right-nudge",              obstacle->Id(), object_decision);      }    }

GenerateObjectStopDecision主要用以生成停止决策。

ObjectStop PathDecider::GenerateObjectStopDecision(    const Obstacle &obstacle) const {  ObjectStop object_stop;  // Calculate stop distance with the obstacle using the ADC's minimum turning radius  double stop_distance = obstacle.MinRadiusStopDistance(      VehicleConfigHelper::GetConfig().vehicle_param());  object_stop.set_reason_code(StopReasonCode::STOP_REASON_OBSTACLE);  object_stop.set_distance_s(-stop_distance);  // 停止时的参考位置  const double stop_ref_s =      obstacle.PerceptionSLBoundary().start_s() - stop_distance;  const auto stop_ref_point =      reference_line_info_->reference_line().GetReferencePoint(stop_ref_s);  object_stop.mutable_stop_point()->set_x(stop_ref_point.x());  object_stop.mutable_stop_point()->set_y(stop_ref_point.y());  object_stop.set_stop_heading(stop_ref_point.heading());  return object_stop;}

对于停止距离的计算,会调用MinRadiusStopDistance函数,
modules/planning/common/obstacle.cc

double Obstacle::MinRadiusStopDistance(    const common::VehicleParam& vehicle_param) const {  if (min_radius_stop_distance_ > 0) {    return min_radius_stop_distance_;  }  // 定义一个停止距离的缓冲区0.5m  static constexpr double stop_distance_buffer = 0.5;  // 获取最小安全转弯半径  const double min_turn_radius = VehicleConfigHelper::MinSafeTurnRadius();  // 计算横向距离  double lateral_diff =      vehicle_param.width() / 2.0 + std::max(std::fabs(sl_boundary_.start_l()),                 std::fabs(sl_boundary_.end_l()));  const double kEpison = 1e-5;  lateral_diff = std::min(lateral_diff, min_turn_radius - kEpison);  // 勾股定理求得停止距离  double stop_distance =      std::sqrt(std::fabs(min_turn_radius * min_turn_radius -                          (min_turn_radius - lateral_diff) *  (min_turn_radius - lateral_diff))) +      stop_distance_buffer;  // 减掉车辆前端到后轴中心的距离  stop_distance -= vehicle_param.front_edge_to_center();  // 限幅  stop_distance = std::min(stop_distance, FLAGS_max_stop_distance_obstacle); // 10.0  stop_distance = std::max(stop_distance, FLAGS_min_stop_distance_obstacle); // 6.0  return stop_distance;}

计算示意图如下:
在这里插入图片描述

modules/common/configs/vehicle_config_helper.cc

double VehicleConfigHelper::MinSafeTurnRadius() {  const auto &param = vehicle_config_.vehicle_param();  double lat_edge_to_center =      std::max(param.left_edge_to_center(), param.right_edge_to_center());  double lon_edge_to_center =      std::max(param.front_edge_to_center(), param.back_edge_to_center());  return std::sqrt((lat_edge_to_center + param.min_turn_radius()) *                       (lat_edge_to_center + param.min_turn_radius()) +                   lon_edge_to_center * lon_edge_to_center);}

MinSafeTurnRadius这段函数是获取当车辆以最大转向角转弯时的最大安全转弯半径。具体计算参考下图:
在这里插入图片描述
A , B , C , D A,B,C,D A,B,C,D分别是车辆的四个角, X O XO XO是车辆的最小转弯半径VehicleParam.min_turn_radius() X X X A D AD AD之间的距离是左边缘到中心的距离left_edge_to_center X X X A B AB AB之间的距离是前边缘到中心的距离front_edge_to_center。最大安全转弯半径则是 A O AO AO,定义中心到横向边缘最长的距离为 l l a t l_{lat} llat,到纵向边缘最长的距离为 l l o n l_{lon} llon A O AO AO计算公式如下:
AO= ( X O + l l a t ) 2 + l l o n 2 AO=\sqrt{(XO+l_{lat})^2+{l_{lon}}^2} AO=(XO+llat)2+llon2
个人感觉这么做是为了获得足够的安全冗余量。

[1] 路径决策

来源地址:https://blog.csdn.net/sinat_52032317/article/details/132549818

免责声明:

① 本站未注明“稿件来源”的信息均来自网络整理。其文字、图片和音视频稿件的所属权归原作者所有。本站收集整理出于非商业性的教育和科研之目的,并不意味着本站赞同其观点或证实其内容的真实性。仅作为临时的测试数据,供内部测试之用。本站并未授权任何人以任何方式主动获取本站任何信息。

② 本站未注明“稿件来源”的临时测试数据将在测试完成后最终做删除处理。有问题或投稿请发送至: 邮箱/279061341@qq.com QQ/279061341

【Apollo学习笔记】——规划模块TASK之PATH_DECIDER

下载Word文档到电脑,方便收藏和打印~

下载Word文档

猜你喜欢

【Apollo学习笔记】——规划模块TASK之PATH_DECIDER

文章目录 前言PATH_DECIDER功能简介PATH_DECIDER相关配置PATH_DECIDER总体流程路径决策代码流程及框架MakeStaticObstacleDecision PATH_DECIDER相关子函数参考
2023-08-30

Nodejs学习笔记之Stream模块

一,开篇分析 流是一个抽象接口,被 Node 中的很多对象所实现。比如对一个 HTTP 服务器的请求是一个流,stdout 也是一个流。流是可读,可写或兼具两者的。 最早接触Stream是从早期的unix开始的, 数十年的实践证明Strea
2022-06-04

Nodejs学习笔记之NET模块

一,开篇分析 从今天开始,我们来深入具体的模块学习,这篇文章是这个系列文章的第三篇,前两篇主要是以理论为主,相信大家在前两篇的学习中, 对NodeJS也有一个基本的认识,没事!!!趁热打铁,让我们继续将NodeJS进行到底,好了废话不多说,
2022-06-04

NodeJS学习笔记之MongoDB模块

一,开篇分析 这篇属于扩展知识篇,因为在下面的文章中会用到数据库操作,所以今天就来说说它(Mongodb模块)。 (1),简介MongoDB是一个基于分布式文件存储的数据库。由C++语言编写。旨在为WEB应用提供可扩展的高性能数据存储解决方
2022-06-04

NodeJS学习笔记之Http模块

一,开篇分析 首先“Http”这个概念大家应该比较熟悉了,它不是基于特定语言的,是一个通用的应用层协议,不同语言有不同的实现细节,但是万变不离其宗,思想是相同的, NodeJS作为一个宿主运行环境,以JavaScript为宿主语言,它也有自
2022-06-04

Python学习笔记之json模块和pickle模块

json和pickle模块是将数据进行序列化处理,并进行网络传输或存入硬盘,下面这篇文章主要给大家介绍了关于Python学习笔记之json模块和pickle模块的相关资料,文中通过实例代码介绍的非常详细,需要的朋友可以参考下
2023-05-18

NodeJS学习笔记之FS文件模块

一,开篇分析 文件系统模块是一个简单包装的标准 POSIX 文件 I/O 操作方法集。可以通过调用 require("fs") 来获取该模块。文件系统模块中的所有方法均有异步和同步版本。 (1),文件系统模块中的异步方法需要一个完成时的回调
2022-06-04

NodeJS学习笔记之(Url,QueryString,Path)模块

一,开篇分析 这篇文章把这三个模块拿来一起说,原因是它们各自的篇幅都不是很长,其次是它们之间存在着依赖关系,所以依次介绍并且实例分析。废话不多说了,请看下面文档: (1),"Url模块"来个小栗子:  var url = require('
2022-06-04

NodeJS学习笔记之Connect中间件模块(二)

一,开篇分析 大家好,今天这篇文章主要是对"Connect"中间件以及相关辅助中间件,做一个源码分析系列,我想上一篇文章大家也看了, 介绍了使用方式及用途,而这篇也是出于本人的兴趣,让读者对其有一个更深入的认识,如在分析阶段有什么不正确的地
2022-06-04

NodeJS学习笔记之Connect中间件模块(一)

希望大家能够坚持看下去我这系列的文章,这也是对我的最大的鼓励与支持,让我们共同进步,以文会友,相互帮助。好了直接进入今天的主题, 什么是“Connect”,中间件又当如何理解,带着问题来看今天的文章。如何理解 "中间件" 我的理解是这样的
2022-06-04

Python常见库matplotlib学习笔记之画图中各个模块的含义及修改方法

matplotlib是python最著名的绘图库,它提供了一整套和matlab相似的命令API,十分适合交互式地进行制图,下面这篇文章主要给大家介绍了关于Python常见库matplotlib学习笔记之画图中各个模块的含义及修改方法的相关资料,需要的朋友可以参考下
2023-05-19

编程热搜

目录