CN111857149B - 一种a*算法与d*算法复合的自主路径规划方法 - Google Patents

Abstract

本发明公开了一种A*算法与D*算法复合的自主路径规划方法，其步骤包括：1、通过A*算法生成AGV当前位置到目标位置的全局最优路径；2、当AGV在全局路径行驶遇到障碍物时，根据障碍物和全局路径的相对位置关系，确定AGV绕开障碍物的局部搜索范围；3、结合局部路径搜索范围和D*算法，将生成的局部避障路径拼接至已有全局路径，完成了路径拼接，以实现动态障碍场景下AGV到达目标位置时的全局路径更新。本发明能将A*算法和D*算法结合规划路径，从而能实现AGV在工业现场的动态规划路径。

Description

一种A*算法与D*算法复合的自主路径规划方法

技术领域

本发明属于自动导引车路径规划领域，具体的说是一种A*算法与D*算法复合的自主路径规划方法。

背景技术

如今，工厂逐渐转向智能化的发展趋势愈加明显，AGV(Automated GuidedVehicle)成为众多学者研究的热点课题。路径规划作为AGV智能化的核心部分，目前主要有两种规划方式：基于环境已知的全局路径规划和基于环境未知的局部路径规划方法。在这两种类型的规划方法中，基于环境已知的全局路径规划对环境变化的适应性较低，无法处理工业现场的突发状况；基于环境未知的局部路径规划方法对某些工业现场变化少的情况效率较低，无法及时处理常规情景。

发明内容

本发明是为了解决上述现有技术存在的不足之处，提出一种A*算法与D*算法复合的自主路径规划方法，以期能结合A*算法在全局路径规划上的优势和D*算法在局部路径规划上的优势，从而提高了工业现场的路径规划的效率和对不同工业现场的适应性。

本发明为达到上述发明目的，采用如下技术方案：

本发明一种A*算法与D*算法复合的自主路径规划方法的特点是按如下步骤进行：

步骤1：以试验场地的外接矩形的任意一个顶角作为原点o，以所述原点o相连的两条边分别为x轴和y轴，从而建立直角坐标系oxy；

在所述直角坐标系oxy中，将所述试验场地划分成M×N的栅格地图

其中，ST_n表示所述栅格地图A_M×N上坐标为(n_x,n_y)的站点Ld_n的状态，若ST_n＝0表示所述站点Ld_n为空闲点，若ST_n＝1表示所述站点Ld_n为障碍点M表示所述栅格地图A_M×N的横向栅格数，N表示栅格地图A_M×N的纵向栅格数；

在栅格地图A_M×N中设置坐标为(f_x,f_y)的起始点Ld_f，设置坐标为(m_x,m_y)的目标点Ld_m，起始点Ld_f与目标点Ld_m的相对距离大于两个站点的距离；以Y轴正方向作为所述起始点Ld_f的路径搜索方向；

步骤2:根据起始点Ld_f与目标点Ld_m，利用式(1)所示的A*算法的启发式函数搜索并生成全局路径L₁＝{Ld_f,Ld_f+1,…,Ld_d,…,Ld_m}，其中，Ld_d表示栅格地图A_M×N中坐标为(d_x,d_y)的站点，所述站点Ld_d的状态为ST_d，且ST_d∈{0,1}；

f(d_x,d_y)＝g(d_x,d_y)+h(d_x,d_y)(1)

式(1)中：f(d_x,d_y)表示坐标为(d_x,d_y)的站点Ld_d的估价函数值；g(d_x,d_y)表示从起始点Ld_f到站点Ld_d的实际的代价；h(d_x,d_y)表示从站点Ld_d到目标点Ld_m的预估代价值；

步骤3：根据所述全局路径L₁，令AGV小车的初始位置为坐标为(f_x,f_y)的起始点Ld_f，假设所述AGV小车的当前位置为坐标为(i_x,i_y)的当前站点Ld_i，并记录当前站点Ld_i在全局路径L₁中的序号为n_i；

步骤4：初始化i＝f；n_i＝0；

步骤5：所述AGV小车根据所述全局路径L₁行驶到当前站点Ld_i，并推算出所述AGV小车的当前位置的目标站点为坐标为(j_x,j_y)的站点Ld_j；

遍历所述栅格地图A_M×N的状态获得站点Ld_j的状态值为ST_j；

判断i＝f是否成立，若成立，则根据当前站点Ld_i在全局路径L₁中的序号n_i，令站点Ld_j等于全局路径L₁中序号为n_i+1的站点；否则，令站点Ld_j等于全局路径L₁中序号为n_i+2的站点；

步骤6：判断站点Ld_j的状态ST_j，若ST_j＝0，则跳转至步骤9执行，若ST_j＝1，则顺序执行步骤7；

步骤7：根据当前站点Ld_i在全局路径L₁中的序号n_i，获得序号为n_i+1的站点Ld_r以及序号为n_i+3的站点Ld_s；

调用D*算法从站点Ld_s向站点Ld_r搜索，利用式(2)计算各个站点到站点Ld_s的实际估价函数值h(s_x,s_y)，从而根据所述实际估价函数值h(s_x,s_y)得到站点Ld_s到站点Ld_r的局部路径L₂＝{Ld_r,Ld_r+1,…,Ld_e,…,Ld_s}，其中，Ld_e代表所述栅格地图A_M×N中坐标为(e_x,e_y)的站点；

h(s_x,s_y)＝c((s_x,s_y),f(s_x,s_y))+f(s_x,s_y) (2)

式(2)中：h(s_x,s_y)表示前站点Ld_i在到站点Ld_s的实际估价函数值，c((s_x,s_y),f(s_x,s_y))表示当前点Ld_i到下一站点的新加权值；

步骤8：将所述局部路径L₂和全局路径L₁按照拼接规则进行拼接后形成新的路径{Ld_f,Ld_f+1,…,Ld_r,Ld_r+1,…,Ld_e,...,Ld_s,...,Ld_m}并赋值给全局路径L₁；

所述拼接规则为：将所述全局路径L₁中的序号为n_i+1,n_i+2,n_i+3的三个连续站点替换为所述局部路径L₂；

步骤9：将n_i+1赋给n_i，将i+1赋给i后，判断i＞m是否成立，若成立，则表示路径规划完成，且所述AGV小车行驶完整条路径L₁，否则返回步骤5顺序执行。

与现有技术相比，本发明的有益效果在于：

1.本发明通过将A*和D*算法相结合的方式，将A*算法和D*算法的估价值加权，形成新的估价值。在动态环境下调用A*算法，在未知的环境下调用D*算法并结合A*算法的方式解决了现有技术中在存在动态环境和静态环境的复合环境下的效率低下和难以自适应的问题，从而可以有效处理固定工业环境下的路径规划时间长和不同工业现场算法难以自适应的问题。

2、本发明通过使用A*算法提高了对于动态环境处理的高效性，从而提高了路径规划的柔性，降低了工业现场对于处理设备高性能的要求。

3.本发明通过将A*算法和D*算法的估价值加权，并形成新的估价值，有效的提高了对于静态环境的路径规划的时效性、自适应性、高效性。

附图说明

图1为本发明根据自动化仓库的外接矩形确定的栅格地图；

图2为本发明根据A*算法的启发式函数搜索并生成的全局路径图。

图3为本发明在遇到动态环境时根据A*和D*算法的启发式函数搜索并生成的局部路径图。

具体实施方式

本实例中以三相叉AGV为例，自重5t，正常行驶速度为1.5m/s，急刹车以及制动时间t约为2s。是按如下过程实现A*算法与D*算法复合的自主路径规划方法：1.根据试验场地的外接矩形确定栅格地图的大小；2.根据试验场地的环境情况确定栅格地图的所有站点的状态。3.根据A*算法计算得到适合此时已经环境的最短全局路径。4.在遇到动态环境时调用D*算法和A*算法的加权方法计算得到局部路径。5.将局部路径和全局路径以拼接规则拼接，并取代之前的全局路径。具体地说，该自主路径规划方法，是应用于由调度中心、若干个固定式货架和AGV小车组成的自动化仓库中。如图1所示，在整个自动化仓库内，以矩阵的形式等距离L＝5m的方式按x方向和y方向划分成栅格地图。栅格地图的每个站点都包含坐标属性和状态属性。坐标属性值代表了该站点相较于整个自动化仓库的相对位置，状态属性值代表了自动化仓库中该位置的是否可通行状态。并按如下过程进行：

步骤1：如图1所示，以自动化仓库的外接矩形的任意一个顶角作为原点o，以原点o相连的两条边分别为x轴和y轴，从而建立直角坐标系oxy；

在直角坐标系oxy中，将试验场地划分成M×N的栅格地图

其中，ST_n表示栅格地图A_M×N上坐标为(n_x,n_y)的站点Ld_n的状态，若ST_n＝0表示站点Ld_n为空闲点，若ST_n＝1表示站点Ld_n为障碍点。M表示栅格地图A_M×N的横向栅格数，N表示栅格地图A_M×N的纵向栅格数，全图共M×N个站点；

AGV小车接收到调度中心发送的任务，任务内容：将货物从坐标为(f_x,f_y)的取货点Ld_f送到坐标为(m_x,m_y)的货架站点Ld_m。起始点Ld_f与目标点Ld_m的相对距离大于两个站点的距离，仅有两个站点的路径规划空间太小，无法给AGV小车提供足够的应急反应时间；以AGV小车的当前车头方向作为起始点Ld_f的路径搜索方向；

步骤2:根据取货点Ld_f与货架站点Ld_m，利用式(1)所示的A*算法的启发式函数搜索并生成如图2所示的全局路径L₁＝{Ld_f,Ld_f+1,…,Ld_d,…,Ld_m}，其中，Ld_d表示栅格地图A_M×N中坐标为(d_x,d_y)的站点，站点Ld_d的状态为ST_d，且ST_d∈{0,1}；此时的路径为针对静态环境下时生成的全局路径。A*算法为一种静态路网中求解最短路最有效的直接搜索方法。采用A*算法简洁的启发式函数可有效地减少AGV小车在现场取货点的等待时间，提高了工业节拍，从而减少了现场的AGV小车的数量，为自动化仓库提高了单台设备的效率并节约了成本。

f(d_x,d_y)＝g(d_x,d_y)+h(d_x,d_y)(1)

式(1)中：f(d_x,d_y)表示坐标为(d_x,d_y)的站点Ld_d的估价函数值；g(d_x,d_y)表示从起始点Ld_f到站点Ld_d的实际的代价；h(d_x,d_y)表示从站点Ld_d到目标点Ld_m的预估代价值；

步骤3：AGV小车的初始位置为坐标为(f_x,f_y)的取货点Ld_f，AGV小车在自动化现场的起点一般为任务的入库点或者出库点，一方面方便在路径规划初始时不会出现其他AGV小车挡路等静态环境阻碍，另一方面AGV小车正常的行驶路线为取货点和货架来回执行任务，可大大的提高工作效率。如图2所示，AGV小车的当前位置为坐标为(i_x,i_y)的当前站点Ld_i，并记录当前站点Ld_i在全局路径L₁中的序号为n_i。由于工业现场可能出现动态环境，比如全局路径中的某个站点突然出现障碍物，后续的处理会改变全局路径，全局路径的总站数也会发生改变，因此n_i只代表AGV行驶到该站点时此站点在全局路径中的排序。由于全局路径的站点总数是个变量值，因此在判定路径是否行驶完的标志不可用n_i来判定。而i为当前站点Ld_i的下标，在栅格地图A_M×N中存在且唯一，所以以当前站点的下标与终点Ld_m一致为路径行驶结束的标志；

步骤4：初始化i＝f；n_i＝0；

步骤5：AGV小车根据全局路径L₁行驶到当前站点Ld_i，并推算出AGV小车的当前位置的目标站点为坐标为(j_x,j_y)的站点Ld_j；

遍历栅格地图A_M×N的状态获得站点Ld_j的状态值为ST_j；

判断i＝f是否成立，若成立，则根据当前站点Ld_i在全局路径L₁中的序号n_i，令站点Ld_j等于全局路径L₁中序号为n_i+1的站点，这是为了防止起点在全局路径L₁的下一个站点为障碍点，AGV小车仅仅距离一个站点，无法有效避开障碍点，AGV的小车速度v为1.5m/s，每个站点为5m，发现障碍和系统处理时间约为0.5s，急刹车以及制动时间t约为2s，此时AGV小车的制动距离为S＝v×t＝1.5×2.5＝3.75m，距离下个站点仅有1.25m，远远小于安全距离2m。AGV的车型比较大，过短的安全距离容易出现安全事故；否则，令站点Ld_j等于全局路径L₁中序号为n_i+2的站点，全局路径L₁中序号为n_i+1站点为发现障碍点时的应急缓冲站点；

步骤6：判断站点Ld_j的状态ST_j，若ST_j＝0，则跳转至步骤9执行，若ST_j＝1，则顺序执行步骤7；

步骤7：根据当前站点Ld_i在全局路径L₁中的序号n_i，获得序号为n_i+1的如图3所示的站点Ld_r以及序号为n_i+3的站点Ld_s；

调用D*算法从站点Ld_s向站点Ld_r搜索，利用式(2)计算各个站点到站点Ld_s的实际估价函数值h(s_x,s_y)，从而根据实际估价函数值h(s_x,s_y)得到站点Ld_s到站点Ld_r的如图3所示的局部路径L₂＝{Ld_r,Ld_r+1,…,Ld_e,…,Ld_s}，其中，Ld_e代表栅格地图A_M×N中坐标为(e_x,e_y)的站点，路径L₂将绕过障碍点序号为n_i+2的站点Ld_j；D*算法在含障碍点环境中寻路非常有效，向目标点移动中，只检查最短路径上下一个站点的变化情况，如机器人寻路等情况。对于距离远的全局路径规划，则耗时过长，效率低下。此处使用A*与D*的加权估价函数值，减少了计算的复杂度，从而减少了对工业现场计算设备的高性能要求，降低了实施成本，也提高了计算的效率，对自动化仓库常发生的多车抢占资源也有一定的缓解，间接的节约调度中心的资源耗费。

h(s_x,s_y)＝c((s_x,s_y),f(s_x,s_y))+f(s_x,s_y)(2)

式(2)中：h(s_x,s_y)表示前站点Ld_i在到站点Ld_s的实际估价函数值，c((s_x,s_y),f(s_x,s_y))表示当前点Ld_i到下一站点的新加权值；

步骤8：将局部路径L₂和全局路径L₁按照拼接规则进行拼接后形成新的路径{Ld_f,Ld_f+1,…,Ld_r,Ld_r+1,…,Ld_e,...,Ld_s,...,Ld_m}并赋值给如图3所示的全局路径L₁；

该拼接规则为：将全局路径L₁中的序号为n_i+1,n_i+2,n_i+3的三个连续站点替换为局部路径L₂；

步骤9：将n_i+1赋给n_i，将i+1赋给i后，判断i＞m是否成立，若成立，则表示路径规划完成，且AGV小车行驶完整条路径L₁，任务完成后上报调度中心货物安全送达，等待下一条任务，若是没有任务则在取货点等待。否则返回步骤5顺序执行。

Claims (1)

1.一种A*算法与D*算法复合的自主路径规划方法，其特征是按如下步骤进行：

步骤1：以试验场地的外接矩形的任意一个顶角作为原点o，以所述原点o相连的两条边分别为x轴和y轴，从而建立直角坐标系oxy；

在所述直角坐标系oxy中，将所述试验场地划分成M×N的栅格地图

其中，ST_n表示所述栅格地图A_M×N上坐标为(n_x,n_y)的站点Ld_n的状态，若ST_n＝0表示所述站点Ld_n为空闲点，若ST_n＝1表示所述站点Ld_n为障碍点M表示所述栅格地图A_M×N的横向栅格数，N表示栅格地图A_M×N的纵向栅格数；

在栅格地图A_M×N中设置坐标为(f_x,f_y)的起始点Ld_f，设置坐标为(m_x,m_y)的目标点Ld_m，起始点Ld_f与目标点Ld_m的相对距离大于两个站点的距离；以Y轴正方向作为所述起始点Ld_f的路径搜索方向；

步骤2:根据起始点Ld_f与目标点Ld_m，利用式(1)所示的A*算法的启发式函数搜索并生成全局路径L₁＝{Ld_f,Ld_f+1,…,Ld_d,…,Ld_m}，其中，Ld_d表示栅格地图A_M×N中坐标为(d_x,d_y)的站点，所述站点Ld_d的状态为ST_d，且ST_d∈{0,1}；

f(d_x,d_y)＝g(d_x,d_y)+h(d_x,d_y) (1)

式(1)中：f(d_x,d_y)表示坐标为(d_x,d_y)的站点Ld_d的估价函数值；g(d_x,d_y)表示从起始点Ld_f到站点Ld_d的实际的代价；h(d_x,d_y)表示从站点Ld_d到目标点Ld_m的预估代价值；

步骤3：根据所述全局路径L₁，令AGV小车的初始位置为坐标为(f_x,f_y)的起始点Ld_f，假设所述AGV小车的当前位置为坐标为(i_x,i_y)的当前站点Ld_i，并记录当前站点Ld_i在全局路径L₁中的序号为n_i；

步骤4：初始化i＝f；n_i＝0；

步骤5：所述AGV小车根据所述全局路径L₁行驶到当前站点Ld_i，并推算出所述AGV小车的当前位置的目标站点为坐标为(j_x,j_y)的站点Ld_j；

遍历所述栅格地图A_M×N的状态获得站点Ld_j的状态值为ST_j；

判断i＝f是否成立，若成立，则根据当前站点Ld_i在全局路径L₁中的序号n_i，令站点Ld_j等于全局路径L₁中序号为n_i+1的站点；否则，令站点Ld_j等于全局路径L₁中序号为n_i+2的站点；

步骤6：判断站点Ld_j的状态ST_j，若ST_j＝0，则跳转至步骤9执行，若ST_j＝1，则顺序执行步骤7；

步骤7：根据当前站点Ld_i在全局路径L₁中的序号n_i，获得序号为n_i+1的站点Ld_r以及序号为n_i+3的站点Ld_s；

调用D*算法从站点Ld_s向站点Ld_r搜索，利用式(2)计算各个站点到站点Ld_s的实际估价函数值h(s_x,s_y)，从而根据所述实际估价函数值h(s_x,s_y)得到站点Ld_s到站点Ld_r的局部路径L₂＝{Ld_r,Ld_r+1,…,Ld_e,…,Ld_s}，其中，Ld_e代表所述栅格地图A_M×N中坐标为(e_x,e_y)的站点；

h(s_x,s_y)＝c((s_x,s_y),f(s_x,s_y))+f(s_x,s_y) (2)

式(2)中：h(s_x,s_y)表示前站点Ld_i在到站点Ld_s的实际估价函数值，c((s_x,s_y),f(s_x,s_y))表示当前点Ld_i到下一站点的新加权值；

步骤8：将所述局部路径L₂和全局路径L₁按照拼接规则进行拼接后形成新的路径{Ld_f,Ld_f+1,…,Ld_r,Ld_r+1,…,Ld_e,...,Ld_s,...,Ld_m}并赋值给全局路径L₁；

所述拼接规则为：将所述全局路径L₁中的序号为n_i+1,n_i+2,n_i+3的三个连续站点替换为所述局部路径L₂；

步骤9：将n_i+1赋给n_i，将i+1赋给i后，判断i＞m是否成立，若成立，则表示路径规划完成，且所述AGV小车行驶完整条路径L₁，否则返回步骤5顺序执行。

Images