test
This commit is contained in:
parent
a582403bbe
commit
513a682bf0
65
Astar.m
Normal file
65
Astar.m
Normal file
@ -0,0 +1,65 @@
|
||||
function [path] = Astar (map, initial, goal)
|
||||
%INPUTS
|
||||
%goal a 1x2 vector stating the x and y coordinates of the goal cell
|
||||
%initial a 1x2 vector stating the x and y coordinated of the initial cell
|
||||
%map a ixj binary matrix describing occupancy with 1
|
||||
w=12;%cm, robot width
|
||||
% map=inflate_map(map); %accounting for robot width in the map
|
||||
msize=size(map);
|
||||
h=[];%heuristic values of every cell. estimated as the straight distance between the goal and cell
|
||||
for i=1:msize(1)
|
||||
for j=1:msize(2)
|
||||
h(i,j)=sqrt((i-goal(1))^2+(j-goal(2))^2);
|
||||
if map(i,j)==1
|
||||
h(i,j)=h(i,j)+1000;
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
open=[initial(1),initial(2),h(initial(1),initial(2)),-1,-1];
|
||||
%open nodes,[X(x coordinate of the cell/column),Y( y coordinate of the
|
||||
%cell/row),f(sum of weight of all previous nodes and included this+heuristic), x
|
||||
%(previous cell),y (previous cell)]
|
||||
closed=[];%explored nodes
|
||||
[m n] = min(open(:,3));
|
||||
current=open(n,:);
|
||||
|
||||
%open min and extract
|
||||
while ~isequal(current(1:2),goal)
|
||||
children= expand_node(current,h, msize(1), msize(2));
|
||||
%place the children with the lower cost in the open list if they are
|
||||
%redundant in that list
|
||||
closed=[closed;current];
|
||||
open=open([1:n-1,n+1:end],:); %deleting the current element from the open
|
||||
if isempty(children)==0
|
||||
for i=1:length(children(:,1))
|
||||
if BinA(open(:,1:2),children(i,1:2))~=0 %if the child is already present in the open but with a more expensive cost
|
||||
if(children(i,3)<open(BinA(open(:,1:2),children(i,1:2)),3))
|
||||
BinA(open(:,1:2),children(i,1:2))
|
||||
open(BinA(open(:,1:2),children(i,1:2)),:)=children(i,:);
|
||||
end
|
||||
else
|
||||
if BinA(closed(:,1:2),children(i,1:2))==0
|
||||
open=[open ; children(i,:)];
|
||||
else
|
||||
if (children(i,3)<closed(BinA(closed(:,1:2),children(i,1:2)),3))
|
||||
BinA(open(:,1:2),children(i,1:2));
|
||||
closed(BinA(open(:,1:2),children(i,1:2)),:)=children(i,:);
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
[m n] = min(open(:,3));
|
||||
current=open(n,:);
|
||||
end
|
||||
closed=[closed ;current];
|
||||
%reiterate to start to find path
|
||||
node=current;
|
||||
path=current(1:2);
|
||||
while ~isequal(node(4:5),initial)
|
||||
path=[node(4:5); path];
|
||||
node=closed(BinA(closed(:,1:2),node(4:5)),:);
|
||||
end
|
||||
path
|
||||
end
|
||||
14
BinA.m
Normal file
14
BinA.m
Normal file
@ -0,0 +1,14 @@
|
||||
function [i] = BinA (a, b)
|
||||
i=0;
|
||||
if isempty(a)~=1
|
||||
for n=1:length(a(:,1))
|
||||
if isequal(a(n,:),b)==1
|
||||
i=n;
|
||||
break;
|
||||
else
|
||||
i=0;
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
19
expand_node.m
Normal file
19
expand_node.m
Normal file
@ -0,0 +1,19 @@
|
||||
function [children]=expand_node (parent, h, V, H)
|
||||
children=[];
|
||||
for i=-1:1
|
||||
for j=-1:1
|
||||
if j~=0 && i~=0 && parent(1)+i>0 && parent(1)+i<=V && parent(2)+j>0 && parent(2)+j<=H
|
||||
c=parent(3)+1.4142+h(parent(1)+i,parent(2)+j)-h(parent(1),parent(2));
|
||||
if ((parent(1)+i)~=parent(4)|| (parent(2)+j)~=parent(5))% && (((j==0 || i==0) && j~=i)||(j~=0 && i~=0))
|
||||
children=[children; [parent(1)+i, parent(2)+j, c, parent(1),parent(2)]];
|
||||
end
|
||||
elseif (j==0 || i==0) && j~=i && parent(1)+i>0 && parent(1)+i<=V && parent(2)+j>0 && parent(2)+j<=H
|
||||
c=parent(3)+1+h(parent(1)+i,parent(2)+j)-h(parent(1),parent(2));
|
||||
if ((parent(1)+i)~=parent(4)|| (parent(2)+j)~=parent(5)) %&& (((j==0 || i==0) && j~=i)||(j~=0 && i~=0))
|
||||
children=[children; [parent(1)+i, parent(2)+j, c, parent(1),parent(2)]];
|
||||
end
|
||||
else
|
||||
|
||||
end
|
||||
end
|
||||
end
|
||||
79
homework_map.m
Normal file
79
homework_map.m
Normal file
@ -0,0 +1,79 @@
|
||||
% 创建地图
|
||||
% 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
|
||||
map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0; % 1
|
||||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0; % 2
|
||||
0 0 0 1 1 1 1 0 0 1 0 0 0 0 0 1 1 0 0 0; % 3
|
||||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0; % 4
|
||||
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0; % 5
|
||||
1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 1 0 0 1 1; % 6
|
||||
1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 1; % 7
|
||||
1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0; % 8
|
||||
1 0 0 0 1 1 1 1 1 1 0 0 1 1 0 0 0 0 0 0; % 9
|
||||
1 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0; %10
|
||||
0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0; %11
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0; %12
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %13
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %14
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %15
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0; %16
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1; %17
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; %18
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0; %19
|
||||
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; %20
|
||||
|
||||
% 设置起点和终点
|
||||
start = [2, 2];
|
||||
goal = [19, 19];
|
||||
|
||||
% 调用A*算法
|
||||
path = Astar(map, start, goal);
|
||||
|
||||
visualize_path(map, start, goal, path);
|
||||
|
||||
function visualize_path(map, start, goal, path)
|
||||
% 可视化地图，0为空地，1为障碍
|
||||
imagesc(map);
|
||||
colormap(flipud(gray)); % 灰度图（0白 1黑）
|
||||
axis equal tight;
|
||||
hold on;
|
||||
|
||||
% 蓝色起点
|
||||
plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
|
||||
|
||||
% 红色终点
|
||||
plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
|
||||
|
||||
if ~isempty(path)
|
||||
% 如果起点不等于路径首元素，插入
|
||||
if ~isequal(path(1,:), start)
|
||||
path = [start; path];
|
||||
end
|
||||
% 如果终点不等于路径末元素，插入
|
||||
if ~isequal(path(end,:), goal)
|
||||
path = [path; goal];
|
||||
end
|
||||
|
||||
% 显示路径线
|
||||
plot(path(:,2), path(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
|
||||
plot(path(:,2), path(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
|
||||
end
|
||||
|
||||
% 网格线
|
||||
[rows, cols] = size(map);
|
||||
for i = 1:rows+1
|
||||
line([0.5, cols+0.5], [i-0.5, i-0.5], 'Color', 'k'); % 横线
|
||||
end
|
||||
for j = 1:cols+1
|
||||
line([j-0.5, j-0.5], [0.5, rows+0.5], 'Color', 'k'); % 竖线
|
||||
end
|
||||
|
||||
set(gca, ...
|
||||
'XTick', 1:cols, ...
|
||||
'YTick', 1:rows, ...
|
||||
'XTickLabel', 1:cols, ...
|
||||
'YTickLabel', 1:rows, ...
|
||||
'XAxisLocation', 'top', ... % x轴
|
||||
'YDir', 'reverse'); % y轴
|
||||
title('地图');
|
||||
hold off;
|
||||
end
|
||||
Loading…
x
Reference in New Issue
Block a user