[PYTHON] Führen Sie mit Networkx eine Pfadberechnung in einem zweidimensionalen Raster durch
Es schien einfach zu sein, die Python-Bibliothek Networkx für eine einfache Pfadberechnung zu verwenden, daher habe ich versucht, sie zu verwenden.
Betrachten Sie zunächst die folgende zweidimensionale Gitterumgebung. Die Rastergröße beträgt 20x20.
import networkx as nx
import matplotlib.pyplot as plt
ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
nx.draw(gp,
pos=dict((n, n) for n in gp.nodes()),
node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
node_size=200)
plt.axis('equal')
plt.show()
Ich wollte auch, dass der gitterartige diagonale Pfad in Ordnung ist, also füge ich jedem Knoten diagonale Kanten hinzu.
import networkx as nx
import matplotlib.pyplot as plt
def add_cross_edge(gp, shape):
"""
Fügen Sie einem 2D-Rasterdiagramm diagonale Kanten hinzu
"""
for node in gp.nodes_iter():
nx_node = (node[0] + 1, node[1] + 1)
if nx_node[0] < shape[0] and nx_node[1] < shape[1]:
gp.add_edge(node, nx_node)
nx_node = (node[0] + 1, node[1] - 1)
if nx_node[0] < shape[0] and nx_node[1] >= 0:
gp.add_edge(node, nx_node)
ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
add_cross_edge(gp, [ngrid, ngrid])
nx.draw(gp,
pos=dict((n, n) for n in gp.nodes()),
node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
node_size=200)
plt.axis('equal')
plt.show()
Setzen Sie schließlich Hindernisse und starten Sie Ziele, und Sie sind mit dem Aufbau der Umgebung selbst fertig. Hier sind Hindernisse in Schwarz farbcodiert, beginnen in Grün und Ziele in Rot. In dieser Umgebung berechnen wir den Weg zwischen Start und Ziel und vermeiden dabei Hindernisse.
import networkx as nx
import matplotlib.pyplot as plt
def add_cross_edge(gp, shape):
"""
Fügen Sie einem 2D-Rasterdiagramm diagonale Kanten hinzu
"""
for node in gp.nodes_iter():
nx_node = (node[0] + 1, node[1] + 1)
if nx_node[0] < shape[0] and nx_node[1] < shape[1]:
gp.add_edge(node, nx_node)
nx_node = (node[0] + 1, node[1] - 1)
if nx_node[0] < shape[0] and nx_node[1] >= 0:
gp.add_edge(node, nx_node)
ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
add_cross_edge(gp, [ngrid, ngrid])
idcs = np.random.choice(len(gp.nodes()), int(ngrid * ngrid * 0.2), replace=False)
#Start / Ziel / Hindernis einstellen
st, gl, obs = gp.nodes()[idcs[0]], gp.nodes()[idcs[1]], [gp.nodes()[i] for i in idcs[2:]]
gp.node[st]['color'] = 'green'
gp.node[gl]['color'] = 'red'
for o in obs:
gp.node[o]['color'] = 'black'
nx.draw(gp,
pos=dict((n, n) for n in gp.nodes()),
node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
node_size=200)
plt.axis('equal')
plt.show()
Stellen Sie dann zwischen jeder Kante ein Gewicht ein, um Hindernissen auszuweichen. Ich versuche eine Strafe zu verhängen, wenn der Knoten ein Hindernis trägt.
import numpy as np
def cost(a, b):
"""
Kostenfunktion
"""
x1 = np.array(a, dtype=np.float32)
x2 = np.array(b, dtype=np.float32)
dist = np.linalg.norm(x1 - x2)
if any([(a == o or b == o) for o in obs]):
penalty = 1.0e6
else:
penalty = 0.0
return dist + penalty
for u, v, d in gp.edges_iter(data=True):
d['weight'] = cost(u, v)
In Networkx gibt es mehrere Pfadsuchfunktionen, diesmal verwenden wir jedoch den A * -Algorithmus. Der A * -Algorithmus erfordert eine sogenannte heuristische Funktion, die wie folgt eingestellt ist.
def dist(a, b):
"""
Hulistische Funktion
"""
x1 = np.array(a, dtype=np.float32)
x2 = np.array(b, dtype=np.float32)
return np.linalg.norm(x1 - x2)
path = nx.astar_path(gp, st, gl, dist)
length = nx.astar_path_length(gp, st, gl, dist)
print(path)
print(length)
Zum Schluss zeichnen wir das Ergebnis der Pfadsuche. Die Knoten auf dem Pfad werden blau angezeigt.
for p in path[1:-1]:
if gp.node[p].get('color', '') == 'black':
print('Invalid path')
continue
gp.node[p]['color'] = 'blue'
nx.draw(gp,
pos=dict((n, n) for n in gp.nodes()),
node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
node_size=200)
plt.axis('equal')
plt.show()
Der gesamte Code sieht folgendermaßen aus:
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import networkx as nx
import matplotlib.pyplot as plt
def add_cross_edge(gp, shape):
"""
Fügen Sie einem 2D-Rasterdiagramm diagonale Kanten hinzu
"""
for node in gp.nodes_iter():
nx_node = (node[0] + 1, node[1] + 1)
if nx_node[0] < shape[0] and nx_node[1] < shape[1]:
gp.add_edge(node, nx_node)
nx_node = (node[0] + 1, node[1] - 1)
if nx_node[0] < shape[0] and nx_node[1] >= 0:
gp.add_edge(node, nx_node)
ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
add_cross_edge(gp, [ngrid, ngrid])
idcs = np.random.choice(len(gp.nodes()), int(ngrid * ngrid * 0.2), replace=False)
#Start / Ziel / Hindernis einstellen
st, gl, obs = gp.nodes()[idcs[0]], gp.nodes()[idcs[1]], [gp.nodes()[i] for i in idcs[2:]]
gp.node[st]['color'] = 'green'
gp.node[gl]['color'] = 'red'
for o in obs:
gp.node[o]['color'] = 'black'
def dist(a, b):
"""
Hulistische Funktion
"""
x1 = np.array(a, dtype=np.float32)
x2 = np.array(b, dtype=np.float32)
return np.linalg.norm(x1 - x2)
def cost(a, b, k1=1.0, k2=10.0, kind='intsct'):
"""
Kostenfunktion
"""
x1 = np.array(a, dtype=np.float32)
x2 = np.array(b, dtype=np.float32)
dist = np.linalg.norm(x1 - x2)
if any([(a == o or b == o) for o in obs]):
penalty = 1.0e6
else:
penalty = 0.0
return dist + penalty
for u, v, d in gp.edges_iter(data=True):
d['weight'] = cost(u, v)
path = nx.astar_path(gp, st, gl, dist)
length = nx.astar_path_length(gp, st, gl, dist)
print(path)
print(length)
for p in path[1:-1]:
if gp.node[p].get('color', '') == 'black':
print('Invalid path')
continue
gp.node[p]['color'] = 'blue'
nx.draw(gp,
pos=dict((n, n) for n in gp.nodes()),
node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
node_size=200)
plt.axis('equal')
plt.show()
Networkx ist praktisch. Die Umgebung mit Hindernissen des zweidimensionalen Gitters scheint diesmal für das verstärkte Lernen nutzbar zu sein.