一种智能汽车拟人化速度与轨迹规划方法及装置

本发明涉及智能汽车运动规划领域，尤其是涉及一种山区高速公路的智能汽车拟人化速度与轨迹规划方法及装置。

背景技术：

1、高速公路的自由流交通环境，即通常没有其他车辆影响驾驶过程的环境，是智能汽车的重要应用场景。在这种场景下，道路线形与环境是影响驾驶员速度与轨迹行为的主要因素，尤其是在具有大量组合线形的山区高速公路上，需要一种兼顾舒适性和安全性的速度和轨迹规划方法，即运动规划(motion planning)方法，以供运动控制(motioncontrol)模块参考。

2、现有的智能汽车运动规划方法尚未完全解决山区高速公路上组合线形路段的挑战。现有运动规划方法通过为规划问题成本函数中设置舒适性约束，如横向加速度、纵向加速度、加加速度、方向盘转角等，以此考虑驾驶人对舒适性的要求。这些方法缺乏从交通工程的角度识别人类驾驶员的速度和轨迹行为习惯。

技术实现思路

1、本发明的目的就是为了提供一种山区高速公路的智能汽车拟人化速度与轨迹规划方法及装置，通过采集驾驶人在自由流交通环境下手动驾驶车辆的运行数据、道路几何数据与环境数据，建立以道路几何变量与环境变量为外生自变量，速度与轨迹变量为内生自变量的联立方程模型，并将车辆运行速度与轨迹模型结果作为拟人化运动规划方法的基础，将道路几何变量与环境变量作为输入，将联立方程模型预测的速度与轨迹结果进行安全性校正，最终将速度与轨迹结果作为拟人化规划方法的输出，得到符合驾驶人行为习惯的智能汽车速度与轨迹规划结果。

2、本发明的目的可以通过以下技术方案来实现：

3、一种山区高速公路的智能汽车拟人化速度与轨迹规划方法，包括以下步骤：

4、步骤1)获取山区高速公路的车辆运行数据、道路几何数据和环境数据；

5、步骤2)数据预处理；

6、步骤3)以道路几何数据与环境数据作为解释变量，基于联立方程模型建立理论性的车辆运行速度与轨迹模型；

7、步骤4)基于车辆运行速度与轨迹模型的结果，建立拟人化运动规划基本规则；

8、步骤5)基于车辆运行速度与轨迹模型的结果、拟人化运动规划基本规则构建拟人化运动规划模型；

9、步骤6)对拟人化运动规划模型进行校正，规避由于驾驶人手动驾驶车辆导致偏移出车道的行为；

10、步骤7)将校正后的拟人化运动规划模型输出作为一种软约束，供智能汽车运动控制模块参考，实现智能汽车拟人化速度与轨迹规划。

11、所述车辆运行数据包括速度和轨迹。

12、所述道路数据包括道路横断面、纵断面及平面几何数据。

13、所述环境数据包括天气、光照、路面状况等。

14、所述步骤2)包括以下步骤：

15、步骤2-1)剔除车辆运行数据中受道路几何与道路环境之外因素影响的数据；

16、步骤2-2)定义三种类型的车辆偏移行为：偏向弯道外侧、偏向弯道内侧、保持车道内行驶，根据车辆和道路特征确定轨迹偏移安全阈值td0，车辆轨迹偏移达到td0，则表明存在安全隐患；

17、步骤2-3)将车辆运行数据、道路几何数据和环境数据匹配到同一空间坐标中，基于道路中心线按照预配置距离的间隔集计原始数据，得到车辆在各个距离间隔内的速度、轨迹偏移，以及道路几何数据与环境数据的平均值。

18、所述车辆运行速度与轨迹模型依据不同组合线形类型、车辆偏移行为建立，以标定不同应用场景下的模型显著变量的系数，所述车辆运行速度与轨迹模型表示为：

19、

20、

21、其中，speedcrntj表示点j处的预测速度，tdj表示预测的横向轨迹偏移，avgspeedupj表示上游路段的平均速度，其中上游表示刚驶过的路段，下游表示即将驶入的路段，以上为内生变量；xi,j表示点j处的道路几何变量与环境变量，包括当前点及上下游路段的变量，为外生变量；m和n表示显著变量数量；α和β表示模型系数，μ1和μ2表示模型误差项。

22、所述拟人化运动规划基本规则包括：

23、智能汽车以车辆运行速度与轨迹模型输出结果作为参考，确定运动规划的基础速度与轨迹值；

24、根据车辆运行速度与轨迹模型输出结果调整轨迹，确保车辆轨迹不偏移出车道，即tdj＜td0，并根据模型结果所示的驾驶人行为习惯同时调整速度。

25、所述步骤5)包括以下步骤：

26、步骤5-1)确定智能汽车所处场景，所述场景包括平曲线上坡、平曲线下坡、平曲线凹曲线、平曲线凸曲线；

27、步骤5-2)确定所述场景中组合线形上的车辆预测偏移方向，基于步骤2)的数据分析，确定智能汽车在所处组合线形类型下最大概率的偏移方向；

28、步骤5-3)根据所处场景及确定的偏移方向，基于车辆运行速度与轨迹模型构建拟人化运动规划模型，其中，所述拟人化运动规划模型的输入为点j处的道路几何变量与环境变量数据，模型输出为点j处模型预测的速度与横向轨迹偏移结果。

29、所述步骤6)包括以下步骤：

30、步骤6-1)基于拟人化运动规划模型，确定速度显著变量及其系数，并定义2种类型，类型i：速度与轨迹呈正相关；类型ii：速度与轨迹不相关或呈负相关；

31、步骤6-2)若所处场景的拟人化运动规划模型为类型i，同时对tdav,j和vav,j进行校正，校正目标如下：

32、tdav,j＝tdcorrect,j＜tdhuman,j

33、tdcorrect≤td0

34、vav,j＝vcorrect,j＜vhuman,j

35、其中，tdav,j表示点j处的智能汽车的横向轨迹，tdhuman,j表示点j处手动驾驶的轨迹，tdcorrect,j表示未超过阈值的校正轨迹；vav,j表示智能汽车点j处的速度，vhuman,j表示点j处手动驾驶的速度，vcorrect,j表示校正速度参考值；校正后的tdav,j小于等于td0；vcorrect,j基于车辆运行速度与轨迹模型的β1或β2，以及tdcorrect,j与tdhuman,j的差值确定：

36、δtdj＝tdhuman,j-tdcorrect,j＝β·δvj

37、δvj＝vhuman,j-vcorrect,j

38、

39、β根据模型显著变量情况取β1或β2；

40、步骤6-3)若所处场景的拟人化运动规划模型为类型ii，仅对tdav,j进行校正，校正后的tdav,j小于等于td0，vav,j保持与vhuman,j相同，无需根据模型进行校正。

41、一种山区高速公路的智能汽车拟人化速度与轨迹规划装置，包括存储器、处理器，以及存储于所述存储器中的程序，所述处理器执行所述程序时实现如上述所述的方法。

42、与现有技术相比，本发明具有以下有益效果：

43、(1)本发明通过建立联立方程模型，将速度与轨迹作为内生变量，可以考虑两个车辆运行指标内部影响，提高驾驶人速度与轨迹行为模型的精度，相较于独立建模，结果更加可靠和实用。

44、(2)本发明在山区高速公路不同场景中建立车辆运行速度与轨迹模型作为拟人化运动规划方法的基础，较其他设置舒适性约束的运动规划方法，学习人类根据道路上下游线形与环境调整车辆运动的行为，可以改善智能汽车运动规划策略，使智能汽车顺利通过复杂的组合线形路段，更能符合驾驶人在智能汽车自动驾驶下乘坐的速度与轨迹期望，满足乘坐的安全性与舒适性预期。