固定翼无人机飞行的动力学模型python代码

# 下面将搭建军用小型固定翼无人机的刚体动力学模型
# 想以伊朗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()