基于GNSS/SLAM组合高精度室内动态目标定位法

张晓娜，张淑芳

(大连海事大学信息科学技术学院，辽宁大连116026)

1 引言

在大多数应用场景，GPS定位系统虽然在室外条件下有着较高的定位精度，但复杂的室内环境产生的信号易受建筑物等障碍遮挡、多径传播等效应使得GPS难以满足在室内的定位需求[1-3]。因此，发展导航及目标定位信息的相关技术，对于如何实时、稳定、高效的提供精确室内的位置信息具有重要意义[4]。

石琴琴[5]等人提出基于RSSI测距的室内动态目标定位方法，通过信标间的真实距离和RSSI衰减值对RSSI测距模型参数进行反向实时修正，通过牛顿迭代法获取室内动态目标的定位，该方法没有对位置信息周跳值进行探测与修复，导致位置误差较大。周牧[6]等人提出基于信号分布混合假设检验的室内动态目标定位方法，通过混合Mann-Whitney U检验/T检验方法构造匹配参考点集合，定位目标区域，该方法不能对各个频率上的小周跳进行高精度修复，导致姿态误差较为严重。王安然[7]等人提出基于卡尔曼滤波预估的室内动态目标定位方法，采用卡尔曼滤波方法对动态目标坐标进行估计，利用分区定位方式进行定位，该方法在探测与修复中噪声波动范围较大，导致速度不稳定，即定位精准度较低。

为了解决存在的问题，提出基于GNSS/SLAM组合高精度室内动态目标定位方法，通过对室内动态观测数据进行预处理，探测出不同频率的周跳值，对大周跳、小周跳均进行修复，获得高质量数据及准确定位，GNSS/SLAM组合导航具有定位精度高，稳定性强等优势，对室内定位及无线通信技术的发展具有广阔的应用前景。

2 室内动态目标定位预处理

2.1 室内动态目标定位去噪处理

信号处理目前已成为现代科学技术中不可或缺的一部分，而在信号处理的许多应用中，信号传输过程中经常受到环境的影响，导致初始信号变成含噪信号，因此，对取得的信号进行去噪是信号处理的关键[8]。基于GNSS/SLAM组合高精度室内动态目标定位方法，通过EMD-阈值处理组合进行去噪，具体步骤如下：

假设原信号为x(t)，高斯白噪声为s(t)，得到含噪信号为y(t)，通过对含噪信号进行去噪处理获得距离原始信号最为接近的去噪信号x(t)，即

(1)

基于EMD对含噪信号进行分解，得到的分量由高到低进行排列，一般情况下，噪声能量大部分存在于高频分量中，低频分量中较少，因此低通滤波去噪方法需舍弃高频分量阶数，重构低频分量，实现去噪目的[9]。从分解后的分量中提取噪声能量和有用信号作为主要模态分界点。运用连续均方根误差准则对IMF分量噪声能量密度进行计算，获得有用信号分量分界点与噪声分量，IMFk表示为有用信号分量分界点，具体步骤如下

(2)

式中，n为分解阶数，N为信号长度。IMF分量噪声能量密度的变化曲线可通过连续均方误差准则获得，在选取对于噪声信号与期望信号均能起主导作用的位置时，应选择曲线局极小值位置作为分界点，并找到对应的噪声能量分布突变每个分解点的IMF分量，分解点k计算公式如下

(3)

当有用信号能量起主导作用时，此时分界点的噪声能量成为全局最小值前的局部极小值。当在全局极小值前存在局部极小值时，k的表达式如下

(4)

(5)

为了有效保留部分有用信号，在阈值处理及选取阈值函数时，假设特定阈值为λ。据小波阈值去噪方法可知，设置阈值的规则主要包括极值阈值准则、启发式阈值准则、固定阈值原则、无偏风险估计阈值准则。固定阈值相比之下比较简单，通常会被优先选用，其计算公式为

(6)

式中，N为信号长度，λi为固定阈值，σe为噪声标准差，Wi，j为信号分解后的分量系数，噪声标准差可采用鲁棒估计进行计算。

小波去噪阈值处理一般会使用硬阈值函数与软阈值函数，通常软阈值函数效果更好一些[11]。软阈值去噪时，对于小于阈值系数的分量进行归零处理，对于大于阈值λ的系数Wλ则选取最初系数与阈值的差额进行计算，经过软阈值处理后，整体效果平展性与持续性较好，表达式如下

(7)

(8)

基于GNSS/SLAM组合高精度室内动态目标定位方法，将EMD-阈值处理组合进行去噪，可高效的剔除大部分存在于高频分量的噪声能量，并使分解中的小部分噪声也随之清除，使去噪效果更佳，去噪目的更准确。

2.2 周跳探测与修复

周跳探测与修复是基于GNSS/SLAM组合高精度室内动态目标定位方法观测数据预处理的核心[12]。具体步骤如下：

1)φ(0，1，-1)组合观测值周跳的探测与修复

设φ(0，1，-1)为无周跳的载波相位组合观测值，利用式(9)、(10)可探测与修复φ(0，1，-1)组合观测值的周跳，表达式如下

(9)

2)φ(1，-3，2)组合观测值周跳的探测与修复

当φ(0，1，-1)载波相位组合观测值周跳实现探测与修复后，根据φ(1，-3，2)组合观测值和无周跳φ(0，1，-1)载波相位组合观测值获得几何无关观测值，通过下式对φ(1，-3，2)相应的周跳进行探测与修复

S(1，-3，2)λ(1，-3，1)=I1+Δφ(0，1，-1)λ(0，1，-1)-Δφ(1，-3，2)λ(1，-3，2)

(10)

式中，Δ描述的是历元差分，S(1，-3，2)为组合观测值φ(1，-3，2)的周跳，I1为历元之间对应几何无关组合观测值的变化程度。

在数据处理时，线性拟合与高次差对φ(0，1，-1)组合观测值的周跳进行探测与修复。

3)φ(-3，1，3)组合观测值周跳的探测与修复

由1)、2)步获得无周跳的φ(0，1，-1)与φ(1，-3，2)组合观测值，可形成无周跳且观测值噪声较小的φ(1，0，-1)组合观测值，即

φ(1，0，-1)=3φ(0，1，-1)+φ(1，-3，2)

(11)

利用该组合与φ(-3，1，3)组合观测值构建对应的组合观测值，通过上述步骤2)的方法对φ(-3，1，3)组合观测值的周跳进行探测与修复，公式如下

S(-3，1，3)λ(-3，1，3)=I2+Δφ(1，0，-1)λ(1，0，-1)-Δφ(-3，1，3)λ(-3，1，3)

(12)

4)初始载波观测值的周跳

上述过程结束后，如前期组合观测值均无周跳发生，那么原始载波相应的观测值也无周跳发生。但如有组合观测值发生周跳，那么可通过下式进行计算周调值[13]。

(13)

周跳探测与修复在高精度数据处理中一直都是比较重要的环节，对于高采样率的非差相位观测数据能够快速和准确地探测及修复周跳，为高精度室内定位提供了较好的数据质量控制。

3 高精度室内动态目标定位方法

SLAM为同步定位与制图技术，GNSS为优越的导航技术，普遍被各个领域所应用。在部分场景中，由于传播环境复杂，传播通道受到干扰，GNSS信号无法被接收，其导航功能失效，因此GNSS/SLAM组合可以获取高精度的环境特征，提供的准确的定位信息[14]。在GNSS SLAM组合算法中，对于参考坐标系下激光雷达坐标系的姿态角、传感器及已观测到的全部地标点的位置向量，均为k时刻的系统状态向量，即表示为：

(14)

N(Xk/k，Pk/k)为系统状态向量的后验概率密度。期望均值与协方差分别表示为

(15)

如GNSS/SLAM组合观测到新的地标点，则将新地标点的位置向量增加到系统状态向量中，详细流程如下

首先进行时间更新，具体方程如下

(16)

式中

(17)

式中，Q为过程噪声矩阵。

其次进行数据关联，将新观测的地标点与初始地标点进行匹配，通过已存在的地标观测值进行计算

k|k=k|k-1+Kkμk

Pk|k=(I-KkHk)Pk|k-1

(18)

其中

(19)

式中，Ra为已存在地标的观测噪声矩阵，za，k为已存在的地标点，zb，k为新观测到的地标点。系统状态增广为

(20)

式中，mb为新观测到地标点距离。将新观测到的地标点位置向量增加到系统状态向量中。

从上述过程中看出，GNSS SLAM组合相对于传统的GNSS增加了数据关联和状态增广的过程，数据关联是GNSS SLAM组合中重要一步，另外特征提取也是GNSS SLAM组合的前提条件[15]。

4 实验结果与分析

为了验证基于GNSS SLAM组合高精度室内动态目标定位方法的整体有效性，进行相关实验。本次实验在开发环境(Graphic User Interface Design Environment GUIDE)下完成开发。UWB 测距性能可能受到外部环境和系统硬件等因素影响，因此在进行目标定位之前首先需要对 UWB 测距进行误差修正。

图1 实验的硬件平台

采用基于GNSS/SLAM组合高精度室内动态目标定位方法(方法1)、基于RSSI测距的室内动态目标定位方法(方法2)、基于信号分布混合假设检验的室内动态目标定位方法(方法3)进行测试。

测试位于一间实验室与一间办公室内，内有桌椅，设备等，其室内布局图如图下图2所示。

图2 实验室环境示意图

假设测量得到的第k个动态目标坐标为Mk(xk，yk)，真实轨迹所有点坐标的集合为Mr(xr，yr)，则定位误差de表示为：

de=min|Mk-Mr|

(21)

具体测试结果如图3所示。

图3 定位误差

由图3可知，在位置误差测试中方法1的误差更接近于0，证明此方法的定位更接近动态目标，位置更精准。方法2与方法3的由于纬度误差、经度误差、高度误差浮动较大，因此位置误差较大，即定位精准度较低。因为方法1将观测数据进行周跳探测与修复，不管对于大周跳还是小周跳，均能探测出各个频率上发生的周跳，能够以一定的精度进行修复，获得高质量数据，使得位置误差更接近于0，即准确的实现了室内目标定位。

在实验中，一个测试人员沿着一个长方形的轨迹匀速移动。动态图目标跟踪测量结果如图4所示。

图4 动态图目标跟踪测量结果

动态目标测量的误差较小。主要原因在于本文方法对观测数据进行周跳探测与修复，探测与修复各个频率上发生的周跳，准确的实现室内目标定位。

5 结束语

动态背景下的运动目标检测与定位，当前方法在周跳探测与修复中对不同频率区域无法进行高精度修复，难以获取高质量数据，导致位置及姿态误差较大、速度不稳定等现象。即室内动态目标定位的精准度较低，对此问题提出了基于GNSS/SLAM组合高精度室内动态目标定位方法，对室内动态观测数据进行预处理，探测出各个频率上发生的周跳，高精度进行修复，获得高质量数据，精准定位室内动态目标，该方法有效地解决了当前方法中存在的问题，为高精度室内动态目标定位的实现提供基础。