CN110207706A - 一种基于栅格地图的自动归位椅路径规划算法 - Google Patents

Abstract

本发明属于自动归位椅领域，具体涉及一种基于栅格地图的自动归位椅路径规划算法，包括以下步骤：创建一个开列表和一个关列表；初始化起点；判断开列表是否为空；判断当前节点是否为终点；在扩展时，判断该节点是否已经在关列表中；重新计算g(n)；直到找到目标点，遍历结束，沿着父节点反向回溯，获得最优路径。本发明的有益效果:一是改进了A*算法的启发式函数，加入了安全代价估计；二是规划出的全局路径远离障碍物，保证了自动归位椅在行驶过程中的安全性。

Description

一种基于栅格地图的自动归位椅路径规划算法

技术领域

本发明属于自动归位椅领域，具体涉及一种基于栅格地图的自动归位椅路径规划算法。

背景技术

随着巡检面积的不断增大和巡检环境的日益复杂，传统的人工巡检方式面临着劳动强度大、管理成本高且工作效率低等问题。为了部分取代或完全取代人工日常巡检的任务，本发明针对诸如配电室等场景，提出一种基于栅格地图的自动归位椅智能路径规划算法。

SLAM中常用的地图有栅格地图、拓扑地图、特征地图等。栅格地图因其容易构建和保存、每个格子所表示位置的唯一性、方便利用图搜索来路径规划等特点被大多研究者和商业公司所使用。拓扑地图常应用与服务自动归位椅领域，利于人机交互，如对自动归位椅下发“去门口”等指令，自动归位椅很容易在拓扑地图上找到门口区域，但工业自动归位椅往往需要极高的重复定位精度，此时拓扑地图就不适合了。特征地图通过点、线、面等一些几何特征来表示，一般在视觉SLAM中结合GPS或UWB等融合产生。

栅格法的基本思想是将一块需要巡检的场地划分为一个个大小相同的小栅格，分辨率根据自动归位椅本体大小、场地大小和系统硬件配置等因素综合确定，栅格划分的越细，相应的控制精度会提高，但是计算量会呈指数增长；相反栅格若太大，则路径规划不准确。一般常用为5cm。每个栅格表示一个节点，这些节点可以表示成“可行区域”、“未知区域”和“障碍物区域”三种。

栅格地图法的主要目的是在完备的地图库中，标记处自动归位椅可移动的路径和障碍物区域，用搜索法生成一条从起点到终点的无碰撞路径。自动归位椅在扫描环境的过程中，同时定位并利用传感器时时获取到的每个栅格的属性，根据地图编码方法，对每个栅格进行赋值存储。如图1所示，图中白色区域为自动归位椅可行区域，灰色为未知区域，黑色为障碍物区域。

在移动自动归位椅领域，路径规划是一个重要的研究方向，其只要目的是在存在障碍物的环境中，找到一条起点到目标点的最优或次优的无碰撞路径。

目前常用的路径规划算法有：基于图搜索的Dijkstra、A*、D*等；基于随机采样的PRM、RRT等；基于启发式的遗传算法、蚁群算法等。对于栅格地图，图搜索是最简便、快速的路径规划方法。Dijkstra是图搜索方法的鼻祖，保证能找到一条全局最优解，但其遍历了整个地图的所有栅格，特别在大地图下，计算量很大。A*在Dijkstra基础上引入了启发式搜索解决了上述问题，在保证了最优解的前提下大大提高了搜索的效率，因其简单且易于实现而盛行至今。

现有算法存在的问题：

一是传统的A*算法规划出的路径折线多，转折次数多、累计转折角度大，不利于自动归位椅直接跟随；

二是A*算法规划的路径可能会距离障碍物很近；

三是A*规划的路径离散不平滑，不适合自动归位椅直接执行。

发明内容

为了弥补现有技术的不足，本发明提供一种基于栅格地图的自动归位椅路径规划算法技术方案。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于所述算法为基于Dijkstra基础的A*算法，所述A*算法上加入了启发函数，所述A*算法包括以下步骤：

1.1)首先创建一个开列表和一个关列表，分别用来存放未访问和访问过的节点；

1.2)初始化起点，计算其f(n)，并将其放入开列表；

1.3)判断开列表是否为空，若空，跳至1.7)，若不空，则将f(n)值最小对应的节点更新为当前节点，上一个节点更新为父节点；

1.4)判断当前节点是否为终点，若是，跳至1.7)，若不是，则先将该节点从开列表移至关列表，接着继续向四周扩展节点；

1.5)在扩展时，判断该节点是否已经在关列表中，若在，说明该节点已经访问过，跳过，若不在，则继续判断该节点是否在开列表中，若在，说明已经在开列表中了，跳过，若不在，将其添加到开列表中；

1.6)重新计算g(n)，若大于之前的g(n)，跳过，若小于，说明较原来有更近的路径，则更新f(n)，用新的节点替换原来的节点，并将其放入开列表，跳至1.3)；

1.7)直到找到目标点，遍历结束，沿着父节点反向回溯，获得最优路径。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于在节点遍历过程中根据节点与障碍物的距离来融入一个节点的安全代价，并结合自动归位椅的安全半径，当节点与障碍物的距离小于该安全半径时，应该增大代价值，当节点与障碍物距离超过自动归位椅安全半径时，该安全代价为0。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于计算安全代价的步骤包括：

2.1)初始化安全代价矩阵D和机器人安全半径r；

2.2)确定待计算的节点N；

2.3)遍历N在r内所有节点，判断是否有障碍物，若是，计算N与障碍物的最小距离d，然后计算安全代价，再将该安全代价存入D矩阵；若否，N的安全代价为0，将该安全代价存入D矩阵。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于利用直线替代法对路径进行平滑，当路径中的节点几次出现在同一方向上的折线路径时，检测节点之间是否有障碍物，如没有，则将最前和最后的两个节点用直线直接连接，用插值后的两个节点的线段作为新的路径。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于平滑路径的步骤包括：

3.1)原始路径为P，连接点为p1，平滑路径为空；

3.2)将下一个节点设为当前节点；

3.3)判断当前阶段是否为空，若否，连接所有节点，得到平滑路径；若是，进入下一步；

3.4)连接下一个节点，判断该节点是否有障碍物，若是，则删除当前节点，连接下一个节点并将其设为当前节点，并回到步骤3.2)，若否，则直接回到步骤3.2)。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于判断节点间是否有障碍物的步骤包括：

4.1)连接点，当前点的下一个节点；

4.2)连接点的下一个节点赋给当前节点；

4.3)用线段连接该点与当前点的下一个节点；

4.4)判断线段是否有障碍物，是，则不可行；否，则可行。

所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于A*算法中，

其中：f(n)是从初始节点到节点n的总代价估计；g(n)为初始节点到节点n的实际代价累计；h(n)是当前节点n到目标节点的最佳路径的代价估计；l_i为自动归位椅在i个节点时实际的里程代价；s_i为安全代价；w₁和w₂为权值系数，w₁+w₂＝1，调节权值系数可以得到不同安全程度的路径。

本发明的有益效果:一是改进了A*算法的启发式函数，加入了安全代价估计；二是规划出的全局路径远离障碍物，保证了自动归位椅在行驶过程中的安全性；三是路径平滑使离散路径变得连续、平滑，减少了路径的折弯次数。

附图说明

图1为本发明的栅格地图；

图2为本发明的A*算法拓展节点方向图；

图3为本发明的A*算法流程图；

图4为本发明的安全代价计算流程图；

图5为本发明的平滑路径流程图；

图6为本发明的障碍物判断流程图。

具体实施方式

下面结合附图对本发明作进一步说明。

如图2、3所示，一种基于栅格地图的自动归位椅路径规划算法，所述算法为基于Dijkstra基础的A*算法，所述A*算法上加入了启发函数，所述A*算法包括以下步骤：

1.1)首先创建一个开列表和一个关列表，分别用来存放未访问和访问过的节点；

1.2)初始化起点，计算其f(n)，并将其放入开列表；

1.3)判断开列表是否为空，若空，跳至1.7)，若不空，则将f(n)值最小对应的节点更新为当前节点，上一个节点更新为父节点；

1.4)判断当前节点是否为终点，若是，跳至1.7)，若不是，则先将该节点从开列表移至关列表，接着继续向四周扩展节点；

1.5)在扩展时，判断该节点是否已经在关列表中，若在，说明该节点已经访问过，跳过，若不在，则继续判断该节点是否在开列表中，若在，说明已经在开列表中了，跳过，若不在，将其添加到开列表中；

1.6)重新计算g(n)，若大于之前的g(n)，跳过，若小于，说明较原来有更近的路径，则更新f(n)，用新的节点替换原来的节点，并将其放入开列表，跳至1.3)；

1.7)直到找到目标点，遍历结束，沿着父节点反向回溯，获得最优路径。其中，A*算法基于Dijkstra基础上加入了启发函数，是一种启发式算法。传统的启发式函数为

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

假设当前节点到目标节点的距离为d(n)，使用欧式距离，那么

其中：f(n)是从初始节点到节点n的总代价估计；g(n)为初始节点到节点n的实际代价累计；h(n)是当前节点n到目标节点的最佳路径的代价估计；(x_i,y_i)为当前节点的坐标，(x_g,y_g)为目标节点的坐标。

在h(n)＜d(n)的条件下，A*算法可以通过遍历节点后找到一条最短路径。

传统的A*算法在遍历节点的过程中，无论节点距离障碍物多远，自动归位椅通过它的代价估计是相等的。然而这会使得A*算法规划出的路径有可能会接近障碍物，存在着安全隐患，应该将障碍物与自动归位椅的距离考虑进去，障碍物离自动归位椅越近，评估代价越大。

为了保证规划的路径与障碍物之间保持一定的安全距离，在节点遍历过程中根据节点与障碍物的距离来融入一个节点的安全代价，并结合自动归位椅的安全半径，当节点与障碍物的距离小于该安全半径时，应该增大代价值。当节点与障碍物距离超过自动归位椅安全半径时，该安全代价为0。将其加入实际代价中，则

其中：l_i为自动归位椅在i个节点时实际的里程代价；s_i为安全代价；

w₁和w₂为权值系数，w₁+w₂＝1，调节权值系数，可以得到不同安全程度的路径。d_i表示第i个节点与障碍物的最小距离，r为自动归位椅的安全半径，k表示节点安全代价的变化系数。如图4所示为计算安全代价的流程图。

计算安全代价的步骤包括：

2.1)初始化安全代价矩阵D和机器人安全半径r；

2.2)确定待计算的节点N；

2.3)遍历N在r内所有节点，判断是否有障碍物，若是，计算N与障碍物的最小距离d，然后计算安全代价，再将该安全代价存入D矩阵；若否，N的安全代价为0，将该安全代价存入D矩阵。

节点n处的启发式函数为：

由公式(3)～(5)可知，总的代价函数为：

公式中的i表示第i个节点，x,y是指第i个节点的index值。XY代表的是第几行第几列。

传统的A*算法规划的路径折弯较多，不利于自动归位椅直接跟随行走，

因此要对其进行平滑处理。

利用直线替代法对路径进行平滑。当路径中的节点出现几次在同一方向上的折线路径时，检测节点之间是否有障碍物，如没有，则将最前和最后的两个节点用直线直接连接，用插值后的两个节点的线段作为新的路径。如图5和6分别为路径平滑和节点处障碍物检测的流程图。

平滑路径的步骤包括：

3.1)原始路径为P，连接点为p1，平滑路径为空；

3.2)将下一个节点设为当前节点；

3.3)判断当前阶段是否为空，若否，连接所有节点，得到平滑路径；若是，进入下一步；

3.4)连接下一个节点，判断该节点是否有障碍物，若是，则删除当前节点，连接下一个节点并将其设为当前节点，并回到步骤3.2)，若否，则直接回到步骤3.2)。

判断节点间是否有障碍物的步骤包括：

4.1)连接点，当前点的下一个节点；

4.2)连接点的下一个节点赋给当前节点；

4.3)用线段连接该点与当前点的下一个节点；

4.4)判断线段是否有障碍物，是，则不可行；否，则可行。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (7)

1.一种基于栅格地图的自动归位椅路径规划算法，其特征在于所述算法为基于Dijkstra基础的A*算法，所述A*算法上加入了启发函数，所述A*算法包括以下步骤：

1.1)首先创建一个开列表和一个关列表，分别用来存放未访问和访问过的节点；

1.2)初始化起点，计算其f(n)，并将其放入开列表；

1.3)判断开列表是否为空，若空，跳至1.7)，若不空，则将f(n)值最小对应的节点更新为当前节点，上一个节点更新为父节点；

1.4)判断当前节点是否为终点，若是，跳至1.7)，若不是，则先将该节点从开列表移至关列表，接着继续向四周扩展节点；

1.5)在扩展时，判断该节点是否已经在关列表中，若在，说明该节点已经访问过，跳过，若不在，则继续判断该节点是否在开列表中，若在，说明已经在开列表中了，跳过，若不在，将其添加到开列表中；

1.6)重新计算g(n)，若大于之前的g(n)，跳过，若小于，说明较原来有更近的路径，则更新f(n)，用新的节点替换原来的节点，并将其放入开列表，跳至1.3)；

1.7)直到找到目标点，遍历结束，沿着父节点反向回溯，获得最优路径。

2.根据权利要求1所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于在节点遍历过程中根据节点与障碍物的距离来融入一个节点的安全代价，并结合自动归位椅的安全半径，当节点与障碍物的距离小于该安全半径时，应该增大代价值，当节点与障碍物距离超过自动归位椅安全半径时，该安全代价为0。

3.根据权利要求2所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于计算安全代价的步骤包括：

2.1)初始化安全代价矩阵D和机器人安全半径r；

2.2)确定待计算的节点N；

2.3)遍历N在r内所有节点，判断是否有障碍物，若是，计算N与障碍物的最小距离d，然后计算安全代价，再将该安全代价存入D矩阵；若否，N的安全代价为0，将该安全代价存入D矩阵。

4.根据权利要求3所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于利用直线替代法对路径进行平滑，当路径中的节点几次出现在同一方向上的折线路径时，检测节点之间是否有障碍物，如没有，则将最前和最后的两个节点用直线直接连接，用插值后的两个节点的线段作为新的路径。

5.根据权利要求4所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于平滑路径的步骤包括：

3.1)原始路径为P，连接点为p1，平滑路径为空；

3.2)将下一个节点设为当前节点；

3.3)判断当前阶段是否为空，若否，连接所有节点，得到平滑路径；若是，进入下一步；

3.4)连接下一个节点，判断该节点是否有障碍物，若是，则删除当前节点，连接下一个节点并将其设为当前节点，并回到步骤3.2)，若否，则直接回到步骤3.2)。

6.根据权利要求4或5所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于判断节点间是否有障碍物的步骤包括：

4.1)连接点，当前点的下一个节点；

4.2)连接点的下一个节点赋给当前节点；

4.3)用线段连接该点与当前点的下一个节点；

4.4)判断线段是否有障碍物，是，则不可行；否，则可行。

7.根据权利要求6所述的一种基于栅格地图的自动归位椅路径规划算法，其特征在于A*算法中，

其中：f(n)是从初始节点到节点n的总代价估计；g(n)为初始节点到节点n的实际代价累计；h(n)是当前节点n到目标节点的最佳路径的代价估计；l_i为自动归位椅在i个节点时实际的里程代价；s_i为安全代价；w₁和w₂为权值系数，w₁+w₂＝1，调节权值系数可以得到不同安全程度的路径。