宾夕法尼亚大学：Robotics: Computational Motion Planning 课程习题以及个人代码一：A*以及Dijkstra

非常感谢 Kin__Zhang 的B站视频以及代码解释。
可以在coursera课程中选择旁听下载代码。
附上链接：B站
\qquad \qquad 优快云博客

视频1-4为week 1的内容。主要是Dijkstra算法和A*算法。个人感觉视频中讲解较为简单。我自己感觉还不错的两篇讲解文章：Dijkstra \qquad A*+Dijkstra

代码中TestScript1.m为测试用程序

map = false(10);

% Add an obstacle
map (1:3, 6) = true;
map (5:8, 6) = true 
start_coords = [6, 2];
dest_coords  = [4, 9];

%%
close all;
%分别是两个函数的入口
[route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);
[route, numExpanded] = AStarGrid (map, start_coords, dest_coords);

Dijkstra

程序中前一部分就只说明下作用，代码中也带有注释，不详细介绍。
distanceFromStart和地图大小一样，每个点存储的是和地图对应的点到start的距离。初始化为inf，因为后续遍历时找的是min_dist。start点初始距离是0，因为要选择min_dist的点开始寻找。matlab中的sub2ind和ind2sub分别是将(i,j)转换为整个的第几个，注意matlab是竖着排列的 (应该是自己用的matlab不多，不太注意)

还有就是，程序中需要返回扩散的次数numExpended。

主循环：

while true
    
    % Draw current map
    %设置状态，分别是1-6，刚开始有说
    map(start_node) = 5;
    map(dest_node) = 6;
    
    % make drawMapEveryTime = true if you want to see how the 
    % nodes are expanded on the grid. 用来看效果的
    if (drawMapEveryTime)
        image(1.5, 1.5, map);
        grid on;
        axis image;
        drawnow;
    end
    
    % Find the node with the minimum distance
    [min_dist, current] = min(distanceFromStart(:)); %min_dist为distaceFromStart中的最小值，current为索引(如果多个就是第一个)
    %如果当前就是终点，或者最小值就是inf，表示遍历了所有的都没找到路径，也返回。
    if ((current == dest_node) || isinf(min_dist))
        break;
    end;
    
    % Update map 表示当前节点已经被访问。
    map(current) = 3;         % mark current node as visited
    distanceFromStart(current) = Inf; % remove this node from further consideration
    
    % Compute row, column coordinates of current node  返回节点的(i,j)。不过我没用到
    [i, j] = ind2sub(size(distanceFromStart), current);
    % ********************************************************************* 
    % YOUR CODE BETWEEN THESE LINES OF STARS
    % Visit each neighbor of the current node and update the map, distances
    % and parent tables appropriately.
    numExpanded = numExpanded+1; 计算次数+1
    %把当前节点移除，在map中把其当做3,已经更新了
    current_up = current-1;
    current_down = current+1;
    current_right = current+nrows;
    current_left = current-nrows;  %matlab中是竖着排列的，周围4个点
    neighborhood = [current_up,current_down,current_right,current_left];
%     current 
%     map
%     对周围的点判断三个条件，不是障碍物，在地图内，未遍历过   少判断了不是起点, still exists problem
%     distanceFromStart
    for i = 1:4
    	%首先判断是否在地图内，要不然后边操作没意义，也会因为不在0-max内报错
        if((neighborhood(i)>0) && (neighborhood(i)<(nrows*ncols)))
        	%判断是否是障碍物，是否被遍历。是否是起点,这里判断是否是起点的原因是，后续的父节点那块。会导致，起点的父节点一直是起点
        	%是死循环了
            if((map(neighborhood(i)) ~= 2) && map(neighborhood(i))~=3 && map(neighborhood(i))~=5 )
                %Kin_Zhang博主的是用的当前节点的减去start_node，实际应该是当前点+1。因为障碍存在，实际的距离可能会大于这个计算结果。
                if(distanceFromStart(neighborhood(i))>min_dist+1)
                    map(neighborhood(i)) = 4; % add on list  没写
                    %最后问题出在这,是因为前边已经把那个当前节点的距离改成Inf了，防止再次遍历，但是是不是可以放到这块进行
                    %用min_dist就行。
                    distanceFromStart(neighborhood(i)) = min_dist+1;
                    distanceFromStart(neighborhood(i))
                    %更新父节点
                    parent(neighborhood(i)) = current;
%                     parent
%                     map(neighborhood(i)) = 3;
%                 current
                end            
            end
        end
    end
end

其中父节点每个点的值对应是上一个节点的位置。如图：

目标点的值是74，对应的应该去找第74个点，是64，再依次类推，直到为0。
以下程序是找父节点以及渲染用的

if (isinf(distanceFromStart(dest_node)))
    route = [];
else
    route = [dest_node];
    
    while (parent(route(1)) ~= 0)
        route = [parent(route(1)), route];
    end
    
        % Snippet of code used to visualize the map and the path
    for k = 2:length(route) - 1        
        map(route(k)) = 7;
        pause(0.3);
        image(1.5, 1.5, map);
        grid on;
        axis image;
    end
end

end

Dijkstra的示意图：(少了几帧)

A*算法

主要区别在于更新的值不一样，在Dijkstra中更新的是distanceFromStart，在A*中更新的是$f$值
$f=g+h$

程序段

    numExpanded = numExpanded + 1;
    current_left = current - nrows;
    current_right = current + nrows;
    current_up = current - 1;
    current_down = current + 1;
    neighborhood = [current_up, current_right, current_down, current_left];
    for i=1:4
        current_next = neighborhood(i);        
       if(current_next>0&&current_next<(nrows*ncols))
          if(map(current_next)~=2 && map(current_next)~=3 && map(current_next)~=5)
              g(current_next) = g(current) + 1;
              if(f(current_next) > g(current_next) + H(current_next))
                 map(current_next) = 4;
                 f(current_next) = g(current_next) + H(current_next);
                 parent(current_next) = current;
              end
          end
       end
    end

示意图：