A*保姆级讲解（路径规划经典代码1）

​
A算法，A(A-Star)算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是解决许多搜索问题的有效算法。算法中的距离估算值与实际值越接近，最终搜索速度越快。

A-Star算法是一种静态路网中求解最短路径最有效的直接搜索方法，也是许多其他问题的常用启发式算法。注意——是最有效的直接搜索算法，之后涌现了很多预处理算法（如CH），在线查询效率是A*算法的数千甚至上万倍。

公式表示为： f*(n)=g*(n)+h*(n),

其中， f*(n) 是从初始状态经由状态n到目标状态的最小代价估计，

g*(n) 是在状态空间中从初始状态到状态n的最小代价，

h*(n) 是从状态n到目标状态的路径的最小估计代价。

（对于路径搜索问题，状态就是图中的节点，代价就是距离）

真实h(n)的选取：

保证找到最短路径（最优解的）条件，关键在于估价函数f(n)的选取（或者说h(n)的选取）。

以h(n)表达状态n到目标状态估计的距离，那么h(n)的选取大致有如下三种情况：​
1.如果h(n)< h*(n)，这种情况下，搜索的点数多，搜索范围大，效率低。但能得到最优解
​2.如果h(n)=h*(n)，此时的搜索效率是最高的
3.如果 h(n)>h*(n)，搜索的点数少，搜索范围小，效率高，但不能保证得到最优解。

主程序

mapOriginal=im2bw(imread('map6db.bmp')); % 读取地图map6db.bmp，转化为灰度图像  input map read from a bmp file. for new maps write the file name here
resolutionX=100;     %X方向分辨率100
resolutionY=100;     %Y方向分辨率100
source=[250 250]; %  %设置起始点坐标  source position in Y, X format
goal=[490 490]; %    %设置终点坐标 goal position in Y, X format

conn=[1 1 1; % robot (marked as 2) can move up, left, right and down (all 1s), but not diagonally (all 0). you can increase/decrease the size of the matrix
      1 2 1;   
      1 1 1];

%   conn=[1 1 1 1 1; % another option of conn
%         1 1 1 1 1;
%         1 1 2 1 1;
%         1 1 1 1 1
%         1 1 1 1 1];

%   conn=[0 1 0; % another option of conn
%         1 2 1;
%         0 1 0];  
  
display=true; % display processing of nodes节点的显示处理 给display初始值为真

%%%%% parameters end here %%%%%参数到此结束

mapResized=imresize(mapOriginal,[resolutionX resolutionY]);  %调整图像大小至X×Y大小
map=mapResized;                                              % 将边界增长一个单位像素
for i=1:size(mapResized,1)                    %size（.....）：返回地图第一维度的长度（行方向尺寸）
    for j=1:size(mapResized,2)                %size（.....）：返回地图第二维度的长度（列方向尺寸）
        if mapResized(i,j)==0                 %如果地图此点为0（前面有循环结构）
            if i-1>=1, map(i-1,j)=0; end      %如果此点横坐标>=2（不在边界处），则将此处位设置为0（可通过）
            if j-1>=1, map(i,j-1)=0; end      %将纵坐标边界以外设置为可通过
            if i+1<=size(map,1), map(i+1,j)=0; end     %上边界以下设置为可通过
            if j+1<=size(map,2), map(i,j+1)=0; end     %右边界以左设置为可通过
            %（最外层边界比呢来就是之前加上去的，不对结果产生会影响）
            if i-1>=1 && j-1>=1, map(i-1,j-1)=0; end   %保证不是左下节点
            if i-1>=1 && j+1<=size(map,2), map(i-1,j+1)=0; end   %保证不是左上节点
            if i+1<=size(map,1) && j-1>=1, map(i+1,j-1)=0; end
            if i+1<=size(map,1) && j+1<=size(map,2), map(i+1,j+1)=0; end
            %以上4步骤排除四个角的节点
        end
    end
end
source=double(int32((source.*[resolutionX resolutionY])./size(mapOriginal)));  %将原本的开始坐标点对应到现在的地图上
goal=double(int32((goal.*[resolutionX resolutionY])./size(mapOriginal)));      %将原本的结束点。。。

if ~feasiblePoint(source,map), error('source lies on an obstacle or outside map'); end  %检查初始点是不是在障碍物里面
if ~feasiblePoint(goal,map), error('goal lies on an obstacle or outside map');     end  %检查终点是不是在障碍物里面
if length(find(conn==2))~=1, error('no robot specified in connection matrix');     end  %连接矩阵中未指定机器人
   %length：返回最大数组维数长度  find:寻找数组中这些数值的位置
%structure of a node is taken as positionY, positionX, historic cost, heuristic cost, total cost, parent index in closed list (-1 for source) 
%节点的形式为（X,Y,离开代价，启发代价，在封闭表中索引值
Q=[source 0 heuristic(source,goal) 0+heuristic(source,goal) -1]; % the processing queue of A* algorihtm, open list A* 算法的处理队列，打开列表
closed=ones(size(map)); % the closed list taken as a hash map. 1=not visited, 0=visited  在封闭列表中1为未访问，0为已访问
%ones：创建全为1的数组
closedList=[]; % the closed list taken as a list  创一个空的封闭列表
pathFound=false;  %给pathFound赋值为否
tic;  %启动秒表计时器
counter=0;  %计数器赋值0
colormap(gray(256));%查看并设置颜色图
while size(Q,1)>0  %当开发列表里面还有点
     [A, I]=min(Q,[],1);
     n=Q(I(5),:); % smallest cost element to process 要处理的最小成本要素
     Q=[Q(1:I(5)-1,:);Q(I(5)+1:end,:)]; % delete element under processing
     if n(1)==goal(1) && n(2)==goal(2) % goal test  如果找到了目标点
         pathFound=true;break;  %路径成立（结束）
     end
     [rx,ry,rv]=find(conn==2); % robot position at the connection matrix机器人在栅格中位置
     [mx,my,mv]=find(conn==1); % array of possible moves  可行的移动数组
     for mxi=1:size(mx,1) %iterate through all moves  迭代所有动作
         newPos=[n(1)+mx(mxi)-rx n(2)+my(mxi)-ry]; % possible new node可行的新节点
         if checkPath(n(1:2),newPos,map)
        %if path from n to newPos is collission-free如果从n到newPos的路径没有碰撞
              if closed(newPos(1),newPos(2))~=0 % not already in closed
                  historicCost=n(3)+historic(n(1:2),newPos);%离开代价
                  heuristicCost=heuristic(newPos,goal);%接近代价
                  totalCost=historicCost+heuristicCost;%总代价
                  add=true; % not already in queue with better cost  队列里没有更好的代价
                  if length(find((Q(:,1)==newPos(1)) .* (Q(:,2)==newPos(2))))>=1% find - 查找非零元素的索引和值
                      J=find((Q(:,1)==newPos(1)) .* (Q(:,2)==newPos(2)));
                      if Q(J,5)<totalCost, add=false;
                      else Q=[Q(1:J-1,:);Q(J+1:end,:);];add=true;  %路径替代
                      end
                  end
                  if add
                      Q=[Q;newPos historicCost heuristicCost totalCost size(closedList,1)+1]; % add new nodes in queue 将新节点加入列表中
                  end
              end
         end
     end
     closed(n(1),n(2))=0;closedList=[closedList;n]; % update closed lists 更新关闭列表
     if display
        image((map==0).*0 + ((closed==0).*(map==1)).*125 + ((closed==1).*(map==1)).*255);
        counter=counter+1;    %计数器加1
        M(counter)=getframe;  %getframe - 捕获坐标区或图窗作为影片帧  M 是一个 struct 类型的变量。
     end
end
if ~pathFound
    error('no path found')   %找不到路径
end
if display 
    disp('click/press any key');
    waitforbuttonpress; % 等待按钮按下
end

path=[n(1:2)]; %retrieve path from parent information从父节点检索路径
prev=n(6);
while prev>0
    path=[closedList(prev,1:2);path];
    prev=closedList(prev,6);
end
path=[(path(:,1)*size(mapOriginal,1))/resolutionX (path(:,2)*size(mapOriginal,2))/resolutionY];
pathLength=0;
for i=1:length(path)-1, pathLength=pathLength+historic(path(i,:),path(i+1,:)); end
fprintf('processing time=%d \nPath Length=%d \n\n', toc,pathLength);% fprintf - 将数据写入文本文件
imshow(mapOriginal);
rectangle('position',[1 1 size(mapOriginal)-1],'edgecolor','k') %建带有尖角或圆角的矩形
line(path(:,2),path(:,1));%划线

**

checkPath.m

**

function feasible=checkPath(n,newPos,map)
feasible=true;
dir=atan2(newPos(1)-n(1),newPos(2)-n(2));%计算角度
for r=0:0.5:sqrt(sum((n-newPos).^2))
    posCheck=n+r.*[sin(dir) cos(dir)];%计算距离
    if ~(feasiblePoint(ceil(posCheck),map) && feasiblePoint(floor(posCheck),map) && ... 
            feasiblePoint([ceil(posCheck(1)) floor(posCheck(2))],map) && feasiblePoint([floor(posCheck(1)) ceil(posCheck(2))],map))
        feasible=false;break;
        %Y = ceil(X) 将 X 的每个元素四舍五入到大于或等于该元素的最接近整数
        %feasible是判断该点是否在障碍物里的子函数
    end
    if ~feasiblePoint(newPos,map), feasible=false; end
end

feasiblePoint.m

function feasible=feasiblePoint(point,map)
feasible=true;  %feasible为逻辑值，true为初始变量
% check if collission-free spot and inside maps 检查地图中是否有无碰撞的地点和内部
if ~(point(1)>=1 && point(1)<=size(map,1) && point(2)>=1 && point(2)<=size(map,2) && map(point(1),point(2))==1)
    %在地图中，且有障碍物
    feasible=false;
end

heuristic.m

function h=heuristic(X,goal)
h = sqrt(sum((X-goal).^2));  %当前点到终点的距离

historic.m

function h=heuristic(X,goal)
h = sqrt(sum((X-goal).^2));  %当前点到终点的距离

%特此感谢R.Kala（2014）《使用A*算法的机器人路径规划代码》，印度信息技术研究所阿拉哈巴德

本人还在学习阶段，可能还存在错误。欢迎指正批评，共同进步。谢谢。