拓扑图_路径查询

graph = {'A': ['B', 'C','D'],
         'B': [ 'E'],
         'C': ['D','F'],
         'D': ['B','E','G'],
         'E': [],
         'F': ['D','G'],
         'G': ['E']}
import networkx as nx
import numpy as np
import matplotlib.pyplot as plt

G = nx.DiGraph()
G.add_edges_from(
    [('A', 'B'), ('A', 'C'),('A','D'),('B','E'),('C','D'),('C','F'),('D','B'),('D','E'),('D','G'),('F','D'),('F','G'),('G','E')])

nx.draw(G,with_labels=True)
plt.show()

在这里插入图片描述

# 找到一条从start到end的路径
def findPath(graph,start,end,path=[]):   
    path = path + [start]
    if start == end:
        return path 
    for node in graph[start]:
        if node not in path:
            newpath = findPath(graph,node,end,path)
            if newpath:
                return newpath
    return None
 
# 找到所有从start到end的路径
def findAllPath(graph,start,end,path=[]):
    path = path +[start]
    if start == end:
        return [path]
 
    paths = [] #存储所有路径    
    for node in graph[start]:
        if node not in path:
            newpaths = findAllPath(graph,node,end,path) 
            for newpath in newpaths:
                paths.append(newpath)
    return paths
 
# 查找最短路径
def findShortestPath(graph,start,end,path=[]):
    path = path +[start]
    if start == end:
        return path
    
    shortestPath = []
    for node in graph[start]:
        if node not in path:
            newpath = findShortestPath(graph,node,end,path)
            if newpath:
                if not shortestPath or len(newpath)<len(shortestPath):
                    shortestPath = newpath
    return shortestPath
 

onepath = findPath(graph,'A','G')
print('一条路径:',onepath)
 
allpath = findAllPath(graph,'A','G')
print('\n所有路径:',allpath)
 
shortpath = findShortestPath(graph,'A','G')
print('\n最短路径:',shortpath)

一条路径: ['A', 'C', 'D', 'G']

所有路径: [['A', 'C', 'D', 'G'], ['A', 'C', 'F', 'D', 'G'], ['A', 'C', 'F', 'G'], ['A', 'D', 'G']]

最短路径: ['A', 'D', 'G']
  • 0
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
栅格地图和拓扑地图都是常见的用于机器人路径规划的地图表示方法。下面分别介绍常见的路径规划算法。 1. 栅格地图路径规划算法 常见的栅格地图路径规划算法包括 A*算法、Dijkstra算法和模拟退火算法等。 A*算法是一种启发式搜索算法,它通过估计每个节点到目标节点的距离来指导搜索方向,从而减少搜索的时间和空间复杂度。在栅格地图,每个栅格被看作一个节点,栅格之间的距离为1。A*算法在搜索过程考虑栅格的代价,代价包括从起点到当前栅格的代价和从当前栅格到目标栅格的估计代价。通过优先考虑估计代价较小的栅格,A*算法能够找到一条最优路径。 Dijkstra算法是一种无权图最短路径算法,它通过从起点开始逐步扩展搜索范围,直到找到目标节点为止。在栅格地图,可以将每个栅格看作一个无权图上的节点,Dijkstra算法可以用于寻找起点到目标栅格的最短路径。 模拟退火算法是一种随机化搜索算法,它通过接受有一定概率的差解来避免局部最优解。在栅格地图,模拟退火算法可以通过随机生成路径,然后根据路径长度和目标距离进行优化,最终得到一条较优路径。 2. 拓扑地图路径规划算法 常见的拓扑地图路径规划算法包括D* Lite算法、RRT算法和PRM算法等。 D* Lite算法是一种增量路径规划算法,它通过不断更新路径,使得路径能够适应环境的变化。在拓扑地图,D* Lite算法可以通过将地图表示成一个图,每个节点表示一个拓扑位置,边表示在两个拓扑位置之间的运动。通过不断更新起点和终点之间的路径,D* Lite算法能够适应环境的变化,得到一个较优的路径。 RRT算法是一种随机化路径规划算法,它通过随机采样机器人运动空间的点,然后在树结构不断扩展,得到一条从起点到目标点的路径。在拓扑地图,RRT算法可以通过将地图表示成一个图,每个节点表示一个拓扑位置,边表示在两个拓扑位置之间的运动。通过随机采样和树结构扩展,RRT算法也能够得到一条较优路径。 PRM算法是一种基于采样的路径规划算法,它通过在机器人运动空间随机采样一些点,并以这些点为基础建立一个图,然后通过图搜索得到一条较优路径。在拓扑地图,PRM算法可以通过将地图表示成一个图,每个节点表示一个拓扑位置,边表示在两个拓扑位置之间的运动。通过随机采样和图搜索,PRM算法能够得到一条较优路径
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值