CN113029151B - 一种智能车辆路径规划方法 - Google Patents

Abstract

本发明涉及智能车辆路径规划方法，该方法实时获取自动驾驶车辆GPS信息，结合车辆动力学模型和控制约束条件，生成规划范围的纵向和横向边界；在规划范围内进行目标状态位置采样；按水平视角和俯视视角混合标定的各目标状态的行车风险值并组成风险矩阵；最后对行车风险矩阵进行卷积并按结果选取部分目标状态进行路径规划，将最小化损失函数的路径作为最优路径。本方法从驾驶人的水平视角和传统场理论的俯视视角共同出发，根据道路采样点的实际可见性和场理论给出的风险值对其所在位置的行车风险进行重新标定，然后依照给出的行车风险选取部分由状态网格法的目标状态进行规划，最后根据损失函数求解出最优路径。

Description

一种智能车辆路径规划方法

技术领域

本发明涉及车辆路径规划领域，尤其是一种智能车辆路径规划方法，利用了场理论的混合视角实现了自动驾驶车辆的路径规划。

背景技术

目前，自动驾驶技术中感知、决策、执行环节的发展正推动自动驾驶技术走向成熟。其中决策环节根据感知信息对车辆进行行为预测及规划并交予执行环节对车辆的转向、制动进行控制。在道路行驶中，复杂多变的道路环境及驾驶人对车辆的舒适性、高效性需求，给自动驾驶决策环节带来了挑战。在决策环节中，基于车辆当前状态和短期目标的局部路径规划要求车辆在满足自身动力学模型的前提下，根据给定约束条件(道路限速、瞬时加速度变化率、路径的最大曲率等)和损失函数求出车辆状态-时间的最优规划方案。

国内外学者对自动驾驶局部路径规划已经开展了大量的研究，但是仍存在众多不足之处。通常，以人工势能场为代表的场类方法通过车辆状态信息和两车间相对运动关系信息对车辆进行行驶视角下的行车风险评估。不同的场类方法各具优势，但都存在一个共同的缺陷，即辨识风险的视角与驾驶人视角不统一，场类方法往往以俯视道路的视角对道路行车风险进行标定，并未考虑视线遮挡对驾驶人进行风险判断时产生的影响。并且场类方法容易出现局部最小点，车辆陷入其中时无法主动脱困。同时，现有的基于代价函数选择最优点行车路径的状态网格法运算量较大，路径的可行性检查仅要求路径不存在碰撞且满足车辆动力学模型，没有从驾驶人主观视角对道路环境进行判断，导致车辆道路行驶表现更趋近于避障而不是拟人化驾驶。

发明内容

针对上述问题，为了在保留状态网格法轨迹平滑、舒适等优点的同时使决策更加拟人化且运行更为高效，从驾驶人视角和对规划目标进行子集选择的角度出发，通过新的基于场理论的风险标定方法和对规划目标分批寻优的运算模式，提出一种混合视角下的启发式路径规划方法。该方法从驾驶人的水平视角和传统场理论的俯视视角共同出发，根据道路采样点的实际可见性和场理论给出的风险值对其所在位置的行车风险进行重新标定，然后依照给出的行车风险选取部分由状态网格法的目标状态进行规划，最后根据损失函数求解出最优路径。

本发明提供如下技术方案：一种智能车辆路径规划方法，包括如下步骤：

S1、实时获取自动驾驶车辆GPS信息，结合车辆动力学模型和控制约束条件，生成规划范围的纵向和横向边界；

S2、获得车辆的最大作用距离d_max，在规划范围内按照不大于2d_max的采样间隔进行目标状态位置采样；

S3、在道路横向纵向分别进行m和n次采样，定义行车风险矩阵为m×n的矩阵DF，其第i行第j列的元素DF_ij为自车以当前状态在对应采样点处按水平视角和俯视视角混合标定的行车风险值；

S4、给定p×q的卷积核KN对DF进行卷积运算，结果CR为(m-p+1)×(n-q+1)的矩阵；

S5、选择CR中最小值的对应区域进行路径规划，若该区域包含可行路径，则停止遍历，该区域内最小化损失函数的路径即为本次规划的最优路径；反之取次小值的对应区域进行路径规划，直至求解出本次规划的最优路径。

优选的，在步骤S1中，实时获取GPS信息、道路环境信息、环境感知信息和道路参与者的实时信息，包括道路边界及车道线坐标、道路限速、其他车辆的几何参数、速度及位置坐标。

优选的，步骤S3中：S31，令

OF＝f(s, obsc)，              (1)

其中s为自动驾驶车辆的当前状态，obsc为道路中其他车辆的状态，f为路径规划算法使用的场理论模型，OF为得出的自动驾驶车辆在s状态下的俯视行车风险。按照采样频率可得到m×n的俯视风险矩阵OM，其第i行第j列的元素OM_ij为通过式(1)计算得到的各目标状态的俯视行车风险；

S32、令f_max为所使用场理论f能产生的最大风险值，以水平视角从车辆当前所处位置对OM中各元素所对应的道路位置进行观察，若视线被障碍车辆遮挡，则该位置的风险值修改为f_max，反之不做改动，这样将OM进行水平视角的风险调整，获得的结果为行车风险矩阵DF。

优选的：在步骤S2中，给定状态网格的轨迹预测时间为t秒，车辆最大制动减速度为a_min，最大加速度为a_max，当前速度为v，则对目标状态进行采样时沿道路行驶方向的规划范围应在s_min和s_max范围内，其中

垂直于道路方向的目标状态采样直接以左侧和右侧车道中心为边界，将车辆垂直于道路方向的目标位置记为[d₁,d₂…d_m]，沿道路方向的目标位置记为[s₁,s₂…s_n]，目标位置点集TP_G＝{(d_i,s_j)|i＝1,2…m；j＝1,2…n}。

优选的，在S31中：

S311、将两车c₁、c₂之间的场力表示如下：

其中，

为c₁的自身属性，

和

为两车速度，

为两车距离，d₀为驾驶人的跟车距离，d_min和d_max为场理论的距离判定参数。式6为按照多普勒频移效应对两车距离做出的调整，

和

分别表示车辆c₁纵向和横向上与车辆c₂的距离，假设两车均平行于x轴方向行驶，则k_y＝1，k_x定义如下：

其中，

和

分别为两车在x轴上的坐标和速度，v_max为风险传播速度。

S312、令

为自动驾驶车辆的当前速度，

为障碍车速度，

为障碍车与TP_G中点(d_i,s_j)的距离，计算障碍车在该点产生的行车风险OF，将所有障碍车在该点的OF相加即为该点处的行车风险，令OM_ij为点(d_i,s_j)处的行车风险。

优选的，步骤S5包括：

S51、选定CR中最小元素CR_gh，该元素对应[d_g,…d_g+p-1]和[s_h,…s_h+q-1]组成的DF中的相应区域；

S52、对于车辆横向运动规划问题，给定初始时刻t₀＝0，初始状态

和目标状态

用于描述运动的五次多项式及其导数使用矩阵方程进行表示：

其中，令t＝t₀可解得a_d0＝d_t0，

令t＝t₁，

此时根据式(8)可以对t₀至t₁时刻的车辆运动状态进行求解，由于纵向的车辆位置无需加以约束，可使用四次多项式求解车辆纵向运动过程，给定车辆的初始状态

目标状态

当前时刻t₀＝0，矩阵方程表示如下：

与五次多项式同理，令t＝t₀可解得

令t＝t₁，

令车辆横向目标状态TS_D＝[d_t1,0,0]，因无需考虑纵向目标位置，纵向规划中车辆目标状态只由位置的一、二阶导数给定并使用四次多项式进行描述，即

将[d_g,…d_g+p-1]和[s_h,…s_h+q-1]逐个带入上述过程即得各个目标的状态方程；

S53、将横向纵向目标的状态方程两两组合成p×q条备选路径，检查备选路径是否超出最大曲率和最大加速度及是否存在碰撞，若无可行路径，选择CR中的次小值重新进行上述运算；

S54、使用损失函数式14对通过检查的备选路径进行寻优，选择最小化Cost的路径为本次路径规划的最优路径，式中C_lon和C_lat分别为横向和纵向损失函数，d_center为目标状态下车辆所在车道的中心坐标，k为比例系数；

通过上述描述可以看出，本方案的方法，1、从传统的场理论视角和驾驶人视角共同出发，使行车风险的标定过程更加拟人化。2、通过场理论对路径规划区域进行选定，基于评价函数对区域内的路径进行选择，有效地降低了车辆陷入局部最小的可能性。3、利用卷积选定目标区域，仅对该区域内的目标状态进行规划，大大降低了算法的运算复杂度。4、间接地将行车风险引入最优路径的评价过程，使得对路径的选定不再仅依赖车辆的运动状态，还能按驾驶人对行驶风险的主观感受进行判断和评估。

附图说明

图1是本发明具体实施方式中道路环境及目标位置分布图。

图2是使用场理论视角下的行车风险示意图。

图3是驾驶员的视线分布情况示意图。

图4是具体实施方式中混合视角下的行车风险示意图。

图5为本发明具体实施方式的流程图。

具体实施方式

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

通过附图可以看出，本发明给定宽度为15米的三车道公路，行驶方向为水平向右。车辆Ego为自动驾驶车辆，假设驾驶人的观察位置为车辆前边界中心偏左0.75米处。道路中所有车辆长宽分别为5米和3米。道路中障碍车辆1、2、3分别处于Ego前方10米、18米和40米处的不同车道上。

S1，实时获取GPS信息、道路环境信息、环境感知信息和道路参与者的实时信息，包括道路边界及车道线坐标、道路限速、其他车辆的几何参数、速度及位置坐标。

S2，给定状态网格的轨迹预测时间为t秒，车辆最大制动减速度为a_min，最大加速度为a_max，当前速度为v。则对目标状态进行采样时沿道路行驶方向的距离应在s_min和s_max范围内，其中，

垂直于道路方向的目标状态采样直接以左侧和右侧车道中心为边界，将车辆垂直于道路方向的目标位置记为[d₁,d₂…d_m]，沿道路方向的目标位置记为[s₁,s₂…s_n]，目标位置点集TP_G＝{(d_i,s_j)|i＝1,2…m；j＝1,2…n}。图1为道路车辆分布状态及选定的目标位置点集，即t秒后Ego可能到达的位置。

S3，以行车风险量化方式为场理论模型f，通过式(1)对各目标位置的行车风险进行量化形成风险矩阵OM，该场理论视角下道路行车风险如图2。

S4，从驾驶人的视角对各个选定的目标位置进行观察，若视线被障碍车辆遮挡，则将被观察的目标位置处的行车风险修改为f_max，修改后的风险矩阵记为DF。图3给出了驾驶人的视线分布情况，其中实线和坐标轴共同包围的部分即为驾驶人的视野盲区。图4为将水平视角下的风险判断加入后的道行车路风险分布示意图。

S5，给定p×q的卷积核KN对DF进行卷积运算，结果CR为(m-p+1)×(n-q+1)的矩阵。所采用的Valid卷积方式以及CR中元素与DF中区域的对应关系如图5所示。

S6，选择CR中最小值的对应区域进行路径规划。若该区域包含可行路径，则停止遍历，该区域内最小化损失函数的路径即为本次规划的最优路径；反之取次小值的对应区域进行路径规划，直至求解出本次规划的最优路径。

在本具体实施方式中，S3包括：

S31，采用的场理论将两车c₁、c₂之间的场力表示如下：

其中，

为c₁的自身属性，

和

为两车速度，

为两车距离，d₀为驾驶人的跟车距离，d_min和d_max为场理论的距离判定参数。式6为按照多普勒频移效应对两车距离做出的调整，

和

分别表示车辆c₁纵向和横向上与车辆c₂的距离。假设两车均平行于x轴方向行驶，则k_y＝1，k_x定义如下：

其中，

和

分别为两车在x轴上的坐标和速度，v_max为风险传播速度。

S32，令

为Ego的当前速度，

为障碍车速度，

为障碍车与TP_G中点(d_i,s_j)的距离，计算障碍车在该点产生的行车风险OF，将所有障碍车在该点的OF相加即为该点处的行车风险，令OM_ij为点(d_i,s_j)处的行车风险。

在本具体实施方式中，S6具体包括：

S61，选定CR中最小元素CR_gh，该元素对应[d_g,…d_g+p-1]和[s_h,…s_h+q-1]组成的DF中的相应区域。

S62，对于车辆横向运动规划问题，给定初始时刻t₀＝0，初始状态

和目标状态

用于描述运动的五次多项式及其导数使用矩阵方程进行表示：

其中，令t＝t₀可解得

令t＝t₁，

此时根据式(8)可以对t₀至t₁时刻的车辆运动状态进行求解。由于纵向的车辆位置无需加以约束，可使用四次多项式求解车辆纵向运动过程。给定车辆的初始状态

目标状态

当前时刻t₀＝0。矩阵方程表示如下：

与五次多项式同理，令t＝t₀可解得

令t＝t₁，

令车辆横向目标状态TS_D＝[d_t1,0,0]。因无需考虑纵向目标位置，纵向规划中车辆目标状态只由位置的一、二阶导数给定并使用四次多项式进行描述，即

将[d_g,…d_g+p-1]和[s_h,…s_h+q-1]逐个带入上述过程即得各个目标的状态方程。

S63，将横向纵向目标的状态方程两两组合成p×q条备选路径，检查备选路径是否超出最大曲率和最大加速度及是否存在碰撞，若无可行路径，选择CR中的次小值重新进行上述运算。

S64，使用损失函数式14对通过检查的备选路径进行寻优，选择最小化Cost的路径为本次路径规划的最优路径。式中C_lon和C_lat分别为横向和纵向损失函数，d_center为目标状态下车辆所在车道的中心坐标，k为比例系数。

尽管已经示出和描述了本发明的具体实施方式，对于本领域的普通技术人员而言，可以理解在不脱离发明的原理和精神的情况下可以对这些具体实施方式进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。

Claims (6)

1.一种智能车辆路径规划方法，其特征在于包括如下步骤：

S1、实时获取自动驾驶车辆GPS信息，结合车辆动力学模型和控制约束条件，生成规划范围的纵向和横向边界；

S2、获得车辆的最大作用距离d_max，在规划范围内按照不大于2d_max的采样间隔进行目标状态位置采样；

S3、在道路横向纵向分别进行m和n次采样，定义行车风险矩阵为m×n的矩阵DF，其第i行第j列的元素DF_ij为自车以当前状态在对应采样点处按水平视角和俯视视角混合标定的行车风险值；

S4、给定p×q的卷积核KN对DF进行卷积运算，结果CR为(m-p+1)×(n-q+1)的矩阵；

S5、选择CR中最小值的对应区域进行路径规划，若该区域包含可行路径，则停止遍历，该区域内最小化损失函数的路径即为本次规划的最优路径；反之取次小值的对应区域进行路径规划，直至求解出本次规划的最优路径。

2.根据权利要求1所述的智能车辆路径规划方法，其特征是：

在步骤S1中，实时获取GPS信息、道路环境信息、环境感知信息和道路参与者的实时信息，包括道路边界及车道线坐标、道路限速、其他车辆的几何参数、速度及位置坐标。

3.根据权利要求1所述的智能车辆路径规划方法，其特征是：

步骤S3中

S31，令

OF＝f(s, obsc)，              (1)

其中s为自动驾驶车辆的当前状态，obsc为道路中其他车辆的状态，f为路径规划算法使用的场理论模型，OF为得出的自动驾驶车辆在s状态下的俯视行车风险，按照采样频率可得到m×n的俯视风险矩阵OM，其第i行第j列的元素OM_ij为通过式(1)计算得到的各目标状态的俯视行车风险；

S32、令f_max为所使用场理论f能产生的最大风险值，以水平视角从车辆当前所处位置对OM中各元素所对应的道路位置进行观察，若视线被障碍车辆遮挡，则该位置的风险值修改为f_max，反之不做改动，这样将OM进行水平视角的风险调整，获得的结果为行车风险矩阵DF。

4.根据权利要求1所述的智能车辆路径规划方法，其特征是：

在步骤S2中，给定状态网格的轨迹预测时间为t秒，车辆最大制动减速度为a_min，最大加速度为a_max，当前速度为v，则对目标状态进行采样时沿道路行驶方向的规划范围应在s_min和s_max范围内，其中

垂直于道路方向的目标状态采样直接以左侧和右侧车道中心为边界，将车辆垂直于道路方向的目标位置记为[d₁,d₂…d_m]，沿道路方向的目标位置记为[s₁,s₂…s_n]，目标位置点集TP_G＝{(d_i,s_j)|i＝1,2…m；j＝1,2…n}。

5.根据权利要求4所述的智能车辆路径规划方法，其特征是：

S311、将两车c₁、c₂之间的场力表示如下：

其中，m_c1为c₁的自身属性，

和

为两车速度，

为两车距离，d₀为驾驶人的跟车距离，d_min和d_max为场理论的距离判定参数，式6为按照多普勒频移效应对两车距离做出的调整，

和

分别表示车辆c₁纵向和横向上与车辆c₂的距离，假设两车均平行于x轴方向行驶，则k_y＝1，k_x定义如下：

其中，

和

分别为两车在x轴上的坐标和速度，v_max为风险传播速度；

S312、令

为自动驾驶车辆的当前速度，

为障碍车速度，

为障碍车与TP_G中点(d_i,s_j)的距离，计算障碍车在该点产生的行车风险OF，将所有障碍车在该点的OF相加即为该点处的行车风险，令OM_ij为点(d_i,s_j)处的行车风险。

6.根据权利要求5所述的智能车辆路径规划方法，其特征是：

所述步骤S5具体包括：

S51、选定CR中最小元素CR_gh，该元素对应[d_g,…d_g+p-1]和[s_h,…s_h+q-1]组成的DF中的相应区域；

S52、对于车辆横向运动规划问题，给定初始时刻t₀＝0，初始状态

和目标状态

用于描述运动的五次多项式及其导数使用矩阵方程进行表示：

其中，令t＝t₀可解得

令t＝t₁，

此时根据式(10)可以对t₀至t₁时刻的车辆运动状态进行求解，由于纵向的车辆位置无需加以约束，可使用四次多项式求解车辆纵向运动过程，给定车辆的初始状态

目标状态

当前时刻t₀＝0，矩阵方程表示如下：

与五次多项式同理，令t＝t₀可解得

令t＝t₁，

令车辆横向目标状态TS_D＝[d_t1,0,0]，因无需考虑纵向目标位置，纵向规划中车辆目标状态只由位置的一、二阶导数给定并使用四次多项式进行描述，即

将[d_g,…d_g+p-1]和[s_h,…s_h+q-1]逐个带入上述过程即得各个目标的状态方程；

S53、将横向纵向目标的状态方程两两组合成p×q条备选路径，检查备选路径是否超出最大曲率和最大加速度及是否存在碰撞，若无可行路径，选择CR中的次小值重新进行上述运算；

S54、使用损失函数式14对通过检查的备选路径进行寻优，选择最小化Cost的路径为本次路径规划的最优路径，式中C_lon和C_lat分别为横向和纵向损失函数，d_center为目标状态下车辆所在车道的中心坐标，k为比例系数；

Images