写个代码了解A星算法

2019-01-11

转载请注明出处。https://rhirufxmbcyj.gitlab.io

前几天对象学算法时问我，把二维数组当成一个地图，地图上有一些障碍物，怎么从起点走到终点

她说从起点开始，拿到起点周围的节点，再将这些节点当成起点，挨个遍历就拿到路径了

我一听，想着以前听课听过有个算最短路径的算法叫A*算法，不过当时没注意听，也根本不懂原理，所以就搜了一些原理博客学习一下

看了几篇感觉概念大致都了解了，但是还是感觉糊里糊涂的，就是不会自己用代码实现，于是就上github上找了个源码看了看，看了别人写的实现的源码，再对照着讲原理的文章一看，豁然开朗

所以就想着把这些记录下来，把我的理解和我的代码放在一起写下来，以便于其他人学习

A*算法如何做到找到最短路径

因为A*算法是启发式搜索，利用估算函数F(x) = g(x) + h(x)算到从起点到终点可能经过的点的估算值

根据这些估算值从小到大排序，自然就可以得到最优路径

一些概念

可以先混个脸熟，后边算法中遇到再回头看

启发式搜索：就是在状态空间中的搜索对每一个搜索的位置进行评估，得到最好的位置，再从这个位置进行搜索直到目标（在这里，就是从起点开始，得到起点周围的可以”走”的点，估算这些点到终点需要的代价，选一个代价最小的，再将这个点认为是起点，重复操作，直至到达终点）
估算函数：F(x) = G(x) + H(x)
F：可以认为是起点到终点的全部代价（长度）
G：从起点到当前点的代价（长度）
H：从当前点到终点的估算代价（长度），我使用的是曼哈顿距离算法来估算的
OPEN列表：参与估算的点组成的集合（路径中所有有可能经过的点）
CLOSE列表：不需要参与估算的点组成的集合
曼哈顿距离算法：用来估算两点距离，使用公式length = abs(A.x - B.x) + abs(A.y - B.y)

算法大致介绍

这个算法长篇大论介绍原理的文章太多了，而且图文并茂，我就不多说了（懒）。。。

就简单介绍一下

将起点加入open列表
//开始循环处理
while(1)
{
    拿出open列表中第一个值（节点）
    如果该节点等于终点 则循环结束
    根据这个节点寻找四周可以走的节点，并计算他们的g和h
    将这些节点加入open列表
    从小到大排序open列表
}

动手实现

1.构造地图

在C中也就是二维数组了。但是二维数组存放的信息太少，所以我们来构造一个结构体来存放每个节点的信息。

typedef struct a_node_t
{
    int x;
    int y;
    //从起点移动到当前节点的长度
    int g;
    //当前节点到终点的估算长度
    int h;
    //状态
    int attr;
    struct a_node_t *parent;
}a_node;

其中，x、y是坐标不用说，必须记录

g和h也就是上边提到的概念，在初始构造时不需要赋值，给0就可以了

attr状态也是必须要有的，可以记录地图上不可走的点、CLOSE等属性

enum attribute
{
    ATTR_NULL,
    ATTR_OPEN,
    ATTR_CLOSE,
    ATTR_FORBID,
    //打印地图用到 最短路径的每个节点的属性是它
    ATTR_RIGHT
};

parent，他可以帮助我们拿到从起点到终点完整的路径，要不然一直找到终点，却不知如何回去，就得不到路径了

接下来就是可以用该结构体构造一个结构体二位数组，长宽根据地图定义，然后遍历赋值x、y、attr

2.开始搜索

从起点开始搜索，将起点加入open列表中

open列表本想用链表实现，后来感觉太麻烦了，所以就改用c++的vector存放了。
1
2
3
定义：vector<a_node*> open_list;

open_list.push_back(_start);
取出open列表中最小的点
1
2
3
4
//拿到open列表中f最小的节点
curr_node = open_list.at(0);
open_list.erase(open_list.begin());
curr_node->attr = ATTR_CLOSE;
从open列表中取出来之后就加入close列表中，但是我觉得close列表没有用到，只是个概念性的东西，所以我就只改了属性，没有真正的创建close列表

加入close列表的原因就是已经遍历过该节点了，避免下次重复遍历

获得该节点周围可以走的点，并加入open列表中

在这里加入了判断：如果是”墙”或者已经在CLOSE列表中或者已经加入OPEN列表了就不再加入OPEN列表了

STEP_LENGTH：这个是自定义的宏，可以假设一个格子任意个长度

int insert_neighbor_to_openlist(a_node *curr_node, a_node *_end, a_node _map[MAP_ROW][MAP_COLUMN])
{
    a_node *tmp = NULL;
    vector<a_node*>::iterator is_contain;
    //取到当前节点的上下左右节点加入open列表 并设置这些节点的parent为当前节点
    //坐标系与数组下标有所不同
    //上
    if (curr_node->x > 0)
    {
        tmp = &_map[curr_node->x-1][curr_node->y];
        is_contain = std::find(open_list.begin(), open_list.end(), tmp);
        if (tmp->attr != ATTR_FORBID && tmp->attr != ATTR_CLOSE && is_contain == open_list.end())
        {
            tmp->h = (abs(tmp->x - _end->x) + abs(tmp->y - _end->y))*STEP_LENGTH;
            tmp->g = curr_node->g + STEP_LENGTH;
            tmp->parent = curr_node;
            open_list.push_back(tmp);
        }
    }

    //下
    if (curr_node->x < MAP_ROW - 1)
    {
        tmp = &_map[curr_node->x+1][curr_node->y];
        is_contain = std::find(open_list.begin(), open_list.end(), tmp);
        if (tmp->attr != ATTR_FORBID && tmp->attr != ATTR_CLOSE && is_contain == open_list.end())
        {
            tmp->h = (abs(tmp->x - _end->x) + abs(tmp->y - _end->y))*STEP_LENGTH;
            tmp->g = curr_node->g + STEP_LENGTH;
            tmp->parent = curr_node;
            open_list.push_back(tmp);
        }
    }

    //左
    if (curr_node->y > 0)
    {
        tmp = &_map[curr_node->x][curr_node->y-1];
        is_contain = std::find(open_list.begin(), open_list.end(), tmp);
        if (tmp->attr != ATTR_FORBID && tmp->attr != ATTR_CLOSE && is_contain == open_list.end())
        {
            tmp->h = (abs(tmp->x - _end->x) + abs(tmp->y - _end->y))*STEP_LENGTH;
            tmp->g = curr_node->g + STEP_LENGTH;
            tmp->parent = curr_node;
            open_list.push_back(tmp);
        }
    }

    //右
    if (curr_node->y < MAP_COLUMN - 1)
    {
        tmp = &_map[curr_node->x][curr_node->y+1];
        is_contain = std::find(open_list.begin(), open_list.end(), tmp);
        if (tmp->attr != ATTR_FORBID && tmp->attr != ATTR_CLOSE && is_contain == open_list.end())
        {
            tmp->h = (abs(tmp->x - _end->x) + abs(tmp->y - _end->y))*STEP_LENGTH;
            tmp->g = curr_node->g + STEP_LENGTH;
            tmp->parent = curr_node;
            open_list.push_back(tmp);
        }
    }
    return 0;
}

排序open列表

因为上边取open列表中节点是取第一个节点，所以就从小到大排序，这样就可以保证每次都去寻找总代价最小的节点当下一步要遍历的节点，节省时间

//排序open列表 让f最小的节点放在最前边
sort(open_list.begin(), open_list.end(), comp_f);

bool comp_f(a_node *a, a_node *b)
{
    //比较f值
    int a_f = a->g + a->h;
    int b_f = b->g + b->h;
    return a_f < b_f;
}

结束

至此，搜索代码就结束了，可以在循环中加入判断增强健壮性，例如起点终点设成一样的，终点是不可走的区域或者遍历时拿到的节点超出了地图范围等。

可以在找到路径以后根据节点的parent从终点到起点的完整路径打印出来

void print_path(a_node *node)
{
    printf("print path backward order\n");
    while (node != NULL)
    {
        node->attr = ATTR_RIGHT;
        printf("%d,%d\n", node->x, node->y);
        node = node->parent;
    }
}

void print_map(a_node _map[MAP_ROW][MAP_COLUMN])
{
    printf("\n");
    for (int i = 0; i < MAP_ROW; i++)
    {
        for (int j = 0; j < MAP_COLUMN; j++)
        {
            if (_map[i][j].attr == ATTR_RIGHT)
                printf("* ");
            else if (_map[i][j].attr == ATTR_FORBID)
                printf("0 ");
            else
                printf("  ");
        }
        printf("\n");
    }
}

我的项目链接：https://github.com/rhirufxmbcyj/Some-Algorithm/tree/master/AStarSearch

有任何疑问或错误的地方欢迎指出