# 下面将搭建军用小型固定翼无人机的刚体动力学模型
# 想以伊朗shahed-135自杀式无人机的刚体运动学模型为例
# 但是实际上并没有找到这样的动力学模型,只能退而求其次
# 代码中的动力学模型来自魏瑞轩的《先进无人机系统与作战运用》.P60.
import numpy as np
import matplotlib.pyplot as plt
from numpy import cos,sin,tan,pi,sqrt
from Env3D import *
# ---------------下面是无人机的重要超参数------------
UAV_M = 300 # 无人机的质量
UAV_FLAY_RANGE = 30 # 无人机的安全半径
M,N,L_bar = 1.0,1.0,1.0 # 重要系数
I_x,I_y,I_z,I_xz = 1.0,1.0,1.0,0 # x,y,z,xz轴的转动惯量
g = 9.81 # 重力加速度
DELTA_t = 0.1 # 采样时间
# --------------下面开始声明UAV类的信息-------------
# UAV的结点类
class Node:
def __init__(self,x_index,y_index,z_index,
phi_index,theta_index,psi_index,
Fx,Fy,Fz,radius_safe = UAV_FLAY_RANGE,
x_list=None,y_list=None,z_list=None,
phi_list=None, theta_list=None, psi_list=None,
u_list = None,v_list = None,w_list = None,
p_list = None,q_list = None,r_list = None,
length=0,parent_index=None,cost=None):
self.x_index = x_index
self.y_index = y_index
self.z_index = z_index
self.phi_index = phi_index
self.theta_index = theta_index
self.psi_index = psi_index
self.x_list = x_list
self.y_list = y_list
self.z_list = z_list
self.phi_list = phi_list
self.theta_list = theta_list
self.psi_list = psi_list
self.u_list = u_list
self.v_list = v_list
self.w_list = w_list
self.p_list = p_list
self.q_list = q_list
self.r_list = r_list
self.radius_safe = radius_safe
self.parent_index = parent_index
self.cost = cost
self.Fx = Fx
self.Fy = Fy
self.Fz = Fz
# 表示从起点开始到当前点走的距离
self.length = length
# --------------下面声明UAV类的非线性动力学模型-------------
class UAV:
def __init__(self,
x_g=0,y_g=0,h=500,
u=0,v=0,w=0,
p=0,q=0,r=0,
phi=0,theta=0,psi=0,
M=M, N=N, L_bar=L_bar, range=UAV_FLAY_RANGE,
I_x=I_x,I_y=I_y,I_z=I_z,I_xz=I_xz,
m=UAV_M,g=g,delta_t=DELTA_t):
# 无人机的结构参数
self.range = range # 无人机的雷达探测半径
# 下面声明重要的超参数
self.delta_t = delta_t
self.m = m
self.g = g
self.I_x = I_x
self.I_y = I_y
self.I_z = I_z
self.I_xz = I_xz
self.M = M
self.N = N
self.L_bar = L_bar
# 下面声明重要的状态量
self.x_g,self.y_g,self.h = x_g,y_g,h
self.u,self.v,self.w = u,v,w
self.p,self.q,self.r = p,q,r
self.phi,self.theta,self.psi = phi,theta,psi
# 最终的状态量
# self.state = array([x_g,y_g,h,u,v,w,p,q,r,phi,theta,psi])
self.state = np.array([self.x_g,self.y_g,self.h,
self.u,self.v,self.w,
self.p,self.q,self.r,
self.phi,self.theta,self.psi])
self.state_dot = np.zeros_like(self.state)
def update_state(self,F_x,F_y,F_z):
g,m,delta_t = self.g,self.m,self.delta_t
I_x,I_y,I_z,I_xz = self.I_x,self.I_y,self.I_z,self.I_xz
L_bar,M,N = self.L_bar,self.M,self.N
x_g,y_g,h = self.state[0],self.state[1],self.state[2]
u, v, w = self.state[3],self.state[4],self.state[5]
p, q, r = self.state[6],self.state[7],self.state[8]
phi, theta, psi = self.state[9],self.state[10],self.state[11]
# 计算状态量的导数
x_g_dot = u * cos(theta)*cos(psi) + \
v * (sin(phi)*sin(theta)*cos(psi) - cos(phi)*sin(psi)) + \
w * (sin(phi)*sin(psi) + cos(phi)*sin(theta)*cos(psi))
y_g_dot = u * cos(theta) * sin(psi) + \
v * (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)) + \
w * (-sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi))
h_dot = u * sin(theta) - v * sin(phi) * cos(theta) - w * cos(phi) * cos(theta)
u_dot = v * r - w * q - g * sin(theta) + F_x/m
v_dot = - u * r + w * p + g * cos(theta) * sin(phi) + F_y/m
w_dot = u * q - v * p + g * cos(theta) * cos(phi) + F_z/m
# I_matrix = np.array([[I_x,-I_xz,0],[-I_xz,I_z,0],[0,0,I_y]])
b = np.array([ L_bar - q * r * (I_z - I_y) + p*q * I_xz ,
N - p * q * (I_y - I_x) - q * r * I_xz ,
M - p * r * (I_x - I_z) - (p**2 - r**2) * I_xz])
p_dot = (b[0] * I_z + b[1] * I_xz) / (I_z * I_x - I_xz**2)
q_dot = b[2] / I_y
r_dot = (b[0] * I_xz + b[1] * I_x) / (I_z * I_x - I_xz**2)
phi_dot = p + (r * cos(phi) + q * sin(phi)) * tan(theta)
theta_dot = q * cos(phi) - r * sin(phi)
psi_dot = (r * cos(phi) + q * sin(phi))/cos(theta)
# 更新状态导数
state_dot = np.array([x_g_dot,y_g_dot,h_dot,u_dot,v_dot,w_dot,
p_dot,q_dot,r_dot,phi_dot,theta_dot,psi_dot])
# 直接采用差分更新,后面可以考虑Rung-Kutta更新
self.state = self.state + state_dot * delta_t
self.state_dot = state_dot
# 获得当前无人机的位置和姿态
def get_state(self):
# 返回x_g,y_g,h,phi,theta,psi
return self.state[0],self.state[1],self.state[2],self.state[9],self.state[10],self.state[11]
# ------------下面是全局函数类的声明--------------------
# 当前角度全部采用弧度制,同样对于超过2pi的角度也适用
def pi_2_pi(angle):
return (angle+pi) % (2*pi) - pi
# ------------无人机的运动学方程--------------------
def move( F_x,F_y,F_z,
current_state,
delta_t = DELTA_t,m=UAV_M):
# state : array([x_g,y_g,h,u,v,w,p,q,r,phi,theta,psi])
# F_x,F_y,F_z:3自由度控制量
x_g, y_g, h = current_state[0],current_state[1],current_state[2]
u, v, w = current_state[3],current_state[4],current_state[5]
p, q, r = current_state[6],current_state[7],current_state[8]
phi, theta, psi = current_state[9],current_state[10],current_state[11]
x_g_dot = u * cos(theta) * cos(psi) + \
v * (sin(phi) * sin(theta) * cos(psi) - cos(phi) * sin(psi)) + \
w * (sin(phi) * sin(psi) + cos(phi) * sin(theta) * cos(psi))
y_g_dot = u * cos(theta) * sin(psi) + \
v * (sin(phi) * sin(theta) * sin(psi) + cos(phi) * cos(psi)) + \
w * (-sin(phi) * cos(psi) + cos(phi) * sin(theta) * sin(psi))
h_dot = u * sin(theta) - v * sin(phi) * cos(theta) - w * cos(phi) * cos(theta)
u_dot = v * r - w * q - g * sin(theta) + F_x / m
v_dot = - u * r + w * p + g * cos(theta) * sin(phi) + F_y / m
w_dot = u * q - v * p + g * cos(theta) * cos(phi) + F_z / m
# I_matrix = np.array([[I_x,-I_xz,0],[-I_xz,I_z,0],[0,0,I_y]])
b = np.array([L_bar - q * r * (I_z - I_y) + p * q * I_xz,
N - p * q * (I_y - I_x) - q * r * I_xz,
M - p * r * (I_x - I_z) - (p ** 2 - r ** 2) * I_xz])
p_dot = (b[0] * I_z + b[1] * I_xz) / (I_z * I_x - I_xz ** 2)
q_dot = b[2] / I_y
r_dot = (b[0] * I_xz + b[1] * I_x) / (I_z * I_x - I_xz ** 2)
phi_dot = p + (r * cos(phi) + q * sin(phi)) * tan(theta)
theta_dot = q * cos(phi) - r * sin(phi)
psi_dot = (r * cos(phi) + q * sin(phi)) / cos(theta)
state_dot = np.array([x_g_dot,y_g_dot,h_dot,
u_dot,v_dot,w_dot,
p_dot,q_dot,r_dot,
phi_dot,theta_dot,psi_dot])
next_state = current_state + delta_t * state_dot
return next_state
if __name__ == "__main__":
uav = UAV(x_g=107,y_g=28,h=600,u=0,v=0,w=500)
numbers = 10
plt.ion()
# -----------下面具体的环境--------------
print("--------下面导入环境----------")
# env = Env()
# ax = env.draw_DEM()
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
x_g_list,y_g_list,h_list,phi_list,theta_list,psi_list = [],[],[],[],[],[]
# state : array([x_g,y_g,h,u,v,w,p,q,r,phi,theta,psi])
for i in range(numbers):
x_g, y_g, h, phi, theta, psi = uav.get_state()
uav.update_state(F_x=0,F_y=0,F_z=10000)
plot_uav_3d(ax,uav.state[0],uav.state[1],uav.state[2],uav.state_dot[0],uav.state_dot[1],uav.state_dot[2],
length_max=5)
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.pause(0.01)
print("第{:}次的坐标为({:},{:},{:}),速度为({:},{:},{:})".format(i,uav.state[0],uav.state[1],uav.state[2],
uav.state_dot[0],uav.state_dot[1],uav.state_dot[2]))
plt.ioff()
# plot_uav_3d(ax, x, y, z, vx, vy, vz,
# v_max=V_MAX, length_max=5, center_color='b',
# safe_range=UAV_SAFE_RADIUS, center_v_color='r'):
plt.show()
固定翼无人机飞行的动力学模型python代码
最新推荐文章于 2024-04-26 03:48:12 发布