比例导引理想弹道仿真

参考文献：[1]高尚.比例导引理想弹道仿真[J].计算机工程与设计, 2003, 24(8):3.DOI:10.3969/j.issn.1000-7024.2003.08.023.

​ 本文主要写的是对《比例导引理想弹道仿真》的部分公式推导，以及对文章源码的一些改动。如有错误，恳请大家留言指点。

部分公式推导

​ 比例导引法在导弹导向目标的过程中，弹道速度向量的旋转角速度的变化率$d\theta =dt$与目标视线的旋转角速度$dq/dt$成正比。导引方程为$d\theta =mdq/dt$，$m$为比例系数。

​ 比例导引法

​ 导弹与目标在同一平面内运动，设$M_k$表示第$k$个时间间隔时，导弹的空间位置；$M_{k+1}$表示第$k+1$个时间间隔时，导弹的空间位置；$T_k$表示第$k$个时间间隔时，目标的空间位置；$r(k)$表示第$k$个时间间隔时，导弹与目标的距离；$S_m$表示导弹在一个时间间隔内运动的距离($S_m=v_m\ast \Delta t$)。$x_m$、$y_m$、$z_m$分别表示导弹的立体直角纵坐标，$x_t$、$y_t$、$z_t$分别表示目标的立体直角纵坐标，$M_{k-1}{M}_k$表示导弹在一个时间间隔内运动的距离$S_m$，$T_{k-1}{T}_k$表示目标在一个时间间隔内运动的距离，大小是$S_t$，作$M_{k-1}{T}_{k-1}$的平行线$M_{k}B$。设$M_{k-1}{T}_c=c$，$A{M}_{K-1}=c_2$，$\angle A{M}_{k-1}{T}_{k-1}={\alpha }_{k-1}$，由几何知识可知，

${\beta }_{k-1}=\angle A{M}_{k-1}{T}_{k-1}=arccos(\frac{{\left[r_i(k-1)\right]}^2+s_t^2+c^2}{2\ast r_i(k-1)\ast s_t})$

$\Delta q_{k-1}=\angle A{M}_k{T}_k\approx \angle T{M}_{k-1}{T}_{k-1}=arccos(\frac{{\left[r_i(k-1)\right]}^2-s_t^2+c^2}{2\ast r_i(k-1)\ast c})$

$r_i(k)=\sqrt{(x_i(k)-x_m(k){)}^2+(y(k)-y_m(k){)}^2+(z_i(k)-z_m(k){)}^2}$

$c=\sqrt{(x_i(k)-x_m(k-1{)}^2+(y_i(k)-y_m(k-1){)}^2+(z_r(k)-z_m(k-1){)}^2}$

​$c_1=\frac{r_i(k-1)sin({\beta }_{k-1})}{sin({\alpha }_{k-1}+{\beta }_{k-1})}$

$c_2=\frac{r_i(k-1)sin({\alpha }_{k-1})}{sin({\alpha }_{k-1}+{\beta }_{k-1})}$

​ 导弹的直角纵坐标的差分方程为：$$\{\begin{array}{l}{q}_k=q_{k-1}+\Delta q\\ {\theta }_k={\theta }_{k-1}+m{\Delta }_q\\ {\alpha }_k={\theta }_k-q_{k-1}\\ x_t(k)=x_t(k-1)+\frac{c_2}{s_t}(x_t(k)-x_t(k-1))\\ y_t(k)=y_t(k-1)+\frac{c_2}{s_t}(y_t(k)-y_t(k-1))\\ z_t(k)=z_t(k-1)+\frac{c_2}{s_t}(z_t(k)-z_t(k-1))\\ x_m(k)=x_m(k-1)+\frac{s_m}{c_1}(x_t(k)-x_m(k-1))\\ y_m(k)=y_m(k-1)+\frac{s_m}{c_1}(y_t(k)-y_m(k-1))\\ z_m(k)=z_m(k-1)+\frac{s_m}{c_1}(z_t(k)-z_m(k-1))\end{array}$$

​ 导弹与目标接近时，用$\angle A{M}_{k-1}{T}_{k-1}$近似$\Delta q_{k-1}$时误差比较大，因此要修正：

$c_3=\sqrt{(c_1-s_m{)}^2+(c_2-s_t{)}^2+x(c_1-s_m)(c_2-s_t)cos(\alpha +\beta )}$

$\Delta q=\alpha -arccos(\frac{(c_1-s_m{)}^2+c_3^2-(c_2-s_t{)}^2}{2(c_t-s_m)c_3})$

部分证明如下：

自$M_{k-1}$作$\Delta M_{k-1}{T}_{k-1}{T}_k$的高$M_{k-1}H$，设$T_{k-1}H$为$x$，那么有

​ $r(k-1{)}^2-x^2=c^2-(s_t^2-x{)}^2$

​ $x=\frac{s_t^2-c^2+r(k-1)}{2\ast s_t\ast r(k-1)}$

设$\angle A{T}_{k-1}{M}_{k-1}$为${\beta }_{k-1}$，则

​ ${\beta }_{k-1}=arccos(\frac{s_t^2-c^2+r(k-1)}{2\ast s_t\ast r(k-1)})$

​ $sin({\beta }_{k-1}+{\alpha }_{k-1})=\frac{h}{c_1}=\frac{sin({\beta }_{k-1})\ast r(k-1)}{c_1}$

可得，$c_1=\frac{r(k-1)sin({\beta }_{k-1})}{sin({\alpha }_{k-1}+{\beta }_{k-1})}$。同理从$T_k$作$\Delta M_{k-1}{T}_{k-1}{T}_k$的高$T_{k-1}N$，可得$c_2=\frac{r(k-1)sin({\alpha }_{k-1})}{sin({\alpha }_{k-1}+{\beta }_{k-1})}$

源码改动

​ 文章短小但精悍，作者很贴心的把源码都贴上去了。只要你看懂文章的前半部分，就能看懂代码了。我在基于对文章的理解上加上了一些简单的注释。在将代码敲到MATLAB，滚动框那边有黄色待优化的提示，我个人就按照提示将部分变量预分配了大小。其中，作者源码中的c1和c2的代码不知道为什么和文章中的公式表达的不一样。我干脆就按照作者的表达式将代码改了。不知道是我理解错了还是作者少敲了几个数字。下面贴出作者源代码和我修改过的代码以及两者之间的效果图。

源代码

% *************************************************************************
% 文件名：proportionGuideRaw.m
% 创建人：高尚
% 创建时间：2024-01-19
% 仿真来源：[1]高尚.比例导引理想弹道仿真[J].计算机工程与设计, 2003, 24(8):3.DOI:10.3969/j.issn.1000-7024.2003.08.023.
% 文件描述：文中自带的代码
% *************************************************************************
clear;clc;
tt=0.1;
sm=0.6*tt;
st=0.42*tt;
x(1)=0;y(1)=0;z(1)=0;
pmr(:,1)=[x(1);y(1);z(1)];
ptr(:,1)=[25;5;10];
m=3;
q(1)=0;
o(1)=0;
a(1)=0;
for(k=2:600)
    ptr(:,k)=[25-0.42*cos(pi/6)*tt*k;5;10+0.42*sin(pi/6)*k*tt];
    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);
    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);
    b=acos((r(k-1)^2+st^2-c^2)/(2*r(k-1)*st));
    dq=acos((r(k-1)^2-st^2+c^2)/(2*r(k-1)*c));
    if abs(imag(b))>0
        b=0.0000001;
    end
    if abs(imag(dq))>0
        dq=0.0000001;
    end
    q(k)=q(k-1)+dq;
    o(k)=o(k-1)+m*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-sm)^2+(c2-st)^2+2*(c1-sm)*(c2-st)*cos(a(k)+b));
    dq=a(k)-acos(((c1-sm)^2+c3^2-(c2-st)^2)/(2*(c1-sm)*c3));
    if abs(imag(dq))>0
        dq=0.0000001;
    end
    q(k)=q(k-1)+dq;
    o(k)=o(k-1)+m*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-sm)^2+(c2-st)^2+2*(c1-sm)*(c2-st)*cos(a(k)+b));
    dq=a(k)-acos(((c1-sm)^2+c3^2-(c2-st)^2)/(2*(c1-sm)*c3));
    if abs(imag(dq))>0
        dq=0.0000001;
    end
    q(k)=q(k-1)+dq;
    o(k)=o(k-1)+m*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-sm)^2+(c2-st)^2+2*(c1-sm)*(c2-st)*cos(a(k)+b));
    x1(k)=ptr(1,k-1)+c2/st*(ptr(1,k)-ptr(1,k-1));
    y1(k)=ptr(2,k-1)+c2/st*(ptr(2,k)-ptr(2,k-1));
    z1(k)=ptr(3,k-1)+c2/st*(ptr(3,k)-ptr(3,k-1));
    x(k)=pmr(1,k-1)+sm/c1*(x1(k)-pmr(1,k-1));
    y(k)=pmr(2,k-1)+sm/c1*(y1(k)-pmr(2,k-1));
    z(k)=pmr(3,k-1)+sm/c1*(z1(k)-pmr(3,k-1));
    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,plot3(pmr(1,1:k),pmr(2,1:k),pmr(3,1:k),'g',ptr(1,:),ptr(2,:),ptr(3,:),'r');
axis([0 25 0 5 0 25]);
text(x(80),y(80),z(80),'\leftarrow 比例导引');
grid on

修改后的代码

% *************************************************************************
% 文件名：proportionGuide.m
% 创建人：qhy
% 创建时间：2024-01-19
% 仿真来源：[1]高尚.比例导引理想弹道仿真[J].计算机工程与设计, 2003, 24(8):3.DOI:10.3969/j.issn.1000-7024.2003.08.023.
% 文件描述：对文中自带的代码加注释、改变每次迭代的c1&c2表达式以及对部分变量进行预分配空间
% *************************************************************************
clc;clear;

%% 比例制导仿真
deltaT = 0.1;   % 时间间隔
Sm = 0.6*deltaT;    % 导弹在时间间隔内移动距离
St = 0.42*deltaT;   % 载机在时间间隔内移动距离
Xm(1,:) = 0;Ym(1,:) = 0;Zm(1,:) = 0; % 导弹位置矩阵，row为x,y,z；column为k*t
pmr(:,1) = [Xm(1);Ym(1);Zm(1)];    % 导弹起始位置
X1(1,:) = 0;Y1(1,:) = 0;Z1(1,:) = 0;    % 载机的位置矩阵
X1(1) = 25;Y1(1) = 5; Z1(1) = 10;
ptr(:,1) = [X1(1);Y1(1);Z1(1)];   % 载机起始位置
m = 3;  % 比例导引系数
r(1,:) = 0; % 某个时间间隔，导弹与载机之间的距离
q(1,:) = 0;theta(1,:) = 0;a(1,:) = 0;    % 角度矩阵


for k = 2:600
    ptr(:,k) = [25-0.42*cos(pi/6)*deltaT*k;5;10 + 0.42*sin(pi/6)*k*deltaT]; % 载机的运动方程
    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); %载机和导弹相对距离
    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);
    beta = acos((r(k-1)^2 + St^2-c^2)/(2*r(k-1)*St));
    deltaQ = acos((r(k-1)^2-St^2 + c^2)/(2*r(k-1)*c));
    if abs(imag(beta))>0
        beta = 0.0000001;
    end
    if abs(imag(deltaQ))>0
        deltaQ = 0.0000001;
    end
    % 导弹的差分方程
    q(k) = q(k-1) + deltaQ;
    theta(k) = theta(k-1) + m*deltaQ;
    a(k) = theta(k)-q(k);
    c1 = r(k-1)*sin(beta)/sin(a(k-1) + beta);
    c2 = r(k-1)*sin(a(k-1))/sin(a(k-1) + beta);
    c3 = sqrt((c1-Sm)^2 + (c2-St)^2 + 2*(c1-Sm)*(c2-St)*cos(a(k) + beta));
    deltaQ = a(k)-acos(((c1-Sm)^2 + c3^2-(c2-St)^2)/(2*(c1-Sm)*c3));
    if abs(imag(deltaQ))>0
        deltaQ = 0.0000001;
    end
    q(k) = q(k-1) + deltaQ;
    theta(k) = theta(k-1) + m*deltaQ;
    a(k) = theta(k)-q(k);
    c1 = r(k-1)*sin(beta)/sin(a(k-1) + beta);
    c2 = r(k-1)*sin(a(k-1))/sin(a(k-1) + beta);
    c3 = sqrt((c1-Sm)^2 + (c2-St)^2 + 2*(c1-Sm)*(c2-St)*cos(a(k) + beta));
    deltaQ = a(k)-acos(((c1-Sm)^2 + c3^2-(c2-St)^2)/(2*(c1-Sm)*c3));
    if abs(imag(deltaQ))>0
        deltaQ = 0.0000001;
    end
    q(k) = q(k-1) + deltaQ;
    theta(k) = theta(k-1) + m*deltaQ;
    a(k) = theta(k)-q(k);
    c1 = r(k-1)*sin(beta)/sin(a(k-1) + beta);
    c2 = r(k-1)*sin(a(k-1))/sin(a(k-1) + beta);
    c3 = sqrt((c1-Sm)^2 + (c2-St)^2 + 2*(c1-Sm)*(c2-St)*cos(a(k) + beta));
    
    X1(k) = ptr(1,k-1) + c2/St*(ptr(1,k)-ptr(1,k-1));
    Y1(k) = ptr(2,k-1) + c2/St*(ptr(2,k)-ptr(2,k-1));
    Z1(k) = ptr(3,k-1) + c2/St*(ptr(3,k)-ptr(3,k-1));
    Xm(k) = pmr(1,k-1) + Sm/c1*(X1(k)-pmr(1,k-1));
    Ym(k) = pmr(2,k-1) + Sm/c1*(Y1(k)-pmr(2,k-1));
    Zm(k) = pmr(3,k-1) + Sm/c1*(Z1(k)-pmr(3,k-1));
    pmr(:,k) = [Xm(k);Ym(k);Zm(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.1fs',0.1*k)
figure,plot3(pmr(1,:),pmr(2,:),pmr(3,:),'g',ptr(1,:),ptr(2,:),ptr(3,:),'r');
text(Xm(80),Ym(80),Zm(80),'\leftarrow 比例导引');
axis([0 25 0 5 0 25]);title('比例引导法m=3的理想弹道');
grid on

效果图

​ 源代码效果展示

​ 修改后的代码效果展示