[Python] Understanding the potential_field_planning of Python Robotics


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.

Potential method

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


main function and parameters

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:

    # path generation
    _, _ = potential_field_planning(
        sx, sy, gx, gy, ox, oy, grid_size, robot_radius)

    if show_animation:

calc_potential_field function (calculation of potential field)

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

calc_attractive_potential and calc_repulsive_potential functions (calculation of attractive / repulsive potential)

calc_attractive_potential function (calculation of gravitational potential)

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.

calc_repulsive_potential function (calculation of repulsive potential)

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
        return 0.0

potential_field_planning function

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:
        # for stopping simulation with the esc key.
                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
                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)

        if show_animation:
            plt.plot(ix, iy, ".r")


    return rx, ry

at the end

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

[Python] Understanding the potential_field_planning of Python Robotics
the zen of Python
[Python] A rough understanding of the logging module
Towards the retirement of Python2
About the ease of Python
About the features of Python
Full understanding of Python debugging
The Power of Pandas: Python
The story of Python and the story of NaN
[Python] The stumbling block of import
Existence from the viewpoint of Python
Change the Python version of Homebrew
Review of the basics of Python (FizzBuzz)
About the basics list of Python basics
Learn the basics of Python ① Beginners
A memorandum of understanding for the Python package management tool ez_setup
Change the length of Python csv strings
[Understanding in 3 minutes] The beginning of Linux
Check the behavior of destructor in Python
[Python3] Understand the basics of Beautiful Soup
Pass the path of the imported python module
The story of making Python an exe
Learning notes from the beginning of Python 1
Check the existence of the file with python
About the virtual environment of python version 3.7
[Python] Understand the content of error messages
[Python3] Rewrite the code object of the function
I didn't know the basics of Python
The result of installing python in Anaconda
[Python] Try pydash of the Python version of lodash
[python] Checking the memory consumption of variables
Check the path of the Python imported module
The story of manipulating python global variables
[python] [meta] Is the type of python a type?
The basics of running NoxPlayer in Python
Pandas of the beginner, by the beginner, for the beginner [Python]
The Python project template I think of.
In search of the fastest FizzBuzz in Python
Python Basic Course (at the end of 15)
Set the process name of the Python program
[Python] Get the character code of the file
The story of blackjack A processing (python)
Intuitively learn the reshape of Python np
Python Note: The secret role of commas
Full understanding of Python threading and multiprocessing
Learning notes from the beginning of Python 2
Japanese translation: PEP 20 --The Zen of Python
[Python3] Understand the basics of file operations
Introduction of Python
Understanding Python Coroutine
Basics of Python ①
Basics of python ①
Copy of python
Understanding python self
Introduction of Python
Get the contents of git diff from python
Output the number of CPU cores in Python
[Python] Read the source code of Bottle Part 2
Try the python version of emacs-org parser orgparse
The story of low learning costs for Python
[Python] Sort the list of pathlib.Path in natural sort