2.参考线平滑算法解析及实现
星火计划2.0基础课:https://apollo.baidu.com/community/online-course/2
星火计划2.0专项课:https://apollo.baidu.com/community/online-course/12
中间这一块是planning的职责
下面来梳理一些apollo参考线的重要的数据结构:
1.ReferenceLine数据结构
其存储了参考线的所有的信息:
SpeedLimit表示起点、重点的位置,以及该路径上的速度限制
而SpeedLimit又是一个数组,存放在speed_limitb_变量里,表示某一段路径有多段的速度限制
ReferencePoint也是一个数组,表示参考线中具体的离散点
Map_path表示地图中的参考线,其存放的参考点和参考线是一致的,为一一对应的关系。
接下来学习ReferencePoint
最上面的父类Vec2d,包含了参考点的二维坐标,MapPathPoint继承自Vec2d,MapPathPoint增加了两个重要的数据结构,一个点的朝向,另一个是地图上的点对应道路上的点(对应Map_path),因为LaneWayPoint是一个数组,地图上的车道有可能有重合的地方,所以地图上的一个点可能同属于多个车道,所以就会有多个LaneWayPoint。ReferencePoint由MapPathPoint继承得来,其描述了参考线当中的点,除了其父类中的信息之外,又增加了曲线的曲率和曲率的导数。
以下是处理参考线的流程:
在planning模块主要有两个任务(两个线程),一个为参考线提供的线程,另一个是规划线程,其根据参考线提供线程输出的参考线进行相应的路径规划。提供的线程在直行的情况下只会生成一条参考线,如果有换道的情况就会有多条参考线以供换道的选择。对于参考线的提供者(ReferenceLineProvider),其会根据车辆的位置及PNC的地图来生成参考线,其对参考线的处理主要分成了两部分:一部分是参考线的生成,另一部分是参考线的平滑。
参考线的平滑主要支持三种方式:离散点的平滑,螺旋线的平滑,样条曲线的平滑。apollo中主要采用的是离散点的平滑。
下面讲解参考线的生成:
其主要在CreateReferenceLine这个函数中进行,首先其根据UpdateVehicleState(获取车辆状态),然后获取Routing结果,根据routing的结果的路线里去生成RouteSegments(路径片段),然后在对参考线进行平滑和分割,如果没有新的routing进来就可以用上一个周期处理的参考线(延用,以减少计算量)。
接下来介绍参考线平滑算法:
参考线平滑算法一共有三种:
一种是离散点的平滑,另一种是螺旋线的平滑,最后是样条曲线的平滑,apollo中主要使用离散点的平滑,其包含两种平滑的方法:costheta平滑算法、FemPos平滑算法。
可以通过配置参数选择要采用的平滑算法:
参考线的平滑函数在ReferenceProvider这个类当中(SmoothReferenceLine)
可以看到,该函数的输入是raw_reference_line(猜测使用了引用应该就是输入),输出是reference_line(猜测使用了指针就是输出),raw_reference_line经过了设置中间点和平滑,最终输出了平滑后的结果smoothed_reference_line。
现在来讲解如何设置中间点:
第一步:根据原始的参考线来选取中间点
这一步就是根据referenceline的S值在纵向的投影进行一个均匀的采样,采样的间隔大概是0.25米,采样完毕后就能得到一系列的AnchorPoint,每个AnchorPoint包含了一个path_point和一个横纵向的裕度(采样点周围可以平移的空间)。强约束表示必须必须严格遵守横纵向的裕度,所以在平滑过程中只有首尾两个点是强约束,中间的点都不是强约束。
参考线平滑比较重要的默认配置主要为:纵向边界(2.0),横向边界(最大值0.5,最小值0.1)
参考线平滑中主体的函数(smooth函数),是通过原始参考线上的离散点有限的平移对原始的参考线进行平滑。
可以看到这个类当中还有两个不同的平滑算法,分别是刚才提到的costheta和FemPos平滑算法,这两种通过在配置文件中的配置去选择。
现在来看FemPos平滑算法:
从其输入的参数来看,主要是AnchorPoint中的x,y和横纵向的裕度,输出是平滑后的point。
首先对横向和纵向的裕度进行一定的收缩,收缩后对二次化的问题进行求解,现在来看求解函数:
可以看到其有三种求解方式,分别是利用不同的求解器实现了求解的方法,如果考虑了曲率的约束,该优化问题就是非线性的,就可以用Ipopt非线性求解器进行求解,也可以将曲率约束进行线性化之后用Osqp进行求解。
如果不考虑曲率约束的话,就可以直接用Osqp进行求解该二次优化的问题。
目前apollo中默认的是不考虑曲率约束,这样求解会快一些。
下面是默认的平滑的求解过程,其默认的是基于离散点的平滑,把参考线平滑构造成了一个二次优化的问题,并且使用Osqp求解器进行求解
二次优化问题,主要的优化变量就是Anchor Point的离散点,优化目标是平滑度、参考线的长度、相对原始路线的偏移量,优化函数cost有三部分组成:平滑度、长度、两个点偏离误差。Cost length通过计算两点的长度来的草,偏移误差通过各个点偏移量的求和得到。
对于cost smooth,其有两种计算方式:FemPos和CosTheta
FemPosSmooth的计算方式:
假设参考线上有相邻的三个点,这三个点组成两个向量(蓝色箭头),其结果相当于这两个向量和向量的模的平方。若P0,P1,P2在同一条线上,P1,Ps就会比较小,得到的值也就比较小,相应的平滑度的代价也就比较小。
如果这三个点组成的两个向量的夹角越小,那么和向量的值就越大,平滑度的代价也就越大。所以最终得到的值和平滑度是相关的。
CosTheta的计算方式:
可以理解为相邻两个向量夹角的cos值,这个值越大表示夹角越小,P0,P1,P2也就越接近直线,此时参考线也就越平滑,所以平滑度的代价可以用costheta的负值来表示,这种计算方式需要采用非线性优化器。
接下来讲解二次优化问题的约束条件:
我们希望平滑度的参考线距离原始路线的偏移量不能太大,虽然在代价函数中已经有了偏移量的惩罚项,但是惩罚项只是一个软约束,不能保证偏离的具体数值能够确定的限制在这个范围内,所以这里添加了关于偏移量的硬约束,偏移的范围就是设置的横纵向裕度构成的box(黄色方块)范围内,box构成的约束方程就是对x和y分别进行限制。
第二个是曲率的约束,曲率的约束在apollo中是通过近似的假设,找到相邻的三个点,其三个点的外接圆,外接圆之间的离散点是均匀采样的,两两之间的距离是相等的(第一个假设),第二个假设,相邻线段之间的夹角很小,第三个假设弧长与弦长接近。经过这三个假设进行推导,换算出来我们对曲率的约束,根据曲率的方程,得到如下的约束方程:
以上就是参考线的平滑算法计算原理。
2.参考线平滑算法解析及实现相关推荐
- Apollo星火计划学习笔记——参考线平滑算法解析及实现(以U型弯道场景仿真调试为例)
文章目录 1. Apollo参考线介绍 1.1 参考线的作用 1.2 导航规划的路线 1.3 为什么需要重新生成参考线 1.4 ReferenceLine数据结构 1.5 ReferencePoint ...
- Apollo planning之参考线平滑算法
Apollo studio 官网:Apollo开发者社区 (baidu.com) 星火计划2.0基础课:Apollo星火计划2.0_Apollo精品课 (baidu.com) 星火计划2.0专项课:A ...
- 无人驾驶算法——Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑
无人驾驶算法--Baidu Apollo代码解析之ReferenceLine Smoother参考线平滑 Apollo 参考线平滑类 reference_line_provider.cc 代价函数 c ...
- 激光IMU融合——LIO-Mapping / LIOM / LINS / LIO-SAM算法解析
激光IMU融合--LIO-Mapping / LIOM / LINS / LIO-SAM算法解析 激光IMU融合--LIO-Mapping / LIOM / LINS / LIO-SAM算法解析 1. ...
- AI学习笔记(七)图像滤波器、OpenCV算法解析
AI学习笔记之图像滤波器.OpenCV算法解析 图像滤波器 图像噪声 噪声的产生 信噪比 高斯噪声 椒盐噪声 其他噪声 图像滤波 滤波的目的 滤波的要求 各种滤波器 均值滤波 中值滤波 最大最小值滤波 ...
- 八种常见回归算法解析及代码
目录 一.线性回归 1.最小二乘法 -导数/偏导为0求参数 最小二乘法求解参数优缺点 2.迭代求解参数-梯度下降.坐标轴下降.最小角回归 2.1使用梯度下降-对回归系数中w的每个元素分别求偏导并乘 ...
- R语言平滑算法LOESS局部加权回归、三次样条、变化点检测拟合电视节目《白宫风云》在线收视率
最近我们被客户要求撰写关于LOESS的研究报告,包括一些图形和统计输出. 此示例基于电视节目的在线收视率.我们将从抓取数据开始. # 加载软件包. packages <- c("gpl ...
- 飞利浦公司音频指纹识别算法解析
飞利浦公司音频指纹识别算法解析 一个音频指纹系统通常包括两个部分:即一个计算听觉重要特征的指纹提取算法和一个在指纹数据库中进行有效搜索的比对算法.当要识别一段未知音频时,首先按照指纹提取算法计算其音频 ...
- 回归算法分类,常用回归算法解析
回归算法分类,常用回归算法解析 回归是数学建模.分类和预测中最古老但功能非常强大的工具之一.回归在工程.物理学.生物学.金融.社会科学等各个领域都有应用,是数据科学家常用的基本工具. 回归通常是机器学 ...
最新文章
- 《XML程序设计》要点
- mpvue生命周期初探
- 世界最厉害的14位程序员,你认识几个?
- 遭银行账号诈骗最快最有效自救法
- easyui-combobox
- 【转】什么是磁珠(Ferrite Bead 即 FB)
- linux驱动编写(字符设备编写框架)
- vue路由配置src/router/index.js
- 计算机中丢失api-ms 解决方法
- 《深入剖析Tomcat》源码
- macbook pro python开发_年轻人第一台 Mac,来自一个开发者的 Macbook Pro 2019 16寸简评...
- Python的Numpy库简述
- mysql卸载注意问题_mysql卸载注意事项
- 10个免费的web压力测试工具
- macbook更新windows11
- B站李永乐讲解傅里叶变换--笔记
- 关于网络、交换机、路由器
- 线性代数(2)向量线性组合、向量内积的意义
- 谷歌浏览器87版本 iframe_谷歌Chrome的“混合内容”更新将会影响电商网站,自建站卖家如何应对?...
- ImportError: DLL load failed while importing qhull:问题解决