✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。
更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）
⛄一、四旋翼飞行器简介
0 引 言
四旋翼飞行器由于具有可垂直起降、机动性强、操作方便等诸多优点,在军事和民用场合得到广泛应用,从而成为众多学者的研究热点。四旋翼飞行器是具有四输入、六输出的欠驱动、非线性、强耦合系统。其姿态控制精度和抗干扰问题一直是研究重点。目前国内较普遍的飞行器控制算法主要包括:反步法、自适应控制、H控制、滑模控制、自抗扰控制等,对实现四旋翼飞行器的姿态控制具有重要的理论和实践意义。文献提出应用反步算法为飞行器上下、前后、左右、偏航4个子系统配置控制律,实现了四旋翼飞行器对设定轨迹的精确跟踪。该算法在构造Lyapunov函数的过程中,是以其导数小于零为前提,因此应用受到局限。文献针对传统的离散线性滑模应用于四旋翼飞行器控制具有跟踪误差大、响应速度慢、不能有限时间收敛等问题,提出了干扰观测器补偿的自适应离散终端滑模控制,使响应时间更快、跟踪效果更理想、鲁棒性更强。文献利用线性扩张状态观测器对四旋翼飞行器内部不确定干扰和外部干扰进行实时估计,进而采取线性状态反馈控制对扰动的估计值进行在线补偿,以实现四旋翼飞行器的姿态控制。
1 四旋翼飞行器动力学模型的建立
1. 1 四旋翼飞行器受力分析
对于飞行器的每个旋翼,剖面呈非对称,一旦旋翼旋转,由于上表面空气流速比下表面快,故上表面受到的空气压力小于下表面,上下表面受到的压差形成升力,如图1所示。旋翼1、3逆时针旋转,旋翼2、4顺时针旋转。由叶素动量理论可知,每个旋翼产生的升力Fi与电机转速ωi的平方成正比,即Fi=kFω2i(i=1,2,3,4),其中kF为升力系数。
图1 四旋翼飞行器受力分析图
当旋翼旋转时,空气阻力会阻碍其旋转。这种阻力形成施加在机体上的反扭转力矩,当4个旋翼转速相等时,旋翼产生的反扭矩作用相互抵消。四旋翼飞行器通过改变2对正反螺旋桨的转速实现对其运动控制。在4个旋翼转速相等的情况下,同时增加或者减少4个旋翼转速,可以实现飞行器上升或者下降。如果4个旋翼产生的升力之和等于机体重力,可以实现飞行器空中悬停。保持旋翼2、4的转速不变,改变旋翼1或者旋翼3的转速,飞行器在力矩l(F3-F1)或l(F1-F3)的作用下(其中l为电机轴线到飞行器中心距离),可实现俯仰运动。保持旋翼1和旋翼3的转速不变,改变旋翼2或者旋翼4的转速,飞行器在力矩l(F4-F2)或l(F2-F4)的作用下,可实现横滚运动。如果同时改变旋翼1和旋翼3的转速,或者同时改变旋翼2和旋翼4的转速,并保持飞行器总升力与机体重力相等,飞行器会在反扭矩的作用下实现偏航运动。由此可见,实现飞行器垂直运动的升力,以及实现俯仰、横滚、偏航运动的旋转力矩可以表示为
式中: F——飞行器垂直运动的升力;
Mx、My、Mz——四旋翼俯仰、横滚和偏航运动的旋转力矩;
kF——升力系数;
kM——旋转力矩系数。
1. 2 动力学模型建立
为了描述飞行器的姿态和运动状态,需要引入地理坐标系n(X,Y,Z)与载体坐标系b(x,y,z)。地理坐标系又称为东北天坐标系,载体坐标系与飞行器固连,原点为飞行器中心。将地理坐标系与载体坐标系原点重合,并将地理坐标系分别绕X、Y、Z轴旋转3次之后可得到载体坐标系,地理坐标系到载体坐标系的转换矩阵可表示为
由式(5)和式(2)可求得地理坐标系n中飞行器在X、Y、Z轴方向所受旋翼升力向量:
在低速飞行过程中,角速度矢量较小,式(8)中左边第二项可近似认为为零,则式(8)可化简为
将式(10)转换可得:
由式(7)和式(11)可得四旋翼飞行器在低速飞行情况下的非线性动力学模型:
由式(12)可知,四旋翼飞行器线运动不影响角运动,但是角运动会影响线运动。以u1、u2、u3、u4为系统输入,通过改变这4个输入变量的值,可以改变飞行器的3个线位移和3个角位移,从而实现对飞行器的运动控制。
2 四旋翼飞行器的控制系统构建与仿真
经典PID算法结构简单,基于偏差设计反馈律,不依赖受控对象的具体数学模型,在很多过程控制中均有良好表现。尽管各种新的控制算法不断涌现,但是并没有改变PID控制算法在工业控制中的主导地位。本文根据四旋翼在飞行过程中经常会遇到不确定外界干扰等情况,设计了基于小扰动的PID控制器,如图2所示。
图2 PID控制器结构图
⛄二、部分源代码
clear all
demo_0 = 1;
rad = pi/180;
deg = 180/pi;
pos = [30pi/180, 120pi/180, 200]';
vn = [10, 10, 10];
att0 = [10, 10, 10]‘*rad;
v0 = [10, 20, 30]’;
% ans = euler2dcm(att0)
b = fir1(20, 0.01, ‘low’);
b = b/sum(b);
% x = repmat([att0; vby]', length(b), 1);
% rv = [10, 2, 1]‘*pi/180;
% sqrt(rv’*rv)
wm = [1, 1, 1
2, 2, 2
3, 3, 3
4, 4, 4
5, 5, 5];
%
% wmm = wm(1:2, 😃
% wm1 = wm(2:3, 😃
% cross(wmm,wm1, 2)
% cs = [ [ 2, 0, 0, 0, 0]/3
% [ 9, 27, 0, 0, 0]/20
% [ 54, 92, 214, 0, 0]/105
% [ 250, 525, 650, 1375, 0]/504
% [2315, 4558, 7296, 7834, 15797]/4620 ];
%
% cs(3, 1:3)*wm(1:3, 😃
% wm(1:3, 😃
% eth = earth(pos, vn)
% skew(v0)
% u = [1, 2, 3]‘;
% u = u/norm(u);
% theta = 0.8pi;
%
% phi1 = theta;
% phi2 = -(2pi - theta) ;
%
% PHI1 = phi1u;
% PHI2 = phi2u;
% % norm(PHI)
% qq1 = rv2q(PHI1)’
% qq2 = rv2q(PHI2)’
%
% PHI1’*PHI1
% rk = [[0.1; 0.1; 0.1]; [10; 10; 10]]';
% Rk = diag(rk)^2;
% R =1 + 0.1.*randn(10000, 1);
% mean®
% var®
%
% clearvars -except R
% imu_err = imuerror();
name = ‘selfdefine’;
exist(‘name’, ‘var’) && ~strcmp(name, ‘’) && ~strcmp(name, ‘zero’)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 名称：Opitiaml Based Alignment (GPS velocity aided) version 1.0
% 功能：
%
% 缩写的含义：
% msr: measurement;
% ref: reference; 特指真实值
% sv: save;
% prv: previous; 对应变量在上次解算后的值或在本次解算开始时刻的值
% opt: optimal
% Author: Kun Gan, Tongji University
% Email : ciaotigre@126.com
% Date : 2020/12/1
%%
% 全局变量
close all
gvar_earth;
% ** 下载数据 **
% 数据包含
% 1. trajectory_ref.(pos, vn, att)
% 2. imu.ref.(acc, gyr)
% 轨迹仿真程序生成的数据，trajectory的长度会比imu长1。trajectory首个数据为
% t0时刻的avp_ref, imu首个数据为理想imu在t1时刻输出的(t0, t1]内角度和速
% 度增量。
load(‘trj_and_imu.mat’);
% ** 与测量数据相关的量 **
% imu输出频率 (Hz)
f_imu = 100;
ts_imu = 1/f_imu;
% gps输出频率 (Hz)
f_gps = f_imu;
ts_gps = 1/f_gps;
% 总数据长度 (个)
num_imu_data = size(imu_ref.acc, 1);
% *** 给imu数据添加误差 ***
imu_err = imuerrorset(‘selfdefine’);
% add imu error
[imu_msr.gyr, imu_msr.acc] = imuadderr(imu_ref.gyr, imu_ref.acc, …
imu_err.eb, imu_err.web, imu_err.db, imu_err.wdb, ts_imu);
% ** 设置全局变量 **
% 第一个"读入"的数据所对应的时刻以及在imu_ref和trajectory_ref中的位置 (s)
% 可以认为导航系统是在第start_time秒启动的
start_time = 0;
first_data_index = round(start_time/ts_imu) + 1;
% 对准时间长度 (s)
total_alignment_time = 92;
% 更新算法子样数 (个)
num_subsample = 2;
nts = num_subsample*ts_imu;
% 初始位置、速度、姿态
pos0 = trajectory_ref.pos(first_data_index, 😃‘;
vn0 = trajectory_ref.vn(first_data_index, 😃’;
att0 = trajectory_ref.att(first_data_index, 😃';
att0_ref = att0;
Cbn0_ref = a2mat(att0);
q0_ref = a2qua(att0);
% *** 一些变量在上一时刻的值 ***
% 速度、位置、姿态
% 目前还没有准备对初始时刻值添加误差
pos_prv = pos0;
vn_prv = vn0;
att_prv = att0;
% eth保存惯导解算需要的变量，例如wien, winn等。
eth_prv = earth(pos_prv, vn_prv);
% bt系和nt系相对惯性空间变化量
Cntn0_prv = eye(3);
Cbtb0_prv = eye(3);
qbtb0 = [1, 0, 0, 0]';
% 分配动态变量存储空间
phi_sv = zeros(round(total_alignment_time/ts_imu), 3);
phi0_sv = zeros(round(total_alignment_time/ts_imu), 3);
% 计数变量
% 当前时刻 s
current = 0;
% 当前循环时第i次循环
i = 0;
% index
k = 0;
%%
% 我们的终极目标是让算法能够在实际中运行，实际中情况大概是这样的：
% 1. t=0，惯导开机
% 2. t=ts_imu，imu在此时输出第一组速度增量vm(1)和角增量wm(1)
% 3. t=nts,imu输出凑足了执行一次惯导更新所需要的子样数，导航计算机立即执行
% 惯导更新程序并输出导航结果p(t),vn(t),att(t)
% 4. 导航计算机继续等待imu测得n组测量信息再进行惯导更新
while current < total_alignment_time
% 每过nts秒，imu就会测量出num_subsample组采样结果。获得足够imu输出后立
% 即在current时刻（亦称：当前时刻）进行导航更新
current = current + nts;
% current时刻，载体的位置、速度、姿态参考值
pos_ref = trajectory_ref.pos(k, :)';
vn_ref = trajectory_ref.vn(k, :)';
att_ref = trajectory_ref.att(k, :)';
qbn_ref = a2qua(att_ref);
% 用参考速度模拟当前时刻GPS速度,其中位置信息暂时不加误差
vn_gps = vn_ref + 0.*randn(3, 1);
pos_gps = pos_ref;
vn_gps_sv(i, :) = vn_gps;
% 用gps输出的速度作为组合导航系统给出的速度
vn = vn_gps;
pos = pos_gps;
% 从imu_ref中读出(current - nts, current]这段时间内imu输出的n组量测信息
wm = imu_msr.gyr(k-num_subsample+1 : k, :);
vm = imu_msr.acc(k-num_subsample+1 : k, :);
wm_sv(2*i-1:2*i, :) = wm;
vm_sv(2*i-1:2*i, :) = vm;
% 步长圆锥/划桨误差
[phim, dvbm] = cnscl(wm, vm);
% *** 计算alpha(n0)和beta(b0) ***
% alpha(n0)
% 1. 目前用vn0_ref计算alpha, 暂时不考虑滑动窗口。
% 2. 用gps输出的位置计算wien和gn
alpha_sigma = alpha_sigma + ...
Cntn0_prv*(cross(eth_prv.wien, vn_prv) - eth_prv.gn)*nts;
alpha = Cntn0*vn - vn0 + alpha_sigma;
% beta(b0)
beta = beta_prv + Cbtb0_prv*dvbm;
% QUEST 方法计算qbn0
[ qbn0, K ] = QUEST( beta, alpha, K_prv );
% 姿态解算
Cbn0 = q2mat(qbn0);
Cbn = Cntn0'*Cbn0*Cbtb0;
att = m2att(Cbn);
phi0_sv(i, :) = atterrnorml(q2att(qbn0) - att0_ref)*deg;
phi_sv(i, :) = atterrnorml(att - att_ref)*deg;
% 计算导航解算时所需要的相关参数
eth = earth(pos, vn);
% 将本次更新后的导航参数作为下次更新初值
pos_prv = pos;
vn_prv = vn;
att_prv = att;
eth_prv = eth;
Cntn0_prv = Cntn0;
Cbtb0_prv = Cbtb0;
alpha_prv = alpha;
beta_prv = beta;
K_prv = K;
end
% 注意：如果end前面加了空格就会出错!
phi_sv(i+1:end, 😃 = [];
phi0_sv(i+1:end, 😃 = [];
%% 绘图
time_axis = (1:1:i)nts;
time_axis_imu = (1:1:2i)*ts_imu;
% Cbn误差
msplot(311, time_axis, phi_sv(:, 1), ‘pitch error / \circ’);
msplot(312, time_axis, phi_sv(:, 2), ‘roll error / \circ’);
msplot(313, time_axis, phi_sv(:, 3), ‘yaw error / \circ’);
%Cbn0 误差
msplot(311, time_axis, phi0_sv(:, 1), ‘pitch error / \circ’);
msplot(312, time_axis, phi0_sv(:, 2), ‘roll error / \circ’);
msplot(313, time_axis, phi0_sv(:, 3), ‘yaw error / \circ’);
⛄三、运行结果
⛄四、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1]张萍.四旋翼飞行器姿态控制建模与仿真[J].电机与控制应用. 2019,46(12)
[2]刘岩,杨牧.四旋翼飞行器飞行控制系统研究与设计[J].山东工业技术. 2019,(07)
3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除
🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化
2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断
3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知
4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化
5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配
6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化
7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化
8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置
9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长
10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合
2732
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
