[源码]基于ZYNQ的无人机定位算法原理

前一章节内容：【源码】一文读懂 TDOA 无源时差定位原理

01

—

时差估计算法已经提出了很多种，直接相关算法是最基本的一种时差估计算法，其主要原理就是：目标源发出的同一信号经过不同的传输路径到达不同的测量基站，然后测量基站对接收到的信号进行互相关处理，之后再对处理过的信号进行峰值检测，进而得到同一信号到达不同基站的时间差。这种时差估计算法在信号与噪声、噪声与噪声之间不相关的情况下适用。直接相关算法原理简单，计算不复杂易于实现。

假设目标辐射源发出的信号为s(t)，并假设空间有两个接收基站，其接收到的信号 x1(t)、x2(t)可表示为：

其中A1、A2表示信号S(t)在经过传输后的幅度增益；d1、d2为信号传输过程中的时延；q1、q2为零均值的高斯噪声。

假设d1<d2，则式（4.1）就可以改写为：

其中A=A2/A1，是两个接收信号X1 、X2的幅度值之比；D是需要测得的时差值。

两个接收机所接收到的基带离散形式的信号可表示为：

接收的两基带信号X1、X2的互相关函数为：

其中，E求均值，*表示取共轭。将式子（4.3）代入上式中，有：

因为噪声与信号、噪声与噪声之间是互不相关，则式子（4.5）化简可得：

其中的Rss为目标信号源的子相关函数。sn为非周期信号时，由相关函数性质可知，当t-D=0时，相关函数会取得最大值。在实际项目中，可以通过用时间平均来对平稳随机过程的相关函数进行估计，可表示为：

据式子（4.6）、（4.7）可知：

Chan定位算法

Chan定位算法在无源时差定位技术中是很有优势的一种定位算法，其优势在于它的计算量小并且定位方式原理简洁也无需作迭代运算。Chan算法的主要特点为测量误差服从高斯分布的环境下，定位精度高。但测量误差不服从高斯分布的情况下其定位精度会有所下降。

三维空间上四个测量基站的无源时差定位，具体的Chan算法推算如下：

根据上一章节的原理：【源码】一文读懂 TDOA 无源时差定位原理，的公式3.6可知：

则式（3.7）中C、X、B具体分别为：

式（3.7）有唯一解的要求是 的行列式的值不为0，即：

三维四测量基站系统来讲就是，只要是个基站不在同一平面上，即：

因此求得的最小二乘解为：

再令：

则方程组的解为：

其中：

将式（4.13）代入式（3.2）的平方中，化简可得：

由式（4.17）得：

结合式（4.12）、（4.13）、（4.16）就可以解算出目标源的坐标

通过对式子（4.14）分析，判断方程的解就会出现以下几种情况：

通过分析，当方程组有两个解时，由于目标源位置与主站位置之间的距离 是正数，所以当方程组的解为一正一负时，可以将负的解舍去，正根为定位解，这时是不存在定位模糊问题；当方程组两根相等时也不存在定位模糊；但当方程组的两根都为正时就会出现目标源定位模糊，这时必须要借助其他的信息来实现定位。

Chan定位算法实现目标定位流程如图4.1所示。

泰勒定位算法

泰勒算法是属于递归算法，其算法精度高。但它有一个最大的特点就是对定位初值有很强的依赖性。若初值与实际目标源位置相差很大就会出现泰勒级数不收敛从而不能实现定位。现在基于三维空间中四个测量基站的无源时差定位系统对泰勒定位算法做研究推算。

泰勒级数法（Taylor series, TS）基本思想是将时差距离看作是关于目标位置的函数。设目标源发出信号到测量基站之间的时差距离关于目标源的位置函数为：

泰勒定位算法是需要给一个目标源的定位初值，所以设目标源的初始估计位置为 

将函数在初始位置点处用泰勒级数展开法进行展开，根据实际情况可忽略二次及更高次项，则可得到：

并设：

将式（4.19）转为矩阵表示可得：

其中：

由式（3.1）、（3.3）可对 进行化简得到，

因此 最小二乘估计为：

由于泰勒级数展开法中初始位置的选取对定位结果影响很大。如果初始位置选取不合适，就有可能导致算法的不收敛，或者就算可以收敛，也会增加迭代运算的次数。因此，可以设想先通过其它的某种算法，对测量数据进行初始定位，将定位的结果作为泰勒级数展开法的初始值，然后再利用泰勒级数展开法进行定位计算。为了减小算法的运算复杂度和尽量避免牺牲太大的运算速度，作为初始定位的算法应该具备运算速度快、定位结果较准确的特点，而前面所述的Chan算法恰好具有以上的优点。虽然，Chan算法在信道环境比较差的情况下，定位精度有所下降，但它的定位结果仍然反映了辐射源位置与测量值之间的关联特征,有利于泰勒级数展开法的收敛。因此，可以将泰勒级数展开法与Chan算法组合在一起，构成一种协同定位算法。

首先利用Chan定位算法对TDOA测量值进行定位计算，得到一个粗定位结果，然后将此粗定位结果作为泰勒级数展开算法的初始值，利用泰勒级数展开算法再次对辐射源进行精定位估计。该方法的实现流程如图所示。

本章对三种定位算法原理进行了分析。由Chan定位算法原理公式可知，Chan定位算法存在定位精度不高和定位模糊问题。泰勒定位算法定位精度高，但必须有与目标源真实位置差距不大的初始估计值这个先决条件，泰勒级数才能收敛实现目标源定位。基于两种定位算法的优缺点提出了结合两种定位算法优势的协同定位算法。这种定位算法解决了泰勒定位算法需要初始值所满足的条件，也解决了Chan定位算法精度不高的问题。

源码:TDOA无人机无源定位系统全套工程源码—基于Zynq平台的时差提取与定位解算

推荐合集阅读

👇