在MATLAB中,在20×20的栅格图中随机设置起点、终点和障碍物,并用A*算法运算得出路径
时间: 2023-07-16 20:12:36 浏览: 100
好的,以下是一个示例代码,你可以根据需要进行修改:
```matlab
% 设置栅格图大小
n = 20;
m = 20;
% 随机设置起点、终点和障碍物
start = randi([1 n], 1, 2);
goal = randi([1 n], 1, 2);
obs_num = ceil(n * m / 5); % 随机生成的障碍物数量为栅格数的五分之一
obs = randi([1 n*m], 1, obs_num);
obs = unique(obs); % 确保障碍物不重复
obs = [mod(obs-1, n)+1, floor((obs-1)/n)+1]; % 将随机生成的障碍物转化为二维坐标
% 初始化栅格图
grid = zeros(n, m);
for i = 1:size(obs,1)
grid(obs(i,1), obs(i,2)) = 1; % 将障碍物标记为1
end
% 显示栅格图
figure(1)
clf
imagesc(grid);
hold on
plot(start(2), start(1), 'rx', 'MarkerSize', 15, 'LineWidth', 3)
plot(goal(2), goal(1), 'gx', 'MarkerSize', 15, 'LineWidth', 3)
axis equal
axis tight
% 计算启发函数
H = zeros(n, m);
for i = 1:n
for j = 1:m
H(i,j) = sqrt((i-goal(1))^2 + (j-goal(2))^2);
end
end
% A*算法
openSet = [start, 0, H(start(1),start(2))];
closedSet = [];
while ~isempty(openSet)
[~, idx] = min(openSet(:,3));
current = openSet(idx,:);
if isequal(current(1:2), goal)
% 找到路径
path = [current(1:2)];
while ~isequal(path(end,:), start)
for i = 1:size(closedSet,1)
if isequal(closedSet(i,1:2), path(end,:))
path = [path; closedSet(i,4:5)];
break
end
end
end
path = flip(path);
% 显示路径
plot(path(:,2), path(:,1), 'm', 'LineWidth', 3);
break
end
openSet(idx,:) = [];
closedSet = [closedSet; current];
neighbors = [current(1)-1, current(2); current(1)+1, current(2); ...
current(1), current(2)-1; current(1), current(2)+1];
for i = 1:size(neighbors,1)
if any(neighbors(i,:) < 1) || any(neighbors(i,:) > n) || ...
grid(neighbors(i,1), neighbors(i,2)) == 1
continue
end
temp_g = current(3) + 1;
temp_h = H(neighbors(i,1),neighbors(i,2));
temp_f = temp_g + temp_h;
if any(ismember(neighbors(i,:), closedSet(:,1:2), 'rows'))
continue
end
idx_open = find(ismember(openSet(:,1:2), neighbors(i,:), 'rows'));
if isempty(idx_open)
openSet = [openSet; neighbors(i,:), temp_g, temp_h, temp_f];
elseif temp_f < openSet(idx_open,6)
openSet(idx_open,3:6) = [temp_g, temp_h, temp_f];
end
end
end
```
这段代码会生成一个随机的栅格图,并用 A* 算法计算出起点到终点的最短路径,并将路径显示在图中。你可以运行这段代码,看看效果如何。
阅读全文