多弹协同攻击时的无源定位(二)

当水面舰船目标实施机动时，考虑到导弹和目标同时移动，为了实现对水面舰船目标高精度的事实定位，需要实时计算水面舰船的位置，此时，需将第一问的静态模型更改为动态模型，即需要考虑导弹和目标舰船的运动轨迹。

模型

设水面舰船目标随时间的移动方程为：

$$y_T\left(t\right)=f_T\left(t\right)$$

导弹A、B、C随时间的移动方程为

$$y_X\left(t\right)=f_X\left(t\right)$$

其中X=A,B,C，为了保证导弹飞行时不碰撞，则有

$$f_{X_i}\left(t\right)\ne f_{X_j}\left(t\right)$$

选择平面内任意一点作为坐标原点，舰船T在t时刻的坐标为$\left(x_T\left(t\right),y_T\left(t\right),z_T\left(t\right)\right)$。以第一枚导弹的0时刻时延为参考基准，利用广义互相关算法(GCC)估计源信号到达第i枚导弹的t时刻时差为

$$d_{i1}(t)=d_i(t)-d_1(0)$$

$r_i\left(t\right)$为t时刻目标到第i枚导弹的距离，信号t时刻到第i枚导弹的与到第1枚导弹0时刻的距离之差为

$$r_{i1}(t)=c{d}_{i1}(t)=r_i(t)-r_1(0),i=2,3,\cdots ,M$$

根据问题一，可知计算的得到的目标点T的坐标$x{’}_T(t),y{’}_T(t),z{’}_T(t)$为

[ x ′ T ( t ) y ′ T ( t ) z ′ T ( t ) ] = [ x 21 ( t )    y 21 ( t )    z 21 ( t ) x 31 ( t )    y 31 ( t )    z 31 ( t )   x 41 ( t )    y 41 ( t )    z 41 ( t )   ] − 1 [ 1 2 ( K 2 ( t ) − K 1 ( t ) − r 21 2 ( t ) ) − r 21 ( t ) r 1 ( t ) 1 2 ( K 3 ( t ) − K 1 ( t ) − r 31 2 ( t ) ) − r 31 ( t ) r 1 ( t ) 1 2 ( K 4 ( t ) − K 1 ( t ) − r 41 2 ( t ) ) − r 41 ( t ) r 1 ( t ) ] \left[\begin{matrix}x{{'}_{T}}(t) \\ y{{'}_{T}}(t) \\ z{{'}_{T}}(t) \end{matrix}\right]= \left[\begin{matrix} {{x}_{21}}(t)\,\,{{y}_{21}}(t)\,\,{{z}_{21}}(t) \\ {{x}_{31}}(t)\,\,{{y}_{31}}(t)\,\,{{z}_{31}}(t)\, \\ {{x}_{41}}(t)\,\,{{y}_{41}}(t)\,\,{{z}_{41}}(t)\, \\\end{matrix}\right]^{-1} \left[\begin{matrix} \frac{1}{2}\left( {{K}_{2}}(t)-{{K}_{1}}(t)-r_{21}^{2}(t) \right)-{{r}_{21}}(t){{r}_{1}}(t) \\ \frac{1}{2}\left( {{K}_{3}}(t)-{{K}_{1}}(t)-r_{31}^{2}(t) \right)-{{r}_{31}}(t){{r}_{1}}(t) \\ \frac{1}{2}\left( {{K}_{4}}(t)-{{K}_{1}}(t)-r_{41}^{2}(t) \right)-{{r}_{41}}(t){{r}_{1}}(t) \\\end{matrix}\right] ​x′T​(t)y′T​(t)z′T​(t)​ ​= ​x21​(t)y21​(t)z21​(t)x31​(t)y31​(t)z31​(t)x41​(t)y41​(t)z41​(t)​ ​−1 ​21​(K2​(t)−K1​(t)−r212​(t))−r21​(t)r1​(t)21​(K3​(t)−K1​(t)−r312​(t))−r31​(t)r1​(t)21​(K4​(t)−K1​(t)−r412​(t))−r41​(t)r1​(t)​ ​

仿真

假设信号是再均匀介质中传播，即信号传输速度一致，为c，假设导弹和目标舰船在一定时间内运动轨迹都是直线，考虑直线的参数方程，设$s_{\Omega }=\left(m_{\Omega },n_{\Omega },p_{\Omega }\right)$为直线路径$\Omega$的方向向量，$\Omega =\{A,B,C,T\},$令导弹或水面目标舰船的直线参数方程为

$$\{\begin{array}{c}{x}_{\Omega }=x_{\Omega }(0)+m_{\Omega }t\\ y_{\Omega }=y_{\Omega }(0)+n_{\Omega }t\\ z_{\Omega }=z_{\Omega }(0)+p_{\Omega }t\end{array}$$

其中$m_{\Omega }^2+n_{\Omega }^2+p_{\Omega }^2=s^2$，s为导弹飞行速度。另外

$$r_i\left(t\right)=\sqrt{{\left(x_i\left(t\right)-x_T\left(t\right)\right)}^2+{\left(y_i\left(t\right)-y_T\left(t\right)\right)}^2+{\left(z_i\left(t\right)-z_T\left(t\right)\right)}^2}$$

python代码

import numpy as np
import math
import matplotlib.pyplot as plt
 
def distance(x1,y1,z1,x2,y2,z2):
    dist =math.sqrt((x1-x2)*(x1-x2) + (y1-y2)*(y1-y2)+(z1-z2)**2)
    return dist

speed = 340
c = 1000
speed_t = 150

# A0 = (10,10,10)
A0 = [240,20,20]
B0 = [124,250,60]
C0 = [51,53,244]

# 三维直线参数方程
sa = [10,math.sqrt(200),math.sqrt(40)]
sb = [10,10,math.sqrt(140)]
sc = [math.sqrt(160),10,math.sqrt(80)]

T = [30,50,0]
st = [10,math.sqrt(50),0]

dt = 0.1
t = np.arange(0+dt,10+dt,dt)

x1 = A0[0]
y1 = A0[1]
z1 = A0[2]

for ti in t:
    x2 = A0[0]+sa[0]*ti
    y2 = A0[1]+sa[1]*ti
    z2 = A0[2]+sa[2]*ti
    
    x3 = B0[0]+sb[0]*ti
    y3 = B0[1]+sb[1]*ti
    z3 = B0[2]+sb[2]*ti
    
    x4 = C0[0]+sc[0]*ti
    y4 = C0[1]+sc[1]*ti
    z4 = C0[2]+sc[2]*ti
    
    xt = T[0]+st[0]*ti
    yt = T[1]+st[1]*ti
    zt = T[2]+st[2]*ti

    r1 = distance(x1,y1,z1,xt,yt,zt)
    r2 = distance(x2,y2,z2,xt,yt,zt)
    r3 = distance(x3,y3,z3,xt,yt,zt)
    r4 = distance(x4,y4,z4,xt,yt,zt)
    
    r21 = r2 - r1
    r31 = r3 - r1
    r41 = r4 - r1
    print(r21,r31,r41)
    
    x21 = x2 - x1
    x31 = x3 - x1
    x41 = x4 - x1
    y21 = y2 - y1
    y31 = y3 - y1
    y41 = y4 - y1
    z21 = z2 - z1
    z31 = z3 - z1
    z41 = z4 - z1


    # print([x21, x31, y21, y31])

    P1_tmp  = np.array([[x21,y21,z21],[x31,y31,z31],[x41,y41,z41]])
    print("P1_tmp:")
    print(P1_tmp)

    P1 = (-1)*np.linalg.inv(P1_tmp)
    print(P1)

    P2=  np.array([[r21], [r31],[r41]])
    print("P2")
    print(P2)

    K1 = x1*x1 + y1*y1 + z1*z1;
    K2 = x2*x2 + y2*y2 + z2*z2;
    K3 = x3*x3 + y3*y3 + z3*z3;
    K4 = x4*x4 + y4*y4 + z4*z4;
    print(K1,K2,K3,K4)

    P3 = np.array([ [ (-K2 + K1 + r21*r21)/2],  [(-K3 + K1 + r31*r31)/2 ],[(-K4 + K1 + r41*r41)/2]])
    print("P3:")
    print(P3)

    xy_esti = (np.dot(P1 , P2)) * r1 +np.dot( P1 , P3)

    print("xy_esti")
    #print(type(xy_esti[0]))
    print(int(xy_esti[0]),int(xy_esti[1]),int(xy_esti[2]))