聊聊A*算法与Dijkstra算法的Matlab及C实现-深圳市維司達科技有限公司

A*算法matlab程序，附送c程序 Djikstra算法matlab程序代码特点： 1. matlab读入excel制作的地图，障碍物为1； 2.设置起始点和终止点，A*算法会输出一条近最优路径，因为这是启发式算法； 3.Dijkstra算法的输入是邻接矩阵，输出是一个点到所有点的最优路径

在路径规划领域，A算法和Dijkstra算法都是赫赫有名的存在。今天就来分享一下它们在Matlab中的实现，顺带送上A算法的C程序代码。

A*算法Matlab程序

A*算法是一种启发式搜索算法，它能在地图中找到一条近最优路径。先来看Matlab里如何实现从Excel制作的地图读入并进行路径搜索。

Matlab代码实现

% 读入Excel地图 map = xlsread('your_map_file.xlsx'); % 设置起始点和终止点 start = [1, 1]; goal = [size(map, 1), size(map, 2)]; % A*算法核心代码 openSet = [start]; cameFrom = containers.Map; gScore = containers.Map; gScore(start) = 0; fScore = containers.Map; fScore(start) = heuristic(start, goal); while ~isempty(openSet) [~, currentIndex] = min([fScore.values{:}]); current = openSet(currentIndex); if isequal(current, goal) path = reconstructPath(cameFrom, current); break; end openSet(currentIndex) = []; neighbors = getNeighbors(current, map); for i = 1:size(neighbors, 1) neighbor = neighbors(i, :); tentativeGScore = gScore(current) + 1; if ~gScore.isKey(neighbor) || tentativeGScore < gScore(neighbor) cameFrom(neighbor) = current; gScore(neighbor) = tentativeGScore; fScore(neighbor) = tentativeGScore + heuristic(neighbor, goal); if ~ismember(neighbor, openSet, 'rows') openSet = [openSet; neighbor]; end end end end function h = heuristic(a, b) h = abs(a(1) - b(1)) + abs(a(2) - b(2)); end function neighbors = getNeighbors(node, map) x = node(1); y = node(2); neighbors = []; if x > 1 && map(x - 1, y) ~= 1 neighbors = [neighbors; x - 1, y]; end if x < size(map, 1) && map(x + 1, y) ~= 1 neighbors = [neighbors; x + 1, y]; end if y > 1 && map(x, y - 1) ~= 1 neighbors = [neighbors; x, y - 1]; end if y < size(map, 2) && map(x, y + 1) ~= 1 neighbors = [neighbors; x, y + 1]; end end function path = reconstructPath(cameFrom, current) totalPath = {current}; while cameFrom.isKey(current) current = cameFrom(current); totalPath = [current; totalPath{:}]; end path = totalPath; end

代码分析

地图读入：xlsread('yourmapfile.xlsx')这行代码从Excel文件中读取地图数据，假设障碍物在Excel地图中标记为1。
设置起始和终止点：start = [1, 1];和goal = [size(map, 1), size(map, 2)];分别设定了起始点和终止点。这里简单地把左上角设为起始，右下角设为终止，实际应用中可以根据需求更改。
A*算法核心循环：
-openSet存放待探索的节点，一开始只包含起始点。
-cameFrom记录每个节点是从哪个节点过来的，方便最后回溯路径。
-gScore记录从起始点到每个节点的实际代价。
-fScore记录从起始点到当前节点的实际代价加上到终止点的预估代价，heuristic函数就是计算这个预估代价，这里采用曼哈顿距离计算。
- 在循环中，每次从openSet中选取fScore最小的节点进行探索，如果找到了终止点，就通过reconstructPath函数回溯得到路径。探索节点时，检查其邻居节点，如果邻居节点的gScore可以更新（即有更优路径到达该邻居），就更新相关信息并把邻居加入openSet。

A*算法C程序

下面是A*算法的C语言实现，和Matlab实现思路类似，但语法上有较大差异。

C代码实现

#include <stdio.h> #include <stdlib.h> #include <string.h> #define ROWS 10 #define COLS 10 typedef struct { int x; int y; int gScore; int fScore; int cameFromX; int cameFromY; } Node; typedef struct { Node data[ROWS * COLS]; int size; } OpenSet; void initOpenSet(OpenSet *openSet) { openSet->size = 0; } void addToOpenSet(OpenSet *openSet, Node node) { openSet->data[openSet->size++] = node; } int isOpenSetEmpty(OpenSet *openSet) { return openSet->size == 0; } Node getLowestFScoreNode(OpenSet *openSet) { int minIndex = 0; for (int i = 1; i < openSet->size; i++) { if (openSet->data[i].fScore < openSet->data[minIndex].fScore) { minIndex = i; } } Node node = openSet->data[minIndex]; openSet->data[minIndex] = openSet->data[openSet->size - 1]; openSet->size--; return node; } int isValid(int x, int y) { return x >= 0 && x < ROWS && y >= 0 && y < COLS; } int heuristic(int x1, int y1, int x2, int y2) { return abs(x1 - x2) + abs(y1 - y2); } void aStar(int map[ROWS][COLS], int startX, int startY, int goalX, int goalY) { int gScore[ROWS][COLS]; int fScore[ROWS][COLS]; int cameFromX[ROWS][COLS]; int cameFromY[ROWS][COLS]; int visited[ROWS][COLS]; memset(gScore, -1, sizeof(gScore)); memset(fScore, -1, sizeof(fScore)); memset(cameFromX, -1, sizeof(cameFromX)); memset(cameFromY, -1, sizeof(cameFromY)); memset(visited, 0, sizeof(visited)); OpenSet openSet; initOpenSet(&openSet); Node start; start.x = startX; start.y = startY; start.gScore = 0; start.fScore = heuristic(startX, startY, goalX, goalY); start.cameFromX = -1; start.cameFromY = -1; addToOpenSet(&openSet, start); gScore[startX][startY] = 0; fScore[startX][startY] = start.fScore; while (!isOpenSetEmpty(&openSet)) { Node current = getLowestFScoreNode(&openSet); if (current.x == goalX && current.y == goalY) { // 回溯路径并打印 printf("Path found: \n"); while (current.x!= -1 && current.y!= -1) { printf("(%d, %d) ", current.x, current.y); int tempX = current.cameFromX; int tempY = current.cameFromY; current.x = tempX; current.y = tempY; } printf("\n"); return; } visited[current.x][current.y] = 1; int dx[] = {-1, 1, 0, 0}; int dy[] = {0, 0, -1, 1}; for (int i = 0; i < 4; i++) { int newX = current.x + dx[i]; int newY = current.y + dy[i]; if (isValid(newX, newY) && map[newX][newY]!= 1) { int tentativeGScore = gScore[current.x][current.y] + 1; if (gScore[newX][newY] == -1 || tentativeGScore < gScore[newX][newY]) { cameFromX[newX][newY] = current.x; cameFromY[newX][newY] = current.y; gScore[newX][newY] = tentativeGScore; fScore[newX][newY] = tentativeGScore + heuristic(newX, newY, goalX, goalY); if (!visited[newX][newY]) { Node neighbor; neighbor.x = newX; neighbor.y = newY; neighbor.gScore = tentativeGScore; neighbor.fScore = fScore[newX][newY]; neighbor.cameFromX = current.x; neighbor.cameFromY = current.y; addToOpenSet(&openSet, neighbor); } } } } } printf("No path found.\n"); }

C代码分析

数据结构定义：定义了Node结构体来存储节点信息，包括坐标、gScore、fScore以及父节点坐标。OpenSet结构体用于存放待探索节点。
初始化和操作OpenSet：initOpenSet函数初始化OpenSet，addToOpenSet函数添加节点到OpenSet，isOpenSetEmpty函数判断OpenSet是否为空，getLowestFScoreNode函数从OpenSet中取出fScore最小的节点。
A*算法主体：和Matlab实现类似，初始化各种分数数组和访问标记数组。从起始点开始，在循环中不断取出fScore最小的节点进行探索，如果找到终止点则回溯路径并打印。探索邻居节点时，更新分数和父节点信息，并将符合条件的邻居加入OpenSet。

Dijkstra算法Matlab程序

Dijkstra算法是一种基于广度优先搜索的算法，用于在加权图中找到从一个给定顶点到所有其他顶点的最短路径。这里假设输入是邻接矩阵。

Matlab代码实现

% 假设邻接矩阵 adjMatrix = [0 1 0 1 0; 1 0 1 0 0; 0 1 0 1 1; 1 0 1 0 1; 0 0 1 1 0]; numNodes = size(adjMatrix, 1); startNode = 1; distance = inf(numNodes, 1); distance(startNode) = 0; visited = false(numNodes, 1); while any(~visited) [~, currentNode] = min(distance(~visited)); currentNode = find(~visited, 1, 'first'); visited(currentNode) = true; neighbors = find(adjMatrix(currentNode, :)); for i = 1:numel(neighbors) neighbor = neighbors(i); newDistance = distance(currentNode) + 1; if newDistance < distance(neighbor) distance(neighbor) = newDistance; end end end

代码分析

邻接矩阵定义：这里简单定义了一个邻接矩阵adjMatrix，实际应用中可以从文件读入或根据具体需求生成。
初始化：distance数组记录从起始点到每个节点的距离，初始化为无穷大，起始点距离设为0。visited数组标记节点是否已访问。
Dijkstra核心循环：在循环中，每次从未访问节点中选取距离最小的节点作为当前节点，标记为已访问。然后遍历当前节点的邻居，更新邻居到起始点的距离，如果通过当前节点到达邻居的距离更短，则更新distance数组。

通过以上Matlab和C语言的代码实现，希望能帮助大家更好地理解A*算法和Dijkstra算法在路径规划中的应用。不同语言实现虽然语法不同，但核心思路保持一致，大家可以根据实际场景灵活选择。