CN107015199A - 一种考虑无人机姿态角的双无人机测向时差定位方法 - Google Patents

Abstract

一种考虑无人机姿态角的双无人机测向时差定位方法，包括如下步骤：(1)在主站无人机的机载坐标系下测得目标信号的方位角、俯仰角和目标信号到达主站与辅站的时间差；(2)根据步骤(1)测得的数据解算出目标与主站无人机的距离r₀；(3)根据目标与主站无人机的距离、主站无人机测得目标信号的方位角和俯仰角解算出目标在主站无人机机载坐标系下的坐标；(4)通过相关坐标系的坐标变换关系得到目标在大地直角坐标系下的坐标。本发明的方法通过坐标系的转换，将无人机自身观测坐标系转换成大地直角坐标系，实现无人机对目标进行测向时差定位的位置解算，并建立了目标位置与无人机姿态之间的直接联系。

Description

一种考虑无人机姿态角的双无人机测向时差定位方法

技术领域

本发明涉及无源定位技术领域，尤其是一种考虑无人机姿态角的双无人机测向时差定位方法。

背景技术

在未来的信息化空中战场上，无人机协同作战将是主要的空战方式之一。无人机可执行的作战任务有情报侦察、目标的定位打击等，由于无人机具有体积小、机动性强以及隐蔽性好等优点，可以对敌方目标进行长时间的侦察定位，为打击敌方目标提供精确位置信息。但无人机体积小的特点也造成无人机所能承载的有效载荷有限。一般用于定位的有源雷达，需要大功率的信号发射装备，因此有源探测设备一般不适合无人机搭载。与有源探测设备相比，无源探测设备不需要向被探测目标发射信号，只是接收目标向外界发射或反射的信号，因此无源探测设备一般重量较轻，比较适合无人机搭载。通过多架无人机搭载无源探测设备构成的无源定位***，对敌方目标进行定位时更加隐蔽、及时和准确，并且克服了由于受地球曲率影响地面无源***不能实现低空/地面目标定位的缺点。无源定位***可以通过对辐射信号的辐射源定位来确定搭载该辐射源设备的具***置。由于它具有无源的特性，不会被敌方电子侦察设备所干扰，比较适合在敌方不知道的情况下，对敌方目标进行长时间定位和监视，在有源定位设备受到干扰而无法工作时起到补充作用。

无人机无源定位***和地基、舰载等无源定位手段相比具有独特的优势，它的作用距离远，覆盖地域大，机动能力强，危险性小，不会造成人员伤亡。无人机定位被广泛地用于军事和民用领域，在野外执行各种任务，需要无人机定位达到信息采集、数据融合等目的,同时,无人机定位也能够精确制导武器提供实时的制导信息。在无人机诸多的军事应用中，无人机目标定位功能是实现这些应用的前提条件。只有实现高精度的目标定位，才能实现情报侦察、火炮校射等军事任务。

按照观测站对目标观测时得到的参数不同，无源定位可以分为多普勒频差定位、时差定位、测向交叉定位、时差和频差联合定位以及测向和时差联合定位。测向交叉定位通过多个观测站测得目标辐射信号的到达角，根据测得角度参数建立目标位置求解方程，目标的定位精度受观测距离的影响比较大，当对远距离目标进行观测时，由于观测的角度本身就比较小，因此很小的角度观测误差，将会对目标的定位误差产生剧烈的影响。时差定位是根据不同观测站接收到目标辐射信号的时间差，对目标的位置进行解算，该定位体制一般由四个观测站的组成，是目前定位误差相对较小的一种定位体制。但由于***组成需要四个观测站，而且对四个观测站的时间统一有较高的要求，这些因素将不利于无人机作为观测站组建无源定位***。多普勒频差定位是根据不同观测站测得目标信号的频率差进行目标位置解算，对三维空间中的目标定位时，需要至少四个观测站且观测站与目标之间要有相对运动，定位精度受观测平台的几何配置的影响较大。测向时差定位是通过对接收信号的角度和时差的观测进行目标定位，相比于单独的时差和测向定位利用了多个观测量的信息，在提高定位精度同时降低了***的复杂度，最少只需要两个无人机搭载无源探测设备就可组成测向时差无源定位***。测向时差定位技术是根据不同观测站接收到的目标辐射信号的时间差及目标辐射信号的波达方向来确定目标的位置，本身不需要对目标发射信号，适用于对能发射雷达信号的目标进行定位，比如带有探空雷达地面侦察装备、自带雷达的武器***等。

在机载无源定位方面，美国处于领先地位，其中具有代表性的有F-22战斗机协作组建的定位***、精确攻击定位***(PLSS)。F-22战斗机编队无源组网定位***，通过在F-22战斗机上搭载ALR-94雷达警告接收机和被动工作模式的APG-77雷达作为无源探测设备，对目标进行精确定位；精确打击定位***(PLSS)是将多架侦察机作为无源定位***中的观测站，对目标辐射源进行观测并将观测数据传回到地面观测站，根据相关数据进行目标位置的解算。国内的相关院校及研究所如中国电子科技集团51所、29所，中国航空工业集团8511所，西安电子科技大学等也对单机作为观测站的无源定位举行了相关的飞行试验，关于多机无源定位目前尚不成熟，但它是一种能够实现目标的快速高精度定位的有效方式，相对于单机无源定位，它具有更高的定位精度；与观测站安装在地面的无源定位相比，它可以更加灵活地实现特定地域的目标侦察定位。

目前已有的测向时差定位技术中，观测站多为地面观测站，其多将观测站看作一个质点对目标进行定位解算，且仍存在定位模糊问题，目标位置解不唯一。无人机作为观测站对目标进行测向时差定位时，由于无人机的自身姿态在不断变化，姿态信息会影响目标定位，通过仿真实验，证明姿态角观测误差会对定位误差有一定的影响。而常规的侧向时差定位方法，特别是以地面站为观测站的定位***都采用无人机自身观测坐标系，因此只能将观测站作为一个质点，无法考虑无人机姿态角误差的影响。

发明内容

本发明所要解决的技术问题在于，提供一种考虑无人机姿态角的双无人机测向时差定位方法，能够实现无人机对目标进行测向时差定位的位置解算，并建立了目标位置与无人机姿态之间的直接联系。

为解决上述技术问题，本发明提供一种考虑无人机姿态角的双无人机测向时差定位方法，包括如下步骤：

(1)在主站无人机的机载坐标系下测得目标信号的方位角、俯仰角和目标信号到达主站与辅站的时间差；

(2)根据步骤(1)测得的数据解算出目标与主站无人机的距离r₀；

(3)根据目标与主站无人机的距离、主站无人机测得目标信号的方位角和俯仰角解算出目标在主站无人机机载坐标系下的坐标；

(4)通过相关坐标系的坐标变换关系得到目标在大地直角坐标系下的坐标。

优选的，大地直角坐标OXYZ，原点为地球椭球的球心；测量直角坐标系oxyz，原点为观测站中心；主站机载坐标系S₀xyz，原点为载机中心；

大地直角坐标系与测量直角坐标系的矩阵关系式具体为：假设目标在大地直角坐标系下和测量直角坐标系下的坐标分别为(X，Y，Z)和(x，y，z)，观测站的坐标在大地直角坐标系和大地坐标系下分别为(X₀，Y₀，Z₀)和(L₀，B₀，H₀)，则大地直角坐标系与测量直角坐标系的矩阵表达式如下：

式中：

观测站的测量直角坐标系与机载坐标系的矩阵关系式具体为：假设目标在机载坐标系下和观测站测量直角坐标系下的坐标分别为(x_b，y_b，z_b)和(x，y，z)，由于建立的观测站的测量直角坐标系与机载坐标系的坐标原点重合，而且都为观测站的质心，因此目标在观测站的观测直角坐标系与机载坐标系的位置关系如下：

式中：

其中，δ_a、δ_b、δ_c为载机的姿态角(横滚角、偏航角以及俯仰角)。

优选的，定位方法具体过程为：假设在主站的机载坐标系中，目标的坐标为(x_s，y_s，z_s)，辅站的坐标为(x_1s，y_1s，z_1s)，由中的目标与主站和辅站的位置关系可得到：

假设主站、辅站和大地直角坐标系下目标的位置分别为S₀(X₀,Y₀,Z₀)、S₁(X₁,Y₁,Z₁)、S(X,Y,Z)，在主站机载坐标系S₀xyz下，目标的位置为S'(x_s,y_s,z_s)，辅站的为S₁'(x_1s,y_1s,z_1s)，其中主站和辅站在大地直角坐标系下的位置为已知量，在主站机载坐标系下，测得目标相对主站的方位角为α，俯仰角为β，主站和辅站接受到目标信号的时间差为Δt，可将时间差转化为目标到达两个观测站的距离差Δr(Δr＝cΔt＝r₁-r₀)；

在主站机载坐标系下可将目标的位置表示为：

如能求得主站到目标的距离r₀，即可得到目标在主站机载坐标系下的坐标，再根据主站无人机此时的姿态信息得到转换矩阵Q₁，可将主站机载坐标系下的目标位置坐标变换到到主站测量直角坐标系下；最后根据主站在大地直角坐标系下和大地坐标系下的坐标(X₀，Y₀，Z₀)和(L₀，B₀，H₀)得到转换矩阵Q₂，经过坐标系变换可以得到目标在大地直角坐标系下的位置如下：

式(8)中，(X，Y，Z)为目标在大地直角坐标系下的坐标，和为坐标系的变换矩阵，(X₀，Y₀，Z₀)为主站在大地直角坐标系下的坐标。

优选的，目标到主观测站的距离r₀为待求得的未知量，其具体算法为：

由式(6)可得

r₁ ²-r₀ ²＝x_1s ²+y_1s ²+z_1s ²-2(x_1sx_s+y_1sy_s+z_1sz_s) (9)

由Δr＝r₁-r₀可得

Δr²＝r₁ ²-r₀ ²-2Δrr₀ (10)

将式(9)与式(10)联立消掉r₁ ²-r₀ ²后，可得

2Δrr₀+2(x_1sx_s+y_1sy_s+z_1sz_s)＝x_1s ²+y_1s ²+z_1s ²-Δr² (11)

将式(7)中主站机载坐标系下目标的位置S'(x_s,y_s,z_s)带入式(11)中，可得

根据大地直角坐标系与观测站测量直角坐标系的矩阵变换关系、观测站直角坐标系与机载坐标系的矩阵变换关系，可将辅站在大地直角坐标系下的坐标变换到主站的机载坐标系下，因此可以得到如下关系：

其中：L₀、B₀分别为主站无人机此时的的经度和纬度；由主站无人机姿态角信息可以得到

因为Q₁与Q₂两个矩阵满足其中E为单位矩阵，所以可得：

由式(12)、(13)、(14)即可得到目标到主观测站的距离r₀为

其中：S₁'(x_1s,y_1s,z_1s)可由式(13)得到；

根据得到的目标到主观测站的距离r₀以及目标的方位角α、俯仰角β，可以计算在主站机载坐标系下的目标位置坐标，如式(7)所示；再根据式(8)将目标的坐标从主站机载坐标系变换到大地直角坐标系中，得到目标的位置S(X,Y,Z)。

本发明的有益效果为：本发明主要用于无人机对目标进行无源定位，通过两个无人机对目标进行测向时差定位，不仅解决了传统测向时差定位解算方法的定位模糊问题，而且将无人机自身姿态参数引入到目标位置解算方程中；针对无人机观测站位置状态不断变化的特点，将无人机姿态变化与目标位置建立了直接联系。本发明的方法通过坐标系的转换，将无人机自身观测坐标系转换成大地直角坐标系，实现无人机对目标进行测向时差定位的位置解算，并建立了目标位置与无人机姿态之间的直接联系。在分析无人机定位精度时，可以定量分析无人机姿态误差的影响，为进一步提高无人机定位精度提供理论依据，也为无人机姿态传感器的选取提供理论依据。

附图说明

图1是本发明的大地直角坐标系与测量直角坐标系的位置关系示意图。

图2是本发明的双无人机测向时差定位模型示意图。

图3是本发明的主站机载坐标系下目标与无人机观测站位置关系示意图。

图4是本发明的理论测向时差定位GDOP分布图。

图5是本发明的双无人机测向时差定位GDOP分布图。

图6是本发明的双无人机测向时差定位与理论测向时差定位误差对比曲线图。

图7是本发明的引入姿态角误差0.2°后无人机测向时差定位GDOP分布图。

具体实施方式

如图1和图2所示，需要将主站机载坐标系下的目标位置坐标变换到大地直角坐标系下，这里主要用到大地直角坐标OXYZ，原点为地球椭球的球心；测量直角坐标系oxyz，原点为观测站中心；主站机载坐标系S₀xyz，原点为载机中心。下面给出相关坐标系之间变换的表达式。

大地直角坐标系与测量直角坐标系的矩阵关系式，假设目标在大地直角坐标系下和测量直角坐标系下的坐标分别为(X，Y，Z)和(x，y，z)，观测站的坐标在大地直角坐标系和大地坐标系下分别为(X₀，Y₀，Z₀)和(L₀，B₀，H₀)，则大地直角坐标系与测量直角坐标系的矩阵表达式如下：

式中：

观测站的测量直角坐标系与机载坐标系的矩阵关系式，假设目标在机载坐标系下和观测站测量直角坐标系下的坐标分别为(x_b，y_b，z_b)和(x，y，z)。由于建立的观测站的测量直角坐标系与机载坐标系的坐标原点重合，而且都为观测站的质心，因此目标在观测站的观测直角坐标系与机载坐标系的位置关系如下：

式中：

其中，δ_a、δ_b、δ_c为载机的姿态角(横滚角、偏航角以及俯仰角)。

如图2所示，假设在主站的机载坐标系中，目标的坐标为(x_s，y_s，z_s)，辅站的坐标为(x_1s，y_1s，z_1s)。由中的目标与主站和辅站的位置关系可得到：

假设主站、辅站和大地直角坐标系下目标的位置分别为S₀(X₀,Y₀,Z₀)、S₁(X₁,Y₁,Z₁)、S(X,Y,Z)。如图3所示，在主站机载坐标系S₀xyz下，目标的位置为S'(x_s,y_s,z_s)，辅站的为S₁'(x_1s,y_1s,z_1s)，其中主站和辅站在大地直角坐标系下的位置为已知量。在主站机载坐标系下，测得目标相对主站的方位角为α，俯仰角为β，主站和辅站接受到目标信号的时间差为Δt，可将时间差转化为目标到达两个观测站的距离差Δr(Δr＝cΔt＝r₁-r₀)。

由图3中的目标与主站的位置关系，在主站机载坐标系下可将目标的位置表示为：

由式(7)可知，如能求得主站到目标的距离r₀，即可得到目标在主站机载坐标系下的坐标，再根据主站无人机此时的姿态信息得到转换矩阵Q₁，可将主站机载坐标系下的目标位置坐标变换到到主站测量直角坐标系下。最后根据主站在大地直角坐标系下和大地坐标系下的坐标(X₀，Y₀，Z₀)和(L₀，B₀，H₀)得到转换矩阵Q₂，经过坐标系变换可以得到目标在大地直角坐标系下的位置如下：

式(8)中，(X，Y，Z)为目标在大地直角坐标系下的坐标，和为坐标系的变换矩阵，(X₀，Y₀，Z₀)为主站在大地直角坐标系下的坐标。其中目标到主观测站的距离r₀为待求得的未知量，因此下面需要对r₀进行求解。

由式(6)可得

r₁ ²-r₀ ²＝x_1s ²+y_1s ²+z_1s ²-2(x_1sx_s+y_1sy_s+z_1sz_s) (9)

由Δr＝r₁-r₀可得

Δr²＝r₁ ²-r₀ ²-2Δrr₀ (10)

将式(9)与式(10)联立消掉r₁ ²-r₀ ²后，可得

2Δrr₀+2(x_1sx_s+y_1sy_s+z_1sz_s)＝x_1s ²+y_1s ²+z_1s ²-Δr² (11)

将式(7)中主站机载坐标系下目标的位置S'(x_s,y_s,z_s)带入式(11)中，可得

根据大地直角坐标系与观测站测量直角坐标系的矩阵变换关系、观测站直角坐标系与机载坐标系的矩阵变换关系，可将辅站在大地直角坐标系下的坐标变换到主站的机载坐标系下，因此可以得到如下关系：

其中：L₀、B₀分别为主站无人机此时的的经度和纬度；由主站无人机姿态角信息可以得到

又因为Q₁与Q₂两个矩阵满足其中E为单位矩阵，所以可得

由式(12)、(13)、(14)即可得到目标到主观测站的距离r₀为

其中：S₁'(x_1s,y_1s,z_1s)可由式(13)得到。

根据得到的目标到主观测站的距离r₀以及目标的方位角α、俯仰角β，可以计算在主站机载坐标系下的目标位置坐标，如式(7)所示。再根据式(8)将目标的坐标从主站机载坐标系变换到大地直角坐标系中，得到目标的位置S(X,Y,Z)。

本发明不同于常规测向时差定位方法，对所测的目标进行坐标变换，将无人机自身观测坐标系转变为大地直角坐标系，建立目标与姿态角的直接联系，实现无人机对目标的位置解算。而寻常测向时差定位方法，将观测站看作一个质点，无法考虑无人机自身姿态角所带来的影响。相比之下，本发明用在无人机定位精度的时候，不仅可以定量分析无人机姿态误差的影响，也能给无人机姿态传感器的选取提供有效的理论依据，同时为进一步提高无人机定位精度提供了理论支持。

为了验证无人机作为观测站对目标进行测向时差定位的解算算法正确性，将其定位误差与测向时差理论定位误差的分布进行对比分析。根据双无人机测向时差定位解算算法，进行多次仿真得到目标的定位误差，表达式如下：

式中，N为仿真次数，(x,y,z)为目标位置的真实值，为第i次的目标估计值。

仿真条件设定如下：站址误差σ_s＝0m，测角误差σ_α＝σ_β＝0.1°，测时误差σ_Δt＝100ns，主站位置(-5,0,10)km，辅站位置(5,0,10)km。对x,y方向均为-20km～20km，z＝0km的区域内目标定位。仿真次数N＝1000。

图4为理论测向时差定位的GDOP分布图，图5为双无人机测向时差定位的GDOP分布图，其观测区域都为x∈[-20 20]km，y∈[-20 20]km，z＝0km内，且两者都只考虑了测角误差和测时误差。分布图反映的是，在指定观测区域内误差的等高图，图中的每条的曲线是指对应误差值分布的位置。从两幅分布图中可看出，在测量参数存在相同的误差时，双无人机测向时差定位解算算法对目标位置的定位误差与理论上的定位误差一致，说明所提出的双无人机测向时差定位解算算法可以对目标位置进行解算。

图6为观测区域x∈[-20 20]km，y＝0km，z＝0km内，双无人机测向时差定位与理论测向时差定位的定位误差对比曲线图。从对比曲线进一步反映出，观测量存在一定误差时，双无人机测向时差定位解算算法的定位误差与理论上出现的定位误差一致。因此，通过仿真验证了双无人机测向时差定位解算算法的有效性。

与一般的地面观测站不同，无人机作为观测站对目标进行测向时差定位时，无人机的姿态信息也会在目标定位中考虑进去，因此可以分析姿态角的观测误差对目标定位带来的误差。通过仿真验证姿态角的观测误差对目标定位产生影响。

图7为加入一定的姿态角测量误差后，通过蒙特卡洛仿真得到定位误差GDOP分布图，仿***要参数为：测角误差σ_α＝σ_β＝0.1°，站址误差σ_s＝0m，测时误差σ_Δt＝100ns，主站坐标(-5,0,10)km,辅站坐标(5,0,10)km，其观测区域为x∈[-20 20]km，y∈[-20 20]km，z＝0km，此外，加入了0.2°的姿态角测量误差。

对比图5和图7中数值为0.1km的等高线可以看出，图5与图6相比误差为0.1km等高线所含区域在缩小，说明加入姿态角观测误差后会增大定位误差。因此验证了姿态角误差会对定位误差产生影响。

综合以上分析所述，本发明的方法可以实现无人机对目标进行测向时差定位的位置解算，并建立了目标位置与无人机姿态之间的直接联系。

尽管本发明就优选实施方式进行了示意和描述，但本领域的技术人员应当理解，只要不超出本发明的权利要求所限定的范围，可以对本发明进行各种变化和修改。

Claims (4)

1.一种考虑无人机姿态角的双无人机测向时差定位方法，其特征在于，包括如下步骤：

(1)在主站无人机的机载坐标系下测得目标信号的方位角、俯仰角和目标信号到达主站与辅站的时间差；

(2)根据步骤(1)测得的数据解算出目标与主站无人机的距离r₀；

(3)根据目标与主站无人机的距离、主站无人机测得目标信号的方位角和俯仰角解算出目标在主站无人机机载坐标系下的坐标；

(4)通过相关坐标系的坐标变换关系得到目标在大地直角坐标系下的坐标。

2.如权利要求1所述的考虑无人机姿态角的双无人机测向时差定位方法，其特征在于，大地直角坐标OXYZ，原点为地球椭球的球心；测量直角坐标系oxyz，原点为观测站中心；主站机载坐标系S₀xyz，原点为载机中心；

大地直角坐标系与测量直角坐标系的矩阵关系式具体为：假设目标在大地直角坐标系下和测量直角坐标系下的坐标分别为(X，Y，Z)和(x，y，z)，观测站的坐标在大地直角坐标系和大地坐标系下分别为(X₀，Y₀，Z₀)和(L₀，B₀，H₀)，则大地直角坐标系与测量直角坐标系的矩阵表达式如下：

式中：

观测站的测量直角坐标系与机载坐标系的矩阵关系式具体为：假设目标在机载坐标系下和观测站测量直角坐标系下的坐标分别为(x_b，y_b，z_b)和(x，y，z)，由于建立的观测站的测量直角坐标系与机载坐标系的坐标原点重合，而且都为观测站的质心，因此目标在观测站的观测直角坐标系与机载坐标系的位置关系如下：

式中：

其中，δ_a、δ_b、δ_c为载机的姿态角(横滚角、偏航角以及俯仰角)。

3.如权利要求1所述的考虑无人机姿态角的双无人机测向时差定位方法，其特征在于，定位方法具体过程为：假设在主站的机载坐标系中，目标的坐标为(x_s，y_s，z_s)，辅站的坐标为(x_1s，y_1s，z_1s)，由中的目标与主站和辅站的位置关系可得到：

假设主站、辅站和大地直角坐标系下目标的位置分别为S₀(X₀,Y₀,Z₀)、S₁(X₁,Y₁,Z₁)、S(X,Y,Z)，在主站机载坐标系S₀xyz下，目标的位置为S'(x_s,y_s,z_s)，辅站的为S₁'(x_1s,y_1s,z_1s)，其中主站和辅站在大地直角坐标系下的位置为已知量，在主站机载坐标系下，测得目标相对主站的方位角为α，俯仰角为β，主站和辅站接受到目标信号的时间差为Δt，可将时间差转化为目标到达两个观测站的距离差Δr(Δr＝cΔt＝r₁-r₀)；

在主站机载坐标系下可将目标的位置表示为：

如能求得主站到目标的距离r₀，即可得到目标在主站机载坐标系下的坐标，再根据主站无人机此时的姿态信息得到转换矩阵Q₁，可将主站机载坐标系下的目标位置坐标变换到到主站测量直角坐标系下；最后根据主站在大地直角坐标系下和大地坐标系下的坐标(X₀，Y₀，Z₀)和(L₀，B₀，H₀)得到转换矩阵Q₂，经过坐标系变换可以得到目标在大地直角坐标系下的位置如下：

式(8)中，(X，Y，Z)为目标在大地直角坐标系下的坐标，和为坐标系的变换矩阵，(X₀，Y₀，Z₀)为主站在大地直角坐标系下的坐标。

4.如权利要求1所述的考虑无人机姿态角的双无人机测向时差定位方法，其特征在于，目标到主观测站的距离r₀为待求得的未知量，求解r₀的具体算法为：

由式(6)可得

r₁ ²-r₀ ²＝x_1s ²+y_1s ²+z_1s ²-2(x_1sx_s+y_1sy_s+z_1sz_s) (9)

由Δr＝r₁-r₀可得

Δr²＝r₁ ²-r₀ ²-2Δrr₀ (10)

将式(9)与式(10)联立消掉r₁ ²-r₀ ²后，可得

2Δrr₀+2(x_1sx_s+y_1sy_s+z_1sz_s)＝x_1s ²+y_1s ²+z_1s ²-Δr² (11)

将式(7)中主站机载坐标系下目标的位置S'(x_s,y_s,z_s)带入式(11)中，可得

根据大地直角坐标系与观测站测量直角坐标系的矩阵变换关系、观测站直角坐标系与机载坐标系的矩阵变换关系，可将辅站在大地直角坐标系下的坐标变换到主站的机载坐标系下，因此可以得到如下关系：

其中：L₀、B₀分别为主站无人机此时的的经度和纬度；由主站无人机姿态角信息可以得到

因为Q₁与Q₂两个矩阵满足其中E为单位矩阵，所以可得：

由式(12)、(13)、(14)即可得到目标到主观测站的距离r₀为

其中：S₁'(x_1s,y_1s,z_1s)可由式(13)得到；

根据得到的目标到主观测站的距离r₀以及目标的方位角α、俯仰角β，可以计算在主站机载坐标系下的目标位置坐标，如式(7)所示；再根据式(8)将目标的坐标从主站机载坐标系变换到大地直角坐标系中，得到目标的位置S(X,Y,Z)。