引言
在一張固定地圖上選擇一條路徑，當存在多條可選的路徑之時，需要選擇代價最小的那條路徑。我們稱這類問題為最短路徑的選擇問題。解決這個問題最經典的演算法為Dijikstra演算法，其通過貪心選擇的步驟從源點出發逐步逼近目標點，從而得到起始點與目標點的最短路徑。A*演算法是在Dijikstra演算法上做了改進，使其能夠在 開闊空間(也就是四通八達或具有少量障礙物的方格路，可以近似看成各邊權重均相等的完全圖) 上具有比Dijikstra演算法有更好的搜尋效率。 但Dijikstra演算法和A*演算法無法很好的適用動態路網的情況，D*演算法卻能夠很好的適應。Dijikstra演算法和A*演算法在動態路網中進行重新規劃所花費的代價比D*演算法高，且效率比D*的低。
ps: 動態路網是指在沿著最優路徑前進時，突然出現既定路徑中斷(地圖上的路徑變更等)需要進行重新規劃的情況。
Dijikstra演算法
Dijikstra演算法是單向最短路徑的查詢演算法，其要求對應的圖必須無負權。其適用有向圖和無向圖的情況(一般會將無向圖的每條邊看成兩條反向的有向邊)。Dijikstra演算法是一個貪心演算法，每一步都是基於貪心選擇策略來進行。
演算法描述： 對於具有V個頂點和U條邊的圖 E=(U,V),演算法會將圖的頂點V分為兩部分，一部分是已遍歷過且已找到從源點source到目標點的最短路徑的節點集合S。一部分是未遍歷過的節點集合K。每次遍歷過程都會從未找到最短路徑的節點集合K中取出距離源點路徑最短的節點n。然後遍歷該節點的連通節點i，若該節點的連通節點I不存在於集合S中，且 dist[source][i] > dist[source][n] + graph[n][i]; 則更新dist[source][i] = dist[source][n] + graph[n][i]，並標註節點i最短路徑中，其上一個節點為n，即path[i] = n，且將節點n從集合K中刪除，並將其放入集合S中。然後進入下一次迴圈，直至將目標節點放入集合S中，或遍歷了圖中的全部節點。
function dijikstra(graph,source,target):
initDist(dist,graph.nodeSize,graph.nodeSize,Integer.MAX_VALUE);
dist[source][source] <- 0;
initPath(path,pathNodeSelfIndex);
queue.push(source);
while(!queue.isEmpty()):
// 得到佇列中當前dist值最小的節點
node <- queue.pop(sorted by dist[source][obj]);
S.push(node);
if node == target:
break;
for neighborNode in node:
if !S.contains(neighborNode) and dist[source][neighborNode] > dist[source][node] + graph[node][neighborNode]:
dist[source][neighborNode] = dist[source][node] + graph[node][neighborNode];
path[neighborNode] = node;
queue.push(new SearchNode(neighborNode,dist[source][neighborNode]));
return path,dist;
實現:
import lombok.AllArgsConstructor;
import lombok.Data;
import lombok.NoArgsConstructor;
import java.util.*;
/**
* @author 學徒
*/
public class Dijikstra {
/**
* 圖最短路徑查詢結果
*/
@AllArgsConstructor
@NoArgsConstructor
@Data
public class SearchResult {
/**
* 最短路徑查詢的路徑大小結果儲存
*/
private int[][] dist;
/**
* 最短路徑查詢的路徑節點過程結果儲存
*/
private int[] path;
/**
* 源節點
*/
private int source;
/**
* 目標節點
*/
private int target;
/**
* 列印出最短路徑查詢結果
*/
public void print() {
System.out.print("路徑查詢:" + source + "->" + target + ":");
int temp = target;
while (path[temp] != temp) {
System.out.print(temp + "<-");
temp = path[temp];
}
System.out.println(temp);
System.out.println("路徑長度:" + (Objects.equals(dist[source][target], Integer.MAX_VALUE) ? "無限長，節點不可達" : dist[source][target]));
}
}
/**
* 實現dijikstra演算法
*
* @param graph 由領接矩陣表示的圖，值表示邊權重
* @param source 查詢的起始點
* @param target 查詢的目標點
* @return 查詢的結果
*/
public SearchResult search(int[][] graph, int source, int target) {
if (Objects.isNull(graph) || graph.length == 0 || graph[0].length != graph.length) {
throw new IllegalArgumentException("圖存在節點數目異常問題");
}
if (source < 0 || target < 0 || source > graph.length - 1 || target > graph.length - 1) {
throw new IllegalArgumentException("查詢點引數存在異常");
}
// 記錄兩點之間路徑的最短距離
int[][] dist = new int[graph.length][graph.length];
for (int[] temp : dist) {
Arrays.fill(temp, Integer.MAX_VALUE);
}
dist[source][source] = 0;
// 用於暫時儲存最短路徑節點編號，且維護當前最短距離的最小值節點編號
Queue<Integer> queue = new PriorityQueue<>(Comparator.comparing((obj) -> dist[source][obj]));
// 用於儲存已經查詢到最短路徑的節點的編號
Set<Integer> set = new HashSet<>();
// 用於記錄最短路徑中某個節點的上一個節點編號
int[] path = new int[graph.length];
for (int i = 0; i < graph.length; i++) {
path[i] = i;
}
queue.add(source);
while (!queue.isEmpty()) {
Integer node = queue.poll();
set.add(node);
// 查詢到目標節點時，
if (node == target) {
break;
}
for (int neighborNode : getNeighborNodes(graph, node)) {
int distance = dist[source][node] + graph[node][neighborNode];
if (set.contains(neighborNode) || dist[source][neighborNode] < distance) {
continue;
}
dist[source][neighborNode] = distance;
path[neighborNode] = node;
queue.add(neighborNode);
}
}
return new SearchResult(dist, path, source, target);
}
/**
* 用於得到某個圖中某個節點的連通節點的節點編號
*
* @param nodeNumber 要查詢連通節點的編號
* @return 連通節點的列表
*/
private List<Integer> getNeighborNodes(int[][] graph, int nodeNumber) {
if (Objects.isNull(graph) || nodeNumber > graph.length - 1 || nodeNumber < 0) {
return new ArrayList<>();
}
List<Integer> result = new LinkedList<>();
for (int temp = 0; temp < graph.length; temp++) {
if (Objects.equals(graph[nodeNumber][temp], Integer.MAX_VALUE) || Objects.equals(nodeNumber, temp)) {
continue;
}
result.add(temp);
}
return result;
}
}
分析： Dijikstra演算法的時間複雜度為O(\(n^2\))，空間複雜度為O(\(n^2\))。其中n為圖中節點的數目。
總結： Dijikstra演算法能夠很好的實現起始點到目標點之間最短距離的搜尋。但是其要求圖中不能有負權邊。且當圖為開放空間，甚至是各邊權重均相等的條件下，效率會較差。
A*演算法
Dijikstra演算法在開闊空間中查詢源點到目標點的最短路徑之時，其執行過程類似於以源點為中心，源點到目標點的距離為半徑，進行廣度搜尋的狀態。也就是從源點一層一層的向外擴的方式進行查詢，直至找到目標點停止。出現這個問題的根本原因在於每次從未遍歷過的節點集合K中取出要進行訪問的節點時，只考慮距離源節點最近的節點，且與源節點之間距離最短的節點數目較多，沒有考慮取出的節點與目標節點之間的距離的問題。 而使得每次取出的節點都無法很好的逼近目標節點，只是“盲目”的在圖中向目標節點搜尋。A*演算法在Dijikstra演算法的基礎上進行改進，其引入了一個評估函式。f(x) = g(x) + h(x)。其中g(x)是從起點到當前節點x的實際距離量度，也就是在Dijikstra演算法中，源點到當前節點的最短距離dist[source][x]。h(x)是從節點x到終點的最小距離估計，h(x)可以從歐幾里得距離或者曼哈頓距離中選取，也可以是其它的距離度量。 ，因此我們可知，f(x)綜合考慮了節點x與源節點以及目標節點之間的距離關係。A*演算法相較於Dijikstra演算法，在其它步驟上都沒有任何變化，只是在每次從未訪問過的節點集合K中取出節點進行訪問時，判斷的依據從原先的與源節點距離最短的節點變為f(x)中值最小的那個節點。
function AStar(graph,source,target):
initDist(dist,graph.nodeSize,graph.nodeSize,Integer.MAX_VALUE);
dist[source][source] <- 0;
queue.push(new SearchNode(source));
while(!queue.isEmpty()):
// 得到佇列中當前dist值最小的節點
node <- queue.pop(sorted by obj.f(target,dist[source][obj]));
S.push(node.nodeNumber);
if node.nodeNumber == target:
break;
for neighborNode in getNeighbors(node.nodeNumber):
if !S.contains(neighborNode) and dist[source][neighborNode] > dist[source][node.nodeNumber] + graph[node.nodeNumber][neighborNode]:
dist[source][neighborNode] = dist[source][node.nodeNumber] + graph[node.nodeNumber][neighborNode];
path[neighborNode] = node.nodeNumber;
queue.push(new SearchNode(neighborNode));
return path,dist;
struct SearchNode{
int nodeNumber;
function f(target,distance);
}
實現:
import lombok.AllArgsConstructor;
import lombok.Data;
import lombok.NoArgsConstructor;
import java.util.*;
/**
* @author 學徒
*/
public class AStar {
/**
* 用於輔助實現最短路徑的查詢節點
*/
@AllArgsConstructor
@NoArgsConstructor
@Data
private class SearchNode {
/**
* 節點編號
*/
private int nodeNumber;
/**
* 用於得到評估函式的值
*
* @param distance 從源節點到當前節點的最短距離
* @param targetNodeNumber 目標節點編號
* @return 評估函式的值
*/
public int f(int distance, int targetNodeNumber) {
int gx = distance;
// 這裡評估與目標節點的距離為編號差
int hx = Math.abs(nodeNumber - targetNodeNumber);
return gx + hx;
}
}
/**
* 圖最短路徑查詢結果
*/
@AllArgsConstructor
@NoArgsConstructor
@Data
public class SearchResult {
/**
* 最短路徑查詢的路徑大小結果儲存
*/
private int[][] dist;
/**
* 最短路徑查詢的路徑節點過程結果儲存
*/
private int[] path;
/**
* 源節點
*/
private int source;
/**
* 目標節點
*/
private int target;
/**
* 列印出最短路徑查詢結果
*/
public void print() {
System.out.print("路徑查詢:" + source + "->" + target + ":");
int temp = target;
while (path[temp] != temp) {
System.out.print(temp + "<-");
temp = path[temp];
}
System.out.println(temp);
System.out.println("路徑長度:" + (Objects.equals(dist[source][target], Integer.MAX_VALUE) ? "無限長，節點不可達" : dist[source][target]));
}
}
/**
* 實現dijikstra演算法
*
* @param graph 由領接矩陣表示的圖，值表示邊權重
* @param source 查詢的起始點
* @param target 查詢的目標點
* @return 查詢的結果
*/
public SearchResult search(int[][] graph, int source, int target) {
if (Objects.isNull(graph) || graph.length == 0 || graph[0].length != graph.length) {
throw new IllegalArgumentException("圖存在節點數目異常問題");
}
if (source < 0 || target < 0 || source > graph.length - 1 || target > graph.length - 1) {
throw new IllegalArgumentException("查詢點引數存在異常");
}
// 記錄兩點之間路徑的最短距離
int[][] dist = new int[graph.length][graph.length];
for (int[] temp : dist) {
Arrays.fill(temp, Integer.MAX_VALUE);
}
dist[source][source] = 0;
// 用於暫時儲存最短路徑節點，且維護當前f(x)的最小值節點
Queue<SearchNode> queue = new PriorityQueue<>(Comparator.comparing((obj) -> obj.f(dist[source][obj.nodeNumber], target)));
// 用於儲存已經查詢到最短路徑的節點的編號
Set<Integer> set = new HashSet<>();
// 用於記錄最短路徑中某個節點的上一個節點編號
int[] path = new int[graph.length];
for (int i = 0; i < graph.length; i++) {
path[i] = i;
}
queue.add(new SearchNode(source));
while (!queue.isEmpty()) {
SearchNode node = queue.poll();
set.add(node.nodeNumber);
// 查詢到目標節點時，
if (node.nodeNumber == target) {
break;
}
for (int neighborNode : getNeighborNodes(graph, node.nodeNumber)) {
int distance = dist[source][node.nodeNumber] + graph[node.nodeNumber][neighborNode];
if (set.contains(neighborNode) || dist[source][neighborNode] < distance) {
continue;
}
dist[source][neighborNode] = distance;
path[neighborNode] = node.nodeNumber;
queue.add(new SearchNode(neighborNode));
}
}
return new SearchResult(dist, path, source, target);
}
/**
* 用於得到某個圖中某個節點的連通節點的節點編號
*
* @param nodeNumber 要查詢連通節點的編號
* @return 連通節點的列表
*/
private List<Integer> getNeighborNodes(int[][] graph, int nodeNumber) {
if (Objects.isNull(graph) || nodeNumber > graph.length - 1 || nodeNumber < 0) {
return new ArrayList<>();
}
List<Integer> result = new LinkedList<>();
for (int temp = 0; temp < graph.length; temp++) {
if (Objects.equals(graph[nodeNumber][temp], Integer.MAX_VALUE) || Objects.equals(nodeNumber, temp)) {
continue;
}
result.add(temp);
}
return result;
}
}
分析： A*演算法不一定能夠得到最優解。當h*(x)表示節點x到達目標節點的真實代價。那麼存在如下四種情況：
- 如果h(x) = 0,這種情況下，A*演算法變為了Dijikstra演算法
- 如果h(x) < h*(x),這種情況下，搜尋的點數多，搜尋範圍大，效率低。但能得到最優解
- 如果h(x) = h*(x)，此時的搜尋效率是最高的。
- 如果h(x) > h*(x)，搜尋的點數少，搜尋範圍小，效率高，但不能保證得到最優解。
總結： A*演算法是一種啟發式搜尋演算法，h(x)作為搜尋代價的啟發函式，其影響著搜尋的精確度和效率。對於A*演算法，其能夠適應靜態路網的情況，而對於動態路網的情況，其不能很好的適應。
D*演算法
D*演算法是一種增量式的路徑搜尋演算法，適合面對周圍環境未知或者周圍環境存在動態變化的場景，但也能相容靜態環境，與A*演算法不同的是，A*演算法從起點向目標點進行搜尋，而D*演算法是從目標點向起始點進行反向搜尋。
ps: 啟發式搜尋是利用啟發函式來對搜尋進行指導，從而實現高效的搜尋。增量搜尋是對以前的搜尋結果資訊進行再利用來實現高效搜尋，大大減少搜尋範圍和時間。D*演算法採用反向搜尋的目的在於後期需要重新規劃路徑的時候，能夠用到先前搜尋到的最短路徑資訊，減少搜尋量。因為以目標向起始點進行搜尋得到的最短路徑圖，是以目標點為中心輻射出的最短路徑圖，圖上目標點到各點之間都是最短路徑，為此其在既定路徑上遇到問題需要重新路徑規劃的時候，可以很好的利用原先得到的資訊。而以起始點向目標點搜尋得到的最短路徑圖，其是以起始點為中心輻射出的最短路徑圖，當沿著既定路徑前行遇到障礙物之後，需要重新進行路徑規劃之時，沒有辦法很好的利用原先搜尋得到的資訊。
演算法描述：
D*演算法有幾個重要的概念：
-
G：表示進行路徑搜尋的目標點
-
c(x,y)：從節點x移動到節點y的代價
-
t(x)：節點的狀態。每個節點(作者論文中稱為state)都有一個狀態，其中總共有三種可能NEW,OPEN,CLOSED。NEW表示從未加入到openList中的，也就是從未被遍歷查詢過的。OPEN表示節點在被查詢中，節點在openList中。CLOSED表示從openList中被移出。OPEN和CLOSED狀態會相互轉化，當節點從openList中移出的時候，狀態從OPEN變為CLOSED，當節點再次加入openList，進行 降低節點自身h值或者傳播當前節點的h值變更資訊給鄰居節點，讓鄰居節點進行h值修改變更 操作時，狀態從CLOSED變為OPEN
-
h(x)：表示地圖上的點x到達目標點G的代價。由於D*演算法是從目標點開始進行路徑規劃的，為此，初始化的時候，令h(G) = 0。此後，其代價的變動可能會在兩個地方出現，一個是路徑搜尋的過程中，當節點x的鄰居節點被執行搜尋過程的時候，如果其能夠讓h(x)的代價更小，則更新h(x) = h(y) + cost(y,x)。一個是在路徑搜尋完成後執行路徑搜尋結果的過程中遇到障礙物之時，通過insert(x,y,val)函式改變障礙物節點y的代價h(y)
-
k(x)：可以理解為節點最小的h(x)值。隨著節點遍歷和搜尋過程的進行，節點的h(x)值會不斷的變動
-
b(x) = y：用於記錄當前節點x的父節點，這裡b(x) = y表示x節點的父節點為y節點。在搜尋完成之後，能夠根據b(x)的值，回溯追蹤到目標節點
-
openList：存放節點狀態為OPEN的節點。且其按照節點的k值從小到大進行排序
D*演算法有三個重要的函式：
-
process_state()：該函式是用來降低openlist表中的某個節點x(state)的h(x)值或者傳播節點x的h(x)值變更資訊給鄰居節點，讓鄰居節點進行相應變更的同時進行路徑搜尋查詢的
-
insert(x,val)：該函式是用來修改節點x的狀態以及h(x)值和k(x)值的
-
modify_cost(x,y,val)：該函式是用來修改節點x和y之間的移動代價cost(x,y)，而且根據節點y的狀態t(y)的情況，可能對節點y的h(y)值和狀態t(y)進行修改的
D*演算法的執行過程：
-
在演算法搜尋路徑的起始階段，其操作過程類似於Dijikstra演算法，但是其是從目標點往起始點進行搜尋的過程。首先將t(G) = OPEN，h(G) = 0，k(G) = 0。並將點G放入openList中，同時將其餘各點的t(x) = NEW,k(x) = MAX_VALUE,h(x) = MAX_VALUE（實際上，當t(x)值為NEW時，h(x)和k(x)的值並不重要，但為了統一process_state函式的描述，這裡用了MAX_VALUE表示最大值）。之後反覆執行process_state()函式，用於搜尋路徑，直到起始點x的狀態變為CLOSED或openList中已經沒有節點。路徑搜尋結束，當存在路徑時，可以根據b(x)不斷回溯上一個節點，直到目標點
-
搜尋了最短路徑之後，在根據b(x)進行路徑執行的過程中，若當前點為x，且探測到要走的下一個節點y存在障礙，或者節點y本來存在障礙物，但是繼續行進到x的時候，障礙物被移除。這時就要呼叫modify_cost(x,y,val)函式，將最新的cost(x,y)以及h(y)的值進行變更，並反覆執行process_state()函式用於傳播節點h值變更資訊和路徑搜尋直到process_state()函式返回的openList中所有節點的k值中的最小k值\(k_{min}\) >= h(x) 或者openList沒有任何節點時，表示重新路徑規劃完成，或者無法找到別的路徑從x點規劃到目標點G
D*演算法的幾個說明：
-
為何有了h值還需要k值：在靜態環境下，按照h值進行排序然後執行演算法是可以找到一條全域性最優路徑的(其相當於Dijkstra演算法)。但是在動態環境下，假如某個節點變成了障礙物，變得不可達，此時其h值會被修改為\(\infty\)，這時將該節點假如openList中進行重新規劃時，該節點會被置於openList中的最後。也就是說，此時路徑規劃會從openList中剩餘的處於OPEN狀態的節點開始，一直擴張至全圖都沒有不可達節點之後，才會訪問該節點。這顯然並不合理，因為我們的目的就是要在節點狀態動態變化的時候減少搜尋的空間，提高搜尋效率。而用最小的h值也就是k值在openList中進行排序，表示這裡曾有一條捷徑，那麼就會優先在這附近進行搜尋。
-
為何重新進行路徑規劃的時候，當執行到 \(k_{min}\) >= h(y) 時停止：因為process_state()函式的功能是用鄰居節點來減低自身節點的h值的，當所有處於OPEN狀態的節點中的k值的最小值 \(k_{min}\) 也要大於等於h(y)時，表示不可能再通過process_state()函式的執行來降低h(y)值了，那麼自然就沒有再搜尋的必要，且已經完成了路徑的修正了。
相關虛擬碼如下：
function process_state():
x = openList.poll(sortedBy k);
if(x == NULL):
return -1;
x.state = CLOSED;
// 當h值大於k值時，表示當前該節點處於h值被修改為較大的狀態(raise狀態)
// 為此查詢鄰居節點來得到減低自身的h值
if(x.k < x.h):
for each neighbor y of x:
if(y.h < x.k and x.h > y.h + c(y,x)):
x.b = y;
x.h = y.h + c(y,x);
// 該過程類似於dijikstra，用來傳播資訊當前節點h值變化的資訊和降低鄰居節點的h值
if(x.k == x.h):
for each neighbor y of x:
if(y.state == NEW or
(y.b == x and y.h != x.h + c(x,y)) or
(y.b != x and y.h > x.h + c(x,y))):
y.b = x;
insert(y,x.h + c(x,y))
// k值和h值不相同，表示節點處於調整狀態
else:
for each neighbor y of x:
// 傳播資訊當前節點h值變化的資訊和降低鄰居節點的h值
if(y.state == NEW or
(y.b == x and y.h != x.h + c(x,y))):
y.b = x;
insert(y,x.h + c(x,y));
else:
// 鄰居節點存在更短的路徑
// 調整當前節點並重新傳播當前節點的h值變化資訊給周圍節點
if(y.b != x and y.h > x.h + c(x,y)):
insert(x,x.h);
// 傳播鄰居節點的資訊，使其可以影響當前節點進而修改當前節點的h值和路徑資訊、
// 因為這裡存在比當前節點的h值更低的值
else:
if(y.b != x and x.h > y.h + c(y,x) and
y.state == CLOSED and y.h > x.k):
insert(y,y.h);
return openList.peek().k;
function insert(x,val):
if(x.state == NEW):
x.h = val;
x.k = val;
x.state = OPEN;
openList.add(x);
else if(x.state == OPEN):
openList.remove(x);
x.k = Math.min(x.k,val);
openList.add(x);
else if(x.state == CLOSED):
x.k = Math.min(x.h,val);
x.h = val;
x.state = OPEN;
openList.add(x);
function modify_cost(x,y,val):
c(x,y) = val;
if(y.state == CLOSED):
insert(y,val);
return openList.peek().k;
function replan(y):
while True:
if(result = process_state() >= y.h || result == -1){
break;
}
// 返回值大於-1表示調整好路徑，等於-1 表示沒有路徑可以從當前點到達目標點
return result;
function run(start,end):
init(graph);
insert(end,0);
//進行路徑規劃
while True:
min_k = process_state();
if(start.state == CLOSED || min_k == -1):
break;
// 不能找到從起始點到目標點的最短路徑
if(min_k == -1):
return;
current = start.b;
while True:
//執行查詢到的路徑，直到上層節點無法通過，或者到達目標節點
while (current != end and current.b.state == CLOSED):
current = current.b;
// 執行移動操作
gogogo(current);
if(current == end):
// 將其移動到終點,結束路徑執行過程
gogogo(current);
break;
// 當前節點節點的上層節點不能移動了，因為某種原因(可能是障礙物)被人修改了執行代價,也就是被呼叫過該節點的modify_cost(current,current.b,newH)函式
min_k = replan(current);
// 重新規劃路徑後，無法找到一條有效的路
if(min_k == -1):
return;
實現
import lombok.Data;
import lombok.extern.slf4j.Slf4j;
import java.util.*;
/**
* @author 學徒
*/
@Slf4j
public class DGraph {
/**
* 用於對映節點標識和節點物件
*/
private Map<Integer, DNode> nodeMap;
/**
* 用於對映兩個節點之間的移動代價
*/
private Map<String, Integer> costMap;
/**
* 用於對映節點的鄰居節點
*/
private Map<DNode, Set<DNode>> neighborsMap;
public DGraph(int[][] graph) {
if (Objects.isNull(graph) || graph.length == 0 || graph.length != graph[0].length) {
throw new IllegalArgumentException("圖引數出錯");
}
neighborsMap = new HashMap<>(graph.length);
nodeMap = new HashMap<>(graph.length);
costMap = new HashMap<>(graph.length);
init(graph);
}
/**
* 用於完成圖的初始化工作
*
* @param graph 圖模型的鄰接矩陣
*/
private void init(int[][] graph) {
final Integer UNGO = Integer.MAX_VALUE;
// 用於初始化所有節點
for (int i = 0; i < graph.length; i++) {
nodeMap.put(i, new DNode(i));
}
// 用於初始化鄰接節點列表和節點間的移動代價
for (int i = 0; i < graph.length; i++) {
for (int j = 0; j < graph[i].length; j++) {
if (Objects.equals(UNGO, graph[i][j]) || Objects.equals(i, j)) {
continue;
}
DNode x = nodeMap.get(i);
DNode y = nodeMap.get(j);
// 此處調轉y與x之間的位置是因為有向圖中是從x指向y的，但DStar演算法是從目標點向起始點進行規劃的，為此需要調轉方向
costMap.put(generateCostKey(y, x), graph[i][j]);
neighborsMap.computeIfAbsent(y, obj -> new HashSet<>()).add(x);
//設定一個反方向的邊，將其值代價設定得很大。目的是為了相容有向圖中某些節點之間沒有雙向邊,只有單向邊，
//在有向圖只有單向邊的情況下，會得不到最優解
neighborsMap.computeIfAbsent(x,obj -> new HashSet<>()).add(y);
}
}
}
/**
* 圖上的節點
*/
@Data
public class DNode {
/**
* 用於標識該節點
*/
private int index;
/**
* 節點的h值
*/
private long h;
/**
* 節點的k值
*/
private long k;
/**
* 最短路徑的回撥節點
*/
private DNode b;
/**
* 節點的狀態
*/
private STATE state;
public DNode(int index) {
this(index, STATE.NEW);
}
public DNode(int index, STATE state) {
this.index = index;
this.state = state;
}
@Override
public int hashCode() {
return this.index;
}
@Override
public boolean equals(Object obj) {
if (obj == this) {
return true;
}
if (obj instanceof DNode) {
DNode node = (DNode) obj;
return node.index == this.index;
}
return false;
}
}
/**
* 節點的狀態列舉值
*/
public enum STATE {
NEW,
OPEN,
CLOSED;
}
/**
* 通過節點的唯一標識獲取節點物件
*
* @param index 節點的唯一標識
* @return 節點物件
*/
public DNode getDNodeByIndex(int index) {
return nodeMap.get(index);
}
/**
* 通過x節點獲取其對應的鄰居節點
*
* @param x 查詢鄰居節點的相關節點
* @return 鄰居節點列表
*/
public Set<DNode> getNeighbors(DNode x) {
return neighborsMap.getOrDefault(x,new HashSet<>());
}
/**
* 得到從x節點移動到y節點的移動代價
*
* @param x 起始點
* @param y 目標點
*/
public long getCost(DNode x, DNode y) {
Integer val = costMap.get(generateCostKey(x, y));
// 當不存在這樣的路徑的移動代價時，也就是不存在對應方向的路徑，為此將其設定為最大值
return Objects.isNull(val) ? Integer.MAX_VALUE : val;
}
/**
* 修改節點x移動到節點y的移動代價
*
* @param x 起始點
* @param y 目標點
* @param cost 新的移動代價
*/
public void setCost(DNode x, DNode y, int cost) {
costMap.put(generateCostKey(x, y), cost);
}
/**
* 用於生成儲存兩個節點的移動代價的鍵
*
* @param x 起始點
* @param y 目標點
* @return 從起始點移動到目標點的鍵
*/
private String generateCostKey(DNode x, DNode y) {
final String SEPARATOR = "~";
return x.index + SEPARATOR + y.index;
}
}
import lombok.extern.slf4j.Slf4j;
import java.util.Comparator;
import java.util.Objects;
import java.util.PriorityQueue;
import java.util.Queue;
import java.util.concurrent.TimeUnit;
/**
* @author 學徒
*/
@Slf4j
public class DStar {
/**
* 開放列表
*/
private Queue<DGraph.DNode> openList;
/**
* 地圖
*/
private DGraph graph;
public DStar(int[][] graph) {
this(new DGraph(graph));
}
private DStar(DGraph graph) {
this.graph = graph;
openList = new PriorityQueue<>(Comparator.comparingLong(DGraph.DNode::getK));
}
/**
* 用於修改節點x的狀態及其對應的h和k值
*
* @param x 節點x
* @param val 新的h值
*/
private void insert(DGraph.DNode x, long val) {
if (Objects.equals(x.getState(), DGraph.STATE.NEW)) {
x.setH(val);
x.setK(val);
} else if (Objects.equals(x.getState(), DGraph.STATE.OPEN)) {
openList.remove(x);
x.setK(Math.min(x.getK(), val));
} else if (Objects.equals(x.getState(), DGraph.STATE.CLOSED)) {
x.setK(Math.min(x.getH(), val));
x.setH(val);
}
x.setState(DGraph.STATE.OPEN);
openList.add(x);
}
/**
* 用於處理對應的節點
*
* @return openList中所有節點的最小k值，當openList為空時，返回-1;
*/
private long process_state() {
DGraph.DNode x = openList.poll();
if (Objects.isNull(x)) {
return -1;
}
x.setState(DGraph.STATE.CLOSED);
// 當h值大於k值時，表示當前該節點處於h值被修改為較大的狀態(raise狀態)
// 為此查詢鄰居節點來得到減低自身的h值
if (x.getK() < x.getH()) {
for (DGraph.DNode y : graph.getNeighbors(x)) {
if (y.getH() < x.getK() && x.getH() > y.getH() + graph.getCost(y, x)) {
x.setB(y);
x.setH(y.getH() + graph.getCost(y, x));
}
}
}
// 該過程類似於dijikstra，用來傳播資訊當前節點h值變化的資訊和降低鄰居節點的h值
if (Objects.equals(x.getK(), x.getH())) {
for (DGraph.DNode y : graph.getNeighbors(x)) {
if (Objects.equals(y.getState(), DGraph.STATE.NEW)
|| (Objects.equals(y.getB(), x) && !Objects.equals(y.getH(), x.getH() + graph.getCost(x, y)))
|| (!Objects.equals(y.getB(), x) && y.getH() > x.getH() + graph.getCost(x, y))) {
y.setB(x);
insert(y, x.getH() + graph.getCost(x, y));
}
}
} else {
for (DGraph.DNode y : graph.getNeighbors(x)) {
if (Objects.equals(y.getState(), DGraph.STATE.NEW)
|| (Objects.equals(y.getB(), x) && !Objects.equals(y.getH(), x.getH() + graph.getCost(x, y)))) {
y.setB(x);
insert(y, x.getH() + graph.getCost(x, y));
} else {
if (!Objects.equals(y.getB(), x) && y.getH() > x.getH() + graph.getCost(x, y)) {
insert(x, x.getH());
} else {
if (!Objects.equals(y.getB(), x)
&& x.getH() > y.getH() + graph.getCost(y, x)
&& Objects.equals(y.getState(), DGraph.STATE.CLOSED)
&& y.getH() > x.getK()) {
insert(y, y.getH());
}
}
}
}
}
return Objects.isNull(x = openList.peek()) ? -1 : x.getK();
}
/**
* 用於修改某兩個節點之間的移動代價，且y節點為x節點的上一節點
*
* @param xIndex 節點1
* @param xIndex 節點2
* @param val 節點的移動代價
* @return 當前新的最小k值
*/
public long modify_cost(int xIndex, int yIndex, int val) {
DGraph.DNode x = graph.getDNodeByIndex(xIndex);
DGraph.DNode y = graph.getDNodeByIndex(yIndex);
// 此處調轉y與x之間的位置是因為有向圖中是從x指向y的，但DStar演算法是從目標點向起始點進行規劃的，為此需要調轉方向
graph.setCost(y, x, val);
if (Objects.equals(y.getState(), DGraph.STATE.CLOSED)) {
insert(y, val);
}
DGraph.DNode node = null;
return Objects.isNull(node = openList.peek()) ? -1 : node.getK();
}
/**
* @param y 需要進行重新進行路徑規劃的點，就是可能是障礙點，或者移除了障礙之後的點
* @return 返回值大於-1表示調整好路徑，等於-1 表示沒有路徑可以從當前點到達目標點
*/
private long replan(DGraph.DNode y) {
long result = -1;
while (true) {
if ((result = process_state()) >= y.getH() || Objects.equals(result, -1L)) {
break;
}
}
return -1;
}
/**
* 執行從start點到end點的最短路徑執行過程
*
* @param start 起始節點
* @param end 目標節點
*/
public void run(int start, int end) {
insert(graph.getDNodeByIndex(end), 0);
// 執行路徑規劃
long min_k = -1;
while (true) {
min_k = process_state();
if (Objects.equals(graph.getDNodeByIndex(start).getState(), DGraph.STATE.CLOSED)
|| Objects.equals(min_k, -1L)) {
break;
}
}
// 不能找到從起始點到目標點的最短路徑
if (Objects.equals(min_k, -1)) {
log.info("無法找到執行路徑");
return;
}
// 用於除錯使用
printPath(start, end);
DGraph.DNode current = graph.getDNodeByIndex(start);
DGraph.DNode next = current.getB();
while (true) {
while (Objects.nonNull(next) && Objects.equals(next.getState(), DGraph.STATE.CLOSED)) {
gogogo(next);
current = next;
next = next.getB();
}
if (Objects.isNull(next)) {
break;
}
// 當前節點節點的上層節點不能移動了，因為某種原因(可能是障礙物)被人修改了執行代價,也就是被呼叫過該節點的modify_cost(current,current.b,newH)函式
min_k = replan(current);
// 重新規劃路徑後，無法找到一條有效的路
if (Objects.equals(min_k, -1)) {
log.info("無法找到有效執行路徑");
return;
}
log.info("重新規劃後路徑");
// 用於除錯使用
printPath(current.getIndex(), end);
next = current.getB();
}
}
/**
* 用於執行移動到指定節點操作
*
* @param node 需要移動到的對應節點
*/
private void gogogo(DGraph.DNode node) {
log.info("移動到節點：" + node.getIndex());
try {
TimeUnit.SECONDS.sleep(1);
} catch (Exception e) {
log.info(e.getMessage());
}
}
/**
* 用於列印路徑，除錯使用
*
* @param start 起始點
* @param end 目標點
*/
private void printPath(int start, int end) {
DGraph.DNode s = graph.getDNodeByIndex(start);
while (s.getIndex() != end) {
log.info(s.getIndex() + "->");
s = s.getB();
}
log.info(String.valueOf(end));
}
}
測試用程式碼：
/**
* @author 學徒
*/
public class DStarTest {
public static void main(String[] args) {
int[][] graph = new int[][]{
{0, 1, 12, Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE},
{Integer.MAX_VALUE, 0, 19, 3, Integer.MAX_VALUE, Integer.MAX_VALUE},
{Integer.MAX_VALUE, Integer.MAX_VALUE, 0, Integer.MAX_VALUE, 5, Integer.MAX_VALUE},
{Integer.MAX_VALUE, Integer.MAX_VALUE, 4, 0, 13, 15},
{Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE, 0, 4},
{Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE, Integer.MAX_VALUE, 0}
};
int source = 0, target = 5;
DStar dStar = new DStar(graph);
new Thread(() -> {
try {
Thread.sleep(2);
}catch (Exception e){
e.printStackTrace();
}
dStar.modify_cost(3, 2, 1000);
}).start();
dStar.run(source, target);
}
}
總結： D*演算法能過相容靜態環境。在動態變化的環境中，其校正最短路徑的效率比重新執行靜態路徑規劃的演算法效率要高。
總結
對於路徑規劃，除了常見的Dijikstra演算法，A*演算法以及D*演算法。在面對各種複雜的應用場景和前提條件時，存在著多種多樣的其它演算法，如D*Lite，LPA等。我們需要對這些演算法的基本思路和適用場景有個基本的認識，才能夠使高效的達成我們的目標。
以下是一個github連結，其包括了各種路徑規劃演算法。