小的無人機仿真代碼
標簽： python
無人機仿真代碼，至少能飛
改善轉彎函數應該可以得到好的仿真
上代碼吧
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)
版權聲明：本文為qq_23093643原創文章，遵循 CC 4.0 BY-SA 版權協議，轉載請附上原文出處鏈接和本聲明。
智能推薦
unity無人機3D仿真總結
開發平臺：unity2017 個人版 目前實現的效果： 無人機的模型是采用moya軟件制作。 控制代碼是使用C#語言。 總結C#語言的用法： （1）腳本的生命周期 1）Awake():在游戲運行時調用，用于初始化，在加載場景時運行。 2）Start()在游戲開始時執行一次，在Awake()函數后面執行。一般將變量初始化、游戲對象的獲取是放在這個函數中運行。 3）Update()在游戲的每一幀都會運...
PX4無人機ROS下仿真開發
PX4無人機ROS下仿真開發 Overview Simulation Px4_control Slam Map Image_process Planning Volans 項目地址volans 注：有任何疑問都可在issues提問:) Simulation 此simulation 包含2D、3D激光雷達模型、深度相機模型、雙目相機模型、realsense相機模型、IRlock相機模型。 配置PX4...
freemarker + ItextRender 根據模板生成PDF文件
1. 制作模板 2. 獲取模板，并將所獲取的數據加載生成html文件 2. 生成PDF文件 其中由兩個地方需要注意，都是關于獲取文件路徑的問題，由于項目部署的時候是打包成jar包形式，所以在開發過程中時直接安照傳統的獲取方法沒有一點文件，但是當打包后部署，總是出錯。于是參考網上文章，先將文件讀出來到項目的臨時目錄下，然后再按正常方式加載該臨時文件； 還有一個問題至今沒有解決，就是關于生成PDF文件...
電腦空間不夠了？教你一個小秒招快速清理 Docker 占用的磁盤空間！
Docker 很占用空間，每當我們運行容器、拉取鏡像、部署應用、構建自己的鏡像時，我們的磁盤空間會被大量占用。 如果你也被這個問題所困擾，咱們就一起看一下 Docker 是如何使用磁盤空間的，以及如何回收。 docker 占用的空間可以通過下面的命令查看： TYPE 列出了docker 使用磁盤的 4 種類型： Images：所有鏡像占用的空間，包括拉取下來的鏡像，和本地構建的。 Con...
猜你喜歡
requests實現全自動PPT模板
http://www.1ppt.com/moban/ 可以免費的下載PPT模板，當然如果要人工一個個下，還是挺麻煩的，我們可以利用requests輕松下載 訪問這個主頁，我們可以看到下面的樣式 點每一個PPT模板的圖片，我們可以進入到詳細的信息頁面，翻到下面，我們可以看到對應的下載地址 點擊這個下載的按鈕，我們便可以下載對應的PPT壓縮包 那我們就開始做吧 首先，查看網頁的源代碼，我們可以看到每一...
Linux C系統編程-線程互斥鎖（四）
互斥鎖 互斥鎖也是屬于線程之間處理同步互斥方式，有上鎖/解鎖兩種狀態。 互斥鎖函數接口 1）初始化互斥鎖 pthread_mutex_init() man 3 pthread_mutex_init (找不到的情況下首先 sudo apt-get install glibc-doc sudo apt-get install manpages-posix-dev) 動態初始化 int pthread_...
統計學習方法 - 樸素貝葉斯
引入問題：一機器在良好狀態生產合格產品幾率是 90%，在故障狀態生產合格產品幾率是 30%，機器良好的概率是 75%。若一日第一件產品是合格品，那么此日機器良好的概率是多少。 貝葉斯模型 生成模型與判別模型 判別模型，即要判斷這個東西到底是哪一類，也就是要求y，那就用給定的x去預測。 生成模型，是要生成一個模型，那就是誰根據什么生成了模型，誰就是類別y，根據的內容就是x 以上述例子，判斷一個生產出...
styled-components —— React 中的 CSS 最佳實踐
https://zhuanlan.zhihu.com/p/29344146 Styled-components 是目前 React 樣式方案中最受關注的一種，它既具備了 css-in-js 的模塊化與參數化優點，又完全使用CSS的書寫習慣，不會引起額外的學習成本。本文是 styled-components 作者之一 Max Stoiber 所寫，首先總結了前端組件化樣式中的最佳實踐原則，然后在此基...
