question3.py

import numpy as np
from question1 import generate_points, throwing
from question2 import blockage
from tqdm import tqdm
import random
import matplotlib.pyplot as plt
from numba import njit
import copy


@njit
def _core(M1_xyz0, M1_direction, M1_speed, TIME_MAX,
               smoke_xyz0, real_time, smoke_speed, delta_time,
               points, seperate):
    M1_xyz = M1_xyz0.copy()
    smoke_xyz = smoke_xyz0.copy()
    smoking_time = np.zeros(3)
    effective = np.zeros(3)
    effective_time = 0.0
    time = np.min(real_time)
    while time < TIME_MAX and np.any(smoking_time < 20.0):
        blocked = False
        for i in range(3):
            if real_time[i] <= time <= real_time[i] + 20.0:
                if (not blocked) or seperate:
                    if blockage(points, smoke_xyz[i], 10.0, M1_xyz).all():
                        blocked = True
                        effective[i] += delta_time

                smoke_xyz[i, 2] -= smoke_speed * delta_time
                smoking_time[i] += delta_time

        if blocked:
            effective_time += delta_time

        # 导弹一步前进
        M1_xyz += M1_direction * M1_speed * delta_time
        time += delta_time


    return effective_time, effective


def block_time(sita, FY1_speed, throwing_time, smoke_time, times=150, seperate=False):
    # throwing_time smoke_time (3,)
    # throwing_time t1 (t2-t1) (t3-t2) throwing_time[0] >0, throwing_time[1], throwing_time[2] >=1
    time = 0
    target_xyz = np.array([0, 0, 0])

    FY1_xyz_0 = np.array([17800, 0, 1800]) # 起始位置
    FY1_direction = np.array([np.cos(sita), np.sin(sita), 0])

    M1_xyz = np.array([20000, 0, 2000]) # 起始位置
    M1_speed = 300 # m/s
    M1_direction = target_xyz - M1_xyz
    M1_direction = M1_direction / np.linalg.norm(M1_direction) # 导弹运动方向

    TIME_MAX = np.linalg.norm(M1_xyz) / M1_speed

    #真实目标数据
    points = generate_points()
    FY1_xyz = np.zeros((3,3)) # 投掷烟雾弹时的位置
    smoke_xyz = np.zeros((3,3)) # 烟雾弹位置

    for i in range(3):
        if i ==0:
            FY1_xyz[i] = FY1_xyz_0 + FY1_direction * FY1_speed * throwing_time[i]
        else:
            FY1_xyz[i] = FY1_xyz[i-1] + FY1_direction * FY1_speed * throwing_time[i]
        smoke_xyz[i] = throwing(FY1_xyz[i], FY1_direction, FY1_speed, smoke_time[i])
    
    smoke_speed = 3
    delta_time = 0.01
    # if times>140:
    #     delta_time =0.01
    smoking_time = np.zeros((3))

    real_time = np.zeros((3))
    real_time[0] = throwing_time[0] + smoke_time[0] #真实时间
    real_time[1] = throwing_time[0] + throwing_time[1] + smoke_time[1]
    real_time[2] = throwing_time[0] + throwing_time[1] + throwing_time[2] + smoke_time[2]
  
    time = np.min(real_time)
    M1_xyz = M1_xyz + M1_direction * M1_speed * time

    effective_time, effective =  _core(M1_xyz, M1_direction, M1_speed, TIME_MAX, smoke_xyz.copy(), real_time, smoke_speed, delta_time, points, seperate)
    if seperate:
        return effective_time, effective
    else:
        return effective_time


def location(sita, FY1_speed, throwing_time, smoke_time):
    throw_time = np.zeros((3))
    throw_time[0] = throwing_time[0]
    throw_time[1] = throwing_time[0] + throwing_time[1]
    throw_time[2] = throwing_time[0] + throwing_time[1] + throwing_time[2]
    FY1_xyz_0 = np.array([17800, 0, 1800])
    FY1_direction = np.array([np.cos(sita), np.sin(sita), 0])
    FY1_xyz = np.zeros((3,3))
    smoke_xyz = np.zeros((3,3))
    for i in range(3):
        FY1_xyz[i] = FY1_xyz_0 + FY1_speed * FY1_direction * throw_time[i]
        smoke_xyz[i] = throwing(FY1_xyz[i], FY1_direction, FY1_speed, smoke_time[i])

    return FY1_xyz, smoke_xyz


class PSO:
    def __init__(self, w, c1, c2, D, N, M):
        self.w = w # 惯性权值
        self.c1 = c1 # 个体学习因子
        self.c2 = c2 # 群体学习因子
        self.D = D # 维度
        self.N = N # 粒子个数
        self.M = M # 最大迭代次数
        self.x = np.zeros((self.N, self.D)) # 粒子位置(N,D)
        self.v = np.zeros((self.N, self.D)) # 粒子速度(N,D)
        self.pbest = np.zeros((self.N, self.D)) #个体最佳位置
        self.pbest_data = np.zeros(self.N) #个体最优数值
        self.gbest = np.zeros((self.D)) #群体最佳位置(1,D)
        self.gbest_data = 0 #群体最优数值

        self.x_range = np.array([[0, 0.25* np.pi],  # sita
                        [70, 140],      # FY1_speed
                        [0, 10],        # throwing_time_1
                        [1, 2],        # throwing_time_2
                        [1, 2],        # throwing_time_3
                        [0, 10],         # smoke_time1
                        [0, 10],        # smoke_time2
                        [0, 10]         # smoke_time3
                        ]      )  
        self.v_range = [-3, 3]
        # init
        for i in range(self.N):
            for j in range(self.D):
                self.x[i,j] = random.uniform(self.x_range[j, 0], self.x_range[j, 1])
                self.v[i,j] = random.uniform(self.v_range[0], self.v_range[1])
            self.pbest[i] = self.x[i]
            self.pbest_data[i] = block_time(self.x[i,0],self.x[i,1],np.array([self.x[i,2], self.x[i,3], self.x[i, 4]]),np.array([self.x[i, 5], self.x[i, 6], self.x[i, 7]]))
            if self.pbest_data[i]> self.gbest_data:
                self.gbest = self.pbest[i]
                self.gbest_data = self.pbest_data[i]
            
    def update(self,times):
        for i in range(self.N):
            self.v[i] = self.w * self.v[i] + self.c1 * random.uniform(0,1) * (self.pbest[i] - self.x[i]) + self.c2 * random.uniform(0,1) * (self.gbest - self.x[i])
            #速度限制
            self.v[i] = np.clip(self.v[i], self.v_range[0], self.v_range[1])
            self.x[i] = self.x[i] +self.v[i]
            
            for j in range(self.D):
                if j == 0:
                    continue
                if self.x[i,j]<self.x_range[j,0]:
                    self.x[i,j] = self.x_range[j,0]
                if self.x[i,j]>self.x_range[j,1]:
                    self.x[i,j] = self.x_range[j,1]

            #更新个体群体最优
            data = block_time(self.x[i,0],self.x[i,1],np.array([self.x[i,2], self.x[i,3], self.x[i, 4]]),np.array([self.x[i, 5], self.x[i, 6], self.x[i, 7]]),times)
            if data > self.pbest_data[i]:
                self.pbest_data[i] = copy.deepcopy(data)
                self.pbest[i] = copy.deepcopy(self.x[i])
            if data > self.gbest_data:
                self.gbest_data = copy.deepcopy(data)
                self.gbest = copy.deepcopy(self.x[i])
                print("data update")
                print(self.gbest, self.gbest_data)

    def run(self):
        best_list = []
        
        for i in tqdm(range(self.M)):
            self.update(i)
            self.w = self.w * 0.995
            
            best_list.append(self.gbest_data)
        print(self.gbest_data)
        print(self.gbest)
        while self.gbest[0] > 2*np.pi or self.gbest[0] < 0:
            if self.gbest[0] < 0:
                self.gbest[0] +=2*np.pi
            if self.gbest[0] > 2*np.pi:
                self.gbest[0] -=2*np.pi
        effective_time, effective = block_time(self.gbest[0],self.gbest[1],
                         np.array([self.gbest[2],self.gbest[3],self.gbest[4]]),
                         np.array([self.gbest[5],self.gbest[6],self.gbest[7]]), 150, True)
        print(effective_time)
        print("各自有效遮挡时间：", effective[0],'\n',effective[1],'\n',effective[2])
        print(self.gbest)
              
        FY1, smoke = location(self.gbest[0],self.gbest[1],np.array([self.gbest[2],self.gbest[3],self.gbest[4]]),np.array([self.gbest[5],self.gbest[6],self.gbest[7]]))
        print(f"火箭角度：{self.gbest[0]}, 飞行速度：{self.gbest[1]}, 烟幕干扰弹投放点：{FY1}, 烟幕干扰弹起爆点：{smoke}")
        plt.figure()
        plt.plot(best_list, label='Best Fitness')
        plt.xlabel("iter")
        plt.ylabel("blockage time")
        plt.title("Iter process")
        plt.show()

if __name__ == "__main__":
    pso = PSO(1, 1.0, 1.0, 8, 40, 150)
    pso.run()
    # 提示：粒子群优化算法具有随机性，每次的运行结果可能并不相同