devbox
Cpp五条
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

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

作者：Cpp五条 | 2024-06-06 23:21:23

赞

踩

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表示每一次直角拐弯的代价，使规划的路径尽可能减少直角拐弯的次数，也使的路径更加圆滑。

  1.  
  2. %%功能参数的设定部分
  3. n = 40;   % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
  4. wallpercent = 0.3;  % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
  5. Weights=2;       %动态衡量启发式A星算法中的h（n）权重系数
  6. quarter_turn=10; %直角转弯一次需要消耗的代价参数，
  7.  
  8. Corner_amend=0;  %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
  9. Environmental_Set=0; %这个参数用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
  10. heuristicmethod=1;   %这个参数用来选择是否使用欧几里得距离，若设为0，则启发式函数h（n）使用对角公式，若设为1，则使用欧几里得距离公式
  11. eight_directions=1; %这个参数用来选择是否可以斜向拓展，若设为0，则只能向四周4个方向拓展，若设为1，则可以向四周8个方向拓展
  12. Corner_obstacles=1; %这个参数用来选择是否关注障碍物的四角，若设为0，则忽略障碍物的四角，若设为1，则不能穿越障碍物的四角 
  13. Reset_GS=0;   %这个参数用来选择是否重新设定起始点和终止点，若设定为1，开始重新设定起始点和终止点，同时需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则关闭
  14. New_startposind=380;   New_goalposind=270;  %若将Reset_GS设定为1，则需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，要确保新设的这两个点处没有障碍物
  15.  
  16.  
  17. %用来存放规划的路径的长度
  18. Road_Long=0;
  19. %用来存放拐弯次数
  20. turn_count=0; 
  21.  
  22.  
  23. % 路径规划中用到的一些矩阵的初始化
  24. setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
  25. setClosed = []; setClosedCosts = [];
  26. movementdirections = {'R','L','D','U','RD','LD','RU','LU'};  %移动方向,增加右上、左上、右下、左下四个方向
  27.  
  28. %初始化一些进行路径的修正需要用到的变量
  29. Parent_node=0; %Parent_node初始化，否则会报错
  30. Expected_note=0;%Expected_note初始化，否则会报错
  31. untext_ii=0;  %未经过检验的新的ii
  32. amend_count=0; % 记录修正的次数
  33. temp1=0;   %初始化将扩展点至起始点的代价为0
  34.  
  35. % 这个函数用来随机生成环境，障碍物，起点，终点
  36. axishandle = createFigure(field,costchart,startposind,goalposind);    %将随机生成的方格及障碍物的数据生成图像
  37.  
  38.  
  39.  
  40.  
  41. %% 利用随机生成的环境数据来进行环境的绘制
  42.  
  43. function axishandle = createFigure(field,costchart,startposind,goalposind)
  44.  
  45.       % 这个if..else结构的作用是判断如果没有打开的figure图，则按照相关设置创建一个figure图
  46.       if isempty(gcbf)                                       %gcbf是当前返回图像的句柄，isempty(gcbf)假如gcbf为空的话，返回的值是1，假如gcbf为非空的话，返回的值是0
  47.       figure('Position',[560 70 700 700], 'MenuBar','none');  %对创建的figure图像进行设置，设置其距离屏幕左侧的距离为450，距离屏幕下方的距离为50，长度和宽度都为700，并且关闭图像的菜单栏
  48.       axes('position', [0.01 0.01 0.99 0.99]);               %设置坐标轴的位置，左下角的坐标设为0.01,0.01   右上角的坐标设为0.99 0.99  （可以认为figure图的左下角坐标为0 0   ，右上角坐标为1 1 ）
  49.       else
  50.       gcf; cla;   %gcf 返回当前 Figure 对象的句柄值，然后利用cla语句来清除它
  51.       end
  52.       
  53.       n = length(field);  %获取矩阵的长度，并赋值给变量n
  54.       field(field < Inf) = 0; %将fieid矩阵中的随机数（也就是没有障碍物的位置处）设为0
  55.       pcolor(1:n+1,1:n+1,[field field(:,end); field(end,:) field(end,end)]);%多加了一个重复的（由n X n变为 n+1 X n+1 ）
  56.  
  57.       cmap = flipud(colormap('jet'));  %生成的cmap是一个256X3的矩阵，每一行的3个值都为0-1之间数，分别代表颜色组成的rgb值
  58.       cmap(1,:) = zeros(3,1); cmap(end,:) = ones(3,1); %将矩阵cmap的第一行设为0 ，最后一行设为1
  59.       colormap(flipud(cmap)); %进行颜色的倒转 
  60.       hold on;
  61.       axishandle = pcolor([1:n+1],[1:n+1],[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);  %将矩阵costchart进行拓展，插值着色后赋给axishandle
  62.       [goalposy,goalposx] = ind2sub([n,n],goalposind);
  63.       [startposy,startposx] = ind2sub([n,n],startposind);
  64.        plot(goalposx+0.5,goalposy+0.5,'ys','MarkerSize',10,'LineWidth',6); % 画出起点和终点
  65.        plot(startposx+0.5,startposy+0.5,'go','MarkerSize',10,'LineWidth',6);
  66.        %uicontrol('Style','pushbutton','String','RE-DO', 'FontSize',12, 'Position', [1 1 60 40], 'Callback','astardemo');
  67. end
  68.  
  1. %% 这个while循环是本程序的核心，利用FindValue函数中的循环进行迭代来寻找终止点
  2.  
  3. tic
  4.  
  5. while ~max(ismember(setOpen,goalposind)) && ~isempty(setOpen)
  6.    
  7.     [temp, ii] = min(setOpenCosts +Weights*setOpenHeuristics);     %寻找拓展出来的最小值 
  8.    
  9.     if ((setOpen(ii)~=startposind) && (Corner_amend==1))
  10.     [new_ii,amend_count_1]=Path_optimization(temp1,temp,ii,fieldpointers,setOpen,setOpenCosts,startposind,Weights,setOpenHeuristics,Parent_node,Expected_note,untext_ii,amend_count); %进行路径的修正，在保证不增加距离的基础上，使其减少转弯的次数
  11.     ii=new_ii;
  12.     amend_count=amend_count_1;
  13.     end
  14.     
  15.     %这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价costs找到
  16.     [costs,heuristics,posinds] = findFValue(quarter_turn,fieldpointers,eight_directions,Corner_obstacles,setOpen(ii),setOpenCosts(ii),field,goalposind,heuristicmethod);
  17.     
  18.   setClosed = [setClosed; setOpen(ii)];     % 将找出来的拓展出来的点中代价最小的那个点串到矩阵setClosed 中 
  19.   setClosedCosts = [setClosedCosts; setOpenCosts(ii)];    % 将拓展出来的点中代价最小的那个点的代价串到矩阵setClosedCosts 中
  20.   
  21.   % 从setOpen中删除刚才放到矩阵setClosed中的那个点
  22.   %如果这个点位于setopen矩阵的内部
  23.   if (ii > 1 && ii < length(setOpen))
  24.     setOpen = [setOpen(1:ii-1); setOpen(ii+1:end)];
  25.     setOpenCosts = [setOpenCosts(1:ii-1); setOpenCosts(ii+1:end)];
  26.     setOpenHeuristics = [setOpenHeuristics(1:ii-1); setOpenHeuristics(ii+1:end)];
  27.     
  28.   %如果这个点位于setopen矩阵第一行
  29.   elseif (ii == 1)
  30.     setOpen = setOpen(2:end);
  31.     setOpenCosts = setOpenCosts(2:end);
  32.     setOpenHeuristics = setOpenHeuristics(2:end);
  33.     
  34.   %如果这个点位于setopen矩阵的最后一行
  35.   else
  36.     setOpen = setOpen(1:end-1);
  37.     setOpenCosts = setOpenCosts(1:end-1);
  38.     setOpenHeuristics = setOpenHeuristics(1:end-1);
  39.   end
  40.     
  41.   % 把拓展出来的点中符合要求的点放到setOpen 矩阵中，作为待选点
  42.   for jj=1:length(posinds)   % 在新拓展的4或8个点中，依次寻找代价最小的点，每次循环需要判断的点为posinds(jj)
  43.   
  44.     if ~isinf(costs(jj))   % 判断该点（方格）处没有障碍物       
  45.       % 判断一下该点是否 已经存在于setOpen 矩阵或者setClosed 矩阵中
  46.       % 如果我们要处理的拓展点既不在setOpen 矩阵，也不在setClosed 矩阵中
  47.       if ~max([setClosed; setOpen] == posinds(jj))
  48.         fieldpointers(posinds(jj)) = movementdirections(jj);
  49.         costchart(posinds(jj)) = costs(jj);
  50.         setOpen = [setOpen; posinds(jj)];
  51.         setOpenCosts = [setOpenCosts; costs(jj)];
  52.         setOpenHeuristics = [setOpenHeuristics; heuristics(jj)];
  53.         
  54.       % 如果我们要处理的拓展点已经在setOpen 矩阵中
  55.       elseif max(setOpen == posinds(jj))
  56.         I = find(setOpen == posinds(jj));
  57.         % 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
  58.         if setOpenCosts(I) > costs(jj)
  59.           costchart(setOpen(I)) = costs(jj);
  60.           setOpenCosts(I) = costs(jj);
  61.           setOpenHeuristics(I) = heuristics(jj);
  62.           fieldpointers(setOpen(I)) = movementdirections(jj);
  63.         end
  64.         
  65.         % 如果我们要处理的拓展点已经在setClosed 矩阵中
  66.       else
  67.         I = find(setClosed == posinds(jj));
  68.         % 如果通过目前的方法找到的这个点，比之前的方法好（代价小）就更新这个点
  69.         if setClosedCosts(I) > costs(jj)
  70.           costchart(setClosed(I)) = costs(jj);
  71.           setClosedCosts(I) = costs(jj);
  72.           fieldpointers(setClosed(I)) = movementdirections(jj);
  73.         end
  74.       end
  75.     end
  76.   end
  77.   
  78.   if isempty(setOpen) break; end
  79.   set(axishandle,'CData',[costchart costchart(:,end); costchart(end,:) costchart(end,end)]);
  80.   set(gca,'CLim',[0 1.1*max(costchart(find(costchart < Inf)))]);
  81.   drawnow; 
  82. end
  83.  
  84. toc
  85.  
  1. %% 拐角修正函数
  2.  
  3. 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)
  4.    n = length(fieldpointers);  %获取矩阵的长度，并赋值给变量n
  5.    
  6.  %获取其父节点的索引值
  7.   switch fieldpointers {setOpen(ii)}
  8.         case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的
  9.            Parent_node = setOpen(ii) - n;
  10.         case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的
  11.            Parent_node = setOpen(ii) + n;
  12.         case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的
  13.            Parent_node = setOpen(ii) -1;
  14.         case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的
  15.            Parent_node = setOpen(ii) + 1;  
  16.         case 'LU' % ’LU’ 表示当前的点是由左上边的点拓展出来的
  17.            Parent_node = setOpen(ii) - n - 1;
  18.         case 'RU' % ’RU’ 表示当前的点是由右上边的点拓展出来的
  19.            Parent_node = setOpen(ii) + n - 1;
  20.         case 'LD' % ’LD’ 表示当前的点是由左下面的点拓展出来的
  21.            Parent_node = setOpen(ii) - n + 1;
  22.         case 'RD' % ’RD’ 表示当前的点是由右下边的点拓展出来的
  23.            Parent_node = setOpen(ii) + n + 1;
  24.   end
  25.   
  26.   if Parent_node==startposind  %如果这个点的父节点是起始点的话，跳过修正
  27.      new_ii=ii;
  28.      amend_count_1=amend_count;
  29.   else 
  30.   
  31.        %获取期望下一步要走的点的索引值
  32.      switch fieldpointers{Parent_node}
  33.           
  34.         case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的,走直线的话，我们期望要走的下一个点为此点右边的点
  35.            Expected_note = Parent_node + n;
  36.         case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点左边的点
  37.            Expected_note = Parent_node - n;
  38.         case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的，走直线的话，我们期望要走的下一个点为此点下面的点
  39.            Expected_note = Parent_node +1;
  40.         case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点上面的点
  41.            Expected_note = Parent_node - 1;
  42.         case 'LU' % ’LU’ 表示当前的点是由左上边的点拓展出来的,走直线的话，我们期望要走的下一个点为此点右下边的点
  43.            Expected_note = Parent_node + n + 1;
  44.         case 'RU' % ’RU’ 表示当前的点是由右上边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点左下边的点
  45.            Expected_note = Parent_node - n + 1;
  46.         case 'LD' % ’LD’ 表示当前的点是由左下面的点拓展出来的，走直线的话，我们期望要走的下一个点为此点右上面的点
  47.            Expected_note = Parent_node + n - 1;
  48.         case 'RD' % ’RD’ 表示当前的点是由右下边的点拓展出来的，走直线的话，我们期望要走的下一个点为此点左上面的点
  49.            Expected_note = Parent_node - n - 1;
  50.      end
  51.    
  52.      if ((Expected_note<=0)||(Expected_note>n*n))   %如果我们期望的点不在待选点矩阵setOpen中，或者超出边界，跳过修正
  53.           new_ii=ii;
  54.           amend_count_1=amend_count;
  55.      else
  56.            
  57.            %计算新的要进行拓展的点在setOPen中的索引值 
  58.          if fieldpointers{setOpen(ii)}==fieldpointers{Parent_node}   %如果修正之前要走的点就是我们期望的构成直线的点，跳出修正
  59.                 new_ii=ii;
  60.                 amend_count_1=amend_count;
  61.          elseif  find(setOpen == Expected_note)  %如果我们期望要走的点在待选点矩阵setOpen中
  62.              
  63.                  untext_ii=find(setOpen == Expected_note);
  64.                  now_cost=setOpenCosts(untext_ii);   %不考虑weight，计算期望点要花费的代价
  65.                  temp1 = setOpenCosts(ii);        %不考虑weight，计算将要拓展的点要花费的代价
  66.                  
  67.                 if  temp1==now_cost       %如果我们期望的点要花费的代价等于修正之前要走的点花费的代价，就进行修正（因为之前要走的点，是待选点矩阵setOPen中代价最小的一个点之一，所以期望的点的代价不可能小于该点）
  68.                      new_ii=untext_ii;   %将新的setOPen矩阵的索引值赋值给new_ii输出
  69.                      amend_count=amend_count+1;
  70.                      amend_count_1=amend_count; %amend_count_1中记录了我们进行修正的次数，为了查看这个函数是否有发挥作用
  71.                 else
  72.                      new_ii=ii;          %如果我们期望的点要花费的代价大于修正之前要走的点花费的代价，就跳过修正（A星算法要保证进行拓展的点是待选点中代价最小的，这也是导致远离终止点的哪一类拐角无法得到修正的原因）
  73.                      amend_count_1=amend_count;
  74.                 end
  75.          else
  76.                  new_ii=ii;    %如果我们期望的点不在待选点矩阵setOpen中（也就是这个点是障碍物或者超出边界了），则跳过修正
  77.                  amend_count_1=amend_count;
  78.          end
  79.      end   
  80.   end     
  81. end

  1. %% findWayBack函数,用来进行路径回溯，这个函数的输入参数是终止点goalposind和矩阵fieldpointers，输出参数是P
  2. function [p,Road_Long,turn_count] = findWayBack(turn_count,goalposind,fieldpointers,Road_Long)
  3.  
  4.     n = length(fieldpointers);  % 获取环境的长度也就是n
  5.     posind = goalposind;
  6.     [py,px] = ind2sub([n,n],posind); % 将索引值posind转换为坐标值 [py,px]
  7.     p = [py px];
  8.     
  9.     %利用while循环进行回溯，当我们回溯到起始点的时候停止，也就是在矩阵fieldpointers中找到S时停止
  10.     while ~strcmp(fieldpointers{posind},'S')
  11.       switch fieldpointers{posind}
  12.           
  13.         case 'L' % ’L’ 表示当前的点是由左边的点拓展出来的
  14.           px = px - 1;
  15.           Road_Long=Road_Long+1;
  16.         case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的
  17.           px = px + 1;
  18.           Road_Long=Road_Long+1;
  19.         case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的
  20.           py = py - 1;
  21.           Road_Long=Road_Long+1;
  22.         case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的
  23.           py = py + 1;
  24.           Road_Long=Road_Long+1;
  25.         case 'LU' % ’LU’ 表示当前的点是由左上边的点拓展出来的
  26.           px = px - 1;py = py - 1;
  27.           Road_Long=Road_Long+sqrt(2);
  28.         case 'RU' % ’RU’ 表示当前的点是由右上边的点拓展出来的
  29.           px = px + 1;py = py - 1;
  30.           Road_Long=Road_Long+sqrt(2);
  31.         case 'LD' % ’LD’ 表示当前的点是由左下面的点拓展出来的
  32.           px = px - 1;py = py + 1;
  33.           Road_Long=Road_Long+sqrt(2);
  34.         case 'RD' % ’RD’ 表示当前的点是由右下边的点拓展出来的
  35.           px = px + 1; py = py + 1;
  36.           Road_Long=Road_Long+sqrt(2);
  37.       end      
  38.       posind_1=posind;
  39.       p = [p; py px];
  40.       posind = sub2ind([n n],py,px);% 将坐标值转换为索引值
  41.       if ~strcmp(fieldpointers{posind_1},fieldpointers{posind})
  42.         turn_count=turn_count+1;
  43.       end
  44.     end
  45.     turn_count=turn_count-1;
  46. end
  47.  
  48. %% findFValue函数，主要循环中用到的路径拓展函数
  49.  
  50. %这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价找到。
  51. %函数的输出量中costs表示拓展的子节点到起始点的代价，heuristics表示拓展出来的点到终止点的距离大约是多少，posinds表示拓展出来的子节点
  52. function [cost,heuristic,posinds] = findFValue(quarter_turn,fieldpointers,eight_directions,Corner_obstacles,posind,costsofar,field,goalind,heuristicmethod)
  53.     n = length(field);  % 获取矩阵的长度
  54.     [currentpos(1) currentpos(2)] = ind2sub([n n],posind);   %将要进行拓展的点（也就是父节点posind）的索引值拓展成坐标值
  55.     [goalpos(1) goalpos(2)] = ind2sub([n n],goalind);        %将终止点的索引值拓展成坐标值
  56.     cost = Inf*ones(8,1); heuristic = Inf*ones(8,1); pos = ones(8,2); %将矩阵cost和heuristic初始化为8x1的无穷大值的矩阵，pos初始化为8x2的值为1的矩阵
  57.     
  58.     % 拓展方向一:?左
  59.     newx = currentpos(2) - 1; newy = currentpos(1);
  60.     if newx > 0
  61.       pos(1,:) = [newy newx];
  62.           if heuristicmethod
  63.               heuristic(1) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  64.           else
  65.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  66.           end
  67.       if strcmp(fieldpointers{posind},'U')||strcmp(fieldpointers{posind},'D')
  68.           cost(1) = costsofar + field(newy,newx)+quarter_turn;
  69.       else
  70.           cost(1) = costsofar + field(newy,newx);
  71.       end
  72.     end
  73.  
  74.     % 拓展方向二：右
  75.     newx = currentpos(2) + 1; newy = currentpos(1);
  76.     if newx <= n
  77.       pos(2,:) = [newy newx];
  78.          if heuristicmethod      %判断使用欧氏距离模式还是对角距离模式来计算启发式函数
  79.               heuristic(2) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  80.           else
  81.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  82.          end
  83.          
  84.        if strcmp(fieldpointers{posind},'U')||strcmp(fieldpointers{posind},'D')
  85.           cost(2) = costsofar + field(newy,newx)+quarter_turn;
  86.        else
  87.           cost(2) = costsofar + field(newy,newx);
  88.        end
  89.     end
  90.  
  91.     % 拓展方向三：上
  92.     newx = currentpos(2); newy = currentpos(1)-1;
  93.     if newy > 0
  94.       pos(3,:) = [newy newx];
  95.           if heuristicmethod
  96.               heuristic(3) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  97.           else
  98.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  99.           end 
  100.           
  101.        if strcmp(fieldpointers{posind},'L')||strcmp(fieldpointers{posind},'R')
  102.           cost(3) = costsofar + field(newy,newx)+quarter_turn;
  103.        else
  104.           cost(3) = costsofar + field(newy,newx);
  105.        end
  106.     end
  107.  
  108.     % 拓展方向四：下
  109.     newx = currentpos(2); newy = currentpos(1)+1;
  110.     if newy <= n
  111.       pos(4,:) = [newy newx];
  112.           if heuristicmethod
  113.               heuristic(4) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  114.           else
  115.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  116.           end
  117.           
  118.        if strcmp(fieldpointers{posind},'L')||strcmp(fieldpointers{posind},'R')
  119.           cost(4) = costsofar + field(newy,newx)+quarter_turn;
  120.        else
  121.           cost(4) = costsofar + field(newy,newx);
  122.        end
  123.     end
  124.     
  125.     % 拓展方向五：左上
  126.     if eight_directions           % 判断参数eight_directions是否为1，是否选择斜向拓展
  127.          newx = currentpos(2)-1; newy = currentpos(1)-1;
  128.     else
  129.          newx = 0;
  130.     end   
  131.     if (newy > 0) && (newx > 0)
  132.       pos(5,:) = [newy newx];
  133.           if heuristicmethod
  134.               heuristic(5) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  135.           else
  136.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  137.           end
  138.       
  139.       if Corner_obstacles            % 是否忽略障碍物四角的障碍性
  140.           cost(5) = costsofar + sqrt(2)* field(newy,newx)+field(newy+1,newx)+field(newy,newx+1)-20;  
  141.           if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')   %是否是直角转弯，判断需不需要加上直角转弯代价
  142.               cost(5) = cost(5)+quarter_turn; 
  143.           end
  144.       else
  145.           cost(5) = costsofar + sqrt(2)* field(newy,newx);
  146.           if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')
  147.              cost(5) =cost(5)+quarter_turn;
  148.           end
  149.       end
  150.     end
  151.     
  152.     % 拓展方向六：右上
  153.     if eight_directions
  154.          newx = currentpos(2)+1; newy = currentpos(1)-1;
  155.     else
  156.          newy = 0;
  157.     end 
  158.     if (newy > 0) && (newx <= n)
  159.       pos(6,:) = [newy newx];
  160.           if heuristicmethod
  161.               heuristic(6) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  162.           else
  163.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  164.           end
  165.           
  166.       if Corner_obstacles            % 是否忽略障碍物四角的障碍性
  167.           cost(6) = costsofar + sqrt(2)* field(newy,newx)+field(newy+1,newx)+field(newy,newx-1)-20;  
  168.           if strcmp(fieldpointers{posind},'LU')||strcmp(fieldpointers{posind},'RD')   %是否是直角转弯，判断需不需要加上直角转弯代价
  169.               cost(6) = cost(6)+quarter_turn; 
  170.           end
  171.       else
  172.           cost(6) = costsofar + sqrt(2)* field(newy,newx);
  173.           if strcmp(fieldpointers{posind},'LU')||strcmp(fieldpointers{posind},'RD')
  174.              cost(6) = cost(6)+quarter_turn;
  175.           end
  176.       end
  177.     end
  178.     
  179.     % 拓展方向七：左下
  180.     if eight_directions
  181.          newx = currentpos(2)-1; newy = currentpos(1)+1;
  182.     else
  183.          newx = 0;
  184.     end 
  185.     if (newy <= n) && (newx > 0)
  186.       pos(7,:) = [newy newx];
  187.           if heuristicmethod
  188.               heuristic(7) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  189.           else
  190.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  191.           end  
  192.           
  193.       if Corner_obstacles            % 是否忽略障碍物四角的障碍性
  194.           cost(7) = costsofar + sqrt(2)* field(newy,newx)+field(newy-1,newx)+field(newy,newx+1)-20;  
  195.           if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')   %是否是直角转弯，判断需不需要加上直角转弯代价
  196.               cost(7) =cost(7)+quarter_turn; 
  197.           end
  198.       else
  199.           cost(7) = costsofar + sqrt(2)* field(newy,newx);
  200.           if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')
  201.              cost(7) = cost(7)+quarter_turn;
  202.           end
  203.       end
  204.     end
  205.     
  206.     % 拓展方向八：右下
  207.     if eight_directions
  208.          newx = currentpos(2)+1; newy = currentpos(1)+1;
  209.     else
  210.          newx = n +1 ;
  211.     end 
  212.     if (newy <= n) && (newx <= n)
  213.       pos(8,:) = [newy newx];
  214.           if heuristicmethod
  215.               heuristic(8) = 10*sqrt((goalpos(2)-newx)^2 +(goalpos(1)-newy)^2);     % 欧几里得距离计算公式
  216.           else
  217.               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)，在保证最优路径的基础上最大程度上减少无关点的拓展计算
  218.           end
  219.           
  220.       if Corner_obstacles            % 是否忽略障碍物四角的障碍性
  221.           cost(8) = costsofar + sqrt(2)* field(newy,newx)+field(newy-1,newx)+field(newy,newx-1)-20;  
  222.           if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')   %是否是直角转弯，判断需不需要加上直角转弯代价
  223.               cost(8) =cost(8)+quarter_turn; 
  224.           end
  225.       else
  226.           cost(8) = costsofar + sqrt(2)* field(newy,newx);
  227.           if strcmp(fieldpointers{posind},'RU')||strcmp(fieldpointers{posind},'LD')
  228.              cost(8) = cost(8)+quarter_turn;
  229.           end
  230.       end
  231.     end
  232.  
  233.      posinds = sub2ind([n n],pos(:,1),pos(:,2)); % 将拓展出来的子节点的坐标值转换为索引值
  234. end
  235.

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Cpp五条/article/detail/683020
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号