CN108664022B - 一种基于拓扑地图的机器人路径规划方法及*** - Google Patents
一种基于拓扑地图的机器人路径规划方法及*** Download PDFInfo
- Publication number
- CN108664022B CN108664022B CN201810393365.3A CN201810393365A CN108664022B CN 108664022 B CN108664022 B CN 108664022B CN 201810393365 A CN201810393365 A CN 201810393365A CN 108664022 B CN108664022 B CN 108664022B
- Authority
- CN
- China
- Prior art keywords
- path
- objective function
- function value
- optimal solution
- judgment result
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 36
- 238000010586 diagram Methods 0.000 claims abstract description 116
- 239000002245 particle Substances 0.000 claims abstract description 51
- 230000004888 barrier function Effects 0.000 claims abstract description 18
- 238000010276 construction Methods 0.000 claims description 5
- 230000007547 defect Effects 0.000 abstract description 7
- 230000006870 function Effects 0.000 description 104
- 238000011156 evaluation Methods 0.000 description 6
- 238000005457 optimization Methods 0.000 description 5
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000004364 calculation method Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种基于拓扑地图的机器人路径规划方法及***。所述方法包括:获取散点图,并在所述散点图中设置机器人的初始运动点和目标点,所述散点图是由包含有障碍物节点的实际环境抽象而成;根据所述散点图构建与所述散点图对应的维诺图;计算所述维诺图中各边的行驶代价;根据所述行驶代价采用改进的迪杰斯特拉算法,得到多条路径;根据粒子群算法从多条所述路径中确定最短路径。采用本发明的方法或***,能够避免目标函数权重赋值靠经验,搜索空间不完整的缺点。
Description
技术领域
本发明涉及机器人路径规划领域,特别是涉及一种基于拓扑地图的机器人路径规划方法及***。
背景技术
路径规划技术是机器人研究的核心内容之一,机器人定位与环境地图构建就是为路径规划服务的。所谓机器人路径规划技术,就是机器人根据自身传感器对环境的感知,自行规划处一条安全的运行路线,同时高效的完成作业任务。通常,移动机器人路径规划需要解决三个问题:使机器人能从初始位置运动到目标位置;用一定的算法能绕开障碍物;在完成以上任务的前提下,尽量优化机器人运行轨迹。专利号为CN107368077A的发明专利,一种基于遗传-蚁群算法的机器人路径规划方法,采用栅格法对环境建模,将全局空间转化为大量小栅格,并采用进化算法优化路径。全局环境较小时,该方法有较高准确性和实用性,但存在大环境下存储资源占用过多的缺陷。专利号为CN107065876A的发明专利,基于改进粒子群优化的移动机器人路径规划方法,采用多目标加权求和转化为单目标问题的方法,相较于多目标粒子群算法,限制了搜索空间,且权重的不断更新,增加了算法复杂性。
发明内容
本发明的目的是提供一种基于拓扑地图的机器人路径规划方法及***,避免目标函数权重赋值靠经验,搜索空间不完整的缺点。
为实现上述目的,本发明提供了如下方案:
一种基于拓扑地图的机器人路径规划方法,所述方法包括:
获取散点图,并在所述散点图中设置机器人的初始运动点和目标点,所述散点图是由包含有障碍物节点的实际环境抽象而成;
根据所述散点图构建与所述散点图对应的维诺图;
计算所述维诺图中各边的行驶代价;
根据所述行驶代价采用改进的迪杰斯特拉算法,得到多条路径;
根据粒子群算法从多条所述路径中确定最短路径。
可选的,所述根据所述散点图构建与所述散点图对应的维诺图,具体包括:
依次连接所述散点图中距离最近的三个障碍物节点,得到多个三角形,所述三角形的外接圆中不含有其他障碍物节点,各所述三角形构成三角网;
对所述三角网中每条边作中垂线,得到各所述中垂线相交构成的网格图,所述网格图为维诺图。
可选的,所述行驶代价包括路径长度和危险程度。
可选的,所述计算所述维诺图中各边的行驶代价,具体包括:
根据计算路径长度d;
其中,d为两个不同点之间的路径长度,(x1,y1)为边端点1的坐标,(x2,y2)为边端点2的坐标;
根据计算危险程度w;
其中,w为所有障碍物节点对边的威胁值之和,(x1,y1)为边端点1的坐标,(x2,y2)为所述边端点2的坐标,(xi,yi)为障碍物节点的坐标,d1表示障碍物节点到边端点1的第一路径的长度,d2表示障碍物节点到边端点2的第二路径的长度。
可选的,所述根据粒子群算法确定最短路径,具体包括:
构建粒子群,并初始化所述粒子群的点最优解,所述粒子群中的点最优解对应所述维诺图中的路径,每个所述路径包括第一目标函数值和第二目标函数值;
判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值,获得第一判断结果;
若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值,则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值,获得第二判断结果;
若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值,则选取所述第一路径作为点最优解;
若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值,则随机选取所述第一路径或者所述第二路径作为点最优解;
若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值,则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值,获得第三判断结果;
若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值,则选取所述第二路径作为点最优解;
若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值,则随机选取所述第一路径或者所述第二路径作为点最优解;
根据所述第三判断结果更新所述粒子群的速度和位置;
判断迭代次数是否小于迭代阈值,获得第四判断结果;
若所述第四判断结果表示所述迭代次数小于迭代阈值,则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值,获得第一判断结果;
若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值,则根据当前点最优解确定最短路径。
为实现上述目的,本发明提供了如下方案:
一种基于拓扑地图的机器人路径规划***,所述***包括:
散点图获取模块,用于获取散点图,并在所述散点图中设置机器人的初始运动点和目标点,所述散点图是由包含有障碍物节点的实际环境抽象而成;
维诺图获取模块,用于根据所述散点图构建与所述散点图对应的维诺图;
行驶代价获取模块,用于计算所述维诺图中各边的行驶代价;
路径获取模块,用于根据所述行驶代价采用改进的迪杰斯特拉算法,得到多条路径;
最短路径确定模块,用于根据粒子群算法从多条所述路径中确定最短路径。
可选的,所述维诺图获取模块,具体包括:
三角网单元,用于依次连接所述散点图中距离最近的三个障碍物节点,得到多个三角形,各所述三角形的外接圆中不含有其他障碍物节点,多个所述三角形构成三角网;
网格图单元,对所述三角网中每条边作中垂线,得到各所述中垂线相交构成的网格图,所述网格图为维诺图。
可选的,所述行驶代价包括路径长度和危险程度。
可选的,所述行驶代价获取模块,具体包括:
路径长度确定单元,用于根据计算路径长度d;
其中,d为两个不同点之间的路径长度,(x1,y1)为边端点1的坐标,(x2,y2)为边端点2的坐标;
危险程度确定单元,用于根据计算危险程度w;
其中,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;
其中,d为两个不同点之间的路径长度,(x1,y1)为边端点1的坐标,(x2,y2)为边端点2的坐标;
根据计算危险程度w;
其中,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;
其中,d为两个不同点之间的路径长度,(x1,y1)为边端点1的坐标,(x2,y2)为边端点2的坐标;
危险程度确定单元,用于根据计算危险程度w;
其中,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对应概率即网格中点越多,对应概率则越低。然后根据轮盘赌算法从这m个网格中选出最不拥挤的网格,在选中网格中随机选取一个解作为当前全局最优解gBest。本例中划分网格时取x=1,y=1。
步骤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中已将把原问题的可行解集映射到了二进制序列空间,所以还要把算法最终解转化为原问题的可行解形式,也就是一个最短路径。最终得到的最短路径是①③⑦加上机器人起点和机器人终点,最终规划路径如图7所示。
本说明书中各个实施例采用递进的方式描述,每个实施例重点说明的都是与其他实施例的不同之处,各个实施例之间相同相似部分互相参见即可。对于实施例公开的***而言,由于其与实施例公开的方法相对应,所以描述的比较简单,相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处。综上所述,本说明书内容不应理解为对本发明的限制。
Claims (4)
1.一种基于拓扑地图的机器人路径规划方法,其特征在于,所述方法包括:
获取散点图,并在所述散点图中设置机器人的初始运动点和目标点,所述散点图是由包含有障碍物节点的实际环境抽象而成;
根据所述散点图构建与所述散点图对应的维诺图;
计算所述维诺图中各边的行驶代价;所述行驶代价包括路径长度和危险程度;所述计算所述维诺图中各边的行驶代价,具体包括:
根据计算路径长度d;
其中,d为两个不同点之间的路径长度,(x1,y1)为边端点1的坐标,(x2,y2)为边端点2的坐标;
根据计算危险程度w;
其中,w为所有障碍物节点对边的威胁值之和,(x1,y1)为边端点1的坐标,(x2,y2)为所述边端点2的坐标,(xi,yi)为障碍物节点的坐标,d1表示障碍物节点到边端点1的第一路径的长度,d2表示障碍物节点到边端点2的第二路径的长度;
根据所述行驶代价采用改进的迪杰斯特拉算法,得到多条路径;
根据粒子群算法从多条所述路径中确定最短路径,具体包括:
构建粒子群,并初始化所述粒子群的点最优解,所述粒子群中的点最优解对应所述维诺图中的路径,每个所述路径包括第一目标函数值和第二目标函数值;路径的第一目标函数值表示该路径经过的所有边的路径长度之和;路径的第二目标函数值表示该路径经过的所有边的危险程度之和;
判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值,获得第一判断结果;
若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值,则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值,获得第二判断结果;
若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值,则选取所述第一路径作为点最优解;
若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值,则随机选取所述第一路径或者所述第二路径作为点最优解;
若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值,则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值,获得第三判断结果;
若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值,则选取所述第二路径作为点最优解;
若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值,则随机选取所述第一路径或者所述第二路径作为点最优解;
根据所述第三判断结果更新所述粒子群的速度和位置;
判断迭代次数是否小于迭代阈值,获得第四判断结果;
若所述第四判断结果表示所述迭代次数小于迭代阈值,则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值,获得第一判断结果;
若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值,则根据当前点最优解确定最短路径。
2.根据权利要求1所述的基于拓扑地图的机器人路径规划方法,其特征在于,所述根据所述散点图构建与所述散点图对应的维诺图,具体包括:
依次连接所述散点图中距离最近的三个障碍物节点,得到多个三角形,各所述三角形的外接圆中不含有其他障碍物节点,各所述三角形构成三角网;
对所述三角网中每条边作中垂线,得到各所述中垂线相交构成的网格图,所述网格图为维诺图。
3.一种基于拓扑地图的机器人路径规划***,其特征在于,所述***包括:
散点图获取模块,用于获取散点图,并在所述散点图中设置机器人的初始运动点和目标点,所述散点图是由包含有障碍物节点的实际环境抽象而成;
维诺图获取模块,用于根据所述散点图构建与所述散点图对应的维诺图;
行驶代价获取模块,用于计算所述维诺图中各边的行驶代价;所述行驶代价包括路径长度和危险程度;所述行驶代价获取模块,具体包括:
路径长度确定单元,用于根据计算路径长度d;
其中,d为两个不同点之间的路径长度,(x1,y1)为边端点1的坐标,(x2,y2)为边端点2的坐标;
危险程度确定单元,用于根据计算危险程度w;
其中,w为所有障碍物节点对边的威胁值之和,(x1,y1)为边端点1的坐标,(x2,y2)为所述边端点2的坐标,(xi,yi)为障碍物节点的坐标,d1表示障碍物节点到边端点1的第一路径长度,d2表示障碍物节点到边端点2的第二路径长度;
路径获取模块,用于根据所述行驶代价采用改进的迪杰斯特拉算法,得到多条路径;
最短路径确定模块,用于根据粒子群算法从多条所述路径中确定最短路径;
所述最短路径确定模块,具体包括:
粒子群构建单元,用于构建粒子群,并初始化所述粒子群的点最优解,所述粒子群中的点最优解对应所述维诺图中的路径,各所述路径包括第一目标函数值和第二目标函数值;路径的第一目标函数值表示该路径经过的所有边的路径长度之和;路径的第二目标函数值表示该路径经过的所有边的危险程度之和;
第一判断结果获取单元,用于判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值,获得第一判断结果;
第二判断结果获取单元,用于若所述第一判断结果表示所述第一路径的第一目标函数值小于所述第二路径的第一目标函数值,则判断所述第一路径的第二目标函数值是否小于所述第二路径的第二目标函数值,获得第二判断结果;
第一最优解单元,用于若所述第二判断结果表示所述第一路径的第二目标函数值小于所述第二路径的第二目标函数值,则选取所述第一路径作为点最优解;
第二最优解单元,用于若所述第二判断结果表示所述第一路径的第二目标函数值大于或等于所述第二路径的第二目标函数值,则随机选取所述第一路径或者所述第二路径作为点最优解;
第三判断结果获取单元,用于若所述第一判断结果表示所述第一路径的第一目标函数值大于或等于所述第二路径的第一目标函数值,则判断所述第一路径的第二目标函数值是否大于所述第二路径的第二目标函数值,获得第三判断结果;
第三最优解单元,用于若所述第三判断结果表示所述第一路径的第二目标函数值大于所述第二路径的第二目标函数值,则选取所述第二路径作为点最优解;
第四最优解单元,用于若所述第三判断结果表示所述第一路径的第二目标函数值小于或等于所述第二路径的第二目标函数值,则随机选取所述第一路径或者所述第二路径作为点最优解;
更新单元,用于根据所述第三判断结果更新所述粒子群的速度和位置;
第四判断结果获取单元,用于判断迭代次数是否小于迭代阈值,获得第四判断结果;
重新判断单元,用于若所述第四判断结果表示所述迭代次数小于迭代阈值,则重新判断所述第一路径的第一目标函数值是否小于所述第二路径的第一目标函数值,获得第一判断结果;
最短路径确定单元,用于若所述第四判断结果表示所述迭代次数大于或者等于迭代阈值,则根据当前点最优解确定最短路径。
4.根据权利要求3所述的基于拓扑地图的机器人路径规划***,其特征在于,所述维诺图获取模块,具体包括:
三角网单元,用于依次连接所述散点图中距离最近的三个障碍物节点,得到多个三角形,各所述三角形的外接圆中不含有其他障碍物节点,多个所述三角形构成三角网;
网格图单元,对所述三角网中每条边作中垂线,得到各所述中垂线相交构成的网格图,所述网格图为维诺图。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810393365.3A CN108664022B (zh) | 2018-04-27 | 2018-04-27 | 一种基于拓扑地图的机器人路径规划方法及*** |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201810393365.3A CN108664022B (zh) | 2018-04-27 | 2018-04-27 | 一种基于拓扑地图的机器人路径规划方法及*** |
Publications (2)
Publication Number | Publication Date |
---|---|
CN108664022A CN108664022A (zh) | 2018-10-16 |
CN108664022B true CN108664022B (zh) | 2023-09-05 |
Family
ID=63781322
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201810393365.3A Active CN108664022B (zh) | 2018-04-27 | 2018-04-27 | 一种基于拓扑地图的机器人路径规划方法及*** |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN108664022B (zh) |
Families Citing this family (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109668572B (zh) * | 2018-12-28 | 2022-05-17 | 芜湖哈特机器人产业技术研究院有限公司 | 一种基于floyd算法的激光叉车路径搜索方法 |
CN109947129A (zh) * | 2019-03-29 | 2019-06-28 | 华南理工大学 | 基于Dijkstra与改进粒子群算法的旋翼无人机路径规划方法 |
CN110057361B (zh) * | 2019-04-23 | 2020-10-09 | 河南宙合网络科技有限公司 | 一种基于gps的最短路径规划方法 |
CN110427032B (zh) * | 2019-08-12 | 2022-07-22 | 湘潭大学 | 一种基于流动型数据采集点的农业数据采集方法及*** |
CN111209626B (zh) * | 2020-01-09 | 2023-04-21 | 北京林业大学 | 一种确定沙障铺设路径的方法和*** |
CN111752302B (zh) * | 2020-02-26 | 2021-09-10 | 广州极飞科技股份有限公司 | 路径规划方法、装置、电子设备和计算机可读存储介质 |
CN111447005B (zh) * | 2020-03-26 | 2022-03-04 | 北京卫星信息工程研究所 | 软件定义卫星网络的链路规划方法和装置 |
CN111860880B (zh) * | 2020-06-01 | 2024-05-24 | 北京骑胜科技有限公司 | 一种路径确定方法、装置、电子设备和存储介质 |
CN111620023B (zh) * | 2020-06-04 | 2021-08-10 | 南京音飞峰云科技有限公司 | 基于动态边权值拓扑图实现密集库设备路径规划的方法 |
CN112070165A (zh) * | 2020-09-09 | 2020-12-11 | 深圳市城市规划设计研究院有限公司 | 一种基于三角拓展的哈密顿路径快速求解方法 |
CN112344941B (zh) * | 2020-11-06 | 2022-04-26 | 杭州国辰机器人科技有限公司 | 路径规划方法、***、机器人及存储介质 |
CN112783169B (zh) * | 2020-12-31 | 2022-11-01 | 科大讯飞(苏州)科技有限公司 | 一种路径规划方法、设备及计算机可读存储介质 |
CN112911535A (zh) * | 2021-02-01 | 2021-06-04 | 合肥工业大学 | 基于Dirichlet顶点的水下AUV路径形成方法 |
CN114255241B (zh) * | 2021-11-16 | 2023-10-20 | 鹏城实验室 | 用于路径规划的区域分割方法、装置、设备及存储介质 |
CN114065520B (zh) * | 2021-11-17 | 2022-07-05 | 西安理工大学 | 一种鱼类洄游通道确定方法及*** |
CN114115286B (zh) * | 2021-12-06 | 2024-03-19 | 国网山东省电力公司汶上县供电公司 | 变电站机器人巡检***与方法 |
CN115265577B (zh) * | 2022-09-30 | 2023-02-10 | 北京智行者科技股份有限公司 | 基于维诺图的拓扑关系构建方法、***及移动工具 |
CN115930969B (zh) * | 2023-01-09 | 2023-06-02 | 季华实验室 | 移动机器人的路径规划方法、装置、电子设备及存储介质 |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2005032196A (ja) * | 2003-07-11 | 2005-02-03 | Japan Science & Technology Agency | 移動ロボット用経路計画システム |
CN101122974A (zh) * | 2007-09-13 | 2008-02-13 | 北京航空航天大学 | 基于Voronoi图和蚁群优化算法的无人机航路规划方法 |
CN101387888A (zh) * | 2008-09-27 | 2009-03-18 | 江南大学 | 基于二进制量子粒子群算法的移动机器人路径规划方法 |
KR20100137091A (ko) * | 2009-06-22 | 2010-12-30 | 재단법인대구경북과학기술원 | 로봇을 위한 지형 데이터 저장 방법 및 그 장치 |
CN102129249A (zh) * | 2011-01-10 | 2011-07-20 | 中国矿业大学 | 一种危险源环境下的机器人全局路径规划方法 |
CN102880186A (zh) * | 2012-08-03 | 2013-01-16 | 北京理工大学 | 基于稀疏a*算法和遗传算法的航迹规划方法 |
US8793200B1 (en) * | 2009-09-22 | 2014-07-29 | Hrl Laboratories, Llc | Method for particle swarm optimization with random walk |
CN106444835A (zh) * | 2016-10-11 | 2017-02-22 | 哈尔滨工程大学 | 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 |
CN106650298A (zh) * | 2017-01-11 | 2017-05-10 | 湘潭大学 | 基于环境特征和随机方法的柔性针路径规划方法 |
CN106705970A (zh) * | 2016-11-21 | 2017-05-24 | 中国航空无线电电子研究所 | 一种基于蚁群算法的多无人机协同路径规划方法 |
CN107065876A (zh) * | 2017-04-28 | 2017-08-18 | 西北工业大学 | 基于改进粒子群优化的移动机器人路径规划方法 |
-
2018
- 2018-04-27 CN CN201810393365.3A patent/CN108664022B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2005032196A (ja) * | 2003-07-11 | 2005-02-03 | Japan Science & Technology Agency | 移動ロボット用経路計画システム |
CN101122974A (zh) * | 2007-09-13 | 2008-02-13 | 北京航空航天大学 | 基于Voronoi图和蚁群优化算法的无人机航路规划方法 |
CN101387888A (zh) * | 2008-09-27 | 2009-03-18 | 江南大学 | 基于二进制量子粒子群算法的移动机器人路径规划方法 |
KR20100137091A (ko) * | 2009-06-22 | 2010-12-30 | 재단법인대구경북과학기술원 | 로봇을 위한 지형 데이터 저장 방법 및 그 장치 |
US8793200B1 (en) * | 2009-09-22 | 2014-07-29 | Hrl Laboratories, Llc | Method for particle swarm optimization with random walk |
CN102129249A (zh) * | 2011-01-10 | 2011-07-20 | 中国矿业大学 | 一种危险源环境下的机器人全局路径规划方法 |
CN102880186A (zh) * | 2012-08-03 | 2013-01-16 | 北京理工大学 | 基于稀疏a*算法和遗传算法的航迹规划方法 |
CN106444835A (zh) * | 2016-10-11 | 2017-02-22 | 哈尔滨工程大学 | 基于Lazy Theta星和粒子群混合算法的水下潜器三维路径规划方法 |
CN106705970A (zh) * | 2016-11-21 | 2017-05-24 | 中国航空无线电电子研究所 | 一种基于蚁群算法的多无人机协同路径规划方法 |
CN106650298A (zh) * | 2017-01-11 | 2017-05-10 | 湘潭大学 | 基于环境特征和随机方法的柔性针路径规划方法 |
CN107065876A (zh) * | 2017-04-28 | 2017-08-18 | 西北工业大学 | 基于改进粒子群优化的移动机器人路径规划方法 |
Non-Patent Citations (1)
Title |
---|
郭国强.基于空间约束的路径规划与视景仿真研究.中国优秀硕士学位论文全文数据库工程科技Ⅱ辑.2012,(第3期),C034-4. * |
Also Published As
Publication number | Publication date |
---|---|
CN108664022A (zh) | 2018-10-16 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN108664022B (zh) | 一种基于拓扑地图的机器人路径规划方法及*** | |
Hu et al. | Voronoi-based multi-robot autonomous exploration in unknown environments via deep reinforcement learning | |
CN111240319B (zh) | 室外多机器人协同作业***及其方法 | |
CN104050390B (zh) | 一种基于可变维粒子群膜算法的移动机器人路径规划方法 | |
CN110147101B (zh) | 一种基于深度强化学习的端到端分布式多机器人编队导航方法 | |
Renzaglia et al. | Multi-robot three-dimensional coverage of unknown areas | |
Wang et al. | Autonomous robotic exploration by incremental road map construction | |
Jun et al. | Multi-objective mobile robot path planning based on improved genetic algorithm | |
CN112462803B (zh) | 一种基于改进nsga-ii的无人机路径规划方法 | |
Kim et al. | PLGRIM: Hierarchical value learning for large-scale exploration in unknown environments | |
CN110544296A (zh) | 一种敌方威胁不确定环境下无人机三维全局航迹智能规划方法 | |
CN109597425B (zh) | 基于强化学习的无人机导航和避障方法 | |
CN116954233A (zh) | 一种巡检任务与航线自动匹配方法 | |
CN112947591A (zh) | 基于改进蚁群算法的路径规划方法、装置、介质及无人机 | |
Sahoo et al. | Navigational path planning of multi-robot using honey bee mating optimization algorithm (HBMO) | |
CN112484732B (zh) | 一种基于ib-abc算法的无人机飞行路径规划方法 | |
CN112415997A (zh) | 一种用于多机器人协作的路径规划方法及*** | |
CN115454062A (zh) | 一种基于贝兹曲线的机器人动态路径规划方法及*** | |
Li et al. | Path planning of mobile robot based on improved td3 algorithm | |
CN114815801A (zh) | 一种基于策略-价值网络及mcts的自适应环境路径规划方法 | |
Tusi et al. | Using ABC and RRT algorithms to improve mobile robot path planning with danger degree | |
CN108227718B (zh) | 一种自适应切换的自动搬运小车路径规划方法 | |
CN115576333B (zh) | 最优向避障策略 | |
Li et al. | Mobile Robot Navigation Algorithm Based on Ant Colony Algorithm with A* Heuristic Method | |
CN114397894B (zh) | 一种模仿人类记忆的移动机器人目标搜索方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |