CN109387214A - 一种基于虚拟墙的机器人路径规划算法 - Google Patents

Abstract

本发明公开了一种基于虚拟墙的机器人路径规划算法，步骤为：将环境栅格地图文件下载至虚拟墙设置单元，设置虚拟墙，将虚拟墙的位置加入到栅格地图文件中，并上传至机器人；对地图模型进行描述，并利用目前栅格相对于目标栅格的位置关系确定方向权重向量，利用目前栅格与周围栅格的关联程度确定关联矩阵；根据得到的权重值和关联矩阵，结合栅格的位置，计算每一个待选栅格的选择概率，并从中选择概率最大的作为下一个栅格，得到一个初步的路径规划；对初步路径规划进行优化，并重新进行路径规划，直至初步路径规划的优化结果收敛，至此就完成了路径规划。本发明不需要额外的硬件介质，实际操作简单，规划效率、精度高。

Description

一种基于虚拟墙的机器人路径规划算法

技术领域

本发明属于机器人控制领域，具体涉及一种基于虚拟墙的机器人路径规划算法。

背景技术

移动机器人(Robot)是自动执行工作的机器装置，它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动，能够协助或取代人类完成危险的工作，例如在山区变电站这种极端环境下就可以投入移动机器人。因此引起了国内外学者越来越多的重视。机器人的工作环境有需要禁止机器人通过或者进入的地方，因此，在机器人路径规划的过程中，需要考虑虚拟墙的功能。目前，广泛应用于扫地机器人中的虚拟墙，是采用红外传感器发射红外信号，机器人的红外探测器接受红外信号感知虚拟墙来识别工作区域的边界。这种方式需要额外的硬件成本，并且实际操作起来比较复杂。

发明内容

本发明的目的提供一种基于虚拟墙的机器人路径规划算法，不需要额外的硬件介质，实际操作简单。

实现本发明目的的技术解决方案为：一种基于虚拟墙的机器人路径规划算法，在由定位系统生成的机器人运行环境栅格地图上设置虚拟墙，根据设置完虚拟墙的新栅格地图进行路径规划，包括如下步骤：

步骤1、将环境栅格地图文件下载至虚拟墙设置单元，设置虚拟墙，并将虚拟墙的位置加入到栅格地图文件中，将画好的虚拟墙的栅格地图文件上传至机器人；

步骤2、对地图模型进行描述，并利用目前栅格相对于目标栅格的位置关系确定方向权重向量，利用目前栅格与周围栅格的关联程度确定关联矩阵；

步骤3、根据得到的权重值和关联矩阵，结合栅格的位置，计算每一个待选栅格的选择概率，并从中选择概率最大的作为下一个栅格，得到一个初步的路径规划；

步骤4、对初步路径规划进行优化，并转至步骤3重新进行路径规划，直至初步路径规划的优化结果收敛，至此就完成了路径规划。

本发明与现有技术相比，其显著优点为：1)本发明不需要额外的介质，能够生成使机器人绕开虚拟墙的行动路线，并且在栅格地图上设定的虚拟墙不会对原来基于地图匹配的定位系统造成任何影响；2)本发明在路径规划陷入死路时，采取惩罚措施，能够避免再次走入此死路的情况；3)本发明路径规划完成后采取路径优化和组数选择措施，能够起到改善局部绕路和提高算法执行效率的作用。

附图说明

图1是本发明巡检机器人基于虚拟墙的机器人路径规划算法的流程图。

图2是本发明用栅格法建立的地图模型。

图3是本发明方向权重向量的示意图。

具体实施方式

下面结合附图和具体实施例，进一步说明本发明的原理和方案。

如图1所示，基于虚拟墙的机器人路径规划算法，包括如下步骤：

步骤1、将得到的环境栅格地图文件下载至虚拟墙设置单元，设置虚拟墙，并将虚拟墙的位置加入到栅格地图文件中，并将画好的虚拟墙的栅格地图文件上传至机器人。

通过ftp将机器人本体的环境栅格地图下载至虚拟墙设置单元，虚拟墙设置单元可运行在单独的笔记本上，并通过无线局域网与机器人本体进行通信连接。在该栅格地图文件中，可以用数字来表示有无障碍物区域，比如用1表示无障碍物，用0表示障碍物区域。在设置虚拟墙之前，在下载至虚拟墙设置单元的栅格地图文件打开，将其显示在电脑屏幕上，在该文件中，一个像素对应一个栅格。

用鼠标在栅格地图上画出虚拟墙的位置，将该位置与栅格地图文件相对应的位置比较，如果设置虚拟墙位置对应的栅格原来没有障碍物，则此处用0来表示，写入栅格地图文件。这样，在栅格地图文件中，标识为1表示无虚拟墙无障碍物区域，标识为0表示有虚拟墙有障碍物区域。将画好虚拟墙的栅格地图文件保存并通过ftp上传至机器人本体。这样我们就完成了包含虚拟墙的栅格地图的创建。

步骤2、对地图模型进行描述，并利用目前栅格相对于目标栅格的位置关系，给每个相邻的待选栅格以不同的权重值确定方向权重向量，利用目前栅格与周围栅格的关联程度确定关联矩阵；

得到了包含虚拟墙的栅格地图后，我们需要对地图进行描述。为方便路径规划算法，本算法同时采用坐标法和序号法对栅格地图中的栅格进行编号，两者可用公式相互换算。如图2所示，栅格旁边行列标号为栅格横纵坐标，栅格坐标可用于快速获得该栅格在地图中的位置。栅格中间标号为栅格序号，栅格序号适用于栅格的访问和路径的记录。栅格x即指栅格序号为x的栅格。

建立好模型后，就可以确定关联矩阵和方向权重向量。

关联矩阵用于表示栅格与周边栅格的关联程度，假设路径规划场地为矩形，根据栅格边长N将场地划分为多个小正方形栅格(当边界不满一个栅格时，按一个栅格计算)，得到row行col列的栅格地图，用map(x)来表示表示栅格x对应的关联矩阵的值：

式中：当map(x)＝0时，表示该栅格上有障碍物，不能通行；当map(x)＝C>0时，表示该栅格无障碍物可通行。在栅格地图建立时，C为设定的常值，作为关联矩阵中可通行栅格的关联初值。在路径规划算法中，可以通过改变每个栅格对应的关联值(即该栅格对应的关联矩阵的值)来表示该栅格与周边栅格的关联程度，即该值越大，其与周围栅格的关联程度越大，在路径规划算法中被选择的概率也越大。

方向权重向量(简称方向向量)在路径规划初始阶段中具有一定的引导性。利用目前栅格相对于目标栅格的位置关系，来给每个相邻的待选栅格以不同的权重值，不同情况有不同的权重值选择，本发明提出了一种情况。

如图2，假设目前栅格的序号为12，目标栅格的序号为89，方向向量为：ω(k)＝[3,6,8,6,3,2,1,2],1≤k≤8；

如上式所示ω为方向向量，k表示方向向量所对应的栅格访问顺序，本文中设定栅格访问顺序为左下、下、右下、右、右上、上、左上、左，与表示待选栅格序号与序号向量nextindex所对应。如图3所示，“*”代表当前所在栅格，周围8个栅格即为相邻的待选栅格。即当k＝3时，目前栅格的右下方栅格的权重值为8。该权重值最大，故就方向向量来看，该栅格被选择的可能性最大，反之左上角的栅格的权重值为1，其被选择的可能性最小。方向向量中的各权重值均为经验值。

由于有8种不同的目前栅格和目标栅格的相对位置关系，故共有8种不同的方向权重向量，可由上式所示方向向量左移或右移得到，也可重新设定权重值。每当完成选择下一个栅格后，目前栅格和目标栅格的相对位置关系都有可能发生变化，因此在完成选择下一个栅格时都要重新选择方向向量。

在路径规划的初始阶段，各栅格的关联矩阵的值差异不大时，通过方向向量的引导来加快算法收敛速度，并在该阶段通过增减关联矩阵的值，使后续规划中算法能收敛到一条较好的路径。

步骤3、根据得到的权重值和关联矩阵，结合栅格的位置，计算每一个待选栅格的选择概率，并从中选择概率最大的作为下一个栅格，得到一个初步的路径规划。

上一步得到了关联矩阵和方向权重向量，其将在概率选择中作为重要参数。假设在第i组第j次路径规划中的目前栅格为u。假设栅格u不在地图的边缘，则与它相邻的待选栅格共有8个，这8个栅格组成待选栅格集合。在待选栅格集合中，分别按下式计算每一个待选栅格的选择概率，并从中选择概率最大的作为下一个栅格。若最大概率栅格有两个或两个以上，则等可能选择其中某个栅格作为下一步的栅格，这种栅格的选择方法给算法带来一定的随机性，即同一地图执行该算法后得到两条最终的路径也许会不同，但也保证路径的多样性，使路径规划全局最优的可能性更大。

式中，P_u,v为从栅格u到栅格v的概率，map_i(v)为第i组路径规划算法执行时，栅格v的关联值，ω(k_v)为栅格v对应的方向向量的值，map_i(z)为第i组路径规划算法执行时，栅格v的关联值，ω(k_z)为栅格z对应的方向向量的值，而z为待选栅格集合Cand中的栅格，Visi为本组已访问过的所有栅格集合。使用此集合能够在同次路径规划中避免上一个经过的栅格再次成为下一栅格的待选栅格，而导致回退的现象；同时在同组异次的路径规划中，已访问过的栅格也不能再次重复选择，避免最初路径规划沿单一方向行进的情况，保证在路径规划初始阶段的路径多样性。

另外，当栅格位于角落或是边线时，其有效的相邻待选栅格分别为3个和5个(超出地图范围的栅格为无效)，由它们组成该栅格的待选栅格集合。完成概率选择以后我们就可以得到一个初步的路径规划，为了得到一个良好的路径，我们还要进行一些优化。

步骤4：进行奖励措施、惩罚措施和路径优化措施中的一种或多种，对初步规划进行优化。

(1)奖励措施：在地图规模较大较复杂时，采用奖励措施可以更快的到达最优的路径规划，本算法定义大小为row*col的记忆矩阵τ，用于存储已规划的最短路径中栅格的关联程度改变量，例如，在第i–1组路径规划完成后，栅格x对应的记忆量τ_i-1(x)的大小为：

式中：Q为设定的常数(Q>0)，代表最短路径的权重，其值越大，已规划的最短路径所占的权重就越大，但会降低全局路径的寻优能力，使算法过早收敛；l_i-1为第i–1组路径规划完成后最短路径的长度。故当已规划的最短路径的长度较短时，其栅格的关联量增加得越多，而不在已规划的最短路线的其他栅格对应的记忆量为零。由于记忆矩阵的每个数值均非负，故也可以称这种方法为奖励措施。在每组路径规划均完成后，用记忆矩阵τ对栅格关联矩阵map进行更新。例如，当第i–1组路径规划完成后，整个关联矩阵的更新表达式为：

map_i＝map_i-1+τ_i-1

(2)惩罚措施：由于地图规模较大较复杂时，路径规划算法往往易陷入障碍物死路或者陷入自己走过栅格所围堵的死路，故在本算法中加入惩罚措施，当某一组的路径全部规划完毕且均为无效路径规划(当目前栅格的周边栅格均无法选择，记为一次无效的路径规划)，则启用惩罚措施，对该条路径经过的所有栅格的关联值按惩罚因子p减少，以此来减小再次走入该条死路的可能性。

由于惩罚措施是对一整条路径上所有栅格进行的惩罚，故启用惩罚措施来惩罚栅格时，当该栅格的关联值减小到大于零的极小值时，便不能再减小。此法利于区分障碍栅格和惩罚的可通行栅格，同时也保障了每一个可通行栅格有被选择的可能性。

(3)路径优化：由于方向向量的全局寻优能力相对较差且同组异次路径规划不能重复访问同一栅格，可能出现同组路径规划中前一次规划的路径搜索到最短的后段路径，而在后一次路径规划中即使搜索到最短的前段路径，在后段路径中也会绕道而行的情况(为便于说明，前段和后段路径的说法只是相对而言)，从而使整条最短路径难以匹配到一起。为了优化这种情况带来的局部绕路情况，在栅格地图上对规划路径采用优化措施，从而减少路径行走的长度。路径优化的方式为在已规划的最短路径中依次取若干个节点，判断其中任意两节点之间规划路径是否为最短路径，若不是最短路径且节点之间无障碍物，则取两节点的连接线段代替原路径段，以此来减小路径的长度。

步骤5、转至步骤3重新进行路径规划，直到步骤3产生的路径规划对步骤4的优化算法的结果收敛，至此就完成了路径规划。

Claims (10)

1.一种基于虚拟墙的机器人路径规划算法，其特征在于，在由定位系统生成的机器人运行环境栅格地图上设置虚拟墙，根据设置完虚拟墙的新栅格地图进行路径规划，包括如下步骤：

步骤1、将环境栅格地图文件下载至虚拟墙设置单元，设置虚拟墙，并将虚拟墙的位置加入到栅格地图文件中，将画好的虚拟墙的栅格地图文件上传至机器人；

步骤2、对地图模型进行描述，并利用目前栅格相对于目标栅格的位置关系确定方向权重向量，利用目前栅格与周围栅格的关联程度确定关联矩阵；

步骤3、根据得到的权重值和关联矩阵，结合栅格的位置，计算每一个待选栅格的选择概率，并从中选择概率最大的作为下一个栅格，得到一个初步的路径规划；

步骤4、对初步路径规划进行优化，并转至步骤3重新进行路径规划，直至初步路径规划的优化结果收敛，至此就完成了路径规划。

2.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤1中，通过ftp将机器人本体的环境栅格地图下载至虚拟墙设置单元，虚拟墙设置单元运行在单独的计算机，并通过无线局域网与机器人本体进行通信连接，画好虚拟墙后，保存画好虚拟墙的栅格地图文件，并通过ftp上传至机器人本体。

3.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤1中，在栅格地图文件中，通过数字来表示有无障碍物区域，用1表示无障碍物，用0表示障碍物区域，在栅格地图上设置虚拟墙位置时，将该位置与栅格地图文件相对应的位置比较，如果设置虚拟墙位置对应的栅格原来没有障碍物，则此处用0来表示，写入栅格地图文件，这样，在栅格地图文件中，标识为1表示无虚拟墙无障碍物区域，标识为0表示有虚拟墙有障碍物区域。

4.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤2中，进行地图描述时，同时采用坐标法和序号法对栅格地图中的栅格进行编号，栅格旁边行列标号为栅格横纵坐标，栅格坐标用于快速获得该栅格在地图中的位置，栅格中间标号为栅格序号，栅格序号适用于栅格的访问和路径的记录，栅格x即指栅格序号为x的栅格。

5.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤2中，关联矩阵用于表示栅格与周边栅格的关联程度，假设路径规划场地为矩形，根据栅格边长N将场地划分为多个小正方形栅格，当边界不满一个栅格时，按一个栅格计算，得到row行col列的栅格地图，用map(x)来表示表示栅格x对应的关联矩阵的值：

式中：当map(x)＝0时，表示该栅格上有障碍物，不能通行；当map(x)＝C>0时，表示该栅格无障碍物可通行，在栅格地图建立时，C为设定的常值，作为关联矩阵中可通行栅格的关联初值，在路径规划算法中，通过改变每个栅格对应的关联关联矩阵的值来表示该栅格与周边栅格的关联程度，即该值越大，其与周围栅格的关联程度越大，在路径规划算法中被选择的概率也越大。

6.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤2中，方向权重向量的具体方法为：设定栅格访问顺序，根据目前栅格相对于目标栅格的位置关系，给每个相邻的待选栅格以不同的权重值，栅格被选择的可能性大的权重大，被选择的可能性小的权重小，由于有8种不同的目前栅格和目标栅格的相对位置关系，故共有8种不同的方向权重向量。

7.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤3中，根据栅格的位置确定待选栅格集合，若栅格u不在地图的边缘，则与它相邻的待选栅格共有8个，这8个栅格组成待选栅格集合，若栅格位于角落或是边线时，超出地图范围的栅格为无效，其有效的相邻待选栅格分别为3个和5个，由它们组成该栅格的待选栅格集合。

8.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤3中，计算待选栅格选择概率的公式为：

式中：P_u,v为从栅格u到栅格v的概率，map_i(v)为第i组路径规划算法执行时，栅格v的关联值，ω(k_v)为栅格v对应的方向向量的值，map_i(z)为第i组路径规划算法执行时，栅格v的关联值，ω(k_z)为栅格z对应的方向向量的值，而z为待选栅格集合Cand中的栅格，Visi为本组已访问过的所有栅格集合。

9.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤3中，若最大概率栅格有两个或两个以上，则等可能选择其中某个栅格作为下一步的栅格。

10.根据权利要求1所述的基于虚拟墙的机器人路径规划算法，其特征在于，步骤4中，对初步规划进行优化为奖励措施、惩罚措施和路径优化措施中的一种或多种，具体是：

(1)奖励措施：定义大小为row*col的记忆矩阵τ，用于存储已规划的最短路径中栅格的关联程度改变量，例如，在第i–1组路径规划完成后，栅格x对应的记忆量τ_i-1x的大小为：

式中：Q为设定的常数(Q>0)，代表最短路径的权重，其值越大，已规划的最短路径所占的权重就越大，l_i-1为第i–1组路径规划完成后最短路径的长度；

在每组路径规划均完成后，用记忆矩阵τ对栅格关联矩阵map进行更新，能够更快的到达最优的路径规划，例如，当第i–1组路径规划完成后，整个关联矩阵的更新表达式为：

map_i＝map_i-1+τ_i-1

(2)惩罚措施：当某一组的路径全部规划完毕且均为无效路径规划，即当目前栅格的周边栅格均无法选择，则启用惩罚措施，对该条路径经过的所有栅格的关联值按惩罚因子p减少，以此来减小再次走入该条死路的可能性，当该栅格的关联值减小到大于零的极小值时，便不能再减小；

(3)路径优化：在已规划的最短路径中依次取若干个节点，判断其中任意两节点之间规划路径是否为最短路径，若不是最短路径且节点之间无障碍物，则取两节点的连接线段代替原路径段，以此来减小路径的长度。