制导控制1：二维比例导引律

1、问题描述
为了使战术弹道导弹实现自主命中目标，需要使导弹的飞控系统按照一套设定好的导引律进行控制，在知道导弹自身的位置、速度等信息的条件下，同时，利用外部的或弹上的探测设备得到目标信息，就可以通过导引律计算出实现拦截轨迹想要的控制指令，控制弹上的舵翼、气动喷口等执行机构动作，调整导弹姿态，最终飞向目标，命中。
目前实际使用最多的、应用效果最好的（比较简单的）导引律就是“比例导引律”（proportional navigation guidance law，PN），比例导引顾名思义就是按照一个比例控制，数学形式上非常简单：
\(n_{c} = N^{'} V_{c} \dot{\lambda }\)
其中：
\(n_{c}\)是过载向量，\(N^{'}\)是有效导航系数系数（一般取3-5），\(V_{c}\)是弹目相对运动速度，\(\dot{\lambda}\)是视线角速度。理论上期望的\(n_{c}\)方向是垂直于瞬时视线角

2、问题计算

二维“弹-目”拦截模型

上图给出了二维情况下导弹拦截目标的几何关系示意图。其中$V_{M}$为导弹速度矢量，$L$是导弹前置角，由导弹速度矢量$VM$与视线$RTM$之间的夹角，$HE$为指向误差。

\(V_{c}\)定义为弹目相对距离的变化率：

$ V_{c} = - \dot{R_{TM}}$ ,当导弹和目标遭遇时，\(V_{c}\)的符号会变化，也就是在某个时刻其值应当为0

对于目标运动模型，目标前置角为\(\beta\)，由于目标的过载\(n_{T}\)方向垂直于速度\(V_{T}\)，因此目标前置角速度为：
\(\dot{\beta} = \frac{n_{T}}{V_{T}}\),

将目标速度\(V_{T}\)分解到坐标系两个方向：
\( \begin{matrix} V_{T1} = -V_{T}cos(\beta)\\ V_{T2} = V_{T}sin(\beta)\\\dot{R_{T1}}=V_{T1}\\ \dot{R_{T2}}=V_{T2} \end{matrix} \)
类似的，导弹速度写为：
\( \begin{matrix} \dot{V_{M1}} = a_{M1}\\ \dot{V_{M2}} = a_{M2}\\\dot{R_{M1}}=V_{M1}\\ \dot{R_{M2}}=V_{M2} \end{matrix} \)
弹--目相对运动距离的关系写为：
\( \begin{matrix} R_{TM1} = R_{T1} - R_{M1}\\ R_{TM2} = R_{T2} - R_{M2} \end{matrix} \)
根据二维情况下导弹拦截目标的几何关系示意图，可以得到视线角为：
\(\lambda = arctan\frac{R_{TM2}}{R_{TM1}}\)
定义相对运动速度分量为：
\( \begin{matrix} V_{TM1} = V_{T1} - V_{M1}\\ V_{TM2} = V_{T2} - V_{M2} \end{matrix} \)
通过视线角对时间求导得到视线角速度：
\(\dot{\lambda }=\frac{R_{TM1}V_{TM2}-R_{TM2}V_{TM1}}{R_{TM}^{2}}\)
相对运动速度定义为：
\( \begin{matrix} V_{c} = - \dot{R_{TM}} \\R_{TM} = \sqrt{R_{TM1}^2+R_{TM2}^2} \\V_{c} = -\frac{R_{TM1}V_{TM1}+R_{TM2}V_{TM2}}{R_{TM}} \end{matrix} \)
现在已经得到了所有比例导引中所需变量：
\(n_{c} = N^{'} V_{c} \dot{\lambda }\)
导弹的加速度在两个方向上的分量为：
\( \begin{matrix} a_{M1} = -n_{c}sin\lambda\\ a_{M2} = n_{c}cos\lambda \end{matrix} \)
导弹前置角\(L\)通过弹、目速度计算：
\(L = arcsin\frac{V_{T}sin(\beta +\lambda )}{V_{m}}\)
指向误差\(HE\)根据实际情况自己设定。

3、仿真计算
仿真初始条件

Vm=3000;  %导弹速度
Vt=1000;  %目标速度
XNt = 96.6;  %目标过载
HEdeg = 0.0; %指向误差
XNP = 5;  %有效导航系数
RM1 = 0;  %导弹位置1轴
RM2 = 10000;  %导弹位置2轴
RT1 = 40000;  %目标位置1轴
RT2 = 10000;  %目标位置2轴
Beta = 0.0;  %目标速度方向
Vt1 = -Vt*cos(Beta); %目标速度分量1
Vt2 = Vt*sin(Beta);  %目标速度分量2
HE = HEdeg/57.3;  %度转化为弧度

RTM1 = RT1 - RM1;
RTM2 = RT2 - RM2;
RTM = sqrt(RTM1^2 + RTM2^2);  %弹目相对距离
Lamda = atan(RTM2/RTM1);  %视线角
Lead = asin(Vt*sin(Beta + Lamda)/Vm);  %导弹前置角
Theta = Lamda + Lead;
Vm1 = Vm*cos(Theta + HE);  %导弹速度分量1
Vm2 = Vm*sin(Theta + HE);  %导弹速度分量2
Vtm1 = Vt1 - Vm1;
Vtm2 = Vt2 - Vm2;
Vc = -(RTM1*Vtm1 + RTM2*Vtm2)/RTM;  %弹目相对运动速度

通过二阶龙格库塔格式迭代求解二维弹目运动方程：

VAR = zeros(7);  %7个变量
VAR(1) = Beta;
VAR(2) = RT1;
VAR(3) = RT2;
VAR(4) = RM1;
VAR(5) = RM2;
VAR(6) = Vm1;
VAR(7) = Vm2;

k=1;
for n=1:7
    save(k,n)= VAR(n);
end

while Vc >= 0.0  %截止条件
    k= k+1;
    if(RTM < 1000)  %根据弹目距离调整二阶龙格库塔格式时间步长
        H = 0.0002;
    else
        H = 0.01;
    end

    var_old = turnvar1(VAR);  %保留迭代前的旧变量

    [v_temp,Vc,RTM] = cal(VAR,Vt1,Vt2,XNt,XNP);  %根据比例导引律计算上面7个变量的导数，更新目标速度和弹目距离
    Vt1 = v_temp(2);
    Vt2 = v_temp(3);

    v_new1 = turnvar2(VAR,v_temp,H);  %通过旧变量和导数计算得到初步预测变量
       
    [v_temp,Vc,RTM] = cal(v_new1,Vt1,Vt2,XNt,XNP);  %根据比例导引律计算上面7个变量的导数，更新目标速度和弹目距离
    Vt1 = v_temp(2);
    Vt2 = v_temp(3);

    v_new2 = turnvar3(var_old,v_new1,v_temp,H); %旧变量+初步预测变量+初步预测导数，通过二阶龙格库塔格式得到新变量

    T = T+H;  %时间更新
    VAR = v_new2;  %新变量赋值给下个迭代步作为旧变量
    for n=1:7
        save(k,n)= v_new2(n);  %保存数据
    end
    
end
figure 
hold on
plot(save(:,4),save(:,5),save(:,2),save(:,3),'LineWidth',3),grid
xlabel('X (Ft) ')
ylabel('Y (Ft)')
%%
function OLD = turnvar1(var)
    OLD = zeros(7);
    for i=1:7
        OLD(i) = var(i);
    end
end

function NEW1 = turnvar2(var1,var2,h)
    NEW1 = zeros(7);
    for i=1:7
        NEW1(i) = var1(i) + h*var2(i);
    end
end

function NEW2 = turnvar3(var1,var2,var3,h)
    NEW2 = zeros(7);
    for i=1:7
        NEW2(i) = 0.5*(var1(i) + var2(i) + h*var3(i));
    end
end

function [V0,VC,RTM] = cal(var,VT1,VT2,XNt,XNP)
    RTM1 = var(2) - var(4);
    RTM2 = var(3) - var(5);
    RTM = sqrt(RTM1^2 + RTM2^2);
    Vtm1 = VT1 - var(6);
    Vtm2 = VT2 - var(7);
    VC = -(RTM1*Vtm1 + RTM2*Vtm2)/RTM;

    Lamda = atan(RTM2/RTM1);
    D_Lamda = (RTM1 * Vtm2 - RTM2 * Vtm1)/(RTM*RTM);
    Nc = XNP * VC *D_Lamda;  %比例导引律
    am1 = -Nc*sin(Lamda);
    am2 = Nc*cos(Lamda);
    %VT = 1000;
    VT = sqrt(VT1^2+VT2^2);
    VT1 = -VT*cos(var(1));
    VT2 = VT*sin(var(1));
    D_Beta = XNt/VT;

    V0 = zeros(7);
    V0(1) = D_Beta;
    V0(2) = VT1;
    V0(3) = VT2;
    V0(4) = var(6);
    V0(5) = var(7);
    V0(6) = am1;
    V0(7) = am2;
end

4.计算结果
第一个算例结果如下所示，导弹的初始位置为（0,10000），速度为3000ft/s,初始的指向误差为20°；目标的初始位置为（4000,10000），速度为1000ft/s，\(\beta = 0\)。计算时将有效导航系数分别设为4和5进行计算，当\(N^{'}\)增大时，弹道转向目标的速度就越快，此时飞行过程起始和末端的过载都会偏大，为了尽快消除指向误差并瞄准目标。

第二个算例结果如下所示，导弹的初始位置为（0,10000），速度为3000ft/s,初始的指向误差为0°；目标的初始位置为（4000,10000），速度为1000ft/s，$\beta = 0$,$n_{T} = 96.6ft/s^2$，目标初始的加速度方向垂直于弹目视线。

参考文献
[1]Paul Zarchan. Tactical and Strategic Missile Guidance[M]. Six edition.