融合改进势场与动态窗口的汽车路径规划方法

本发明涉及智能汽车，特别是涉及一种融合改进势场与动态窗口的汽车路径规划方法。

背景技术：

1、智能汽车路径规划，即为智能汽车规划一条行驶路径，所规划出的路径必须要满足汽车从开始到结束所使用的行驶时间、避撞效果、路线平滑稳定等基本条件，从而获得最佳的规划路径，可以很好的帮助智能汽车完成行驶任务。由于智能汽车所应用的环境复杂多样，其中不但有结构化的城市道路还有非结构化的乡村道路，所以规划的路径要能很好的适应复杂的现实环境、有良好避撞能力，尽可能的提高效率及安全可靠性。

2、通过规划方法使用的情况不同，可以将路径规划方法分为全局规划和局部规划。用途比较广泛的全局路径规划方法有dijkstra算法、a*算法、蚁群算法及rrt算法等，用途比较广泛的局部路径规划方法有人工势场法、遗传算法、动态窗口法等。其中人工势场法方便高效，计算速度快，易于理解，得到快速发展。

3、但是局部最优及目标不可达是目前人工势场法的最大问题。此外在实际的路径规划中还存在一些不可预测的复杂变化情况，而现有的人工势场法无法实时的动态更新规划路径，难以满足实际驾驶环境中的要求。

技术实现思路

1、本发明的目的在于提供一种融合改进势场与动态窗口的汽车路径规划方法，以解决现有技术存在的局部最优及目标不可达的问题，且实现实时的动态更新规划路径，以更好的满足实际驾驶环境中的要求。

2、一种融合改进势场与动态窗口的汽车路径规划方法，包括以下步骤：

3、步骤1，采用势场法对行车环境进行建模，得到行车环境模型；

4、步骤2，在行车环境模型中，获得汽车、目标点、障碍物、道路边界的坐标位置，再根据汽车与目标点、汽车与障碍物、汽车与道路边界之间的位置关系，建立多个势场函数，多个势场函数具体包括目标引力函数、障碍物斥力函数、相对速度斥力函数、道路纵向力函数；

5、步骤3，基于行车环境模型和多个势场函数，得到初步的规划路径；

6、步骤4，基于初步的规划路径，获取汽车到每一个障碍物的关键路径节点；

7、步骤5，对动态窗口的评价函数及加权系数进行改进，得到改进的动态窗口；

8、步骤6，将汽车到每一个障碍物的关键路径节点输入到改进的动态窗口中，通过改进的动态窗口的评价函数对关键路径节点上汽车与障碍物的相对速度、汽车的位姿、汽车轨迹与障碍物距离及汽车行驶最小安全距离进行采样，利用改进的动态窗口对初步的规划路径进行实时优化更新，得到优化后的规划路径。

9、根据本发明提供的融合改进势场与动态窗口的汽车路径规划方法，具有以下有益效果：

10、（1）在传统势场函数的基础上，增加了相对速度斥力函数、道路纵向力函数，能有效解决传统算法存在的局部最优及目标不可达问题；

11、（2）在传统势场函数中加入的道路纵向力可以保证汽车朝道路中心方向行驶，提高汽车行驶过程的安全稳定性；

12、（3）通过改进的动态窗口对各关键路径节点中汽车与障碍物的相对速度、位姿、轨迹与障碍物距离进行采样，能够对初步的规划路径进行实时动态更新，能够更好的满足实际驾驶环境中的要求。

13、此外，上述的融合改进势场与动态窗口的汽车路径规划方法，还具有以下技术特征：

14、进一步的，步骤2中，目标引力函数、障碍物斥力函数、相对速度斥力函数的表达式分别为：

15、

16、

17、

18、其中，为目标引力，为障碍物斥力，为相对速度斥力，d1为汽车与目标点的距离，d2为汽车与障碍物的距离，，，x和y是汽车自身位置坐标，xrep和yrep是障碍物位置坐标，、、分别为引力增益系数、斥力增益系数、相对速度斥力增益系数，vrel为汽车与障碍物的相对速度，为引力与斥力之间的角度。

19、进一步的，步骤2中，道路纵向力函数的表达式为：

20、

21、其中，表示道路纵向力，d为汽车与道路的纵向距离，d0为道路纵向力生效距离，为道路纵向力增益系数。

22、进一步的，步骤3具体包括：

23、步骤301，在行车环境模型中，以道路边界建立坐标系；

24、步骤302，基于建立的坐标系，选取汽车、目标点、障碍物的位置坐标，设定汽车参数；

25、步骤303，按照设定的汽车参数控制汽车前进，在汽车前进过程中实时检测是否达到道路纵向力生效距离，若达到道路纵向力生效距离，则进入步骤304，若未达到道路纵向力生效距离，则进入步骤305；

26、步骤304，道路纵向力生效，汽车在目标引力函数、障碍物斥力函数、相对速度斥力函数、道路纵向力函数的共同作用下前行，得到初步的规划路径；

27、步骤305，汽车在目标引力函数、障碍物斥力函数、相对速度斥力函数的共同作用下前行，得到初步的规划路径。

28、进一步的，步骤5中，对动态窗口的评价函数及加权系数进行如下改进：

29、

30、

31、其中，表示改进后的动态窗口的评价函数，表示改进后的加权系数，为位姿函数，为行驶轨迹与目标点距离函数，为行驶轨迹的线速度函数，为行驶轨迹与障碍物距离函数，为位姿函数的加权系数，为行驶轨迹与目标点距离函数的加权系数，为行驶轨迹的线速度函数的加权系数，为行驶轨迹与障碍物距离函数的加权系数，min表示取最小值，max表示取最大值。

技术特征：

1.一种融合改进势场与动态窗口的汽车路径规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的融合改进势场与动态窗口的汽车路径规划方法，其特征在于，步骤2中，目标引力函数、障碍物斥力函数、相对速度斥力函数的表达式分别为：

3.根据权利要求2所述的融合改进势场与动态窗口的汽车路径规划方法，其特征在于，步骤2中，道路纵向力函数的表达式为：

4.根据权利要求3所述的融合改进势场与动态窗口的汽车路径规划方法，其特征在于，步骤3具体包括：

5.根据权利要求4所述的融合改进势场与动态窗口的汽车路径规划方法，其特征在于，步骤5中，对动态窗口的评价函数及加权系数进行如下改进：

技术总结
本发明提供一种融合改进势场与动态窗口的汽车路径规划方法，本发明在传统势场函数的基础上，增加了相对速度斥力函数、道路纵向力函数，能有效解决传统算法存在的局部最优及目标不可达问题；本发明在传统势场函数中加入的道路纵向力可以保证汽车朝道路中心方向行驶，提高汽车行驶过程的安全稳定性；此外，本发明通过改进的动态窗口对各关键路径节点中汽车与障碍物的相对速度、位姿、轨迹与障碍物距离进行采样，能够对初步的规划路径进行实时动态更新，能够更好的满足实际驾驶环境中的要求。

技术研发人员：陈齐平,皮思源,曾德全,刘钦,钟秤平,曾小春,张小波,丁俊岭,胡一明,吴昊,熊祖琪
受保护的技术使用者：华东交通大学
技术研发日：
技术公布日：2024/1/13