基于多级适应方法的无人机(UAV)在发动机输出情况下的导航和路径规划(Matlab代码实现)
👨🎓个人主页:研学社的博客
💥💥💞💞欢迎来到本博客❤️❤️💥💥
🏆博主优势:🌞🌞🌞博客内容尽量做到思维缜密,逻辑清晰,为了方便读者。
⛳️座右铭:行百里者,半于九十。
📋📋📋本文目录如下:🎁🎁🎁
目录
💥1 概述
文献来源:
@article{gu2022multi,
title={Multi-level Adaptation for Automatic Landing with Engine Failure under Turbulent Weather},
author={Gu, Haotian and Jafarnejadsani, Hamidreza},
journal={arXiv preprint arXiv:2209.04132},
year={2022}
}
运行MASC示例
1.打开定义引擎输出纬度、经度和高度的程序。然后首先把飞机放到发动机坏掉的地方。然后单击右上角的停止按钮×平面窗口的角落。
2在故障位置配置块中设置发动机输出全局位置
3.在simulink框架中设置机场坐标
4.单击运行按钮,首先启动模拟模型。
📚2 运行结果
部分代码:
tic
e = zeros(1,8000);
c = zeros(1,8000);
aileron_e = zeros(1,8000);
psi_ref = zeros(1,8000);
gamma_ref = zeros(1,8000);
%% This function is for testing for converge to planned straight line
xb = 31018;
yb = -23100;
xf = 34018;
yf = -27100;
Rl = 1016; % R value can not be small otherwise, the path following result is not good
psif = 0;
xl = xf + 4 * Rl * cos(psif - pi);
yl = yf + 4 * Rl * sin(psif - pi);
xu = xl + Rl * cos(psif - pi);
yu = yl + Rl * sin(psif - pi);
%Ru = sqrt((xl + Rl * cos(psif - pi) - xi)^2 + (yl + Rl * sin(psif - pi) - yi)^2);
%thetau = atan2( yi - yl - Rl * sin(psif - pi), xi - xl - Rl * cos(psif - pi));
%%
r = Rl; %radius of loiter curve
O = [xl yl]; %center of loiter or circular orbit
g = 9.81;%gravitational acceleration
%p = [curr_x curr_y];
p = [96900/3.2808 -84870/3.2808]; %UAV start position
psi = 4; %start heading
delta = 0.3; %look ahead position
%^^^^^^^^^^^^^^^^definition of controller parameters^^^^^^^^^^^^^^^^^^^^^^^
k_p=0.8; %proportional gain
k_i=0.01; %integral gain
k_d=1; %derivative gain
%^^^^^^^^^^^^^^^^^^Specification of time step^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
dt=0.1 % this is a time unit which shoud match simulator x-plane
U_0=400; %initial UAV speed
U_d=435.9;%desired UAV speed
theta = atan2((p(2)-O(2)),(p(1)-O(1)));%Calculation of LOS angle
%^^^^^^^^^^^^^^^^^^^^^^^Definition of the look ahead point^^^^^^^^^^^^^^^^^
x_i = ((r*(cos(theta+delta)))+O(1));
y_i = ((r*(sin(theta+delta)))+O(2));
psi_d = atan2((y_i-p(2)),(x_i-p(1))); %commanded heading angle
u = (psi_d-psi); %controller input for changing heading angle
%^^^^^^^^^^^^^^^^^^^^^^^^^^^Motion of UAV^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
x_d=U_0*(cos(psi_d))*dt;
y_d=U_0*(sin(psi_d))*dt;
%^^^^^^^^^^^^^^^estimation of heading angle and position^^^^^^^^^^^^^^^^^^^
P_new = [(p(1)+x_d),(p(2)+y_d)];
psi_new = (psi+u);
%^^^^^^^^^^^^^^^^^^^^^over time positioning and heading of UAV^^^^^^^^^^^^^
X=[p(1)];
Y=[p(2)];
S=511; %area of UAV wing
rho=0.3045; %density of air
b=59.64; %span of wing
mass = 333400 %mass of UAV
I_xx=0.247e8; %inertial moment
L_p=-1.076e7; %rolling moment
Cl_da=0.668e-2; %roll moment due to aileron deflection coefficient
Q_dS=1/2*rho*U_0^2*S; %dynamic pressure
L_da=Q_dS*b*Cl_da; %roll moment due to aileron
%^^^^^^^^^^^^^^^^^^^^^^^^^initialising controller^^^^^^^^^^^^^^^^^^^^^^^^^^
roll_ref=0; %initial UAV roll position
rollrate_ref=0; %initial UAV rollrate
t_ei=0; %thrust PI integrator
ei=0; %aileron PID integrator
%^^^^^^^^^^^^^^^^^^^estimation of stability derivatives^^^^^^^^^^^^^^^^^^^^
a=L_p/I_xx;
beta=L_da/I_xx;
roll_d=atan(u*U_0/g); %desired roll calculation
if abs(roll_d) > 1.5;
if roll_d < 0;
roll_d = -1.5;
else if roll_d>0;
roll_d = 1.5;
end
end
end
rollrate_d=roll_d*dt; %desired rollrate
aileron = k_p*(roll_d-roll_ref)+(k_i*ei)+k_d*(rollrate_d-rollrate_ref);
%deflection of aileron
rollrate_new = (((a*rollrate_ref)+(beta*aileron))*dt); %new roll rate output
roll_new = (rollrate_new/dt)+roll_ref; %new roll output
roll_old=roll_ref; %initialising old roll for feedback
rollrate_old=rollrate_ref; %initiallising old rollrate for feedback
%^^^^^^^^^^^^^^^^^^^^^^^^^control of thurst^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
t_ei=t_ei+(U_d-U_0)*dt;
thrust=k_p*(U_d-U_0)+(k_i*t_ei);
V_new=U_0+(thrust*dt);
V_old=V_new;
count = 0
while count<1000
Ru = abs ((((P_new(1) - O(1))^2) + ((P_new(2) - O(2))^2)) ^(1/2)-r);
%Calculation of UAV distance from center
theta_new = atan2((P_new(2)-O(2)),(P_new(1)-O(1))); %new path angle calculation
x_i = ((r*(cos(theta_new+delta)))+O(1));
y_i = ((r*(sin(theta_new+delta)))+O(2));
psi_d = atan2((y_i-P_new(2)),(x_i-P_new(1)));%calculation of desired heading angle
u = wrapToPi(psi_d-psi_new); %controller input for changing heading angle
ei=ei+((roll_d-roll_old)*dt); %updating the integrator
roll_d=atan(u*V_old/g); %desired roll calculation
if abs(roll_d) > 1.5; %limit of roll
if roll_d < 0;
roll_d = -1.5;
else if roll_d>0;
roll_d = 1.5;
end
end
end
rollrate_d=(roll_d-roll_old)*dt; %calculation of desired rollrate
aileron = (k_p*(roll_d-roll_old)+(k_i*ei)+(k_d*(rollrate_d-rollrate_old))); %calculation of deflection of aileron
rollrate_new = (((a*rollrate_old)+(beta*aileron))*dt); %new rollrate calculation
roll_new = (rollrate_new/dt)+roll_old; %new roll angle calculation
rollrate_old=rollrate_new; %rollrate as feedback
roll_old=roll_new; %roll angle as feedback
psi_old = psi_new; %UAV heading as feedback
psi_b=g/V_old*(tan(roll_new));
%due to new roll change in heading
psi_new = wrapToPi(psi_new+psi_b); %calculation of new heading angle
gamma_new = -15*pi/180;
Q_dS=1/2*rho*V_old^2*S; %calculation of dynamic pressure
L_da=Q_dS*b*Cl_da;
%due to aileron calculation of roll moment
beta=L_da/I_xx;
a=L_p/I_xx;
%^^^^^^^^^^^^^^^^^^^^^Calculation of UAV movements^^^^^^^^^^^^^^^^^^^^^^^^^
x_d=V_old*(cos(psi_new))*dt;
y_d=V_old*(sin(psi_new))*dt;
P_new = [(P_new(1)+x_d) (P_new(2)+y_d)];
%^^^^^^^^^^^^^^^^^^^^^^^^^contorl of thrust^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
t_ei=t_ei+(U_d-V_old)*dt;
thrust=k_p*(U_d-V_old)+(k_i*t_ei);
V_new=V_old+(thrust*dt);
V_old=V_new;
figure(1)
Y=[ Y P_new(2)];
X=[ X P_new(1)];
plot(X,Y)
hold on
Q = 0 : 0.01 : 2*pi;
W_c = (r * (cos(Q)))+O(1);
A_c = (r * (sin(Q)))+O(2);
plot(W_c,A_c,':')
xlim([xl-2*Rl xl+2*Rl])
ylim([yl-2*Rl yl+2*Rl])
xlabel('x-direction in ft')
ylabel('y-direction in ft')
title('Followed path using carrot chasing algorithm')
drawnow
count = count+1
hold on
for j = count;
%array of measurements
d = (abs(((O(1)-P_new(1))^2)+((O(2)-P_new(2))^2))^(1/2)) ;
e(1,j) = u;
c(1,j) = Ru;
aileron_e(1,j) = aileron;
if psi_d >=0
psi_ref(1,j) = psi_d;
elseif psi_new < 0
psi_ref(1,j) = psi_d+2*pi;
end
%psi_ref(1,j) = psi_new;
DesiredHeading = psi_ref(1,j);
disp(DesiredHeading)
gamma_ref(1,j) = gamma_new;
DesiredFlightPath = gamma_ref(1,j);
disp(DesiredFlightPath)
end
hold off
end
toc
%^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^measurment plots^^^^^^^^^^^^^^^^^^^^^^^^^^^^
figure(2)
f = [1:1:count];
plot(f,e)
xlabel('time in (sec/100)')
ylabel('Change in heading in radian')
title('Variation in controller effort with time')
figure(3)
plot(f,c)
xlabel('time in (sec/100)')
ylabel('cross track deviation(ft)')
title('Variation of cross track deviation with time')
figure(4)
plot(f,aileron_e)
xlabel('time in (sec/100)')
ylabel('Deflection of aileron in radian')
title('Variation in aileron control with time')
figure(5)
plot(f,psi_ref)
xlabel('time in (sec/100)')
ylabel('heading new in radian')
title('Variation in controller effort with time')
figure(6)
plot(f,gamma_ref)
xlabel('time in (sec/100)')
ylabel('pitch angle in radian')
title('Variation in controller effort with time')
time=count*dt
🎉3 参考文献
部分理论来源于网络,如有侵权请联系删除。
@article{gu2022multi,
title={Multi-level Adaptation for Automatic Landing with Engine Failure under Turbulent Weather},
author={Gu, Haotian and Jafarnejadsani, Hamidreza},
journal={arXiv preprint arXiv:2209.04132},
year={2022}
}
🌈4 Matlab代码实现
相关文章
- 【STM32F429的DSP教程】第10章 Matlab的WIFI通信实现
- 计及光伏电站快速无功响应特性的分布式电源优化配置方法(Matlab代码实现)
- 实时电价机制下交直流混合微网优化运行方法(Matlab代码实现)
- 【微电网_储能】基于启发式状态机策略和线性程序策略优化方法的微电网中的储能研究【给定系统约束和定价的情况下】(Matlab代码实现)
- 基于模型预测人工势场的船舶运动规划方法,考虑复杂遭遇场景下的COLREG(Matlab代码实现)
- 【BA-FNN】基于蝙蝠优化的模糊神经网络FNN研究(Matlab代码实现)
- 利用深度学习生成数据的时间序列预测(Matlab代码实现)
- 一种数据驱动的自动驾驶汽车前馈补偿器优化方法(Matlab代码实现)
- 二进制数据的贝叶斯非参数聚类算法(Matlab代码实现)
- 【状态估计】基于随机方法优化PMU优化配置(Matlab代码实现)
- 考虑充电负荷空间可调度特性的分布式电源与电动汽车充电站联合配置方法(Matlab代码实现)
- 配电网电压调节及通信联系研究(Matlab代码实现)
- 处理非线性分类的 SVM一种新方法(Matlab代码实现)
- 基于模糊认知图谱和遗传算法的龋齿度检测(Matlab代码实现)
- 基于MIMO-NOMA的可见光通信系统性能分析(Matlab代码实现)
- 【图像处理】基于形状提取和模式匹配组合的面部特征点提取方法(Matlab代码实现)
- m无线通信的信道建模matlab仿真,仿真分析了6种不同的无线通信信道模型
- 基于蛙跳算法的最优值计算matlab仿真
- m基于PSO粒子群优化的第四方物流的作业整合算法matlab仿真,对比有代理人和无代理人两种模式下最低运输费用、代理人转换费用、运输方式转化费用和时间惩罚费用
- 【MATLAB】进阶绘图 ( fill 填充二维多边形 | fill 函数 | 绘制文字 | text 函数 )
- MATLAB-蒙特卡罗方法