AStar算法（MATLAB实现）

METTERS

已于 2024-03-06 14:18:53 修改

阅读量1.5k

点赞数 12

文章标签：算法 matlab 启发式算法图搜索算法

于 2024-03-06 13:46:56 首次发布

本文链接：https://blog.csdn.net/METTERS/article/details/136503792

版权

A*算法简介：

A*（A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。

结合Dijkstra算法与BFS算法的优点，得到的就是A星算法，A*算法是一种启发式搜索算法，它是在状态空间中的搜索，首先对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标。这样可以省略大量无谓的搜索路径，提高了效率。在启发式搜索中，对位置的估价是十分重要的。采用了不同的估价可以有不同的效果。公式表示为： f(n)=g(n)+h(n),其中， f(n) 是从初始状态经由状态n到目标状态的代价估计，g(n) 是在状态空间中从初始状态到状态n的实际代价，h(n) 是从状态n到目标状态的最佳路径的估计代价，对于路径搜索问题，状态就是图中的节点，代价就是距离。h(n)的选取：保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取（或者说h(n)的选取）。我们以d(n)表达状态n到目标状态的距离，那么h(n)的选取大致有如下三种情况：

（1）如果h(n)< d(n)到目标状态的实际距离，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解。

（2）如果h(n)=d(n)，即距离估计h(n)等于最短距离，那么搜索将严格沿着最短路径进行，此时的搜索效率是最高的。

（3）如果 h(n)>d(n)，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

此外：，如果h(n)为0，则只g(n)起作用，A*变成Dijkstra算法，保证找到最短路径。如果h(n)总是低于（或等于）从目标移动n到目标的成本，则保证 A* 找到最短路径。越低h(n)，节点 A* 扩展得越多，速度越慢。如果h(n)正好等于从n到目标的移动成本，那么 A* 将只遵循最佳路径，而不会扩展其他任何东西，使其非常快。尽管您不能在所有情况下都做到这一点，但您可以在某些特殊情况下做到这一点。很高兴知道给定完美的信息，A* 将表现完美。如果h(n)有时大于从移动n到目标的成本，则不能保证 A* 找到最短路径，但它可以运行得更快。在另一个极端，如果h(n)相对于非常高g(n)，则仅h(n)起作用，并且 A* 变成贪婪的最佳优先搜索。

A*算法伪代码：

把起始格添加到 "开启列表"

do{

寻找开启列表中F值最低的格子, 我们称它为当前格.

把它切换到关闭列表.

对当前格相邻的8格中的每一个

if (它不可通过 || 已经在 "关闭列表" 中) {

什么也不做.

}

if (它不在开启列表中) {

把它添加进 "开启列表", 把当前格作为这一格的父节点, 计算这一格的 FGH

}

if (它已经在开启列表中) {

if (用 G 值为参考检查新的路径是否更好, 更低的G值意味着更好的路径) {

把这一格的父节点改成当前格, 并且重新计算这一格的 GF 值.

}

} while( 目标格已经在 "开启列表", 这时候路径被找到)

如果开启列表已经空了, 说明路径不存在.

最后从目标格开始, 沿着每一格的父节点移动直到回到起始格, 这就是路径.

代码：

%% 单次路径规划
clear all;

%% 初始化智能体
fig = figure();				% 创建图形窗口
hold on;

% 设置区域边界
Estart_x = 0;  Estart_y = 0;
Weight = 80;  High = 80;
dL = 1;   %设置网格大小

axis([Estart_x-5 Weight 0 High]);
axis equal;  %设置屏幕高宽比，使得每个坐标轴的具有均匀的刻度间隔
%添加模糊刻度
set(gca,'XMinorTick','on')
set(gca,'YMinorTick','on')
xlabel('X(m)');    ylabel('Y(m)'); 
grid minor   %在刻度线之间添加次网格线
rectangle('position',[Estart_x,Estart_y,Weight,High],'LineWidth',2);   %画边界

%% 区域网格化
xL = 0:dL:Weight;
yL = 0:dL:High;
[XL , YL] = meshgrid(xL,yL);   %获得网格化后的X矩阵和Y矩阵
C = zeros(size(XL));    %存储网格的高度值
%--------------------%
C(1,:) = 100;   C(:,1) = 100;
C(end,:) = 100; C(:,end) = 100;

%% 设置障碍物覆盖值
%--------------------%
a1(1:21) = 20;
C((20:40),20)=80;
plot((20:40),a1,'k-','LineWidth',2);
%--------------------%
a2(1:30) = 30;
C((1:29),30)=80;
plot((0:29),a2,'k-','LineWidth',2);
%--------------------%
a3(1:20) = 50;
C(50,(31:50))=80;
plot(a3,(31:50),'k-','LineWidth',2);
%--------------------%
a4(1:21) = 60;
C((40:60),60)=80;
plot((40:60),a4,'k-','LineWidth',2);
%--------------------%
a5(1:21) = 25;
C(25,(40:60))=80;
plot(a5,(40:60),'k-','LineWidth',2);

close_list=0;open_list=0;                    %初始化节点容器
start_x=70;start_y=3;                         %设置起点位置
end_x=5;end_y=70;                            %设置终点位置

i=1;
%本节点x坐标
father_node(:,1)=start_x;                                                     
%本节点y坐标
father_node(:,2)=start_y;                                                    
%本节点f(n)
father_node(:,3)=norm([father_node(:,1),father_node(:,2)]-[start_x,start_y])+(father_node(:,1)-end_x)^2+(father_node(:,2)-end_y)^2;    
%本节点g(n)
father_node(:,4)=norm([father_node(:,1),father_node(:,2)]-[start_x,start_y]);
%父节点y坐标
father_node(:,5)=start_x;                                                     
%父节点y坐标
father_node(:,6)=start_y;   
close_list=father_node;

 %返回8个邻居节点
[N,index]=land(father_node(:,1),father_node(:,2),start_x,start_y,end_x,end_y,Estart_x,Estart_x+Weight,Estart_y,Estart_x+High,C,close_list);
last_open_list = sortrows(N,3);                                         %排序并存入临时open_list
father_node=last_open_list(1,:);                                        %f(n)最小邻居设为父节点
last_open_list(1,:)=[];                                                 %删除临时open_list中最小f(n)节点
cend=1;

while close_list(cend,1)~=end_x||close_list(cend,2)~=end_y
    cend=cend+1;
    x=father_node(1,1);                                                     %返回f(n)最小节点坐标
    y=father_node(1,2);
    [N,index]=land(father_node(:,1),father_node(:,2),start_x,start_y,end_x,end_y,Estart_x,Estart_x+Weight,Estart_y,Estart_x+High,C,close_list);
    for i=1:index
        a=N(i,1)==father_node(1,1); 
        b=N(i,1)==father_node(1,2); 
        f=N(i,1);
        g=N(i,1);
        if a>0&&b>0
            if father_node(1,3)>g
                father_node(1,1)=N(i,1); 
                father_node(1,1)=N(i,2); 
                father_node(1,1)=N(i,3); 
                father_node(1,1)=N(i,4); 
                father_node(1,1)=N(i,5); 
                father_node(1,1)=N(i,6); 
            end
        else
            last_open_list(i,:)=N(i,:);
            plot(N(i,1),N(i,2),'bs');     %正方形(表示待检测节点)
        end
        
    end
    last_open_list = sortrows(last_open_list,3);
    close_list(cend,1)=father_node(1,1);
    close_list(cend,2)=father_node(1,2);
    close_list(cend,3)=father_node(1,3);
    close_list(cend,4)=father_node(1,4);
    close_list(cend,5)=father_node(1,5);
    close_list(cend,6)=father_node(1,6);
    
    father_node=last_open_list(1,:);
    last_open_list(1,:)=[];
    
end
plot(close_list(:,1),close_list(:,2),'r*');   %当前节点标绿色*号



function [N,index]=land(x,y,start_x,start_y,end_x,end_y,xmin,xmax,ymin,ymax,C,close_list)
    index=0;
    %上邻域
    if judge(x,y+1,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x;
        N(index,2)=y+1;
        N(index,3)=norm([x,y+1]-[start_x,start_y])+(x-end_x)^2+(y+1-end_y)^2;
        N(index,4)=norm([x,y+1]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;
        
    end
    %下邻域
    if judge(x,y-1,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x;
        N(index,2)=y-1;
        N(index,3)=norm([x,y-1]-[start_x,start_y])+(x-end_x)^2+(y-1-end_y)^2;
        N(index,4)=norm([x,y-1]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;

    end
    %左邻域
    if judge(x-1,y,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x-1;
        N(index,2)=y;
        N(index,3)=norm([x-1,y]-[start_x,start_y])+(x-1-end_x)^2+(y-end_y)^2;
        N(index,4)=norm([x-1,y]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;

    end
    %右邻域
    if judge(x+1,y,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x+1;
        N(index,2)=y;
        N(index,3)=norm([x+1,y]-[start_x,start_y])+(x+1-end_x)^2+(y-end_y)^2;
        N(index,4)=norm([x+1,y]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;

    end
    %左上邻域
    if judge(x-1,y+1,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x-1;
        N(index,2)=y+1;
        N(index,3)=norm([x-1,y+1]-[start_x,start_y])+(x-1-end_x)^2+(y+1-end_y)^2;
        N(index,4)=norm([x-1,y+1]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;

    end
    %右上邻域
    if judge(x+1,y+1,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x+1;
        N(index,2)=y+1;
        N(index,3)=norm([x+1,y+1]-[start_x,start_y])+(x+1-end_x)^2+(y+1-end_y)^2;
        N(index,4)=norm([x+1,y+1]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;
    end
    %左下邻域
    if judge(x-1,y-1,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x-1;
        N(index,2)=y-1;
        N(index,3)=norm([x-1,y-1]-[start_x,start_y])+(x-1-end_x)^2+(y-1-end_y)^2;
        N(index,4)=norm([x-1,y-1]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;
    end
    %右下邻域
    if judge(x+1,y-1,xmin,xmax,ymin,ymax,C,close_list)==1
        index=index+1;
        N(index,1)=x+1;
        N(index,2)=y-1;
        N(index,3)=norm([x+1,y-1]-[start_x,start_y])+(x+1-end_x)^2+(y-1-end_y)^2;
        N(index,4)=norm([x+1,y-1]-[start_x,start_y]);
        N(index,5)=x;
        N(index,6)=y;
    end
end

function ruselt=judge(x,y,xmin,xmax,ymin,ymax,C,close_list)
    if x<xmin||x>xmax||y<ymin||y>ymax
        ruselt=0;
        return
    end
    if C(x,y)==100||C(x,y)==80
        ruselt=0;
        return
    end

    [i,j]=size(close_list);

    for ii=1:i
        if close_list(ii,1)==x&&close_list(ii,2)==y
        ruselt=0;
        return
        end
    end
    
    ruselt=1;


end