基于TWR-TDOA估计和MPGA布局优化的UWB定位跟踪系统的实现方法

基于twr-tdoa估计和mpga布局优化的uwb定位跟踪系统的实现方法
技术领域
1.本发明涉及定位跟踪系统的实现方法，尤其涉及一种基于twr-tdoa估计和mpga布局优化的uwb定位跟踪系统的实现方法。


背景技术：

2.随着通信技术的发展，越来越多的无线传感器网络节点定位系统开始出现。目前最流行的技术有蓝牙、zigbee、rfid和uwb无线定位等。其中，超宽带技术是一种十分优越的测距方法，它利用极窄的脉冲进行数据传输，因此频谱范围宽，通信速度快。
3.提高超宽带系统定位性能可以从两个方面出发：一是从定位算法上，二是从硬件平台上超宽带锚点的设计和布置上。定位算法分为传统定位算法和混合定位算法。rss、aoa、doa和tdoa是四种经典的位置计算方法。近年来，在这四种基本算法的基础上，通过建立新的数学模型或寻求最优解，不断提出一系列新的混合算法来提高系统精度。tdoa-uwb技术在定位理论上越来越成熟。大多数基于最小二乘理论的传统tdoa相关算法在实际测试中的精度在10-30cm之间。这些算法在时钟同步和传输延迟方面存在瓶颈。一些学者提出了新的算法，如二阶锥规划(socp)、半定规划(sdp)和非凸约束优化，这些算法带来了沉重的计算负担。许多混合算法在理论上进一步提高了定位精度，但由于算法的复杂性，尚未在实际场景的硬件平台上进行测试。在某些定位场景中，如大型商场、医院等，上述主流算法可以满足需求，但可能无法满足小房间的精确定位。对于目标特性而言，内部运动目标相对随机，而对于室外场景，大多数运动目标都有一定的运动规律可循，通信场景较为复杂，如多路径、nlos等，因此对算法的精度和稳定性都有较高的要求。当移动目标为随机运动且通信场景复杂时，位置轨迹存在不稳定和精度低的问题。
4.另一个重要的因素是超宽带锚点的布局。以往的研究大多基于二维平面定位。由于室内外场景的复杂性，3d定位更加实用。已有的一些关于二维/三维场景中多个节点位移的研究，主要集中在定位网络上，但关于较少锚点布置(少于4个)的理论研究较少。但是为了节省功耗和节点数量，需要研究节点数较少时的最优布局。


技术实现要素：

5.发明目的：本发明的目的是提供一种能有效地减小由传感器分布带来的测算误差，提高系统定位精度的基于twr-tdoa估计和mpga布局优化的uwb定位跟踪系统的实现方法。
6.技术方案：本发明的uwb定位跟踪系统的实现方法，包括以下步骤：
7.s1，根据室内室外场景，设置目标真实运行轨迹；
8.s2，采用ads-twr算法计算目标与两个基站之间的距离差；
9.s3，根据距离差，采用tdoa算法，获得定位精度；
10.s4，采用多种群遗传算法来获得最优的锚点分布；
11.s5，对tdoa结果进行卡尔曼滤波处理；
12.s6，完成目标的定位跟踪后，将定位跟踪结果与目标真实运动轨迹对比进行误差分析，得到理论上的定位跟踪精度。
13.进一步，步骤s2中，采用三条消息的ads-twr算法完成测距，飞行时间计算公式如下：
[0014][0015]
误差公式如下：
[0016][0017]
其中，t
reply1
为device a基站/标签接收信号后的回复延迟，t
round1
为device a基站与标签之间每轮通信的总时间；t
reply2
为device b基站/标签接收信号后的回复延迟，t
round2
为device b基站与标签之间每轮通信的总时间；device a的实际频率是期望频率的ka倍，device b的实际频率是期望频率的kb倍。
[0018]
进一步，步骤s3中，采用twr-tdoa定位方法，该定位方法的模型是由一个tag和四个uwb锚组成uwb三维定位网络；设目标标签坐标为t(x，y，z)，超宽带锚点个数为m，其中有一个主传感器s0和m-1个次传感器，次传感器的坐标为(xi，yi，zi),i＝1，
…
，m-1；设到达各站的时间为ti，各次传感器与主传感器的时间差记为δtofi，其中m为锚点个数，得到每个次级传感器与主传感器之间的距离差：
[0019]
δri＝cδtofi；
[0020]
根据cramer准则判据，求解判定定位结果的根r0。
[0021]
进一步，tdoa算法在三维情况下，有三种不同的定位结果：精确、模糊和丢失；
[0022]
对于tdoa定位结果模糊的情况，r0存在两个解，取共轭复根的实部；
[0023]
对tdoa定位结果精确的情况，r0存在单个解，取此根为精确定位结果下的r0；
[0024]
对于tdoa定位丢失的情况，r0无解，取前一时刻位置和运动状态，按照cv模型进行缺失值填补。
[0025]
进一步，当tdoa算法中不存在解时，对于目标先前时刻的位置和运动状态，采用cv模型填充缺失位置；cv模型为：
[0026]
x(k+1)＝f(k)x(k)+w(k)
[0027]
z(k+1)＝h(k+1)x(k+1)+v(k+1)
[0028]
x(k)为状态向量，为n维列向量；f(k)称为状态转移矩阵；w(k)为过程噪声，其协方差矩阵为q(k)，为系统中的n
×
n矩阵；
[0029]
z(k)为测量向量，为m维列向量，其中m＝3；h(k)为测量矩阵；v(k)为测量噪声。
[0030]
进一步，所述步骤s4中，定义aver
gdop
为一个平面的平均gdop，其高度为h：
[0031][0032]
当定位目标的gdop最小时，分布达到最佳状态；
[0033]
所述多种群遗传算法优化分配的过程如下：
[0034]
m1，输入x2×
12
包括给定空间的范围和测量平面的高度h；设置迭代次数genmax，选择初始化10个种群并且根据x2×
12
在每个种群中生成大量随机坐标；
[0035]
m2，在每个种群中，将坐标转换为二值染色体，通过对适应度值aver
gdop
(h)进行评测；m3，随机生成在0.7-0.9范围内的交叉概率pc和0.001-0.05范围内的变异概率pr；
[0036]
m4，前一个种群的最佳染色体取代后一个种群的最劣染色体；第一个种群中的染色体被最后一个种群中的染色体所取代；然后从每个群体中选出最优的染色体，形成一个精英群体；
[0037]
m5，精英群体中的染色体经过人工选择，由二值染色体解码为十值染色体；所有转换后的染色体都被选中，找出最小值，重新组装染色体以供下一代使用；
[0038]
m6，执行步骤m3，直到迭代的不变最小值等于genmax；
[0039]
m7，输出不变的最小值aver
gdop
(h)和对应的锚点坐标。
[0040]
本发明与现有技术相比，其显著效果如下：
[0041]
1、利用twr算法测得的距离计算出目标与两个基站之间的距离差，然后利用距离差建立双曲方程，提出了一种求解tdoa方程的新方法，且该方法易于在decawave公司的dwm1000超宽带定位平台上实现；
[0042]
2、采用简化的三维多种群遗传算法mpga，计算锚点的最优布局，以便有效地减小由传感器分布带来的测算误差，从而提高系统定位精度；
[0043]
3、采用卡尔曼滤波，解决了室外场景中tdoa算法的不稳定性，做到了室外场景位置误差的均方根误差为9.8cm，室内场景位置误差约为11.21cm；
[0044]
4、当采用新的twr-tdoa解决方案和布局优化时，本发明的系统具有最佳的精度性能，可以适应不同的环境。
附图说明
[0045]
图1为本发明系统的测量模型示意图；
[0046]
图2为三条消息的ads-twr示意图；
[0047]
图3为uwb三维定位网络模型示意图；
[0048]
图4(a)为不同情况下的定位结果示意图，
[0049]
图4(b)为在不同的情况下cdf的结果示意图；
[0050]
图5(a)为室外农田实验场景示意图，
[0051]
图5(b)为室外场景定位跟踪结果与实际轨迹的比较图；
[0052]
图6(a)为室内移动目标定位场景示意图，
[0053]
图6(b)为室内场景定位跟踪结果与真实轨迹的比较图；
[0054]
图7(a)为室外场景的系统定位最小均方误差图，
[0055]
图7(b)为室内场景的位置测量的最小均方误差图。
具体实施方式
[0056]
下面结合说明书附图和具体实施方式对本发明做进一步详细描述。
[0057]
本实施例中，首先利用ads-twr算法计算目标与两个基站之间的距离差，ads-twr算法对系统同步要求较低。然后根据测量距离，采用tdoa算法，提出一种新的双曲方程的求
解方法，以获得较高的定位精度。接着采用简化空间多种群遗传算法(mpga)求解最优锚固布置。最后采用卡尔曼滤波解决室外位置系统的不稳定性问题。
[0058]
(一)定位跟踪系统实现过程
[0059]
如图1所示为本发明跟踪系统的测量模型。硬件部分由单片机和超宽带通信模块组成。标签与基站之间通过uwb通信模块进行ads-twr测距和无线数据传输。测量完距离后，副bs(基站，后文中出现的bs都代指基站)将数据传输到主bs，主bs将数据通过usb传输到上位机，计算出目标位置。然后使用基于简化mpga算法的最优锚点分布以及使用卡尔曼滤波解决tdoa不稳定的问题。具体实现步骤如下：
[0060]
步骤一，标签与基站之间通过uwb通信模块进行ads-twr测距和无线数据传输。测量完距离后，副bs将数据传输到主bs，主bs将数据通过usb传输到上位机，计算出目标位置。
[0061]
图1为3条消息的ads-twr。本发明采用基于dwm1000的使用手册提供的三条消息的ads-twr算法完成测距程序的编写。此算法通过使用第一往返测量的答复作为第二往返测量的发起者将四条消息的ds-twr简化为三条消息。基站与标签一轮通信的全过程为：
[0062]
(11)标签向基站发送自己的时间戳；
[0063]
(12)基站接收到标签的时间戳的一瞬间记下自己的时间戳；
[0064]
(13)基站向标签发送收到信息时自己的时间戳和发送一瞬间自己的时间戳；
[0065]
(14)标签收到基站发送的信息，并记下自己此时的时间戳。
[0066]
其原理示意图如图2所示，易知t
round
＝2t
prop
+t
reply
，在此情况下飞行时间可由下式计算：
[0067][0068]
其中，t
prop
为信号从基站到标签的飞行时间，t
reply
为基站/标签接收信号后因为处理信息等原因产生的回复延迟，t
round
为基站与标签之间每轮通信的总时间，δtof为总的飞行时间；t
reply1
为device a基站/标签接收信号后的回复延迟，t
round1
为device a基站与标签之间每轮通信的总时间；t
reply2
为device b基站/标签接收信号后的回复延迟，t
round2
为device b基站与标签之间每轮通信的总时间。
[0069]
在非对称双边双向测距中，两个设备的应答时间不必是同步的，这意味着降低了系统的时钟要求。在相同的信息传输条件下，ads-twr可以节省消息流，即节约电池电量和空间时间。当晶体振荡器质量不高时，时钟频率误差可控制在皮秒级。影响精度的最重要的误差取决于下列公式：
[0070][0071]
device a的实际频率是期望频率的ka倍，device b的实际频率是期望频率的kb倍。实际情况中，ka与kb都接近于1。
[0072]
然后导出twr数据进行定位处理。以100m uwb通信范围为例，tof在300ns左右，而误差时间在6ps左右，对应的距离误差为2mm。
[0073]
图3为uwb三维定位网络模型。本发明提出了一种新的twr-tdoa定位方法。该定位模型由一个目标标签(tag)和四个uwb锚(anchor)组成了uwb三维定位网络。本发明考虑放置在固定地点的四个锚，它们的位置是已知的。假设目标标签坐标为t(x,y,z)，超宽带锚点个数为m，其中有一个主传感器s0和m-1个次传感器，次传感器的坐标为(xi，yi，zi),i＝
1，
…
，m-1。设到达各站的时间为ti，各次传感器与主传感器的时间差记为δtofi。得到每个次级传感器与主传感器之间的距离差：
[0074]
δri＝cδtofiꢀꢀ
(3)
[0075]
其中，c为光速，这个距离差也可以直接写成目标到副站的欧氏距离减去目标到主站的距离，如式(4)所示。
[0076]
δri＝d
i-r0ꢀꢀ
(4)
[0077]
其中，di＝‖t-si‖2,当m＝4时，i＝1,2,3,r0＝‖t-s0‖2[0078]
然后可以写成
[0079]
(δri+r0)2＝d
i2
ꢀꢀꢀ
(5)两边同时减去r
02
，得到：
[0080]
δr
i2
+2δrir0＝d
i2-r
02
[0081]
＝2x(x
0-xi)+2y(y
0-yi)+2z(z
0-zi)+d
i2-d
02
ꢀꢀ
(6)
[0082]
其中，di＝‖si‖2,当m＝4时，i＝1,2,3和d0＝‖s0‖2[0083]
得到：
[0084][0085]
式(7)中有3个方程，可以写成式(8)中的矩阵形式，其中有未知数和
[0086][0087]
让ax＝b，得到
[0088][0089][0090]
根据线性方程的线性性质，ax＝b的解是ax＝c的解与ax＝d的解的和的r0倍。
[0091]
当m＝4时，a为方阵，根据cramer准则判据，解为其中aj为用常数项替换第j列元素得到的行列式。可以得到解
[0092][0093]
其中a
l
(l＝1,2,3)是ax＝c的三个解，b
l
是ax＝c的三个解。
[0094]
式(11)中r0为未知参数。由r0的定义可以得到一个二元二次方程，如式(12)所示
[0095]
er
02
+2fr0+g＝0
ꢀꢀꢀ
(12)
[0096]
其中
[0097][0098]
可以得到这个单变量二次方程的根：
[0099][0100]
根据根r0，三维情况下的tdoa算法有三种不同的位置结果：模糊、精确和丢失。本发明以不同的方式处理这三个例子。
[0101]
c1)r0有两个根(其中一个是假解)，可以得到两个定位结果，即定位模糊，需要增加锚点。在这个算法中，取共轭复数的实部作为根。
[0102]
c2)r0只有一个根，可以唯一确定目标位置。
[0103]
c3)没有根，这意味着目标位置无法确定。对上一时刻目标位置与运动状态进行缺失位置填充，接下来进行偏移误差分析。tdoa算法的核心是主锚和次锚的区别，首先对差分进行了推导：
[0104]
dr＝hdu+ds
ꢀꢀ
(15)
[0105]
其中，dr＝[dδr
1 dδr2…
dδr4]
t
[0106][0107]
du＝[dx dy dz]
t
[0108][0109]
对式(15)进行变换，得到标签位置误差矢量：
[0110]
du＝h-1
(dr-ds)
ꢀꢀꢀ
(16)
[0111]
由公式(16)可以看出，标签位置的精度与传感器分布和测量误差相关。下面将从两个方面(即布阵优化算法和跟踪算法)提高tdoa(time difference of arrival到达时间差)算法的准确性。
[0112]
步骤二，基于多种群遗传算法的优化分配
[0113]
不同的传感器布置策略会影响目标的gdop(geometric dilution precision几何精度因子)，gdop可以用下面的公式表示：
[0114][0115]
其中p＝(h
t
h)-1ht
mh(h
t
h)-1
,
[0116]
定位误差的方差为σ
s2
，测量误差的方差为σ
rd2
(d＝1,2,
…
,m)。
[0117]
当定位目标的gdop最小时，分布达到最佳状态。本发明采用多种群遗传算法(mpga)寻找空间gdop的最优锚点布局。
[0118]
其中，定义aver
gdop
为一个平面的平均gdop，其高度为h：
[0119][0120]
遗传算法通过遗传算子从随机种群中生成后代来搜索最优结果。与遗传算法相比，多种群遗传算法利用更多的种群产生个体作为多种群交叉。在整个空间范围内搜索会带来巨大的计算量，而在一个水平平面搜索可以大大减少计算量，并且具有相近的精度。于是采用这种简化的mpga方法来计算最优布局。该mpga优化分配的过程如下：
[0121]
m1)初始化：输入x2×
12
包括给定空间的范围和测量平面的高度h。设置迭代次数genmax。选择初始化10个种群并且根据x2×
12
在每个种群中生成大量随机坐标。
[0122]
m2)编码和评测：在每个种群中，将坐标转换为二值染色体，通过对适应度值aver
gdop
(h)进行评测。所有的染色体都根据适应度值进行排序。
[0123]
m3)选择：随机生成在0.7-0.9范围内的交叉概率pc和0.001-0.05范围内的变异概率pr。交叉和突变：选择染色体越好，适应度值越大的染色体作为双亲。被选择的双亲进行杂交以产生新的后代的概率是pc。新的二元染色体改变一个比特的概率是pr。
[0124]
m4)人工选择移民：前一个种群的最佳染色体取代后一个种群的最劣染色体。第一个种群中的染色体被最后一个种群中的染色体所取代。然后从每个群体中选出最优的染色体，形成一个精英群体。
[0125]
m5)解码与评测：精英群体中的染色体经过人工选择，由二值染色体解码为十值染色体。所有转换后的染色体都被选中，找出最小值，重新组装染色体以供下一代使用。
[0126]
m6)执行步骤m3，直到迭代的不变最小值等于genmax。
[0127]
m7)解决方法：输出不变的最小值aver
gdop
(h)和对应的锚点坐标。
[0128]
在每次实验开始时，该算法可以保证在测量范围内平均gdop的最小值。考虑到放置和计算的方便，在场景的两侧设置锚点，并固定一个锚点。表1为空间三个随机测试点在优化布阵和非优化布阵情况下gdop值的对比。
[0129]
表1不同锚点的gdop值
[0130][0131]
由表1可以看到，如果基站设置在mpga所定位的位置，可以大大降低gdop，这意味着优化后的分布可以减少实测数据的偏差。因此，该算法可以有效地减小由系统引起的传感器分布误差。
[0132]
步骤三，卡尔曼滤波
[0133]
定位过程中tdoa不稳定，导致测量误差。为了解决这一问题，将采用卡尔曼滤波来提高后置处理的稳定性和精度。当目标的测量方程和运动方程为线性且过程噪声服从高斯分布时，卡尔曼滤波可获得最佳的跟踪性能。当tdoa算法中不存在解时，对于目标先前时刻的位置和运动状态，采用cv模型填充缺失位置。cv模型为：
[0134]
x(k+1)＝f(k)x(k)+w(k)
ꢀꢀ
(19)
[0135]
z(k+1)＝h(k+1)x(k+1)+v(k+1)
ꢀꢀ
(20)
[0136]
式(19)为状态方程：x(k)为状态向量，为n维列向量，其中n＝9；f(k)称为状态转移矩阵；w(k)为过程噪声，其协方差矩阵为q(k)，即系统中的n
×
n矩阵。
[0137]
式(21)为测量方程：z(k)为测量向量，为m维列向量，其中m＝4；h(k)为测量矩阵；v(k)为测量噪声。填充方法的核心思想是利用状态转移矩阵和测量矩阵估计缺失位置。
[0138]
系统的初始测试状态可以通过两点差分法进行初始化得到：
[0139][0140]
初始测试状态误差的协方差矩阵为：
[0141][0142]
其中，r为正态分布中的协方差，t为数据间隔周期。初始化后，卡尔曼滤波器可以从时间k＝2开始工作。
[0143]
测量范围数据的标准差由下式计算：
[0144][0145]
其中μ为状态的均值。在此，用matlab实现算法和仿真环境，对不同场景进行模拟，该模拟器实现了在一个标签和四个锚的情况下的定位过程，标签被随机放置在三维空间中，4个锚点采用mpga优化布置，测试记录测量结果20次：在第一种情况下，只考虑tdoa方法；另一种情况是同时使用tdoa和卡尔曼滤波器。定位结果如图4(a)所示，可以看出，tdoa的偏差小于tdoa和卡尔曼滤波的方法；图4(b)显示了基于循环误差概率(cep)的累积分布函数(cdf)。每个锚点68％数据计算的具体值见表2，与使用twr-tdoa算法相比，在使用twr-tdoa算法后再使用卡尔曼滤波的方法有较小的偏差。仿真结果表明，卡尔曼滤波可以减小由于环境或设备等因素造成的测量误差。
[0146]
表2每个锚点68％数据计算值
[0147][0148]
(二)实验验证
[0149]
本发明对室内、室外两种场景进行实验。
[0150]
场景一：室外场景
[0151]
室外实验场景的布置如图5(a)所示，并且是在麦田中进行的。锚点1与pc连接为主bs，锚2、锚3、锚4为次bs。在这个实验中h＝40,genmax＝10,通过mpga计算后，锚点1设为(0,0,0)，其他锚点分别设为(0,470,370)，(490,370)，(495,470,0)。
[0152]
实验员以0.8m/s的匀速手持标签，真实运动轨迹为从anchor 3、anchor 4基站中点到anchor 1、anchor 2基站中点的直线。使用link pg系统获取移动数据，然后用本发明的方法对这些数据进行处理，结果如图5(b)所示。上面的实线是目标的真实运动轨迹，点是位置结果；虚色的线是使用卡尔曼滤波后的结果，是最终的定位轨迹。结果表明，卡尔曼滤波将分散的锚点轨迹转化为平滑的轨迹曲线，偏离真实轨迹的锚点轨迹得到了一定程度的修正。
[0153]
场景二：室内场景
[0154]
由于gps信号不能穿透建筑物，也不能在室内工作，所以设计了一个室内的位置实验。场景如图6(a)所示，长640cm，宽430cm，高374cm。真实的目标轨迹为桌子上的灰色矩形，其高度为80厘米，同时加入一个均值为0，方差为0.03的加性高斯白噪声。bs a是主传感器，其他的bs是次级传感器。其中，h＝40,genmax＝10,运行mpga后，主基站设为(0,0,0)，其他站点分别设为(500,0,370)，(500,400,0)，(0,400,370)。在室内实验中，也采用了本发明提出的算法，定位结果与真实轨迹的3d对比如图6(b)所示。
[0155]
对于室内和室外环境，进行了两次有效的测量实验。评估了实际位置和估计位置之间的均方根误差(rmse)。rmse的计算公式如下：
[0156][0157]
其中(x,y,z)表示标签的实际位置，(xk,yk,zk)表示标签的估计位置。
[0158]
图7(a)为室外场景的rmse(均方根误差)，图7(b)为室内场景的rmse。
[0159]
室外场景位置误差的均方根误差为9.8cm，室内场景位置误差约为11.21cm。室内误差大于室外，可能是相邻频率的电磁干扰造成的。结合室外和室内场景，整个系统对三维运动目标的rmse位置误差约为10cm。
[0160]
表3给出了本发明的系统与其他主流定位算法在定位精度和实现方法上的比较。
[0161]
表3本发明的系统与其他主流定位算法在定位精度和实现方法对比结果
[0162][0163]
当采用twr-tdoa解决方案和布局优化时，本发明的系统具有最佳的精度性能，其优点是可以适应不同的环境。