在matlab中使用A*算法进行二维路径规划

 首先指路博主慕羽★的博客：详细介绍用MATLAB实现基于A*算法的路径规划（附完整的代码，代码逐行进行解释）（一）--------A*算法简介和环境的创建_a*算法路径规划matlab_慕羽★的博客-CSDN博客

非常感谢博主的分享和解惑，在他的代码基础上我添加了可斜向拐弯（同时避免触碰到障碍物尖角）等功能，但也去除了音乐播放等功能。基于他的算法思路，将算法从二维拓展到三维环境中进行路径规划，以下将分别进行阐述，代码基本都有注释，少量参数设置将稍加说明。

一、算法改进

1、将每次可从四方向拓展改进为八方向拓展，即可斜向拓展

        首先将储存方向的数组初始化，添加右上、左上、右下、左下四个方向，并分别定义为'RD','LD','RU','LU'。

movementdirections = {'R','L','D','U','RD','LD','RU','LU'};  %移动方向,增加右上、左上、右下、左下四个方向

        在findvalue函数中，将原有的四方向增加为八方向，并根据之前定义的每个方向的含义编写代码。再根据该函数的写法，补充路径溯回函数findWayBack（）等。

2、增设了参数getDistance_h，用来选择是否使用欧几里得距离

        若设为0，则启发式函数h（n）使用对角公式，若设为1，则使用欧几里得距离公式，可用来对比启发函数用欧几里得距离或是对角距离的区别。同样地，若要对比另两种启发函数的区别，只需要更改heuristic()后的式子即可。

        当完成方向拓展后，运行结果如下：发现规划出来的路线可能会经过障碍物的四个角落，如左下图。因此增设参数Corner_obstacles，在计算斜向拓展代价时，判断垂直其拓展方向的两侧有没有障碍物，如向父节点左上方拓展时，需判断父节点的左方以及上方是否有障碍物，若有则不能拓展，以此获得不触碰障碍物四角的安全路线，如右下图。

 3、增设直角拐弯代价参数quarter_turn

        考虑无人机等在进行直角拐弯时，速度会受到影响，进而增加耗能，因此增设参数quarter_turn表示每一次直角拐弯的代价，使规划的路径尽可能减少直角拐弯的次数，也使的路径更加圆滑。

%%功能参数的设定部分
n = 40;   % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.3;  % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=2;       %动态衡量启发式A星算法中的h（n）权重系数
quarter_turn=10; %直角转弯一次需要消耗的代价参数，

Corner_amend=0;  %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
Environmental_Set=0; %这个参数用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
heuristicmethod=1;   %这个参数用来选择是否使用欧几里得距离，若设为0，则启发式函数h（n）使用对角公式，若设为1，则使用欧几里得距离公式
eight_directions=1; %这个参数用来选择是否可以斜向拓展，若设为0，则只能向四周4个方向拓展，若设为1，则可以向四周8个方向拓展
Corner_obstacles=1; %这个参数用来选择是否关注障碍物的四角，若设为0，则忽略障碍物的四角，若设为1，则不能穿越障碍物的四角 
Reset_GS=0;   %这个参数用来选择是否重新设定起始点和终止点，若设定为1，开始重新设定起始点和终止点，同时需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则关闭
New_startposind=380;   New_goalposind=270;  %若将Reset_GS设定为1，则需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，要确保新设的这两个点处没有障碍物


%用来存放规划的路径的长度
Road_Long=0;
%用来存放拐弯次数
turn_count=0; 


% 路径规划中用到的一些矩阵的初始化
setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
movementdirections = {'R','L','D','U','RD','LD','RU','LU'};  %移动方向,增加右上、左上、右下、左下四个方向

%初始化一些进行路径的修正需要用到的变量
Parent_node=0; %Parent_node初始化，否则会报错
Expected_note=0;%Expected_note初始化，否则会报错
untext_ii=0;  %未经过检验的新的ii
amend_count=0; % 记录修正的次数
temp1=0;   %初始化将扩展点至起始点的代价为0

% 这个函数用来随机生成环境，障碍物，起点，终点
axishandle = createFigure(field,costchart,startposind,goalposind);    %将随机生成的方格及障碍物的数据生成图像




%% 利用随机生成的环境数据来进行环境的绘制

function axishandle = createFigure(field,costchart,startposind,goalposind)

      % 这个if..else结构的作用是判断如果没有打开的figure图，则按照相关设置创建一个figure图
      if isempty(gcbf)                                       %gcbf是当前返回图像的句柄，isempty(gcbf)假如gcbf为空的话，返回的值是1，假如gcbf为非空的话，返回的值是0
      figure('Position',[560 70 700 700], 'MenuBar','none');  %对创建的figure图像进行设置，设置其距离屏幕左侧的距离为450，距离屏幕下方的距离为50，长度和宽度都为700，并且关闭图像的菜单栏
      axes('position', [0.01 0.01 0.99 0.99]);               %设置坐标轴的位置，左下角的坐标设为0.01,0.01   右上角的坐标设为0.99 0.99  （可以认为figure图的左下角坐标为0 0   ，右上角坐标为1 1 ）
      else
      gcf; cla;   %gcf 返回当前 Figure 对象的句柄值，然后利用cla语句来清除它
      end
      
      n = length(field);  %获取矩阵的长度，并赋值给变量n
      field(field < Inf) = 0; %将fieid矩阵中的随机数（也就是没有障碍物的位置处）设为0
      pcolor(1:n+1,1:n+1,[field field(:,end); field(end,:) field(end,end)]);%多加了一个重复的（由n X n变为 n+1 X n+1 ）
 
      cmap = flipud(colormap('jet'));  %生成的cmap是一个256X3的矩阵，每一行的3个值都为0-1之间数，分别代表颜色组成的rgb值
      cmap(1,:) = zeros(3,1); cmap(end,:) = ones(3,1); %将矩阵cmap的第一行设为0 ，最后一行设为1
      colormap(flipud(cmap)); %进行颜色的倒转 
      hold on;
      axishandle = pcolor([1:n+1],[1:n+1],[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);  %将矩阵costchart进行拓展，插值着色后赋给axishandle
      [goalposy,goalposx] = ind2sub([n,n],goalposind);
      [startposy,startposx] = ind2sub([n,n],startposind);
       plot(goalposx+0.5,goalposy+0.5,'ys','MarkerSize',10,'LineWidth',6); % 画出起点和终点
       plot(startposx+0.5,startposy+0.5,'go','MarkerSize',10,'LineWidth',6);
       %uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, 'Position', [1 1 60 40], 'Callback','astardemo');
end

%% 这个while循环是本程序的核心，利用FindValue函数中的循环进行迭代来寻找终止点

tic

while ~max(ismember(setOpen,goalposind)) && ~isempty(setOpen)
   
    [temp, ii] = min(setOpenCosts +Weights*setOpenHeuristics);     %寻找拓展出来的最小值 
   
    if ((setOpen(ii)~=startposind) && (Corner_amend==1))
    [new_ii,amend_count_1]=Path_optimization(temp1,temp,ii,fieldpointers,setOpen,setOpenCosts,startposind,Weights,setOpenHeuristics,Parent_node,Expected_note,untext_ii,amend_count); %进行路径的修正，在保证不增加距离的基础上，使其减少转弯的次数
    ii=new_ii;
    amend_count=amend_count_1;
    end
    
    %这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价costs找到
    [costs,heuristics,posinds] = findFValue(quarter_turn,fieldpointers,eight_directions,Corner_obstacles,setOpen(ii),setOpenCosts(ii),field,goalposind,heuristicmethod);
    
  setClosed = [setClosed; setOpen(ii)];     % 将找出来的拓展出来的点中代价最小的那个点串到矩阵setClosed 中 
  setClosedCosts = [setClosedCosts; setOpenCosts(ii)];    % 将拓展出来的点中代价最小的那个点的代价串到矩阵setClosedCosts 中
  
  % 从setOpen中删除刚才放到矩阵setClosed中的那个点
  %如果这个点位于setopen矩阵的内部
  if (ii > 1 && ii < length(setOpen))
    setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];
    setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];
    setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];
    
  %如果这个点位于setopen矩阵第一行
  elseif (ii == 1)
    setOpen = setOpen(2:end);
    setOpenCosts = setOpenCosts(2:end);
    setOpenHeuristics = setOpenHeuristics(2:end);
    
  %如果这个点位于setopen矩阵的最后一行
  else
    setOpen = setOpen(1:end-1);
    setOpenCosts = setOpenCosts(1:end-1);
    setOpenHeuristics = setOpenHeuristics(1:end-1);
  end
    
  % 把拓展出来的点中符合要求的点放到setOpen 矩阵中，作为待选点
  for jj=1:length(posinds)   % 在新拓展的4或8个点中，依次寻找代价最小的点，每次循环需要判断的点为posinds(jj)
  
    if ~isinf(costs(jj))   % 判断该点（方格）处没有障碍物       
      % 判断一下该点是否 已经存在于setOpen 矩阵或者setClosed 矩阵中
      % 如果我们要处理的拓展点既不在setOpen 矩阵，也不在setClosed 矩阵中
      if ~max([setClosed; setOpen] == posinds(jj))
        fieldpointers(posinds(jj)) = movementdirections(jj);
        costchart(posinds(jj)) = costs(jj);
        setOpen = [setOpen; posinds(jj)];
        setOpenCosts = [setOpenCosts; costs(jj)];
        setOpenHeuristics = [setOpenHeuristics; heuristics(jj)];
        
      % 如果我们要处理的拓展点已经在setOpen 矩阵中
      elseif max(setOpen == posinds(jj))
        I = find(setOpen == posinds(jj));
        % 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
        if setOpenCosts(I) > costs(jj)
          costchart(setOpen(I)) = costs(jj);
          setOpenCosts(I) = costs(jj);
          setOpenHeuristics(I) = heuristics(jj);
          fieldpointers(setOpen(I)) = movementdirections(jj);
        end
        
        % 如果我们要处理的拓展点已经在setClosed 矩阵中
      else
        I = find(setClosed == posinds(jj));
        % 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
        if setClosedCosts(I) > costs(jj)
          costchart(setClosed(I)) = costs(jj);
          setClosedCosts(I) = costs(jj);
          fieldpointers(setClosed(I)) = movementdirections(jj);
        end
      end
    end
  end
  
  if isempty(setOpen) break; end
  set(axishandle,'CData',[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);
  set(gca,'CLim',[0 1.1*max(costchart(find(costchart < Inf)))]);
  drawnow; 
end

toc

%% 拐角修正函数

function [new_ii,amend_count_1] = Path_optimization(temp1,temp, ii,fieldpointers,setOpen,setOpenCosts,startposind,Weights,setOpenHeuristics,Parent_node,Expected_note,untext_ii,amend_count)
   n = length(fieldpointers);  %获取矩阵的长度，并赋值给变量n
   
 %获取其父节点的索引值
  switch fieldpointers {setOpen(ii)}
        case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的
           Parent_node = setOpen(ii) - n;
        case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的
           Parent_node = setOpen(ii) + n;
        case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的
           Parent_node = setOpen(ii) -1;
        case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的
           Parent_node = setOpen(ii) + 1;  
        case 'LU' % ’LU’ 表示当前的点是由左上边的点拓展出来的
           Parent_node = setOpen(ii) - n - 1;
        case 'RU' % ’RU’ 表示当前的点是由右上边的点拓展出来的
           Parent_node = setOpen(ii) + n - 1;
        case 'LD' % ’LD’ 表示当前的点是由左下面的点拓展出来的
           Parent_node = setOpen(ii) - n + 1;
        case 'RD' % ’RD’ 表示当前的点是由右下边的点拓展出来的
           Parent_node = setOpen(ii) + n + 1;
  end
  
  if Parent_node==startposind  %如果这个点的父节点是起始点的话，跳过修正
     new_ii=ii;
     amend_count_1=amend_count;
  else 
  
       %获取期望下一步要走的点的索引值
     switch fieldpointers{Parent_node}
          
        case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的,走直线的话，我们期望要走的下一个点为此点右边的点
           Expected_note = Parent_node + n;
        case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点左边的点
           Expected_note = Parent_node - n;
        case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的，走直线的话，我们期望要走的下一个点为此点下面的点
           Expected_note = Parent_node +1;
        case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点上面的点
           Expected_note = Parent_node - 1;
        case 'LU' % ’LU’ 表示当前的点是由左上边的点拓展出来的,走直线的话，我们期望要走的下一个点为此点右下边的点
           Expected_note = Parent_node + n + 1;
        case 'RU' % ’RU’ 表示当前的点是由右上边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点左下边的点
           Expected_note = Parent_node - n + 1;
        case 'LD' % ’LD’ 表示当前的点是由左下面的点拓展出来的，走直线的话，我们期望要走的下一个点为此点右上面的点
           Expected_note = Parent_node + n - 1;
        case 'RD' % ’RD’ 表示当前的点是由右下边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点左上面的点
           Expected_note = Parent_node - n - 1;
     end
   
     if ((Expected_note<=0)||(Expected_note>n*n))   %如果我们期望的点不在待选点矩阵setOpen中，或者超出边界，跳过修正
          new_ii=ii;
          amend_count_1=amend_count;
     else
           
           %计算新的要进行拓展的点在setOPen中的索引值 
         if fieldpointers{setOpen(ii)}==fieldpointers{Parent_node}   %如果修正之前要走的点就是我们期望的构成直线的点，跳出修正
                new_ii=ii;
                amend_count_1=amend_count;
         elseif  find(setOpen == Expected_note)  %如果我们期望要走的点在待选点矩阵setOpen中
             
                 untext_ii=find(setOpen == Expected_note);
                 now_cost=setOpenCosts(untext_ii);   %不考虑weight，计算期望点要花费的代价
                 temp1 = setOpenCosts(ii);        %不考虑weight，计算将要拓展的点要花费的代价
                 
                if  temp1==now_cost       %如果我们期望的点要花费的代价等于修正之前要走的点花费的代价，就进行修正（因为之前要走的点，是待选点矩阵setOPen中代价最小的一个点之一，所以期望的点的代价不可能小于该点）
                     new_ii=untext_ii;   %将新的setOPen矩阵的索引值赋值给new_ii输出
                     amend_count=amend_count+1;
                     amend_count_1=amend_count; %amend_count_1中记录了我们进行修正的次数，为了查看这个函数是否有发挥作用
                else
                     new_ii=ii;          %如果我们期望的点要花费的代价大于修正之前要走的点花费的代价，就跳过修正（A星算法要保证进行拓展的点是待选点中代价最小的，这也是导致远离终止点的哪一类拐角无法得到修正的原因）
                     amend_count_1=amend_count;
                end
         else
                 new_ii=ii;    %如果我们期望的点不在待选点矩阵setOpen中（也就是这个点是障碍物或者超出边界了），则跳过修正
                 amend_count_1=amend_count;
         end
     end   
  end     
end

%% findWayBack函数,用来进行路径回溯，这个函数的输入参数是终止点goalposind和矩阵fieldpointers，输出参数是P
function [p,Road_Long,turn_count] = findWayBack(turn_count,goalposind,fieldpointers,Road_Long)

    n = length(fieldpointers);  % 获取环境的长度也就是n
    posind = goalposind;
    [py,px] = ind2sub([n,n],posind); % 将索引值posind转换为坐标值 [py,px]
    p = [py px];
    
    %利用while循环进行回溯，当我们回溯到起始点的时候停止，也就是在矩阵fieldpointers中找到S时停止
    while ~strcmp(fieldpointers{posind},'S')
      switch fieldpointers{posind}
          
        case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的
          px = px - 1;
          Road_Long=Road_Long+1;
        case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的
          px = px + 1;
          Road_Long=Road_Long+1;
        case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的
          py = py - 1;
          Road_Long=Road_Long+1;
        case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的
          py = py + 1;
          Road_Long=Road_Long+1;
        case 'LU' % ’LU’ 表示当前的点是由左上边的点拓展出来的
          px = px - 1;py = py - 1;
          Road_Long=Road_Long+sqrt(2);
        case 'RU' % ’RU’ 表示当前的点是由右上边的点拓展出来的
          px = px + 1;py = py - 1;
          Road_Long=Road_Long+sqrt(2);
        case 'LD' % ’LD’ 表示当前的点是由左下面的点拓展出来的
          px = px - 1;py = py + 1;
          Road_Long=Road_Long+sqrt(2);
        case 'RD' % ’RD’ 表示当前的点是由右下边的点拓展出来的
          px = px + 1; py = py + 1;
          Road_Long=Road_Long+sqrt(2);
      end      
      posind_1=posind;
      p = [p; py px];
      posind = sub2ind([n n],py,px);% 将坐标值转换为索引值
      if ~strcmp(fieldpointers{posind_1},fieldpointers{posind})
        turn_count=turn_count+1;
      end
    end
    turn_count=turn_count-1;
end

%% findFValue函数，主要循环中用到的路径拓展函数

%这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价找到。
%函数的输出量中costs表示拓展的子节点到起始点的代价，heuristics表示拓展出来的点到终止点的距离大约是多少，posinds表示拓展出来的子节点
function [cost,heuristic,posinds] = findFValue(quarter_turn,fieldpointers,eight_directions,Corner_obstacles,posind,costsofar,field,goalind,heuristicmethod)
    n = length(field);  % 获取矩阵的长度
    [currentpos(1) currentpos(2)] = ind2sub([n n],posind);   %将要进行拓展的点（也就是父节点posind）的索引值拓展成坐标值
    [goalpos(1) goalpos(2)] = ind2sub([n n],goalind);        %将终止点的索引值拓展成坐标值
    cost = Inf*ones(8,1); heuristic = Inf*ones(8,1); pos = ones(8,2); %将矩阵cost和heuristic初始化为8x1的无穷大值的矩阵，pos初始化为8x2的值为1的矩阵
    
    % 拓展方向一:?左
    newx = currentpos(2) - 1; newy = currentpos(1);
    if newx > 0
      pos(1,:) = [newy newx];
          if heuristicmethod
              heuristic(1) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(1) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end
      if strcmp(fieldpointers{posind},'U')||strcmp(fieldpointers{posind},'D')
          cost(1) = costsofar + field(newy,newx)+quarter_turn;
      else
          cost(1) = costsofar + field(newy,newx);
      end
    end

    % 拓展方向二：右
    newx = currentpos(2) + 1; newy = currentpos(1);
    if newx <= n
      pos(2,:) = [newy newx];
         if heuristicmethod      %判断使用欧氏距离模式还是对角距离模式来计算启发式函数
              heuristic(2) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(2) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
         end
         
       if strcmp(fieldpointers{posind},'U')||strcmp(fieldpointers{posind},'D')
          cost(2) = costsofar + field(newy,newx)+quarter_turn;
       else
          cost(2) = costsofar + field(newy,newx);
       end
    end

    % 拓展方向三：上
    newx = currentpos(2); newy = currentpos(1)-1;
    if newy > 0
      pos(3,:) = [newy newx];
          if heuristicmethod
              heuristic(3) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(3) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end 
          
       if strcmp(fieldpointers{posind},'L')||strcmp(fieldpointers{posind},'R')
          cost(3) = costsofar + field(newy,newx)+quarter_turn;
       else
          cost(3) = costsofar + field(newy,newx);
       end
    end

    % 拓展方向四：下
    newx = currentpos(2); newy = currentpos(1)+1;
    if newy <= n
      pos(4,:) = [newy newx];
          if heuristicmethod
              heuristic(4) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(4) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end
          
       if strcmp(fieldpointers{posind},'L')||strcmp(fieldpointers{posind},'R')
          cost(4) = costsofar + field(newy,newx)+quarter_turn;
       else
          cost(4) = costsofar + field(newy,newx);
       end
    end
    
    % 拓展方向五：左上
    if eight_directions           % 判断参数eight_directions是否为1，是否选择斜向拓展
         newx = currentpos(2)-1; newy = currentpos(1)-1;
    else
         newx = 0;
    end   
    if (newy > 0) && (newx > 0)
      pos(5,:) = [newy newx];
          if heuristicmethod
              heuristic(5) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(5) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end
      
      if Corner_obstacles            % 是否忽略障碍物四角的障碍性
          cost(5) = costsofar + sqrt(2)* field(newy,newx)+field(newy+1,newx)+field(newy,newx+1)-20;  
          if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')   %是否是直角转弯，判断需不需要加上直角转弯代价
              cost(5) = cost(5)+quarter_turn; 
          end
      else
          cost(5) = costsofar + sqrt(2)* field(newy,newx);
          if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')
             cost(5) =cost(5)+quarter_turn;
          end
      end
    end
    
    % 拓展方向六：右上
    if eight_directions
         newx = currentpos(2)+1; newy = currentpos(1)-1;
    else
         newy = 0;
    end 
    if (newy > 0) && (newx <= n)
      pos(6,:) = [newy newx];
          if heuristicmethod
              heuristic(6) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(6) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end
          
      if Corner_obstacles            % 是否忽略障碍物四角的障碍性
          cost(6) = costsofar + sqrt(2)* field(newy,newx)+field(newy+1,newx)+field(newy,newx-1)-20;  
          if strcmp(fieldpointers{posind},'LU')||strcmp(fieldpointers{posind},'RD')   %是否是直角转弯，判断需不需要加上直角转弯代价
              cost(6) = cost(6)+quarter_turn; 
          end
      else
          cost(6) = costsofar + sqrt(2)* field(newy,newx);
          if strcmp(fieldpointers{posind},'LU')||strcmp(fieldpointers{posind},'RD')
             cost(6) = cost(6)+quarter_turn;
          end
      end
    end
    
    % 拓展方向七：左下
    if eight_directions
         newx = currentpos(2)-1; newy = currentpos(1)+1;
    else
         newx = 0;
    end 
    if (newy <= n) && (newx > 0)
      pos(7,:) = [newy newx];
          if heuristicmethod
              heuristic(7) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(7) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end  
          
      if Corner_obstacles            % 是否忽略障碍物四角的障碍性
          cost(7) = costsofar + sqrt(2)* field(newy,newx)+field(newy-1,newx)+field(newy,newx+1)-20;  
          if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')   %是否是直角转弯，判断需不需要加上直角转弯代价
              cost(7) =cost(7)+quarter_turn; 
          end
      else
          cost(7) = costsofar + sqrt(2)* field(newy,newx);
          if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')
             cost(7) = cost(7)+quarter_turn;
          end
      end
    end
    
    % 拓展方向八：右下
    if eight_directions
         newx = currentpos(2)+1; newy = currentpos(1)+1;
    else
         newx = n +1 ;
    end 
    if (newy <= n) && (newx <= n)
      pos(8,:) = [newy newx];
          if heuristicmethod
              heuristic(8) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
          else
              heuristic(8) =10*(abs(goalpos(2)-newx)+abs(goalpos(1)-newy)+(sqrt(2)-2)*min(abs(goalpos(2)-newx),abs(goalpos(1)-newy))); %対角距离，此时有h(n)=h*(n)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
          end
          
      if Corner_obstacles            % 是否忽略障碍物四角的障碍性
          cost(8) = costsofar + sqrt(2)* field(newy,newx)+field(newy-1,newx)+field(newy,newx-1)-20;  
          if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')   %是否是直角转弯，判断需不需要加上直角转弯代价
              cost(8) =cost(8)+quarter_turn; 
          end
      else
          cost(8) = costsofar + sqrt(2)* field(newy,newx);
          if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')
             cost(8) = cost(8)+quarter_turn;
          end
      end
    end

     posinds = sub2ind([n n],pos(:,1),pos(:,2)); % 将拓展出来的子节点的坐标值转换为索引值
end

还有路径长度、拐弯次数、运行时间等参数及函数的使用，不再一一解释了，有不懂的地方需要多看一下代码，有时间有耐心等待的也可以私信我。 

指路三维：在matlab中使用A*算法进行三维路径规划-CSDN博客