GNSS受限条件下的UWB增强组合导航方法

姜 维，曹琢健，陆德彪，蔡伯根，上官伟，王 剑

(1．北京交通大学 电子信息工程学院，北京 100044；2．北京市轨道交通电磁兼容与卫星导航工程技术研究中心，北京 100044)

目前，全球导航卫星系统(Global Navigation Satellite System，GNSS)仍然作为解决用户定位导航的主要手段之一被广泛使用。它能够全天候、全时段地为全球用户提供低成本、高精度的三维位置、速度和精确定时等导航信息。同时由于GNSS与惯性导航系统(Inertial Navigation System，INS)测量值的互补特性[1]，使得GNSS与INS的组合导航研究与应用最为广泛[2]。GNSS可提供长时间的高精度位置、速度信息，INS可提供短时间内准确的姿态测量信息，显然，二者的组合导航系统性能远远高于单一系统。

针对GNSS/INS组合导航，很多学者展开了大量的研究工作。文献[3]提出了一种新的自适应过程噪声调整算法应用于GNSS/INS组合导航中，实验验证，这种自适应调整机制提高了总体组合性能。文献[4]提出一种将神经网络与低成本GPS/INS系统集成的新方法，实验结果表明，在GPS失效时，神经网络辅助INS的位置解在系统运行270 s后的北向误差为220 m，东向误差为40 m，垂直方向误差为29 m。文献[5]利用低成本MEMS传感器和GPS组合，并将研制出的自主导航系统在汽车和遥控飞机上测试，测试结果表明，面对模拟的GPS失效环境，能够分别在7、6 s之内维持可接受的导航精度。综上所述，虽然GNSS和INS组合导航系统的性能远优于单一导航系统，但在城市环境等多径效应严重、信号遮挡严重的区域，二者组合导航系统也难以保持较高精度的定位性能，定位结果无法使用。在一些GNSS卫星信号失锁的场景下，单独依靠INS进行定位，定位结果会迅速发散，在几秒钟之后就无法使用。

由于UWB信号的时域脉冲极窄，时间分辨率高，因此UWB系统具有很好的抗多径干扰能力和厘米级别的距离分辨率。基于以上优势，UWB技术尤其适用于密集多径场景的应用中。所以，UWB技术可以极好地弥补GNSS受限环境下的劣势，维持定位功能的可用性。

在组合方式上，GNSS/INS组合导航主要有松组合和紧组合两种组合模式[8]。松组合是利用GNSS接收机输出的位置、速度信息和INS输出的信息进行组合，这种组合方式的优点是系统灵活、运算量低。但是其必须在GNSS可以输出定位信息的情况下才可以使用。当GNSS可见星数少于4颗时，松组合方式将无法使用。紧组合是利用GNSS接收机输出的伪距、伪距率等观测量，与INS结合星历反算得到的伪距、伪距率进行组合[9]，其最主要的优势是增强了系统的可用性，允许在GNSS可见星数量较少时获取定位结果。因此，紧组合的GNSS/INS系统是目前应用最广泛的组合体系结构。由于GNSS接收机跟踪的卫星少于4颗或UWB接收到的基站距离信息少于3个时，无法直接从GNSS或UWB计算待定位置，松组合系统将无法工作，同时为了更好地利用GNSS接收机的伪距、伪距率以及UWB距离测量值的原始信息，本文对GNSS、INS、UWB的三组合导航系统采用紧组合方式，确保在可用信号较少的情况下，系统仍可以工作。

卡尔曼滤波作为组合导航采用的最广泛的滤波算法，能提供系统状态的最优估计。本文针对多种传感器组合，采用集中式结构的卡尔曼滤波的融合方法，利用全部的、有限的量测数据，增强滤波的可靠性和系统的生存能力。集中式卡尔曼滤波在计算时，将各个子系统的数据统一输入到信息融合中心，通过扩展量测维度的方式，集中对多个数据源的数据进行处理，保证了信息的充分利用。

1 GNSS/INS组合导航

1.1 惯性导航递推算法

惯性导航系统建立在惯性原理基础上，不需要任何外来信息，也不会向外辐射任何信息，仅靠惯性导航系统本身就能在全天候条件下，在全球范围内和任何介质环境里自主地、隐蔽地进行连续的三维定位和三维定向。惯性导航系统内置三轴加速度计和三轴陀螺仪，分别用来测量运载体的角运动信息和线运动信息，机载计算机根据这些测量信息解算出运载体的航向、姿态、速度及位置。惯性导航计算过程包含姿态更新、速度更新、位置更新三部分，其微分方程[10]为

(1)

(2)

(3)

(4)

(5)

(6)

1.2 GNSS/INS松组合系统模型

GNSS/INS松组合导航方法流程[11]见图1。GNSS输出的位置和速度值，作为测量输出给卡尔曼滤波器，组合卡尔曼滤波器用它与INS计算的位置、速度值的差来估计INS误差。卡尔曼滤波器估计的INS误差，对INS导航参数进行校正，经过校正后的INS导航参数构成组合导航的输出。

图1 GNSS/INS松组合导航方法流程

松组合导航的2个主要优点是简单和冗余。在组合导航中，除了组合导航参数，通常还有独立的GNSS导航输出。但松组合方法有很多问题。尽管短时间内可以使用3颗卫星的信号输出导航解，但通常GNSS导航输出需要来自4颗不同卫星的信号。如果只能跟踪更少的卫星，GNSS数据不能用于对INS进行辅助，依靠INS单独进行解算的结果将会迅速发散。

1.3 GNSS/INS紧组合系统模型

GNSS/INS紧组合导航方法流程见图2。源于GNSS的原始数据伪距和伪距率构成卡尔曼滤波的量测输入，滤波器采用这些测量信息来估计INS和GNSS系统的误差。与松组合导航参数相同，组合导航参数是校正后的惯性导航参数。紧组合的优势在于不需要一个完整的GNSS解来辅助INS，即使只跟踪了一个GNSS卫星信号，GNSS的伪距数据仍然能够作为组合系统的输入[12-13]。

图2 GNSS/INS紧组合导航方法流程

1.3.1 GNSS/INS紧组合导航状态模型

GNSS/INS紧组合导航状态模型的构建基于INS的误差方程，假设惯导系统姿态失准角是一个小角度，并且不考虑地球重力模型误差，经过推导得出惯导系统误差方程[10]如下。

姿态误差方程为

(7)

速度误差方程为

(8)

位置误差方程为

(9)

度计误差向量，在本文中采用常值来定义。

仅利用单台GNSS接收机的量测信息进行组合，状态模型中包含GNSS接收机的钟差和时钟漂移率的误差。用dt表示接收机的钟差，则

(10)

综合以上各式，INS误差方程写成矩阵形式为

(11)

式中：X为卡尔曼滤波的状态向量，由17维向量组成，见式(12)；F为系统转移矩阵，其定义了状态向量随时间变化的规律，在卡尔曼滤波系统模型中是惯性导航系统的误差传递动力学方程；W为系统过程噪声，在这里建模成高斯白噪声，对应的协方差矩阵为Q。

(12)

1.3.2 GNSS/INS紧组合导航量测模型

在紧组合系统中，观测值由GNSS伪距和伪距率与根据INS自身计算的位置、速度得到的伪距、伪距率作差构造而成，定义为

(13)

卡尔曼滤波量测方程为

Z=HX+V

(14)

式中：V为系统量测噪声，在这里建模成高斯白噪声，对应的协方差矩阵为R；H为量测矩阵，由式(16)确定。

(15)

(16)

2 GNSS/INS/UWB紧组合导航模型

2.1 UWB测距定位技术

UWB技术是一种不用载波，而利用纳秒至微微秒级的非正弦波窄脉冲传输数据的无线通信技术，使用频段为3.1～10.6 GHz和低于41 dBm·W的发射功率，其数据速率可达几十Mbps到几百Mbps。UWB技术具有抗干扰性强、低发射功率、可全数字化实现、保密性好的特点，近年来成为无线定位技术的热点。UWB定位主要采用TOA算法[14]，其工作原理见图3。

图3 基于TOA算法的UWB定位原理

一旦取得了多个TOA测量值，就可得到未知标签和多个基站的距离，从而构成圆周方程组，求解该方程组就能得到移动标签的距离。

(17)

(18)

式中：(δx，δy，δz)为位置偏移量；δri为第i个基站的距离偏移量。为方便起见，引进axi、ayi、azi以简化式(18)。axi、ayi、azi各项代表由近似位置指向第i号基站的单位矢量的方向余弦。

(19)

简化后方程的矩阵形式为

(20)

2.2 GNSS/INS/UWB集中卡尔曼滤波模型

为了将GNSS、INS、UWB三者的测量数据充分利用，使用集中式卡尔曼滤波进行数据融合。图4给出了误差状态集中式卡尔曼滤波组合的结构。集中式融合[15]就是将所有传感器的量测数据都传送到一个中心处理器进行集中处理。卡尔曼滤波需要对所有导航传感器的系统误差和噪声源进行建模，以确保所有相关的误差都已经考虑，所有的测量都已经根据权值优化，并且使用尽可能多的信息标定每个误差。从精度和鲁棒性来说，集中式的组合结构提供了最优的导航参数。

图4 集中式卡尔曼滤波组合结构

集中滤波的状态向量由17维组成，分别是3维位置误差、3维速度误差、3维姿态误差、3维加速度误差向量、3维陀螺仪误差向量、GNSS接收机钟差和钟差漂移率，其表达式为

(21)

对应的状态转移矩阵为

(22)

(23)

(24)

(25)

系统的量测模型见式(27)，包含了GNSS和UWB测量值的所有信息，为2m+n维向量，m为可见卫星数，n为UWB基站个数。

(26)

量测转移矩阵H为

(27)

(28)

(29)

(30)

惯性导航系统主要的噪声源是由加速度计比力测量噪声导致的速度误差，以及由陀螺仪角速度测量噪声带来的姿态误差。采用惯性导航器件的零偏来定义系统噪声协方差矩阵为

(31)

式中：bf为加速度计零偏；bω为陀螺仪零偏。

(32)

集中卡尔曼滤波过程包含两个更新过程，时间更新和量测更新。时间更新的两个步骤预测了系统下一时刻的状态值X和其协方差矩阵P。

后期编辑：除了前端录制以外，录播系统还提供了后期视频编辑。教师可以在录播主机管理后台对录制视频编辑、剪辑、添加片头、片尾和学院的LOGO。

(33)

Q(k-1)

(34)

在量测更新的流程里，首先是用最新观测信息更新状态估计和误差协方差矩阵；其次是卡尔曼滤波的增益矩阵K的计算，此步可根据当前状态估计的不确定度和观测噪声情况来对状态的修正量进行优化加权；然后是用卡尔曼增益加权的观测数据更新状态估计；最后是协方差矩阵的更新，用观测数据的新信息更新误差协方差矩阵。

HT(k)+R(k)]-1

(35)

(36)

(37)

3 实验与结果分析

基于GNSS/INS/UWB紧组合的定位实验，在北京交通大学主校区思源楼前空地进行。思源楼高12层，由于楼体遮挡，思源楼正门前空地GNSS信号遮挡严重，因此选择在此区域进行定位实验，以验证算法的有效性。

实验装置被安装在测试车上，包含1台UB370GNSS接收机，1个SPAN INS(由NovAtel OEM6 GNSS接收器和iMAR-FSAS IMU组成)，5个UWB基站和1个UWB标签。惯性传感器的性能参数见表1。GNSS频率与UWB测量值频率均为10 Hz，INS频率为200 Hz。UWB标签与GNSS天线处在同一垂线上。

表1 iMAR-FSAS IMU性能参数

与GNSS导航解算类似，UWB信号的几何分布也影响着定位精度。引入精度因子的概念，精度因子取决于参与定位信号之间的几何分布关系。给定测距信号，沿某轴向的定位精度与该轴向和用户至基站信号矢量之间的夹角有关，夹角越小，该轴向上的定位精度越高。因此，用户至基站的视线矢量在水平面内均匀分布时定位精度较高。

实验环境和由SPAN输出的实验轨迹见图5。SPAN是商用高精度GPS/INS组合导航系统，但是在本实验恶劣的实验环境下，SPAN的定位结果精度不足，无法作为参考轨迹来验证本文算法，但可从图5中看到大致的实验轨迹。

图5 实验环境和SPAN定位轨迹

实验中选择5个静态测试点，便于进行实验结果准确性分析。采用上海华测公司研发的多模RTK GNSS接收机，接入千寻CORS服务，利用千寻平台发布的差分数据，进行静态RTK测量，得到静态测试点的位置。测试点具有毫米级的位置精度，因此可作为参考位置来评估本文实验的定位精度。测试从点1开始，在每个点静止2～4 min后，移动至下一个点，且移动过程基本近似为直线。测试点的静止时长和测试周期见表2。

表2 测试点静止时长和测试周期

图6 实验轨迹对比

GNSS单点定位的可见卫星数和定位精度(DOP)值见图7,GNSS/INS/UWB组合定位的可见信号数和DOP值见图8。对比两图可见，由于UWB的引入，信号数量的增加，三组合的信号具有更好的几何分布特性，精度因子明显低于GNSS单点定位的精度因子值。这为导航系统的精度性能提供了基础。

图7 GNSS单点定位可见卫星数与DOP值

图8 GNSS/INS/UWB组合定位可见信号数与DOP值

测试点水平分量位置误差的散点图见图9。其中图9(a)～9(e)分别是测试点1～5的误差散点，从图中可以看出，GNSS单点定位结果和GNSS/INS定位结果误差基本都在1 m以上，且准确度不足，轨迹跳动剧烈；而GNSS/INS/UWB组合定位结果基本收敛在测试点0.5 m的范围内。需要说明的一点是，由于在实验开始阶段，三组合卡尔曼滤波需要收敛时间，所以图9(a)中的测试点1定位结果有一个收敛的过程，收敛后基本稳定在测试点附近。图9(f)为所有测试点的GNSS/INS/UWB组合定位结果误差散点图，从图中可以看出，稳定后的GNSS/INS/UWB定位结果具有很好的精密度。

图9 测试点水平分量误差散点

图10是测试点的东向、北向、天向误差，对其进行量化评估，3个方向的RMS值柱状图见图11。从图中可以看出，东向、北向的均方根误差均在0.2 m以下，而垂直方向的均方根误差也均在0.4 m以下。

图10 测试点东向、北向、天向误差

图11 测试点东向、北向、天向RMS值

水平面的距离均方根误差(DRMS)描述了水平面的位置误差，而平均径向球面误差(MRSE)描述了三维位置误差。5个测试点的DRMS值和MRSE值见表3。针对所有测试点的DRMS值为0.1 236 m，MRSE值为0.2 727 m。因此，实验数据表明在GNSS多径效应严重、定位困难的环境下，依靠本文提出的UWB辅助增强GNSS/INS组合导航的方法能够获得较高的定位精度。水平方向定位精度能达到2 dm以下，而三维球面的定位精度能达到5 dm以下。且系统具有较好的稳定性，基本消除了GNSS信号造成的位置跳动的影响。

表3 测试点的DRMS和MRSE值

4 结论

UWB技术作为近年来备受关注的无线电技术，在抗多径效应、定位精度等方面具有强大的优势。为了克服城市区域内的严重多径效应、信号衰落，本文提出了将UWB技术引入GNSS/INS组合的组合导航模型。由于在城市环境下，GNSS信号遮挡严重，面对可能的GNSS可见星数不足的情况，本文采用了紧组合的组合模式，保证了对有限信息的充分利用，提高了系统的可用性。集中式卡尔曼滤波结构用于融合GNSS、INS、UWB的测量信息，保证系统能获得全局最优估计。

为了对本文提出的组合定位方法进行评估，在GNSS信号建筑物遮挡严重的场所进行了实验测试。实验过程中分别在5个位置已知的测试点静止了几分钟。实验结果表明：

(1)在GNSS单点定位可靠性严重下降的情况下，本文所提出的导航系统能够提供水平面精度优于0.2 m，3D球面精度优于0.4 m的定位精度。

(2)系统所采用的组合结构和滤波方法能够提供较快的收敛速度和稳定性，基本消除了GNSS信号造成的位置跳动的影响。