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深度学习库DJL实现Python的NumPy方式

Java深度学习库DJL实现Python的NumPy方式

《Java深度学习库DJL实现Python的NumPy方式》本文介绍了DJL库的背景和基本功能,包括NDArray的创建、数学运算、数据获取和设置等,同时,还展示了如何使用NDArray进行数据预处理... 目录1 NDArray 的背景介绍1.1 架构2 JavaDJL使用2.1 安装DJL2.2 基本操
阅读更多...
Go中sync.Once源码的深度讲解

Go中sync.Once源码的深度讲解

《Go中sync.Once源码的深度讲解》sync.Once是Go语言标准库中的一个同步原语,用于确保某个操作只执行一次,本文将从源码出发为大家详细介绍一下sync.Once的具体使用,x希望对大家有... 目录概念简单示例源码解读总结概念sync.Once是Go语言标准库中的一个同步原语，用于确保某个操
阅读更多...
Python中的随机森林算法与实战

Python中的随机森林算法与实战

《Python中的随机森林算法与实战》本文详细介绍了随机森林算法,包括其原理、实现步骤、分类和回归案例,并讨论了其优点和缺点,通过面向对象编程实现了一个简单的随机森林模型,并应用于鸢尾花分类和波士顿房... 目录1、随机森林算法概述2、随机森林的原理3、实现步骤4、分类案例：使用随机森林预测鸢尾花品种4.1
阅读更多...
Java汇编源码如何查看环境搭建

Java汇编源码如何查看环境搭建

《Java汇编源码如何查看环境搭建》：本文主要介绍如何在IntelliJIDEA开发环境中搭建字节码和汇编环境,以便更好地进行代码调优和JVM学习,首先,介绍了如何配置IntelliJIDEA以方... 目录一、简介二、在IDEA开发环境中搭建汇编环境2.1 在IDEA中搭建字节码查看环境2.1.1 搭建步
阅读更多...
HarmonyOS学习(七)——UI（五）常用布局总结

HarmonyOS学习(七)——UI（五）常用布局总结

自适应布局 1.1、线性布局（LinearLayout） 通过线性容器Row和Column实现线性布局。Column容器内的子组件按照垂直方向排列，Row组件中的子组件按照水平方向排列。 属性说明space通过space参数设置主轴上子组件的间距，达到各子组件在排列上的等间距效果alignItems设置子组件在交叉轴上的对齐方式，且在各类尺寸屏幕上表现一致，其中交叉轴为垂直时，取值为Vert
阅读更多...
Ilya-AI分享的他在OpenAI学习到的15个提示工程技巧

Ilya-AI分享的他在OpenAI学习到的15个提示工程技巧

Ilya（不是本人，claude AI）在社交媒体上分享了他在OpenAI学习到的15个Prompt撰写技巧。 以下是详细的内容： 提示精确化:在编写提示时,力求表达清晰准确。清楚地阐述任务需求和概念定义至关重要。例:不用"分析文本",而用"判断这段话的情感倾向:积极、消极还是中性"。 快速迭代:善于快速连续调整提示。熟练的提示工程师能够灵活地进行多轮优化。例:从"总结文章"到"用
阅读更多...
不懂推荐算法也能设计推荐系统

不懂推荐算法也能设计推荐系统

本文以商业化应用推荐为例，告诉我们不懂推荐算法的产品，也能从产品侧出发， 设计出一款不错的推荐系统。 相信很多新手产品，看到算法二字，多是懵圈的。 什么排序算法、最短路径等都是相对传统的算法（注：传统是指科班出身的产品都会接触过）。但对于推荐算法，多数产品对着网上搜到的资源，都会无从下手。特别当某些推荐算法 和 “AI”扯上关系后，更是加大了理解的难度。 但，不了解推荐算法，就无法做推荐系
阅读更多...
【前端学习】AntV G6-08 深入图形与图形分组、自定义节点、节点动画（下）

【前端学习】AntV G6-08 深入图形与图形分组、自定义节点、节点动画（下）

【课程链接】 AntV G6：深入图形与图形分组、自定义节点、节点动画（下）_哔哩哔哩_bilibili 本章十吾老师讲解了一个复杂的自定义节点中，应该怎样去计算和绘制图形，如何给一个图形制作不间断的动画，以及在鼠标事件之后产生动画。（有点难，需要好好理解） <!DOCTYPE html><html><head><meta charset="UTF-8"><title>06
阅读更多...
学习hash总结

学习hash总结

2014/1/29/   最近刚开始学hash，名字很陌生，但是hash的思想却很熟悉，以前早就做过此类的题，但是不知道这就是hash思想而已，说白了hash就是一个映射，往往灵活利用数组的下标来实现算法，hash的作用：1、判重；2、统计次数；
阅读更多...
康拓展开（hash算法中会用到）

康拓展开（hash算法中会用到）

康拓展开是一个全排列到一个自然数的双射（也就是某个全排列与某个自然数一一对应） 公式： X=a[n]*(n-1)!+a[n-1]*(n-2)!+...+a[i]*(i-1)!+...+a[1]*0! 其中,a[i]为整数,并且0<=a[i]<i,1<=i<=n。（a[i]在不同应用中的含义不同）； 典型应用： 计算当前排列在所有由小到大全排列中的顺序，也就是说求当前排列是第
阅读更多...

Copyright China编程 23002807@qq.com

沪ICP备2023033072号-2