北太天元动画演示A星算法的计算过程
% 北太天元实现 A星 搜索算法, 增加了动画显示openSet和closedSet的变化过程
% @地图[input] 是一个取值为布尔值的方阵，表示网格化的地图上的点
% 其中取值为true的点表示可以经过，取值为false的点表示不能经过
% @耗时[input] 是一个和地图的大小一样的矩阵， 耗时(i,j) 表示到达
% (i,j)点走单位距离需要耗费的时间.
% @起点[input] 是1x2的矩阵或者1x1的矩阵，记录出发点的序号，
% 如果是1x2的矩阵[m,n]，那么 起点 的行列号是(m,n)，
% 如果 起点 是一个1x1的矩阵,那么认为它的值是起点的线性序号 ind
% 和行列号 (m,n) 的关系是
% ind = (n-1)*矩阵的行数 + m,
% @终点[input] 是1x2的矩阵或者1x1的矩阵，记录 终点 的序号，
% 如果是1x2的矩阵[m,n]，那么 终点 的行列号是(m,n)，
% 如果 终点 是一个1x1的矩阵,那么认为它的值是 终点 的线性序号 ind,
% 和行列号(m,n)关系是
% ind = (n-1)*矩阵的行 + m,
% @路线坐标[output] 把路线上的点的线性序号转成行列号
% @路线[output] 是一个向量，记录着从 起点 到 终点 的路线上经过的所有的点的线性序号
% %耗时_起终[output] 路线上的总耗时
%例1:
load_plugin("time");
clc
clf
clear all
close all
m = 10;
地图 = zeros(m);
地图(1,:) = 1;
地图(:,end) = 1;
for i=1:m
地图(i,i)=1;
end
耗时 = ones(size(地图));
% 耗时(3,3) = 1000;
[路线坐标, 路线, 耗时_起终] = a_星算法(logical(地图), 耗时, [1,1], [m,m])
hold on
line( [-1, m+1] , [-1,-1] )
line( [-1, m+1] , [m+1,m+1] )
line( 路线坐标(2,:), 路线坐标(1,:) )
text(路线坐标(2,1), 路线坐标(1,1), '起点','FontSize',24);
text( 路线坐标(2,end),路线坐标(1,end), '终点', 'FontSize',24);
for i=2:(size(路线坐标,2)-1)
text( 路线坐标(2,i),路线坐标(1,i), ...
['(',num2str(路线坐标(1,i)),',',num2str(路线坐标(2,i)),')'],'FontSize',24);
end
hold off
% 地图
% 1 1 1 1 1
% 0 1 0 0 1
% 0 0 1 0 1
% 0 0 0 1 1
% 0 0 0 0 1
% 地图到达每个节点的单位距离的耗时
% 1 1 1 1 1
% 1 1 1 1 1
% 1 1 1 1 1
% 1 1 1 1 1
% 1 1 1 1 1
/*
路线坐标 =
2x5 double
1 2 3 4 5
1 2 3 4 5
路线 =
1x5 double
1 7 13 19 25
耗时_起终 =
1x1 double
5.6569
*/
function [路线坐标, 路线, 耗时_起终] = a_星算法(地图, 耗时, 起点, 终点)
if ~islogical(地图)
error('输入变量 地图 必须取值为布尔矩阵')
end
if ~isa(耗时, 'double')
error('输入变量 耗时 必须是double矩阵')
end
% 避免0做除数 (而且如果用户输入的耗时是负数，也修正为eps)
耗时( 耗时 < 0) = eps;
耗时单位 = min(min(耗时));
% 把耗时无量gang化
耗时 = 耗时 / 耗时单位;
% 默认返回值 - 空表示寻找路线失败
路线 = [];
路线坐标 = zeros(2,0);
耗时_起终 = inf;
地图尺寸 = size(地图);
地图线性尺寸 = numel(地图尺寸);
% 用起点初始化开放集(open set 这和数学里的开集的定义不同)
openSet = false(地图尺寸);
openSet(起点) = true;
% 初始化闭合集(closed set 和数学的闭集的定义不同)
closedSet = false(地图尺寸);
heatmap(-double(openSet)+2*double(closedSet))
title("紫色是开放集")
pause(1);
父节点 = zeros(1, 地图线性尺寸);
% 检查起点和终点是否是线性序号
if( length(起点) ~=1 && length(起点) ~= 2)
disp("输入参数 起点 应该是 1x1 或者1x2 的矩阵");
return;
end
if( length(终点) ~=1 && length(终点) ~= 2)
disp("输入参数 终点点 应该是 1x1 或者1x2 的矩阵");
return;
end
if( length(起点) == 2 )
起点 = (起点(2)-1)*地图尺寸(1) + 起点(1);
end
if( length(终点) == 2 )
终点 = (终点(2)-1)*地图尺寸(1) + 终点(1);
end
耗时_from起点 = inf(地图尺寸(1),地图尺寸(2));
耗时_from起点(起点) = 0;
%线性序号 -> 行列号
[gr, gc] = ind2sub(地图尺寸, 终点);
估计耗时_距离终点 = inf(地图尺寸(1),地图尺寸(2));
估计耗时_距离终点(起点) = compute_估计耗时(地图尺寸, 起点, gr, gc);
const_根号2 = sqrt(2);
% 只要 开放集 非空 就循环
while any(openSet(:) > 0)
% 估计耗时_距离终点 的最小值就是
% 就是 开放集 中距离 终点 的估计距离的最小值，因为
% 当一个点被剔除出开放集的时候，相应的 估计耗时_距离终点 已经在前面修改为 inf 了。
[~, 活动点] = min(估计耗时_距离终点(:));
% 如果 我们已经到了终点
if 活动点 == 终点
% 获取 从 起点 到 终点 的 路线
路线 = 获取路线(父节点, 活动点);
路线 = 路线(end:-1:1);
[r,c] = ind2sub(地图尺寸, 路线);
路线坐标 = double([r;c]);
耗时_起终 = 估计耗时_距离终点(终点)*耗时单位;
return
end
%线性序号 -> 行列号
[rc, cc] = ind2sub(地图尺寸, 活动点);
% 把 活动点 从开放集 中剔除
openSet(rc, cc) = false;
% 把 活动点 放入 闭合集
closedSet(rc, cc) = true;
heatmap(-double(openSet)+2*double(closedSet))
title("红色是闭合集，紫色是开放集")
pause(1);
估计耗时_距离终点(rc, cc) = inf;
耗时_from起点_to_活动点 = 耗时_from起点(rc, cc);
% 获得所有的邻居点，这里考虑的是8个邻居，包括上下左右 和
% 左上、右上、左下、右下8个点
% 第一列是行号，第二列是列号，第三列是和活动点的距离
信息_邻居点s = [ ...
rc + 1, cc + 1, const_根号2 ; ...
rc + 1, cc + 0, 1 ; ...
rc + 1, cc - 1, const_根号2 ; ...
rc + 0, cc - 1, 1 ; ...
rc - 1, cc - 1, const_根号2 ; ...
rc - 1, cc - 0, 1 ; ...
rc - 1, cc + 1, const_根号2 ; ...
rc - 0, cc + 1, 1 ; ...
];
%检查邻居点是否在地图范围之内
有效性检验_row = 信息_邻居点s(:,1) >= 1 & 信息_邻居点s(:,1) <= 地图尺寸(1);
有效性检验_col = 信息_邻居点s(:,2) >= 1 & 信息_邻居点s(:,2) <= 地图尺寸(2);
信息_邻居点s = 信息_邻居点s(有效性检验_row & 有效性检验_col, :);
% 行列号 -> 线性序号
邻居点 = sub2ind(地图尺寸, 信息_邻居点s(:,1), 信息_邻居点s(:,2) );
% 只保留在地图中，且不在 闭合集 中的 邻居点
线性序号_地 = 地图(邻居点) & ~closedSet(邻居点);
邻居点 = 邻居点(线性序号_地);
% 活动点 到 这些筛选出的 邻居点 的距离
距离 = 信息_邻居点s(线性序号_地, 3);
% 把每一个筛选出的 邻居点 放入 开放集
openSet(邻居点) = true;
heatmap(-double(openSet)+2*double(closedSet))
title("红色是闭合集，紫色是开放集")
pause(2);
% 暂定_GSCORE 是从 起点 经过 活动点 这个点 到 邻居点的 耗时
暂定_耗时_起邻 = 耗时_from起点_to_活动点 + 耗时(邻居点) .* 距离;
% IXBETTER 指示 经过 活动点 是否 找到了更短的耗时路径
ixBetter = 暂定_耗时_起邻 < 耗时_from起点(邻居点);
bestNeighbors = 邻居点(ixBetter);
% 对于 找到了更短路径的 邻居点，修改它的父节点为 活动点
% 修改它的 耗时_from起点 为 经由 活动点 得到的 暂定_耗时_起邻
父节点(bestNeighbors) = 活动点;
耗时_from起点(bestNeighbors) = 暂定_耗时_起邻(ixBetter);
估计耗时_距离终点(bestNeighbors) = 耗时_from起点(bestNeighbors) ...
+ compute_估计耗时(地图尺寸, bestNeighbors, gr, gc);
end % while
end
% 返回路径, 用的方法就是从活动点 活动点 利用父节点一个一个向前回溯
function p = 获取路线(父节点, 活动点)
序号_非零点 = find(父节点);
p = nan(1, length(序号_非零点));
p(1) = 活动点;
next = 1;
while any(活动点 == 序号_非零点)
活动点 = 父节点(活动点);
next = next + 1;
p(next) = 活动点;
end
p(isnan(p)) = [];
end
% @地图尺寸[input] 是一个1x2的矩阵，给出地图矩阵的行数和列数
% @出发点们 [input] 是一个1xk 的矩阵, 是k个出发点的线性序号
% @终点行号 [input] 是终点的 行号
% @终点列号 [input] 是终点的 列好
% @耗时_估计 [output] 是估算从 出发点们 到 终点的 估计耗时 (不是精确耗时)
function 耗时_估计 = compute_估计耗时(地图尺寸, 出发点们, 终点行号, 终点列号)
[行号_出s,列号_出s] = ind2sub(地图尺寸, 出发点们);
耗时_估计 = sqrt((行号_出s - 终点行号).^2 + (列号_出s - 终点列号).^2);
end
