課程的網址：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