本发明涉及机器人路径规划领域，特别是涉及一种基于拓扑地图的机器人路径规划方法及系统。
背景技术：
路径规划技术是机器人研究的核心内容之一，机器人定位与环境地图构建就是为路径规划服务的。所谓机器人路径规划技术，就是机器人根据自身传感器对环境的感知，自行规划处一条安全的运行路线，同时高效的完成作业任务。通常，移动机器人路径规划需要解决三个问题：使机器人能从初始位置运动到目标位置；用一定的算法能绕开障碍物；在完成以上任务的前提下，尽量优化机器人运行轨迹。专利号为cn107368077a的发明专利，一种基于遗传-蚁群算法的机器人路径规划方法，采用栅格法对环境建模，将全局空间转化为大量小栅格，并采用进化算法优化路径。全局环境较小时，该方法有较高准确性和实用性，但存在大环境下存储资源占用过多的缺陷。专利号为cn107065876a的发明专利，基于改进粒子群优化的移动机器人路径规划方法，采用多目标加权求和转化为单目标问题的方法，相较于多目标粒子群算法，限制了搜索空间，且权重的不断更新，增加了算法复杂性。
技术实现要素：
本发明的目的是提供一种基于拓扑地图的机器人路径规划方法及系统，避免目标函数权重赋值靠经验，搜索空间不完整的缺点。
为实现上述目的，本发明提供了如下方案：
一种基于拓扑地图的机器人路径规划方法，所述方法包括：
获取散点图，并在所述散点图中设置机器人的初始运动点和目标点，所述散点图是由包含有障碍物节点的实际环境抽象而成；
根据所述散点图构建与所述散点图对应的维诺图；
计算所述维诺图中各边的行驶代价；
根据所述行驶代价采用改进的迪杰斯特拉算法，得到多条路径；
根据粒子群算法从多条所述路径中确定最短路径。
可选的，所述根据所述散点图构建与所述散点图对应的维诺图，具体包括：
依次连接所述散点图中距离最近的三个障碍物节点，得到多个三角形，所述三角形的外接圆中不含有其他障碍物节点，各所述三角形构成三角网；
对所述三角网中每条边作中垂线，得到各所述中垂线相交构成的网格图，所述网格图为维诺图。
可选的，所述行驶代价包括路径长度和危险程度。
可选的，所述计算所述维诺图中各边的行驶代价，具体包括：
根据
其中，d为两个不同点之间的路径长度，(x1,y1)为边端点1的坐标，(x2,y2)为边端点2的坐标；
根据
其中，w为所有障碍物节点对边的威胁值之和，(x1,y1)为边端点1的坐标，(x2,y2)为所述边端点2的坐标，(xi,yi)为障碍物节点的坐标，d1表示障碍物节点到边端点1的第一路径的长度，d2表示障碍物节点到边端点2的第二路径的长度。
可选的，所述根据粒子群算法确定最短路径，具体包括：
构建粒子群，并初始化所述粒子群的点最优解，所述粒子群中的点最优解对应所述维诺图中的路径，每个所述路径包括第一目标函数值和第二目标函数值；
判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值，获得第二判断结果；
若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值，则选取所述第一路径作为点最优解；
若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值，获得第三判断结果；
若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值，则选取所述第二路径作为点最优解；
若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
根据所述第三判断结果更新所述粒子群的速度和位置；
判断迭代次数是否小于迭代阈值，获得第四判断结果；
若所述第四判断结果表示所述迭代次数小于迭代阈值，则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值，则根据当前点最优解确定最短路径。
为实现上述目的，本发明提供了如下方案：
一种基于拓扑地图的机器人路径规划系统，所述系统包括：
散点图获取模块，用于获取散点图，并在所述散点图中设置机器人的初始运动点和目标点，所述散点图是由包含有障碍物节点的实际环境抽象而成；
维诺图获取模块，用于根据所述散点图构建与所述散点图对应的维诺图；
行驶代价获取模块，用于计算所述维诺图中各边的行驶代价；
路径获取模块，用于根据所述行驶代价采用改进的迪杰斯特拉算法，得到多条路径；
最短路径确定模块，用于根据粒子群算法从多条所述路径中确定最短路径。
可选的，所述维诺图获取模块，具体包括：
三角网单元，用于依次连接所述散点图中距离最近的三个障碍物节点，得到多个三角形，各所述三角形的外接圆中不含有其他障碍物节点，多个所述三角形构成三角网；
网格图单元，对所述三角网中每条边作中垂线，得到各所述中垂线相交构成的网格图，所述网格图为维诺图。
可选的，所述行驶代价包括路径长度和危险程度。
可选的，所述行驶代价获取模块，具体包括：
路径长度确定单元，用于根据
其中，d为两个不同点之间的路径长度，(x1,y1)为边端点1的坐标，(x2,y2)为边端点2的坐标；
危险程度确定单元，用于根据
其中，w为所有障碍物节点对边的威胁值之和，(x1,y1)为边端点1的坐标，(x2,y2)为所述边端点2的坐标，(xi,yi)为障碍物节点的坐标，d1表示障碍物节点到边端点1的第一路径长度，d2表示障碍物节点到边端点2的第二路径长度。
可选的，所述最短路径确定模块，具体包括：
粒子群构建单元，用于构建粒子群，并初始化所述粒子群的点最优解，所述粒子群中的点最优解对应所述维诺图中的路径，各所述路径包括第一目标函数值和第二目标函数值；
第一判断结果获取单元，用于判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
第二判断结果获取单元，用于若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值，获得第二判断结果；
第一最优解单元，用于若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值，则选取所述第一路径作为点最优解；
第二最优解单元，用于若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
第三判断结果获取单元，用于若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值，获得第三判断结果；
第三最优解单元，用于若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值，则随机选取所述第二路径作为点最优解；
第四最优解单元，用于若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
更新单元，用于根据所述第三判断结果更新所述粒子群的速度和位置；
第四判断结果获取单元，用于判断迭代次数是否小于迭代阈值，获得第四判断结果；
重新判断单元，用于若所述第四判断结果表示所述迭代次数小于迭代阈值，则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
最短路径确定单元，用于若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值，则根据当前点最优解确定最短路径。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明提供一种基于拓扑地图的机器人路径规划方法及系统，通过使用维诺图对机器人行动环境建模，避免了大环境下栅格法表示时间与空间复杂度过高的缺点。通过改进的迪杰斯特拉算法求得可行解空间，再采用进化算法优化的方案，解决了以往非可行解优化到可行解方案中难以找到可行解的问题。采用多目标粒子群算法，进行机器人路径规划，避免了单目标粒子群算法中目标函数权重赋值靠经验，搜索空间不完整的缺点。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例基于拓扑地图的机器人路径规划方法流程图；
图2为本发明实施例基于拓扑地图的机器人路径规划系统结构图；
图3-图5为本发明实施例维诺图构建示意图；
图6为本发明实施例某一点对某条边的威胁程度计算示意图；
图7为本发明实施例最终规划路线图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于拓扑地图的机器人路径规划方法及系统，避免目标函数权重赋值靠经验，搜索空间不完整的缺点。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
具体实施例1：
图1为本发明实施例基于拓扑地图的机器人路径规划方法流程图。如图1所示，一种基于拓扑地图的机器人路径规划方法，所述方法包括：
步骤101：获取散点图，并在所述散点图中设置机器人的初始运动点和目标点，所述散点图是由包含有障碍物节点的实际环境抽象而成；
步骤102：根据所述散点图构建与所述散点图对应的维诺图；
具体包括，依次连接所述散点图中距离最近的三个障碍物节点，得到多个三角形，所述三角形的外接圆中不含有其他障碍物节点，各所述三角形构成三角网；
对所述三角网中每条边作中垂线，得到各所述中垂线相交构成的网格图，所述网格图为维诺图。
步骤103：计算所述维诺图中各边的行驶代价，所述行驶代价包括路径长度和危险程度，具体包括：
根据
其中，d为两个不同点之间的路径长度，(x1,y1)为边端点1的坐标，(x2,y2)为边端点2的坐标；
根据
其中，w为所有障碍物节点对边的威胁值之和，(x1,y1)为边端点1的坐标，(x2,y2)为所述边端点2的坐标，(xi,yi)为障碍物节点的坐标，d1表示障碍物节点到边端点1的第一路径的长度，d2表示障碍物节点到边端点2的第二路径的长度。
步骤104：根据所述行驶代价采用改进的迪杰斯特拉算法，得到多条路径；
步骤105：根据粒子群算法从多条所述路径中确定最短路径；
具体包括：构建粒子群，并初始化所述粒子群的点最优解，所述粒子群中的点最优解对应所述维诺图中的路径，每个所述路径包括第一目标函数值和第二目标函数值；
判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值，获得第二判断结果；
若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值，则选取所述第一路径作为点最优解；
若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值，获得第三判断结果；
若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值，则选取所述第二路径作为点最优解；
若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
根据所述第三判断结果更新所述粒子群的速度和位置；
判断迭代次数是否小于迭代阈值，获得第四判断结果；
若所述第四判断结果表示所述迭代次数小于迭代阈值，则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值，则根据当前点最优解确定最短路径。
上述基于拓扑地图的机器人路径规划方法，通过使用维诺图对机器人行动环境建模，避免了大环境下栅格法表示时间与空间复杂度过高的缺点。通过改进的迪杰斯特拉算法求得可行解空间，再采用进化算法优化的方案，解决了以往非可行解优化到可行解方案中难以找到可行解的问题。采用多目标粒子群算法，进行机器人路径规划，避免了单目标粒子群算法中目标函数权重赋值靠经验，搜索空间不完整的缺点，本方法是一个具有典型性和通用性的机器人路径规划方法，用于解决危险品运输、无人机避障飞行、通信路由、电力通信路由规划等问题，有着广泛的应用前景。经过路径规划的机器人能够选择一条尽可能满足用户需求的路线向目标点运动，可以更可靠地执行任务、提高系统效率，可以减少系统消耗。
图2为本发明实施例基于拓扑地图的机器人路径规划系统结构图。如图2所示，一种基于拓扑地图的机器人路径规划系统，所述系统包括：
散点图获取模块201，用于获取散点图，并在所述散点图中设置机器人的初始运动点和目标点，所述散点图是由包含有障碍物节点的实际环境抽象而成；
维诺图获取模块202，用于根据所述散点图构建与所述散点图对应的维诺图；
行驶代价获取模块203，用于计算所述维诺图中各边的行驶代价，所述行驶代价包括路径长度和危险程度；
路径获取模块204，用于根据所述行驶代价采用改进的迪杰斯特拉算法，得到多条路径；
最短路径确定模块205，用于根据粒子群算法从多条所述路径中确定最短路径。
所述维诺图获取模块202，具体包括：
三角网单元，用于依次连接所述散点图中距离最近的三个障碍物节点，得到多个三角形，各所述三角形的外接圆中不含有其他障碍物节点，多个所述三角形构成三角网；
网格图单元，对所述三角网中每条边作中垂线，得到各所述中垂线相交构成的网格图，所述网格图为维诺图。
所述行驶代价获取模块203，具体包括：
路径长度确定单元，用于根据
其中，d为两个不同点之间的路径长度，(x1,y1)为边端点1的坐标，(x2,y2)为边端点2的坐标；
危险程度确定单元，用于根据
其中，w为所有障碍物节点对边的威胁值之和，(x1,y1)为边端点1的坐标，(x2,y2)为所述边端点2的坐标，(xi,yi)为障碍物节点的坐标，d1表示障碍物节点到边端点1的第一路径长度，d2表示障碍物节点到边端点2的第二路径长度。
所述最短路径确定模块205，具体包括：
粒子群构建单元，用于构建粒子群，并初始化所述粒子群的点最优解，所述粒子群中的点最优解对应所述维诺图中的路径，各所述路径包括第一目标函数值和第二目标函数值；
第一判断结果获取单元，用于判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
第二判断结果获取单元，用于若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值，获得第二判断结果；
第一最优解单元，用于若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值，则选取所述第一路径作为点最优解；
第二最优解单元，用于若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
第三判断结果获取单元，用于若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值，则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值，获得第三判断结果；
第三最优解单元，用于若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值，则随机选取所述第二路径作为点最优解；
第四最优解单元，用于若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值，则随机选取所述第一路径或者所述第二路径作为点最优解；
更新单元，用于根据所述第三判断结果更新所述粒子群的速度和位置；
第四判断结果获取单元，用于判断迭代次数是否小于迭代阈值，获得第四判断结果；
重新判断单元，用于若所述第四判断结果表示所述迭代次数小于迭代阈值，则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值，获得第一判断结果；
最短路径确定单元，用于若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值，则根据当前点最优解确定最短路径。
具体实施例2：
步骤1：系统初始化。将包含有障碍物的实际环境抽象成一张散点图，生成的散点图如图3所示。其中，障碍物点，分别标号为1、2、3..11、12，机器人起点标号为s，机器人终点标号为e。
步骤2：构建图3中散点图对应的维诺图，具体包括：
①依次连接图3散点图中距离最近的三个障碍物点，要求每次得到的三角形，它的外接圆中不含有其他障碍物点，由此得到delaunay三角网，如图4黑线所示。
②对构造的delaunay三角网中每条边作其中垂线，所有中垂线相交构成的网格即为维诺图，如图4虚线所示。最终生成的维诺图如图5所示，维诺图顶点依次标记为①、②..
由于路径规划是在维诺图的边上进行，选择距离起点欧几里德距离最近的维诺图顶点作为路径规划的起点，选择距离终点欧几里德距离最近的维诺图顶点作为路径规划的终点。在本例中，图5所示维诺图顶点①作为路径规划的起点，图5所示的维诺图顶点
步骤3：计算维诺图中每条边行驶代价，用来作为机器人在路径上行驶好坏的尺度，路径总的代价越小，机器人选择该条路径行驶的几率应越大。代价有两个评测点：路径长度和危险程度。
路径长度为两点间的欧几里德距离，若求点(x1，y1)与点(x2，y2)之间的路径长度d，则用下列公式计算：
危险程度为所有障碍物节点对该边的威胁值之和。图6为本发明实施例某一点对某条边的威胁程度计算示意图。若求边端点1(x1,y1)与边端点2(x2,y2)之间路径的危险程度，使用(xi′,yi′)表示障碍物点i的坐标，则所有n个障碍物节点对该边的威胁为：
其中，d1表示障碍物节点i到边端点1的欧几里德距离，d2表示障碍物节点i到边端点2的欧几里德距离，根号中的内容是计算障碍物节点i到边中点的距离。
步骤4：使用改进的dijkstra算法求出可行解集。将边的多个评测值加权转化为单个评测值，此时边只有单个评测值，使用dijkstra算法，可以求得一个可行解，也就是一个对应于当前加权方法的最短路径。不断修改上述多个评测值加权用到的权值，可以得到多个可行解，即为可行解集。
步骤5：粒子群算法初始化。考虑到可行解集中解的离散性，直接将步骤4求得的可行解集作为粒子群算法的可行解空间存在难以有效定义粒子群算法速度的问题，故对可行解集中的可行解进行无冲突的二进制编码，将可行解集映射到一个二进制编码集合，并以此来作为粒子群算法可行解空间。可以将可行解集进行1、2、3…的编号，然后把编号改为二进制。
算法的解为二进制序列，其值为对应解在解集中的位置的二进制编码。种群中各粒子初始化为随机解，即随机的0/1序列，序列长度n＝[log2解空间的大小]。
粒子速度为多个交换对，交换对由下标对组成，是指解序列中任意两个下标位置的数字交换。如交换解序列中下标i和下标j处的数据，则表示为(i,j)，速度为随机多个这样的交换对。
点最优解集初始化为当前种群中出现的最优解。最优解集大小根据具体问题选取，示例中最优解集大小与种群规模相等。
步骤6：根据目标更新计算点最优解pbest和全局最优解gbest。目标函数定义为解对应最短路径中所有相邻顶点对应边的代价之和，由于代价有两个评测点，解的代价对应两个目标函数，假设解的点序列为p1p2…pn,则目标函数：
用以表示该路径经过的所有边的路径长度之和。
用以表示该路径经过的所有边的危险程度之和。
对于两个解的优劣比较：
由于解对应有两个目标函数值，可能存在解的目标函数值一个大一个小的情况，故统一用如下方法进行比较。两个不同的解，分别设为解1和解2，相比较，若解1的目标函数值1与目标函数值2均优于解2，则解1优于解2；若解1的目标函数值1与目标函数值2均劣于解2，则解1劣于解2；否则，随机选择解1或者解2作为较优解。
根据目标函数更新点最优解，若点当前解优于点历史最优解，则更新点最优解，否则不更新。
对于非支配解集的更新。选取步骤点最优解集中的最优解，用该解替换非支配解集res中劣于该解的解。
选取非支配解集中的最优解来更新全局最优解。
构建非支配解集的自适应网格。为自适应网格建立二维坐标系，横坐标为目标函数f1，纵坐标为目标函数f2。首先计算出非支配解集中每个解的目标函数值，将该解表示的点表示在该坐标系中。选取横坐标每x(根据实际情况确定值，本例中选取1)，纵坐标每y(根据实际情况确定值，本例中选取1)划分网格，为每个网格赋值，值为网格中对应的点数。
定义xi为网格i中点数的倒数，对于所有点数不为0的m个网格，
则网格i对应概率
步骤7：更新速度和粒子位置。
v[i]＝w*v[i]+c1*rand()*(pbest[i]-present[i])+c2*rand()*(gbest-present[i])
present[i]＝present[i]+v[i]
其中v[i]代表第i个粒子的速度。w代表惯性权值，取值在0-1之间，本例中取值为0.5。c1和c2表示学习参数，通常取值c1＝c2＝2，本例取值c1＝c2＝2，rand()表示在0-1之间的随机数，pbest[i]表示第i个粒子的点最优解，gbest全局最优解，present[i]代表第i个粒子的当前解。其中，present与pbest或gbest的差值为present到pbest或gbest的交换对。
步骤8：为当前粒子位置添加变异操作。本例设置变异概率为0.1，在更新粒子位置后，产生一个0-1之间的随机数，若其值小于0.1，则产生两个1-n之间的随机数a、b，这里的n在步骤5①中被赋值为上述二进制序列长度，所以a和b的值都小于等于该长度，形成交换对(a,b)，以此来对位置进行调整，具体调整方法是，交换二进制序列中a下标位置的数和b下标位置的数；若其值不小于0.1，则直接转到步骤8。
步骤9：判断是否满足最大迭代次数，若未满足，则返回步骤6执行；若满足则算法结束，当前全局最优解为算法解。由于在步骤5中已将把原问题的可行解集映射到了二进制序列空间，所以还要把算法最终解转化为原问题的可行解形式，也就是一个最短路径。最终得到的最短路径是①③⑦
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。