多模态轨迹预测自动驾驶纵向决策规划前处理方法

本发明涉及于自动驾驶，具体地说，尤其涉及一种自动驾驶系统决策规划方法领域的多模态轨迹预测自动驾驶纵向决策规划前处理方法。

背景技术：

1、决策规划是基于知识自动驾驶系统的算法模块之一，依据数学建模过程中几何背景的形态，现有决策规划技术路线可划分为时空联合和时空分立两类。其中，时空分立类决策规划算法的横向(路径方向)决策规划与纵向(速度方向)决策规划在算法流程上分别实现。在自动驾驶系统中，决策规划模块处于交通参与者行为预测模块数据流下游，因而其解决方案受后者结果输出形式的影响。例如，行为预测模块以目标的确定轨迹作为输出时，决策规划模块基于确定轨迹构建确定避障约束；行为预测模块以目标位移概率场作为结果输出时，决策规划模块则需构建相应的场约束实现避障功能。

2、在面向多模态轨迹预测的自动驾驶纵向决策规划领域，现阶段主流解决方案存在如下问题：决策规划模块选择目标概率最高意图对应的轨迹，抛弃其余意图对应的轨迹，从而将决策规划任务转化为确定预测轨迹避障约束问题，忽略了多模态预测轨迹的统计信息，无法体现多模态预测方法的优势。决策规划模块选择所有预测意图中对规划主体当前运动状态影响最大的预测轨迹，同样将该任务转化为确定预测轨迹避障约束问题，在选定轨迹概率较小时易导致驾驶行为过于保守问题。决策规划模块考虑目标所有意图对应的预测轨迹，在构建避障约束时依据各预测轨迹对应的概率值相应放宽约束条件，在预测轨迹的概率分布未收敛时无法生成合理有效的避障约束。

技术实现思路

1、本发明的目的在于克服多模态轨迹预测存在的统计信息丢失、行为过分保守及行为响应不及时等问题，提供一种多模态轨迹预测自动驾驶纵向决策规划前处理方法，将多模态预测轨迹合理转化为单模态输出，从而在最大限度继承多模态轨迹统计信息的前提下，输出及时且合理的决策规划结果。

2、为了实现上述目的，本发明所采用的技术方案如下：

3、一种多模态轨迹预测自动驾驶纵向决策规划前处理方法，包括以下步骤：

4、基于不同意图的预测轨迹分类结果，构建多模态预测轨迹矩阵，得到预测目标的多模态预测轨迹簇；

5、对多模态预测轨迹簇的每条轨迹进行预决策，基于预测轨迹的决策结果，修正多模态预测轨迹矩阵；

6、基于轨迹的预决策结果提示的规划主体速度变化倾向，构建特征预测轨迹簇，修正多模态预测轨迹矩阵；

7、计算每条特征预测轨迹的预期加速度，表示执行预决策结果的需求状态，修正多模态预测轨迹矩阵；

8、计算每条特征预测轨迹的叠加权重，依据各特征预测轨迹的叠加权重，逐条叠加计算叠加预测轨迹。

9、进一步地，基于不同意图的预测轨迹分类结果，构建多模态预测轨迹矩阵，具体为：

10、构建多模态预测轨迹矩阵，表示不同预测意图与规划主体的冲突关系，矩阵每一行代表一条预测轨迹，矩阵包括两列，第一列为意图标识，第二列为预测轨迹与规划主体的潜在冲突情况；

11、基于行驶场景对预测意图进行分类，行驶场景分为多车道场景和路口场景，多车道场景包括车道保持、向左换道、向右换道和调头四类意图，路口场景包括直行、左转、右转和调头四类意图；

12、预测轨迹与规划主体的潜在冲突情况包括存在冲突和不存在冲突两类情况。

13、进一步地，基于预测轨迹的决策结果，修正多模态预测轨迹矩阵，具体为：

14、应用纵向决策方法，对多模态预测轨迹簇中的每条轨迹进行预决策，在原有多模态预测轨迹矩阵的基础上，对决策认为有冲突的轨迹结果增加补充信息，补充信息作为矩阵的第三列，补充信息包括让行决策结果、跟行决策结果、超越决策结果和停车决策结果。

15、进一步地，基于轨迹的预决策结果提示的规划主体速度变化倾向，构建特征预测轨迹簇，修正多模态预测轨迹矩阵，具体为：

16、基于轨迹的预决策结果提示的规划主体速度变化倾向，将决策结果分为减速倾向和加速倾向两个大类，减速倾向和加速倾向作为矩阵的第四列，跟行、让行和停车为减速倾向大类，在多模态预测轨迹矩阵中以“-1”表示；超车为加速倾向大类，在多模态预测轨迹矩阵中以“1”表示。

17、进一步地，计算每条特征预测轨迹的预期加速度，表示执行预决策结果的需求状态，修正多模态预测轨迹矩阵，具体为：

18、基于每条特征预测轨迹的预决策结果及目标侵入状态，计算规划主体在预期时间达到预期速度需要的加速度，定义为特征预测轨迹的预期加速度，作为多模态预测轨迹矩阵的第五列，表示执行该预决策结果的需求状态。

19、进一步地，对于预决策结果为让行的预测线，预期加速度为

20、

21、式中，v0为规划主体当前速度，tmin为目标预测线在规划主体路径st投影上的最小时间值，smin为目标预测线在规划主体路径st投影上的最小位移值；

22、对于预决策结果为跟行或停车的预测线，预期加速度为

23、

24、式中，v1为目标当前速度；a1为目标当前加速度；

25、对于预决策结果为超越的预测线，预期加速度为

26、

27、式中，smax为目标预测线在规划主体路径st投影上的最大位移值。

28、进一步地，计算每条特征预测轨迹的叠加权重，具体为：

29、特征轨迹的叠加权重受轨迹的概率和决策需求状态影响，轨迹概率直接从预测信息中读取，决策需求状态以预期加速度占加速度限值的比值表征：预期加速度为正时，加速度比值为预期加速度除以加速上限；预期加速度为负时，加速度比值为预期加速度除以加速度下限；

30、以加速度比值表示的决策需求状态乘以对应的轨迹概率，得到轨迹的叠加权重：

31、wi＝eipi

32、式中，pi为第i条预测轨迹的概率，ei为加速度比值；加速度比值的计算公式为：

33、

34、式中，ai为依据第i条预测轨迹计算出的规划主体预期加速度，amin为规划主体速度规划的最小加速度，amax为规划主体速度规划的最大加速度。

35、进一步地，依据各特征预测轨迹的叠加权重，逐条叠加计算叠加预测轨迹，具体为：

36、设待叠加轨迹i和轨迹j的时间点序列均为

37、t＝[t0，t1，…，tn]

38、则待叠加轨迹i和轨迹j对应的空间点序列分别为

39、ci＝[(xi0，yi0)，(xi1，yi1)，…，(xin，yin)]

40、cj＝[(xj0，yj0)，(xj1，yj1)，…，(xjn，yjn)]

41、叠加方式为两条轨迹同时刻点进行空间叠加；设待叠加的轨迹i和轨迹j在tk时刻的轨迹点空间坐标分别为(xik，yik)和(xjk，yjk)，则两轨迹点连线的参数方程为

42、

43、式中，(xijk，yijk)为叠加轨迹在tk时刻的空间坐标；θij为(xik，yik)和(xjk，yjk)两点连线的方向角，即两点连线从(xik，yik)指向(xjk，yjk)方向与x坐标轴正向的夹角；li为叠加轨迹点空间坐标沿叠加前两点方向到轨迹i的距离，通过两条轨迹的叠加权重求得：

44、

45、式中，wi为轨迹i的叠加权重；wj为轨迹j的叠加权重；

46、通过计算连线方向矢量和坐标轴正向单位矢量的夹角求得：

47、

48、对待叠加轨迹的每一个点进行上述操作，得到叠加后的轨迹；特征轨迹簇内存在多条轨迹时，以逐条叠加的方式计算叠加轨迹。

49、一种电子设备，包括存储器、处理器以及存储在存储器中并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述任一项所述的多模态轨迹预测自动驾驶纵向决策规划前处理方法。

50、一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现上述任一项所述的多模态轨迹预测自动驾驶纵向决策规划前处理方法。

51、与现有技术相比，本发明具有如下优点和有益效果：

52、本发明以轨迹叠加的方式处理多模态预测轨迹，依据预测轨迹的概率和决策需求状态计算轨迹的叠加权重，以叠加权重计算多模态预测轨迹的叠加态，叠加轨迹依权重向某一轨迹收敛，从而在继承全部预测信息的情况下，准确地执行决策规划任务。

53、本发明基于多模态预测轨迹预决策的决策需求状态，用于描述某一特定预测轨迹需要规划主体做出响应的紧急程度，综合考虑决策需求状态和轨迹概率执行决策规划任务，从而有效避免错误或过分保守的决策规划结果。