MATLAB实现的A*算法_a*算法matlab

MATLAB实现的A*算法

总共三个.m文件

1.Astar.m

clear;
clc;
%%
%设置地图参数
WIDTH  = 40;          %地图长度，即行数
LENGTH = 40;		  %地图长度，即列数
STARTPOINTX = WIDTH/4+2;      %起点横坐标
STARTPOINTY = LENGTH/4+2;	   %起点纵坐标
ENDPOINTX   = WIDTH-STARTPOINTX;     %终点横坐标
ENDPOINTY   = LENGTH-STARTPOINTY;    %终点纵坐标
OBSTACLEAMOUNT = WIDTH*LENGTH/4;     %障碍物数量,随机产生障碍物

%%
%将地图上的点初始化
point(WIDTH,LENGTH) = PointInfo;
for i = 1:WIDTH          	
    for j = 1:LENGTH
        point(i,j).xCoordinate = i;   %坐标设置好
        point(i,j).yCoordinate = j;
        point(i,j).h = 10*(abs(i-ENDPOINTX) + abs(j-ENDPOINTY) );      %可以事先计算出H，H这里用曼哈顿距离
    end                                                                %将H改为0，即Dijastra算法
end                                                                    %将H改为Inf，即广度优先算法BFS
point(STARTPOINTX,STARTPOINTY).g = 0; %起始点的g实际代价为0             %将障碍物数量设置为0，可以更好地看出他们的区别

%%
%产生障碍物
point(STARTPOINTX,STARTPOINTY).isStartPoint = true;
point(ENDPOINTX,ENDPOINTY).isEndPoint       = true;
tempX = 0;    %定义随机障碍物的临时坐标
tempY = 0;
tempObs = OBSTACLEAMOUNT;
%rng(0);      %去掉注释可使产生的随机障碍物不变   
while tempObs > 0
	tempX = randi(WIDTH,1);        %randi产生1-WIDTH之间的随机整数
	tempY = randi(LENGTH,1);
	if ~( ((tempX == STARTPOINTX) && (tempY == STARTPOINTY)) || ((tempX == ENDPOINTX) && (tempY == ENDPOINTY)) ) %#ok<*ALIGN> %随机产生的点不能为起始点和终点
    	if ~point(tempX,tempY).isObstacle        %只有不是障碍物的才能变成障碍物，不能重复设置
			point(tempX,tempY).isObstacle = true;
			tempObs = tempObs-1;
        end
    end
end

%%
%Astar算法的核心
openList = [];  %#ok<NASGU>  存放待扩展的节点
closeList = []; %存放以后不需要处理的点
gCost = [14 10 14;10 0 10;14 10 14] ; %当前点的8个相邻点的实际代价值设置
flag1 = false; %算法结束的两种情况：终点在openList中或者openList为空，
flag2 = false;
%1.将起点加入openList
openList = [STARTPOINTX,STARTPOINTY,point(STARTPOINTX,STARTPOINTY).f]; %分为3列，分别代表点的x,y,f
point(STARTPOINTX,STARTPOINTY).isOpenList = true;
%2.重复以下步骤
tic   %输出算法计算的时间，可用来比较算法的效率
while ~flag1 && ~flag2
    %3.选取F最小的放在open表首部
    [~,I] = sort(openList(:,3));
    openList = openList(I,:);
    %4.将F最小的点从openList移至closedList
    currentX = openList(1,1);   %存放当前最小F点的坐标
    currentY = openList(1,2);
    point(currentX,currentY).isCloseList = true;
    point(currentX,currentY).isOpenList = false;
    closeList = [closeList;openList(1,:)];  %#ok<AGROW>
    openList(1,:)=[];
    %5.根据周围的点是否在openList，采取两种不同的操作，分别如下：
    for i = -1:1
        tempX = currentX+i;  %用for循环获取相邻点的坐标(tempX,tempY)
        for j =-1:1
            tempY = currentY+j;       %如果周围点不是边界或者不在closedList或者障碍物，才能继续往下走
            if (tempX>=1) && (tempY>=1) && (tempX<=WIDTH) && (tempY<=LENGTH) && (~point(tempX,tempY).isObstacle) && (~point(tempX,tempY).isCloseList) 
                %i.如果周围的点在openList里,检查这条路径(即经由当前方格到达它那里)是否更好,用G值作参考;
                 tempG = gCost(i+2,j+2)+point(currentX,currentY).g;  %计算经由当前方格到该点的G值
                if point(tempX,tempY).isOpenList
                    %如果G更小，则把它的父亲设置为当前方格，并重新计算它的G和F值,并重新排序openList，否则什么都不做 
                    if tempG < point(tempX,tempY).g    %如果G更小
                        point(tempX,tempY).parent = point(currentX,currentY);   %把它的父亲设置为当前方格
						point(tempX,tempY).g = tempG;   %并重新计算它的G和F值
						[~,I] = sort(openList(:,3));    %重新排序openList
                        openList = openList(I,:);
                    end
                %ii.如果不在openList，把它加入 openList，并且把当前方格设置为它的父亲，记录该方格的 F，G 和 H 值。    
                else
                    point(tempX,tempY).g = tempG;       %先计算G和H值，再加入openList
					point(tempX,tempY).parent = point(currentX,currentY);
                    point(tempX,tempY).isOpenList = true;                   %加入openList
                    openList = [openList;tempX,tempY,point(tempX,tempY).f]; %#ok<AGROW> 
                end
            end
        end
        %6.检查是否能够结束算法，即终点是否已经在openList中或者openList是否为空。
        flag1 = point(ENDPOINTX,ENDPOINTY).isOpenList;
        flag2 = isempty(openList);
    end
 end
 toc
 %%
if flag1   %flag1位true,即找到了最短路径，用pointPrint输出路线
    disp( "路线总代价为:"+ point(ENDPOINTX,ENDPOINTY).f );
    disp("路线为(从下向上看):");
	pointPrint(point,ENDPOINTX,ENDPOINTY);
else
    disp("未找到路径");
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106

2.PointInfo.m

classdef PointInfo
    %点的类属性
    %点的一些基本信息
    properties
	  xCoordinate;	   
      yCoordinate;
      
      isStartPoint = false;    %是否是起点
	  isEndPoint   = false;    %是否是终点
	  isObstacle   = false;    %是否是障碍物   
	  isCloseList  = false;    %是否在closed表
	  isOpenList   = false;    %是否在open表
	  iskeyPoint   = false;    %是否是最短路径上的点
	
	  g = Inf;    %g，点到待扩展点的实际代价
	  h = Inf;    %h，待扩展点到终点的估计代价
      
      parent;     %点的父节点，倒着遍历即可找到路径
    end
    
    properties(Dependent)  %f一直等于g+h，故将其设为独立的属性，自动计算
      f;
    end
    methods
      function value = get.f(obj)
          value = obj.g+obj.h;
      end
    end
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

3.pointPrint.m

function pointPrint(point,ENDPOINTX,ENDPOINTY)
%POINTPRINT：输出地图及路径的函数
%输入地图信息及终点，根据其父节点，即可找到路径
    %首先根据父节点找出路线中的关键点，根据最后一个节点往前找
	tempPoint = point(ENDPOINTX,ENDPOINTY);  %定义一个临时结点存放父节点信息
    path = [];   %PATH保存路径点
    while ~isempty(tempPoint.parent)
        tempPoint = tempPoint.parent;
		tempX = tempPoint.xCoordinate;
        tempY = tempPoint.yCoordinate; 
        disp("("+tempX+","+tempY+")");
		point(tempX,tempY).iskeyPoint = true;
        path = [path;tempX,tempY];   %#ok<AGROW>
    end
    
	%输出地图情况
    colormap([0 0 0;1 1 1;1 0 0;0 1 0;0 0 1]);%设置颜色，分别代表0黑色、1白色、2红色、3绿色、4蓝色
    map = ones(size(point,1),size(point,2));
	for i = 1:size(map,1)     %#ok<ALIGN>
		for j = 1:size(map,2) %#ok<ALIGN>
            if point(i,j).isStartPoint
				map(i,j) = 2;   %起点用红色表示
            elseif point(i,j).isEndPoint
    		    map(i,j) = 2;   %终点也用红色表示
            elseif point(i,j).isObstacle
				map(i,j) = 0;   %障碍物用黑色表示
            elseif point(i,j).isCloseList || point(i,j).isOpenList 
                map(i,j) = 4;   %扩展点用蓝色表示
            else
                map(i,j) = 1;   %普通点用白色表示 
            end
        end
    end
    map(i+1,j+1) = 5;   %需将map扩展一格
   
    pcolor(map);    %输出除路径点以外的静态点
    %colorbar;      %可查看颜色情况
    set(gca,'XTick',1:size(map,2)-1,'YTick',1:size(map,1)-1);   %设置坐标轴
    axis image xy;      %沿每个坐标轴使用相同的数据单位，保持一致
    
    hold on; 
    
    t =text(j+2,i,'起终点');  %用text输出图例
    t.BackgroundColor = 'r';
     t.FontWeight = 'bold' ;

    t =text(j+2,i*3/4,'障碍点');
    t.BackgroundColor =[0 0 0];
    t.FontWeight = 'bold' ;
    t.Color = [1 1 1];

    t =text(j+2,i/2,'路径点');
    t.BackgroundColor ='g';
    t.FontWeight = 'bold' ;
    
    t =text(j+2,i/4,'扩展点');
    t.BackgroundColor = 'b';
    t.FontWeight = 'bold' ;
  
    %动态输出路径点,用绿色表示
    for i = flip(1:size(path,1)-1) %因路径点是倒着存放的，需翻转一下，且最后一个点是起点，无需输出
        x = path(i,1);
        y = path(i,2);
        fill([y,y+1,y+1,y],[x,x,x+1,x+1],'g'); %输出除路径点以外的静态点
        pause(0.01);  %延时0.01s，可据此设置动态输出频率
    end
    hold off
  
end

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70