球面双站交叉定位计算方法

写在前面

之前自己写的word丢了，为避免丢失，在网上发一下，主要是备忘，有些表达不严谨请，见谅。

方法和模型图片来自引文:张静.杜剑平.蒋俊,基于球体模型的短波固定多站交叉定位选站方法[j].信息工程大学学报,2020,(1),9-14 26

再吐槽知网:下个论文收费3.5，表示理解；充值最小30，每次下载都要收一遍，手机app上藏得老深了，我就想要张图，有这功夫自己都画出来了。

另外，电脑没在身边，手机码字，写公式不易，转发引用请注明出处。
计算模型

当目标与侧向站不在同一平面时，侧向交叉定位必须考虑地球曲率的影响，将地球看成球体，半径 R = 6731 k m R=6731km R=6731km,考虑到侧向误差远大于地球椭球偏心率影响，简化计算把地球看成正球。此时，侧向交叉定位如图1所示。

图1 基于球体模型的侧向交叉定位示意图

图中 P ( ϕ s , λ s ) P(\phi_s,\lambda_s) P(ϕs​,λs​)为辐射源; S n ( ϕ n , λ n ) , n = 1 , 2 , 3... S_n(\phi_n,\lambda_n),n=1,2,3... Sn​(ϕn​,λn​),n=1,2,3...,为侧向站， θ n \theta_n θn​为 S n S_n Sn​对 P P P的侧向角真实值， β n \beta_n βn​为测量值。
为便于描述观察模型，建立以球形为原点的空间大地坐标系，用纬度 ϕ \phi ϕ，经度 λ \lambda λ和大地高 H H H来表示空间位置，如图2所示。
图2 空间大地坐标系示意图

在空间大地坐标系中， S n S_n Sn​的坐标为 S n ( ϕ n , λ n , 0 ) , P ( ϕ s , λ s , 0 ) S_n(\phi_n,\lambda_n,0),P(\phi_s,\lambda_s,0) Sn​(ϕn​,λn​,0),P(ϕs​,λs​,0),两点与北极点形成球面三角形 S n N P S_nNP Sn​NP，以 S n N ⌢ , S n P ⌢ , N P ⌢ \overset{\frown} {S_nN},\overset{\frown} {S_nP},\overset{\frown} {NP} Sn​N⌢​,Sn​P⌢​,NP⌢表示球面三角形大圆弧，以球面角 ∠ N S n P , ∠ P N S n , ∠ N P S n \angle NS_nP,\angle PNS_n,\angle NPS_n ∠NSn​P,∠PNSn​,∠NPSn​表示球面三角形中的三个角,则由球面三角形的余切定理可得: cot ⁡ ( ∠ N S n P ) sin ⁡ ( ∠ P N S n ) = cot ⁡ ( N P ⌢ ) sin ⁡ ( S n N ⌢ ) − cos ⁡ ( S n N ⌢ ) cos ⁡ ( ∠ P N S n ) \cot(\angle NS_nP)\sin(\angle PNS_n)=\cot(\overset{\frown} {NP})\sin(\overset{\frown} {S_nN})-\cos(\overset{\frown} {S_nN})\cos(\angle PNS_n) cot(∠NSn​P)sin(∠PNSn​)=cot(NP⌢)sin(Sn​N⌢​)−cos(Sn​N⌢​)cos(∠PNSn​) (1)
由经纬度的定义可知:
{ ∠ P N S n = λ s − λ n S n N ⌢ = π 2 − ϕ n N P ⌢ = π 2 − ϕ s \left\{
∠PNSnSnN⌢NP⌢=λs−λn=π2−ϕn=π2−ϕs

\right . ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​∠PNSn​Sn​N⌢​NP⌢​=λs​−λn​=2π​−ϕn​=2π​−ϕs​​ (2)
由侧向角的定义可知:
∠ N S n P = θ n \angle NS_nP=\theta_n ∠NSn​P=θn​ (3)
求解过程

将式(2)(3)代入(1)得:
cot ⁡ ( θ ) sin ⁡ ( λ s − λ n ) = cot ⁡ ( π 2 − ϕ s ) sin ⁡ ( π 2 − ϕ n ) − cos ⁡ ( π 2 − ϕ n ) cos ⁡ ( λ s − λ n ) \cot(\theta)\sin(\lambda_s-\lambda_n)=\cot(\frac \pi 2 -\phi_s )\sin(\frac \pi 2 - \phi_n) - \cos(\frac \pi 2 - \phi_n)\cos(\lambda_s - \lambda_n) cot(θ)sin(λs​−λn​)=cot(2π​−ϕs​)sin(2π​−ϕn​)−cos(2π​−ϕn​)cos(λs​−λn​) (4)
即:
sin ⁡ θ n cos ⁡ θ n = cos ⁡ ϕ s sin ⁡ ( λ s − λ n ) cos ⁡ ϕ n sin ⁡ ϕ s − sin ⁡ ϕ n cos ⁡ ϕ s cos ⁡ ( λ s − λ n ) \frac {\sin \theta_n} {\cos \theta_n}=\frac {\cos \phi_s \sin(\lambda_s-\lambda_n)} {\cos \phi_n\sin \phi_s - \sin \phi_n \cos \phi_s \cos(\lambda_s- \lambda_n)} cosθn​sinθn​​=cosϕn​sinϕs​−sinϕn​cosϕs​cos(λs​−λn​)cosϕs​sin(λs​−λn​)​ (5)
和差化积:
sin ⁡ θ n cos ⁡ θ n = cos ⁡ ϕ s ( sin ⁡ λ s cos ⁡ λ n − cos ⁡ λ s sin ⁡ λ n ) cos ⁡ ϕ n sin ⁡ ϕ s − sin ⁡ ϕ n cos ⁡ ϕ s ( cos ⁡ λ s cos ⁡ λ n + sin ⁡ λ s sin ⁡ λ n ) \frac {\sin \theta_n} {\cos \theta_n}=\frac {\cos \phi_s (\sin \lambda_s \cos \lambda_n - \cos \lambda_s \sin\lambda_n)} {\cos \phi_n\sin \phi_s - \sin \phi_n \cos \phi_s (\cos \lambda_s \cos \lambda_n + \sin \lambda_s \sin\lambda_n)} cosθn​sinθn​​=cosϕn​sinϕs​−sinϕn​cosϕs​(cosλs​cosλn​+sinλs​sinλn​)cosϕs​(sinλs​cosλn​−cosλs​sinλn​)​ (6)
令已知数: a , b , c , d , e , f = sin ⁡ λ n , cos ⁡ λ n , sin ⁡ ϕ n , cos ⁡ ϕ n , sin ⁡ θ n , cos ⁡ θ n a,b,c,d,e,f=\sin \lambda_n,\cos \lambda_n,\sin \phi_n,\cos \phi_n,\sin \theta_n,\cos \theta_n a,b,c,d,e,f=sinλn​,cosλn​,sinϕn​,cosϕn​,sinθn​,cosθn​ (7)
令未知数: x , y , u , v = sin ⁡ λ s , cos ⁡ λ s , sin ⁡ ϕ s , cos ⁡ ϕ s x,y,u,v=\sin \lambda_s,\cos \lambda_s,\sin \phi_s,\cos \phi_s x,y,u,v=sinλs​,cosλs​,sinϕs​,cosϕs​ (8)
将(7)(8)代入(6)简化后得:
e d u = ( e c f − f a ) v y + ( e c a + f b ) v x edu=(ecf - fa)vy+(eca+fb)vx edu=(ecf−fa)vy+(eca+fb)vx (9)
令已知数:
l i = e d , m i = e c b − f a , n i = e c a − f b , i = 1   o r   2 l_i=ed,m_i=ecb-fa,n_i=eca-fb,i=1\ or\ 2 li​=ed,mi​=ecb−fa,ni​=eca−fb,i=1 or 2(10)
将(10)代入(9)简化后:
l i u = m i v y + n i v x l_i u= m_ivy+n_ivx li​u=mi​vy+ni​vx (11)
已知两个侧向站的坐标和基本三角公式可联立方程组:
{ l 1 u = m 1 v y + n 1 v x l 2 u = m 2 v y + n 2 v x 1 = x 2 + y 2 1 = u 2 + v 2 \left\{
l1ul2u11=m1vy+n1vx=m2vy+n2vx=x2+y2=u2+v2
\right . ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧​l1​ul2​u11​=m1​vy+n1​vx=m2​vy+n2​vx=x2+y2=u2+v2​ (12)
令已知数:
A = l 1 n 2 − l 2 n 1 l 2 m 1 − l 1 m 2 A=\frac {l_1 n_2-l_2 n_1}{l_2m_1-l_1m_2} A=l2​m1​−l1​m2​l1​n2​−l2​n1​​ (13)
如果 l 2 m 1 − l 1 m 2 = 0 {l_2m_1-l_1m_2}=0 l2​m1​−l1​m2​=0,辐射源经度为0或180度，或者3点在同一经线圆上。
得到2组经度解:
x = ± 1 A 2 + 1 , y = A x x=\pm \sqrt{ \frac 1 {A^2+1}},y=Ax x=±A2+11​
​,y=Ax (14)
令已知数:
B = m i y + n i x l i B=\frac {m_iy+n_ix}{l_i} B=li​mi​y+ni​x​ (15)
由(14)代入可知B有2个值，已知数，也可以使用1号或者2号侧向站代入计算，避免 l i = 0 l_i=0 li​=0,如果 l 1 = l 2 = 0 l_1=l_2=0 l1​=l2​=0,代表 P P P在两极。
得到2组纬度解:
v = ± 1 B 2 + 1 , u = B v v=\pm \sqrt{ \frac 1 {B^2+1}},u=Bv v=±B2+11​
​,u=Bv (16)
对应每个 A A A有2个解，共4组解。
分别对 ( x , y ) , ( u , v ) (x,y),(u,v) (x,y),(u,v)使用atan2函数计算经纬度得到4组经纬度坐标，其中两组纬度坐标不在 [ − π 2 , π 2 ] [-\frac \pi 2,\frac \pi 2] [−2π​,2π​]范围内，剔除后得到两组坐标，是球面上的过心对称点。
是用Haversin函数，分别求两个侧向站到两组坐标的距离，得到4个值，其中最小值就是目标点到其中一个站的最小距离，对应的坐标就是最终目标点 P ( ϕ s , λ s ) P(\phi_s,\lambda_s) P(ϕs​,λs​)的坐标。
————————————————

                            版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。

import numpy as np
import math

def get_lmn(s):
    lon,lat,az = s
    a,b,c,d,e,f = np.sin(lon),np.cos(lon),np.sin(lat),np.cos(lat),np.sin(az),np.cos(az)
    return (e*d,e*c*b-f*a,e*c*a+f*b)

def cov2cood(sincos):
    claCoord = lambda scsc:(math.atan2(scsc[2],scsc[3]),math.atan2(scsc[0],scsc[1]))
    result = [claCoord(sincos[0]),claCoord(sincos[1]),claCoord(sincos[2]),claCoord(sincos[3])]
    return np.rad2deg(result)


#在s点在p1,p2 的连线上，没有处理
def cross_location(s1,s2):
    '''
    目标点为S(xlon,xlat)
    点P(lon,lat,az)与S的关系(1)
    (1):sin(az)/cos(az) = cos(xlat)sin(xlon-lon)/[cos(lat)sin(xlat)-sin(lat)cos(xlat)cos(xlon-lon)]
    令x,y,u,v = sin(xlon),cos(xlon),sin(xlat),cos(xlat)
    (1)可将简化为(2):l*u=m*vy+n*vx
    两点分别带入(2),得到2个方程
    加上(3):u^2+v^2=1，(4):x^2+y^2=1,共4个方程形成的4元2次方程组
    令 a = (l1*n2-l2*n1)/(l2*m1-l1*m2)
    x=±√(1/(a^2+1)),y=ax
    令 b = (m1*y+n1*x)/l2,换成m2,n2,l2也可以
    v=±√(1/(b^2+1)),u=bv
    得到4组(x,y,u,v)
    换算成经纬角(arctan2(x,y),arctan2(u,v)) =>(xlon,xlat) 
    排除xlat < -pi/2 ,或 xlat > pi/2 ,只剩两组坐标
    这两个点是过心对称点
    '''
    s1 = np.deg2rad(s1)
    s2 = np.deg2rad(s2)
    l1,m1,n1 = get_lmn(s1)
    l2,m2,n2= get_lmn(s2)
    if (l2*m1-l1*m2) != 0:
        A = (l1*n2-l2*n1)/(l2*m1-l1*m2) 
        X = np.array([np.sqrt(1/(A**2+1)),-np.sqrt(1/(A**2+1))])
        Y = A * X
    else:
        X = np.array([0,0])
        Y = np.array([1,-1])
    #防止除0错误
    if l1!=0 or l2!=0:
        L,M,N = l1,m1,n1
        if l1 == 0:
            L,M,N = l2,m2,n2
        calb = lambda x,y:(M*y+N*x)/L
        b0 = calb(X[0],Y[0])
        b1 = calb(X[1],Y[1])
        V0 = np.array([np.sqrt(1/(b0**2+1)),-np.sqrt(1/(b0**2+1))])
        U0 = V0*b0
        V1 = np.array([np.sqrt(1/(b1**2+1)),-np.sqrt(1/(b1**2+1))])
        U1 = V1*b1
    else:
        # cos(lat)不会为0 ，不然就az就没有意义只有 sin(az)==0 即az均为 =0 或 180，指向极点 
        U0=[1,1]
        U1=[-1,1]
        V0=[0,-0.1]
        V1=[0,-0.1] 
    result = [(U0[0],V0[0],X[0],Y[0]),(U0[1],V0[1],X[0],Y[0]),(U1[0],V1[0],X[1],Y[1]),(U1[1],V1[1],X[1],Y[1])]
    result = cov2cood(result)
    frsl = []

    for i in range(4):
        lat = result[i][1] 
        if lat>= -90 and lat <=90:
            frsl.append(result[i])
    return np.array(frsl)

def distance_haversine(p1,p2,r=1):
    '''
    hav(x) = sin(x/2)^2 = (1-cos(x))/2
    a(alpha) 两点过心角
    hav(a) = hav(lat1-lat2)+cos(lat1)*cos(lat2)*hav(lon1-lon2)
    input:
    p1:[lon,lat] in degree
    p2:[lon,lat] in degree
    r:球半径
    return:球面距离
    '''
    p1=np.deg2rad(p1)
    p2=np.deg2rad(p2)
    hav_lon = math.sin((p1[0]-p2[0])/2)**2
    hav_lat = math.sin((p1[1]-p2[1])/2)**2
    hav_a = hav_lat + math.cos(p1[1])*math.cos(p2[1])*hav_lon
    a = 2*math.atan2(math.sqrt(hav_a),math.sqrt(1-hav_a))
    return a*r

def distance_greate_circle(p1,p2,r=1):
    '''
    使用弦长计算角,求弧长
    '''
    dpp = distance_chord_line(p1,p2)
    #余弦定理求角
    a = math.acos((2-dpp**2)/2)
    return r*a
    

def distance_chord_line(p1,p2,r=1):
    '''
    计算弦长
    x= cos(lat)sin(lon)
    y= cos(lat)cos(lon)
    z= sin(lat)
    '''
    to_xyz = lambda lon,lat:(math.cos(lat)*math.sin(lon),math.cos(lat)*math.cos(lon),math.sin(lat))
    p1=np.deg2rad(p1)
    p2=np.deg2rad(p2)
    p1 = to_xyz(p1[0],p1[1])
    p2 = to_xyz(p2[0],p2[1])
    d = (p1[0]-p2[0])**2+(p1[1]-p2[1])**2+(p1[2]-p2[2])**2
    d = math.sqrt(d)
    return r*d

def pick_nearest_root(s1,s2,roots):
    '''
    选出离侧向站最近的点
    '''
    nroot = len(roots)
    adist = []
    for i in range(nroot):
        adist.append(distance_chord_line(s1,roots[i]))
        adist.append(distance_chord_line(s2,roots[i]))
    minv = math.pi*2
    min_i = 0
    for i in range(len(adist))  :
        if minv > adist[i] :
            minv = adist[i]
            min_i = i
    return roots[min_i//2]

def cross_location_nearest(s1,s2):
    roots = cross_location(s1,s2)
    return pick_nearest_root(s1[:2],s2[:2],roots)

if __name__ == "__main__":
    s1,s2 = [61.1,32.2,120],[52,28.1,33]
    print("s1[lon,lat,az]  ---------------------------\n",s1)
    print("s2[lon,lat,az]  ---------------------------\n",s2)
    locs = cross_location(s1,s2)
    print("roots of equation set[lon,lat]  -----------\n",locs)
    loc=cross_location_nearest(s1,s2)
    print("nearest loaction[lon,lat]  ----------------\n",loc)
    print("dist to s1  ---------------------------\n",distance_haversine(s1[:2],loc))
    print("dist to s2  ---------------------------\n",distance_haversine(s2[:2],loc))

                        
原文链接：https://blog.csdn.net/weixin_42780086/article/details/122765969