一种防碰撞卡尔曼滤波同步定位算法

雷智杰，沈 重

（海南大学信息与通信工程学院，海南 海口 570228）

高精度的室内信号跟踪和移动物体定位扮演着越来越重要的角色，并且从曾经只用在军事用途到现在已经逐步走向了民用领域，诸多科研单位都在从事着高精度室内定位技术的相关研究.由于室内外环境有着巨大的差异，室内空间往往是一个封闭多障碍物，对信号传输有着许多干扰的复杂环境［1］.因此，对室内移动节点位置的精准预测是困难并且具有挑战性的工作.

脉冲超宽带（Impulse Radio Ultra-Wide Band，IR-UWB）是一类不使用载波来进行信息传输和交互的通信手段，在其交互信息的过程中依靠使用微秒级的脉冲信号来完成信息交互.这些脉冲信号通常都是非正弦窄波，所以信号更容易生成，所占用的频谱范围也更宽［2］.IR-UWB技术被应用到室内定位中，在确定位置的精确程度，传送信息所使用的带宽宽度，包括整个系统的稳定性上与其他技术相比都有着较大的领先.由于IR-UWB技术有较好的抗多径时延效果，穿透障碍物的能力优秀，布局和操作性都较为简单，定位精确度可以达到厘米级别，这一系列优势使其快速地脱颖而出成为室内定位最重要的研究领域.

IR-UWB室内定位技术涵盖许多种测距方法，在讨论时钟同步问题中所使用的是到达时间差值（Time Difference of Arrival，TDOA）的方法［3］.在此系统中想要精准地得到移动节点的位置信息必须严格执行主基站和从基站之间的时钟同步，因此解决主从基站间时钟同步信号所产生的偏移和漂移是提升该系统定位精确度和系统稳定性的重中之重.文献［4］在一个同步周期内，主基站只需要播放一次时钟同步消息，系统便可以自建一个有着全网络节点的树，根据贝叶斯估计算法便可以准确地估计出母节点与子节点相对漂移.文献［5］将接收方对接收方同步（Receiver-to-Receiver Synchronization，RRS）和发射方对接收方同步（Sender-to-Receiver Synchronization，SRS）2种同步网络协议相联合，使用卡尔曼滤波和递归算法不断迭代来预测和校正相对偏移，来提高时钟同步的性能.文献［6］使用分量解耦融合原理修正主基站和从基站的时钟偏差，考虑了精度与能耗的关系，可以实现在不增高能耗的同时提升精度.文献［7］考虑到双向测距定位在可伸缩方面的局限性，提出了一种基于多用户时差的无线时钟同步定位方法，使得时钟同步率在1Hz左右就足以达到分米波范围内的精度.文献［8］总结了DW1000 ScenSor（一种使用IEEE 802.15.4a的收发器）用作测试平台的情况，比较了几种定位系统中所有传感器的无线同步方法.为了使主从基站时钟同步的性能和系统的稳定性提高，笔者在基于卡尔曼滤波追踪的前提下对脉冲超宽带室内定位系统的时钟同步追踪性能开展讨论.

1 TDOA定位时钟同步方案

本文进行仿真讨论的是TDOA时钟同步系统，共有4个基站，其中一个为主基站，其余3个全部为从基站，构成1主3从的时钟同步系统，具体如图1所示.主基站根据一定时间周期不间断地发送时钟同步信号，3个从基站依据从主基站收到的时钟同步信号和定位标签的有效信息分别针对每一组的不同情况进行相对的时钟补偿，推算出TAG较为精准的时间到达数值，然后将TAG的时间到达数值，和TAG相应的ID数值返回发送给中央服务器CLE，中央服务器通过上位机软件里的TDOA定位算法，相关函数等一些其他算法推算出TAG相对于各个基站的位置［9］，最终给出一个较为精准的标签位置信息.

图1 IR-UWB主从基站时钟同步示意模型

假定全部基站都可以接发携有时间标记的时钟同步数据检验包（Clock Check Packet，CCP），并且起始主基站发射CCP信号的时间周期为T，则第j轮发送时钟校正消息时，主基站在T1，j时间点发射CCP信号，则3个从基站收到该数据包同时依据当地时间记录数据到达时刻为T2，j，Δj代表收到时钟信号时刻和下一个相邻时钟信号的时刻间隔，因为有着系统时钟相位偏移所以产生了信号时刻间隔，如图2所示.因此依据文献［10］，发射第j个时钟信号信息的时刻模型为

图2 主从基站时空关系模型

其中，ψj代表时钟信号在分组传达的过程中随机所产出的时延.

主从基站间的时钟同步一般情况下是追踪2个接收时间来完成，首先是主基站广播CCP的时刻（依据主基站当地时钟）和CCP在传播所需要的时间，其次是从基站收到CCP的时刻（依据从基站当地时钟）.

从图3可以看出，主从基站时钟同步步骤是使用主基站的CCP发射时刻，3个从基站的CCP收到时刻，还有主从基站的最优预测时间.假定主从基站的位置信息是确定的，则CCP在信道中传播的时间可以依据传播距离与传播时间的比值来取得，最后通过时钟追踪便可以推导出主基站和从基站的真实时钟误差和最优预测时钟误差.卡尔曼滤波（Kalman filter）和数字锁相环（Digital Phase-Locked-Loop，DPLL）其原理性能还有结构都较为相似，因此2种技术在时钟校正和时钟同步追踪中都有着广泛的应用.与传统卡尔曼滤波相比，DPLL要进行周期更短的时钟同步信号发射，也就意味着要更多的发射次数.另外，卡尔曼滤波还降低了时钟同步信息分组的占用空间，可以将更多的时间用来接收定位信息的数据包.因此，在IR-UWB定位中采用卡尔曼滤波进行时钟同步追踪是更好的选择.

图3 相对时间偏移追踪

2 基于卡尔曼滤波的时钟追踪方法

卡尔曼滤波进行时钟追踪的原理是使用眼前时间的观察数值，前一时间节点的估计数值和估计偏差一起推算出一个最佳数值来估计下一个时间节点的数值，相当于是使用最小均方误差来完成最优估计［11］.在滤波器的真实操作中，第k+1个同步信号主基站和从基站所产生的信号漂移是依据第k个周期的数值进行估计.假定发射时钟同步信号的发射周期为T，Uθ，k是第k个周期信号所产生的校对更正偏移量，Uγ，k是第k个周期信号所产生校对更正漂移量，则θk是第kT周期信号的偏移量，γk是第kT周期信号的漂移量.因此在(k+1)T这一时间点，时钟信号的迭代关系式为

其中，ωθ，k是时钟信号偏移方差值，ωγ，k为时钟信号漂移方差值.给定ωk=[ωθ，kωγ，k]T，其加性协方差矩阵为Q.界定输入向量和矩阵如下

卡尔曼滤波的迭代过程为：

估计

最小均方误差矩阵估计

其中，P k是X k的最小误差估计矩阵.

卡尔曼增益矩阵

其中，R k+1是观测噪声的协方差矩阵，测量矩阵H k+1为单位矩阵.

校正

最小均方误差矩阵

3 防碰撞卡尔曼滤波算法

综上可知，卡尔曼追踪器比较适合用作IR-UWB室内定位主从基站的时钟同步追踪.但是使用卡尔曼滤波有一个前提：全部的计算数值必须是实际信号信息和高斯噪声所构成.基于此原则，卡尔曼滤波器才能够在有噪声的信息中通过计算获取准确的预测.当TAG的位置信息数据和主基站发射的CCP信号碰撞在一起便容易产生错误数据，这些错误的数据会干扰滤波器做出正确的判断，常常将那些错误的数据认定成可以信赖的偏移和漂移量，并且依据这些错误的量值进行计算和预测最终产生较大误差.因为卡尔曼滤波器是一个低通滤波器，纠正错误的能力和速度都比较差.所以一旦有碰撞或错误引起误差，此误差会持续影响主从基站时钟同步算法的鲁棒性.

因此，笔者提出一种监控错误和避免错误发生的算法.算法如下：

卡尔曼滤波增益矩阵

界定一个信息矩阵X，

其中，X代表实际偏移与估计偏移的误差值，该信息可以反馈出卡尔曼滤波在这一时刻预测结果与真实结果的关联程度.

假定一个偏离指标（Outlier Metric，OM）认定为此刻读取数值不被信赖的门限指标，依据仿真结果选定为上一时刻预测结果的4～7倍.一旦数据超过偏离指标时，改进的可纠错卡尔曼滤波会将此数据认定为一个错误数据并且停止对当前状态的更替，继续采用上一个正确的预测值来对该时刻预测结果进行平滑处理，防止较大的数据错误对卡尔曼滤波状态更新的准确性产生强烈的干扰.

4 实验结果与分析

采用Matlab进行仿真，对真实偏移量和卡尔曼滤波器预测的偏移量进行记录和追踪.给定主基站和从基站的位置信息坐标分别是（1.2，1.21，1.88）和（12.1，1.22，1.31）.主基站发射CCP信号到从基站需要的时间为T OF主-从=0.000 000 035 625 26 s，发射CCP的时间间隔为200 ms，仿真过程给定噪声为4e-12，仿真结果如图4所示.

图4 时钟同步消息在有较大错误信息时引起的时钟偏差

图4a和b中蓝色虚线为真实偏移数值，红色实线为卡尔曼预测的偏移值.在仿真过程中，在100 s和400 s时人为引入大数据误差，可以从图4a中看出使用普通的卡尔曼滤波进行时钟偏移预测，当遇到较大的错误时，预测结果会误以为错误数据是可信值，从而使用错误值来更新当前状态，从而使系统产生一个巨大的波动，并且修正也需要一定时间.在使用改进的一种防碰撞的尔曼滤波时，由于引入了偏离指标，当数据的数值超过偏离指标时则默认此数据为不信任的数据，对输出的预测时钟偏移量进行平滑处理.从图4b中可以看出，改进的防碰撞可纠错卡尔曼滤波抗突发错误的能力大大提升，能够识别错误数据，并迅速对错误数据做出抛弃处理，整个滤波系统的准确性和稳定性都有较好的提升.

为了更好地验证改进的防碰撞卡尔曼滤波时钟同步性能，以实验室团队开发的HN EVK RTLS 4.0系统为硬件测试平台，搭建测试环境.选取实验室办公室场景为本次的测试环境，区域大小为6*10的矩形区域，由一个主基站和3个从基站所构成，基站坐标分别为（0，0）（10，0）（10，6）（0，6）.测试环境如图5所示.

图5 测试环境照片与平面示意图

设定基站M B1为时钟同步基准基站，基准基站MB1周期性地向区域内3个从基站发射时钟同步数据检验包（CCP），使该区域内基站间时钟同步保持一致.移动节点发射定位数据，所在接受范围的基站都会接收定位数据包.依据衡量出的到达时间差来预测移动节点的坐标.基站M B1和基站SB2的到达时间差为

定义距离误差e则

在基站间保持绝对时钟同步的前提下，移动节点发射时钟检验包到基站间的传送时间差值即为TDOA数值，假设没有其他噪声干扰，则距离误差e等于0.在实际环境测试中，在基站间没有保持绝对时钟同步的情况下，TDOA衡量值误差除了噪声影响，剩下的主要影响便是基站间时钟差所造成的，距离误差e会随着时钟差增大而增大.

使用相同的方法获取距离误差e2和e3.将每一个距离误差进行30次的测量，获取30个数据.测试传统协议的时钟同步方案和改进的防碰撞卡尔曼滤波同步算法，2个方案的距离误差计算出的累计分布函数（CDF）如图6所示.

图6 2种同步算法C D F误差

从图6中可以看出，e1在最左边表明测距误差是最低，e3在最右边表明测距误差是最大.对比图6（a）和6（b）可以看出，在使用改进卡尔曼滤波同步算法后，可以将距离误差缩小3 cm左右.由于改进的防碰撞可纠错卡尔曼滤波，在碰撞错误发生时能快速识别出错误数据，对预测结果进行快速有效的平滑处理，较好地提升了系统的时钟同步性能.

5 小结

对于IR-UWB定位系统，主从基站间的时钟同步对系统的稳定性和定位的精准性有着重要的影响.采用卡尔曼滤波进行时钟偏移预测是主从基站时钟同步的一个常用方案，但卡尔曼滤波纠正错误的能力和速度都比较差，这会使得当错误发生时系统的鲁棒性较低.笔者提出一种防碰撞可纠错的卡尔曼滤波，在碰撞错误发生时能快速识别出错误数据，对预测结果进行快速有效的平滑处理，较好地提升了系统准确性和鲁棒性.