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

?

轉載于:https://www.cnblogs.com/dannierdeshenghuo/p/6474179.html

總結

以上是生活随笔為你收集整理的路径搜索 – Dijkstra 算法 (MATLAB实现）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。