一种多源融合室内外无缝定位方法*

姚子扬尚俊娜*孙建强肖江宁王奕腾

(1.杭州电子科技大学通信工程学院，浙江 杭州 310018；2.通信信息控制和安全技术重点实验室，浙江 嘉兴 314033；3.中国电子科技集团公司第三十六研究所，浙江 嘉兴 314033)

随着信息时代的来临，基于位置信息的服务在各种应用场景中扮演的角色愈加重要。室外场景下，全球卫星导航系统(global navigation satellite system，GNSS)已经可以提供实时的可靠稳定的全球定位导航服务。然而在地下车库、交通隧道以及城市峡谷等众多类似室内的场景下，卫星信号丢失，由此也催生了诸多室内定位无线技术，比如超宽带、WIFI技术。如何借助现有的定位技术进行准确的定位愈加关键，多源融合定位技术也相应被提出。它采用信息融合的方法，将卫星导航定位、无线传感器定位以及其他辅助定位技术进行处理，最终获得可靠准确且稳定的定位服务。

多源信息融合最早提出于1973年，美国系统科学家Bar-Shalom Y研究了一种数据融合处理滤波器[1]，初次引出了多源融合技术的概念。多源数据融合技术广泛应用在定位与组合导航领域。文献[2]提出了一种具有普适性的多源融合定位系统架构，从数据处理、定位算法、效能评估等方面对多源融合定位进行了研究。文献[3]研究了异构环境下的融合定位，提出了一种惯性导航辅助的RSSI(received signal strength indicator，RSSI)快速建库方法。文献[4]针对室内外无缝定位进行了研究，在使用多个不同定位源的情况下，提出了新的异构定位系统间切换算法，将不同定位系统的定位信息进行融合处理。文献[5]提出了一种在复杂室内环境下的多源信息融合定位系统，该系统第一个阶段为人群感知阶段，第二个阶段为定位导航阶段，充分使用了蓝牙、WiFi、地磁和图像信息。文献[6]在移动机器人同步定位与建图中，对各类传感器信息在不同层次进行了融合，研究了一种惯导信息组合以及激光视觉信息处理方法。文献[7]将GNSS定位与地磁技术进行了融合处理，根据卫星数以及精度因子实现切换算法设计；文献[8]采用GPS、DR(dead reckoning，DR)和WiFi组合室内外定位，根据信号类型自动切换定位；文献[9]根据各误差系数改进了相关的发生器和滤波器，采用了WPS(wireless positioning system，WPS)、GPS、以及MIMU(miniature inertial measurement unit，MIMU)三种定位源进行了定位实现。文献[10]提出了一种基于计数以及阈值机制的智能切换算法，能在室内外切换的定位场景下满足低能耗高精度的定位。针对组合导航中GPS短时失效的问题，文献[11]采用基于偏最小二乘的辅助高斯过程回归预测设备的轨迹，能在短时间内得到良好的估计精度。捷联惯性导航作为多数多源融合方法中非常重要的组成部分，其计算方法对定位结果有一定的影响，文献[12]比较了基于欧拉角和基于四元数的两种非线性组合导航滤波的定位精度和计算量，实验证明前者精度较差但计算量小，后者则反之。文献[13]利用预测残差建立的统计量调节位置与速度向量，削弱状态模型误差和异常扰动导致的组合导航中卡尔曼滤波性能下降问题。针对精密单点定位动态定位精度低、收敛速度慢等问题，文献[14]采用PPP/INS紧组合系统来改善动态定位性能。在GNSS信号受遮蔽的情况下，文献[15]主要利用了地磁与惯性器件动态修正航向角，结合GNSS的定位结果该方法比未修正时提高了60%的准确率。文献[16]提出采用反距离加权内插的多基准站伪距差分法，修正北斗伪距方程减小空间伪距误差，构建了基于北斗伪距差分与WiFi的室内外融合定位方法，有效解决了单一系统的定位缺陷和定位盲区问题。文献[17]提出了一种结合UWB的GNSS组合导航方法，分别采用了集中式卡尔曼滤波和联邦式卡尔曼滤波的多传感器组合滤波结构进行融合定位，实验验证后者对于异常值具有更强的抗干扰能力。

基于以上背景，为了在常用设备的基础上进一步探究如何实现室内外无缝切换的高精度导航定位，本文主要对气压计、UWB定位、惯性导航定位以及GNSS定位的多源融合定位技术进行研究，设计实现了相应的组合导航定位算法，并最终完成多源融合定位。

1 多源融合室内外定位方案

首先，本文在前人研究的基础上提出了多源融合室内外定位方案的整体设计框图，如图1所示，接下来着重详细讲述室外定位以及室内外无缝定位部分。

图1 多源融合室内外定位设计框图

因为要完成室内外的融合定位设计，待选取定位场景较大，基于UWB的位置指纹定位方案前期要进行较多的准备工作，适用于小的定位场景，因此选用PDR辅助UWB的定位方案；室外定位阶段，选择GNSS组合INS的定位技术手段。使用GNSS信号进行室外定位时，可以采用单点定位或者RTK差分定位。单点定位只需一根天线即可，成本低但定位精度不高，RTK采用了载波相位测量技术，差分定位时使用双天线数据进行差分处理，从算法上极大的改善了定位的精度与性能，接下来首先对GNSS的两种定位模式的实现原理进行讲述，然后进行实验研究测试。

1.1 GNSS定位技术

随着全球卫星数量的增加以及定位系统的完善，基本可以实现全天候、实时的室外定位。根据不同的划分指标，GNSS可以分为不同的定位方式，其中最为常用的是基于伪距的单点定位技术以及基于载波相位测量的RTK定位技术。

本文主要对单点定位以及载波相位定位进行了初步研究。单点定位方法是利用一台卫星信号接收机根据伪距信息确定位置，载波相位定位方法则是利用载波相位测量技术，通过对相位差分确定位置[18]。

①伪距单点定位方法

目标卫星与响应接收机之间的伪距等于卫星信号从发射到到达对应接收机的时间与光速的乘积，计算公式如式(1)所示。

式中:Δt为传播时间，c为光速。

伪距单点定位首先获取当前时刻的广播星历，然后使用为卫星信号解算算法确定相应卫星的位置[19]，接着计算得到卫星信号接收机的到相应卫星之间的伪距值，最终确定目标载体的位置坐标。

②载波相位定位方法

将理想状态的GPS时间作为标准，假设在历元t j(gps)卫星s j发射的载波信号相位为φj[t j(gps)]，此时在历元t i(gps)接收机T i的参考载波信号为φi[t i(gps)]，那么两者的载波相位差为:

考虑钟差、大气折射以及整周模糊度等因素，载波相位观测方程最终可表示为:

由于目标卫星和对应接收机之间的距离远大于接收站处两个天线间的距离，可以忽略卫星信号到对应接收天线的干扰误差。天线1和天线2可以同时测量到两颗GPS卫星k和GPS卫星j的情况，如图2所示，即载波相位测量采用双差法，消除了接收机有关的载波相位及其钟差。

图2 GNSS双差示意图

沿载体的纵轴方向安装两根GPS天线1和2，其坐标分别为(x1，y1，z1)和(x2，y2，z2)，利用载波相位测量方程可得天线1和2基线向量沿载体坐标系的分量:

可以得载体的航向角和俯仰角为:

同时，通过对卫星数据按照相应的格式进行解析，可以获得物体运动的经纬度，在同一个度量基准中，通过式(6)把大地坐标系中的位置信息映射到空间直角坐标系，映射关系如图3所示。

图3 空间直角坐标系与大地坐标

式中:e是椭球的第一偏心率，b是椭球的短半轴，b＝6 356.752 km。

1.2 GNSS定位模式对比实验

1.2.1 单点定位

使用中科微不同类型的单点定位模块，包括5r、5s以及5p型号模块，如图4所示。

图4 单点定位设备

进行室外静态定位精度测试，主要目的时观测单点定位静态情况下的定位误差情况。测试地点为中科苏州地理科学与技术研究院大厦5楼，测试环境如图5所示，测试时长为2.5 h。

图5 测试环境及静态单点定位情况

测试结果显示，单点定位东向最大偏差3.511 m，北向最大偏差4.442 m，东向平均偏差2.896 m，北向平均偏差2.448 m，北向平均偏差2.448 m，定位精度在米级，可见单点定位模式可用于对定位精度要求较低的场景，比如区分两栋大楼的入口或者进行米级模糊定位的场景。

1.2.2 RTK差分定位

使用RTK差分定位设备进行室外静态定位精度测试，测试设备如图6所示，设备集成了GPS卫星信号模块、4G模块，可以通过网络RTK技术实现数据差分定位，获得RTK定位数据。

图6 RTK差分定位设备

测试地点为中科苏州地理科学与技术研究院大厦6楼楼顶，采用上述RTK差分定位设备针对静态情形下的RTK定位进行定位精度的测试，测试时长为1 h，具体测试情况如图7所示。

图7 测试环境及静态RTK定位情况

由图7(b)可以看到，RTK静态差分定位精度在mm级，当动态定位时，虽然定位精度会有一定的下降，但厘米级定位已经可以完成高精度定位的目的，进一步验证了理论的准确性。因此后续定位方案GNSS定位模式采用RTK差分定位，并辅以惯性导航进行修正，并结合其他无线传感器，通过多源融合滤波算法，最终完成室内外无缝定位方案设计。

1.3 自身差分气压测高技术

大气压强是大气状态的一项关键指标，气压能在一定程度上反应对应压强下的高度，但气压具有周期性的日变化和年变化，以及非周期性的与天气密切相关的变化，为了避免这种变化削弱气压测高的准确性和稳定性，本文采用自身差分气压测高技术获得高度变化量，从而增强在信号遮挡没有固定解的情况下的定位鲁棒性。高度变化量为:

式中:T m为当前气温，P i－1与P i分别是相邻两个时刻的气压值。

2 基于卡尔曼滤波的航向角修正算法设计

空旷的室外环境下，可以取得良好的GNSS信号，借助于RTK定位技术可以实现高精度的定位，但其信号的质量会因定位环境的改变而发生改变，因此采用GNSS与惯导组合的方式实现室外的连续的定位。惯性导航长时间定位时会产生误差的累积，且其定位的精确性主要依赖于航向角以及算法的设计，因此，为了改善GNSS组合INS的定位性能，提高定位的准确性，提出了一种基于卡尔曼滤波的航向角修正算法，将RTK定位的航向角α与惯导定位航向角Δα用进行组合处理，通过数据融合解算，降低航向角测定误差。

2.1 GNSS/INS航向角修正算法实现

具体算法处理设计过程如下:

首先，令系统的状态量X＝[αΔα]T，以线性运动的载体为研究对象，建立线性状态方程模型:

即

式中:A是状态转移矩阵，且是状态噪声。

取观测量Y＝[αΔα]T，则观测方程模型为:

式中:H是观测矩阵，是观测噪声。

当前RTK定位的绝对角度等于上一时刻RTK的绝对角度与惯导变化的相对角度之和，经滤波算法处理后，即可得到组合航向角。

接下来利用组合航向角以及RTK定位信息、惯导定位信息进行组合导航航向角修正算法设计。基于GNNS和INS两种定位源，以卡尔曼滤波为例，令观测矩阵和状态转移矩阵分别如式(12)和(13)所示。

状态噪声W和观测噪声V如式(14)及式(15)所示。

令系统状态量X＝[x，v x，a x，y，v y，a y]，观测量Y＝[x，y，v·cosα，v·sinα]，代入卡尔曼滤波方程，即可得到组合导航定位结果。

组合导航工作过程如图8所示，首先终端处理设备会集成惯性测量单元以及卫星信号接收机。使用GPS卫星信号，采用惯性测量单元测量载体的姿态信息如加速度及角速度，通过卫星信号接收机获取当前的经度、纬度、高度以及速度、加速度等信息，然后通过终端设备上的数据融合处理单元进行位置的预测和更新，获得载体的运动轨迹，实现定位。

图8 组合导航位置预测更新框图

组合导航整体处理框图如图9所示。

图9 组合导航框图

2.2 实验验证与结果分析

2.2.1 实验设备及解析数据格式

使用DHNAV－S300组合导航端机(内置凌思陀螺仪、卫星信号接收模块)，进行室内外的无缝定位的算法设计及验证。DHNAV－S300可接收GNSS全系统多频段信号，用来实现室外RTK定位。本文通过滤波算法将GNSS数据与惯导数据融合处理，可在GNSS信号失锁情况下提供高精度的位置、航向、姿态、时间、信息。通过双天线解算出载体相对正北的朝向夹角，同时可通过载波相位差分解算提供厘米级位置信息。具体实验设备如图10所示，包括测试笔记本，实验车，组合导航端机、双天线等。

图10 测试设备

通过RS232转UWB串口，将端机采集的数据保存到本地计算机上，主要处理的数据格式包括0183标准协议格式GPGGA、惯导信息GTIMU以及组合导航信息GPFPD。

2.2.2 航向角测试实验

测试地点在中科苏州地理科学与技术研究院大厦四周及其地下车库，测试路径包括了室外部分以及室内部分，室外情况下采用GNSS信号与IMU组合定位，室内情况下GNSS信号完全失锁，采用惯导和轮速计进行轨迹外推定位。组合导航的航向角初始值由天线A指向天线B的绝对方向确定，本文按照双天线安装位置，根据安装角度补偿270°作为小车前进的航向角。

首先对本小节提出的组合导航航向角修正算法进行了实验验证，实验设备的定位轨迹如图11所示，光纤陀螺所测的标准航向角如图12所示，卫导模块所测航向角如图13所示。为验证航向角测量精度，将卫导模块测试所得航向角与光纤陀螺测试结果进行了对比，如图14所示。

图11 定位轨迹

图12 光纤陀螺所测航向角

图13 组合导航模块测量航向角图

图14 航向角对比图

分析定位及定向结果，由定位轨迹图可以看出，定位轨迹非常平滑，没有较大(大于1 m)跳点，在信号较好处均有RTK定位，由RTK定位精度(厘米量级)可推算出全程定位误差均小于1 m。

比较定向结果与高精度光纤所测航向角，两者变化趋势基本一致，由于二者测量起始时间，输出频率，时钟不一致，难以给出具体的误差曲线，但比较各个拐点角度值(减去一个大约8°的安装偏差角)及变化趋势分析可知，平均定向精度小于1°。

同时，针对不同场景下航向角算法性能进行了验证，室外航向角测试以及室外→室内→室外航向角测试分别如图15和16所示。

图15 室外航向角测试

图15的室外航向角测试中，由平缓以及拐弯变化的局部放大图可以看到，室外卫星信号良好的情况下，组合后的航向角以GNSS的航向角所在数据为中心，航向角变化更加平滑且变化趋势也与惯导的航向角一致。

在图16的室外→室内→室外航向角测试中，由室外到室内的局部放大图可以看到，当GNSS信号不稳定或失锁情况下，组合后的航向角以惯导的航向角为准，由室内到室外的局部放大图可以看到，当重新接收到良好的GNSS卫星信号后，组合航向角能够根据GNSS信号快速修正，回到准确的位置，在室外室内情况下都得到连续且准确的运动姿态信息。

图16 室外→室内→室外航向角测试

3 基于扩展卡尔曼滤波的多源融合算法设计

目前，本文已使用的定位源包括UWB、INS、GNSS以及气压计，分别在不同场景下实现了定位或者参数修正的目的，接下来如何充分使用上述定位源，设计一个具有稳定性高、定位精度好的融合算法是主要问题。

3.1 多源融合滤波算法实现

由融合滤波算法理论及仿真可得，扩展卡尔曼滤波在处理非线性运动问题与卡尔曼有较好的性能，并且与广义延拓滤波相比，理论更加成熟。因此，本小节以扩展卡尔曼滤波为基础对融合定位算法设计研究。

基于扩展卡尔曼滤波的信息融合示意图如图17所示。

图17 扩展卡尔曼滤波信息融合示意图

多源信息融合中的扩展卡尔曼状态空间模型与观测空间模型，如式(16)所示，式中，Hik表示不同的融合源在k时刻的系统状态观测系数。

本文基于UWB、GNSS、INS以及气压计融合定位源，通过扩展卡尔曼滤波的完成位置更新处理。令设状态向量X＝[x，y，xpdr，ypdr，xgnss，ygnss，xuwb，yuwb，Δh]T，(x，y)表示多源融合定位得到的位置坐标，(xpdr，ypdr)为PDR定位解算的位置，(xgnss，ygnss)为GNSS定位解算的位置坐标，(xuwb，yuwb)为超宽带定位得到的位置坐标，Δh为差分气压计解算的高度值。令观测量Y＝[x，y，xpdr，ypdr，xgnss，ygnss，xuwb，yuwb，Δh]T。状态方程如式(17)所示，观测方程如式(18)所示，式中w i表示不同定位源对融合定位结果(x，y)的权值。

由文献[2]可知，多元融合定位中，如果两个及以上的定位源定位效果较好，则融合系统的性能就可优于单一定位源；而如果只有一个定位源的定位效果远优于其他定位源，则融合系统的性能就无法得到提升。根据定位源的置信度来判断其在融合系统中的权值，可以削弱不理想定位源对整体定位效果的影响。另一方面，为了降低计算量从而提高导航定位的实时性，本文多源融合方法采用了基于多源置信度的滑动窗权重计算方法，置信度矩阵如式(19)所示。

式中:p i与¯p分别为第i个定位源的定位结果与全部k个定位源定位结果平均值，对上述置信度矩阵进行列归一化处理，然后对每行求和后再进行一次归一化，得到置信度向量S＝[S1…S i…S k]T，S i即为第i个定位源在当前位置的置信度，设滑动窗窗口和步长均为n，该值主要取决于惯性导航定位轨迹发散时间，误差累积越慢，窗口长度越长。第i个定位源在一个滑动窗时间段内的权重如式(20)所示。

状态转换矩阵A和观测矩阵H分别如式(21)和式(22)所示。

状态向量和测量向量的噪声协方差矩阵Q和R分别如式(23)和式(24)所示。

当各个参数的初始值确定后，代入到扩展卡尔曼滤波的更新方程中，获得相应的估计值，并与测量值相结合，得到最优估计值，从而获取最终的融合定位结果。

在开阔的室外环境下，RTK的定位精度能够达到厘米级，而在室内环境中，UWB能够也能够实现厘米级的定位。为了充分地合理地利用高精度定位源，我们设置了重置时间间隔，其值与上述权重计算中滑动窗窗长一致，在超出重置时间间隔后，如果存在高信噪比的GNSS定位结果或高信号强度的UWB定位结果，我们会以此为初始值开始新一轮的融合定位。

3.2 多源融合算法对比

在本小节中，将采集获得的实验数据通过MATLAB进行对其他多源融合算法的分析对比。在文献[17]通过实验验证了联邦式卡尔曼滤波比集中式卡尔曼滤波具有更强的抗干扰能力，故实验中我们采用联邦式卡尔曼滤波进行对比，此外我们还对比了信号质量加权融合方法[16]。

我们在建筑物内设置了4个基站提供UWB信号，配备有组合导航终端的实验车的运行轨迹为绕建筑物顺时针一周，并在中途驶入建筑物内以验证室内外切换的定位性能，其起始点位置在图18中坐标为(5，5)。

图18 多源融合算法对比轨迹图

由图18分析对比轨迹可知，三种算法的在室内外切换的情况下均可以做到轨迹连贯平滑不出现较大的跳变，另一方面，由于室外GNSS信号不稳定，信号质量加权融合方法的定位轨迹受其影响较大，而本文算法与联邦式卡尔曼滤波则能从一定程度上避免受其影响。由表1中定位误差的平均值与标准差可得，本文算法在定位精度与稳定性均优于其他两种算法。表中平均运行时间是由整体运行时间除于定位点个数计算得到的，可以得知，信号质量加权融合用时最少，而其余两者的用时比较接近，信号质量加权融合的方法虽然有最低的运行时间，但其最大定位误差有1.225 m，定位轨迹跳变明显。本文提出的多源融合室内外无缝定位方法能够在较低的时延下获得平均定位误差0.299 m的定位效果。

表1 多源融合算法定位误差及复杂度对比

3.3 实验验证与结果分析

接下来进行整体的定位测试，首先在测试定位环境中部署足够的UWB基站，然后按照一定的实验路径行驶，定位终端设备包括计算机、UWB标签以及差分气压测高模块移动站，将定位终端搭载在无人车上，控制在中科院地理信息产业楼周围绕行，周围有灌木楼宇遮挡，并进入地下车库测试信号完全失锁情况下定位情况，最后使用计算机将各组定位源数据通过融合算法进行融合处理。实验路径及定位场景如图19和图20所示，实验路径包括室外和室内部分，分别用不同的线型进行了表示，整体测试路线为室外→室内→室外。

图19 实验场景平面示意图

图20 地下车库测试场景

定位数据通过RS232转USB串口输出到计算机上进行同步的定位解算。获取到的定位信息包括试验车的运动的经纬度、位置坐标、速度、航向角、姿态信息等等，将获取的经纬度数据导入谷歌地图观察多源融合导航定位以及单一的GNSS定位情况，如图21所示。可以看到，当实验车进入地下车库后，GNSS定位消失，多源融合导航依旧有高精度的定位信息，当实验车行驶出车库后，GNSS信号重新捕获，GNSS定位从单点定位逐渐过渡到高精度RTK定位，将融合导航定位重新拉回到准确值，实现连续平稳且准确的定位输出。轨迹曲线平滑无跳点，并且能清晰分辨出进出车库口轨迹。在车库内信号完全丢失情况下依然能提供高精度的定位数据，信号重新捕获后能与定位轨迹有较好的衔接。

图21 楼宇遮挡及车库环境测试

4 结论

本文为整体上实现室内外无缝定位的目的，首先提出了一种多源融合室内外定位方案，并对GNSS定位模式进行了实验验证。然后，基于不同场景对定位算法进行了研究:室外情况下，首先采用GNSS组合惯性导航进行定位解算，并提出了一种基于滤波方法的航向角修正算法。进入到室内后，采用作者提出的PDR辅助UWB的多源融合室内定位方法，并结合气压计进行定位高度方向的修正。同时，提出了一种新的融合定位滤波算法将不同定位源的数据相融合，实现多源融合定位。测试显示，该方法在室外情况下能够实现厘米级定位，在室内外过渡阶段，依旧可以保持连续准确的定位，在室内GNSS卫星信号丢失的场景中，仍然能够实现分米级定位。在接下来的工作中，我们会探究不同场景下多源融合定位的定位表现，以期实现智能识别多场景的高精度多源融合定位。