-
随着无线定位技术的不断发展，基于位置服务的研究越来越受到人们的关注[1]。目前，室外定位技术已趋于成熟，在室内定位技术中，超宽带(ultra wideband，UWB)技术利用功率谱密度极低、脉冲宽度极窄的脉冲携带信息，具有极高的时间分辨率和一定的穿透能力，具有比WLAN、蓝牙、RFID等技术更高的定位精度[2-3]。
UWB室内定位一般通过测距方式间接定位，常用的有基于到达角度(angle of arrival, AOA)、基于接收信号强度(received signal strength, RSS)、基于到达时间/时间差(time/time difference of arrival, TOA/TDOA)和基于往返时间(round trip time, RTT)4种测距技术[3-4]。在室内环境下，由于墙体和障碍物的存在会使UWB信号发生折射和反射，从而增加信号传输时间，形成非视距(non line of sight，NLOS)测距误差，极大降低定位精度[5]。文献[6-7]利用视距(line of sight，LOS)和NLOS环境下不同的UWB脉冲波形特征，通过支持向量机进行NLOS的鉴别和误差改正，能有效地鉴别NLOS环境并进行改正，但该方法需要大量的数据样本；文献[8]利用粒子群优化的高斯混合模型对距离量测值进行优化和估计，再利用残差加权最小二乘算法进行定位，能有效削弱NLOS误差的影响；文献[9]将NLOS误差看作有色噪声，提出有色自适应卡尔曼滤波算法对原始测距信息进行滤波；文献[10]提出了基于RTT和AOA混合测距方式，并设计了一种轻量级的网格聚类定位算法，但AOA方法需要天线阵列，会增加额外硬件成本。
针对量测信息异常误差的问题，有学者提出抗差估计理论以抵制量测信息异常误差的影响[11-15]。文献[13]利用新息向量及其对应的协方差矩阵构造自适应因子，利用神经网络输出值构造伪残差向量，从而自适应地调节增益矩阵和量测噪声的协方差矩阵；文献[14]针对量测信息异常和动力学模型误差，利用支持向量回归并结合整体异常检测方法自动选择抗差或自适应滤波，从而提高导航的精度和可靠性；但神经网络和支持向量回归方法均需要大量合理的样本训练，且增加了算法的复杂度。文献[15]利用新息向量构建量测信息的等价协方差矩阵，从而提高系统的抗差能力，但该方法在LOS环境下为标准Kalman滤波，由于LOS环境下系统噪声协方差矩阵无法确定，测距误差也并不服从严格的高斯分布，因而无法进一步提高导航精度。
针对系统噪声协方差矩阵不确定的问题，Sage-Husa算法能对未知系统噪声进行估计和修正，并对滤波的发散进行判断和抑制[16]。本文提出了一种改进的抗差卡尔曼滤波算法，该算法利用新息向量鉴别LOS和NLOS环境, 并分别构造量测噪声协方差矩阵，同时自适应调节系统噪声协方差矩阵，从而消除NLOS测距误差的影响，提高UWB室内导航的精度和可靠性。
HTML
-
在UWB室内平面定位中，至少3个UWB基准站(base station, BS)置于固定位置上，通过测量与UWB移动站(moving station, MS)的距离信息进行定位[3]。以MS平面上的位置和速度作为状态参数Xk=[xk yk vx, k vy, k]T，其中，xk和yk分别为第k时刻MS的x和y方向上的位置；vx, k和vy, k分别为第k时刻MS的x和y方向上的速度，则UWB室内定位系统的状态方程为：
式中，Xk和Xk－1为第k和k－1时刻的状态向量；F为状态转移矩阵，$\mathit{\boldsymbol{F}} = \left[{\begin{array}{*{20}{c}} 1&0&T&0 \\ 0&1&0&T \\ 0&0&1&0 \\ 0&0&0&1 \end{array}} \right]$，T为UWB数据采样时间；wk为动力学模型误差向量，其协方差矩阵为Qk。
UWB测距中RTT方法通过UWB脉冲信号在BS和MS的往返时间确定两者的距离，其不需要BS和MS之间进行时间同步，可消除TOA/TDOA测距中的时间同步误差，且不需要额外的硬件成本[4]，故本文通过RTT方法可得到BS与MS之间的测距信息。假设有1个MS，M≥3个BS，那个第k时刻第i个BS和MS之间的测距模型为：
式中，di, k为第k时刻第i个BS和MS之间的RTT测距值；ri, k为第k时刻第i个BS和MS之间的真实距离，${r_{i, k}} = \sqrt {{{\left( {{x_k}-x_i^b} \right)}^2} + {{\left( {{y_k}-y_i^b} \right)}^2}} $，(xib，yib)为第i个BS的平面坐标；ni, k为测距误差，i=1, 2…m。当第i个BS和MS之间为LOS环境时，ni, k服从零均值的高斯分布；当第i个BS和MS之间为NLOS环境时，ni, k为测距异常值或粗差。
对式(2)进行开平方展开后得到：
式中，mi, k=2di, kni, k－ni, k2。以i=1为基础，以i=2, 3…M进行差值，则可消去(xk2+yk2)项，从而得到：
那么UWB定位系统的量测方程为：
式中，Zk为量测向量；H为量测矩阵；Vk为量测噪声量，Vk=[m2, 1k m3, 1k … mM, 1k]T，mi, 1k=mi, k－m1, k；Vk的协方差矩阵为Rk，且有：
-
基于式(1)和式(5)，UWB室内定位系统为线性系统，其标准Kalman滤波算法[11, 17]预测过程为：
更新过程为：
式中，${\mathit{\boldsymbol{\hat X}}_{k, k-1}}$为预测状态向量；Pk, k－1为预测状态协方差矩阵；Kk为增益矩阵；εk为新息向量；${\mathit{\boldsymbol{\hat X}}_{k-1}}$和${\mathit{\boldsymbol{\hat X}}_k}$分别为k－1时刻和k时刻的估计状态向量；Pk－1和Pk分别为k－1时刻和k时刻的估计状态协方差矩阵; I为单位矩阵。
基于式(8)，当BS与MS之间均为LOS环境时，新息向量服从均值为零的高斯分布；当BS与MS之间出现NLOS环境时，此时存在NLOS测距误差，可认为新息向量服从均值为${\mathit{\boldsymbol{\tilde Z}}_k}-{\mathit{\boldsymbol{\hat Z}}_k}$的高斯分布：
式中，${\mathit{\boldsymbol{\tilde Z}}_k}$为NLOS环境下实际的量测信息；${\mathit{\boldsymbol{\hat Z}}_k}$为NLOS环境下真实的量测信息；Dk为新息向量的协方差矩阵，Dk=HPk, k－1HT+Rk。令c为常数，取检验信息Δεk=εkTDk－1εk，则当Δεk≤c时，可认为BS与MS之间均为LOS环境；当Δεk>c时，可认为BS与MS之间出现NLOS环境，测距信息存在NLOS误差[15]。
在LOS/NLOS混合环境下，既存在测距信息良好值，也存在含NLOS测距误差的测距异常值，为了充分利用测距信息良好值，利用单个新息值构造检验信息：
当Vk, i含有NLOS测距误差引起的异常量测信息时，|Δεk, i|>c；当Vk, i为BS与MS之间为LOS环境下的量测信息时，|Δεk, i|≤c，c为常数阈值。利用检验信息Δεk, i并通过Huber函数[11]构造抗差因子：
令α=diag[α1 α2 … αM－1]，则量测信息的抗差协方差矩阵Rk=αRk，抗差Kalman滤波的更新过程为：
若所有的BS和MS之间均为LOS环境时，式(7)和式(12)组成的抗差Kalman滤波算法退化为标准的Kalman滤波算法，由于系统噪声协方差矩阵无法确定，且BS和MS之间的LOS误差并不服从严格的高斯分布[4]，抗差Kalman滤波算法并不能进一步提高UWB室内定位的精度。Sage-Husa算法能对系统噪声统计特性进行实时估计和修正，能进一步提高滤波的精度，且具有一定的抗干扰能力[16]。结合文献[16]，对系统噪声协方差矩阵Qk进行实时估计：
式中，dk=(1－b)/(1－bk+1)，b为遗忘因子，0 < b < 1。则改进抗差Kalman滤波的预测过程为：
利用式(12)和式(14)实现抗差Kalman滤波的解算。当量测信息中含有NLOS测距误差项时，通过单个新息值检测判断量测异常情况，利用单个新息的检验信息构建量测信息的抗差协方差矩阵，通过增益矩阵增强UWB室内定位系统的抗差性能；同时，对系统噪声协方差矩阵进行实时估计和修正，得到更可靠和更精确的位置和速度信息。
-
数据由带宽为3.1~5.3 GHz、LOS环境下测距精度为厘米级的通信与测距模块采集，数据采集频率设为2 Hz。3个UWB基准站置于固定位置，其平面坐标分别为(3.549，100.477)m、(-3.336，100.521)m、(-3.051，93.963)m，UWB移动站固定在实验小车上，初始平面坐标为(-3.103，97.177)m，沿预定轨迹缓慢移动。整个实验中，UWB基准站高度不变, 并假定UWB移动站高度不变，在原始测距信息中减去高度差部分获得平面的测距信息。为了验证所提出算法的有效性，在第30~620 s间对其中1个测距信息每隔10 s添加随机噪声，添加噪声后的测距信息如图 1所示，并设计以下4种方案对计算的位置和速度结果进行对比：①标准Kalman滤波；②改进的Sage-Husa自适应滤波；③新息检测的抗差Kalman滤波；④改进的抗差Kalman滤波。
Figure 1. Range Information Added Noise
4种方案中的初始Qk和Rk均设为相同，方案②采用文献[16]中提出的改进Sage-Husa算法2，其中遗忘因子b1和b2分别设置为0.8和0.995。方案③采用文献[15]中提出的抗差算法。在静态LOS环境下, 利用标准Kalman滤波估计的新息检验值(见图 2)确定方案③和方案④中的阈值c=0.01。方案④中的遗忘因子b设置为0.995。
Figure 2. Check Values of Innovation in Static Test
图 3-6分别为4种方案解算的平面轨迹，其中黑色实线为各方案解算结果，红色虚线为参考轨迹。
Figure 3. Trajectory of Scheme
Figure 4. Trajectory of Scheme
Figure 5. Trajectory of Scheme
Figure 6. Trajectory of Scheme
图 7和图 8分别为4种方案在x方向和y方向上的位置误差序列图，图 9和图 10分别为4种方案在x方向和y方向上的速度误差序列图，图 11和图 12分别为方案②、方案③和方案④在x方向和y方向上的速度误差序列图，其中黑色、绿色、蓝色和红色线分别为方案①-④的误差序列，表 1给出了4种方案位置和速度误差的均方根和最大值。
Figure 7. Position Errors in x Direction of for Schemes
Figure 8. Position Errors in y Direction of Four Schemes
Figure 9. Velocity Errors in x Direction of Four Schemes
Figure 10. Velocity Errors in y Direction of Four Schemes
Figure 11. Velocity Errors in x Direction of Three Schemes
Figure 12. Velocity Errors in y Direction of Three Schemes
项目 方案 均方根误差 最大误差 x y x y 位置/m ① 0.089 0.086 0.681 0.668 ② 0.055 0.048 0.363 0.249 ③ 0.029 0.027 0.205 0.216 ④ 0.017 0.013 0.098 0.070 速度/m·s-1 ① 0.244 0.199 1.320 1.185 ② 0.012 0.014 0.073 0.047 ③ 0.049 0.041 0.191 0.208 ④ 0.009 0.010 0.065 0.033 Table 1. RMS and Maximum Errors of Four Schemes
分析以上实验结果，可以得出：
1) 由图 3和图 7-12可知，由于人为加入随机噪声模拟NLOS测距异常值，此时标准Kalman滤波导航位置和速度解算中出现大量粗差，最大位置误差约70 cm，最大速度误差大于1 m/s，不具有抗干扰能力。
2) 由图 4和图 7-12可知，Sage-Husa自适应滤波能对系统噪声协方差矩阵和量测噪声协方差矩阵进行实时估计和修正，具有一定的抵制NLOS测距误差能力，使最大位置和速度误差降低至40 cm和7 cm/s，但不能完全消除NLOS测距误差对导航解算的影响，抗干扰能力有限。
3) 由图 5和图 7-12可知，新息χ2检测的抗差Kalman滤波能合理利用量测信息，通过单个新息量构造的检验因子合理地调节量测噪声协方差矩阵，能有效地消除NLOS测距误差对滤波解算的影响，其最大位置误差约20 cm，较好地抵制粗差干扰，但由于不能对系统噪声进行较好的估计，在不存在NLOS测距误差的情况下无法进一步提高滤波解算精度。
4) 由图 6和图 7-12可知，所提改进算法在考虑到系统噪声无法确定情况下，不断地对系统噪声协方差矩阵进行估计和修正，提高在LOS环境下的导航解算精度；当量测信息中存在NLOS测距误差项时，利用新息向量合理调节量测噪声协方差矩阵，降低含NLOS测距误差的量测信息权重，有效地消除NLOS测距误差对导航解算的影响，其最大位置误差和速度误差分别约为9 cm和6 cm/s。相较于方案①-③，所提改进算法能大幅提高位置和速度解算精度，抗差性能更强，可靠性更好。
-
UWB室内导航中由于受到NLOS测距误差的影响而降低其精度和可靠性，本文将抗差估计理论引入到UWB室内导航中，在此基础上针对系统噪声不确定的问题，提出基于新息向量的抗差Kalman滤波方法并应用于UWB室内导航中。在实验中加入NLOS测距误差形成量测信息异常，对标准Kalman滤波、Sage-Husa自适应滤波、新息χ2检测的抗差Kalman滤波和提出的新息向量抗差Kalman滤波进行对比和精度分析，验证了所提改进算法的有效性和可靠性。这说明该方法能有效地消除NLOS测距误差对导航解算的影响，提高室内导航的精度和可靠性，同时也为NLOS误差的鉴别和消除提供了一种新的方法。
DownLoad:
