-
Notifications
You must be signed in to change notification settings - Fork 2
/
main_cylinder.m
222 lines (216 loc) · 8.3 KB
/
main_cylinder.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
clear;clc;close all;
%% initializing
o1 = Obstacle('cylinder',[30 15 0 30 15 50 5]);
o2 = Obstacle('cylinder',[50 25 0 50 25 50 5]);
o3 = Obstacle('cylinder',[30 35 0 30 35 50 5]);
o4 = Obstacle('cylinder',[50 45 0 50 45 50 5]);
o5 = Obstacle('cylinder',[30 55 0 30 55 50 5]);
o6 = Obstacle('cylinder',[50 65 0 50 65 50 5]);
o7 = Obstacle('cylinder',[30 75 0 30 75 50 5]);
o8 = Obstacle('cylinder',[50 85 0 50 85 50 5]);
o9 = Obstacle('cuboid',[50 50 -5 100 100 10 0 0 0]);
o10 = Obstacle('cylinder',[70 15 0 70 15 50 5]);
o11 = Obstacle('cylinder',[70 35 0 70 35 50 5]);
o12 = Obstacle('cylinder',[70 55 0 70 55 50 5]);
o13 = Obstacle('cylinder',[70 75 0 70 75 50 5]);
Obstacles = [o1 o2 o3 o4 o5 o6 o7 o8 o10 o11 o12 o13];
Tmax = 50;% test time
dt = 0.05;% diff t
N = 5; % swarm's size
P_saclm = [7.0789 1.8849 0.01 0.00032157 0.9495]; %[ps pa pc lambda mu]
swarm = [];
goal = [80;75;40];
for i = 1:N
swarm = [swarm Drone([randi([5,15]);randi([5,15]);40],[0;0;0],6,P_saclm,goal)];
if swarm(i).position == [NaN;NaN;NaN]
swarm(i) = Drone([randi([5,15]);randi([5,15]);40],[0;0;0],6,P_saclm,goal);
end
end
%% data diary
Aver_d = zeros(1,Tmax/dt+1); % Average distance between UAVs
Aver_v = zeros(1,Tmax/dt+1); % Average velocity of UAVs
min_d = zeros(1,Tmax/dt+1); % Minimum distance between UAVs
max_d = zeros(1,Tmax/dt+1); % Maximum distance between UAVs
historyTrajectory = zeros(3,Tmax/dt+1,N); % to save history trajectory
historyVel = zeros(3,Tmax/dt+1,N); % to save history velocity
min_v = zeros(1,Tmax/dt+1); % Minimum velocity of UAVs
max_v = zeros(1,Tmax/dt+1); % Maximum velocity of UAVs
%% actual simulation
simulation_fig = figure('Color','w'); hold on;
hold on;
drawCylinder(o1.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o2.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o3.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o4.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o5.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o6.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o7.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o8.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o10.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o11.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o12.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o13.Size,'FaceColor',[0.97,0.98,0.78]);
drawCuboid(o9.Size,'FaceColor',[1,1,1]);
plot3(swarm(1).p_goal(1),swarm(1).p_goal(2),swarm(1).p_goal(3),'r*')
axis equal;
grid on;
% view(3);
% view([-3.3 83.6]);
% view([26.8 67.1]);
% view([-22.0 35.6]);
% view([-16.6 83.3])
view([-0.1 80.2]);
axis([0 100 0 100 0 100]);
set(gcf, 'Position', [100, 100, 800, 600]);
lighting gouraud;
light;
scatterHandles = gobjects(N, 1);
scatterColors = hsv(N); % save color
rotate3d on;
sum_coll = zeros(1,N); % to account times of collision.
sum_arrive = zeros(1,N); % to account numbers of arrival
for t = 1:dt:Tmax
for i = 1:N
if isvalid(scatterHandles(i))
delete(scatterHandles(i));
end
scatterHandles(i) = scatter3(swarm(i).position(1),swarm(i).position(2),swarm(i).position(3),'filled','MarkerEdgeColor', 'k', 'LineWidth', 1);
scatterHandles(i).CData = scatterColors(i, :);
swarm(i).updateV(swarm,Obstacles);
swarm(i).updateP(dt);
current_t = floor((t-1)/dt+1.01);
historyTrajectory(:,current_t,i) = swarm(i).position;
historyVel(:,current_t,i) = swarm(i).velocity;
end
pause(0.05)
drawnow
% if t == 1 || t == 100*dt || t == 150*dt || t == 200*dt || t == 250*dt || t == 300*dt || t == 350*dt || t == 400*dt
% % print([['运行于第' num2str(t) 's'],'.eps'],'-depsc','-r600');
% % saveas(simulation_fig,['D:\Study\导师任务\发表论文\23_11_30\runtime\cylinder\' ['运行于第' num2str(t) 's'] '.eps']);
% end
for i = 1:N
if swarm(i).isInsideObs == true
sum_coll(i) = 1;
continue;
else
sum_coll(i) = 0;
end
if norm(swarm(i).position - swarm(i).p_goal) <= 4.9
sum_arrive(i) = 1;
else
sum_arrive(i) = 0;
end
end
if sum(sum_arrive) == N
disp('Mission completed, all UAVs reached their goal.');
break;
elseif sum_arrive + sum_coll == ones(1,N)
disp('Mission failed, some UAVs have hit the wall.');
break;
end
end
% print([['运行于第' num2str(t) 's'],'.eps'],'-depsc','-r600');
hold off;
%% plot trajectory
figure;
hold on;
for i = 1:N
x = historyTrajectory(1,1:current_t,i);
y = historyTrajectory(2,1:current_t,i);
z = historyTrajectory(3,1:current_t,i);
plot3(x,y,z,'LineWidth',1,'DisplayName', ['UAV' num2str(i)]);
end
legend('Location','northwest');
title('Route Planning of UAVs');
plot3(swarm(1).p_goal(1),swarm(1).p_goal(2),swarm(1).p_goal(3),'r*','DisplayName','Goal Point');
% drawCuboid(o9.Size,'FaceColor',[1,1,1],'HandleVisibility','off');
drawCylinder(o1.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o2.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o3.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o4.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o5.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o6.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o7.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o8.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o10.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o11.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o12.Size,'FaceColor',[0.97,0.98,0.78]);
drawCylinder(o13.Size,'FaceColor',[0.97,0.98,0.78]);
axis equal;
axis([0 100 0 100 0 100]);
box on;
light;
hold off;
% print('Route_Planning_of_UAVs.eps', '-depsc','-r600');
%% plot other parameters
for t = 1:length(Aver_d)
dmin = 100; vmin = 100;
dmax = 0; vmax = 0;
for i = 1:N
for j = i+1:N
d = norm(historyTrajectory(:,t,i)-historyTrajectory(:,t,j));% d(i,j)
Aver_d(t) = Aver_d(t) + d;
if d < dmin
dmin = d;
end
if d > dmax
dmax = d;
end
end
v = norm(historyVel(:,t,i));
Aver_v(t) = Aver_v(t) + v;
if v < vmin
vmin = v;
end
if v > vmax
vmax = v;
end
end
min_d(t) = dmin;
max_d(t) = dmax;
min_v(t) = vmin;
max_v(t) = vmax;
end
Aver_d = Aver_d/nchoosek(N,2);
Aver_v = Aver_v/N;
% plot distance
figure('Position', [100, 100, 1200, 300]);
hold on;
title('distance between UAVs');
plot(0:dt:(current_t-1)*dt,Aver_d(1:current_t),'b','LineWidth',2,'DisplayName','d_{Average}');
x = 0:dt:(current_t-1)*dt;
min_d_values = min_d(1:current_t);
max_d_values = max_d(1:current_t);
fill([x, fliplr(x)], [min_d_values, fliplr(max_d_values)], 'b', 'FaceAlpha', 0.2, 'EdgeColor', 'none', 'DisplayName', 'Range');
% plot(0:dt:(current_t-1)*dt,min_d(1:current_t),'b','LineWidth',0.5,'DisplayName','d_{min}');
% plot(0:dt:(current_t-1)*dt,max_d(1:current_t),'b','LineWidth',0.5,'DisplayName','d_{max}');
plot(0:dt:(current_t-1)*dt,swarm(1).rs*ones(1,current_t),'k--','LineWidth',1,'DisplayName','d_{safe}');
plot(0:dt:(current_t-1)*dt,0.5*ones(1,current_t),'r--','LineWidth',1,'DisplayName','d_{warn}');
legend('Location','northwest');
xlim([0 (current_t-1)*dt])
xlabel('Time/s');
ylabel('distance/m');
grid on;
box on;
hold off;
% print('distance_between_UAVs.eps', '-depsc');
% plot velocity
figure('Position', [200, 200, 1200, 300]);
hold on;
title('velocity of UAVs');
plot(0:dt:(current_t-1)*dt,Aver_v(1:current_t),'b','LineWidth',2,'DisplayName','V_{Average}');
min_v_values = min_v(1:current_t);
max_v_values = max_v(1:current_t);
fill([x, fliplr(x)], [min_v_values, fliplr(max_v_values)], 'b', 'FaceAlpha', 0.2, 'EdgeColor', 'none', 'DisplayName', 'Range');
% plot(0:dt:(current_t-1)*dt,min_v(1:current_t),'b','LineWidth',1,'DisplayName','V_{min}');
% plot(0:dt:(current_t-1)*dt,max_v(1:current_t),'b','LineWidth',1,'DisplayName','V_{max}');
plot(0:dt:(current_t-1)*dt,swarm(1).vel_0*ones(1,current_t),'k--','LineWidth',1,'DisplayName','V_{0}');
legend('Location','northeast');
xlim([0 (current_t-1)*dt])
ylim([0 12.5])
xlabel('Time/s');
ylabel('velocity/(m \cdot s^{-1})');
grid on;
box on;
hold off;
% print('velocity_of_UAVs.eps', '-depsc');