-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAPF.m
178 lines (136 loc) · 6.35 KB
/
APF.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
classdef APF
%APF Summary of this class goes here
% Detailed explanation goes here
properties
% Feild data sets
attBound = 5;
repDist1 = 2.5;
repDist2 = 3.5;
% Coefficients
epsilon = 5;
etaR1 = 5;
etaR2 = 200;
util;
end
methods
function self = APF(illuminationCellSize)
self.util = Utility();
% self.repDist1 = illuminationCellSize;
% self.repDist2 = self.repDist1 * 5/4;
end
% Compute the attractive force
function f_att = attraction(self,drone,attBound,epsilon)
dist = norm(drone.target - drone.position);
% To prevent attraction force grown too big when it's far from target
% Set an upper bound to the arraction force
dist = min(dist, attBound);
if norm(drone.target - drone.position)
% Return a the attraction force vector
f_att = epsilon * (drone.target - drone.position) * dist/norm(drone.target - drone.position);
else
f_att = [0,0,0];
end
end
% Calculate the total Velocity-Repulsive force
function f_Rep = repulsion(self,drone,dronePositions,repDist1,repDist2,etaR1,etaR2)
f_Rep = [0, 0, 0]; %Initialize the force
blockingDroneNum = 0;
for i = 1 : size(dronePositions,1)
if isequal(drone.position,dronePositions(i,:))
continue;
end
distToObst = norm(drone.position-dronePositions(i,:));
%Drone is affecting by abstacle's repulsivefield
if distToObst <= repDist1
eta = etaR2;
blockingDroneNum = blockingDroneNum + 1;
elseif distToObst <= repDist2 && distToObst > repDist1
eta = etaR1;
blockingDroneNum = blockingDroneNum + 1;
else
eta = 0;
end
% Calculate the repulsive force
fRepByObst = eta * (1/distToObst - 1/repDist2) * (1/(distToObst^2)) * (-1) * self.util.differential(drone.position,dronePositions(i,:));
f_Rep = f_Rep + fRepByObst ;
end
end
%Calculate the next step for current drone
% Consider add up kinematicConstrant later
function [drone,targetExchange] = getNextStep(self,drone,dronePositions)
targetExchange = 0;
if size(dronePositions,1) ~= 0
obstPositions = dronePositions(:,1:3);
else
obstPositions = [];
end
force = self.getTotalForce(drone,obstPositions);
D_SR = norm(drone.position - drone.startPt);
D_RE = norm(drone.position - drone.target);
D_RO = inf;
affectingDrone = [];
closerObst = [];
% calculate is there obstacles between drone and target
distToTarget = norm(drone.target - drone.position);
for i = 1:size(dronePositions,1)
obstToTarget = norm(drone.target - dronePositions(i,1:3));
distToObst = norm(drone.position - dronePositions(i,1:3));
if distToObst < 3.5 && dronePositions(i,5)
affectingDrone = [affectingDrone,dronePositions(i,4)];
if obstToTarget < distToTarget
closerObst = [i];
end
end
D_RO = min([D_RO,distToObst]);
%fprintf("Dist to Obstacle [%.2f,%.2f,%.2f] = %.2f\n", dronePositions(i,:),D_RO)
end
% l = min([D_SR, D_RE, D_RO/2]) * drone.timeUnit;
l = min([D_SR, D_RE]) * drone.timeUnit*5;
if l == D_SR * drone.timeUnit*5
md = "D_SR";
elseif l == D_RE * drone.timeUnit*5
md = "D_RE";
else
md = "D_RO";
end
if l < 0.5 * drone.accMax * drone.timeUnit^2
l = 0.5 * drone.accMax * drone.timeUnit^2;
elseif l > drone.vMax * drone.timeUnit
l = drone.vMax * drone.timeUnit;
end
distanceMove = l * force;
drone.position = drone.position + distanceMove;
drone.distTraveled = drone.distTraveled + norm(distanceMove);
lastV = drone.velocity;
drone.velocity = ((2 * norm(distanceMove) / drone.timeUnit) - norm(drone.velocity)) * force;
if norm(drone.velocity) >= 3
drone.velocity = drone.velocity * 3/norm(drone.velocity);
end
drone.acceleration = (drone.velocity - lastV)/drone.timeUnit;
% if D_RO <= 2.55
% disp(dronePositions);
% end
fprintf("Drone %d at position [%.2f,%.2f,%.2f], targeting [%.2f,%.2f,%.2f] moving %.4f based on %s with speed %.4f, with %.4f left, dist to obstacle %.4f\n", ...
drone.ID, drone.position,drone.target, l, md, norm(drone.velocity), D_RE, D_RO);
newDroneToTarget = norm(drone.target - drone.position);
if newDroneToTarget > distToTarget && (length(affectingDrone)>=1) && (length(closerObst)>=1)
targetExchange = closerObst(1);
drone.targetExchangeCounter = drone.targetExchangeCounter+1;
end
end
% Calculate the total force of the field on the drone
function f_total = getTotalForce(self,drone,dronePositions)
f_att = self.attraction(drone,self.attBound,self.epsilon);
f_rep = self.repulsion(drone,dronePositions,self.repDist1,self.repDist2,self.etaR1,self.etaR2);
if ~all(f_rep == 0)
pause(0.1);
end
if all(f_att == 0)&& all(f_rep == 0)
f_total = [0,0,0];
else
f_total = f_att + f_rep;
f_total = f_total/norm(f_total);
end
end
end
end