基于A*算法的路径规划方法与流程

本发明涉及智能机器人领域，尤其涉及基于a*算法的路径规划方法。

背景技术：

随着巡检面积的不断增大和巡检环境的日益复杂，传统的人工巡检方式面临着劳动强度大、管理成本高且工作效率低等问题。为了部分取代或完全取代人工日常巡检的任务，研发出来巡检机器人进行日常巡检。

在智能机器人领域，路径规划是一个重要的研究方向，其只要目的是在存在障碍物的环境中，找到一条起点到目标点的最优或次优的无碰撞路径。目前常用的路径规划算法有：基于图搜索的dijkstra、a*、d*等；基于随机采样的prm、rrt等；基于启发式的遗传算法、蚁群算法等。对于栅格地图，图搜索是最简便、快速的路径规划方法。dijkstra是图搜索方法的鼻祖，保证能找到一条全局最优解，但其遍历了整个地图的所有栅格，特别在大地图下，计算量很大。a*在dijkstra基础上引入了启发式搜索解决了上述问题，在保证了最优解的前提下大大提高了搜索的效率，因其简单且易于实现而盛行至今。

a*算法基于dijkstra基础上加入了启发函数，是一种启发式算法，同时也是已知静态地图下求解最短路径的最有效的方法。但是传统的a*算法在遍历节点的过程中，无论节点距离障碍物多远，机器人通过它的代价估计是相等的。然而这会使得a*算法规划出的路径有可能会接近障碍物，存在着安全隐患。

技术实现要素：

为解决上述问题，本发明提出基于a*算法的路径规划方法。

基于a*算法的路径规划方法，包括以下步骤：

s1：根据节点与障碍物的距离，并结合移动单元的安全半径，建立安全代价函数；

s2：根据安全代价函数，建立启发式函数；

s3：根据启发式函数通过a*算法进行路径规划。

优选的，所述根据节点与障碍物的距离，并结合移动单元的安全半径，建立安全代价函数包括：

建立安全代价函数：

其中，si表示第i个节点的安全代价，di表示第i个节点与障碍物的最小距离，r表示移动单元的安全半径，k表示节点安全代价的变化系数。

优选的，根据安全代价函数，建立启发式函数包括以下步骤：

s201：先根据安全代价函数计算起点到当前节点n的实际代价g(n)：

其中：li表示移动单元在i个节点时实际的里程代价；si表示安全代价；w1和w2表示权值系数，w1+w2＝1；

s202：计算从当前节点n到终点的估计代价：

其中，x表示当前节点n的横坐标；y表示当前节点n的纵坐标；

s203：根据实际代价g(n)和估计代价启发式函数h(n)建立启发式函数：

进一步的，该方法还包括以下步骤：

s4：利用直线替代法对路径进行平滑处理。

优选的，所述利用直线替代法对路径进行平滑处理包括以下步骤：

s401：检查当前节点n与下一节点n+1之间是否有障碍物；

s402：如果没有，则检查当前节点n与再下一个节点n+2之间是否有障碍物，直到检查到障碍物或历遍所有节点；

s403：若果检查到障碍物，则将当前节点n和最后节点的上一个节点n+k连接起来；如果历遍所有节点都没检查到障碍物，则将当前节点n和最后节点p之间连接起来；

s404：用线性插值取代上述已连接的两个节点之间的不规则的点；

s405：以节点n+k+1作为当前节点重复步骤s401-s404，直到路径中所有的节点遍历结束算法收敛。

优选的，所述线性插值的计算公式为：

其中：(x0,y0)和(x1,y1)分别为连接的两个节点的坐标，(x,y)为所得的插值点坐标。

通过使用本发明，可以实现以下效果：

通过改进a*算法的启发式函数，在其中加入了安全代价估计，使得规划出的路径远离障碍物，保证了机器人在行驶过程中的安全性；利用直线替代法对路径进行平滑处理，使路径变得连续、平滑，减少了路径的折弯次数。

附图说明

下面结合附图和具体实施方式对本发明作进一步详细的说明。

图1是本发明实施例的整体流程示意图；

图2是本发明实施例的障碍物判断的流程示意图；

图3是本发明实施例的建立启发式函数的流程示意图；

图4是本发明实施例的a*算法路径规划的流程示意图；

图5是本发明实施例的安全代价计算的流程示意图；

图6是本发明实施例的未平滑处理的路径图；

图7是本发明实施例的平滑处理后的路径图。

具体实施方式

以下结合附图，对本发明的技术方案作进一步的描述，但本发明并不限于这些实施例。

传统的a*算法在遍历节点的过程中，无论节点距离障碍物多远，移动单元通过它的代价估计是相等的。这使得a*算法规划出的路径有可能会接近障碍物，存在着安全隐患。本发明实施例提出一种基于a*算法的路径规划方法，改进a*算法的启发式函数，在其中加入了安全代价估计，使得规划出的路径远离障碍物，保证了机器人在行驶过程中的安全性；利用直线替代法对路径进行平滑处理，使路径变得连续、平滑，减少了路径的折弯次数。

如图1所示，该方法具体包括以下步骤：

s1：根据节点与障碍物的距离，并结合移动单元的安全半径，建立安全代价函数。

建立安全代价函数：

其中，si表示第i个节点的安全代价，di表示第i个节点与障碍物的最小距离，r表示移动单元的安全半径，k表示节点安全代价的变化系数。

其中，如图2所示，障碍物的判断算法为：

s101：连接当前节点与下一节点；

s102：将下一节点赋给当前节点；

s103：连接当前节点与下一节点；

s104：判断线段是否经过障碍物。

为了保证规划的路径与障碍物之间保持一定的安全距离，在节点遍历过程中根据节点与障碍物的距离来融入一个节点的安全代价，并结合移动单元的安全半径，当节点与障碍物的距离小于该安全半径时，应该增大代价值。根据安全代价函数可知，障碍物离移动单元越近，则安全代价越大。当障碍物离移动单元的距离超过设定的安全半径，则安全代价为零。这里安全半径一般为移动单元的半径。

s2：根据安全代价函数，建立启发式函数。如图3所示，具体包括以下步骤：

s201：先根据安全代价函数计算起点到当前节点n的实际代价g(n)：

其中：li表示移动单元在i个节点时实际的里程代价；si表示安全代价；w1和w2表示权值系数，w1+w2＝1；

s202：计算从当前节点n到终点的估计代价：

其中，x表示当前节点n的横坐标；y表示当前节点n的纵坐标；

s203：根据实际代价g(n)和估计代价启发式函数h(n)建立启发式函数：

s3：根据启发式函数通过a*算法进行路径规划。具体步骤如图4所示：

s301：首先创建openlist列表，openlist列表用于存储需要遍历的节点，一开始初始化时openlist为空，在指定初始节点n后，将n节点放入openlist列表；

s302：查看与n节点相邻的8个节点示，如果没有访问过且可达到(即没有障碍物)，那么将其放入openlist列表中，同时设n为父节点；

s303：创建一个closelist列表，将n节点放入closelist列表，选取上一次搜索中启发式函数f(n)最小的节点m，将其更新为父节点；

s304：将节点m从openlist列表中取出，放入closelist列表。遍历相邻的所有节点，如该节点不在openlist列表，将其放入openlist列表；

s305：如果某个节点已经在openlist列表中，则检查这条路径的实际代价g(n)是否更小，如果是，则将该节点的父节点设为当前节点，然后重新计算节点的实际代价g(n)和启发式函数f(n)值；如果没有，则不更新；

结合附图5，实际代价g(n)中安全代价的计算过程为：

s3051：初始化安全代价矩阵d和移动单元的安全半径r；

s3052：确定待计算的n个节点，并判断该节点直线连接的路径中是否有障碍物，若无障碍物，则这n个节点的安全代价为0，并存入安全代价矩阵d；若有障碍物，则进行步骤s3053；

s3053：计算第i个节点与障碍物的最小距离di；

s3054：通过步骤二中的安全代价函数计算安全代价，并将计算得到的安全代价入安全代价矩阵d。

s306：重复上述步骤s301-s305，直到目标点在openlist列表中，表明遍历结束，所有父节点反向回溯就是最优路径。

s4：利用直线替代法对路径进行平滑处理。具体包括以下步骤：

s401：检查当前节点n与下一节点n+1之间是否有障碍物；

s402：如果没有，则检查当前节点n与再下一个节点n+2之间是否有障碍物，直到检查到障碍物或历遍所有节点；

s403：若果检查到障碍物，则将当前节点n和最后节点的上一个节点n+k连接起来；如果历遍所有节点都没检查到障碍物，则将当前节点n和最后节点p之间连接起来；

s404：用线性插值取代上述已连接的两个节点之间的不规则的点；

s405：以节点n+k+1作为当前节点重复步骤s401-s404，直到路径中所有的节点遍历结束算法收敛。

其中，线性插值的计算公式为所述线性插值的计算公式为：

其中：(x0,y0)和(x1,y1)分别为连接的两个节点的坐标，(x,y)为所得的插值点坐标。

图6是本发明实施例的未平滑处理的路径图，图7是本发明实施例的平滑处理后的路径图。其中，a是未平滑处理的路径，b是平滑处理后的路径。由图6可知，在引入安全代价后，使得路径远离了障碍物，但是依然存在折弯多、不平滑、离散等问题，移动单元无法直接跟踪该路径。根据图7，可以看出该路径是连续光滑的，移动单元可以直接跟踪该路径。

本发明所属技术领域的技术人员可以对所描述的具体实施例做各种各样的修改或补充或采用类似的方式替代，但并不会偏离本发明的精神或者超越所附权利要求书所定义的范围。