import networkx as nx import numpy as np import random import math import numpy as np import matplotlib.pyplot as plt from mpl_toolkits import mplot3d ind = 10000 n = 35 memlen = 3 state = np.zeros([n, ind + 1]) initx = np.zeros(n) inity = np.zeros(n) initz = np.zeros(n) finalx = np.zeros(n) finaly = np.zeros(n) finalz = np.zeros(n) for i in range(0, n): # initx[i] = random.random() * 100 # inity[i] = random.random() * 100 # initz[i] = random.random() * 100 # finalx[i] = random.random() * 100 # finaly[i] = random.random() * 100 # finalz[i] = random.random() * 100 # initx[i] = 20*random.random() + 30 # inity[i] = 20*random.random() + 30 # initz[i] = 0 # finalx[i] = 2+i*40/n # finalz[i] = 75 # finaly[i] = 22-abs(i-16)*20/22 # initx[i] = 2 + i*40/n # initz[i] = 75 # inity[i] = 22-abs(i-17)*20/17 # finalx[i] = 5+3*int(i%7) # finalz[i] = 75+3*int(i/7) # finaly[i] = finalz[i]-15 initx[i] = 15+3*int(i%7) initz[i] = 75+3*int(i/7) inity[i] = initz[i]+5 psai = 2*math.pi*i/35 finalx[i] = 120+10*math.cos(psai) finaly[i] = 80+10*math.sin(psai) finalz[i] = 50 # initx[i] = 20 + 8 * math.cos(math.pi * 2 * i / n) # inity[i] = 80 + 8 * math.sin(math.pi * 2 * i / n) # finalx[i] = 30 + i * 10 / n # finaly[i] = 15 + i * 15 / n # initx[i] = 30 + i * 10 / n # inity[i] = 15 + i * 15 / n # finalx[i] = 85-15*abs(i-7)/7 # finaly[i] = 60-40*i/n # initx[i] = 10 + 2 * i * 11 / n # inity[i] = 20 #+ 10 * math.sin(math.pi * 2 * i / n) # finalx[i] = 50 - 2 * i * 11 / n # finaly[i] = 65 # if i <10: # initx[i] = random.random()*30 # else: # initx[i] = random.random()*30 + 70 # inity[i] = random.random()*30 # finalx[i] = 50 + 10 * math.cos(math.pi * 2 * i / n) # finaly[i] = 80 + 10 * math.sin(math.pi * 2 * i / n) #计算无人机到目标点距离，输入：无人机群的xy坐标，目标位置的xy坐标 def getdis(x1, y1, z1,x2, y2,z2): dis1 = np.zeros((n, n)) for i in range(0, n): for j in range(0, n): dis1[i, j] = math.sqrt((x1[i] - x2[j])**2 + (y1[i] - y2[j])**2+(z1[i] - z2[j])**2) return dis1 dis = getdis(initx,inity,initz,finalx,finaly,finalz) #计算无人机之间距离并判断连接关系，输入：无人机群的xy坐标 def getcon(x1, y1,z1): juli1 = np.zeros((n, n)) for i in range(0, n): for j in range(0, n): juli1[i, j] = math.sqrt((x1[i] - x1[j])**2 + (y1[i] - y1[j])**2+(z1[i] - z1[j])**2) con = [] for i in range(0, n): tmpcon = [] for j in range(0, n): if (juli1[i][j] <= 1000 and i != j): tmpcon.append(j) con.append(tmpcon) return con # for i in range(0, n): # for j in range(0, n): # dis[i, j] = math.sqrt((initx[i] - finalx[j])**2 + # (inity[i] - finaly[j])**2) # juli[i, j] = math.sqrt( # (initx[i] - initx[j])**2 + (inity[i] - inity[j])**2) #建立距离存储表格 mem = np.zeros([n, memlen]) #创建记忆存储表格 # con = [] # for i in range(0, n): # tmpcon = [] # for j in range(0, n): # if (juli[i][j] <= 150 and i != j): # tmpcon.append(j) # con.append(tmpcon) def gettasks(dis): tasks = [] for i in range(0, n): temptsk = [] for j in range(0, n): if dis[i, j] <= 1400: temptsk.append(j) tasks.append(temptsk) return tasks con = getcon(initx,inity,initz) #创建任务集：距离80以内才能作为可选任务 tasks = gettasks(dis) for i in range(0, n): state[i, 0] = int(i) #定义个体寻找相同点的函数 def findrep(s1, co1, repi, t): count = 0 for vb in range(0, len(co1[repi])): if s1[repi, t] == s1[co1[repi][vb], t] and repi != vb: count = count + 1 return count #定义寻找list最优点位置程序 def findmin(c): listmin = min(c) for r in range(0, len(c)): if c[r] == listmin: stra = r break return stra def memre(mem): for q in range(1, memlen): for p in range(0, n): mem[p, memlen - q] = mem[p, memlen - 1 - q] def findrep(choice_place, con, i, t): a1 = 0 for j in range(0, len(con[i])): if choice_place == state[con[i][j], t]: a1 = a1 + 1 return a1 def calchoice(t): if (t == 0): choice = 0 else: if (t < memlen): choice = int(random.random() * t) else: choice = int(random.random() * memlen) return choice costsum = np.zeros([n, ind]) stop = np.zeros(n) # def distribute(tasks, dis, con): # t = 0 # while t < ind: # for i in range(0, n): # costsum[i, t] = (findrep(state[i, t], con, i, t) * 1000 + # dis[i, int(state[i, t])]) # for p in range(1, memlen): # for q in range(0, n): # mem[q, memlen - p] = mem[q, memlen - 1 - p] # for i in range(0, n): # if (stop[i] == 0): # cost = [] # for k in range(0, len(tasks[i])): # cost.append(dis[i, tasks[i][k]] + # 1000 * findrep(tasks[i][k], con, i, t)) # mem[i, 0] = tasks[i][findmin(cost)] # else: # mem[i, 0] = state[i, t] # for l in range(0, n): # choice = calchoice(t) # state[l, t + 1] = mem[l, choice] # t = t + 1 # return state # #如果进行再分配，无人机即使避开了此次的重复选择，在选择新的位置时，仍然将选择其他无人机已经选择但是它不知道的位置。 # state = distribute(tasks, dis, con) # np.savetxt('D:/ws0.csv', state, fmt="%d", delimiter=",") #开始计算博弈 for t in range(0, ind): memre(mem)#需要考虑顺序的影响吗 for i in range(0, n): costsum[i, t] = (findrep(state[i, t], con,i,t) * 1000 + dis[i, int(state[i, t])]) for i in range(0, n): cost = [] for k in range(0, len(tasks[i])): cost.append(dis[i, tasks[i][k]] + 1000 * findrep(tasks[i][k], con,i,t)) #存储每个选择的花费 #应该改成：首先生成一轮最优解，后续的每一轮依据之前的选择存入状态，用这个状态判断重复。 #经过如此完成多次的反思最终确定结果 #state[i, t] = findmin(cost) #需要后续修改 mem[i, 0] = tasks[i][findmin(cost)] for l in range(0, n): if (t == 0): choice = 0 else: if (t < memlen): choice = int(random.random() * t) else: choice = int(random.random() * memlen) state[l, t + 1] = mem[l, choice] plt.rcParams['font.sans-serif'] = ['Microsoft YaHei'] # plt.title("15架无人机视角下花费变化图") # plt.xlabel('迭代次数') # plt.ylabel("花费") # for i in range(0, n): # for j in range(0, 500): # plt.plot([j, j + 1], [costsum[i, j], costsum[i, j + 1]]) # plt.show() np.savetxt('D:/ws0.csv', state, fmt="%d", delimiter=",") # np.savetxt('D:/ws3.csv', costsum, fmt="%f", delimiter=",") #画图，1-500次时无人机利润 list = [] for r in range(0, n): list.append(state[r, ind - 1]) list.sort() for i in range(0, len(list) - 1): if (list[i]) == list[i + 1]: print(list) print("存在冲突") break def getlength(x1, x2, y1, y2,z1,z2): return math.sqrt((x1 - x2)**2 + (y1 - y2)**2 + (z1 - z2)**2) def gravi(x1, x2, y1, y2,z1,z2): length = getlength(x1, x2, y1, y2,z1,z2) fgra = 30 + length**2 fgrax = fgra * (x2 - x1) / length fgray = fgra * (y2 - y1) / length fgraz = fgra * (z2 - z1) / length return [fgrax, fgray, fgraz] def repul(x1, x2, y1, y2, z1 , z2): distance = getlength(x1, x2, y1, y2,z1,z2) if (distance < 2): frep = 20 * (2 / distance - 1)**2 else: frep = 0 frepx = frep * (x1 - x2) / (distance) frepy = frep * (y1 - y2) / (distance) frepz = frep * (z1 - z2) / (distance) return [frepx, frepy, frepz] def vituralpoint1(x,y,z): if getlength(x,22,y,45,z,30)<25 and y>(0.5*x+33.5): fv1x = -40*abs(x-22) else: fv1x = 0 fv1y = 0.5*fv1x fv1z = 0.5*fv1x return [fv1x,fv1y,fv1z] position = np.zeros([3, n]) destina = np.zeros([3, n]) for i in range(0, n): point = int(state[i, ind - 1]) destina[0, i] = finalx[point] destina[1, i] = finaly[point] destina[2, i] = finalz[point] position[0, i] = initx[i] position[1, i] = inity[i] position[2, i] = initz[i] cntstep = 2000 stop = np.zeros(n) hisplacex = np.zeros([n, cntstep]) hisplacey = np.zeros([n, cntstep]) hisplacez = np.zeros([n, cntstep]) step = 0.5 speedx = np.zeros([n,ind]) speedy = np.zeros([n,ind]) speedz = np.zeros([n,ind]) #引入斥力场 #返回横线与纵向受力，椭圆形力场，原直线为ax+by=c #那么距离=abs(ax+by+c)/math.sqrt(a**2+b**2) #修改x1,x2位置改变初始 def cirfield(x, y, z, x0, y0, z0, r): length1 = getlength(x,x0,y,y0,z,z0) if length1 >= 1.2*r: fr = 0 else: fr = 3000*(1.2*r/length1 -1) frx = fr * (x - x0)/length1 fry = fr * (y - y0)/length1 frz = fr * (z - z0)/length1 return [frx,fry,frz] def linefield(x0,y0,z0,x1,y1,z1,x,y,z): flx = fly = flz = 0 pointnum = 80 xmid = np.zeros(pointnum) ymid = np.zeros(pointnum) zmid = np.zeros(pointnum) for i in range(0,pointnum-1): xmid[i] = (x1-x0)/pointnum*i+x0 ymid[i] = (y1-y0)/pointnum*i+y0 zmid[i] = (z1-z0)/pointnum*i+z0 for j in range(0,pointnum-1): flx = flx+ ballfield(x,y,z,xmid[j],ymid[j],zmid[j],2)[0] fly = fly+ ballfield(x,y,z,xmid[j],ymid[j],zmid[j],2)[1] flz = flz+ ballfield(x,y,z,xmid[j],ymid[j],zmid[j],2)[2] return [flx,fly,flz] def virpoi(x,y,z,x0,y0,z0): destan1 = getlength(x,x0,y,y0,z,z0) fx = (x0-x)/destan1 fy = (y0-y)/destan1 fz = (z0-z)/destan1 return [fx,fy,fz] def ballfield(x,y,z,x0,y0,z0,r1): destan2 = getlength(x,x0,y,y0,z,z0) if destan2 >1.5*r1: f = 0 else: f = -8000*(1.5*r1/destan2-1) fx = f*(x0-x)/destan2 fy = f*(y0-y)/destan2 fz = f*(z0-z)/destan2 return [fx,fy,fz] def surfield1(x,y,z): #法向量为（a,b,c） if(x + y < (60+2.5*math.sqrt(2))and y<(20) and z<60): ff1 = 40*(5/(x+y-60)-1) else: ff1 = 0 fx = ff1/math.sqrt(2) fy = fx return [fx,fy] def surfield2(x,y,z): if((0.5*x+y)<(40+2.5*math.sqrt(5)) and z<60 and x<40): f2 = 50*(5/(0.5*x+y-40)-1) else: f2 = 0 fx = f2/math.sqrt(5) fy = f2*2/math.sqrt(5) return[fx,fy] def yuanzhu(x,y,z): jl1 = math.sqrt((x-50)**2+(y-60)**2) if(z<80 and jl1<15): f = 1000*(15/jl1-1) else: f = 0 fx = f*(x-50)/jl1 fy = f*(y-60)/jl1 return[fx,fy] def getangel(v1, v2): # 分别计算两个向量的模： module_x = math.sqrt(v1[1]**2 + v1[0]**2 + v1[2]**2) module_y = math.sqrt(v2[1]**2 + v2[0]**2 + v2[2]**2) # 计算两个向量的点积 dot_value = v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] # 计算夹角的cos值： cos_theta = dot_value / (module_x * module_y) return cos_theta def renewposi(force): # if (force != 0): deltax = position[0, i] - destina[0, i] deltay = position[1, i] - destina[1, i] deltaz = position[2, i] - destina[2, i] line1 = [deltax, deltay, deltaz] line2 = [forcex, forcey, forcez] cosangle = getangel(line1, line2) step = 0.3 * 2**(0.5*cosangle) # if cosangle > 0.8: # position[0, i] = position[0, i] - step * forcey / force # position[1, i] = position[1, i] + step * forcex / force # else: position[0, i] = position[0, i] + step * forcex / force position[1, i] = position[1, i] + step * forcey / force position[2, i] = position[2, i] + step * forcez / force speedx[i,jishu] = step * forcex / force speedy[i,jishu] = step * forcey / force speedz[i,jishu] = step * forcez / force # else: # position[0, i] = position[0, i] + random.uniform(-0.05, 0.05) # position[1, i] = position[1, i] + random.uniform(-0.05, 0.05) #添加一个斥力源：中心在某处的圆形 #另一种斥力源的思路：横线障碍，方向为：两个半圆斥力场与两个方向上直线型斥力场 #思考如何处理中途的改进：每20步重新分配一次 #在分配时如何处理已经到达的无人机？ #在博弈时，发现对方已经到达目标位置，就让该无人机变换利润：提高他机，固定本位置为0，或者修改记忆！ jishu = 0 while (1): for i in range(0, n): forcelist = [] for m in range(0, n): if (stop[m] == 0 and m != i): forcelist.append(m) frepx1 =(0 + ballfield(position[0,i],position[1,i],position[2,i],55,80,65,7)[0] # +linefield(25,45,35,50,position[0, i],position[1, i])[0] # #+linefield(70,35,80,55,position[0, i],position[1, i])[0] # +vituralpoint1(position[0, i],position[1, i])[0] # +cirfield(position[0, i],position[1, i],65,40,5)[0] ) frepy1 =(0 + ballfield(position[0,i],position[1,i],position[2,i],55,80,65,7)[1] # +linefield(25,45,35,50,position[0, i],position[1, i])[1] # #+linefield(70,35,80,55,position[0, i],position[1, i])[1] # +vituralpoint1(position[0, i],position[1, i])[1] # +cirfield(position[0, i],position[1, i],65,40,5)[1] ) frepz1 =(0+ballfield(position[0,i],position[1,i],position[2,i],55,80,65,7)[2] ) for count in range(0, len(forcelist)): listrep = repul( position[0, i], position[0, forcelist[count]], position[1, i], position[1, forcelist[count]], position[2, i], position[2, forcelist[count]]) frepx1 = frepx1 + listrep[0] frepy1 = frepy1 + listrep[1] frepz1 = frepz1 + listrep[2] if stop[i] == 0: listgra = gravi(position[0, i], destina[0, i],position[1, i], destina[1, i],position[2, i], destina[2, i]) forcex = frepx1 + listgra[0] forcey = frepy1 + listgra[1] forcez = frepz1 + listgra[2] force = math.sqrt(forcex**2 + forcey**2 + forcez**2) renewposi(force) #一轮位置变化,一次0.5m，接下来判断是否应该停止 hisplacex[i, jishu] = position[0, i] hisplacey[i, jishu] = position[1, i] hisplacez[i, jishu] = position[2, i] if getlength(position[0, i], destina[0, i], position[1, i], destina[1, i], position[2, i],destina[2, i]) <= 0.2: stop[i] = 1 #到达目标点后将斥力变为0 if jishu %100 == 0 : con_re = [] dis_re = np.zeros([n, n]) julire = np.zeros([n, n]) tasksre = [] for d in range(0, n): for f in range(0, n): dis_re[d, f] = math.sqrt((position[0, d] - destina[0, f])**2 + (position[1, d] - destina[1, f])**2 + (position[2, d] - destina[2, f])**2) julire[d, f] = math.sqrt((position[0, d] - position[0, f])**2 + (position[1, d] - position[1, f])**2 + (position[2, d] - position[2, f])**2) tasksre = gettasks(dis_re) for x in range(0, n): tmpcon = [] for c in range(0, n): if (julire[x][c] <= 80 and x != c): tmpcon.append(c) con_re.append(tmpcon) statere = np.zeros(n) for t2 in range(0, n): statere[t2] = state[t2, ind - 1] relist = [] recost = [] findsame = [] t1 = ind - 1 for r in range(0, n): count = 0 for vb in range(0, len(con_re[r])): if state[r, t1] == state[con_re[r][vb], t1] and r != vb: count = count + 1 tmsame = [] tmsame.append(r) tmsame.append(con_re[r][vb]) findsame.append(tmsame) if (count != 0 ): relist.append(r) renum = len(relist) print(findsame) #位置变化后，寻找到重复的无人机，对其重新分配 for t6 in range(0,int(len(findsame)/2)): choicelist1 = [] tmpcost = [] for t7 in range(1,len(findsame[t6])): rebianhao = findsame[t6][t7] relist1 = [] for t8 in range(0,len(con_re[rebianhao])): if statere[con_re[rebianhao][t8]] not in relist1: relist1.append(statere[con_re[rebianhao][t8]]) for t9 in range(len(tasksre[rebianhao])): if tasksre[rebianhao][t9] not in relist1: choicelist1.append(tasksre[rebianhao][t9]) for t10 in range(0,len(choicelist1)): tmpcost.append(dis_re[rebianhao,choicelist1[t10]]) statere[rebianhao] = choicelist1[findmin(tmpcost)] liata = np.sort(statere) print(liata) for t11 in range(0,n): state[t11,t1] = statere[t11] for b in range(0, int(len(findsame)/2)): for b1 in range(0,len(findsame[b])): point = int(statere[int(findsame[b][b1])]) stop[point] = 0 destina[0, findsame[b][b1]] = finalx[point] destina[1, findsame[b][b1]] = finaly[point] destina[2, findsame[b][b1]] = finalz[point] tostop = 1 for cnt in range(0, n): tostop = tostop * stop[cnt] if tostop == 1 or jishu == cntstep-1: print(jishu) break else: jishu = jishu + 1 #或许需要在第30步时再一次分配 np.savetxt('D:/wsf2x.csv', hisplacex, fmt="%f", delimiter=",") np.savetxt('D:/wsf2y.csv', hisplacey, fmt="%f", delimiter=",") np.savetxt('D:/wsf2z.csv', hisplacez, fmt="%f", delimiter=",") np.savetxt('D:/speed1x.csv', speedx, fmt="%f", delimiter=",") np.savetxt('D:/speed1y.csv', speedy, fmt="%f", delimiter=",") np.savetxt('D:/speed1z.csv', speedz, fmt="%f", delimiter=",") placex = [] placey = [] placez = [] for i in range(0, n): for j in range(0, cntstep): if (hisplacex[i, j]) != 0: placex.append(hisplacex[i, j]) placey.append(hisplacey[i, j]) placez.append(hisplacez[i, j]) fig = plt.figure(1) ax = plt.axes(projection='3d') # new_tick = np.linspace(0, 115, 24) # plt.xticks(new_tick) # plt.yticks(new_tick) # plt.title("无人机轨迹散点图") # plt.xlabel('x方向 单位：m') # plt.ylabel("y方向 单位：m") # plt.plot([25,35],[45,50]) # # plt.plot([70,80],[35,55]) # plt.scatter(initx, inity, c='blue', s=10) # plt.scatter(finalx, finaly, c='black', s=10) #蓝色点为起点，黑色点为终点 # plt.scatter(placex, placey, c='red', s=1) #画出散点图，但是不表示顺序 # theta = np.arange(0, 2 * np.pi, 0.01) # x = 65 + 5 * np.cos(theta) # y = 40 + 5 * np.sin(theta) # plt.plot(x, y) # #调用 ax.plot3D创建三维线图 # x = 50+7.5*np.cos(theta) # y = 60+7.5*np.sin(theta) # z = np.linspace(0, 80, 1000) # theta = np.linspace(0,2*np.pi,30) # z,theta = np.meshgrid(z,theta) theta1 = np.linspace(0,2*np.pi,30) fai = np.linspace(0,2*np.pi,30) theta1,fai = np.meshgrid(theta1,fai) xr = 55+6*np.cos(theta1)*np.sin(fai) yr = 80+6*np.sin(theta1)*np.sin(fai) zr = 65+6*np.cos(fai) ax.plot_surface(xr, yr, zr,cmap='viridis', edgecolor='none') # ax.plot_surface(x, y, z,cmap='viridis', edgecolor='none') # x1 = np.linspace(0,40,100) # z1 = np.linspace(0,60,100) # x1,z1 = np.meshgrid(x1,z1) # y1 = -0.5*x1+40 # ax.plot_surface(x1, y1, z1,cmap='viridis', edgecolor='none') # y2 = np.linspace(0,20,80) # y2,z1 = np.meshgrid(y2,z1) # x2 = 60-y2 # ax.plot_surface(x2, y2, z1,cmap='viridis', edgecolor='none') # z3 = 60 # x3 = np.linspace(0,40,100) # x3, z3 = np.meshgrid(x3,z3) # y3 = 40-0.5*x3 # ax.plot_surface(x3, y3, z3,cmap='viridis', edgecolor='none') # ax.set_title('3D line plot') ax.plot3D(0,0,0) ax.plot3D(100,100,0) ax.scatter3D(placex,placey,placez,c='blue',s = 0.1) ax.scatter3D(initx,inity,initz,c='red',s = 0.5) ax.scatter3D(finalx,finaly,finalz,c='red',s = 0.5) # xcnt = [] # for i in range(0,500): # xcnt.append(i) # plt.scatter(hisplacex) plt.show()