- Dijkstra算法
迪傑斯特拉(Dijkstra)算法是典型的最短路徑的算法,由荷蘭計算機科學家迪傑斯特拉于1959年提出,用來求得從起始點到其他所有點最短路徑。該算法采用了貪心的思想,每次都查找與該點距離最近的點,也因為這樣,它不能用來解決存在負權邊的圖。解決的問題可描述為:在無向圖 G=(V,E) 中,假設每條邊 E[i] 的長度為 w[i],找到由頂點vs到其餘各點的最短路徑。
1) 基本思想:
通過Dijkstra計算圖G中的最短路徑時,需要指定起點vs(即從頂點vs開始計算)。此外,引進兩個集合S和U。S的作用是記錄已求出最短路徑的頂點(以及相應的最短路徑長度),而U則是記錄還未求出最短路徑的頂點(以及該頂點到起點vs的距離)。初始時,S中隻有起點vs;U中是除vs之外的頂點,并且U中頂點的路徑是"起點vs到該頂點的路徑"。然後,從U中找出路徑最短的頂點,并将其加入到S中;接着,更新U中的頂點和頂點對應的路徑。 然後,再從U中找出路徑最短的頂點,并将其加入到S中;接着,更新U中的頂點和頂點對應的路徑。重複該操作,直到周遊完所有頂點。
2) 算法步驟:
a.初始時,S隻包含源點,即S={vs},vs的距離為0。U包含除vs外的其他頂點,即U={其餘頂點},若u不是vs的出邊鄰接點,則<u,vs>權值為∞;
b.從U中選取一個距離vs最小的頂點k,把k加入S中(該標明的距離就是vs到k的最短路徑長度min);
c.以k為新考慮的中間點,修改U中各頂點的距離;若從源點vs到頂點u的距離(經過頂點k)比原來距離(不經過頂點k)短,則修改頂點u的距離值,即dist[u] = min( dist[u], min + w[k][u] );
d.重複步驟b和c直到所有頂點都包含在S中。
3) 算法執行個體:
先給出一個無向圖
用Dijkstra算法找出以A為起點的單源最短路徑步驟如下:
具體實作的代碼如下:
#include <stdio.h>
#include <string.h>
#define MAX_LEN 100
#define INFINITE 1000
typedef struct graph
{
int nodenum; // 頂點數
int edgenum; // 邊數
int matrix[MAX_LEN][MAX_LEN]; // 存儲圖的鄰接矩陣
}Graph;
typedef struct stack // 定義堆棧結構體類型
{
int top;
int printout[MAX_LEN];
}Stack;
void InitStack(Stack *s) // 初始化堆棧
{
s->top = -1; // 棧為空時棧頂指針top=-1
memset(s->printout,0,sizeof(int)*MAX_LEN); // 對記憶體塊進行置零
}
void push(Stack *s,int m) // 入棧
{
s->printout[++(s->top)] = m;
}
int pop(Stack *s) // 出棧
{
return s->printout[s->top--];
}
int in[MAX_LEN]; // 若in[i] = 1,則說明頂點vi已在集合S中
int dist[MAX_LEN]; // dist[i]是"頂點vs"到"頂點i"的最短路徑的長度
int prev[MAX_LEN]; // prev[i]的值是"頂點vs"到"頂點i"的最短路徑所經曆的全部頂點中,位于"頂點i"之前的那個頂點
void InitGraph(Graph *g,int n)
{
int i, j;
for(i = 1; i <= n; i++) // 初始化鄰接矩陣
{
for(j = 1; j <= n;j++)
{
if(i == j) g->matrix[i][j] = 0;
else g->matrix[i][j] = INFINITE;
}
}
for(i=1;i<=n;i++)
{
in[i] = 0;
dist[i] = INFINITE;
prev[i] = 0;
}
}
int main()
{
int n,m,i,I,J,weight,count,min,k,temp;
while(scanf("%d %d",&n,&m)) // 輸入頂點數和邊數
{
Graph mGraph;
mGraph.edgenum = m;
mGraph.nodenum = n;
InitGraph(&mGraph, n);
for(i = 0; i < m; i++)
{
scanf("%d %d %d", &I, &J, &weight);
mGraph.matrix[I][J] = weight; // 根據輸入填充鄰接矩陣
mGraph.matrix[J][I] = weight;
}
in[1] = 1; // 初始時隻包含源頂點1,即S={1}
prev[1] = 1;
dist[1] = 0; // 1到1的距離為0
for(i = 2; i <= n; i++)
{
dist[i] = mGraph.matrix[1][i]; // 其它各點到頂點vs的距離
if(dist[i] != INFINITE) prev[i] = 1;
}
count = 0;
while(count < n-1) // 周遊n-1次
{
min = INFINITE;
for(i = 1; i <= n; i++)
{
if(in[i] == 0 && dist[i] < min) // 查找最近點(從U中選取一個距離vs最小的頂點k)
{
min = dist[i];
k = i;
}
}
in[k] = 1; // 标記查找到的最近點,加入S中
for(i = 1; i <= n; i++) // update the distance
{
if( in[i]==0 && (min + mGraph.matrix[k][i])<dist[i] ) // 判斷是vs直接連v[i]短,還是經過v[k]連接配接v[i]更短
{
dist[i] = min + mGraph.matrix[k][i]; // 更新dist
prev[i] = k; // 記錄前驅頂點
}
}
count++;
}
Stack s;
for(i = 1; i <= n; i++)
{
temp = i;
InitStack(&s);
if(prev[temp] == 0)
{
printf("no path\n");
continue;
}
while(prev[temp] != 1)
{
push(&s, prev[temp]);
temp = prev[temp];
}
printf("1-->");
while(s.top != -1) // 判斷棧是否為空
{
printf("%d-->", pop(&s)); // 輸出最短路徑
}
printf("%d min length is %d\n", i, dist[i]);
}
}
getchar();
return 0;
}
View Code
輸入和輸出如下圖所示:
- A*算法
路徑規劃是指的是機器人的最優路徑規劃問題,即依據某個或某些優化準則(如工作代價最小、行走路徑最短、行走時間最短等),在工作空間中找到一個從起始狀态到目标狀态能避開障礙物的最優路徑。機器人的路徑規劃應用場景極豐富,最常見如遊戲中NPC及控制角色的位置移動,百度地圖等導航問題,小到家庭掃地機器人、無人機大到各公司正争相開拓的無人駕駛汽車等。
目前路徑規劃算法分為:
A*算法原理:
在計算機科學中,A*算法作為Dijkstra算法的擴充,因其高效性而被廣泛應用于尋路及圖的周遊,如星際争霸等遊戲中就大量使用。在了解算法前,我們需要知道幾個概念:
- 搜尋區域(The Search Area):圖中的搜尋區域被劃分為了簡單的二維數組,數組每個元素對應一個小方格,當然我們也可以将區域等分成是五角星,矩形等,通常将一個機關的中心點稱之為搜尋區域節點(Node)。
- 開放清單(Open List):我們将路徑規劃過程中待檢測的節點存放于Open List中,而已檢測過的格子則存放于Close List中。
- 父節點(parent):在路徑規劃中用于回溯的節點,開發時可考慮為雙向連結清單結構中的父結點指針。
- 路徑排序(Path Sorting):具體往哪個節點移動由以下公式确定:F(n) = G + H 。G代表的是從初始位置A沿着已生成的路徑到指定待檢測格子的移動開銷。H指定待測格子到目标節點B的估計移動開銷。
- 啟發函數(Heuristics Function):H為啟發函數,也被認為是一種試探,由于在找到唯一路徑前,我們不确定在前面會出現什麼障礙物,是以用了一種計算H的算法,具體根據實際場景決定。在我們簡化的模型中,H采用的是傳統的曼哈頓距離(Manhattan Distance),也就是橫縱向走的距離之和。
如下圖所示,綠色方塊為機器人起始位置A,紅色方塊為目标位置B,藍色為障礙物。
我們把要搜尋的區域劃分成了正方形的格子。這是尋路的第一步,簡化搜尋區域。這個特殊的方法把我們的搜尋區域簡化為了 2 維數組。數組的每一項代表一個格子,它的狀态就是可走 (walkalbe)或不可走 (unwalkable) 。現用A*算法尋找出一條自A到B的最短路徑,每個方格的邊長為10,即垂直水準方向移動開銷為10。是以沿對角移動開銷約等于14。具體步驟如下:
從起點 A 開始,把它加入到一個由方格組成的open list(開放清單) 中,這個open list像是一個購物清單。Open list裡的格子是可能會是沿途經過的,也有可能不經過。是以可以将其看成一個待檢查的清單。檢視與A相鄰的8個方格 ,把其中可走的 (walkable) 或可到達的(reachable) 方格加入到open list中。并把起點 A 設定為這些方格的父節點 (parent node) 。然後把 A 從open list中移除,加入到close list(封閉清單) 中,close list中的每個方格都是不需要再關注的。
如下圖所示,深綠色的方格為起點A,它的外框是亮藍色,表示該方格被加入到了close list 。與它相鄰的黑色方格是需要被檢查的,他們的外框是亮綠色。每個黑方格都有一個灰色的指針指向他們的父節點A。
下一步,我們需要從open list中選一個與起點A相鄰的方格。但是到底選擇哪個方格好呢?選F值最小的那個。我們看看下圖中的一些方格。在标有字母的方格中G = 10 。這是因為水準方向從起點到那裡隻有一個方格的距離。與起點直接相鄰的上方,下方,左方的方格的 G 值都是 10 ,對角線的方格 G 值都是14 。H值通過估算起點到終點 ( 紅色方格 ) 的 Manhattan 距離得到,僅作橫向和縱向移動,并且忽略沿途的障礙。使用這種方式,起點右邊的方格到終點有3 個方格的距離,是以 H = 30 。這個方格上方的方格到終點有 4 個方格的距離 ( 注意隻計算橫向和縱向距離 ) ,是以 H = 40 。
比較open list中節點的F值後,發現起點A右側節點的F=40,值最小。選作目前處理節點,并将這個點從Open List删除,移到Close List中。
對這個節點周圍的8個格子進行判斷,若是不可通過(比如牆,水,或是其他非法地形)或已經在Close List中,則忽略。否則執行以下步驟:
- 若目前處理節點的相鄰格子已經在Open List中,則檢查這條路徑是否更優,即計算經由目前處理節點到達那個方格是否具有更小的 G值。如果沒有,不做任何操作。相反,如果G值更小,則把那個方格的父節點設為目前處理節點 ( 我們選中的方格 ) ,然後重新計算那個方格的 F 值和 G 值。
- 若目前處理節點的相鄰格子不在Open List中,那麼把它加入,并将它的父節點設定為該節點。
按照上述規則我們繼續搜尋,選擇起點右邊的方格作為目前處理節點。它的外框用藍線打亮,被放入了close list 中。然後我們檢查與它相鄰的方格。它右側的3個方格是牆壁,我們忽略。它左邊的方格是起點,在 close list 中,我們也忽略。其他4個相鄰的方格均在 open list 中,我們需要檢查經由目前節點到達那裡的路徑是否更好。我們看看上面的方格,它現在的G值為14 ,如果經由目前方格到達那裡, G值将會為20( 其中10為從起點到達目前方格的G值,此外還要加上從目前方格縱向移動到上面方格的G值10) ,是以這不是最優的路徑。看圖就會明白直接從起點沿對角線移動到那個方格比先橫向移動再縱向移動要好。
當把4個已經在 open list 中的相鄰方格都檢查後,沒有發現經由目前節點的更好路徑,是以不做任何改變。接下來要選擇下一個待處理的節點。是以再次周遊open list ,現在open list中隻有 7 個方格了,我們需要選擇F值最小的那個。這次有兩個方格的F值都是54,選哪個呢?沒什麼關系。從速度上考慮,選擇最後加入 open list 的方格更快。是以選擇起點右下方的方格,如下圖所示。
接下來把起點右下角F值為54的方格作為目前處理節點,檢查其相鄰的方格。我們發現它右邊是牆(牆下面的一格也忽略掉,假定牆角不能直接穿越),忽略之。這樣還剩下 5 個相鄰的方格。目前方格下面的 2 個方格還沒有加入 open list ,是以把它們加入,同時把目前方格設為他們的父親。在剩下的 3 個方格中,有 2 個已經在 close list 中 ( 一個是起點,一個是目前方格上面的方格,外框被加亮的 ) ,我們忽略它們。最後一個方格,也就是目前方格左邊的方格,檢查經由目前方格到達那裡是否具有更小的 G 值。沒有,是以我們準備從 open list 中選擇下一個待處理的方格。
不斷重複這個過程,直到把終點也加入到了 open list 中,此時如下圖所示。注意在起點下方 2 格處的方格的父親已經與前面不同了。之前它的G值是28并且指向它右上方的方格。現在它的 G 值為 20 ,并且指向它正上方的方格。這是由于在尋路過程中的某處使用新路徑時G值更小,是以父節點被重新設定,G和F值被重新計算。
那麼我們怎樣得到實際路徑呢?很簡單,如下圖所示,從終點開始,沿着箭頭向父節點移動,直至回到起點,這就是你的路徑。
A*算法總結:
1. 把起點加入 open list 。
2. 重複如下過程:
a. 周遊open list ,查找F值最小的節點,把它作為目前要處理的節點,然後移到close list中
b. 對目前方格的 8 個相鄰方格一一進行檢查,如果它是不可抵達的或者它在close list中,忽略它。否則,做如下操作:
□ 如果它不在open list中,把它加入open list,并且把目前方格設定為它的父親
□ 如果它已經在open list中,檢查這條路徑 ( 即經由目前方格到達它那裡 ) 是否更近。如果更近,把它的父親設定為目前方格,并重新計算它的G和F值。如果你的open list是按F值排序的話,改變後你可能需要重新排序。
c. 遇到下面情況停止搜尋:
□ 把終點加入到了 open list 中,此時路徑已經找到了,或者
□ 查找終點失敗,并且open list 是空的,此時沒有路徑。
3. 從終點開始,每個方格沿着父節點移動直至起點,形成路徑。
根據算法描述,僞代碼如下:
function A*(start, goal)
// The set of nodes already evaluated.
closedSet := {}
// The set of currently discovered nodes still to be evaluated. Initially, only the start node is known.
openSet := {start}
// For each node, which node it can most efficiently be reached from.
// If a node can be reached from many nodes, cameFrom will eventually contain the most efficient previous step.
cameFrom := the empty map
// For each node, the cost of getting from the start node to that node.
gScore := map with default value of Infinity
// The cost of going from start to start is zero.
gScore[start] := 0
// For each node, the total cost of getting from the start node to the goal
// by passing by that node. That value is partly known, partly heuristic.
fScore := map with default value of Infinity
// For the first node, that value is completely heuristic.
fScore[start] := heuristic_cost_estimate(start, goal)
while openSet is not empty
current := the node in openSet having the lowest fScore[] value
if current = goal
return reconstruct_path(cameFrom, current)
openSet.Remove(current)
closedSet.Add(current)
for each neighbor of current
if neighbor in closedSet
continue // Ignore the neighbor which is already evaluated.
// The distance from start to a neighbor
tentative_gScore := gScore[current] + dist_between(current, neighbor)
if neighbor not in openSet // Discover a new node
openSet.Add(neighbor)
else if tentative_gScore >= gScore[neighbor]
continue // This is not a better path.
// This path is the best until now. Record it!
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
return failure
function reconstruct_path(cameFrom, current)
total_path := [current]
while current in cameFrom.Keys:
current := cameFrom[current]
total_path.append(current)
return total_path
根據上面的僞代碼,用Python實作一個簡單的最短路徑搜尋。使用二維數組表示地圖,其中1表示有障礙節點,0表示無障礙節點。
import numpy
from heapq import heappush,heappop
def heuristic_cost_estimate(neighbor, goal):
x = neighbor[0] - goal[0]
y = neighbor[1] - goal[1]
return abs(x) + abs(y)
def dist_between(a, b):
return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2
def reconstruct_path(came_from, current):
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
return path
# astar function returns a list of points (shortest path)
def astar(array, start, goal):
directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] # 8個方向
close_set = set()
came_from = {}
gscore = {start:0}
fscore = {start:heuristic_cost_estimate(start, goal)}
openSet = []
heappush(openSet, (fscore[start], start)) # 往堆中插入一條新的值
# while openSet is not empty
while openSet:
# current := the node in openSet having the lowest fScore value
current = heappop(openSet)[1] # 從堆中彈出fscore最小的節點
if current == goal:
return reconstruct_path(came_from, current)
close_set.add(current)
for i, j in directions: # 對目前節點的 8 個相鄰節點一一進行檢查
neighbor = current[0] + i, current[1] + j
## 判斷節點是否在地圖範圍内,并判斷是否為障礙物
if 0 <= neighbor[0] < array.shape[0]:
if 0 <= neighbor[1] < array.shape[1]:
if array[neighbor[0]][neighbor[1]] == 1: # 1為障礙物
continue
else:
# array bound y walls
continue
else:
# array bound x walls
continue
# Ignore the neighbor which is already evaluated.
if neighbor in close_set:
continue
# The distance from start to a neighbor via current
tentative_gScore = gscore[current] + dist_between(current, neighbor)
if neighbor not in [i[1] for i in openSet]: # Discover a new node
heappush(openSet, (fscore.get(neighbor, numpy.inf), neighbor))
elif tentative_gScore >= gscore.get(neighbor, numpy.inf): # This is not a better path.
continue
# This path is the best until now. Record it!
came_from[neighbor] = current
gscore[neighbor] = tentative_gScore
fscore[neighbor] = tentative_gScore + heuristic_cost_estimate(neighbor, goal)
return False
if __name__ == "__main__":
nmap = numpy.array([
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,0,1,1,1,1,1,1,1,1,1,1,1,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,0,1,1,1,1,1,1,1,1,1,1,1,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0],
[1,1,1,1,1,1,1,1,1,1,1,1,0,1],
[0,0,0,0,0,0,0,0,0,0,0,0,0,0]])
path = astar(nmap, (0,0), (10,13))
for i in range(len(path)):
nmap[path[i]] = 100
使用Spyder IDE可以在Variable explorer中檢視和修改二維數組,數組中的值根據大小以不同顔色顯示。将搜尋到的路徑節點賦予一個較大的值,可以直覺地看出從起點到終點的路徑。
使用Pillow、OpenCV或Matplotlib等圖像處理庫,可以在自己繪制的圖檔上進行尋路:
# -*- coding: utf-8 -*-
import numpy as np
from heapq import heappush,heappop
def heuristic_cost_estimate(neighbor, goal):
x = neighbor[0] - goal[0]
y = neighbor[1] - goal[1]
return abs(x) + abs(y)
def dist_between(a, b):
return (b[0] - a[0]) ** 2 + (b[1] - a[1]) ** 2
def reconstruct_path(came_from, current):
path = [current]
while current in came_from:
current = came_from[current]
path.append(current)
return path
# astar function returns a list of points (shortest path)
def astar(array, start, goal):
directions = [(0,1),(0,-1),(1,0),(-1,0),(1,1),(1,-1),(-1,1),(-1,-1)] # 8個方向
close_set = set()
came_from = {}
gscore = {start:0}
fscore = {start:heuristic_cost_estimate(start, goal)}
openSet = []
heappush(openSet, (fscore[start], start)) # 往堆中插入一條新的值
# while openSet is not empty
while openSet:
# current := the node in openSet having the lowest fScore value
current = heappop(openSet)[1] # 從堆中彈出fscore最小的節點
if current == goal:
return reconstruct_path(came_from, current)
close_set.add(current)
for i, j in directions: # 對目前節點的 8 個相鄰節點一一進行檢查
neighbor = current[0] + i, current[1] + j
## 判斷節點是否在地圖範圍内,并判斷是否為障礙物
if 0 <= neighbor[0] < array.shape[0]:
if 0 <= neighbor[1] < array.shape[1]:
if array[neighbor[0]][neighbor[1]] == 0: # 0為障礙物
continue
else:
# array bound y walls
continue
else:
# array bound x walls
continue
# Ignore the neighbor which is already evaluated.
if neighbor in close_set:
continue
# The distance from start to a neighbor via current
tentative_gScore = gscore[current] + dist_between(current, neighbor)
if neighbor not in [i[1] for i in openSet]: # Discover a new node
heappush(openSet, (fscore.get(neighbor, np.inf), neighbor))
elif tentative_gScore >= gscore.get(neighbor, np.inf): # This is not a better path.
continue
# This path is the best until now. Record it!
came_from[neighbor] = current
gscore[neighbor] = tentative_gScore
fscore[neighbor] = tentative_gScore + heuristic_cost_estimate(neighbor, goal)
return False
if __name__ == "__main__":
from PIL import Image
import matplotlib.pyplot as plt
img = Image.open('C:\Users\Administrator\Desktop\map.bmp')
map = np.array(img) # 圖像轉化為二維數組
path = astar(map, (0,0), (260,260))
# 繪制路徑
img = np.array(img.convert('RGB'))
for i in range(len(path)):
img[path[i]] = [0,0,255]
plt.imshow(img)
plt.axis('off')
plt.show()
在畫圖程式中繪制一個300×300像素的地圖,填充黑色表示障礙,并将其存為灰階圖。計算出路徑後轉換為彩色圖,并繪制出路線:
上面産生的路徑貼着障礙物邊緣,如果對實際機器人或者遊戲中的角色進行路徑規劃,要考慮其實際大小,将地圖上的障礙物尺寸擴大(inflate),避免與障礙物發生碰撞。
參考:
Dijkstra算法(一)之 C語言詳解
最短路徑—Dijkstra算法和Floyd算法
A*,Dijkstra,BFS算法性能比較及A*算法的應用
用python實作基本A*算法
http://theory.stanford.edu/~amitp/GameProgramming/
Introduction to A*
implementation guide
PYTHON A* PATHFINDING (WITH BINARY HEAP)
http://www.policyalmanac.org/games/aStarTutorial.htm
A-Star (A*) Implementation in C# (Path Finding, PathFinder)
Implementing A Star (A*) Path Planning Algorithm On A 3-DOF PR2 Robot In Python