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

news2024/10/5 17:23:59

文章目录

  • 前言
  • PATH_DECIDER功能简介
  • PATH_DECIDER相关配置
  • PATH_DECIDER总体流程
    • 路径决策代码流程及框架
    • MakeStaticObstacleDecision
  • 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

PATH_DECIDER功能简介

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

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

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

//
// PathDeciderConfig

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

PATH_DECIDER总体流程

输入:

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);
      }
    }

PATH_DECIDER相关子函数

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计算公式如下：
A O = ( 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+llon​2 ​
个人感觉这么做是为了获得足够的安全冗余量。

参考

[1] 路径决策

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/947942.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

那些在职场上最吃香的人，其实都偷偷学了Python

那些在职场上最吃香的人，其实都偷偷学了Python

Python为什么这么火&#xff1f; 不可否认&#xff0c;Python在机器学习和数值计算等不断增长的科技领域获得了非常广泛的应用。 但Python这么受欢迎的原因&#xff0c;最主要的还是因为它简单易用、上手容易&#xff0c;非程序员也能使用&#xff0c;而不是一种只适合高级程序…
阅读更多...
ssm毕业生就业状况管理系统源码和论文

ssm毕业生就业状况管理系统源码和论文

ssm毕业生就业状况管理系统源码和论文093 开发工具&#xff1a;idea 数据库mysql5.7 数据库链接工具&#xff1a;navcat,小海豚等 技术&#xff1a;ssm 摘 要 现代经济快节奏发展以及不断完善升级的信息化技术&#xff0c;让传统数据信息的管理升级为软件存储&#xff…
阅读更多...
【安装包】JDK 17安装教程

【安装包】JDK 17安装教程

软件下载 软件&#xff1a;JDK版本&#xff1a;17语言&#xff1a;简体中文大小&#xff1a;151.24M安装环境&#xff1a;Win11/Win10/Win8/Win7硬件要求&#xff1a;CPU2.0GHz 内存4G(或更高&#xff09;下载通道①百度网盘丨64位下载链接&#xff1a;https://pan.baidu.com/…
阅读更多...
0基础学习VR全景平台篇 第93篇：智慧景区教程

0基础学习VR全景平台篇 第93篇：智慧景区教程

一、上传素材 1.上传全景素材 第一步&#xff1a;进入【素材管理】 第二步&#xff1a;选择【全景图智慧景区】分类 第三步&#xff1a;选择相对景区作品分组&#xff0c;上传全景素材 2.素材标注 第一步&#xff1a;选择上传成功后素材&#xff0c;点击【未标注】 第二步&…
阅读更多...
Kubernetes技术--k8s核心技术Service服务

Kubernetes技术--k8s核心技术Service服务

1.service概述 Service 是 Kubernetes 最核心概念，通过创建 Service,可以为一组具有相同功能的容器应用提供一个统一的入口地址，并且将请求负载分发到后端的各个容器应用上。 2.service存在的意义 -1:防止pod失联(服务发现) 我们先说一下什么叫pod失联。 -2:
阅读更多...
JAVA反射+动态代理

JAVA反射+动态代理

一.什么是反射&#xff1f; 反射就是对封装的成员信息与变量进行编程式访问 简单来说就是从类里面拿东西 比如属性 或者构造方法 二.获取Class对象: 获取Class的三种方式 代码实现&#xff1a; 首先封装一个javabean Student类 public class Student {private Strin…
阅读更多...
Makerbase_VESC 常用VESC TOOL配置（一）

Makerbase_VESC 常用VESC TOOL配置（一）

VESC TOOL电机配置&#xff08;一&#xff09; 欢迎加入 创客基地 电机控制企鹅群 讨论电机控制相关问题&#xff1a;732557609 欢迎光临 创客基地 tao宝店 采购产品&#xff1a; https://makerbase.taobao.com/ 提示&#xff1a;可以按快捷键“CtrlF”快速寻找相关问题。 …
阅读更多...
微信聊天记录删除恢复导出工具(文字/语音/图片/视频/文件/表情包)

微信聊天记录删除恢复导出工具(文字/语音/图片/视频/文件/表情包)

微信的聊天记录加密保存在电脑中&#xff0c;有时我们想将自己微信中的聊天记录导出来&#xff0c;但微信软件并不提供该功能。此软件可将自己电脑版微信中的聊天内容批量导出来&#xff0c;方便备份&#xff0c;后期不登录也可方便快速查阅。它还能够尝试恢复之前删除过的好友…
阅读更多...
echarts 饼图 图例在右侧时，文字在图例点右边

echarts 饼图 图例在右侧时，文字在图例点右边

echarts图例在右侧时&#xff0c;文字在图例点右边 需求 现在实现的 实现需求 主要使用的参数&#xff1a; legend.align ’left‘代码&#xff1a; 可直接放到echarts示例中使用 option {tooltip: {trigger: item},legend: {orient: "vertical" /*标签文字垂…
阅读更多...
【Java从0到1学习】13 Java IO流

【Java从0到1学习】13 Java IO流

1. 流 1.1 流的概念 流(stream)的概念源于UNIX中管道(pipe)的概念。在UNIX中&#xff0c;管道是一条不间断的字节流&#xff0c;用来实现程序或进程间的通信&#xff0c;或读写外围设备、外部文件等。 一个流&#xff0c;必有源端和目的端&#xff0c;它们可以是计算机内存的…
阅读更多...
揭开波动性的神秘面纱【02/2】：简要介绍预测市场走势

揭开波动性的神秘面纱【02/2】：简要介绍预测市场走势

一、说明 本文是数据专家的体会&#xff0c;他之前写了一系列关于时间序列的文章&#xff0c;在这些文章之后&#xff0c;他想给出一个关于我们如何通过投资组合分析在潜在风险情况下将自己保持在安全区域的想法。文章专业性很强&#xff0c;但机器学习方面的工作还是有参考价值…
阅读更多...
【LeetCode】409. 最长回文串

【LeetCode】409. 最长回文串

409. 最长回文串&#xff08;简单&#xff09; 方法&#xff1a;哈希表 贪心 思路 不难发现&#xff0c;回文字符串一定是由 若干偶数个字符 至多一个奇数个字符 组成 。我们可以使用一个长度为 128 的 hash表来记录每一个字符的出现次数&#xff0c;当该字符出现了两次&am…
阅读更多...
企业想用CRM提高销售业绩该如何操作？

企业想用CRM提高销售业绩该如何操作？

在当今市场环境中&#xff0c;客户的需求更偏向于个性化&#xff0c;企业面对的竞争更加激烈。如何有效地获取和维护客户&#xff0c;提高收入成为了企业的核心问题。作为一种强大的销售管理工具&#xff0c;CRM如何提高销售业绩&#xff1f; 提高客户转化率&#xff1a; 企业…
阅读更多...
Day50｜动态规划part11：188.买卖股票的最佳时机IV、123. 买卖股票的最佳时机III

Day50｜动态规划part11：188.买卖股票的最佳时机IV、123. 买卖股票的最佳时机III

188. 买卖股票的最佳时机IV leetcode链接&#xff1a;188 题「买卖股票的最佳时机 IVopen in new window」 视频链接&#xff1a;动态规划来决定最佳时机&#xff0c;至多可以买卖K次&#xff01;| LeetCode&#xff1a;188.买卖股票最佳时机4 给你一个整数数组 prices 和一…
阅读更多...
基于ssm车库智能管理平台源码和论文

基于ssm车库智能管理平台源码和论文

基于ssm车库智能管理平台源码和论文092 开发工具&#xff1a;idea 数据库mysql5.7 数据库链接工具&#xff1a;navcat,小海豚等 技术&#xff1a;ssm 选题的根据&#xff1a;1&#xff09;说明本选题的理论、实际意义 2&#xff09;综述国内外有关本选题的研究动态和自己…
阅读更多...
【LeetCode75】第四十一题 二叉搜索树中的搜索

【LeetCode75】第四十一题 二叉搜索树中的搜索

目录 题目&#xff1a; 示例&#xff1a; 分析&#xff1a; 代码&#xff1a; 题目&#xff1a; 示例&#xff1a; 分析&#xff1a; 题目给我们一个搜索二叉树&#xff0c;让我们找出节点值等于目标的节点并返回出去。 首先我们可以直接遍历整棵二叉树&#xff0c;找到值…
阅读更多...
Mybatis2，注解实现CRUD

Mybatis2，注解实现CRUD

2&#xff0c;注解实现CRUD 使用注解开发会比配置文件开发更加方便。如下就是使用注解进行开发 Select(value "select * from tb_user where id #{id}") public User select(int id);注意&#xff1a; 注解是用来替换映射配置文件方式配置的&#xff0c;所以使用了…
阅读更多...
使用 Privoxy 在 Linux 上配置本地代理服务器详细教程

使用 Privoxy 在 Linux 上配置本地代理服务器详细教程

Privoxy 是一个功能强大的开源网络代理软件&#xff0c;它可以帮助我们在 Linux 系统上搭建本地代理服务器。通过配置和使用 Privoxy&#xff0c;您可以实现更安全、匿名以及自定义过滤规则等高级特性。本文将详细介绍如何在 Linux 环境下利用 Privoxy 配置并运行本地代理服务器…
阅读更多...
文字转语音怎么转？几种简单转换方法快速转换

文字转语音怎么转？几种简单转换方法快速转换

将文本转换为语音&#xff0c;可以帮助那些有视觉障碍的人士更便捷地获取信息&#xff0c;以便于听觉上的理解和处理。对于那些需要进行多任务处理的人士&#xff0c;例如在开车或做家务时需要获取信息的人&#xff0c;文字转语音也可以提供便利。那么怎么将文字转换成语音呢&a…
阅读更多...
【100天精通python】Day47：python网络编程_Web编程，前后端以及静态服务器

【100天精通python】Day47：python网络编程_Web编程，前后端以及静态服务器

目录 1 网络编程与web编程 1.1 网络编程 1.2 web编程 1.3 前后端交互的基本原理 2 Web开发基础 2.1 HTTP协议 2.2 Web服务器 2.3 前端基础 2.3.1 HTML&#xff08;超文本标记语言&#xff09; 2. 3.2 CSS&#xff08;层叠样式表&#xff09; 2.3.3 JavaScript 2.…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023