程式扎記: [ Algorithm ] Route search problem - Using A* algorithm

標籤

2012年10月15日 星期一

[ 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 代碼如下: 
  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. 下面為該演算法的實現代碼: 
  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) 很大時, 記憶體的使用量可能是需要注意的重點! 上面範例代碼的完整版可以到 這裡 下載.

沒有留言:

張貼留言

網誌存檔

關於我自己

我的相片
Where there is a will, there is a way!