Apollo9.0 Lattice Planner算法源码学习

2024-03-20 22:44
文章标签 算法 源码 学习 planner lattice apollo9.0

本文主要是介绍Apollo9.0 Lattice Planner算法源码学习，希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

一、概述

主文件：lattice_planner.cc，相关路径如下：

modules\planning\planners\lattice\lattice_planner.cc
modules\planning\planners\lattice\lattice_planner.h

 主程序函数：

Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {}

二、主程序学习(详细注释) 

/******************************************************************************* Copyright 2017 The Apollo Authors. All Rights Reserved.** Licensed under the Apache License, Version 2.0 (the "License");* you may not use this file except in compliance with the License.* You may obtain a copy of the License at** http://www.apache.org/licenses/LICENSE-2.0** Unless required by applicable law or agreed to in writing, software* distributed under the License is distributed on an "AS IS" BASIS,* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.* See the License for the specific language governing permissions and* limitations under the License.*****************************************************************************//*** @file**/#include "modules/planning/planners/lattice/lattice_planner.h"#include <limits>
#include <memory>
#include <utility>
#include <vector>#include "cyber/common/log.h"
#include "cyber/common/macros.h"
#include "cyber/time/clock.h"
#include "modules/common/math/cartesian_frenet_conversion.h"
#include "modules/common/math/path_matcher.h"
#include "modules/planning/planners/lattice/behavior/collision_checker.h"
#include "modules/planning/planners/lattice/behavior/path_time_graph.h"
#include "modules/planning/planners/lattice/behavior/prediction_querier.h"
#include "modules/planning/planners/lattice/trajectory_generation/backup_trajectory_generator.h"
#include "modules/planning/planners/lattice/trajectory_generation/lattice_trajectory1d.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory1d_generator.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory_combiner.h"
#include "modules/planning/planners/lattice/trajectory_generation/trajectory_evaluator.h"
#include "modules/planning/planning_base/gflags/planning_gflags.h"
#include "modules/planning/planning_base/math/constraint_checker/constraint_checker.h"namespace apollo {
namespace planning {using apollo::common::ErrorCode;
using apollo::common::PathPoint;
using apollo::common::Status;
using apollo::common::TrajectoryPoint;
using apollo::common::math::CartesianFrenetConverter;
using apollo::common::math::PathMatcher;
using apollo::cyber::Clock;namespace {//---------该函数将输入的参考线进行离散化---------//
//---------输入：为ReferencePoint类型的vector----//
//---------输出：为PathPoint类型的vector---------//
std::vector<PathPoint> ToDiscretizedReferenceLine(const std::vector<ReferencePoint>& ref_points) {double s = 0.0;std::vector<PathPoint> path_points;for (const auto& ref_point : ref_points) {PathPoint path_point;    //-----Pathpoint相当于定义的一种结构体,包含了很多路径点信息path_point.set_x(ref_point.x());path_point.set_y(ref_point.y());path_point.set_theta(ref_point.heading());path_point.set_kappa(ref_point.kappa());path_point.set_dkappa(ref_point.dkappa());if (!path_points.empty()) {double dx = path_point.x() - path_points.back().x();double dy = path_point.y() - path_points.back().y();s += std::sqrt(dx * dx + dy * dy);    //-----s的计算方式：以直代曲！}path_point.set_s(s);path_points.push_back(std::move(path_point));}return path_points;
}//---------该函数将计算规划起点的Frenet坐标（状态）---------//
//---------其中调用了Cartesian 转 Frenet坐标的函数---------//
void ComputeInitFrenetState(const PathPoint& matched_point,const TrajectoryPoint& cartesian_state,std::array<double, 3>* ptr_s,std::array<double, 3>* ptr_d) {CartesianFrenetConverter::cartesian_to_frenet(matched_point.s(), matched_point.x(), matched_point.y(),matched_point.theta(), matched_point.kappa(), matched_point.dkappa(),cartesian_state.path_point().x(), cartesian_state.path_point().y(),cartesian_state.v(), cartesian_state.a(),cartesian_state.path_point().theta(),cartesian_state.path_point().kappa(), ptr_s, ptr_d);
}}  // namespace/*-----------------------------------------------
以下的planner函数就是规划主函数
输入为：planning_start_point 规划起点；frame 一次规划所需要的所有环境信息
ptr_computed_trajectory 待规划的轨迹？？
-----------------------------------------------*/
Status LatticePlanner::Plan(const TrajectoryPoint& planning_start_point,Frame* frame,ADCTrajectory* ptr_computed_trajectory) {size_t success_line_count = 0;size_t index = 0;
/*-----------------------------------------------
由于一个规划帧开始之前有定位和导航routing模块都会得到若干
条参考线，因此规划开始之前要根据给定的cost计算每条参考
线的cost，然后选择其中cost最低的一条进行离散化。SetPriorityCost
-----------------------------------------------*/for (auto& reference_line_info : *frame->mutable_reference_line_info()) {if (index != 0) {reference_line_info.SetPriorityCost(FLAGS_cost_non_priority_reference_line);} else {reference_line_info.SetPriorityCost(0.0);}/*-----------------------------------------------PlanOnReferenceLine()该函数为Lattice算法中最重要的函数，即：在选定cost最优的参考线之后，在该参考线上进行规划！-----------------------------------------------*/auto status =PlanOnReferenceLine(planning_start_point, frame, &reference_line_info);if (status != Status::OK()) {if (reference_line_info.IsChangeLanePath()) {AERROR << "Planner failed to change lane to "<< reference_line_info.Lanes().Id();} else {AERROR << "Planner failed to " << reference_line_info.Lanes().Id();}} else {success_line_count += 1;}++index;}if (success_line_count > 0) {return Status::OK();}return Status(ErrorCode::PLANNING_ERROR,"Failed to plan on any reference line.");
}/*-----------------------------------------------
对每一条参考线都会执行以下规划（？）
以下为PlanOnReferenceLine()的具体实现，分为7个步骤：
1、离散化参考线上的点，并计算s的值（目的：以直代曲，
为了在进行坐标转化以及计算障碍物距离自车的s坐标的时
候可以使用，如制作index2s表格，即根据参考线上点的索
引号映射到s值的表）
2、在参考线上计算“规划起点”的匹配点
3、根据匹配点，计算Frenet坐标系的S-L值
4、计算障碍物的S-T图（斜率表示速度）
5、生成纵横向采样路径
6、计算cost值，进行碰撞检测（依据S-T图）和动力学约束检测
7、优选出cost值最小的trajectory
-----------------------------------------------*/
Status LatticePlanner::PlanOnReferenceLine(const TrajectoryPoint& planning_init_point, Frame* frame,ReferenceLineInfo* reference_line_info) {static size_t num_planning_cycles = 0;static size_t num_planning_succeeded_cycles = 0;double start_time = Clock::NowInSeconds();double current_time = start_time;ADEBUG << "Number of planning cycles: " << num_planning_cycles << " "<< num_planning_succeeded_cycles;++num_planning_cycles;reference_line_info->set_is_on_reference_line();// 1. obtain a reference line and transform it to the PathPoint format.// 以下为参考线离散化的过程：auto ptr_reference_line =std::make_shared<std::vector<PathPoint>>(ToDiscretizedReferenceLine(reference_line_info->reference_line().reference_points()));// 2. compute the matched point of the init planning point on the reference// line.// 以下将计算规划起点的匹配点// 该函数的实现是在 path_matcher.cc 文件里面PathPoint matched_point = PathMatcher::MatchToPath(*ptr_reference_line, planning_init_point.path_point().x(),planning_init_point.path_point().y());// 3. according to the matched point, compute the init state in Frenet frame

这篇关于Apollo9.0 Lattice Planner算法源码学习的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！


http://www.chinasem.cn/article/830994。

相关文章

Java中的雪花算法Snowflake解析与实践技巧

Java中的雪花算法Snowflake解析与实践技巧

《Java中的雪花算法Snowflake解析与实践技巧》本文解析了雪花算法的原理、Java实现及生产实践,涵盖ID结构、位运算技巧、时钟回拨处理、WorkerId分配等关键点,并探讨了百度UidGen... 目录一、雪花算法核心原理1.1 算法起源1.2 ID结构详解1.3 核心特性二、Java实现解析2.
阅读更多...
Go学习记录之runtime包深入解析

Go学习记录之runtime包深入解析

《Go学习记录之runtime包深入解析》Go语言runtime包管理运行时环境,涵盖goroutine调度、内存分配、垃圾回收、类型信息等核心功能,：本文主要介绍Go学习记录之runtime包的... 目录前言：一、runtime包内容学习1、作用：① Goroutine和并发控制：② 垃圾回收：③ 栈和
阅读更多...
Android学习总结之Java和kotlin区别超详细分析

Android学习总结之Java和kotlin区别超详细分析

《Android学习总结之Java和kotlin区别超详细分析》Java和Kotlin都是用于Android开发的编程语言,它们各自具有独特的特点和优势,：本文主要介绍Android学习总结之Ja... 目录一、空安全机制真题 1：Kotlin 如何解决 Java 的 NullPointerExceptio
阅读更多...
8种快速易用的Python Matplotlib数据可视化方法汇总(附源码)

8种快速易用的Python Matplotlib数据可视化方法汇总(附源码)

《8种快速易用的PythonMatplotlib数据可视化方法汇总(附源码)》你是否曾经面对一堆复杂的数据,却不知道如何让它们变得直观易懂？别慌,Python的Matplotlib库是你数据可视化的... 目录引言1. 折线图（Line Plot）——趋势分析2. 柱状图（Bar Chart）——对比分析3
阅读更多...
使用雪花算法产生id导致前端精度缺失问题解决方案

使用雪花算法产生id导致前端精度缺失问题解决方案

《使用雪花算法产生id导致前端精度缺失问题解决方案》雪花算法由Twitter提出,设计目的是生成唯一的、递增的ID,下面：本文主要介绍使用雪花算法产生id导致前端精度缺失问题的解决方案,文中通过代... 目录一、问题根源二、解决方案1. 全局配置Jackson序列化规则2. 实体类必须使用Long封装类3.
阅读更多...
重新对Java的类加载器的学习方式

重新对Java的类加载器的学习方式

《重新对Java的类加载器的学习方式》：本文主要介绍重新对Java的类加载器的学习方式,具有很好的参考价值,希望对大家有所帮助,如有错误或未考虑完全的地方,望不吝赐教... 目录1、介绍1.1、简介1.2、符号引用和直接引用1、符号引用2、直接引用3、符号转直接的过程2、加载流程3、类加载的分类3.1、显示
阅读更多...
Springboot实现推荐系统的协同过滤算法

Springboot实现推荐系统的协同过滤算法

《Springboot实现推荐系统的协同过滤算法》协同过滤算法是一种在推荐系统中广泛使用的算法,用于预测用户对物品（如商品、电影、音乐等）的偏好,从而实现个性化推荐,下面给大家介绍Springboot... 目录前言基本原理 算法分类 计算方法应用场景 代码实现 前言协同过滤算法（Collaborativ
阅读更多...
Android实现一键录屏功能(附源码)

Android实现一键录屏功能(附源码)

《Android实现一键录屏功能(附源码)》在Android5.0及以上版本,系统提供了MediaProjectionAPI,允许应用在用户授权下录制屏幕内容并输出到视频文件,所以本文将基于此实现一个... 目录一、项目介绍二、相关技术与原理三、系统权限与用户授权四、项目架构与流程五、环境配置与依赖六、完整
阅读更多...
Android实现定时任务的几种方式汇总(附源码)

Android实现定时任务的几种方式汇总(附源码)

《Android实现定时任务的几种方式汇总(附源码)》在Android应用中,定时任务（ScheduledTask）的需求几乎无处不在：从定时刷新数据、定时备份、定时推送通知,到夜间静默下载、循环执行... 目录一、项目介绍1. 背景与意义二、相关基础知识与系统约束三、方案一：Handler.postDel
阅读更多...
Java学习手册之Filter和Listener使用方法

Java学习手册之Filter和Listener使用方法

《Java学习手册之Filter和Listener使用方法》：本文主要介绍Java学习手册之Filter和Listener使用方法的相关资料,Filter是一种拦截器,可以在请求到达Servl... 目录一、Filter（过滤器）1. Filter 的工作原理2. Filter 的配置与使用二、Listen
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2