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

最新推荐文章于 2024-04-26 03:48:12 发布
最新推荐文章于 2024-04-26 03:48:12 发布
阅读量832 收藏 16
点赞数 1
文章标签： 无人机 python 开发语言
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/shengzimao/article/details/135218051
版权 
# 下面将搭建军用小型固定翼无人机的刚体动力学模型
# 想以伊朗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()
赛亚茂
关注
  • 1
    点赞
  • 踩
  • 16
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
固定翼无人机运动学建模
走向CTO的路上...
09-01 1130
鱼弦：CSDN内容合伙人、CSDN新星导师、全栈领域创作新星创作者 、51CTO(Top红人+专家博主) 、github开源爱好者（go-zero源码二次开发、游戏后端架构 https://github.com/Peakchen） 固定翼无人机的运动学建模是描述无人机在空中运动的数学模型，它基于刚体运动的原理和航空力学理论。下面是对固定翼无人机运动学建模的原理详细解释、流程图、使用场景解释、代码示例实现和文献材料链接的解释： 原理详细解释： 固定翼无人机的运动学建模基于刚体运动学的原理，考虑了无
小型固定翼无人机Aerosonde建模与控制仿真
06-18
本实验以小型固定翼无人机 Aerosonde 为对象，通过动力学分析，建立了固定翼飞机非线性动力学模型，并利用 matlab/simulink 对所建模型进行了仿真。本实验选择的控制方法为 PID 控制，其物理意义明确，适用范围广。利用matlab/simulink 对设计的飞行控制系统进行仿真，可以看出，在 PID 控制下，飞机能有较好的飞行效果。
基于matlab设计固定翼太阳能无人机
Matlab_dashi的博客
05-15 134
✅作者简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，matlab项目合作可私信。????个人主页：Matlab科研工作室????个人信条：格物致知。更多Matlab仿真内容点击????智能优化算法 神经网络预测 雷达通信 无线传感器 电力系统信号处理 ...
【飞控初体验】（三）四旋翼无人机动力系统建模
qq_38041309的博客
11-15 2851
四旋翼无人机动力系统建模，包括螺旋桨、电机、电调及电池
基于运动学模型的无人机模型预测控制(MPC)-2
weixin_43822768的博客
06-02 3257
基于无人机自身模型的模型预测控制-无约束情况 1. 模型建立 无人机运动学模型： {x˙=vxvx˙=uxy=vyvy˙=uy \left\{ \begin{aligned} \dot x & = v_x \qquad \dot{v_x}=u_x\\ y & = v_y \qquad \dot{v_y}=u_y \\ \end{aligned} \right. {...
【无人机】采用最基本的自由空间路损模型并且不考虑小尺度衰落(多径多普勒)固定翼无人机轨迹规划（Matlab代码实现）
weixin_66436111的博客
04-15 207
无人机无线通信(UA Vs)是未来通信系统的一个很有前途的技术。在本文中，假设UAV以固定高度水平飞行，通过优化无人机飞行轨迹来研究无人机与地面终端的节能通信，这是一种综合考虑通信吞吐量和无人机能耗的新设计范式。为此，我们首先推导了固定翼无人机推进能量消耗随飞行速度、方向和加速度变化的理论模型。在此基础上，在忽略辐射和信号处理能量消耗的前提下，将无人机通信能量效率定义为有限时间范围内无人机推进能量消耗归一化后的总通信信息比特数。
【无人机】小型固定翼无人机数学模型附matlab代码 上传.zip
04-13
2.领域：智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，更多内容可点击博主头像 3.内容：标题所示，对于介绍可点击主页搜索博客 4.适合人群：本科，硕士...
基于simulink模拟固定翼无人机模型试验.zip
05-18
2.领域：智能优化算法、神经网络预测、信号处理、元胞自动机、图像处理、路径规划、无人机等多种领域的Matlab仿真，更多内容可点击博主头像 3.内容：标题所示，对于介绍可点击主页搜索博客 4.适合人群：本科，硕士...
固定翼无人机检测创建的数据集
12-28
固定翼无人机检测创建的数据集。数据集包含554张图像及其yolo txt注释文件。最初它有319个独特的图像，这个数字随着水平翻转增强而增加。它可以用于目标检测和目标跟踪算法的尝试。 固定翼无人机检测创建的数据集。...
几百张yolo大型固定翼无人机检测数据集
03-23
几百张yolo大型固定翼无人机检测数据集，数据集目录已经配置好，yolo格式(txt)的标签，划分好 train，val, test，并附有data.yaml文件，yolov5、yolov7、yolov8等算法可以直接进行训练模型， 数据集和检测结果参考：...
C语言嵌入式无人机开源项目源代码（包含详细的设计和说明文档）
12-05
该项目源代码是一个典型的嵌入式无人机项目，包含了无人机硬件控制、传感器数据采集、飞行控制算法实现等多个部分。代码结构清晰，注释丰富，易于阅读和理解。其中，无人机硬件控制部分主要通过微控制器对无人机的电机、GPS模块、遥控器模块等进行控制；传感器数据采集部分主要通过加速度传感器、陀螺仪传感器、气压传感器等对无人机的姿态、位置等信息进行采集；飞行控制算法实现部分主要通过PID控制算法对无人机的飞行姿态、飞行速度、飞行高度等进行控制。此外，该项目还包含了一些图像处理和无线通信模块，方便用户进行图像传输和远程控制。总体来说，该项目的源代码具有一定的参考价值和实用性。
MARSIM 无人机动力学模型 代码阅读
chunchun2021的博客
09-07 436
之前一直都不太明白如何获得无人机的位置加速度等信息，在MARSIMA中提供了一种办法：订阅电机的转速话题，可以得知飞行器上的力和扭矩。然后，使用这些力和扭矩，结合飞行器的质量和惯性属性，来估计飞行器的加速度、速度和位置。但是在在实际的飞控系统中，还会有其他的考虑，如扰动、气动效应、控制算法等，MARSIM中提供提供了一个基础的动力学模型。
【无人机姿态运动学模型】
zhy1123380424的博客
04-21 1010
无人机姿态运动学
固定翼飞机数学建模入门（姿态角篇）
高能阿博特的博客
08-22 5387
本文给出了固定翼数学建模的姿态角部分，但并未对气流角与位置建模做进一步探讨，关于这部分的内容后续会慢慢补充。
【系统建模与仿真技术-实验】四旋翼无人机模型建立 基于python
weixin_45598502的博客
12-13 831
参考老师上课给的代码和老师讲的内容。
【无人机姿态动力学模型】
最新发布
zhy1123380424的博客
04-26 1017
无人机姿态动力学模型
python实现飞行控制仿真(二)——三自由度仿真
热门推荐
风雨潇潇一书生
08-04 1万+
一、问题：利用python实现三自由度 【总结不对的地方请评论或私信我，谢谢！】 三自由度中简化的几个问题 1、3个控制量，（相比较6自由度中会使用4个控制量），三自由度中一般使用推力、升力和滚转角来控制，或使用其变形模式 2、三自由度中不考虑攻角和侧滑角，假设其为0， 3、速度方向和机体方向近似重合 二、求解过程 三自由度模型中主要包含速度、角度及其变化的模型，不包含力学和力矩关系方程。 上图方程中的x,y,z为惯性坐标系下的位置坐标，v为飞行器的速度，为飞行器轨迹的航迹角，.
Pixhawk学习9——固定翼位置控制（L1控制+TECS总能量控制）
victoryqian528的博客
03-02 8681
本篇博客本来没有在之前的计划中。但由于最近项目上遇到了固定翼轨迹控制的一些问题，所以就结合pix的程序学习总结了一下。 不同于旋翼，固定翼要实现位置变化必须得有一个轨迹变化过程，因为它不像旋翼那样可以直接调整力的矢量，也不能旋翼那样悬停和直上直下。 Pixhawk中的固定翼位置控制分为两部分：横航向的L1控制和纵向的TECS控制（总能量）。作为普通固定翼的执行机构，分为横滚（副翼）、偏航（方向舵）...
【基于MPC飞行器最佳控制】针对固定翼飞行器的最短时间航迹的最佳控制策略，考虑航路点约束研究（Matlab代码实现）
Ke_Yan_She的博客
01-18 840
这样，MPC控制器就能够在保证航路点到达的同时，满足其他约束条件，实现飞行器的最短时间航迹。在每个控制周期内，MPC控制器将对未来一段时间内的飞行器状态进行预测，并通过优化算法计算出最优的控制输入，以实现最短时间航迹的目标。另外，航路点约束的考虑也可以通过将航路点的到达时间和顺序等信息纳入MPC控制器中，从而实现对航路点的约束。综上，基于MPC的最佳控制策略可以很好地应用于固定翼飞行器的最短时间航迹规划，并且能够充分考虑航路点约束，以实现飞行器的最佳性能。博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。
给出一个带有一个Leader三个Follower的基于固定翼无人机模型预测控制例子
03-30
这里给出一个基于固定翼无人机模型的预测控制例子，其中有一个Leader和三个Follower。 首先，我们需要建立一个固定翼无人机模型，可以使用动力学模型来描述它的运动。该模型可以表示为： $$ \begin{aligned} \dot{x} &= v_x \\ \dot{y} &= v_y \\ \dot{z} &= v_z \\ \dot{v}_x &= \frac{1}{m} (F \cos \theta \sin \phi - D \sin \theta + g \cos \theta \cos \phi) \\ \dot{v}_y &= \frac{1}{m} (F \cos \theta \cos \phi + D \cos \theta \sin \phi + g \cos \theta \sin \phi) \\ \dot{v}_z &= \frac{1}{m} (F \sin \theta - D \cos \theta - g \sin \theta) \\ \dot{\phi} &= p + \tan \theta (q \sin \phi + r \cos \phi) \\ \dot{\theta} &= q \cos \phi - r \sin \phi \\ \dot{\psi} &= \frac{1}{\cos \theta} (q \sin \phi + r \cos \phi) \\ \dot{p} &= \frac{1}{I_x} (l + I_y r q - I_z q r) \\ \dot{q} &= \frac{1}{I_y} (m + I_z p r - I_x p r) \\ \dot{r} &= \frac{1}{I_z} (n + I_x p q - I_y p q) \end{aligned} $$ 其中，$x, y, z$是无人机的位置坐标，$v_x, v_y, v_z$是无人机的速度，$\phi, \theta, \psi$是无人机的欧拉角，$p, q, r$是无人机的角速度，$m$是无人机的质量，$F$是无人机的推力，$D$是无人机的阻力，$g$是重力加速度，$l, m, n$是无人机的力矩，$I_x, I_y, I_z$是无人机的惯性矩。 接下来，我们需要进行预测控制。首先，我们将Leader无人机的位置和速度作为参考信号，然后我们使用MPC（Model Predictive Control）算法来计算Follower无人机的控制输入，使其跟随Leader无人机。MPC算法需要在每个时间步骤中求解一个优化问题来计算控制输入。 在本例中，我们将使用Python编写MPC算法。我们需要定义有限时间内的预测模型，包括状态方程和输出方程，以及控制输入的限制。然后，我们使用QP（Quadratic Programming）求解器来求解优化问题，得到控制输入。 最后，我们将控制输入应用于Follower无人机的动力学模型中，使其跟随Leader无人机。我们可以使用ROS（Robot Operating System）来实现无人机的控制和通信，以及Gazebo来模拟无人机的运动。 总体来说，基于固定翼无人机模型的预测控制例子需要涉及到动力学模型、MPC算法、QP求解器、ROS、Gazebo等多个方面的知识和技术。
评论 2
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值