1
完善资料让更多小伙伴认识你,还能领取20积分哦, 立即完善>
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 方法 步骤
代码参考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 红色为起点和终点 蓝色是路径 白色是墙 靠着墙不能走对角,类似象棋中的“蹩马腿”。 |
|
|
|
只有小组成员才能发言,加入小组>>
3310 浏览 9 评论
2991 浏览 16 评论
3492 浏览 1 评论
9055 浏览 16 评论
4086 浏览 18 评论
1174浏览 3评论
603浏览 2评论
const uint16_t Tab[10]={0}; const uint16_t *p; p = Tab;//报错是怎么回事?
596浏览 2评论
用NUC131单片机UART3作为打印口,但printf没有输出东西是什么原因?
2333浏览 2评论
NUC980DK61YC启动随机性出现Err-DDR是为什么?
1894浏览 2评论
小黑屋| 手机版| Archiver| 德赢Vwin官网 ( 湘ICP备2023018690号 )
GMT+8, 2024-12-21 16:30 , Processed in 1.065271 second(s), Total 50, Slave 41 queries .
Powered by 德赢Vwin官网 网
© 2015 bbs.elecfans.com
关注我们的微信
下载发烧友APP
德赢Vwin官网 观察
版权所有 © 湖南华秋数字科技有限公司
德赢Vwin官网 (电路图) 湘公网安备 43011202000918 号 电信与信息服务业务经营许可证:合字B2-20210191 工商网监 湘ICP备2023018690号