function path=AStar(obstacle,p)
path=[];%用于存储路径
%OpenList
open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2)];
%CloseList
close=[];
next=MotionModel();%更新状态--下一步的八个点
findFlag=false;%目标标志
while ~findFlag
if isempty(open(:,1)) disp('No path to goal!!');
return;
end
[Y,I] = sort(open(:,3))%对OpenList中第三列排序
open=open(I,:);
%判断是否达到目标
if isSamePosi(open(1,1:2),p.goal)
disp('Find Goal!!');
close=[open(1,:);close]
open(1,:)=[];
findFlag=true;
break;
end
%找到最优轨迹放到close list保存
for in=1:length(next(:,1))
m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)];
m(3)=next(in,3)+h(m(1:2),p.goal);%m--F值
if isObstacle(m,obstacle)
continue;
end
%判断周围的点在不在openlist中。如果不在则加入到open list中
[flag, targetInd]=FindList(m,open,close)
if flag==1 %在Openlist中
disp('已存在OpenList中')
continue;
elseif flag==2
disp('已存在CloseList中')
continue;
else
open=[open;[m open(1,1) open(1,2)]]
end
end
if findFlag==false
%将OpenList中的第一行移动到CloseList中
close=[close; open(1,:)]
open(1,:)=[];
end
end
path=GetPath(close,p.start);
end
评论2