A* search算法

今天，仍是國慶和中秋雙節的時間節點，一個天氣不錯的日子，孩子已經早早的睡覺了，玩了一成天，也不睡覺，累的實在扛不住了，勉強洗澡結束，倒牀即睡着的節奏。。。

 

很少說題外話，進入正題。

 

什麼是A*搜索算法呢？就用百科的解說吧：

A*算法，A*（A-Star)算法是一種靜態路網中求解最短路徑最有效的直接搜索方法，也是解決許多搜索問題的有效算法。算法中的距離估算值與實際值越接近，最終搜索速度越快。

 

A*搜索的實際應用場景不少，可是你們最爲熟悉的恐怕莫過於遊戲了。典型的遊戲就是穿越障礙尋寶，要求在最少的代價內找到寶貝，一般遊戲中的代價，就是用最少的步驟實現寶貝的找尋。還有一種遊戲場景是，給定入口地點，用最少的步驟走到指定的出口地點，中間有障礙物，每次只能在上下左右四個方向上走一步，且不能穿越障礙物。

 

就拿第二種遊戲場景來解釋A* search具體指的是什麼內容吧。

如上圖所示，假設咱們有一個7*5的迷宮方格，綠色的點表示起點，紅色的點表示終點。中間三個藍色的格子表示一堵牆，是障礙物。遊戲的規則，是綠色的起點，每次只能向上下左右4個方向中移動一步，且不能穿越中間的牆，以最少的步驟到達紅色的終點。

 

在解決這個問題以前，先要引入A*搜索算法的核心集合和公式：

核心集合：OpenList，CloseList

核心公式：F=G+H

其中，OpenList和CloseList用來存儲格子點信息，OpenList表示可到達格子節點集合，CloseList表示已到達格子節點集合。

F=G+H表示對格子價值的評估，G表示從起點到當前點的代價；H表示從當前點到達終點的代價，指不考慮障礙物遮擋的狀況下，這裏，代價是指走的步數。至於F，就是對G和H的綜合評估了，固然F越小，則從起點到達終點付出的代價就越小了。

 

就實際操做一下吧。仍是上面的圖，每一個節點，用n(x,y)表示,x表示橫座標，y表示縱座標，好比綠色的起點是n(1,2)：

第一步：把起點放入OpenList裏面。

OpenList： n(1,2)

CloseList

 

第二步：找出OpenList中F值最小的方格，即惟一的方格n(1,2)做爲當前方格，並把當前格移出OpenList，放入CloseList。表示這個格子已到達且驗證過了。

OpenList

CloseList：n(1,2)

 

第三步：找出當前格上下左右全部可到達的格子，看它們是否在OpenList當中。若是不在，加入OpenList，計算出相應的G、H、F值，並把當前格子做爲它們的「父節點」。

OpenList：n(0,2), n(1,1), n(2,2), n(1,3)

CloseList：n(1,2) 

其中，n(0,2), n(1,1), n(2,2), n(1,3)的父節點是n(1,2).所謂的父節點，表示當前的這幾個節點n(0,2), n(1,1), n(2,2), n(1,3)都是從這個所謂的父節點出發獲得的分支節點，父節點用做後續找出最短路徑用的。

 

上述3步，是一次局部尋路的過程，咱們須要不斷的重複第二步第三步，最終找到到達終點的最短路徑。

第二輪 ~ 第一步：找出OpenList中F值最小的方格，即方格n(2,2)做爲當前方格，並把當前格移出OpenList，放入CloseList。表明這個格子已到達並檢查過了。

此時的兩個核心集合的節點信息：

OpenList：n(0,2), n(1,1), n(1,3)

CloseList：n(1,2), n(2,2)

其中，n(0,2), n(1,1), n(1,3)的父節點是n(1,2)，n(2,2)的上一級節點（也能夠稱爲父節點）是n(1,2).

 

第二輪 ~ 第二步：找出當前格上下左右全部可到達的格子，看它們是否在OpenList當中。若是不在，加入OpenList，計算出相應的G、H、F值，並把當前格子做爲它們的「父節點」。

此時的兩個核心集合的節點信息：

OpenList：n(0,2), n(1,1), n(1,3)；n(2,1), n(2,3)

CloseList：n(1,2) <-----n(2,2)

其中，n(0,2), n(1,1), n(1,3)的父節點是n(1,2)，而n(2,1), n(2,3)的父節點是n(2,2). CloseList中節點的指向關係，反映了尋路的路徑過程。

爲何這一次OpenList只增長了兩個新格子呢？由於n(3,2)是牆壁，天然不用考慮，而n(1,2)在CloseList當中，說明已經檢查過了，也不用考慮。

 

第三輪 ~ 第一步：找出OpenList中F值最小的方格。因爲這時候多個方格的F值相等，任意選擇一個便可，好比n(2,3)做爲當前方格，並把當前格移出OpenList，放入CloseList。表明這個格子已到達並檢查過了。 

此時的兩個核心集合的節點信息：

OpenList：n(0,2), n(1,1), n(1,3)；n(2,1) 

CloseList：n(1,2) <-----n(2,2)<-----n(2,3)

其中，n(0,2), n(1,1), n(1,3)的父節點是n(1,2)，而n(2,1)的父節點是n(2,2)。CloseList中節點的指向關係，反映了尋路的路徑過程。

 

第三輪 ~ 第二步：找出當前格上下左右全部可到達的格子，看它們是否在OpenList當中。若是不在，加入OpenList，計算出相應的G、H、F值，並把當前格子做爲它們的「父節點」。

此時的兩個核心集合的節點信息：

OpenList：n(0,2), n(1,1), n(1,3)；n(2,1) ；n(2,4)

CloseList：n(1,2) <-----n(2,2)<-----n(2,3)

其中，n(0,2), n(1,1), n(1,3)的父節點是n(1,2)，而n(2,1)的父節點是n(2,2)。n(2,4)的父節點是n(2,3). CloseList中節點的指向關係，反映了尋路的路徑過程。

 

剩下的就是之前面的方式繼續迭代，直到OpenList中出現終點方格爲止。

 

實際的推理，就到這裏，下面，將結合上述的推理理論，用java程序，加以實現。今天，先將僞代碼附上，改天將具體的java實現代碼貼上來。

public Node AStarSearch(Node start, Node end) {
    // 把起點加入openList  
    openList.add(start);
    //主循環，每一輪檢查一個當前方格節點
    while (openList.size() > 0) {
        // 在OpenList中查找F值最小的節點做爲當前方格節點
        Node current = findMinNode();
        // 當前方格節點從open list中移除
        openList.remove(current);
        // 當前方格節點進入closeList
        closeList.add(current);
        // 找到全部鄰近節點
        List<Node> neighbors = findNeighbors(current);
        for (Node node : neighbors) {
            if (!openList.contains(node)) {
                //鄰近節點不在openList中，標記父親、G、H、F，並放入openList
                markAndInvolve(current, end, node);
            }
        }
        //若是終點在OpenList中，直接返回終點格子
        if (find(openList, end) != null) {
            return find(openList, end);
        }
    }
    //OpenList用盡，仍然找不到終點，說明終點不可到達，返回空
    return null;
}

 

2017-10-13  11:28

過了幾天了，今天終於回來補全未完成的最終實現代碼邏輯，直接上代碼：

package com.shihuc.nlp.astarsearch;

import java.io.File;
import java.io.FileNotFoundException;
import java.util.ArrayList;
import java.util.List;
import java.util.Scanner;

class Node {
    int x;  //當前節點的x座標
    int y;  //當前節點的y座標
    int px; //指向父節點的x座標
    int py; //指向父節點的y座標
}

public class Solution {    
    static List<Node> openList = new ArrayList<Node>();
    static List<Node> closeList = new ArrayList<Node>();    
    
    
    /**
     * @param args
     * @throws FileNotFoundException
     */
    public static void main(String[] args) throws FileNotFoundException {

        File file = new File("./src/com/shihuc/nlp/astarsearch/sample.txt");
        Scanner sc = new Scanner(file);
        int N = sc.nextInt();
        for (int n = 0; n < N; n++) {
            int Y = sc.nextInt();
            int X = sc.nextInt();
            int sx = sc.nextInt();
            int sy = sc.nextInt();
            int ex = sc.nextInt();
            int ey = sc.nextInt();
            Node start = new Node();
            start.x = sx; start.y = sy;
            Node end = new Node();
            end.x = ex; end.y = ey;
            int grid[][] = new int[X][Y];
            openList.clear();
            closeList.clear();
            for (int x = 0; x < X; x++) {
                for (int y = 0; y < Y; y++) {
                    grid[x][y] = sc.nextInt();
                }
            }
            Node ne = AStarSearch(start,end, grid);
            if(ne == null){
                System.out.println("No." + n + " Can not reach the end node");
            }else{
                add2Cl(ne);
                //printRawPath(n);
                printRealPath(n, start);
            }
        }
        if(sc != null){
            sc.close();
        }
    }
    
    /**
     * 打印當前節點以及其父節點的debug函數，查看父子節點關係
     * 
     * @author shihuc
     * @param idx
     */
    public static void printRawPath(int idx){
        System.out.println("No." + idx);
        for(Node p: closeList){             
            System.out.println("([" + p.x + "," + p.y + "][" + p.px + "," + p.py + "])"  );
        }
        System.out.println();
    }
    
    /**
     * 打印最終的路徑信息的輸出函數，起點節點用於輸出結束判決
     * 
     * @author shihuc
     * @param start
     */
    public static void printRealPath(int idx, Node start){
        List<Node> path = new ArrayList<Node>();
        Node cn = closeList.get(closeList.size() - 1);
        Node temp = new Node();
        temp.x = cn.x;temp.y = cn.y;temp.px = cn.px;temp.py = cn.py;
        path.add(cn);
        do{
            for(int i=0; i<closeList.size(); i++){
                Node pn = closeList.get(i);
                if(temp.px == pn.x && temp.py == pn.y){                    
                    temp.px = pn.px;
                    temp.py = pn.py;
                    temp.x = pn.x;
                    temp.y = pn.y;
                    path.add(pn);
                    closeList.remove(pn);
                    break;
                }
            }    
        }while(!(temp.x == start.x && temp.y == start.y));
        System.out.print("No." + idx + " ");
        for(int i=path.size()-1 ; i >=0; i--){
            Node n = path.get(i);
            System.out.print("[" + n.x + "," + n.y + "]->");
        }
        System.out.println();
    }
    

    /**
     * A*搜索的完整算法實現。
     * 
     * @author shihuc
     * @param start 起點的座標
     * @param end 目標點的座標
     * @param grid 待搜索的網絡
     * @return
     */
    public static Node AStarSearch(Node start, Node end, int grid[][]) {
        // 把起點加入 openList  
        add2Ol(start);
        // 主循環，每一輪檢查一個當前方格節點
        while (openList.size() > 0) {
            // 在OpenList中查找 F值最小的節點做爲當前方格節點
            Node current = findMinNode(start,end);
            // 當前方格節點從openList中移除
            remove4Ol(current);
            // 當前方格節點進入 close list
            add2Cl(current);
            // 找到全部鄰近節點
            List<Node> neighbors = findNeighbors(current, grid);
            for (Node node : neighbors) {
                if (openListMarkedNode(node) == null) {
                    //鄰近節點不在OpenList中,標記父節點，並放入OpenList
                    markAndInvolve(current, node);                    
                }
            }
            // 若是終點在OpenList中，直接返回終點格子
            Node last = findInOpenList(end);
            if ( last != null) {                
                return last;
            }
        }
        // OpenList用盡，仍然找不到終點，說明終點不可到達，返回空
        return null;
    }
        
    /**
     * 向openList添加節點。若節點已經存在，則不添加。
     * 
     * @author shihuc
     * @param n 待添加的節點
     */
    private static void add2Ol(Node n){
        if(openListMarkedNode(n) == null){
            openList.add(n);
        }
    }
    
    /**
     * 向closeList添加節點信息。若節點已經存在，則不添加。
     * 
     * @author shihuc
     * @param n
     */
    private static void add2Cl(Node n){        
        for(Node pn: closeList){            
            if(pn.x == n.x && pn.y == n.y){                
                return;
            }
        }        
        closeList.add(n);
    }
    
    /**
     * 從openList中刪除指定的節點。經過座標信息定位指定節點。
     * 
     * @author shihuc
     * @param n
     */
    private static void remove4Ol(Node n){
        for(Node ne:openList){
            if(ne.x == n.x && ne.y == n.y){
                openList.remove(ne);
                return;
            }
        }
    }
    
    /**
     * openlist中如有已經標記的指定節點，則返回該節點，不然返回null節點。
     * 
     * @author shihuc
     * @param n
     * @return
     */
    private static Node openListMarkedNode(Node n){
        for(Node ne: openList){
            if(ne.x == n.x && ne.y == n.y){
                return ne;
            }
        }
        return null;
    }
    
    /**
     * 從closeList檢查是否存在指定的節點。
     * 
     * @author shihuc
     * @param n
     * @return
     */
    private static boolean isInCloseList(Node n){
        for(Node pn: closeList){            
            if(pn.x == n.x && pn.y == n.y){
                return true;
            }
        }
        return false;
    }
        
    /**
     * 利用相似勾股定理的方式計算H值以及G值。
     * 
     * @author shihuc
     * @param x
     * @param y
     * @return
     */
    private static int gouguLaw(int x, int y){
        return x*x + y*y;
    }
    
    /**
     * 在openList中查找F=G+H的值最小的節點。
     * 
     * @author shihuc
     * @param start
     * @param end
     * @return
     */
    public static Node findMinNode(Node start, Node end){
        int fMin = 0;
        int sx = start.x, sy = start.y;
        int ex = end.x, ey = end.y;
        Node nm = new Node();
        Node n0 = openList.get(0);
        nm.x = n0.x;nm.y = n0.y;
        fMin = gouguLaw(n0.x - sx, n0.y - sy) + gouguLaw(n0.x - ex, n0.y - ey);
        for(int i=1; i<openList.size(); i++){
            Node n = openList.get(i); 
            int g = gouguLaw(n.x - sx, n.y - sy);
            int h = gouguLaw(n.x - ex, n.y - ey);            
            if(fMin > g+h){
                nm.x = n.x;
                nm.y = n.y;
                nm.px = n.px;
                nm.py = n.py;
                fMin = g+h;                
            }
        }
        return nm;
    }
    
    /**
     * 以當前節點爲中心，查找沒有驗證過（不在closeList中）的上下左右鄰居節點。
     * 
     * @author shihuc
     * @param current
     * @param grid
     * @return
     */
    public static List<Node> findNeighbors(Node current, int grid[][]){
        int x = current.x;
        int y = current.y;
        
        int Y = grid.length;
        int X = grid[0].length;
        
        List<Node> neigs = new ArrayList<Node>();
        if(x - 1 >= 0 && grid[y][x - 1] != 1){
            Node nu = new Node();
            nu.x = x - 1;
            nu.y = y;    
            if(!isInCloseList(nu)){
                neigs.add(nu);            
            }
        }
        if(x + 1 < X && grid[y][x+1] != 1){
            Node nu =  new Node();
            nu.x = x + 1;
            nu.y = y;
            if(!isInCloseList(nu)){
                neigs.add(nu);            
            }
        }
        if(y - 1 >= 0  && grid[y - 1][x] != 1){
            Node nu =  new Node();
            nu.x = x;
            nu.y = y - 1;
            if(!isInCloseList(nu)){
                neigs.add(nu);            
            }
        }
        if(y + 1 < Y && grid[y + 1][x] != 1){
            Node nu =  new Node();
            nu.x = x;
            nu.y = y + 1;
            if(!isInCloseList(nu)){
                neigs.add(nu);            
            }
        }    
        return neigs;        
    }
    
    /**
     * 檢查指定節點是否在openList中。
     * 
     * @author shihuc
     * @param ed
     * @return
     */
    public static Node findInOpenList(Node ed){
        return openListMarkedNode(ed);            
    }
    
    /**
     * 這個函數很是重要，標記當前節點的鄰居節點的父親節點爲當前節點，這個標記關係，用於後續輸出A*搜索的最終路徑
     * 
     * @author shihuc
     * @param current
     * @param n
     */
    public static void markAndInvolve(Node current, Node n){
        n.px = current.x;
        n.py = current.y;
        openList.add(n);
    }
}

 

測試用到的數據樣本（sample.txt內容）：

3
7 5
1 2 5 2
0 0 0 0 0 0 0
0 0 0 1 0 0 0
0 0 0 1 0 0 0
0 0 0 1 0 0 0
0 0 0 0 0 0 0
7 5
1 2 5 2
0 0 0 1 0 0 0
0 0 0 1 0 0 0
0 0 0 1 0 0 0
0 0 0 1 0 0 0
0 0 0 1 0 0 0
20 15
1 1 19 14
0 0 0 0 0 1 0 0 0 1 0 0 0 0 0 0 0 0 1 1
0 0 1 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1
0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0 0 0 1
0 0 0 0 0 1 0 1 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 1
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 1 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 1 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0
0 0 0 0 1 0 0 1 0 0 0 0 1 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0
0 1 0 0 0 0 1 0 0 0 0 0 0 1 0 1 0 0 0 1
0 0 0 0 1 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0

 

最終的數據測試結果：

No.0 [1,2]->[2,2]->[2,1]->[2,0]->[3,0]->[4,0]->[4,1]->[4,2]->[5,2]->
No.1 Can not reach the end node
No.2 [1,1]->[1,2]->[2,2]->[3,2]->[4,2]->[4,3]->[4,4]->[5,4]->[6,4]->[7,4]->[7,5]->[7,6]->[8,6]->[9,6]->[9,7]->[10,7]->[11,7]->[11,8]->[11,9]->[12,9]->[12,10]->[13,10]->[13,11]->[14,11]->[14,12]->[15,12]->[16,12]->[16,13]->[16,14]->[17,14]->[18,14]->[19,14]->

 

上述算法，場景比較簡單，當前節點的上下左右四個方向，有的要求8個方向的，甚至障礙物有其餘的要求的。都離不開這裏的最要思想，F=G+H.

 

歡迎探討，歡迎評論以及轉帖，請註明出處！