First let’s start with some basic assumptions. We define a 2D environment of size x_max, y_max. We can easily represent this environment in Python as a Grid of size (x_max, y_max). The resolution of this Grid is not important for now, since later it is possible to convert Grid values to meters for example. We want to simulate an obstacle in a particular position of this environment, let’s say that the obstacle position will be p_obs = (x_obs, y_obs) and that the complete shape of this obstacle can be fitted inside a circle of radius = 10 grid units.

One way of representing this obstacle for potential field navigation is by using a Gaussian repulsor field in the center of the obstacle. The following code creates a Grid and then calculates the potential field for the given obstacle position.

def gaussian_obstacle(X,Y, x_obs, y_obs, size_robot, size_obstacle): #We extend the size of the obstacle with the size of the robot (border expansion) ext_size = size_robot + size_obstacle sigma_x = (ext_size/2)/2.3548 sigma_y = (ext_size/2)/2.3548 theta = 0 A = 100 #Weight of the Gaussian Z = zeros_like(X) a = cos(theta)**2/2/sigma_x**2 + sin(theta)**2/2/sigma_y**2 b = -sin(2*theta)/4/sigma_x**2 + sin(2*theta)/4/sigma_y**2 c = sin(theta)**2/2/sigma_x**2 + cos(theta)**2/2/sigma_y**2 Z[:] = Z[:]+A*exp( - (a*(X-x_obs)**2 + 2*b*(X-x_obs)*(Y-y_obs) + c*(Y-y_obs)**2)) return Z Z = gaussian_obstacle(X, Y, 50, 50, 10, 10)

We then create a gradient field based on the scalar field:

def calculate_vector_field(Z): U, V = gradient(Z,1,1) U = -U V = -V return U, V U, V = calculate_vector_field(Z)

Which will produce this vectors centered in the position of the obstacle

Finally to obtain a vector reference (velocity) for a robot that is currently at position (41,41), you may use the following function:

def get_velocity_command(U, V, x_robot, y_robot):

return U[x_robot,y_robot], V[x_robot,y_robot]

Vx_robot, Vy_robot = get_velocity_command(U, V, 41, 41)

If you have many moving obstacles and a big environment it may be too computationally expensive to do the calculation for the whole Grid, a possible solution is to do the calculation only in the surrounding area of the robot and obtain only the reference vector for the current position of the robot:

p_robot = (rob_x, rob_y) #We create a minimal grid around the position of our robot X, Y = mgrid[(rob_x-1):(rob_x+1)+1,(rob_y-1):(rob_y+1)+1]

Complete code with plots using Mayavi from enthought.

from pylab import * import matplotlib.pyplot as plt from mayavi import mlab x_min = 0 y_min = 0 x_max = 100 y_max = 100 X, Y = mgrid[ x_min:x_max+1:1, y_min: y_max+1:1] def gaussian_obstacle(X,Y, x_obs, y_obs, size_robot, size_obstacle): #We extend the size of the obstacle with the size of the robot (border expansion) ext_size = size_robot + size_obstacle sigma_x = (ext_size/2)/2.3548 sigma_y = (ext_size/2)/2.3548 theta = 0 A = 100 #Weight of the Gaussian Z = zeros_like(X) a = cos(theta)**2/2/sigma_x**2 + sin(theta)**2/2/sigma_y**2 b = -sin(2*theta)/4/sigma_x**2 + sin(2*theta)/4/sigma_y**2 c = sin(theta)**2/2/sigma_x**2 + cos(theta)**2/2/sigma_y**2 Z[:] = Z[:]+A*exp( - (a*(X-x_obs)**2 + 2*b*(X-x_obs)*(Y-y_obs) + c*(Y-y_obs)**2)) return Z def calculate_vector_field(Z): U, V = gradient(Z,1,1) U = -U V = -V return U, V Z = gaussian_obstacle(X, Y, 50, 50, 10, 10) U, V = calculate_vector_field(Z) def get_velocity_command(U, V, x_robot, y_robot): return U[x_robot,y_robot], V[x_robot,y_robot] Vx_robot, Vy_robot = get_velocity_command(U, V, 40, 40) # Plot mlab.figure(size=(800, 600)) mlab.quiver3d(X, Y, zeros_like(X), U, V,zeros_like(U)) mlab.mesh(X, Y, Z) mlab.show()

What is Mayavi?

Is it an app??