A*算法及相关拓展算法

1.Dijkstra算法与最佳优先搜索算法

．
　　Dijkstra算法是一种典型的单源最短路径算法，属于广度优先算法。
　　Dijkstra算法从物体所在的初始点开始，访问图中的结点。它迭代检查待检查结点集中的结点，并把和该结点最靠近的尚未检查的结点加入待检查结点集。
　　该结点集从初始结点向外扩展，直到到达目标结点。
　　Dijkstra算法保证能找到一条从初始点到目标点的最短路径，只要所有的边都有一个非负的代价值。（我说“最短路径”是因为经常会出现许多差不多短的路径。）
　
　　下图是 Dijkstra算法在无障碍情况下的搜索过程。

1.1 无障碍情况

　　最佳优先搜索（BFS）是一种启发式的算法，通过计算任意点到目标点的代价值。能够更快的找到一条路径，但不能保证是最短路径。
　　下图是无障碍情况下，越黄的结点代表越高的启发式值（移动到目标的代价高），而越黑的结点代表越低的启发式值（移动到目标的代价低）。

1.2 有障碍情况

这表明了与Dijkstra 算法相比，BFS运行得更快。在无障碍的情况下，可以达到非常不错的效果。但是在有障碍情况下，BFS算法虽然能找到一条有效路径，却无法保证路径最短。


BFS基于的是一种贪心策略，仅仅考虑到达目标的距离代价，而忽略了当前已花费的代价。
D算法运行速度相对较慢，但能保证找到一条最短路径。

2. A*算法

A算法正是将两个算法的特点进行融合，是一种启发式的广度搜素算法。
　　把Dijkstra算法（靠近初始点的结点）和BFS算法（靠近目标点的结点）的信息块结合起来。.g(n)表示从初始结点到任意结点n的代价，h(n)表示从结点n到目标点的启发式评估代价。当从初始点向目标点移动时，A权衡这两者。每次进行主循环时，它检查f(n)最小的结点n，其中f(n) = g(n) + h(n)。
　　
　　

2.1 启发h(n)

• 一种极端情况，如果h(n)是0，则只有g(n)起作用，此时A*演变成Dijkstra算法，这保证能找到最短路径。
• 另一种极端情况，如果h(n)比g(n)大很多，则只有h(n)起作用，A*演变成BFS算法

如何调整h(n)需要参考D算法和BSF算法的特点进行尝试。我们想最快地得到最短路径。如果我们的目标代价太低，我们仍会得到最短路径，不过速度变慢了；如果我们的目标代价太高，那我们就放弃了最短路径，但A*运行得更快。
构造精确启发函数的一种方法是预先计算任意一对结点之间最短路径的长度。如果在2D栅格地图中，直接计算任意点距目标点的实际距离值作为代价值即可得到较好的结果，如果是3D地图，需要考虑到机器人在各地形的移动能力以确定合适的目标代价值。

3. 网格地图的距离

3.1 欧式距离

在一个N维度的空间里，求两个点的距离，这个距离肯定是一个大于等于零的数字，那么这个距离需要用两个点在各自维度上的坐标相减，平方后加和再开方。

3.2 曼哈顿距离

曼哈顿距离也叫”曼哈顿街区距离”。想象你在曼哈顿街道上，从一个十字路口开车到另一个十字路口，驾驶距离就是这个“曼哈顿距离”。

3.3夹角余弦（向量部分）

也叫余弦相似度，是用向量空间中两个向量夹角的余弦值作为衡量两个个体间差异的大小的度量。如果两个向量的方向一致，即夹角接近零，那么这两个向量就越相近。要确定两个向量方向是否一致，要用到余弦定理计算向量的夹角。

3.4 切比雪夫距离(对角线距离)

切比雪夫距离公式简单理解为就是各坐标数值差的最大值

从实际的A*算法运行效果来看，使用曼哈顿距离作为距离测量值使得算法能有较好的效果。

4.算法实现

如果不考虑具体实现代码，A*算法是相当简单的。有两个集合，OPEN集和CLOSED集。其中OPEN集保存待考查的结点。开始时，OPEN集只包含一个元素：初始结点。CLOSED集保存已考查过的结点。开始时，CLOSED集是空的。如果绘成图，OPEN集就是被访问区域的边境（frontier）而CLOSED集则是被访问区域的内部（interior）。每个结点同时保存其父结点的指针因此我们可以知道它是如何被找到的。

在主循环中重复地从OPEN集中取出最好的结点n（f值最小的结点）并检查之。如果n是目标结点，则我们的任务完成了。否则，结点n被从OPEN集中删除并加入CLOSED集。然后检查它的邻居n’。如果邻居n’在CLOSED集中，那么它是已经被检查过的，所以我们不需要考虑它*；如果n’在OPEN集中，那么它是以后肯定会被检查的，所以我们现在不考虑它*。否则，把它加入OPEN集，把它的父结点设为n。到达n’的路径的代价g(n’)，设定为g(n) + movementcost(n, n’)。

(*)这里我忽略了一个小细节。你确实需要检查结点的g值是否更小了，如果是的话，需要重新打开（re-open）它。

使用优化队列实现了A*算法，可以直接运行

#include <cstdio>
#include <iostream>
#include <queue>
#include <vector>
#include <time.h>
#include <algorithm>
using namespace std;
//地图
int raw[15][15] = {
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 2, 0, 0, 0, 1, 0, 3, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1},
    {1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1}};
enum Type
{
    Road,
    Wall,
    Start,
    End
    //路，墙，起点，终点
};
int endX = 0, endY = 0;
struct node
{
    int x, y;
    int f, g, h; // f = g + h;
    int type;
    bool is_open;
    bool is_close;
    node *parent = NULL;
    void print()
    {
        printf("%d   %d: f: %d  g: %d  h: %d   type: %d\n", x, y, f, g, h, type);
    }
    bool operator<(const node &cmp) const
    {
        //这里是给优先队列排序用的，优先队列默认大的在前面，所以我们写<的时候判断用>号
        return f > cmp.f;
    }
} map[15][15];
priority_queue<node> open; //优化队列
/*判断是否为边界*/
bool inside(int x, int y, int n, int m)
{
    return x >= 0 && y >= 0 && x < n && y < m;
}
void A_()
{
    /* 遍历open队列 */
    while (!open.empty())
    {
        /* 取出队首元素进行处理 */
        node cur = open.top();
        open.pop();
        /* 将当前点加入close队列 */
        map[cur.x][cur.y].is_close = true;
        /* 访问当前点四周的8个点 */
        for (int i = -1; i <= 1; i++)
            for (int j = -1; j <= 1; j++)
            {
                if (i == j && i == 0) //i=j=0 是当前点
                    continue;
                /* 判断当前点是否为边界点 */
                if (!inside(cur.x + i, cur.y + j, 15, 15))
                    continue;
                /* 不访问open和close中的点 */
                if (map[cur.x + i][cur.y + j].is_close || map[cur.x + i][cur.y + j].is_open)
                    continue;
                /* 不访问障碍物点 */
                if (map[cur.x + i][cur.y + j].type == Wall)
                    continue;

                node &u = map[cur.x + i][cur.y + j];
                /* 设置当前点的父节点 */
                u.parent = &(map[cur.x][cur.y]);
                /* 更新到起点的距离 */
                u.g = map[cur.x][cur.y].g + ((i && j) ? 14 : 10);
                /* 曼哈顿距离更新到终点距离 */
                u.h = abs(u.x - map[endX][endY].x) + abs(u.y - map[endX][endY].y);
                /* 更新f的值 */
                u.f = u.g + u.f;
                /* 加入open表 */
                u.is_open = true;
                open.push(u);
            }
    }
}
void CreateMap()
{
    for (int i = 0; i < 15; i++)
        for (int j = 0; j < 15; j++)
        {
            map[i][j].x = i;
            map[i][j].y = j;
            map[i][j].type = raw[i][j];
            map[i][j].f = map[i][j].g = map[i][j].h = 0;
            map[i][j].is_open = map[i][j].is_close = false;
            if (map[i][j].type == Start)
            {
                /* 将初始点加入open */
                map[i][j].is_open = true;
                open.push(map[i][j]);
            }
            if (map[i][j].type == End)
            {
                endX = i;
                endY = j;
            }
        }
}
int main()
{
    /* 创建地图 */
    CreateMap();
    /* A*算法 */
    A_();
    if (map[endX][endY].is_close)
    { //从终点走回起点
        int nx = endX, ny = endY;
        while (map[nx][ny].type != Start)
        {
            node *x = map[nx][ny].parent;
            nx = x->x;
            ny = x->y;
            if (map[nx][ny].type != Start)
                g[nx][ny] = 6;
        }
    }
    /* 输出结果地图 */
    for (int i = 0; i < 15; i++)
    {
        for (int j = 0; j < 15; j++)
        {
            printf("%d ", g[i][j]);
        }
        puts("");
    }
    return 0;
}