基于DS-TWR 的室外远距离定位算法优化

刘 唯，尹 磊，沈子阳

（南京理工大学 机械工程学院，南京 210094）

UWB 定位技术[1]是一种基于超宽带无线电波传输的定位技术，具有较高的定位精度和抗干扰能力，适用于室内移动设备定位、室外无人空旷地带定位、多路径等复杂环境下的高精度定位应用。文献[2]利用UWB 技术进行煤矿井下定位研究，在复杂环境下，其定位误差控制在了0.5 m 以内。文献[3]通过UWB 测距技术设计出用于医院内的三维定位系统，最大误差为0.244 m，最小误差达0.01 m。由于本次应用的定位目标较大，预计为2 m×2 m 的占地面积的障碍物，故将采用多个标签对应1 个目标的方式。

常用的UWB 定位方法有基于到达时间（TOA）、基于到达时间差（TDOA）、基于到达角度（AOA）和基于接收信号强度（RSS）等。TDOA 标签容量大、精度高，但需要所有基站保持时间同步，较难实现[4]。TOA 不需要基站的时钟严格同步，大大降低应用难度，在成本和可靠性上更具优势[5]。考虑到精度问题，衍生出了双边测距（TWR）法，从精度的角度出发，DS-TWR 是最优选择，能够有效减少晶振偏移导致的测距误差。

出于对实际应用场景的考虑，基于常规DS-TWR定位系统，作出如下改进：基站对标签的应答不再是一对一及时回复，而是延时的广播回复，同时引入动态查询机制保证系统的性能和稳定性；考虑到系统的容量问题，本文引入了Hash 算法，对标签进行标记和管理。此外，系统加入一种改进卡尔曼滤波，对DS-TWR 算法得到的测距值的精确度进一步提高。

1 DS-TWR 测距原理

SS-TWR 的信息交互过程如图1 所示。节点A、B 每接收或发送信息时都将记录下当时时间戳[6]，节点A、B 间通信飞行时间可表示为

图1 SS-TWR 信息交互图Fig.1 SS-TWR information interaction diagram

式中：Tround表示节点A 发送消息到节点B 和接收到回复消息的时间间隔；Treply表示节点B 接收到消息并发送回复消息的延时；Tf表示通信飞行时间。

受设备晶振偏移影响，测量的飞行时间和其误差函数可表示为

式中：eA、eB是节点A、B 的晶振偏移（单位：ppm）。易看出，Treply将会引起系统误差[7]。

为解决此问题，DS-TWR 将重点放在减少晶振时钟偏移的影响[8]。在SS-TWR 的基础上，DS-TWR多了一轮节点间的通信，额外获得2 个时间戳，即多获得1 次测距结果。DS-TWR 的信息交互过程如图2 所示。在实际应用中，一般将延时Treply1和Treply2设置一样，故DS-TWR 的飞行时间、测量的飞行时间和误差函数可表示为

图2 DS-TWR 信息交互图Fig.2 DS-TWR information interaction diagram

从式（6）可看出，测距的误差主要来源于Treply1和Treply2之间的差值[9]，且呈线性关系，考虑到时钟偏差，故二者差值会非常小（在设置一样的情况下）。

对DS-TWR 的误差函数进行数学建模，如图3所示，得出在不同延时差值和时钟偏差的情况下，测量飞行时间的误差。在所取的最极端情况下，即延时差值100 μs，时钟偏差20 ppm 时，测量的飞行时间飞行误差约为0.0005 μs，此时的测距误差大约为0.15 m，满足精度要求。

图3 DS-TWR 信号飞行时间测量误差Fig.3 DS-TWR signal time-of-flight measurement error

2 改进方案的实现

2.1 通信方法的改进

常规的DS-TWR 算法中，以较多的通信次数为代价来减少测距误差，但是单次测距的通信次数变多会需要更多的信道容量，从而系统的定位标签的容纳量将会减少[10]。因此，本文提出一种改进的通信流程，如图4 所示。由标签发起测距消息，基站在接收到消息后延时Δt，再对所有的标签消息统一回复。每次测距的步骤如下：

图4 改进通信流程Fig.4 Improved communication process

步骤1标签1 向基站发送RNG_Tag1 消息，并记录此时的时间戳TRNG_Tag1_tx。

步骤2基站接收到RNG_Tag1 消息时，记录时间戳TRNG_Tag1_rx，并在延时的Δt 时间内保持接收状态，在此时间内，基站会依次记录接收到其他标签的消息时的时间戳TRNG_TagN_rx。

步骤3Δt 延时后，基站开始向所有已发送RNG_TagN 消息的标签发送RES 消息，并记录时间戳TRES_tx。

步骤4标签接收到RES 消息后，记录接收时的时间戳TRES_TagN_rx，依次在延时ΔtN后，将收到的其他时间戳打包进FIN_TagN 消息并发送给基站，记录下发送FIN_TagN 消息的时间戳TFIN_TagN_tx。

步骤5基站接收到FIN_TagN 消息后，记录时间戳TFIN_TagN_rx，解析消息获得各个时间戳，再通过DS-TWR 算法计算距离。

在此通信方式下，基站多数时间将处于接收状态，大大降低在发送状态时无法接收标签消息而导致的测距冲突的发生，从而增加了系统的标签容纳量。实际应用中，将设定延时Treply都为Δt 以降低通信产生的误差[11]。

2.2 Hash 算法

本次Hash 算法采用的是取模算法，方法如下：首先针对一个标签的通信地址，即一个与之对应的ID，使其乘上一个很大的质数2654435761（232与黄金比例相乘的近似整数，以确保Hash 函数的结果在模运算下的均匀性和散列性能），并加上Hash 算法迭代次数的平方，这里的迭代次数用于解决Hash冲突，表示每发现一个被占用的Hash 值，将进行重新计算并迭代次数加1，然后对此结果用基站（本文应用场景为4 基站）的总数取模并加1，得到最终的Hash 值。同时，为解决Hash 冲突问题，本文采用了开放地址法中的线性探测法。如果一个标签的Hash值产生冲突，则在线性探测法下会逐个检查后续的Hash 值，直到找到未被占用的Hash 值为止[12]。如果所有的Hash 值都被占用了，那么这个标签就无法和该基站进行通信。

此外，为了保证系统的性能和稳定性，本文加入了动态查询机制，当标签数量增加时，增加基站响应时间来减小Hash 冲突的可能性，以处理更多的标签。同样，当标签数量减少时，可减少基站响应时间以提高响应速度。本文采用MATLAB 仿真模拟动态查询机制的应用，标签数量的变化和响应时间变化的关系如图5 所示。

图5 动态查询Fig.5 Dynamic query

2.3 改进卡尔曼滤波

卡尔曼滤波算法根据系统的运动方程由上一时刻的系统状态和预测状态推测下一时刻的系统真实状态[13]，解决估计值和测量值之间的权重关系，从而得到一个更接近真实值的结果。约定：以下方程中，带有符号“^”的为估计状态，带有符号“-”的为先验状态。故常规卡尔曼滤波的状态预测方程可以写成：

状态更新方程可以写成：

式中：Xk为系统状态方程；Uk为系统控制输入；A、B是状态转移矩阵；Pk为估计误差的协方差矩阵；Qk为系统过程噪声的协方差矩阵，会随着预测误差加权调整；Kk为卡尔曼增益；H 为观测矩阵；Rk为观测噪声的协方差矩阵，调整过程与Qk类似；Zk为观测变量；I 为单位矩阵。在本系统中可以忽略系统控制输入。

在原有卡尔曼滤波的基础上，为获取更高的测距精度，本文进行了自适应的加权改进，如式（12）、式（13）所示，增加加权系数wk，根据噪声观测值与预测值之间的偏差，不断更新自适应加权系数。偏差越小，加权系数越趋近于1，观测值的置信度较高，应获得较高权重，反之，加权系数越趋近于0，加权系数的置信度较低，应给予较低的权重。

使用MATLAB 仿真该算法的优化程度，仿真结果如图6 所示。

图6 滤波算法仿真Fig.6 Filter algorithm simulation

3 实验结果

3.1 实验环境

实验硬件平台采用蓝点无限公司的BP-400模块。在模块中，使用STM32F401CC 为主控芯片，DW1000 为射频芯片，采用38.4 MHz 的晶体振荡器作为外部晶振，天线为200 mm 的3000～7000 MHz棒状全向天线。采用DS-TWR 测距方式，在空旷环境下采集定位标签与这4 个基站间的距离信息。设定1 号基站与上位机连接，并通过串口将信息传输至上位机，在上位机通过本文的改进卡尔曼滤波算法对得到的距离信息进行处理，从而得到更为精确的测距值。

实验场景选取一个空旷开阔的100 m×100 m 的户外环境，在实验区域内建立直角坐标系，5 个标签为一组，模拟一个需定位的障碍物，在上位机上显示出标签和基站之间的距离测量值，再与真实距离进行误差的比对分析。

3.2 冲突率对比

由于测试通信冲突的实验需要大量的定位标签，从成本方面考虑，本文采用MATLAB 仿真模拟多标签通信。模拟通信规定：设定固定的4 个基站，编号为1～4 号，每个标签和4 个基站按照编号顺序依次通信，若标签发起通信时，基站正与其他标签通信，则标签进入1 s 的等待时间，等待时间结束，记录为1 次通信冲突并继续与下一个基站通信，以此类推，最终统计通信冲突次数并计算冲突率。

改进前，在500 个标签中随机选择80 个标签与4 个基站进行通信[14]，单次通信2 ms，测距6 ms，总计8 ms，在周期400 ms 内进行仿真，改进前结果如图7 所示。改进后，在500 个标签中随机选择80 个标签与4 个基站进行通信，单次通信2 ms，3～20 ms内基站处于接收状态，在400 ms 内进行仿真，改进后结果如图8 所示。改进前的通信冲突率达到47.5%，改进后的通信冲突率下降至11.25%，结果表明，经过改进，通信冲突率大幅降低，能够有效缓解通信失败的问题。

图7 改进前通信次数Fig.7 Number of communications before improvement

图8 改进后通信次数Fig.8 Number of communications after improvement

3.3 改进卡尔曼滤波测距实验

在无卡尔曼滤波、标准卡尔曼滤波、改进卡尔曼滤波3 种情况下进行测距实验，分别以20 m、40 m、60 m、80 m 这4 个距离进行多组测距对比，使用平均绝对误差（MAE）、均方误差（MSE）和均方根误差（RMSE）进行分析[15]，结果保留3 位小数，如表1～表4 所示。

表1 20 m 测距误差分析Tab.1 Error analysis of 20 m range measurement

表2 40 m 测距误差分析Tab.2 Error analysis of 40 m range measurement

表3 60 m 测距误差分析Tab.3 Error analysis of 60 m range measurement

表4 80 m 测距误差分析Tab.4 Error analysis of 80 m range measurement

由表中数据可见，距离对测距的精度存在较大的影响。在无滤波的情况下，测距值与实际值相比，存在较大误差，而在使用标准卡尔曼滤波算法后，测距值相比在无滤波的情况下更接近实际值。同时，使用改进的自适应加权卡尔曼滤波算法后，测距的精度又有了进一步的提升，与无滤波的情况相比，误差减小了约76%。

4 结语

本文提出了一种基于DS-TWR 算法的室外远距离定位的算法优化方案，该方案在通信方面能有效减少系统完成测距所需要的通信次数，从而降低冲突发生。同时引入Hash 算法和动态查询机制，保证系统的标签容纳量和测距成功率。在测距算法方面加入了自适应加权卡尔曼滤波算法，提高测距精度以确保定位坐标的准确性，通过实验，证明了该算法的有效性和准确性。