一种三维鲁棒估计定位方法与流程

本发明涉及一种三维鲁棒估计定位方法，属于无线定位技术领域。

背景技术：

目前在无线定位过程中，因障碍物的遮挡而造成的非视距(nlos)误差会对定位精度产生很大的影响。已有研究表明，在仅存在视距(los)传播的理想环境中或在移动台(ms)与所有基站(bs)之间的nlos误差可被消除的环境中，能够实现较高的定位精度。然而，真实的室内外环境往往是复杂的los/nlos混合环境，由于ms和bs的直达路径总是被障碍物所阻挡，所测得的观测数据包含由过度反射路径引起的大量误差，导致难以获得较好的定位精度。

技术实现要素：

本发明的目的是提供一种三维鲁棒估计定位方法，用以解决在复杂的los/nlos混合环境下进行无线定位，难以获得较好的定位精度的问题。

为实现上述目的，本发明提供了一种三维鲁棒估计定位方法，步骤如下：

(1)获取关于定位目标的多个tdoa测量组，根据tdoa测量组的测量质量选择2个以上的有效测量组；

(2)计算每个有效测量组对应的初始权重；

(3)构建每个有效测量组关于定位目标的定位方程组，求解每个有效测量组对应的初始估计位置；

(4)针对每个有效测量组，根据其定位方程组以及其对应的初始权重和对应的初始估计位置，利用鲁棒估计对估计位置进行迭代更新，直至得到满足迭代停止条件的估计位置作为最终估计位置；其中，每次迭代还更新权重；

(5)计算所有最终估计位置的平均值作为定位目标的最终定位结果。

本方法的有益效果是：在求解最终估计位置时，不仅对估计位置进行迭代更新，还更新权重，能够有效克服nlos误差带来的不利影响，取得较高的定位精度；另外，通过选用多组测量质量较高的tdoa测量组进行定位，一方面能够从源头上降低nlos误差的影响，另一方面取所有最终估计位置的平均值作为最终定位结果，也能有效减小定位误差，进一步提高定位精度。

为了选择出多个有效测量组，作为对上述三维鲁棒估计定位方法的一种改进，所述步骤(1)包括：计算每个tdoa测量组的残差平方和；选择残差平方和小于设定阈值的tdoa测量组作为有效测量组。

为了计算每个有效测量组对应的初始权重，作为对上述三维鲁棒估计定位方法的另一种改进，所述步骤(2)包括：计算所有有效测量组的残差平方和的平均值，将平均值取倒数作为每个有效测量组对应的初始权重；或者分别计算各有效测量组的残差平方和的倒数，作为相应测量组对应的初始权重。

为了计算每个有效测量组对应的初始估计位置，作为对上述三维鲁棒估计定位方法的又一种改进，所述步骤(3)包括：利用最小二乘算法求解每个有效测量组对应的初始估计位置。

为了实现对定位目标的精准定位，作为对上述三维鲁棒估计定位方法的再一种改进，所述步骤(4)包括：对每个定位方程组进行taylor级数展开，忽略二阶以上分量，构建关于定位目标位置偏差和残差的矩阵方程；利用最小二乘算法对所述残差对应的残差平方和进行微分求极值，得到定位目标的位置偏差通式；将对应的初始权重和对应的初始估计位置代入所述位置偏差通式对估计位置进行迭代更新，更新后的估计位置等于前一次估计位置加本次迭代所得的位置偏差，直至得到满足迭代停止条件的估计位置作为最终估计位置；其中，每次迭代还更新权重，更新后的权重等于前一次权重乘于权因子，所述权因子利用本次迭代所得的残差平方和得到。

本发明还提供了另一种三维鲁棒估计定位方法，步骤如下：

(1)获取关于定位目标的多个tdoa测量组，选择测量质量最高的一组作为有效测量组；

(2)计算有效测量组对应的初始权重；

(3)构建有效测量组关于定位目标的定位方程组，求解有效测量组对应的初始估计位置；

(4)针对有效测量组，根据其定位方程组以及其对应的初始权重和对应的初始估计位置，利用鲁棒估计对估计位置进行迭代更新，直至得到满足迭代停止条件的估计位置作为定位目标的最终定位结果；其中，每次迭代还更新权重。

本方法的有益效果是：在求解定位目标的最终定位结果时，不仅对估计位置进行迭代更新，还更新权重，能够有效克服nlos误差带来的不利影响，取得较高的定位精度；另外，通过选用测量质量最高的tdoa测量组进行定位，也从源头上降低了nlos误差的影响，能够进一步提高定位精度。

为了选择出测量质量最高的tdoa测量组作为有效测量组，作为对上述三维鲁棒估计定位方法的一种改进，所述步骤(1)包括：计算每个tdoa测量组的残差平方和，选择残差平方和最小的一个tdoa测量组作为有效测量组。

为了计算有效测量组对应的初始权重，作为对上述三维鲁棒估计定位方法的另一种改进，所述步骤(2)包括：计算有效测量组的残差平方和的倒数作为有效测量组对应的初始权重。

为了计算有效测量组对应的初始估计位置，作为对上述三维鲁棒估计定位方法的又一种改进，所述步骤(3)包括：利用最小二乘算法求解有效测量组对应的初始估计位置。

为了实现对定位目标的精准定位，作为对上述三维鲁棒估计定位方法的再一种改进，所述步骤(4)包括：对定位方程组进行taylor级数展开，忽略二阶以上分量，构建关于定位目标位置偏差和残差的矩阵方程；利用最小二乘算法对所述残差对应的残差平方和进行微分求极值，得到定位目标的位置偏差通式；将对应的初始权重和对应的初始估计位置代入所述位置偏差通式对估计位置进行迭代更新，更新后的估计位置等于前一次估计位置加本次迭代所得的位置偏差，直至得到满足迭代停止条件的估计位置作为定位目标的最终定位结果；其中，每次迭代还更新权重，更新后的权重等于前一次权重乘于权因子，所述权因子利用本次迭代所得的残差平方和得到。

附图说明

图1是本发明实施例1的定位方法流程图；

图2是本发明实施例1对应的ms的仿真结果图；

图3是本发明实施例1对应的ms的定位误差波动情况图；

图4是本发明实施例1对应的测距误差变化时ms的定位效果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及具体实施例对本发明进行进一步详细说明。

在三维直角坐标系中，假设ms的真实位置坐标为x(x,y,z)；第i个基站bsi的位置坐标为xi(xi,yi,zi),i＝1,2,3,...,m。

ms到第i个基站bsi的距离为：

令bs1为参考基站，di,1表示ms到基站bsi和bs1的实际距离差：

di,1＝di-d1＝||xi-x||-||x1-x||(2)

利用超宽带(uwb)技术测量得到的tdoa测量值，可得到ms到基站bsi和bs1的测量距离差：

ri,1＝c·ti,1(3)

其中，c为电磁波传播速度，ti,1为tdoa测量值，即ms到基站bsi和bs1的时间差。

实施例1

结合图1，本发明实施例1的定位方法主要包含以下步骤：

步骤(1)：确定有效测量组

在利用tdoa算法进行定位的过程中，tdoa测量值的质量对定位精度有着很大的影响。在真实环境中，ms和bs的直达路径总是被障碍物所阻挡，因nlos产生的时间延迟，将导致ms到基站bsi和bs1的时间差ti,1偏大，从而导致测量距离差ri,1与实际距离差di,1存在较大偏差，严重影响定位精度。

设利用uwb技术获得k个tdoa测量组，当基站个数为m时，每组包含m-1个tdoa测量值，定义每个tdoa测量组的残差平方和为s(x)，则

根据上面的分析可知，当由nlos导致到达时间差ti,1偏大时，残差平方和s(x)会变大。因此，可建立残差平方和s(x)与tdoa测量组的测量质量之间的关系：s(x)越大，tdoa测量组的测量质量越差。

为了获得较好的定位精度，希望ms的定位结果能够更多的依赖于含有nlos测量值较少的测量组。为了实现该目的：

首先，计算每个tdoa测量组的残差平方和；

然后，令残差平方和小于设定阈值s的测量组作为有效测量组。

本步骤，利用残差平方和衡量tdoa测量组的测量质量；作为其他实施方式，也可以用克拉美罗下界(crlb)来衡量tdoa测量组的测量质量。

步骤(2)：初始化测量值权重

本步骤，计算每个有效测量组对应的初始权重，即计算每个有效测量组对应的测量值的初始权重。

假设经筛选共有k个有效测量组，用sk(x)表示第k组的残差平方和。为了消除残差平方和组间大小的关系，计算k个有效测量组的残差平方和的平均值为：

定义上述k个有效测量组中，每个测量值的初始权重为：

本步骤，计算所有有效测量组的残差平方和的平均值，将平均值取倒数作为每个有效测量组对应的初始权重；作为其他实施方式，也可以计算出每个有效测量组的残差平方和后，乘以预定的系数得到每个有效测量组对应的初始权重。

步骤(3)：ms初始位置估计

本步骤，构建每个有效测量组关于ms的定位方程组，并求解每个有效测量组对应的初始估计位置，即求解每个有效测量组对应的ms的初始估计位置。

在步骤(2)中，共筛选出k个有效测量组，因此可构建k个关于ms的定位方程组，进而可求得k个ms的初始估计位置。

下面以单个有效测量组为例，详细介绍利用总体最小二乘法(tls)求解ms的初始估计位置的过程，其余有效测量组的计算过程类似：

由一个有效测量组，构建出关于ms初始估计位置的定位方程组，即tdoa方程组：

di,1＝c·ti,1＝di-d1(7)

由于tdoa方程组是非线性方程组，需将其转化为线性方程组之后，再利用总体最小二乘法求解，具体过程如下：

由公式(7)易得：

di＝di,1+d1(8)

将公式(8)两端同时平方可得：

di²＝di,1²+d1²+2di,1d1(9)

由公式(1)可知：

代入公式(9)，可得：

(xi-x)²+(yi-y)²+(zi-z)²＝di,1²+d1²+2di,1d1(10)

当i＝1时：

d1²＝(x1-x)²+(y1-y)²+(z1-z)²

代入公式(10)，可得：

2(xi-x1)x+2(yi-y1)y+2(zi-z1)z+2di,1d1＝xi²+yi²+zi²-x1²-y1²-z1²-di,1²

令

xi,1＝xi-x1

yi,1＝yi-y1

zi,1＝zi-z1

ki＝xi²+yi²+zi²

可得：

又di,1＝c·ti,1，则最终公式(7)可表示为：

其中，ti,1能通过测量得到，同时由于所有基站bsi的坐标xi(xi,yi,zi)已知，则公式(12)中除x、y、z以外都是已知的，因此公式(12)实际上是未知数为x、y、z的线性方程组，实现了公式(7)的线性化。

将公式(12)向量化可得矩阵方程：

aβ＝b(13)

其中，a为系数矩阵：

向量β为：

β＝[xyzd1]^t

观测向量b为：

由于到达时间差ti,1的测量值均存在误差，导致系数矩阵a以及观测向量b均会受到外界因素的干扰，因此可以采用tls估计ms的初始位置。

首先构造增广矩阵h＝[ab]，然后采用奇异值分解法(svd)进行求解，求解共需三个步骤：

i.对增广矩阵h＝[ab]进行奇异值分解，得到：

其中，u＝[u1u2]∈r^a×a是[ab]·[ab]^t的a个特征向量组成的正交矩阵；

是的b+1个特征向量组成的正交矩阵；

是矩阵h的e+1个奇异值组成的矩阵，且σ1≥σ2≥…≥σt＞σe+1。

ii.根据eckart-young-mirsky矩阵逼近理论，h＝[ab]的最优解满足：

其中，σ1＝diag(σ1,σ2,…,σe,0)。

iii.判断v22非奇异，则初始估计为：

根据系数矩阵a和观测向量b可以很容易得到v12和v22，从而利用tls得到公式(13)的解β⁰＝[x⁰y⁰z⁰d1]，即得到ms的初始估计位置为x⁰(x⁰,y⁰,z⁰)。

本步骤，利用总体最小二乘法求解ms的初始估计位置；作为其他实施方式，还可以利用chan算法、fang算法、加权最小二乘法等进行求解。

步骤(4)：鲁棒估计

鲁棒估计的本质是基于雅克比矩阵的初始化权重和gauss-newton迭代算法。本步骤，针对每个有效测量组，根据其定位方程组以及其对应的初始权重和对应的初始估计位置，求解每个有效测量组对应的ms的最终估计位置。

下面以单个有效测量组为例，详细介绍求解ms的最终估计位置的具体过程，其余有效测量组的计算过程类似：

1)taylor级数展开

对公式(7)进行taylor级数展开，并忽略二阶以上分量，构造出关于ms位置偏差δ和残差ε的矩阵方程：

h＝gδ+ε(15)

式中，向量ε表示残差；

δ为ms的位置偏差：

δ＝[δxδyδz]^t

g为系数矩阵：

h为测量值与估计距离差之间的差值：

利用加权最小二乘算法求解公式(15)，得到ms位置偏差δ的估计值为：

δ＝(g^tpg^-1)g^tph(16)

其中，p为测量值权重。

2)迭代更新

每次迭代结束后，更新ms的估计位置，并判断更新后的ms的估计位置是否满足迭代停止条件，若满足，则该更新后的ms的估计位置即是ms的最终估计位置；否则，在不超过最大迭代次数的情况下，利用残差平方和更新测量值权重，并重复迭代过程，直至满足迭代停止条件。

第n次迭代过程如下：n＝1,2,3,...,n，n为最大迭代次数。

首先，在前一次迭代得到的ms的估计位置x^n-1(x^n-1,y^n-1,z^n-1)处进行泰勒展开，得到gn-1、hn-1，和前一次迭代得到的测量值权重pn-1一起代入公式(16)得到ms的位置偏差δⁿ：

δⁿ＝(gn-1^tpn-1gn-1^-1)gn-1^tpn-1hn-1(17)

其中，

从而，更新ms的估计位置为xⁿ：

xⁿ＝x^n-1+δⁿ(20)

然后，利用均方根误差(rootmeansquareerror，rmse)判断xⁿ是否满足迭代停止条件，此时xⁿ为(xⁿ,yⁿ,zⁿ)，均方根误差为：

设定门限值γ：

①当rmse<γ时，则xⁿ满足迭代停止条件，此时停止迭代并返回ms的估计位置xⁿ，xⁿ即为ms的最终估计位置；

②当rmse>γ且迭代次数n≤n时，则利用残差平方和更新测量值权重，进入下一次迭代，更新后的测量值权重为pn：

pn＝pn-1ωn(22)

其中，ω为权因子，也称等效权函数，此处选取iggiii权函数对权重进行更新：

式中，s表示残差平方和，一般取k0＝1.0～2.5，k1＝3.0～8.0。

令第n次迭代时的s用sn表示、ω用ωn表示、ε用εn表示，更新测量值权重时，先利用δⁿ求得残差εn：

εn＝hn-1-gn-1δⁿ(24)

进一步求得该残差下的残差平方和sn，再将sn代入公式(23)即得到ωn，最后代入公式(22)即可得到更新后的测量值权重为pn。

③当迭代次数大于n时，则输出错误并退出运算。

例如：

在初始权重p0和初始估计位置x⁰的基础上进行第1次迭代，得到ms的位置偏差δ¹＝(g0^tp0g0^-1)g0^tp0h0，其中：

然后，更新ms的估计位置为x¹＝x⁰+δ¹，判断x¹是否满足迭代停止条件，即判断是否小于门限值γ，若是，则x¹即为ms的最终估计位置；否则，在不超过最大迭代次数的情况下，利用残差平方和更新测量值权重为p1＝p0ω1，ω1通过将δ¹代入公式(24)先求得残差ε1，进一步求得该残差下的残差平方和s1，再将s1代入公式(23)得到。

本步骤，利用加权最小二乘算法求解ms位置偏差的通式；作为其他实施方式，也可以利用稳健最小二乘算法进行求解。

本步骤，每次更新测量值权重所用的权因子，是通过iggiii权函数将残差平方和进行转换得到；作为其他实施方式，还可以用huber权函数、andrews权函数、tukey权函数或iggi权函数计算权因子。

步骤(5)：计算ms的最终定位结果

本步骤，在步骤(4)的基础上，计算k个ms的最终估计位置的平均值作为ms的最终定位结果。

实施例2

本实施例，与实施例1的区别仅在于：步骤(2)初始化测量值权重的过程为：分别计算各有效测量组的残差平方和的倒数，作为相应测量组对应的测量值的初始权重；其余步骤均相同，此处不再赘述。

实施例3

本实施例，与实施例1的区别在于：步骤(1)中的有效测量组通过选择残差平方和最小的一个测量组得到；相应的，步骤(2)中测量值的初始权重为该测量组的残差平方和的倒数。之后针对该测量组求解ms的初始估计位置和求解ms的最终估计位置的方法与步骤(3)和步骤(4)相同，此处不再赘述，需要指出的是，由于本实施例的有效测量组只有1个，因此ms的最终估计位置即为ms的最终定位结果。

作为其他实施方式，上述实施例1、实施例2和实施例3中步骤(2)和步骤(3)的顺序可以调换。

下面给出具体的仿真验证实验：

仿真过程中，为模拟nlos非视距误差对定位造成的影响，假设各tdoa测量值误差之间相互独立，且服从零均值、方差为δ²的高斯分布，此处将tdoa测量值误差转化为测距误差，测距误差为tdoa测量值误差与电磁波速的乘积。

利用5个定位基站实现对ms的定位，ms的真实坐标为x(3,10,8)，基站bs1、bs2、bs3、bs4、bs5的坐标分别为：x1(0,0,10)、x2(20,0,0)、x3(0,20,0)、x4(-20,0,0)、x5(0,-20,0)，其中bs1为参考基站。

利用uwb技术获得关于ms的20个tdoa测量组，测距误差的方差δ²＝4，利用上文实施例1所述的定位方法，对ms的定位结果进行仿真，仿真结果如图2所示。图中，圆形代表基站，星形代表ms的真实位置，三角形代表ms的估计位置。

由仿真结果可得，ms的最终定位结果为：(2.96,9.92,7.48)，相对于ms的真实位置坐标(3,10,8)，由测距误差引入的定位误差为52.76cm。

为避免定位误差的随机性，对ms的定位进行了100次的仿真实验，定位误差波动情况如图3所示，可以看出：在测距误差的方差δ²＝4时，定位误差rmse主要在0.3m到0.5m之间波动，即定位误差主要在30cm到50cm之间，相对于该定位环境，此时的误差是很小的，可以认为是精确定位。

进一步考察测距误差对定位效果的影响，如图4所示：约定算法a为实施例1所述的定位方法，算法b为基于ms真实位置的taylor级数展开算法。

由图4可知，在测距误差不大(δ²＜7)时，算法a与算法b的定位误差rmse基本一致，说明两者的定位精度基本一致；当测距误差进一步变大时，算法a与算法b的定位误差rmse的差值始终保持在1.5m之内，说明算法a的定位精度也较接近算法b的定位精度。

综上，本发明提出的三维鲁棒估计定位方法，利用测量质量高的tdoa测量组进行定位，并在迭代过程中不仅更新定位结果，还利用残差平方和更新测量值权重，能够在复杂的los/nlos混合环境下获得较高的定位精度，得到的ms定位结果可以较好的反映ms的真实位置。

以上给出了本发明涉及的具体实施方式，但本发明不局限于所描述的实施方式。在本发明给出的思路下，采用对本领域技术人员而言容易想到的方式对上述实施例中的技术手段进行变换、替换、修改，并且起到的作用与本发明中的相应技术手段基本相同、实现的发明目的也基本相同，这样形成的技术方案是对上述实施例进行微调形成的，这种技术方案仍落入本发明的保护范围内。