机器人轨迹规划A*算法代码

function path=AStar(obstacle,p) path=[]; open=[p.start(1) p.start(2) h(p.start,p.goal) p.start(1) p.start(2)]; close=[]; next=MotionModel(); findFlag=false;%目标标志 while ~findFlag if isempty(open(:,1)) disp('No path to goal!!'); return; end [Y,I] = sort(open(:,3)) open=open(I,:); if isSamePosi(open(1,1:2),p.goal) disp('Find Goal!!'); close=[open(1,:);close] open(1,:)=[]; findFlag=true; break; end for in=1:length(next(:,1)) m=[open(1,1)+next(in,1) open(1,2)+next(in,2) open(1,3)]; %m(3)=m(3)+next(in,3)+h(m(1:2),p.goal)-h(open(1,1:2),p.goal); m(3)=next(in,3)+h(m(1:2),p.goal); if isObstacle(m,obstacle) continue; end [flag, targetInd]=FindList(m,open,close) if flag==1 disp("cao") continue; elseif flag==2 continue; else open=[open;[m open(1,1) open(1,2)]] end end if findFlag==false close=[close; open(1,:)] open(1,:)=[]; end end path=GetPath(close,p.start);