基于STM32 + LED点阵屏的A算法该怎样去实现呢

STM32 + LED点阵屏 实现寻路

A*搜寻算法


算法介绍

A算法最初发表于1968年。它可以被认为是Dijkstra算法的扩展。
由于借助启发函数的引导，A算法通常拥有更好的性能。
Dijkstra算法

在Dijkstra算法中，需要计算每一个节点距离起点的总移动代价。同时，还需要一个优先队列结构。对于所有待遍历的节点，放入优先队列中会按照代价进行排序。
在算法运行的过程中，每次都从优先队列中选出代价最小的作为下一个遍历的节点。直到到达终点为止。

启发式算法

与Dijkstra算法类似，我们也使用一个优先队列，但此时以每个节点到达终点的距离作为优先级，每次始终选取到终点移动代价最小（离终点最近）的节点作为下一个遍历的节点。

虽然启发式搜索比Dijkstra算法更快得出结果，但它所生成的路径并不是最优的，其中出现了一些绕弯路的状况。

一方面，我们需要算法有方向地进行扩散（启发式），另一方面我们需要得到尽可能最短的路径，因此A*就诞生了， 它结合了Dijkstra和启发式算法的优点。
A*算法

A*（A-Star）算法是一种静态路网中求解最短路径最有效的直接搜索方法。
原理

算法通过函数F = G + H 来计算每个节点的优先级
这里，
G= 从起点 A移动到当前方格的实际代价。
H = 从指定的方格移动到终点 B 的估算成本。使用 Manhattan 方法

步骤

• 把起点加入 open list 。
  
• 重复如下过程：
  a. 遍历 open list ，查找 F 值最小的节点，把它作为当前要处理的节点。
  b. 把这个节点移到 close list 。
  c. 对当前方格的 8 个相邻方格的每一个方格做如下操作
       ◆ 如果它是不可抵达的或者它在 close list 中，忽略它。否则，做如下操作。
       ◆ 如果它不在open list 中，把它加入 open list ，并且把当前方格设置为它的父亲，记录该方格的 F ， G 和 H 值。
       ◆ 如果它已经在open list 中，检查这条路径 ( 即经由当前方格到达它那里 ) 是否更好，用 G 值作参考。更小的 G 值表示这是更好的路径。如果G 值更小，把它的父亲设置为当前方格，并重新计算它的 G 和 F 值。（如果你的 open list 是按 F 值排序的话，改变后你可能需要重新排序。）
  d. 停止，当你把终点加入到了 open list 中，此时路径已经找到了。或者查找终点失败，并且 open list 是空的，此时没有路径。
  
• 保存路径。从终点开始，每个方格沿着父节点移动直至起点，这就是你的路径。
  
  

代码实现

代码参考A*搜寻算法百度百科。

在本例中，横向和纵向的移动代价为 10 ，对角线的移动代价为 14 。以避免浮点运算

//节点属性
#define Reachable    0 //可以到达的节点
#define Bar         1 //障碍物
#define Pass         2 //
#define Source          3 //起点
#define Destination  4 //终点


//8bits分别代表相邻8个节点是否可到达
#define North (1 << 0)
#define North_West   (1 << 1)
#define West (1 << 2)
#define South_West   (1 << 3)
#define South (1 << 4)
#define South_East    (1 << 5)
#define East (1 << 6)
#define North_East    (1 << 7)




const Point dir[8] =                 //上北下南左西右东
{
    {-1, 0},    // North  //向北移动即x-1
    {-1, -1},   // North_West //向西移动即y-1
    {0, -1},    // West
    {1, -1},   // South_West
    {1, 0},   // South
    {1, 1},  // South_East
    {0, 1},   // East
    {-1, 1}    // North_East
};
typedef struct //坐标
{
    int x, y;
}Point;


typedef struct //节点
{
    int x, y;               //节点坐标
    uint16_t reachable; //节点属性
    uint16_t sur; //相邻节点的方位
    uint16_t value;
}MapNode;


typedef struct Close //list链表成员
{
    MapNode *cur;                 //节点
    char vis;                  //有效值，是否已被放入close list
    struct Close *from;             //父节点 链表成员
    uint16_t F, G;
    uint16_t H;
}Close;


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

MapNode  graph[Height][Width];        //地图节点
Close  close[Height][Width] = {0};       //地图成员


/* 导入一张地图，设置地图各节点属性，及其可移动方向*/
void initGraph(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[j].x = i; //地图坐标X
            graph[j].y = j; //地图坐标Y
            graph[j].value = map[j];
            graph[j].reachable = (graph[j].value == Reachable);    // 节点是否可到达
            graph[j].sur = 0; //可到达邻接节点位置
            if (!graph[j].reachable)
            {
                continue;
            }
            //设置除边框外，可到达邻接节点位置
            if (j > 0)
            {
                if (graph[j - 1].reachable)     // left节点可到达
                {
                    graph[j].sur |= West;
                    graph[j - 1].sur |= East;
                }
                if (i > 0)
                {
                    if (graph[i - 1][j - 1].reachable
                        && graph[i - 1][j].reachable
                        && graph[j - 1].reachable) // up-left节点可到达
                    {
                        graph[j].sur |= North_West;
                        graph[i - 1][j - 1].sur |= South_East;
                    }
                }
            }
            if (i > 0)
            {
                if (graph[i - 1][j].reachable)     // up节点可到达
                {
                    graph[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[j + 1] == Reachable) // up-right节点可到达
                    {
                        graph[j].sur |= North_East;
                        graph[i - 1][j + 1].sur |= South_West;
                    }
                }
            }
        }
    }
}




//地图节点成员初始化操作
//导入起点及终点
void initClose(Close cls[Height][Width], int sx, int sy, int dx, int dy)
{   
   // 地图Close表初始化配置
    int i, j;
    for (i = 0; i < Height; i++)
    {
        for (j = 0; j < Width; j++)
        {
            cls[j].cur = &graph[j];         // Close表所指节点
            cls[j].vis = !graph[j].reachable;         // 能否被访问
            cls[j].from = NULL;                 // 父节点
            cls[j].G = cls[j].F = 0;
            cls[j].H = 10*abs(dx - i) + 10*abs(dy - j);    // 扩大十倍，避免浮点运算
        }
    }
    cls[sx][sy].F = cls[sx][sy].H;            //起始点评价初始值
    cls[sy][sy].G = 0;                        //移步花费代价值
    //cls[dx][dy].G = Infinity;
}

// Open表初始化
void initOpen(Open *q)    //优先队列初始化
{
    q->length = 0;        // 队内成员数初始为0
}




/*向优先队列（Open表）中添加成员
   并排序*/
void push(Open *q, Close cls[Height][Width], uint16_t x, uint16_t y, uint16_t g)
{   
    Close *t;
    int i, mintag;

    cls[x][y].G = g;    //所添加节点的坐标
    cls[x][y].F = cls[x][y].G + cls[x][y].H;

    q->Array[q->length++] = &(cls[x][y]);
    mintag = q->length - 1;
    for (i = 0; i < q->length - 1; i++) //确认最F值最小的节点
    {
        if (q->Array->F < q->Array[mintag]->F)
        {
            mintag = i;
        }
    }
    t = q->Array[q->length - 1];
    q->Array[q->length - 1] = q->Array[mintag];
    q->Array[mintag] = t;     //将F值最小节点置于表头
}


//取出Open list表中的F值最小的成员
Close* shift(Open *q)
{
    return q->Array[--q->length];
}

百度百科源代码中，对已存在Open List中的成员，并未重新判断G值。


Open q;                //Open表
Close *p;              //list表成员
int astar()
{    // A*算法遍历
    int i, curX, curY, surX, surY;    //当前节点坐标，目标节点坐标
    uint16_t surG;
  
    initOpen(&q);
    initClose(close, srcX, srcY, dstX, dstY);  //导入起点 和 终点，并将起点设置为成员
    close[srcX][srcY].vis = 1;            
    push(&q, close, srcX, srcY, 0);       //起点放入Close list，设置为不可访问
  
    while (q.length)
    {   
        p = shift(&q);
        curX = p->cur->x;
        curY = p->cur->y;
        if (!p->H)
        {
            return Sequential;
        }
        for (i = 0; i < 8; i++)
        {
            if (! (p->cur->sur & (1 << i)))
            {
                continue;
            }
            surX = curX + dir.x;
            surY = curY + dir.y;

//surG = p->G + sqrt((curX - surX) * (curX - surX) + (curY - surY) * (curY - surY));
//surG = p->G + abs(curX - surX) + abs(curY - surY);
//将距离扩大十倍，避免浮点运算
if(abs(dir.x)>0 && abs(dir.y)>0)
    surG = p->G + 14;            //1.414
else
    surG = p->G + 10;
if (!close[surX][surY].vis)             //当前节点成员不在Openlist中
{
    close[surX][surY].vis = 1;          //放入
    close[surX][surY].from = p;
    push(&q, close, surX, surY, surG);
}
else
{
if(surG < close[surX][surY].G)   //Openlist中已经存在时，如果G更小时，更新当前节点成员
{
close[surX][surY].vis = 1;
close[surX][surY].from = p;   
        push(&q, close, surX, surY, surG);
   }
}
        }
    }
    return NoSolution; //无结果
}


算法遍历后，根据终点节点close[dstX][dstY]，遍历父节点，即可获得路径。


STM32 + LED屏
地图规格40*40
红色为起点和终点 蓝色是路径 白色是墙
靠着墙不能走对角，类似象棋中的“蹩马腿”。