A* —— Path Planning

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接

 

        A*是一种启发式搜索算法，或者说是一种最佳优先搜索，意思是它是根据加权图来制定的:从图的特定起始节点出发，寻找到给定目标节点的路径，其代价最小(旅行距离最小，时间最短等)。它通过维护从开始节点开始的路径树，并一次扩展这些路径的一条边，直到满足其终止条件。

       在主循环的每次迭代中，A*需要确定要扩展哪条路径。这样做是基于路径的成本以及将路径一直扩展到目标所需的成本估算。具体地说，A*选择最小的路径。

                                                                                 f (n) = g (n) + h (n)

       其中，n是路径上的下一个节点，g(n)是从开始节点到n的路径的代价，h(n)是一个启发式函数，估计从n到目标的最便宜路径的代价。如果选择扩展的路径是从开始到目标的路径，或者没有适合扩展的路径，则A*终止。启发式函数是针对特定问题的。如果启发式函数是可接受的，这意味着它从不高估达到目标的实际代价，那么A*保证返回从开始到目标的最小代价路径。

        A*的典型实现使用优先队列来重复选择要扩展的最小(估计)代价节点。这个优先队列称为开放集或边缘。在算法的每一步，将f(x)值最低的节点从队列中移除，相应更新其邻居的f和g值，并将这些邻居添加到队列中。该算法一直持续到目标节点的f值低于队列中的任何节点(或者直到队列为空)。a .目标的f值是最短路径的代价，因为目标处的h在可容许启发式中为零。

       目前所描述的算法只给出了最短路径的长度。为了找到实际的步骤序列，可以很容易地修改算法，使路径上的每个节点都能跟踪它的前任。该算法运行后，结束节点将指向它的前任节点，以此类推，直到某个节点的前任节点成为开始节点。例如，在地图上搜索最短路径时，h(x)可能表示到目标的直线距离，因为这是物理上任意两点之间的最小距离。对于电子游戏中的网格地图，根据可用的移动集合(4路或8路)，使用曼哈顿距离或八位距离会更好。

        如果启发式h满足图的每条边(x, y)的附加条件h(x)≤d(x, y) + h(y)(其中d表示边的长度)，则h称为单调或一致。在一致启发式下，a *可以保证在不多次处理任何节点的情况下找到最优路径，a *相当于运行Dijkstra算法，降低代价d'(x, y) = d(x, y) + h(y)−h(x)。

        本算法的优点是搜索能力较快，可以在线实现。缺点是时间复杂度微高，路径不顺滑，也只能处理静态的障碍物。

        下面借用了知乎大神的程序（链接）学习验证一下本算法：

clc;
clear;
close all;
%% load obs
obs = load('obsdata.txt');  % txt内容在下边
numofobs = length(obs(:,1));
%% set up color map for display 
cmap = [1 1 1; ...% 1 - white - 空地
        0 0 0; ...% 2 - black - 障碍 
        1 0 0; ...% 3 - red - 已搜索过的地方
        0 0 1; ...% 4 - blue - 下次搜索备选中心 
        0 1 0; ...% 5 - green - 起始点
        1 1 0];...% 6 - yellow -  到目标点的路径
colormap(cmap); 
map = zeros(25); 
% 设置障碍
for j=1:numofobs
    xobs(j) = obs(j,1);
    yobs(j) = obs(j,2);
    map(xobs(j),yobs(j)) = 2;
end
map(1,25) = 5; % 起始点
map(25,1) = 6; % 目标点
image(1.5,1.5,map);
%grid on;
axis image;
%% 建立地图
nrows = 25;
ncols = 25;
start_node = sub2ind(size(map), 1, 25); 
dest_node = sub2ind(size(map), 25, 1); 
% 距离数组初始化
distanceFromStart = Inf(nrows,ncols); 
distanceFromStart(start_node) = 0; 
%定义启发函数
[X, Y] = meshgrid (1:ncols, 1:nrows);
H = abs(Y - 4) + abs(X - 8);
f = Inf(nrows,ncols); 
f(start_node) = H(start_node);
% 对于每个网格单元，这个数组保存其父节点的索引。 
parent = zeros(nrows,ncols); 
% 主循环
t0=clock;
while true 
 % 画出现状图
 map(start_node) = 5; 
 map(dest_node) = 6; 
 image(1.5, 1.5, map); 
 %grid on; 
 axis image; 
 drawnow; 
  [~, current] = min(f(:)); %返回当前fn最小值的索引。
  [min_dist, ~] = min(distanceFromStart(:)); %返回当前距离数组的最小值。
  if ((current == dest_node) || isinf(min_dist)) %搜索到目标点或者全部搜索完，结束循环。
       break; 
  end; 

 map(current) = 3; %将当前颜色标为红色。
f(current) = Inf;  %当前区域在距离数组中设置为无穷，表示已搜索。
 [i, j] = ind2sub(size(distanceFromStart), current); %返回当前位置的坐标
neighbor = [i-1,j;... 
            i+1,j;... 
            i,j+1;... 
            i,j-1] %确定当前位置的上下左右区域。
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
                   (neighbor(:,2)<1) + (neighbor(:,2)>ncols ) %判断下一次搜索的区域是否超出限制。
locate = find(outRangetest>0); %返回超限点的行数。
neighbor(locate,:)=[] %在下一次搜索区域里去掉超限点。
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) %返回下次搜索区域的索引号。
for i=1:length(neighborIndex) 
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) 
    map(neighborIndex(i)) = 4; %如果下次搜索的点不是障碍，不是起点，没有搜索过就标为蓝色。
  if distanceFromStart(neighborIndex(i))> min_dist + 1      distanceFromStart(neighborIndex(i)) = min_dist+1; 
        parent(neighborIndex(i)) = current; %如果在距离数组里，。
        f(neighborIndex(i))=H(neighborIndex(i));
  end 
 end 
end 
end
TimeCost=etime(clock,t0)
%%
if (isinf(distanceFromStart(dest_node))) 
    route = [];
else 
    %提取路线坐标
   route = [dest_node]; 
      while (parent(route(1)) ~= 0) 
              route = [parent(route(1)), route]; 
       end 
  % 动态显示出路线     
        for k = 2:length(route) - 1 
          map(route(k)) = 7; 
                pause(0.1); 
                image(1.5, 1.5, map); 
              grid on; 
              axis image; 
              end 
end
axis off;  

10 1 0.5
20 1 0.5
21 1 0.5
17 2 0.5
18 2 0.5
1 3 0.5
5 3 0.5
18 3 0.5
3 4 0.5
9 4 0.5
15 4 0.5
22 4 0.5
23 4 0.5
2 5 0.5
7 5 0.5
9 5 0.5
13 5 0.5
18 5 0.5
3 6 0.5
7 6 0.5
24 7 0.5
5 7 0.5
9 7 0.5
18 7 0.5
13 8 0.5
15 8 0.5
5 9 0.5
19 9 0.5
21 9 0.5
6 11 0.5
11 11 0.5
16 11 0.5
17 11 0.5
2 12 0.5
11 12 0.5
19 12 0.5
5 14 0.5
12 14 0.5
13 14 0.5
5 15 0.5
2 16 0.5
7 16 0.5
10 16 0.5
18 16 0.5
5 17 0.5
11 17 0.5
21 17 0.5
23 17 0.5
16 18 0.5
2 19 0.5
9 19 0.5
17 19 0.5
24 19 0.5
5 20 0.5
13 20 0.5
15 20 0.5
16 20 0.5
21 20 0.5
25 20 0.5
3 21 0.5
4 21 0.5
7 21 0.5
15 21 0.5
20 21 0.5
24 21 0.5
6 22 0.5
5 23 0.5
9 23 0.5
11 23 0.5
16 23 0.5
20 23 0.5
23 23 0.5
4 25 0.5
9 25 0.5
18 25 0.5

经过部分修改后，本算法仿真示意图

                                                         

                                                         

本文作者水平有限，还望各路大神指点一二。。。。。。