A*算法在matlab上的实现_set(gcf, 'renderer', 'painters');

先看看算法的效果

图中蓝绿色大圆为障碍物，蓝色小圆为路径节点，红色号为目的地，蓝色为起点

算法下载位置：
https://gitee.com/bingobinlw/volans/tree/master/src/modules/matlab/2D_Astar

代码结构如下：

A_star_search.m为算法的核心，main.m是函数用演示的一个脚本，obstacle_map.m为随机生成障碍物的函数，visualize_map.m是将路径可视化的函数，其他的函数为一些辅助用的函数

main函数:

% Used for Motion Planning for Mobile Robots
% Thanks to HKUST ELEC 5660 
close all; clear all; clc;

set(gcf, 'Renderer', 'painters');%设置渲染器属性为painters
set(gcf, 'Position', [500, 50, 700, 700]);%设置保存图片的位置
%set(gcf, 'Position', [500, 50, 700, 700]);
%中的前两个参数表示显示框出现的位置，后面的两个参数表示图片的长与宽。
%Renderer(扫描器)是一种图像数据转换程序，通过这个程序，
%可以把图像数据转化为Matlab可以用来画图的内容。
%可以这样理解：Renderer(渲染器，描绘器)是matlab用来画图（即将数据转化为图像）的方式。
%它三种方式：1）Painters   2)  Z-buffer  3)  OpenGL
%2.三种方式的比较
%1)painters  再画简单的图画时，比较快；
%2)z-buffer 又称深度缓冲技术，它是现在内存中画图，
%然后再将最终的图像显示在显示终端。这样的话，当在一个像素点，
%如果被多次赋值的话，它只会显示最高层的图像。
%3) opengl 是利用硬件进行绘图的方式。
% Environment map in 2D space 
xStart = 1.0;
yStart = 1.0;
xTarget = 9.0;
yTarget = 9.0;
MAX_X = 10;
MAX_Y = 10;
%调用obstacle_map函数（这个函数返回一个包含随机分布障碍的地图。）
map = obstacle_map(xStart, yStart, xTarget, yTarget, MAX_X, MAX_Y);

% Waypoint Generator Using the A* 使用A*的路径点生成器
path = A_star_search(map, MAX_X,MAX_Y);

% visualize the 2D grid map画出地图
visualize_map(map, path);
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

obstacle_map函数

function map = obstacle_map(xStart,yStart,xTarget,yTarget,MAX_X,MAX_Y)
%这个函数返回一个包含随机分布障碍的地图
    rand_map = rand(MAX_X,MAX_Y);%生成一个矩阵,数值区间[0,1]
    map = [];
    map(1,1) = xStart;
    map(1,2) = yStart;
    k=2;
    obstacle_ratio = 0.25;
    for i = 1:1:MAX_X
        for j = 1:1:MAX_Y
            if( (rand_map(i,j) < obstacle_ratio) && (i~= xStart || j~=yStart) && (i~= xTarget || j~=yTarget))
                %如果随机矩阵值小于0.25该点就记录为障碍点
                map(k,1) = i;
                map(k,2) = j;
                k=k+1;
            end    
        end
    end
    %将最后一个点记录在map中
    map(k,1) = xTarget;
    map(k,2) = yTarget;
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22

A_star_search函数

function path = A_star_search(map,MAX_X,MAX_Y)
%%
%This part is about map/obstacle/and other settings
    %pre-process the grid map, add offset
    size_map = size(map,1);%返回map的大小
    Y_offset = 0;
    X_offset = 0;
    
    %Define the 2D grid map array.
    %Obstacle=-1, Target = 0, Start=1障碍物值为-1，目标点值为0，
    MAP=2*(ones(MAX_X,MAX_Y));%生成全2的矩阵，矩阵长宽为maxx,maxy
    
    %初始化地图的目标点
    xval=floor(map(size_map, 1)) + X_offset;
    yval=floor(map(size_map, 2)) + Y_offset;
    xTarget=xval;
    yTarget=yval;
    MAP(xval,yval)=0;
    
    %初始化地图的障碍点
    for i = 2: size_map-1
        xval=floor(map(i, 1)) + X_offset;
        yval=floor(map(i, 2)) + Y_offset;
        MAP(xval,yval)=-1;
    end 
    
    %初始化地图的起始点
    xval=floor(map(1, 1)) + X_offset;
    yval=floor(map(1, 2)) + Y_offset;
    xStart=xval;
    yStart=yval;
    MAP(xval,yval)=1;

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %LISTS USED FOR ALGORITHM
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %OPEN 列表结构如下:
    %--------------------------------------------------------------------------
    %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)|
    %--------------------------------------------------------------------------
    OPEN=[];
    %CLOSED列表的结构如下:
    %--------------
    %X val | Y val |
    %--------------
    % CLOSED=zeros(MAX_VAL,2);
    CLOSED=[];

    %将所有的障碍点记录在CLOSE列表中
    k=1;%Dummy counter
    for i=1:MAX_X
        for j=1:MAX_Y
            if(MAP(i,j) == -1)
                CLOSED(k,1)=i;
                CLOSED(k,2)=j;
                k=k+1;
            end
        end
    end
    CLOSED_COUNT=size(CLOSED,1);
    %将起始节点设置为第一个节点
    xNode=xval;
    yNode=yval;
    OPEN_COUNT=1;
    goal_distance=distance(xNode,yNode,xTarget,yTarget);%将节点到终点的距离设置为初始的hn值
    path_cost=0;%初始的gn值
    OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,goal_distance,path_cost,goal_distance);
    OPEN(OPEN_COUNT,1)=0;%OPEN列表中的第一列元素用于判断元素是否是已经被确定的节点，值为0是表示该节点已经被采用，值为1表示该节点并没有被采用
    %将已经被确定的节点放入CLOSE列表中
		CLOSED_COUNT=CLOSED_COUNT+1;
    CLOSED(CLOSED_COUNT,1)=xNode;
    CLOSED(CLOSED_COUNT,2)=yNode;
    NoPath=1;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 算法开始
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    while(1)
     gn = distance(xStart,yStart,xNode,yNode);%记录起始点到节点的距离作为gn值
		 %记录一个扩充数组，数组的具体内容在函数expand_array中
     exp_array=expand_array(xNode,yNode,gn,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);
     %获取数组长度
		 size_array = size(exp_array,1);
     for i=size_array:-1:1
         flag=0;
        for j=1:OPEN_COUNT
						%判断，确保队列不往回走（这部分小编没看懂，懂的大神可以在评论区解释一下）
            if(OPEN(j,2) == exp_array(i,1) && OPEN(j,3) == exp_array(i,2))
                if(exp_array(i,4)<OPEN(j,7))
                   OPEN(j,7) = exp_array(i,4);
                end
                flag=1;
            end
        end
        if(flag == 0)%将节点附近的一个单位的节点加入OPEN列表
           OPEN_COUNT=OPEN_COUNT+1;
           OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5));
           OPEN(OPEN_COUNT,1)=1;%将第一个元素设为1代表该点是临时节点并不是已经确定的节点
        end
     end
		 %计算OPEN列表中的临时节点，计算fn值，将最小的fn值的列表索引返回
     list_min=min_fn(OPEN,OPEN_COUNT,xTarget,yTarget);
		 %当最后一个节点为目标点时，跳出循环
     if(OPEN(list_min,2) == xTarget && OPEN(list_min,3) == yTarget)   
         break;
     end
		%更新参数，进入下一个节点
    xNode = OPEN(list_min,2);
    yNode = OPEN(list_min,3);
    OPEN(list_min,1)=0;
    CLOSED_COUNT=CLOSED_COUNT+1;
    CLOSED(CLOSED_COUNT,1)=xNode;
    CLOSED(CLOSED_COUNT,2)=yNode;

    end %End of While Loop
   %将目标点记录在path里
   path = [];
   path(1,1)=OPEN(list_min,2);
   path(1,2)=OPEN(list_min,3);
   i=2;
   n_index = node_index(OPEN,OPEN(list_min,4),OPEN(list_min,5));
   while(1)
       list_min = n_index;
       path(i,1)=OPEN(n_index,2);
       path(i,2)=OPEN(n_index,3);
       if(path(i,1) == 1 && path(i,2) == 1)
           break;
       end
       i=i+1;
       n_index = node_index(OPEN,OPEN(list_min,4),OPEN(list_min,5));
   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
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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132