前言
這個學(xué)期學(xué)校開設(shè)了相應(yīng)的課程，同時也在學(xué)習(xí)古月居機器人學(xué)系列的《基于柵格地圖的機器人路徑規(guī)劃指南》，為了鞏固知識，方便自己的學(xué)習(xí)與整理，遂以學(xué)習(xí)筆記的形式記錄。
1. 深度優(yōu)先（DFS）和廣度優(yōu)先（BFS）
????深度優(yōu)先搜索（ Depth First Search , DFS )：首先從某個頂點出發(fā)，依次從它的各個未被訪問的鄰接點出發(fā)深度優(yōu)先搜索遍歷圖，直至圖中所有和該頂點有路徑相通的頂點都被訪問到。若此時尚有其他頂點未被訪問到，則另選一個未被訪問的頂點作起始點，重復(fù)上述過程，直至圖中所有頂點都被訪問到為止。
???? 廣度優(yōu)先搜索（ Breadth First Search , BFS )：從圖中某頂點出發(fā)，依次訪問的各個未曾訪問過的鄰接點，然后分別從這些鄰接點出發(fā)依次訪問它們的鄰接點，并使得“先被訪問的頂點的鄰接點先于后被訪問的頂點的鄰接點被訪問，直至圖中所有已被訪問的頂點的鄰接點都被訪問到。
????深度優(yōu)先搜索和廣度優(yōu)先搜索分別類似于樹的前序遍歷和層次遍歷。
????Dijkstra 算法屬于典型的廣度優(yōu)先搜索算法。
2. 深度優(yōu)先搜索(DFS)
2.1 算法基本思想
- 首先訪問圖中某一個頂點Vi，以該頂點為出發(fā)點；
- 任選一與該頂點Vi鄰接的未被訪問的頂點Vj；訪問Vj；
- 以Vj為新的出發(fā)點繼續(xù)進行深度優(yōu)先搜索，直至圖中所有和Vi有路徑的頂點被訪問到。
2.2 深度優(yōu)先搜索算法（C）
從圖的某一點 v 出發(fā)，遞歸地進行深度優(yōu)先遍歷算法描述：
void DFSTraverse(Graph G)
{for (v=0; v<G.vexnum; ++v)
visited[v] = FALSE; /*訪問標志數(shù)組初始化*/
for (v=0; v<G.vexnum; ++v)
if (!visited[v]) DFS(G, v); /*對尚未訪問的頂點調(diào)用 DFS*/
}
void DFS(Graph G,int v ) /*從第 v 個頂點出發(fā)遞歸地深度優(yōu)先遍歷圖 G*/
{ visited[v]=TRUE;Visit(v); /*訪問第 v 個頂點*/
for(w=FisrtAdjVex(G,v);w>=0; w=NextAdjVex(G,v,w))
if (!visited[w]) DFS(G,w); /*對 v 的尚未訪問的鄰接頂點 w 遞歸調(diào)用 DFS*/
}
以鄰接表為存儲結(jié)構(gòu)的整個圖 G 進行深度優(yōu)先遍歷實現(xiàn)的 C 語言描述:
void DFSTraverseAL(ALGraph G) /*深度優(yōu)先遍歷以鄰接表存儲的圖 G*/
{ int i;
for (i=0;i<G.vexnum;i++)
visited[i]=FALSE; /*標志向量初始化*/
for (i=0;i<G.vexnum;i++)
if (!visited[i]) DFSAL(G,i); /*vi未訪問過，從 vi開始 DFS 搜索*/
}
void DFSAL(ALGraph G,int i) /*以 vi為出發(fā)點對鄰接表存儲的圖 G 進行 DFS 搜索*/
{ ArcNode *p;
Visit(G.adjlist[i]); /*訪問頂點 vi*/
visited[i]=TRUE; /*標記 vi已訪問*/
p=G.adjlist[i].firstarc; /*取 vi邊表的頭指針*/
while(p) /*依次搜索 vi的鄰接點 vj，j=p->adjvex*/
{ if (!visited[p->adjvex]) /*若 vj尚未訪問，則以 vj為出發(fā)點向縱深搜索*/
DFSAL(G,p->adjvex);
p=p->nextarc; /*找 vi的下一個鄰接點*/
} }
此部分詳細原理解釋可以參考嚴蔚敏的數(shù)據(jù)結(jié)構(gòu)（C語言版）
????遍歷圖的過程實質(zhì)上是對每個頂點查找其鄰接點的過程，其耗費的時間則取決于所采用的存儲結(jié)構(gòu)。當以鄰接矩陣為圖的存儲結(jié)構(gòu)時，查找每個頂點的鄰接點所需時間為O(n2) ，其中 n 為圖中頂點數(shù)。而當以鄰接表作圖的存儲結(jié)構(gòu)時，找鄰接點所需時間為 O(e)，其中e 為無向圖中邊的數(shù)或有向圖中弧的數(shù)。由此，當以鄰接表作存儲結(jié)構(gòu)時，深度優(yōu)先搜索遍歷圖的時間復(fù)雜度為 O(n+e) 。
3. 廣度優(yōu)先搜索(BFS)
3.1 算法基本思想
????廣度優(yōu)先搜索（BFS）遍歷類似于樹的按層次遍歷。
（1）首先訪問圖中某一指定的出發(fā)點Vi；
（2）然后依次訪問VI的所有鄰接點Vi1，Vi2……Vit；
（3）再依次以Vi1，Vi2……Vit為頂點，訪問各頂點未被訪問的鄰接點，依此類推，直到圖中所有頂點均被訪問為止。
3.2 廣度優(yōu)先搜索(BFS)（C）
從圖的某一點 v 出發(fā)，進行廣度優(yōu)先遍歷算法描述：
void BFSTraverse (MGraph G) /*按廣度優(yōu)先非遞歸遍歷圖 G，使用輔助隊列 Q*/
{
for (v=0; v<G.vexnum; ++v)
visited[i] = FALSE; /*訪問標志數(shù)組初始化*/
for (v=0; v<G.vexnum; ++v)
if (!visited[v]) BFS(G, v); /*對尚未訪問的頂點調(diào)用 BFS*/
}
void BFS (Graph G,int v) {InitQueue(Q); /*置空的輔助隊列 Q*/
visited[v]=TRUE; Visit(v); /*訪問 v*/
EnQueue(Q,v); /*v 入隊列*/
while (!QueueEmpty(Q))
{ DeQueue(Q,u); /*隊頭元素出隊并置為 u*/
for(w=FistAdjVex(G,u); w>=0; w=NextAdjVex(G,u,w))
if(!visited[w])
{visited[w]=TRUE; Visit(w);
EnQueue(Q,w); /*u 尚未訪問的鄰接頂點 w 入隊列 Q*/
}
}
}
以鄰接矩陣為存儲結(jié)構(gòu)的整個圖 G 進行廣度優(yōu)先遍歷實現(xiàn)的 C 語言描述。
void BFSTraverseAL(MGraph G) /*廣度優(yōu)先遍歷以鄰接矩陣存儲的圖 G*/
{
int i;
for (i=0;i<G.vexnum;i++)
visited[i]=FALSE; /*標志向量初始化*/
for (i=0;i<G.vexnum;i++)
if (!visited[i]) BFSM(G,i); /* vi未訪問過，從 vi開始 BFS 搜索*/
}
void BFSM(MGraph G,int k) /*以 vi為出發(fā)點，對鄰接矩陣存儲的圖 G 進行 BFS 搜索*/
{
int i,j;
sqQueue Q;
InitQueue(Q);
Visit(G.vexs[k]); /*訪問原點 Vk*/
visited[k]=TRUE;
EnQueue(Q,k); /*原點 Vk入隊列*/
while (!QueueEmpty(Q))
{i=DeQueue(Q); /*Vi出隊列*/
for (j=0;j<G.vexnum;j++) /*依次搜索 Vi的鄰接點 Vj*/
if(G.edges[i][j]==1 && !visited[j]) /*若 Vj未訪問*/
{Visit (G.vexs[j]); /*訪問 Vj */
visited[j]=TRUE;
EnQueue(Q,j); /*訪問過的 Vj入隊列*/
}
}
}
此部分詳細原理解釋可以參考嚴蔚敏的數(shù)據(jù)結(jié)構(gòu)（C語言版）
4. Dijkstra算法
????Dijkstra 算法是由荷蘭計算機科學(xué)家迪杰斯特拉于1959年提出的，是從一個節(jié)點遍歷其余各節(jié)點的最短路徑算法，解決的是有權(quán)圖中最短路徑問題。迪杰斯特拉算法主要特點是從起始點開始，采用貪心算法的策略，每次遍歷到始點距離最近且未訪問過的頂點的鄰接節(jié)點，直到擴展到終點為止。
4.1 Dijkstra算法原理
????初始點看作一個集合S，其它點看作另一個集合挨個的把離初始點最近的點找到并加入集合S，集合中所有的點的d[i]都是該點到初始點最短路徑長度，由于后加入的點是根據(jù)集合S中的點為基礎(chǔ)拓展的，所以能找到最短路徑。
????用途： 用于求圖中指定兩點之間的最短路徑，或者是指定一點到其它所有點之間的最短路徑。
4.2 Dijkstra算法基本步驟
- 將圖上的初始點看作一個集合S，其它點看作另一個集合
- 根據(jù)初始點，求出其它點到初始點的距離d[i] （若相鄰，則d[i]為邊權(quán)值；若不相鄰，則d[i]為無限大）
- 選取最小的d[i]（記為d[x]），并將此d[i]邊對應(yīng)的點（記為x）加入集合S（實際上，加入集合的這個點的d[x]值就是它到初始點的最短距離）
- 再根據(jù)x，更新跟 x 相鄰點 y 的d[y]值：d[y] = min{ d[y], d[x] + 邊權(quán)值w[x][y] }，因為可能把距離調(diào)小，所以這個更新操作叫做松弛操作。
- 重復(fù)3，4兩步，直到目標點也加入了集合，此時目標點所對應(yīng)的d[i]即為最短路徑長度。（注：重復(fù)第三步的時候，應(yīng)該從所有的d[i]中尋找最小值，而不是只從與x點相鄰的點中尋找）
圖片中 B（23） 應(yīng)該是B（13）
????Dijkstra 算法十分簡潔，能夠有效的找到最優(yōu)解，不足之處在數(shù)據(jù)節(jié)點龐大時所需的節(jié)點繁多，效率隨著數(shù)據(jù)節(jié)點的增加而下降，耗費大量內(nèi)存空間與計算時間。
5. MATLAB編寫Dijkstra算法
MATLAB 編寫 Dijkstra 算法的幾個核心要素：
- 可以考慮將第1講的柵格地圖場景定義單獨存為一個函數(shù)，無需出現(xiàn)在算法的主程序里。
- 前面的案例基于拓撲地圖展開敘述，那么對于柵格地圖，通常采用8鄰域?？紤]到“遍歷每一個節(jié)點的鄰近節(jié)點”的功能在程序中會多次出現(xiàn)，故可以考慮將其單獨存為一個函數(shù)，以便調(diào)用。
5.1 defColormap.m
????可以將之前所創(chuàng)建的柵格地圖作為一個函數(shù)來使用
詳見——路徑規(guī)劃——基于MATLAB的柵格地圖
function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ... % 1-白色-空地
0 0 0; ... % 2-黑色-靜態(tài)障礙
1 0 0; ... % 3-紅色-動態(tài)障礙
1 1 0;... % 4-黃色-起始點
1 0 1;... % 5-品紅-目標點
0 1 0; ... % 6-綠色-到目標點的規(guī)劃路徑
0 1 1]; % 7-青色-動態(tài)規(guī)劃的路徑
% 構(gòu)建顏色MAP圖
colormap(cmap);
% 定義柵格地圖全域，并初始化空白區(qū)域
field = ones(rows, cols);
% 障礙物區(qū)域
obsRate = 0.2;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;
5.2 getNeighborNodes.m
function neighborNodes = getNeighborNodes(rows, cols, lineIndex, field)
[row, col] = ind2sub([rows,cols], lineIndex);
% neighborNodes = inf(4,2);
neighborNodes = inf(8,2);
%% 查找當前父節(jié)點臨近的周圍8個子節(jié)點 注釋掉后為4鄰域
% 左上節(jié)點
if row-1 > 0 && col-1 > 0
child_node_sub = [row-1, col-1];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
child_brother_node_sub1 = [row-1, col];
child_brother_node_sub2 = [row, col-1];
neighborNodes(1,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
cost = norm(child_node_sub - [row, col]); % 歐式距離，計算出代價
neighborNodes(1,2) = cost;
end
end
end
% 上節(jié)點
if row-1 > 0
child_node_sub = [row-1, col];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
neighborNodes(2,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
cost = norm(child_node_sub - [row, col]);
neighborNodes(2,2) = cost;
end
end
% 右上節(jié)點
if row-1 > 0 && col+1 <= cols
child_node_sub = [row-1, col+1];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
child_brother_node_sub1 = [row-1, col];
child_brother_node_sub2 = [row, col+1];
neighborNodes(3,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
cost = norm(child_node_sub - [row, col]);
neighborNodes(3,2) = cost;
end
end
end
% 左節(jié)點
if col-1 > 0
child_node_sub = [row, col-1];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
neighborNodes(4,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
cost = norm(child_node_sub - [row, col]);
neighborNodes(4,2) = cost;
end
end
% 右節(jié)點
if col+1 <= cols
child_node_sub = [row, col+1];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
neighborNodes(5,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
cost = norm(child_node_sub - [row, col]);
neighborNodes(5,2) = cost;
end
end
% 左下節(jié)點
if row+1 <= rows && col-1 > 0
child_node_sub = [row+1, col-1];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
child_brother_node_sub1 = [row, col-1];
child_brother_node_sub2 = [row+1, col];
neighborNodes(6,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
cost = norm(child_node_sub - [row, col]);
neighborNodes(6,2) = cost;
end
end
end
% 7.下節(jié)點
if row+1 <= rows
child_node_sub = [row+1, col];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
neighborNodes(7,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
cost = norm(child_node_sub - [row, col]);
neighborNodes(7,2) = cost;
end
end
% 8.右下節(jié)點
if row+1 <= rows && col+1 <= cols
child_node_sub = [row+1, col+1];
child_node_line = sub2ind([rows,cols], child_node_sub(1), child_node_sub(2));
child_brother_node_sub1 = [row, col+1];
child_brother_node_sub2 = [row+1, col];
neighborNodes(8,1) = child_node_line;
if field(child_node_sub(1), child_node_sub(2)) ~= 2
if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
cost = norm(child_node_sub - [row, col]);
neighborNodes(8,2) = cost;
end
end
end
5.3 Dijkstra.m
% 基于柵格地圖的機器人路徑規(guī)劃算法
% 第2節(jié)：Dijkstra算法
clc
clear
close all
%% 柵格界面、場景定義
% 行數(shù)和列數(shù)
rows = 20;
cols = 20;
[field,cmap] = defColorMap(rows, cols);
% 起點、終點、障礙物區(qū)域
startPos = 2;
goalPos = rows*cols -1 ;
field(startPos) = 4;
field(goalPos) = 5;
%% 算法初始化
% S/U的第一列表示柵格節(jié)點線性索引編號
% 對于S，第二列表示從源節(jié)點到本節(jié)點已求得的最小距離，不再變更；
% 對于U，第二列表示從源節(jié)點到本節(jié)點暫時求得的最小距離，可能會變更
U(:,1) = (1: rows*cols)';
U(:,2) = inf;
S = [startPos, 0];
U(startPos,:) = [];
% 更新起點的鄰節(jié)點及代價
neighborNodes = getNeighborNodes(rows, cols, startPos, field);
% for i = 1:4
for i = 1: 8
childNode = neighborNodes(i,1);
% 判斷該子節(jié)點是否存在
if ~isinf(childNode)
idx = find(U(:,1) == childNode);
U(idx,2) = neighborNodes(i,2);
end
end
% S集合的最優(yōu)路徑集合
for i = 1:rows*cols
path{i,1} = i;
end
% for i = 1:4
for i = 1: 8
childNode = neighborNodes(i,1);
if ~isinf(neighborNodes(i,2))
path{childNode,2} = [startPos,neighborNodes(i,1)];
end
end
%% 循環(huán)遍歷
while ~isempty(U)
% 在U集合找出當前最小距離值的節(jié)點,視為父節(jié)點，并移除該節(jié)點至S集合中
[dist_min, idx] = min(U(:,2));
parentNode = U(idx, 1);
S(end+1,:) = [parentNode, dist_min];
U(idx,:) = [];
% 獲得當前節(jié)點的臨近子節(jié)點
neighborNodes = getNeighborNodes(rows, cols, parentNode, field);
% 依次遍歷鄰近子節(jié)點，判斷是否在U集合中更新鄰節(jié)點的距離值
% for i = 1:4
for i = 1: 8
% 需要判斷的子節(jié)點
childNode = neighborNodes(i,1);
cost = neighborNodes(i,2);
if ~isinf(childNode) && ~ismember(childNode, S)
% 找出U集合中節(jié)點childNode的索引值
idx_U = find(childNode == U(:,1));
% 判斷是否更新
if dist_min + cost < U(idx_U, 2)
U(idx_U, 2) = dist_min + cost;
% 更新最優(yōu)路徑
path{childNode, 2} = [path{parentNode, 2}, childNode];
end
end
end
end
%% 畫柵格界面
% 找出目標最優(yōu)路徑
path_opt = path{goalPos,2};
% 給所有訪問過的節(jié)點上色
for i = 1:rows*cols
if ~isempty(path{i,2})
field((path{i,1})) = 7;
end
end
field(startPos) = 4;
field(goalPos) = 5;
field(path_opt(2:end-1)) = 6;
% 畫柵格圖
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
hold on;
% 畫出軌跡
[y0,x0] = ind2sub([rows,cols], path_opt);
y = y0 + 0.5;
x = x0 + 0.5;
plot(x,y,'-','Color','r','LineWidth',2.5);
hold on;
% 對軌跡進行平滑——貝塞爾曲線
points = [x',y'];
M = 1000;
[x1,y1] = bezir_n(points, M);
plot(x1,y1,'-','Color','y','LineWidth',2.5);
% 對軌跡進行平滑——spcrv
hold on;
values = spcrv([[x(1) x x(end)];[y(1) y y(end)]],3);
plot(values(1,:),values(2,:), 'b','LineWidth',2.5);
S/U集的內(nèi)容
| 第一列索引值 | 第二列索引值(代價值——距離) | 代表含義 |
|---|---|---|
| lnf | lnf | 未遍歷 |
| number | lnf | 障礙物 |
| number | number | 空地 |
5.4 實驗效果
Dijkstra 8鄰域
注： 黃色——起點
?????紫色——終點
?????白色——空地
?????黑色——障礙物
?????綠色——最終路徑
???? 可以看到，Dijkstra算法幾乎將所有能訪問的點都訪問了，其運算量較大，但同時能夠得到最優(yōu)解。
Dijkstra 4鄰域
???? 4鄰域走的路徑大多是直角，相比8鄰域，不太平滑。
???? 如下圖所示：8鄰域會出現(xiàn)這樣一種狀況。顯然在現(xiàn)實中這種走法會與障礙相碰撞。對此，需要進行相關(guān)約束。例如，當要走左下方向時，還需要保證其左節(jié)點和下節(jié)點不為障礙物。
改進后的效果
???? 可以看到不會再出現(xiàn)上述問題。但仍有一個缺陷——在部分轉(zhuǎn)角時，會發(fā)生轉(zhuǎn)向過大的問題，這對實際的無人車控制顯然是不太合理的，對此采用貝塞爾曲線進行平滑。
平滑后的效果
????紅色為未平滑路徑，綠色方塊為最終路徑，黃色為貝塞爾曲線擬合得到，藍色為spcrv函數(shù)平滑得到。
運行時長
柵格地圖大?。?0x20）
柵格地圖大小（30x30）
柵格地圖大?。?0x40）
????可以看到Dijkstra算法對于柵格地圖越大的情況，搜索時間越長，但其總耗時較長，不適用于實時的路徑規(guī)劃，不適用于局部路徑規(guī)劃，適用于全局路徑規(guī)劃。文章來源：http://www.zghlxwxcb.cn/news/detail-814083.html
聲明
本人所有文章僅作為自己的學(xué)習(xí)記錄，若有侵權(quán)，聯(lián)系立刪。文章來源地址http://www.zghlxwxcb.cn/news/detail-814083.html
到了這里，關(guān)于自動駕駛路徑規(guī)劃——Dijkstra算法的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
