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

写在前面

之前自己写的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 SnNP，以 S n N ⌢ , S n P ⌢ , N P ⌢ \overset{\frown} {S_nN},\overset{\frown} {S_nP},\overset{\frown} {NP} SnN⌢,SnP⌢,NP⌢表示球面三角形大圆弧，以球面角 ∠ N S n P , ∠ P N S n , ∠ N P S n \angle NS_nP,\angle PNS_n,\angle NPS_n ∠NSnP,∠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(∠NSnP)sin(∠PNSn)=cot(NP⌢)sin(SnN⌢)−cos(SnN⌢)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 . ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧∠PNSnSnN⌢NP⌢=λs−λn=2π−ϕn=2π−ϕs (2)
由侧向角的定义可知:
∠ N S n P = θ n \angle NS_nP=\theta_n ∠NSnP=θ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θnsinθn=cosϕnsinϕs−sinϕncosϕscos(λs−λn)cosϕssin(λ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θnsinθn=cosϕnsinϕs−sinϕncosϕs(cosλscosλn+sinλssinλn)cosϕs(sinλscosλn−cosλssinλ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 liu=mivy+nivx (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 . ⎩⎪⎪⎪⎪⎨⎪⎪⎪⎪⎧l1ul2u11=m1vy+n1vx=m2vy+n2vx=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=l2m1−l1m2l1n2−l2n1 (13)
如果 l 2 m 1 − l 1 m 2 = 0 {l_2m_1-l_1m_2}=0 l2m1−l1m2=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=limiy+nix (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 版权协议，转载请附上原文出处链接和本声明。

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

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))