Kennen Sie PythonRopbotics? Dies ist ein OSS, das Roboteralgorithmen in Python schreibt, das auch auf arXiv veröffentlicht ist. Es gibt auch seinen Blog. Seit ich mit der Erforschung von Robotern angefangen habe, habe ich es sorgfältig gelesen und als Referenz verwendet.
Wie ich in dem Artikel hier geschrieben habe, gibt es eine mögliche Methode in der Pfadplanungsmethode für Roboter. Es wird verwendet, um Hindernisse von Robotern usw. zu vermeiden. Es ist eine Methode zum Definieren einer potenziellen Funktion für Hindernisse und Ziele und zum Aufnehmen einer Route zum Ziel entlang der Steigung der potenziellen Funktion.
In diesem Artikel werde ich es mir selbst erklären, um das mögliche Methodenprogramm in Python Robotics zu verstehen.
Referenzpapier der möglichen Methode https://www.mhi.co.jp/technology/review/pdf/511/511040.pdf
Ich importiere Numpy und Matplotlib. Als Parameter ・ Gewicht des Zielpotentials ・ Gewicht des Hindernispotentials ・ Flächenbreite zur Berechnung der potenziellen Methode Angegeben.
In der Hauptfunktion werden Start, Ziel, Hindernisposition, potenzielles Methodenraster und Robotergröße angegeben. Sie können dies in den Kommentaren sehen.
import numpy as np
import matplotlib.pyplot as plt
# Parameters
KP = 5.0 # attractive potential gain
ETA = 100.0 # repulsive potential gain
AREA_WIDTH = 30.0 # potential area width [m]
show_animation = True
def main():
print("potential_field_planning start")
sx = 0.0 # start x position [m]
sy = 10.0 # start y positon [m]
gx = 30.0 # goal x position [m]
gy = 30.0 # goal y position [m]
grid_size = 0.5 # potential grid size [m]
robot_radius = 5.0 # robot radius [m]
ox = [15.0, 5.0, 20.0, 25.0, 10.0] # obstacle x position list [m]
oy = [25.0, 15.0, 26.0, 25.0, 10.0] # obstacle y position list [m]
if show_animation:
plt.grid(True)
plt.axis("equal")
# path generation
_, _ = potential_field_planning(
sx, sy, gx, gy, ox, oy, grid_size, robot_radius)
if show_animation:
plt.show()
Diese Funktion berechnet das gesamte Potentialfeld.
Das Argument dieser Funktion ist ・ Zielkoordinaten (x, y) ・ Hinderniskoordinaten (x, y) ·Gitter ・ Robotergröße Es ist geworden.
ox, oy enthält eine Liste von Hinderniskoordinaten. Daher sind minx, y und maxx, y wie folgt.
minx,y =Die kleinste der Hinderniskoordinaten-Flächenbreite/ 2 \\
maxx,y =Die größte der Hinderniskoordinaten-Flächenbreite/ 2
Mit ihnen wird "xw, yw" ausgegeben, das für jeden Berechnungsbereich von x und y verwendet wird. Bereiten Sie als Nächstes ein Array (`` `pmap```) vor, das das Potenzial aus den obigen Werten und dem Raster speichert.
Danach werden das Potential aus dem Ziel (`ug```) und das Potential aufgrund von Hindernissen (` uo) berechnet und die Summe (`` `uf) von ihnen für jede Koordinate berechnet. Es wird in `` `pmap``` gespeichert.
def calc_potential_field(gx, gy, ox, oy, reso, rr):
minx = min(ox) - AREA_WIDTH / 2.0
miny = min(oy) - AREA_WIDTH / 2.0
maxx = max(ox) + AREA_WIDTH / 2.0
maxy = max(oy) + AREA_WIDTH / 2.0
xw = int(round((maxx - minx) / reso))
yw = int(round((maxy - miny) / reso))
# calc each potential
pmap = [[0.0 for i in range(yw)] for i in range(xw)]
for ix in range(xw):
x = ix * reso + minx
for iy in range(yw):
y = iy * reso + miny
ug = calc_attractive_potential(x, y, gx, gy)
uo = calc_repulsive_potential(x, y, ox, oy, rr)
uf = ug + uo
pmap[ix][iy] = uf
return pmap, minx, miny
Diese Funktion berechnet das Anziehungskraftpotential aus dem Ziel. Es wird einfach das Hypot von Numpy verwendet, um den Abstand zwischen den beiden Punkten zu berechnen und sie zu gewichten.
Diese Funktion ist eine Funktion zur Berechnung des Abstoßungspotentials aufgrund von Hindernissen.
Verwenden Sie für jedes der ox- und oy-Elemente das Hypot von numpy, um den Abstand zwischen den beiden Punkten zu berechnen, und für das kleinste Element (hier durch einen Index verbunden ( `minid```)). Der Abstand zwischen zwei Punkten ( `dq```) wird berechnet.
Und wenn `` `dq``` kleiner oder gleich der Größe des Roboters ist Wenn es 0,1 oder weniger ist, setzen Sie es auf 0,1 und ersetzen Sie es durch die folgende Formel. Der Rückgabewert dieser Gleichung ist das Potential aufgrund von Hindernissen.
0.5 × ETA(Mögliches Gewicht) × (\frac{1}{dq} - \frac{1}{rr})^2
Wenn `` `dq``` größer als die Größe des Roboters ist, ist das Potential aufgrund von Hindernissen 0.
def calc_attractive_potential(x, y, gx, gy):
return 0.5 * KP * np.hypot(x - gx, y - gy)
def calc_repulsive_potential(x, y, ox, oy, rr):
# search nearest obstacle
minid = -1
dmin = float("inf")
for i, _ in enumerate(ox):
d = np.hypot(x - ox[i], y - oy[i])
if dmin >= d:
dmin = d
minid = i
# calc repulsive potential
dq = np.hypot(x - ox[minid], y - oy[minid])
if dq <= rr:
if dq <= 0.1:
dq = 0.1
return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2
else:
return 0.0
Dies ist die Funktion, die den Pfadplanungsteil der potenziellen Methode berechnet.
Empfange pmap, minx, miny von der obigen Funktion Potential_Feldplanung.
Der Abstand "d" zwischen dem Start und dem Ziel und die berechneten Koordinaten "ix, iy, gix, giy" werden in dem Teil direkt unter dem "# Suchpfad" des Kommentars berechnet. (Ich verstehe die Formeln für "ix, iy, gix, giy" nicht vollständig ... Ich werde es später beheben.)
Danach gibt es eine Schleife aus dem Teil von "while d> = reso", um das Ziel durch Einstellen der Animation zu erreichen.
Die Funktion get_motiobn_model ist eine Funktion, die eine Reihe von Bewegungen zurückgibt und sich auf den x- und y-Koordinaten nach oben, unten, links und rechts bewegt.
Der folgende Teil.
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
Das Potential dieser "inx, iny" -Koordinaten wird aus "pmap" erhalten, die Position, an der das Potential am kleinsten ist, wird berechnet, und der Prozess wird in dieser Richtung ausgeführt. Die Entfernung zwischen den Koordinaten und dem Ziel nach der Fahrt wird berechnet und wiederholt, bis die Entfernung kleiner als der Wert des Gitters wird.
Das ist alles für die Erklärung der beweglichen Teile.
def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr):
# calc potential field
pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr)
# search path
d = np.hypot(sx - gx, sy - gy)
ix = round((sx - minx) / reso)
iy = round((sy - miny) / reso)
gix = round((gx - minx) / reso)
giy = round((gy - miny) / reso)
if show_animation:
draw_heatmap(pmap)
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
plt.plot(ix, iy, "*k")
plt.plot(gix, giy, "*m")
rx, ry = [sx], [sy]
motion = get_motion_model()
while d >= reso:
minp = float("inf")
minix, miniy = -1, -1
for i, _ in enumerate(motion):
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
if inx >= len(pmap) or iny >= len(pmap[0]):
p = float("inf") # outside area
else:
p = pmap[inx][iny]
if minp > p:
minp = p
minix = inx
miniy = iny
ix = minix
iy = miniy
xp = ix * reso + minx
yp = iy * reso + miny
d = np.hypot(gx - xp, gy - yp)
rx.append(xp)
ry.append(yp)
if show_animation:
plt.plot(ix, iy, ".r")
plt.pause(0.01)
print("Goal!!")
return rx, ry
Es gibt einige Teile, die ich noch nicht vollständig verstanden habe, aber ich habe versucht, es so zu verstehen. Wenn Sie etwas falsch finden, würde ich es begrüßen, wenn Sie einen Kommentar abgeben könnten.
Python Robotics-Seite https://github.com/AtsushiSakai/PythonRobotics
Den vollständigen Text von potentielle_Feldplanung finden Sie hier. https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
↑ Der Blog, den die Person macht https://myenigma.hatenablog.com/
Recommended Posts