本系列文章主要介绍基于A*算法的路径规划的实现，并使用MATLAB进行仿真演示。本文作为本系列的第四篇文章主要对前三篇文章的内容进行一些细节的补充
本系列文章链接：
-----------------------------------------------------------------------------
详细介绍用MATLAB实现基于 A * 算法的路径规划（附完整的代码，代码逐行进行解释）（一）--------A * 算法简介和环境的创建
详细介绍用MATLAB实现基于 A * 算法的路径规划（附完整的代码，代码逐行进行解释）（二）--------利用 A * 算法进行路径规划
详细介绍用MATLAB实现基于 A * 算法的路径规划（附完整的代码，代码逐行进行解释）（三）--------总结及 A * 算法的优化处理
详细介绍用MATLAB实现基于 A * 算法的路径规划（附完整的代码，代码逐行进行解释） （四）--------固定障碍物，进一步对比
-----------------------------------------------------------------------------
十二、固定环境的方法以及对比效果展示
1、起始点，终止点，障碍物信息均不变的情况
为了共明显的展示优化的效果，我们让优化前后的算法都对相同的环境进行优化，在前文的介绍中，我们每次运行程序都是把之前的信息清除掉，重新随机的生成环境信息，现在我们需要把我们想要的环境信息保存下来，提供给下一次运行使用，实现的方法如下：
我们新设一个功能参数取名为Environmental_Set，我们设定当这个参数设定为1时，我们运行时将会利用initializeField函数随机生成环境信息，并保存在名为Environmental的文件中（注意保存语句的位置必须位于生成环境图像的函数前（createFigure函数），否则保存的就是被修改过的环境信息），这样在下一次，我们想用这个环境进行路径规划时，只需要将参数Environmental_Set修改为0，然后载入保存的环境信息就可以了
修改的代码区域如下（功能参数设定部分，方格及障碍物创建）：
该区域原来的代码 ：
%功能参数的设定部分
n = 100; % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.4; % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=2; %动态衡量启发式A星算法中的h（n）权重系数
Corner_amend=1; %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
%方格以及障碍物的创建
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,wallpercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵
修改后的代码 ：
%功能参数的设定部分
n = 100; % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.4; % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=2; %动态衡量启发式A星算法中的h（n）权重系数
Corner_amend=1; %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
Environmental_Set=0; %这个参数用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
%方格以及障碍物的创建
if(Environmental_Set)
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,wallpercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵
save('Environmental','field','startposind','goalposind','costchart','fieldpointers' )
else
load('Environmental')
end
有了前文的解释，上面的改动应该很容易理解，上面代码的最后一行被注释掉了，他是用来在保留原来的障碍物信息的
2、障碍物信息不变，自主设定新的起始点和终止点
要实现这个功能看起来只需要简单的将起始点和终止点的坐标修改一下就行了，但是不要忘了，起始点和终止点的坐标将直接影响field矩阵, costchart矩阵 ，fieldpointers元胞数组中的信息，所有需要将这几个矩阵（元胞数组）中的内容也进行相关的修改因为代码行数较多，因此我把这部分内容的实现也封装成了一个函数取名为Reset_G_S
同样我们在功能参数的设定部分区域新增一个选择参数Reset_GS，当我们将其设定为1时，重新设定起始点和终止点，同时需要将新增的变量New_goal posind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则不设定新的起始点和终止点，同时在代码中我们新增了重新设定起始点和终止点的区域
这部分区域修改后的代码如下：
%功能参数的设定部分
n = 100; % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.4; % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=1; %动态衡量启发式A星算法中的h（n）权重系数
Corner_amend=1; %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
Environmental_Set=0; %这个参数用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
Reset_GS=1; %这个参数用来选择是否重新设定起始点和终止点，若设定为1，开始重新设定起始点和终止点，同时需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则关闭
New_startposind=4100; New_goalposind=8200; %若将Reset_GS设定为1，则需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，要确保新设的这两个点处没有障碍物
%方格以及障碍物的创建
if(Environmental_Set)
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,wallpercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵
save('Environmental','field','startposind','goalposind','costchart','fieldpointers' )
else
load('Environmental')
end
%重新设定起始点和终止点
if(Reset_GS)
[field, startposind, goalposind, costchart, fieldpointers] = Reset_G_S(field, startposind, goalposind, costchart, fieldpointers,New_startposind,New_goalposind);
end
Reset_G_S函数的代码如下，这个函数的输入参数是initializeField函数生成的环境信息加上新的起始点和终止点的索引值，输出参数是修改了新的起始点和终止点后的环境信息，这个函数是如何实现我们想要的功能的，在注释中已经详细介绍了，在这里就不啰嗦了
function [field, startposind, goalposind, costchart, fieldpointers] = Reset_G_S(field, startposind, goalposind, costchart, fieldpointers,New_startposind,New_goalposind)
%在initializeField函数创建环境时，将field矩阵中没有障碍物的点处设定为10，有障碍物的点处设定为inf，起始点和终止点设为0
%现在需要将新的起始点和终止点处设为0，原来起始点和终止点处设为10
field(startposind) = 10; field(goalposind) = 10;
field(New_startposind) = 0; field(New_goalposind) = 0;
%在initializeField函数创建环境时，将costchart矩阵中的起始点设定为0，其他点设定为NAN
%现在需要将新的起始点设定为0，原来的起始点设定为1
costchart(startposind) = NaN;
costchart(New_startposind) =0;
% 在initializeField函数创建环境时，将fieldpointers元胞数组中起始点的位置处设为'S'，终止点处设为'G'，有障碍物的点设为'0'，没有障碍物的点设为'1'
% 现在需要将新的起始点处设为'S'，新的终止点处设为'G'，原来的起始点和终止点处设为'1'
fieldpointers{startposind} = '1'; fieldpointers{goalposind} = '1';
fieldpointers{New_startposind} = 'S'; fieldpointers{New_goalposind} = 'G';
%最后我们将新的起始点和新的终止点的索引值分别赋值给startposind和goalposind进行输出
startposind=New_startposind;
goalposind=New_goalposind;
end
在这里总结一下功能参数的设定部分所能实现的效果，我们只需要按照注释提示，简单修改该区域的参数就能得到不同效果的A星算法
①参数n可以设定生成环境的方格数
②参数wallpercent 可以设定生成的障碍物占总方格数的比例
③参数Weights可以设定动态衡量启发式A星算法中的h（n）权重系数 ，当他的值为1时，也就是经过初步优化的传统的A星算法
④参数Corner_amend用来选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
⑤参数Environmental_Set用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
⑥参数Reset_GS用来选择是否重新设定起始点和终止点，若设定为1，开始重新设定起始点和终止点，同时需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则关闭
⑦参数New_startposind和New_goalposindReset_GS设定为1，则需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，要确保新设的这两个点处没有障碍物
当我们想要实现起始点，终止点，障碍物信息均不变的情况只需要将Environmental_Set设定为0，Reset_GS设定为0就可以了（注意：附件中Environmental_Set的值我设定为0了，这种情况下环境信息是从名为Environmental的文件中导入的，但是这个文件是当Environmental_Set设定为1时，save函数生成的，也就是在大家拿到附件时需要先将Environm ental_Set的值设为1运行一次，或者运行多次直至随机生成了你想要的的环境信息为止，此时再将Environmental_Set的值设为0）
当我们想要实现障碍物信息不变，自主设定新的起始点和终止点时，只需要将Environmental_Set的值设定为0，Reset_GS的值设定为1，同时将你想设定的新的起始点和终止点的索引值分别赋值给New_goalposind和New _startposind（索引值的计算方法，在本系列之前的文章介绍过了，在这里就不介绍了
同理，在需要随机生成障碍物位置，并且要自主设定起始点和终止点时，将Environmental_Set和Reset_GS都设定为1，并对New_goalposind和New _startposind赋值就可以了
3、在相同的环境下的对比
（1）对比1：优化前后搜索效率方面的对比，依次为未进行任何优化的A星算法、优化后的传统的A星算法、动态衡量式A星算法
A星算法固定障碍物优化前后的效果展示1.mp4
（2）对比2：拐角优化函数效果对比，依次为进行拐角优化和不进行拐角优化的效果，进行效果演示的界面，视频左侧变量区可以观察到该函数进行拐角纠正的次数，也就是参数amend_count的值，为了更好的观察我把环境的大小设定为n=30,这也导致拐角优化函数效果不太明显，但是n较大时不易于观察…
A星算法固定障碍物优化前后的效果展示2.mp4
注意：在添加了固定障碍物对比的功能后，若不重新生成环境时，也就是Environmental_Set设为0时，拐角优化函数无法发挥功能，如果想要对比效果，需要让拐角优化的那个程序的Environmental_Set设为1，去随机生成环境，而其他的对比程序的Environmental_Set设为0.去使用拐角优化的那个程序所生成的环境，我这段时间比价忙，这个bug就先不解决了，以后再说吧
4、到目前为止完整的代码(也就是附件中的A_ROAD_book05文件中的内容)
%matlab初始化
clc; %清除命令窗口的内容
clear all; %清除工作空间的所有变量，函数，和MEX文件
close all; %关闭所有的figure窗口
[y,Fs] = audioread('001.wav'); sound(y,Fs); % 播放名为001的音乐，注意该文件需要跟matlab文件位于同一文件夹下
%功能参数的设定部分
n = 100; % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.4; % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=1; %动态衡量启发式A星算法中的h（n）权重系数
Corner_amend=1; %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
Environmental_Set=0; %这个参数用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
Reset_GS=1; %这个参数用来选择是否重新设定起始点和终止点，若设定为1，开始重新设定起始点和终止点，同时需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则关闭
New_startposind=4100; New_goalposind=8200; %若将Reset_GS设定为1，则需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，要确保新设的这两个点处没有障碍物
%方格以及障碍物的创建
if(Environmental_Set)
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,wallpercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵
save('Environmental','field','startposind','goalposind','costchart','fieldpointers' )
else
load('Environmental')
end
%重新设定起始点和终止点
if(Reset_GS)
[field, startposind, goalposind, costchart, fieldpointers] = Reset_G_S(field, startposind, goalposind, costchart, fieldpointers,New_startposind,New_goalposind);
end
% 路径规划中用到的一些矩阵的初始化
setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
movementdirections = {'R','L','D','U'}; %移动方向
%初始化一些进行路径的修正需要用到的变量
Parent_node=0; %Parent_node初始化，否则会报错
Expected_note=0;%Expected_note初始化，否则会报错
untext_ii=0; %未经过检验的新的ii
amend_count=0;% 记录修正的次数
% 这个函数用来随机生成环境，障碍物，起点，终点
axishandle = createFigure(field,costchart,startposind,goalposind); %将随机生成的方格及障碍物的数据生成图像
%%
% 这个while循环是本程序的核心，利用循环进行迭代来寻找终止点
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(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,heuristics,posinds] = findFValue(setOpen(ii),setOpenCosts(ii), field,goalposind,'euclidean');
setClosed = [setClosed; setOpen(ii)]; % 将找出来的拓展出来的点中代价最小的那个点串到矩阵setClosed 中
setClosedCosts = [setClosedCosts; setOpenCosts(ii)]; % 将拓展出来的点中代价最小的那个点的代价串到矩阵setClosedCosts 中
% 从setOpen中删除刚才放到矩阵setClosed中的那个点
%如果这个点位于矩阵的内部
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)];
%如果这个点位于矩阵第一行
elseif (ii == 1)
setOpen = setOpen(2:end);
setOpenCosts = setOpenCosts(2:end);
setOpenHeuristics = setOpenHeuristics(2:end);
%如果这个点位于矩阵的最后一行
else
setOpen = setOpen(1:end-1);
setOpenCosts = setOpenCosts(1:end-1);
setOpenHeuristics = setOpenHeuristics(1:end-1);
end
%%
% 把拓展出来的点中符合要求的点放到setOpen 矩阵中，作为待选点
for jj=1:length(posinds)
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
%%
%调用findWayBack函数进行路径回溯，并绘制出路径曲线
if max(ismember(setOpen,goalposind))
disp('Solution found!');
p = findWayBack(goalposind,fieldpointers); % 调用findWayBack函数进行路径回溯，将回溯结果放于矩阵P中
plot(p(:,2)+0.5,p(:,1)+0.5,'Color',0.2*ones(3,1),'LineWidth',4); %用 plot函数绘制路径曲线
drawnow;
drawnow;
clear sound
[y,Fs] = audioread('002.wav'); sound(y,Fs); % 播放名为000的音乐，注意该文件需要跟matlab文件位于同一文件夹下
elseif isempty(setOpen)
disp('No Solution!');
clear sound
[y,Fs] = audioread('000.wav');
sound(y,Fs);
end
%%
%findWayBack函数用来进行路径回溯，这个函数的输入参数是终止点goalposind和矩阵fieldpointers，输出参数是P
function p = findWayBack(goalposind,fieldpointers)
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;
case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的
px = px + 1;
case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的
py = py - 1;
case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的
py = py + 1;
end
p = [p; py px];
posind = sub2ind([n n],py,px);% 将坐标值转换为索引值
end
end
%%
%这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价找到。
%函数的输出量中costs表示拓展的子节点到起始点的代价，heuristics表示拓展出来的点到终止点的距离大约是多少，posinds表示拓展出来的子节点
function [cost,heuristic,posinds] = findFValue(posind,costsofar,field,goalind,heuristicmethod)
n = length(field); % 获取矩阵的长度
[currentpos(1) currentpos(2)] = ind2sub([n n],posind); %将要进行拓展的点（也就是父节点）的索引值拓展成坐标值
[goalpos(1) goalpos(2)] = ind2sub([n n],goalind); %将终止点的索引值拓展成坐标值
cost = Inf*ones(4,1); heuristic = Inf*ones(4,1); pos = ones(4,2); %将矩阵cost和heuristic初始化为4x1的无穷大值的矩阵，pos初始化为4x2的值为1的矩阵
% 拓展方向一
newx = currentpos(2) - 1; newy = currentpos(1);
if newx > 0
pos(1,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(1) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(1) = 10*abs(goalpos(2)-newx) +10*abs(goalpos(1)-newy);
end
cost(1) = costsofar + field(newy,newx);
end
% 拓展方向二
newx = currentpos(2) + 1; newy = currentpos(1);
if newx <= n
pos(2,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(2) = 10*abs(goalpos(2)-newx) +10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(2) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
end
cost(2) = costsofar + field(newy,newx);
end
% 拓展方向三
newx = currentpos(2); newy = currentpos(1)-1;
if newy > 0
pos(3,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(3) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(3) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
end
cost(3) = costsofar + field(newy,newx);
end
% 拓展方向四
newx = currentpos(2); newy = currentpos(1)+1;
if newy <= n
pos(4,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(4) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(4) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
end
cost(4) = costsofar + field(newy,newx);
end
posinds = sub2ind([n n],pos(:,1),pos(:,2)); % 将拓展出来的子节点的坐标值转换为索引值
end
%%
%这个矩阵的作用就是随机生成环境，障碍物，起始点，终止点等
function [field, startposind, goalposind, costchart, fieldpointers] = initializeField(n,wallpercent)
field = 10*ones(n,n);%设置任意两方格间的距离为10
field(ind2sub([n n],ceil(n^2.*rand(n*n*wallpercent,1)))) = Inf;%向上取整
% 随机生成起始点和终止点
startposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %随机生成起始点的索引值
goalposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %随机生成终止点的索引值
field(startposind) = 0; field(goalposind) = 0; %把矩阵中起始点和终止点处的值设为0
costchart = NaN*ones(n,n);%生成一个nxn的矩阵costchart，每个元素都设为NaN。就是矩阵初始NaN无效数据
costchart(startposind) = 0;%在矩阵costchart中将起始点位置处的值设为0
% 生成元胞数组
fieldpointers = cell(n,n);%生成元胞数组n*n
fieldpointers(:)= {'1'};
fieldpointers{startposind} = 'S'; fieldpointers{goalposind} = 'G'; %将元胞数组的起始点的位置处设为 'S'，终止点处设为'G'
fieldpointers(field==inf)={'0'};
end
% end of this function
%%
%利用随机生成的环境数据来进行环境的绘制
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
%%
function [new_ii,amend_count_1] = Path_optimization(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;
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;
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) +Weights*setOpenHeuristics(untext_ii); %计算期望点要花费的代价
if temp==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
function [field, startposind, goalposind, costchart, fieldpointers] = Reset_G_S(field, startposind, goalposind, costchart, fieldpointers,New_startposind,New_goalposind)
%在initializeField函数创建环境时，将field矩阵中没有障碍物的点处设定为10，有障碍物的点处设定为inf，起始点和终止点设为0
%现在需要将新的起始点和终止点处设为0，原来起始点和终止点处设为10
field(startposind) = 10; field(goalposind) = 10;
field(New_startposind) = 0; field(New_goalposind) = 0;
%在initializeField函数创建环境时，将costchart矩阵中的起始点设定为0，其他点设定为NAN
%现在需要将新的起始点设定为0，原来的起始点设定为1
costchart(startposind) = NaN;
costchart(New_startposind) =0;
% 在initializeField函数创建环境时，将fieldpointers元胞数组中起始点的位置处设为'S'，终止点处设为'G'，有障碍物的点设为'0'，没有障碍物的点设为'1'
% 现在需要将新的起始点处设为'S'，新的终止点处设为'G'，原来的起始点和终止点处设为'1'
fieldpointers{startposind} = '1'; fieldpointers{goalposind} = '1';
fieldpointers{New_startposind} = 'S'; fieldpointers{New_goalposind} = 'G';
%最后我们将新的起始点和新的终止点的索引值分别赋值给startposind和goalposind进行输出
startposind=New_startposind;
goalposind=New_goalposind;
end
十三、如何查看当前规划路径的长度
有的小伙伴，想要查看当前规划路径的长度，如何实现呢？通过以下几步简单的修改就行了
1、在上面的代码的基础上，在功能参数的设定部分的后面添加一个变量Road_Long并将其初始化为0，用来存放规划的路径的长度，程序运行后在左侧的变量区查看该变量值就可以知道当前规划路径的长度了（以方格的边长为一个单位）
%用来存放规划的路径的长度
Road_Long=0;
2、将findWayBack函数增加一个输入参数和输出参数Road_Long，并在循环中添加变量findWayBack累加1的语句 Road_Long=Road_Long+1;
3、将调用findWayBack函数进行回溯处时的语句更新为以下语句
[p,Road_Long]= findWayBack(goalposind,fieldpointers,Road_Long); % 调用findWayBack函数进行路径回溯，将回溯结果放于矩阵P中
4、运行后查看左边变量Road_Long的值就知道当前路径的长度了
5、按以上修改后的完整代码如下：
%matlab初始化
clc; %清除命令窗口的内容
clear all; %清除工作空间的所有变量，函数，和MEX文件
close all; %关闭所有的figure窗口
[y,Fs] = audioread('001.wav'); sound(y,Fs); % 播放名为001的音乐，注意该文件需要跟matlab文件位于同一文件夹下
%功能参数的设定部分
n = 10; % 产生一个n x n的方格，修改此值可以修改生成图片的方格数
wallpercent = 0.4; % 这个变量代表生成的障碍物占总方格数的比例 ，如0.5 表示障碍物占总格数的50%
Weights=1; %动态衡量启发式A星算法中的h（n）权重系数
Corner_amend=1; %选择是否进行拐角的修正，该变量设为0则不进行拐角修正，设为1则进行拐角修正
Environmental_Set=1; %这个参数用来选择是否随机生成障碍物，若设定为0，则使用上一次创建的环境信息，若设定为1，则重新随机生成障碍物
Reset_GS=0; %这个参数用来选择是否重新设定起始点和终止点，若设定为1，开始重新设定起始点和终止点，同时需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，设为0则关闭
New_startposind=4100; New_goalposind=8200; %若将Reset_GS设定为1，则需要将变量New_goalposind和New_startposind的值修改为你所选择的起始点和终止点的索引值，要确保新设的这两个点处没有障碍物
%用来存放规划的路径的长度
Road_Long=0;
%方格以及障碍物的创建
if(Environmental_Set)
[field, startposind, goalposind, costchart, fieldpointers] =initializeField(n,wallpercent); %随机生成包含障碍物，起始点，终止点等信息的矩阵
save('Environmental','field','startposind','goalposind','costchart','fieldpointers' )
else
load('Environmental')
end
%重新设定起始点和终止点
if(Reset_GS)
[field, startposind, goalposind, costchart, fieldpointers] = Reset_G_S(field, startposind, goalposind, costchart, fieldpointers,New_startposind,New_goalposind);
end
% 路径规划中用到的一些矩阵的初始化
setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
movementdirections = {'R','L','D','U'}; %移动方向
%初始化一些进行路径的修正需要用到的变量
Parent_node=0; %Parent_node初始化，否则会报错
Expected_note=0;%Expected_note初始化，否则会报错
untext_ii=0; %未经过检验的新的ii
amend_count=0;% 记录修正的次数
% 这个函数用来随机生成环境，障碍物，起点，终点
axishandle = createFigure(field,costchart,startposind,goalposind); %将随机生成的方格及障碍物的数据生成图像
%%
% 这个while循环是本程序的核心，利用循环进行迭代来寻找终止点
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(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,heuristics,posinds] = findFValue(setOpen(ii),setOpenCosts(ii), field,goalposind,'euclidean');
setClosed = [setClosed; setOpen(ii)]; % 将找出来的拓展出来的点中代价最小的那个点串到矩阵setClosed 中
setClosedCosts = [setClosedCosts; setOpenCosts(ii)]; % 将拓展出来的点中代价最小的那个点的代价串到矩阵setClosedCosts 中
% 从setOpen中删除刚才放到矩阵setClosed中的那个点
%如果这个点位于矩阵的内部
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)];
%如果这个点位于矩阵第一行
elseif (ii == 1)
setOpen = setOpen(2:end);
setOpenCosts = setOpenCosts(2:end);
setOpenHeuristics = setOpenHeuristics(2:end);
%如果这个点位于矩阵的最后一行
else
setOpen = setOpen(1:end-1);
setOpenCosts = setOpenCosts(1:end-1);
setOpenHeuristics = setOpenHeuristics(1:end-1);
end
%%
% 把拓展出来的点中符合要求的点放到setOpen 矩阵中，作为待选点
for jj=1:length(posinds)
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
%%
%调用findWayBack函数进行路径回溯，并绘制出路径曲线
if max(ismember(setOpen,goalposind))
disp('Solution found!');
[p,Road_Long]= findWayBack(goalposind,fieldpointers,Road_Long); % 调用findWayBack函数进行路径回溯，将回溯结果放于矩阵P中
plot(p(:,2)+0.5,p(:,1)+0.5,'Color',0.2*ones(3,1),'LineWidth',4); %用 plot函数绘制路径曲线
drawnow;
drawnow;
clear sound
[y,Fs] = audioread('002.wav'); sound(y,Fs); % 播放名为000的音乐，注意该文件需要跟matlab文件位于同一文件夹下
elseif isempty(setOpen)
disp('No Solution!');
clear sound
[y,Fs] = audioread('000.wav');
sound(y,Fs);
end
%%
%findWayBack函数用来进行路径回溯，这个函数的输入参数是终止点goalposind和矩阵fieldpointers，输出参数是P
function [p,Road_Long] = findWayBack(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;
case 'R' % ’R’ 表示当前的点是由右边的点拓展出来的
px = px + 1;
case 'U' % ’U’ 表示当前的点是由上面的点拓展出来的
py = py - 1;
case 'D' % ’D’ 表示当前的点是由下边的点拓展出来的
py = py + 1;
end
p = [p; py px];
posind = sub2ind([n n],py,px);% 将坐标值转换为索引值
Road_Long=Road_Long+1;
end
end
%%
%这个函数的作用就是把输入的点作为父节点，然后进行拓展找到子节点，并且找到子节点的代价，并且把子节点距离终点的代价找到。
%函数的输出量中costs表示拓展的子节点到起始点的代价，heuristics表示拓展出来的点到终止点的距离大约是多少，posinds表示拓展出来的子节点
function [cost,heuristic,posinds] = findFValue(posind,costsofar,field,goalind,heuristicmethod)
n = length(field); % 获取矩阵的长度
[currentpos(1) currentpos(2)] = ind2sub([n n],posind); %将要进行拓展的点（也就是父节点）的索引值拓展成坐标值
[goalpos(1) goalpos(2)] = ind2sub([n n],goalind); %将终止点的索引值拓展成坐标值
cost = Inf*ones(4,1); heuristic = Inf*ones(4,1); pos = ones(4,2); %将矩阵cost和heuristic初始化为4x1的无穷大值的矩阵，pos初始化为4x2的值为1的矩阵
% 拓展方向一
newx = currentpos(2) - 1; newy = currentpos(1);
if newx > 0
pos(1,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(1) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(1) = 10*abs(goalpos(2)-newx) +10*abs(goalpos(1)-newy);
end
cost(1) = costsofar + field(newy,newx);
end
% 拓展方向二
newx = currentpos(2) + 1; newy = currentpos(1);
if newx <= n
pos(2,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(2) = 10*abs(goalpos(2)-newx) +10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(2) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
end
cost(2) = costsofar + field(newy,newx);
end
% 拓展方向三
newx = currentpos(2); newy = currentpos(1)-1;
if newy > 0
pos(3,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(3) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(3) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
end
cost(3) = costsofar + field(newy,newx);
end
% 拓展方向四
newx = currentpos(2); newy = currentpos(1)+1;
if newy <= n
pos(4,:) = [newy newx];
switch lower(heuristicmethod)
case 'euclidean'
heuristic(4) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
case 'taxicab'
heuristic(4) = 10*abs(goalpos(2)-newx) + 10*abs(goalpos(1)-newy);
end
cost(4) = costsofar + field(newy,newx);
end
posinds = sub2ind([n n],pos(:,1),pos(:,2)); % 将拓展出来的子节点的坐标值转换为索引值
end
%%
%这个矩阵的作用就是随机生成环境，障碍物，起始点，终止点等
function [field, startposind, goalposind, costchart, fieldpointers] = initializeField(n,wallpercent)
field = 10*ones(n,n);%设置任意两方格间的距离为10
field(ind2sub([n n],ceil(n^2.*rand(n*n*wallpercent,1)))) = Inf;%向上取整
% 随机生成起始点和终止点
startposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %随机生成起始点的索引值
goalposind = sub2ind([n,n],ceil(n.*rand),ceil(n.*rand)); %随机生成终止点的索引值
field(startposind) = 0; field(goalposind) = 0; %把矩阵中起始点和终止点处的值设为0
costchart = NaN*ones(n,n);%生成一个nxn的矩阵costchart，每个元素都设为NaN。就是矩阵初始NaN无效数据
costchart(startposind) = 0;%在矩阵costchart中将起始点位置处的值设为0
% 生成元胞数组
fieldpointers = cell(n,n);%生成元胞数组n*n
fieldpointers(:)= {'1'};
fieldpointers{startposind} = 'S'; fieldpointers{goalposind} = 'G'; %将元胞数组的起始点的位置处设为 'S'，终止点处设为'G'
fieldpointers(field==inf)={'0'};
end
% end of this function
%%
%利用随机生成的环境数据来进行环境的绘制
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
%%
function [new_ii,amend_count_1] = Path_optimization(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;
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;
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) +Weights*setOpenHeuristics(untext_ii); %计算期望点要花费的代价
if temp==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
function [field, startposind, goalposind, costchart, fieldpointers] = Reset_G_S(field, startposind, goalposind, costchart, fieldpointers,New_startposind,New_goalposind)
%在initializeField函数创建环境时，将field矩阵中没有障碍物的点处设定为10，有障碍物的点处设定为inf，起始点和终止点设为0
%现在需要将新的起始点和终止点处设为0，原来起始点和终止点处设为10
field(startposind) = 10; field(goalposind) = 10;
field(New_startposind) = 0; field(New_goalposind) = 0;
%在initializeField函数创建环境时，将costchart矩阵中的起始点设定为0，其他点设定为NAN
%现在需要将新的起始点设定为0，原来的起始点设定为1
costchart(startposind) = NaN;
costchart(New_startposind) =0;
% 在initializeField函数创建环境时，将fieldpointers元胞数组中起始点的位置处设为'S'，终止点处设为'G'，有障碍物的点设为'0'，没有障碍物的点设为'1'
% 现在需要将新的起始点处设为'S'，新的终止点处设为'G'，原来的起始点和终止点处设为'1'
fieldpointers{startposind} = '1'; fieldpointers{goalposind} = '1';
fieldpointers{New_startposind} = 'S'; fieldpointers{New_goalposind} = 'G';
%最后我们将新的起始点和新的终止点的索引值分别赋值给startposind和goalposind进行输出
startposind=New_startposind;
goalposind=New_goalposind;
end
本篇文章到这里就结束了，本文介绍的内容的完整代码的MATLAB文件我会放到附件里，听说在上传的时候设为粉丝可下载是不需要花费积分的，大家看一下需不需要积分，若还是需要积分，在评论区留言，我直接传给你
本篇文章内容的附件链接： 新的A星路径规划matlab文件合集.zip （https://download.csdn.net/download/qq_44339029/12917360）
涉及的音乐文件链接：动态衡量式A星算法代码涉及的音乐文件（https://download.csdn.net/download/qq_44339029/12905811）
附件里A_ROAD_book02文件是没有进行任何优化的传统的A星算法，A_ROAD_book03是优化后的A星算法（当Weights=1时，为传统A星算法，不为1时，为动态衡量式A星算法），A_ROAD_book05是添加了拐角优化函数的动态衡量式A星算法，这三个文件均添加了本篇文章中介绍的固定障碍物的功能
欢迎大家积极交流，本文未经允许谢绝转载
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
