本发明涉及路径规划,尤其涉及一种基于角度估价函数的二维栅格地图路径规划方法。
背景技术:
1、二维栅格地图便于理论计算和结果展示,常作为自动驾驶、机器人导航、游戏ai等路径规划、最优决策或者搜索问题领域的必备地图。自1968年美国计算机科学家peterhart,nils nilsson和bertram raphael提出用于计算二维栅格地图最短路径的a*算法以来,针对a*算法的改进始终没有停止,先后经历了weighted a*算法、dynamic a*算法、theta*算法、anytime repairing a*算法(ara*)、lifelong planning a*算法(lpa*)和跳点搜索(jump point search,jps)算法等多个重要改进方法,但上述算法和其各种改进版本均立足于无障碍物的可用坐标点搜索,搜索的路径不是最短路径,切路径上的坐标点较多,耗时较长,从而效率较低。
技术实现思路
1、本发明所要解决的技术问题是如何提供一种能够减少规划路径上的点数,找到最短路径的二维栅格地图路径规划方法。
2、为解决上述技术问题,本发明所采取的技术方案是:一种基于角度估价函数的二维栅格地图路径规划方法,包括如下步骤:
3、s1:找到二维栅格地图中所有障碍物的顶点坐标,放入数组pointlists[]中;
4、s2:规定一个探测距离l;
5、s3:将pointlists[]数组中同时满足以下三个条件的点纳入openlists[]数组,条件1:从当前起点开始以探测距离l为半径,pointlists[]数组中的点与当前起点距离小于l;条件2:在pointlists[]数组中与当前起点距离小于l的点与当前起点的连线和水平正方向坐标轴的夹角α不大于90°;条件3:当前起点与pointlists[]数组中的点的连线构成的向量及延长向量之间没有障碍物;
6、s4:计算当前起点至终点之间连线与水平正方向坐标轴夹角为β,计算当前点与openlists[]数组中所有连线夹角,设openlists[]数组中有k个点,则当前起点与这k个点的连线与水平正向夹角分别为α1,α2,……αk,将β和α1,α2,……αk的值带入估价函数式(1)得到一个最小值对应点,将该点放入closelists[]数组;
7、f(θ)=min|β-α| (1)
8、其中:β∈{β1,β2,…,βn},α∈{α1,α2,…,αn},β,α∈[0°,90°],n∈n*,n*为自然数;
9、s5:通过步骤s4选出来下一个可能起点即为下一步的当前节点,按照上述方法反复重复步骤s3和步骤s4,将pointlists[]数组中的点清空,并通过openlists[]数组找到符合公式(1)的点,并最终放入closelists[]数组,将起点,closelists[]数组中的点和终点连接起来,找到最优路径。
10、采用上述技术方案所产生的有益效果在于:本发明所述方法改善了现有技术中的a*算法、jps算法、jps+算法和上述三种算法到目前的各种版本,是目前基于栅格地图进行路径规划方案中找到最短路径和路径上点数最少的一种有效算法,通过仿真运算验证了算法的有效性。
1.一种基于角度估价函数的二维栅格地图路径规划方法,其特征在于包括如下步骤:
2.如权利要求1所述的基于角度估价函数的二维栅格地图路径规划方法,其特征在于:估价函数(1)中,β为当前起点坐标至终点坐标的连线与横轴正向线夹角,当起点位置发生变化后,当前起点至终点连线与横轴正向线夹角β取值假设有m个,则β∈{β1,β2,…,βm};定义当前起点到下一步所有可能起点的连线与横轴正向方向的夹角为α,假设下一步可能的起点有k个,则α∈{α1,α2,…,αk}。
3.如权利要求2所述的基于角度估价函数的二维栅格地图路径规划方法,其特征在于:由于β和α满足0°~90°的条件,当β和α大于该范围时,要将其变换到该范围内,变换方法使用如下公式计算:
4.如权利要求2所述的基于角度估价函数的二维栅格地图路径规划方法,其特征在于: