gistlib - create a map for potential function motion planner in matlab

To create a map for potential function motion planner in MATLAB, you can follow these steps:

Define the size of the map and the resolution:


main.m
mapSizeX = 10; % meters
mapSizeY = 10; % meters
mapResolution = 0.1; % meters/cell
83 chars
4 lines

Create an occupancy grid map using the binaryOccupancyMap function:


main.m
map = binaryOccupancyMap(mapSizeX, mapSizeY, mapResolution);
61 chars
2 lines

This will create a binary occupancy grid map with mapSizeX by mapSizeY size and mapResolution cell size.

Add obstacles to the map using the setOccupancy function:


main.m
obs1x = [1 2 2 1 1]*mapResolution;
obs1y = [1 1 2 2 1]*mapResolution;
setOccupancy(map, [obs1x' obs1y'], 1);

obs2x = [8 9 9 8 8]*mapResolution;
obs2y = [8 8 9 9 8]*mapResolution;
setOccupancy(map, [obs2x' obs2y'], 1);
219 chars
8 lines

This will add two rectangular obstacles to the map.

Define the goal and start positions:


main.m
start = [1.5 1.5];
goal = [9.5 9.5];
37 chars
3 lines

Define the attractive and repulsive potential functions:


main.m
k_att = 1;
k_rep = 100;
24 chars
3 lines

Define a function to compute the potential field of the map:


main.m
function [potentialField, gradX, gradY] = getPotentialField(map, start, goal, k_att, k_rep)
  [x, y] = meshgrid(map.XWorldLimits(1):map.XWorldLimits(2), map.YWorldLimits(1):map.YWorldLimits(2));

  distToStart = sqrt((x - start(1)).^2 + (y - start(2)).^2);
  distToGoal = sqrt((x - goal(1)).^2 + (y - goal(2)).^2);
  potentialAtt = 0.5 * k_att * (distToStart.^2);

  potentialField = potentialAtt;
  for i = 1:length(map.occgrid(:))
    if map.occgrid(i) == 1
      distToObs = sqrt((x(i) - start(1)).^2 + (y(i) - start(2)).^2);
      potentialRep = 0.5 * k_rep * (1 ./ distToObs - 1 / mapResolution).^2;

      potentialField(i) = potentialField(i) + potentialRep;
    end
  end

  [gradX, gradY] = gradient(potentialField, map.XWorldLimits(2) - map.XWorldLimits(1), map.YWorldLimits(2) - map.YWorldLimits(1));
end
816 chars
20 lines

Compute the potential field of the map:


main.m
[potentialField, gradX, gradY] = getPotentialField(map, start, goal, k_att, k_rep);
84 chars
2 lines

Visualize the map and the potential field:


main.m
figure();
show(map);
hold on;
contour(map.XWorldLimits(1):mapResolution:map.XWorldLimits(2), map.YWorldLimits(1):mapResolution:map.YWorldLimits(2), potentialField', 'LineWidth', 2);
plot(start(1), start(2), 'ro', 'MarkerSize', 10);
plot(goal(1), goal(2), 'gx', 'MarkerSize', 10);
quiver(x, y, -gradX, -gradY);
310 chars
8 lines

This will create a MATLAB figure that shows the occupancy grid map, the potential field, the start and goal positions, and the gradient vectors of the potential field.

create a map for potential function motion planner in matlab

similar matlab code snippets

related categories