一种基于车辆多尺度运动学模型的轨迹评价方法、系统及介质与流程

本发明涉及自动驾驶车辆，尤其是涉及一种基于车辆多尺度运动学模型的轨迹评价方法、系统及介质。

背景技术：

1、近年来，随着智能汽车的迅速发展，自动驾驶技术应运而生，目前高级别自动驾驶汽车已经逐渐应用在港口、机场、封闭园区等场景下。自动驾驶汽车需要规划系统来提供一条最优的无碰撞轨迹，但是由于感知/定位/跟踪控制误差的不确定性，导致该无碰撞轨迹无法被完美执行。通常，在选取最优的无碰撞轨迹时，会通过膨胀车身实际大小预留一定的缓冲区间来防止由上述感知/定位/跟踪控制误差导致的车辆碰撞或无法通过的问题。但是，在狭窄场景下，车辆膨胀的大小很难界定，过大的膨胀会导致车辆无法通过狭窄区域，膨胀过小又会存在安全风险。如何自适应的调整车辆膨胀大小，量化与障碍物之间的碰撞风险等级成为了目前自动驾驶领域的一个关键问题。

2、现有技术中，专利(申请号：202010236474.1)公开了一种基于学习采样式的汽车自动驾驶运动规划方法及系统，其包括：建立车辆运动学模型；初始化open表和closed表；计算每条前向仿真轨迹的评价值，选取评价值最高的轨迹作为规则最优轨迹；对前向仿真轨迹进行q值函数估计，选择q值最大的轨迹作为强化学习轨迹；从规则最优轨迹和强化学习轨迹中选取初段最优轨迹，并存入closed表中；利用碰撞检测方法筛选不碰撞前向仿真轨迹，将不碰撞的前向仿真轨迹存入open表中；计算每条前向仿真轨迹的评价值，选取评价值最高的前向仿真轨迹作为候选最优轨迹，并存入closed表中；候选最优轨迹终点在运动规划所需求的终点范围内时结束运动规划过程；将closed表格中的初段最优轨迹和候选最优轨迹连接，形成最终规划轨迹。但是该方案做的是基于学习型采用的自动驾驶运动规划算法，在选取最优轨迹的时候考虑的是q值函数，并没有对车辆运动学模型的膨胀范围做考虑，在狭窄空间下可能会产生通过性问题。

技术实现思路

1、鉴于以上现有技术的不足，本发明提供了一种基于车辆多尺度运动学模型的轨迹评价方法、系统及介质，不仅解决了狭窄场景下自动驾驶汽车的通过性问题，而且解决轨迹评价中车辆的安全缓冲范围难以量化评价的问题。

2、为了实现上述目的及其他相关目的，本发明提供的技术方案如下：

3、一种基于车辆多尺度运动学模型的轨迹评价方法，所述方法包括：

4、l1.车辆行驶在道路上，以车辆重心为中心的单车运动学模型作为车辆的控制输入，并采集车辆的运动学模型参数，输出车辆的运动学模型参数数据信息；

5、l2.基于所述车辆的运动学模型参数数据信息，采用高斯混合分布算法对车辆的运动状态进行采样，并构建不同尺度的车辆运动学模型，得到多尺度的车辆运动学模型数据信息；

6、l3.基于所述多尺度的车辆运动学模型数据信息，采用车辆轨迹的膨胀等级算法对不同尺度的车辆运动学模型的车辆运行轨迹进行评价，得到不同尺度的车辆运动学模型的车辆运动轨迹膨胀等级数据信息；

7、l4.基于所述不同尺度的车辆运动学模型的车辆运动轨迹膨胀等级数据信息，建立车辆最优轨迹的代价函数q，对车辆的运动轨迹进行规划，输出车辆的最优轨迹数据信息。

8、进一步的，所述车辆的运动学模型参数数据信息包括：车辆的中心点数据信息、车辆的前后轮转向角数据信息、车辆的重心数据信息、车辆的速度数据信息、车辆的位置数据信息、车辆的航向角数据信息、车辆的轨迹半径数据信息和车辆的航迹角数据信息。

9、进一步的，在步骤l2中，所述采用高斯混合分布算法对车辆的运动状态进行采样，并构建不同尺度的车辆运动学模型包括：

10、l21.基于所述车辆的运动学模型参数数据信息，建立车辆的高斯混合分布的概率密度函数p，

11、

12、其中，n为样本容量，αi为概率密度常量因子，xi为车辆的运动学模型参数数据信息，对车辆的运动状态进行采样，得到车辆运动状态的采样数据信息；

13、l22.基于所述车辆运动状态的采样数据信息，构建不同尺度的车辆运动学模型hj,

14、

15、

16、其中，(qj,yj)为车辆的位置数据信息，θj为车辆的航向角数据信息，j为车辆运动学模型的数量，λj为权重系数，s为车辆的速度，θ为车辆的均值航向角，φ为前轮后轮的转向角，δ为车身长度，得到多尺度的车辆运动学模型数据信息。

17、进一步的，所述概率密度常量因子αi的约束条件为，

18、

19、所述权重系数λj的约束条件为，

20、

21、进一步的，在步骤l3中，所述采用车辆轨迹的膨胀等级算法对不同尺度的车辆运动学模型的车辆运行轨迹进行评价包括：

22、l31.基于所述多尺度的车辆运动学模型数据信息，建立车辆运动轨迹的拟合函数g，

23、g＝∫χ(β3b3+β2b2+β1b+β0)db，

24、其中，χ为多尺度的车辆运动学模型数据信息，b为多尺度的车辆运动学模型数据信息下的车辆运动状态，β0，β1，β2和β3为车辆运动状态的拟合因子，对车辆的运动轨迹进行拟合，得到车辆的运动轨迹数据信息；

25、l32.基于所述车辆的运动轨迹数据信息，建立车辆的碰撞检测函数m，

26、

27、其中，σ为车辆的运动轨迹数据信息，对车辆运动轨迹进行碰撞检测，得到车辆运动轨迹的碰撞检测数据信息；

28、l33.基于所述车辆运动轨迹的碰撞检测数据信息，建立车辆的轨迹的膨胀尺寸评级函数u，

29、

30、其中，uk为车辆运动轨迹的碰撞检测数据信息，k为膨胀检测的样本容量，ρk为膨胀评级因子，对车辆的运动轨迹进行膨胀尺寸评级，输出不同尺度的车辆运动学模型的车辆运动轨迹膨胀等级数据信息。

31、进一步的，所述膨胀评级因子ρk的约束条件为，

32、

33、所述车辆运动状态的拟合因子β3的取值范围为(0,1)，所述车辆运动状态的拟合因子β2的取值范围为(0,1),所述车辆运动状态的拟合因子β1的取值范围为(0,1)，所述车辆运动状态的拟合因子β0的取值范围为(0,1)。

34、进一步的，在步骤l4中，所述车辆最优轨迹的代价函数q为，q＝φ1expansion(t1)+φ2length(t1)+φ3curvature(t1)+φ4fitti ng(t1)，

35、其中，t1为车辆的运动轨迹数据信息，expansion(t1)为车辆运动轨迹t1的不同尺度的车辆运动学模型的车辆运动轨迹膨胀等级数据信息，length(t1)为车辆运动轨迹t1的轨迹长度，curvature(t1)为车辆运动轨迹t1的轨迹曲率，fitting(t1)为车辆运动轨迹t1与已知参考轨迹的拟合程度，φ1，φ2，φ3和φ4为轨迹代价常量因子。

36、进一步的，所述车辆运动轨迹t1的轨迹曲率curvature(t1)为，

37、

38、其中，length(t1)为车辆运动轨迹t1的轨迹长度，

39、所述车辆运动轨迹t1与已知参考轨迹的拟合程度fitting(t1)为，

40、

41、其中，t1为车辆运动轨迹，t0为车辆的已知参考轨迹。

42、为了实现上述目的及其他相关目的，本发明还提供了一种基于车辆多尺度运动学模型的轨迹评价系统，包括计算机设备，该计算机设备被编程或配置以执行任意一项所述的基于车辆多尺度运动学模型的轨迹评价方法的步骤。

43、为了实现上述目的及其他相关目的，本发明还提供了一种计算机可读存储介质，该计算机可读存储介质上存储有被编程或配置以执行任意一项所述的基于车辆多尺度运动学模型的轨迹评价方法的计算机程序。

44、本发明具有以下积极效果：

45、1.本发明通过采用高斯混合分布算法对车辆的运动状态进行采样，并构建不同尺度的车辆运动学模型，得到多尺度的车辆运动学模型数据信息，不仅能够对车辆的运动轨迹进行精确的计算，而且多尺度运动学模型计算得到的最大膨胀等级，可以量化自动驾驶道路上的碰撞风险，可以解决狭窄场景下由于最优参考路径不方便考虑碰撞风险而导致的通过性问题。

46、2.本发明通过采用车辆轨迹的膨胀等级算法对不同尺度的车辆运动学模型的车辆运行轨迹进行评价，并结合车辆最优轨迹的代价函数q，对车辆的运动轨迹进行规划，不仅可以在保证轨迹质量的同时，最大程度上保障轨迹的安全性，而且降低车辆能耗，提高车辆的驾驶体验。