一种基于矢量法的全局静态路径规划方法与流程

本发明涉及移动机器人的路径规划技术领域，特别涉及一种基于矢量法的全局静态路径规划方法。

背景技术：

栅格法是机器人对环境建模的主要方法之一，现有的基于栅格的路径规划算法有很多，如a*，d*，d*lite，focussedd*等。其中，d*，d*lite,，focussedd*主要是解决环境动态变化所带来的行驶代价变化的问题，而对于一个已知的静态环境，a*算法能更快速有效地求解出两点之间的最短路径。

但是传统的a*算法在搜索较长的路径时，一些距离终点较远的结点和近处的结点的估价函数值相同，其在计算过程中会加入这些节点，从而会产生大量无用的结点，降低了算法运行效率。此外，a*算法通常采用的是八近邻搜索策略，使得路径规划的路线结果容易产生锯齿，不够平滑，进而增加了规划路径的长度。

论文《time-efficienta*algorithmforrobotpathplanning》(doi:10.1016/j.protcy.2016.03.010)提出了一种计算斜率的检测方式，这种方式减少了近邻搜索次数，进而提高了传统a*算法的运行效率。该种方法虽然提高了算法效率，但是增加了规划路径的长度，且因为其在遇到障碍物时沿用了传统a*算法的八近邻搜索方案，造成了路径不够平滑。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种基于矢量法的全局静态路径规划方法。

为了实现上述目的，本发明采用的技术方案为：一种基于矢量法的全局静态路径规划方法，包括如下步骤：

s1：输入阶段，首先输入机器人所处的环境地图和机器人的起点终点；

s2：接近测试阶段：计算起点和终点之间的距离，与设置的阈值比较，若小于距离阈值，采用a*算法进行路径规划；否则进入s3；

s3：矢量方法检测阶段：通过近邻栅格搜索方式搜索下一个可能的目标点，计算起点分别与多个下一个可能的目标点所构成的向量，比较这些向量与起点、终点所构成的向量之间的夹角大小，将夹角最小对应的起点和下一个可能目标点对应的向量中的下一个可能目标点作为目标点形成路径；并将将目标点放入关闭列表中；

s4:在步骤s3中，若当前搜索的目标点对应的夹角最小的下一可能目标点被障碍物占据时，则当前搜索的目标点为新的起点；若没有被障碍物占据时，以当前目标点与夹角最小的下一个目标点形成路径，以夹角最小的下一个可能的目标点作为新的目标点，并将将目标点放入关闭列表中；

s5：判断终点是否加入关闭列表中，若是，路径规划结束；若否，更新起点位置并返回步骤s3中继续进行路线规划。

所述近邻栅格搜索方式采用k近邻栅格搜索方式，k取8的整数倍。

s2中起点和终点之间的距离为欧氏距离，其计算方法为：其中(xn、yn)、(x0、y0)分别为终点位置、起点位置。

本发明的优点在于：将矢量夹角加入了启发式信息中，提高了算法运行效率；八近邻栅格搜索策略更换为多近邻栅格搜索策略，减少了规划路径的长度且更加平滑。

附图说明

下面对本发明说明书各幅附图表达的内容及图中的标记作简要说明：

图1为本发明根据矢量夹角确定目标点示意图；

图2为本发明的方法流程图。

具体实施方式

下面对照附图，通过对最优实施例的描述，对本发明的具体实施方式作进一步详细的说明。

1.输入阶段：首先输入机器人所处的环境地图和机器人的起点、终点。地图为经过栅格法处理后的二进制位地图，其中存在障碍物不可通行的区域所占栅格设为1，可通行区域所占栅格设为0；

2.接近检测阶段：通过计算起点与终点的距离，如果距离较短则判断满足接近条件，执行a*算法，否则执行步骤4；

3.步骤2中所述距离为欧氏距离，其计算方法为：

4.矢量方法检测阶段：采用近邻搜索方式搜索本目标点或起点对应的下一个可能的目标节点，比较起点和下一目标结点所构成的矢量与起点和终点构成矢量的夹角大小，将目标点投射到最小的矢量夹角点，也就是下一个目标点为与起点终点矢量夹角最小对应的下一个可能的目标点。如图1所示，o为起点，n为终点，a和b分别为a*算法进行近邻搜索过程中所考察的目标点，矢量与

矢量之间的夹角为α，矢量与之间的夹角为β，若采用欧氏距离作为启发函数，假设a*算法中估价函数f(a)与f(b)的值相等，由于α<β，节点a更接近于终点n，所以a点所处的路径为最优路径，也就是0a路径为规划路径，而在计算过程中，虽然a点和b点都会被考察，但由于a点位于最优路径上，a点会优先被考察，b点虽然会被考察但是会变为无用节点。.减少了算法的遍历次数和算法运行时间；

5.步骤4中所述的近邻搜索方式采用的是k近邻栅格搜索方式，k为阈值，取8的整数倍。采用n近邻搜索矩阵的路径规划算法相较于传统的8近邻搜索矩阵拥有更多的备选方向，所以在近邻搜索过程中能够更大范围的进行启发式函数的遍历计算，不但可以使得路径规划过程更加迅速，还能够减少路径结果的锯齿，使得路径更加平滑。

6.步骤4中所述估价函数在本发明中为：

f(n)＝g(n)+h(n)

h(n)＝hn(n)+hcos(n)

hcos(n)＝(1-cosθ)·m

式中g(n)为起点到目标节点n的行走代价，h(n)是从节点到终点的最优路径估价值，hn(n)为节点n的欧式距离估价值，hcos(n)为起点到节点n构成矢量与起点到终点构成矢量夹角θ的余弦值的估价值。m代表一定的权值。

在图1中有：

对于a、b两点，有：

hcos(a)＝(1-cosα)·m

hcos(b)＝(1-cosβ)·m

易得hcos(a)<hcos(b),所以f(a)<f(b)，即在计算过程中a点比b点优先考查，而对于a点的后续节点ai存在f(ai)≤f(b)，b点在整个计算结束前都不会被考察，从而减少了考察无用的节点数，使得算法的效率得到提高。

7.判断规划的路径oa在接近目标节点a的过程中是否存在障碍物，若否，判断a点是否为终点，若是，执行步骤8，若否，则记录规划的路径oa并以a点作为目标点返回步骤4中进一步进行规划；若是，当目标节点a接近障碍物后，标记目标节点为switcha，同时将switcha作为新的起点，并从该点利用上述方法步骤4继续进行规划路径。以目标点a位新的起点，遇到障碍物时说明障碍物占据了a点的下一个可能目标点(即此时栅格被占据，设为1)，此时以a点为新的起点搜索到终点的可通行路径。规划过程中的两个某目标点之间应该无障碍物，有障碍物就说明路径无法通行，每次搜索路径只搜索相邻可通行目标点，若该目标点被占据，则在栅格地图中将该目标点所占据的栅格为1，若当前搜索的目标点下一可能目标点被障碍物占据时，则以当前目标点为新起点，重新采用邻域搜索方式搜索下一个可能的目标点，而矢量角度比较时，以该目标点作为新的起点做矢量并做夹角对比。

8.结束阶段：当算法将终点加入关闭列表后，即可得到最终路径规划的结果。

显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，均在本发明的保护范围之内。