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