A*算法路径规划的matlab核心代码

function [astar_pathcost_astarastar_time]=astararithmetic2（Astar_coornodmatobstaclenumbernooxnooynoozsd）
tic;
astar_path=[];

f_value_start=max（[abs（nodmat（d2）-nodmat（s2））abs（nodmat（d3）-nodmat（s3））abs（nodmat（d4）-nodmat（s4））]）;          %初始源节点的f代价
second_value（s）=abs（nodmat（d2）-nodmat（s2））+abs（nodmat（d3）-nodmat（s3））+abs（nodmat（d4）-nodmat（s4））; %附属代价值
node（s）.message=[f_value_startsecond_value（s）snodmat（s2）nodmat（s3）nodmat（s4）0];
openlist=[node（s）.message]; %初始化openlist
closedlist=[0 0 0 0 0 0 0]; %初始化closedlist
cost_astar=0;
node（s）.pionter=[s];  %指针初始化

%% *************************************************************************



while （isempty（find（closedlist（:3）==d1）） && ~isempty（openlist））
%%
    cost_astar=cost_astar+1;
    if isempty（openlist）
        disp（‘erorropenlist is empty‘）;
        return
    end  %判断open列表是否为空，为空则搜索失败
  %%%%%%%%%%%%%%  
    openlist=sortrows（openlist）; %将openlist中的节点按f代价值进行升序排列
    n=openlist（13）;             %n为openlist中f代价最小的节点的编号
    closedlist=[closedlist;openlist（1:）]; %将openlist中f代价最小的节点放入closedlist中
    if n==d
        astar_path=node（n）.pionter;
        break
    end
        
    openlist=setdiff（openlistopenlist（1:）‘rows‘）; %将openlist中f代价最小的节点从openlist中删除
   %%%%%%%%%%%%%%%%%%% 
    
    node（n）.succeed=node_expand（nnooxnooynooz）;  %产生后继节点
    
    node（n）.succeed=setdiff（node（n）.succeedobstaclenumber）;     %排除障碍节点
    
    for i=1:length（node（n）.succeed）
        node（node（n）.succeed（i））.pionter_t=[node（n）.pionternode（n）.succeed（i）];  %为每个后继节点生成临时指针
    
        g_value（node（n）.succeed（i））=length（node（node（n）.succeed（i））.pionter_t）;  %后继节点的g代价值
        
        if find（openlist（:3）==node（n）.succeed（i））       %如果后继节点已在openlist表中
            j=find（openlist（:3）==node（n）.succeed（i））;   %找出节点中openlist表中行的位置
            if openlist（j7）>g_value（node（n）.succeed（i））  %如果后继的g代价比原始路径的g代价小
                openlist（j1）=openlist（j1）-（openlist（j