小的无人机仿真代码

无人机仿真代码，至少能飞

改善转弯函数应该可以得到好的仿真

上代码吧

import math
import matplotlib.pyplot as plt
import random
pi = math.pi

class Fly:
    def __init__(self, vec, rad, x, y):
        self.vec = vec  # 速度
        self.rad = rad  # 方向
        self.x = x  # 坐标
        self.y = y
        self.trace_x = []
        self.trace_y = []   # 移动记录
        self.trace_x.append(int(self.x))
        self.trace_y.append(int(self.y))
        self.sn = -1
        self.old_x=0
        self.old_y=0

    def move(self):
        self.x += self.vec * math.cos(self.rad)
        self.y += self.vec * math.sin(self.rad)
        # print(self.x, ' , ', self.y)
        self.trace_x.append(int(self.x))
        self.trace_y.append(int(self.y))

    def try_move(self, rad):
        self.old_x = self.x
        self.old_y = self.y
        self.x += self.vec * math.cos(rad)
        self.y += self.vec * math.sin(rad)
        # self.trace_x.append(int(self.x))
        # self.trace_y.append(int(self.y))


    def back(self):
        self.x = self.old_x
        self.y = self.old_y
        # self.trace_x.pop()
        # self.trace_y.pop()

    def red_cul_sn(self, blue, r):

        weight = 0
        where = 0
        for b in blue:
            d = distance(self, b)
            if self.x - b.x > 30000:
                self.rad = pi
                return
            sn = blue_k * norlize(math.pow(d/100, -2))
            dir = (math.atan2(b.y -self.y, b.x -self.x) + 2 * pi) % (2 * pi)

            # if dir > b.rad:
            #     if dir - b.rad > pi:
            #         dir = beta * (b.rad + 2 * pi) + dir * (1 - alpha)
            #     else:
            #         dir = dir * (1 - alpha) + beta * b.rad
            # else:
            #     if b.rad - dir > pi:
            #         dir = (dir + 2 * pi) * (1 - beta) + beta * b.rad
            #     else:
            #         dir = beta * b.rad + dir * (1 - beta)
            # if d > 5000:
            #     if dir > b.rad:
            #         if dir - b.rad > pi:
            #             dir = alpha * (b.rad + 2 * pi) + dir * (1 - alpha)
            #         else:
            #             dir = dir * (1 - alpha) + alpha * b.rad
            #     else:
            #         if b.rad - dir > pi:
            #             dir = (dir + 2 *pi) * (1 - alpha) + alpha * b.rad
            #         else:
            #             dir = alpha * b.rad + dir * (1 - alpha)
            # else:
            #     if dir > b.rad:
            #         if dir - b.rad > pi:
            #             dir = beta * (b.rad + 2 * pi) + dir * (1 - alpha)
            #         else:
            #             dir = dir * (1 - alpha) + beta * b.rad
            #     else:
            #         if b.rad - dir > pi:
            #             dir = (dir + 2 * pi) * (1 - beta) + beta * b.rad
            #         else:
            #             dir = beta * b.rad + dir * (1 - beta)
            dir = (dir + 2*pi + pi/4) % (2*pi)
            if weight == 0:
                weight = sn
                where = dir
                # print(where)
                continue
            weight, where = cal_where(weight, where, sn, dir)

        flag = 0
        # if self.rad > where:
        #     if self.rad - where > pi:
        #         if (min(self.rad, 2 * pi - self.rad) + min(where, 2 * pi - where)) > red_turn_rad:
        #             # print('1')
        #             self.rad = self.rad + red_turn_rad
        #             flag = 1
        #     else:
        #         if self.rad - where > red_turn_rad:
        #             # print('2')
        #             self.rad = self.rad - red_turn_rad
        #             flag = 1
        # else:
        #     if where - self.rad > pi:
        #         # print('3')
        #         if min(self.rad, 2 * pi - self.rad) + min(where, 2 * pi - where) > red_turn_rad:
        #             self.rad = self.rad - red_turn_rad
        #             flag = 1
        #     else:
        #         # print('4')
        #         if where - self.rad > red_turn_rad:
        #             self.rad = self.rad + red_turn_rad
        #             flag = 1
        # if flag == 0:
        self.rad = where
        self.rad = (self.rad +2 * pi) % (2 * pi)

        # cc = 0
        # while (1):
        #     print(cc)
        #     cc += 1
        #     rad = (self.rad + (random.random() - 0.5) % (0.0001 * pi) + 2 * pi) % (2 * pi)
        #     self.try_move(rad)
        #     if self.constrain(r):
        #         self.back()
        #         self.rad = rad
        #         break
        #     self.back()


    def blue_cul_sn(self, red):
        self.rad = 0
        return
        weight = 1
        where = self.rad
        for r in red:
            for rr in r:
                d = distance(self, rr)
                dir = rr.rad
                sn = red_k * (math.pow(d/100, -2)) * math.fabs(dir - self.rad)
                if weight == 1:
                    weight = sn
                    where = dir
                    continue
                weight, where = cal_where(weight, where, sn, dir)
                # print(where)

        # 对于上边界
        up_dir = math.pi * 3 / 2
        if y_up[1] - self.y <= 1000:
            up_sn = up_k * norlize(math.pow((y_up[1] - self.y)/100, -2))
            weight, where = cal_where(weight, where, up_sn, up_dir)

        # 对于下边界
        down_dir = math.pi / 2
        if 0< self.y <= 1000:
            down_sn = down_k * norlize(math.pow(self.y/100, -2))
            weight, where = cal_where(weight, where, down_sn, down_dir)

        # 对于右边界（目的地）
        right_dir = 0
        right_sn = right_k
        # print(where)
        weight, where = cal_where(weight, where, right_sn, right_dir)

        # print(where)

        # 完成角度计算
        flag = 0
        if self.rad > where:
            if self.rad - where > pi:
                if min(self.rad, 2*pi - self.rad) + min(where, 2*pi - where) > blue_turn_rad:
                    # print('1')
                    self.rad = self.rad + blue_turn_rad
                    flag = 1
            else:
                if self.rad - where > blue_turn_rad:
                    # print('2')
                    self.rad = self.rad - blue_turn_rad
                    flag = 1
        else:
            if where - self.rad > pi:
                # print('3')
                if min(self.rad, 2*pi - self.rad) + min(where, 2*pi - where) > blue_turn_rad:
                    self.rad = self.rad - blue_turn_rad
                    flag = 1
            else:
                # print('4')
                if where - self.rad > blue_turn_rad:
                    self.rad = self.rad + blue_turn_rad
                    flag = 1
        if flag == 0:
            self.rad = where
        self.rad = (self.rad + 2 * pi) % (2 * pi)

    def constrain(self, r):
        min_count = 0
        for rr in r:
            if distance(self, rr) < 30:
                min_count +=1
            if min_count > 1:
                return False
        max_count = 0
        for rr in r:
            if distance(self, rr) < 200:
                max_count += 1
        if max_count < 3:
            return False
        return True


def draw(blue, red, x_up, y_up):
    # plt.ion()
    plt.ylim(y_up[0], y_up[1])
    plt.xlim(x_up[0], x_up[1])
    # llen = len(blue[0].trace_x)
    point = range(len(blue[0].trace_x))
    for b in blue:
        plt.scatter(b.trace_x, b.trace_y, c=point, cmap=plt.cm.Blues, s=5)
        # plt.pause(0.2)

    for r in red:
        for rr in r:
            plt.scatter(rr.trace_x, rr.trace_y, c=point, cmap=plt.cm.Reds, s=5)
    plt.show()



def cal_where(v1_w, v1_d, v2_w, v2_d):
    v1_x = v1_w * math.cos(v1_d)
    v1_y = v1_w * math.sin(v1_d)
    v2_x = v2_w * math.cos(v2_d)
    v2_y = v2_w * math.sin(v2_d)
    new_w = math.sqrt(math.pow(v1_x + v2_x, 2) + math.pow(v1_y + v2_y, 2))
    new_d = (math.atan2(v1_y + v2_y, v1_x + v2_x) + 2 * math.pi) % (2 * math.pi)
    return new_w, new_d

def blue_success(blue):
    # 判断蓝色方是否逃脱了
    count = 0
    for i in range(len(blue)):
        if blue[i].x >= x_up[1]:
            print('蓝色飞机- ', i + 1, ' -逃脱成功！')
            count += 1
    return count == len(blue)

def distance(f1, f2):
    # 判断任意两个飞机的距离
    return math.sqrt(math.pow((f1.x - f2.x), 2) + math.pow((f1.y - f2.y), 2))

def distance1(f1, f2):
    # 判断任意两个飞机的距离
    return math.sqrt(math.pow((f1.x - f2.x), 2) + math.pow((f1.y - f2.y), 2))

def red_success(red, blue):
    # 判断红色方是否捕捉了 蓝色
    for i in range(len(blue)):
        count = 0
        for r in red:
            for rr in r:
                if distance1(rr, blue[i]) <= cap_distance:
                    count += 1
                if count > 1:
                    print('红色飞机捕获了蓝色飞机- ', )
                    return True
    return False

def red_distance(f1, f2):
    # 判断红色小飞机是否满足约束 30
    return distance(f1, f2) >= red_min_distance

def cal_sn(red, blue):
    # 计算方向操作
    for b in blue:
        b.blue_cul_sn(red)

    for r in red:
        for rr in r:
            rr.red_cul_sn(blue, r)

def all_move(red, blue):
    # 移动操作
    for b in blue:
        b.move()

    for r in red:
        for rr in r:
            rr.move()

def initial_red(red_num, cycle_x, cycle_y):
    avg = 2 * math.pi / red_num
    angles = []
    for i in range(red_num):
        angles.append(i * avg)
    FY = []
    for a in angles:
        x = red_radius * math.cos(a) + cycle_x
        y = red_radius * math.sin(a) + cycle_y
        fy = Fly(vec=rr_vec, rad=math.pi, x=x, y=y)
        FY.append(fy)
    return FY

def beyond(blue):
    count = 0
    for i in range(len(blue)):
        if blue[i].x < x_up[0] or blue[i].y > y_up[1] or blue[i].y < y_up[0]:
            print('蓝色飞机- ', i + 1, ' -撞墙！')
            count += 1
    return count == len(blue)

def norlize(s):
    return (s - nor_min) / (nor_max - nor_min)

x_up = [0, 50000]  # x为 0 ~ 50 km
y_up = [0, 70000]
cap_distance = 300.
blue_turn_rad = 0.0795774
red_turn_rad = 0.09094568
red_min_distance = 30.
red_radius = 100.
rr_vec = 200
blue_vec = 250
red_k = 0.05      # 红色飞机排斥系数
blue_k = 1      # 蓝色飞机吸引系数
up_k = 10      # 上界的排斥系数
down_k = 10      # 下界的排斥系数
right_k = 0.0005   # 目标的吸引系数
epochs = 360
alpha = 0.3
beta = 0.7
nor_max, nor_min = 1, 1 / 10000


# 初始化个数以及出生位置
blue1 = Fly(vec=blue_vec, rad=0., x=x_up[0], y=y_up[1] /2)
# blue2 = Fly(vec=blue_vec, rad=0., x=x_up[0], y=y_up[1] /2)
# blue3 = Fly(vec=blue_vec, rad=0., x=x_up[0], y=y_up[1] *3/4)
blue = [blue1]
red1 = initial_red(5, x_up[1], y_up[1]-20000)
red2 = initial_red(5, x_up[1], y_up[0]+20000)
# red3 = initial_red(5, x_up[1], y_up[1] * 2/3)
# red4 = initial_red(5, x_up[1], y_up[1] /3)
# red5 = initial_red(5, x_up[1], y_up[1] /6)
red = [red1, red2]
x_history = []
y_history = []
# for i in range(1, x_up[1] - 1, 250):
#     for j in range(1, y_up[1]-1, 250):
#         blue1 = Fly(vec=blue_vec, rad=0., x=i, y=j)
#         blue = [blue1]
for i in range(1, epochs + 1):
    '''
    每个东西 都计算势能
    然后根据势能计算下一步的位置
    '''
    cal_sn(red, blue)

    all_move(red, blue)


    if blue_success(blue):
        # x_history.append(i)
        # y_history.append(j)
        print('当前 ', i, ' 秒')
        print('蓝色飞机 1 成功逃脱')
        break

    if red_success(red, blue):
        print('当前 ', i, ' 秒')
        print('红色飞机成功捕获 蓝色飞机 1')
        break

    if beyond(blue):
        print('当前 ', i, ' 秒')
        print('蓝色飞机 1 撞墙失败')
        break

def ddraw(x, y):
    plt.ylim(y_up[0], y_up[1])
    plt.xlim(x_up[0], x_up[1])
    plt.scatter(x, y, s=5)
    plt.show()

draw(blue, red, x_up, y_up)
# ddraw(x_history,y_history)