2 Robotics: Computational Motion Planning 第1周（内含Dijkstra 和 A* MATLAB代码手把手教学）课后习题解答

首先这个系列的第一个单元是空中机器人，博客如下：
1 Robotics: Aerial Robotics 第1+2周 课程学习记录及课后习题解答
1 Robotics: Aerial Robotics 第3+4周 课程学习记录及课后习题解答

此课程在Coursera需要科学上网才能观看，放一下B站和Coursera的课程链接

1. UP主 博主自己做的字幕版本 第二单元更新完毕
2. Coursera的链接介绍
   此文仅为听课记录以及做题思考，可以的话我会将题目和代码题都搬运一下
   有所错误或是好的想法欢迎评论区交流。（点个赞就更好了，鼓励一下~）

• 2.17：等不起全部课程要在2.24才给我通过（申请的助学金），开了个小号作为旁听来听课吧，突然发现旁听竟然没有资格做作业（这也太坑了）；看着这么好的课没人翻译，那就一人字幕组走起；刚刚做完第一周的编程作业了，又… 中招好几处，明天更编程作业吧
• 2.25：今天突然发现原来CSDN是能插入视频的！！过来把视频插一下。

2 Robotics: Computational Motion Planning

WEEK - 1

这一单元为什么… 没有笔记了呢？
——因为… 博主都去做字幕了… 一节课得看4-5遍基本都理解完了
emm 所以大家多多点一下，给点鼓励（三连暗示）：UP主 博主自己做的字幕版本
第一周的课真的非常好，其实以前参加过天池一期：未来已来天气那题，用到过A* star听了看了这么多，现在再回来看一遍，觉得恍然大悟，也给后面看的人一点引领吧（课程代码可以白嫖走直接用到自己需要的场合，我发现我资源设的 0C币，但是还是要，后期我会弄到百度云~大家也可以留言让我发一下）

Motion and Planning - 1.2 Grassfire Algorithm：（能在b站投个币，一键三连就更好了）

【自制中英字幕】宾夕法尼亚大学机器人专项课程（Robotics）持续更新

Quiz

1.If you use the Grassfire or breadth first search procedure to plan a path through a grid from a node A to a node B, then you use the same procedure to plan a path from node B to node A, will the two paths have the same length?
Yes
解释：emm，这个不用解释了吧，就是从起点到终点和终点到起点，这么规划都是一样的

2.If you use the Grassfire or breadth first search procedure to plan a path through a grid from a node A to a node B, then you use the same procedure to plan a path from node B to node A, are the two paths guaranteed to be the same except in opposite directions?
No

3.If you use the grassfire algorithm to plan a path through a series of grids with increasing dimension, 2 dimensional, 3 dimensional, 4 dimensional etc. The amount of computational effort required increases ___________ with the dimension of the problem.
exponentially

4.Generally speaking, which procedure would take less time to find a solution to a typical path planning problem on a discrete grid or graph?
A*

Programming Assignment

Assignment 1: Dijkstra Algorithm

先看视频理解过程！！（能在b站投个币，一键三连就更好了）
Motion and Planning - 1.3 Dijkstra’s Algorithm：

【自制中英字幕】宾夕法尼亚大学机器人专项课程（Robotics）持续更新

第一个作业！直接大招：教你怎么写Dijkstra Algorithm，完整的题目我后面贴在资源里面。
这里我自己写的时候有两个注意的地方：

1. 这里看清楚函数的返回有numExpanded也就是往外扩散的次数。每次访问完一个节点的所有邻居需要加1的

function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)

2. 在这if条件语句中：注意！如果第一个邻居就是终点的话得结束的（所以可以看到我的if最后有==6的情况）：不然就有这条过不了的提示：Dijkstra Test 3 failed because path length is not as expected
3. 这个代码有注释，我觉得听完课应该都能看得懂的，这系列的课我预计都会做中英字幕的，欢迎大家来看，三连一下，鼓励一人字幕组 UP主 博主自己做的字幕版本（只是没更新完）注意记得选一下Motion Planning开头的，毕竟我也做了点其他单元的
4. 给大家先看一下这两个函数的调用：

%
% TestScript for Assignment 1
%

%% Define a small map
map = false(10);

% Add an obstacle
map (1:5, 6) = true;

start_coords = [6, 2];
dest_coords  = [8, 9];

%%
close all;
[route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);
% Uncomment following line to run Astar
[route, numExpanded] = AStarGrid (map, start_coords, dest_coords);

%HINT: With default start and destination coordinates defined above, numExpanded for Dijkstras should be 76, numExpanded for Astar should be 23.

然后是Dijkstra的整体代码，Your code are Here是需要我们填充的地方

function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)
% Run Dijkstra's algorithm on a grid.
% Inputs : 
%   input_map : a logical array where the freespace cells are false or 0 and
%   the obstacles are true or 1
%   start_coords and dest_coords : Coordinates of the start and end cell
%   respectively, the first entry is the row and the second the column.
% Output :
%    route : An array containing the linear indices of the cells along the
%    shortest route from start to dest or an empty array if there is no
%    route. This is a single dimensional vector
%    numExpanded: Remember to also return the total number of nodes
%    expanded during your search. Do not count the goal node as an expanded node.


% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue  - on list
% 5 - green - start
% 6 - yellow - destination

cmap = [1 1 1; ...
        0 0 0; ...
        1 0 0; ...
        0 0 1; ...
        0 1 0; ...
        1 1 0; ...
    0.5 0.5 0.5];

colormap(cmap);

% variable to control if the map is being visualized on every
% iteration
drawMapEveryTime = true;

[nrows, ncols] = size(input_map);

% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);

map(~input_map) = 1;   % Mark free cells
map(input_map)  = 2;   % Mark obstacle cells

% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));

map(start_node) = 5;
map(dest_node)  = 6;

% Initialize distance array
distanceFromStart = Inf(nrows,ncols);

% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);

distanceFromStart(start_node) = 0;

% keep track of number of nodes expanded 
numExpanded = 0;

% Main Loop
while true
    
    % Draw current map
    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(:));
    
    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] = 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; %运行一次就有一次增加了
    current_node=[i,j];%i是row j是col
    south_node=[i+1,j];
    north_node=[i-1,j];
    east_node=[i,j+1];
    west_node=[i,j-1];
    neighbourhood=[south_node;north_node;east_node;west_node];
    for k=1:4
        %第一组邻居开始查找确保 有邻居且没有出边界
        if( (neighbourhood(k,1)>0 && neighbourhood(k,2)>0) && (neighbourhood(k,1)<=nrows && neighbourhood(k,2)<=ncols))
            %此邻居没有被访问过，不是障碍物，也不是起点
            if( map(neighbourhood(k,1),neighbourhood(k,2)) ~=3 && map(neighbourhood(k,1),neighbourhood(k,2)) ~=2 && map(neighbourhood(k,1),neighbourhood(k,2)) ~=5 || map(neighbourhood(k,1),neighbourhood(k,2)) == 6)
                %距离比以前存下来的距离更近
                if(distanceFromStart(neighbourhood(k,1),neighbourhood(k,2)) >  1+sum(abs(start_coords-current_node)) || map(neighbourhood(k,1),neighbourhood(k,2)) == 6)
                    map(neighbourhood(k,1),neighbourhood(k,2))=4;%先进入现在探索列表中
                    distanceFromStart(neighbourhood(k,1),neighbourhood(k,2))=1+sum(abs(start_coords-current_node));
                    parent(neighbourhood(k,1),neighbourhood(k,2))=sub2ind(size(map),current);%把现在的邻居点加入到父列表中
                    map(neighbourhood(k,1),neighbourhood(k,2))=3;%放入已经访问的标识符
                end
            end
        end
    end
    %*********************************************************************
    
end

%% Construct route from start to dest by following the parent links
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.1);
        image(1.5, 1.5, map);
        grid on;
        axis image;
    end
end

end

Assignment 2: A* Algorithm

先看视频，理解基本过程！！
Motion and Planning - 1.4 A* Algorithm：（能在b站投个币，一键三连就更好了）

【自制中英字幕】宾夕法尼亚大学机器人专项课程（Robotics）持续更新

手把手教你写A 算法系列，如果能把上一个作业写出来A的应该就很简单了，主要是两个值得更新，得仔细看清楚了，这里我截图课程吧，给大家看一下伪代码，具体的都有中英字幕了，也就6mins的视频，很简单就能看懂了，多看看，结合代码作业理解

整体代码附上：

function [route,numExpanded] = AStarGrid (input_map, start_coords, dest_coords)
% Run A* algorithm on a grid.
% Inputs : 
%   input_map : a logical array where the freespace cells are false or 0 and
%   the obstacles are true or 1
%   start_coords and dest_coords : Coordinates of the start and end cell
%   respectively, the first entry is the row and the second the column.
% Output :
%    route : An array containing the linear indices of the cells along the
%    shortest route from start to dest or an empty array if there is no
%    route. This is a single dimensional vector
%    numExpanded: Remember to also return the total number of nodes
%    expanded during your search. Do not count the goal node as an expanded node. 

% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue  - on list
% 5 - green - start
% 6 - yellow - destination

cmap = [1 1 1; ...
    0 0 0; ...
    1 0 0; ...
    0 0 1; ...
    0 1 0; ...
    1 1 0; ...
    0.5 0.5 0.5];

colormap(cmap);

% variable to control if the map is being visualized on every
% iteration
drawMapEveryTime = true;

[nrows, ncols] = size(input_map);

% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);

map(~input_map) = 1;   % Mark free cells
map(input_map)  = 2;   % Mark obstacle cells

% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));

map(start_node) = 5;
map(dest_node)  = 6;

% meshgrid will `replicate grid vectors' nrows and ncols to produce
% a full grid
% type `help meshgrid' in the Matlab command prompt for more information
parent = zeros(nrows,ncols);

% 
[X, Y] = meshgrid (1:ncols, 1:nrows);

xd = dest_coords(1);
yd = dest_coords(2);

% Evaluate Heuristic function, H, for each grid cell
% Manhattan distance
H = abs(X - xd) + abs(Y - yd);
H = H';
% Initialize cost arrays
f = Inf(nrows,ncols);
g = Inf(nrows,ncols);

g(start_node) = 0;
f(start_node) = H(start_node);

% keep track of the number of nodes that are expanded
numExpanded = 0;

% Main Loop

while true
    
    % Draw current map
    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 f value
    [min_f, current] = min(f(:));
    
    if ((current == dest_node) || isinf(min_f))
        break;
    end;
    
    % Update input_map
    map(current) = 3;
    f(current) = Inf; % remove this node from further consideration
    
    % Compute row, column coordinates of current node
    [i, j] = ind2sub(size(f), current);
    
    % *********************************************************************
    % ALL YOUR CODE BETWEEN THESE LINES OF STARS
    % Visit all of the neighbors around the current node and update the
    % entries in the map, f, g and parent arrays
    %
    numExpanded=numExpanded+1;
    current_node=[i,j];%i是row j是col
    south_node=[i+1,j];
    north_node=[i-1,j];
    east_node=[i,j+1];
    west_node=[i,j-1];
    neighbourhood=[south_node;north_node;east_node;west_node];
    for k=1:4
        %第一组邻居开始查找确保 有邻居且没有出边界
        if( (neighbourhood(k,1)>0 && neighbourhood(k,2)>0) && (neighbourhood(k,1)<=nrows && neighbourhood(k,2)<=ncols))
            %此邻居没有被访问过，不是障碍物，也不是起点
            if( map(neighbourhood(k,1),neighbourhood(k,2)) ~=3 && map(neighbourhood(k,1),neighbourhood(k,2)) ~=2 && map(neighbourhood(k,1),neighbourhood(k,2)) ~=5)
                %距离比以前存下来的距离更近
                if(g(neighbourhood(k,1),neighbourhood(k,2))>  1+sum(abs(start_coords-current_node)) )
                    map(neighbourhood(k,1),neighbourhood(k,2))=4;%先进入现在探索列表中
                    g(neighbourhood(k,1),neighbourhood(k,2)) = 1+sum(abs(start_coords-current_node));
                    f(neighbourhood(k,1),neighbourhood(k,2))=g(neighbourhood(k,1),neighbourhood(k,2))+H(neighbourhood(k,1),neighbourhood(k,2));
                    parent(neighbourhood(k,1),neighbourhood(k,2))=sub2ind(size(map),current);%把现在的邻居点加入到父列表中
                    map(neighbourhood(k,1),neighbourhood(k,2))=3;%放入已经访问的标识符
                end
            end
        end
    end    
    
    %*********************************************************************
    
    
end

%% Construct route from start to dest by following the parent links
if (isinf(f(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.1);
        image(1.5, 1.5, map);
        grid on;
        axis image;
    end
end

end