抗差自适应容积卡尔曼滤波在UWB室内定位中的应用_高嵩.pdf

第 11 卷 第 1 期 导航定位学报 Vol.11，No.1 2023 年 2 月 Journal of Navigation and Positioning Feb.，2023 引文格式：高嵩，宋佳鹏，房穹，等.抗差自适应容积卡尔曼滤波在 UWB 室内定位中的应用J.导航定位学报,2023,11(1):142-147.（GAO Song,SONG Jiapeng,FANG Qiong,et al.Application of adaptively robust cubature Kalman filter in UWB indoor locationJ.Journal of Navigation and Positioning,2023,11(1):142-147.）DOI:10.16547/ki.10-1096.20230121.抗差自适应容积卡尔曼滤波在 UWB 室内定位中的应用 高 嵩，宋佳鹏，房 穹，张熙为（辽宁工程技术大学 测绘与地理科学学院，辽宁 阜新 123000）摘要：针对超宽带（UWB）测距异常值、传统滤波方法中动力学模型不精准、状态向量误差协方差阵非正定等问题，提出一种基于奇异值分解的抗差自适应容积卡尔曼滤波算法，并将其应用于 UWB 室内定位中：以标准容积卡尔曼滤波（CKF）算法为基础，利用残差向量构造抗差因子消除观测异常值对定位解的影响；利用自适应因子对整体模型误差进行实时调整和修正以提高滤波精度；同时用奇异值分解代替乔莱斯基（Cholesky）分解以进一步提高滤波的稳定性。实验结果表明，所提算法相比传统的扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、CKF 算法，能够进一步提高 UWB 系统的定位精度和抗干扰能力，定位最大误差由 1.5 m 降至 0.3 m，均方根误差小于 0.05 m。关键词：超宽带（UWB）定位；奇异值分解；容积卡尔曼滤波；测距异常值；系统噪声 中图分类号：P228 文献标志码：A 文章编号：2095-4999(2023)01-0142-06 Application of adaptively robust cubature Kalman filter in UWB indoor location GAO Song,SONG Jiapeng,FANG Qiong,ZHANG Xiwei（School of Geomatics,Liaoning Technical University,Fuxin,Liaoning 123000,China）Abstract：Aiming at the problems of ultra-wide band(UWB)ranging outliers,the inaccuracy of dynamic model in traditional filtering methods,and the non-positive definite state vector error covariance matrix,a robust adaptive volume Kalman filter algorithm based on singular value decomposition was proposed and applied to UWB indoor positioning.Based on the standard cubature Kalman filter(CKF)algorithm,the residual vector is used to construct the robust factor to eliminate the influence of the observed outliers on the positioning solution,and the adaptive factor is used to adjust and correct the overall model error in real time to improve the filtering accuracy.At the same time,the singular value decomposition is used to replace Cholesky decomposition to further improve the filtering stability.Experimental results show that compared with traditional extended Kalman filter(EKF),unscented Kalman filter(UKF)and CKF algorithms,the proposed algorithm can improve the positioning accuracy and anti-jamming ability of UWB system better.The maximum positioning error is reduced from 1.5 m to 0.3 m,and the root mean square error is less than 0.05 m.Keywords:ultra-wide band(UWB)positioning;singular value decomposition;cubature Kalman filter(CKF);ranging outliers;system noise 0 引言 随着中国经济的转型升级，特别是中国制造2025 国家战略的实施以来，目标定位技术在诸多领域成为研究的热点，人们对高精度定位技术的需求愈发强烈，希望其可以加快国民经济建设的 收稿日期：2021-08-05 基金项目：国家自然科学基金项目（42074012）；辽宁省重点研发计划项目（2020JH2/10100044）；辽宁省自然科学基金计划指导计划项目（2019-ZD-0051）；辽宁省教育厅基础研究项目（LJ2020JCL016）。第一作者简介：高嵩（2000），女，辽宁沈阳人，本科生，研究方向为室内外一体化定位。通信作者简介：张熙为（1998），男，辽宁阜新人，硕士研究生，研究方向为室内外导航定位。第 1 期 高 嵩，等.抗差自适应容积卡尔曼滤波在 UWB 室内定位中的应用 143 步伐并助力产业结构的升级。在良好的室外环境下，全球卫星导航系统（global navigation satellite system，GNSS）可提供连续高精度定位结果；但在复杂的室内环境下，由于 GNSS 信号受遮挡以及强烈的多路径效应影响，GNSS 的室内定位精度显著下降甚至无法定位。因此需要考虑采用其他定位技术实现室内定位。在众多室内定位技术中，超宽带（ultra-wide band，UWB）由于其测距精度高、抗多路径性能强等优点，逐渐成为室内定位技术的研究热点。虽然 UWB 凭借其带宽大、时间分辨率高的优点可以获得高达厘米级的测距精度，但在实际的应用过程中将不可避免地产生测量误差1。UWB 测距误差主要为标准时间偏差和非视距（non-line of sight，NLOS）误差。标准时间偏差一般容易改正，可通过改正函数对其进行校正2。针对 NLOS 误差的处理，主要分为抗差滤波处理方法和根据数据信号特征鉴别方法 2 类3。经过标准时间偏差改正和 NLOS 误差削弱后的 UWB测距值的精度得到很大的提高；但 UWB 测距值中还会存在一些测量值误差，这种误差在滤波定位解算过程中表现为观测值异常，会严重影响滤波解算结果的精度与稳定。针对观测值异常的问题，有学者将抗差估计理论与滤波算法相结合，提出了抗差卡尔曼滤波、抗差扩展卡尔曼滤波、抗差无迹卡尔曼滤波等滤波算法4-5，这些算法通过对异常值降权处理抑制其对滤波解算结果的影响，能大幅度提高滤波解算结果的精度。此外在实际的运动过程中，载体的运动模型一般难以用精确的数学模型构建，导致动力学模型与载体的真实运动模型之间存在误差，这会使滤波器的动态性能受到较大影响6。为了有效控制动力学模型误差的影响，有学者提出了自适应无迹卡尔曼滤波和自适应容积卡尔曼滤波7-8，这些滤波算法通过自适应因子控制动态扰动异常，能提高滤波算法的精度和稳定性。本文在充分吸收抗差滤波、自适应滤波和容积卡尔曼滤波各自优点的基础上，同时顾及在实际运算过程中，由于计算舍入误差、系统噪声的先验统计信息不准确等原因导致状态向量的误差协方差阵失去对称正定性的问题9-10，利用基于奇异值分解的抗差自适应容积卡尔曼滤波（singular value decomposition adaptively robust cubature Kalman filter，SVD-ARCKF）算法进行UWB 定位解算。1 UWB 定位系统的动力学模型和量测模型 UWB 定位模式主要有基于接收信号强度、基于信号到达时间、基于信号到达时间差、基于信号到达角度和信号双向传播时间（round-trip time，RTT）模式11-12。RTT 测距不需要基准站和流动站时间同步13，因此本文选用 RTT 测距模式。在 UWB 的定位中，一般以 UWB 流动站天线的相位中心点的位置和速度信息作为滤波的状态向量。则 UWB 定位系统的动力学模型为 ,kk kkk-=+11xFxw（1）式中：T,xyxypp v v=x为状态向量，其中xp和yp表示流动站天线相位中心的平面位置坐标，xv和yv表示流动站天线相位中心的平面速度；,k k-1F为k时刻状态转移矩阵；kw为 UWB 在k时刻噪声向量，kw对应的协方差矩阵为kQ，且有,pppvpppvk kkvpvvvpvvqqTqqTqqqqq-=10010000010000010000001，FQ 其中：T为 UWB 的采样间隔；q为系统噪声的方差；ppqT=44，pvvpqqT=32，vvqT=2。UWB 的量测模型为 kkkk=+ZH xv（2）式 中：()T,uwbuwbuwbkkkN kNdddddd=-0001122Z，,uwbi kd为k时 刻UWB 流 动 站 到 第i个 基 站 的 平距，,iN=1 2，N为 基 站 个 数，id=0()()iixyxpyp-+-2200，x0和y0为 UWB 流动站概略坐标，ixp和iyp为第i个 UWB 基准站位置坐标；kNNaaaaaa=1112212212000000H，,N kN kNNx ky kddaapp=12，,N kd为k时刻 UWB 流动站和第N个基准站间的真实平距，,x kp和,y kp为k时刻 UWB 流动站天线相位中心的平面位置坐标；kv为 UWB 在k时刻的观测噪声，其对应的协方差矩阵为kR。2 基于 SVD-ARCKF 的 UWB 定位算法 该 算 法 以 标 准 容 积 卡 尔 曼 滤 波（cubature Kalman filter，CKF）算法为基础，利用残差向量 144 导航定位学报 2023 年 2 月 构造抗差因子，消除测距异常值对定位解的影响，利用自适应因子对整体模型误差进行实时调整和修正以提高滤波精度，同时用奇异值分解（singular value decomposition，SVD）代替 Cholesky 分解以进一步提高滤波的稳定性，具体实现步骤为：1）设置初始状态向量0 x及其协方差矩阵0P，有 0T=EE()()=-000000 xxPxxxx（3）2）由于 SVD 数值计算的稳定性较强14，使用SVD 代替传统 CKF 中的 Cholesky 分解。使用 SVD对给定的协方差|kk-11P进行分解并计算容积点,|i kk-11X|T|,|-kkkkkkkki kkkkkkikkinnm-=+=111111111111111121001000100102001001SOPUVOOXUSx（4）式 中：|-kk