Connaissez-vous PythonRopbotics? Il s'agit d'un OSS qui écrit des algorithmes de robot en Python, qui est également posté sur arXiv. Il y a aussi son blog. Depuis que j'ai commencé mes recherches sur les robots, je l'ai lu attentivement et je l'ai utilisé comme référence.
Comme je l'ai écrit dans l'article ici, il existe une méthode potentielle dans la méthode de planification de chemin pour les robots. Il est utilisé pour éviter les obstacles des robots, etc. C'est une méthode pour définir une fonction potentielle pour les obstacles et les destinations et pour prendre un itinéraire vers la destination le long du gradient de la fonction potentielle.
Dans cet article, je vais m'expliquer moi-même dans le but de comprendre le programme de méthode potentiel dans Python Robotics.
Document de référence de la méthode potentielle https://www.mhi.co.jp/technology/review/pdf/511/511040.pdf
J'importe numpy et matplotlib. En tant que paramètre ・ Poids du potentiel de destination ・ Poids du potentiel d'obstacle ・ Largeur de la zone pour calculer la méthode du potentiel Est spécifié.
Dans la fonction principale, le départ, l'objectif, la position de l'obstacle, la grille de méthode potentielle et la taille du robot sont spécifiés. Vous pouvez le voir dans les commentaires.
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()
Cette fonction calcule l'ensemble du champ potentiel.
L'argument de cette fonction est ・ Coordonnées de l'objectif (x, y) ・ Coordonnées d'obstacles (x, y) ·la grille ・ Taille du robot Il est devenu.
ox, oy contient une liste de coordonnées d'obstacles. Par conséquent, minx, y et maxx, y sont les suivants.
minx,y =La plus petite des coordonnées d'obstacle-Largeur de la zone/ 2 \\
maxx,y =La plus grande des coordonnées d'obstacle-Largeur de la zone/ 2
En les utilisant, nous générons «xw, yw» utilisé pour chaque plage de calcul de x et y. Ensuite, préparez un tableau (`` pmap '') qui stocke le potentiel des valeurs et de la grille ci-dessus.
Après cela, le potentiel du but (ug) et le potentiel dû aux obstacles (uo) sont calculés, et le total (uf) d'entre eux est calculé pour chaque coordonnée. Il est stocké dans `` pmap ''.
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
Cette fonction est une fonction qui calcule le potentiel de force attractive du but. Il utilise simplement l'hypot de numpy pour calculer la distance entre les deux points et les pondérer.
Cette fonction est une fonction permettant de calculer le potentiel de force répulsive dû aux obstacles.
Pour chacun des éléments ox et oy, utilisez l'hypot de numpy pour calculer la distance entre les deux points, et pour le plus petit élément (ici lié par l'indice ( minid '')) La distance entre deux points ( `` dq```) est calculée.
Et si `` dq '' est inférieur ou égal à la taille du robot S'il est inférieur ou égal à 0,1, définissez-le sur 0,1 et remplacez-le par la formule suivante. La valeur de retour de cette équation est le potentiel dû aux obstacles.
0.5 × ETA(Poids potentiel) × (\frac{1}{dq} - \frac{1}{rr})^2
De plus, si `` dq '' est plus grand que la taille du robot, le potentiel dû aux obstacles sera de 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
C'est la fonction qui calcule la partie planification de chemin de la méthode potentielle.
Recevez pmap, minx, miny de la fonction potential_field_planning ci-dessus.
Dans la partie directement sous le # chemin de recherche du commentaire, la distance entre le début et le but d et les coordonnées calculées ʻix, iy, gix, giy sont calculées. (Je ne comprends pas parfaitement les formules pour ʻix, iy, gix, giy ... je vais le réparer plus tard)
Après cela, il y a une boucle de la partie de while d> = reso pour atteindre l'objectif en définissant une animation.
La fonction get_motiobn_model est une fonction qui renvoie un tableau de mouvements, et se déplace vers le haut, le bas, la gauche et la droite sur les coordonnées x et y.
La partie suivante.
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
Le potentiel de ces coordonnées «inx, iny» est obtenu à partir de «pmap», et où aller vers le haut, le bas, la gauche et la droite pour trouver le plus petit potentiel est obtenu, et le processus consiste à procéder dans cette direction. La distance entre les coordonnées et le but après le déplacement est calculée et répétée jusqu'à ce que la distance devienne inférieure à la valeur de la grille.
C'est tout pour l'explication des pièces mobiles.
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
Il y a certaines parties que je n'ai pas encore complètement comprises, mais j'ai essayé de les comprendre ainsi. Si vous trouvez quelque chose qui ne va pas, je vous serais reconnaissant de bien vouloir commenter.
Page de robotique Python https://github.com/AtsushiSakai/PythonRobotics
Le texte complet de potential_field_planning peut être trouvé ici. https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
↑ Le blog que la personne fait https://myenigma.hatenablog.com/
Recommended Posts