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

news2024/11/22 20:54:55

文章目录

  • 前言
  • PATH_BORROW_DECIDER功能简介
  • PATH_BORROW_DECIDER相关配置
  • PATH_BORROW_DECIDER总体流程
  • PATH_BORROW_DECIDER相关子函数
    • IsNecessaryToBorrowLane
    • IsBlockingObstacleFarFromIntersection
    • IsNonmovableObstacle
    • CheckLaneBorrow
  • 参考

前言

在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

本文将继续介绍第三个TASK——PATH_BORROW_DECIDER

PATH_BORROW_DECIDER功能简介

在这里插入图片描述

主要功能:产生是否借道的决策
判断条件
• 是否只有一条车道
• 是否存在阻塞道路的障碍物
• 阻塞障碍物是否远离路口
• 阻塞障碍物长期存在
• 旁边车道是实线还是虚线
当所有判断条件都满足时,会产生借道决策。

PATH_BORROW_DECIDER相关配置

PATH_BORROW_DECIDER的相关配置集中在以下两个文件:modules/planning/conf/planning_config.pb.txtmodules/planning/conf/scenario/lane_follow_config.pb.txt

// modules/planning/conf/scenario/lane_follow_config.pb.txt
  task_config: {
    task_type: PATH_LANE_BORROW_DECIDER
    path_lane_borrow_decider_config {
      allow_lane_borrowing: true
    }
  }
// modules/planning/conf/planning_config.pb.txt
default_task_config: {
  task_type: PATH_LANE_BORROW_DECIDER
  path_lane_borrow_decider_config {
    allow_lane_borrowing: true
  }
}

PATH_BORROW_DECIDER总体流程

在这里插入图片描述

流程依旧看Process:

主要完成数据检查;判断是否启用reuse path,若有则跳过剩余步骤;判断是否有必要进行借道操作等步骤。

Status PathLaneBorrowDecider::Process(
    Frame* const frame, ReferenceLineInfo* const reference_line_info) {
  // Sanity checks.
  CHECK_NOTNULL(frame);
  CHECK_NOTNULL(reference_line_info);

  // skip path_lane_borrow_decider if reused path
  if (FLAGS_enable_skip_path_tasks && reference_line_info->path_reusable()) {
    // for debug
    AINFO << "skip due to reusing path";
    return Status::OK();
  }

  // By default, don't borrow any lane.
  reference_line_info->set_is_path_lane_borrow(false);
  // Check if lane-borrowing is needed, if so, borrow lane.
  if (Decider::config_.path_lane_borrow_decider_config()
          .allow_lane_borrowing() &&
      IsNecessaryToBorrowLane(*frame, *reference_line_info)) {
    reference_line_info->set_is_path_lane_borrow(true);
  }
  return Status::OK();
}

同时还可以注意一下相关结构体定义:
modules/planning/proto/planning_status.proto

message PathDeciderStatus {
  enum LaneBorrowDirection {
    LEFT_BORROW = 1;   // borrow left neighbor lane
    RIGHT_BORROW = 2;  // borrow right neighbor lane
  }
  optional int32 front_static_obstacle_cycle_counter = 1 [default = 0];
  optional int32 able_to_use_self_lane_counter = 2 [default = 0];
  optional bool is_in_path_lane_borrow_scenario = 3 [default = false];
  optional string front_static_obstacle_id = 4 [default = ""];
  repeated LaneBorrowDirection decided_side_pass_direction = 5;
}

PATH_BORROW_DECIDER相关子函数

IsNecessaryToBorrowLane

借道判断主要通过核心函数IsNecessaryToBorrowLane()判断是否借道,主要涉及一些rules,包括距离信号交叉口的距离,与静态障碍物的距离,是否是单行道,是否所在车道左右车道线是虚线等规则。主要有两个功能:

  • 已处于借道场景下判断是否退出避让;
  • 未处于借道场景下判断是否具备借道能力。
bool PathLaneBorrowDecider::IsNecessaryToBorrowLane(
    const Frame& frame, const ReferenceLineInfo& reference_line_info) {
  auto* mutable_path_decider_status = injector_->planning_context()
                                          ->mutable_planning_status()
                                          ->mutable_path_decider();
  // 如果当前处于借道场景中                                        
  if (mutable_path_decider_status->is_in_path_lane_borrow_scenario()) {
    // If originally borrowing neighbor lane:
    // 如果当前已经在借道状态了,并且自车车道可用的counter >=6,取消lane_borrow scenario 状态
    if (mutable_path_decider_status->able_to_use_self_lane_counter() >= 6) {
      // If have been able to use self-lane for some time, then switch to
      // non-lane-borrowing.
      mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
      mutable_path_decider_status->clear_decided_side_pass_direction();
      AINFO << "Switch from LANE-BORROW path to SELF-LANE path.";
    }
  } else {
    // If originally not borrowing neighbor lane:
    // 如果未在借道的状态
    ADEBUG << "Blocking obstacle ID["
           << mutable_path_decider_status->front_static_obstacle_id() << "]";
    // ADC requirements check for lane-borrowing:
    // 只有一条参考线,才能借道
    if (!HasSingleReferenceLine(frame)) {
      return false;
    }
    // 起点速度小于最大借道允许速度
    if (!IsWithinSidePassingSpeedADC(frame)) {
      return false;
    }

    // Obstacle condition check for lane-borrowing:
    // 阻塞障碍物是否远离路口
    if (!IsBlockingObstacleFarFromIntersection(reference_line_info)) {
      return false;
    }
    // 阻塞障碍物长期存在
    if (!IsLongTermBlockingObstacle()) {
      return false;
    }
    // 阻塞障碍物是否在终点位置与自车间距之内
    if (!IsBlockingObstacleWithinDestination(reference_line_info)) {
      return false;
    }
    // 为可侧面通过的障碍物
    if (!IsSidePassableObstacle(reference_line_info)) {
      return false;
    }

    // switch to lane-borrowing
    // set side-pass direction
    const auto& path_decider_status =
        injector_->planning_context()->planning_status().path_decider();
    if (path_decider_status.decided_side_pass_direction().empty()) {
      // first time init decided_side_pass_direction
      bool left_borrowable;
      bool right_borrowable;
      CheckLaneBorrow(reference_line_info, &left_borrowable, &right_borrowable);
      if (!left_borrowable && !right_borrowable) {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(false);
        return false;
      } else {
        mutable_path_decider_status->set_is_in_path_lane_borrow_scenario(true);
        if (left_borrowable) {
          mutable_path_decider_status->add_decided_side_pass_direction(
              PathDeciderStatus::LEFT_BORROW);
        }
        if (right_borrowable) {
          mutable_path_decider_status->add_decided_side_pass_direction(
              PathDeciderStatus::RIGHT_BORROW);
        }
      }
    }

    AINFO << "Switch from SELF-LANE path to LANE-BORROW path.";
  }
  return mutable_path_decider_status->is_in_path_lane_borrow_scenario();
}

IsBlockingObstacleFarFromIntersection

IsBlockingObstacleFarFromIntersection流程部分在总体流程图中已经展示。

bool PathLaneBorrowDecider::IsBlockingObstacleFarFromIntersection(
    const ReferenceLineInfo& reference_line_info) {
  const auto& path_decider_status =
      injector_->planning_context()->planning_status().path_decider();
  const std::string blocking_obstacle_id =
      path_decider_status.front_static_obstacle_id();
  if (blocking_obstacle_id.empty()) {
    ADEBUG << "There is no blocking obstacle.";
    return true;
  }
  const Obstacle* blocking_obstacle =
      reference_line_info.path_decision().obstacles().Find(
          blocking_obstacle_id);
  if (blocking_obstacle == nullptr) {
    ADEBUG << "Blocking obstacle is no longer there.";
    return true;
  }

  // Get blocking obstacle's s.
  double blocking_obstacle_s =
      blocking_obstacle->PerceptionSLBoundary().end_s();
  ADEBUG << "Blocking obstacle is at s = " << blocking_obstacle_s;
  // Get intersection's s and compare with threshold.
  const auto& first_encountered_overlaps =
      reference_line_info.FirstEncounteredOverlaps();
  for (const auto& overlap : first_encountered_overlaps) {
    ADEBUG << overlap.first << ", " << overlap.second.DebugString();
    // if (// overlap.first != ReferenceLineInfo::CLEAR_AREA &&
    // overlap.first != ReferenceLineInfo::CROSSWALK &&
    // overlap.first != ReferenceLineInfo::PNC_JUNCTION &&
    if (overlap.first != ReferenceLineInfo::SIGNAL &&
        overlap.first != ReferenceLineInfo::STOP_SIGN) {
      continue;
    }

    auto distance = overlap.second.start_s - blocking_obstacle_s;
    if (overlap.first == ReferenceLineInfo::SIGNAL ||
        overlap.first == ReferenceLineInfo::STOP_SIGN) {
      if (distance < kIntersectionClearanceDist) {
        ADEBUG << "Too close to signal intersection (" << distance
               << "m); don't SIDE_PASS.";
        return false;
      }
    } else {
      if (distance < kJunctionClearanceDist) {
        ADEBUG << "Too close to overlap_type[" << overlap.first << "] ("
               << distance << "m); don't SIDE_PASS";
        return false;
      }
    }
  }

  return true;
}

IsNonmovableObstacle

在IsNonmovableObstacle() 中主要对前方障碍物进行判断,利用预测以及参考线的信息来进行判断。代码位置在modules/planning/common/obstacle_blocking_analyzer.cc。大致流程在总体流程图中已经展示。

bool IsNonmovableObstacle(const ReferenceLineInfo& reference_line_info,
                          const Obstacle& obstacle) {
  // Obstacle is far away.
  const SLBoundary& adc_sl_boundary = reference_line_info.AdcSlBoundary();
  if (obstacle.PerceptionSLBoundary().start_s() >
      adc_sl_boundary.end_s() + kAdcDistanceThreshold) {
    ADEBUG << " - It is too far ahead and we are not so sure of its status.";
    return false;
  }

  // Obstacle is parked obstacle.
  if (IsParkedVehicle(reference_line_info.reference_line(), &obstacle)) {
    ADEBUG << "It is Parked and NON-MOVABLE.";
    return true;
  }

  // Obstacle is blocked by others too.
  for (const auto* other_obstacle :
       reference_line_info.path_decision().obstacles().Items()) {
    if (other_obstacle->Id() == obstacle.Id()) {
      continue;
    }
    if (other_obstacle->IsVirtual()) {
      continue;
    }
    if (other_obstacle->PerceptionSLBoundary().start_l() >
            obstacle.PerceptionSLBoundary().end_l() ||
        other_obstacle->PerceptionSLBoundary().end_l() <
            obstacle.PerceptionSLBoundary().start_l()) {
      // not blocking the backside vehicle
      continue;
    }
    double delta_s = other_obstacle->PerceptionSLBoundary().start_s() -
                     obstacle.PerceptionSLBoundary().end_s();
    if (delta_s < 0.0 || delta_s > kObstaclesDistanceThreshold) {
      continue;
    }

    // TODO(All): Fix the segmentation bug for large vehicles, otherwise
    // the follow line will be problematic.
    ADEBUG << " - It is blocked by others, and will move later.";
    return false;
  }

  ADEBUG << "IT IS NON-MOVABLE!";
  return true;
}

CheckLaneBorrow

主要根据前方道路的线型判断是否可以借道;在此函数中2m间隔一个点遍历前视距离。

void PathLaneBorrowDecider::CheckLaneBorrow(
    const ReferenceLineInfo& reference_line_info,
    bool* left_neighbor_lane_borrowable, bool* right_neighbor_lane_borrowable) {
  const ReferenceLine& reference_line = reference_line_info.reference_line();
  // 定义左、右车道是否可借用的标志位,并初始化为true
  *left_neighbor_lane_borrowable = true;
  *right_neighbor_lane_borrowable = true;
  
  static constexpr double kLookforwardDistance = 100.0;
  // 获取自车的S坐标
  double check_s = reference_line_info.AdcSlBoundary().end_s();
  // 前视距离
  const double lookforward_distance =
      std::min(check_s + kLookforwardDistance, reference_line.Length());
  while (check_s < lookforward_distance) {
    auto ref_point = reference_line.GetNearestReferencePoint(check_s);
    // 找不到在参考线上的邻近点,左右车道均不可借用,直接返回
    if (ref_point.lane_waypoints().empty()) {
      *left_neighbor_lane_borrowable = false;
      *right_neighbor_lane_borrowable = false;
      return;
    }

    const auto waypoint = ref_point.lane_waypoints().front();
    hdmap::LaneBoundaryType::Type lane_boundary_type =
        hdmap::LaneBoundaryType::UNKNOWN;

    if (*left_neighbor_lane_borrowable) {
      lane_boundary_type = hdmap::LeftBoundaryType(waypoint);
      if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW ||
          lane_boundary_type == hdmap::LaneBoundaryType::DOUBLE_YELLOW ||
          lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
        *left_neighbor_lane_borrowable = false;
      }
      ADEBUG << "s[" << check_s << "] left_lane_boundary_type["
             << LaneBoundaryType_Type_Name(lane_boundary_type) << "]";
    }
    if (*right_neighbor_lane_borrowable) {
      lane_boundary_type = hdmap::RightBoundaryType(waypoint);
      if (lane_boundary_type == hdmap::LaneBoundaryType::SOLID_YELLOW ||
          lane_boundary_type == hdmap::LaneBoundaryType::SOLID_WHITE) {
        *right_neighbor_lane_borrowable = false;
      }
      ADEBUG << "s[" << check_s << "] right_neighbor_lane_borrowable["
             << LaneBoundaryType_Type_Name(lane_boundary_type) << "]";
    }
    check_s += 2.0;
  }
}

参考

[1] Apollo Planning决策规划代码详细解析 (8): PathLaneBorrowDecider
[2] Apollo规划模块详解(六):算法实现-path lane borrow decider
[3] https://www.cnblogs.com/icathianrain/p/14407619.html

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

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

相关文章

MySql014——分组的GROUP BY子句和排序ORDER BYSELECT子句顺序

前提&#xff1a;使用《MySql006——检索数据&#xff1a;基础select语句》中创建的products表 一、GROUP BY子句基础用法 SELECT vend_id, COUNT(*) AS num_prods FROMstudy.products GROUP BY vend_id;上面的SELECT语句指定了两个列&#xff0c;vend_id包含产品供应商的ID&…

Protobuf 语法详解

&#x1f337;&#x1f341; 博主猫头虎 带您 Go to New World.✨&#x1f341; &#x1f984; 博客首页——猫头虎的博客&#x1f390; &#x1f433;《面试题大全专栏》 文章图文并茂&#x1f995;生动形象&#x1f996;简单易学&#xff01;欢迎大家来踩踩~&#x1f33a; &a…

mybatisplus批量写入

1.新建MybatisPlusConfig /*** MybatisPlusConfig.*/ Configuration MapperScan("com.test.mapper") public class MybatisPlusConfig {/*** 自定义批量插入 SQL 注入器.*/Beanpublic InsertBatchSqlInjector insertBatchSqlInjector() {return new InsertBatchSqlI…

微服务(多级缓存)

多级缓存 1.什么是多级缓存 传统的缓存策略一般是请求到达Tomcat后&#xff0c;先查询Redis&#xff0c;如果未命中则查询数据库&#xff0c;如图&#xff1a; 存在下面的问题&#xff1a; 请求要经过Tomcat处理&#xff0c;Tomcat的性能成为整个系统的瓶颈Redis缓存失效时&…

【Kubernetes】Kubernetes的监控工具Promethues

Prometheus 一、Prometheus 概念1. Prometheus 概述2. Prometheus 的监控数据3. Prometheus 的特点4. Prometheus 和 zabbix 区别5. Prometheus 的生态组件5.1 Prometheus server5.2 Client Library5.3 Exporters5.4 Service Discovery5.5 Alertmanager5.6 Pushgateway5.7 Graf…

【PHP面试题81】php-fpm是什么?它和PHP有什么关系

文章目录 &#x1f680;一、前言&#xff0c;php-fpm是什么&#x1f680;二、php-fpm与PHP之间的关系&#x1f680;三、php-fpm解决的问题&#x1f50e;3.1 进程管理&#x1f50e;3.2 进程池管理&#x1f50e;3.3 性能优化&#x1f50e;3.4 并发处理 &#x1f680;四、php-fpm常…

每日一题——排序链表(递归 + 迭代)

排序链表&#xff08;递归 迭代&#xff09; 题目链接 注&#xff1a;本体的解法建立在归并排序的基础之上&#xff0c;如果对这一排序还不太了解&#xff0c;建议看看&#xff1a; &#x1f449;归并排序 &#x1f449;八大排序算法详解 &#x1f449;合并两个有序链表 既…

初学Zephyr系统,相关文档参考

https://docs.zephyrproject.org/3.0.0/reference/kconfig/index-all.html Zephyr下所有配置项Configuration Options 链接如上&#xff0c;写个博客防止自己找不到 我在调试NCS中的例程的时候会需要对prj.conf进行配置从而对Kconfig进行配置 BLE的相关API可参考https://do…

[oneAPI] 基于BERT预训练模型的英文文本蕴含任务

[oneAPI] 基于BERT预训练模型的英文文本蕴含任务 Intel DevCloud for oneAPI 和 Intel Optimization for PyTorch基于BERT预训练模型的英文文本蕴含任务语料介绍数据集构建 模型训练 结果参考资料 比赛&#xff1a;https://marketing.csdn.net/p/f3e44fbfe46c465f4d9d6c23e38e0…

Spring@Scheduled定时任务接入XXL-JOB的一种方案(基于SC Gateway)

背景 目前在职的公司&#xff0c;维护着Spring Cloud分布式微服务项目有25个。其中有10个左右微服务都写有定时任务逻辑&#xff0c;采用Spring Scheduled这种方式。 Spring Scheduled定时任务的缺点&#xff1a; 不支持集群&#xff1a;为避免重复执行&#xff0c;需引入分…

基于jeecg-boot的flowable流程加签功能实现

更多nbcio-boot功能请看演示系统 gitee源代码地址 后端代码&#xff1a; https://gitee.com/nbacheng/nbcio-boot 前端代码&#xff1a;https://gitee.com/nbacheng/nbcio-vue.git 在线演示&#xff08;包括H5&#xff09; &#xff1a; http://122.227.135.243:9888 今天我…

【C++】C++ 引用详解 ① ( 变量的本质 - 引入 “ 引用 “ 概念 | 引用语法简介 | 引用做函数参数 | 复杂类型引用做函数参数 )

文章目录 一、变量的本质 - 引入 " 引用 " 概念1、变量的本质 - 内存别名2、引入 " 引用 " 概念 - 已定义变量的内存别名3、" 引用 " 的优点 二、引用语法简介1、语法说明2、代码示例 - 引用的定义和使用 三、引用做函数参数1、普通引用必须初始…

minikube安装

minikube也是需要docker环境的&#xff0c;首先看一下docker 下载docker.repo源到本地 通过repo里面查找最新的docker 开始安装docker 修改docker 下载加速地址&#xff0c; systemctl deamon-reload 下载minikube minikube start | minikube curl -LO https://storage.goog…

Mybatis(二)映射文件配置与动态SQL

Mybatis&#xff08;二&#xff09;映射文件配置 1.Mybatis映射文件配置 1.入参 1.1.parameterType(了解) CRUD标签都有一个属性parameterType&#xff0c;底层的statement通过它指定接收的参数类型。入参数据有以下几种类型&#xff1a;HashMap&#xff0c;基本数据类型&a…

会计资料基础

会计资料 1.会计要素及确认与计量 1.1 会计基础 1.2 六项会计要素小结 1.3 利润的确认条件 1.3.1 利润的定义和确认条件 1.4 会计要素及确认条件 2.六项会计要素 2.1 资产的特征及其确认条件 这部分资产可以给企业带来经济收益&#xff0c;但是如果不能带来经济利益&#xff…

提升团队合作效率:企业网盘的文件管理和协作利用方法

随着信息技术的飞速发展&#xff0c;企业越来越依赖于网络和云服务来提高工作效率。在这样的背景下&#xff0c;企业网盘作为一种重要的在线存储和协作工具&#xff0c;正在被越来越多的企业所采用。本文将探讨如何利用企业网盘进行文件管理和协作&#xff0c;从而构建高效的团…

Windows快捷键常用介绍,提高工作(摸鱼)效率

一&#xff1a;背景 本文主要是讲解Windows电脑常见的快捷键&#xff0c;包括ctrl快捷键&#xff0c;win快捷键&#xff0c;不管是开发人员还是普通办公人员&#xff0c;都是很方便的。我们平时没事操作都是用鼠标去选择对应的功能&#xff0c;或者在我的电脑--控制面板寻找&a…

把matlab的m文件打包成单独的可执行文件

安装Matlab Compiler Adds-on在app里找到Application Compiler 选择要打包的文件matlab单独的运行程序的话需要把依赖的库做成runtime. 这里有两个选项. 上面那个是需要对方在联网的情况下安装, 安装包较小.下面那个是直接把runtime打包成安装程序, 大概由你的程序依赖的库的多…

谷粒商城环境搭建一:Docker容器部署

Docker容器部署 VMware虚拟机安装 参考&#xff1a;VMware虚拟机安装Linux教程 Docker安装 Linux安装Docker # 1.更新apt包索引 sudo apt-get update# 2.安装以下包以使apt可以通过HTTPS使用存储库&#xff08;repository&#xff09; sudo apt-get install -y apt-transpor…

滑动窗口介绍

1.基本概念 利用单调性&#xff0c;使用同向双指针&#xff0c;两个指针之间形成一个窗口 子串与子数组都是连续的一段子序列时不连续的 2.为什么可以用滑动窗口&#xff1f; 暴力解决时发现两个指针不需要回退&#xff08;没必要回退&#xff0c;一定不会符合结果&#xf…