加入任务分配
This commit is contained in:
parent
9328c9959f
commit
f29623b0b4
|
|
@ -6,4 +6,6 @@ project
|
|||
*.pbstream
|
||||
sensing/slam/vio/VINS-Fusion/camera_models/camera_calib_example
|
||||
.gitee
|
||||
.vscode
|
||||
.vscode
|
||||
ddpg
|
||||
dqn
|
||||
|
|
@ -13,11 +13,7 @@ def darknet_callback(data):
|
|||
for target in data.bounding_boxes:
|
||||
if(target.id==0):
|
||||
print('find human')
|
||||
z = height / math.cos(math.radians(45))
|
||||
'''
|
||||
y_error=(x_center-(target.xmax+target.xmin)/2)*z/(fx+x_center)
|
||||
x_error=(y_center-(target.ymax+target.ymin)/2-y_center*y_error/z)*z/fy
|
||||
'''
|
||||
z = height / math.cos(theta)
|
||||
u = (target.xmax+target.xmin)/2
|
||||
v = (target.ymax+target.ymin)/2
|
||||
u_ = u-u_center
|
||||
|
|
@ -26,7 +22,6 @@ def darknet_callback(data):
|
|||
v_velocity = -Kp_xy*v_
|
||||
x_velocity = v_velocity*z/(-v_*math.sin(theta)+fy*math.cos(theta))
|
||||
y_velocity = (u_*math.sin(theta)*x_velocity+z*u_velocity)/fx
|
||||
#print(x_error,y_error,height)
|
||||
twist.linear.x = x_velocity
|
||||
twist.linear.y = y_velocity
|
||||
twist.linear.z = Kp_z*(target_height-height)
|
||||
|
|
|
|||
|
|
@ -0,0 +1,243 @@
|
|||
import random
|
||||
|
||||
import numpy as np
|
||||
|
||||
import math
|
||||
|
||||
class Aco():
|
||||
|
||||
def __init__(self, vehicle_num, target_num,vehicle_speed, target, time_lim):
|
||||
|
||||
self.num_type_ant = vehicle_num
|
||||
|
||||
self.num_city = target_num+1 #number of cities
|
||||
|
||||
self.group = 200
|
||||
|
||||
self.num_ant = self.group*self.num_type_ant #number of ants
|
||||
|
||||
self.ant_vel = vehicle_speed
|
||||
|
||||
self.cut_time = time_lim
|
||||
|
||||
self.oneee = np.zeros((4,1))
|
||||
|
||||
self.target = target
|
||||
|
||||
self.alpha = 1 #pheromone
|
||||
|
||||
self.beta = 1 #qiwangyinzi
|
||||
|
||||
self.k1 = 0.03
|
||||
|
||||
self.iter_max = 300
|
||||
|
||||
#matrix of the distances between cities
|
||||
|
||||
def distance_matrix(self):
|
||||
|
||||
dis_mat = []
|
||||
|
||||
for i in range(self.num_city):
|
||||
|
||||
dis_mat_each = []
|
||||
|
||||
for j in range(self.num_city):
|
||||
|
||||
dis = math.sqrt(pow(self.target[i][0]-self.target[j][0],2)+pow(self.target[i][1]-self.target[j][1],2))
|
||||
|
||||
dis_mat_each.append(dis)
|
||||
|
||||
dis_mat.append(dis_mat_each)
|
||||
|
||||
return dis_mat
|
||||
|
||||
def get_assignment_aco(self):
|
||||
|
||||
#distances of nodes
|
||||
|
||||
dis_list = self.distance_matrix()
|
||||
|
||||
dis_mat = np.array(dis_list)
|
||||
|
||||
value_init = self.target[:,2].transpose()
|
||||
|
||||
delay_init = self.target[:,3].transpose()
|
||||
|
||||
pheromone_mat = np.ones((self.num_type_ant,self.num_city))
|
||||
|
||||
#velocity of ants
|
||||
path_new = [[0]for i in range (self.num_type_ant)]
|
||||
|
||||
count_iter = 0
|
||||
|
||||
while count_iter < self.iter_max:
|
||||
|
||||
path_sum = np.zeros((self.num_ant,1))
|
||||
|
||||
time_sum = np.zeros((self.num_ant,1))
|
||||
|
||||
value_sum = np.zeros((self.num_ant,1))
|
||||
|
||||
path_mat=[[0]for i in range (self.num_ant)]
|
||||
|
||||
value = np.zeros((self.group,1))
|
||||
|
||||
for ant in range(self.num_ant):
|
||||
|
||||
ant_type = ant % self.num_type_ant
|
||||
|
||||
visit = 0
|
||||
|
||||
if ant_type == 0:
|
||||
|
||||
unvisit_list=list(range(1,self.num_city))#have not visit
|
||||
|
||||
for j in range(1,self.num_city):
|
||||
|
||||
#choice of next city
|
||||
|
||||
trans_list=[]
|
||||
|
||||
tran_sum=0
|
||||
|
||||
trans=0
|
||||
|
||||
#if len(unvisit_list)==0:
|
||||
#print('len(unvisit_list)==0')
|
||||
|
||||
for k in range(len(unvisit_list)): # to decide which node to visit
|
||||
|
||||
#trans +=np.power(pheromone_mat[ant_type][visit][unvisit_list[k]],alpha)*np.power(k1*value_mat[visit][unvisit_list[k]],beta)
|
||||
trans +=np.power(pheromone_mat[ant_type][unvisit_list[k]],self.alpha)*np.power(0.05*value_init[unvisit_list[k]],self.beta)
|
||||
|
||||
trans_list.append(trans)
|
||||
|
||||
tran_sum = trans
|
||||
|
||||
rand = random.uniform(0,tran_sum)
|
||||
|
||||
for t in range(len(trans_list)):
|
||||
|
||||
if(rand <= trans_list[t]):
|
||||
|
||||
visit_next = unvisit_list[t]
|
||||
|
||||
break
|
||||
|
||||
else:
|
||||
|
||||
continue
|
||||
|
||||
path_mat[ant].append(visit_next)
|
||||
|
||||
path_sum[ant] += dis_mat[path_mat[ant][j-1]][path_mat[ant][j]]
|
||||
|
||||
time_sum[ant] += path_sum[ant] / self.ant_vel[ant_type] + delay_init[visit_next]
|
||||
|
||||
if time_sum[ant] > self.cut_time:
|
||||
|
||||
time_sum[ant]-=path_sum[ant] / self.ant_vel[ant_type] + delay_init[visit_next]
|
||||
|
||||
path_mat[ant].pop()
|
||||
|
||||
break
|
||||
|
||||
value_sum[ant] += value_init[visit_next]
|
||||
|
||||
unvisit_list.remove(visit_next)#update
|
||||
|
||||
visit = visit_next
|
||||
|
||||
if (ant_type) == self.num_type_ant-1:
|
||||
|
||||
small_group = int(ant/self.num_type_ant)
|
||||
|
||||
for k in range (self.num_type_ant):
|
||||
|
||||
value[small_group]+= value_sum[ant-k]
|
||||
|
||||
#diedai
|
||||
|
||||
print(max(value))
|
||||
|
||||
if count_iter == 0:
|
||||
|
||||
value_new = max(value)
|
||||
|
||||
value = value.tolist()
|
||||
|
||||
for k in range (0,self.num_type_ant):
|
||||
|
||||
path_new[k] = path_mat[value.index(value_new)*self.num_type_ant+k]
|
||||
|
||||
path_new[k].remove(0)
|
||||
|
||||
#time[k] = time_sum[value.index(value_new)*self.num_type_ant+k]
|
||||
|
||||
else:
|
||||
|
||||
if max(value) > value_new:
|
||||
|
||||
value_new = max(value)
|
||||
|
||||
value = value.tolist()
|
||||
|
||||
for k in range (0,self.num_type_ant):
|
||||
|
||||
path_new[k] = path_mat[value.index(value_new)*self.num_type_ant+k]
|
||||
|
||||
path_new[k].remove(0)
|
||||
|
||||
#time[k] = time_sum[value.index(value_new)*self.num_type_ant+k]
|
||||
|
||||
# update pheromone
|
||||
|
||||
pheromone_change=np.zeros((self.num_type_ant,self.num_city))
|
||||
|
||||
for i in range(self.num_ant):
|
||||
|
||||
length = len(path_mat[i])
|
||||
|
||||
m = i%self.num_type_ant
|
||||
|
||||
n = int(i/self.num_type_ant)
|
||||
|
||||
for j in range(length-1):
|
||||
|
||||
if value[n] == value_new:
|
||||
|
||||
atten = 0.1
|
||||
|
||||
else:
|
||||
|
||||
atten = 0.3
|
||||
|
||||
#a=value_sum[i]/(np.power((value[n]-value_new),2)+1)
|
||||
|
||||
pheromone_change[m][path_mat[i][j+1]]+= self.k1*value_init[path_mat[i][j+1]]
|
||||
|
||||
|
||||
for m in range (self.num_type_ant):
|
||||
|
||||
#print('pheromone_change[m]:',pheromone_change[m])
|
||||
|
||||
pheromone_mat[m]=(1-atten)*pheromone_mat[m]+pheromone_change[m]
|
||||
|
||||
count_iter += 1
|
||||
|
||||
print('count_iter:',count_iter)
|
||||
|
||||
print('the largest value：',value_new)
|
||||
|
||||
print('the best road：',path_new)
|
||||
|
||||
#print('time_limit:',self.cut_time)
|
||||
|
||||
#print('time：',time)
|
||||
|
||||
return path_new
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,101 @@
|
|||
,time,reward
|
||||
0,3.337860107421875e-06,0
|
||||
1,2.86102294921875e-06,0
|
||||
2,2.86102294921875e-06,0
|
||||
3,2.6226043701171875e-06,0
|
||||
4,2.6226043701171875e-06,0
|
||||
5,2.384185791015625e-06,0
|
||||
6,2.1457672119140625e-06,0
|
||||
7,2.1457672119140625e-06,0
|
||||
8,2.1457672119140625e-06,0
|
||||
9,2.1457672119140625e-06,0
|
||||
10,2.384185791015625e-06,0
|
||||
11,2.1457672119140625e-06,0
|
||||
12,2.384185791015625e-06,0
|
||||
13,2.6226043701171875e-06,0
|
||||
14,2.384185791015625e-06,0
|
||||
15,2.384185791015625e-06,0
|
||||
16,2.384185791015625e-06,0
|
||||
17,2.384185791015625e-06,0
|
||||
18,2.1457672119140625e-06,0
|
||||
19,2.6226043701171875e-06,0
|
||||
20,2.1457672119140625e-06,0
|
||||
21,2.1457672119140625e-06,0
|
||||
22,1.9073486328125e-06,0
|
||||
23,2.1457672119140625e-06,0
|
||||
24,2.384185791015625e-06,0
|
||||
25,2.384185791015625e-06,0
|
||||
26,1.9073486328125e-06,0
|
||||
27,1.9073486328125e-06,0
|
||||
28,1.9073486328125e-06,0
|
||||
29,2.1457672119140625e-06,0
|
||||
30,2.384185791015625e-06,0
|
||||
31,2.384185791015625e-06,0
|
||||
32,2.384185791015625e-06,0
|
||||
33,2.1457672119140625e-06,0
|
||||
34,2.1457672119140625e-06,0
|
||||
35,2.384185791015625e-06,0
|
||||
36,2.1457672119140625e-06,0
|
||||
37,1.9073486328125e-06,0
|
||||
38,1.9073486328125e-06,0
|
||||
39,2.1457672119140625e-06,0
|
||||
40,1.9073486328125e-06,0
|
||||
41,2.1457672119140625e-06,0
|
||||
42,2.1457672119140625e-06,0
|
||||
43,1.9073486328125e-06,0
|
||||
44,2.1457672119140625e-06,0
|
||||
45,2.384185791015625e-06,0
|
||||
46,1.9073486328125e-06,0
|
||||
47,1.9073486328125e-06,0
|
||||
48,2.1457672119140625e-06,0
|
||||
49,2.384185791015625e-06,0
|
||||
50,1.9073486328125e-06,0
|
||||
51,1.9073486328125e-06,0
|
||||
52,1.9073486328125e-06,0
|
||||
53,1.9073486328125e-06,0
|
||||
54,2.384185791015625e-06,0
|
||||
55,2.1457672119140625e-06,0
|
||||
56,2.1457672119140625e-06,0
|
||||
57,2.384185791015625e-06,0
|
||||
58,2.384185791015625e-06,0
|
||||
59,2.1457672119140625e-06,0
|
||||
60,2.1457672119140625e-06,0
|
||||
61,2.1457672119140625e-06,0
|
||||
62,2.384185791015625e-06,0
|
||||
63,2.1457672119140625e-06,0
|
||||
64,1.9073486328125e-06,0
|
||||
65,2.1457672119140625e-06,0
|
||||
66,1.9073486328125e-06,0
|
||||
67,2.1457672119140625e-06,0
|
||||
68,2.1457672119140625e-06,0
|
||||
69,2.1457672119140625e-06,0
|
||||
70,2.1457672119140625e-06,0
|
||||
71,2.1457672119140625e-06,0
|
||||
72,2.384185791015625e-06,0
|
||||
73,2.1457672119140625e-06,0
|
||||
74,2.1457672119140625e-06,0
|
||||
75,2.1457672119140625e-06,0
|
||||
76,2.1457672119140625e-06,0
|
||||
77,1.9073486328125e-06,0
|
||||
78,2.1457672119140625e-06,0
|
||||
79,2.1457672119140625e-06,0
|
||||
80,2.1457672119140625e-06,0
|
||||
81,2.1457672119140625e-06,0
|
||||
82,2.384185791015625e-06,0
|
||||
83,2.1457672119140625e-06,0
|
||||
84,2.1457672119140625e-06,0
|
||||
85,1.9073486328125e-06,0
|
||||
86,2.1457672119140625e-06,0
|
||||
87,2.1457672119140625e-06,0
|
||||
88,2.1457672119140625e-06,0
|
||||
89,1.9073486328125e-06,0
|
||||
90,2.384185791015625e-06,0
|
||||
91,2.1457672119140625e-06,0
|
||||
92,2.384185791015625e-06,0
|
||||
93,2.384185791015625e-06,0
|
||||
94,2.384185791015625e-06,0
|
||||
95,2.384185791015625e-06,0
|
||||
96,2.384185791015625e-06,0
|
||||
97,2.384185791015625e-06,0
|
||||
98,1.9073486328125e-06,0
|
||||
99,2.1457672119140625e-06,0
|
||||
|
|
|
@ -0,0 +1,122 @@
|
|||
import numpy as np
|
||||
import matplotlib.pyplot as plt
|
||||
import random
|
||||
import time
|
||||
import pandas as pd
|
||||
|
||||
class Env():
|
||||
def __init__(self, vehicle_num, target_num, map_size, visualized=True, time_cost=None, repeat_cost=None):
|
||||
self.vehicles_position = np.zeros(vehicle_num,dtype=np.int32)
|
||||
self.vehicles_speed = np.zeros(vehicle_num,dtype=np.int32)
|
||||
self.targets = np.zeros(shape=(target_num+1,4),dtype=np.int32)
|
||||
self.map_size = map_size
|
||||
self.speed_range = [10, 15, 30]
|
||||
#self.time_lim = 1e6
|
||||
self.time_lim = self.map_size / self.speed_range[1] * 2
|
||||
self.vehicles_lefttime = np.ones(vehicle_num,dtype=np.float32) * self.time_lim
|
||||
self.distant_mat = np.zeros((target_num+1,target_num+1),dtype=np.float32)
|
||||
self.total_reward = 0
|
||||
self.reward = 0
|
||||
self.visualized = visualized
|
||||
self.time = 0
|
||||
self.time_cost = time_cost
|
||||
self.repeat_cost = repeat_cost
|
||||
self.done = False
|
||||
self.assignment = [[] for i in range(vehicle_num)]
|
||||
self.task_generator()
|
||||
|
||||
def task_generator(self):
|
||||
for i in range(self.vehicles_speed.shape[0]):
|
||||
choose = random.randint(0,2)
|
||||
self.vehicles_speed[i] = self.speed_range[choose]
|
||||
for i in range(self.targets.shape[0]-1):
|
||||
self.targets[i+1,0] = random.randint(1,self.map_size) - 0.5*self.map_size # x position
|
||||
self.targets[i+1,1] = random.randint(1,self.map_size) - 0.5*self.map_size # y position
|
||||
self.targets[i+1,2] = random.randint(1,10) # value
|
||||
self.targets[i+1,3] = random.randint(5,30) # time to stay
|
||||
for i in range(self.targets.shape[0]):
|
||||
for j in range(self.targets.shape[0]):
|
||||
self.distant_mat[i,j] = np.linalg.norm(self.targets[i,:2]-self.targets[j,:2])
|
||||
|
||||
def step(self, action):
|
||||
count = 0
|
||||
for j in range(len(self.action)):
|
||||
k = self.action[j]
|
||||
delta_time = self.distant_mat[self.vehicles_position[j],k] / self.vehicles_speed[j] + self.targets[k,3]
|
||||
self.vehicles_lefttime[j] = self.vehicles_lefttime[j] - delta_time
|
||||
if self.vehicles_lefttime < 0:
|
||||
count = count + 1
|
||||
continue
|
||||
else:
|
||||
if k == 0:
|
||||
self.reward = - self.repeat_cost
|
||||
else:
|
||||
self.reward = self.targets[k,2] - delta_time * self.time_cost
|
||||
if self.targets[k,2] == 0:
|
||||
self.total_reward = self.total_reward - self.repeat_cost
|
||||
self.vehicles_position[j] = k
|
||||
self.targets[k,2] = 0
|
||||
self.total_reward = self.total_reward + self.rewards
|
||||
self.assignment[j].append(action)
|
||||
if count == len(self.action):
|
||||
self.done = True
|
||||
|
||||
def run(self, assignment, algorithm):
|
||||
self.assignment = assignment
|
||||
self.algorithm = algorithm
|
||||
self.get_total_reward()
|
||||
if self.visualized:
|
||||
self.visualize()
|
||||
|
||||
|
||||
def get_total_reward(self):
|
||||
for i in range(len(self.assignment)):
|
||||
speed = self.vehicles_speed[i]
|
||||
for j in range(len(self.assignment[i])):
|
||||
k = self.assignment[i][j]
|
||||
self.total_reward = self.total_reward + self.targets[k,2]
|
||||
self.vehicles_lefttime[i] = self.vehicles_lefttime[i] - self.distant_mat[self.vehicles_position[i],k] / speed - self.targets[k,3]
|
||||
if self.vehicles_lefttime[i] < 0:
|
||||
self.done = True
|
||||
break
|
||||
self.vehicles_position[i] = k
|
||||
if self.done:
|
||||
self.total_reward = 0
|
||||
break
|
||||
|
||||
def visualize(self):
|
||||
if self.assignment == None:
|
||||
plt.scatter(x=0,y=0,s=200,c='k')
|
||||
plt.scatter(x=self.targets[1:,0],y=self.targets[1:,1],s=self.targets[1:,2]*10,c='r')
|
||||
plt.title('Target distribution')
|
||||
plt.show()
|
||||
else:
|
||||
plt.title('Task assignment by '+self.algorithm +', total reward : '+str(self.total_reward))
|
||||
plt.scatter(x=0,y=0,s=200,c='k')
|
||||
plt.scatter(x=self.targets[1:,0],y=self.targets[1:,1],s=self.targets[1:,2]*10,c='r')
|
||||
for i in range(len(self.assignment)):
|
||||
trajectory = np.array([[0,0,20]])
|
||||
for j in range(len(self.assignment[i])):
|
||||
position = self.targets[self.assignment[i][j],:3]
|
||||
trajectory = np.insert(trajectory,j+1,values=position,axis=0)
|
||||
plt.scatter(x=trajectory[1:,0],y=trajectory[1:,1],s=trajectory[1:,2]*10,c='b')
|
||||
plt.plot(trajectory[:,0], trajectory[:,1])
|
||||
plt.show()
|
||||
|
||||
if __name__=='__main__':
|
||||
time_record = []
|
||||
reward_record = []
|
||||
for i in range(100):
|
||||
vehicle_num = random.randint(5,15)
|
||||
target_num = random.randint(30,100)
|
||||
map_size = random.randint(5e3,2e4) # meter
|
||||
env = Env(vehicle_num,target_num,map_size,visualized=True)
|
||||
time_start = time.time()
|
||||
task_assignment = [[i+1 for i in range(0,5)],[i+1 for i in range(5,20)],[i+1 for i in range(20,30)]]
|
||||
time_end = time.time()
|
||||
time_record.append(time_end-time_start)
|
||||
algorithm = 'customized'
|
||||
env.run(task_assignment,algorithm)
|
||||
reward_record.append(env.total_reward)
|
||||
dataframe = pd.DataFrame({'time':time_record,'reward':reward_record})
|
||||
dataframe.to_csv(algorithm+'.csv',sep=',')
|
||||
|
|
@ -0,0 +1,161 @@
|
|||
import numpy as np
|
||||
import random
|
||||
|
||||
|
||||
class ga():
|
||||
def __init__(self,vehicle_num,vehicles_speed,target_num,targets,time_lim):
|
||||
#vehicles_speed,targets in the type of narray
|
||||
self.vehicle_num=vehicle_num
|
||||
self.vehicles_speed=vehicles_speed
|
||||
self.target_num=target_num
|
||||
self.targets=targets
|
||||
self.time_lim=time_lim
|
||||
self.map=np.zeros(shape=(target_num+1,target_num+1),dtype=float)
|
||||
self.pop_size=100
|
||||
self.p_cross=0.6
|
||||
self.p_mutate=0.005
|
||||
for i in range(target_num+1):
|
||||
self.map[i,i]=0
|
||||
for j in range(i):
|
||||
self.map[j,i]=self.map[i,j]=np.linalg.norm(targets[i,:2]-targets[j,:2])
|
||||
self.pop=np.zeros(shape=(self.pop_size,vehicle_num-1+target_num-1),dtype=np.int32)
|
||||
self.ff=np.zeros(self.pop_size,dtype=float)
|
||||
for i in range(self.pop_size):
|
||||
for j in range(vehicle_num-1):
|
||||
self.pop[i,j]=random.randint(0,target_num)
|
||||
for j in range(target_num-1):
|
||||
self.pop[i,vehicle_num+j-1]=random.randint(0,target_num-j-1)
|
||||
self.ff[i]=self.fitness(self.pop[i,:])
|
||||
self.tmp_pop=np.array([])
|
||||
self.tmp_ff=np.array([])
|
||||
self.tmp_size=0
|
||||
|
||||
def fitness(self,gene):
|
||||
ins=np.zeros(self.target_num+1,dtype=np.int32)
|
||||
seq=np.zeros(self.target_num,dtype=np.int32)
|
||||
ins[self.target_num]=1
|
||||
for i in range(self.vehicle_num-1):
|
||||
ins[gene[i]]+=1
|
||||
rest=np.array(range(1,self.target_num+1))
|
||||
for i in range(self.target_num-1):
|
||||
seq[i]=rest[gene[i+self.vehicle_num-1]]
|
||||
rest=np.delete(rest,gene[i+self.vehicle_num-1])
|
||||
seq[self.target_num-1]=rest[0]
|
||||
i=0 #index of vehicle
|
||||
pre=0 #index of last target
|
||||
post=0 #index of ins/seq
|
||||
t=0
|
||||
reward=0
|
||||
while i<self.vehicle_num:
|
||||
if ins[post]>0:
|
||||
i+=1
|
||||
ins[post]-=1
|
||||
pre=0
|
||||
t=0
|
||||
else:
|
||||
t+=self.targets[pre,3]
|
||||
past=self.map[pre,seq[post]]/self.vehicles_speed[i]
|
||||
t+=past
|
||||
if t<self.time_lim:
|
||||
reward+=self.targets[seq[post],2]
|
||||
pre=seq[post]
|
||||
post+=1
|
||||
return reward
|
||||
|
||||
def selection(self):
|
||||
roll=np.zeros(self.tmp_size,dtype=float)
|
||||
roll[0]=self.tmp_ff[0]
|
||||
for i in range(1,self.tmp_size):
|
||||
roll[i]=roll[i-1]+self.tmp_ff[i]
|
||||
for i in range(self.pop_size):
|
||||
xx=random.uniform(0,roll[self.tmp_size-1])
|
||||
j=0
|
||||
while xx>roll[j]:
|
||||
j+=1
|
||||
self.pop[i,:]=self.tmp_pop[j,:]
|
||||
self.ff[i]=self.tmp_ff[j]
|
||||
|
||||
def mutation(self):
|
||||
for i in range(self.tmp_size):
|
||||
flag=False
|
||||
for j in range(self.vehicle_num-1):
|
||||
if random.random()<self.p_mutate:
|
||||
self.tmp_pop[i,j]=random.randint(0,self.target_num)
|
||||
flag=True
|
||||
for j in range(self.target_num-1):
|
||||
if random.random()<self.p_mutate:
|
||||
self.tmp_pop[i,self.vehicle_num+j-1]=random.randint(0,self.target_num-j-1)
|
||||
flag=True
|
||||
if flag:
|
||||
self.tmp_ff[i]=self.fitness(self.tmp_pop[i,:])
|
||||
|
||||
def crossover(self):
|
||||
new_pop=[]
|
||||
new_ff=[]
|
||||
new_size=0
|
||||
for i in range(0,self.pop_size,2):
|
||||
if random.random()<self.p_cross:
|
||||
x1=random.randint(0,self.vehicle_num-2)
|
||||
x2=random.randint(0,self.target_num-2)+self.vehicle_num
|
||||
g1=self.pop[i,:]
|
||||
g2=self.pop[i+1,:]
|
||||
g1[x1:x2]=self.pop[i+1,x1:x2]
|
||||
g2[x1:x2]=self.pop[i,x1:x2]
|
||||
new_pop.append(g1)
|
||||
new_pop.append(g2)
|
||||
new_ff.append(self.fitness(g1))
|
||||
new_ff.append(self.fitness(g2))
|
||||
new_size+=2
|
||||
self.tmp_size=self.pop_size+new_size
|
||||
self.tmp_pop=np.zeros(shape=(self.tmp_size,self.vehicle_num-1+self.target_num-1),dtype=np.int32)
|
||||
self.tmp_pop[0:self.pop_size,:]=self.pop
|
||||
self.tmp_pop[self.pop_size:self.tmp_size,:]=np.array(new_pop)
|
||||
self.tmp_ff=np.zeros(self.tmp_size,dtype=float)
|
||||
self.tmp_ff[0:self.pop_size]=self.ff
|
||||
self.tmp_ff[self.pop_size:self.tmp_size]=np.array(new_ff)
|
||||
|
||||
def run(self):
|
||||
cut=0
|
||||
e=self.tmp_ff>cut
|
||||
while e.sum()<=self.tmp_size*0.5:
|
||||
self.crossover()
|
||||
self.mutation()
|
||||
self.selection()
|
||||
cut=self.tmp_ff.max()*0.9
|
||||
e=self.tmp_ff>cut
|
||||
i=0
|
||||
for j in range(self.tmp_size):
|
||||
if self.tmp_ff[i]<self.tmp_ff[j]:
|
||||
i=j
|
||||
gene=self.tmp_pop[i]
|
||||
|
||||
ins=np.zeros(self.target_num+1,dtype=np.int32)
|
||||
seq=np.zeros(self.target_num,dtype=np.int32)
|
||||
ins[self.target_num]=1
|
||||
for i in range(self.vehicle_num-1):
|
||||
ins[gene[i]]+=1
|
||||
rest=np.array(range(1,self.target_num+1))
|
||||
for i in range(self.target_num-1):
|
||||
seq[i]=rest[gene[i+self.vehicle_num-1]]
|
||||
rest=np.delete(rest,gene[i+self.vehicle_num-1])
|
||||
seq[self.target_num-1]=rest[0]
|
||||
task_assignment=[[] for i in range(self.vehicle_num)]
|
||||
i=0 #index of vehicle
|
||||
pre=0 #index of last target
|
||||
post=0 #index of ins/seq
|
||||
t=0
|
||||
while i<self.vehicle_num:
|
||||
if ins[post]>0:
|
||||
i+=1
|
||||
ins[post]-=1
|
||||
pre=0
|
||||
t=0
|
||||
else:
|
||||
t+=self.targets[pre,3]
|
||||
past=self.map[pre,seq[post]]/self.vehicles_speed[i]
|
||||
t+=past
|
||||
if t<self.time_lim:
|
||||
task_assignment[i].append(seq[post])
|
||||
pre=seq[post]
|
||||
post+=1
|
||||
return task_assignment
|
||||
|
|
@ -0,0 +1,289 @@
|
|||
# coding: utf-8
|
||||
import numpy as np
|
||||
import random
|
||||
import matplotlib.pyplot as plt
|
||||
import math
|
||||
import cmath
|
||||
from environment import Env
|
||||
# ----------------------优化方案----------------------------------
|
||||
# 发现粒子群算法优化结果不好，收敛没有到全局最优
|
||||
# 优化思路：1. 增加收敛因子k；2. 动态改变惯性因子w
|
||||
|
||||
# ----------------------PSO参数设置---------------------------------
|
||||
class PSO():
|
||||
def __init__(self, pN, dim, max_iter,uav_num,Distance,
|
||||
v,Value,test_num,time_all):
|
||||
self.w = 0.8
|
||||
self.c1 = 2
|
||||
self.c2 = 2
|
||||
self.r1 = 0.6
|
||||
self.r2 = 0.3
|
||||
self.pN = pN # 方案数量
|
||||
self.dim = dim # 方案维度
|
||||
self.max_iter = max_iter # 迭代次数
|
||||
self.X = np.zeros((self.pN, self.dim)) # 所有粒子的位置
|
||||
self.V = np.zeros((self.pN, self.dim)) # 所有粒子的速度
|
||||
self.pbest = np.zeros((self.pN, self.dim)) # 个体经历的最佳位置
|
||||
self.gbest = np.zeros((1, self.dim)) # 个体经历的全局最佳位置
|
||||
self.p_fit = np.zeros(self.pN) # 每个个体的历史最佳适应值
|
||||
self.fit = 0 # 全局最佳适应值
|
||||
self.uav_num=uav_num
|
||||
self.time_all=time_all
|
||||
self.Distance=Distance # Distance是(dim+1）*（dim+1)的对称矩阵
|
||||
self.v=v # 无人机真正飞行速度 m/s
|
||||
self.Value=Value # 目标位置的价值数组 1*dim
|
||||
self.TEST=[]
|
||||
self.test_num=test_num
|
||||
self.dim1=dim
|
||||
self.add_num=0
|
||||
self.time_all=time_all
|
||||
self.k=0 # 收敛因子
|
||||
self.wini=0.9
|
||||
self.wend=0.4
|
||||
# --------------------取整---------------------------------
|
||||
def fun_Ceil(self):
|
||||
num1=self.dim/self.uav_num
|
||||
num1=math.ceil(num1)
|
||||
dim1=num1*self.uav_num
|
||||
num1=dim1-self.dim
|
||||
self.add_num=num1
|
||||
self.dim1=dim1
|
||||
# -------------------排序式转排列式--------------------------
|
||||
def position(self,X):
|
||||
Position_All=list(range(1,self.dim+1))
|
||||
X2=[]
|
||||
for i in range(self.dim):
|
||||
m1=X[i]
|
||||
m1=int(m1)
|
||||
X2.append(Position_All[m1-1])
|
||||
del Position_All[m1-1]
|
||||
return X2
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# ---------------------目标函数-----------------------------
|
||||
def function(self, X):
|
||||
# X 是一个方案，x[i]
|
||||
# 由无人机排序式转为排列式
|
||||
X=self.position(X)
|
||||
# 把X扩充到可以整除的数组
|
||||
for i in range(self.add_num):
|
||||
X.append(i+self.dim+1)
|
||||
# 由方案导出五个无人机各自搜索路径
|
||||
UAV=[]
|
||||
k=0
|
||||
for i in range(self.uav_num):
|
||||
UAV.append([])
|
||||
for j in range(self.dim1//self.uav_num):
|
||||
UAV[i].append(X[k])
|
||||
k=k+1
|
||||
# 计算某一个方案的目标函数值
|
||||
l1=len(UAV[0])
|
||||
value=0
|
||||
for i in range(self.uav_num):
|
||||
t=self.Distance[0][UAV[i][0]]/self.v
|
||||
for j in range(l1-1):
|
||||
if t<=self.time_all:
|
||||
d1=UAV[i][j]
|
||||
d2=UAV[i][j+1]
|
||||
if d2<=self.dim:
|
||||
distance=self.Distance[d1][d2]
|
||||
t=t+distance/self.v
|
||||
value=value+self.Value[UAV[i][j]-1]
|
||||
return value
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# ---------------------初始化种群----------------------------------
|
||||
def init_Population(self):
|
||||
# 取整
|
||||
self.fun_Ceil()
|
||||
# 初始化位置和历史最优、全局最优
|
||||
for i in range(self.pN):
|
||||
for j in range(self.dim):
|
||||
self.X[i][j] = int(random.randint(1,self.dim-j))
|
||||
#self.V[i][j] = random.uniform(0, 1)
|
||||
self.pbest[i] = self.X[i]
|
||||
tmp = self.function(self.X[i])
|
||||
self.p_fit[i] = tmp
|
||||
if tmp > self.fit:
|
||||
self.fit = tmp
|
||||
self.gbest = self.X[i]
|
||||
# 初始化速度
|
||||
for i in range(self.pN):
|
||||
for j in range(self.dim):
|
||||
self.V[i][j] =random.uniform(-0.3*(self.dim-j),0.3*(self.dim-j))
|
||||
self.V[i][j]=int(self.V[i][j])
|
||||
# 计算收敛因子k
|
||||
phi=self.c1+self.c2
|
||||
k=abs(phi*phi-4*phi)
|
||||
k=cmath.sqrt(k)
|
||||
k=abs(2-phi-k)
|
||||
k=2/k
|
||||
self.k=k
|
||||
# 初始化测试集
|
||||
for i in range(self.test_num):
|
||||
self.TEST.append([])
|
||||
for j in range(self.dim):
|
||||
self.TEST[i].append(int(random.randint(1,self.dim-j)))
|
||||
|
||||
#self.V[i][j] = random.uniform(0, 1)
|
||||
|
||||
|
||||
# ----------------------更新粒子位置----------------------------------
|
||||
|
||||
def iterator(self):
|
||||
fitness = []
|
||||
for t in range(self.max_iter):
|
||||
# 更新惯性因子w
|
||||
w=(self.wini-self.wend)*(self.max_iter-t)/self.max_iter+self.wend # w的变化规律是线性递减的
|
||||
self.w=w # 用计算出的w更新属性
|
||||
for i in range(self.pN): # 更新gbest\pbest
|
||||
temp = self.function(self.X[i])
|
||||
if temp > self.p_fit[i]: # 更新个体最优
|
||||
self.p_fit[i] = temp
|
||||
self.pbest[i] = self.X[i]
|
||||
if self.p_fit[i] > self.fit: # 更新全局最优
|
||||
self.gbest = self.X[i]
|
||||
self.fit = self.p_fit[i]
|
||||
|
||||
for i in range(self.pN):
|
||||
# 速度更新
|
||||
self.V[i] = self.k*(self.w * self.V[i] + self.c1 * self.r1 * (self.pbest[i] - self.X[i])) + \
|
||||
self.c2 * self.r2 * (self.gbest - self.X[i])
|
||||
# 设置速度边界
|
||||
for j in range(self.dim):
|
||||
v_max=int(0.3*(self.dim-j))
|
||||
v_min=-v_max
|
||||
self.V[i][j]=int(self.V[i][j])
|
||||
if self.V[i][j] > v_max:
|
||||
self.V[i][j] = v_max
|
||||
if self.V[i][j] < v_min:
|
||||
self.V[i][j] = v_min
|
||||
|
||||
# 位置更新
|
||||
self.X[i] = self.X[i] + self.V[i]
|
||||
# 设置位置边界
|
||||
for j in range(self.dim):
|
||||
self.X[i][j]=int(self.X[i][j])
|
||||
if self.X[i][j] > self.dim-j:
|
||||
self.X[i][j] = self.dim-j
|
||||
if self.X[i][j] < 1:
|
||||
self.X[i][j] = 1
|
||||
|
||||
|
||||
fitness.append(self.fit)
|
||||
'''
|
||||
print(self.X[0], end=" ")
|
||||
print(self.fit) # 输出最优值
|
||||
'''
|
||||
self.gbest=self.position(self.gbest)
|
||||
return fitness
|
||||
# ---------------------数据处理---------------------------
|
||||
def fun_Data(self):
|
||||
for i in range(self.add_num):
|
||||
self.gbest.append(i+self.dim+1)
|
||||
UAV=[]
|
||||
k=0
|
||||
for i in range(self.uav_num):
|
||||
UAV.append([])
|
||||
for j in range(self.dim1//self.uav_num):
|
||||
UAV[i].append(self.gbest[k])
|
||||
k=k+1
|
||||
l1=len(UAV[0])
|
||||
best=[]
|
||||
for i in range(self.uav_num):
|
||||
t=self.Distance[0][UAV[i][0]]/self.v
|
||||
best.append([])
|
||||
for j in range(l1-1):
|
||||
if t<=self.time_all:
|
||||
best[i].append(UAV[i][j])
|
||||
d1=UAV[i][j]
|
||||
d2=UAV[i][j+1]
|
||||
if d2<=self.dim:
|
||||
distance=self.Distance[d1][d2]
|
||||
t=t+distance/self.v
|
||||
for i in range(self.uav_num):
|
||||
l1=len(best[i])
|
||||
k=0
|
||||
for j in range(l1):
|
||||
if best[i][k]>self.dim:
|
||||
del best[i][k]
|
||||
k=k-1
|
||||
k=k+1
|
||||
return best
|
||||
|
||||
|
||||
# ---------------------测试------------------------------
|
||||
def fun_TEST(self):
|
||||
Test_Value=[]
|
||||
for i in range(self.test_num):
|
||||
Test_Value.append(self.function(self.TEST[i]))
|
||||
return Test_Value
|
||||
# ----------------------程序执行-----------------------
|
||||
# --------------------定义初始参数---------------------
|
||||
uav_num = 10
|
||||
target_num = 45
|
||||
env = Env(uav_num,target_num,100,1000)
|
||||
env.targets
|
||||
env.vehicles_speed
|
||||
# 定义总搜索时间
|
||||
time_all=np.random.randint(0.6*10*dim,0.8*10*dim)
|
||||
Value=[]
|
||||
for i in range(dim):
|
||||
Value.append(np.random.randint(0,200))
|
||||
Distance=np.zeros((dim+1,dim+1))
|
||||
for i in range(dim+1):
|
||||
for j in range(i+1,dim+1):
|
||||
Distance[i][j]=np.random.uniform(200,400)
|
||||
Distance[j][i]=Distance[i][j]
|
||||
# -----------------------PSO优化----------------------
|
||||
my_pso = PSO(pN, dim, max_iter,uav_num,Distance,
|
||||
v,Value,test_num,time_all)
|
||||
my_pso.init_Population()
|
||||
fitness = my_pso.iterator()
|
||||
best=my_pso.fun_Data()
|
||||
# --------------------------测试---------------------
|
||||
|
||||
print("fitness is",fitness)
|
||||
for i in range(uav_num):
|
||||
print("第",i+1,"架无人机的搜索路径为:",best[i])
|
||||
Test_Value=my_pso.fun_TEST()
|
||||
l1=len(fitness)
|
||||
k=0
|
||||
Test_Value_out=[]
|
||||
for i in range(test_num):
|
||||
if Test_Value[i]>fitness[l1-1]:
|
||||
k=k+1
|
||||
Test_Value_out.append(Test_Value[i])
|
||||
print("测试结果超过优化后的目标值的个数是：",k,"个")
|
||||
if k > 0:
|
||||
print("这些测试结果分别是：",Test_Value_out)
|
||||
#print("测试的适应值是",Test_Value)
|
||||
|
||||
# ------------------测试---------------------
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
# -------------------画图--------------------
|
||||
'''
|
||||
plt.figure(1)
|
||||
plt.title("Figure1")
|
||||
plt.xlabel("iterators", size=14)
|
||||
plt.ylabel("fitness", size=14)
|
||||
t = np.array([t for t in range(0, 100)])
|
||||
fitness = np.array(fitness)
|
||||
plt.plot(t, fitness, color='b', linewidth=3)
|
||||
plt.show()
|
||||
'''
|
||||
Loading…
Reference in New Issue