CN114114930A - 汽车局部参考路径生成方法、装置、设备及介质 - Google Patents
汽车局部参考路径生成方法、装置、设备及介质 Download PDFInfo
- Publication number
- CN114114930A CN114114930A CN202210103608.1A CN202210103608A CN114114930A CN 114114930 A CN114114930 A CN 114114930A CN 202210103608 A CN202210103608 A CN 202210103608A CN 114114930 A CN114114930 A CN 114114930A
- Authority
- CN
- China
- Prior art keywords
- local reference
- point
- automobile
- path
- reference path
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 238000005070 sampling Methods 0.000 claims abstract description 50
- 230000006870 function Effects 0.000 claims abstract description 46
- 238000005457 optimization Methods 0.000 claims abstract description 28
- 238000009826 distribution Methods 0.000 claims abstract description 23
- 238000010606 normalization Methods 0.000 claims abstract description 13
- 230000008447 perception Effects 0.000 claims abstract description 10
- 238000009499 grossing Methods 0.000 claims description 40
- 238000004364 calculation method Methods 0.000 claims description 17
- 230000008569 process Effects 0.000 claims description 13
- 239000000126 substance Substances 0.000 claims description 13
- 238000004590 computer program Methods 0.000 claims description 6
- 238000010586 diagram Methods 0.000 description 7
- 238000004891 communication Methods 0.000 description 5
- 230000006399 behavior Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 239000000463 material Substances 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 239000000470 constituent Substances 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000013178 mathematical model Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000010355 oscillation Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Traffic Control Systems (AREA)
Abstract
本申请涉及路径搜索技术领域,特别涉及一种汽车局部参考路径生成方法、装置、设备及介质,方法包括:以基于汽车环境感知信息计算的当前导航路点上的匹配点为采样初始点,按照采样点个数沿汽车前进方向及相反方向取多个路点,生成局部参考路径原始点集;基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集;采用多项式曲线连接平滑的局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的汽车的局部参考路径。由此,快速获得当前汽车位置匹配点,提高局部参考路径生成效率。
Description
技术领域
本申请涉及路径搜索技术领域,特别涉及一种汽车局部参考路径生成方法、装置、设备及介质。
背景技术
局部参考路径是自动驾驶汽车行为决策与运动规划的基础和前提,其平滑程度及生成效率决定了自动驾驶汽车的控制效果。为实现在线快速生成局部参考路径,目前主要有两种解决方案:(1)降低规划频率;(2)充分利用上一周期生成的参考路径。另外,如何快速找到自车当前位置在导航路点上的匹配点也直接影响局部参考路径的生成效率,通常做法是在每个规划周期遍历导航路径点,寻找距离最近的导航点作为匹配点,但对于导航路径较长的情况将会导致搜索时间成本过高,如果能充分利用上一个周期局部参考路径信息,从上一个周期的匹配点开始并沿汽车行驶方向搜索,将能大大提高匹配效率。
相关技术中,在结构化道路场景,可以根据车载导航或高精度图中汽车当前行驶道路的左右边界计算局部跟踪参考路径。
然而,在实际应用中,采用该方法计算得到的局部参考路径存在路点稀疏且不等距、曲率不连续且不平滑等问题;并且若直接采用该方法计算局部参考路径,将增大自动驾驶汽车轨迹规划难度,且汽车底层控制跟踪曲率不连续的局部参考路径易出现控制超调和震荡现象,亟待解决。
发明内容
本申请提供一种汽车局部参考路径生成方法、装置、设备及介质,可快速获得当前汽车位置匹配点,提高局部参考路径生成效率,进而提高自动驾驶汽车的乘坐舒适性。
本申请第一方面实施例提供一种汽车局部参考路径生成方法,包括以下步骤:
基于汽车的环境感知信息,计算所述汽车在当前导航路点上的匹配点;
以所述匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集;
基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集;以及
采用多项式曲线连接所述平滑的局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的所述汽车的局部参考路径。
可选地,所述计算所述汽车在当前导航路点上的匹配点,包括:
获取上一周期所述汽车在导航路径上的匹配点信息;
根据所述匹配点信息确定当前周期匹配点在所述当前导航路点上的搜索方向;
根据所述搜索方向,在所述当前导航路点上搜索确认所述当前周期对应的匹配点。
可选地,所述以所述匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集,进一步包括:
在检测到所述汽车的当前位置距离导航起点小于第一预设距离,沿着所述前进反方向的路点个数小于第一目标个数时,从所述导航起点出发,采样第一预设个数的路点;
在检测到所述汽车的当前位置距离导航终点小于第二预设距离,沿着所述相反方向的路点个数小于第二目标个数时,从所述导航终点出发,采样第二预设个数的路点。
可选地,所述基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集,包括:
基于数值优化进行局部参考路点平滑,其中,由所述路径的平滑性、所述导航路径的几何形状相似性、所述局部参考路径点的分布均匀性和所述参考路径行驶平顺性获取对应的付代价函数,并由所述对应的付代价函数得到平滑综合目标函数,结合约束条件构建局部参考路点平滑模型,得到平滑参考路径点集;
基于曲线拟合进行局部参考路径平滑,其中,对所述平滑的参考路径点集进行拟合,得到所述平滑的局部参考路点集。
可选地,所述对所述平滑的参考路径点集进行拟合,得到所述平滑局部参考路点集,包括:
采用5次多项式曲线连接所述平滑的参考路径点集中相邻路点,其中,所述相邻路点间多项式曲线表达式如下:
可选地,归一化处理的表达式如下:
其中,t∈[0,1];
建立所述多项式曲线参数统一优化模型的表达式如下:
本申请第二方面实施例提供一种汽车局部参考路径生成装置,包括:
计算模块,用于基于汽车的环境感知信息,计算所述汽车在当前导航路点上的匹配点;
第一生成模块,用于以所述匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集;
获取模块,用于基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集;以及
第二生成模块,用于采用多项式曲线连接所述平滑的局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的所述汽车的局部参考路径。
可选地,所述计算模块,具体用于:
获取上一周期所述汽车在导航路径上的匹配点信息;
根据所述匹配点信息确定当前周期匹配点在所述当前导航路点上的搜索方向;
根据所述搜索方向,在所述当前导航路点上搜索确认所述当前周期对应的匹配点。
可选地,所述第一生成模块,进一步用于:
在检测到所述汽车的当前位置距离导航起点小于第一预设距离,沿着所述前进反方向的路点个数小于第一目标个数时,从所述导航起点出发,采样第一预设个数的路点;
在检测到所述汽车的当前位置距离导航终点小于第二预设距离,沿着所述相反方向的路点个数小于第二目标个数时,从所述导航终点出发,采样第二预设个数的路点。
可选地,所述获取模块,具体用于:
基于数值优化进行局部参考路点平滑,其中,由所述路径的平滑性、所述导航路径的几何形状相似性、所述局部参考路径点的分布均匀性和所述参考路径行驶平顺性获取对应的付代价函数,并由所述对应的付代价函数得到平滑综合目标函数,结合约束条件构建局部参考路点平滑模型,得到平滑参考路径点集;
基于曲线拟合进行局部参考路径平滑,其中,对所述平滑的参考路径点集进行拟合,得到所述平滑的局部参考路点集。
可选地,所述获取模块,还用于:
采用5次多项式曲线连接所述平滑的参考路径点集中相邻路点,其中,所述相邻路点间多项式曲线表达式如下:
可选地,归一化处理的表达式如下:
其中,t∈[0,1];
建立所述多项式曲线参数统一优化模型的表达式如下:
本申请第三方面实施例提供一种电子设备,包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述程序,以实现如上述实施例所述的汽车局部参考路径生成方法。
本申请第四方面实施例提供一种计算机可读存储介质,所述计算机可读存储介质存储计算机指令,所述计算机指令用于使所述计算机执行如上述实施例所述的汽车局部参考路径生成方法。
由此,本申请实施例的汽车局部参考路径生成方法具有以下优点:
(1)针对原始导航路点稀疏且相邻路点间距较大的场景,本申请实施例提出的分层局部参考路径生成及平滑方法,可以在线获得路点分布均匀且数量充足的局部参考路径;
(2)针对原始导航路径曲率不连续且变化剧烈的场景,本申请实施例提出的在线平滑方法,可以得到曲率连续且平滑、满足道路几何约束的局部参考路径;
(3)汽车在导航路径上匹配点方法可以快速寻找到准确的匹配点,提高局部参考路径生成效率。
本申请附加的方面和优点将在下面的描述中部分给出,部分将从下面的描述中变得明显,或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解,其中:
图1为根据本申请实施例的局部参考路径生成及平滑流程图;
图2为根据本申请实施例提供的一种汽车局部参考路径生成方法的流程图;
图3为根据本申请一个实施例的匹配点搜索方向原理示意图;
图4为根据本申请一个实施例的汽车在导航路径上匹配点计算流程图;
图5为根据本申请一个实施例的具有双层结构的局部参考路径生成及平滑流程图;
图6为根据本申请一个实施例的局部参考路径原始点集与平滑点集对应关系的示例图;
图7为根据本申请实施例的汽车局部参考路径生成装置的方框示意图;
图8为根据本申请实施例的电子设备的示例图。
具体实施方式
下面详细描述本申请的实施例,所述实施例的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的,旨在用于解释本申请,而不能理解为对本申请的限制。
下面参考附图描述本申请实施例的汽车局部参考路径生成方法、装置、设备及介质。
在介绍本申请实施例的汽车局部参考路径生成方法之前,先简单介绍下相关中的几种参考路径生成方法。
(1)相关技术中提出一种用于自动驾驶的基于QP(Quadratic Programming,二次规划)样条路径和螺旋路径的参考线平滑方法,将初始参考线分割成多个参考线段,按顺序包括第一参考线段、第二参考段和第三参考线段,使用二次规划(QP)样条平滑算法平滑所述第一参考线段和所述第三参考线段,使用螺旋平滑算法平滑所述第二参考线段,以及连接来自所述多个参考线段的平滑参考线段以生成平滑参考线。
然而,该方法初始参考线直接根据高清晰度地图和路线制定结果生成,并未考虑原始参考线中路点疏密程度,原始参考线通常存在曲率不连续且不平滑的问题,而且未考虑如何在导航路点快速选择初始参考线的起点。
(2)相关技术中还提出一种确定用于以类似于人类驾驶的方式导航自动驾驶汽车的平滑参考线的方法,使用高清晰度地图生成道路车道的中心线,使用中心线生成与中心线的曲率相关的多个样本点,在每个样本点处的中心线两侧生成几个调整点。样本点处的候选点包括调整点和样本点。通过样本点中的每个候选点集中的每个确定最低成本路径以生成平滑参考线。
然而,该方法直接使用高清晰度地图生成的道路车道中心线作为原始参考路点集,并且该方法中提出的平滑参考线的方法计算量取决于候选点集的规模,计算量随候选点规模增大而快速增加,难以实现在线平滑。
正是基于上述问题,本申请提供了一种汽车局部参考路径生成方法,如图1所示,本申请实施例以高精地图、汽车定位及网联交通等环境感知信息为输入,首先计算自车在导航路点上的匹配点,以自车匹配点为采用初始点,按照固定采样点个数,分别沿汽车前进方向和相反方向取适量路点形成局部参考路径原始点集,然后综合考虑参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性以及参考路径行驶平顺性四方面性能设计目标函数,采用非线性规划方法获得平滑的局部参考路点集,最后采用多项式曲线连接平滑局部参考路点集中相邻路点,获得路点分布均匀且数量充足、曲率连续且平滑的局部参考路径,为后续轨迹规划及跟踪控制提供高质量的平滑参考路径。
其中,本申请实施例生成的局部参考路径主要取决于汽车定位信息、高精地图信息和网联交通信息。用于局部参考路径生成的输入信息主要包括导航路径和自车定位两方面,其中,导航路径与驾驶任务起点和终点相关,智能汽车结合高精度地图和网联交通信息动态规划车道级的导航路径,将所规划导航路径上导航路点信息记为,其中,为驾驶任务导航起点,大地直角坐标系中坐标记为,为驾驶任务导航终点,大地直接坐标系中坐标记为。自车在t时刻定位信息记为,其中,为自车在大地直角坐标系中的位置坐标,为自车车速,为自车航向角。
具体而言,图2为本申请实施例所提供的一种汽车局部参考路径生成方法的流程示意图。
如图2所示,该汽车局部参考路径生成方法包括以下步骤:
在步骤S201中,基于汽车的环境感知信息,计算汽车在当前导航路点上的匹配点。
可选地,计算汽车在当前导航路点上的匹配点,包括:获取上一周期汽车在导航路径上的匹配点信息;根据匹配点信息确定当前周期匹配点在当前导航路点上的搜索方向;根据搜索方向,在当前导航路点上搜索确认当前周期对应的匹配点。
具体而言,本申请实施例可以将导航路点上距离自车位置最近的点定义为匹配点。为提高匹配点搜索效率,本申请实施例以上一周期自车在导航路径上的匹配点为起点,根据自车行驶方向,在导航路径上搜索当前周期自车在导航路径上的匹配点。具体流程如下:
(2)判断本周期匹配点在导航路点上的搜索方向。如图3所示,若与向量之间夹角小于π/2,则搜索方向沿导航路径点序号增大方向;若与向量之间夹角大于π/2,则搜索方向沿导航路径点序号减小的方向;若,则当前周期自车匹配点与上一周期相同,否则转到下一步。
(3)根据步骤(2)中确定的搜索方向,在导航路点上搜索确认当前周期自车对应匹配点。以沿导航路径点序号增大方向为例,说明匹配点搜索过程,流程如图4所示。以上一周期匹配点为起点,沿导航路径点序号增大方向,逐个计算i=1,2,3时对应的导航点与自车当前位置之间的欧式距离,直到找到满足条件的匹配点为止。结合图4所示,具体条件判断如下:
①根据输入计算di-1,di和di+1;
②若不满足di<di-1或di<di+1,则执行i=i+1,并返回步骤①继续搜索;
③若di<di-1且di<di+1,则将A(i)作为可能匹配点并进行存储,同时记Dmin=di和NumT=i;
④为进一步判断A(i)是否为最终匹配点,继续沿搜索方向从A(i)点开始连续计算导航点与自车当前位置之间的欧式距离,并判断是否满足Dmin<di+1;
⑤若满足Dmin<di,则Count=Count+1,并继续判断下一路点是否满足Dmin<di+1;
⑥若出现不满足Dmin<di,则令Count=0,从当前路点开始返回步骤①继续搜索。
⑦若Count>Nth,则匹配点搜索完成,输出:DT=Dmin,MT=NumT。
特别地,其中,Nth为设定的搜索置信阈值,为正整数。
在步骤S202中,以匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集。
可选地,在一些实施例中,以匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集,进一步包括:在检测到汽车的当前位置距离导航起点小于第一预设距离,沿着前进反方向的路点个数小于第一目标个数时,从导航起点出发,采样第一预设个数的路点;在检测到汽车的当前位置距离导航终点小于第二预设距离,沿着相反方向的路点个数小于第二目标个数时,从导航终点出发,采样第二预设个数的路点。
具体而言,在进行参考路径点采样过程,本申请实施例中选择固定采样点个数完成每个周期的采样,可以保证路径平滑问题的优化变量个数不变,实现每个周期都是计算相同规模的路径平滑问题,降低在线求解难度,便于工程实现。本申请实施例局部参考路径原始点集采样从当前周期T匹配点出发,沿着汽车前进方向取Nf个点,沿着汽车前进反方向取Nr个点,累计选取NR=Nr+Nf+1个点作为局部参考路径点集。
其中,两种特殊情况说明如下:
(1)当汽车当前位置靠近导航起点,沿着汽车前进反方向采用不足Nr个点时,本申请实施例采取从导航起点出发,采样NR=Nr+Nf+1个点作为局部参考路径点集;
(2)当汽车当前位置靠近导航终点,沿着汽车前进方向采样不足Nf个点时,本申请实施例采取从导航终点出发,反向采样NR=Nr+Nf+1个点作为局部参考路径原始点集。记周期T采样得到的局部参考路径原始点集为。
在步骤S203中,基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集。
可选地,在一些实施例中,基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集,包括:基于数值优化进行局部参考路点平滑,其中,由路径的平滑性、导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性获取对应的付代价函数,并由对应的付代价函数得到平滑综合目标函数,结合约束条件构建局部参考路点平滑模型,得到平滑参考路径点集;基于曲线拟合进行局部参考路径平滑,其中,对平滑的参考路径点集进行拟合,得到平滑的局部参考路点集。
可选地,在一些实施例中,对平滑的参考路径点集进行拟合,得到平滑的局部参考路点集,包括:采用5次多项式曲线连接所述平滑的参考路径点集中相邻路点,其中,所述相邻路点间多项式曲线表达式如下:
在步骤S204中,采用多项式曲线连接所述平滑的局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的所述汽车的局部参考路径。
其中,在一些实施例中,归一化处理的表达式如下:
其中,t∈[0,1];
建立所述多项式曲线参数统一优化模型的表达式如下:
应当理解的是,如图5所示,本申请实施例可以包括基于数值优化的局部参考路点平滑和基于曲线拟合的局部参考路径平滑。
其中,上层为基于数值优化的局部参考路点平滑,具体步骤如下:
首先,目标函数的建立考虑了路点的平滑性、相似性、均匀性和平顺性四方面性能,针对各方面性能定义子目标函数具体如下所述:
(1)为保证局部参考路径的平滑性,路点平滑性代价函数定义如下:
(2)为保证平滑后的局部参考路点集与对应导航路点集几何形状的相似性,几何相似性代价函数定义如下:
(3)为保证平滑后的局部参考路点集中相邻路点间距离的一致性,路点均匀性代价函数定义如下:
(4)为减少平滑后的局部参考路点集中相邻路点间航向角变化幅度,路点平顺性代价函数定义如下:
其中,为路点对应航向角。为提高路点对应航向角的计算精度,采用中点欧拉法计算航向角,考虑到相邻路点直接采用反正切函数求解角度的多值性,本申请实施例首先对连续三个路点坐标采用中点欧拉法进行处理,然后再通过反正切函数计算航向角,具体计算式如下:
基于数值优化的局部参考路点平滑综合目标函数计算式如下:
建立局部参考路点平滑模型需要考虑汽车转向系统物理约束、道路边界约束以及导航路点的位置约束,各约束条件具体计算式如下所述:
(3)考虑到汽车沿参考路点行驶方向应保持与导航路点方向的一致性,同时为缩小局部参考路点平滑问题的搜索空间,平滑后的路点应该满足导航路点附近,导航路点的位置约束计算式如下:
其中,下层是基于曲线拟合的参考路径平滑方法,具体步骤如下:
通过前述基于数值优化的局部参考路点平滑方法,可以获得与局部参考路径原始点集维度相同的平滑局部参考路点集,考虑到导航路点可能会存在路点稀疏且相邻路点距离相差较大的问题,本申请实施例采用参数方程对平滑的局部参考点集进行拟合,进而获得分布均匀且数量充足的平滑参考路径点集。
为得到均匀分布路点,本申请实施例可以采用5次多项式曲线连接平滑局部参考路点集中相邻路点,相邻路点间多项式曲线表达式如下:
平滑局部参考路点集中NR个点将局部参考路径划分为Nr+Nf个子区间,为实现所有区间段多项式曲线的统一表达,本申请实施例中将相邻路点间距进行标准化处理,相邻路点构成的子区间,实现间距标准化的计算式如下:
则所有区间段的多项式曲线可统一表达为如下形式:
为求解各区间多项式曲线参数,本申请实施例采用非线性优化方法进行求解,优化目标函数定义如下:
多项式曲线参数求解过程中,为确保各段多项式曲线连接形成的局部参考路径连续且平滑,需满足如下约束:
(1)起点约束。要求根据第一段多项式曲线计算得到的初始位置和方向与平滑的局部参考路点集起始点保持一致,具体需满足如下约束:
(2)连接点约束。要求相邻子区间根据多项式曲线计算得到的区间连接点保持连续性,即要求上一个子区间的终点等于下一段子区间的起点,具体需满足如下约束:
(3)位置误差约束。要求子区间根据多项式区间计算得到的断电需要在平滑的局部参考路点集对应点的领域内,具体需满足如下约束:
综上,结合目标函数和以上约束式构成的数学优化模型,可采用非线性规划方法进行求解,即可获得各段多项式曲线的参数。
由此,本申请实施例可以基于上述步骤S201至S203中求得的分段多项式曲线连接起来,生成平滑的局部参考路径。基于平滑的局部参考路径曲线方程,本申请实施例可以在大地坐标直角系中利用等间距横坐标插值获得均匀分布的平滑路点,或以平滑的局部参考路点集起始点为原点,基于平滑的局部参考路径建立曲线坐标系,利用等距离获得均匀分布的平滑路点,再通过曲线坐标系到大地直角坐标系变换得到均匀分布的路点,其中,为路点对应的航向角,为路点对应的曲率,计算式如下:
由此,将上述优化得到的平滑局部参考路径为作为智能汽车跟踪目标,根据汽车实际定位位置信息,依次完成行为决策、轨迹规划和轨迹跟踪控制,实现智能汽车安全、高效、舒适的完成驾驶任务。
根据本申请实施例提出的汽车局部参考路径生成方法,可以基于汽车的环境感知信息,计算汽车在当前导航路点上的匹配点,并以匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集,并基于由路径的平滑性、导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑局部参考路点集,并采用多项式曲线连接平滑局部参考路点集中相邻路点,生成路点分布满足平滑条件的汽车的局部参考路径。由此,通过合理设置局部参考线平滑问题的优化目标和约束条件,可以降低局部参考路径在线平滑问题求解规模和难度,便于工程应用,同时可以提高局部参考路径生成效率;通过获得路点分布均匀且数量充足、曲率连续且平滑的局部参考路径,降低后续轨迹规划难度,为实现轨迹精确跟踪提供基础。
其次参照附图描述根据本申请实施例提出的汽车局部参考路径生成装置。
图7是本申请实施例的汽车局部参考路径生成装置的方框示意图。
如图7所示,该汽车局部参考路径生成装置10包括:计算模块100、第一生成模块200、获取模块300和第二生成模块400。
其中,计算模块100用于基于汽车的环境感知信息,计算汽车在当前导航路点上的匹配点;
第一生成模块200用于以匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集;
获取模块300用于基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集;以及
第二生成模块400用于采用多项式曲线连接平滑局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的汽车的局部参考路径。
可选地,计算模块,具体用于:
获取上一周期汽车在导航路径上的匹配点信息;
根据匹配点信息确定当前周期匹配点在当前导航路点上的搜索方向;
根据搜索方向,在当前导航路点上搜索确认当前周期对应的匹配点。
可选地,第一生成模块200进一步用于:
在检测到汽车的当前位置距离导航起点小于第一预设距离,沿着前进反方向的路点个数小于第一目标个数时,从导航起点出发,采样第一预设个数的路点;
在检测到汽车的当前位置距离导航终点小于第二预设距离,沿着相反方向的路点个数小于第二目标个数时,从导航终点出发,采样第二预设个数的路点。
可选地,获取模块300具体用于:
基于数值优化进行局部参考路点平滑,其中,由路径的平滑性、导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性获取对应的付代价函数,并由对应的付代价函数得到平滑综合目标函数,结合约束条件构建局部参考路点平滑模型,得到平滑参考路径点集;
基于曲线拟合进行局部参考路径平滑,其中,对平滑的参考路径点集进行拟合,得到平滑的局部参考路点集。
可选地,获取模块300还用于:
采用5次多项式曲线连接所述平滑的参考路径点集中相邻路点,其中,所述相邻路点间多项式曲线表达式如下:
可选地,归一化处理的表达式如下:
其中,t∈[0,1];
建立所述多项式曲线参数统一优化模型的表达式如下:
需要说明的是,前述对汽车局部参考路径生成方法实施例的解释说明也适用于该实施例的汽车局部参考路径生成装置,此处不再赘述。
根据本申请实施例提出的汽车局部参考路径生成装置,可以基于汽车的环境感知信息,计算汽车在当前导航路点上的匹配点,并以匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集,并基于由路径的平滑性、导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑局部参考路点集,并采用多项式曲线连接平滑局部参考路点集中相邻路点,生成路点分布满足平滑条件的汽车的局部参考路径。由此,通过合理设置局部参考线平滑问题的优化目标和约束条件,可以降低局部参考路径在线平滑问题求解规模和难度,便于工程应用,同时可以提高局部参考路径生成效率;通过获得路点分布均匀且数量充足、曲率连续且平滑的局部参考路径,降低后续轨迹规划难度,为实现轨迹精确跟踪提供基础。
图8为本申请实施例提供的电子设备的结构示意图。该电子设备可以包括:
存储器801、处理器802及存储在存储器801上并可在处理器802上运行的计算机程序。
处理器802执行程序时实现上述实施例中提供的汽车局部参考路径生成方法。
进一步地,电子设备还包括:
通信接口803,用于存储器801和处理器802之间的通信。
存储器801,用于存放可在处理器802上运行的计算机程序。
存储器801可能包含高速RAM存储器,也可能还包括非易失性存储器(non-volatile memory),例如至少一个磁盘存储器。
如果存储器801、处理器802和通信接口803独立实现,则通信接口803、存储器801和处理器802可以通过总线相互连接并完成相互间的通信。总线可以是工业标准体系结构(Industry Standard Architecture,简称为ISA)总线、外部设备互连(PeripheralComponent,简称为PCI)总线或扩展工业标准体系结构(Extended Industry StandardArchitecture,简称为EISA)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示,图8中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
可选地,在具体实现上,如果存储器801、处理器802及通信接口803,集成在一块芯片上实现,则存储器801、处理器802及通信接口803可以通过内部接口完成相互间的通信。
处理器802可能是一个中央处理器(Central Processing Unit,简称为CPU),或者是特定集成电路(Application Specific Integrated Circuit,简称为ASIC),或者是被配置成实施本申请实施例的一个或多个集成电路。
本实施例还提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上的汽车局部参考路径生成方法。
在本说明书的描述中,参考术语“一个实施例”、“一些实施例”、 “示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中,对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且,描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外,在不相互矛盾的情况下,本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中,“N个”的含义是至少两个,例如两个,三个等,除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为,表示包括一个或更N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分,并且本申请的优选实施方式的范围包括另外的实现,其中可以不按所示出或讨论的顺序,包括根据所涉及的功能按基本同时的方式或按相反的顺序,来执行功能,这应被本申请的实施例所属技术领域的技术人员所理解。
在流程图中表示或在此以其他方式描述的逻辑和/或步骤,例如,可以被认为是用于实现逻辑功能的可执行指令的定序列表,可以具体实现在任何计算机可读介质中,以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用,或结合这些指令执行系统、装置或设备而使用。就本说明书而言,"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下:具有一个或N个布线的电连接部(电子装置),便携式计算机盘盒(磁装置),随机存取存储器(RAM),只读存储器(ROM),可擦除可编辑只读存储器(EPROM或闪速存储器),光纤装置,以及便携式光盘只读存储器(CDROM)。另外,计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质,因为可以例如通过对纸或其他介质进行光学扫描,接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序,然后将其存储在计算机存储器中。
应当理解,本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中,N个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如,如果用硬件来实现和在另一实施方式中一样,可用本领域公知的下列技术中的任一项或他们的组合来实现:具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路,具有合适的组合逻辑门电路的专用集成电路,可编程门阵列(PGA),现场可编程门阵列(FPGA)等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成,所述的程序可以存储于一种计算机可读存储介质中,该程序在执行时,包括方法实施例的步骤之一或其组合。
此外,在本申请各个实施例中的各功能单元可以集成在一个处理模块中,也可以是各个单元单独物理存在,也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现,也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时,也可以存储在一个计算机可读取存储介质中。
上述提到的存储介质可以是只读存储器,磁盘或光盘等。尽管上面已经示出和描述了本申请的实施例,可以理解的是,上述实施例是示例性的,不能理解为对本申请的限制,本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。
Claims (10)
1.一种汽车局部参考路径生成方法,其特征在于,包括以下步骤:
基于汽车的环境感知信息,计算所述汽车在当前导航路点上的匹配点;
以所述匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集;
基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集;以及
采用多项式曲线连接所述平滑的局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的所述汽车的局部参考路径。
2.根据权利要求1所述的方法,其特征在于,所述计算所述汽车在当前导航路点上的匹配点,包括:
获取上一周期所述汽车在导航路径上的匹配点信息;
根据所述匹配点信息确定当前周期匹配点在所述当前导航路点上的搜索方向;
根据所述搜索方向,在所述当前导航路点上搜索确认所述当前周期对应的匹配点。
3.根据权利要求1所述的方法,其特征在于,所述以所述匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集,进一步包括:
在检测到所述汽车的当前位置距离导航起点小于第一预设距离,沿着所述前进反方向的路点个数小于第一目标个数时,从所述导航起点出发,采样第一预设个数的路点;
在检测到所述汽车的当前位置距离导航终点小于第二预设距离,沿着所述相反方向的路点个数小于第二目标个数时,从所述导航终点出发,采样第二预设个数的路点。
4.根据权利要求1所述的方法,其特征在于,所述基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集,包括:
基于数值优化进行局部参考路点平滑,其中,由所述路径的平滑性、所述导航路径的几何形状相似性、所述局部参考路径点的分布均匀性和所述参考路径行驶平顺性获取对应的付代价函数,并由所述对应的付代价函数得到平滑综合目标函数,结合约束条件构建局部参考路点平滑模型,得到平滑参考路径点集;
基于曲线拟合进行局部参考路径平滑,其中,对所述平滑的参考路径点集进行拟合,得到所述平滑的局部参考路点集。
7.一种汽车局部参考路径生成装置,其特征在于,包括:
计算模块,用于基于汽车的环境感知信息,计算所述汽车在当前导航路点上的匹配点;
第一生成模块,用于以所述匹配点为采样初始点,按照预设采样点个数分别沿汽车前进方向及其相反方向取多个路点,生成局部参考路径原始点集;
获取模块,用于基于参考路径的平滑性、与导航路径的几何形状相似性、局部参考路径点的分布均匀性和参考路径行驶平顺性得到的目标函数,获取平滑的局部参考路点集;以及
第二生成模块,用于采用多项式曲线连接所述平滑的局部参考路点集中相邻路点,通过相邻路点距离区间的归一化处理,建立多项式曲线参数统一优化模型,生成路点分布均匀且满足平滑条件的所述汽车的局部参考路径。
8.根据权利要求7所述的装置,其特征在于,所述计算模块,具体用于:
获取上一周期所述汽车在导航路径上的匹配点信息;
根据所述匹配点信息确定当前周期匹配点在所述当前导航路点上的搜索方向;
根据所述搜索方向,在所述当前导航路点上搜索确认所述当前周期对应的匹配点。
9.一种电子设备,其特征在于,包括:存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序,所述处理器执行所述程序,以实现如权利要求1-5任一项所述的汽车局部参考路径生成方法。
10.一种计算机可读存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行,以用于实现如权利要求1-5任一项所述的汽车局部参考路径生成方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210103608.1A CN114114930B (zh) | 2022-01-28 | 2022-01-28 | 汽车局部参考路径生成方法、装置、设备及介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210103608.1A CN114114930B (zh) | 2022-01-28 | 2022-01-28 | 汽车局部参考路径生成方法、装置、设备及介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114114930A true CN114114930A (zh) | 2022-03-01 |
CN114114930B CN114114930B (zh) | 2022-04-19 |
Family
ID=80361824
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210103608.1A Expired - Fee Related CN114114930B (zh) | 2022-01-28 | 2022-01-28 | 汽车局部参考路径生成方法、装置、设备及介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114114930B (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190250624A1 (en) * | 2018-02-15 | 2019-08-15 | Wipro Limited | Method and system for real-time generation of reference navigation path for navigation of vehicle |
CN111898804A (zh) * | 2020-07-10 | 2020-11-06 | 东风商用车有限公司 | 局部规划的道路自适应采样方法 |
CN112362074A (zh) * | 2020-10-30 | 2021-02-12 | 重庆邮电大学 | 一种结构化环境下的智能车辆局部路径规划方法 |
CN112964271A (zh) * | 2021-03-15 | 2021-06-15 | 西安交通大学 | 一种面向多场景的自动驾驶规划方法及系统 |
CN113448335A (zh) * | 2021-07-02 | 2021-09-28 | 广州小鹏自动驾驶科技有限公司 | 路径规划方法和装置、车辆和可读存储介质 |
CN113483776A (zh) * | 2021-07-01 | 2021-10-08 | 广州小鹏自动驾驶科技有限公司 | 路径规划方法、装置及汽车 |
-
2022
- 2022-01-28 CN CN202210103608.1A patent/CN114114930B/zh not_active Expired - Fee Related
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20190250624A1 (en) * | 2018-02-15 | 2019-08-15 | Wipro Limited | Method and system for real-time generation of reference navigation path for navigation of vehicle |
CN111898804A (zh) * | 2020-07-10 | 2020-11-06 | 东风商用车有限公司 | 局部规划的道路自适应采样方法 |
CN112362074A (zh) * | 2020-10-30 | 2021-02-12 | 重庆邮电大学 | 一种结构化环境下的智能车辆局部路径规划方法 |
CN112964271A (zh) * | 2021-03-15 | 2021-06-15 | 西安交通大学 | 一种面向多场景的自动驾驶规划方法及系统 |
CN113483776A (zh) * | 2021-07-01 | 2021-10-08 | 广州小鹏自动驾驶科技有限公司 | 路径规划方法、装置及汽车 |
CN113448335A (zh) * | 2021-07-02 | 2021-09-28 | 广州小鹏自动驾驶科技有限公司 | 路径规划方法和装置、车辆和可读存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN114114930B (zh) | 2022-04-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109033176B (zh) | 道路曲率的确定方法、装置、存储介质和计算机设备 | |
CN110316193B (zh) | 预瞄距离的设置方法、装置、设备及计算机可读存储介质 | |
CN113448335A (zh) | 路径规划方法和装置、车辆和可读存储介质 | |
CN111060108B (zh) | 路径规划方法和装置、工程车辆 | |
CN110657814A (zh) | 轨迹规划方法和装置、车辆及其控制方法和系统 | |
CN111177934B (zh) | 参考路径规划的方法、设备和存储介质 | |
CN111709579B (zh) | 船舶航速优化方法及装置 | |
CN110262506A (zh) | 预瞄点的确认方法、车辆行驶控制方法、装置及设备 | |
CN112000090B (zh) | 一种用于非结构化道路的轨迹队列管理方法 | |
CN108061555B (zh) | 一种车辆定位误差矫正方法及装置 | |
US11926339B2 (en) | Method for constructing driving coordinate system, and application thereof | |
CN108106620A (zh) | 一种拓扑道路匹配方法、系统及电子设备 | |
CN111703436A (zh) | 一种自动驾驶车辆的控制方法及装置 | |
CN114676939B (zh) | 一种多车型参数自适应的参考线平滑方法和系统 | |
Yang et al. | An algorithm of curved path tracking with prediction model for autonomous vehicle | |
CN117146844A (zh) | 路径规划中参考线平滑优化方法、装置、设备及介质 | |
CN115900742A (zh) | 车辆的参考轨迹规划方法、装置、电子设备及存储介质 | |
CN114802214A (zh) | 纯跟踪自动泊车控制方法、性能评估工具及可读存储介质 | |
CN114114930B (zh) | 汽车局部参考路径生成方法、装置、设备及介质 | |
Schindler et al. | Exploiting arc splines for digital maps | |
CN117250957A (zh) | 路径跟踪控制关键参考点选取方法、装置、设备和介质 | |
CN109911013B (zh) | 一种车辆方向盘转角的调整方法和装置 | |
CN116295478A (zh) | 车辆的导航方法及装置 | |
Brummer et al. | Evaluation of a mapping strategy based on smooth arc splines for different road types | |
CN116576874A (zh) | 车辆行驶路径优化方法、装置、电子设备及存储介质 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20220419 |