1、基于自适应 Kalman 滤波的 DS-TWR 测距方法研究张紫沁冯玉田（上海大学通信与信息工程学院，上海200444）Research on DS-TWR Method Based on Adaptive Kalman摘要：DS-TWR是基于UWB的室内定位技术的主流测距方式。现有方法使用改进卡尔曼滤波算法，利用先验信息和观测点调整观测协方差可以提高定位精度，但在直接提高DS-TWR实时测距值精度方面的研究较少。为了直接减小测距信息中的随机误差，采用经验模型降低DS-TWR存在的天线时延误差，并且提出一种改进的自适应卡尔曼滤波，对得到的实时测距信息进行去噪，从而进一步提高实时测距精度。在真实
2、环境下对该算法做了经验模型和卡尔曼滤波算法两组对照实验，结果表明使用改进后的算法得到的测距估计值与真实距离信息最贴近，且经过经验模型改良和自适应卡尔曼滤波改进后与原标准DS-TWR的误差值明显变小，在厘米级标准上进一步提升了DS-TWR算法的实时测距精度。关键词：超宽带技术；卡尔曼滤波；双边双向测距；室内测距Abstract:Double-sided Two-way Ranging(DS-TWR)is the mainstream ranging method based on UWB indoor posi-tioning technology.Few studies have increa
3、sed DS-TWR range accuracy.Researchers employ the enhanced Kalman filter toincrease location accuracy.This work recommends employing an updated adaptive Kalman filter to denoise real-time rangeinformation to minimize antenna delay error in DS-TWR.Real-world testing compare Kalman filter with empirica
4、l model.Theenhanced approach is more accurate than the empirical model and adaptive Kalman filter.The DS-TWR algorithms real-time ranging accuracy is better than the typical error value at the centimeter level.Keywords:ultra wide band(UWB),Kalman,DS-TWR,indoor ranging基于UWB的室内定位技术的测距方式主要可以分为三类：基于时间的获
5、取方式、基于信号强度的获取方式和基于信号到达角度的获取方式1。其中，基于信号强度的获取方式和基于信号到达角度的获取方式容易受非视距环境和多径效应干扰，用于UWB定位精度差2。因此，基于双向测距的方法和基于到达时间差的定位方法是UWB目前两种主流实现方法，而基于到达时间差方法的定位精度虽高，但需要全基站同步及较大的带宽，不利于实现3。因此，本文选择基于双向测距的方法作为底层测距算法进行研究和改进。基于双向测距方法主要分为单边双向测距（Single-sidedTwo-way Ranging，SS-TWR）和双边双向测距（Dingle-sidedTwo-way Ranging，DS-TWR）两种实现
6、方式。DS-TWR是SS-TWR的一种扩展测距方法，与SS-TWR相比，DS-TWR测距方法增加了响应的时间，但由于其时钟误差只与信号飞行时间段相关，抑制了由于时钟偏差而产生的测距误差值，提高了测距精度4。然而，DS-TWR测距方法在实际测距应用中依然存在其他误差，如由内部芯片、外部组件及印刷电路板所引起天线延迟误差，以及由于测距滤波算法使定位结果与真实位置的定位延迟误差5。卡尔曼滤波算法有着计算量小、精度高的优势，为进一步优化测距精度，文献6通过扩展卡尔曼滤波算法与运动传感器融合提高室内定位精度；文献7提出融合间接平差和卡尔曼滤波算法削弱测距随机误差对UWB定位系统的影响；文献8在无迹卡尔曼
7、滤波过程中引入自适应因子，利用残差向量不断修正增益矩阵。但是以上算法都是利用先验信息和观测点来调整观测协方差来提高定位精度，在直接提高室内定位前的观测量精度方面的研究较少。因此，为了减小观测数据中存在的随机误差，本文提出通过经验模型改进DS-TWR算法的天线时延误差并使用一种改进自适应卡尔曼滤波对DS-TWR算法得到的观测值进行滤波，从而进一步提高高速运动时物体测距值的实时精度。1双向测距系统模型分析1.1双边双向测距原理DS-TWR通过记录两个往返的时间戳，得到基站与标签传输飞行时间9。如图1所示，在时刻A，标签TX主动发起第一次测距消息，在时刻B到达基站RX，Tprop1为时间差。再经过一
8、段时间Treply1后，RX回复给TX测距信息，记为时刻C，并且再经过Tprop2后TX在时刻D收到信息。记录得到A、B、C、D四个时间戳，A时刻和D时刻时间戳相减记作Tround1。当TX收到消息之后马上发起第二次测距消息，消息经过一次往返，又得到四个时间戳A、B、C、D、时间间隔Treply2、Tround2。图1DS-TWR算法流程图最终通过公式（1）求得信号传输的时间，从而换算成距离：Tprop=（Tround1Tround2-Treply1Treply2）（Tround1+Tround2+Treply1+Treply2）（1）1.2系统传播误差分析DS-TWR测距系统在实际应用中得出
9、的测距值存在的误差主要可以分为两类10：1）天线延迟误差：由于设备型号不同，产生的天线延迟各不相同，会造成观测值的不准确。本文通过经验模型进行修正。2）随机误差：主要包括时钟漂移和噪声。时钟漂移会使得基站与标签之间时钟不同步，但由于DS-TWR测距方法是通过两个往返的时间戳基站与标签响应时间差，因此一定程度上降低了时钟不同步的误差。然而，系统中存在的噪声也会对观测值造成影响。本文通过改进的自适应卡尔曼滤波降低该误差。基于自适应Kalman滤波的DS-TWR测距方法研究86工业控制计算机2023年第36卷第2期2改进自适应卡尔曼滤波算法原理2.1卡尔曼滤波算法原理卡尔曼滤波算法根据系统的运动方程
10、由上一时刻的系统状态和预测状态推测下一时刻的系统真实状态11，主要分为时间预测阶段和量测更新阶段：1）时间预测阶段，负责及时推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计：状态预测方程，A为状态转移矩阵：xk=Axk-1（2）预测协方差矩阵，得到系统状态协方差矩阵：Pk=APk-1AT+Q（3）Pk为预测协方差矩阵，Pk-1为k-1时刻的估计状态协方差矩阵，Q=0.01 000系统噪声协方差矩阵。2）量测更新阶段，将先验估计和新的测量变量结合以构造改进的后验估计：计算卡尔曼增益方程：Kk=PkHT（HPkHT+R）-1（4）式（4）中，Kk为k时刻的卡尔曼增益，H=1
11、0为观测转移矩阵，R为观测噪声的协方差矩阵。目标状态更新，xk为k时刻估计状态向量：xk=xk+Kk（Zk-Hxk）（5）优化协方差矩阵，I为单位矩阵：Pk+1=（I-KkH）Pk（6）2.2改进的自适应卡尔曼滤波算法原理理论上，随着滤波次数的递增，卡尔曼滤波误差协方差阵将逐渐趋于稳定。但在实际应用中，随着量测值数目的增加，由于观测噪声协方差和估计误差协方差与真实情况的偏差可能越来越大，滤波值与真实值会造成一定的偏差，因此本文提出一种改进的自适应卡尔曼滤波算法。1）时间预测阶段。状态预测方程，qk为k时刻自适应估计噪声值：xk=Axk-1+qk（7）预测协方差矩阵：Pk=APk-1AT+GQk
12、GT（8）式（8）中，G=1 0.57500为系统噪声转移矩阵，Qk为自适应系统噪声的协方差矩阵。2）量测更新阶段。计算残差e（k），rk为k时刻的残差误差：e（k）=Zk-Hxk-rk（9）计算卡尔曼增益方程：Kk=PkHT（HPkHT+Rk）-1（10）目标状态更新：xk=xk+Kke（k）（11）优化协方差矩阵：Pk+1=（I-KkH）Pk（12）更新自适应估计噪声qk+1、Qk+1、rk与Rk：qk+1=（1-d*bk-1）*qk+d*bk（xk-Axk-1）（13）Qk+1=（1-d*bk-1）Qk+d*bk（Kke（k）e（k）-1Kk-1+Pk-APk+1A-1）（14）rk+1
13、=（1-dbk-1）*rk+d*bk（Zk-Hxk）（15）Rk+1=（1-dbk-1）Rk+d*bk（ZkZk-1）-HPk+1H-1（16）其中，d=1-b1-bk为加权系数，0b1为渐消因子。为证明该自适应算法的有效性，使用Matlab仿真了一段加噪声的正弦信号经过卡尔曼滤波算法与改进自适应卡尔曼滤波算法进行比较，仿真结果如图2所示：图2滤波算法仿真图如图2可见，带噪声的正弦信号经过两个滤波算法后均有所改善。经过卡尔曼滤波算法的信号与带噪声信号的绝对差值和为1220.74，与原始信号绝对差值和为674.45；而经过改进滤波算法的信号与带噪声信号的绝对差值和为1461.23，与原始信号绝对
14、差值和为433.86。由此可见，经改进的自适应卡尔曼算法的滤波效果较标准卡尔曼滤波算法有所提升。3实验结果与分析3.1实验平台搭建测距采用的基站和标签硬件部分均采用蓝点无限公司的BP-400模块作为DS-TWR系统原型硬件平台，该模块使用STM32F401CC控 制 器、射 频 芯 片 为DW1000、外 部 晶 振 为38.4 MHz的晶体振荡器、天线为200 mm的30007000 MHz棒状全向天线标签搜索最近的四个基站。采用DS-TWR测距方式采集在真实环境下采集需定位的标签与这四个基站间的距离信息。1号基站将这些信息进行汇总后通过串口输出至上位机，在上位机通过本文的自适应卡尔曼算法对
15、得到的实时距离信息进行滤波去噪的处理，从而得到更为精确的实时测距信息。本实验的实验环境是一个开阔无遮挡的室内环境。由于该算法主要评估的是高精度厘米级室内测距结果，因此选取了110 cm110 cm的小区域，从而对测距结果更好地进行厘米级误差的分析。在实验区域内建立直角坐标系，载有标签的小车匀速沿设定好80 cm80 cm正方形轨道绕一周，通过Matlab进行建模，估计出小车运动时每个时刻的坐标点，得到小车与四个基站之间的真实距离与时间变化的关系，与实验得到的观测距离值进行误差的比对分析。873.2经验模型方法测距实验本文使用经验模型对该天线延时误差进行调整修正。经过多次实验，设置了合适天线延时
16、时钟数，在最后将测距得到的总传输时钟数减去该天线延时时钟数。以1号基站为例，图3为使用经验模型前后距离观测值测距与真实数据的实验结果对比图。图3经验模型实验结果图3.3卡尔曼滤波算法测距实验本文使用经验模型后得到的实时数据通过串口输出至上位机，上位机把解析后得到的测距信息进行卡尔曼滤波去噪，得到实时去噪后标签与四个基站的距离信息。为证明该自适应滤波效果，记录下通过标准卡尔曼算法和自适应卡尔曼滤波算法的两组测距信息进行对照实验。以3号基站与标签的测距实验结果为例，图4为经过标准滤波卡尔曼算法和自适应卡尔曼滤波算法后的距离观测值与真实数据的实验对比图。图4滤波算法实验结果图3.4实验误差分析对3.
17、2和3.3中的实验结果进行误差分析，实验误差分析使用的是均方误差、平均绝对误差以及均方根误差。其中，均方误差为实验得到的测量值与真实值之差平方的期望值；平均绝对误差为实验得到的测量值与真实值之差绝对值的平均值；均方根误差为均方误差的平方根。表1为本文四个基站与标签运动过程中180个点的实时测距观测值与真实值的误差分析表。由此可见，使用经验模型后的实时测距观测值比标准DS-TWR测距方法得到的实时测距观测值要更加贴近真实值。该经验模型一定程度上消除了天线延时误差对观测值的影响。同时，使用自适应卡尔曼滤波算法后的实时测距观测值比标准卡尔曼滤波算法得到的实时测距观测值要更加贴近真实值。使用自适应卡尔
18、曼滤波算法改进后的DS-TWR测距方法比标准DS-TWR测距方法误差减少约三分之二。在真实环境下，对于室内小范围快速移动的标签测距效果有显著提升。4结束语在使用该方法改进后的测距实验中，高速移动的标签与周围四个基站的平均绝对误差均从原来的40左右降低到20左右，均方根误差均从原来的70左右降低到20左右，显著提升了标签在室内小区域的测距精度，证明了算法的有效性和准确性。参考文献1孙大洋，章荣炜，李赞室内定位技术综述J无人系统技术，2020，3（3）：32-462詹杰，吴伶锡，唐志军无线传感器网络RSSI测距方法与精度分析J.电讯技术，2010（4）：83-873DO T H,YOO M.TDO
19、A-based indoor positioning using visiblelightJ.Photonic Network Communications,2014,27(2):80-884刘永立，吴闻轩，单麒源，等.井下人员定位的超宽带TWR改进算法J.黑龙江科技大学学报，2021，31（2）：135-1395孙宏伟，曹雪虹，焦良葆，等.DS-TWR算法室内定位批量测距系统的优化研究J.电信科学，2022，38（1）：106WANGYAN,LIXIN.AnimprovedrobustEKFalgorithmbased on sigma points for UWB and foot-mou
20、nted IMU fusionpositioningJ.Journal of Spatial Science,2021,66(2):329-3507董佳琪，连增增，徐精诚，等.融合卡尔曼滤波算法在超宽带定位中的研究J.测绘科学，2022，47（5）：10178王孜洲，臧利国，唐一鸣，等.UWB精确定位自适应无迹卡尔曼滤波算法J.农业装备与车辆工程，2021，59（5）：24-28，419H KIM.Double-sided two-way ranging algorithm to reduceranging timeJ.IEEE communications letters:A publication ofthe IEEE Communications Society,2009,13(7):486-48810袁枫，焦良葆，陈楠，等.室内定位中DS-TWR测距算法的优化J.计算机与现代化，2021（10）：100-10611CHEN BADONG,LIU XI,ZHAO HAIQUAN,et al.Maximumcorrentropy Kalman filterJ.Automatica,2017,76:70-77收稿日期：2022-06-18表1位置估计算法数据误差分析基于自适应Kalman滤波的DS-TWR测距方法研究88
