尋路算法——A*算法詳解並附帶實現代碼

一、前言

前天看了一篇博客介紹A*算法，按照自己的理解記錄一下A*算法。

二、應用場景

一副地圖中有座標A和B，需要找到一條路徑（如果有的話）能從A到B，地圖中可能有河流或牆壁不能直接穿過，我們需要怎樣找到這條路徑呢？

在我們以往學習到的路徑尋找中，我們可以想到廣度優先搜索（BFS：Breadth First Search）和深度優先搜索（DFS：Depth-First-Search）進行路徑尋找。先看一下廣度優先搜索如下圖（圖片來源網上）。BFS以起點A爲圓心，先搜索A周圍的所有點，形成一個類似圓的搜索區域，再擴大搜索半徑，進一步搜索其它沒搜索到的區域，直到終點B進入搜索區域內被找到。

再看一下深度優先搜索，這裏的深度優先搜索不是所有路徑都搜索而是沿着B點方向搜索。（圖片來源網上）。DFS則是讓搜索的區域離A儘量遠，離B儘量近，比如現在你在一個陌生的大學校園裏，你知道校門口在你的北方，雖然你不知道你和校門口之間的路況如何，地形如何，但是你會盡可能的往北方走，總能找到校門口。

比起BFS，DFS因爲儘量靠近終點的原則，其實是用終點相對與當前點的方向爲導向，所以有一個大致的方向，就不用盲目地去找了，這樣，就能比BFS能快地找出來最短路徑，但是這種快速尋找默認起點A終點B之間沒有任何障礙物，地形的權值也都差不多。如果起點終點之間有障礙物，那麼DFS就會出現繞彎的情況。

圖中DFS算法使電腦一路往更右下方的區域探索，可以看出，在DFS遇到障礙物時，其實沒有辦法找到一條最優的路徑，只能保證DFS會提供其中的一條路徑（如果有的話）。

大概瞭解了BFS和DFS，對比這兩者可以看出來，BFS保證的是從起點到達路線上的任意點花費的代價最小（但是不考慮這個過程是否要搜索很多格子）；DFS保證的是通過不斷矯正行走方向和終點的方向的關係，使發現終點要搜索的格子更少（但是不考慮這個過程是否繞遠）。

A*算法的設計同時融合了BFS和DFS的優勢，既考慮到了從起點通過當前路線的代價（保證了不會繞路），又不斷的計算當前路線方向是否更趨近終點的方向（保證了不會搜索很多圖塊），是一種靜態路網中最有效的直接搜索算法。

閒談：我們知道BFS和DFS，但將這兩種思想融會貫通，創造一種新的解決問題方法（A*算法），這在思路太棒了。膜拜學習。

三、A*算法

3.1 思想

A*算法運用的是估價思想。查找過程：

在待遍歷列表中（剛開始只有點A），我們在列表中查找一個估價（當前點到終點距離估價，後續會講）最小的點（k），
對點k進行一次廣度優先查找，也就是它移動一次到底的下一個座標（右，右上，上，左上，左，左下，下，右下）不包含已經遍歷過的點和不能到達的點，將能查找的點添加到隊列中，並將點K從隊列中移除。
重複1、2步驟直到到底B點，或者隊列已經爲空說明沒有路徑可以到達點B。

運用的思想：先進行一次DFS搜索再進行一次BFS搜索，循環這個過程直到找到目標點B。

過程1：運用DFS思想，儘量找離B近的點（也就是估值最小的點）。

過程2：運用BFS思想，以點K爲圓心，搜索A周圍的所有還未搜索的點。

3.2 怎樣估價

3.2.1 公式：F = G + H。

G = 從起點 A 移動到指定方格的移動代價，沿着到達該方格而生成的路徑。我們約定直行移動一次代價是10，對角線的移動代價爲 14。（實際對角移動距離是 2 的平方根，或者是近似的 1.414 倍的橫向或縱向移動代價）。

H = 從指定的方格移動到終點 B 的估算成本。計算從當前方格橫向或縱向移動到達目標所經過的方格數，忽略對角移動，然後把總數乘以 10 。

3.2.2 計算

我們設當前點爲K

H 值很容易計算，H = （兩個點橫座標距離 + 兩個點縱座標距離） X 10

G 值計算，計算K到A的最小估價我們只需要計算K點的周圍八個點（可以被訪問且已經被訪問點）的g值+到K點的移動代價，其中最小估價即爲K點的g值，這個點我們稱爲K點的父節點。k點正在訪問，那麼它周圍至少有一個點已經被訪問了。

3.2.2 約定

在一個方格中我們將FGH標記在它的左上，左下和右下三個位置，以便我們觀察每次估價的結果。

箭頭指向的是它父節點的座標，後續找路線需要用到。

3.3 實例演示一無障礙物（對應編碼實現中測試用例9）

說明：座標訪問和父節點查找約定順序：右，右上，上，左上，左，左下，下，右下，沿X軸增加的方向爲右，沿Y軸增加的方向爲上，父節點可能會有多個，這裏選擇代價最小最後搜索的爲父節點。

座標A(2，2)，目標座標B(6，3)，已經對座標A進行了估值。

1. 對點（2，2）八個方向的座標進行估值，它們的父節點都是（2，2），最小估值座標紫色（3，3），標記紫色只是爲了方便下一次尋找。估值順序我們約定（右，右上，上，左上，左，左下，下，右下），此後我們都按照這個順序進行。

2. 對點（3，3）八個方向的座標進行估值（已經估值的不用再計算），我們稱已經估值的點爲已經被訪問，最小估值座標紫色（4，3）。父節點搜索順序約定（右，右上，上，左上，左，左下，下，右下），g值最小最後訪問的點爲父節點。如下圖。這個圖我們需要理解箭頭是怎樣確定的。例如點（4，3）它的父節點既可以是點（3，3）也可以是點（3，2），訪問順序是先訪問點（3，3）後訪問點（3，2）所以我們把點（3，2）作爲點（4，3）的父節點。

3. 對點（4，3）繼續尋找，最小估值座標紫色（5，3）

4. 對點（5，3）繼續尋找，搜索到了終點，停止搜索

5. 通過終點依次查找它們的父節點直到起點，然後將座標點逆序，就是我們要的路線了。

路線：（2，2）、（3，2）、（4，2）、（5，2）、（6，3）

3.4 實例演示二有障礙物（對應編碼實現中測試用例10）

有無障礙物處理是一樣的。

座標A(2，2)，目標座標B(6，3)，已經對座標A進行了估值。其中座標（4，1）、（4，2）、（4，3）無障礙物不能訪問

1. 對點（2，2）八個方向的座標進行估值，它們的父節點都是（2，2），最小估值座標紫色（3，3）

2. 對點（3，3）繼續尋找，最小估值座標紫色（2，3）

3. 對點（3，2）繼續尋找，最小估值座標紫色（4，4）

4. 對點（4，4）繼續尋找，最小估值座標紫色（5，3）

5. 對點（5，3）繼續尋找，搜索到了終點，停止搜索

6. 通過終點依次查找它們的父節點直到起點，然後將座標點逆序，就是我們要的路線了。

路線：（2，2）、（3，3）、（4，4）、（5，3）、（6，3）

四、編碼實現

//==========================================================================
/**
* @file    : Astar.h
* @author  : niebingyu
* @title   : A*算法
* @purpose : A*算法實現
*
* 博客：https://blog.csdn.net/nie2314550441/article/details/106733189
*/
//==========================================================================

#pragma once

#include <assert.h>
#include <vector>
#include <list>
#include <iostream>
using namespace std;

#define NAMESPACE_ASTAR namespace NAME_ASTAR {
#define NAMESPACE_ASTAREND }
NAMESPACE_ASTAR

#define GET_ARRAY_LEN(array) (sizeof(array)/sizeof(array[0]))
struct Point
{
    int x;		// 寬
    int y;		// 高
    Point(int tx = 0, int ty = 0) : x(tx), y(ty) {}

    // 兩個座標距離：橫座標距離 + 縱座標距離
    int operator - (const Point& p)
    {
        return abs(x - p.x) + abs(y - p.y);
    }

    bool operator == (const Point& p)
    {
        return x == p.x && y == p.y;
    }
};

struct PointV : public Point
{
    int value;	// 0 :無障礙; 1:有障礙
    PointV(int nx = 0, int ny = 0, int v = 0) : Point(nx, ny), value(v) {}
};

struct PointAStart : public Point
{
    int f, g, h;
    bool visited;	// 是否被訪問過，0：未被訪問，1已經被訪問
    Point parentNode;

    PointAStart(int tf = 0, int tg = 0, int th = 0, int tx = 0, int ny = 0) : Point(tx, ny), f(tf), g(tg), h(th), visited(false), parentNode() {};
    bool operator < (const PointAStart& t) { return f < t.f; }
    void SetFGH(int tf, int tg, int th) { f = tf; g = tg, h = th; }
};

// A* 算法
class AStar
{
public:
    // arr 是一個二維數組 
    // s 起點; e 終點
    vector<Point> operator()(const vector<vector<int>>& arr, Point s, Point e)
    {
        if (arr.empty() || s == e)
            return {};

        int lenY = (int)arr.size() - 1;		    // 高
        int lenX = (int)arr[0].size() - 1;		// 寬

        if (s.x > lenX || s.y > lenY || e.x > lenX || e.y > lenY)
            return {};

        if (arr[s.y][s.x] != 0 || arr[e.y][e.x] != 0)
            return {};

        for (int i = 0; i < lenY; ++i)
            assert(lenX == (int)arr[i].size() - 1);

        vector<vector<PointAStart>> pArr(lenY + 1, vector<PointAStart>(lenX + 1));	// 父結點
        list<PointAStart*> openList;		// 1 開放列表

        int g = 0, h = (s - e) * 10, f = g + h;
        PointAStart pt(f, g, h, s.x, s.y);
        pt.visited = true;
        pArr[s.y][s.x] = pt;
        openList.push_back(&pArr[s.y][s.x]);

        bool seek = true;
        const int dirs[8][3] = { {0,1,10},{1,1,14},{1,0,10},{1,-1,14},{0,-1,10},{-1,-1,14},{-1,0,10},{-1,1,14} };//8個移動方向（右，右上，上，左上，左，左下，下，右下）
        while (seek && !openList.empty())
        {
            PointAStart& p = *openList.back();
            p.visited = true;

            openList.pop_back();
            for (int i = 0; i < GET_ARRAY_LEN(dirs) && seek; ++i)
            {
                Point t(p.x + dirs[i][1], p.y + dirs[i][0]);
                // t 需要未被訪問
                if (t.x < 0 || t.x > lenX || t.y < 0 || t.y > lenY || arr[t.y][t.x] == 1 || pArr[t.y][t.x].visited)
                    continue;

                // 找父節點
                g = p.g + dirs[i][2];
                h = (t - e) * 10;
                f = g + h;
                int minf = f;
                PointAStart newPoint(f, g, h, t.x, t.y);
                newPoint.visited = 1;
                newPoint.parentNode.x = p.x;
                newPoint.parentNode.y = p.y;
                for (int j = 0; j < GET_ARRAY_LEN(dirs); ++j)
                {
                    Point pp(t.x + dirs[j][1], t.y + dirs[j][0]);  //父節點Parent Point
                                // 父節點pp, 在需要已經被訪問
                    if (pp.x < 0 || pp.x > lenX || pp.y < 0 || pp.y > lenY || arr[pp.y][pp.x] == 1 || !pArr[pp.y][pp.x].visited)
                        continue;

                    g = pArr[pp.y][pp.x].g + dirs[j][2];
                    f = g + h;
                    if (f <= minf)
                    {
                        minf = f;
                        f = g + h;

                        newPoint.SetFGH(f, g, h);
                        newPoint.parentNode = pp;
                    }

                }

                pArr[t.y][t.x] = newPoint;
                openList.push_back(&pArr[t.y][t.x]);
                orderedList(openList);

                if (t == e)
                    seek = false;
            }
        }

        if (!pArr[e.y][e.x].visited)
        {
            cout << "無法到達" << endl;
            return {};
        }
        else
        {
            vector<Point> path;
            path.push_back(e);
            Point p = pArr[e.y][e.x].parentNode;
            while (true)
            {
                if (!pArr[p.y][p.x].visited)
                {
                    cout << "無法到達" << endl;
                    return {};
                }

                path.push_back(p);
                if (p == s)
                    break;

                p = pArr[p.y][p.x].parentNode;
            }

            reverse(path.begin(), path.end());
            SetVisitedCount(pArr);	// 輔助測試，記錄訪問的結點數

            return path;
        }
    }

    // list 除最後一個結點其餘是降序排序有序列表
    void orderedList(list<PointAStart*>& list)
    {
        if (list.size() <= 1)
            return;

        PointAStart* p = list.back();
        auto it = list.rbegin();
        auto nextIt = it;
        ++nextIt;
        while (nextIt != list.rend() && **nextIt < *p)
        {
            *it = *nextIt;
            ++it;
            ++nextIt;
        }

        *it = p;
    }

    // 輔助測試，用於獲取訪問的結點數
    void SetVisitedCount(const vector<vector<PointAStart>>& pArr)
    {
        visitCount = 0;
        for (int i = 0; i < pArr.size(); ++i)
        {
            for (int j = 0; j < pArr[i].size(); ++j)
            {
                if (pArr[i][j].visited)
                    ++visitCount;
            }
        }
    }

    int visitCount;
};

//////////////////////////////////////////////////////////////////////
// 測試 用例 START
void test(const char* testName, const vector<vector<int>>& arr, Point s, Point e)
{
    AStar as;
    vector<Point> result = as(arr, s, e);

    cout << testName << "[" << as.visitCount << ", " << result.size() << "]";
    for (int i = 0; i < result.size(); ++i)
    {
        cout << ", (" << result[i].x << "," << result[i].y << ")";
    }
    cout << endl;
}

// 測試用例
void Test1()
{
    vector<vector<int>> arr =
    {
        {0,0},
    };

    Point s(0, 0);
    Point e(1, 0);
    test("Test1()", arr, s, e);
}

void Test2()
{
    vector<vector<int>> arr =
    {
        {0},
        {0}
    };

    Point s(0, 0);
    Point e(0, 1);

    test("Test2()", arr, s, e);
}

void Test3()
{
    vector<vector<int>> arr =
    {
        {0,0,},
        {0,0,},
    };

    Point s(0, 0);
    Point e(1, 1);

    test("Test3()", arr, s, e);
}

void Test4()
{
    vector<vector<int>> arr =
    {
        {0,0,0,},
        {0,0,0,},
    };

    Point s(0, 0);
    Point e(2, 1);

    test("Test4()", arr, s, e);
}

void Test5()
{
    vector<vector<int>> arr =
    {
        {0,1,0,},
        {0,1,0,},
        {0,0,0,},
    };

    Point s(0, 0);
    Point e(2, 0);

    test("Test5()", arr, s, e);
}

void Test6()
{
    vector<vector<int>> arr =
    {
        {0,0,0,0,0,0,0,0},
        {0,0,0,0,1,0,0,0},
        {0,0,0,0,1,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},
    };

    Point s(2, 2);
    Point e(6, 2);

    test("Test6()", arr, s, e);
}

void Test7()
{
    vector<vector<int>> arr =
    {
        {0,0,0,0,0,0,0,0},
        {0,0,0,0,1,0,0,0},
        {0,0,0,0,1,0,0,0},
        {0,0,0,0,1,0,0,0},
        {0,0,0,0,1,0,0,0},
        {0,0,0,0,1,0,0,0},
    };

    Point s(2, 2);
    Point e(6, 2);

    test("Test7()", arr, s, e);
}

void Test8()
{
    vector<vector<int>> arr =
    {
        {0,0,0,0,0,0,0,0,0},
        {0,0,0,0,1,1,1,1,0},
        {0,0,0,0,1,0,0,1,0},
        {0,0,0,0,1,0,0,1,0},
        {0,0,0,0,1,0,1,1,0},
        {0,0,0,0,1,0,0,0,0},
    };

    Point s(2, 2);
    Point e(6, 2);

    test("Test8()", arr, s, e);
}

void Test9()
{
    vector<vector<int>> arr =
    {
        {0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,0,0},
    };

    Point s(2, 2);
    Point e(6, 3);

    test("Test9()", arr, s, e);
}

void Test10()
{
    vector<vector<int>> arr =
    {
        {0,0,0,0,0,0,0,0,0,0},
        {0,0,0,0,1,0,0,0,0,0},
        {0,0,0,0,1,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,0,0,0,0,0,0,0,0},
    };

    Point s(2, 2);
    Point e(6, 3);

    test("Test10()", arr, s, e);
}

void Test11()
{
    vector<vector<int>> arr =
    {
        {0,0,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,1,1,1,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,0,1,0,1,0,0,0,0,0,0,0,0},
        {0,1,0,0,1,0,0,0,1,0,0,0,0,1,0,1,1,0,1,0,0,0,0,0,0,0,0},
        {0,1,1,1,1,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,1,0,1,0,0,1,0,0,0,0,0,0,0,0},
        {0,0,0,0,0,0,0,0,1,0,0,0,0,1,1,1,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,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},
    };

    Point s(2, 7);
    Point e(17, 5);

    test("Test11()", arr, s, e);
}
NAMESPACE_ASTAREND
// 測試 用例 END
//////////////////////////////////////////////////////////////////////

void AStar_Test()
{
#if 1
    NAME_ASTAR::Test1();
    NAME_ASTAR::Test2();
    NAME_ASTAR::Test3();
    NAME_ASTAR::Test4();
    NAME_ASTAR::Test5();
    NAME_ASTAR::Test6();
    NAME_ASTAR::Test7();
    NAME_ASTAR::Test8();
    NAME_ASTAR::Test9();
    NAME_ASTAR::Test10();
    NAME_ASTAR::Test11();
#endif
    //NAME_ASTAR::Test10();
}

執行結果：

五、拓展

5.1 如果現在需求有變，不能沿對角線移動，也就是隻能上下左右移動，需要怎樣實現呢？

修改第82行代碼 const int dirs[8][3] = { {0,1,10},{1,1,14},{1,0,10},{1,-1,14},{0,-1,10},{-1,-1,14},{-1,0,10},{-1,1,14} };//8個移動方向（右，右上，上，左上，左，左下，下，右下）

改爲 const int dirs[4][3] = { {0,1,10},{1,0,10},{0,-1,10},{-1,0,10} };//8個移動方向（右，上，左，下）

5.2 需求再變動一下，只能沿着上下左右移動，但向右可以一次移動1格或者2格，需要怎樣實現呢？

同理修改第82行代碼 const int dirs[8][3] = { {0,1,10},{1,1,14},{1,0,10},{1,-1,14},{0,-1,10},{-1,-1,14},{-1,0,10},{-1,1,14} };//8個移動方向（右，右上，上，左上，左，左下，下，右下）

改爲 const int dirs[5][3] = { {0,1,10}, {0,2,20},{1,0,10},{0,-1,10},{-1,0,10} };//8個移動方向（右1格，右2格，上，左，下）

六、閒談

前面我們介紹的都是起點去找終點，觀察一下下面這種情況，黃色是起點，紅色是終點。起點找終點會搜索大量無用的座標點，而如果是終點去尋找起點搜索需要座標點會少很多。那我們可不可以起點和終點一起去尋找對方呢？

參考：https://blog.csdn.net/nie2314550441/article/details/106673966

https://zhuanlan.zhihu.com/p/23199073

2020/6/14 補充說明

六、延伸擴展

已經知道A*算法過程，再逆序思索一下這個算法。起點A，終點B，假設存在一條最優的路線能從A到B。

我們繼續觀察這個圖，如果存在最優路線。到達B的上一個座標點一定是B點周圍的一個座標稱之爲父節點。也就是我們只需要找到這個點父節點。這種思路不就是動態規劃中的自頂向下。自頂向下效率不佳，我們將其轉換成自底向上求解就可以了。我們在求解動態規劃問題往往都是計算最終結果，如果需要求解這個過程是怎樣的呢？

A*算法就是這個問題的自底向上求解的過程。A*算法給我們提供了一個很好的思路，我們只需要記錄當前結果產生的父節點，通過父節點倒推這個過程。例如《算法導論——鋼條切割》需要求解的是最大價值，那最多價值切割的方法是怎樣呢？可以通過上面說的方法求解。

可以理解A*算法是結合了深度優先、廣度優先、以及動態規劃的思想。個人理解。

尋路算法——A*算法詳解並附帶實現代碼

一、前言

二、應用場景