结合地图栅格与势场法避障的巡检机器人路径规划方法与流程

本发明属于巡检机器人避障路径规划的技术领域，尤其涉及一种结合地图栅格与势场法避障的巡检机器人路径规划方法。

背景技术：

巡检机器人是移动机器人的一种，是现代变电站系统中的重要的巡检设备，主要用来代替人来监测变电站中的各类仪器仪表。巡检机器人的避障是机器人路径规划中至关重要的一部分,障碍物在环境中的不同分布情况直接影响到规划的路径。避障算法的选择直接关系到路径规划的效果。

在实际中，机器人的所处环境大多数情况下既不是完全已知的也不是完全未知的，而是部分已知，部分未知的。针对这种情况，现有技术利用全局环境信息，先进行全局路径规划得到一条从起始点到目标点的较优路径，然后当机器人在实际向目标点运动当中，针对未知的静态或者动态障碍，再选择合适的局部避障方法绕开障碍。

在现实中,机器人所遇到的障碍物可能是静态的或动态的，若遇到的障碍物是动态的，那么沿用一套参数固定的排斥势函数，那么可能造成在障碍物附近路径规划效率不高，导致了机器人巡检时间的加长。

技术实现要素：
：

本发明要解决的技术问题是：提供一种结合地图栅格与势场法避障的巡检机器人路径规划方法，以解决现有技术机器人巡检路径规划存在的路径规划效率不高，导致了机器人巡检时间的加长等技术问题。

本发明技术方案：

一种结合地图栅格与势场法避障的巡检机器人路径规划方法，它

包括：

步骤1.1、全局栅格法路径规划：利用栅格法得到一条从起始点到目标点的全局最优路径；

步骤1.2、向目标点运动：判断是否遇到障碍物，无则继续向目标点运动，有则进行局部避障路径规划，直到达到目标点；

步骤1.3、局部避障路径规划、运用参数可变的人工势场法进行避障路径规划，并判断是否到达目标点，无则继续返回步骤2，到达则结束。

步骤1所述利用栅格法得到一条从起始点到目标点的全局最优路径的方法包括：

步骤2.1、栅格化场地，确定目标点、障碍物和机器人自身所在栅格在全局地图中的坐标；

步骤2.2、初始化栅格，将有障碍物的栅格标为1，将自由栅格标为0；

步骤2.3、搜索机器人相邻栅格，选出自由栅格，定义评价函数h(Grid[i,j])用来计算此栅格中心点与目标栅格中心点的距离：Grid[i,j]为机器人相邻的栅格的坐标，goal代表目标点栅格的坐标；

步骤2.4、比较各栅格的h(Grid[i,j])值，选出使h(Grid[i,j])最小的那个栅格Grid[i,j]_min,将Grid[i,j]_min栅格作为机器人下一步要运动至的栅格；

步骤2.5、判断机器人是否到达目标点,未到达则转至步骤1.3，到达则任务完成，进入下一个任务。

步骤3所述的运用参数可变的人工势场法进行避障路径规划的方法包括：

步骤3.1、参数初始化，初始化势场函数的参数，所述势场函数包括排斥势函数和引力势函数；

步骤3.2、通过全局激光雷达判断所遇到的障碍物是否运动，无则直接根据初始化的参数计算斥力与引力，并直接跳转到步骤3.4，有则通过全局激光雷达获取障碍物的运动矢量与相对机器人的位置；

步骤3.3、根据障碍物的的运动矢量相对机器人的位置，调节势场函数的参数，并计算机器人所受斥力矢量和引力矢量；

步骤3.4、判断机器人所受引力和斥力的方向，相反则机器人往引力的垂直方向上运动距离d以避免机器人进入极值点；

步骤3.5、计算机器人所受势场合力矢量，控制机器人沿合力方向运动。

步骤3.1所述初始化势场函数的参数，所述参数包括：机器人此时位置与激光雷达所观测到的障碍物之间的距离d1，斥力加权系数k1，机器人此时位置与目标位置点之间的距离d2，引力加权系数k2。

所述排斥势函数表达式为：

式中：d₁为机器人测量点与障碍物的距离，d_max为势场作用的最大范围，k₁为加权系数；

机器人受的斥力计算公式为：

斥力的角度为：

所述引力势函数表达式为：

U_a＝k₂s

式中：k₂为加权系数，s为机器人离目标点的距离；

机器人受到的引力数值为：F_a＝k₂；

引力的角度为：

式中：(x_obst,y_obst)为障碍物在全局地图中的坐标，(x_robot,y_robot)为机器人在全局地图中的坐标。

所述计算机器人所受势场合力矢量的计算公式为：

合力的大小：

合力的方向：

式中：F_r为斥力、F_a为引力，F_rx为F_r在x轴方向的分力、F_ry为F_r在y轴方向的分力；F_ax为引力F_a在x轴方向的分力、F_ay为引力F_a在y轴方向的分力。

本发明的有益效果：

本发明的全局路径规划应用栅格法，将已知地图信息栅格化，可以提高路径规划的有效性。

本发明局部避障路径规划采用改进了的参数可变的人工势场法，此规划方法规划的路径距离短，避障快速，能够准确的到达目标点，有效的解决了动态障碍物的路径规划问题，提高机器人在动态环境中的巡检任务完成效率；解决了现有技术机器人巡检路径规划存在的路径规划效率不高，导致了机器人巡检时间的加长等技术问题。

附图说明

图1为本发明的整体流程图；

图2为本发明的全局路径规划程序流程图；

图3为本发明的局部避障路径规划程序流程图。

具体实施方式：

一种结合地图栅格与势场法避障的巡检机器人路径规划方法，它包括：

步骤1、全局栅格法路径规划：利用栅格法得到一条从起始点到目标点的全局最优路径；

步骤2、向目标点运动：判断是否遇到障碍物，无则继续向目标点运动，有则进行局部避障路径规划，直到达到目标点；

步骤3、局部避障路径规划、运用参数可变的人工势场法进行避障路径规划，并判断是否到达目标点，无则继续返回步骤2，到达则结束。

步骤1所述全局栅格法路径规划，包括以下步骤：

(1)栅格化场地，确定目标点、障碍物和机器人自身所在栅格在全局地图中的坐标；

(2)初始化栅格，将有障碍物的栅格标为1，将自由栅格标为0；

(3)搜索机器人相邻栅格，选出自由栅格，定义评价函数h(Grid[i,j])用来计算此栅格中心点与目标栅格中心点的距离：，Grid[i,j]为与机器人相邻的栅格的坐标，goal代表目标点栅格的坐标；

(4)比较各栅格的h(Grid[i,j])值，选出使h(Grid[i,j])最小的那个栅格Grid[i,j]_min,将Grid[i,j]_min栅格作为机器人下一步要运动至的栅格；

(5)判断机器人是否到达目标点,如果没有达到，则转至第三步(3)，如果到达，则任务完成，机器人进入下一个任务。

2.局部避障路径规划，包括以下步骤：

(1)参数初始化，初始化势场函数的以下参数，d1:机器人此时位置与激光雷达所观测到的障碍物之间的距离，k1：斥力加权系数，暂设为0.5，调试过程中根据避障效果进行具体调节，d2:机器人此时位置与目标位置点之间的距离，k2:引力加权系数，暂设为0.5，调试过程中根据避障效果具体调节；

势场函数的意义是空间中点与点之间相互作用的数学抽象。此发明中的势场函数即为障碍物或者目标点对机器人的作用力的数值表示，势场函数包括排斥势函数和引力势函数，其中，障碍物对机器人的作用力的数值表示采用排斥势函数的数值结果，合力方向与机器人运动方向相反，表现为阻碍作用；目标点对机器人的作用力数值表示采用引力势函数的数值结果，合力方向与机器人运动方向一致，表现为吸引作用。后文所述势场函数、引力势函数、排斥势函数与此处概念一样，不再赘述。

(2)通过全局激光雷达连续采集几帧图像来判断所遇到的障碍物是否运动，障碍没有运行则直接根据初始化的参数计算斥力与引力，并直接跳到(4)，否则通过激光雷达的角度和距离信息来获取障碍物的运动矢量与相对机器人的位置；

(3)根据障碍物的的运动矢量与相对机器人的位置，调节排斥势函数的参数，调试阶段可以采用人工调节方式，实际运行中可以根据人工调节过程中的经验给排斥势函数的参数，建立相应的参数表，根据障碍物与机器人之间的距离自动选取相关参数进行调节，并计算机器人所受斥力矢量和引力矢量；

(4)判断机器人所受引力和斥力的方向是否正好相反，若正好相反，则让机器人往引力的垂直方向上运动一段距离L(L根据障碍物大小以及机器人尺寸综合确定，一般取机器人外接圆的直径大小即可，即可理解为一个“身位”)来避开机器人可能进入的极值点；

(5)计算机器人所受势场合力矢量，控制机器人沿合力方向缓慢运动。

下面将结合附图以及具体实施例来详细说明本发明技术方案：

实施例1

结合图1，图1为本发明的整体流程图。首先进行全局路径规划，运用栅格法得到一条从起始点到目标点的全局较优路径；根据这条较优路径向目标点运动，判断是否遇到障碍物，无则继续向目标点运动，否则进行局部避障路径规划；局部避障路径规划，运用参数可变的人工势场法进行避障路径规划，判断是否到达目标点，无则继续向目标点运动，否则结束。

实施例2

结合图2，图2为本发明的全局路径规划程序流程图。首先栅格化场地，确定目标点、障碍物和机器人自身所在栅格；初始化栅格，将有障碍物的栅格标为1，将自由栅格标为0；搜索机器人相邻栅格，选出自由栅格，定义评价函数h(Grid[i,j])用来计算此栅格中心点与目标栅格中心点的距离：Grid[i,j]为与机器人所在栅格相邻的栅格的坐标，goal代表目标点栅格坐标；比较各栅格的h(Grid[i,j])值，选出使h(Grid[i,j])最小的那个栅格Grid[i,j]_min,将Grid[i,j]_min栅格作为机器人下一步要运动至的栅格；判断机器人是否到达目标点,如果没有达到，则继续搜索机器人相邻栅格，如果到达，则任务完成，机器人进入下一个任务。

实施例3

结合图3，图3为本发明的局部避障路径规划程序流程图。首先初始化势场函数的参数，d1:机器人此时位置与激光雷达所观测到的障碍物之间的距离，k1：斥力加权系数，暂设为0.5，调试过程中根据避障效果进行具体调节，d2:机器人此时位置与目标位置点之间的距离，k2:引力加权系数，暂设为0.5，调试过程中根据避障效果进行具体调节；然后通过全局激光雷达传感器判断所遇到的障碍物是否运动，无则直接根据初始化的参数计算斥力与引力，否则通过激光雷达传感器获取障碍物的运动矢量与相对机器人的位置，并据此首先人工调节排斥势函数的参数来计算机器人所受斥力矢量和引力矢量，实际运行时的参数是通过人工建立参数表来进行自动的选取和调节；判断机器人所受引力和斥力的方向是否相反，若相反，则让机器人往引力的垂直方向上运动一段距离d(d根据障碍物大小以及机器人尺寸综合确定，一般取机器人外接圆直径)来避开机器人可能进入的极值点；计算机器人所受势场合力矢量，机器人沿合力方向运动距离i，i的大小根据机器人尺寸和障碍物大小综合确定。

1.排斥势函数的选取：

在势场中，由障碍物产生的势场对机器人产生排斥作用，且距离越近，排斥作用应越大，即机器人与障碍物越近，说明机器人具有的势能越大，反之就越小。这种势场与电势场相似，势能与距离成反比，因此排斥势函数可取为：

其中d₁为为机器人测量点与障碍物的距离，d_max为势场作用的最大范围，k₁为加权系数。

机器人受的斥力为：

斥力的角度为：

即斥力的方向为背离障碍物。当d₁→0时，F_r→∞。即机器人与障碍物相碰时，受到的排斥力为无穷大。若要避免机器人与障碍物相碰，可设一个最小的安全距离d₀，当d₁→d₀时，F_r→∞。(x_obst,y_obst)为障碍物在全局地图中的坐标，(x_robot,y_robot)为机器人在全局地图中的坐标为使Fr连续，Fr可修改为：

d₀，d_max的大小取决于机器人的大小、速度以及环境中障碍物的稀疏程度等。

但在实际中,机器人所遇到的障碍物可能是静态的或动态的，若遇到的障碍物是动态的，那么沿用一套参数固定的排斥势函数，那么可能造成在障碍物附近路径规划效率不高，导致了机器人巡检时间的加长。所以针对此问题通过采用带有可调参数的排斥势函数,对障碍物不同的运动情况调节排斥势场的强度,能有效地解决移动机器人在有动态障碍物的环境中的路径规划问题，提高了规划效率。

为了解决动态障碍物的路径规划问题，将障碍物的速度矢量和与机器人的相对位置关系引入到人工势场中,应用基于矢量合成的方法可以较好地解决上述的缺陷。所以可重新定义排斥势函数为:

Ur＝Q Ur

Ur为传统的排斥势函数，Q为可调参数，其中Q取决于障碍物的速度矢量和与机器人的相对位置，同样采取先人工调试，再建相应参数表的调节方式。

所以斥力Fr修改为：

这样,根据障碍物的速度矢量和相对位置,调节排斥势函数的大小,以达到更好的避障效果,这可以看成是一种动态人工势场法。

2.引力势函数的选取：

目标点对机器人的势函数同样是基于距离的概念。目标点距机器人距离越远，吸引作用越大，即机器人距目标点越远，所具有的势能就越大；反之，距离越近，势能越小。当距离为零时，机器人的势能为零，此时机器人到达终点。这种性质和重力势能相似。

故引力势函数可取为：

U_a＝k₂s，

其中k₂为加权系数，s为机器人离目标点的距离。机器人受到的引力数值大小为：F_a＝k₂，引力的角度为：

(x_obst,y_obst)为障碍物在全局地图中的坐标，(x_robot,y_robot)为机器人在全局地图中的坐标，即引力的方向是指向目标点的。由引力势场函数可知引力为一个常值，在实际应用过程中可以通过调节参数来选取一个合适的引力值。

3.合力的计算：

机器人所受的合力为斥力与引力的矢量叠加，合力的方向为机器人运动方向。机器人在合力的作用下可以绕过障碍物到达终点。具体计算方法如下：

1)计算斥力F_r和引力F_a；

2)分解斥力和引力，分别计算斥力F_r在x轴和y轴方向的分力F_rx、F_ry和引力F_a在x轴和y轴方向的分力F_ax、F_ay；

3)计算合力的大小：

4)计算合力的方向：