路径规划A*算法matlab

function obstacle=GetObstacle(Network) %% Step1:统计已有障碍点 R=zeros(Network.Limit+2,1); S=(0:Network.Limit+1)'; T=(Network.Limit+1)*ones(Network.Limit+2,1); Bottom=[S,R]; % 底边 Top=[S,T]; % 顶边 Left=[R,S]; % 左边 Right=[T,S]; % 右边 boundary=[Bottom;Right;Top;Left]; % 网络边界障碍点 Node_In=[11,11;9,1;10,2;11,3;10,1;11,2;11,1]; % 内部已有障碍点 obstacle_Ex=[boundary;Node_In]; % 已有障碍点 %% Step2:随机添加新障碍点 obstacle_New=round(rand([Network.nObstacle,2])*Network.Limit); % 随机生成 removeInd=[]; for i=1:size(obstacle_New,1) if (isSamePosi(obstacle_New(i,:),Network.start) || isSamePosi(obstacle_New(i,:),Network.goal)) % 判断是和起终点是否一致 removeInd=[removeInd;i]; end end obstacle_New(removeInd,:)=[]; % 删除和起终点一致的障碍点 %% Step3:总障碍点汇总 obstacle=[obstacle_Ex;obstacle_New];