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

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

模型

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

y T ( t ) = f T ( t ) y_T\left(t\right)=f_T\left(t\right) yT​(t)=fT​(t)

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

y X ( t ) = f X ( t ) y_X\left(t\right)=f_X\left(t\right) yX​(t)=fX​(t)

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

f X i ( t ) ≠ f X j ( t ) f_{X_i}\left(t\right)\neq f_{X_j}\left(t\right) fXi​​(t)=fXj​​(t)

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

d i 1 ( t ) = d i ( t ) − d 1 ( 0 ) d_{i1}(t)=d_i(t)-d_1(0) di1​(t)=di​(t)−d1​(0)

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

r i 1 ( t ) = c d i 1 ( t ) = r i ( t ) − r 1 ( 0 ) , i = 2 , 3 , ⋯   , M r_{i1}(t)=cd_{i1}(t)=r_i(t)-r_1(0),i=2,3,\cdots,M ri1​(t)=cdi1​(t)=ri​(t)−r1​(0),i=2,3,⋯,M

根据问题一，可知计算的得到的目标点T的坐标 x ’ T ( t ) , y ’ T ( t ) , z ’ T ( t ) x’_T(t),y’_T(t),z’_T(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Ω​=xΩ​(0)+mΩ​tyΩ​=yΩ​(0)+nΩ​tzΩ​=zΩ​(0)+pΩ​t​

其中 m Ω 2 + n Ω 2 + p Ω 2 = s 2 m_{\Omega }^{2}+n_{\Omega }^{2}+p_{\Omega }^{2}={{s}^{2}} mΩ2​+nΩ2​+pΩ2​=s2，s为导弹飞行速度。另外

r i ( t ) = ( x i ( t ) − x T ( t ) ) 2 + ( y i ( t ) − y T ( t ) ) 2 + ( z i ( t ) − z T ( t ) ) 2 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} ri​(t)=(xi​(t)−xT​(t))2+(yi​(t)−yT​(t))2+(zi​(t)−zT​(t))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]))