CN113377112B - 一种自动驾驶速度规划及状态协调方法和装置 - Google Patents

Abstract

本发明涉及一种自动驾驶速度规划及状态协调方法和装置，车辆根据行为规划层发出的行为指令处于对应的工况，所述自动驾驶速度规划及状态协调方法用于输出最优速度策略至纵向控制层以实现在不同工况下进行速度规划以及在不同工况切换时进行状态协调，其包括步骤：基于行为规划层发出的行为指令采集对应的车辆状态信息；对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略。

Description

一种自动驾驶速度规划及状态协调方法和装置

技术领域

本发明涉及自动驾驶技术领域，特别涉及一种自动驾驶速度规划及状态协调方法和装置。

背景技术

随着自动驾驶技术的不断发展，为了提高安全性和舒适性，对速度规划技术的要求也越来越高。目前市场上的辅助驾驶系统中使用CC系统和ACC系统进行速度规划，速度规划模块考虑单一场景下的速度控制，工况状态发生切换后，其适应性不强。

相关技术(CN 110979324A)提到一种智能驾驶中安全舒适高效的ACC跟车速度规划方法。不论前车在加速或者减速，还是以恒定速度行驶，自车规划出合理的期望速度和期望加速度，然后将速度和加速度输出给控制模块。相关技术(CN 110531771A)发明专利提出了一种速度规划方法及装置、车辆，该方法包括：从目标行驶路径上确定出预设数量原始路径点；利用预设线条将原始路径点进行连接；计算预设线条上的原始路径点对应的曲率；根据原始路径点对应的曲率、预设加速度和相邻的原始路径点之间的距离，计算目标行驶路径上的跟踪路径点对应的目标速度，其中，跟踪路径点为自动驾驶过程中车辆在各个时刻所处的目标行驶路径上的路径点。

然而，在高速自动驾驶系统中会经常面对不同工况的切换，如何进一步保证安全驾驶的前提下提高驾驶员的舒适感，需要对速度规划系统做全新的设计。

发明内容

本发明实施例提供一种自动驾驶速度规划及状态协调方法和装置，以实现在不同工况下进行速度规划以及在不同工况切换时进行状态协调。

一方面提供了一种自动驾驶速度规划及状态协调方法，其特征在于，自动驾驶中车辆根据行为规划层发出的行为指令处于对应的工况，所述自动驾驶速度规划及状态协调方法用于输出最优速度策略至纵向控制层以实现在不同工况下进行速度规划以及在不同工况切换时进行状态协调，其包括步骤：

基于行为规划层发出的行为指令采集对应的车辆状态信息；

对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；

根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；

根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略。

一些实施例中,基于行为规划层发出的行为指令采集对应的车辆状态信息，包括步骤：

当所述行为指令为紧急刹车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

当所述行为指令为停车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

当所述行为指令为定速巡航时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度；

当所述行为指令为自适应跟车时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度。

一些实施例中,对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略，包括步骤：

当所述行为指令为紧急刹车时，根据第一公式获取对应工况下的待选速度策略；

所述第一公式为：S_i(t)＝α_i0+α_i1t+α_i2t²；

其中，S_i(t)表示第i条二次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数，系数α_i0、α_i1、α_i2表示为：

其中，t₁为当前时刻，t₂为目标时刻，v₁为当前时刻车辆的纵向速度，s₂为目标时刻车辆的纵向位移，v₂为目标时刻车辆的纵向速度。

一些实施例中,对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略，包括步骤：

当所述行为指令为停车时，根据第二公式获取对应工况下的待选速度策略；

所述第二公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³；

其中，S_i(t)表示第i条三次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度。

一些实施例中,对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略，包括步骤：

当所述行为指令为定速巡航时，根据第三公式获取对应工况下的待选速度策略；

所述第三公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴；

其中，S_i(t)表示第i条四次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

一些实施例中,对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略，包括步骤：

当所述行为指令为自适应跟车时，根据第四公式获取对应工况下的待选速度策略；

所述第四公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴+α_i5t⁵；

其中，S_i(t)表示第i条五次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4、α_i5表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵

向速度，a₂为目标时刻t₂车辆的纵向加速度。

一些实施例中,所述车辆运动学约束条件包括：

纵向速度规划方向上的车辆位移为单调递增；

纵向速度规划方向上的车辆速度不超过预设最大限速；

纵向速度规划方向的加速度不超过预设最大加速度限制。

一些实施例中,根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略，包括步骤：

根据第五公式获取最优速度策略；

所述第五公式为：

其中，f_i为总的速度成本函数，k₁、k₂、k₃分别为纵向位移系数、纵向速度系数和纵向加速度系数，S_i(t)为车辆在第i条候选ST纵向位移曲线函数，

为车辆在第i条候选ST纵向目标点的速度曲线函数，

为车辆在第i条候选ST纵向目标点的加速度曲线函数，v_tar为期望速度，a_tar为期望加速度；

所述最优速度策略为根据第五公式约束后的S_i(t)、S_i'(t)和S_i”(t)。

一些实施例中,根据工况切换情况以及车辆运行状态判断是否启动重新规划，包括步骤：

当车辆发生工况切换、车辆的运行状态达到目标规划的状态或车辆的运行速度相对于目标规划速度超出阈值时，启动重新规划，否则不启动重新规划并输出最优速度策略；

所述启动重新规划包括：

基于当前行为规划层发出的行为指令采集对应的车辆状态信息；

对采集的车辆状态信息选择对应的速度平滑算法计算对应工况下的待选速度策略；

根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略。

另一方面，提供了一种自动驾驶速度规划及状态协调装置，其特征在于，其包括：

状态信息采集模块，其用于根据行为规划层发出的行为指令采集对应的车辆状态信息；

待选速度计算模块，其用于对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；

速度筛选模块，其用于根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；

速度协调模块，其用于根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略。

本发明实施例，根据自动驾驶的实际情况，可能出现各种不同的工况(包括定速巡航、自适应跟车、定点停车、紧急制动以及速度规划完成后重新规划等工况)，针对不同工况进行相对应的参数(车辆状态信息)采集，并针对不同工况选择对应的算法进行处理，可实时兼顾多种不同的期望速度和加速度需求；同时，考虑实际车辆运动学约束限制，对候选的速度曲线进行动态筛选，可保证最终跟车速度收敛在目标车速且跟车距离收敛在(目标时距*目标车速)，使最终停车距离稳定在安全距离内，从而提高了自动驾驶的稳定性和安全性。最后，当发生工况之间进行状态切换的情况时，通过设置判断是否重新启动速度规划的条件，对状态切换时的速度进行进一步的优化，以保证在状态切换下自动驾驶车辆速度和加速度的适应性。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种自动驾驶速度规划及状态协调方法流程示意图；

图2为本发明实施例提供的不同工况下采集对应工况的车辆状态信息示意图；

图3为本发明实施例提供的速度约束条件下的速度规划V-T图；

图4为本发明实施例提供的加速度约束条件下的加速度规划a-T图；

图5为本发明实施例提供的车辆运动学约束条件下的速度规划S-T/V-T/a-T图；

图6为本发明实施例提供的自动驾驶车辆在不同工况下的速度规划和状态协调；

图7为本发明实施例提供的一种自动驾驶速度规划及状态协调装置示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1所示，本发明实施例提供了一种自动驾驶速度规划及状态协调方法，其包括步骤：

S100:基于行为规划层发出的行为指令采集对应的车辆状态信息；

S200:对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；

S300:根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；

S400:根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略。

需要说明的是，自动驾驶中车辆根据行为规划层发出的行为指令处于对应的工况，所述自动驾驶速度规划及状态协调方法用于输出最优速度策略至纵向控制层以实现在不同工况下进行速度规划以及在不同工况切换时进行状态协调。

可以理解的是，根据自动驾驶的实际情况，可能出现各种不同的工况(包括定速巡航、自适应跟车、定点停车、紧急制动以及速度规划完成后重新规划等工况)，本实施例通过针对不同工况进行相对应的参数(车辆状态信息)采集，并针对不同工况选择对应的算法进行处理，可实时兼顾多种不同的期望速度和加速度需求；同时，考虑实际车辆运动学约束限制，对候选的速度曲线进行动态筛选，可保证最终跟车速度收敛在目标车速且跟车距离收敛在(目标时距*目标车速)，使最终停车距离稳定在安全距离内，从而提高了自动驾驶的稳定性和安全性。最后，当发生工况之间进行状态切换的情况时，通过设置判断是否重新启动速度规划的条件，对状态切换时的速度进行进一步的优化，以保证在状态切换下自动驾驶车辆速度和加速度的适应性。

在一些实施例中,步骤S100包括：

S110:当所述行为指令为紧急刹车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

S120:当所述行为指令为停车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

S130:当所述行为指令为定速巡航时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度；

S140:当所述行为指令为自适应跟车时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度。

具体而言，如图2(a)所示，当上游行为规划层突然发现目标障碍物切入时，发出紧急刹车的行为指令后，则本车获取当前状态速度，以及目标状态的停车距离和速度，即进行从[v₁]到[s₂,v₂]的速度时间采样。其中，v₁为当前时刻t₁车辆的纵向速度，s₁为当前时刻t₁的车辆纵向位移，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度。通过设定紧急制动时距，即目标距离s_t可表示为：

s_t＝(s₂-s₁)-s_safe

其中，s_safe为安全距离，即可保证最终的刹车距离稳定在安全距离内。

如图2(b)所示，当上游行为规划层发现前方静止的目标障碍物时，发出停车的行为指令后，则本车获取当前状态的速度和加速度，以及目标状态的停车距离和速度，即从[v₁,a₁]到[s₂，0]的速度时间采样，其中v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，s₂为目标时刻t₂车辆的纵向位移。

如图2(c)所示，当上游行为规划层发现周围道路环境良好时，发出定速巡航的行为指令后，则本车获取当前状态的位置、速度和加速度，以及目标状态的期望速度和加速度，即从[s₁,v₁,a₁]到[v₂,a₂]的速度时间采样，其中，a₂为目标时刻t₂车辆的纵向加速度。

如图2(d)所示，当上游行为规划层发现前方行驶的目标障碍物时，发出跟车的行为指令后，则本车获取当前状态的位置、速度和加速度，以及目标状态的期望距离、速度和加速度，即从[s₁,v₁,a₁]到[s₂,v₂,a₂]的速度时间采样；其中，通过设定跟车时距，即目标速度v₂可表示为：

v₂＝v_f+k{(s₂-s₁)-max(s_safe,v_fT_s)}

v_f是前车绝对速度，k是距离系数，(s₂-s₁)表示当前相对距离，max(s_safe,v_fT_s)则表示最大安全距离，最终跟车速度收敛在目标车速，跟车距离收敛在v_fT_s(目标时距*目标车速)。

可优选地，考虑到驾驶行为、路况、限速等因素，可用一个期望车辆保持的驾驶速度来约束整个纵向速度规划过程。即自动驾驶车辆从t₁时刻开始速度规划至t₂时刻完成，则在这段时间内，需要不断地实时更新速度曲线。自动驾驶车辆实时规划从s₁位置到s₂位置的速度和加速度等参数，输出给控制执行系统。当到达目标速度时，则反馈给行为规划层已完成相应的状态指令。

在一些实施例中,步骤S200包括步骤S210：

当所述行为指令为紧急刹车时，根据第一公式获取对应工况下的待选速度策略；

所述第一公式为：S_i(t)＝α_i0+α_i1t+α_i2t²；

其中，S_i(t)表示第i条二次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数(i＝1，2…)，系数α_i0、α_i1、α_i2表示为：

其中，t₁为当前时刻，t₂为目标时刻，v₁为当前时刻车辆的纵向速度，s₂为目标时刻车辆的纵向位移，v₂为目标时刻车辆的纵向速度。

可以理解的是，当上游行为规划层突然发现目标障碍物切入时，发出紧急刹车的行为指令后，则本车获取当前状态速度，以及目标状态的停车距离和速度，迅速进行紧急减速。即车辆需要在一定的距离内，迅速地从当前车速减速到紧急制动。将系数α_i0、α_i1、α_i2代入第一公式中，可计算出速度曲线的ST模型，保证紧急制动时速度的连续性。

在一些实施例中,步骤S200包括步骤S220：

当所述行为指令为停车时，根据第二公式获取对应工况下的待选速度策略；

所述第二公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³；

其中，S_i(t)表示第i条三次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度。

可以理解的是，当上游行为规划层发现前方静止的目标障碍物时，发出停车的行为指令后，则本车获取当前状态的速度，以及目标状态的停车距离和速度，进行停车减速。即车辆需要在一定的距离内，平稳地从当前车速减速到静止。将系数α_i0、α_i1、α_i2、α_i3代入第二公式，计算出速度曲线的ST模型可保证定点停车时加速度的连续性。

在一些实施例中,步骤S200包括步骤S230：

当所述行为指令为定速巡航时，根据第三公式获取对应工况下的待选速度策略；

所述第三公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴；

其中，S_i(t)表示第i条四次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

可以理解的是，当上游行为规划层发现周围道路环境良好时，发出定速巡航的行为指令后，则本车获取当前状态的位置、速度和加速度，以及目标状态的期望速度和加速度，进行定速巡航。即车辆只需要从当前车速平滑的加速或减速到目标速度，没有距离的约束。将系数α_i0、α_i1、α_i2、α_i3、α_i4代入第三公式，计算出速度曲线的ST模型可保证定速巡航时加加速度(S_i(t)位移函数的3阶导数)的连续性。

在一些实施例中,步骤S200包括步骤S240：

当所述行为指令为自适应跟车时，根据第四公式获取对应工况下的待选速度策略；

所述第四公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴+α_i5t⁵；

其中，S_i(t)表示第i条五次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4、α_i5表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

可以理解的是，当上游行为规划层发现前方行驶的目标障碍物时，发出跟车的行为指令后，则本车获取当前状态的位置、速度和加速度，以及目标状态的期望速度和加速度，进行自适应跟车。即车辆需要在目标距离内，从当前车速平滑的加速或减速到目标障碍物速度。将系数α_i0、α_i1、α_i2、α_i3、α_i4、α_i5代入第四公式，计算出速度曲线的ST模型可保证自适应跟车时加加速度的稳定性。

可优选地，由速度规划ST模型可知，每条候选速度曲线的速度、加速度和加加速度由S(t)函数确定。其中，第j条Polynomial曲线上第i个点的速度、加速度和加加速度分别表述如下：

在一些实施例中,所述车辆运动学约束条件包括：

纵向速度规划方向上的车辆位移为单调递增；

纵向速度规划方向上的车辆速度不超过预设最大限速；

纵向速度规划方向的加速度不超过预设最大加速度限制。

需要说明的是，当车辆偏离车道中心线且存在偏角时，若行驶轨迹为从当前位置到目标点位置车辆需要行驶的累积距离，对应的纵向速度规划方向为实时行驶轨迹的切向方向。

具体而言，考虑车辆不能出现倒车现象，即纵向的位移为单调递增，在位移约束条件下需要满足：

S(t_i)≤S(t_i+1)

即优化目标为：S'(t_i)≥0，其中，S(t_i)为第i条多项式ST位移曲线函数，S(t_i+1)为第i+1条多项式ST位移曲线函数，S'(t_i)为待优化的目标。

为了达到设定目标点的速度S'(t)，车辆最大和最小速度的约束为：

v_min≤S'(t)≤v_max

如图3(a)所示，车辆从起点速度逐渐降低到负值，然后再加速到目标速度。虽然规划出的速度曲线目标速度满足要求，但实际速度不会低于零，因此需要对速度曲线进行筛选。

如图3(b)所示，车辆从起点到目标点，速度先增大后减小，虽然到达目标速度，但在行驶过程中规划的最大速度超过限制，即v_max＞v_limt，因此需要筛选掉该速度。

可采用加速度的最大值来保留出加速度较小的路径，淘汰加速度较大的路径。从当前点到达设定目标点规划的加速度S”(t)，车辆加减速度的上下界约束为：

a_min≤S”(t)≤a_max

如图4所示，车辆从起点到目标点进行速度规划，其中的加速度范围在[-2m/s²，4m/s²]。虚线和实线曲线分别表示不同纵向距离对应的加速度，可以看出实线曲线加速度波动范围较小来保留，而虚线曲线加速度波动范围较大所以筛选掉。

一些实施例中,所述车辆运动学约束条件包括：

纵向速度规划方向上的车辆位移为单调递增；

纵向速度规划方向上的车辆速度不超过预设最大限速；

纵向速度规划方向的加速度不超过预设最大加速度限制。

可优选地，步骤S300包括步骤S310：

根据第五公式获取最优速度策略；

所述第五公式为：

其中，f_i为总的速度成本函数，k₁、k₂、k₃分别为纵向位移系数、纵向速度系数和纵向加速度系数，S_i(t)为车辆在第i条候选ST纵向位移曲线函数，

为车辆在第i条候选ST纵向目标点的速度曲线函数，

为车辆在第i条候选ST纵向目标点的加速度曲线函数，v_tar为期望速度，a_tar为期望加速度；

所述最优速度策略为根据第五公式约束后的S_i(t)、S_i'(t)和S_i”(t)。

如图5所示，以当前点作为起点，当前速度0m/s以及加速度0m/s2，目标点的纵向距离50m，目标速度10m/s，目标加速度0m/s2。可见位移曲线单调递增，速度均匀地加速到10m/s，加速度区间在[0m/s²，1.5m/s²]之间满足约束限制。

在一些实施例中，步骤S400包括步骤：

S410:当车辆发生工况切换、车辆的运行状态达到目标规划的状态或车辆的运行速度相对于目标规划速度超出阈值时，启动重新规划，否则不启动重新规划并输出最优速度策略；

所述启动重新规划包括：

基于当前行为规划层发出的行为指令采集对应的车辆状态信息；

对采集的车辆状态信息选择对应的速度平滑算法计算对应工况下的待选速度策略；

根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略。

可以理解的是，考虑到高速场景下定速巡航、自适应跟车、前车切入切出、紧急制动、变道以及规划完成后进行重新速度规划RePlan等工况，很可能频繁出现不同状态的切换。因此，设计了一种速度状态协调策略，实现在不同工况下的速度规划，保证在状态切换下自动驾驶车辆速度的稳定性。

具体而言，当获取实时的行为指令后，如果行为发生切换(巡航/跟车/停车/制动之间互相发生状态切换)，或当前车辆已完成一次规划(本车实际速度达到目标车速)，或当前时刻下本车的实际速度与规划的期望速度超过一定阈值等条件下，则触发重新速度规划RePlan(即重复前述采集车辆状态信息、计算待选速度策略以及筛选出最优速度策略)，从而获取重新速度规划RePlan后的最优速度策略。可在这些情况下获取更优的速度，提高用户体验。而在其他不需要RePlan的情况下，可直接输出当前已经获取的最优速度策略。

如图6所示，横坐标均为规划时间，图6(a)为实时的行为ID与规划时间图，图6(b)为实时的重规划状态RePlan与规划时间图,图6(c)为实时目标速度、规划速度与规划时间图，图6(d)为实时相对距离与规划时间图。

定义行为ID＝15表示人工驾驶模式，ID＝11表示进入自动驾驶模式，ID＝5表示自动驾驶起步模式，ID＝1表示进入定速巡航模式，ID＝2表示进入自适应跟车模式，ID＝4表示进入紧急制动模式。

初始时刻，依次进入ID＝15和ID＝11，车辆进入自动驾驶准备模式；

然后进入ID＝1，系统默认的定速巡航车速为40km/h，本车从起步稳定加速到t＝40s时，速度达到40km/h；

当t＝40s时，ID＝2进入自适应跟车模式，触发了RePlan。通过设定跟车时距，即目标速度v₂为68.4km/h。

其中，v₂＝v_f+k{(s₂-s₁)-max(s_safe,v_fT_s)}。

接着，本车开始从40km/h加速到60km/h，此时相对距离从t＝40s的252m降低到t＝80s的66m。这时目标车速v₂开始逐渐减小，规划的速度也开始减小，并稳定收敛到前车速度Tar_V＝60km/h，此时对应的相对距离为60/3.6*2.5＝41.7m。

同样地，当t＝160s时，前车从60km/h减速到40km/h，规划的自动驾驶速度随着目标车速v₂逐渐减小，最后也稳定收敛到前车速度Tar_V＝40km/h，此时对应的相对距离为40/3.6*2.5＝27.7m。

最后，当t＝300s时，ID＝2继续保持在跟车状态，但前车速度减速到静止，这时本车需要进行定点停车。定点停车时最终停车安全距离。

如果，当t＝300s时，触发ID＝4紧急制动，触发了RePlan，本车从当前车速40km/h减速到静止。紧急制动最终保持在安全距离内。

本发明实施例的有益效果包括：

1)考虑了定速巡航、自适应跟车、定点停车等不同工况，采用不同的多项式算法进行速度规划，实时计算出不同的目标速度和目标加速度曲线；

2)考虑车辆约束条件进行筛选，建立了成本函数模型进行最优的速度筛选；

3)分别设定跟车时距和紧急制动时距，配置不同的目标速度和目标距离，保证了最终的跟车速度收敛在目标车速，跟车距离也收敛在稳定距离，最终的停车距离稳定在安全距离内，提高了稳定性和安全性；

4)设计了一种速度状态协调策略，实现在不同工况下的速度规划，保证了在状态切换下自动驾驶车辆速度和加速度的适应性。

另一方面，如图7所示，本发明实施例还提供一种自动驾驶速度规划及状态协调装置，其特征在于，其包括：

状态信息采集模块，其用于根据行为规划层发出的行为指令采集对应的车辆状态信息；

待选速度计算模块，其用于对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；

速度筛选模块，其用于根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；

速度协调模块，其用于根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略。

一些实施例中，状态信息采集模块还用于：

当所述行为指令为紧急刹车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

当所述行为指令为停车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

当所述行为指令为定速巡航时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度；

当所述行为指令为自适应跟车时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度。

一些实施例中，待选速度计算模块还用于：

当所述行为指令为紧急刹车时，根据第一公式获取对应工况下的待选速度策略；

所述第一公式为：S_i(t)＝α_i0+α_i1t+α_i2t²；

其中，S_i(t)表示第i条二次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数，系数α_i0、α_i1、α_i2表示为：

其中，t₁为当前时刻，t₂为目标时刻，v₁为当前时刻车辆的纵向速度，s₂为目标时刻车辆的纵向位移，v₂为目标时刻车辆的纵向速度。

一些实施例中，待选速度计算模块还用于：

当所述行为指令为停车时，根据第二公式获取对应工况下的待选速度策略；

所述第二公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³；

其中，S_i(t)表示第i条三次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数，系数α_i0、α_i1、α_i2、α_i3表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度。

一些实施例中，待选速度计算模块还用于：

当所述行为指令为定速巡航时，根据第三公式获取对应工况下的待选速度策略；

所述第三公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴；

其中，S_i(t)表示第i条四次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数，系数α_i0、α_i1、α_i2、α_i3、α_i4表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

一些实施例中，待选速度计算模块还用于：

当所述行为指令为自适应跟车时，根据第四公式获取对应工况下的待选速度策略；

所述第四公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴+α_i5t⁵；

其中，S_i(t)表示第i条五次多项式ST纵向位移曲线函数，其中i取值1至5，系数α_i0、α_i1、α_i2、α_i3、α_i4、α_i5表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

一些实施例中,所述车辆运动学约束条件包括：

纵向速度规划方向上的车辆位移为单调递增；

纵向速度规划方向上的车辆速度不超过预设最大限速；

纵向速度规划方向的加速度不超过预设最大加速度限制。

一些实施例中，速度筛选模块还用于：

根据第五公式获取最优速度策略；

所述第五公式为：

其中，f_i为总的速度成本函数，k₁、k₂、k₃分别为纵向位移系数、纵向速度系数和纵向加速度系数，S_i(t)为车辆在第i条候选ST纵向位移曲线函数，

为车辆在第i条候选ST纵向目标点的速度曲线函数，

为车辆在第i条候选ST纵向目标点的加速度曲线函数，v_tar为期望速度，a_tar为期望加速度；

所述最优速度策略为根据第五公式约束后的S_i(t)、S_i'(t)和S_i”(t)。一些实施例中，速度协调模块还用于：

当车辆发生工况切换、车辆的运行状态达到目标规划的状态或车辆的运行速度相对于目标规划速度超出阈值时，启动重新规划，否则不启动重新规划并输出最优速度策略；

所述启动重新规划包括：

基于当前行为规划层发出的行为指令采集对应的车辆状态信息；

对采集的车辆状态信息选择对应的速度平滑算法计算对应工况下的待选速度策略；

根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略。

在本发明的描述中，需要说明的是，术语“上”、“下”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

需要说明的是，在本发明中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

以上所述仅是本发明的具体实施方式，使本领域技术人员能够理解或实现本发明。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所申请的原理和新颖特点相一致的最宽的范围。

Claims (6)

1.一种自动驾驶速度规划及状态协调方法，其特征在于，自动驾驶中车辆根据行为规划层发出的行为指令处于对应的工况，所述自动驾驶速度规划及状态协调方法用于输出最优速度策略至纵向控制层以实现在不同工况下进行速度规划以及在不同工况切换时进行状态协调，其包括步骤：

基于行为规划层发出的行为指令采集对应的车辆状态信息；

对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；

根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；

根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略；

对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略，包括步骤：

当所述行为指令为紧急刹车时，根据第一公式获取对应工况下的待选速度策略；

所述第一公式为：S_i(t)＝α_i0+α_i1t+α_i2t²；

其中，S_i(t)表示第i条二次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数，系数α_i0、α_i1、α_i2表示为：

其中，t₁为当前时刻，t₂为目标时刻，v₁为当前时刻车辆的纵向速度，s₂为目标时刻车辆的纵向位移，v₂为目标时刻车辆的纵向速度；

当所述行为指令为停车时，根据第二公式获取对应工况下的待选速度策略；

所述第二公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³；

其中，S_i(t)表示第i条三次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度；

当所述行为指令为定速巡航时，根据第三公式获取对应工况下的待选速度策略；

所述第三公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴；

其中，S_i(t)表示第i条四次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度；

当所述行为指令为自适应跟车时，根据第四公式获取对应工况下的待选速度策略；

所述第四公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴+α_i5t⁵；

其中，S_i(t)表示第i条五次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4、α_i5表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

2.如权利要求1所述的自动驾驶速度规划及状态协调方法，其特征在于,基于行为规划层发出的行为指令采集对应的车辆状态信息，包括步骤：

当所述行为指令为紧急刹车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

当所述行为指令为停车时，采集车辆当前状态的速度以及目标状态的停车距离和速度；

当所述行为指令为定速巡航时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度；

当所述行为指令为自适应跟车时，采集车辆当前状态的位置、速度和加速度以及目标状态的期望速度和加速度。

3.如权利要求1所述的自动驾驶速度规划及状态协调方法，其特征在于,所述车辆运动学约束条件包括：

纵向速度规划方向上的车辆位移为单调递增；

纵向速度规划方向上的车辆速度不超过预设最大限速；

纵向速度规划方向的加速度不超过预设最大加速度限制。

4.如权利要求1所述的自动驾驶速度规划及状态协调方法，其特征在于,根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略，包括步骤：

根据第五公式获取最优速度策略；

所述第五公式为：

其中，f_i为总的速度成本函数，k₁、k₂、k₃分别为纵向位移系数、纵向速度系数和纵向加速度系数，S_i(t)为车辆在第i条候选ST纵向位移曲线函数，

为车辆在第i条候选ST纵向目标点的速度曲线函数，

为车辆在第i条候选ST纵向目标点的加速度曲线函数，v_tar为期望速度，a_tar为期望加速度；

所述最优速度策略为根据第五公式约束后的S_i(t)、S_i'(t)和S_i”(t)。

5.如权利要求1所述的自动驾驶速度规划及状态协调方法，其特征在于,根据工况切换情况以及车辆运行状态判断是否启动重新规划，包括步骤：

当车辆发生工况切换、车辆的运行状态达到目标规划的状态或车辆的运行速度相对于目标规划速度超出阈值时，启动重新规划，否则不启动重新规划并输出最优速度策略；

所述启动重新规划包括：

基于当前行为规划层发出的行为指令采集对应的车辆状态信息；

对采集的车辆状态信息选择对应的速度平滑算法计算对应工况下的待选速度策略；

根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略。

6.一种自动驾驶速度规划及状态协调装置，其特征在于，其包括：

状态信息采集模块，其用于根据行为规划层发出的行为指令采集对应的车辆状态信息；

待选速度计算模块，其用于对采集的车辆状态信息选择对应的速度平滑算法以计算对应工况下的待选速度策略；

速度筛选模块，其用于根据车辆运动学约束条件对所述待选速度策略进行优化以获得最优速度策略；

速度协调模块，其用于根据工况切换情况以及车辆运行状态判断是否启动重新规划，若不启动重新规划则输出所述最优速度策略；

所述待选速度计算模块还用于：

当所述行为指令为紧急刹车时，根据第一公式获取对应工况下的待选速度策略；

所述第一公式为：S_i(t)＝α_i0+α_i1t+α_i2t²；

其中，S_i(t)表示第i条二次多项式ST纵向位移曲线函数，其中i表示待选曲线的个数且i为大于或等于1的整数，系数α_i0、α_i1、α_i2表示为：

其中，t₁为当前时刻，t₂为目标时刻，v₁为当前时刻车辆的纵向速度，s₂为目标时刻车辆的纵向位移，v₂为目标时刻车辆的纵向速度；

当所述行为指令为停车时，根据第二公式获取对应工况下的待选速度策略；

所述第二公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³；

其中，S_i(t)表示第i条三次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度；

当所述行为指令为定速巡航时，根据第三公式获取对应工况下的待选速度策略；

所述第三公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴；

其中，S_i(t)表示第i条四次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度；

当所述行为指令为自适应跟车时，根据第四公式获取对应工况下的待选速度策略；

所述第四公式为：S_i(t)＝α_i0+α_i1t+α_i2t²+α_i3t³+α_i4t⁴+α_i5t⁵；

其中，S_i(t)表示第i条五次多项式ST纵向位移曲线函数，系数α_i0、α_i1、α_i2、α_i3、α_i4、α_i5表示为：

其中，t₁为当前时刻，t₂为目标时刻，s₁为当前时刻t₁的车辆纵向位移，v₁为当前时刻t₁车辆的纵向速度，a₁为当前时刻t₁车辆的纵向加速度，s₂为目标时刻t₂车辆的纵向位移，v₂为目标时刻t₂车辆的纵向速度，a₂为目标时刻t₂车辆的纵向加速度。

Images