**
A*算法涉及时间的路径规划
**
创建4个.m文件，拷贝进去，直接可以运行
test_hash.m
%构造一个包含障碍物信息的map对象
keySet = {'00100601', '00508902', '09807705', '06603398'};
valueSet = [1, 2, 3, 4];
mapObj = containers.Map(keySet,valueSet);
%查找映射中的值
str = [num2str(5,'%03d'), num2str(89,'%03d'), num2str(2,'%02d')];
disp(str);
object_1 = mapObj('00508902');
disp(object_1);
object_1 = mapObj(str);
disp(object_1);
disp('hello');
%向映射中添加单个键和值
mapObj('04408278') = 1;
%获取映射中的键或值
allkeys = keys(mapObj);
disp(allkeys);
allvalues = values(mapObj);
disp(allvalues);
partvalues = values(mapObj, {'00100601', '00508902'});
disp(partvalues);
%删除键和值
remove(mapObj, {'00100601', '00508902'});
allvalues = values(mapObj);
disp(allvalues);
%double 转 char
%str = num2str(A);
%str = [num2str(1,'%03d'), num2str(2,'%03d'), num2str(3,'%03d')]
test_Astar.m
%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);
space_time_map = ones(10, 10, 100);
space_map = ones(10, 10);
[nrows, ncols] = size(space_map);
start_coords = [5, 3];
dest_coords = [10, 5];
%construct the graph
space_time_map(start_coords(1), start_coords(2), :) = 5;
space_time_map(dest_coords(1), dest_coords(2), :) = 6;
for t=1:100
tt = rem(t, 10);
switch tt
case 0
space_time_map(4, 1:5, t) = 2*ones(1,5);
space_time_map(7, 6:10, t) = 2*ones(1,5);
case 1
space_time_map(4, 2:6, t) = 2*ones(1,5);
space_time_map(7, 5:9, t) = 2*ones(1,5);
case 2
space_time_map(4, 3:7, t) = 2*ones(1,5);
space_time_map(7, 4:8, t) = 2*ones(1,5);
case 3
space_time_map(4, 4:8, t) = 2*ones(1,5);
space_time_map(7, 3:7, t) = 2*ones(1,5);
case 4
space_time_map(4, 5:9, t) = 2*ones(1,5);
space_time_map(7, 2:6, t) = 2*ones(1,5);
case 5
space_time_map(4, 6:10, t) = 2*ones(1,5);
space_time_map(7, 1:5, t) = 2*ones(1,5);
case 6
space_time_map(4, 5:9, t) = 2*ones(1,5);
space_time_map(7, 2:6, t) = 2*ones(1,5);
case 7
space_time_map(4, 4:8, t) = 2*ones(1,5);
space_time_map(7, 3:7, t) = 2*ones(1,5);
case 8
space_time_map(4, 3:7, t) = 2*ones(1,5);
space_time_map(7, 4:8, t) = 2*ones(1,5);
case 9
space_time_map(4, 2:6, t) = 2*ones(1,5);
space_time_map(7, 5:9, t) = 2*ones(1,5);
end
end
%initial parent array, and H
parent = zeros(10, 10, 100);
[X, Y] = meshgrid(1:ncols, 1:nrows);
xd = dest_coords(1);
yd = dest_coords(2);
H =abs(X-xd) + abs(Y-yd);
H = H';
%initial cost arrays
g = inf(10, 10, 100);
f = inf(10, 10, 100);
g(start_coords(1), start_coords(2), 1) = 0;
f(start_coords(1), start_coords(2), 1) = H(start_coords(1), start_coords(2));
end_time = 1;
while true
%find the node with the minimum f values
[min_f, current] = min(f(:));
[current_x, current_y, current_t] = ind2sub(size(space_time_map), current);
if((current_x == dest_coords(1) && current_y == dest_coords(2)) || isinf(min_f))
end_time = current_t;
break;
end
%add it to close list
space_time_map(current) = 3;
f(current) = Inf;
%compute row, column, layer
[i, j, k] = ind2sub(size(space_time_map), current);
neighbours = [i-1, j, k+1;
i+1, j, k+1;
i, j-1, k+1;
i, j+1, k+1;
i, j, k+1];
for index = 1:size(neighbours, 1)
px = neighbours(index, 1);
py = neighbours(index, 2);
pt = neighbours(index, 3);
if (px > 0 && px < 11 && py > 0 && py < 11 )
sub_neighbours = sub2ind(size(space_time_map), px, py, pt);
if (space_time_map(sub_neighbours) ~= 2 && space_time_map(sub_neighbours) ~= 5 && space_time_map(sub_neighbours) ~= 3)
if(g(current) + H(px, py) < f(sub_neighbours) )
if (px ==i && py ==j)
g(sub_neighbours) = g(current) + 0.5;
else
g(sub_neighbours) = g(current) + 1;
end
f(sub_neighbours) = g(sub_neighbours) + H(px, py);
parent(sub_neighbours) = current;
space_time_map(sub_neighbours) = 4;
end
end
end
end
end
for h = 1:end_time-1
image(1.5, 1.5, space_time_map(:, :, h));
grid on;
axis image;
drawnow;
pause(0.1);
end
%show the route
dest_node = sub2ind(size(space_time_map), dest_coords(1), dest_coords(2), end_time);
route = [];
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
for i = 1:size(route, 2)-1
[temp1, temp2, temp3] = ind2sub(size(space_time_map), route(i));
temp = [temp1, temp2, temp3];
space_map(start_coords(1), start_coords(2)) = 5;
space_map(dest_coords(1), dest_coords(2)) = 6;
temp_array_1 = space_time_map(4, :, i);
temp_array_2 = space_time_map(7, :, i);
for j = 1:size(temp_array_1, 2)
if temp_array_1(j) ~=2
temp_array_1(j) =1;
end
if temp_array_2(j) ~=2
temp_array_2(j) =1;
end
end
space_map(4, :) = temp_array_1;
space_map(7, :) = temp_array_2;
space_map(temp1, temp2) = 7;
image(1.5, 1.5, space_map);
grid on;
axis image;
drawnow;
pause(1);
disp(temp);
end
Time_Astar_For_Cooperative_Path_Finding.m
function [route] = Time_Astar_For_Cooperative_Path_Finding(start_coords, end_coords, space_time_map, reservation_table)
%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);
[size_x, size_y, size_t] = size(space_time_map);
%add the start point and the end point
space_time_map(start_coords(1), start_coords(2), :) =5;
space_time_map(end_coords(1), end_coords(2), :) =6;%给三维矩阵第三维中前两维的位置赋值给5和6
%initial parent array and Heuristic array
parent = zeros(size_x, size_y, size_t);
[X, Y] = meshgrid(1:size_y, 1:size_x);
xd = end_coords(1);
yd = end_coords(2);
H =abs(X-xd) + abs(Y-yd);
H = H';
%initial cost arrays
g = inf(size_x, size_y, size_t);
f = inf(size_x, size_y, size_t);
g(start_coords(1), start_coords(2), 1) = 0;
f(start_coords(1), start_coords(2), 1) = H(start_coords(1), start_coords(2));
end_time = 1;
while true
%find the node with the minimum f values
[min_f, current] = min(f(:));
[current_x, current_y, current_t] = ind2sub(size(space_time_map), current);
if((current_x == end_coords(1) && current_y == end_coords(2)) || isinf(min_f))
end_time = current_t;
disp('hheheh');
break;
end
%add the current node to close list
space_time_map(current) = 3;
f(current) = Inf;
[i, j, k] = ind2sub(size(space_time_map), current);
neighbours = [i-1, j, k+1;
i+1, j, k+1;
i, j-1, k+1;
i, j+1, k+1;
i, j, k+1];
for index = 1:size(neighbours, 1)
px = neighbours(index, 1);
py = neighbours(index, 2);
pt = neighbours(index, 3);
% judge whether out of bound or not
if (px >0 && px<=size_x && py >0 && py<= size_y )
sub_neighbours = sub2ind(size(space_time_map), px, py, pt);
%judge whether the node is obstacle, start point, end
%point, or not
if(space_time_map(sub_neighbours) ~= 2 && space_time_map(sub_neighbours) ~= 5 && space_time_map(sub_neighbours) ~= 3)
%judge whether the node has less f
if(g(current) +1+ H(px, py) < f(sub_neighbours))
%judge whether the node is in reservation table or not
if (~reservation_table(sub_neighbours ))
%judge whether the special action
[special_action] = Special_action(current, sub_neighbours,reservation_table);
if (~special_action)
g(sub_neighbours) = g(current) + 1;
f(sub_neighbours) = g(sub_neighbours) + H(px, py);
parent(sub_neighbours) = current;
space_time_map(sub_neighbours) = 4;
end
end
end
end
end
end
end
%obstain the route
dest_node = sub2ind(size(space_time_map), end_coords(1), end_coords(2), end_time);
route = [];
route = [dest_node];
while (parent(route(1)) ~= 0)
route = [parent(route(1)), route];
end
% for i = 1:end_time-1
% image(1.5, 1.5, space_time_map(:,:, i));
% grid on;
% axis image;
% drawnow;
% pause(1);
% end
end
function [special_action] = Special_action(current, sub_neighbours, reservation_table)
special_action = false;
[c_x, c_y, c_t] = ind2sub(size(reservation_table), current);
[s_x, s_y, s_t] = ind2sub(size(reservation_table), sub_neighbours);
if(reservation_table(c_x, c_y, s_t) &&reservation_table(s_x, s_y, c_t) && (reservation_table(c_x, c_y, s_t) ==reservation_table(s_x, s_y, c_t)))
special_action = true;
else
special_action = false;
end
end
Time_Astar_test.m
close all; clear all;
%initial the map size
size_x = 10;
size_y = 10;
size_t = 100;
%1 - white - clear cell
%2 - black - obstacle
%3 - Grayish blue - robot 1
%4 - Reddish blue - robot 2
%5 - purple - robot 3
%6 - vivid green - robot 4
%7 - Plum red - robot 5
cmap = [1 1 1;
0 0 0;
0.69 0.87 0.90;
0.25 0.41 0.88;
0.63 0.13 0.94;
0 1 0.5;
0.87 0.63 0.87;];
colormap(cmap);
%initial the space_time_map and reservation table
space_map = ones(size_x, size_y);
space_time_map = ones(size_x, size_y, size_t);
reservation_table = zeros(size_x, size_y, size_t);
%add the static obstacle
space_time_map(1:5, 5, :) = 2*ones(5,1, size_t);
reservation_table(1:5, 5, :) = 2*ones(5,1, size_t);
%the start point array and end point array
start_points= [2 4; 5 1; 1 1; 9 6;7 3 ];%设置机器人的起点坐标
end_points = [8 7; 10 10; 7 3; 3 2; 1 1];%设置机器人的终点坐标
len_route_max = 0;
route_all = [];
for index = 1:5 %五个机器人
%initial the start point and the end point
route = [];
start_coords =start_points(index, :);%获取该机器人的起点坐标
disp(start_coords);
end_coords = end_points(index, :);
disp(end_coords);
[route] = Time_Astar_For_Cooperative_Path_Finding(start_coords, end_coords, space_time_map, reservation_table);
route_all(index, 1:length(route)) =route; %route_all是存放所有机器人路径的数组
disp(route);
if (length(route) >len_route_max)
len_route_max = length(route);
end
for i = 1:size(route, 2)%size(route, 2)返回矩阵route列数
reservation_table(route(i)) =index+2;%？？
end
end
%show the moving process
for i = 1:len_route_max
show_map = ones(size_x, size_y);
show_map(1:5, 5) = 2*ones(5,1);%设置障碍物，空白地方为1，障碍物为2
for j = 1:5
if (route_all(j, i))%如果机器人没走到终点
[x, y, t] = ind2sub(size(space_time_map), route_all(j, i));
%ind2sub将线性索引转换成下标
show_map(x, y) = j+2;
else
route_all(j, i) = route_all(j, i-1);
[x, y, t] = ind2sub(size(space_time_map), route_all(j, i));
show_map(x, y) = j+2;
end
end
image(1.5, 1.5, show_map)
grid on;
axis image;
drawnow;
pause(2);
end
2188
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
