A*算法matlab代码实现

代码结构

1.地图初始化

2.open_list和close_list初始化（分别包含——point，cost，parent）

循环

3.将open_llis中f最小点放入close_list中并将其移除

4.判断当前点（即最小点）是否为终点——若为则利用close_parent进行节点回溯

5.搜索当前点周围点

6.进行g值累加

7.计算周围点代价

8.若周围点在open_list中则进行f值和parent值修改

9.更新open_list

clear()
%% 地图初始化
cmap = [1 1 1; ...       % 1-白色-空地
    0 0 0; ...           % 2-黑色-静态障碍
    1 0 0; ...           % 3-红色-动态障碍
    1 1 0;...            % 4-黄色-起始点 
    1 0 1;...            % 5-品红-目标点
    0 1 0; ...           % 6-绿色-到目标点的规划路径   
    0 1 1];              % 7-青色-动态规划的路径
colormap(cmap);
rows=20;
cols=20;
start=1;
goal=rows*cols;

% 定义栅格地图全域，并初始化空白区域
field = ones(rows, cols);
% 障碍物设置
obsRate = 0.2;
obsNum = floor(rows*cols*obsRate);
obstacle = randi([1,rows*cols],obsNum,1);
field(obstacle) = 2;

%% cast初始化
g=0;
h=0;
f=0;
current_point=start;
current_cost=0;
current_parent=-1;
open_point=current_point;
open_cost=current_cost;
open_parent=current_parent;
close_point=[];
close_cost=[];
close_parent=[];
parent_list=[];
count=0;
%% 循环修改
while ~isempty(open_point)
    point_min=find(open_cost==min(open_cost),1);%最小值下标
    current_point=open_point(point_min);
    current_cost=open_cost(point_min);
    current_parent=open_parent(point_min);
    %open_list
    open_point(point_min)=[];
    open_cost(point_min)=[];
    open_parent(point_min)=[];
    %close_list
    close_point=[close_point,current_point];
    close_cost=[close_cost,current_cost];
    close_parent=[close_parent,current_parent];
    %到达终点进行节点回溯
    if current_point==goal
        parent=close_parent(end);
        parent_list=close_parent(end);
        while ~(parent==-1)
            point=find(close_point==parent);
            parent=close_parent(point);
            parent_list=[parent_list,parent];
        end
        break
    end
    %探索当前点的周围点
    [path_line,path_incline]=search(current_point,close_point,obstacle,rows,cols);
    open=[path_line,path_incline];
    %g值变换
    if current_cost==0
        g=0;
    else
        g=current_cost-distance(goal,current_point,cols); 
    end
    %循环遍历能经过的点
    for i=1:length(open)
        h=distance(goal,open(i),cols);
        if i<=length(path_line)
            g_change=g+1;
        else
            g_change=g+1.414;
        end
        f=g_change+h;
        %当遍历点在open_point中时进行判断更新
        if ismember(open(i),open_point)
            if g_change<open_cost(open_point==open(i))-h
               open_cost(open_point==open(i))=f;
               %更改父节点
               open_parent(open_point==open(i))=current_point;
            end 
        else
            open_point=[open_point,open(i)];
            open_cost=[open_cost,f];
            open_parent=[open_parent,current_point];
        end
    end
end

%% 画栅格图
if ~isempty(parent_list)
    parent_list(end)=[];
end
field(open_point)=6;
field(close_point)=7;
field(parent_list)=3;
field(start)=4;
field(goal)=5;
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',1.5,'GridAlpha',1);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
%% 函数
function [path_line,path_incline]=search(current,close_list,obstacle,rows,cols)
line=current;
grid_num=cols*rows;
%boundary
if mod(line,cols)==0%最后一行
    path_line=[line-cols,line-1,0,line+cols];
    path_incline=[line-rows-1,0,line+rows-1,0];
elseif mod(line,cols)==1%第一行
    path_line=[line-rows,0,line+1,line+rows];
    path_incline=[0,line-rows+1,0,line+rows+1];
else
    path_line=[line-rows,line-1,line+1,line+rows];
    path_incline=[line-rows-1,line-rows+1,line+rows-1,line+rows+1];
end
for i=1:4
    if path_line(i)<0 || grid_num<path_line(i)
        path_line(i)=0;
    end
end
for i=1:4
    if path_incline(i)<0 || grid_num<path_incline(i)
        path_incline(i)=0;
    end
end
%obstacle
for i=1:length(path_line)
    if ismember(path_line(i),obstacle) || ismember(path_line(i),close_list)
        path_line(i)=0;
    end
end
for i=1:length(path_incline)
    if ismember(path_incline(i),obstacle) || ismember(path_incline(i),close_list)
        path_incline(i)=0;
    end
end
%边角判断
%1
if path_line(1)==0 && path_line(3)==0
    path_incline(1)=0;
end
%2
if path_line(1)==0 && path_line(4)==0
    path_incline(2)=0;
end

%3
if path_line(2)==0 && path_line(4)==0
    path_incline(3)=0;
end
%4
if path_line(3)==0 && path_line(4)==0
    path_incline(4)=0;
end
path_line(path_line==0)=[];
path_incline(path_incline==0)=[];
end

%% 哈曼顿距离
function h=distance(goal,current,cols)
if goal==current
    h=0;
    return
end
x1=mod(goal,cols);
if x1==0
    x1=cols;
end
y1=fix(goal/cols);
x2=mod(current,cols);
y2=fix(current/cols)+1;
h=(x1-x2)^2+(y1-y2)^2;
end