基于改进A*-TEB融合算法的无人车路径规划方法及系统

本发明属于无人车路径规划领域，尤其涉及基于改进a*-teb融合算法的无人车路径规划方法及系统。

背景技术：

1、本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。

2、近年来，自动驾驶技术的迅猛发展已经使无人车技术成为交通领域的焦点；无人车不仅被广泛用于城市交通，还应用于货运、物流、农业和紧急救援等领域；这项技术被视为提高交通安全、减少交通拥堵和改善出行体验的潜力解决方案。

3、在无人车领域，路径规划是一项至关重要的任务；无人车必须能够智能地规划路径，以避免障碍物、考虑车辆动力学，同时实时适应不断变化的环境条件；路径规划的质量直接影响无人车的安全性和效率。

4、根据周围环境的已知、部分已知或未知，分为全局与局部路径规划算法；全局路径规划算法负责规划从起始点到目标点的整个路径，通常在考虑整个地图或环境的情况下进行规划，常见算法有rrt、dijkstra、a*算法等，此类算法依然存在着路径规划时间长、路径长度并非最优、拐点多以及无法有效的避免动态障碍物等问题；局部路径规划算法负责在机器人或车辆运动中实时规划并调整路径，以应对动态环境和变化的情况，常见算法有dwa算法、teb算法以及mpc算法，此类算法能够有效的规划出局部路径，实现实时避障，但是存在路径不够平滑以及容易陷入局部最优等问题。

5、因此，现有的无人车路径规划方法依然存在全局路径规划时间长、全局路径非最优、路径拐点多和局部路径规划不平滑等问题，导致无人车路径规划效率不高。

技术实现思路

1、为克服上述现有技术的不足，本发明提供了基于改进a*-teb融合算法的无人车路径规划方法及系统，将改进a*算法与改进teb算法相互融合，有效解决全局路径单一、动态性能差以及局部路径容易陷入局部最优的问题，从而实现了优势互补，提升无人车路径规划效率、全局最优性以及实时避障性能。

2、为实现上述目的，本发明的一个或多个实施例提供了如下技术方案：

3、本发明第一方面提供了基于改进a*-teb融合算法的无人车路径规划方法。

4、基于改进a*-teb融合算法的无人车路径规划方法，包括：

5、构建静态栅格地图，并在静态栅格地图中指定无人车的全局起始点与全局目标点；

6、采用改进的a*全局路径规划方法，在静态栅格地图中构建出一条由全局起始点到全局目标点的全局最优路径；

7、无人车按照全局最优路径行进过程中，不断截取距离无人车预设范围的局部路径，对局部路径进行重新规划，使得无人车不断朝着局部目标点行进，直到达到最终的全局目标点；

8、其中，所述改进的a*全局路径规划方法，以全局起始点为初始当前节点，搜索当前节点的一组临近子节点，将父节点到目标点的成本估计值和动态指数权值引入到成本函数中，计算各个临近子节点的成本估计值，选择成本估计值最小的临近子节点作为当前节点，进行迭代的搜索和计算，直到当前节点为全局目标点。

9、进一步的，所述全局最优路径的具体构建过程为：

10、(1)设置开放列表存储静态栅格地图中节点的信息，将全局起始点封装为栅格节点信息放入开放列表中；

11、(2)从开放列表的队头取出当前节点，采用指向性5邻域搜索方法，搜索当前节点的一组临近子节点，并通过改进的成本函数计算各个临近子节点的成本估计值，选择成本估计值最小的临近子节点放入开放列表中，并更新临近节点的信息；

12、(3)重复执行步骤(2)，直到找到目标节点，并通过回溯算法，搜寻成本最小的路径，得到临时的全局路径；

13、(4)采用最短路径算法，去除临时的全局路径中的冗余拐点，获取最终的全局最优路径。

14、进一步的，所述指向性5邻域搜索方法，是根据当前节点与全局目标点的方向，从上邻域、下邻域、左邻域、右邻域中选择搜索邻域，在搜索邻域中，以当前节点为原点，选择邻近的一组子节点。

15、进一步的，所述改进的成本函数的数学表达式为：

16、f(n)＝g(n)+w(n)*(h(n)+h(n-1))

17、其中，g(n)为起始点到当前节点的实际成本，h(n)为当前节点到目标点的成本估计值，h(n-1)为父节点到目标节点的成本估计值，w(n)为动态指数权值；

18、成本估计值采用对角线距离，数学表达式为：

19、

20、其中，(xg，yg)和(xn，yn)分别为目标点坐标和当前点坐标；

21、动态指数权值的数学表达式为：

22、w(n)＝e-(h(n)+h(n-1))+β

23、其中，β为权值系数，为常数。

24、进一步的，所述最短路径算法，具体为：

25、(1)选出路径的关键点，包括起始点和拐点，放入关键点集合中；

26、(2)遍历关键点集合，采用布雷森汉姆直线算法，判断当前节点的前后节点是否可以连为直线并且不会经过障碍物，是则舍弃当前点，否则保留；

27、(3)重复执行(2)，直到遍历到目标点则结束，最终得到一个剔除冗余拐点的最优路径。

28、进一步的，所述截取距离无人车预设范围的局部路径，具体为：

29、基于全局最优路径，截取预设距离，并进行序列化，得到一组位姿点，组成位姿序列，采用b样条曲线平滑算法对位姿序列进行平滑处理，得到平滑的位姿序列，作为局部路径，以相邻的两个位姿点为局部路径的局部起始点和局部目标点。

30、进一步的，所述对局部路径进行重新规划，采用改进的teb局部路径规划方法，在原有teb的多目标约束中，增加加速度变化率约束，具体为：

31、

32、

33、

34、

35、其中，分别为线加速度变化率约束和角加速度变化率约束，分别为线加速度变化率和角加速度变化率，ai+1、ai分别为第i+1个节点和第i个节点的线加速度，分别为第i+1个节点和第i个节点的角加速度，δti为相邻位姿点的时间间隔，和为系数，是一个常量。

36、本发明第二方面提供了基于改进a*-teb融合算法的无人车路径规划系统。

37、基于改进a*-teb融合算法的无人车路径规划系统，包括初始化模块、全局规划模块和局部规划模块：

38、初始化模块，被配置为：构建静态栅格地图，并在静态栅格地图中指定无人车的全局起始点与全局目标点；

39、全局规划模块，被配置为：采用改进的a*全局路径规划方法，在静态栅格地图中构建出一条由全局起始点到全局目标点的全局最优路径；

40、局部规划模块，被配置为：无人车按照全局最优路径行进过程中，不断截取距离无人车预设范围的局部路径，对局部路径进行重新规划，使得无人车不断朝着局部目标点行进，直到达到最终的全局目标点；

41、其中，所述改进的a*全局路径规划方法，以全局起始点为初始当前节点，搜索当前节点的一组临近子节点，将父节点到目标点的成本估计值和动态指数权值引入到成本函数中，计算各个临近子节点的成本估计值，选择成本估计值最小的临近子节点作为当前节点，进行迭代的搜索和计算，直到当前节点为全局目标点。

42、本发明第三方面提供了计算机可读存储介质，其上存储有程序，该程序被处理器执行时实现如本发明第一方面所述的基于改进a*-teb融合算法的无人车路径规划方法中的步骤。

43、本发明第四方面提供了电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的程序，所述处理器执行所述程序时实现如本发明第一方面所述的基于改进a*-teb融合算法的无人车路径规划方法中的步骤。

44、以上一个或多个技术方案存在以下有益效果：

45、本发明提出了一种基于改进a*-teb融合算法的无人车路径规划方法，通过将改进a*算法与改进teb算法相互融合，有效解决全局路径单一，动态性能差以及局部路径容易陷入局部最优的问题，从而实现了优势互补，提升无人车路径规划效率、全局最优性以及实时避障性能。

46、本发明在传统a*算法基础上，改进了成本函数，考虑了父节点距离目标点的距离信息，提高了路径规划的实时性，同时引入了动态指数权值，使路径尽可能达到最优，改进了邻域搜索方案，提出了指向性5邻域搜索，使得搜索的节点进一步减少，从而提高规划效率。针对冗余拐点多的问题，提出最短路径算法，能够减少路径冗余拐点，减少了路径长度。因此本发明提出的改进a*算法相对于传统a*在规划效率与路径长度均有所改善。

47、本发明中使用的改进teb算法，采用b样条曲线平滑算法去平滑初始位姿序列，使得局部路径更加平滑，同时在原有的多目标约束中，增加了加速度变化率约束，使得无人车速度和加速度变化的更加顺滑，能够有效地防止无人车速度以及加速度的突变，提高电机寿命。

48、本发明附加方面的优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。