A星算法Matlab版本详细讲解
算法总览
map_astar.m
- A星算法的主函数
%% Define a small map
map = false(10);
% Add an obstacle
% 添加障碍物
map (1:5, 6) = true;
map (6,8:10) = true;
map (3:7,2) = true;
%% 设置起始点与目标点
start_coords = [1,3];
dest_coords = [1,9];
% 传入参数，进行A星算法运行
[route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true);
pause(3);
%%
start_coords = [10,1];
dest_coords = [2,9];
[route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true);
pause(3);
%%
start_coords = [6,3];
dest_coords = [8,9];
[route, numExpanded ,cost_matrix] = AStarGrid (map, start_coords, dest_coords,true);
pause(3);
%%
start_coords = [6,1];
dest_coords = [10,10];
[route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true);
pause(3);
%%
start_coords = [6,1];
dest_coords = [1,10];
[route, numExpanded ,cost_matrix] = AStarGrid(map, start_coords, dest_coords,true);
pause(3);
AStarGrid.m
- A星算法的具体实现过程
function [route,numExpanded,cost_matrix] = AStarGrid (input_map, start_coords, dest_coords, drawMap)
% Run A* algorithm on a grid.
% Inputs :
% input_map : a logical array where the freespace cells are false or 0 and
% the obstacles are true or 1
% start_coords and dest_coords : Coordinates of the start and end cell
% respectively, the first entry is the row and the second the column.
% Output :
% route : An array containing the linear indices of the cells along the
% shortest route from start to dest or an empty array if there is no
% route. This is a single dimensional vector
% numExpanded: Remember to also return the total number of nodes
% expanded during your search. Do not count the goal node as an expanded node.
% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue - on list
% 5 - green - start
% 6 - yellow - destination
cmap = [1 1 1; ...
0 0 0; ...
1 0 0; ...
0 0 1; ...
0 1 0; ...
1 1 0; ...
0.5 0.5 0.5];
colormap(cmap);
% variable to control if the map is being visualized on every
% iteration
% 通过传入变量对每次迭代是否显示进行控制
drawMapEveryTime = drawMap;
% 获取传入地图的行数和列数
[nrows, ncols] = size(input_map);
% map - a table that keeps track of the state of each grid cell
% 建一个大小为nrows × ncols的二维数组map，并将数组中的所有元素初始化为0。
% 0表示网格未被标记
map = zeros(nrows, ncols);
% 将空闲网格标为1 占用网格标为2
map(~input_map) = 1; % Mark free cells
map(input_map) = 2; % Mark obstacle cells
% Generate linear indices of start and dest nodes
%使用sub2ind函数将起始节点的行列坐标start_coords转换为地图map中对应的线性索引。
%size(map)获取地图的大小，start_coords(1)和start_coords(2)分别表示起始节点的行和列坐标。
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));
map(start_node) = 5;
map(dest_node) = 6;
% meshgrid will `replicate grid vectors' nrows and ncols to produce
% a full grid
% type `help meshgrid' in the Matlab command prompt for more information
% 创建一个大小为nrows × ncols的二维数组parent，用于存储每个节点的父节点索引。
% 将起始节点的索引值赋给parent数组对应位置，表示起始节点本身没有父节点。
parent = zeros(nrows, ncols);
parent(start_node)=start_node;
% 用于初始化启发式函数值、代价数组以及其他相关变量
[X, Y] = meshgrid (1: ncols, 1: nrows);
% 获取目标节点的坐标，将X坐标赋给变量xd，Y坐标赋给变量yd
xd = dest_coords(1);
yd = dest_coords(2);
% Evaluate Heuristic function, H, for each grid cell
% Manhattan distance
% 计算启发式函数H，即曼哈顿距离，用于估计从每个网格单元到目标节点的代价
H = abs(X - xd) + abs(Y - yd);
H = H';
% Initialize cost arrays
% 两个数组用于记录每个节点的代价函数值 F(X)+G(X)构成代价函数
% 代价数组用于记录每个节点的代价函数值，其中f表示总代价（启发式函数值加上代价函数值），g表示从起始节点到当前节点的代价。
f = Inf(nrows, ncols);% 初始化为无穷
g = Inf(nrows, ncols);
% 起始节点的代价函数值g设置为0，将起始节点的启发式函数值H设置为起始节点在启发式函数中的值
g(start_node) = 0;
f(start_node) = H(start_node);
% 初始化一个代价矩阵cost_matrix，用于记录每个节点到起始节点的代价
cost_matrix = Inf(nrows, ncols);%这个数组用于记录每个节点到起始节点的代价值
cost_matrix(start_node) = 0;%起始节点的代价值设置为0
% keep track of the number of nodes that are expanded
% 初始化一个计数器numExpanded，用于跟踪已扩展的节点数目
numExpanded = 0;
% Main Loop
while true
% Draw current map
map(start_node) = 5;%特定标记 起始节点标记为5
map(dest_node) = 6;%特定标记 目标节点标记为6
% make drawMapEveryTime = true if you want to see how the
% nodes are expanded on the grid.
if (drawMapEveryTime)
image(1.5, 1.5, map); % 使用image函数将地图绘制在图形窗口中
grid on; % 用于显示网格线
axis image; % 设置坐标轴比例一致
drawnow; % 实时更新图形窗口
end
% Find the node with the minimum f value
% 找到代价数组f中的最小代价值min_f以及对应的节点索引current
% 表示选择具有最小代价的节点作为当前节点进行扩展
[min_f, current] = min(f(:));
%如果当前节点等于目标节点或者最小代价为正无穷，则跳出循环
% 表示已经找到路径或者无法到达目标节点。
if ((current == dest_node) || isinf(min_f))
break
end
% Update input_map
map(current) = 3;%特定标记 将当前节点在地图中标记为3，表示该节点已经被扩展
%将当前节点在代价数组f中的值设为正无穷，表示不再考虑该节点，避免重复扩展
f(current) = Inf; % remove this node from further consideration
% Compute row, column coordinates of current node
% 根据当前节点的线性索引current计算其在代价数组f中的行列坐标。
% size(f)获取代价数组的大小。然后使用ind2sub函数将线性索引转换为行列坐标
[i, j] = ind2sub(size(f), current);
% *********************************************************************
if ((i>=1)&&(i<10)&&(j>=1)&&(j<=10)&&(parent(i+1,j) == 0)&& input_map(i+1,j) ~=1)
g(i+1,j) = g(i,j)+1;
f(i+1,j) = g(i+1,j)+H(i+1,j);
cost_matrix(i+1,j) = g(i+1,j)+H(i+1,j);
parent(i+1,j)= current;
numExpanded = numExpanded+1;
end
if ((i>1)&&(i<=10)&&(j>=1)&&(j<=10)&&(parent(i-1,j)==0)&& input_map(i-1,j) ~=1)
g(i-1,j)= g(i,j)+1;
f(i-1,j)= g(i-1,j)+H(i-1,j);
cost_matrix(i-1,j)= g(i-1,j)+H(i-1,j);
parent(i-1,j)= current;
numExpanded = numExpanded+1;
end
if ((i>=1)&&(i<=10)&&(j>1)&&(j<=10)&&(parent(i,j-1)==0)&& input_map(i,j-1) ~=1)
g(i,j-1)= g(i,j)+1;
f(i,j-1)= g(i,j-1)+H(i,j-1);
cost_matrix(i,j-1)= g(i,j-1)+H(i,j-1);
parent(i,j-1)=current;
numExpanded = numExpanded+1;
end
if ((i>=1)&&(i<=10)&&(j>=1)&&(j<10)&&(parent(i,j+1)==0)&& input_map(i,j+1)~=1)
g(i,j+1)= g(i,j)+1;
f(i,j+1)= g(i,j+1)+H(i,j+1);
cost_matrix(i,j+1)= g(i,j+1)+H(i,j+1);
parent(i,j+1)=current;
numExpanded = numExpanded+1;
end
%*********************************************************************
end
%% Construct route from start to dest by following the parent links
% 代码检查目标节点的代价函数值f(dest_node)是否为正无穷
% 如果是，则表示无法到达目标节点，路径为空。否则，将目标节点作为路径的起点。
if (isinf(f(dest_node)))
route = [];
else
route = dest_node;
% 使用一个循环来构建路径，从目标节点开始，通过查找父节点来逐步向前回溯，直到回溯到起始节点为止
% 在每次循环中，将当前节点的父节点加入路径中，以便构建完整的路径。
while (parent(route(1)) ~= start_node)
route = [parent(route(1)), route];
end
% Snippet of code used to visualize the map and the path
% 代码使用循环遍历路径中的节点
for k = 1:length(route) - 1
map(route(k)) = 7;% 将这些节点在地图中标记为特定的值 7
pause(0.1);
image(1.5, 1.5, map);
grid on;
axis image;
end
end
end
算法详细讲解
代码段1
cmap = [1 1 1; ...
0 0 0; ...
1 0 0; ...
0 0 1; ...
0 1 0; ...
1 1 0; ...
0.5 0.5 0.5];
colormap(cmap);
定义了一个7×3的矩阵cmap，每行包含RGB三个分量的值。这些分量定义了不同颜色的组合，每个行表示一个颜色。
使用colormap函数将定义好的颜色映射应用到当前图形窗口或坐标轴上。通过指定cmap作为参数，将颜色映射设置为cmap中定义的颜色。
代码段2
map = zeros(nrows, ncols);
map(~input_map) = 1; % Mark free cells
map(input_map) = 2; % Mark obstacle cells
在这行代码中，~input_map表示对input_map中的元素进行逻辑非运算，即将空闲区域的值变为true（逻辑值为1）而将障碍物区域的值变为false（逻辑值为0）。然后，将逻辑值为true的元素的对应位置在map中标记为1，表示该位置是空闲的。这里假设input_map是一个逻辑数组，与map具有相同的大小。
input_map表示障碍物区域的逻辑值为true。将逻辑值为true的元素的对应位置在map中标记为2，表示该位置是障碍物区域。
code 3
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node = sub2ind(size(map), dest_coords(1), dest_coords(2));
sub2ind函数的作用是将给定的行索引和列索引转换为对应的线性索引。它在处理多维数组时非常有用。让我解释一下它的用法和作用：
语法：
idx = sub2ind(size(A), sub1, sub2, ..., subN)
参数：
-
size(A): 数组A的大小，返回的线性索引将在该大小范围内。 -
sub1, sub2, ..., subN: 要转换为线性索引的各维度的子索引值。这些子索引值可以是标量、向量或矩阵。
返回值：
-
idx: 转换后的线性索引值，用于在多维数组中定位元素。
作用：
sub2ind函数的作用是根据给定的子索引值和数组的大小，将多维数组中的元素位置转换为对应的线性索引值。线性索引是一个单一的整数值，用于唯一地标识数组中的元素。这在处理多维数组时非常有用，可以将多维索引转换为线性索引进行高效的元素访问和操作。
在给定大小为[M, N]的二维数组时，sub2ind函数将二维坐标(i, j)转换为线性索引idx的公式为：
idx = (j-1) * M + i
sub2ind函数可以用于索引多维数组中的元素，例如在地图或图像处理中将二维坐标转换为线性索引以快速访问数组中的元素。
4
matlabCopy code
[X, Y] = meshgrid(1:ncols, 1:nrows);
使用meshgrid函数生成网格坐标矩阵。meshgrid函数用于在指定的一维向量范围内生成二维矩阵，其中矩阵的行表示Y坐标，列表示X坐标。在这里，1:ncols生成X坐标向量，1:nrows生成Y坐标向量，然后使用meshgrid函数生成网格坐标矩阵。
code 5
if ((i>=1)&&(i<10)&&(j>=1)&&(j<=10)&&(parent(i+1,j) == 0)&& input_map(i+1,j) ~=1)
g(i+1,j) = g(i,j)+1;
f(i+1,j) = g(i+1,j)+H(i+1,j);
cost_matrix(i+1,j) = g(i+1,j)+H(i+1,j);
parent(i+1,j)= current;
numExpanded = numExpanded+1;
end
这段代码用于判断当前节点的下方相邻节点是否可扩展。判断条件包括：下方相邻节点在地图范围内，其父节点未被设置（parent(i+1,j) == 0），下方相邻节点不是障碍物（input_map(i+1,j) ~=1）。如果满足条件，则更新下方相邻节点的代价和其他信息，包括更新到起始节点的代价g，计算代价函数值f，更新代价矩阵cost_matrix，设置父节点为当前节点parent(i+1,j) = current，并增加已扩展节点的计数器numExpanded。
类似地，下面的代码块分别用于判断当前节点的上方、左侧和右侧相邻节点是否可扩展，并进行相应的代价和信息更新。
父节点数组有什么用处
父节点数组在路径规划算法中用于记录每个节点的父节点索引。每个节点的父节点是在路径搜索过程中，到达当前节点的最佳路径上的前一个节点。
在路径规划算法中，从起始节点开始，通过不断扩展节点和更新代价的过程，寻找到达目标节点的最佳路径。父节点数组可以用于回溯路径，从目标节点开始，通过查找父节点索引，依次找到到达起始节点的最佳路径上的所有节点。
通过父节点数组，我们可以重构最佳路径，了解从起始节点到目标节点的具体路径。这在可视化路径、导航指引、路径评估等应用中非常有用。
父节点数组还可以用于其他相关操作，例如判断两个节点是否相邻，判断节点是否已经被访问等。通过访问父节点数组，我们可以获取与当前节点相关的信息，辅助路径规划算法的执行和路径搜索的优化。
H = H'有什么用
在这段代码中，计算启发式函数H时，我们使用了H = H'来对计算结果进行转置操作。这是因为在网格地图中，行表示Y坐标，列表示X坐标。而在计算启发式函数时，我们需要每个网格单元与目标节点的曼哈顿距离，这就要求我们的计算结果与地图的行列顺序保持一致。
当我们使用abs(X - xd) + abs(Y - yd)计算每个网格单元与目标节点的曼哈顿距离时，X和Y都是从meshgrid函数生成的网格坐标矩阵。默认情况下，X和Y的行表示Y坐标，列表示X坐标。然而，在地图中，我们通常使用行列表示Y坐标，列表示X坐标。因此，为了使计算结果与地图的行列顺序一致，我们需要对计算结果进行转置操作。
通过H = H'，我们将计算结果H的行列顺序进行了交换，使得H的行表示X坐标，列表示Y坐标，与地图的行列顺序一致。这样，在后续的路径规划算法中，我们可以方便地通过行和列索引来访问和处理启发式函数值H。
