一种室内外无缝弹性融合定位方法、系统、介质及设备
文献发布时间:2024-04-18 19:58:26
技术领域
本发明属于定位技术领域,尤其涉及一种室内外无缝弹性融合定位方法、系统、介质及设备。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术信息,不必然构成在先技术。
随着现代化社会智慧城市的建设和导航位置服务需求的不断提升,室内外无缝导航定位受到了人们极大的关注。
陆地场景多源传感器融合导航中,包括北斗卫星导航系统(BeiDou NavigationSatellite System,BDS)在内的全球导航卫星系统(Global Navigation SatelliteSystem,GNSS)对于室外开阔环境可以提供准确定位和最大范围覆盖。但对于城市环境,GNSS信号无法穿透建筑物且受室内环境影响很大,在许多室内环境中并不可行,高精度、低成本、易安装的室内定位系统仍然是室内外无缝导航定位的难题和挑战。惯性导航系统(Inertial Navigation System,INS)采用加速度计和陀螺仪等惯性器件获取的数据经过积分运算可以获得位置、姿态以及速度等信息,但是,受限于器件工艺及积分计算原理,INS定位精度随着时间推移会不断降低,因而独立的INS很难满足长时间高精度导航定位要求。超宽带(Ultra Wide Band,UWB)具有宽带宽、窄脉冲和高时间分辨率的优势,主要体现在基于时间到达的测量方式。因此,基于到达时间(Time of Arrival,TOA)技术的UWB定位模型凭借其测距精度和技术可行性高的优势得到了广泛的研究与应用,基本实现了分米级定位精度。然而,针对室内复杂环境仍然存在着诸多问题和不足,比如:因为信号传播路径上遮挡物的材质、数量、材质等环境因素,极有可能存在严重的多路径效应、非视距传播等影响定位精度的情况;另外定位精度和定位服务范围与UWB基站的数量和位置也有较大关联。UWB作为室内绝对定位传感器,相较于蓝牙、Wi-Fi等具有高精度、穿透力强、传输效率高等优势,是城市复杂环境下提供位置服务的一种重要手段,解决非视距误差对定位精度的影响是亟需解决的问题之一。
现有方法在面对室内外无缝环境时可以提供一定的导航定位能力,但还存在各种各样的问题。随着卫星精密轨道和精密钟差精度的显著提升,目前精密单点定位(PrecisePoint Positioning,PPP)技术已成为一种提供全天候、全天时位置服务的比较有效的方法,而且多系统GNSS PPP相比GPS PPP具有更快的收敛速度和更高的精度,然而在GNSS卫星可见数较少和星座几何构型较差的情况下,PPP技术难以提供高精度的位置服务。INS技术可以提供短时间高精度的位置推算,但由于其存在累计误差,定位精度随时间发散,一般不能单独提供位置服务。一些研究将GNSS和INS联合起来,以提高在城市复杂环境下定位的鲁棒性,GNSS接收机在INS协助下,可大大改善信号捕获、跟踪性能,INS在GNSS辅助下,可实现导航器件误差的定期校正,二者优势互补,实现了高于各自独立系统的定位性能。GNSS/INS松组合直接融合GNSS和INS的位置信息,因其结构简单而被广泛应用,但当子系统无法工作时,提供的位置服务将是不连续的;GNSS/INS紧组合是观测信息层面上的融合,当GNSS可见卫星数少于4颗时,也能正常工作,但当GNSS卫星中断时间较长时,稀疏的GNSS观测也难以校正INS累计误差,特别是低精度的微机械惯性器件。
基于联邦滤波的GNSS RTK(Real - time kinematic,实时动态)、UWB、INS融合的室内外无缝定位技术,利用GNSS和UWB的观测信息对惯导误差状态向量进行量测更新,在子滤波器中实现观测值层面的紧组合,最后在主滤波器中将各子滤波器信息进行融合,并依据观测值质量进行动态信息分配,从而实现室内外连续无缝定位;基于GNSS PPP技术和UWB定位技术,联立GNSS PPP无电离层组合观测方程和UWB TOA 观测方程,在GNSS可见卫星数较少和观测几何构型较差的情况下,UWB 作为外部传感器可以很好的补充到观测方程中,实现了GNSS和UWB观测层面上的融合,在GNSS卫星数少于4颗时也能提供位置服务。
虽然以上研究致力于多源融合室内外无缝定位方法的研究中,但,由于城市室内外复杂环境造成的非视距误差,位置信息估计性能较低,且时延偏差对定位精度的影响较大。
发明内容
为了解决上述背景技术中存在的技术问题,本发明提供一种室内外无缝弹性融合定位方法、系统、介质及设备,将GNSS、UWB、INS三者在观测值层面进行融合且针对城市室内外复杂环境造成的非视距误差,通过多因子自适应调节量测噪声等价协方差矩阵,提高了位置信息估计性能。
为了实现上述目的,本发明采用如下技术方案:
本发明的第一个方面提供一种室内外无缝弹性融合定位方法,其包括:
获取GNSS观测值,通过无电离层组合,得到无电离层观测值;
获取UWB测距信息,进行时延补偿,得到时延补偿后的UWB测距信息,并计算标签等效时延偏差;
获取INS测得的角速度和加速度,进行惯导机械编排,得到惯导机械编排结果;
基于标签等效时延偏差,计算得到量测的当前时刻误差状态变量,结合惯导机械编排结果,通过状态传播模型,预测得到下一时刻误差状态变量;
基于所述量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值,计算视距环境下检验信息,并通过多因子自适应调节量测噪声等价协方差矩阵;
基于量测噪声等价协方差矩阵,分别对无电离层观测值和时延补偿后的UWB测距信息进行更新,进而更新所述当前时刻误差状态变量,对惯导机械编排结果进行校正,得到定位结果。
进一步地,所述标签等效时延偏差满足:
式中:
进一步地,所述基站相对于主基站的时延偏差为,基站的时延偏差与主基站的时延偏差的差值。
进一步地,所述时延补偿后的UWB测距信息为,UWB测距信息减去基站相对于主基站的时延偏差。
进一步地,所述惯导机械编排结果包括惯导姿态、惯导速度和惯导位置。
进一步地,所述状态传播模型为:
式中:
进一步地,所述误差状态变量包括:位置误差、速度误差、姿态误差、陀螺仪零偏误差、加速度计零偏误差以及所述标签等效时延偏差。
本发明的第二个方面提供一种室内外无缝弹性融合定位系统,其包括:
GNSS模块,其被配置为:获取GNSS观测值,通过无电离层组合,得到无电离层观测值;
UWB模块,其被配置为:获取UWB测距信息,进行时延补偿,得到时延补偿后的UWB测距信息,并计算标签等效时延偏差;
INS模块,其被配置为:获取INS测得的角速度和加速度,进行惯导机械编排,得到惯导机械编排结果;
第一融合模块,其被配置为:基于标签等效时延偏差,计算得到量测的当前时刻误差状态变量,结合惯导机械编排结果,通过状态传播模型,预测得到下一时刻误差状态变量;
第二融合模块,其被配置为:基于所述量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值,计算视距环境下检验信息,并通过多因子自适应调节量测噪声等价协方差矩阵;
第三融合模块,其被配置为:基于量测噪声等价协方差矩阵,分别对无电离层观测值和时延补偿后的UWB测距信息进行更新,进而更新所述当前时刻误差状态变量,对惯导机械编排结果进行校正,得到定位结果。
本发明的第三个方面提供一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上述所述的一种室内外无缝弹性融合定位方法中的步骤。
本发明的第四个方面提供一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现如上述所述的一种室内外无缝弹性融合定位方法中的步骤。
与现有技术相比,本发明的有益效果是:
本发明提供了一种室内外无缝弹性融合定位方法,其将GNSS、UWB和INS三者结合,实现了观测值层面的紧组合,在某一或多个传感器观测信息不足的情况下也可以进行解算,且针对城市室内外复杂环境造成的非视距误差,通过多因子自适应调节量测噪声等价协方差矩阵,以提高位置信息估计性能。
本发明提供了一种室内外无缝弹性融合定位方法,其基于UWB定位模型,建立UWB时间同步站,以UWB某一时钟稳定的基站作为主基站,利用时间同步站与UWB基站的时差观测信息提前标定基站相对于主基站的时延偏差,对UWB测距信息进行补偿,解算参数中包含标签端位置信息和等效时延偏差,充分考虑时延偏差对定位精度的影响。
本发明提供了一种室内外无缝弹性融合定位方法,其利用集中式卡尔曼滤波将GNSS、UWB、INS三者在观测值层面进行融合,并利用IAE算法实现室内外等复杂环境下的弹性选择,有效的挖掘不同传感器的互补性,增强融合定位算法的有效性,提升复杂环境位置服务的精度和可靠性。
附图说明
构成本发明的一部分说明书附图用来提供对本发明的进一步理解,本发明的示意性实施例及其说明用于解释本发明,并不构成对本发明的限定。
图1是本发明实施例一的一种室内外无缝弹性融合定位方法的流程图。
具体实施方式
下面结合附图与实施例对本发明作进一步说明。
应该指出,以下详细说明都是例示性的,旨在对本发明提供进一步的说明。除非另有指明,本文使用的所有技术和科学术语具有与本发明所属技术领域的普通技术人员通常理解的相同含义。
实施例一
为解决单一传感器的脆弱性,实现室内外精确和连续的导航定位性能,本实施例提供了一种室内外无缝弹性融合定位方法,其采用集中式卡尔曼滤波将GNSS、UWB和INS三者结合,实现了观测值层面的紧组合,充分考虑UWB时延偏差(UWB标签时延偏差
虽然现有研究致力于多源融合室内外无缝定位方法的研究中,但如何有效的挖掘不同传感器的互补性,增强融合定位算法的有效性,提升复杂环境位置服务的精度和可靠性,仍有待更加深入的研究。
本实施例提供的一种室内外无缝弹性融合定位方法,基于UWB定位模型,建立UWB时间同步站,以UWB某一时钟稳定的基站作为主基站,利用时间同步站与UWB基站的时差观测信息提前标定基站相对于主基站的时延偏差,对UWB测距信息进行补偿,解算参数中包含标签端位置信息和等效时延偏差,充分考虑时延偏差对定位精度的影响;采用集中式卡尔曼滤波对三种传感器进行融合定位解算,利用GNSS、UWB原始观测信息和INS预测的距离作为集中式卡尔曼滤波的量测输入,在某一或多个传感器观测信息不足的情况下也可以进行解算;针对城市室内外复杂环境造成的非视距误差,基于IAE算法,通过多因子自适应调节原始观测信息含有非视距误差的测距信息权重,以提高位置信息估计性能。
本实施例提供的一种室内外无缝弹性融合定位方法,利用集中式卡尔曼滤波将GNSS、UWB、INS三者在观测值层面进行融合;将INS误差方程作为滤波器的时间更新,当获取到新时刻的GNSS观测时,将GNSS无电离层观测值作为量测值加入到滤波器的量测更新部分,当获取到新时刻的UWB观测时,将进行时延补偿后的UWB测距信息加入到滤波器的量测更新部分;当GNSS或UWB原始观测信息含有非视距异常时,通过IAE算法构造多因子自适应调节原始观测信息中含有非视距异常观测的权重信息;进行时间更新和量测更新后,获得系统位置、速度、姿态误差以及陀螺仪、加速度计零偏误差,对惯导系统进行反馈校正,从而获得最终的导航结果。
本实施例提供的一种室内外无缝弹性融合定位方法,如图1所示,包括如下步骤:
步骤1、获取GNSS观测值,通过无电离层组合,得到无电离层观测值。
在本实施例中,GNSS观测值采用伪距观测值和载波相位观测值。
GNSS单点定位的主要原理为距离后方交会,其算法模型函数表达式为:
(1)
式中:
无电离层组合模型利用不同频率信号间的组合消除电离层参数,重新组成定位方程,也是目前双频、三频信号最常用的组合方式。不同频率的电离层延迟有以下关系:
(2)
其中,
根据式(2),可定义
(3)
式中:
将式(3)代入式(1)中,可得无电离层观测值:
(4)
式中:
步骤2、获取UWB观测,并进行UWB时延补偿,得到时延补偿后的UWB测距信息。
在本实施例中,UWB观测采用UWB测距信息,即基站与标签之间的距离观测值。
针对UWB定位中存在时延偏差的问题,以UWB定位中某一时钟稳定的基站作为主基站,并建立时间同步站,时间同步站为任意能同时接收UWB基站信号的标签。利用时间同步站时差观测值
UWB TOA观测方程可建模为:
(5)
式中:
假设以基站0作为主基站,UWB基于时差(TDOA)的观测方程可建模为:
(6)
式中:
由于基站
(7)
式中:
利用基站
步骤3、获取INS(惯导)中陀螺仪和加速度计得到的角速度和加速度,进行惯导机械编排,机械编排分别包括姿态(惯导姿态矩阵)、惯导速度和惯导位置更新,得到惯导机械编排结果(包括更新的惯导姿态、惯导速度和惯导位置)。
选取E-N-U地理坐标系(站心坐标系)作为系统的导航坐标系,即
(8)
(9)
(10)
式中:
(1)姿态更新。惯导姿态更新直接采用姿态阵链乘拆解方法,根据姿态微分方程式(8)得:
(11)
式中:
假设陀螺仪在
(12)
(13)
式中:
在积分周期
(14)
(15)
(2)速度更新。对速度微分方程式(9)两边同时积分,得:
(16)
式中:
将式(16)移项,得到惯导速度更新算法递推形式为:
(17)
采用二子样假设补偿
(18)
式中:等式右边第二项为导航系旋转修正,等式右边第三项分别为旋转效应补偿和划桨效应补偿;
在积分周期
(19)
(3)位置更新。惯导位置更新通过对位置微分方程式(10)进行积分,并离散化处理,求得惯导的位置参数。假设积分周期
(20)
其中,
步骤4、基于标签等效时延偏差,计算得到量测的当前时刻误差状态变量,结合惯导机械编排结果,通过状态传播模型进行状态更新,预测得到下一时刻误差状态变量
本实施例的GNSS/UWB/INS融合采用误差(Errors)(包括,位置误差、速度误差、姿态误差、陀螺仪与加速度计零偏误差)以及标签等效时延偏差作为系统状态变量,维数为16×1,即量测的当前时刻误差状态变量为:
(21)
式中:
系统状态变量误差微分方程为:
(22)
式中:
(23)
(24)
(25)
其中,
为方便使用离散时间卡尔曼滤波,对系统状态变量误差微分方程进行离散化,构建离散时间系统状态方程,即状态传播模型:
(26)
式中:
为离散时间状态噪声协方差阵,可简化为梯形积分:
(27)
式中:
步骤5、基于IAE的多因子自适应弹性模型。当GNSS或UWB观测值检测到含有非视距误差时,基于IAE算法将GNSS和UWB量测协方差进行弹性调整,从而得到更加鲁棒的组合定位结果。
由于非视距距离误差与传播通道内障碍物数量、材质、厚度、几何形状等因素相关,目前缺乏有效的方法来精确估计非视距误差的大小,虽然加入IMU信息可以削弱非视距误差对定位结果的影响,但如果一直保持协方差不变,随着非视距误差持续时间的增加,IMU作用会越来越低,直至非视距误差把INS解算的结果拉偏为止。这里将IAE算法应用于GNSS/UWB/INS定位中,根据观测信息的可靠性自适应调节每个观测信息的权重,避免可靠的观测值可能会损失其使用效率而对于误差较大的观测值而得不到应有的控制的影响。
构造检验信息
(28)
式中:
构造等价权因子
(29)
式中:
令
(30)
式中:
步骤6、进行多因子自适应后,将量测噪声等价协方差同步骤1得到的无电离层观测值和步骤2得到的时延补偿后的UWB测距信息输入EKF的量测更新部分,更新误差状态向量
本实施例GNSS/UWB/INS融合EKF量测更新方程为:
(31)
式中:
量测更新根据当前历元观测值的不同情况分为GNSS/INS量测更新、UWB/INS量测更新。
(1)GNSS/INS量测更新。GNSS/INS量测更新部分可表示为:
(32)
式中:
GNSS/INS组合的量测更新系数阵
(33)
(34)
(35)
(36)
(37)
式中:
(38)
(39)
(2)UWB/INS量测更新。由INS导航结果及杆臂测量值推导出UWB标签天线中心位置为:
(40)
式中:
INS导航结果及杆臂测量值推导出的UWB标签天线中心位置与UWB基站位置之间的概略距离可表示为:
(41)
式中:
UWB/INS量测更新部分可表示为:
(42)
式中:
UWB/INS组合的量测更新系数阵
(43)
步骤7、进行时间更新和量测更新后,得到更新后的当前时刻误差状态变量,获得系统位置、速度、姿态误差以及陀螺仪、加速度计零偏误差,对惯导机械编排结果进行校正,从而获得最终的导航结果(定位结果)。
通过仿真和实测数据对比分析,结果表明:相较于传统方法,采用本实施例的方法可以有效削弱UWB时延偏差和非视距误差对室内外无缝导航定位的影响,实现连续可靠的室内外位置估计。
实施例二
本实施例提供了一种室内外无缝弹性融合定位系统,其具体包括:
GNSS模块,其被配置为:获取GNSS观测值,通过无电离层组合,得到无电离层观测值;
UWB模块,其被配置为:获取UWB测距信息,进行时延补偿,得到时延补偿后的UWB测距信息,并计算标签等效时延偏差;
INS模块,其被配置为:获取INS测得的角速度和加速度,进行惯导机械编排,得到惯导机械编排结果;
第一融合模块,其被配置为:基于标签等效时延偏差,计算得到量测的当前时刻误差状态变量,结合惯导机械编排结果,通过状态传播模型,预测得到下一时刻误差状态变量;
第二融合模块,其被配置为:基于所述量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值,计算视距环境下检验信息,并通过多因子自适应调节量测噪声等价协方差矩阵;
第三融合模块,其被配置为:基于量测噪声等价协方差矩阵,分别对无电离层观测值和时延补偿后的UWB测距信息进行更新,进而更新所述当前时刻误差状态变量,对惯导机械编排结果进行校正,得到定位结果。
此处需要说明的是,本实施例中的各个模块与实施例一中的各个步骤一一对应,其具体实施过程相同,此处不再累述。
实施例三
本实施例提供了一种计算机可读存储介质,其上存储有计算机程序,该程序被处理器执行时实现如上述实施例一所述的一种室内外无缝弹性融合定位方法中的步骤。
实施例四
本实施例提供了一种计算机设备,包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序,所述处理器执行所述程序时实现如上述实施例一所述的一种室内外无缝弹性融合定位方法中的步骤。
以上所述仅为本发明的优选实施例而已,并不用于限制本发明,对于本领域的技术人员来说,本发明可以有各种更改和变化。凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
- 一种基于智能终端的定位方法、系统、存储介质及设备
- 一种定位方法、装置、设备、系统及存储介质
- 一种低功耗定位方法、装置、系统、计算机设备和存储介质
- 一种室内外无缝定位方法、系统、设备及介质
- 一种基于多传感器融合的机器人室内外无缝定位方法及系统