一种基于tdoa和fdoa的无人机高精度定位方法
阅读说明：本技术 一种基于tdoa和fdoa的无人机高精度定位方法 (High-precision positioning method for unmanned aerial vehicle based on TDOA and FDOA ) 是由 熊海良 侯强 朱维红 许玉丹 王宏蕊 庄众 任美婷 于 2020-10-21 设计创作，主要内容包括：本发明公开了一种基于TDOA和FDOA的无人机高精度定位方法,包括以下步骤：利用空间中的接收站接收TDOA测量值和FDOA测量值,并将其等效为距离差测量值和距离差变化率测量值；对接收站的位置坐标进行联合估计构建待估计矢量；构建对应距离差测量值、距离差变化率测量值以及接收站位置测量值和待估计矢量之间的表达式；求出代价误差函数的非线性加权最小二乘优化模型；泰勒级数联合迭代求出待估计矢量的最优闭式解；当满足迭代终止条件得到目标的位置和速度。本发明的方法将接收站的位置坐标进行联合迭代估计,降低了接收站位置误差对定位精度的影响；利用定位误差修正提供更加精确的迭代初值,在保证收敛性和提高收敛速度的同时,提高目标定位精度。(The invention discloses a high-precision positioning method of an unmanned aerial vehicle based on TDOA and FDOA, which comprises the following steps: receiving, with a receiving station in space, TDOA measurements and FDOA measurements equivalent to range difference measurements and range difference rate of change measurements; performing joint estimation on the position coordinates of the receiving station to construct a vector to be estimated; constructing corresponding measured values of range difference, measured values of range difference change rate and expressions between the measured values of the receiving station position and the vector to be estimated; solving a nonlinear weighted least square optimization model of the cost error function; solving the optimal closed-form solution of the vector to be estimated by Taylor series combined iteration; and obtaining the position and the speed of the target when the iteration termination condition is met. The method of the invention carries out joint iterative estimation on the position coordinates of the receiving station, thereby reducing the influence of the position error of the receiving station on the positioning precision; and a more accurate iteration initial value is provided by using positioning error correction, and the target positioning precision is improved while the convergence is ensured and the convergence speed is improved.)
技术领域
本发明涉及一种无人机定位方法，特别涉及一种基于TDOA和FDOA的无人机高精度定位方法。
背景技术
当今世界科学技术迅速发展，战争的形式也发生了根本性变化。现代高科技条件下的战争是一场以信息技术为依托，涉及海、陆、空、天、电等领域的多维一体化战争。其中处于主导地位的是电子战，而对辐射源的精确定位与跟踪是现代电子战系统的一项十分重要的功能。在军事领域，对辐射源状态信息的准确估计，有助于战场情报的获取和精确制导武器的使用，能够为打击敌方目标提供强有力的支撑；在民用领域，可以为社会提供准确的导航和定位服务。因此对辐射源的精确定位与跟踪在军用和民用领域中均具有广泛的应用前景。
根据接收站是否向目标发送电磁波信号，目标定位可分为有源定位和无源定位。有源定位方法是指定位系统向外发射电磁波，接收来自目标的回波信号。通过对回波信号的处理和分析，对目标进行检测、定位和跟踪信号。它具有全天候、高精度的优点。但由于其向外辐射电磁波的频带固定，容易被敌方发现和跟踪，进而受到有针对性的电子干扰和精确制导武器的打击，不仅会降低目标的定位性能，而且会危及定位系统的安全。
无源定位方法是指定位系统不向目标发射电磁波信号，而只利用目标辐射源的辐射信息来确定目标位置的技术。无源定位系统通过测量信号从辐射源到接收站的方向角和相位变化率，或通过测量信号到达多个接收站之间的时差，来定位目标辐射源。当辐射源目标和接收站之间存在相对运动时，到达多个接收站的信号之间的频率差也可以用来定位目标。
组合不同测量信息的定位系统可以综合不同测量信息的优点，增强对辐射源信号类型的适应性，在一定程度上提高对辐射源的定位精度，减少接收站的数量。现代定位系统通常安装在无人机、卫星、船舶和其他移动平台上。事实上，移动平台的定位往往存在随机误差，基于时差和频差的辐射源定位精度对接收站的位置非常敏感，因此，需要考虑接收站位置随机误差的统计信息，以提高对辐射源的定位精度。
发明内容
为解决上述技术问题，本发明提供了一种基于TDOA和FDOA的无人机高精度定位方法，将TDOA和FDOA定位技术结合起来使用，并在进行参数估计时将接收站位置误差考虑在内，从而提高对运动目标的定位性能，实现对运动目标的精确定位的目的。
为达到上述目的，本发明的技术方案如下：
一种基于TDOA和FDOA的无人机高精度定位方法，包括以下步骤：
步骤1：利用空间中的K个接收站，将接收站接收到的TDOA测量值和FDOA测量值等效为距离差测量值和距离差变化率测量值；
步骤2：对接收站的位置坐标进行联合估计，构建包含未知的目标真实位置和速度矢量以及未知的接收站真实坐标矢量的待估计矢量η；
步骤3：构建对应距离差测量值、距离差变化率测量值以及接收站位置测量值和待估计矢量η之间的表达式；
步骤4：定义步骤3中表达式的代价误差函数，并求出代价误差函数的非线性加权最小二乘优化模型；
步骤5：将步骤3中的表达式在初始值ηa处利用一阶泰勒级数展开，并带入步骤4中的非线性加权最小二乘优化模型，求出待估计矢量η的最优闭式解；
步骤6：设定终止迭代条件ε，0＜ε＜＜1，重复步骤5；
步骤7：当满足迭代终止条件时，迭代终止，得到目标的位置和速度。
上述方案中，步骤1的具体方法如下：
利用K个接收站，第i个接收站的真实位置为真实速度为目标的真实位置为q0＝[x0,y0,z0]T，目标的真实速度为目标与第i个接收站之间的真实距离表示为：
设第1个接收站为参考接收站，可得目标到达第i个接收站与到参考接收站之间的真实距离差为：
其中，ri 0表示目标与第i个接收站之间的真实距离，r1 0表示目标与参考接收站之间的真实距离，c为电磁波传播速度；
然后对公式(1)的两端分别对时间t求导数，得到目标与第i个接收站之间的真实距离变化率为：
其中，符号(·)T表示对矩阵或者矢量的取转置；
第i个接收站与参考接收站之间的真实速度差为：
其中，为目标与参考接收站之间的真实距离变化率，f0为电磁波信号的频率，为第i个接收站与参考接收站之间真实的FDOA值；
将TDOA测量值等效为距离差测量值，距离差测量值表示为：i＝2,3,...,K，其中，表示目标到达第i个接收站与到参考接收站之间的真实距离差，Δti1表示目标到达第i个接收站与到参考接收站之间的时间差测量误差，即TDOA测量误差；
假定距离差测量值服从均值为0的高斯分布，把i＝2,3,…,K写成矢量形式：其中，n＝[n21,n31,...,nK1]T，n表示距离差的测量误差组成的矢量，rK1表示目标到达第K个接收站与到参考接收站之间的距离差测量值，表示目标到达第K个接收站与到参考接收站之间的真实距离差，nK1表示目标到达第K个接收站与到参考接收站之间的距离差测量值与真实值的误差；
同样，将FDOA测量值等效为距离差变化率的测量值，距离差变化率的测量值表示为i＝2,3,...,K，其中，表示第i个接收站与参考接收站之间的真实速度差，Δfi1表示目标到达第i个接收站与参考接收站之间频率差的测量误差，即FDOA测量误差；
假定距离差变化率的测量值服从均值为0的高斯分布，并且与TDOA测量值不相关；把i＝2,3,...,K写成矢量形式其中， 表示距离差变化率的测量误差组成的矢量，表示第K个接收站与参考接收站之间的真实速度差，表示第K个接收站与参考接收站之间的速度差的测量值与真实值的误差。
上述方案中，步骤2的具体方法如下：
接收站的测量位置表示为：其中，pi＝[xi,yi,zi]T表示接收站的测量位置，Δpi表示接收站测量位置和实际位置的误差，表示接收站的真实位置；
构建待估计矢量其中，q0T表示目标的真实位置取转置，表示目标的真实速度取转置，表示第K个接收站的真实位置取转置。
上述方案中，步骤3的具体方法如下：
构建对应距离差测量值、距离差变化率测量值以及接收站位置测量值和待估计矢量η之间的表达式：
Γ＝Θ-g(η) (5)
其中，由距离差测量值距离差变化率测量值以及接收站位置测量值pi组成，表示取转置，表示取转置，表示第K个接收站位置测量值pK取转置；
表示与Θ对应的测量误差矢量，nT表示距离差测量误差n取转置，表示距离差变化率的测量误差取转置，表示第K个接收站位置测量值误差取转置；
g(η)的前K-1个元素由量测TDOA表达式i＝2,3,...,K确定，第K个元素到第2K-2个元素由量测FDOA表达式i＝2,3,...,K确定，最后第2K-1个元素到第5K-2个元素由接收站位置坐标确定，即pi＝[xi,yi,zi]T；
假设Γ均值为0，协方差矩阵为Q，Q的形式为：
其中，Qt表示TDOA测量噪声的协方差矩阵，Qf表示FDOA测量噪声的协方差矩阵，Qp表示接收站位置测量误差的协方差矩阵，Om×n表示m×n维的所有元素为零的全零矩阵。
上述方案中，步骤4的具体方法如下：
公式(5)的代价误差函数为：
J(η)＝(Θ-g(η))TQ-1(Θ-g(η))
非线性加权最小二乘优化模型为：
η＝arg min(Θ-g(η))TQ-1(Θ-g(η)) (6)
上述方案中，步骤5的具体方法如下：
令ηa为η的初始估计值，通过g(η)在ηa处的一阶泰勒展开式求解方程式(5)，将式(5)展开为如下的形式：
其中，是g(η)在ηa处的雅可比矩阵，其形式如下：
令r＝[r21,a,...,rK1,a]T，rK1,a表示初始估计值为ηa时目标到达第K个接收站与到参考接收站之间的距离差测量值，表示表示初始估计值为ηa时目标到达第K个接收站与到参考接收站之间的真实距离差，表示第K个接收站的测量位置取转置。
则表示成如下形式：
将公式(7)带入到公式(6)中，得到第l+1次迭代结果的线性加权最小二乘优化模型：
显然，上式是关于目标解的二次优化问题，因此存在最优闭式解：
上述方案中，步骤7的具体方法如下：
当满足时，迭代终止，终止时收敛值记为ηb，迭代结束时的ηb的前三个元素构成目标的位置矢量，第4个到第6个元素构成目标的速度矢量，即可得出目标的位置和速度。
进一步的技术方案中，步骤5中ηa的值通过定位误差修正的两步法得到。
更进一步的技术方案中，定位误差修正的两步法具体包括：
第一步，定义待估计量使用加权最小二乘的方法得到的估计值；
第二步，估计第一步中输出的目标位置和速度估计的误差，修正之后得到目标位置和速度的最终初始估计。
更进一步的技术方案中，第一步：
定义待估计量假设q0T,是互不相关的，使用加权最小二乘的方法得到的估计值，的估计方程为：
其中，ξ1表示测量误差项组成的矢量，
根据公式(11)得到的加权最小二乘估计：
上式中，W1定义为：
θ1估计误差的协方差矩阵为：
第二步:
估计第一步中输出的目标位置和速度估计的误差Δq和修正之后得到目标位置和速度的最终估计；
为第一步中q0的估计值，为第一步中的估计值；
利用一阶泰勒展开，将r1 0和在和处展开：
其中，A和B分别为在和处的一阶泰勒级数展开项的梯度矩阵，忽略二阶以上的泰勒展开项，A和B的表达式分别为：
建立第二步的估计方程为：
式中：ξ2表示测量误差项组成的矢量，Δθ1为与θ1的差值；
由(23)式可得第二步待估参数的加权最小二乘解为：
其中，加权矩阵为
利用公式(28)修正θ1，更新目标位置和速度信息的估计值：
θ3＝[θ1(1:3)，θ1(5:7)]T-θ2。
通过上述技术方案，本发明提供的一种基于TDOA和FDOA的无人机高精度定位方法具有如下有益效果：
1、本发明在估计目标位置和速度的同时，对接收站的位置坐标进行泰勒级数联合迭代估计，降低了接收站位置误差对定位精度的影响。
2、本发明同时使用TDOA和FDOA测量值进行估计，定位方法不存在时差模糊问题，可以有效弥补TDOA定位方法的缺陷。
3、本发明利用定位误差修正的两步法提供更加精确的迭代初值，在保证收敛性和提高收敛速度的同时，提高目标定位精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。
图1为本发明实施例所公开的一种基于TDOA和FDOA的无人机高精度定位方法流程示意图；
图2为本发明实施例所公开的无人机与接收站关系示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。
本发明提供了一种基于TDOA和FDOA的无人机高精度定位方法，如图1所示，具体包括如下步骤：
步骤1：利用图2所示的空间中的K个接收站，将接收站接收到的TDOA测量值和FDOA测量值等效为距离差测量值和距离差变化率测量值；
利用K个接收站，第i个接收站的真实位置为真实速度为目标的真实位置为q0＝[x0,y0,z0]T，目标的真实速度为目标与第i个接收站之间的真实距离表示为：
设第1个接收站为参考接收站，可得目标到达第i个接收站与到参考接收站之间的真实距离差为：
其中，ri 0表示目标与第i个接收站之间的真实距离，r1 0表示目标与参考接收站之间的真实距离，c为电磁波传播速度；
然后对公式(1)的两端分别对时间t求导数，得到目标与第i个接收站之间的真实距离变化率为：
其中，符号(·)T表示对矩阵或者矢量的取转置；
第i个接收站与参考接收站之间的真实速度差为：
其中，为目标与参考接收站之间的真实距离变化率，f0为电磁波信号的频率，为第i个接收站与参考接收站之间真实的FDOA值；
将TDOA测量值等效为距离差测量值，距离差测量值表示为：i＝2,3,...,K，其中，表示目标到达第i个接收站与到参考接收站之间的真实距离差，Δti1表示目标到达第i个接收站与到参考接收站之间的时间差测量误差，即TDOA测量误差；
假定距离差测量值服从均值为0的高斯分布，把i＝2,3,...,K写成矢量形式：其中，n＝[n21,n31,...,nK1]T，n表示距离差的测量误差组成的矢量，rK1表示目标到达第K个接收站与到参考接收站之间的距离差测量值，表示目标到达第K个接收站与到参考接收站之间的真实距离差，nK1表示目标到达第K个接收站与到参考接收站之间的距离差测量值与真实值的误差；
同样，将FDOA测量值等效为距离差变化率的测量值，距离差变化率的测量值表示为i＝2,3,...,K，其中，表示第i个接收站与参考接收站之间的真实速度差，Δfi1表示目标到达第i个接收站与参考接收站之间频率差的测量误差，即FDOA的测量误差；假定距离差变化率的测量值服从均值为0的高斯分布，并且与TDOA测量值不相关；把i＝2,3,...,K写成矢量形式其中， 表示距离差变化率的测量误差组成的矢量，
表示第K个接收站与参考接收站之间的真实速度差，表示第K个接收站与参考接收站之间的速度差的测量值与真实值的误差。
步骤2：对接收站的位置坐标进行联合估计，构建包含未知的目标真实位置和速度矢量以及未知的接收站真实坐标矢量的待估计矢量η；
接收站的测量位置表示为：其中，pi＝[xi,yi,zi]T表示接收站的测量位置，Δpi表示接收站测量位置和实际位置的误差，表示接收站的真实位置；
构建待估计矢量其中，q0T表示目标的真实位置取转置，表示目标的真实速度取转置，表示第K个接收站的真实位置取转置。
步骤3：构建对应距离差测量值、距离差变化率测量值以及接收站位置测量值和待估计矢量η之间的表达式：
Γ＝Θ-g(η) (5)
其中，由距离差测量值距离差变化率测量值以及接收站位置测量值pi组成，表示取转置，表示取转置，表示第K个接收站位置测量值pK取转置；
表示与Θ对应的测量误差矢量，nT表示距离差测量误差n取转置，表示距离差变化率的测量误差取转置，表示第K个接收站位置测量值误差取转置；
g(η)的前K-1个元素由量测TDOA表达式i＝2,3,...,K确定，第K个元素到第2K-2个元素由量测FDOA表达式i＝2,3,...,K确定，最后第2K-1个元素到第5K-2个元素由接收站位置坐标确定，即pi＝[xi,yi,zi]T；
假设Γ均值为0，协方差矩阵为Q，Q的形式为：
其中，Qt表示TDOA测量噪声的协方差矩阵，Qf表示FDOA测量噪声的协方差矩阵，Qp表示接收站位置测量误差的协方差矩阵，Om×n表示m×n维的所有元素为零的全零矩阵。
步骤4：定义步骤3中表达式的代价误差函数，并求出代价误差函数的非线性加权最小二乘优化模型；
公式(5)的代价误差函数为：
J(η)＝(Θ-g(η))TQ-1(Θ-g(η))
非线性加权最小二乘优化模型为：
η＝arg min(Θ-g(η))TQ-1(Θ-g(η)) (6)
步骤5：将步骤3中的表达式在初始值ηa处利用一阶泰勒级数展开，并带入步骤4中的非线性加权最小二乘优化模型，求出待估计矢量η的最优闭式解；
令ηa为η的初始估计值，通过g(η)在ηa处的一阶泰勒展开式求解方程式(5)，将式(5)展开为如下的形式：
其中，是g(η)在ηa处的雅可比矩阵，其形式如下：
令r＝[r21,a,...,rK1,a]T，rK1,a表示初始估计值为ηa时目标到达第K个接收站与到参考接收站之间的距离差测量值，表示表示初始估计值为ηa时目标到达第K个接收站与到参考接收站之间的真实距离差，表示第K个接收站的测量位置取转置。
则表示成如下形式：
将公式(7)带入到公式(6)中，得到第l+1次迭代结果的线性加权最小二乘优化模型：
显然，上式是关于目标解的二次优化问题，因此存在最优闭式解：
其中，ηa的值通过定位误差修正的两步法得到，具体如下：
第一步：
定义待估计量假设q0T,r1 0,是互不相关的，使用加权最小二乘的方法得到的估计值，的估计方程为：
其中，ξ1表示测量误差项组成的矢量，
根据公式(11)得到的加权最小二乘估计：
上式中，W1定义为：
θ1估计误差的协方差矩阵为：
第二步:
估计第一步中输出的目标位置和速度估计的误差Δq和修正之后得到目标位置和速度的最终估计；
为第一步中q0的估计值，为第一步中的估计值；
利用一阶泰勒展开，将r1 0和在和处展开：
其中，A和B分别为在和处的一阶泰勒级数展开项的梯度矩阵，忽略二阶以上的泰勒展开项，A和B的表达式分别为：
建立第二步的估计方程为：
式中：ξ2表示测量误差项组成的矢量，Δθ1为与θ1的差值；
由(23)式可得第二步待估参数的加权最小二乘解为：
其中，加权矩阵为
利用公式(28)修正θ1，更新目标位置和速度信息的估计值：
θ3＝[θ1(1:3)，θ1(5:7)]T-θ2。
步骤6：设定终止迭代条件ε，0＜ε＜＜1，重复步骤5。
步骤7：当满足时，迭代终止，终止时收敛值记为ηb，迭代结束时的ηb的前三个元素构成目标的位置矢量，第4个到第6个元素构成目标的速度矢量，即可得出目标的位置和速度。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：大范围、高精度位姿确定方法及其系统