CN106828493A - 一种自动驾驶车辆分层式纵向规划控制系统及方法 - Google Patents

Abstract

本发明涉及一种自动驾驶车辆分层式纵向规划控制系统及方法，首先根据感知设备获得的数据进行局部路径规划，生成纵向局部路径参数；其次根据纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果；然后根据自动驾驶车辆的纵向速度随时间的规划结果判断行驶模式；最后进行纵向速度控制。本发明所述方法和系统能与自动驾驶车辆横向规划协调统一，有利于自动驾驶车辆规划控制方法的进一步发展在不同场景下，纵向规划对车辆的所有纵向行驶情况进行了分类处理，广泛适用于各种城市道路工况，同时能兼容自动驾驶车辆其他系统，如对感知系统的噪声有一定抗干扰性，能适应某些执行机构无法对连续量控制的特性。

Description

一种自动驾驶车辆分层式纵向规划控制系统及方法

技术领域

本发明涉及自动驾驶车辆控制领域，尤其涉及一种自动驾驶车辆分层式纵向规划控制系统及方法。

背景技术

自汽车从诞生以来其操纵者就是人，即由驾驶员观察道路并操纵车辆，形成“人-车-路”的闭环控制系统。该控制系统中，“人”这个子系统驾驶车辆的水平参差不齐，而且绝大多数交通事故都是由人为因素造成。为解决这一问题，自动驾驶车辆应运而生，自动驾驶车辆将“人”的作用淡化，甚至能完全脱离“人”的操纵实现自主驾驶，这要求汽车本身具有一定的自动驾驶功能。因此，为了使汽车更加安全地自动驾驶，需要开展自动驾驶车辆的技术研究。在自动驾驶车辆的关键技术中，车辆的纵向规划与控制是自动驾驶车辆的基础和重点，其在很大程度上决定了自动驾驶车辆的性能和体验。

自动驾驶车辆纵向规划是指在可行驶纵向路径上通过规划算法得到该路径上每一点的期望速度，而纵向控制是指在车辆行车速度方向上的控制，即控制车辆以给定的期望速度或期望车间距离进行行驶。纵向规划控制系统是一个典型的混合控制系统，它不仅包含连续的动态过程，也包含离散的切换过程。在不同场景下，自动驾驶车辆必须调整速度使其与前方目标车辆速度相同，同时与前方车辆保持一定的安全间距。

目前国内外使用的自动驾驶车辆纵向规划控制方法，包括自适应巡航控制(ACC)、STOP&GO(起停控制)、PID控制(比例、积分、微分控制)、模糊控制、MPC(模型预测控制)等，其具有以下的缺点：

1)非分层式控制方法，如ACC及STOP&GO，省略了规划层直接得到控制结果，控制过程不直观，不具有向后预测性，在一定程度上会影响自动驾驶车辆显示系统与乘客的交互，同时难以在速度层面上进行如最大速度、横向稳定性等的限制，不能与自动驾驶车辆横向规划协调；

2)参数调整困难，适用范围有限，如传统单一的PID控制与模糊控制需要反复调整控制参数，同时一套参数很难保证在较大的速度变化范围内适用；

3)动态环境适应性不强，难以在复杂的实际城市道路工况下取得较好效果，如MPC控制方法的计算时间较长，不便于保证实时性，同时其应用于分层式控制时，在期望速度曲线动态变化时效果不佳；

4)过分注重控制的精确性，忽略自动驾驶车辆乘客对舒适性的最高要求。

发明内容

鉴于上述的分析，本发明旨在提供一种自动驾驶车辆分层式纵向规划控制系统及方法，用以解决上述技术问题。

本发明的目的主要是通过以下技术方案实现的：

基于本发明实施例的一个方面，提供了一种自动驾驶车辆分层式纵向规划控制方法，包括步骤，

S1、根据感知设备获得的数据进行局部路径规划，生成纵向局部路径参数；

S2、根据纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果；

S3、根据自动驾驶车辆的纵向速度随时间的规划结果判断行驶模式；

S4、进行纵向速度控制。

基于本发明方法的另一个实施例中，步骤S1具体包括：

S11、通过感知设备获取环境信息；

S12、进行环境信息解析与处理；

S13、以步长为l离散路径，并向前探索一个步长；

S14、判断是否遇到障碍物，否，则以车辆位置、方向等信息更新路径长度并执行步骤S13，是，则纵向局部路径生成完毕。

基于本发明方法的另一个实施例中，步骤S2具体包括：

S21、首先将纵向局部路径参数作为初始规划条件进行相对速度曲线规划，得到基本相对速度规划曲线；

S22、对基本相对速度规划曲线进行约束，得到最终的相对速度规划曲线；

S23、将最终的相对速度规划曲线进行绝对速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果。

基于本发明方法的另一个实施例中，步骤S3具体包括：

S31、分析规划结果产生的平均加速度；

S32、基于平均加速度分析行驶模式。

基于本发明方法的另一个实施例中，步骤S32中行驶模式由平均加速度的范围得到，其中每个行驶模式为一个加速度阈值系数或者为两个加速度阈值系数。

基于本发明方法的另一个实施例中，行驶模式分别为：加速模式、匀速模式、制动模式和紧急制动模式。

基于本发明方法的另一个实施例中，步骤S4具体包括：

S41、判断在当前行使模式下自动驾驶车辆当前速度所在速度区间；

S42、根据当前行驶模式、当前速度所在速度区间及期望速度进行相应控制流程。

基于本发明方法的另一个实施例中，步骤S4还包括：在部分正常规划结果的直接处理方案中，不依赖速度规划结果直接进行车辆控制。

基于本发明方法的另一个实施例中，步骤S42进一步包括：控制流程中要对油门量或制动量进行饱和处理；各模式控制流程的时间累加量在模式切换时清零，开始新模式控制量的累加。

基于本发明实施例的另一个方面，提供了一种自动驾驶车辆分层式纵向规划控制系统，包括：局部路径规划器、纵向速度规划器、纵向速度控制器，并通过空置量反馈与车体动力学系统构成形成闭环，局部路径规划器将感知设备获得的数据进行局部路径规划，生成纵向局部路径规划参数；纵向速度规划器将纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度时间的规划结果并根据规划结果分析自动驾驶车辆的行驶模式；纵向速度控制器将行驶模式和期望速度作为输入，进行纵向速度控制。

本发明有益效果如下：能与自动驾驶车辆横向规划协调统一，有利于自动驾驶车辆规划控制方法的进一步发展在不同场景下，自动驾驶车辆可以调整速度使其与前方目标车辆速度相同，同时与前方车辆保持一定的安全间距，也能保证紧急情况的制动和停车。纵向规划对车辆的所有纵向行驶情况进行了分类处理，因此广泛适用于各种城市道路工况，同时能兼容自动驾驶车辆其他系统，如对感知系统的噪声有一定抗干扰性，能适应某些执行机构无法对连续量控制的特性。

本发明的其他特征和优点将在随后的说明书中阐述，并且，部分的从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在所写的说明书、权利要求书、以及附图中所特别指出的结构来实现和获得。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1为分层式纵向规划控制系统结构；

图2为纵向局部路径规划流程图；

图3为纵向局部路径规划示意图；

图4为纵向速度规划流程图；

图5为基本规划结果1曲线图；

图6为基本规划结果2形式1曲线图；

图7为基本规划结果2形式2曲线图；

图8为基本规划结果3曲线图；

图9为基本规划结果4曲线图；

图10为基本规划结果5曲线图；

图11为基本规划结果6形式1曲线图；

图12为基本规划结果6形式2曲线图；

图13为约束规划结果1曲线图；

图14为约束规划结果2曲线图；

图15为行驶模式判断示意图；

图16为纵向速度控制方法流程图。

具体实施方式

下面结合附图来具体描述本发明的优选实施例，其中，附图构成本申请一部分，并与本发明的实施例一起用于阐释本发明的原理。

根据本发明的一个具体实施例，公开了一种自动驾驶车辆分层式纵向规划控制系统，如图1所示，包括局部路径规划器、纵向速度规划器、纵向速度控制器，并通过车速反馈与车体动力学系统形成闭环。局部路径规划器将感知设备获得的数据进行局部路径规划，生成纵向局部路径规划参数；纵向速度规划器将纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度时间的规划结果并根据规划结果分析自动驾驶车辆的行驶模式；纵向速度控制器将行驶模式和期望速度作为输入，进行纵向速度控制，包括油门量和制动量控制，通过执行机构和车体动力学特性控制自动驾驶车辆的纵向速度。

其中，环境约束包括道路环境，周围车辆、行人情况等约束条件，车辆性能约束包括车辆本身能达到的最高速度、最大转向角等约束条件。二者共同作用影响纵向速度规划器的规划结果。

车速反馈包括反馈上层控制和反馈下层控制。

反馈上层控制作用于车辆纵向速度规划器，为保证规划结果的实时性，速度规划是以当前速度为基准进行车辆整体速度规划的，因此速度反馈对于速度规划非常重要。

反馈下层控制作用于车辆纵向速度控制器，纵向速度控制器是以上层规划及模式判断结果为基本参考量进行速度控制的，但在一些特殊情况中(如停车制动)纵向速度控制器可根据当前速度信息直接干预车辆控制，并且纵向速度控制器根据车速信息对控制量(如油门量、制动量)进行分级限制。

本发明公开了一种城市道路工况下适用广泛并充分保证自动驾驶车辆舒适性的分层式纵向规划控制系统，并能结合自动驾驶车辆平台达到实际应用效果。

一种应用于上述系统的方法，具体包括：

S1、根据感知设备获得的数据进行局部路径规划，生成纵向局部路径参数；

本步骤由局部路径规划器完成，如图2所示具体步骤包括：

S11、感知设备获取环境信息；

感知设备如毫米波雷达、激光测距仪及视频摄像头等获得的数据作为局部路径规划器的输入，示意图如图3所示。

S12、进行环境信息解析与处理；该路径规划器以车辆前方某一宽度范围内的第一个目标作为前车，以该目标与本车的距离作为路径长度L_init并同时得到前车的速度v_terminal以及本车速度v_init；

S13、以步长为l离散路径，并向前探索一个步长；在该路径上以l为步长离散出若干个路径点得到纵向局部路径，

S14、判断是否遇到障碍物，否，则以车辆位置、方向等信息更新路径长度并执行步骤S13，是，则纵向局部路径生成完毕；将上述参数作为局部路径规划器的输出，输入给纵向速度规划器。

S2、根据纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果；

纵向速度规划方法的设计原则如下：

a.为保证安全性，即保证足够的制动距离，应在安全距离之外的路径上进行规划，当自动驾驶车辆与前方障碍物的相对距离等于安全距离时，二者速度应相等，即相对速度为零；

b.为保证行驶效率，应在待规划路径的安全范围内尽可能地高速行驶，即以尽快的时间到达最终控制目标；

c.为保证舒适性以及最大速度等限制，应对加速度、速度进行相应约束。

基于上述原则，纵向速度规划可分为三个步骤，首先进行基本相对速度曲线规划，然后根据限制条件对该曲线进行约束，最后将之转化为绝对速度规划曲线。

S21、首先将纵向局部路径参数作为初始规划条件进行相对速度曲线规划，得到基本相对速度规划曲线；

根据初始相对速度Δv_init(Δv_init＝v_init-v_terminal)、初始距离L_init等初始边界条件的不同，可以衍生出若干种形式不同的基本规划结果，其规划流程图如图4所示。

其中，Δv_max＝v_max-v_terminal，v_max为该规划路径上的最大允许速度，由行驶环境决定；D_safe为安全距离，由安全距离模型得到,如式(1)所示：

D_safe＝τv_init+kv_terminal+d₀ (1)

其中τ为系统延时时间，k为安全距离系数，d₀为停车时的间距。

同时，图4中所指的加速指加速度为a_a的行驶过程；匀速为加速度为零的行驶过程；减速为减速度为a_{d_exp}的行驶过程，其满足a_{d_min}≤a_{d_exp}≤a_{d_max}<0。此外，由于该纵向速度规划控制系统每过一个周期都会根据最新信息重新规划，这段时间内可视前方障碍物为匀速运动，因此，本发明的方法皆以前方障碍物匀速运动计算。

根据城市道路工况下不同的车距、车速情况，纵向速度规划可分为如下若干种情况：

规划结果1：当相对距离L_init、安全距离D_safe、初始相对速度Δv_init和最大相对速度Δv_max分别满足式(2)、式(3)、式(4)时,以直接减速为规划结果，减速度即为期望减速度a_{d_exp}，如图5所示。

L_init>D_safe (2)

Δv_init≤Δv_max (3)

式(4)中，当Δv_init>＝0，不等式左边取符号；反之取负号。

图5中，l为离散路径点的步长(下同)；点O表示规划起始状态，点A表示规划目的状态，自动驾驶车辆的纵向状态从点O开始沿图中曲线的方式变化，最终达到点A的目的状态，即与前方障碍物的相对速度为零，相对距离为安全距离D_safe。

以Δv_i表示第i个离散路径点上相对速度的规划结果(下同)，则可得到如式(5)所示的规划方程:

其中L_OA表示从初始状态到达目的状态时自动驾驶车辆与前方障碍物之间相对距离的变化。

规划结果2：若初始边界条件满足式(2)、式(3)，而不满足式(4)，则可以存在加速段，即以加速、减速两个阶段为规划结果，而这种情况下，规划结果又可以出现两种形式。

若Δv_init>＝0，则规划结果第一种形式如图6所示。

由此可得到该初始边界条件下的规划方程，如式(6)所示。

若初始条件Δv_init<0，则图6将变形为图7，得到该规划结果的第二种形式。

该初始条件下的规划方程如式(7)所示。

其中L_O→B＝L_OA+L_AB，L_O→C同理，下同。

规划结果3：当规划初始条件满足式(2)而不满足式(3)时，需要判断初始相对距离L_init是否足够大，能够使得规划结果存在匀速段，判断方法如式(8)所示。

若初始条件同时满足式(8)，则规划结果存在匀速段，即以减速、匀速、减速三个阶段为规划结果，如图8所示：

该初始条件下的规划方程如式(9)所示：

d.规划结果4

若规划初始条件满足式(2)，不满足式(3)和式(4)，则说明初始安全距离L_init不够长，不能存在匀速段，因此需要直接减速，减速度为计算所得的期望减速度a_{d_exp}，计算方法同式(4)。此时规划结果如图9所示。

该初始条件下的规划方程如式(10)所示：

规划结果5：由于本发明针对于真实的城市交通环境，而这种工况中经常会出现插车的现象，即等同于本车前方近距离处突然出现障碍物，并且该距离经常会小于二者应有的安全距离，因此同样有必要对这种状况进行适当的处理。

当初始条件不满足式(2)时，说明当前时刻自动驾驶车辆和前方障碍物的相对距离L_init已经小于或等于安全距离D_safe，此时若还有Δv_init>0，即自动驾驶车辆速度高于前方障碍物速度，则必须要减速行驶。

有必要指出的是，在减速行驶的过程中，二者的相对距离还会有一段减小的过程，直到二者速度相等，达到最小相对距离L_min。此时有必要引入一个允许最小相对距离D_min，若D_min<L_init≤D_safe，则可以根据D_min≤L_min的要求设计期望减速度a_{d_exp}；若L_init≤D_min，则认为已经处于危险情况，此时应直接采用最大减速度a_{d_min}。

允许最小相对距离D_min应是一个与安全距离D_safe有关的量，可以以式(11)描述：

D_min＝βD_safe (11)

规划结果如图10所示。在该图中，初始条件满足D_min<L_init≤D_safe，则以D_min＝L_min的条件设计期望减速度，如式(12)所示：

自动驾驶车辆的纵向状态变化过程为S_O→S_A→S_B→S_C，其中S_O→S_B为以a_{d_exp}为减速度的减速过程，S_B→S_C为以a_a为加速度的加速过程，最终达到纵向目的状态S_C。

该初始条件下的规划方程如式(13)所示：

其中L_O→B、L_O→C意义同规划结果2。

若初始条件有L_init≤D_min，则此时处于危险情况，直接采用最大减速度a_{d_min}，即a_{d_exp}＝a_{d_min}。这种情况的规划结果示意图和规划方程与图10和式(13)一致，即二者只是计算期望减速度的方法不同而已。

规划结果6：当初始条件不满足式(2)且有Δv_init≤0时，虽然自动驾驶车辆与前方障碍物当前的相对距离小于安全距离，但本车速度并不高于前方障碍物速度，故此时并不一定需要减速，这由L_init与D_safe的相对大小决定，如式(14)所示：

当初始条件满足式(14)时，说明自动驾驶车辆与前方障碍物的相对距离已经接近安全距离，此时的规划结果如图11所示。

该初始条件下的规划方程如式(15)所示：

而当初始条件不满足式(14)时，则得到该规划结果的第二种形式，如图12所示：

这种情况下，虽然本车速度小于前方障碍物的速度，但二者相对距离已经较小，为达到纵向目的状态，需要先进行减速后加速。

该初始条件下的规划方程如式(16)所示：

以上即为几种基本相对速度曲线的规划结果，同时这几种规划结果已经涵盖了所有初始边界条件可能出现的情况。

S22、根据限制条件对步骤S21的结果进行约束，得到最终的相对速度规划曲线；

限制条件包括纵向约束特性及横向约束特性。纵向约束特性包括：最大速度约束、最大加速度约束、最大减速度约束；横向约束特性包括：横向稳定性约束(即侧向加速度约束)。

在步骤S21得到基本相对速度规划曲线之后，还应该对该曲线上的相对速度大小进行限制，该限制主要包括最大允许速度Δv_max(对于基本规划中已考虑该限制的不用再进行相应约束)、最小允许速度Δv_min以及横向动力学限制。

最大允许速度Δv_max主要由行驶环境决定，最小允许速度Δv_min除了决定于行驶环境外，还由绝对零速约束，如式(17)所示：

Δv_min＝-v_terminal (17)

最小允许速度指相对速度规划中最小可以达到的速度，由公式Δv_init＝v_init-v_terminal可知，因速度规划时车辆绝对速度不小于零，代入公式可计算Δv_min最小值。

与此同时，对于自动驾驶车辆而言，其纵、横向规划系统应当协调控制才能实现安全行驶。横向规划以车辆避障为主，规划结果为一条可行驶路径，该路径上的各路径点都包含有转向曲率的信息，因此为提高路径跟踪精度，同时满足横向安全性和舒适性要求，纵向速度规划应当受横向规划的约束，该约束主要是由转向曲率决定的车辆横向动力学约束。

车辆的横向动力学约束主要体现在横向加速度上，只有对横向加速度进行限定才能保证不发生侧滑、侧翻等事故。根据车辆纵向速度与路径曲率、横向加速度之间的动力学关系，可以对纵向速度进行进一步约束。

设期望路径上车辆的最大横向加速度为a_L，路径上第i个路径点的曲率为k_i，则该路径点上的纵向速度约束如式(18)所示：

上述限制条件的约束可由式(19)和式(20)描述：

经过约束，图6可能变化为如图13所示的形式，图10可能变化为如图14所示的形式。

S23、将最终的相对速度规划曲线进行绝对速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果；

经过上述两步，已经得到了最终的相对速度规划曲线，即Δv＝Δv(L)，由于上述规划任意两个相邻点之间都是匀变速运动，故可以计算出相对速度规划与时间t的关系，如式(21)所示：

其中t_i表示到达第i个路径点的时间。

同时根据式(22)，可以得到自动驾驶车辆的绝对速度v_init与时间t的关系v_init(t)：

v_init(t)＝Δv(t)+v_terminal(t)＝Δv(t)+v_terminal (22)

其中v_terminal意义同上，为规划初始时刻前方障碍物的速度。

相对速度规划不作为车辆真实期望速度进行其他模块速度判断及使用，只有将相对速度转换为大地坐标系中车辆真实期望速度才可进行车辆期望速度下发及使用。

至此，已经得到了自动驾驶车辆的纵向速度随时间的规划。规划曲线总是指向目的状态，能有效达到最终控制目的，即保持安全距离的同时保证车速相等，也能保证紧急情况的制动和停车；由于在可规划路径上采用了尽可能大的速度，能尽快达到目的状态。

S3、根据自动驾驶车辆的纵向速度随时间的规划结果判断行驶模式；

针对自动驾驶车辆的行驶状态，模拟驾驶员行驶意图，本发明实施例中引入四种模式，分别为：加速模式、匀速模式、制动模式、紧急制动模式，各模式的判断依据自动驾驶车辆的纵向速度随时间的规划结果。

以某一规划周期内的规划结果为例，如图15所示：

S31、分析规划结果产生的平均加速度

图中纵坐标为车辆绝对速度，横坐标为时间，规划结果由加速、匀速、减速三段组成，t_terminal为达到纵向目的状态的时间，ΔT为待分析时间段。

基于速度规划结果，向前预测ΔT时刻，对当前时刻起ΔT时间段内的规划情况进行分析。为全面考虑ΔT时刻内的整体情况，计算这段时间内的平均速度作为该规划周期的期望速度v_d，如式(23)所示：

与此同时，为分析该时间段内的曲线形状，考虑其平均加速度如式(24)所示：

S32、基于平均加速度分析行驶模式；

行驶模式由平均加速度的范围得到，如表1所示：

表1行驶模式判断表

上表中，a_a、a_{d_max}意义同上，即为标准加速和标准减速度；k_a、k_d、k_{d_h}分别为加速、制动、紧急制动模式的阈值系数，即当时，认为ΔT时间段内规划结果处于加速状态，故该时刻应该以加速为期望模式。

S321、改进的行驶模式判断分析

进一步分析，如果某一个模式只存在一个阈值系数，即其进出条件一样，则当在阈值左右波动时，容易造成模式的来回切换，因此对每个模式设置双阈值系数，并考虑上一周期行驶模式的历史信息，保留一个缓冲区域，对行驶模式进行滤波处理，避免其来回跳变。改进后的行驶模式判断表如表2所示：

表2改进行驶模式判断表

表格第一行为上一周期规划模式，即车辆是基于之前规划结果的再规划，而非纯加速度阈值判断；第一列为加速度的范围判断，即根据规划加速度判断车辆处于的加速度区间段；其余为车辆根据模式判断产生的当前规划模式结果，以此为基准进行模式控制。

上表中，k_a1、k_a2、k_d1、k_d2、k_{d_h1}、k_{d_h2}分别为相应的阈值系数，都为正数。k_a1反映了规划出加速模式的频繁程度，此值越小，进入加速模式的次数越频繁，即车辆行驶越激进，为了使控制效果尽量与规划曲线一致，该值一般取0.7～1；k_a2为k_a1的对应阈值系数，为保证足够的缓冲区域，该值应与k_a1有一定的偏差，同时为了充分利用匀速模式，该值不能太小；k_d2反映了规划出制动模式的频繁程度，此值越小，进入制动模式的次数越频繁，即车辆行驶越保守，为了使控制效果尽量与规划曲线一致，该值一般取0.7～1；k_d1为k_d2的对应阈值系数，该值同样应与k_d2有一定的偏差，且为了充分利用匀速模式该值不能太小；k_{d_h2}反映了规划出紧急制动模式的频繁程度，同时也定义了紧急制动时减速度的大小，该值的选取应与驾驶习惯有关，如一般情况下，认为紧急制动时减速度超过-2m/s²，即当期望减速度超过-2m/s²时应该进入紧急制动模式，由此可推算出k_{d_h2}的大小；k_{d_h1}为k_{d_h2}的对应阈值系数，同样应与k_{d_h2}存在偏差，同时为使控制效果尽量与规划曲线一致，该值一般取1左右。

此外，该表还能避免模式的跃变，即某一模式只能向其相邻的模式变化，如紧急制动模式不会直接变化为加速模式。至此，基于行驶模式分析的纵向速度规划完成，得到最终的输出量，即当前时刻应该进行的行驶模式以及作为辅助控制的期望速度v_d。提出行驶模式的概念，具有向前预测性，能综合考虑预测时间内的整个规划内容，将加速度的概念模糊化，有效避免规划方法中加速度突变的问题；在控制上以行驶模式为控制对象，可独立设计各模式的控制方法，并能充分利用匀速模式小幅度精确控制的特性，实现加速、匀速、减速之间的平滑过渡和合理的油门、制动切换，同时使用双阈值滤波避免了加速度在切换边界波动时模式的突变和跃变，在达到规划预期效果的同时有效提升乘坐舒适性，具有很强的实际应用性；采用实验建表逆查询的方法，回避了复杂的模型建立问题，减小计算量，同时建表的过程中就充分考虑了主观舒适性，实际应用时能得到良好的体现；

S4、进行纵向速度控制；

S41、判断在当前行使模式下自动驾驶车辆当前速度所在速度区间；

速度区间判断依据为事先定义完成的速度区间表，例如，以车辆最大速度限制为26.4m/s为例，可将速度区间划分为0、1.5、3.5、7.4、11.3、14.9、18.8、26.4七个速度区间段，完成速度区间表。

该表由现实自动驾驶车辆平台实验所得，记录了该平台各换挡点的速度，以该速度点将整个速度范围划分为若干区间(以换挡点划分速度段是为避免相同油门下换挡造成加速度突变)。

S42、根据当前行驶模式、当前速度所在速度区间及期望速度进行相应控制流程；

当处于加速模式时候，首先根据当前速度所在区间逆查询加速油门初值表，得到该区间对应的加速油门初值C_gas1。

在速度跟随控制中，一般需要建立精确的强非线性驱动、制动系统动力学逆模型作为车速与节气门开度、制动控制量之间的桥梁，同时测量包括发动机MAP图在内的诸多车辆动力学参数，而这些参数往往难以准确测量，而自动驾驶车辆平台本身的改装性又进一步增加了建立动力学模型和测量参数的难度。基于此，并借鉴模糊控制理论，本发明建立了若干个基于道路试验的油门量、制动量查询表，以此来回避上述必要的参数辨识问题，模拟人类驾驶员的经验学习，降低了设计复杂性，同时提高了控制系统的鲁棒性，具有很强的实用价值。

基于上述原因，加速模式中查询的加速油门初值表同样由现实自动驾驶车辆平台实验所得，该表记录了各个速度区间达到a_a加速度的油门值。由于a_a的选取已经考虑了自动驾驶车辆具体的动力学性能以及乘客的舒适度感受，因此该加速油门值能充分保证各个速度区间的舒适性要求。

除了油门初值外，最终的加速油门值还包括一个加速模式下的时间累加量C_gas2，即：

C_gas＝C_gas1+C_gas2 (25)

C_gas2为时间累加量，定义为油门增益，即认为当自动驾驶车辆一直处于加速状态时，车辆一直没有达到目的状态，即说明当前油门不足以使之达到目的状态，此时应当增大油门控制量，尽快结束加速模式。其算计方法如式(26)所示：

C_gas2＝k_gas2Δt (26)

其中，k_gas2为增益系数，Δt为加速模式下的连续累加时间，等同一个P调节。

与此同时，为防止油门量的无限增长，需要对之进行饱和处理，即保证当前速度区间的最终油门量不大于下一速度区间的油门初值，这样同时也保证了车辆在不同速度区间变化时油门量的连续。

当处于匀速模式时，匀速模式的控制方法与加速模式类似，首先根据当前速度所在区间逆查询匀速油门初值表得到匀速油门初值C_{gas_e1}。该表也由实车道路实验所得，记录了各个速度区间保持匀速运动的油门值。

同样地，最终的匀速油门量也由两部分构成，即：

C_{gas_e}＝C_{gas_e1}+C_{gas_e2} (27)

其中C_{gas_e2}为经过PID调节的时间累加量。

由于匀速模式的定义不仅包括匀速行驶，还包括一定程度上的加速和减速，同时，匀速油门表是在特定试验工况下建立的，而实际行驶时，车辆自身状态与外部环境均有可能发生较大的变化，因此，为了准确体现匀速模式的功能，以期望速度为辅助控制参数通过离散PID控制方法对最终油门进行微调，补偿由于实际汽车总质量、坡阻、风阻等与实验条件不同带来的误差，实现期望速度的准确跟踪。

PID控制律如式(28)和式(29)所示：

ΔC_{gas_e2}(k)＝k_p[e(k)-e(k-1)]+k_ie(k)

+k_d[e(k)-2e(k-1)+e(k-2)] (28)

C_{gas_e2}(k)＝C_{gas_e2}(k-1)+ΔC_{gas_e2}(k) (29)

其中，k_p、k_i、k_d分别为比例、积分、微分系数；e(k)为第k次PID控制时期望速度v_d与当前速度v_init的差值，即e(k)＝v_d-v_init。

与加速模式一样，需要对匀速油门进行饱和处理，或积分环节的反终结处理，即保证当前速度区间的最终匀速油门量不大于该速度区间的加速油门初值，这样同时也保证了车辆在匀速、加速模式变化时油门量的连续。

当处于制动、紧急制动模式时，制动、紧急制动模式的最终制动量同样由两部分组成，其计算方法与加速模式一致：首先查询制动或紧急制动初值，再计算相应模式下的时间累加量，最后对制动或紧急制动量进行饱和处理，得到最终控制量。

对油门量、制动量的饱和处理一方面保证了舒适性，一方面也避免了执行机构的大幅度调节；

进一步地，各模式控制流程的时间累加量在模式切换时清零，开始新模式控制量的累加。

S43、本发明还设计了一个特殊处理流程，对一些特殊行驶情况或者自动驾驶车辆特殊性能的处理，之后再将所得控制量发送到车体执行机构，体现到车辆动力学特性上，形成控制闭环。

特殊处理流程包括部分正常规划结果的直接处理方案，主要包括：非行进挡无油门处理、油门制动互锁处理、依速度分级油门上限处理、紧急情况制动处理、停车制动处理、速度反馈直接控制处理等。其可以不依赖速度规划器规划结果直接进行车辆控制，保证车辆行驶安全性与合理性。

本发明有益效果包括：

能与自动驾驶车辆横向规划协调统一，有利于自动驾驶车辆规划控制方法的进一步发展在不同场景下，自动驾驶车辆可以调整速度使其与前方目标车辆速度相同，同时与前方车辆保持一定的安全间距，也能保证紧急情况的制动和停车。纵向规划对车辆的所有纵向行驶情况进行了分类处理，因此广泛适用于各种城市道路工况，同时能兼容自动驾驶车辆其他系统，如对感知系统的噪声有一定抗干扰性，能适应某些执行机构无法对连续量控制的特性。

本领域技术人员可以理解，实现上述实施例方法的全部或部分流程，可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于计算机可读存储介质中。其中，所述计算机可读存储介质为磁盘、光盘、只读存储记忆体或随机存储记忆体等。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。

Claims (10)

1.一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，包括步骤：

S1、根据感知设备获得的数据进行局部路径规划，生成纵向局部路径参数；

S2、根据纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果；

S3、根据自动驾驶车辆的纵向速度随时间的规划结果判断行驶模式；

S4、进行纵向速度控制。

2.如权利要求1所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S1具体包括：

S11、通过感知设备获取环境信息；

S12、进行环境信息解析与处理；

S13、以步长为l离散路径，并向前探索一个步长；

S14、判断是否遇到障碍物；否，则以车辆位置、方向等信息更新路径长度并执行步骤S13；是，则纵向局部路径生成完毕。

3.如权利要求1所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S2具体包括：

S21、首先将纵向局部路径参数作为初始规划条件进行相对速度曲线规划，得到基本相对速度规划曲线；

S22、对所述基本相对速度规划曲线进行约束，得到最终的相对速度规划曲线；

S23、将最终的相对速度规划曲线进行绝对速度规划，得到自动驾驶车辆的纵向速度随时间的规划结果。

4.如权利要求1所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S3具体包括：

S31、分析规划结果产生的平均加速度；

S32、基于平均加速度分析行驶模式。

5.如权利要求4所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S32中行驶模式由平均加速度的范围得到，其中每个行驶模式为一个加速度阈值系数或者为两个加速度阈值系数。

6.如权利要求4所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，所述行驶模式分别为：加速模式、匀速模式、制动模式和紧急制动模式。

7.如权利要求1所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S4具体包括：

S41、判断在当前行使模式下自动驾驶车辆当前速度所在速度区间；

S42、根据当前行驶模式、当前速度所在速度区间及期望速度进行相应控制流程。

8.如权利要求7所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S4还包括：在部分正常规划结果的直接处理方案中，不依赖速度规划结果直接进行车辆控制。

9.如权利要求7所述的一种自动驾驶车辆分层式纵向规划控制方法，其特征在于，步骤S42进一步包括：控制流程中要对油门量或制动量进行饱和处理；各模式控制流程的时间累加量在模式切换时清零，开始新模式控制量的累加。

10.应用权利要求1所述方法的系统，其特征在于，包括：局部路径规划器、纵向速度规划器、纵向速度控制器；

所述局部路径规划器将感知设备获得的数据进行局部路径规划，生成纵向局部路径规划参数；

所述纵向速度规划器将纵向局部路径参数进行纵向速度规划，得到自动驾驶车辆的纵向速度时间的规划结果并根据规划结果分析自动驾驶车辆的行驶模式；

所述纵向速度控制器将行驶模式和期望速度作为输入，进行纵向速度控制。