STM32 + LED点阵屏 实现寻路
算法介绍
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[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;
}
}
}
}
}
}
//地图节点成员初始化操作
//导入起点及终点
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[i][j].cur = &graph[i][j]; // Close表所指节点
cls[i][j].vis = !graph[i][j].reachable; // 能否被访问
cls[i][j].from = NULL; // 父节点
cls[i][j].G = cls[i][j].F = 0;
cls[i][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[i]->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[i].x;
surY = curY + dir[i].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[i].x)>0 && abs(dir[i].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
红色为起点和终点 蓝色是路径 白色是墙
靠着墙不能走对角，类似象棋中的“蹩马腿”
参考链接
- https://blog.csdn.net/hitwhylz/article/details/23089415
- http://www.gamedev.net/reference/articles/article2003.asp
- https://baike.baidu.com/item/A%2A%E6%90%9C%E5%AF%BB%E7%AE%97%E6%B3%95/6773036?fr=aladdin
- https://zhuanlan.zhihu.com/p/54510444
- https://blog.csdn.net/yuxuan20062007/article/details/80914210?utm_medium=distribute.pc_relevant.none-task-blog-title-2&spm=1001.2101.3001.4242
扫一扫
1649
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
