ABSTRACT

For the TDOA positioning method, different algorithms determine different positioning accuracy, and the traditional Taylor series expansion method has a strong dependence on the suitability of the initial value selection. In order to improve the positioning accuracy, this paper proposes a method to correct the initial position error by introducing the Lagrange constraint factor on the basis of Taylor-series estimation. Simulation experiment I shows that under the same simulation conditions, the optimal positioning accuracy of the traditional Taylor series expansion method is lower than that of the Chan algorithm. Simulation experiment II shows that when the number of satellite observation platform networks is 4 and 6 respectively, the positioning accuracy of the modified algorithm proposed in this paper is better than the traditional Chan algorithm and Taylor algorithm.

Keywords:TDOA, Constraint Factor, Taylor-Series Estimation

基于Lagrange约束因子修正迭代初值的TDOA定位算法

苏煜琴，付小宁，张娜

西安电子科技大学，陕西 西安

收稿日期：2020年2月29日；录用日期：2020年3月13日；发布日期：2020年3月20日

摘 要

对于TDOA定位法，不同的解算算法决定了不同的定位精度，传统的Taylor级数展开法对于初值选取的合适与否依赖性较强。为提高定位精度，本文在Taylor级数展开法的基础上提出了一种引入Lagrange约束因子修正初始位置误差的解算方法。仿真实验I表明，在同等仿真条件下，传统Taylor级数展开法的最优定位精度次于Chan算法的最优定位精度。仿真实验II表明，在星载观测平台组网个数分别为4、6时，本文提出的修正算法定位精度都优于传统的Chan算法、Taylor算法。

关键词 :TDOA，约束因子，Taylor级数展开法

Copyright © 2020 by author(s) and Hans Publishers Inc.

This work is licensed under the Creative Commons Attribution International License (CC BY 4.0).

http://creativecommons.org/licenses/by/4.0/

1. 引言

近年来，随着通信技术和电磁信号等科学技术的发展与研究，资源争夺的领地已经不仅仅局限于海洋与陆地，如何掌握空中制导权已经成了各国军备竞赛的重要争夺单元。基于单星载观测平台的定位系统虽然机动性能好，但定位精度往往达不到1 km [1]。增加观测平台可以得到更多的定位参数信息，如到达时间差(Time Difference of Arrival, TDOA) [2] 、到达频率、到达频率差(Frequency Difference of Arrival, FDOA) [3] 、多普勒频率和多普勒频率变化率 [4] 等，其中TDOA定位法是使用最广泛的定位算法。

TDOA定位方程的解算方式与定位精度有直接关系 [5]。最早的定位算法是1976年Foy等人提出的Taylor级数展开法 [6]，本质上是一种迭代算法，类似的，还有利用Newton迭代法进行位置求解，迭代算法的优点是定位精度高 [7]，但是缺点迭代算法计算复杂，且需要选取合适的目标位置初始值，因此如何优化上述算法十分有研究意义。除了迭代法，还有以Y.T. Chan提出的Chan算法 [8] 为代表的解析法。此外，还有一种闭式解法思想，代表算法有球面相交法(SX) [9] 、平面相交法(PX) [10] 和球面插值法(SI) [11]，尽管这些算法都可以求得目标的位置解，但是闭式解法有一个共同的缺点，定位性能无法达到克拉美罗下界 [12]。

2. TDOA定位原理

到达时间差(Time Difference of Arrival, TDOA)定位法，简称时差定位法。目标到达两个星载观测站的时间先后不同，故而可以得到一个时间差，利用速度距离公式可以确定一个双曲面，空间中三组双曲面相交可以得到一个交点，此交点即为近地辐射源目标。获得三组双曲面，至少需要三个星载观测平台，因此时差定位法对三维空间目标进行定位至少需要三个或三个以上的星载平台来组成定位网络。定位原理图如图1。

假设在地心固定坐标系描述下辐射源目标为 $u={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^{\text{T}}$，$S_i,i=1,2,\cdots ,N$ 均为星载观测平台，为了方便描述，以 $S_1$ 为主观测平台，其余观测平台为辅助观测平台，三维坐标表示为 $S_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^{\text{T}}$。

以星载观测平台组网个数是四个为例，根据几何关系有：

图1. 三维TDOA定位原理示意图

$\{\begin{array}{l}{r}_1^2={\left(x-x_1\right)}^2+{\left(y-y_1\right)}^2+{\left(z-z_1\right)}^2\hfill \\ r_2^2={\left(x-x_2\right)}^2+{\left(y-y_2\right)}^2+{\left(z-z_2\right)}^2\hfill \\ r_3^2={\left(x-x_3\right)}^2+{\left(y-y_3\right)}^2+{\left(z-z_3\right)}^2\hfill \\ r_4^2={\left(x-x_4\right)}^2+{\left(y-y_4\right)}^2+{\left(z-z_4\right)}^2\hfill \end{array}$ (1)

将速度距离公式带入式(1)有：

$\{\begin{array}{l}\Delta r_1=r_2-r_1=c\Delta t_1\hfill \\ \Delta r_2=r_3-r_1=c\Delta t_2\hfill \\ \Delta r_3=r_4-r_1=c\Delta t_3\hfill \end{array}$ (2)

整理式(1)、式(2)得：

$\{\begin{array}{l}\left(x_1-x_2\right)x+\left(y_1-y_2\right)y+\left(z_1-z_2\right)z=k_1+r_1\Delta r_1\hfill \\ \left(x_1-x_3\right)x+\left(y_1-y_3\right)y+\left(z_1-z_3\right)z=k_2+r_2\Delta r_2\hfill \\ \left(x_1-x_4\right)x+\left(y_1-y_4\right)y+\left(z_1-z_4\right)z=k_3+r_3\Delta r_3\hfill \end{array}$ (3)

其中： $k_i=\frac{1}{2}\left[\Delta r_i{}^2+\left(x_1^2+y_1^2+z_1^2\right)-\left(x_i^2+y_i^2+z_i^2\right)\right]$，$i=1,2,3$。

解方程组式(3)即可得到近地辐射源的坐标。

3. Chan算法

1994年学者Y.T. Chan提出了一种本质并非迭代法的非线性方程求解算法，该算法本质是解析式法，并且不需要设置初值，计算量也大大降低，于是该算法被命名为Chan算法。

该算法将辐射源目标与星载观测平台之间的距离视为常量，则式(3)可以写为：

$AX=F$ (4)

其中： $A=\left[\begin{array}{ccc}{x}_2-x_1& y_2-y_1& z_2-z_1\\ x_3-x_1& y_3-y_1& z_3-z_1\\ x_4-x_1& y_3-y_1& z_3-z_1\end{array}\right]$，$X=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$，$F=\left[\begin{array}{c}{k}_1+r_1\Delta r_1\\ k_2+r_2\Delta r_2\\ k_3+r_3\Delta r_3\end{array}\right]$。

假设四个星载观测平台呈不规则分布，不会处于同一水平线上，则有 $\det A\ne 0$，此时辐射源目标的位置的最小二乘解可以由伪逆法求得：

$\hat{X}={\left(A^{\text{T}}A\right)}^{-1}{A}^{\text{T}}F$ (5)

记式(8)中的

${\left(A^{\text{T}}A\right)}^{-1}{A}^{\text{T}}={\left[a_{ij}\right]}_{3\times 3}=\left[\begin{array}{ccc}{a}_{11}& a_{12}& a_{13}\\ a_{21}& a_{22}& a_{23}\\ a_{31}& a_{31}& a_{33}\end{array}\right]$ (6)

联立式(8)与式(9)可得近地辐射源目标位置的估计值：

$\hat{X}=\left[\begin{array}{c}\hat{x}\\ \hat{y}\\ \hat{z}\end{array}\right]=\left[\begin{array}{c}{m}_1+n_1{r}_1\\ m_2+n_2{r}_2\\ m_3+n_3{r}_3\end{array}\right]$ (7)

其中： $\{\begin{array}{l}{m}_i={\displaystyle \underset{j=1}{\overset{3}{\sum }}{a}_{ij}{k}_j}\hfill \\ m_i={\displaystyle \underset{j=1}{\overset{3}{\sum }}{a}_{ij}\Delta r_j}\hfill \end{array},i=1,2,3$。

将式(10)带入式(1)得：

$a{r}_1{}^2+2b{r}_1+c=0$ (8)

其中： $\{\begin{array}{l}a=n_1^2+n_2^2+n_3^2-1\hfill \\ b=\left(m_1-x_1\right)n_1+\left(m_2-y_1\right)n_2+\left(m_3-z_1\right)n_3\hfill \\ c={\left(m_1-x_1\right)}^2+{\left(m_2-y_1\right)}^2+{\left(m_3-z_1\right)}^2\hfill \end{array}$。

此时求解辐射源目标的位置解从求解复杂的非线性方程转化为求解一元二次方程，大大降低了计算复杂度，求解过程有以下三种情况：

1) 当判别式 $\Delta >0$ 时，式(11)有两解，此时出现了时差定位模糊现象；

2) 当判别式 $\Delta =0$ 时，式(11)有一解，此时的解便是辐射源目标的位置解，没有出现时差定位模糊现象；

3) 当判别式 $\Delta <0$ 时，式(11)无解，时差定位曲线无交点，定位失效。

当判别式大于等于零时，对该一元二次方程进行求解便得到近地辐射源目标的位置解：

$r_1=\frac{-b\pm \sqrt{b^2-ac}}{a}$ (9)

将式(12)与式(1)、(2)联立即可求得近地辐射源目标的位置。

以上便是Chan算法的解算步骤。但是由以上分析可知Chan算法有可能出现定位模糊甚至无解的情况，此时TDOA定位法的定位结果将受较大影响。

4. Taylor级数展开法

Taylor级数展开法本质上是一种迭代算法，核心思想 [13] 是：将TDOA定位方程按照Taylor级数终值定理展开，并忽略二阶及二阶以上的分量，在开始迭代前为目标位置赋予初值，之后在此基础上进行迭代，在每一次的迭代过程中通过局部最小二乘解(Least Squares, LS)逐渐收敛于辐射源目标位置解。

1) 根据TDOA定位原理，将迭代初始值设为 $X^{\prime }={\left(x^{\prime },y^{\prime },z^{\prime }\right)}^{\text{T}}$，与真实位置的测量误差记为 $\delta ={\left(\Delta x,\Delta y,\Delta y\right)}^{\text{T}}$，则有如下关系：

$X=X^{\prime }+\delta$ (10)

2) 在迭代初始值处按照Taylor级数展开，去除二阶以上高阶分量的同时仅保留一阶分量，得到如下表达式：

$e\approx H-G\delta$ (11)

其中： $e={\left[e_1,e_2,e_3\right]}^{\text{T}}$ 为TDOA定位法中时间测量误差矩阵； $H=\left[\begin{array}{c}c\Delta t_1-\left({r^{\prime }}_2-{r^{\prime }}_1\right)\\ c\Delta t_2-\left({r^{\prime }}_3-{r^{\prime }}_1\right)\\ c\Delta t_3-\left({r^{\prime }}_4-{r^{\prime }}_1\right)\end{array}\right]$ ；

$G=\left[\begin{array}{ccc}{\frac{\partial \Delta r_2}{\delta x^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}& {\frac{\partial \Delta r_2}{\delta y^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}& {\frac{\partial \Delta r_2}{\delta z^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}\\ {\frac{\partial \Delta r_3}{\delta x^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}& {\frac{\partial \Delta r_3}{\delta y^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}& {\frac{\partial \Delta r_3}{\delta z^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}\\ {\frac{\partial \Delta r_4}{\delta x^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}& {\frac{\partial \Delta r_4}{\delta y^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}& {\frac{\partial \Delta r_4}{\delta z^{\prime }}|}_{\left(x^{\prime },y^{\prime },z^{\prime }\right)}\end{array}\right]=\left[\begin{array}{ccc}\frac{x_1-x^{\prime }}{r_1}-\frac{x_2-x^{\prime }}{r_2}& \frac{y_1-y^{\prime }}{r_1}-\frac{y_2-x^{\prime }}{r_2}& \frac{z_1-z^{\prime }}{r_1}-\frac{z_2-z^{\prime }}{r_2}\\ \frac{x_1-x^{\prime }}{r_1}-\frac{x_3-x^{\prime }}{r_3}& \frac{y_1-y^{\prime }}{r_1}-\frac{y_3-x^{\prime }}{r_3}& \frac{z_1-z^{\prime }}{r_1}-\frac{z_3-z^{\prime }}{r_3}\\ \frac{x_1-x^{\prime }}{r_1}-\frac{x_4-x^{\prime }}{r_4}& \frac{y_1-y^{\prime }}{r_1}-\frac{y_4-x^{\prime }}{r_4}& \frac{z_1-z^{\prime }}{r_1}-\frac{z_4-z^{\prime }}{r_4}\end{array}\right].$

3) 用加权最小二乘法得出初始位置处的最大似然估计：

$\delta ={\left[\begin{array}{ccc}\Delta x& \Delta y& \Delta z\end{array}\right]}^{\text{T}}={\left(G^{-1}{Q}^{-1}G\right)}^{-1}{G}^{\text{T}}{Q}^{-1}H$ (12)

其中：Q为TDOA定位法测量误差的协方差矩阵。

4) 设定一个门限精度 ${\epsilon }_t$ ：

如果 $\left|\Delta x\right|+\left|\Delta y\right|+\left|\Delta z\right|<\epsilon$，则终止迭代过程，此时可以得出近地辐射源目标最终位置估计值：

$\{\begin{array}{l}\hat{x}=x^{\prime }+\Delta x\hfill \\ \hat{y}=y^{\prime }+\Delta y\hfill \\ \hat{z}=z^{\prime }+\Delta z\hfill \end{array}$ (13)

如果 ，则继续执行步骤5)。

5) 用最新得到估计值的估计值替代上一个估计值，并重复步骤3)和步骤4)。

以上便是Taylor级数展开法的解算步骤。

5. 基于Lagrange约束因子的修正算法

Chan算法计算简单但是有时差模糊现象，该现象严重可导致系统定位功能失效；Taylor算法尽管计算复杂，但每次都能得到确定的位置解，此外初值选取的合适与否对定位结果有直接的影响。随着计算机计算速度的加快，复杂的计算已经变得不再复杂，为此提出一种基于Lagrange约束因子的解算方法。

式(3)转化为矩阵表达形式为：

$AX=b$ (14)

其中： $A=\left[\begin{array}{llll}{x}_1-x_2\hfill & y_1-y_2\hfill & z_1-z_2\hfill & -\Delta r_1\hfill \\ x_1-x_3\hfill & y_1-y_3\hfill & z_1-z_3\hfill & -\Delta r_2\hfill \\ x_1-x_4\hfill & y_1-y_4\hfill & z_1-z_4\hfill & -\Delta r_3\hfill \end{array}\right]$ ；

$X={\left[\begin{array}{cccc}x& y& z& r_1\end{array}\right]}^{\text{T}}$ ；

$b={\left[\begin{array}{ccc}{k}_1& k_2& k_3\end{array}\right]}^{\text{T}}$。

考虑到系统总含有观测噪声及测量误差，故在计算过程加入误差干扰项，用最小二乘法求得位置估计值为：

$\hat{X}={\left(A^{\text{T}}A-{\sigma }^{2}I\right)}^{-1}{A}^{\text{T}}b=X+\Delta X$ (15)

其中： $\sigma$ 是矩阵 $\left[\begin{array}{cc}b& -A\end{array}\right]$ 的最小奇异值；I为单位阵； $\Delta X$ 为误差项。

为了计算方便，将主观测平台所在的位置设为坐标原点，故式(1)的第一行可简化为：

$r_1^2=x^2+y^2+z^2$ (16)

进一步转化为矩阵表达形式为：

$X^{\text{T}}gX=0$ (17)

其中： $g=diag\left(1,1,1,-1\right)$。

至此，引入Lagrange数乘因子 $\lambda$，Lagrange数乘算法 [14] 的思想由Joseph Lagrange在1971年首次提出，用来解决数学解算过程中的最优问题。利用该思想，得到以下约束表达式：

$L\left(X,\lambda \right)={\left(AX-b\right)}^{\text{T}}\left(AX-b\right)+\lambda X^{\text{T}}gX$ (18)

对 $L\left(X,\lambda \right)$ 求一阶偏导数并令其为零：

$\frac{\partial L\left(X,\lambda \right)}{\partial X}={\left(A^{\text{T}}A+\lambda g\right)}^{-1}-A^{\text{T}}b=0$ (19)

解上述方程，得：

$X={\left(A^{\text{T}}A+\lambda g\right)}^{-1}{A}^{\text{T}}b$ (20)

联立式(20)和式(23)，有：

(21)

为了求解上式，先令 ${\left(A^{\text{T}}A\right)}^{-1}g=U\Lambda U^{-1}$，其中 $\Lambda =diag\left({\lambda }_1,{\lambda }_2,{\lambda }_3,{\lambda }_4\right)$，${\lambda }_i$ 为矩阵 ${\left(A^{\text{T}}A\right)}^{-1}g$ 的特征值，所以有：

${\left(A^{\text{T}}A+\lambda g\right)}^{-1}=U{\left(I+\lambda \Lambda \right)}^{-1}{U}^{-1}{\left(A^{\text{T}}A\right)}^{-1}$ (22)

将式(25)带入式(23)整理为矩阵表达形式为：

$E^{\text{T}}{\left(I+\lambda \Lambda \right)}^{-1}\Lambda {\left(I+\lambda \Lambda \right)}^{-1}h=0$ (23)

其中： $E^{\text{T}}=b^{\text{T}}AU=\left[\begin{array}{cccc}{e}_1& e_2& e_3& e_4\end{array}\right]$ ； $h=U^{-1}{\left(A^{\text{T}}A\right)}^{-1}{A}^{\text{T}}U={\left[\begin{array}{cccc}{h}_1& h_2& h_3& h_4\end{array}\right]}^{\text{T}}$。

将矩阵表达形式整理为代数表达形式为：

${\displaystyle \underset{i=1}{\overset{4}{\sum }}\frac{e_i{\lambda }_i{h}_i}{{\left(\lambda {\lambda }_i+1\right)}^2}}=0$ (24)

解该代数方程便可以得到Lagrange数乘因子 $\lambda$，将其带入式(23)便可以得到位置解X。

根据位置解X得到误差修正项 $\Delta X$ 的表达式：

$\Delta X={\left(A^{\text{T}}A-{\sigma }^{2}I\right)}^{-1}{A}^{\text{T}}b+\lambda \left(A^{\text{T}}A\right)gX-A^{-1}b$ (25)

最终得到修正后的位置：

$\hat{X}={\left(A^{\text{T}}A-{\sigma }^{2}I\right)}^{-1}{A}^{\text{T}}b+\left[\lambda \left(A^{\text{T}}A\right)g+I\right]X-A^{-1}b$ (26)

将修正后的位置作为Taylor展开时的初始值，之后进行Taylor级数展开法，当满足精度要求终止迭代时，本文提出的基于Lagrange约束因子的解算方法解算完成，算法流程图如图2。

图2. 基于Lagrange约束因子修正算法原理图

6. 实验结果及分析

实验I：星载观测平台数量对不同定位算法精度的影响

各星载观测平台的位置坐标如表1所示，以星载观测平台1的位置为坐标原点，近地辐射源目标的位置为 $\left(0.702,70.342,80.867\right)\text{km}$。TDOA测量误差服从理想高斯噪声环境。

定位精度用均方根误差来评判，计算公式为：

$RMSE\left(X\right)=\sqrt{E\left[{\Vert X-\hat{X}\Vert }_2^2\right]}=\sqrt{{\displaystyle \underset{i=1}{\overset{T_s}{\sum }}{\left(X_i-\hat{X}\right)}^2}/T_s}$ (27)

其中：X为第i次估计的近地辐射源目标位置； $\hat{X}$ 为近地辐射源目标的真实位置； $T_s$ 为采样次数。

由图3、图4可知，Chan算法定位精度比Taylor级数展开法对定位系统中星载观测平台的个数更加敏感，尽管如此Chan算法也可以随着星载观测平台的个数增加得到更好的定位精度，但是当参与定位的星载观测平台个数达到5个及以上时，对定位精度改善不大。Taylor级数展开法的定位精度受星载观测平台的个数的影响不大，当个数达到四个或以上时，定位精度达到最优，并很难随着观测平台的增加而明显提高。就总体而言，Taylor级数展开法的最优定位精度次于Chan算法的最优定位精度。

表1. 星载观测平台的位置分布

图3. 不同星载观测平台下Chan算法的性能

图4. 不同星载观测平台下Taylor算法的性能

实验II：不同定位算法的性能比较

基于实验I的仿真结果，实验II选择星载观测平台的个数分别为4、6，星载观测平台的位置同实验I。TDOA测量误差服从理想高斯噪声环境。

定位精度用均方误差来评判，计算公式为：

$MSE\left(X\right)=E\left[{\Vert X-\hat{X}\Vert }_2^2\right]={\displaystyle \underset{i=1}{\overset{T_s}{\sum }}{\left(X_i-\hat{X}\right)}^2}/T_s$ (28)

图5. 四个星载观测平台下三种定位算法性能比较

图6. 六个星载观测平台下三种定位算法性能比较

图5、图6分别给出了同等仿真条件下Taylor级数展开法、Chan算法、本文提出的解算算法在4个星载观测平台、6个星载观测平台下的仿真结果。

由图可知，无论星载观测平台的个数是多少，本文提出的解算算法定位精度最高，Chan算法次之，Taylor级数展开法的定位性能最差，这说明初值的选取对Taylor级数展开法的定位精度有直接影响。本文用Lagrange约束因子修正后的位置值作为Taylor级数展开法的迭代初始值后，定位精度有了明显的提升。

7. 结论

本文提出的基于Lagrange约束因子修正算法引入了Lagrange约束因子，利用星载观测平台位置的已知性作为约束条件，求解出辐射源目标的位置在该约束条件下的修正解，并将修正后的位置作为Taylor级数展开法中的初值。仿真结果表明，传统Taylor级数展开法的最优定位精度次于Chan算法的最优定位精度，而本文提出的Taylor级数展开法修正算法在不同观测平台个数下的定位精度都高于传统的Chan算法、Taylor算法的定位精度。

但是本文仅在高斯噪声环境中做了仿真实验，真实的测量环境的噪声十分复杂，需要进一步探究。基于此，在后续的研究中考虑引入合适的滤波器滤除噪声及误差。同时，如何降低迭代次数，使得估计值快速收敛到目标真实值也是一个重要的研究课题。

文章引用

苏煜琴,付小宁,张 娜. 基于Lagrange约束因子修正迭代初值的TDOA定位算法
TDOA Localization Algorithm Based on Lagrange Constraint Factor to Modify the Initial Value of Iteration[J]. 应用数学进展, 2020, 09(03): 372-381. https://doi.org/10.12677/AAM.2020.93045

参考文献