A*算法的基本原理

A*算法的基本原理

用图的形式表现地图

这里说到的图是graph,而不是map,图(graph)是由一系列的节点和连接节点的边来描述的一种模型.我们使用graph来描述代价地图的时候,只需要把地图中的位置转化为graph中的节点,用连接关系代表位置之间的可到达性,这样以来,在进行A* 算法的时候就可以直接对图(graph)进行操作,而完全抛弃原来的几何地图.
在A* 算法的实现中,是这样将地图(map)转化成图(graph)的:

• 首先,定义节点

typedef struct
{
    int x, y;
    unsigned char reachable, sur, value;
} MapNode;

节点中包括该点在地图中的坐标(x,y),地图到相邻点的可到达性(reachable),在代价地图中是否被占用(value).

• 定义图graph

static MapNode graph[Height][Width];

各节点之间的连接关系是通过每个节点的可达性(reachable)表示的.

• 将地图Map表示成图Graph的形式通过以下代码实现

void initGraph(const int map[Height][Width], int sx, int sy, int dx, int dy)
{    //地图发生变化时重新构造地
    int i, j;
    srcX = sx;    //起点X坐标
    srcY = sy;    //起点Y坐标
    dstX = dx;    //终点X坐标
    dstY = dy;    //终点Y坐标
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            graph[i][j].x = i; //地图坐标X
            graph[i][j].y = j; //地图坐标Y
            graph[i][j].value = map[i][j];
            graph[i][j].reachable = (graph[i][j].value == Reachable);    // 节点可到达性
            graph[i][j].sur = 0; //邻接节点个数
            if (!graph[i][j].reachable)
            {
                continue;
            }
            if (j > 0)
            {
                if (graph[i][j - 1].reachable)    // left节点可以到达
                {
                    graph[i][j].sur |= West;
                    graph[i][j - 1].sur |= East;
                }
                if (i > 0)
                {
                    if (graph[i - 1][j - 1].reachable
                        && graph[i - 1][j].reachable
                        && graph[i][j - 1].reachable)    // up-left节点可以到达
                    {
                        graph[i][j].sur |= North_West;
                        graph[i - 1][j - 1].sur |= South_East;
                    }
                }
            }
            if (i > 0)
            {
                if (graph[i - 1][j].reachable)    // up节点可以到达
                {
                    graph[i][j].sur |= North;
                    graph[i - 1][j].sur |= South;
                }
                if (j < Width - 1)
                {
                    if (graph[i - 1][j + 1].reachable
                        && graph[i - 1][j].reachable
                        && map[i][j + 1] == Reachable) // up-right节点可以到达
                    {
                        graph[i][j].sur |= North_East;
                        graph[i - 1][j + 1].sur |= South_West;
                    }
                }
            }
        }
    }
}

这样以来,我们可以完全抛弃之前的地图的概念,不去考虑地图几何特征,也可以抛开距离长度等要素,而是直接对图Graph进行操作.

Openlist和Closelist

A* 算法是一种基于搜索的路径规划算法,在进行搜索的时候,我们期望有一个"域"来限定我们的搜索范围,以免工作量过大限制效率,也就是说我们希望知道哪些节点是有必要搜索的,哪些节点是不需要搜索而直接忽略的.所以在A* 算法中定义了Openlist来限定需要搜索的节点范围,定义了Closelist用来忽略不需要处理的节点,也就是说,我们每检查一个节点,这个节点必然是在Openlist中而当一个节点在Closelist中时,我们可以跳过检查的步骤,而去考虑其他节点.当然Openlist和Closelist是持续修订的,当我们进入一个区域的时候,会加入应该被搜索的节点到Openlist中,而我们检查过的节点则应该放到Closelist中.

• A* 中这样定义的Openlist

typedef struct //优先队列（Open表）
{
    int length;        //当前队列的长度
    Close* Array[MaxLength];    //评价结点的指针
} Open;

• Closelist的定义为

typedef struct Close
{
    MapNode *cur;
    char vis;
    struct Close *from;
    float F, G;
    int H;
} Close;

在Closelist中包含着被检查节点的信息,所谓被检查节点的信息,就是我们搜索到某个节点的时候对其进行"检查",这种"检查"是指:

• 1.判断该节点是否可以被访问
• 2.计算该节点的估价函数

F=G+H

这里的G是指沿着历史规划路径走到被检查节点的代价,H是指被检查节点到目标点的预估代价.
在A* 算法中,我们抛弃了原来的长度信息,而是用图Graph节点之间的关系来带替,所以我们计算机器人在节点之间运动用代价来衡量,这个代价我们可以自己定义,比如说节点之间横向和纵向平移一个节点,我们用代价1来表示,沿斜线运动一个角度我们用1.41来表示代价,坐45度转向我们用代价0.1表示,90度用0.2,135度用0.5.而被检查节点到目标点的预估我们通常用曼哈顿距离来表示.

搜索过程

有了图Graph的概念和Openlist以及Closelist后,我们就可以通过搜索的方式进行路径规划了.

首先,把起点加入Openlist,重复以下步骤:

• 遍历Openlist,找到F值最小的节点,把它作为当前处理的节点,并把该节点加入Closelist中

• 对该节点的8个相邻格子进行判断,如果格子是不可抵达的或者在Closelist中,则忽略它,否则如下操作:

  a. 如果相邻格子不在Openlist中,把它加入,并将From设置为该节点和计算f,g,h值

  b.如果相邻格子已在Openlist中,并且新的G值比旧的G值小,则把相邻格子的From设置为该节点,并且重新计算f值.

直到终点加入了Openlist中,表示找到路径;或者Openlist空了,表示没有路径,循环结束.

如果开启列表已经空了,说明路径不存在.

最后从目标格开始,沿着每一格的父节点移动直到回到起始格,这就是路径.