CN116572994A - 一种车辆速度规划方法、装置及计算机可读介质 - Google Patents
一种车辆速度规划方法、装置及计算机可读介质 Download PDFInfo
- Publication number
- CN116572994A CN116572994A CN202310836241.9A CN202310836241A CN116572994A CN 116572994 A CN116572994 A CN 116572994A CN 202310836241 A CN202310836241 A CN 202310836241A CN 116572994 A CN116572994 A CN 116572994A
- Authority
- CN
- China
- Prior art keywords
- obstacle
- speed
- vehicle
- acceleration
- track
- 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 31
- 230000001133 acceleration Effects 0.000 claims abstract description 83
- 238000012423 maintenance Methods 0.000 claims abstract description 20
- 238000009825 accumulation Methods 0.000 claims abstract description 11
- 230000006870 function Effects 0.000 claims description 52
- 230000000875 corresponding effect Effects 0.000 claims description 25
- 238000005457 optimization Methods 0.000 claims description 13
- 238000010276 construction Methods 0.000 claims description 4
- 230000001186 cumulative effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 2
- 230000002596 correlated effect Effects 0.000 claims description 2
- 239000004973 liquid crystal related substance Substances 0.000 description 12
- 230000006399 behavior Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W60/00—Drive control systems specially adapted for autonomous road vehicles
- B60W60/001—Planning or execution of driving tasks
- B60W60/0011—Planning or execution of driving tasks involving control alternatives for a single driving scenario, e.g. planning several paths to avoid obstacles
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
- B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
- B60W30/095—Predicting travel path or likelihood of collision
- B60W30/0953—Predicting travel path or likelihood of collision the prediction being responsive to vehicle dynamic parameters
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W30/00—Purposes of road vehicle drive control systems not related to the control of a particular sub-unit, e.g. of systems using conjoint control of vehicle sub-units, or advanced driver assistance systems for ensuring comfort, stability and safety or drive control systems for propelling or retarding the vehicle
- B60W30/08—Active safety systems predicting or avoiding probable or impending collision or attempting to minimise its consequences
- B60W30/095—Predicting travel path or likelihood of collision
- B60W30/0956—Predicting travel path or likelihood of collision the prediction being responsive to traffic or environmental parameters
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W50/0097—Predicting future conditions
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W50/00—Details of control systems for road vehicle drive control not related to the control of a particular sub-unit, e.g. process diagnostic or vehicle driver interfaces
- B60W50/0098—Details of control systems ensuring comfort, safety or stability not otherwise provided for
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2552/00—Input parameters relating to infrastructure
- B60W2552/50—Barriers
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2720/00—Output or target parameters relating to overall vehicle dynamics
- B60W2720/10—Longitudinal speed
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B60—VEHICLES IN GENERAL
- B60W—CONJOINT CONTROL OF VEHICLE SUB-UNITS OF DIFFERENT TYPE OR DIFFERENT FUNCTION; CONTROL SYSTEMS SPECIALLY ADAPTED FOR HYBRID VEHICLES; ROAD VEHICLE DRIVE CONTROL SYSTEMS FOR PURPOSES NOT RELATED TO THE CONTROL OF A PARTICULAR SUB-UNIT
- B60W2720/00—Output or target parameters relating to overall vehicle dynamics
- B60W2720/10—Longitudinal speed
- B60W2720/106—Longitudinal acceleration
-
- 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
Abstract
本发明公开了一种车辆速度规划方法，利用自车规划路径和障碍物预测轨迹得到自车和周边障碍物的第一碰撞点，基于第一碰撞点通过历史试验标定过的经验数据得到第一期望速度和第一期望加速度和维持时间，从而能够实现安全、舒适的避开障碍物。如果维持时间比累积时间长，则对障碍物的预测轨迹与实际轨迹的重合度程度进行比较，基于重合度程度比较结果调整进一步减速的速率从而实现对障碍物的安全且舒适的避让，因此，本发明根据自车周围的障碍物对自车正常行驶时的加塞概率，来对自车进行自适应的减速行为，即加塞对自车行进影响的概率越大则减速越快，从而同时保证安全性及舒适性。本发明还公开了车辆速度规划装置和计算机可读存储介质。
Description
技术领域
本发明属于自动驾驶的运动技术领域，具体涉及一种车辆速度规划方法、装置及计算机可读介质。
背景技术
自动驾驶轨迹规划是指在自动驾驶系统中，根据当前车辆状态、目的地信息以及道路交通情况，计算出一条能够引导车辆安全、准确地到达目的地的行驶轨迹。
自动驾驶轨迹规划通常包括：地图构建：在自动驾驶系统中，需要提前构建高精度地图，包括道路形状、车道线、交通信号灯等信息，以便为轨迹规划提供基础数据。目的地设定：在自动驾驶系统中，需要明确车辆的目的地信息，以便进行轨迹规划。初始轨迹生成：根据当前车辆位置和目的地信息，生成一条初始的行驶轨迹，包括速度、方向和路径等信息。轨迹优化：对初始轨迹进行优化，考虑各种因素，如道路限速、交通信号灯、车辆行驶限制、车辆动态性能等，以生成更加安全、高效和舒适的行驶轨迹。轨迹跟踪：在自动驾驶系统中，需要使用控制器和执行器来跟踪生成的轨迹，包括车辆的速度、方向和位置等，确保车辆按照预定的轨迹行驶。
在自动驾驶系统中，轨迹规划是一个关键组成部分，直接影响着自动驾驶车辆的性能和安全性。随着自动驾驶技术的不断发展，轨迹规划算法也在不断优化和改进。
在自动驾驶轨迹规划中，需要接收上层的预测模块给出的障碍物预测轨迹，来帮助规划模块生成舒适且安全的轨迹，而这其中往往通过速度规划来作出对于动态障碍物的响应。
目前的预测模块与规划模块的接口通常只包含单一的预测轨迹，这造成当障碍物车辆的预测行为不明确时，预测出的轨迹会前后帧跳动，导致规划的速度前后波动，从而影响乘坐的舒适性。
除此之外，单一的预测轨迹接口也会导致当只有一个预测结果超过某设定阈值时，才会下发给规划模块作出响应，在某些场景下将会造成规划处理较晚从而引发安全问题。例如在加塞场景下，只有预测出障碍物车辆的加塞概率超过0.5时，才会下发对应的加塞预测轨迹给到规划，造成减速过晚或乘客觉得过于危险
发明内容
本发明提供了一种车辆速度规划方法，该车流速度规划方法能够使得车辆能够基于周边障碍物的位置形成自适应的加速或减速行为，保证车辆的安全性和舒适性。
本发明具体实施例提供了一种车辆速度规划方法，包括：
S1、获得自车规划路径，正常规划速度，以及障碍物预测轨迹和对应的预测概率，基于自车规划路径和障碍物预测轨迹找出自车与障碍物第一次重叠或相交时对应的轨迹点作为第一碰撞点，获得自车规划路径的起始规划点到第一碰撞点的累积时间和累加距离；
S2、当障碍物预测轨迹对应的预测概率大于预测概率阈值时，设定自车的第一期望速度、第一期望加速度和维持时间，当累积时间大于维持时间时，将第一期望速度和第一期望加速度作为自车的安全规划速度和加速度以减速避开障碍物，当避开障碍物后，自车的速度恢复到正常规划速度；
当累积时间不大于维持时间时，且当障碍物预测轨迹与障碍物实际轨迹的累积重合度高于重合度阈值时，基于障碍物预测轨迹中高于重合度阈值的位置的数量、累积时间和累加距离分别得到第二期望速度和第二期望加速度；
S3、基于第二期望速度和第二期望加速度构建代价函数，通过构建的代价函数基于设定的约束条件对第二期望速度和第二期望加速度进行优化，将优化结果作为自车的安全规划速度和加速度从而实现进一步减速以避开障碍物，当避开障碍物后，自车的速度恢复到正常规划速度。
进一步的，基于障碍物预测轨迹中高于重合度阈值的位置个数、累积时间和累加距离分别得到第二期望速度和第二期望加速度，包括：
基于累积时间和累加距离分别得到初始期望速度和初始期望加速度，基于障碍物预测轨迹中高于重合度阈值的位置个数构建权重函数，通过权重函数得到权重值；
向初始期望速度和初始期望加速度分别赋予权重值得到第二期望速度和第二期望加速度；
所述权重函数与障碍物预测轨迹中高于重合度阈值的位置个数呈正相关，通过权重函数得到的权重值的范围为(0，1]，障碍物预测轨迹中高于重合度阈值的位置个数达到个数阈值时，权重值为1。
进一步的，基于第二期望速度和第二期望加速度构建代价函数，所述代价函数包括速度、加速度和舒适度代价函数；
其中，所述速度代价函数通过将第二期望速度与待优化速度参数相减后平方构建得到；
所述加速度代价函数通过将第二期望加速度和待优化加速度参数相减后平方构建得到；
所述舒适度代价函数通过待优化加速度参数的变化率构建得到。
进一步的，通过构建的代价函数基于设定的约束条件对第二期望速度和加速度进行优化，优化的结果为优化后的待优化速度参数值和待优化加速度参数值；
所述设定的约束包括：
待优化加速度为对应待优化速度的一阶导数；
构建代价函数的起始位置对应的初始待优化速度和初始待优化加速度分别为自车在所述起始规划点处的实际速度和实际加速度；
待优化速度参数的数值小于自车最大速度能力；
和待优化加速度参数的数值范围为小于自车最大加速度能力且大于自车最大减速度能力。
进一步的，还包括：当障碍物预测轨迹对应的预测概率不大于预测概率阈值时，自车通过正常规划速度进行行驶。
进一步的，还包括：当累积时间不大于维持时间时，且当障碍物预测轨迹与障碍物实际轨迹的累积重合度不高于重合度阈值时，重新构建当前障碍物的障碍物预测轨迹；
如果重新构建的障碍物预测轨迹与自车规划路径没有重叠或相交，则自车通过正常规划速度进行行驶；
如果重新构建的障碍物预测轨迹与自车规划路径有重叠或相交，且累积时间不大于维持时间，且障碍物预测轨迹与障碍物实际轨迹的累积重合度高于重合度阈值时，则重复步骤S1-S3，得到自车的安全规划速度和加速度。
进一步的，获得障碍物预测轨迹与障碍物实际轨迹的累积重合度，包括：
基于障碍物实际的位置信息通过重合度函数找到与每个障碍物实际位置最近邻的位于障碍物预测轨迹上的最近预测位置，计算每个最近预测位置与对应的障碍物实际位置的欧式距离，并将计算得到的欧式距离进行累加得到累积重合度；
所述位置信息包括障碍物的中心点坐标、障碍物的朝向和中心点坐标与第一个预测点的障碍物累加距离。
进一步的，自车规划路径上的自车与每个障碍物预测轨迹上的障碍物的第一次重叠或相交，包括：
若障碍物包括多条初始障碍物预测轨迹，从多条初始障碍物预测轨迹中选择预测概率最高且与自车规划路径重叠或相交的预测轨迹作为障碍物预测轨迹；
确定障碍物包围框和自车包围框，遍历障碍物预测轨迹上的预测轨迹点，找出障碍物包围框和自车包围框的第一次重叠或相交的轨迹点，将第一次重叠或相交时对应的轨迹点作为第一碰撞点。
本发明具体实施例还提供了一种车辆速度规划装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现所述的车辆速度规划方法。
本发明具体实施例还提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的车辆速度规划方法。
与现有技术相比，本发明的有益效果为：
本发明利用自车规划路径和障碍物预测轨迹得到自车和周边障碍物第一次碰撞的第一碰撞点，基于第一碰撞点通过历史试验标定过的经验数据得到第一期望速度、第一期望加速度和维持时间，从而能够实现安全、舒适的避开障碍物。如果维持时间比累积时间长，则对障碍物的预测轨迹与实际轨迹的重合度程度进行比较，基于重合度程度比较结果调整进一步减速的速率从而实现对障碍物的安全且舒适的避让，因此，本发明根据自车周围的障碍物对自车正常行驶时的加塞概率，来对自车进行自适应的减速行为，即加塞对自车行进影响的概率越大则减速越快，从而同时保证安全性及舒适性。
附图说明
图1为本发明具体实施例提供的一种车辆速度规划方法的流程图；
图2为本发明具体实施例提供的一种车辆速度规划方法的循环流程图；
图3为本发明具体实施例提供的减速观测情况示意图；
图4为本发明具体实施例提供的减速停障碍情况示意图；
图5为本发明具体实施例提供的减速过程曲线图。
具体实施方式
为了使本发明的目的、技术方案和技术效果更佳清楚明白，以下结合说明书附图和实施例，对本发明作进一步详细说明。
为了达到安全、舒适的避让障碍物的目的，本发明具体实施例通过遍历障碍物预测轨迹得到与自车规划路径第一次重叠和相交的第一碰撞点，基于第一碰撞点与自车规划路径的第一个规划点间形成的路径通过试验车标定经验得到第一期望速度、第一期望加速度和维持时间，以使得用户能够安全、舒适的避开障碍物，当维持时间超过了累积时间，即无法基于第一期望速度、第一期望加速度和维持时间避开障碍物时，则基于障碍物预测轨迹和障碍物实际轨迹的重合程度，对累积时间和累加距离得到的初始期望速度和初始期望加速进行赋权重，从而加快减速的速率，达到安全避开障碍物的目的。
本发明具体实施例提供了一种车辆速度规划方法，如图1和图2所示，包括：
步骤1、获得与自车规划路径相交或重叠的障碍物预测轨迹：获得自车规划路径，正常规划速度，以及障碍物预测轨迹和对应的预测概率，通过预测模块对自车周边的障碍物进行轨迹预测，挑选出与自车规划路径有重叠区域或相交点的轨迹，若同一障碍物存在多条障碍物预测轨迹与自车规划路径产生重叠或相交，则选取预测概率最高或者选取危险系数最高的障碍物预测轨迹，具体步骤如下：
步骤1.1：根据预测模块给出的信息生成障碍物预测轨迹集 ，其中，/> 表示第i个障碍物的第j条障碍物预测轨迹，第i个障碍物的第j条障碍物预测轨迹/>包括n个离散轨迹点/>、/>、… />，其中 ，/> 表示直角坐标系坐标，k为离散轨迹点的索引，/> 表示向量 />与直角坐标系中X轴的夹角，/> 表示到达第i个障碍物第j条预测轨迹上的第k个离散轨迹点的时间。
步骤1.2：对障碍物预测轨迹集中单个障碍物的多条障碍物预测轨迹进行遍历。针对单条障碍物预测轨迹，比较连列该条障碍物预测轨迹中的轨迹点与路径规划模块输出的m个自车路径点 /> 、/>、… />，r为reference的简称，用于表示参考路径点，表示自车路径参考点，从中挑选出预测轨迹中障碍物包围框与路径点上自车包围框有重叠部分的轨迹。若同一障碍物存在多条预测轨迹与规划路径有重叠区域，则按照某种策略挑选一条预测轨迹；本实施例中设计为选择概率最高的与规划路径有重叠区域的预测轨迹。
步骤2、遍历障碍物预测轨迹获得第一碰撞点，第一碰撞点的累积时间和累加距离：计算障碍物预测轨迹与自车规划路径产生重叠或相交的第一处预测轨迹点，称为第一碰撞点，包含该预测轨迹点的累积时间和累加距离，该累加距离值为预测轨迹点的s值，表示该点距离自车规划路径的起始规划点的累加距离，将自车规划路径的起始规划点到第一碰撞点的时间作为累积时间，该重叠的判断条件为该时刻障碍物包围盒与自车包围盒存在交叠部分，具体步骤如下：
步骤2.1、针对每个障碍物从步骤1中挑选出的预测轨迹，计算出第一个与自车规划路径上的点产生重叠的轨迹点。该重叠的判断条件为遍历障碍物预测轨迹点，以 为障碍物中心点坐标，/> 为障碍物朝向，/> 为该障碍物长宽所形成的障碍物包围框，找出第一个与任一路径点上的自车包围框产生重叠的轨迹点，该轨迹点称为第一碰撞点/> ，其中col为collision的缩写，用于表示碰撞点。
步骤2.2、遍历每个障碍物，按照步骤2.1的方式找到每个障碍物与自车规划路径的第一碰撞点，即挑选出每个障碍物最先与路径产生重叠的第一碰撞点，即 最小，从而得出规划路径中与每个障碍物预计发生碰撞的最短时间/> ，对每个第一碰撞点依据策略进行排序，该策略为与路径发生碰撞的累加距离，或预计碰撞的累积时间。
步骤3、根据第一碰撞点的累加时间、累加距离，以及第一碰撞点对应的障碍物预测概率轨迹的预测概率，将减速过程分为观察阶段和停障阶段，在观察阶段并生成对应时刻的第一期望加速度和第一期望速度值，在停障阶段生成对应时刻的第二期望加速度和第二期望速度值，具体步骤如下：
步骤3.1、基于步骤2.2提供的第一碰撞点的排序遍历每个碰撞点对应障碍物的障碍物预测轨迹，并将得到的每条障碍物预测轨迹的预测概率与预测轨迹概率阈值进行比较，当该条障碍物预测轨迹的概率小于预测概率阈值时，自车进行正常规划速度行驶，当该条障碍物预测轨迹的概率不小于预测概率阈值时，进入减速观察阶段，如图3和图5所示，根据历史经验，即通过之前完成的车辆标定经验数据得到第一期望速度、第一期望加速度和维持时间。
当累积时间大于维持时间时，将第一期望速度和第一期望加速度作为自车的安全规划速度和加速度，当避开障碍物后，自车的速度恢复到正常规划速度。
步骤3.2、当累积时间不大于维持时间时，且当障碍物预测轨迹与障碍物实际轨迹的累积重合度高于重合度阈值时，基于障碍物预测轨迹中高于重合度阈值的位置个数、累积时间和累加距离分别得到第二期望速度和第二期望加速度从而实现进一步减速，则进入减速停障过程，如图4所示。
在一具体实施例中，根据该障碍物的后续实际位置及朝向对该条预测轨迹的可能性进行后验，构建重合度函数表示实际位置与预测轨迹的相关性，并随着相关性的累积时间逐渐增大，如下式所示：
其中， 为第i个障碍物的实际位置坐标及朝向，即/>，/>为障碍物预测轨迹中的最近预测位置k的位置坐标。基于障碍物实际的位置信息通过重合度函数找到与每个障碍物实际位置最近邻的位于障碍物预测轨迹上的最近预测位置，计算将每个最近预测位置与对应的障碍物实际的欧式距离，并将计算得到的欧式距离进行累加得到累加重合度。所述位置信息包括障碍物的中心点坐标、障碍物的朝向和中心点坐标与第一个预测点的障碍物累加距离。
步骤3.3、若重合度函数高于重合度阈值，则进入减速停障阶段，如图4所示，根据此时的重合度函数生成该阶段的第二期望速度及第二期望加速度，并随着重合度函数的增大而减少，即重合度越高说明障碍物预测轨迹越准确，碰撞发生的可能性越高，则加快减少期望速度和期望加速度。当重合度函数高于一段时长后，可判定该预测轨迹收敛，按最后生成的第二期望速度及第二加速度进行稳定的速度规划，其中针对第i个障碍物的第二期望速度 及第二期望加速度 /> 如下所示：
其中， 为权重函数，通过权重函数得到的权重值范围为(0,1]，与高于重合度阈值/>的位置个数呈正相关，高于重合度阈值的位置个数达到个数阈值时，权重值为1，n u 为高于重合度阈值的位置次数，N u 为个数阈值，当障碍物预测轨迹中高于重合度阈值的位置个数不断增加，则第二期望速度减速越快，当障碍物预测轨迹中高于重合度阈值的位置数量达到个数阈值时权重值为1，第二期望速度和第二期望加速度趋于稳定，t col 为第i个障碍物的第一碰撞点对应的累积时间，s col 为第i个障碍物的第一碰撞点对应的累加距离，t end 为自车规划轨迹最后点的时间，s end 为自车规划轨迹最后点的累加距离，p 2，p 1为调节的系数权重，c 1和c 2为调节系数。
步骤3.4：若重合度函数不高于重合度阈值，则表示该预测轨迹与障碍物实际位置相差过大，反馈预测模块以重新获取预测轨迹信息。若更新后的障碍物预测轨迹信息经过前序步骤与自车无碰撞风险，则从减速观察阶段的期望速度提到正常道路的规划速度，即正常规划速度，如果重新构建的障碍物预测轨迹与自车规划路径有重叠或相交，且累积时间不大于维持时间，且障碍物预测轨迹与障碍物实际轨迹的累积重合度高于重合度阈值时，则重复步骤2-步骤3，得到自车的安全规划速度和加速度。
步骤4、根据生成的第二期望加速度与第二期望速度，构造并计算该时刻的车辆行驶速度规划的代价函数及约束，对规划速度及加速度进行优化，得到优化结果，具体包括以下子步骤：
步骤4.1、本发明具体实施例基于第二期望速度和第二期望加速度构建代价函数，所述代价函数包括速度、加速度和舒适度代价函数；其中，所述速度代价函数通过将第二期望速度与待优化速度参数相减后平方构建得到；所述加速度代价函数通过将第二期望加速度和待优化加速度参数相减后平方构建得到；所述舒适度代价函数通过待优化加速度参数的变化率构建得到。
本发明具体实施例提供的代价函数 为：
其中， 为待优化变量 />、/>、… 、/>，其中，/> 为待优化速度变量，/> 为待优化加速度变量，/> 为第q个加速度变量 ，/> 为离散时间值；/> 为与期望速度的相似代价权重，/> 为与期望加速度的相似代价权重，/> 为舒适性代价权重，/> 为第i个障碍物的第二期望速度，C 1 （﹒）为速度代价函数，C 2 （﹒）为加速度代价函数，C 3 （﹒）为舒适度代价函数。
步骤4.2：构造速度优化问题的连续性约束，保证相邻优化变量为连续性变化，如下所示：
其中， 为一阶导函数。
步骤4.3：构造速度优化问题的起始点约束，保证优化结果从当前车辆的实际状态值出发，如下所示：
其中， 为车辆当前状态 /> ，即自车的当前速度及加速度。
步骤4.4：构造车辆动力学相关约束，保证优化结果车辆执行器可执行，如下所示：
其中， 表示车辆的最大速度能力，/> 表示车辆的最大加速度能力， 表示车辆的最大减速度能力，v e 和a e 分别为 /> 中的第e个速度和加速度。
步骤4.5：根据步骤4.1得到的速度优化代价函数及步骤4.2至步骤4.4得到的约束条件，通过求解该含约束条件的优化问题，得到最终的优化结果，如下所示：
。
步骤五，将优化后的速度及加速度规划结果与规划路径融合得到带有速度及加速度信息的轨迹规划结果，具体的，根据步骤4.5得到的速度优化结果 ，将其和路径规划模块输出的路径点进行联合，得到最终的规划轨迹点/>,/>,… />。
本发明具体实施例还提供了一种车辆速度规划装置，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现所述的车辆速度规划方法。
本发明具体实施例还提供了一种计算机可读存储介质，其上存储有程序，该程序被处理器执行时，实现所述的车辆速度规划方法。
Claims (10)
1.一种车辆速度规划方法，其特征在于，包括：
S1、获得自车规划路径，正常规划速度，以及障碍物预测轨迹和对应的预测概率，基于自车规划路径和障碍物预测轨迹找出自车与障碍物第一次重叠或相交时对应的轨迹点作为第一碰撞点，获得自车规划路径的起始规划点到第一碰撞点的累积时间和累加距离；
S2、当障碍物预测轨迹对应的预测概率大于预测概率阈值时，设定自车的第一期望速度、第一期望加速度和维持时间，当累积时间大于维持时间时，将第一期望速度和第一期望加速度作为自车的安全规划速度和加速度以减速避开障碍物，当避开障碍物后，自车的速度恢复到正常规划速度；
当累积时间不大于维持时间时，且当障碍物预测轨迹与障碍物实际轨迹的累积重合度高于重合度阈值时，基于障碍物预测轨迹中高于重合度阈值的位置的数量、累积时间和累加距离分别得到第二期望速度和第二期望加速度；
S3、基于第二期望速度和第二期望加速度构建代价函数，通过构建的代价函数基于设定的约束条件对第二期望速度和第二期望加速度进行优化，将优化结果作为自车的安全规划速度和加速度从而实现进一步减速以避开障碍物，当避开障碍物后，自车的速度恢复到正常规划速度。
2.根据权利要求1所述的车辆速度规划方法，其特征在于，基于障碍物预测轨迹中高于重合度阈值的位置个数、累积时间和累加距离分别得到第二期望速度和第二期望加速度，包括：
基于累积时间和累加距离分别得到初始期望速度和初始期望加速度，基于障碍物预测轨迹中高于重合度阈值的位置个数构建权重函数，通过权重函数得到权重值；
向初始期望速度和初始期望加速度分别赋予权重值得到第二期望速度和第二期望加速度；
所述权重函数与障碍物预测轨迹中高于重合度阈值的位置个数呈正相关，通过权重函数得到的权重值的范围为(0，1]，障碍物预测轨迹中高于重合度阈值的位置个数达到个数阈值时，权重值为1。
3.根据权利要求1所述的车辆速度规划方法，其特征在于，基于第二期望速度和第二期望加速度构建代价函数，所述代价函数包括速度、加速度和舒适度代价函数；
其中，所述速度代价函数通过将第二期望速度与待优化速度参数相减后平方构建得到；
所述加速度代价函数通过将第二期望加速度和待优化加速度参数相减后平方构建得到；
所述舒适度代价函数通过待优化加速度参数的变化率构建得到。
4.根据权利要求3所述的车辆速度规划方法，其特征在于，通过构建的代价函数基于设定的约束条件对第二期望速度和加速度进行优化，优化的结果为优化后的待优化速度参数值和待优化加速度参数值；
所述设定的约束包括：
待优化加速度为对应待优化速度的一阶导数；
构建代价函数的起始位置对应的初始待优化速度和初始待优化加速度分别为自车在所述起始规划点处的实际速度和实际加速度；
待优化速度参数的数值小于自车最大速度能力；
和待优化加速度参数的数值范围为小于自车最大加速度能力且大于自车最大减速度能力。
5.根据权利要求1所述的车辆速度规划方法，其特征在于，还包括：当障碍物预测轨迹对应的预测概率不大于预测概率阈值时，自车通过正常规划速度进行行驶。
6.根据权利要求1所述的车辆速度规划方法，其特征在于，还包括：当累积时间不大于维持时间时，且当障碍物预测轨迹与障碍物实际轨迹的累积重合度不高于重合度阈值时，重新构建当前障碍物的障碍物预测轨迹；
如果重新构建的障碍物预测轨迹与自车规划路径没有重叠或相交，则自车通过正常规划速度进行行驶；
如果重新构建的障碍物预测轨迹与自车规划路径有重叠或相交，且累积时间不大于维持时间，且障碍物预测轨迹与障碍物实际轨迹的累积重合度高于重合度阈值时，则重复步骤S1-S3，得到自车的安全规划速度和加速度。
7.根据权利要求1所述的车辆速度规划方法，其特征在于，获得障碍物预测轨迹与障碍物实际轨迹的累积重合度，包括：
基于障碍物实际的位置信息通过重合度函数找到与每个障碍物实际位置最近邻的位于障碍物预测轨迹上的最近预测位置，计算每个最近预测位置与对应的障碍物实际位置的欧式距离，并将计算得到的欧式距离进行累加得到累积重合度；
所述位置信息包括障碍物的中心点坐标、障碍物的朝向和中心点坐标与第一个预测点的障碍物累加距离。
8.根据权利要求1所述的车辆速度规划方法，其特征在于，自车规划路径上的自车与每个障碍物预测轨迹上的障碍物的第一次重叠或相交，包括：
若障碍物包括多条初始障碍物预测轨迹，从多条初始障碍物预测轨迹中选择预测概率最高且与自车规划路径重叠或相交的预测轨迹作为障碍物预测轨迹；
确定障碍物包围框和自车包围框，遍历障碍物预测轨迹上的预测轨迹点，找出障碍物包围框和自车包围框的第一次重叠或相交的轨迹点，将第一次重叠或相交时对应的轨迹点作为第一碰撞点。
9.一种车辆速度规划装置，其特征在于，包括存储器和一个或多个处理器，所述存储器中存储有可执行代码，所述一个或多个处理器执行所述可执行代码时，用于实现权利要求1-8中任一项所述的车辆速度规划方法。
10.一种计算机可读存储介质，其特征在于，其上存储有程序，该程序被处理器执行时，实现权利要求1-8中任一项所述的车辆速度规划方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310836241.9A CN116572994B (zh) | 2023-07-10 | 2023-07-10 | 一种车辆速度规划方法、装置及计算机可读介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310836241.9A CN116572994B (zh) | 2023-07-10 | 2023-07-10 | 一种车辆速度规划方法、装置及计算机可读介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116572994A true CN116572994A (zh) | 2023-08-11 |
| CN116572994B CN116572994B (zh) | 2023-09-22 |
Family
ID=87536149
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310836241.9A Active CN116572994B (zh) | 2023-07-10 | 2023-07-10 | 一种车辆速度规划方法、装置及计算机可读介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116572994B (zh) |
Citations (15)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017165687A1 (en) * | 2016-03-24 | 2017-09-28 | Honda Motor Co., Ltd. | System and method for trajectory planning for unexpected pedestrians |
| US20180186373A1 (en) * | 2016-12-30 | 2018-07-05 | Neusoft Corporation | Method, device and apparatus for planning vehicle speed |
| CN109960261A (zh) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | 一种基于碰撞检测的动态障碍物避让方法 |
| US20200117200A1 (en) * | 2018-10-15 | 2020-04-16 | Zoox, Inc. | Responsive vehicle control |
| CN111032468A (zh) * | 2017-08-30 | 2020-04-17 | 马自达汽车株式会社 | 车辆控制装置 |
| JP2020097346A (ja) * | 2018-12-19 | 2020-06-25 | トヨタ自動車株式会社 | 車両の走行制御装置 |
| WO2021135728A1 (zh) * | 2019-12-30 | 2021-07-08 | 郑州宇通客车股份有限公司 | 一种自动驾驶车辆碰撞预测判断方法及装置 |
| CN113386766A (zh) * | 2021-06-17 | 2021-09-14 | 东风汽车集团股份有限公司 | 一种连续的、周期自适应同步的在线轨迹规划系统及方法 |
| CN115123216A (zh) * | 2022-08-05 | 2022-09-30 | 国汽智控(北京)科技有限公司 | 车辆避障方法、装置、车辆、设备及存储介质 |
| CN115123310A (zh) * | 2022-08-31 | 2022-09-30 | 中汽数据(天津)有限公司 | 无人车避障局部路径规划方法、设备和存储介质 |
| US20220379889A1 (en) * | 2021-05-28 | 2022-12-01 | Zoox, Inc. | Vehicle deceleration planning |
| WO2022257283A1 (zh) * | 2021-06-08 | 2022-12-15 | 新石器慧通（北京）科技有限公司 | 车辆避障方法、装置、电子设备及存储介质 |
| CN115697802A (zh) * | 2020-08-10 | 2023-02-03 | 保时捷股份公司 | 用于预测车辆轨迹的装置和方法 |
| WO2023070258A1 (zh) * | 2021-10-25 | 2023-05-04 | 华为技术有限公司 | 一种车辆的轨迹规划方法、装置及车辆 |
| CN116331257A (zh) * | 2023-03-16 | 2023-06-27 | 嬴彻星创智能科技(上海)有限公司 | 车辆速度规划方法、装置、电子设备及存储介质 |
-
2023
- 2023-07-10 CN CN202310836241.9A patent/CN116572994B/zh active Active
Patent Citations (16)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2017165687A1 (en) * | 2016-03-24 | 2017-09-28 | Honda Motor Co., Ltd. | System and method for trajectory planning for unexpected pedestrians |
| US20180186373A1 (en) * | 2016-12-30 | 2018-07-05 | Neusoft Corporation | Method, device and apparatus for planning vehicle speed |
| CN111032468A (zh) * | 2017-08-30 | 2020-04-17 | 马自达汽车株式会社 | 车辆控制装置 |
| US20200117200A1 (en) * | 2018-10-15 | 2020-04-16 | Zoox, Inc. | Responsive vehicle control |
| JP2020097346A (ja) * | 2018-12-19 | 2020-06-25 | トヨタ自動車株式会社 | 車両の走行制御装置 |
| CN109960261A (zh) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | 一种基于碰撞检测的动态障碍物避让方法 |
| WO2021135728A1 (zh) * | 2019-12-30 | 2021-07-08 | 郑州宇通客车股份有限公司 | 一种自动驾驶车辆碰撞预测判断方法及装置 |
| CN113119964A (zh) * | 2019-12-30 | 2021-07-16 | 郑州宇通客车股份有限公司 | 一种自动驾驶车辆碰撞预测判断方法及装置 |
| CN115697802A (zh) * | 2020-08-10 | 2023-02-03 | 保时捷股份公司 | 用于预测车辆轨迹的装置和方法 |
| US20220379889A1 (en) * | 2021-05-28 | 2022-12-01 | Zoox, Inc. | Vehicle deceleration planning |
| WO2022257283A1 (zh) * | 2021-06-08 | 2022-12-15 | 新石器慧通（北京）科技有限公司 | 车辆避障方法、装置、电子设备及存储介质 |
| CN113386766A (zh) * | 2021-06-17 | 2021-09-14 | 东风汽车集团股份有限公司 | 一种连续的、周期自适应同步的在线轨迹规划系统及方法 |
| WO2023070258A1 (zh) * | 2021-10-25 | 2023-05-04 | 华为技术有限公司 | 一种车辆的轨迹规划方法、装置及车辆 |
| CN115123216A (zh) * | 2022-08-05 | 2022-09-30 | 国汽智控(北京)科技有限公司 | 车辆避障方法、装置、车辆、设备及存储介质 |
| CN115123310A (zh) * | 2022-08-31 | 2022-09-30 | 中汽数据(天津)有限公司 | 无人车避障局部路径规划方法、设备和存储介质 |
| CN116331257A (zh) * | 2023-03-16 | 2023-06-27 | 嬴彻星创智能科技(上海)有限公司 | 车辆速度规划方法、装置、电子设备及存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116572994B (zh) | 2023-09-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106926844B (zh) | 一种基于实时环境信息的动态自动驾驶换道轨迹规划方法 | |
| JP4416020B2 (ja) | 走行計画生成装置 | |
| CN112292646A (zh) | 车辆的控制系统、用于控制车辆的方法和非暂时性计算机可读存储器 | |
| US20190310644A1 (en) | Vehicle path identification | |
| CN113071520A (zh) | 车辆行驶控制方法及装置 | |
| US20110054793A1 (en) | Environment prediction device | |
| US20220314968A1 (en) | Electronic control device | |
| CN112652180A (zh) | 一种无信号灯交叉口下智能车辆高效行驶的分布式控制方法及系统 | |
| JP2015230547A (ja) | 運転操作を生成するシステムおよび方法 | |
| CN112965476B (zh) | 一种基于多窗口模型的高速无人车轨迹规划系统及方法 | |
| WO2021196041A1 (zh) | 一种关键目标选取方法、装置及系统 | |
| CN110782707A (zh) | 汽车变道控制方法和装置、汽车、存储介质 | |
| CN114511999B (zh) | 一种行人行为预测方法及装置 | |
| CN112859838B (zh) | 一种自动驾驶控制方法、装置、设备和介质 | |
| Chen et al. | A hierarchical hybrid system of integrated longitudinal and lateral control for intelligent vehicles | |
| CN111634293A (zh) | 一种基于交通间隙的自动驾驶车辆的自动变道方法 | |
| CN114559956A (zh) | 自动驾驶车辆的避让方法、装置、设备及计算机存储介质 | |
| CN115140096A (zh) | 一种基于样条曲线与多项式曲线的自动驾驶轨迹规划方法 | |
| CN114932901A (zh) | 一种自适应速度规划方法、装置和域控制器 | |
| CN113353070A (zh) | 车辆检测和响应 | |
| US20220266871A1 (en) | Remote support system and remote support method | |
| CN113538909A (zh) | 一种自动驾驶车辆交通事件预测方法及装置 | |
| CN116572994B (zh) | 一种车辆速度规划方法、装置及计算机可读介质 | |
| JP7356892B2 (ja) | 車両の走行環境推定方法、及び、走行環境推定システム | |
| EP4045370A1 (en) | Adaptive cruise control |
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 |