一种基于凸角点搜索机制的改进Astar算法

技术领域

本发明涉及移动机器人全局路径规划领域，具体的为一种基于凸角点搜索机制的改进Astar算法。

背景技术

目前室内扫地机器人、服务机器人等采用二维激光雷达建图，主流二维激光slam生成的是栅格地图。所以应用于栅格地图的全局路径规划算法的研究越来越活跃。应用于栅格地图的全局路径规划算法有很多，如Dijkstra算法、A*算法、JPS算法等。其中Dijkstra算法是典型的广度优先算法，优点在于总是可以找到最优化路径、简单易实现。但是因为Dijkstra算法采用遍历搜索方式，当地图节点较多时，节点网络变得非常庞大从而导致搜索效率低，难以满足快速规划路径的需求。且最终生成的路径转折点过多，不利于机器人移动。A*算法结合best-first search 和Dijkstra算法，它保证了在得到最优路径规划的基础上，采用启发式搜索。A* 算法通过一个估价函数来确定搜索方向，从起点开始向周围扩展，通过估价函数计算得到周围每个节点的代价值，选择最小代价节点作为下一个扩展节点，重复这一过程直到到达目标点，生成最终路径。相比于Dijkstra算法效率得到了提升，但是在搜索过程中遍历了大量不必要操作的点，从而导致传统A*在大地图场景中存在一些缺陷如：内存消耗高、计算时间长。JPS算法为结合跳点搜索的改进A*算法是在A*算法的基础上增加了一个预处理的过程，预处理就是通过跳点搜索算法挑选出一批有代表性的跳点，然后将跳点分别添加到OpenList进行计算，这样一来避免了A*算法寻路过程中对大量中间节点的计算。但是在跳点搜索需要遍历到地图的边界点，如果地图有一块很大的无障碍区域JPS算法会很多时间搜索且无法获得有价值的跳点。所以JPS算法只适合代价均匀的地图，在大地图且障碍物分布不均匀的场景下效率不高。

发明内容

针对现有技术中在凸角点较少且地图大的场景中传统Astar算法搜索效率低的问题，本发明要解决的技术问题是提供一种基于凸角点搜索机制的改进Astar算法。

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

步骤1：寻找栅格地图中所有的凸角点；

步骤2：将终点放入ConvexList中，同时将起点加入OpenList中；

步骤3：判断OpenList是否为空如果为空代表寻路失败跳出循环，不为空执行下一步；

步骤4：计算出OpenList里面最小的预估代价值f(n)节点，将当前点改为这个点然后将该点从OpenList删除并加入CloseList中；

步骤5：判断该点是不是目标点。如果不是目标点则继续进行第六步。如果是则开始输出路径，从终点开始沿着每个父节点达到起点，构成一条起点到终点的完整路径，从当前节点回溯到父节点的方式需与步骤六判断邻居关系时的流程相同；

步骤6：在ConvexList中寻找与当前点Current成邻居关系，且不在CloseList表的所有凸角点，则将满足要求的凸角点加入OpenList并更新其预估代价值f(n)和父节点指向，完成之后返回步骤1。

作为优选的，所述步骤1中寻找凸角点的方法为：在栅格地图中凸角点定义是与障碍物栅格成对角关系的空白栅格，并且同时和该栅格、障碍物栅格相邻的两个栅格没有障碍物。将障碍物栅格坐标设为(x,y)。如果判断(x-1,y)、(x,y-1)和(x-1,y-1)的栅格都是无障碍物的栅格则可以认为(x-1,y-1)的栅格为凸角点栅格，同理(x-1,y)、(x,y+1)和(x-1,y+1)为无障碍物栅格则可以认为(x-1,y+1)的栅格为凸角点栅格，和当(x,y-1)、(x+1,y)和(x+1,y-1)为无障碍物栅格则可以认为(x+1,y-1)的栅格为凸角点栅格，以及当(x,y+1)、(x+1,y)和(x+1,y+1)为无障碍物栅格则可以认为(x+1,y+1)的栅格为凸角点栅格。

作为优选的，所述步骤6中判断邻居关系的方法为：在栅格地图中两个点可以通过最多一条对角斜线和最多一条水平或者垂直直线连接，那这两个点就互相满足邻居关系。具体判断步骤如下：设地图左上角为坐标原点，x轴为垂直向下方向、y轴为水平向右方向，第一个点坐标为（x1,y1),第二个点坐标为(x2,y2)。

第一步先求出两个点x轴坐标差和y轴坐标差，然后用min和max分别存储两者绝对值的最小值和最大值；

第二步，若x1-x2为正则signal1为-1，否则为1。同里若y1-y2为正则signal2为-1，否则为1；

第三步，若min为0直接跳到S4。如果min不为0则i从1到min开始循环判断坐标为(x1+i*signal1,y1+i*signal2)是否为有障碍物栅格（判断对角斜线是否有障碍物），若有障碍物则代表两个点不满足邻居关系直接退出邻居关系判断程序。若一直没有障碍物栅格则进行第四步；

第四步，如果x1-x2的绝对值大于y1-y2的绝对值则i从1到max-min开始循环判断坐标为(x1+min*signal1+i*signal1,y1+min*signal2)是否为有障碍物栅格（判断垂直线上是否有障碍物，如小于则i从1到max-min开始循环判断坐标为(x1+min*signal1,y1+min*signal2+i*signal2)是否为有障碍物栅格（判断水平线上是否有障碍物）若有障碍物则代表两个点不满足邻居关系直接退出邻居关系判断程序。若一直没有障碍物栅格则代表两个点满足邻居关系。

本发明的有益效果是：在大地图场景中，移动机器人全局路径时间长难以满足实时的要求，本发明在大地图且凸角点较少的场景中算法搜索效率高于传统A*算法，能够在更短的时间内获得全局路径规划结果。

附图说明

图1为基于凸角点搜索机制的改进Astar算法流程图。

图2为凸角点示意图。

图3为邻居关系示意图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

步骤1：寻找栅格地图中所有的凸角点，首先遍历整个栅格地图。当遍历的栅格代表有障碍物如图2中的灰色栅格所示，坐标设为(x,y)。如果(x-1,y)、(x,y-1)和(x-1,y-1)都是无障碍物的栅格则可以认为(x-1,y-1)的栅格为凸角点栅格，同理(x-1,y)、(x,y+1)和(x-1,y+1)为无障碍物栅格则可以认为(x-1,y+1)的栅格为凸角点栅格，和当(x,y-1)、(x+1,y)和(x+1,y-1)为无障碍物栅格则可以认为(x+1,y-1)的栅格为凸角点栅格，以及当(x,y+1)、(x+1,y)和(x+1,y+1)为无障碍物栅格则可以认为(x+1,y+1)的栅格为凸角点栅格。代码实现则只需要四个判断语句就可以找出一个障碍物栅格所对应的所有凸角点，如图2中坐标为(x-1,y-1)，(x-1,y+1)，(x+1,y-1)，(x+1,y+1)的栅格为凸角点栅格；

步骤2：将终点和所有凸角点放入ConvexList表中，同时将起点加入OpenList表中；

步骤3：判断OpenList是否为空如果为空跳出，不为空执行下一步；

步骤4：计算出OpenList里面f(n)最小的节点，将当前点Current改为这个点然后将该点从OpenList删除并加入CloseList中；

步骤5：判断该点是不是目标点。如果不是目标点则继续进行第六步。如果是则开始输出路径，从终点开始沿着每个父节点达到起点，构成一条起点到终点的完整路径，从当前节点回溯到父节点的方式需与步骤六判断邻居关系时的顺序相同。即先判断对角斜线是否有障碍物再判断水平或者垂直线是否有障碍物，则输出路径时需要从父节点先沿着对角斜线输出，再水平或者垂直直线输出；

步骤6：在ConvexList中寻找与当前点Current成邻居关系的凸角点。如果该凸角点不在CloseList则将其加入OpenList，设终点坐标为(xn,yn)、该凸角点坐标为(x,y)，计算该起点到凸角点的曼哈顿距离g=g0+(max-min)*10+min*14(g0为当前点Current的g)、凸角点到终点的曼哈顿距离h=|xn-x|+|yn-y|、以及启发函数f(n)=g+h，且该凸角点的父节点指针指向当前节点Current。如果已经在OpenList中则判断由当前节点计算出的f(n)是否比原本f(n)大，如果小则将该凸角点的f(n)和g更新成当前节点计算出g且父节点指向当前节点Current，如果大则不做改变。完成之后返回步骤1。

其中步骤6所述的邻居关系如下：两个点构成邻居关系需要满足下面的条件，两个点可以通过最多一条斜线和最多一条水平或者垂直直线连接。具体流程如下：设地图左上角为坐标原点，x轴为垂直向下方向、y轴为水平向右方向，第一个点坐标为（x1,y1),第二个点坐标为(x2,y2)。下面为判断两个点是否满足邻居关系的详细步骤

S1：先求出两个点x轴坐标差和y轴坐标差，然后用min和max分别存储两者绝对值的最小值和最大值；

S2：若x1-x2为正则signal1为-1，否则为1。同里若y1-y2为正则signal2为-1，否则为1；

S3：若min为0直接跳到S4。如果min不为0则i从1到min开始循环判断坐标为(x1+i*signal1,y1+i*signal2)是否为有障碍物栅格（判断对角斜线是否有障碍物），若有障碍物则代表两个点不满足邻居关系直接退出邻居关系判断程序。若一直没有障碍物栅格则进行第四步；

S4：如果x1-x2的绝对值大于y1-y2的绝对值则i从1到max-min开始循环判断坐标为(x1+min*signal1+i*signal1,y1+min*signal2)是否为有障碍物栅格（判断垂直线上是否有障碍物，如小于则i从1到max-min开始循环判断坐标为(x1+min*signal1,y1+min*signal2+i*signal2)是否为有障碍物栅格（判断水平线上是否有障碍物）若有障碍物则代表两个点不满足邻居关系直接退出邻居关系判断程序。若一直没有障碍物栅格则代表两个点满足邻居关系。如图3所示，序号1栅格和序号2栅格通过一条对角斜线和一条水平线连通，两个点时满足上面提出的邻居关系的要求。

以上所述仅是本发明的优选实施方式，应当理解所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。本发明并非局限于本文所披露的形式，不应看作是对其他实施例的排除，而可用于各种其他组合、修改和环境，并能够在本文所述构想范围内，通过上述教导或相关领域的技术或知识进行改动。而本领域人员所进行的改动和变化不脱离本发明的精神和范围，则都应在本发明所附权利要求的保护范围内。