%% Generate some points
% nrows 表示地图纵坐标 ncols 表示地图横坐标
% 可自己设置地图大小
nrows = 400;
ncols = 600;
goal = [400, 50]; % 目标点位置坐标，可自己更改
start = [50, 350]; % 起始点位置坐标，可自己更改
obstacle = false(nrows, ncols);
[x, y] = meshgrid (1:ncols, 1:nrows);
%% Generate some obstacle
% 设置矩形区域障碍物代码 ： 可自己设置障碍物
% obstacle (y_起始点:y_结束点, x_起始点:x_结束点) = true;
obstacle (300:400, 100:250) = true; % 纵坐标从300到400，横坐标从100到250的区域 值设置为1 表示障碍物 用黑色表示
obstacle (150:200, 400:500) = true;
% 设置圆形区域障碍物代码 ： 可自己设置障碍物
% t = ((x - x_圆心横坐标).^2 + (y - y_圆心纵坐标).^2) < R_圆半径^2;
% obstacle(t) = true;
t = ((x - 200).^2 + (y - 50).^2) < 50^2; % t表示 圆心为（200，50）半径为50 的区域
obstacle(t) = true; % 将t区域值设置为1 表示障碍物 用黑色表示
t = ((x - 400).^2 + (y - 300).^2) < 100^2;
obstacle(t) = true;
%% Compute distance transform
% 璁＄畻鐭╅樀obstacle涓浂鐐瑰埌闈為浂鐐癸紙鍗充腑蹇冪偣锛夌殑鏈?煭璺濈锛屾瀯閫犲嚭涓?釜鏂扮殑鍑犱綍浣?
d = bwdist(obstacle);
% Rescale and transform distances锛堥噸鏂扮缉鏀惧拰杞崲璺濈锛?%浣跨敤璺濈鍑芥暟d2鏋勯?鎺掓枼鍔垮嚱鏁?
d2 = (d/100) + 1;
% 鎺у埗鎺掓枼鍔垮満鍑芥暟鐨勫弬鏁癲0
d0 = 2;
% nu鍔跨缉鏀惧弬鏁?nu = 800;
% 鎺掓枼鍔垮嚱鏁扮殑璁＄畻鍏紡
nu = 800;
repulsive = nu*((1./d2 - 1/d0).^2);
% 濡傛灉鏈哄櫒浜轰笌闅滅鐗╀箣闂寸殑璺濈澶т簬d0锛屽垯鍑芥暟鍏抽棴
repulsive (d2 > d0) = 0;
%% Display repulsive potential
figure;
% mesh鍑芥暟鏄敤鏉ョ敾涓夌淮鐨勭綉鏍艰〃闈㈢殑銆?
m = mesh (repulsive);
% phong鏄厜鐓фā寮忋?鍥惧儚鐨勪竴绉嶆樉绀烘柟寮忋?
m.FaceLighting = 'phong';
axis equal;
title ('Repulsive Potential');
%% Compute attractive force
% xi鍔跨缉鏀惧弬鏁?
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鏄痬atlab涓樉绀哄浘鍍忕殑鍑芥暟銆?
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
route = GradientBasedPlanner (f, start, goal, 1000);
%% Plot the energy surface
figure;
m = mesh (f);
axis equal;
%% Plot ball sliding down hill
% 浠庝笂鍒颁笅鏈?1缁勭偣锛屾墍浠ユ瀯鎴?0灞傘?
% 鐒跺悗锛屽張鍍忓垏瑗跨摐涓?牱锛屼粠椤跺埌搴曞垏浜?0鍒?紝杩欐牱灏辨瀯鎴愪簡21*21涓皬缃戞牸
[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锛堢鍥撅級
skip = 20;
figure;
xidx = 1:skip:ncols;
yidx = 1:skip:nrows;
[gx, gy] = gradient (-f);
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);%璺緞
标签：势场,ncols,%%,goal,路径,nrows,算法,obstacle,axis From： https://www.cnblogs.com/DissertationSubmitted/p/17166416.html