【四轴飞行器】非线性3DOF四轴飞行器模拟器（Matlab代码实现）

wlz249

于 2024-07-24 08:23:55 发布

阅读量698

点赞数 17

文章标签： 3d matlab 开发语言

本文链接：https://blog.csdn.net/weixin_66436111/article/details/140651923

版权

“在代码的海洋里，有无尽的知识等待你去发现。我就是那艘领航的船，带你乘风破浪，驶向代码的彼岸。

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

1. 建立动力学模型

2. 软件工具选择

3. 控制器设计与集成

4. 用户界面与可视化

5. 测试与验证

6. 优化与迭代

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

四轴飞行器，由于其结构简单、机动性强及成本较低等优点，在无人机技术中占有重要地位。然而，四轴飞行器的控制是一个复杂的非线性问题，尤其是当涉及到三维自由度（3DOF）的动态控制时，即控制飞行器的俯仰（pitch）、偏航（yaw）和滚转（roll）三个旋转自由度。为了在真实飞行前测试和优化控制算法，非线性3DOF四轴飞行器模拟器成为了一个不可或缺的工具。3DOF四轴飞行器模拟器基于飞行器的动力学模型，该模型考虑了飞行器在空间中的旋转运动。在实际飞行前，模拟器允许工程师测试和优化飞行控制算法，确保其稳定性和准确性。相比于实物原型，模拟器可以大大降低开发成本，避免因测试失败造成的硬件损坏。非线性3DOF四轴飞行器模拟器是四轴飞行器研发和控制算法开发中不可或缺的工具。它不仅有助于在真实飞行前优化飞行性能，而且在教育、培训和系统测试方面也发挥着重要作用。随着仿真技术的不断进步，未来的模拟器将更加逼真，提供更高级的交互性和更广泛的测试功能。

开发一个非线性3自由度（3DOF）的四轴飞行器模拟器，主要目的是为了在不实际飞行的情况下，预测和分析四轴飞行器的动态行为，这在设计初期阶段特别有用，可以帮助研究人员和工程师优化控制器设计、验证理论模型，并进行故障模式分析。下面是设计这样一个模拟器的主要步骤和要素：

1. 建立动力学模型

非线性动力学方程：首先，需要根据四轴飞行器的动力学原理，建立非线性数学模型。这通常包括位置(x, y, z)和偏航角(ψ)作为自由度，忽略滚转(φ)和俯仰(θ)的影响，因为3DOF模型假设四轴飞行器始终保持水平。模型将包含惯性力、重力、升力（由电机产生）以及由于螺旋桨产生的反扭矩等项。
状态空间表示：将动力学方程转化为一组微分方程，形式上为𝑥˙=𝑓(𝑥,𝑢)x˙=f(x,u)，其中𝑥x是系统状态向量（位置和偏航角），𝑢u是输入向量（电机转速）。

2. 软件工具选择

编程语言与库：可以选择Python、MATLAB/Simulink或其他编程语言。对于Python，可以利用scipy.integrate.solve_ivp进行微分方程求解；MATLAB则有内置的ode45等求解器。另外，也可以使用专门的仿真软件如Gazebo、Blender等，这些软件提供了更高级的物理引擎和图形界面。

3. 控制器设计与集成

PID控制器：针对非线性模型，可能需要设计并集成先进的控制器，如PID控制器（比例-积分-微分控制器）或者更复杂的自适应/非线性控制器，来控制飞行器的位置和姿态。

4. 用户界面与可视化

界面设计：开发一个简单的用户界面，允许用户输入初始条件、设定参数（如质量、转动惯量、电机特性等）和控制指令。
三维可视化：利用图形库（如Python的matplotlib、mayavi或Pygame，MATLAB的simulink 3D Animation）实现飞行器的实时三维可视化，帮助直观理解其飞行轨迹和动态特性。

5. 测试与验证

模型验证：通过与已有文献或实际飞行数据对比，验证模拟器的准确性。
灵敏度分析：对模型参数进行灵敏度分析，了解不同参数变化对飞行性能的影响。

6. 优化与迭代

根据测试结果不断调整和优化模型参数、控制算法，以及用户界面和视觉效果，直至满足要求。

开发这样一个模拟器不仅有助于研究四轴飞行器的基本运动学和动力学特性，还为复杂控制策略的开发和测试提供了一个安全且成本效益高的平台。

📚2 运行结果

主函数部分代码：


clear all;close all;clc;
addpath QuadFiles

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%% Parameter definition %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% user  
Sim.degrees=1;% Shows results in radians (0) or degrees (1)
Sim.Ts=0.005;% Sampling time [s]
Sim.Time=30; % Simulation time  [s]
Sim.t=0:Sim.Ts:Sim.Time; % Time vector  [s]
iniC=[-pi/8;+pi/8;0]; %inital conditions
ref=[0;0;-pi/2]; %[desiredPhi,desiredTheta,desiredPsi] Set Point
Sim.desiredAngFi=repmat(ref,1,size(Sim.t,2));% step signal
Sim.delay=10; %turn control system on after a certain time delay
% for rotating around z axis
%Sim.desiredAngFi(3,:)=((Sim.t-Sim.delay)/8).*(Sim.t>=Sim.delay);
%%  parameter of quadcopter, simulation space states and control system
Sim.ccc=1;   % Auxiliary variable for Ts of control system 
Sim.g = 9.81;% Gravity acceleration [m/s^2]
Sim.lb=0.295;% Arm length of quadcopter [m]
mp=0.084;    % Point mass that represents the engine and propeller [kg]
me=0.742;    % Sphere mass that represents the Quadcopter body [kg]
re=0.075;    % Sphere radius [m] 
Sim.Le = .014; % Arm Thickness [m]
Sim.rH = .127; % Propeller radius  [m]
Sim.kf=0.0757;% Atuador parameter
%%%%%%%%%%%%%%%%% Moment of Inertia (J)  %%%%%%%%%%%%%%%%%%%
Sim.Jx=0.4*(me*re^2)+2*mp*Sim.lb^2; % J Matrix position (1,1)
Sim.Jy=Sim.Jx;                      % J Matrix position (2,2)
Sim.Jz=0.4*(me*re^2)+4*mp*Sim.lb^2; % J Matrix position (3,3)
%%%%%%%%%%%%%%%% Inserting noise in IMU data %%%%%%%%%%%%%%%%
Sim.GyrosBias=[0.0524;-0.0524;0.020838];   % Gyros offset
Sim.GyrosMag=[0.66675;0.53642;0.4403]*1; % Gyros noise mag
Sim.AcelMag=[0.08;-0.06;0.075]*1;       % Acel noise mag
%%%%%%%%%% Partameters for Kalman Filter %%%%%%%%%%%%
Qangle=0.001;
Qbias =0.003;
Sim.AKalman=[1  -Sim.Ts;0        1]; 
Sim.QKalman=[Qangle       0;0       Qbias];     
Sim.biasKalman=zeros(2,1);
Sim.angleG=zeros(2,1);     
Sim.Pphi=zeros(2,2);
Sim.Ptheta=zeros(2,2);
Sim.Rmedido=0.03;
Sim.KphiKalman=zeros(2,1);
Sim.KthetaKalman=zeros(2,1);
Sim.aux=[0;0;0];
%%%%%%%%%%%%%%%%%%% state Space variables %%%%%%%%%%%%%%%%%%%%%%%%
% angular position in Fi
Sim.AngFi=iniC;% actual [phi;theta;psi] 
Sim.AngFiR=[0;0;0];  % [phi;theta;psi] read by IMU with noise
Sim.AngFiM=[0;0;0];  % [phi;theta;psi] filtered using Kalman filter