A*算法原理和代码详解

1、A*算法原理

如上图所示，红色（2.3）起点，红色（6，3）终点，黑色表示障碍物。

如何走出如图所示的路径呢？

这里博主采用A*算法。原理如下：

1、基本定义

A*（A-Star）算法是一种常用的寻路算法，用于图形表达的环境中找到起点到目标点的最短路径。

代价函数𝑓(𝑛)由两部分组成：起点沿着已生成的路径到达当前节点的开销𝑔(𝑛)和当前节点到终点的预估开销ℎ(𝑛)。公式表示： 𝑓(𝑛) = 𝑔(𝑛) + ℎ(𝑛)

这里我更称之为：𝑔(𝑛)已走代价、 ℎ(𝑛)预估代价（预估代价常用的有两种，曼哈顿距离和欧几里得距离本文这里采用曼哈顿距离 

h = abs(end_node(1) - openlist(i,1))+abs(end_node(2) - openlist(i,2));%预估代价这里是曼哈顿距离，欧几里得距离是开方那个）

open列表:一个记录下所有被考虑来寻找最短路径的格子
closed列表: 一个记录下不会再被考虑的格子

2、A*算法的详细原理

这里参考了其他博主的原理

【精选】A*算法（超级详细讲解，附有举例的详细手写步骤）-CSDN博客

 这里重点注意下最后一张图片6(c)里面，当选中的节点a为当前父节点时，此时节点a的子节点里面有（c1、c2、.....c6等等），判断计算得到的子节点是否在open_list中，若此时的子节点c,若在，则比较更新，这个时候要计算下从a->c的g值，计算公式为

             g = openlist_cost(min_index,1)+norm(parent_node - child_node);分为上一个的父节点的g加上从a->c的g值，对比下原先到c的g值，若比原先的小，则做如下操作：

2、详细代码 

clc;
clear;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画地图%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 栅格地图的行数和列数
m = 5;
n =7;
start_node = [2,3];
end_node = [6,3];
obs = [4,2;4,3;4,4];
% 画栅格地图
for i = 1:m 
    plot([0,n],[i,i],'k');%画横线
    hold on;
end
for i = 1:n
   plot([i,i],[0,m],'k'); 
end
axis equal;
xlim([0,n]);
ylim([0,m]);
%绘制起点、终点、障碍物
fill([start_node(1)-1,start_node(1),start_node(1),start_node(1)-1],...
     [start_node(2)-1,start_node(2)-1,start_node(2),start_node(2)] ,'r');
 fill([end_node(1)-1,end_node(1),end_node(1),end_node(1)-1],...
     [end_node(2)-1,end_node(2)-1,end_node(2),end_node(2)] ,'r');
  for i = 1:size(obs,1)%返回矩阵行数,
    temp_node = obs(i,:); 
    fill([temp_node(1)-1,temp_node(1),temp_node(1),temp_node(1)-1],...
     [temp_node(2)-1,temp_node(2)-1,temp_node(2),temp_node(2)] ,'k');
 end
%%%%%%%%%%%%%%%%%%%%%%%%        A*算法                %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%初始化closelist
closelist = start_node;
closelist_path = {start_node,start_node};
closelist_cost = 0;
child_nodes = child_nodes_cal(start_node,m,n,obs,closelist);%子节点搜索函数

%%初始化openlist
%openlist_path这个元胞第一列表示搜索到子节点坐标，第二列是从当前父节点，到子节点的坐标
openlist  = child_nodes;
for i = 1:size(openlist,1)
   openlist_path{i,1} = openlist(i,:);
   openlist_path{i,2} = [start_node;openlist(i,:)];
end
%%计算G()和h()
for i = 1:size(openlist,1)
    g = norm(start_node - openlist(i,1:2));%当前代价
    h = abs(end_node(1) - openlist(i,1))+abs(end_node(2) - openlist(i,2));%预估代价这里是曼哈顿距离，欧几里得距离是开方那个
    f= g+h;
    openlist_cost(i,:)=[g,h,f];
end
 %%开始A*搜索
 %从openlist_cost中搜索移动代价最小的节点
 [~,min_index] = min(openlist_cost(:,3));%这里min函数返回最小值和其对应的索引,这里不需要最小是多少，只需要索引即可，因此用~表示占位符号
 parent_node = openlist(min_index,:);
 %%进入循环
 flag = 1;
 while flag
    child_nodes = child_nodes_cal(parent_node,m,n,obs,closelist); 
    for i = 1:size(child_nodes,1)%这里是为了画出来每次搜索的子节点
        path_tmp2 = child_nodes(i,:);
        fill([path_tmp2(1)-1,path_tmp2(1),path_tmp2(1),path_tmp2(1)-1],...
            [path_tmp2(2)-1,path_tmp2(2)-1,path_tmp2(2),path_tmp2(2)],'b');
    end
    hold on;
    pause(0.5);%给个暂停时间，可以显示动图
    %判断计算得到的子节点是否在open_list中，若在，则比较更新，这里更新是为了回溯父节点；若没在，则追加到openlist中
    for i = 1:size(child_nodes,1)
       child_node = child_nodes(i,:);
       [in_flag,openlist_index] = ismember(child_node,openlist,'rows');%表示当前节点chile_node是否在openlist中，若在则in_flag = 1,openlist_index返回的是对应的索引
       g = openlist_cost(min_index,1)+norm(parent_node - child_node);
       h = abs(child_node(1) - end_node(1))+ abs(child_node(2) - end_node(2));
       f = g+ h;
       if in_flag == 1
           if g < openlist_cost(openlist_index,1)%只有此时计算的g值小于原先的g，才更新，否则不计算
               openlist_cost(openlist_index,1) = g;
               openlist_cost(openlist_index,3) = f;
               openlist_path{openlist_index,2} = [openlist_path{min_index,2};child_node];%这里更换了原先的父节点(原先的父节点为上一次的父节点第一次为起始点（2，3）)，选择此时的父节点也就是计算的openlist(min_index,:)最小的点（2.4）当原先的父节点，此时的openlist_path{min_index,2}，为元胞数组第二列
           end
       else
           openlist(end+1,:) = child_node;
           openlist_cost(end + 1,:) = [g,h,f];
           openlist_path{end + 1,1} = child_node;
           openlist_path{end,2} = [openlist_path{min_index,2};child_node];%这里因为 openlist_past{end + 1,1} = child_node，第一列已经扩展出来了，对应的这一行的第二列补齐为[]，因此这里的openlist_path{end,2}，就相当于没扩展之前的openlist_path{end+ 1,2}
       end
    end
    %从openlist中移除代价最小的节点到closelist中
    closelist(end + 1,:) = parent_node;
    closelist_cost(end + 1,1) = openlist_cost(min_index,3);
    closelist_path(end + 1,:) = openlist_path(min_index,:);
        path_tmp = closelist_path{end,2};
    for i = 1:size(path_tmp,1)%返回矩阵行数,
        path_tmp1 = path_tmp(i,:);
        fill([path_tmp1(1)-1,path_tmp1(1),path_tmp1(1),path_tmp1(1)-1],...
            [path_tmp1(2)-1,path_tmp1(2)-1,path_tmp1(2),path_tmp1(2)] ,'g');
        hold on
%         pause(0.5);
    end
    openlist(min_index,:) = [];
    openlist_cost(min_index,:) = [];
    openlist_path(min_index,:) = [];
    %重新搜索
     [~,min_index] = min(openlist_cost(:,3));%这里min函数返回最小值和其对应的索引,这里不需要最小是多少，只需要索引即可，因此用~表示占位符号
     parent_node = openlist(min_index,:);
     
     %判断是否搜索到终点
     if parent_node == end_node
         closelist(end + 1,:) = parent_node;
         closelist_cost(end + 1,1) = openlist_cost(min_index,3);
         closelist_path(end + 1,:) = openlist_path(min_index,:);
         flag = 0;
     end
 end
 %%画路径
 path_opt = closelist_path{end,2};
 path_opt(:,1) =  path_opt(:,1) - 0.5;%减去0.5是为了画图显示在正中间
 path_opt(:,2) =  path_opt(:,2) - 0.5;
 scatter( path_opt(:,1), path_opt(:,2),'k');%画散点图
 plot( path_opt(:,1), path_opt(:,2),'k')

 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 

子函数 child_nodes_cal

function child_nodes =   child_nodes_cal(parent_node,m,n,obs,closelist)
    child_nodes = [];
    field = [1,1;n,1;n,m;1,m];%限定地图范围
    
    %%第一个子节点
    child_node = [parent_node(1) - 1,parent_node(2) + 1];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第二个子节点
    child_node = [parent_node(1),parent_node(2) + 1];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第3个子节点
    child_node = [parent_node(1) + 1,parent_node(2) + 1];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第4个子节点
    child_node = [parent_node(1) + 1,parent_node(2)];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第5个子节点
    child_node = [parent_node(1) + 1,parent_node(2) - 1];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第6个子节点
    child_node = [parent_node(1),parent_node(2) - 1];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第7个子节点
    child_node = [parent_node(1) - 1,parent_node(2) - 1];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
        %%第8个子节点
    child_node = [parent_node(1) - 1,parent_node(2)];
    if inpolygon(child_node(1),child_node(2),field(:,1),field(:,2))
        if ~ismember(child_node,obs,'rows')%判断child_nodes的行数是否和obs的行数相等
           child_nodes = [child_nodes;child_node]; 
        end
    end
    %%排除已经存在于closelist的节点
    delete_idx = [];
    for i = 1:size(child_nodes,1)
        if ismember(child_nodes(i,:),closelist,'rows')
            delete_idx(end + 1) = i;
        end
    end
    child_nodes(delete_idx,:) = [];
  
end

3、路径结果

这里也可以换一个复杂路径