一种基于tdoa的快速定位方法
【专利摘要】本发明公开了一种基于TDOA的快速定位方法，其主要目的在于解决被测目标在近距、远距多场景下依靠多个信号接收器探测的时间差进行位置求解的问题。本发明的主要步骤包括：坐标与距离方程的建立、中心距离的求解、目标坐标的求解、建立距离约束和球面关系约束，求解目标坐标的修正解。本发明可以解决水下航行器被动式探测、定位问题，也可用于无线电定位、超声波定位、室内定位等其他基于TDOA的定位问题。相比于目前普遍采用的Chan算法，解决了近距、远距算法不统一，存在模糊解的问题，精度和稳定性均得到进一步提高。比较于Taylor算法，无需外部初值和递归运算，大大减小运算量，而精度和稳定性不变。
【专利说明】
一种基于TDOA的快速定位方法
技术领域
[0001] 本发明涉及空间被动定位等应用，尤其是水下导航定位领域，具体地说是涉及一 种基于TD0A的快速定位方法。
【背景技术】
[0002] 卫星导航和惯性导航日益成为民用和军用领域的主要导航方式，而卫星导航和惯 性导航的局限性也促使越来越多的其他方式的导航技术不断发展。比如水下航行器由于高 度隐蔽的特点，不能主动定位，卫星导航没法在水下进行，惯性导航长时间工作会带来误差 积累，因此需要一种可靠的，精确的水下导航系统提供水下坐标的定位。
[0003] 基本ID0A(到达时间差）的定位技术是利用声源信号到达各接收器的时间差，计算 出声源位置的一种导航方法。由于只需要被动地接收声源信号，计算过程在接收端完成，加 上水下声信号传播的高效性，因此十分适合航行器的空间定位和探测。该定位方法从最早 的无线电导航，罗兰一 C系统，到如今日益发展的室内导航系统在原理上都是如此，都需要 解决非线性方程组的求解问题。该领域中，最广为人知的是Chan算法，该算法分为两种情况 对远距和近距分别提供了计算方案，且会出现模糊解的情况。Taylor算法是目前最精准的 算法，但依赖一个初始值进行迭代，计算量大，一旦初始值误差较大，会出现发散的现象。针 对目前主流算法的不足，本发明提出了一种新的定位计算方法，统一了远近距的计算方式， 避免了模糊解的问题，更重要的是在简单的步骤下，能达到Taylor算法的定位精度。对基于 ID0A的快速被动定位技术提供了计算支撑。
【发明内容】
[0004] 发明目的：本发明提供一种基于TD0A的快速定位方法，主要在于解决被测目标在 近距、远距多场景下依靠多个信号接收器探测的时间差进行位置求解的问题。
[0005] 本发明的技术方案具体如下：
[0006] 一种基于TD0A的快速定位方法，包括以下步骤：
[0007] 步骤1:根据已知的各接收器采集的信号71(丨），1 = 0，1，2,3~1信号传播速度
[0008] C，各接收器时延协方差矩阵Q，各接收器布放坐标(Xi，yi，Zi)，i = 0，l，2,3~N， [0009]获取由目标位置发出的声源信号；
[0010]步骤2:建立关于目标位置的方程组；
[0011]步骤3:对步骤2中的方程组进行消元，变化求取仅包含未知量中心距离H)的方程 组；
[0012] 步骤4:计算中心距离r〇;
[0013] 步骤5:根据步骤4得到的中心距离计算目标位置粗估计；
[0014] 步骤6:利用步骤4、5计算出的中心距离ro和目标位置粗估计坐标建立包含约束方 程的方程组；
[0015] 步骤7:根据步骤6的包含约束方程的方程组，重新计算得到更新的目标位置。
[0016] 所述步骤1具体为：
[0017] 将待测目标作为声源发出声信号x(t)，各接收器接收到N+1个信号，记为yi(t)，i =0，1，2,3^々表示各接收器的编号，其中0号接收器为参考接收器;将其他各接收器的信号 yi(t)，i = l，2,3-_N与参考接收器的信号yQ(t)作相关运算，得到N个互相关函数RlQ( T)，i = 1，2，3…N;对RlQ( t )进行峰值监测，结合先验知识排除模糊峰干扰，得到各接收器相对于参 考接收器接收信号x(t)的时间差TlQ，i = l，2,3"_N;进一步可以得到距离差riQ = CTlQ，i = l， 2,3---N〇
[0018]所述步骤2具体为：
[0019]根据空间几何的位置关系，得到关于目标位置的方程组：
[0021]
是目标到参考接收器的距离，即为中心 距离；约:，i = 1，2，3...N是误差;Ki = xi2+yi2+zi2，i = 0，1，2，3...N;
[0022]将上式写成矩阵形式：
[0025]所述步骤3具体为：
[0026]对所述关于目标位置的方程组进行消元，消去x，y，z，得到以下形式的方程组：
[0028]其中ha人，ki，i = l，2,3^_N由矩阵行变换得到;具体计算公式为：
[0033] 这里需要注意的是，|A|乒0,即编号0，1，2,3的四个接收器不能共面;另外N彡4,即 接收器的数量至少5个，保证本算法方程有解;至此得到关于中心距离ro的方程组：
[0034] ga ? r〇 = ha7 2+07 2
[0036]所述步骤4具体为：
[0037]忽略误差?'2，则r〇的最小二乘解为4 = (ga1' ? ga)-1? fe;;
[0038]所述步骤5具体为：
[0039]将所述步骤4求得的4代入目标位置方程，得到关于x，y，z的方程组：
[0042] 利用加权最小二乘法计算出目标位置：
[0043] (x，y，z)T= (GbT ?也―1 ? Gb)-1 ? GbT ?也―1 ? hb
[0044] 其中，协方差阵it = c2 ? B ? Q ? B，
表示目标到各接收器的 距离，利用G 6 A关系计算；
[0045] 所述步骤6具体为:对坐标解(x，y，z)进行修正，采用在步骤2建立的方程组基础上 增加两个约束方程再进行加权最小二乘，一个方程为中心约束方程：
[0046] r0 = 4
[0047] 另一个方程为球面关系约束方程：
[0048] r〇2= (x-xo)2+(y-yo)2+(z-zo)2
[0049] 将上式非线性方程线性化，得到线性化后的近似方程：
[0051 ] 将上述两式和步骤2的方程组整理可得：
[0055] 利用加权最小二乘法计算出最终位置：
[0056] (x，y，z)T= (GcT ? V -1 ? Gc)-1 ? GcT ? V -1 ? he
[0058]将r〇2=(叉1〇)2+(71〇)2+(2-2()) 2非线性方程线性化，要解释这个过程，先从二维空 间说明。对于二维问题，写成ro2 = (x-xo)2+ (y-yo)2，将x，y，ro当一个三维空间，则上式为一 个锥面方程，方程组
^的加权最小二 乘解可以理解为到各平面和锥面的加权距离最小的点，所以把锥面上到粗估计点最近的切 面作为锥面的线性近似，该切面的法线向量为
，加权最小二乘解可以近 似为到各平面和切面的加权距离最小的点。现在把问题扩展至三维，即X，y，Z，rQ组成一个 四维空间，虽然不易直观表述，但原理相同。
[0059] 考虑到计算难度，这里做几点近似假设。首先死^?和供相互独立，即协方差为 零。其次0破，^?呈反比例，在实验中发现，球面关系约束在远距环境下的效果不佳;而在近 距环境下效果很好。也就是说，远距时，中心距离约束误差相对较小，而粗估计求出的切面 作为近似时误差较大;近距时，粗估计可信度高，球面关系约束可使得误差减小，其加权值 应高于中心距离约束的加权值。故以中心距离和时延标准差之比作为加权值平衡的标准。 其效果也得到仿真实验的验证。
[0060] 本发明通过坐标与距离方程的建立、中心距离的求解、目标坐标的求解、建立距离 约束和球面关系约束，求解目标坐标的修正解等步骤，解决水下航行器被动式探测、定位问 题，也可用于无线电定位、超声波定位、室内定位等其他基于TD0A的定位问题。与现有技术 相比，本发明具有如下优点：
[0061] l)Chan算法对于远距情况采用一次加权最小二乘法求解，近距情况在远距情况的 计算基础上增加了一次加权最小二乘，并进行一次修正。而对于远距和近距的划分界限并 没有明确说明，即多远距离属于远距，多近属于近距没有明确的界限。本发明统一了远距和 近距的方法，无论远距近距都适用，也就是说在使用场景上没有距离区分判断的环节； [0062] 2)本发明在最后的位置修正环节采用中心距离约束和球面关系约束，而不是Chan 算法的误差修正，计算简单，效果良好，更重要的是不会出现开方计算产生多个解。因此不 会像Chan算法那样存在后续需要甄别模糊解的步骤；
[0063] 3)本发明在精度方面达到Taylor算法的效果，无论远距还是近距。因此其效果是 优于Chan算法的，尤其是远距环境下，精度可以提高30%。而相比较于Taylor算法递归带来 大量计算量，本发明的方法显然计算更加简单，方便。
[0064] 4)对于噪声方差较大的情况，Taylor迭代发散的几率很大，不宜采用，本发明的方 法由于不需要初始值，在这点上也是超过Tay lor算法的。
【附图说明】
[0065]图1为本发明的计算原理图；
[0066]图2为目标声源到水听器的几何示意图；
[0067]图3为距离仿真实验目标声源和水听器的布放示意图；
[0068]图4为本发明和其他三种算法在不同距离下仿真结果的均方根误差图；
[0069]图5为本发明和其他三种算法在不同距离下仿真结果的误差标准差图；
[0070] 图6为本发明和其他三种算法在不同大小噪声下仿真结果的均方根误差图；
[0071] 图7为本发明和其他三种算法在不同大小噪声下仿真结果的误差标准差图。
【具体实施方式】
[0072] 下面结合具体实施例，进一步阐明本发明，应理解这些实施例仅用于说明本发明 而不用于限制本发明的范围，在阅读了本发明之后，本领域普通技术人员对本发明的各种 等价形式的修改均落于本申请所附权利要求所限定的范围。
[0073]如图1所示，本发明所述的一种基于TD0A的快速定位方法，包括以下步骤：
[0074]步骤1:根据已知的各接收器采集的信号71(〇，1 = 0，1，2,3~1信号传播速度(：， 各接收器时延协方差矩阵Q，各接收器布放坐标(xi，yi，Zi)，i =0，1，2，3…N，获取由目标位 置发出的声源信号;具体为：
[0075]将待测目标作为声源发出声信号x(t)，各接收器接收到N+1个信号，记为yi(t)，i =0，1，2,3^々表示各接收器的编号，其中0号接收器为参考接收器;将其他各接收器的信号 yi(t)，i = l，2,3-_N与参考接收器的信号yQ(t)作相关运算，得到N个互相关函数RlQ( T)，i = 1，2，3…N;对RlQ( t )进行峰值监测，结合先验知识排除模糊峰干扰，得到各接收器相对于参 考接收器接收信号x(t)的时间差TlQ，i = l，2,3"_N;进一步可以得到距离差riQ = CTlQ，i = l， 2,3---N〇
[0076]步骤2:建立关于目标位置的方程组；
[0077]根据空间几何的位置关系，得到关于目标位置的方程组：
[0079]
是目标到参考接收器的距离，即为中心 距离；約，i = 1，2，3...N是误差;Ki = xi2+yi2+zi2，i = 0，l，2,3...N;
[0080] 将上式写成矩阵形式：
[0083] 步骤3:对步骤2中的方程组进行消元，变化求取仅包含未知量中心距离ro的方程 组；
[0084] 对所述关于目标位置的方程组进行消元，消去x，y，z，得到以下形式的方程组：
[0086]其中ha人，ki，i = l，2,3^_N由矩阵行变换得到;具体计算公式为：
[0091] 这里需要注意的是，|A|乒0,即编号0，1，2,3的四个接收器不能共面；另外N>4， 即接收器的数量至少5个，保证本算法方程有解;至此得到关于中心距离ro的方程组：
[0092] ga ? r〇 = ha7 2+(I)/ 2
[0094] 步骤4:计算中心距离ro;
[0095] 忽略误差①'2,则r〇的最小二乘解为々=(gV f/) .gt/' ./w:
[0096] 步骤5:根据步骤4得到的中心距离ro计算目标位置粗估计；
[0097]将所述步骤4求得的&代入目标位置方程，得到关于x，y，z的方程组：
[0100] 利用加权最小二乘法计算出目标位置粗估计：
[0101] (x，y，z)T= (GbT ?边―1 ? Gb)-1 ? GbT ?边―1 ? hb
[0102]其中，
，ri表示目标到各接收器的 距离，利用^ ~&关系计算。
[0103] 步骤6:利用步骤4、5计算出的中心距离ro和目标位置粗估计坐标建立包含约束方 程的方程组；
[0104] 对坐标解(x，y，z)进行修正，采用在步骤2建立的方程组基础上增加两个约束方程 再进行加权最小二乘，一个方程为中心约束方程：
[0105]
[0106] 另一个方程为球面关系约束方程：
[0107] r〇2= (x-xo)2+(y-yo)2+(z-zo)2
[0108] 将上式非线性方程线性化，要解释这个过程，先从二维空间说明。对于二维问题， 上式写成n)2=(x-x〇) 2+(y-y())2,将x，y，r〇当一个三维空间，贝lj上式为一个锥面方程，方程组
的加权最小二乘解可以理解为到各平面和锥面 的加权距离最小的点，所以把锥面上到粗估计点最近的切面作为锥面的线性近似，该切面 的法线向量为
，加权最小二乘解可以近似为到各平面和切面的加权距 离最小的点。现在把问题扩展至三维，即X，y，Z，r〇组成一个四维空间，虽然不易直观表述， 但原理相同，所以球面约束方程可以近似为如下近似方程：
[0110] 将上述两式和第二步的方程组整理可得：
[0114] 利用加权最小二乘法计算出最终位置：
[0115] (x，y，z)T= (GcT ? V -1 ? Gc)-1 ? GcT ? V -1 ? he
[0117] 考虑到计算难度，这里做几点近似假设。首先和灼相互独立，即协方差为 零。其次^呈反比例，在实验中发现，球面关系约束在远距环境下的效果不佳;而在近 距环境下效果很好。也就是说，远距时，中心距离约束误差相对较小，而粗估计求出的切面 作为近似时误差较大;近距时，粗估计可信度高，球面关系约束可使得误差减小，其加权值 应高于中心距离约束的加权值。故以中心距离和时延标准差之比作为加权值平衡的标准。 其效果也得到仿真实验的验证。
[0118] 步骤7:根据步骤6的包含约束方程的方程组，重新计算得到更新的目标位置。
[0119] 如图2、3所示，以水下航行器探测定位为例，当水下目标进入探测区域后，其发出 的特定频率的噪声被各接收器获得，一般水下接收器为水听器。由于各水听器到目标位置 的距离不一样，因此声音信号到达各水听器的时间也有所不同，各水听器呈现出的是波形 相似，强度不同，时序上相差一定平移量的信号。利用互相关解算技术可以求出各水听器波 形相对于参考水听器波形的时序平移量，即时间差。作为算法的输入量，我们还需要提前标 定好该区域的声速值，各水听器的测量误差方差，以及各水听器布放的具体坐标。将上述信 息输入本发明方法，可得出目标位置的坐标。计算结果随水听器测量误差的增大而增大，另 外远距离的定位误差要大于近距离的误差，增加水听器的数目可以提高定位精度。
[0120] 本发明在计算机上对各种输入参数进行了 1000次仿真实验，对Chan算法，SI算法， Taylor算法以及本发明方法进行了对比，统计计算结果的均方根误差均值和标准差。仿真 分为两组，第一组比较的是距离对精度的影响，第二组比较的是测量误差对精度的影响。模 拟的时间差采用目标位置到各水听器的距离加上一个均值为零的白噪声得到。实验中，声 速设为1500m/s。本发明至少需要5个水听器（1个参考水听器+4个水听器），仿真中采用1个 参考水听器+6个水听器组成基阵，具体坐标见表1。
[0121] 表1水听器的布放位置
[0123]第一组仿真实验比较的是距离对精度的影响。设定测量误差为5X1(T3s，位置从(_ 4200,4600,100)起始到（350,50,100)结束的直线路径，位置移动步长为99m。每移动一个步 长，以直线上该点为中心，边长为70m的立方空间随机选1000个点作为每一次的目标位置， 最后统计这1000个测试的均方根误差和标准差，如图3。路径上共65个步长，仿真会得到由 远至近65个计算结果。仿真结果如图4、5。均方根误差图反映的是计算结果偏离实际位置的 大小。图4中可以看出，Chan算法在远距环境下误差非常大，近距环境下效果不错。相反，SI 算法在远距环境效果不错，近距环境下的效果则不如Chan算法。Taylor算法在远距和近距 环境下误差都最小，效果最好。而本发明方法在远近距精度均优于Chan和SI算法，接近于 Taylor算法，考虑到其计算简便，无需初始值等方面优势，因此可以认为发明是具有明显意 义的。误差标准差图反映的是计算结果的误差波动大小，是算法稳定性的体现。图5是误差 标准差对比图，其特点和均方根误差图基本相似。
[0124] 第二组仿真实验比较的是测量误差对精度的影响。设定目标位置为以（3500，200， 0)为中心，200m为边长的矩形区域随机选点。实验的几种测量误差见表2,每种误差进行 1000次蒙特卡洛实验，统计测试的均方根误差和标准差，仿真结果如图6、7。从图6的均方根 误差图可以看出，随着测量误差的增大，定位误差也增大。其中各测量误差下定位误差大小 依次为Chan算法，SI算法，本发明方法和Taylor算法。在测量误差较低的时候，Chan算法的 误差相对来讲过大。另外，在测量误差较大的时候，由于初始值误差大，递归发散等原因， Taylor算法无法得到最终结果。相比较而言，本算法的稳定性则较高，再考虑到其较高的精 度，因此本算法效果较好。
[0125] 表2.时延白噪声标准差
[0127] 通过上述仿真实验的验证，我们可以得出结论，无论在远距还是近距，小噪声还是 大噪声环境下，本发明方法在精度和稳定性上均优于Chan算法和SI算法。Taylor算法虽然 在大部分情况下表现优异，但其精度和稳定性和本发明方法相差无几，而在大噪声环境下 则波动严重，计算异常，这对于抗干扰性较差的系统来说是致命的。考虑到Taylor算法还需 要外部提供初始值，迭代计算量大等因素，本发明方法简单，快速，稳定的特点弥补了较之 于Taylor算法高精度上的微弱劣势。此外，对于处理能力较强的系统，也可以将本发明方法 的结果作为Taylor算法的初始值，由于初始值的精度和稳定性更高，递归发散的概率降低， 其结果只会比现有Taylor算法的效果有过之而无不及。不过从图4、6中可以看出，由于本发 明方法精度很高，因此效果提升微弱，可根据需求自行选择。
【主权项】
1. 一种基于TDOA的快速定位方法，其特征在于，包括以下步骤： 步骤1:根据已知的各接收器采集的信号71(〖），1 = 0，1，2,3 - 1信号传播速度(：，各接收 器时延协方差矩阵Q，各接收器布放坐标(xi，yi，Zi)，i = 0，1，2，3 - N，获取由目标位置(x，y， z)发出的声源信号； 步骤2:建立关于目标位置的方程组； 步骤3:对步骤2中的方程组进行消元，变化求取仅包含未知量中心距离ro的方程组； 步骤4:计算中心距离r〇; 步骤5:根据步骤4得到的中心距离计算目标位置粗估计； 步骤6:利用步骤4、5计算出的中心距离和目标位置粗估计坐标建立包含约束方程的 方程组； 步骤7:根据步骤6的包含约束方程的方程组，重新计算得到更新的目标位置。2. 根据权利要求1所述的基于ID0A的快速定位方法，其特征在于，所述步骤1具体为： 将待测目标作为声源发出声信号x(t)，各接收器接收到N+1个信号，记为 yi(t)，i = 0，1， 2,3…N表示各接收器的编号，其中0号接收器为参考接收器;将其他各接收器的信号71(〇， i = l，2,3~N与参考接收器的信号yo(t)作相关运算，得到N个互相关函数Ri〇⑴，i = l，2， 3…N;对RiQ(t)进行峰值监测，结合先验知识排除模糊峰干扰，得到各接收器相对于参考接 收器接收信号x(t)的时间差T iQ，i = l，2,3…N;进一步可以得到距离差riQ = cTiQ，i = l，2，3."N〇3. 根据权利要求1所述的基于ID0A的快速定位方法，其特征在于，所述步骤2具体为： 根据空间几何的位置关系，得到关于目标位置的方程组：是目标到参考接收器的距离，即为中心距离； 灼，f = 1，2,3…JV是误差;Ki = xi2+yi2+zi2，i = 0，l，2,3."N; 将上式写成矩阵形式：所述步骤3具体为： 对所述关于目标位置的方程组进行消元，消去x，y，z，得到以下形式的方程组：其中ha'，①'，ki，i = l，2,3^_N由矩阵行变换得到;具体计算公式为：这里需要注意的是，|A|乒0,即编号0，1，2,3的四个接收器不能共面；另外N彡4,即接收 器的数量至少5个，保证本算法方程有解;至此得到关于中心距离ro的方程组： ga ? rt^lW。十*!^ 2所述步骤4具体为： 忽略误差①/〗，则ro的最小二乘解为..g<7) ? gaT ? ka2 ； 所述步骤5具体为： 将所述步骤4求得的&代入目标位置方程，得到关于x，y，z的方程组：利用加权最小二乘法计算出目标位置：(x，y，z)T=(GbT ?边―1 ? Gb)-1 ? GbT ?边―1 ? hb 其中，协方差阵it = C2 ? B ? Q ? B，n表示目标到各接收器的距离， 利用n。+4关系计算； 所述步骤6具体为:对坐标解(x，y，z)进行修正，采用在步骤2建立的方程组基础上增加 两个约束方程再进行加权最小二乘，一个方程为中心约束方程： 另一个方程为球面关系约束方程： r〇2= (x-xo)2+(y-yo)2+(z-zo)2 将上式非线性方程线性化，得到线性化后的近似方程：将上述两式和步骤2的方程组整理可得：利用加权最小二乘法计算出最终位置： (x，y，z)T= (GcT ? V -1 ? Gc)-1 ? GcT ? V -1 ? he
【文档编号】G01S5/22GK106054134SQ201610340104
【公开日】2016年10月26日
【申请日】2016年5月20日
【发明人】徐晓苏, 金博楠, 姚逸卿, 童金武, 吴梅, 闫琳宇
【申请人】东南大学