基于A*提取引导点的AGV路径跟踪与避障协调方法与流程

本发明涉及移动机器人导航，尤其是涉及到一种基于A*提取引导点的AGV路径跟踪与避障协调方法。

背景技术：

AGV(Automated Guided Vehicle)是装有自动导引装置，能够沿规定的路径行驶，在车体上具有编程和停车选择装置、安全保护装置以及各种物料移载功能的搬运车辆。其传统的导航方法主要有磁条导引、色带导引、磁钉导引等，仍然简单易行、路径跟踪可靠性好，但均属于固定路径引导方式，灵活性差。新的导航方式，如惯性导航、激光导航等，无需固定路线导引，定位系统具有更高的引导柔性，能够更高效、灵活地完成物料的搬运任务，但是面临着相对复杂的路径规划、路径跟踪与与障碍避让等问题。

A*算法是路径规划算法中在静态地图中搜索最短路径最直接有效的方法，然而用传统的A*算法规划出的最优路径常常与障碍物紧挨，AGV路径跟踪时缺少缓冲地带，尤其在拐角区域无法及时避免一些潜在的风险。再者，A*算法生成的路径为无相邻的栅格路径节点组成，各个路径节点之间距离很小，机器人在路径跟踪过程中由于运动节点太多、节点之间距离太短，难以实现平顺的路径跟踪控制。

此外，还存在如何兼顾路径跟踪与障碍避让的协调统一问题需要进一步解决。

中国专利CN105737838A公开一种AGV路径跟踪方法，包括以下步骤：(a)在AGV的导航装置内建立路径地图，路径地图包括若干路径点，以及由路径点拟合得出的基础路径曲线；(b)AGV的驱动模块驱动AGV沿基础路径曲线行进；(c)AGV内的修正模块提取当前路径点和下一个路径点，根据当前路径点和下一个路径点拟合出实时路径曲线；(d)AGV的定位模块确定AGV的位置，以当前位置确定导航点，以导航点为圆心建立半径为R的跟踪圆，跟踪圆中，沿AGV行进方向±D范围内的圆弧为有效圆弧，所述有效圆弧与实时路径曲线的交点为行进目标点；(e)AGV修正模块导引AGV朝向行进目标点运行。本发明提供的路径跟踪方法，AGV行走路径直接朝向行进目标点，运行线路短。

技术实现要素：

本发明的目的在于针对现有A*全局路径规划中路径与障碍物之间缺少缓冲地带，无法保证AGV机器人安全通行，全局路径节点多、间距小，难以实现AGV机器人的平顺跟踪等问题，提供可实现路径跟踪与障碍避让协调统一的基于A*提取引导点的AGV路径跟踪与避障协调方法。

本发明包括以下步骤：

1)规划安全全局路径

根据环境信息建立初始栅格地图，在初始栅格地图上，通过风险评估函数R(n)对障碍物周围节点的风险等级进行评估，获得新的带有风险区域的安全栅格地图；

2)对规划得到的全局路径上提取关键路径点；

3)路径跟踪与障碍避让的协调，采用基于激光传感器的动态窗口进行避障，并以关键路径点为引导点并实时更新引导点，进行路径跟踪与障碍避让的协调统一。

在步骤1)中，所述根据环境信息建立初始栅格地图，在初始栅格地图上，通过风险评估函数R(n)对障碍物周围节点的风险等级进行评估，获得新的带有风险区域的安全栅格地图的具体步骤可为：

(1)定义风险评估函数为式中的r表示障碍节点到临近节点的距离，α为风险系数，显然，距离障碍物越近的区域风险等级越高；

(2)将初始栅格地图数据转换为图像数据，通过OpenCV中的cvDiate函数以AGV半径进行膨胀处理，再用cvfilter2D函数进行模板操作，初始栅格地图由障碍格和空白格构成，障碍格节点表示确定有障碍物，灰度值为255；空白格表示完全没有障碍物，灰度值为0；在新得到的安全地图上，障碍栅格节点附近会产生一层灰度值介于0～255并逐级递减的风险栅格点；

修改A*算法的代价函数为：

F(n)＝G(n)+H(n)+R(n)

其中，G(n)表示从起点s到当前节点n的实际代价，H(n)表示从当前节点n到终点g的估计代价(采用曼哈顿距离计算)，R(n)表示当前节点的风险评估值。

在安全地图上，通过A*算法规划全局路径，具体方法如下：

(1)读取起始点s与终点g的坐标值，并且创建两个链表，OPEN表和CLOSED表，将CLOSED表初始化为空表，把起始点s放入OPEN表中。判断OPEN表是否为空表，若为空表则终止程序，若不为空则继续执行；从OPEN表中取出F(n)值最小的节点n作为当前节点，并把n移到CLOSED表中。

(2)判断节点n是否是终点g，若是，则已经找到路径，顺次返回依次来自的节点、到达起始节点s，终止算法、得到一个路径；若不是，则继续下一步判断；

(3)按照上下左右四个方向扩展节点n，将当前的节点n的子节点设为m，对于每一个当前节点n的子节点计算估计值H(m)，其次计算启发式函数值F(n)＝G(n)+H(n)+R(n)。进一步判断如下：

①如果子节点m不在两个链表中，把m点添加到OPEN表中。然后给子节点m一个指向当前节点n的指针；当找到终点g时可以根据这个指针找到每个节点的父节点并以此为根据找回到起始节点s形成路径；

②如果节点m已经在OPEN表中，那么比较F(m)的新值和该节点在OPEN表中的旧值；若新的F(m)比较小，表示找到一条更好的路径，则以此新的F(m)值取代子节点m的旧的F(m)的值；修改子节点m的父指针为当前节点n；若子节点m在CLOSED表中，则跳过该节点，继续寻找其他方向的节点；

(4)重复以上步骤直到找到终点g或者OPEN表为空为止。

在步骤2)中，所述对规划得到的全局路径上提取关键路径点的具体步骤可为：

(1)将终点坐标设为第一个关键点，存放于数组中，并且将这个点记为x0；

(2)第二个节点设为x1，第三个节点设为x2；

(3)判断x2是否是起点s，若是s则结束程序；

(4)判断x0，x1和x2是否共线，如果共线则x1和x2沿路径向前移动一格；

(5)当x0，x1和x2三点不共线时：①判断由x0到x2连线上是否存在空白格节点，若存在，则x1是关键点，将x1计入关键路径点的数组中，且令x0＝x1，当前的x1和x2依次沿路径向前移动一格，跳到(3)继续执行；②若x0到x2间都是空白节点，x0不动，x1和x2沿路径依次向前移动一格，返回(3)进行判断；

(6)找到所有的关键点后，把终点放到关键路径点数组的尾部，则该数组就是完整的最优路径的所有关键路径点。

在步骤3)中，所述进行路径跟踪与障碍避让的协调统一的具体步骤可为：

(1)AGV机器人从起点出发时，以起点后的第一个关键路径为引导点；

(2)使用激光传感器探知周围障碍物信息，若引导点在可通行域内，则直接把引导点作为当前目标点；否则在当前窗口内进行局部路径规划：综合考虑安全性和平稳性，用启发式方法寻找即时子目标；

(3)随着运动和窗口的推进，根据局部信息对规划窗口大小进行动态调整，使得局部避障方法有着良好的环境适应性；

(4)根据传感器信息和AGV当前位姿，将当前引导点切换到后续的关键路径点上，直到引导点变为终点。

在步骤3)第(4)部分中，所述引导点切换的具体方法可为：

将提取得到的关键路径点顺序存放在数组中，为集合{p₁,p₂...p_n}，p_n为终点g。关键点两两相连组成路径段{d_1,2,d_2,3...d_n-1,n}。AGV机器人从起点出发时，引导点为离起点后的第一个关键路径点p₁；AGV机器人移动过程中，从终点向前依次遍历当前引导点p_i之后的所有路径段d_n,n-1,d_n-1,n-2,,,d_i,i+1上各路径点，并计算其相对于机器人的角度β与距离S，通过传感器得到的障碍点数据计算在机器人β方向上的可通行距离S_pass；若在路径上找到一点p在AGV的可通行区域内，即其对应S_pass的大于S，因点p在路径段d_j,j+1上，则把当前引导点p_i切换为p_j+1；若没有在路径上找到这样一点，则当前引导点p_i不变；

同时，设定一个合适的距离r，判断AGV机器人与当前引导点p_i的距离l小于r时，引导点变成p_i+1。重复这样的步骤，直到引导点为p_n。

本发明针对现有A*全局路径规划中路径与障碍物之间缺少缓冲地带，无法保证AGV机器人安全通行，全局路径节点多、间距小，难以实现AGV机器人的平顺跟踪等问题，提供可实现路径跟踪与障碍避让协调统一的基于A*提取引导点的AGV路径跟踪与避障协调方法。基于此原则可以验证无论动态障碍物处于关键路径点前面、后面或者覆盖了关键路径点，AGV机器人都能避开动态障碍物找到合适的引导点，并回到路径上。

附图说明

图1为传统A*算法的路径规划结果。

图2为本发明采用安全栅格地图与改进A*算法的路径规划结果。

图3为本发明所述安全全局路径规划方法的流程图。

图4为实施例中提取关键路径点的初始步骤示意图。

图5为实施例中提取关键路径点的中间步骤示意图。

图6为实施例中提取关键路径点的最终结果示意图。

图7为实施例中AGV机器人跟踪路径时引导点切换方法示意图(图中介绍动态障碍分别在关键路径点前面、上面、后面三种情况下的引导点切换情况)。

图8为实施例中静态环境下的AGV机器人的行走轨迹。

图9为实施例中动态环境下的AGV机器人的行走轨迹。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明。

实施例：本发明实施例中AGV机器人的路径规划、跟踪及避障的方法，具体实施的操作过程如下：

1：根据环境信息建立初始栅格地图map。

2：在初始栅格地图上，通过风险评估函数R(n)对障碍物周围节点的风险等级进行评估，获得新的带有风险区域的安全栅格地图map_r。

2.1)将原始地图数据转换为图像数据，通过OpenCV中的cvDiate函数以AGV半径进行膨胀处理。

2.2)对膨胀后的地图使用cvfilter2D函数进行模板操作。初始的栅格地图由障碍格和空白格构成。障碍格节点表示确定有障碍物，灰度值为255；空白格节点表示完全没有障碍物，灰度值为0；所以通过风险评估函数对初始地图处理，获得一张基于带有风险区域的安全地图。在新得到的安全地图上，障碍格附近会产生一层灰度值介于0-255并逐级递减的风险格。

3：读取起始点s与终点g的坐标值，并且创建两个链表，OPEN表和CLOSED表，将CLOSED表初始化为空表，把起始点s放入OPEN表中。判断OPEN表是否为空表，若为空表则终止程序，若不为空则继续执行。从OPEN表中取出F(n)值最小的节点n作为当前节点，并把n移到CLOSED表中。

4：判断节点n是否是终点g，如果是的话则已经找到路径，顺次返回依次来自的节点、直至起始节点s，终止算法、得到一个路径。若节点n不是终点g，进行下一步的判断。

5：按照上下左右四个方向扩展节点n，将当前的节点n的子节点设为m，对于每一个当前节点n的子节点计算估计值H(m)，并带入计算启发式函数值F(n)＝G(n)+H(n)+R(n)。进一步作如下判断：

5.1)如果子节点m不在两个链表中，把m点添加到OPEN表中。然后给子节点m一个指向当前节点n的指针。当找到终点g时可以根据这个指针找到每个节点的父节点，并以此为根据找回到起始节点s形成路径。

5.2)如果节点m已经在OPEN表中，那么比较F(m)的新值和该节点在OPEN表中的旧值；如果新的F(m)比较小，表示找到一条更好的路径。则以此新的F(m)值取代子节点m的旧的F(m)的值。修改子节点m的父指针为当前节点n；如果子节点m在CLOSED表中则跳过该节点，则继续寻找其他方向的节点。

重复以上步骤直到找到终点g或者OPEN表为空为止(流程如图3所示，得到的路径如图2所示)。得到全局路径后转入以下步骤。

6：对全局路径提取关键路径点，步骤如下：

6.1)将终点坐标设为第一个关键点，存放于数组中，并且将这个点记为x0。

6.2)向前第二个节点设为x1、第三个节点设为x2(如图4所示)。

6.3)判断x2是否是起点s，若是s，则结束程序。

6.4)判断x0、x1和x2是否共线，若共线，则x1和x2继续向前移动。

6.5)当x0、x1和x2三点不共线时，判断由x0到x2连线上是否存在非空白栅格节点，若存在则x1是关键路径点，将x1计入关键路径点数组中，且令x0＝x1，当前的x1和x2依次向前移动，跳到6.3)继续执行；若x0到x2间都是空白节点时，x0不动，x1和x2依次向前移动一格，返回6.3)进行判断。(如图5所示)。

6.6)找到所有的关键点后，把终点放到数组的尾部，则该数组就是完整的全局路径的所有关键路径点。(得到的关键路径点如图6所示)。

7：以关键路径点为引导点，基于激光传感器使用动态窗口法进行局部规划/避障。根据传感器信息和AGV当前状态，采用引导点切换方法。1)如图7所示，当障碍物在当前引导点前面时，机器人通过动态窗口法绕开障碍物，并通过激光传感器找到了当前引导点后面的路径点，则把引导点切换到该路径点后面的关键路径点上；2)如图7所示，当动态障碍物覆盖掉当前引导点时，机器人进入到距离引导点r的范围内，则把引导点切换到后一关键路径点上，或者同前面一样在绕开障碍物过程中找到后续路径点则切换引导点；3)如图7所示，当动态障碍物在当前引导点后面时，机器人进入到距离引导点r的范围内，则把引导点切换到后一关键路径点上。不断地将当前引导点切换到后续的关键路径点上，直到引导点变为终点。

1.静态环境下的实验结果

在静态环境中，依照上述实施例操作过程，得到的AGV机器人路径规划及跟踪路径的结果如图8所示，图中静态障碍物周围有一层风险区域，平直的路径为规划得到的全局路径，路径上标出的圆点为关键路径点，较光滑的路径为AGV机器人的行走轨迹。

2.动态环境下的实验结果

为验证在多种动态障碍物下，障碍物的大小、形状和位置对路径跟踪的性能影响。在动态环境中，在上述实施例中的地图上不同位置设置了不同形状的动态障碍物：得到的AGV机器人路径规划及跟踪路径的结果如图9所示。

由如上所述实施例的实验结果可以看出，利用改进的安全A*算法，能够得到从起点到终点的风险值低的安全全局路径；不管是静态还是动态障碍物，只要没有完全堵死通道，且终点上没有障碍物，AGV机器人就能一直找到合适的引导点进行跟踪与避障，最终到达终点。