1.算法原理
本来今天是想学习A*算法的,看完后觉得还是先学习下Dijkstra。
Dijkstra算法从物体所在的初始点开始,访问图中的单元。它迭代搜索单元周围的单元。该结点集从初始结点向外扩展,直到到达目标结点。
2.算法流程
①.首先创建地图,将地图分为10*10的网格,绿色为起始点,黄色为目标点,黑色为障碍物。
②.以绿色方框为中心搜索上下右左四个框,已搜索过的地方表为红色,下次搜索的中心表为蓝色。
③.如此一直搜索将达到黄色区域。
④.最短的那条路径就是我们需要找的解。
3.完整matlab代码
这是我学习的代码:
clc;
clear;
close all;
%% % set up color map for display
cmap = [1 1 1; ...% 1 - white - 空地
0 0 0; ...% 2 - black - 障碍
1 0 0; ...% 3 - red - 已搜索过的地方
0 0 1; ...% 4 - blue - 下次搜索备选中心
0 1 0; ...% 5 - green - 起始点
1 1 0];% 6 - yellow - 到目标点的路径
colormap(cmap);
map = zeros(10);
% 设置障障碍
map (1:5, 7) = 2;
map(6, 2) = 5; % 起始点
map(4, 8) = 6; % 目标点
image(1.5,1.5,map);
grid on;
axis image;
%% 建立地图
nrows = 10;
ncols = 10;
start_node = sub2ind(size(map), 6, 2);
dest_node = sub2ind(size(map), 4, 8);
% 距离数组初始化
distanceFromStart = Inf(nrows,ncols);
distanceFromStart(start_node) = 0;
% 对于每个网格单元,这个数组保存其父节点的索引。
parent = zeros(nrows,ncols);
% 主循环
while true
% 画出现状图
map(start_node) = 5;
map(dest_node) = 6;
image(1.5, 1.5, map);
grid on;
axis image;
drawnow;
% 找到距离起始点最近的节点
[min_dist, current] = min(distanceFromStart(:)); %返回当前距离数组的最小值和索引。
if ((current == dest_node) || isinf(min_dist)) %搜索到目标点或者全部搜索完,结束循环。
break;
end;
map(current) = 3; %将当前颜色标为红色。
distanceFromStart(current) = Inf; %当前区域在距离数组中设置为无穷,表示已搜索。
[i, j] = ind2sub(size(distanceFromStart), current); %返回当前位置的坐标
neighbor = [i-1,j;...
i+1,j;...
i,j+1;...
i,j-1] %确定当前位置的上下左右区域。
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
(neighbor(:,2)<1) + (neighbor(:,2)>ncols ) %判断下一次搜索的区域是否超出限制。
locate = find(outRangetest>0); %返回超限点的行数。
neighbor(locate,:)=[] %在下一次搜索区域里去掉超限点。
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) %返回下次搜索区域的索引号。
for i=1:length(neighborIndex)
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)
map(neighborIndex(i)) = 4; %如果下次搜索的点不是障碍,不是起点,没有搜索过就标为蓝色。
if distanceFromStart(neighborIndex(i))> min_dist + 1 distanceFromStart(neighborIndex(i)) = min_dist+1;
parent(neighborIndex(i)) = current; %如果在距离数组里,。
end
end
end
end
%%
if (isinf(distanceFromStart(dest_node)))
route = [];
else
%提取路线坐标
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
% 动态显示出路线
for k = 2:length(route) - 1
map(route(k)) = 7;
pause(0.1);
image(1.5, 1.5, map);
grid on;
axis image;
end
end
效果图如下
PS
①.distanceFromStart 10*10 ,初始化时,起始点为0,其余位置标记为无穷。
②.map是 10*10 ,里面存放的是颜色对应的值。
③.current 1*1,表示当前搜索的单元。
④.locate 超限单元。
⑤.neighbor 当前单元的相邻单元。
⑥.parent存放的是路径,里面的值表示的是上一次搜索的单元,这样可以反推出路径。
⑦.本例里,比如起始点这周围有四个点,搜索顺序为上下右左。