CN110645979B - 基于gnss/ins/uwb组合的室内外无缝定位方法 - Google Patents

Abstract

本发明提供了一种基于GNSS/INS/UWB组合的室内外无缝定位方法。该方法包括：在待测点位置设置全球卫星导航系统GNSS、惯性导航系统INS和超宽带系统UWB，通过INS解算出待测点的位置、速度和姿态信息，在室外情况下，通过GNSS和INS对待测点进行组合定位，获得待测点的室外导航信息，在室内情况下，通过UWB和INS对待测点进行组合定位，获得待测点的室内导航信息。本发明在室外环境下，GNSS与INS组合系统用于校正GNSS量测值和INS累积误差。在室内，由UWB替代GNSS与INS递推结果进行组合，解决室内GNSS信号质量差的问题。两种定位模式的导航方法可以实现室内室外的无缝定位，提供准确、连续的导航方案。

Description

基于GNSS/INS/UWB组合的室内外无缝定位方法

技术领域

本发明涉及导航定位技术领域，尤其涉及一种基于GNSS/INS/UWB组合的室内外无缝定位方法。

背景技术

导航定位是人类生产生活的基本需要。目前，GNSS(Global NavigationSatellite System,全球导航卫星系统)仍然作为解决用户定位导航的主要手段之一被广泛使用。它能够全天候、全时段地为全球用户提供低成本、高精度的三维位置、速度和精确定时等导航信息。但是，GNSS的主要缺点依然明显，城市环境对GNSS构成了一些最严峻的挑战。由于城市中隧道、墙壁等障碍物的遮挡，GNSS卫星信号在城市环境下会急剧衰减，甚至无法定位。这种定位限制可以通过GNSS系统与其他系统的集成来解决，例如惯性导航技术。GNSS与INS(Inertial Navigation System，惯性导航系统)组合，可以提供连续可靠的位置速度姿态等导航参数。这两种技术的组合定位优势互补，既可以增强GNSS的可用性，又可以限制INS的传感器误差。然而，长时间在室内环境下时，GNSS信号几乎不可用，INS长时间独立运行的结果漂移严重，同样无法使用。

发明内容

本发明提供了一种基于GNSS/INS/UWB组合的室内外无缝定位方法，室外采用GNSS/INS紧组合定位，室内采用UWB/INS紧组合定位，为用户提供连续、可靠的室内外导航解决方案。

为了实现上述目的，本发明采取了如下技术方案。

一种基于GNSS/INS/UWB组合的室内外无缝定位方法，在待测点位置设置全球卫星导航系统GNSS、惯性导航系统INS和超宽带系统UWB，所述方法包括：

通过INS解算出待测点的位置、速度和姿态信息；

在室外情况下，通过GNSS和INS对待测点进行组合定位，获得待测点的室外导航信息；

在室内情况下，通过UWB和INS对待测点进行组合定位，获得待测点的室内导航信息。

优选地，所述的通过INS解算出待测点的位置、速度和姿态信息，包括：

INS基于待测点的初始位置、速度和姿态信息，结合加速度计与陀螺仪输出的测量数据，计算每一更新时刻的待测点的位置、速度和姿态信息，计算公式分别如下：

其中：P表示待测点的位置信息，

表示待测点的位置的变化率矢量，V表示待测点的速度信息，

表示待测点的速度的变化率矢量，

表示由载体坐标系至导航坐标系的姿态转移矩阵，载体坐标系为前-右-下坐标系，对应导航坐标系为北-东-地坐标系，f^b表示加速度计测量的加速度矢量信息，gⁿ表示重力，

表示在导航坐标系下陀螺仪测量的角速度矢量信息，

表示在导航坐标系下地球自转引起的角速度变化矢量，Q表示姿态四元数，

表示姿态四元数率，

表示载体坐标系相对于导航坐标系的瞬时角速度矢量。

优选地，所述的在室外情况下，通过GNSS和INS对待测点进行组合定位，获得待测点的室外导航信息，包括：

在室外GNSS有效时，根据INS解算得到待测点的位置、速度、姿态信息和GNSS提供的卫星星历，计算卫星到待测点之间的距离，将GNSS测量的伪距、伪距率与INS解算的伪距、速度作差，将得到的差值作为卡尔曼滤波器的量测输入，利用所述卡尔曼滤波器的输出值对INS的解算结果进行修正；

所述卡尔曼滤波器的系统状态向量由17个状态参数组成，公式如下：

其中，δA，δP，δV分别是三轴姿态角误差，位置误差和速度误差，δb_ω和δb_f分别是陀螺仪测量误差和加速度计测量误差，dt表示GNSS接收机的时钟钟差，

表示时钟漂移率；

所述卡尔曼滤波器的系统量测向量如下：

其中，

表示GNSS原始测量伪距，

表示GNSS原始测量伪距率，ρ_i ^INS表示INS解算的位置结果与GNSS卫星之间的几何距离，

表示由INS解算结果计算的移动点到卫星的几何距离变化率，m表示当前时刻的可见卫星数；

所述卡尔曼滤波器的量测矩阵H(k)如下：

其中，L(k)_m×3写为：

是GNSS卫星在ECEF坐标系下的坐标，(x y z)是由INS解算的在ECEF坐标系下的标签坐标；

所述卡尔曼滤波器的测量噪声协方差矩阵由定位精度经验值获得。

优选地，所述的在室内情况下，通过UWB和INS对待测点进行组合定位，获得待测点的室内导航信息，包括：

在室内UWB信号有效而GNSS信号失效时，根据INS解算的待测点的位置、速度、姿态信息和UWB定位基站的坐标值，计算定位基站到待测点之间的距离，将UWB得到的原始距离量测与INS解算的距离量测作差，将得到的差值作为卡尔曼滤波器的量测输入，利用所述卡尔曼滤波器的输出值对INS的解算结果进行修正；

所述卡尔曼滤波的系统状态向量由18个状态组成，公式如下：

X＝[δA δP δV δb_ω δb_f δη_L]^T

其中，δA，δP，δV分别是姿态角误差，位置误差和速度误差，δb_ω和δb_f分别是陀螺仪偏差和加速度计偏差，δη_L表示INS几何中心与UWB待测标签的相对位置差，即杠杆臂的值；

所述卡尔曼滤波的系统量测向量为：

其中，r_i ^INS表示INS解算的位置结果与UWB基站之间的几何距离，r_i ^UWB表示UWB原始距离观测量，n代表UWB基站个数；

H(k)是量测矩阵，公式如下：

H(k)＝[0_n×3 D(k)_n×3 0_n×3 0_n×3 0_n×3 D(k)_n×3]

其中，

是UWB基站在ECEF坐标系下的坐标，(x y z)是由INS解算的在ECEF坐标系下的标签坐标；

所述卡尔曼滤波的测量噪声协方差矩阵由定位精度经验值获得。

由上述本发明的实施例提供的技术方案可以看出，本发明实施例首先判断用户处于室内场景下还是室外场景下。在室外环境下，GNSS与INS组合系统用于校正GNSS量测值和INS累积误差。在室内，由UWB替代GNSS与INS递推结果进行组合，解决室内GNSS信号质量差的问题。本发明实施例提供的将GNSS/INS/UWB三种技术集成的两种定位模式的导航方法可以实现室内室外的无缝定位，提供准确、连续的导航方案。

本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种基于GNSS/INS/UWB组合的室内外无缝定位方法的原理框图；

图2为本发明实施例提供的一种基于GNSS/INS/UWB组合的室内外无缝定位方法的具体处理流程图。

具体实施方式

下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。

本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本发明的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组。应该理解，当我们称元件被“连接”或“耦接”到另一元件时，它可以直接连接或耦接到其他元件，或者也可以存在中间元件。此外，这里使用的“连接”或“耦接”可以包括无线连接或耦接。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的任一单元和全部组合。

本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。

为便于对本发明实施例的理解，下面将结合附图以几个具体实施例为例做进一步的解释说明，且各个实施例并不构成对本发明实施例的限定。

为了获取到连续且准确的室内室外定位结果，有必要选择一种室内定位技术集成到GNSS/INS系统中。目前，很多研究者开发了基于红外线、射频识别、蓝牙、超宽带、无线局域网络、伪卫星、计算机视觉等技术的多种室内定位方法。其中，UWB(UltraWideBand，超宽带)是一种穿透能力较强的无载波窄脉冲无线技术。UWB技术的工作原理是定位区域布设四个以上坐标已知的定位基站,在待测目标上安放UWB定位标签,标签按一定频率发射脉冲与定位基站间通信,使用TDOA(TIme difference of Arrival，到达时间差)等方法获取相互间距离,采用多边定位或者指纹定位方法确定位置。超宽带系统具有穿透力强、抗多径效应好、定位精度高、功耗低等优势，适于室内动态和静止物体以及人员定位跟踪与导航。

因此，为了实现城市中室内室外不同场景下的无缝定位和高精度定位水平，本发明实施例提出了一种将GNSS/INS/UWB三种技术集成的导航方法。在室外环境下，GNSS卫星信号提供精确导航信息，GNSS与INS组合系统用于校正GNSS量测值和INS累积误差。同时，当切换到室内场景下时，由UWB替代GNSS与INS递推结果进行组合，校正INS的误差，解决了室内GNSS信号质量差的问题。两种定位模式的导航方法可以实现室内室外的无缝定位，提供准确、连续的导航方案。

本发明实施例提供的一种基于GNSS/INS/UWB组合的室内外无缝定位方法的原理框图如图1所示，该方法的具体处理流程如图2所示，包括如下的处理步骤：

步骤1：INS解算出待测点的位置、速度和姿态。

在待测点位置设置GNSS、INS和UWB系统。

INS基于待测点的初始位置、速度和姿态信息，结合加速度计与陀螺仪输出的测量数据，计算每一更新时刻的待测点的位置、速度和姿态信息。INS需要进行初始姿态矩阵计算，位置、速度、姿态和姿态转移矩阵计算，计算公式分别如下：

INS解算得到待测点的位置、速度和姿态的具体过程可以描述为：

其中：P表示待测点的位置信息，

表示待测点的位置的变化率矢量，V表示待测点的速度信息，

表示待测点的速度的变化率矢量，

表示由载体坐标系至导航坐标系的姿态转移矩阵，这里采用的载体坐标系为前-右-下坐标系，对应导航坐标系为北-东-地坐标系，f^b表示加速度计测量的加速度矢量信息，gⁿ表示重力，

表示在导航坐标系下陀螺仪测量的角速度矢量信息，

表示在导航坐标系下地球自转引起的角速度变化矢量，Q表示姿态四元数，

表示姿态四元数率，

表示载体坐标系相对于导航坐标系的瞬时角速度矢量。

步骤2：卫星导航/惯性导航紧组合定位算法；

在室外GNSS有效时，根据INS解算出的待测点的位置、速度、姿态信息和GNSS提供的卫星星历，计算卫星到待测点的估计距离，将GNSS测量的伪距、伪距率与INS估计的伪距、速度作差，将得到的差值作为卡尔曼滤波器的量测输入，利用所述卡尔曼滤波器的输出值对INS的解算结果进行修正，即滤波结果用于校正INS自身解算的误差。

卡尔曼滤波器的系统状态向量由17个状态组成，公式如下：

其中，δA，δP，δV分别是三轴姿态角误差，位置误差和速度误差，δb_ω和δb_f分别是陀螺仪测量误差和加速度计测量误差，dt表示GNSS接收机时钟钟差，

表示时钟漂移率。

系统量测向量如下：

其中，

表示GNSS原始测量伪距，

表示GNSS原始测量伪距率，ρ_i ^INS表示INS解算的位置结果与GNSS卫星之间的几何距离，

表示由INS解算结果计算的移动点到卫星的几何距离变化率，m表示当前时刻可见卫星数。

量测矩阵H(k)由下式确定：

其中，L(k)_m×3写为：

是GNSS卫星在ECEF(Earth-Centered,Earth-Fixed，地球为中心,符合地区)坐标系下的坐标，(x y z)是由INS估计的在ECEF坐标系下的标签坐标。

测量噪声协方差矩阵由定位精度经验值获得。

步骤3：超宽带/惯性导航紧组合定位算法；

在室内UWB信号有效而GNSS信号失效时，根据INS解算出的待测点的位置、速度、姿态信息和UWB定位基站的坐标值，计算定位基站到待测点的估计距离，将UWB的原始距离量测与INS估计的距离量测作差，将得到的差值作为卡尔曼滤波器的量测输入。滤波结果用于INS自身解算的误差。

卡尔曼滤波的系统状态向量由18个状态组成，公式如下：

X＝[δA δP δV δb_ω δb_f δη_L]^T

其中，δA，δP，δV分别是姿态角误差，位置误差和速度误差，δb_ω和δb_f分别是陀螺仪偏差和加速度计偏差，δη_L表示INS几何中心与UWB待测标签的相对位置差，即杠杆臂的值。

系统量测向量写为：

其中，r_i ^INS表示INS解算的位置结果与UWB基站之间的几何距离，r_i ^UWB表示UWB原始距离观测量，n代表UWB基站个数。

H(k)是量测矩阵，公式如下：

H(k)＝[0_n×3 D(k)_n×3 0_n×3 0_n×3 0_n×3 D(k)_n×3]

其中，

是UWB基站在ECEF坐标系下的坐标，(x y z)是由INS解算的在ECEF坐标系下的标签坐标。

卡尔曼滤波的系统测量噪声协方差矩阵由定位精度经验值获得。

根据上述各模型并通过本发明方法可以得出以下实验结果：

在待测点移动过程中，判断其所处环境，并实时采集GNSS、INS和UWB的原始数据，通过卡尔曼滤波进行滤波分析，得到组合后的连续无缝导航信息。

综上所述，本发明实施例首先判断用户处于室内场景下还是室外场景下。在室外环境下，GNSS与INS组合系统用于校正GNSS量测值和INS累积误差。INS自身解算出位置速度姿态信息，之后利用GNSS星历文件，计算估计伪距和伪距率。将INS估计的与GNSS提供的伪距和伪距率作差，输入滤波器。利用滤波输出结果对INS进行校正。在室内，由UWB替代GNSS与INS递推结果进行组合，解决室内GNSS信号质量差的问题。利用INS解算位置与UWB定位基站之间的距离得到估计距离，与UWB定位标签提供的距离作差，作为卡尔曼滤波器的量测输入。滤波结果修正INS解算位置，得到更可靠的定位结果。这种将GNSS/INS/UWB三种技术集成的导航方法实现了室内室外的无缝定位，解决了室内GNSS信号质量差的问题，可以提供准确、连续的导航方案。

本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置或系统实施例而言，由于其基本相似于方法实施例，所以描述得比较简单，相关之处参见方法实施例的部分说明即可。以上所描述的装置及系统实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (1)

1.一种基于GNSS/INS/UWB组合的室内外无缝定位方法，其特征在于，在待测点位置设置全球卫星导航系统GNSS、惯性导航系统INS和超宽带系统UWB，所述方法包括：

通过INS解算出待测点的位置、速度和姿态信息；

在室外情况下，通过GNSS和INS对待测点进行组合定位，获得待测点的室外导航信息；

在室内情况下，通过UWB和INS对待测点进行组合定位，获得待测点的室内导航信息；

所述的通过INS解算出待测点的位置、速度和姿态信息，包括：

INS基于待测点的初始位置、速度和姿态信息，结合加速度计与陀螺仪输出的测量数据，计算每一更新时刻的待测点的位置、速度和姿态信息，计算公式分别如下：

其中：P表示待测点的位置信息，

表示待测点的位置的变化率矢量，V表示待测点的速度信息，

表示待测点的速度的变化率矢量，

表示由载体坐标系至导航坐标系的姿态转移矩阵，载体坐标系为前-右-下坐标系，对应导航坐标系为北-东-地坐标系，f^b表示加速度计测量的加速度矢量信息，gⁿ表示重力，

表示在导航坐标系下陀螺仪测量的角速度矢量信息，

表示在导航坐标系下地球自转引起的角速度变化矢量，Q表示姿态四元数，

表示姿态四元数率，

表示载体坐标系相对于导航坐标系的瞬时角速度矢量；

所述的在室外情况下，通过GNSS和INS对待测点进行组合定位，获得待测点的室外导航信息，包括：

在室外GNSS有效时，根据INS解算得到待测点的位置、速度、姿态信息和GNSS提供的卫星星历，计算卫星到待测点之间的距离，将GNSS测量的伪距、伪距率与INS解算的伪距、速度作差，将得到的差值作为卡尔曼滤波器的量测输入，利用所述卡尔曼滤波器的输出值对INS的解算结果进行修正；

所述卡尔曼滤波器的系统状态向量由17个状态参数组成，公式如下：

其中，δA，δP，δV分别是三轴姿态角误差，位置误差和速度误差，δb_ω和δb_f分别是陀螺仪测量误差和加速度计测量误差，dt表示GNSS接收机的时钟钟差，

表示时钟漂移率；

所述卡尔曼滤波器的系统量测向量如下：

其中，

表示GNSS原始测量伪距，

表示GNSS原始测量伪距率，ρ_i ^INS表示INS解算的位置结果与GNSS卫星之间的几何距离，

表示由INS解算结果计算的移动点到卫星的几何距离变化率，m表示当前时刻的可见卫星数；

所述卡尔曼滤波器的量测矩阵H(k)如下：

其中，L(k)_m×3写为：

是GNSS卫星在ECEF坐标系下的坐标，(x y z)是由INS解算的在ECEF坐标系下的标签坐标；

所述卡尔曼滤波器的测量噪声协方差矩阵由定位精度经验值获得；

所述的在室内情况下，通过UWB和INS对待测点进行组合定位，获得待测点的室内导航信息，包括：

在室内UWB信号有效而GNSS信号失效时，根据INS解算的待测点的位置、速度、姿态信息和UWB定位基站的坐标值，计算定位基站到待测点之间的距离，将UWB得到的原始距离量测与INS解算的距离量测作差，将得到的差值作为卡尔曼滤波器的量测输入，利用所述卡尔曼滤波器的输出值对INS的解算结果进行修正；

所述卡尔曼滤波的系统状态向量由18个状态组成，公式如下：

X＝[δA δP δV δb_ω δb_f δη_L]^T

其中，δA，δP，δV分别是姿态角误差，位置误差和速度误差，δb_ω和δb_f分别是陀螺仪偏差和加速度计偏差，δη_L表示INS几何中心与UWB待测标签的相对位置差，即杠杆臂的值；

所述卡尔曼滤波的系统量测向量为：

其中，

表示INS解算的位置结果与UWB基站之间的几何距离，

表示UWB原始距离观测量，n代表UWB基站个数；

H(k)是量测矩阵，公式如下：

H(k)＝[0_n×3 D(k)_n×3 0_n×3 0_n×3 0_n×3 D(k)_n×3]

其中，

是UWB基站在ECEF坐标系下的坐标，(x y z)是由INS解算的在ECEF坐标系下的标签坐标；

所述卡尔曼滤波的测量噪声协方差矩阵由定位精度经验值获得。

Images