function [Grids,Open,Close,target_flag] = Update(wknode,goal,obmap,Grids,Open,Close)
dim = size(obmap); % 行和列数目:m*n
target_flag=0; % 搜索到最优路径了
for i = -1:1 % i,j代表8种运动方式,上下左右,左上,右上,左下,右下
for j = -1:1
adjnode = wknode+[i,j]; % 根据不同运动方式计算邻接栅格点的坐标
row = adjnode(1);
col = adjnode(2);
if i == 0 && j == 0 % 忽略自身
continue
elseif row < 1 || row > dim(1) % 忽略行数组越界
continue
elseif col < 1 || col > dim(2) % 忽略列数组越界
continue
elseif obmap(row,col) == 1 % 忽略是障碍物的栅格
continue
elseif ~isempty(Close) && ismember(adjnode,Close,'rows') % 忽略是Close集合中的栅格
continue
end
fcost = Grids(wknode(1),wknode(2),4)+norm([i,j]); %计算当前点到邻接栅格点的f值,f=g+h
if Grids(row,col,4) > fcost % 邻接栅格点的旧f值比新f值大,更新f值
Grids(row,col,1) = wknode(1); % 更新邻接栅格点的父节点行坐标
Grids(row,col,2) = wknode(2); % 更新邻接栅格点的父节点列坐标
Grids(row,col,4) = fcost; % 更新邻栅格点的f值
% 如果邻接栅格点不存在并且栅格点也不是目标点
if ~ismember(adjnode,Open,'rows')
if ~isequal(adjnode,goal)
Open(end+1,:) = adjnode; %#ok<AGROW
else
Open(end+1,:) = adjnode; %#ok<AGROW
target_flag=1;
return
end
end
end
end
end
end
评论19