人工势场法MATLAB程序-带势场图

clc; clear; close all; %% 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 - 300).^2 + (y - 300).^2) < 30^2; obstacle(t) = true; t = ((x - 200).^2 + (y - 100).^2) < 60^2; obstacle(t) = true; %% Compute distance transform d = bwdist(obstacle); % 用于计算元素之间的距离（零元素所在的位置靠近非零元素位置的最短距离），单精度变量存储为single数据类型 % 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'); %% 计算吸引力 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'); %% 规划路径 start = [45, 345]; 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,:)); p.XData = sx + P(1); p.YData = sy + P(2); p.ZData = sz + f(P(2), P(1)); 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); % figure; imshow(~obstacle); hold on; plot(start(1), start(2), 'r.', 'MarkerSize', 30); hold on; plot(goal(1), goal(2), 'g.', 'MarkerSize', 30); hold on; plot(route(:,1), route(:,2), 'r', 'LineWidth', 2); hold off; axis ([0 ncols 0 nrows]); axis xy; axis on; xlabel ('x'); ylabel ('y'); title ('Configuration Space');