用A*算法路径规划， matlab程序

function astardemo_Hong
%ASTARDEMO Demonstration of ASTAR algorithm
%
%   Copyright Bob L. Sturm Ph. D. Assistant Professor
%   Department of Architecture Design and Media Technology
%     formerly Medialogy
%   Aalborg University i Ballerup
%     formerly Aalborg University Copenhagen
%   $Revision: 0.1 $  $Date: 2011 Jan. 15 18h24:24$

n = 20;   % field size n x n tiles  20*20的界面
wallpercent = 0.15;  % this percent of field is walls   45%的界面作为阻碍物（墙）

% create the n x n FIELD with wallpercent walls containing movement costs 
% a starting position STARTPOSIND a goal position GOALPOSIND the costs 
% A star will compute movement cost for each tile COSTCHART 
% and a matrix in which to store the pointers FIELDPOINTERS
[field startposind goalposind costchart fieldpointers] = ...
  initializeField（nwallpercent）;   %初始化界面

% initialize the OPEN and CLOSED sets and their costs
setOpen = [startposind]; setOpenCosts = [0]; setOpenHeuristics = [Inf];
setClosed = []; setClosedCosts = [];
movementdirections = {‘R‘‘L‘‘D‘‘U‘‘DR‘‘DL‘‘UR‘‘UL‘};

% keep track of the number of iterations to exit gracefully if no solution
counterIterations = 1;

% create figure so we can witness the magic
axishandle = createFigure（fieldcostchartstartposindgoalposind）;

% as long as we have not found the goal or run out of spaces to explore
while ~max（ismember（setOpengoalposind）） && ~isempty（setOpen）  %ismember（AB）返回与A同大小的矩阵，其中元素1表示A中相应位置的元素在B中也出现，0则是没有出现
  % for the element in OPEN with the smallest cost
  [temp ii] = min（setOpenCosts + setOpenHeuristics）;   %从OPEN表中选择花费最低的点tempii是其下标（也就是标号索引）
  % find costs and heuristic of moving to neighbor spaces to goal
  % in order ‘R‘‘L‘‘D‘‘U‘
  [costsheuristicsposinds] = findFValue（setOpen（ii）setOpenCosts（ii） ...
    fieldgoalposind‘euclidean‘）;       %扩展temp的四个方向点，获得其坐标posinds，各个方向点的实际代价costs，启发代价heuristics
  % put node in CLOSED and record its cost
  setClosed = [setClosed; setOpen（ii）];     %将temp插入CLOSE表中
  setClosedCosts = [setClosedCosts; setOpenCosts（ii）];  %将temp的花费计入ClosedCosts
  % update OPEN and their associated costs  更新OPEN表 分为三种情况
  if （ii > 1 && ii < length（setOpen））   %temp在OPEN表的中间，删除temp
    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）;   %temp是OPEN表的第一个元素，删除temp
    setOpenCosts = setOpenCosts（2:end）;
    setOpenHeuristics = setOpenHeuristics（2:end）;
  else     %temp是OPEN表的最后一个元素，删除temp
    setOpen = setOpen（1:end-1）;
    setOpenCosts = setOpenCosts（1:end-1）;
    setOpenHeuristics = setOpenHeuristics（1:end-1）;
  end
  % for each of these neighbor spaces assign costs and pointers; 
  % and if some are in the CLOSED set and their costs are smaller 
  % update their costs and pointers
  for jj=1:length（posinds）      %对于扩展的四个方向的坐标