谢晓辉
(安徽理工大学电气与信息工程学院,安徽 淮南 232001)
TDOA定位方法是一种利用时间的差值来定位的方法。只要测量信号到达监测站的时间,就可以检测信号源的距离。以检测站的位置为中心,距离为半径,就能有效确定信号的位置。改进TWR-TDOA在传统的TWR-TDOA上加了一个T5时间戳,每次数据回传时会估计一个估计值,估计值是根据T4加上延时估计出来的。在牺牲处理器少量的运算时间基础上,提高整个系统的定位精度[1-2]。
在传统的TWR-TDOA上加了一个T5时间戳,每次数据回传时会估计一个估计值,估计值是根据T4加上延时估计出来的。在定位系统中,支持多目标节点和参考节点扫描目标节点,参考节点是主动的,移动节点是被动的。在信号过来是会准确读取T5真实值,是在牺牲处理器少量的运算时间,提高整个系统的定位精度。改进方案时间戳的传播示意图如图1:
图1 改进方案时间戳的传播示意图
其中的POLL,RESP,FINA都是包含时间戳的数据包。
在改进后的TDOA定位算法中,要支持多移动节点,需要基站扫描移动节点的ID区分不同的移动节点,所以基站是主动端,移动节点是被动端。在图1中表述了数据流的走向,在T1时刻移动节点广播发送POLL数据包,当基站在T2时刻收到POLL数据包时经过芯片运行的时间,在T3时刻将RESP数据包再转发出去,当T4时刻移动节点接收到RESP时同样芯片需要时间转发,在T5时刻转发出去作为最后一次通信的标志位,数据包FINAL到达T6。到此硬件的定位算法数据通信完成,但是为了增加精确性。
(1)
为了避免误差的时间在运算时要进行偶然误差的影响,软件会检测数据包发送的数据戳。因为定位系统在10cm之内,电磁波在真空中传输的时间为380M/S。在软件判断的时候将会放开到15cm。判断时间戳为:
t≤15/38000≈0.4ms
(2)
那么所有时间戳都要满足:
(3)
当时间戳都不满足上述条件的时候,就丢弃数据包。如果满足上述条件则算其平均值:
(4)
从k-1时刻到k时刻,卡尔曼滤波器的状态方程如下:
Xk=AXk-1+Buk+wk
(5)
其中Xk是状态向量;A是状态转移矩阵;B是输入增益矩阵;uk是系统输入向量;wk是均值为0时,协方差为Q,服从正态分布的过程噪声[3-5]。
X是一个n维的列向量,ui是xi的期望,则协方差矩阵为:
P=E[(X-E[X])(X-E[X])T]=
(6)
式(6)可以看出协方差矩阵都是对称举证而且是半正定,协方差的迹tr(P)是X的均方误差。
卡尔曼的系统状态观测方程:
Zk=HXk+vk
(7)
上式Zk是测量向量,H是测量矩阵,vk是均值为0时,协方差举证为R,且服从正态分布的测量噪声。从观测方程和状态方程看出k-1时刻和k时刻之间有了联系,而wk,vk则是反映出状态和测量的误差。
预测过程:
(8)
Pk=APk-1AT+Q
(9)
校正过程:
(10)
(11)
(12)
更新协方差估计:
Pk=(I-KkH)Pk′
(13)
在矿井下的NLOS影响状态方程:
Xk=AXk-1+Buk+NLOSK
(14)
误差是因为不存在直线距离导致的,那么在校正过程中,已经被校正了。卡尔曼在校正后,自动回归运行且对状态转移协方差矩阵更新。
在无障碍地区放置四个基站,并设立25个测试点,记录设备在各测试点处用UWB测出的坐标和用UWB测出的与各基站之间的距离。设备在每个测试点测试30次,并记录每次数据。
均值误差计算公式:
(15)
计算坐标error的分布矩阵,计算公式如下:
(16)
其中errori为设备在第i点测试点的测量坐标与真实坐标之间的误差,error(i,x)和error(i,y)为设备在第i点测试点的测量坐标与真实坐标x轴和y轴的均值误差
以上计算得出设备与基站error的分布,采用matlab4格点样条函数内插[8],光滑扩充error矩阵,使得error矩阵在可视化图里光滑,便于研究。如图2为设备实际值与测量值的误差分布热图
图2 设备实际值与测量值的误差分布热图
距离误差分析,设备与各基站之间的实际距离和测试距离的均值误差为式(15)的error(i,Aj),对error(i,Aj)做area图(填充区域图),如图3所示。
图3 移动节点与各基站的误差area图
定位误差结果表明,本算法的定位精度满足设计要求,设备在四基站中间部分时定位误差较小,当设备处于基站边缘时误差变大,可将设备处于轨道中间进行定位,以获得较高的定位精度。