# E题（无人机）

## 第一问

5部雷达的地理位置坐标分别为：

1. Search 所有能够由ABC三架无人机按顺序遍历20个点的路线组合：L1，L2，L3...
2. 删除含有相邻点距离大于最大飞行距离的路线
3. 可行解

{
利用可行的决策，求出可行解的一个解元素；
}

##  无人机距离矩阵
import math
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

f1=[]
f2=[]
f3=[]

n=0

def point(x1,y1,z1,x2,y2,z2,z):
x=0
y=0
k=(z-z1)/(z2-z1)
x=round(x1+k*(x2-x1),4)
y=round(y1+k*(y2-y1),4)
return x,y

def distance(x1,y1,x2,y2):
k=math.sqrt((y2-y1)**2+(x2-x1)**2)
return k

n=0
while n<=19:
m=0
#     print('----------')
#     print(n+1)
#     print('----------')
while m<=4:
x,y=point(df1.x[m],df1.y[m],df1.z[m],df.x[n],df.y[n],df.z[n],2.5)
#         print(x,y)
f1.append(x)
f2.append(y)
m+=1
n+=1

i=0
k=0

while k<100:
i=0
while i<100:
x = round(distance(f1[k+4],f2[k+4],f1[i+4],f2[i+4]),4)
#         print(x)
f3.append(x)
i+=5
k+=5

a=np.array(f3).reshape(20,20)
df4=pd.DataFrame(a)
df4

## 理想无人机轨迹图
import math
import pandas as pd
import numpy as np
import matplotlib.pyplot as plt

f1=[]
f2=[]
f3=[]
f4=[]

n=0

def point(x1,y1,z1,x2,y2,z2,z):
x=0
y=0
k=(z-z1)/(z2-z1)
x=round(x1+k*(x2-x1),4)
y=round(y1+k*(y2-y1),4)
return x,y

while n<=19:
m=0
while m<=4:
x,y=point(df1.x[m],df1.y[m],df1.z[m],df.x[n],df.y[n],df.z[n],2)
f1.append(x)
f2.append(y)
m+=1
n+=1

n=0
while n<=19:
m=0
while m<=4:
x,y=point(df1.x[m],df1.y[m],df1.z[m],df.x[n],df.y[n],df.z[n],2.5)
f3.append(x)
f4.append(y)
m+=1
n+=1

fig=plt.figure(221)
plt.scatter(f1,f2,c='red',s=5,alpha=1,label='z=2')
plt.scatter(f3,f4,c='blue',s=5,alpha=1,label='z=2.5')
plt.legend(loc='best',fontsize=12)
plt.grid(True)
plt.show()


##  基于分层的无人机蚁群算法
import numpy as np
import pandas as pd
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import proj3d
from mpl_toolkits.mplot3d import Axes3D

fig = figure()
ax = Axes3D(fig)
X = np.arange(21)
Y = np.arange(21)
X, Y = np.meshgrid(X, Y)
Z = -20*np.exp(-0.2*np.sqrt(np.sqrt(((X-10)**2+(Y-10)**2)/2)))+20+np.e-np.exp((np.cos(2*np.pi*X)+np.sin(2*np.pi*Y))/2)
ax.plot_surface(X, Y, Z, rstride=1, cstride=1, cmap='cool')
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z')
ax.set_title('3D map')

point0 = [0,9,Z[0][9]]
point1 = [20,7,Z[20][7]]

ax.plot([point0[0]],[point0[1]],[point0[2]],'r',marker = u'o',markersize = 15)
ax.plot([point1[0]],[point1[1]],[point1[2]],'r',marker = u'o',markersize = 15)

x0,y0,_ = proj3d.proj_transform(point0[0],point0[1],point0[2], ax.get_proj())
x1,y1,_ = proj3d.proj_transform(point1[0],point1[1],point1[2], ax.get_proj())

label = pylab.annotate(
"start",
xy = (x0, y0), xytext = (-20, 20),
textcoords = 'offset points', ha = 'right', va = 'bottom',
bbox = dict(boxstyle = 'round,pad=0.5', fc = 'yellow', alpha = 1),
arrowprops = dict(arrowstyle = '->', connectionstyle = 'arc3,rad=0'),fontsize=15)
label2 = pylab.annotate(
"end",
xy = (x1, y1), xytext = (-20, 20),
textcoords = 'offset points', ha = 'right', va = 'bottom',
bbox = dict(boxstyle = 'round,pad=0.5', fc = 'yellow', alpha = 1),
arrowprops = dict(arrowstyle = '->', connectionstyle = 'arc3,rad=0'),fontsize=15)
def update_position(e):
x2, y2, _ = proj3d.proj_transform(point0[0],point0[1],point0[2],ax.get_proj())
label.xy = x2,y2
label.update_positions(fig.canvas.renderer)

x1,y1,_ = proj3d.proj_transform(point1[0],point1[1],point1[2],ax.get_proj())
label2.xy = x1,y1
label2.update_positions(fig.canvas.renderer)
fig.canvas.draw()

fig.canvas.mpl_connect('button_release_event', update_position)

def getdistmat(coordinates):
num = coordinates.shape[0]
distmat = np.zeros((52,52))
for i in range(num):
for j in range(i,num):
distmat[i][j] = distmat[j][i]=np.linalg.norm(coordinates[i]-coordinates[j])
return distmat
distmat = getdistmat(coordinates)
numant = 40 #蚂蚁个数
numcity = coordinates.shape[0] #无人机个数
alpha = 1  #信息素重要程度因子
beta = 5  #启发函数重要程度因子
rho = 0.1
Q = 1
iter = 0
itermax = 250
etatable = 1.0/(distmat+np.diag([1e10]*numcity)) #启发函数矩阵，表示蚂蚁从无人机航迹i转移到无人机航迹j的期望程度
pheromonetable = np.ones((numcity,numcity))
pathtable = np.zeros((numant,numcity)).astype(int)
distmat = getdistmat(coordinates)
lengthaver = np.zeros(itermax)
lengthbest = np.zeros(itermax)
pathbest = np.zeros((itermax,numcity))
while iter < itermax:
if numant <= numcity:
pathtable[:,0] = np.random.permutation(range(0,numcity))[:numant]
else:
pathtable[:numcity,0] = np.random.permutation(range(0,numcity))[:]
pathtable[numcity:,0] = np.random.permutation(range(0,numcity))[:numant-numcity]
length = np.zeros(numant)
for i in range(numant):
visiting = pathtable[i,0]
#visited = set()
unvisited = set(range(numcity))
unvisited.remove(visiting)
for j in range(1,numcity):
listunvisited = list(unvisited)
probtrans = np.zeros(len(listunvisited))
for k in range(len(listunvisited)):
probtrans[k] = np.power(pheromonetable[visiting][listunvisited[k]],alpha)\
*np.power(etatable[visiting][listunvisited[k]],alpha)
cumsumprobtrans = (probtrans/sum(probtrans)).cumsum()
cumsumprobtrans -= np.random.rand()
k = listunvisited[find(cumsumprobtrans>0)[0]]
pathtable[i,j] = k
unvisited.remove(k)
length[i] += distmat[visiting][k]
visiting = k
length[i] += distmat[visiting][pathtable[i,0]]

lengthaver[iter] = length.mean()
if iter == 0:
lengthbest[iter] = length.min()
pathbest[iter] = pathtable[length.argmin()].copy()
else:
if length.min() > lengthbest[iter-1]:
lengthbest[iter] = lengthbest[iter-1]
pathbest[iter] = pathbest[iter-1].copy()
else:
lengthbest[iter] = length.min()
pathbest[iter] = pathtable[length.argmin()].copy()

changepheromonetable = np.zeros((numcity,numcity))
for i in range(numant):
for j in range(numcity-1):
changepheromonetable[pathtable[i,j]][pathtable[i,j+1]] += Q/distmat[pathtable[i,j]][pathtable[i,j+1]]
changepheromonetable[pathtable[i,j+1]][pathtable[i,0]] += Q/distmat[pathtable[i,j+1]][pathtable[i,0]]
pheromonetable = (1-rho)*pheromonetable + changepheromonetable
iter += 1
if (iter-1)%20==0:
print iter-1
fig,axes = plt.subplots(nrows=2,ncols=1,figsize=(12,10))
axes[0].plot(lengthaver,'k',marker = u'')
axes[0].set_title('Average Length')
axes[0].set_xlabel(u'iteration')
axes[1].plot(lengthbest,'k',marker = u'')
axes[1].set_title('Best Length')
axes[1].set_xlabel(u'iteration')
fig.savefig('Average_Best.png',dpi=500,bbox_inches='tight')
plt.close()
bestpath = pathbest[-1]
plt.plot(coordinates[:,0],coordinates[:,1],'r.',marker=u'$\cdot$')
plt.xlim([-100,2000])
plt.ylim([-100,1500])
for i in range(numcity-1):#
m,n = bestpath[i],bestpath[i+1]
print m,n
plt.plot([coordinates[m][0],coordinates[n][0]],[coordinates[m][1],coordinates[n][1]],'k')
plt.plot([coordinates[bestpath[0]][0],coordinates[n][0]],[coordinates[bestpath[0]][1],coordinates[n][1]],'b')
ax=plt.gca()
ax.set_title("Best Path")
ax.set_xlabel('X axis')
ax.set_ylabel('Y_axis')
plt.savefig('Best Path.png',dpi=500,bbox_inches='tight')
plt.close()