比例导引理想弹道仿真

Erik_qi

已于 2025-02-17 19:00:55 修改

阅读量1.4k

点赞数 13

CC 4.0 BY-SA版权

分类专栏： Algorithm 文章标签： matlab

于 2024-11-06 08:11:23 首次发布

本文链接：https://blog.csdn.net/qq_38257397/article/details/143554720

Algorithm 专栏收录该内容

4 篇文章

订阅专栏

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

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

部分公式推导

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

比例导引法

导弹与目标在同一平面内运动，设 $M_k$ 表示第 $k$ 个时间间隔时，导弹的空间位置； $M_{k+1}$ 表示第 $k + 1$ 个时间间隔时，导弹的空间位置； $T_k$ 表示第 $k$ 个时间间隔时，目标的空间位置； $r (k)$ 表示第 $k$ 个时间间隔时，导弹与目标的距离； $S_m$ 表示导弹在一个时间间隔内运动的距离( $Sm=vm∗ΔtS_m=v_m*\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_kB$ 。设 $M_{k-1}T_c=c$ ， $AM_{K-1}=c_2$ ， $∠AMk−1Tk−1=αk−1\angle AM_{k-1}T_{k-1}=\alpha_{k-1}$ ，由几何知识可知，

$βk−1=∠AMk−1Tk−1=arccos([ri(k−1)]2+st2+c22∗ri(k−1)∗st)\beta_{k-1}=\angle AM_{k-1}T_{k-1}=arccos(\dfrac{\left[r_i(k-1)\right]^2 +s_t^2+c^2}{2*r_i(k-1)*s_t})$

$Δqk−1=∠AMkTk≈∠TMk−1Tk−1=arccos([ri(k−1)]2−st2+c22∗ri(k−1)∗c)\Delta q_{k-1} = \angle AM_k T_k \approx \angle TM_{k-1}T_{k-1} = arccos(\dfrac{\left[r_i(k-1)\right]^2 - s_t^2+c^2}{2*r_i(k-1)*c})$

$ri(k)=(xi(k)−xm(k))2+(y(k)−ym(k))2+(zi(k)−zm(k))2r_i(k) = \sqrt{(x_i(k)-x_m(k))^2 + (y(k)-y_m(k))^2 + (z_i(k)-z_m(k))^2}$

$c=(xi(k)−xm(k−1)2+(yi(k)−ym(k−1))2+(zr(k)−zm(k−1))2c=\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}$

$c1=ri(k−1)sin(βk−1)sin(αk−1+βk−1)c_1 = \dfrac{r_i(k-1)sin(\beta_{k-1})}{sin(\alpha_{k-1}+\beta_{k-1})}$

$c2=ri(k−1)sin(αk−1)sin(αk−1+βk−1)c_2 = \dfrac{r_i(k-1)sin(\alpha_{k-1})}{sin(\alpha_{k-1}+\beta_{k-1})}$

导弹的直角纵坐标的差分方程为： ${qk=qk−1+Δqθk=θk−1+mΔqαk=θk−qk−1xt(k)=xt(k−1)+c2st(xt(k)−xt(k−1))yt(k)=yt(k−1)+c2st(yt(k)−yt(k−1))zt(k)=zt(k−1)+c2st(zt(k)−zt(k−1))xm(k)=xm(k−1)+smc1(xt(k)−xm(k−1))ym(k)=ym(k−1)+smc1(yt(k)−ym(k−1))zm(k)=zm(k−1)+smc1(zt(k)−zm(k−1))\begin{cases} 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)+\dfrac{c_2}{s_t}(x_t(k)-x_t(k-1))\\ y_t(k)=y_t(k-1)+\dfrac{c_2}{s_t}(y_t(k)-y_t(k-1))\\ z_t(k)=z_t(k-1)+\dfrac{c_2}{s_t}(z_t(k)-z_t(k-1))\\ x_m(k)=x_m(k-1)+\dfrac{s_m}{c_1}(x_t(k)-x_m(k-1))\\ y_m(k)=y_m(k-1)+\dfrac{s_m}{c_1}(y_t(k)-y_m(k-1))\\ z_m(k)=z_m(k-1)+\dfrac{s_m}{c_1}(z_t(k)-z_m(k-1))\\ \end{cases}$

导弹与目标接近时，用 $∠AMk−1Tk−1\angle AM_{k-1}T_{k-1}$ 近似 $Δqk−1\Delta q_{k-1}$ 时误差比较大，因此要修正：

$c3=(c1−sm)2+(c2−st)2+x(c1−sm)(c2−st)cos(α+β)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)}$

$Δq=α−arccos((c1−sm)2+c32−(c2−st)22(ct−sm)c3)\Delta q = \alpha - arccos(\dfrac{(c_1-s_m)^2+c_3^2-(c_2-s_t)^2}{2(c_t -s_m)c_3})$

部分证明如下：

自 $M_{k-1}$ 作 $ΔMk−1Tk−1Tk\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=st2−c2+r(k−1)2∗st∗r(k−1)x=\dfrac{s_t^2 - c^2 +r(k-1)}{2*s_t*r(k-1)}$

设 $∠ATk−1Mk−1\angle AT_{k-1}M_{k-1}$ 为 $βk−1\beta_{k-1}$ ，则

$βk−1=arccos(st2−c2+r(k−1)2∗st∗r(k−1))\beta_{k-1}=arccos(\dfrac{s_t^2-c^2+r(k-1)}{2*s_t*r(k-1)})$

$sin(βk−1+αk−1)=hc1=sin(βk−1)∗r(k−1)c1sin(\beta_{k-1}+\alpha_{k-1})=\dfrac{h}{c_1}=\dfrac{sin(\beta_{k-1})*r(k-1)}{c_1}$

可得， $c1=r(k−1)sin(βk−1)sin(αk−1+βk−1)c_1 = \dfrac{r(k-1)sin(\beta_{k-1})}{sin(\alpha_{k-1}+\beta_{k-1})}$ 。同理从 $T_k$ 作 $ΔMk−1Tk−1Tk\Delta M_{k-1}T_{k-1}T_k$ 的高 $T_{k-1}N$ ，可得 $c2=r(k−1)sin(αk−1)sin(αk−1+βk−1)c_2 = \dfrac{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