[ Algorithm ] Route search problem - Using A* algorithm

Question : 
考慮下面的路線圖, 我們要找出一個演算法能夠找出任兩點間 cost 最低的路徑. 在下圖中 edge 上面的數字是點與點間移動的 cost : 
 

Greedy Best-first Search: 
第一個我們要使用的演算法是 Greedy Best-first Search. 它屬於 Informed-Search, 使用的 evaluation function f(n) 如下: 

f(n) = h(n)

上面的 h(n) 是所謂的 heuristic function, 也就是透過該 function 可以得到從當前的 state 到 goal 預期的 cost. (如果該 heuristic function 得到的預期 cost 總是小於等於實際的 cost, 我們稱該 heuristic function 滿足 admissible). 考慮我們使用點對點間的直線距離 (直線距離一定是小於等於實際的 cost!) 當作我們的 heuristic function, 得到任一點到 Bucharest 的直線距離表格如下: 
 
實作的 Greedy Best-first search 代碼如下: 

view plaincopy to clipboardprint?

1. package c3.route;  
2.   
3. import java.util.HashMap;  
4. import java.util.PriorityQueue;  
5. import java.util.Set;  
6. import java.util.TreeSet;  
7.   
8. import ai.utils.searchs.Graph;  
9. import ai.utils.searchs.SNode;  
10. import ai.utils.searchs.proto.INode;  
11.   
12. public class GreedyBFS {  
13.     public static String GOAL= "Bucharest";     /*Setup your goal*/  
14.     public static String INITIAL = "Arad";      /*Setup your initial state here.*/  
15.     public static Graph g;  
16.     public static HashMap heuristicMap = new HashMap();  
17.     public static Set deadEndSet = new TreeSet();  
18.     public static boolean IS_DONE = false;  
19.       
20.     public static class NodeWrapper implements Comparable{  
21.         INode node;  
22.         int sld=0;  
23.         public NodeWrapper(INode n, int v){node=n; sld=v;}  
24.           
25.         @Override  
26.         public int compareTo(NodeWrapper o) {  
27.             if(sld < o.sld) return -1;  
28.             else if(sld > o.sld) return 1;  
29.             return 0;  
30.         }  
31.     }  
32.       
33.     /** 
34.      * BD : Create Romania graph  
35.      * @return 
36.      */  
37.     public static Graph prepareRomania()  
38.     {  
39.         Graph g = new Graph();  
40.         g.addNode(new SNode("Arad"));  
41.         g.addNode(new SNode("Zerind"));  
42.         g.addNode(new SNode("Oradea"));  
43.         g.addNode(new SNode("Timisoara"));  
44.         g.addNode(new SNode("Sibiu"));  
45.         g.addNode(new SNode("Lugoj"));   
46.         g.addNode(new SNode("Mehadia"));   
47.         g.addNode(new SNode("Drobeta"));  
48.         g.addNode(new SNode("Rimnicu Vilcea"));  
49.         g.addNode(new SNode("Fagaras"));  
50.         g.addNode(new SNode("Pitesti"));  
51.         g.addNode(new SNode("Craiova"));  
52.         g.addNode(new SNode("Bucharest"));  
53.         g.addNode(new SNode("Giurgiu"));  
54.         g.addNode(new SNode("Urziceni"));  
55.         g.addNode(new SNode("Neamt"));  
56.         g.addNode(new SNode("Iasi"));  
57.         g.addNode(new SNode("Vaslui"));  
58.         g.addNode(new SNode("Hirsova"));  
59.         g.addNode(new SNode("Eforie"));  
60.           
61.         g.addUEdge("Arad", "Zerind");  
62.         g.addUEdge("Arad", "Timisoara");  
63.         g.addUEdge("Arad", "Sibiu");  
64.         g.addUEdge("Zerind", "Oradea");  
65.         g.addUEdge("Oradea", "Sibiu");  
66.         g.addUEdge("Sibiu", "Fagaras");  
67.         g.addUEdge("Sibiu", "Rimnicu Vilcea");  
68.         g.addUEdge("Timisoara", "Lugoj");  
69.         g.addUEdge("Lugoj", "Mehadia");  
70.         g.addUEdge("Mehadia", "Drobeta");  
71.         g.addUEdge("Drobeta", "Craiova");  
72.         g.addUEdge("Craiova", "Rimnicu Vilcea");  
73.         g.addUEdge("Craiova", "Pitesti");  
74.         g.addUEdge("Rimnicu Vilcea", "Pitesti");  
75.         g.addUEdge("Pitesti", "Bucharest");  
76.         g.addUEdge("Fagaras", "Bucharest");  
77.         g.addUEdge("Bucharest", "Giurgiu");  
78.         g.addUEdge("Bucharest", "Urziceni");  
79.         g.addUEdge("Urziceni", "Vaslui");  
80.         g.addUEdge("Vaslui", "Iasi");  
81.         g.addUEdge("Iasi", "Neamt");  
82.         g.addUEdge("Urziceni", "Hirsova");  
83.         g.addUEdge("Hirsova", "Eforie");  
84.           
85.         heuristicMap.put("Arad", 366);  
86.         heuristicMap.put("Bucharest", 0);  
87.         heuristicMap.put("Craiova", 160);  
88.         heuristicMap.put("Drobeta", 242);  
89.         heuristicMap.put("Eforie", 161);  
90.         heuristicMap.put("Fagaras", 176);  
91.         heuristicMap.put("Giurgiu", 77);  
92.         heuristicMap.put("Hirsova", 151);  
93.         heuristicMap.put("Iasi", 226);  
94.         heuristicMap.put("Lugoj", 244);  
95.         heuristicMap.put("Neamt", 234);  
96.         heuristicMap.put("Oradea", 380);  
97.         heuristicMap.put("Pitesti", 100);  
98.         heuristicMap.put("Rimnicu Vilcea", 193);  
99.         heuristicMap.put("Sibiu", 253);  
100.         heuristicMap.put("Timisoara", 329);  
101.         heuristicMap.put("Urziceni", 80);  
102.         heuristicMap.put("Vaslui", 199);  
103.         heuristicMap.put("Zerind", 374);  
104.         heuristicMap.put("Mehadia", 241);  
105.         return g;  
106.     }  
107.   
108.       
109.     public static void RecvBFS(INode node, INode from, String path, int depth)  
110.     {  
111.         // If the current node is Goal, stop searching.  
112.         if(node.getName().equals(GOAL))  
113.         {  
114.             System.out.printf("\t[Info] Path: %s->%s\n", path, GOAL);  
115.             IS_DONE = true;  
116.             return;  
117.         }  
118.           
119.         if(depth>0 && !IS_DONE)  
120.         {  
121.             /* 1) Get all connected nodes from current node*/  
122.             Set conn = g.getConnectNodes(node);  
123.               
124.             /* 2) If only one connected node and it is the 'from' node. Skip it and add into dead-end set.*/  
125.             if(conn.size()==1 && conn.iterator().next().equals(from))  
126.             {  
127.                 deadEndSet.add(node);  
128.                 return;  
129.             }  
130.               
131.             if(conn!=null)  
132.             {     
133.                 PriorityQueue queue = new PriorityQueue();  
134.                 for(INode sn:conn)  
135.                 {                     
136.                     /* 3) Skip expansion for dead-end or the 'from' node*/  
137.                     if(deadEndSet.contains(sn) || sn.equals(from)) continue;  
138.                       
139.                     /* 4) Add node into priority queue. Next will pickup the 'best-first' node with smallest estimation cost.*/  
140.                     queue.add(new NodeWrapper(sn,heuristicMap.get(sn.getName())));  
141.                 }  
142.                   
143.                 /* 5) Expand the connected node(s).*/  
144.                 while(!queue.isEmpty())  
145.                 {  
146.                     NodeWrapper n = queue.poll();  
147.                     RecvBFS(n.node, node, path.isEmpty()?node.getName():String.format("%s->%s", path, node.getName()), depth-1);  
148.                     if(IS_DONE) break;  
149.                 }  
150.             }             
151.         }  
152.         else   
153.         {  
154.             // If the depth outside the limit or the searching is done.  
155.         }  
156.     }  
157.       
158.     /** 
159.      * @param args 
160.      */  
161.     public static void main(String[] args) {  
162.         g = prepareRomania();  
163.         INode arad = g.getNode(INITIAL);  
164.         RecvBFS(arad, null, "", 10); // Limit depth to within 10 during searching.  
165.     }  
166. }  

執行結果: 

[Info] Path: Arad->Sibiu->Fagaras->Bucharest

而其 Greedy Best-first Search 演算法的執行過程如下示意圖: 
 

事實上這條路徑 Arad->Sibiu (140) + Sibiu->Fagaras (99) +Fagaras->Bucharest (211)= 460 並不是最佳路徑, 也就是說 Greedy Best-first Search 並不滿足 Optimal (能找到最佳路徑)! 

另外上面的代碼我有改善過, 會避免 Dead-end 與 loop 的發生. 原始的 Greedy Best-first Search 在 Graph Search 且有 loop 時有可能會造成 infinite loop, 也就是說 Greedy Best-first Search 也不滿足 Complete 特性 (在 finite state space 下, 一定找得到 Goal)! 

因此下面要介紹的是 A* Search 演算法, 它不但滿足 Optimal 特性, 也保證 Complete. 

A* Search : 
關於 A* Search 可以參考 wiki 或是 AI 書上的說明, 與 Greedy Best-first Search 最大不同是原先的 evaluation f(n) 改為下面方程式 (增加 g(n)): 

f(n) = g(n) + h(n)

而 g(n) 指的是到第 n state 所需花費的 cost. 在該演算法中會將 reachable 的 node 放在 priority queue 中 (昇序排列) 每次 loop 從裡面挑出 f(n) 值最小的 node 出來進行 expand; 並將 expanded nodes 再次放到 priority queue 中直到抵達 Goal. 下面為該演算法的實現代碼: 

view plaincopy to clipboardprint?

1. package c3.route;  
2.   
3. import java.util.HashMap;  
4. import java.util.PriorityQueue;  
5. import java.util.Set;  
6. import java.util.TreeSet;  
7.   
8. import ai.utils.searchs.Edge;  
9. import ai.utils.searchs.Graph;  
10. import ai.utils.searchs.SNode;  
11. import ai.utils.searchs.proto.INode;  
12.   
13. public class AStarSearch {  
14.     public static String GOAL= "Bucharest"; /*Setup your goal*/  
15.     public static String INITIAL = "Arad";      /*Setup your initial state here.*/  
16.     public static Graph g;  
17.     public static HashMap heuristicMap = new HashMap();  
18.     public static Set deadEndSet = new TreeSet();  
19.     public static boolean IS_DONE = false;  
20.       
21.     public static class ENode implements Comparable  
22.     {  
23.         public double gcost = 0;  
24.         public double estim = 0;  
25.         public INode curNode = null;  
26.         public String from="";  
27.           
28.         public ENode(ENode s, INode en, double cost, double estim)  
29.         {  
30.             if(s!=null)   
31.             {  
32.                 from = String.format("%s->%s", s.from, en.getName());  
33.                 gcost = s.gcost+cost;  
34.             }  
35.             else   
36.             {  
37.                 from = en.getName();  
38.                 gcost = cost;  
39.             }  
40.             curNode = en;  
41.         }  
42.           
43.         /** 
44.          * BD: Evaluation f(n) = g(n) + h(n) 
45.          * @return 
46.          */  
47.         public double overallCost(){return gcost+estim;}  
48.           
49.         @Override  
50.         public int compareTo(ENode n) {  
51.             if(overallCost()>n.overallCost()) return 1;  
52.             else if(overallCost()return -1;  
53.             return 0;  
54.         }  
55.           
56.     }  
57.       
58.     /** 
59.      * BD: Create Romania graph  
60.      * @return 
61.      */  
62.     public static Graph prepareRomania()  
63.     {  
64.         Graph g = new Graph();  
65.         // Nodes  
66.         g.addNode(new SNode("Arad"));  
67.         g.addNode(new SNode("Zerind"));  
68.         g.addNode(new SNode("Oradea"));  
69.         g.addNode(new SNode("Timisoara"));  
70.         g.addNode(new SNode("Sibiu"));  
71.         g.addNode(new SNode("Lugoj"));   
72.         g.addNode(new SNode("Mehadia")); //Mehadia  
73.         g.addNode(new SNode("Drobeta"));  
74.         g.addNode(new SNode("Rimnicu Vilcea"));  
75.         g.addNode(new SNode("Fagaras"));  
76.         g.addNode(new SNode("Pitesti"));  
77.         g.addNode(new SNode("Craiova"));  
78.         g.addNode(new SNode("Bucharest"));  
79.         g.addNode(new SNode("Giurgiu"));  
80.         g.addNode(new SNode("Urziceni"));  
81.         g.addNode(new SNode("Neamt"));  
82.         g.addNode(new SNode("Iasi"));  
83.         g.addNode(new SNode("Vaslui"));  
84.         g.addNode(new SNode("Hirsova"));  
85.         g.addNode(new SNode("Eforie"));  
86.           
87.         // Edges  
88.         g.addUEdge("Arad", "Zerind", 75);  
89.         g.addUEdge("Arad", "Timisoara", 118);  
90.         g.addUEdge("Arad", "Sibiu", 140);  
91.         g.addUEdge("Zerind", "Oradea", 71);  
92.         g.addUEdge("Oradea", "Sibiu", 151);  
93.         g.addUEdge("Sibiu", "Fagaras", 99);  
94.         g.addUEdge("Sibiu", "Rimnicu Vilcea", 80);  
95.         g.addUEdge("Timisoara", "Lugoj", 111);  
96.         g.addUEdge("Lugoj", "Mehadia", 70);  
97.         g.addUEdge("Mehadia", "Drobeta", 75);  
98.         g.addUEdge("Drobeta", "Craiova", 120);  
99.         g.addUEdge("Craiova", "Rimnicu Vilcea", 146);  
100.         g.addUEdge("Craiova", "Pitesti");  
101.         g.addUEdge("Rimnicu Vilcea", "Pitesti", 97);  
102.         g.addUEdge("Pitesti", "Bucharest", 101);  
103.         g.addUEdge("Fagaras", "Bucharest", 211);  
104.         g.addUEdge("Bucharest", "Giurgiu", 90);  
105.         g.addUEdge("Bucharest", "Urziceni", 85);  
106.         g.addUEdge("Urziceni", "Vaslui", 142);  
107.         g.addUEdge("Vaslui", "Iasi", 92);  
108.         g.addUEdge("Iasi", "Neamt", 87);  
109.         g.addUEdge("Urziceni", "Hirsova", 98);  
110.         g.addUEdge("Hirsova", "Eforie", 86);  
111.           
112.         // Heuristic function/map  
113.         heuristicMap.put("Arad", 366);  
114.         heuristicMap.put("Bucharest", 0);  
115.         heuristicMap.put("Craiova", 160);  
116.         heuristicMap.put("Drobeta", 242);  
117.         heuristicMap.put("Eforie", 161);  
118.         heuristicMap.put("Fagaras", 176);  
119.         heuristicMap.put("Giurgiu", 77);  
120.         heuristicMap.put("Hirsova", 151);  
121.         heuristicMap.put("Iasi", 226);  
122.         heuristicMap.put("Lugoj", 244);  
123.         heuristicMap.put("Neamt", 234);  
124.         heuristicMap.put("Oradea", 380);  
125.         heuristicMap.put("Pitesti", 100);  
126.         heuristicMap.put("Rimnicu Vilcea", 193);  
127.         heuristicMap.put("Sibiu", 253);  
128.         heuristicMap.put("Timisoara", 329);  
129.         heuristicMap.put("Urziceni", 80);  
130.         heuristicMap.put("Vaslui", 199);  
131.         heuristicMap.put("Zerind", 374);  
132.         heuristicMap.put("Mehadia", 241);  
133.         return g;  
134.     }  
135.   
136.     /** 
137.      * @param args 
138.      */  
139.     public static void main(String[] args) {  
140.         /* 0) Prepare Romania graph*/  
141.         g = prepareRomania();  
142.           
143.         /* 1) Decide initial state and put into priority queue*/  
144.         INode arad = g.getNode(INITIAL);  
145.         PriorityQueue enQueue = new PriorityQueue();  
146.         enQueue.add(new ENode(null, arad, 0, heuristicMap.get(arad.getName())));  
147.           
148.         /* 2) Loop to search the goal*/  
149.         //   2.1: Pickup smallest f(n) of node to expand  
150.         //   2.2: Check if the selected node is Goal. Yes to terminate the loop  
151.         //   2.3: Put the expanded nodes into priority queue.  
152.         while(!enQueue.isEmpty())  
153.         {  
154.             ENode en = enQueue.poll();  
155.             if(en.curNode.getName().equals(GOAL))  
156.             {  
157.                 System.out.printf("\t[Info] Path=%s(%.01f)\n", en.from, en.overallCost());  
158.                 break;  
159.             }  
160.             Set edgeSet = g.getConnectEdge(en.curNode);  
161.             for(Edge e:edgeSet)  
162.             {  
163.                 //System.out.printf("%s\n", e);  
164.                 enQueue.add(new ENode(en, e.to, e.weight, heuristicMap.get(e.to.getName())));  
165.             }  
166.         }  
167.     }  
168. }  

執行結果如下: 

[Info] Path=Arad->Sibiu->Rimnicu Vilcea->Pitesti->Bucharest(418.0)

A* Search 可以找到從 Arad 到 Bucharest 的最佳路徑 (cost=418), 演算法的過程如下示意圖: 
 
 

雖然 A* Search 既是 Optimal 也有 Complete 特性, 但是它在每次的 expansion 都會保留所有未被 expanded 的 nodes 在記憶體(或是 Priority queue), 因此當 state expansion 的 b(breadth) 很大時, 記憶體的使用量可能是需要注意的重點! 上面範例代碼的完整版可以到 這裡 下載.