Skip to content

Instantly share code, notes, and snippets.

@mickeyouyou mickeyouyou/m
Created Feb 20, 2019

Embed
What would you like to do?
a implement of Artificial Potential Field in matlab
%
% PotentialFieldScript.m
%
%% Generate some points
nrows = 400;
ncols = 600;
obstacle = false(nrows, ncols);
[x, y] = meshgrid (1:ncols, 1:nrows);
%% Generate some obstacle
obstacle (300:end, 100:250) = true;
obstacle (150:200, 400:500) = true;
t = ((x - 200).^2 + (y - 50).^2) < 50^2;
obstacle(t) = true;
t = ((x - 400).^2 + (y - 300).^2) < 100^2;
obstacle(t) = true;
%% Compute distance transform
d = bwdist(obstacle);
% Rescale and transform distances
d2 = (d/100) + 1;
d0 = 2;
nu = 800;
repulsive = nu*((1./d2 - 1/d0).^2);
repulsive (d2 > d0) = 0;
%% Display repulsive potential
figure;
m = mesh (repulsive);
m.FaceLighting = 'phong';
axis equal;
title ('Repulsive Potential');
%% Compute attractive force
goal = [400, 50];
xi = 1/700;
attractive = xi * ( (x - goal(1)).^2 + (y - goal(2)).^2 );
figure;
m = mesh (attractive);
m.FaceLighting = 'phong';
axis equal;
title ('Attractive Potential');
%% Display 2D configuration space
figure;
imshow(~obstacle);
hold on;
plot (goal(1), goal(2), 'r.', 'MarkerSize', 25);
hold off;
axis ([0 ncols 0 nrows]);
axis xy;
axis on;
xlabel ('x');
ylabel ('y');
title ('Configuration Space');
%% Combine terms
f = attractive + repulsive;
figure;
m = mesh (f);
m.FaceLighting = 'phong';
axis equal;
title ('Total Potential');
%% Plan route
start = [50, 350];
route = GradientBasedPlanner (f, start, goal, 1000);
%% Plot the energy surface
figure;
m = mesh (f);
axis equal;
%% Plot ball sliding down hill
[sx, sy, sz] = sphere(20);
scale = 20;
sx = scale*sx;
sy = scale*sy;
sz = scale*(sz+1);
hold on;
p = mesh(sx, sy, sz);
p.FaceColor = 'red';
p.EdgeColor = 'none';
p.FaceLighting = 'phong';
hold off;
for i = 1:size(route,1)
P = round(route(i,:));
z = f(P(2), P(1));
p.XData = sx + P(1);
p.YData = sy + P(2);
p.ZData = sz + f(P(2), P(1));
drawnow;
drawnow;
end
%% quiver plot
[gx, gy] = gradient (-f);
skip = 20;
figure;
xidx = 1:skip:ncols;
yidx = 1:skip:nrows;
quiver (x(yidx,xidx), y(yidx,xidx), gx(yidx,xidx), gy(yidx,xidx), 0.4);
axis ([1 ncols 1 nrows]);
hold on;
ps = plot(start(1), start(2), 'r.', 'MarkerSize', 30);
pg = plot(goal(1), goal(2), 'g.', 'MarkerSize', 30);
p3 = plot (route(:,1), route(:,2), 'r', 'LineWidth', 2);
@Francis235

This comment has been minimized.

Copy link

commented Jun 14, 2019

can share function GradientBasedPlanner()? thank you very much

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.