Do you know PythonRopbotics? This is an OSS that writes robot algorithms in Python, which is also posted on arXiv. There is also his blog. Since I started researching robots, I have read it carefully and used it as a reference.
As I wrote in the article here, there is a potential method in the robot path planning method. It is used for avoiding obstacles of robots, etc. It is a method of defining a potential function for obstacles and destinations and taking a route to the destination along the gradient of the potential function.
In this article, I will explain in my own way for the purpose of understanding the program of potential method in Python Robotics.
Reference paper for the potential method https://www.mhi.co.jp/technology/review/pdf/511/511040.pdf
I'm importing numpy and matplotlib. As a parameter ・ Weight of destination potential ・ Weight of obstacle potential ・ Width of area for calculating potential method Is specified.
In the main function, the start, goal, obstacle position, potential method grid, and robot size are specified. You can see this in the comments.
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()
This function calculates the entire potential field.
The argument of this function is ・ Goal coordinates (x, y) ・ Obstacle coordinates (x, y) ·grid ・ Robot size It has become.
ox, oy contains a list of obstacle coordinates. Therefore, minx, y and maxx, y are as follows.
minx,y =The smallest coordinates of obstacles-Area width/ 2 \\
maxx,y =The largest coordinate of obstacles-Area width/ 2
Using them, xw, yw used for each calculation range of x and y is output.
Next, prepare an array (`` `pmap```) to store the potential from the above values and grid.
After that, the potential from the goal (ug) and the potential due to obstacles (uo) are calculated, and the total (uf) is calculated for each coordinate. It is stored in `` `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
This function is a function that calculates the gravitational potential from the goal. It simply uses numpy's hypot to get the distance between the two points and weight them.
This function is a function that calculates the repulsive force potential due to obstacles.
For each of the ox and oy elements, use numpy's hypot to calculate the distance between the two points, and for the smallest element (here linked by an index (`` `minid)) We are calculating the distance between two points ( dq```).
And if `` `dq``` is less than or equal to the size of the robot If it is 0.1 or less, set it to 0.1 and substitute it in the following formula. The return value of this equation is the potential due to obstacles.
0.5 × ETA(Potential weight) × (\frac{1}{dq} - \frac{1}{rr})^2
Also, if `` `dq``` is larger than the size of the robot, the potential due to obstacles will be 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
This is the function that calculates the path planning part of the potential method.
Receive pmap, minx, miny from the potential_field_planning function above.
The coordinates ʻix, iy, gix, giycalculated as the distance between the start and goald are calculated in the part directly below the # search path of the comment. (I don't fully understand the formulas for ʻix, iy, gix, giy ... I'll fix it later)
After that, there is a loop from the part of while d> = reso to the goal by setting animation.
The get_motiobn_model function is a function that returns an array of movements, and moves up, down, left, and right on the x and y coordinates.
The following part.
inx = int(ix + motion[i][0])
iny = int(iy + motion[i][1])
The potential of these ʻinx, inycoordinates is obtained frompmap`, and where to go up, down, left, and right to find the smallest potential is obtained, and then the direction is reached.
The distance between the coordinates and the goal after traveling is calculated, and it is repeated until the distance becomes smaller than the value of the grid.
That is all for the explanation of the moving parts.
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
There are some parts that I haven't fully understood yet, but I tried to understand it like this. If you find something wrong, I would appreciate it if you could comment.
Python Robotics page https://github.com/AtsushiSakai/PythonRobotics
The full text of potential_field_planning can be found here. https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/PotentialFieldPlanning/potential_field_planning.py
↑ The blog that the person is doing https://myenigma.hatenablog.com/
Recommended Posts