比例导引法Matlab仿真

%三维制导模型，比例导引法求解
%源代码作者不详，注释人：lylogn
%Modified by lylogn，2012年4月17日
clear all;
close all;
clc
dt=0.1;%仿真时间步长
alpha=pi/6;v_t=0.42;s_t=v_t*dt;%目标以0.42的速度沿alpha的角方向匀速飞行，s_t为目标在单位仿真步长前进的距离
v_m=0.60;s_m=v_m*dt;%s_m为导弹在单位仿真步长沿目前速度方向前进的距离
x(1)=0;y(1)=1.0;z(1)=0;
pmr(:,1)=[x(1);y(1);z(1)]; %导弹初始位置，在坐标原点
ptr(:,1)=[25;5;7]; %目标初始位置
K=3; %比例导引系数
q(1)=0; %初始的视线角，设定参考线为t和m初始位置的连线
o(1)=0; %初始导弹速度向量方向角
a(1)=0; %初始导弹相对目标的运动速度向量的方向角
for(k=2:600)
ptr(:,k)=[ptr(1,1)-v_t*cos(alpha)*dt*k;ptr(2,1);ptr(3,1)+v_t*sin(alpha)*k*dt]; %目标运行轨迹方程，匀速直线运动
r(k-1)=sqrt((ptr(1,k-1)-pmr(1,k-1))^2+(ptr(2,k-1)-pmr(2,k-1))^2+(ptr(3,k-1)-pmr(3,k-1))^2); %k-1时刻导弹与目标在三维空间中的欧氏距离
c=sqrt((ptr(1,k)-pmr(1,k-1))^2+(ptr(2,k)-pmr(2,k-1))^2+(ptr(3,k)-pmr(3,k-1))^2); %目标k时刻位置与导弹k-1时刻位置间的距离
b=acos((r(k-1)^2+s_t^2-c^2)/(2*r(k-1)*s_t));
%%%此处参见公式一%%%
dq=acos((r(k-1)^2-s_t^2+c^2)/(2*r(k-1)*c));%k-1时刻到k时刻的视线角变化量（假设导弹不动，目标移动）
%%%此处参见图一%%%
if abs(imag(b))>0 %如果acos的值出现虚数，则说明该角度一定很小，对其进行近似操作b=0.0000001;
end
if abs(imag(dq))>0 %同上
dq=0.0000001;
end
q(k)=q(k-1)+dq; %更新视线角
o(k)=o(k-1)+K*dq; %更新导弹速度向量方向角
a(k)=o(k)-q(k); %更新导弹相对目标的运动速度向量的方向角
c1=r(k-1)*sin(b)/sin(a(k)+b); %计算k-1时刻角b所对边的长度
%%%此处参见公式二%%%
c2=r(k-1)*sin(a(k))/sin(a(k)+b); %计算k-1时刻角a(k)所对边的长度
c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); %计算k时刻导弹m与目标t之间的距离（在导弹不动，目标移动的假设条件下），为假值
dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3)); %k-1时刻到k时刻的视线角变化量（假设导弹移动，目标也移动），以下代码重复以上过程，为假值
%%%此处参见图二%%%
if abs(imag(dq))>0
dq=0.0000001;
end
q(k)=q(k-1)+dq;
o(k)=o(k-1)+K*dq;
a(k)=o(k)-q(k);
c1=r(k-1)*sin(b)/sin(a(k)+b);
c2=r(k-1)*sin(a(k))/sin(a(k)+b);
c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); %计算k时刻导弹m与目标t之间的距离（在导弹移动，目标也移动的假设条件下），逼近真值，以下计算使之更加精确
dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3)); %k-1时刻到k时刻的视线角变化量（假设导弹移动，目标也移动），以下代码重复以上过程，为真值
if abs(imag(dq))>0
dq=0.0000001;
end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%
�ded by lylogn 2012.04.17，To make 'dq' get closer to its true value
q(k)=q(k-1)+dq;
o(k)=o(k-1)+K*dq;
a(k)=o(k)-q(k);
c1=r(k-1)*sin(b)/sin(a(k)+b);
c2=r(k-1)*sin(a(k))/sin(a(k)+b);
c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); %计算k时刻导弹m与目标t之间的距离（在导弹移动，目标也移动的假设条件下），逼近真值，以下计算使之更加精确
dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3)); %k-1时刻到k时刻的视线角变化量（假设导弹移动，目标也移动），以下代码重复以上过程，为真值
if abs(imag(dq))>0
dq=0.0000001;
end
q(k)=q(k-1)+dq;
o(k)=o(k-1)+K*dq;
a(k)=o(k)-q(k);
c1=r(k-1)*sin(b)/sin(a(k)+b);
c2=r(k-1)*sin(a(k))/sin(a(k)+b);
c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); %计算k时刻导弹m与目标t之间的距离（在导弹移动，目标也移动的假设条件下），逼近真值，以下计算使之更加精确
dq=a(k)-acos(((c1-s_m)^2+c3^2-(c2-s_t)^2)/(2*(c1-s_m)*c3)); %k-1时刻到k时刻的视线角变化量（假设导弹移动，目标也移动），以下代码重复以上过程，为真值
if abs(imag(dq))>0
dq=0.0000001;
end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%
q(k)=q(k-1)+dq;
o(k)=o(k-1)+K*dq;
a(k)=o(k)-q(k);
c1=r(k-1)*sin(b)/sin(a(k)+b);
c2=r(k-1)*sin(a(k))/sin(a(k)+b);
c3=sqrt((c1-s_m)^2+(c2-s_t)^2+2*(c1-s_m)*(c2-s_t)*cos(a(k)+b)); %计算k时刻导弹m与目标t之间的距离，终于近似为真值了
x1(k)=ptr(1,k-1)+c2/s_t*(ptr(1,k)-ptr(1,k-1));
y1(k)=ptr(2,k-1)+c2/s_t*(ptr(2,k)-ptr(2,k-1));
z1(k)=ptr(3,k-1)+c2/s_t*(ptr(3,k)-ptr(3,k-1)); %计算出角b所对边与目标运动轨迹的交点：（x1，y1,z1）
%%%参见公式三%%%
x(k)=pmr(1,k-1)+s_m/c1*(x1(k)-pmr(1,k-1));
y(k)=pmr(2,k-1)+s_m/c1*(y1(k)-pmr(2,k-1));
z(k)=pmr(3,k-1)+s_m/c1*(z1(k)-pmr(3,k-1)); %计算出导弹k时刻所运动到的位置：（x,y,z）%%%参见公式三%%%
pmr(:,k)=[x(k);y(k);z(k)];
r(k)=sqrt((ptr(1,k)-pmr(1,k))^2+(ptr(2,k)-pmr(2,k))^2+(ptr(3,k)-pmr(3,k))^2);
if r(k)<0.06;
break;
end;
end
sprintf('遭遇时间：%3.1f',0.1*k);
figure(1);
plot3(pmr(1,1:k),pmr(2,1:k),pmr(3,1:k),'k',ptr(1,:),ptr(2,:),ptr(3,:));
axis([0 25 0 5 0 25]);
text(x(180),y(180),z(180),'\rightarrow 比例导引律制导下的导弹运动轨迹');
text(ptr(1,280),ptr(2,280),ptr(3,280),'\rightarrow 目标运动轨迹');
grid on
之后，鉴于程序中很多地方不结合模型图也很难理解，将其中关键的图例与公式提取如下：
最后，程序的运行过程分析完成，具体的细节详见注释，运行结果如下图所示：
综上所述，本工作对比例导引法求解三维制导问题的仿真程序进行了详细的分析与注释，程序运行正常，希望对大家理解比例导引法有所帮助。

2012年4月17日
于实验室
Created by lylogn。