E题（无人机）

第一问

5部雷达的地理位置坐标分别为：
雷达1（80，0，0），雷达2（30，60，0），雷达3（55，110，0），雷达4（105，110，0），雷达5（130，60，0）（单位：统一为km）。

至少三部雷达探测到

无人机的飞行速度控制120km/h~180km/h
飞行高度控制在2000m~2500m
最大加速度不超过10
无人机间距需控制在100 m以上
同一时刻一架无人机只能干扰一部雷达

贪心算法伪代码：

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

{
　　　　利用可行的决策，求出可行解的一个解元素；
　　 }
由所有解元素组合成问题的一个可行解；

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

df=pd.read_csv('qqq.csv')
df1=pd.read_csv('c.csv')
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

df=pd.read_csv('qqq.csv')
df1=pd.read_csv('c.csv')
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.xlabel('x',fontsize=12,labelpad=5)
plt.xlabel('y',fontsize=12,labelpad=5)
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

 
height3d = pd.read_csv('qqq.csv')
 
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() 
    #visited.add(visiting) 
    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)
      #visited.add(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()