基于A*的栅格地图路径搜索

  1 %创建具有障碍物的栅格地图
  2 %矩阵中1代表黑色栅格
  3 map = zeros(20);
  4 map(3,3:7)=2;
  5 map(3:10,7)=2;
  6 map(10,3:7)=2;
  7 map(17,13:17)=2;
  8 map(10:17,13)=2;
  9 map(10,13:17)=2;
 10 map(5,2)=5;
 11 map(15,15)=6;
 12 % 白色，可行路径
 13 % 黑色，障碍物
 14 % 红色，visited
 15 % 蓝色，on list
 16 % green，start
 17 % yellow,destination
 18 % 对应于下个矩阵
 19 map_color = [1 1 1;
 20         0 0 0; 
 21         1 0 0;
 22         0 0 1; 
 23         0 1 0; 
 24         1 1 0];
 25 %===========================
 26 colormap(map_color);  % 创建颜色
 27 image(map); % 赋予栅格颜色
 28 colorbar;
 29 set(gca,‘XTick‘,1:size(map,1),‘YTick‘,1:size(map,2));  % 设置坐标
 30 axis image xy;  % 沿每个坐标轴使用相同的数据单位，保持一致
 31 grid on;
 32 %========================
 33 nrows = 20;  % 行数
 34 ncols = 20;  % 列数
 35 start_node = sub2ind(size(map), 5, 2);  % 起点索引,先对应列元素
 36 dest_node = sub2ind(size(map), 15, 15);   % 终点索引
 37 %=====================
 38 distanceFromStart = Inf(nrows,ncols);  % 距离初始化
 39 distanceFromStart(start_node) = 0;  % 起点距离初始化
 40 
 41 % disp(distance_FromStart);
 42 
 43 % 计算h值，有八个拓展方向，选择欧式距离
 44 [X, Y] = meshgrid (1:ncols, 1:nrows);
 45 H = sqrt((Y - 15).^2 + (X - 15).^2);
 46 
 47 f = Inf(nrows,ncols); 
 48 f(start_node) = H(start_node); 
 49 %disp(f(start_node));
 50 
 51 parent = zeros(nrows,ncols); 
 52 % Main Loop 
 53 while true
 54     % Draw current map 
 55     map(start_node) = 5; 
 56     map(dest_node) = 6; 
 57     image(map); 
 58     grid on; 
 59     axis image; 
 60     drawnow; 
 61     %====================
 62     % Find the node with the minimum distance 
 63     [~, current] = min(f(:));  % f(:)列向量排列成一个列向量,找到最小值，~忽略输出参数,current位置是索引
 64     [min_dist, ~] = min(distanceFromStart(:));
 65     %===================
 66     if ((current == dest_node) || isinf(min_dist)) 
 67         break; 
 68     
 69     end
 70     
 71 map(current) = 3; 
 72 %============
 73 f(current) =Inf; 
 74 [i, j] = ind2sub(size(distanceFromStart), current);
 75 neighbor = [i-1,j;
 76 i+1,j; 
 77 i,j+1;
 78 i,j-1] ;
 79 outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +(neighbor(:,2)<1) + (neighbor(:,2)>ncols ) ;
 80 locate = find(outRangetest>0); 
 81 neighbor(locate,:)=[] ;
 82 neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) ;
 83 for i=1:length(neighborIndex) 
 84 if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) 
 85 map(neighborIndex(i)) = 4; 
 86 if distanceFromStart(neighborIndex(i))> min_dist + 1 
 87 distanceFromStart(neighborIndex(i)) = min_dist+1; 
 88 parent(neighborIndex(i)) = current; 
 89 f(neighborIndex(i)) =H(neighborIndex(i)); 
 90 end 
 91 end 
 92 end 
 93 end
 94 
 95 
 96 %%
 97 if (isinf(distanceFromStart(dest_node))) 
 98 route = []; 
 99 else 
100 %提取路线坐标
101 route = [dest_node]; 
102 while (parent(route(1)) ~= 0) 
103 route = [parent(route(1)), route]; 
104 end 
105 % 动态显示出路线 
106 for k = 2:length(route) - 1 
107 map(route(k)) = 7; 
108 pause(0.1); 
109 image( map); 
110 grid on; 
111 axis image; 
112 end 
113 end
114 
115