627 lines
20 KiB
Python
627 lines
20 KiB
Python
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() |