CN102155942A - 大范围环境下基于模糊拓扑地图的全局路径规划方法 - Google Patents

Abstract

本发明涉及一种大范围环境下基于模糊拓扑地图的全局路径规划方法，来完成对于单目标和多目标进行路径规划的任务要求。其步骤为：1)首先是建立地图，构建面向巡视管理和路径规划的，基于分布式信息表征的全局模糊拓扑语义地图；2)地图相关数据输入；3)进行地图数据处理与聚集，由旧地图数据建立新映射地图框架；4)进行目标点数据处理，修正新映射地图；5)进行起始点处理，并进一步修正新映射地图；6)依次比较可行的映射目标节点路径链表排列，获取长度最小的排列方式；7)将获得的映射目标节点链转换为原地图体系下的目标节点链排列；8)利用启发式算法扩展获得原地图体系下的路径，这就是所规划的最优路径；9)算法结果数据输出。

Description

大范围环境下基于模糊拓扑地图的全局路径规划方法

技术领域

本发明专利涉及一种大范围环境下基于模糊拓扑地图的全局路径规划方法，属于路径规划算法技术领域。

背景技术

机器人的路径规划算法是实现机器人自主导航的关键技术之一。移动机器人路径规划是指在有障碍物的工作环境下，找到一条从给定起点到指定终点的适当的运动路径，使机器人能够在运动过程中安全的、无碰地到达目标点。

目前有多种算法可以完成规划出机器人的运行路径，但是大多都只能针对于单个目标任务来使用，而无法对于多个目标的任务规划出一个最优路径。在相关地图建模环境下解决规划多个目标的任务成为机器人平台的一个待解决问题。

发明内容

针对于这一问题，本发明提供了一种快速、准确的大范围环境下基于模糊拓扑地图的全局路径规划方法，来完成对于单目标和多目标进行路径规划的任务要求。其中单目标节点的路径规划任务可以看作是特殊的多目标节点路径规划任务，即只目标节点数为一的多目标路径规划。

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

一种大范围环境下基于模糊拓扑地图的全局路径规划方法，它的步骤为：

1)首先是建立地图，构建面向巡视管理和路径规划的，基于分布式信息表征的全局模糊拓扑语义地图；

2)地图相关数据输入；

3)进行地图数据处理与聚集，由旧地图数据建立新映射地图框架；

4)进行目标点数据处理，修正新映射地图；

5)进行起始点处理，并进一步修正新映射地图；

6)依次比较可行的映射目标节点路径链表排列，获取长度最小的排列方式；

7)将获得的映射目标节点链转换为原地图体系下的目标节点链排列；

8)利用启发式算法扩展获得原地图体系下的路径，这就是所规划的最优路径；

9)将所获的目标节点链与路径规划的结果导出，供使用。

所述步骤1)中，以走廊交点以及拐角、房间门、电梯口、楼梯口作为重要位置点，以节点表示；用连线代表两节点之间相连通；对于位置相近或相对的多个房门，用一个节点来表示；同时使用不同方位的房门编号来表示不同的房间；节点间的距离使用加权值来表示，对于楼层中距离值为楼层中普通房间长度的距离，加权值为最低值，对于距离长度超过楼层中普通房间长度两倍以上的距离，可以按照超出倍数增加。

再增加快速索引编号，用于帮助算法快速分辨不同节点；相邻的周围节点的快速索引编号，用于帮助算法快速寻找周围相邻映射节点；节点相对于周围节点的距离信息，用于帮助启发式算法计算路径花费；人工地标，用于帮助机器人在运行时识别地点位置；聚集点协助标志位，用于帮助算法快速分割和聚集节点使用。这些都是节点的属性，用于帮助地图的使用。房间编号帮助人来理解和识别房间，这些属性则用来帮助计算机来理解和识别房间。

所述步骤2)中，地图相关数据为建立好的模糊拓扑地图数据以及起始节点的索引号和目标节点索引号的链表。

所述步骤3)中，将已建立的模糊拓扑地图数据进行数据处理与聚集：首先根据建立地图时标明的地图聚集点协助标志位，将地图分割为不同的部分；将地图聚集点协助标志位所在的节点定为特殊映射节点，而将其他被地图标志位节点分割开的部分的节点按被分割的部分聚合为一个普通映射节点。

对每个新的映射节点新重编写索引编号，以便帮助算法在下一步计算中快速识别映射节点；同时对于特殊映射节点，则将原节点的对应的旧索引编号也作为一个属性输入；普通映射节点则加入数量记录这一属性，将被聚合至此映射节点的原节点中，作为目标节点的节点数目。为每个映射节点加入周边相邻映射节点的快速索引编号，帮助算法快速寻找周围相邻映射节点；加入映射节点周围映射节点的距离信息，将映射节点中聚集节点群的最边缘的点的距离信息表现在地图上，以便于以后计算总路径距离长度；一个节点索引编号链表，包含所聚集原节点群中作为目标节点的旧索引编号，用于帮助算法在所述步骤7)中获得原地图目标路径节点使用。

所述步骤4)中，按照上面步骤3)所搭建的框架将地图目标节点数据依次提取处理并填写进去；并且对于需要聚集为同一映射节点的全部目标节点，按照所搭建的框架要求将目标节点的重要信息提取并处理后，再次对映射节点进行处理，如果内部存在有过长的空荡的距离，达到一般两节点间加权距离的3倍及以上时，要将该节点分割开，将他们的数据按照步骤3)的框架格式重新分割到两个映射节点中去。

所述步骤5)中，获得起始节点在新映射地图下的索引编号和加权值，并单独存放，同时将起始点所在的位置由新的映射地图框架中分割出来，将起始目标节点独立为一个新的映射节点，框架格式按照特殊映射节点来。

起始节点如果处于聚集节点群的边缘位置，能够直接与其他的映射节点相连结，则直接将起始节点按特殊映射节点框架独立出来；若处于聚集节点群的内部，与其他的映射节点不能直接相连结，则需要以起始节点作为分割标志位，将该聚集节点分割为一个特殊映射节点连接两个普通映射节点。

所述步骤6)中，从起始映射节点开始，寻找周边不会穿过其他映射节点而直接到达的映射节点，以深度优先的方式依次寻找下去，从而获得可能的映射目标节点链，并使用循环递归将所有节点链的总加权值都以启发式算法来确定路径并依次比较一遍，在利用前面步骤中获得的各类数据精确考虑并计算加强值大小后，取最小的那个映射目标节点链。

所述步骤7)中，将步骤6)获得的最优映射目标节点链的每个目标映射节点按照路径走向，将所包含的原目标节点依路径走向排序，再使用排序后的原目标节点们替换映射目标节点，从而组成原地图数据可用的目标节点链。

所述步骤8)中，利用启发式算法，将所获得的目标节点链扩展补完，从而获得一条原地图体系下可用的完整的路径规划。

所述步骤9)中，将所获的的目标节点链与路径规划的结果输出，供机器人平台使用。

所述启发式算法使用已经建立好的模糊拓扑地图，为每个节点的如下属性申请存储空间：父节点数据，本节点到起始节点的距离花费G，本节点到目标节点的预估距离花费值H，行动至本节点处所可能距离花费之和F；同时采用了一个单一的表，以优先级的顺序来存放待使用的节点，该表使用二叉堆的方法来存储数据以便快速排序。

所述启发式算法的具体步骤为：

(1)地图数据初始化；初始化地图每个节点如上面所述的属性空间，为待使用的表申请空间；将起始节点加入到表里面，并且将本节点到起始节点的距离花费属性设置为零，估计到目标节点的距离花费值并写入另外两个距离花费属性里，设置父节点数据为自身；

(2)将表中最前面的节点定为当前点，将该点从表中删除并按优先级将表中元素重新排序，同时获取该点附近的无父节点的所有可通行节点，而后将这些节点的父节点指向更改为当前点，以当前点起始节点的距离花费加上当前点到可通行节点的距离花费值，计算可通行节点到标节点的预估花费值和总可能花费之和，并写入相应的可通行节点属性中去；将这些可通行节点来加入到表中并按照总可能花费之和的值来排序；

(3)取新表排在最前面的节点，若是目标节点则结束，否则返回步骤2)；

(4)由目标节点开始，通过节点的父节点来获得上一个节点，并依次回溯至起始节点，反馈路径列表。

本发明的有益效果是：

本发明首先提供了一个地图建立规范，在使用这个规范建立地图时，由于采用了模糊地图的概念，因此不需要精确测量而只需利用个人目测就可以建立出地图，同时使用拓扑地图则使得地图构造简单易懂，便于理解识别。

本发明所提供的路径规划方法，在配合上面所说的模糊拓扑地图的情况下，解决了机器人平台楼道走廊环境下的单目标节点和多目标节点的路径规划问题。

附图说明

图1为本发明的建立模糊拓扑地图的例图。

图2为算法主流程图。

图3为启发式算法流程图。

具体实施方式

下面附图与实施例对本发明做进一步说明。

首先是建立地图。由于走廊为大范围、结构化环境，构建含节点坐标信息的拓扑地图是传统的环境建模方式。但该方法需人为提供环境关键节点的精确位置信息，使机器人的自主性降低，因此针对环境的特殊结构以及典型环境特征，提出并构建了面向巡视管理和路径规划的，基于“分布式信息表征”的全局模糊拓扑语义地图。

以走廊交点以及拐角、房间门、电梯口、楼梯口等作为重要位置点，以节点表示，用连线代表两节点之间相连通。对于位置相近或相对的多个房门，用一个节点来表示，同时使用不同方位的房门编号来表示不同的房间。节点间的距离使用加权值来表示，对于一般的距离，加权值为最低值，对于特别长的距离，则适当增加。

节点构造使用加权值，快速索引编号，周围相邻节点的快速索引编号，节点周边方位的位置点，人工地标，聚集点协助标志位来表示。建立地图如图1所示，圆圈表示重要位置点，圈内为实心而不是空心表示已在节点内加入人工标志位，红圈外增加方块表示人工赋予的用于标识走廊相交位置的地图标志位，编号表示房间号，编号的位置方向表示房门所出节点的方位。

在地图建立后机器人平台进行路径规划时，这一路径规划算法的步骤如下：

1)将建立好的模糊拓扑地图数据导入算法程序，同时输入起始节点的索引号和目标节点索引号的链表。

2)将已建立的模糊拓扑地图数据进行数据处理与聚集：首先根据建立地图时标明的地图聚集点协助标志位，将地图分割为不同的部分；将地图聚集点协助标志位所在的节点定为特殊映射节点，而将其他被地图标志位节点分割开的部分的节点按被分割的部分聚合为一个普通映射节点。

对每个新的映射节点新重编写索引编号，以便帮助算法在下一步计算中快速识别映射节点；同时对于特殊映射节点，则将原节点的对应的旧索引编号也作为一个属性输入；普通映射节点则加入数量记录这一属性，将被聚合至此映射节点的原节点中，作为目标节点的节点数目。为每个映射节点加入周边相邻映射节点的快速索引编号，帮助算法快速寻找周围相邻映射节点；加入映射节点周围映射节点的距离信息，将映射节点中聚集节点群的最边缘的点的距离信息表现在地图上，以便于以后计算总路径距离长度；一个节点索引编号链表，包含所聚集原节点群中作为目标节点的旧索引编号，用于帮助算法在所述步骤6)中获得原地图目标路径节点使用。

3)按照上面步骤2)所搭建的框架将地图目标节点数据依次提取处理并填写进去；并且对于需要聚集为同一映射节点的全部目标节点，按照所搭建的框架要求将目标节点的重要信息提取并处理后，再次对映射节点进行处理，如果内部存在有过长的空荡的距离，达到一般两节点间加权距离的3倍及以上时，要将该节点分割开，将他们的数据按照步骤2)的框架格式重新分割到两个映射节点中去。

4)获得起始节点在新映射地图下的索引编号和加权值，并单独存放，同时将起始点所在的位置由新的映射地图框架中分割出来，将起始目标节点独立为一个新的映射节点，框架格式按照特殊映射节点来。

起始节点如果处于聚集节点群的边缘位置，能够直接与其他的映射节点相连结，则直接将起始节点按特殊映射节点框架独立出来；若处于聚集节点群的内部，与其他的映射节点不能直接相连结，则需要以起始节点作为分割标志位，将该聚集节点分割为一个特殊映射节点连接两个普通映射节点。

5)将起始映射节点作为当前映射节点，以当前映射节点为基准，寻找周边不会穿过其他映射节点而直接到达的映射节点，并将新的映射节点作为当前映射节点，以深度优先的方式依次寻找下去，从而获得可能的映射目标节点链；即取L(n)为长度为n的列表，则以此方法获得的可能的映射目标节点链为集合{L₀(n)，L₁(n)，L₂(n)......L_k(n)......}。

使用循环递归将所有节点链的总加权值都以启发式算法来确定路径并依次比较一遍，在利用前面几个步骤中获得的各类数据考虑并计算加权值大小后，取最小的那个映射目标节点链；即定义G(L(n))为映射目标节点链长度，则目标输出应为min{G(L₀(n))，G(L₁(n))，G(L₂(n))......G(L_k(n))......}。

6)将上一步获得的最优映射目标节点链的每个目标映射节点按照路径走向，将所包含的原目标节点依路径走向排序，再使用排序后的原目标节点们替换映射目标节点，从而组成原地图数据可用的目标节点链。

7)利用启发式算法，将所获得的目标节点链扩展补完，从而获得一条原地图体系下可用的完整的路径规划。

8)将所获的的目标节点链与路径规划的结果输出，供机器人平台使用。

所述启发式算法使用已经建立好的模糊拓扑地图，为每个节点的如下属性申请存储空间：父节点数据(用于在完成搜索后从目标节点回溯到起始节点)，本节点到起始节点的距离花费G(通过算法中不断累积距离数据来获得)，本节点到目标节点的预估距离花费值H(H采用本节点到目标节点的两垂直方向距离加权值差之和，这一简单的预估花费计算函数对于本算法已经够用，因为在机器人在楼层中运行时仅能沿互相垂直的走廊运行)，行动至本节点处所可能距离花费之和F，即F＝G+H；同时采用了一个单一的表，以优先级的顺序来存放待使用的节点，该表使用二叉堆的方法来存储数据以便快速排序。

启发式算法步骤如下：

1)地图数据初始化；初始化地图每个节点如上面所述的属性空间，为待使用的表申请空间；将起始节点加入到表里面，并且将本节点到起始节点的距离花费属性设置为零，估计到目标节点的距离花费值并写入另外两个距离花费属性里，设置父节点数据为自身；

2)将表中最前面的节点定为当前点，将该点从表中删除并按优先级将表中元素重新排序，同时获取该点附近的无父节点的所有可通行节点，而后将这些节点的父节点指向更改为当前点，以当前点起始节点的距离花费加上当前点到可通行节点的距离花费值，计算可通行节点到标节点的预估花费值和总可能花费之和，并写入相应的可通行节点属性中去；将这些可通行节点来加入到表中并按照总可能花费之和的值来排序；

3)取新表排在最前面的节点，若是目标节点则结束，否则返回步骤2)；

4)由目标节点开始，通过节点的父节点来获得上一个节点，并依次回溯

至起始节点，反馈路径列表。

本发明对于机器人平台，基于模糊拓扑地图环境，提供了一种全局路径规划算法，解决了机器人平台对于单目标和多目标节点的路径规划问题。

Claims (10)

1.一种大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，它的步骤为：

1)首先是建立地图，构建面向巡视管理和路径规划的，基于分布式信息表征的全局模糊拓扑语义地图；

2)地图相关数据输入；

3)进行地图数据处理与聚集，由旧地图数据建立新映射地图框架；

4)进行目标点数据处理，修正新映射地图；

5)进行起始点处理，并进一步修正新映射地图；

6)依次比较可行的映射目标节点路径链表排列，获取长度最小的排列方式；

7)将获得的映射目标节点链转换为原地图体系下的目标节点链排列；

8)利用启发式算法，将所获得的目标节点链扩展补完，从而获得一条原地图体系下的完整的路径规划，这就是所规划的最优路径；

9)将所获的目标节点链与路径规划的结果导出，供使用。

2.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤1)中，以走廊交点以及拐角、房间门、电梯口、楼梯口作为重要位置点，以节点表示；用连线代表两节点之间相连通；对于位置相近或相对的多个房门，用一个节点来表示；同时使用不同方位的房门编号来表示不同的房间；节点间的距离使用加权值来表示，对于楼层中距离值为楼层中普通房间长度的距离，加权值为最低值，对于距离长度超过楼层中普通房间长度两倍以上的距离，按照超出倍数增加；再增加快速索引编号，用于帮助算法快速分辨不同节点；相邻的周围节点的快速索引编号，用于帮助算法快速寻找周围相邻映射节点；节点相对于周围节点的距离信息，用于帮助启发式算法计算路径花费；人工地标，用于帮助机器人在运行时识别地点位置；聚集点协助标志位，用于帮助算法快速分割和聚集节点使用；房间编号用于识别房间。

3.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤2)中，地图相关数据为建立好的模糊拓扑地图数据以及起始节点的索引号和目标节点索引号的链表。

4.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤3)中，将已建立的模糊拓扑地图数据进行数据处理与聚集：首先根据建立地图时标明的地图聚集点协助标志位，将地图分割为不同的部分；将地图聚集点协助标志位所在的节点定为特殊映射节点，而将其他被地图标志位节点分割开的部分的节点按被分割的部分聚合为一个普通映射节点；对每个新的映射节点新重编写索引编号，以便帮助算法在下一步计算中快速识别映射节点；同时对于特殊映射节点，则将原节点的对应的旧索引编号也作为一个属性输入；普通映射节点则加入数量记录这一属性，将被聚合至此映射节点的原节点中，作为目标节点的节点数目；为每个映射节点加入周边相邻映射节点的快速索引编号，帮助算法快速寻找周围相邻映射节点；加入映射节点周围映射节点的距离信息，将映射节点中聚集节点群的最边缘的点的距离信息表现在地图上，以便于以后计算总路径距离长度；一个节点索引编号链表，包含所聚集原节点群中作为目标节点的旧索引编号。

5.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤4)中，按照所搭建的框架将地图目标节点数据依次提取处理并填写进去；并且对于需要聚集为同一映射节点的全部目标节点，按照所搭建的框架要求将目标节点的重要信息提取并处理后，再次对映射节点进行处理，如果内部存在有过长的空荡的距离，达到一般两节点间加权距离的3倍及以上时，要将该节点分割开，将他们的数据按照所搭建的框架格式重新分割到两个映射节点中去。

6.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤5)中，获得起始节点在新映射地图下的索引编号和加权值，并单独存放，同时将起始点所在的位置由新的映射地图框架中分割出来，将起始目标节点独立为一个新的映射节点，框架格式按照特殊映射节点来；起始节点如果处于聚集节点群的边缘位置，能够直接与其他的映射节点相连结，则直接将起始节点按特殊映射节点框架独立出来；若处于聚集节点群的内部，与其他的映射节点不能直接相连结，则需要以起始节点作为分割标志位，将该聚集节点分割为一个特殊映射节点连接两个普通映射节点。

7.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤6)中，从起始映射节点开始，寻找周边不会穿过其他映射节点而直接到达的映射节点，以深度优先的方式依次寻找下去，从而获得可能的映射目标节点链，并使用循环递归将所有节点链的总加权值都以启发式算法来确定路径并依次比较一遍，在利用前面步骤中获得的各类数据精确考虑并计算加强值大小后，取最小的那个映射目标节点链。

8.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述步骤6)中，将获得的最优映射目标节点链的每个目标映射节点按照路径走向，将所包含的原目标节点依路径走向排序，再使用排序后的原目标节点们替换映射目标节点，从而组成原地图数据可用的目标节点链。

9.如权利要求1或2或7所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述启发式算法为在已经建立好的模糊拓扑地图上，为每个节点的如下属性申请存储空间：父节点数据，本节点到起始节点的距离花费G，本节点到目标节点的预估距离花费值H，行动至本节点处所可能距离花费之和F；同时采用了一个单一的表，以优先级的顺序来存放待使用的节点，表使用二叉堆的方法来快速排序。

10.如权利要求1所述的大范围环境下基于模糊拓扑地图的全局路径规划方法，其特征是，所述启发式算法的具体步骤为：

(1)地图数据初始化；初始化地图每个节点如上面所述的属性空间，为待使用的表申请空间；将起始节点加入到表里面，并且将本节点到起始节点的距离花费属性设置为零，估计到目标节点的距离花费值并写入另外两个距离花费属性里，设置父节点数据为自身；

(2)将表中最前面的节点定为当前点，将该点从表中删除并按优先级将表中元素重新排序，同时获取该点附近的无父节点的所有可通行节点，而后将这些节点的父节点指向更改为当前点，以当前点起始节点的距离花费加上当前点到可通行节点的距离花费值，计算可通行节点到标节点的预估花费值和总可能花费之和，并写入相应的可通行节点属性中去；将这些可通行节点来加入到表中并按照总可能花费之和的值来排序；

(3)取新表排在最前面的节点，若是目标节点则结束，否则返回步骤2)；

(4)由目标节点开始，通过节点的父节点来获得上一个节点，并依次回溯至起始节点，反馈路径列表。

Images