路径搜索 – Dijkstra 算法 (MATLAB实现)
課程的網址: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实现)的全部內容,希望文章能夠幫你解決所遇到的問題。
- 上一篇: 两款【linux字符界面下】显示【菜单】
- 下一篇: 通过composer安装阿里大于接口扩展