一种基于安全圈的机器人全局路径规划A*改进方法技术

本发明专利技术涉及一种基于安全圈的机器人全局路径规划A*改进方法，属于机器人控制以及路径规划技术领域。包括：1)对A*算法进行初始化，创建open表与close表，将起始点加入open表中；2)将open表中的起始点设为当前搜索点N；3)判断当前搜索点N是否是目标点，若是，搜索结束，从目标点开始，按照close表中代价值F由小到大排列，直到起始点，即可得最佳路径，否则，若不是目标点，则计算地图中与该搜索点N相邻的所有点的代价值F，并将这些所有点加入open表，再将当前搜索点N加入close表。所述方法节省地图预处理阶段的运算量，在地图较大或分辨率较高时，避免处理任务不涉及的区域，从而节省运算资源。资源。资源。

【技术实现步骤摘要】
一种基于安全圈的机器人全局路径规划A*改进方法


[0001]本专利技术涉及一种基于安全圈的机器人全局路径规划A*改进方法，属于机器人控制以及路径规划


技术介绍

[0002]随着移动机器人在各行各业的普及，对移动机器人按照任务需求寻找最佳移动路径的能力也产生了更高的要求。路径规划指在机器人进行导航任务时寻找到达目的地最佳路线的问题，路径规划包括利用已有的地图来确定总体线路的全局路径规划，以及根据实时信息决定机器人短时间行进轨迹的局部路径规划。全局路径规划的过程需要图构造算法和图搜索算法的配合。图构造算法包括可视图法、Voronoi图、切线图法、栅格分解法、概率路标算法、快速随机数算法等，这些图搜索算法可以将复杂的环境地图重构为简单的网格或拓扑结构的骨架图，然后通过图搜索算法在骨架图中找到最佳路径，图搜索算法通常有Dijkstra、A*、D*等算法，其中在国内外的研究中A*算法最为常见。
[0003]传统的全局路径规划算法能够搜索到地图上指定点之间的最短路径，但在移动机器人工作场景中，机器人如果移动轨迹距离障碍物太近则容易导致移动效率降低以及其他安全问题。针对这一问题，本申请基于安全圈方法对全局路径规划的图搜索算法A*进行优化，使A*生成路径时与障碍物保持安全距离。在使用较多的机器人操作系统ROS中，系统通过在地图预处理阶段插入膨胀层来解决该问题，在地图预处理阶段，在所有障碍物周围形成膨胀区域，它们像“缓冲垫”一样围绕着障碍物，使得机器人不会靠近障碍物，其原理是根据地图上各点与障碍物的距离来设置不同的代价值，代价值越高，路径规划时就越倾向于不选择这个点。该路径规划的优点是运行于地图预处理阶段，不影响规划的时间，一次计算可重复利用；不足在于对规模较大或分辨率较高的地图，该路径规划在预处理阶段需要消耗大量时间，且由于机器人不经过的区域也经过预处理，因此可能会浪费计算资源。
[0004]本申请拟通过在A*产生的路径上生成安全圈来保持路径与障碍物的距离，其思路与膨胀层相反，直接以A*搜寻过程中的搜寻点为中心计算与障碍物的距离，从而避免大量的地图预处理计算，也避免了对不经过区域的计算。

技术实现思路

[0005]本专利技术的目的在于针对传统全局路径规划A*算法生成的路径与障碍物距离过近，容易影响机器人行进效率或影响行进安全的问题，提出了一种基于安全圈的机器人全局路径规划A*改进方法。
[0006]为了达到上述目的，本专利技术采取如下技术方案。
[0007]所述机器人全局路径规划A*改进方法，包括如下步骤：
[0008]步骤1、对A*算法进行初始化，创建open表与close表，将起始点加入open表中；
[0009]所述open表与close表中都包含有若干节点；
[0010]步骤2、将open表中的起始点设为当前搜索点N；
[0011]步骤3、初始化i为1；
[0012]步骤4、判断当前搜索点N是否是目标点，若是，则跳至步骤6，否则，若不是目标点，则计算地图中与该搜索点N相邻的所有点的代价值F，并将这些所有点加入open表，再将当前搜索点N加入close表；
[0013]其中，相邻的所有点，总数为K个，所有点记为：M1、M2…
M
K
；
[0014]步骤4中计算搜索点N相邻的所有点的代价值F，具体为：计算M
i
的代价值F，包括如下子步骤：
[0015]步骤4.1、计算点M
i
与起始点的最短距离G；
[0016]其中，最短距离G的计算公式为G＝G
’
+C；且G
’
为点M
i
周围已搜索过的点中G值最小点的G值，C为每增加一格距离的代价常数；
[0017]步骤4.2、计算点M
i
与目标点的距离估计值H；
[0018]所述点M
i
与目标点的距离估计值H为欧几里得距离H1或曼哈顿距离H2中的一种；且H1＝[(N
x
‑
D
x
)2+(N
x
‑
D
x
)2]1/2
及H2＝|N
x
‑
D
x
|+|N
y
‑
D
y
|；
[0019]其中，(Nx,Ny)和(Dx,Dy)分别为当前搜索点以及目标点的坐标；
[0020]步骤4.3、基于安全圈计算点M
i
与最近障碍物距离所对应的代价值P，具体包括如下子步骤：
[0021]步骤4.3.1、在搜索点周围设置一个圆形的安全半径r作为安全圈，将安全圈近似为正方形并分割成数个同心正方形，以最内侧同心正方形为最内层，各个正方形减去其内侧的正方形所得的环状空间为一层，使该正方形即安全圈被分为若干层；
[0022]步骤4.3.2、判断是否已完成对安全圈所有层的搜索，若否，则搜索下一层，若是，则该范围内无障碍物，则返回P值为0，进入步骤4.4；
[0023]所述搜索下一层是指按照从安全圈最内层向外逐层的顺序，对当前层的外侧一层进行搜索；
[0024]步骤4.3.3、判断是否已完成对当前层的全部搜索，若否，则搜索当前层的下一条边，若是，则返回步骤4.3.2；
[0025]所述搜索当前层的下一条边是指按上、右、下、左顺序搜索正方形的四条边时，当前边的下一条边；
[0026]步骤4.3.4、判断是否已完成对当前边的全部搜索，若否，则继续检测下一点，若是，则返回步骤4.3.3；
[0027]所述检测下一点是指沿当前边依次按顺时针方向搜索当前边上的所有点时，当前点的下一点；
[0028]步骤4.3.5、判断当前点是否为障碍物，若否，则返回步骤4.3.4，若是，则计算P值，随后进入步骤4.4；
[0029]所述P值的计算公式为P＝I*exp(
‑
r*d)，其中，I为P的最大取值，r为安全圈的安全半径，d为当前点与最近的障碍物间的距离；
[0030]步骤4.4、计算点M
i
的代价值F；
[0031]代价值F的计算公式为F＝G+P+H；
[0032]H是当前点到目的节点的估测距离对应的代价，G为起始点到当前点距离对应的代价；且F值的大小由节点是否被障碍物占据以及节点与起始点和目标点间的距离决定，与起
始点或目标点越近的节点F值越低，代表越容易被选为路径，将P值加入代价值F中，A*生成的路径就能针对搜索点与障碍物的距离生成新的路径，若搜索点安全圈内存在障碍物，A*则有更小的可能选择该搜索点；
[0033]步骤4.5、判断i是否等于K，若是，则表明当前搜索点N的所有K个相邻点全部搜索完毕，设置i为1，跳至步骤5；若否将i加1，并跳至步骤4.1；
[0034]步骤5、选择当前open表中代价值F最小的节点作为当前搜索点N，跳本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于安全圈的机器人全局路径规划A*改进方法，其特征在于：包括如下步骤：步骤1、对A*算法进行初始化，创建open表与close表，将起始点加入open表中；步骤2、将open表中的起始点设为当前搜索点N；步骤3、初始化i为1；步骤4、判断当前搜索点N是否是目标点，若是，则跳至步骤6，否则，若不是目标点，则计算地图中与该搜索点N相邻的所有点的代价值F，并将这些所有点加入open表，再将当前搜索点N加入close表；步骤4中计算搜索点N相邻的所有点的代价值F，具体为：计算M
i
的代价值F，包括如下子步骤：步骤4.1、计算点M
i
与起始点的最短距离G；步骤4.2、计算点M
i
与目标点的距离估计值H；步骤4.3、基于安全圈计算点M
i
与最近障碍物距离所对应的代价值P，具体包括如下子步骤：步骤4.3.1、在搜索点周围设置一个圆形的安全半径r作为安全圈，将安全圈近似为正方形并分割成数个同心正方形，以最内侧同心正方形为最内层，各个正方形减去其内侧的正方形所得的环状空间为一层，使该正方形即安全圈被分为若干层；步骤4.3.2、判断是否已完成对安全圈所有层的搜索，若否，则搜索下一层，若是，则该范围内无障碍物，则返回P值为0，进入步骤4.4；步骤4.3.3、判断是否已完成对当前层的全部搜索，若否，则搜索当前层的下一条边，若是，则返回步骤4.3.2；步骤4.3.4、判断是否已完成对当前边的全部搜索，若否，则继续检测下一点，若是，则返回步骤4.3.3；步骤4.3.5、判断当前点是否为障碍物，若否，则返回步骤4.3.4，若是，则计算P值，随后进入步骤4.4；步骤4.4、计算点M
i
的代价值F；代价值F的计算公式为F＝G+P+H；H是当前点到目的节点的估测距离对应的代价，G为起始点到当前点距离对应的代价；且F值的大小由节点是否被障碍物占据以及节点与起始点和目标点间的距离决定，与起始点或目标点越近的节点F值越低，代表越容易被选为路径，将P值加入代价值F中，A*生成的路径就能针对搜索点与障碍物的距离生成新的路径，若搜索点安全圈内存在障碍物，A*则有更小的可能选择该搜索点；步骤4.5、判断i是否等于K，若是，则表明当前搜索点N的所有K个相邻点全部搜索完毕，设置i为1，跳至步骤5；若否将i加1，并跳至步骤4.1；步骤5、选择当前open表中代价值F最小的节点作为当前搜索点N，跳至步...

【专利技术属性】
技术研发人员：王军茹，邢量，孙广彬，张菂，易军凯，毛芹，
申请(专利权)人：北京信息科技大学，
类型：发明
国别省市：