说明

博主仅凭代码本身进行理解，并未对轨迹平滑，速度平滑，曲线优化等部分有任何理解（毕竟菜鸡，写这个也只是为了留个档）
如本文说明有任何问题欢迎评论提醒指点，但拒绝指指指指指指点点点点点点

总结

模块代码
该模块的功能是对已经提供出来的路径（含有速度信息的路径），进行速度上的平滑

如图

速度原本是黑色的折线，不连续，通过trajectory_smoother变成平滑的速度曲线（画的有亿点难看233）

代码解读

函数 TrajectorySmoother

该函数作用是通过指数函数生成一张权重表

TrajectorySmoother::TrajectorySmoother(TrajectorySmootherConfig config)
{// 参数s 的作用是作为除数，具体用在后面的x上面float32_t s = 2 * config.standard_deviation * config.standard_deviation;float32_t sum = 0.0;// Generate a gaussian filter kernel// 这里的kernel_size应该可以理解为n，即预先定义了每个点的速度由周围n个点影响，n越大整个速度曲线会越平滑#i从1到kernel_size就有点像是1/n,2/n,.....,(n-1)/n枚举每个点位置的权重for (std::size_t i = 0; i < config.kernel_size; ++i) {// x是计算当前点到n/2的距离，即权重在点n/2的时候取最小（在这里是0）（但通过后面指数函数分析其实这里反而是最大的），向两边扩散时权重以平方的量级扩大int32_t x = static_cast<int32_t>(i - config.kernel_size / 2);// 这里就是通过指数函数的方式，利用上面制作出来的平方权重表，制造一个指数函数的权重表（什么鬼形容啊。。。）float32_t value = std::exp(static_cast<float32_t>(-(x * x)) / s);m_kernel.push_back(value);sum += value;}// Normalize the kernel//对权重表进行归一化for (std::size_t i = 0; i < config.kernel_size; ++i) {m_kernel[i] /= sum;}
}

总的来说这个函数最终会生成一张长度为kernel_size的权重表，其中所有权重之和为1

函数 Filter

该函数是通过预先计算好的权重表，对现有路径的速度信息进行平滑处理

void TrajectorySmoother::Filter(Trajectory & trajectory)
{if (trajectory.points.size() > 2) {// zero out velocity at a few points at the end of trajectory so that the post filter velocity// gradually ramp down to zero. The last point would have already been zeroed by the// estimator.//zero_run_length是速度为0的路径长度//即路径规划到终点的时候车应该停在终点。//但显然如果只设置终点的速度为0，而终点前的点速度是99999999的话// 车不可能停在终点，所以设置了一个zero_run_length参数//旨在通过一段zero_run_length长度的速度为0的路径点，让车停下来std::size_t zero_run_length = std::min(trajectory.points.size() / 2, m_kernel.size() / 2);for (std::size_t i = trajectory.points.size() - 1 - zero_run_length;i < trajectory.points.size() - 1; ++i){trajectory.points[i].longitudinal_velocity_mps = 0;}/*这部分就是通过计算每一个点以及他前后kernel_size/2的点的加权和重新计算路径在当前点的速度*/// avoid changing the start and end point of trajectory// use same padding for points beyond either end of the trajectorystd::vector<float32_t> velocity_profile{};for (std::size_t i = 1; i < trajectory.points.size() - 1; ++i) {float32_t sum = 0;for (std::size_t j = 0; j < m_kernel.size(); ++j) {std::int32_t points_index = static_cast<int32_t>(i - (m_kernel.size() / 2) + j);if (points_index < 0) {points_index = 0;} else if (points_index >= static_cast<int32_t>(trajectory.points.size())) {points_index = static_cast<int32_t>(trajectory.points.size() - 1);}sum +=trajectory.points[static_cast<std::size_t>(points_index)].longitudinal_velocity_mps *m_kernel[j];}velocity_profile.push_back(sum);}//最后再对速度进行重新赋值回去，同时不改变初始点以及终点的速度// Apply the velocity profilefor (std::size_t i = 1; i < trajectory.points.size() - 1; ++i) {trajectory.points[i].longitudinal_velocity_mps = velocity_profile[i - 1];}}
}

autoware planning trajectory_smoother 模块解读相关推荐

解读Autoware.Universe规划模块：Behavior Path Planner
解读Autoware.Universe规划模块:Behavior Path Planner 前言文章列表行为路径规划器(Behavior Path Planner) 目的(Purpose) 场景( ...
python ico_Python协程asynico模块解读
为了搞清楚asynico模块的具体作用,那我们要先明白一些基本概念.一般在爬虫里面,为了加快速度,我们可以使用多进程.多线程.协程,这篇文章详细解读一下这三者的区别的区别.在一个主程序运行的时候,会有 ...
PyTorch框架：（6）图像识别实战常用模块解读
1.TorchVision 官网:torchvision - Torchvision 0.10.0 documentation 在torchvision这个模块当中,包含了很多后续需要的功能: 需要自 ...
skimage exposure模块解读
exposure模块包括: 直方图均衡化 gamma调整.sigmoid调整.log调整判断图像是否对比度太低 exposure模块包括以下函数: histogram 统计颜色的直方图,基于nump ...
【ceph】ceph-mds journal模块解读
目录简单说明为什么需要journal 详细分析简单说明 Journaler类负责将内存中已经序列化的数据写入到对应metadata pool中的obj中,同时提供读取该obj的接口当mds处理 ...
autoware下ndt_mapping节点解读
利用激光雷达进行建图,首先需要得到稠密点云,然后进行体素滤波进行过滤得到包含特征的点云数据.接着利用每一帧扫描的点云地图进行ndt配准逐帧拼接,最后能够得到激光雷达扫描路径下的整体点云地图. ndt_ ...
【导航业务框架】开源无人驾驶项目autoware解读
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.Autoware的整体框架和模块 1.Autoware介绍 2. ...
Apollo Planning模块源代码分析
严正声明:本文系作者davidhopper原创,未经允许,严禁转载! Apollo项目(https://github.com/ApolloAuto/apollo)规划(Planning)模块位于命名空 ...
Autoware入门学习（三）——Autoware软件功能使用介绍（1/3）及demo演示
文章目录 1 Autoware 的官方 demo 演示 1.1 数据下载(点击链接or指令均可) 1.2 Demo 运行 2 Autoware 软件功能使用介绍(1/3) 2.1 Quick Star ...

autoware planning trajectory_smoother 模块解读

autoware planning trajectory_smoother 模块解读

说明

总结

代码解读

函数 TrajectorySmoother

函数 Filter

autoware planning trajectory_smoother 模块解读相关推荐

最新文章

热门文章