课程的网址：https://www.coursera.org/learn/robotics-motion-planning/home/welcome?utm_medium=email&utm_source=other&utm_campaign=opencourse.welcome.robotics-motion-planning.%7Eopencourse.welcome.r8zaNVu-EeW0ugrg2GGh4Q.
对于课程后面留的程序作业中Dijkstra.m的分析。
路径搜索问题：
路径搜索问题，就类似于下图中，机器人要从绿色起点走到黄色终点，沿什么样路线过去？
这是我们从视觉上来理解这个问题，可以沿着红线走过去。先不说怎么让机器人自己能能找到这条线，先来说怎么把诸如“地图”、"线路"、"位置"、“障碍物”等等概念表示成代码呢？
地图
说算法之前，看看这个地图怎么表示。
首先，把地图划分为网格，这样就可以跟矩阵对应起来了。矩阵的行、列就直观的表示地图上的每一个位置坐标。
下面的 ones 命令就生成了一个 10*10 的全 1 阵 map，表示一张 10*10 的地图。那数字 1 表示什么呢？目前为止，什么也不表示，没有任何意义。
rows = 10; ncols = 10; map = ones(rows, ncols);
地图的其它信息，比如起点，终点，障碍物等等，怎么在这个地图上表示出来呢？那就给 map 矩阵赋不同的值呗，比如我们约定好：空地用 1 表示，墙用 2 表示。
我们约定好以后，矩阵的值就变得有意义了。将来判断 map(2,2)~=2，我们就知道 （2,2）这个位置是不是有墙。( 考虑到浮点数判断的精度问题，将来地图的数据类型可以设置为整型，之类。)
map (1:5, 7) = 2; % 设置一堵墙 map(6, 2) = 5; % 设置起点 map(4, 8) = 6; % 设置终点
但 map 其实还只是一堆数字，不直观。所以我们希望能把这个 map 给“画”出来。比如，我希望空地的1对应画白色；而墙的2对应画黑色：
% R G B cmap = [1 1 1; ...% 1 - white - clear cell 0 0 0; ...% 2 - black - obstacle 1 00; ...% 3 - red = visited 0 0 1; ...% 4 - blue - on list 0 1 0; ...% 5 - green - start 1 1 0];% 6 - yellow – destination
colormap(cmap);
这样设置之后，map 矩阵由于是 double 类型的，所以它的值为1-6时，画什么颜色就按照这里配置的颜色映射来定。画地图：
image(1.5,1.5,map); % image 命令画图时，对于超出上下限的值，依旧按照上下限对应的颜色来画。 grid on; axis image;
这样，这张地图就画出来了：
最后我们的目的是，要找到这么条黄色的路线：
可以看到图中还有其他颜色，这是为了表示算法的搜索过程，特意修改了地图的值来做显示用。
比如红色，就是我搜索过的区域，蓝色，就是下一步的搜索候选区。
算法的过程叙述：
1. 从第一个搜索中心：绿点开始，找到上下左右邻居，其实就是起点行列坐标i-1,j之类
2. 把找到的邻居变成蓝色(加入下次的搜索备选中心)，计算它们到起点的路程(其实就是+1)，保存到路程矩阵里；
1’选新的搜索中心(从蓝色邻居里挑 “距离起点路程最近的那个”)，找到上下左右邻居
2’把它的新邻居也变成蓝色，计算新邻居到起点的路程(在当前搜索中心路程1的基础上，再+1)，保存到路程矩阵里；
1”再从所有蓝色邻居里挑选搜索中心，找到新邻居
2”把新邻居变成蓝色，计算邻居到起点的路程，保存到路程矩阵
一直这么循环，总有一天，会一直搜索到终点。
PS：用不同的方法从蓝色邻居里来挑选搜索中心，就构成了 Dijkstra 算法和 A* 算法的主要区别。
Dijkstra 里只有一个原则，就是挑选离起点路程最近的，其实就是无方向向外扩散的。而 A*算法还会加上一个跟终点的距离考量，所以就是带有方向性的扩散。一般情况下，A* 会搜索的快一点。
可以看到，这个算法要保存一些信息，所以先定义一些变量。
start_node = sub2ind(size(map), 6, 2); % 为了方便，把起点和终点的行列下标换成索引，所以 map(6,2)和 map(start_node)就是同一个意思。 dest_node = sub2ind(size(map), 4, 8);
a. 定义一个变量来保存蓝色邻居以及它们到起始格的路程
所以这里定义了 distanceFromStart 来保存这些信息，初始化为 Inf，表示从没有访问过。一旦有值，就说明是蓝色邻居，赋值的大小就表示改点跟起始点的路程。一旦变成红色，就把它的值再改回 Inf。
至于具体要显示动态图，要修改的是 map 的值。
最后，这个矩阵会更新成这个样子。路程更新到了第4行第8列的终点位置，而且它距离起点有8步。那么这八步是怎么走的呢，就需要有矩阵来保存路线信息。
b. 定义一个变量来保存路径。
parent = zeros(nrows,ncols);
这个矩阵最后会更新成什么样，它怎么表示路线呢？
比如下图，第4行第8列的值是75，就表示它的上家的格子编号是75，其实也就是矩阵的索引位置，按列数过来就是第8列第5行。这里的值是76，就表示它的上家的格子第8列第6行。依次类推，这样就可以反推出一条线路来了。
变量定义完了，那么开始循环搜索路径
while true
先画一下当前的地图
map(start_node) = 5; map(dest_node) = 6; image(1.5, 1.5, map); grid on; axis image; drawnow;
这时就出来了这张图
那么，就开始搜索了，可是以哪个点为搜索中心呢。
当然是从蓝色邻居列表选离初始点最近的那一个点。
因为 distanceFromStart 的初始值是 Inf，已知的只有起始点的路程，为0。所以如果是第一次循环，那min函数得到的current自然就是16了，这也是初始点的索引坐标；这时得到的 min_dist =0。
[min_dist, current] = min(distanceFromStart(:)); %搜索中心的索引坐标：current, %搜索中心与起始点的路程：min_dist % 这两个值后面会用。 %这里做一些简单判断，如果已经扩张到终点了，或者没有路径，则退出循环。 if ((current == dest_node) || isinf(min_dist)) break; end;
为了画图效果，把 map 的前点坐标赋值为 3 ，表示本次循环已经以此为中心搜索一次了。
map(current) = 3;
可以看到这是现在 map 的值，如果画出来就是这样：
同时把distanceFromStart的这个位置赋值为 Inf，表示它已经当过搜索中心了。
上面的 min 函数将来就不可能再找到这个坐标。
distanceFromStart(current) = Inf;
把搜索中心的邻居的坐标点找出来，这里只找上下左右的邻居没有计算斜角：
所以(6,2)的四个邻居的坐标计算出来，得到一个四行两列的结果：
neighbor =
5 2
7 2
6 3
6 1
为了方便，现在把这种行列形式变为索引形式：
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2))
neighborIndex =
15
17
26
6
下面就要计算每个邻居的路程，保存每个邻居的路线。
for i=1:length(neighborIndex) if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3&& map(neighborIndex(i))~= 5) map(neighborIndex(i)) = 4; % 在地图上把邻居变成蓝色。这里纯为了显示用。 % distanceFromStart这个矩阵初始值是 Inf ，所以第一次找到它的时候肯定会更新路径的， % 循环多次以后，可能会有多次机会走到这个邻居，所以要看哪条路近。 % 只有邻居已有的路线比“从当前搜索中心走过去”要长的话，才会更新这个邻居的信息。 ifdistanceFromStart(neighborIndex(i))> min_dist + 1 distanceFromStart(neighborIndex(i)) = min_dist+1; %更新邻居的路程信息 parent(neighborIndex(i)) = current; % 更新邻居的路径信息 end end end
第一次搜索后，可以看到这四个邻居的距离值 distanceFromStart，更新为 1 了：
然后，这四个邻居的线路，16 就是表示是从起始点(坐标索引是16)走过来的。
map 就变成这样：
end %这是 while true 对应的end
这是第一轮循环，然后再把上面的 while 循环再走一遍。
从之前的代码可以看到，会先把起始点的颜色重置一下。然后上一轮找到的四个邻居已经标记为蓝色了。
同样，第二次也会先定搜索中心，根据上一次循环得到的 distanceFromStart 矩阵的结果，其实最小值有四个，都是1，Dijkstra算法没有别的考量权重，所以这里只是按顺序，取了第一个 1 作为搜索中心：
[min_dist, current] = min(distanceFromStart(:)); if ((current == dest_node) ||isinf(min_dist)) break; end; map(current) = 3; %把新的搜索中心标记为红色 distanceFromStart(current) = Inf; %把搜索中心从邻居去除。 [i, j] = ind2sub(size(distanceFromStart), current);
得到的current就是左边这个红点，min_dist就是刚才计算出来的1。
所以，你看这时候，搜索中心在边界，左边那个(6,0)超出了边界，就是假邻居了：
neighbor = [i-1,j;... i+1,j;... i,j+1;... i,j-1]
neighbor =
5 1
7 1
6 2
6 0
需要把它给去掉：
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +... (neighbor(:,2)<1) + (neighbor(:,2)>ncols ) locate = find(outRangetest>0); neighbor(locate,:)=[] % =[]就是删除的意思。
neighbor =
5 1
7 1
6 2
同样，开始更新每一个邻居的信息：
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) fori=1:length(neighborIndex) if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) map(neighborIndex(i)) = 4; ifdistanceFromStart(neighborIndex(i))> min_dist + 1 distanceFromStart(neighborIndex(i)) = min_dist+1; parent(neighborIndex(i)) = current; end end end
最后，map 更新，红点就是已经当过搜索中心了，蓝点就是全部的待搜索邻居。
下面的 distanceFromStart 就是所有蓝点的位置(有数值的地方)，以及这些蓝点到起始点的路程（数值）。
下一个循环，便接着再找 distanceFromStart 的最小值所在的位置，作为搜索中心。
可以看到，有1，1就是从起始点1步就走到了这里；2表示从起始点两步就走到了这里。
那，是沿着哪条线路走过来的呢？
就得对照着看 parent 矩阵的数据了，上面那张图里的 2 步，就是下图的箭头这么走出来的：
6,16表示的是矩阵的索引编号。
那么，一直这么循环下去，最后会得到什么呢，也就是这样图：
最后得到的这个 distancdFromStart 矩阵，可以看出，从起始点到目标点[4,8]，需要走8步。
那怎么走呢？parent里表示出了路线，按照格子里所显示的索引坐标，倒回来追溯：
脚本主框架是从课程里拿出来的，算法实现部分我没考虑代码效率，只是为了大概理解算法：
%% % set up color map for display cmap = [1 1 1; ...% 1 - white - clear cell 0 0 0; ...%2 - black - obstacle 1 0 0; ...% 3 - red = visited 0 0 1; ...% 4 - blue - on list 0 10; ...% 5 - green - start 1 1 0];% 6 - yellow - destination colormap(cmap); map = zeros(10); % Add an obstacle map (1:5, 7) = 2; map(6, 2) = 5; % start_coords map(4, 8) =6; % dest_coords image(1.5,1.5,map); grid on; axis image; %% nrows = 10; ncols = 10; start_node = sub2ind(size(map), 6, 2); dest_node = sub2ind(size(map), 4, 8); % Initialize distance array distanceFromStart = Inf(nrows,ncols); distanceFromStart(start_node) = 0; % For each grid cell this array holds the index of its parent parent = zeros(nrows,ncols); % Main Loop while true % Draw current map map(start_node) = 5; map(dest_node) = 6; image(1.5, 1.5, map); grid on; axis image; drawnow; % Find the node with the minimum distance [min_dist, current] = min(distanceFromStart(:)); if ((current == dest_node) ||isinf(min_dist)) break; end; map(current) = 3; distanceFromStart(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)) fori=1:length(neighborIndex) if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) map(neighborIndex(i)) = 4; ifdistanceFromStart(neighborIndex(i))> min_dist + 1 distanceFromStart(neighborIndex(i)) = min_dist+1; parent(neighborIndex(i)) = current; end end end end
%% 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