A*算法在matlab上的实现

先看看算法的效果

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

算法下载位置：
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);

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

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