融合卡尔曼滤波算法在超宽带定位中的研究

近年来，GPS、北斗等全球卫星导航系统为人们的高精度定位导航提供了便利，但是这仅仅局限于室外环境。室内定位，作为导航定位方面的“最后一公里”，依然存在着诸多问题^[1]。

超宽带（ultra-wideband，UWB）技术由于其功耗低、抗干扰性强、穿透性强等特性，在室内无线通信方面非常适用，广泛应用于军事、后勤、安全、医疗、搜索、救援等领域^[2]，成为我们研究的第一选择。但是超宽带技术在测距方面依然存在着不可避免的误差信息，为了能够减少误差对定位的影响，程鑫利用径向基函数（radical basis function，RBF）神经网络，构建UWB测距误差模型以提高测距精度^[3]；文献[4]基于非视距环境下信号传播的误差特性建立卷积神经网络（convolutional neural network，CNN）模型，减小了非视距(nonline of sight，NLOS)误差的影响，文献[5]将UWB信号特征作为分类属性通过随机森林算法进行非视距鉴别，进而修正NLOS测量值，文献[6]利用卡尔曼滤波（Kalman filter，KF）、小波变换、Vondrak滤波3种方法对原始测距信息进行降噪处理。

其中卡尔曼滤波算法在处理线性问题时有着计算量小、精度高的独特优势，应用广泛，如文献[7]通过最小二乘法优化的卡尔曼滤波算法提高室内定位精度；文献[8]利用改进的卡尔曼滤波算法消除测距粗差。但是以上算法都是直接利用观测量修正状态量从而提高定位精度，在提高观测量精度方面的研究较少。

因为仪器误差、人为操作稳定性和实验天气等外界条件的影响，不可避免地出现了测量误差。减小误差对观测量精度的影响从而求得最为接近真实值的结果，并对实验结果精度进行评定，就是间接测量平差的目的。文献[9]在特征目标提取和分析时通过构造间接平差方程提高提取精度，文献[10]证明在相对重力测量数据处理时获取待测点绝对重力值方面间接平差理论同样适用。

因此，为了减小数据中存在的随机误差，本文提出一种融合卡尔曼滤波（fusion Kalman filter，FKF）算法，其优势是在卡尔曼滤波算法中加入间接平差算法，将间接平差算法求解的三维坐标作为卡尔曼滤波算法的观测量，求解得出标签的三维坐标，从而提高定位精度。

本文实验采用Decawave 公司于2012年推出的DWM1000定位模块，通过双边测距算法（double-sided two-way ranging，DS-TWR）获取距离信息，可以减小超宽带在测距过程中因时钟偏移导致不必要的过大测距误差^[11]。

DS-TWR测距算法是在单边测距（single-sided two-way ranging，SS-TWR）算法的基础上增加了一次通讯，如图1所示。

DWM1000超宽带模块因环境因素以及自身系统工作特性的影响，使测量值出现一定的误差。测距误差校正是对测量值进行线性拟合，削弱系统误差，达到更加接近真实值的目的。实验在室内进行，完全处于视距环境，即实验过程中保证接收机与标签之间无遮挡，无行人穿过。按顺序对六个接收机进行误差标定，实验从小到大选定12个待测点，将标签分别放置在待测点处，通过天宝S8全站仪测出接收机到标签的距离作为真实值，同时通过接收机对标签进行测距，每个点采集200次数据并求取平均值作为量测距离。将实验得到的距离信息与实际距离进行对比，确定误差模型。

图2为UWB接收机1拟合曲线，图中正方形横坐标为量测距离，纵坐标为真实距离，对6个UWB基准站依次标定，UWB校正参数如表1所示。

表1 校正参数

平均算法以三边定位算法为基础，流程如图3所示：

图3 平均算法流程图

超宽带定位技术精度较高，但不可避免带有随机误差。为了减少随机误差对定位结果的影响，本文选择间接平差算法求解标签的三维坐标，然后将此三维坐标作为KF算法过程中的观测值，与预测值相结合，求解出更为准确的标签的三维坐标。

KF算法是一种线性滤波器，是在最小均方根误差（minimum mean square error，MMSE）准则下得出的^[12]，它可以根据上一时刻的系统状态和预测状态推测下一时刻的系统真实状态。

当接收机个数等于四个时，可以得到唯一的一组待测标签坐标。但是由这种方式得到的标签坐标带有不可避免的随机误差，为了减少随机误差对结果的影响，本文选择加入多余测站数，从而构建平差模型，通过平差模型解算提升标签坐标值的精度。在一个平差问题中，当所选取的独立参数的个数等于必要观测数时，可将每个观测值表达成这个参数的函数，组建观测方程，这种以观测方程为函数模型的平差方法，就是间接平差^[13]。

KF算法主要包含先验状态估计和后验估计（量测更新）两个部分，先验状态估计的精度依赖建立的状态预测方程，预测方程越接近实际进程，先验估计精度越高。后验估计所得结果的精度主要依赖观测值，传感器得到的观测数据误差越小，KF算法的最优估计值精度越高。

通过间接平差得到的三维坐标可以减小观测值带来的随机误差的影响，并作为KF算法的观测向量，得出精度更高的最优估计值，FKF算法流程如图4所示。

通过MATLAB软件对本文提出的FKF算法进行仿真实验，在仿真实验中，假定有一个5 m×5 m×5 m的场地，标签在场地内从（0,0,0）到（5,5,5）保持匀速直线运动（单位为m），在此过程中，UWB设备持续对标签进行定位，定位点数设定为100个。

根据所选用的UWB传感器，测得UWB设备的测距精度为10 cm，因此，在MATLAB仿真中接收机与标签的真实距离上加入1 db的高斯噪声来模拟UWB定位设备的数据。

通过上述仿真实验，将所获得的数据分别进行不同算法的处理，即平均算法和FKF算法。

首先，对平均算法和间接平差算法获得的标签三维坐标数据进行分析，对定位解算性能进行评估是提高导航系统性能的有效途径^[14]，定位性能评价的指标主要有：均方差与均方根误差、累计分布误差、几何精度因子等^[15]，本文选择均方差作为精度指标。平均算法及间接平差算法结果的均方差如图5所示。

由图5可以看出，平均算法获取的三维坐标的均方差主要分布在5~30 cm之间，占全部数据的90%，而间接平差算法的均方差在5~20 cm之间分布较多，占全部数据的86%；通过计算可知，平均算法均方差的平均值为16.1 cm，而间接平差算法均方差的平均值为12.5 cm，相比平均算法，间接平均算法使精度提升了22.55%。因此，间接平差算法可以有效提升标签三维坐标的定位精度。

其次，将间接平差算法获得的三维坐标作为KF算法的观测数据，从而进行求解，可得如下实验结果，如图6所示。

从图6可以看出，在X、Y、Z任意方向，相比平均算法，FKF算法所得轨迹更加贴合实际，具有更高的平滑度和准确度；为进一步评价这两种算法的性能，根据两种算法的实验结果数据，计算其对应的点位真误差，如图7所示；以及在X、Y、Z方向的真误差和点位误差平均值，如表2所示。

表2 算法结果坐标各方向真误差

通过图7以及表2可知，FKF算法的真误差比平均算法小，在X、Y、Z方向上的精度分别提高了58.76%、41.68%、35.58%，同时在点位精度上也提高了48.08%。因此，通过FKF算法获得的标签坐标更加接近真实值，有效地减小了随机误差对UWB设备精确定位的影响。

扩展卡尔曼滤波（extended Kalman filter，EKF）算法是在标准KF算法的基础上发展起来的，它的基本思想是：在计算初始阶段，将非线性系统线性化，然后再做KF处理。具体是采用泰勒级数展开将非线性系统线性化，然后再用卡尔曼滤波框架对信号作处理，是一种次优滤波^[16]。

粒子滤波（particle filter，PF）算法是一种以贝叶斯推理和重要性采样为基本框架的算法，是通过寻找一组在状态空间中传播的随机样本来近似地表示概率密度函数，用样本均值代替积分运算，进而获得系统状态的最小方差估计的过程^[17]。

用EKF算法和PF算法对上述实验做重复实验，得到如下结果，如图8所示。

通过对上述获取的实验结果数据进行分析可知，FKF算法相比较于EKF算法，在X、Y、Z方向分别提高了34.53%、37.33%、27.59%，在点位坐标精度上提高了33.03%，相比较于PF算法，在X、Y、Z方向以及点位坐标精度上分别提高了30.48%、48.66%、36.17%以及31.81%，因此得出结论，FKF算法相比较EKF算法和PF算法可以有效提高定位精度。

为进一步验证所提算法的可靠性，选取DWM1000超宽带定位模块进行了实地实验。实验范围定为10 m×10 m。为获取标签的三维坐标，选取7个模块，其中6个作为接收机对标签进行定位。测试过程中，标签从坐标点（4.32，-0.04，-0.29）匀速向目标点（7.32，-0.04，-0.29）移动（单位为m）。在标签移动过程中，每隔0.1 m对标签进行定位。获取标签到每个接收机的距离后，采用平均算法和FKF算法分别得出标签坐标值，其点位误差如图9所示。

从图9可以看出，融合卡尔曼滤波算法在定位过程中数据更稳定，随着定位坐标个数的增加，其点位误差趋近于5 cm，平均真误差为5.4 cm，而平均算法点位误差波动范围仍比较大，平均真误差为14.6 cm，相比平均算法，FKF算法定位精度提高了62.74%。因此，采用FKF算法可以有效减小随机误差对标签坐标定位的影响，提高定位精度。

超宽带技术因为高精度测距获得了关注，但是通过此技术获得的距离信息不可避免的带有随机误差。本文融合间接平差和卡尔曼滤波这两种算法各自的优势，提出一种FKF滤波算法来削弱测距随机误差对UWB定位系统的影响，并综合采用仿真和实测的方法进行算法精度验证，得出以下结论。

1）在有多余观测接收机的情况下，通过平均算法和间接平差算法求解标签的三维坐标，其坐标均方差分别为16.1、12.5 cm。因此，相比较平均算法，间接平差算法可以有效提高观测向量坐标值的精度。

2）在仿真实验中，将FKF算法与平均算法、EKF算法、PF算法进行精度比较，得出其三维坐标点位误差分别为8.38、16.14、12.52和12.29 cm，FKF算法可以有效提高点位坐标的定位精度。

3）实地试验结果进一步验证了本文所提算法的有效性，通过FKF算法与平均算法对实测数据进行处理，点位误差分别为5.4 、14.6 cm。因此，FKF算法减小了随机误差对点位坐标的定位精度影响。

值得说明的是，本文仅以视距条件为实验环境验证了所提方法的可行性，在未来的研究中，需要进一步研究动态环境的影响，改进现有的算法，对受NLOS污染的数据校正处理，以扩展该方法的应用范围。