CN114167860B - 一种自动驾驶最优轨迹生成方法及装置 - Google Patents
一种自动驾驶最优轨迹生成方法及装置 Download PDFInfo
- Publication number
- CN114167860B CN114167860B CN202111406002.7A CN202111406002A CN114167860B CN 114167860 B CN114167860 B CN 114167860B CN 202111406002 A CN202111406002 A CN 202111406002A CN 114167860 B CN114167860 B CN 114167860B
- Authority
- CN
- China
- Prior art keywords
- cost
- candidate
- track
- weight
- vehicle
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000004364 calculation method Methods 0.000 claims abstract description 29
- 238000010606 normalization Methods 0.000 claims abstract description 19
- 238000012545 processing Methods 0.000 claims abstract description 18
- 230000008859 change Effects 0.000 claims description 26
- 238000005070 sampling Methods 0.000 claims description 13
- 230000001186 cumulative effect Effects 0.000 claims description 10
- 230000036461 convulsion Effects 0.000 claims description 7
- 230000003542 behavioural effect Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 6
- 238000010276 construction Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 34
- 230000006399 behavior Effects 0.000 description 30
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000004590 computer program Methods 0.000 description 5
- 238000012216 screening Methods 0.000 description 4
- 230000003287 optical effect Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- 238000005065 mining Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000005452 bending Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 239000011159 matrix material Substances 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
- 238000011160 research Methods 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
-
- 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)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
本发明公开了一种自动驾驶最优轨迹生成方法及装置,涉及自动驾驶***技术领域,该方法包括基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹。本发明能够实现最优轨迹的准确生成。
Description
技术领域
本发明涉及自动驾驶***技术领域,具体涉及一种自动驾驶最优轨迹生成方法及装置。
背景技术
轨迹规划是自动驾驶的关键技术,最优轨迹生成是轨迹规划的重要环节。最优轨迹生成主要是根据在可行驶区域内生成的一系列候选轨迹中,确定一条最优的轨迹。通过计算每条候选轨迹的成本,然后根据在矿区场景、车辆速度、道路形状,筛选出成本最小的轨迹,作为最优轨迹。每条候选轨迹的成本由5个因素组成:1、轨迹弯曲程度;2、轨迹弯曲程度变化率;3、轨迹横向偏移量;4、上层指令—行为;5、与障碍物距离。由于每个因素的数量级、范围以及单位不同,需要对其进行归一化处理,同时,权重系数会受道路形状和车辆当前速度以及与相关目标物相对距离的影响。
当前,在进行最优轨迹生成时一般采用以下方法:步骤一、根据相应算法,进行轨迹规划,得到多条期望轨迹;步骤二、根据人工势场,计算各轨迹的势场峰值和累积值,总势场由车道线势场、道路边界线势场、障碍物势场组成,势场峰值即轨迹上最大点的势场值,势场累积值即轨迹上所有点势场值之和;步骤三、采用轨迹成本函数进行每条轨迹的成本计算;步骤四、根据各个期望轨迹的势场峰值和累积值,结合轨迹筛选函数选定最优轨迹。此种最优轨迹生成方法是将道路进行栅格化,并采用相应算法进行轨迹规划,是求最短路径的有效搜索算法,但是其时间复杂度高,耗时长,同时,采用人工势场法计算成本时,当目标点附近有障碍物时,斥力将非常大,引力相对较小,物体很难到达目标点,因此,很难准确的生成得到最优轨迹。
发明内容
针对现有技术中存在的缺陷,本发明的目的在于提供一种自动驾驶最优轨迹生成方法及装置,能够实现最优轨迹的准确生成。
为达到以上目的,本发明提供一种自动驾驶最优轨迹生成方法,具体包括以下步骤:
基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;
根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;
基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;
基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;
基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹。
在上述技术方案的基础上,所述基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成,具体步骤包括:
基于道路形式以及车辆的位姿信息和速度信息,对道路进行纵向距离和横向距离的采样,并将采样点作为目标点;
对起始点和目标点进行状态参数配置,所述起始点为车辆当前的位置,所述状态参数包括航向角、曲率和车辆横纵向坐标;
根据配置的状态参数,采用五次多项式生成候选轨迹。
在上述技术方案的基础上,所述采用五次多项式生成候选轨迹,其中,生成的候选轨迹的表达式为:
Li(s)=ai5*s5+ai4*s4+ai3*s3+ai2*s2+ai1*s+ai0
其中,Li(s)表示候选轨迹,ai5表示五次多项式的五次项系数,ai4表示五次多项式的四次项系数,ai3表示五次多项式的三次项系数,ai2表示五次多项式的二次项系数,ai1表示五次多项式的一次项系数,ai0表示五次多项式的零次项系数,s表示候选轨迹中车辆和起始点的纵向距离。
在上述技术方案的基础上,所述成本影响权重包括平均曲率变化率、平均曲率、平均横向偏移量、累积横向偏移量、行为指令和距离障碍物的最小距离成本。
在上述技术方案的基础上,构建的候选轨迹的成本函数具体为:
Costi=Wi1*Jerki+Wi2*Curi+Wi3*Averg_Lati+Wi4*Acumlt_Lati+Wi5*Bhvi+Wi6*Obs_MinDisti
其中,Costi表示候选轨迹的成本,Jerki表示平均曲率变化率,Wi1表示平均曲率变化率的权重系数,Curi表示平均曲率,Wi2表示平均曲率的权重系数,Averg_Lati表示平均横向偏移量,Wi3表示平均横向偏移量的权重系数,Acumlt_Lati表示累积横向偏移量,Wi4表示累积横向偏移量的权重系数,Bhvi表示行为指令,Wi5表示行为指令的权重系数,Obs_MinDisti表示距离障碍物的最小距离成本,Wi6表示距离障碍物的最小距离成本的权重系数。
在上述技术方案的基础上,
所述平均曲率变化率的计算公式为:
所述平均曲率的计算公式为:
所述平均横向偏移量的计算公式为:
所述累积横向偏移量的计算公式为:
在上述技术方案的基础上,
所述基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,具体为:基于道路曲率、车辆速度和自动驾驶行为指令,并针对不同驾驶工况,配置候选轨迹各成本影响权重的权重系数;
所述驾驶工况包括直道巡航、转弯、静态避障、会车和换道。
在上述技术方案的基础上,所述基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,对于配置过程,具体为:
在直道巡航驾驶工况下,Wi1、Wi2、Wi3和车辆速度成正比,Wi4和车辆速度成反比,Wi3、Wi4和道路曲率成正比;
在转弯驾驶工况下,Wi4和车辆速度成反比,Wi3、Wi4和道路曲率成正比;
在静态避障驾驶工况下,Wi4和碰撞时间成反比,所述碰撞时间基于相对距离和相对速度计算得到;
在会车驾驶工况下,Wi4和碰撞时间成反比;
在换道驾驶工况下,Wi1、Wi2、Wi3和车辆速度成正比,Wi4和车辆速度成反比,Wi4和换道完成时间成正比。
在上述技术方案的基础上,在根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数之前,还包括:去除生成的候选轨迹中超出道路边界的候选轨迹。
在上述技术方案的基础上,基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值,其中,进行归一化处理的计算公式为:
y=tan-1x*2/P1
其中,y表示转换后的成本影响权重,x表示成本影响权重,P1表示圆周率。
本发明提供的一种自动驾驶最优轨迹生成装置,包括:
生成模块,其用于基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;
构建模块,其用于根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;
转换模块,其用于基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;
配置模块,其用于基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;
选取模块,其用于基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹。
与现有技术相比,本发明的优点在于:结合实际道路场景下的车辆信息,采用基于多项式拟合的算法生成候选轨迹,同时根据道路的不同形状以及不同行为指令,考虑成本影响权重的组成,对成本影响权重进行归一化处理,去除成本影响权重的单位限制,同时基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,使得成本影响权重的权重系数会随着不同场景而改变,从而筛选出最优轨迹,有效降低了运算时长,并提高了时效性,实现最优轨迹的准确生成。
附图说明
为了更清楚地说明本申请实施例中的技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例中一种自动驾驶最优轨迹生成方法的流程图;
图2为本发明实施例中的道路采样示意图;
图3为本发明实施例中生成的候选轨迹的示意图。
具体实施方式
本发明实施例提供一种自动驾驶最优轨迹生成方法,本发明结合实际道路场景下的车辆信息,采用基于多项式拟合的算法生成候选轨迹,同时根据道路的不同形状以及不同行为指令,考虑成本影响权重的组成,对成本影响权重进行归一化处理,去除成本影响权重的单位限制,同时基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,使得成本影响权重的权重系数会随着不同场景而改变,从而筛选出最优轨迹,有效降低了运算时长,并提高了时效性,实现最优轨迹的准确生成。本发明实施例相应地还提供了一种自动驾驶最优轨迹生成装置。
为使本申请实施例的目的、技术方案和优点更加清楚,下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本申请的一部分实施例,而不是全部的实施例。
参见图1所示,本发明实施例提供的一种自动驾驶最优轨迹生成方法,具体包括以下步骤:
S1:基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;即根据车辆当前所处道路的形状,以及车辆当前的位姿信息和速度信息,采用五次多项式生成候选轨迹,且生成的候选轨迹包括多条。
本发明实施例中,基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成,具体步骤包括:
S101:基于道路形式以及车辆的位姿信息和速度信息,对道路进行纵向距离和横向距离的采样,并将采样点作为目标点;参见图2所示,以车辆当前所在位置为起始点,然后沿着道路的横纵向方向进行采样,得到多个采样点,图2中的圆点即表示采样点,矩形方框表示车辆。图2中包含了24个采样点,因此在后续会生成24条候选轨迹。
S102:对起始点和目标点进行状态参数配置,所述起始点为车辆当前的位置,所述状态参数包括航向角、曲率和车辆横纵向坐标;即车辆位于起始点或目标点时,车辆横纵向坐标,以及候选轨迹在起始点或目标点处的航向角和曲率。
S103:根据配置的状态参数,采用五次多项式生成候选轨迹。即根据起始点或目标点的状态参数,来配置候选轨迹生成所需要的参数,规划的候选轨迹用五次多项式来表示。
本发明实施例中,采用五次多项式生成候选轨迹,其中,生成的候选轨迹的表达式为:
Li(s)=ai5*s5+ai4*s4+ai3*s3+ai2*s2+ai1*s+ai0
其中,Li(s)表示候选轨迹,具体为第i条候选轨迹,ai5表示五次多项式的五次项系数,ai4表示五次多项式的四次项系数,ai3表示五次多项式的三次项系数,ai2表示五次多项式的二次项系数,ai1表示五次多项式的一次项系数,ai0表示五次多项式的零次项系数,s表示候选轨迹中车辆和起始点的纵向距离。例如,参见图3所示,图3中每条曲线均表示一条候选轨迹,圆点表示采样点,矩形方框表示车辆,且图3中的每条候选轨迹均可采用上述表达式表示。
由上述表达式可知,第i条五次多项式候选轨迹有6个参数,因此需要6个约束参数进行配置。当车辆在起始点,纵向距离为s0时,车辆距离参考线的横向距离为L0,车辆与参考线的航向角为L.0,车辆在参考线上的投影点曲率为L..0,所述参考线为车道中心线;车辆在目标点,纵向距离为si1时,车辆距离参考线的横向距离为Li1,车辆与参考线的航向角为L.i1,车辆在参考线上的投影点曲率为L..i1,由此可计算出Li(s)和之间的多项式系数矩阵,如下:
从而计算出候选轨迹的模型。
S2:根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;本发明实施例中,成本影响权重包括平均曲率变化率、平均曲率、平均横向偏移量、累积横向偏移量、行为指令和距离障碍物的最小距离成本。
本发明实施例中,在根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数之前,还包括:去除生成的候选轨迹中超出道路边界的候选轨迹。
S3:基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;
S4:基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;
S5:基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹。即对于步骤S2构建的成本函数,将步骤S2中成本函数的成本影响权重换成转换后的成本影响权重,权重系数换成重新配置的权重系数,然后进行成本计算,得到每条候选轨迹的成本,将成本最小候选轨迹作为最优轨迹。当得到最优轨迹后,则将最优轨迹输出给自动驾驶的横向控制层。
本发明实施例中,从生成的多条候选轨迹中筛选出一条作为最优轨迹,首先去除超出道路边界的候选轨迹,计算剩下的每条候选轨迹的成本,每条候选轨迹的成本函数由6项权重组成,具体的,构建的候选轨迹的成本函数为:
Costi=Wi1*Jerki+Wi2*Curi+Wi3*Averg_Lati+Wi4*Acumlt_Lati+Wi5*Bhvi+Wi6*Obs_MinDisti
其中,Costi表示候选轨迹的成本,Jerki表示平均曲率变化率,Wi1表示平均曲率变化率的权重系数,Curi表示平均曲率,Wi2表示平均曲率的权重系数,Averg_Lati表示平均横向偏移量,Wi3表示平均横向偏移量的权重系数,Acumlt_Lati表示累积横向偏移量,Wi4表示累积横向偏移量的权重系数,Bhvi表示行为指令,Wi5表示行为指令的权重系数,Obs_MinDisti表示距离障碍物的最小距离成本,Wi6表示距离障碍物的最小距离成本的权重系数。
其中,平均曲率变化率对应候选轨迹的连续性、顺滑性;平均曲率对应候选轨迹的弯曲程度;累积横向偏移量对应车联换道完成时间;行为指令中,行为不同,则选择不同的候选轨迹;对于距离障碍物的最小距离成本,则最小距离越小,车辆越危险。
本发明实施例中,平均曲率变化率的计算公式为:
平均曲率变化率用于表示候选轨迹的平滑性,且Jerki值越小,候选轨迹越平滑。
所述平均曲率的计算公式为:
平均曲率用于表示候选轨迹横摆角的变化,且Curi值越小,横摆角变化相对来说越小。
所述平均横向偏移量的计算公式为:
平均横向偏移量用于表示候选轨迹的平均横向偏移距离;
所述累积横向偏移量的计算公式为:
累积横向偏移量用于候选轨迹的累计横向偏移距离,且候选轨迹纵向距离越大,累计横向偏移量越大。
其中,s0表示车辆在起始点时的纵向距离,si1表示车辆在目标点时的纵向距离,n表示将候选轨迹划分成的等份,n的取值可以为100,也可以为500,表示Li(s)对s的二次求导,/>表示Li(s)对s的三次求导。
行为指令用于表示自动驾驶行为层发出的指令成本,如果行为是巡航、转弯,那么目标点距离当前行驶道路中心线越近,指令成本就越低;如果行为是避障、会车,那么目标点距离会车车辆、障碍物越远,指令成本越低。
距离障碍物的最小距离成本,用于表示候选轨迹中距离障碍物最近的距离成本,距离越近成本越高。
本发明实施例中,基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值,其中,进行归一化处理的计算公式为:
y=tan-1x*2/P1
其中,y表示转换后的成本影响权重,x表示成本影响权重,P1表示圆周率,tan表示正切计算。
因为成本函数中,成本影响权重的单位不同,量级也不同,故需要对成本影响权重进行归一化处理,采用反正切函数实现数据的归一化,映射的区间为[0,1]。
去除数据的单位限制,将其转化为无量纲的纯数值。同时,由于每个成本影响权重的数值均为正,并且不能够确定每个成本影响权重的最大值和最小值,故采用反正切函数,其归一化结果均为正值。
本发明实施例中,基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,具体为:基于道路曲率、车辆速度和自动驾驶行为指令,并针对不同驾驶工况,配置候选轨迹各成本影响权重的权重系数;驾驶工况包括直道巡航、转弯、静态避障、会车和换道。在对权重系数进行配置时,不同的行为,可以确定不同的目标车道,且在配置时,Bhvi应该占比重最大,即Wi5的值应该最大。
本发明实施例中,基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,对于配置过程,具体为:
在直道巡航驾驶工况下,Wi1、Wi2、Wi3和车辆速度成正比,Wi4和车辆速度成反比,Wi3、Wi4和道路曲率成正比;
在转弯驾驶工况下,Wi4和车辆速度成反比,Wi3、Wi4和道路曲率成正比;
在静态避障驾驶工况下,Wi4和碰撞时间成反比,所述碰撞时间基于相对距离和相对速度计算得到;
在会车驾驶工况下,Wi4和碰撞时间成反比;
在换道驾驶工况下,Wi1、Wi2、Wi3和车辆速度成正比,Wi4和车辆速度成反比,Wi4和换道完成时间成正比。
本发明的自动驾驶最优轨迹生成方法,自动驾驶车辆根据当前车辆的位姿形态以及道路形状,进行候选轨迹的生成;考虑规划轨迹平滑性、横向偏移量、驾驶员的体验感、上层行为指令、与障碍物相对距离五个方面组成成本函数,每个方面称其为成本影响权重,考虑到各个成本影响权重的单位不同,对其进行归一化处理,去除单位限制,转化为无量纲的数值;根据成本函数筛选出最优轨迹,决策是自主驾驶的重要组成部分,其决定了车辆能否平稳、准确的完成各种驾驶功能,而轨迹规划是决策层的重要研究领域,轨迹筛选是轨迹规划的重要环节,直接影响到横向控制层执行的情况,影响到自动驾驶车辆的安全性以及驾驶员舒适性的体验。
车辆当前位置点和目标点进行五次多项式曲线拟合,降低了时耗,增加了规划轨迹的平滑性和时效性;针对特殊的自动驾驶场景,如矿区场景,自动驾驶车辆速度一般比较慢,考虑车辆运动学限制、换道以及会车工况、上层行为指令、车辆从当前位置到达目标点位置的时间限制,对成本函数的权重进行相应的改变,权重系数也会做相应的改变,以满足自动驾驶条件。
在一种可能的实施方式中,本发明实施例还提供一种可读存储介质,可读存储介质位于PLC控制器中,可读存储介质上存储有计算机程序,该程序被处理器执行时实现以下所述自动驾驶最优轨迹生成方法的步骤:
基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;
根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;
基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;
基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;
基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹。
存储介质可以采用一个或多个计算机可读的介质的任意组合。计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质。计算机可读存储介质例如可以是但不限于:电、磁、光、电磁、红外线、或半导体的***、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子(非穷举的列表)包括:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本文件中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行***、装置或者器件使用或者与其结合使用。
计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于由指令执行***、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:无线、电线、光缆、RF等等,或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本发明操作的计算机程序代码,所述程序设计语言包括面向对象的程序设计语言,诸如Java、Smalltalk、C++,还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中,远程计算机可以通过任意种类的网络,包括局域网(LAN)或广域网(WAN),连接到用户计算机,或者,可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。
本发明实施例提供的一种自动驾驶最优轨迹生成装置,包括生成模块、构建模块、转换模块、配置模块和选取模块。
生成模块用于基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;构建模块用于根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;转换模块用于基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;配置模块用于基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;选取模块用于基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹。
本发明实施例的自动驾驶最优轨迹生成装置,结合实际道路场景下的车辆信息,采用基于多项式拟合的算法生成候选轨迹,同时根据道路的不同形状以及不同行为指令,考虑成本影响权重的组成,对成本影响权重进行归一化处理,去除成本影响权重的单位限制,同时基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,使得成本影响权重的权重系数会随着不同场景而改变,从而筛选出最优轨迹,有效降低了运算时长,并提高了时效性,实现最优轨迹的准确生成。
以上所述仅是本申请的具体实施方式,使本领域技术人员能够理解或实现本申请。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的,本文中所定义的一般原理可以在不脱离本申请的精神或范围的情况下,在其它实施例中实现。因此,本申请将不会被限制于本文所示的这些实施例,而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。
本发明是参照根据本发明实施例的方法、设备(***)和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
Claims (6)
1.一种自动驾驶最优轨迹生成方法,其特征在于,具体包括以下步骤:
基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;
根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;
基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;
基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;
基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹;
其中,所述基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成,具体步骤包括:
基于道路形式以及车辆的位姿信息和速度信息,对道路进行纵向距离和横向距离的采样,并将采样点作为目标点;
对起始点和目标点进行状态参数配置,所述起始点为车辆当前的位置,所述状态参数包括航向角、曲率和车辆横纵向坐标;
根据配置的状态参数,采用五次多项式生成候选轨迹;
其中,所述采用五次多项式生成候选轨迹,其中,生成的候选轨迹的表达式为:
Li(s)=ai5*s5+ai4*s4+ai3*s3+ai2*s2+ai1*s+ai0
其中,Li(s)表示候选轨迹,ai5表示五次多项式的五次项系数,ai4表示五次多项式的四次项系数,ai3表示五次多项式的三次项系数,ai2表示五次多项式的二次项系数,ai1表示五次多项式的一次项系数,ai0表示五次多项式的零次项系数,s表示候选轨迹中车辆和起始点的纵向距离;
其中,所述成本影响权重包括平均曲率变化率、平均曲率、平均横向偏移量、累积横向偏移量、行为指令和距离障碍物的最小距离成本;
其中,所述平均曲率变化率的计算公式为:
所述平均曲率的计算公式为:
所述平均横向偏移量的计算公式为:
所述累积横向偏移量的计算公式为:
2.如权利要求1所述的一种自动驾驶最优轨迹生成方法,其特征在于,构建的候选轨迹的成本函数具体为:
Costi=Wi1*Jerki+Wi2*Curi+Wi3*Averg_Lati+
Wi4*Acumlt_Lati+Wi5*Bhvi+Wi6*Obs_MinDisti
其中,Costi表示候选轨迹的成本,Jerki表示平均曲率变化率,Wi1表示平均曲率变化率的权重系数,Curi表示平均曲率,Wi2表示平均曲率的权重系数,Averg_Lati表示平均横向偏移量,Wi3表示平均横向偏移量的权重系数,Acumlt_Lati表示累积横向偏移量,Wi4表示累积横向偏移量的权重系数,Bhvi表示行为指令,Wi5表示行为指令的权重系数,Obs_MinDisti表示距离障碍物的最小距离成本,Wi6表示距离障碍物的最小距离成本的权重系数。
3.如权利要求1所述的一种自动驾驶最优轨迹生成方法,其特征在于:
所述基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,具体为:基于道路曲率、车辆速度和自动驾驶行为指令,并针对不同驾驶工况,配置候选轨迹各成本影响权重的权重系数;
所述驾驶工况包括直道巡航、转弯、静态避障、会车和换道。
4.如权利要求3所述的一种自动驾驶最优轨迹生成方法,其特征在于,所述基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数,对于配置过程,具体为:
在直道巡航驾驶工况下,Wi1、Wi2、Wi3和车辆速度成正比,Wi4和车辆速度成反比,Wi3、Wi4和道路曲率成正比;
在转弯驾驶工况下,Wi4和车辆速度成反比,Wi3、Wi4和道路曲率成正比;
在静态避障驾驶工况下,Wi4和碰撞时间成反比,所述碰撞时间基于相对距离和相对速度计算得到;
在会车驾驶工况下,Wi4和碰撞时间成反比;
在换道驾驶工况下,Wi1、Wi2、Wi3和车辆速度成正比,Wi4和车辆速度成反比,Wi4和换道完成时间成正比。
5.如权利要求1所述的一种自动驾驶最优轨迹生成方法,其特征在于,基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值,其中,进行归一化处理的计算公式为:
y=tan-1x*2/P1
其中,y表示转换后的成本影响权重,x表示成本影响权重,P1表示圆周率。
6.一种自动驾驶最优轨迹生成装置,其特征在于,包括:
生成模块,其用于基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成;
构建模块,其用于根据候选轨迹的成本影响权重和各成本影响权重的权重系数,构建候选轨迹的成本函数;
转换模块,其用于基于反正切函数,对候选轨迹的各成本影响权重进行归一化处理,转换为无量纲的数值;
配置模块,其用于基于道路曲率、车辆速度和自动驾驶行为指令,配置候选轨迹各成本影响权重的权重系数;
选取模块,其用于基于构建的成本函数,并结合配置的权重系数和转换后的成本影响权重,进行各候选轨迹的成本计算,并将成本最小候选轨迹作为最优轨迹;
其中,所述基于道路形式以及车辆的位姿信息和速度信息,进行候选轨迹的生成,具体步骤包括:
基于道路形式以及车辆的位姿信息和速度信息,对道路进行纵向距离和横向距离的采样,并将采样点作为目标点;
对起始点和目标点进行状态参数配置,所述起始点为车辆当前的位置,所述状态参数包括航向角、曲率和车辆横纵向坐标;
根据配置的状态参数,采用五次多项式生成候选轨迹;
其中,所述采用五次多项式生成候选轨迹,其中,生成的候选轨迹的表达式为:
Li(s)=ai5*s5+ai4*s4+ai3*s3+ai2*s2+ai1*s+ai0
其中,Li(s)表示候选轨迹,ai5表示五次多项式的五次项系数,ai4表示五次多项式的四次项系数,ai3表示五次多项式的三次项系数,ai2表示五次多项式的二次项系数,ai1表示五次多项式的一次项系数,ai0表示五次多项式的零次项系数,s表示候选轨迹中车辆和起始点的纵向距离;
其中,所述成本影响权重包括平均曲率变化率、平均曲率、平均横向偏移量、累积横向偏移量、行为指令和距离障碍物的最小距离成本;
其中,所述平均曲率变化率的计算公式为:
所述平均曲率的计算公式为:
所述平均横向偏移量的计算公式为:
所述累积横向偏移量的计算公式为:
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111406002.7A CN114167860B (zh) | 2021-11-24 | 2021-11-24 | 一种自动驾驶最优轨迹生成方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111406002.7A CN114167860B (zh) | 2021-11-24 | 2021-11-24 | 一种自动驾驶最优轨迹生成方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114167860A CN114167860A (zh) | 2022-03-11 |
CN114167860B true CN114167860B (zh) | 2023-07-07 |
Family
ID=80480722
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111406002.7A Active CN114167860B (zh) | 2021-11-24 | 2021-11-24 | 一种自动驾驶最优轨迹生成方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114167860B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114964288A (zh) * | 2022-05-16 | 2022-08-30 | 北京京东乾石科技有限公司 | 路径规划方法及装置、无人车 |
Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2021175313A1 (zh) * | 2020-03-05 | 2021-09-10 | 中国第一汽车股份有限公司 | 自动驾驶控制方法、装置、车辆及存储介质 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10515321B2 (en) * | 2017-09-11 | 2019-12-24 | Baidu Usa Llc | Cost based path planning for autonomous driving vehicles |
US20190204842A1 (en) * | 2018-01-02 | 2019-07-04 | GM Global Technology Operations LLC | Trajectory planner with dynamic cost learning for autonomous driving |
US11378961B2 (en) * | 2018-04-17 | 2022-07-05 | Baidu Usa Llc | Method for generating prediction trajectories of obstacles for autonomous driving vehicles |
US11199847B2 (en) * | 2018-09-26 | 2021-12-14 | Baidu Usa Llc | Curvature corrected path sampling system for autonomous driving vehicles |
CN109375632B (zh) * | 2018-12-17 | 2020-03-20 | 清华大学 | 自动驾驶车辆实时轨迹规划方法 |
CN112824198B (zh) * | 2019-11-21 | 2023-05-02 | 北京京东乾石科技有限公司 | 一种轨迹决策方法、装置、设备和存储介质 |
-
2021
- 2021-11-24 CN CN202111406002.7A patent/CN114167860B/zh active Active
Patent Citations (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
WO2021175313A1 (zh) * | 2020-03-05 | 2021-09-10 | 中国第一汽车股份有限公司 | 自动驾驶控制方法、装置、车辆及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN114167860A (zh) | 2022-03-11 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112677995B (zh) | 一种车辆轨迹规划方法、装置、存储介质及设备 | |
CN110834644B (zh) | 一种车辆控制方法、装置、待控制车辆及存储介质 | |
Sadat et al. | Jointly learnable behavior and trajectory planning for self-driving vehicles | |
CN106371439B (zh) | 一种统一的自动驾驶横向规划方法与*** | |
WO2018176593A1 (zh) | 一种面向无人自行车的局部避障路径规划方法 | |
CN112810630B (zh) | 一种自动驾驶车辆轨迹规划方法及*** | |
CN111238521B (zh) | 一种无人驾驶车辆的路径规划方法及*** | |
CN112389427A (zh) | 车辆轨迹优化方法、装置、电子设备和存储介质 | |
CN115683145B (zh) | 一种基于轨迹预测的自动驾驶安全避障方法 | |
CN111896004A (zh) | 一种狭窄通道车辆轨迹规划方法及*** | |
CN114167860B (zh) | 一种自动驾驶最优轨迹生成方法及装置 | |
Piacentini et al. | A macroscopic model for platooning in highway traffic | |
Gu et al. | Safe-state enhancement method for autonomous driving via direct hierarchical reinforcement learning | |
CN113015981A (zh) | 利用第一原则和约束进行有效、连续和安全学习的***和方法 | |
Zhang et al. | Structured road-oriented motion planning and tracking framework for active collision avoidance of autonomous vehicles | |
Liu et al. | Impact of sharing driving attitude information: A quantitative study on lane changing | |
Ren et al. | Self-learned intelligence for integrated decision and control of automated vehicles at signalized intersections | |
CN114715193A (zh) | 一种实时轨迹规划方法及*** | |
Yu et al. | Hierarchical framework integrating rapidly-exploring random tree with deep reinforcement learning for autonomous vehicle | |
Zheng et al. | Learning-based safe control for robot and autonomous vehicle using efficient safety certificate | |
CN117360546A (zh) | 一种车辆控制方法、车辆、电子设备及存储介质 | |
Smit et al. | Informed sampling-based trajectory planner for automated driving in dynamic urban environments | |
CN115973179A (zh) | 模型训练方法、车辆控制方法、装置、电子设备及车辆 | |
CN114537435B (zh) | 一种自动驾驶中的实时整车轨迹规划方法 | |
Hsu et al. | Design of smooth path based on the conversion between η 3 spline and Bezier curve |
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 |