三维纯比例制导源代码

%具有碰撞角约束的三维纯比例导引律研究
%无人机空中加油自主会合控制器设计
%说明：在仿真中，下标"_m"表示追踪器参数，下标"_t"表示目标参数clear
clc%---------------------追踪器参数设置-----------------------
V_m=300; % 单位：m/s，飞行速度
theta_m=45*pi/180;%单位：rad，初始弹道倾角
fea_m=45*pi/180;%单位：rad，初始弹道偏角
X_m=0; %单位：m，位置坐标，X轴
Y_m=0; %单位：m，位置坐标，Y轴
Z_m=0; %单位：m，位置坐标，Z轴%---------------------目标参数设置-----------------------
V_t=50; %单位：m/s，目标飞行速度
theta_t=0*pi/180;%单位：rad，初始弹道偏角
fea_t=0*pi/180;%单位：rad，初始弹道偏角
X_t=1000; %单位：m，目标位置坐标，X轴
Y_t=1000; %单位：m，目标位置坐标，Y轴
Z_t=0; %单位：m，目标位置坐标，Z轴%------------------比例导引律参数设置-----------------------
N1=20; %纵向通道导引系数
N2=20;%偏航通道导引系数
g=9.8;t=0; %单位：s，仿真时刻
dt=0.001;%单位：s，仿真时间步长
n=1;%计数用R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);  % 计算弹目相对距离
dR=-1;
while(dR<0)    %当弹目相对距离增大时，仿真停止dX_m=V_m*cos(theta_m)*cos(fea_m);dY_m=V_m*sin(theta_m);dZ_m=-V_m*cos(theta_m)*sin(fea_m);dX_t=V_t*cos(theta_t)*cos(fea_t);dY_t=V_t*sin(theta_t);dZ_t=-V_t*cos(theta_t)*sin(fea_t);dR=((X_m-X_t)*(dX_m-dX_t)+(Y_m-Y_t)*(dY_m-dY_t)+(Z_m-Z_t)*(dZ_m-dZ_t))/sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);%-------------------------纵向通道------------------------------r1=sqrt((X_t-X_m)^2+(Y_t-Y_m)^2);  %r为x-y平面内导弹和目标间的距离delta_x=X_t-X_m;delta_y=Y_t-Y_m;Ddelta_x=V_t*cos(theta_t)*cos(fea_t)-V_m*cos(theta_m)*cos(fea_m);Ddelta_y=V_t*cos(theta_t)-V_m*cos(theta_m);Dxl1=(Ddelta_y*delta_x-Ddelta_x*delta_y)/(delta_x^2+delta_y^2);theta_l=atan(delta_y/delta_x);  %计算期望会合点的视线角Vx_los1=V_m*cos(fea_m)*cos(theta_m-theta_l)-V_t*cos(fea_t)*cos(theta_t-theta_l); Dxd1=(.1*Vx_los1*(theta_t-theta_l))/(N1*r1); Dtheta_c=N1*(Dxl1-Dxd1)*dt;  %无人机空中加油自主会合控制器设计-式（1）theta_m=theta_m+Dtheta_c;  %计算指令弹道倾角%-------------------------偏航通道------------------------------r2=sqrt((X_t-X_m)^2+(Z_t-Z_m)^2);  %r为x-z平面内两者间的距离delta_x=X_t-X_m;delta_z=Z_t-Z_m;Ddelta_x=V_t*cos(theta_t)*cos(fea_t)-V_m*cos(theta_m)*cos(fea_m);Ddelta_z=V_t*cos(theta_t)*sin(fea_t)-V_m*cos(theta_m)*sin(fea_m);Dxl=(Ddelta_z*delta_x-Ddelta_x*delta_z)/(delta_x^2+delta_z^2);chi_l=-atan(delta_z/delta_x);  %计算期望会合点的视线角Vx_los=V_m*cos(theta_m)*cos(fea_m-chi_l)-V_t*cos(theta_t)*cos(fea_t-chi_l); %式（3）Dxd=(80*Vx_los*(fea_t-chi_l))/(N2*r2); %式（2）Dfea_c=N2*(Dxl-Dxd)*dt;  %式（1）fea_m=fea_m+Dfea_c;  %计算指令航向角%----------------------------计算拦截弹坐标----------------------------X_m=X_m+V_m*cos(theta_m)*cos(fea_m)*dt;Y_m=Y_m+V_m*sin(theta_m)*dt;Z_m=Z_m-V_m*cos(theta_m)*sin(fea_m)*dt;%----------------------------计算目标弹坐标----------------------------X_t=X_t+V_t*cos(theta_t)*cos(fea_t)*dt;Y_t=Y_t+V_t*sin(theta_t)*dt;Z_t=Z_t-V_t*cos(theta_t)*sin(fea_t)*dt;R=sqrt((X_m-X_t)^2+(Y_m-Y_t)^2+(Z_m-Z_t)^2);%相对距离%-------------------------保存结果------------------------------theta_m_store(n)=theta_m;  %保存弹道倾角fea_m_store(n)=fea_m;   %保存弹道偏角   P_m_store(:,n)=[X_m;Y_m;Z_m]; %保存追踪器坐标P_t_store(:,n)=[X_t;Y_t;Z_t]; %保存目标坐标R_store(n)=R;%保存相对距离n=n+1;  %每循环一次，计数加一t=t+dt; %往前推进一个仿真步长
end
disp('脱靶量为（m）：')
Rfigure(1)
plot((1:n-1)*dt,theta_m_store*180/pi)
xlabel('time/s')
ylabel('弹道倾角/deg')
title('末制导：弹道倾角变化情况')figure(2)
plot((1:n-1)*dt,fea_m_store(1,:)*180/pi)
xlabel('time/s')
ylabel('弹道偏角/deg')
title('末制导：弹道偏角变化情况')figure(3)%注意matlab三维坐标画图X、Y、Z轴方向与导弹飞行力学坐标系不同
plot3(P_m_store(1,:),P_m_store(3,:),P_m_store(2,:),P_t_store(1,:),P_t_store(3,:),P_t_store(2,:))
legend('追踪器运动轨迹','目标运动轨迹')
xlabel('X/m')
ylabel('-Z/m')
zlabel('Y/m')figure(4)
plot((1:n-1)*dt,R_store)
xlabel('time/s')
ylabel('相对距离/m')
title('相对距离变化情况')

三维纯比例制导源代码相关推荐

三维视频融合开放源代码混合模式内嵌web容器 c++ js 混合开发时空克隆引擎数字孪生点卯系列魔镜系列
元宇宙的兴起,让以三维视频融合与三维投影融合为核心.以倾斜摄影和三维地图构筑的CIM作为托底的时空克隆引擎,成为时代的主流技术,以此打造了混合架构的魔镜平台济南用魔镜打造元宇宙城市,难道是真的? 开 ...
比例导引律Matlab程序,是否有关于比例导引方面的仿真程序？
本人想学习这方面的知识,上网看了一个类似代码 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% % ...
PID控制器（比例-积分-微分控制器）
PID控制器(比例-积分-微分控制器)- I 形象解释PID算法小明接到这样一个任务: 有一个水缸点漏水(而且漏水的速度还不一定固定不变),要求水面高度维持在某个位置,一旦发现水面高度低于要求位置, ...
PID控制器（比例-积分-微分控制器）- I
原文链接:https://www.cnblogs.com/shangdawei/p/4825259.html 形象解释PID算法小明接到这样一个任务:有一个水缸点漏水(而且漏水的速度还不一定固定不变 ...
PID（比例积分微分）介绍
写在前面: 本文章旨在总结备份.方便以后查询,由于是个人总结,如有不对,欢迎指正:另外,内容大部分来自网络.书籍.和各类手册,如若侵权请告知,马上删帖致歉. 来源:http://blog.sina.c ...
数据挖掘之关联分析（实验展示以及源代码）
数据挖掘之关联分析算法实现:Apriori算法和FP-growth 算法源代码简单描述下,关联分析概念关联分析概念主要参考下面的博文原文:https://blog.csdn.net/qq_4 ...
现在常用的java框架有哪些
java开发都是站在jar包这个巨人的肩膀上开发,速度更快. 各个web框架综合排名: 框架从何性能测试报告: In the following tests, we have measured the ...
动态规划常见类型总结
本文针对动态规划的常见类型进行总结.虽说总结的是动态规划,但顺便把递推也放了进来.严格来说,递推不属于动态规划问题,因为动态规划不仅有递推过程,还要有决策(即取最优),但广义的动态规划是可以包含递推的 ...
Planar Evasive Aircrafts Maneuvers Using Reinforcement Learning
原文下载:https://download.csdn.net/download/qq_29675093/10950893 虽然是2012年的论文,但是无论从理论深度还是实用价值都不足.动作限定很死,左 ...

三维纯比例制导源代码

三维纯比例制导源代码相关推荐

最新文章

热门文章