CN117148406A - 一种室内外无缝弹性融合定位方法、系统、介质及设备 - Google Patents

一种室内外无缝弹性融合定位方法、系统、介质及设备 Download PDF

Info

Publication number
CN117148406A
CN117148406A CN202311413863.7A CN202311413863A CN117148406A CN 117148406 A CN117148406 A CN 117148406A CN 202311413863 A CN202311413863 A CN 202311413863A CN 117148406 A CN117148406 A CN 117148406A
Authority
CN
China
Prior art keywords
inertial navigation
indoor
state variable
error state
time delay
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202311413863.7A
Other languages
English (en)
Other versions
CN117148406B (zh
Inventor
徐天河
代晓霁
李敏
姚凌寒
江楠
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong University
Original Assignee
Shandong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shandong University filed Critical Shandong University
Priority to CN202311413863.7A priority Critical patent/CN117148406B/zh
Publication of CN117148406A publication Critical patent/CN117148406A/zh
Application granted granted Critical
Publication of CN117148406B publication Critical patent/CN117148406B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/023—Services making use of location information using mutual or relative location information between multiple location based services [LBS] targets or of distance thresholds
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W56/00—Synchronisation arrangements
    • H04W56/004—Synchronisation arrangements compensating for timing error of reception due to propagation delay
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination

Abstract

本发明涉及定位技术领域，本发明公开了一种室内外无缝弹性融合定位方法、系统、介质及设备，包括：基于标签等效时延偏差，计算得到量测的当前时刻误差状态变量，结合惯导机械编排结果，通过状态传播模型，预测得到下一时刻误差状态变量；基于量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值，计算视距环境下检验信息，并通过多因子自适应调节量测噪声等价协方差矩阵；基于量测噪声等价协方差矩阵，分别对无电离层观测值和时延补偿后的UWB测距信息进行更新，进而更新所述当前时刻误差状态变量，对惯导机械编排结果进行校正，得到定位结果。提高了位置信息估计性能。

Description

一种室内外无缝弹性融合定位方法、系统、介质及设备
技术领域
本发明属于定位技术领域，尤其涉及一种室内外无缝弹性融合定位方法、系统、介质及设备。
背景技术
本部分的陈述仅仅是提供了与本发明相关的背景技术信息，不必然构成在先技术。
随着现代化社会智慧城市的建设和导航位置服务需求的不断提升，室内外无缝导航定位受到了人们极大的关注。
陆地场景多源传感器融合导航中，包括北斗卫星导航系统（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测距信息；/>为基站i相对于主基站的时延偏差；r i为基站i与标签之间的几何距离；/>为噪声。
进一步地，所述基站相对于主基站的时延偏差为，基站的时延偏差与主基站的时延偏差的差值。
进一步地，所述时延补偿后的UWB测距信息为，UWB测距信息减去基站相对于主基站的时延偏差。
进一步地，所述惯导机械编排结果包括惯导姿态、惯导速度和惯导位置。
进一步地，所述状态传播模型为：
式中：k表示第k个离散时间，为状态转移矩阵，/>为离散化时间过程噪声。
进一步地，所述误差状态变量包括：位置误差、速度误差、姿态误差、陀螺仪零偏误差、加速度计零偏误差以及所述标签等效时延偏差。
本发明的第二个方面提供一种室内外无缝弹性融合定位系统，其包括：
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标签时延偏差cδt r和UWB基站时延偏差cδt b）的对定位性能的影响，并利用新息自适应估计（Innovation BasedAdaptive Estimation, IAE）算法，通过多因子自适应调节原始观测信息中含有非视距异常观测的权重信息，实现了室内外无缝弹性融合定位属于陆地场景组合导航范畴；有效削弱了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)
式中：P为伪距观测值，c表示光速，L为相位观测值，为接收机与卫星间的几何距离，/>和/>分别为接收机钟差和卫星钟差，T为对流层延迟，I为电离层延迟，/>和/>分别为接收机端码偏差和卫星端码偏差，/>和/>分别为接收机端相位偏差和卫星端相位偏差，N为整周模糊度，/>和/>分别为伪距观测噪声和载波观测噪声。
无电离层组合模型利用不同频率信号间的组合消除电离层参数，重新组成定位方程，也是目前双频、三频信号最常用的组合方式。不同频率的电离层延迟有以下关系：
(2)
其中，I i为L i频率GNSS信号的电离层延迟；I j为L j频率GNSS信号的电离层延迟；为L i频率GNSS信号的频率大小；/>为L j频率GNSS信号的频率大小。
根据式(2)，可定义L 1和L 2频率的无电离层数学模型：
(3)
式中：和/>分别为GNSS伪距和载波观测组合后的无电离层观测值；P 1为L 1频率GNSS伪距观测值；P 2为L 2频率GNSS伪距观测值；L 1为L 1频率GNSS载波观测值；L 2为L 2频率GNSS载波观测值。
将式(3)代入式(1)中，可得无电离层观测值：
(4)
式中：，/>，/>，在浮点模糊度解中它们都被当成一个整体参数估计；/>为无电离层接收机端码偏差；为无电离层卫星端码偏差；/>为无电离层接收机端相位偏差；/>为无电离层卫星端相位偏差；/>为无电离层整周模糊度；/>为无电离层伪距观测噪声；/>为无电离层载波观测噪声。
步骤2、获取UWB观测，并进行UWB时延补偿，得到时延补偿后的UWB测距信息。
在本实施例中，UWB观测采用UWB测距信息，即基站与标签之间的距离观测值。
针对UWB定位中存在时延偏差的问题，以UWB定位中某一时钟稳定的基站作为主基站，并建立时间同步站，时间同步站为任意能同时接收UWB基站信号的标签。利用时间同步站时差观测值对UWB测距信息/>进行补偿，并将UWB标签等效时延偏差/>作为待求参数，以削弱UWB定位中时延偏差对定位性能的影响。
UWB TOA观测方程可建模为：
(5)
式中：为UWB基站i与标签之间的距离观测值；r i为基站i与标签之间的几何距离；cδt r为标签的时延偏差；cδt b为基站i的时延偏差；/>为其他噪声。
假设以基站0作为主基站，UWB基于时差（TDOA）的观测方程可建模为：
(6)
式中：为基站i与主基站0的时差观测值；r 0i为基站i与主基站0的时差几何距离；/>为基站i相对于主基站0时延偏差，/>，/>为主基站0的时延偏差；为其他噪声。
由于基站i相对于主基站0的时延偏差，可以通过建立时间同步站的方式提前标定或实时播发，在定位过程中将时延偏差/>当作已知值，代入等式（5），则UWB TOA观测方程为：
(7)
式中：为标签等效时延偏差，/>。
利用基站i与标签之间的距离观测值和基站i相对于主基站0的时延偏差，得到时延补偿后的UWB测距信息/>。
步骤3、获取INS（惯导）中陀螺仪和加速度计得到的角速度和加速度，进行惯导机械编排，机械编排分别包括姿态（惯导姿态矩阵）、惯导速度和惯导位置更新，得到惯导机械编排结果（包括更新的惯导姿态、惯导速度和惯导位置）。
选取E-N-U地理坐标系（站心坐标系）作为系统的导航坐标系，即n系。以n系为参考的姿态、速度、位置微分方程分别是：
(8)
(9)
(10)
式中：为载体系（b系）相对于导航坐标系（n系）的姿态阵，/>为b系相对于n系的角速度，/>表示反对称阵，/>为加速度计测量的比力，/>为地球自传角速度，/>为牵连角速度，是由地球曲率引起的导航系旋转，/>为重力加速度在n系下的投影；/>为n系下惯导速度；/>为n系下惯导位置；/>为n系下惯导速度的微分。
（1）姿态更新。惯导姿态更新直接采用姿态阵链乘拆解方法，根据姿态微分方程式(8)得：
(11)
式中：和/>分别为t m-1和t m时刻惯导姿态矩阵，/>和/>分别为以i系为参考基准，n系和b系从t m时刻到t m-1时刻的旋转矩阵；/>为t m时刻b系到i系的旋转矩阵、/>为t m时刻i系到n系的旋转矩阵、/>为t m-1时刻i系到n系的旋转矩阵、为t m-1时刻b系到i系的旋转矩阵。
假设陀螺仪在t m-1时刻到t m时刻进行了两次等间隔采样（对角速度进行采样），角增量分别为和/>，采用双子样补偿算法，对等效旋转矢量微分方程离散化，有：
(12)
(13)
式中：为方向余弦矩阵与等效旋转矢量之间的函数关系。
在积分周期 内，一般认为由地球自转和牵连角速率引起的/>变化很小，可将其视为常值计算，有：
(14)
(15)
（2）速度更新。对速度微分方程式(9)两边同时积分，得：
(16)
式中：和/>分别为以n系为参考的加速度增量和有害加速度增量；和/>分别为/>和/>时刻的惯导速度。
将式(16)移项，得到惯导速度更新算法递推形式为：
(17)
采用二子样假设补偿中角速度和比力向量的转动不可交换误差，得：
(18)
式中：等式右边第二项为导航系旋转修正，等式右边第三项分别为旋转效应补偿和划桨效应补偿；为惯导采样间隔/>；/>表示/>系相对于/>系的旋转；/>为加速度计采样比力速度增量；/>为陀螺采样角增量；/>为加速度计采样比力速度增量；/>为第二次等间隔加速度计采样比力速度增量；/>为第一次等间隔加速度计采样比力速度增量。
在积分周期内引起的导航系旋转和重力矢量变化量很小，/>可以认为是随时间变化的缓慢量，可采用积分区间中点时刻/>进行近似计算，有：
(19)
（3）位置更新。惯导位置更新通过对位置微分方程式(10)进行积分，并离散化处理，求得惯导的位置参数。假设积分周期内惯导速度随时间呈线性变化，由梯形积分即可求得惯导位置：
(20)
其中，为t m-1时刻惯导在n系的位置；/>为t m-1时刻惯导在n系的速度；/>为t m时刻惯导在n系的速度。
步骤4、基于标签等效时延偏差，计算得到量测的当前时刻误差状态变量，结合惯导机械编排结果，通过状态传播模型进行状态更新，预测得到下一时刻误差状态变量，并将其作为EKF的时间更新部分。
本实施例的GNSS/UWB/INS融合采用误差（Errors）（包括，位置误差、速度误差、姿态误差、陀螺仪与加速度计零偏误差）以及标签等效时延偏差作为系统状态变量，维数为16×1，即量测的当前时刻误差状态变量为：
(21)
式中：为n系下位置误差，维数为3×1；/>为n系下速度误差，维数为3×1；/>为姿态误差，维数为3×1；/>为陀螺仪零偏误差，维数为3×1；/>为加速度计零偏误差，维数为3×1；/>为GNSS接收机钟差或UWB标签等效时延偏差。
系统状态变量误差微分方程为：
(22)
式中：F为误差微分方程系统矩阵，如式(23)所示，维数为16×16；w为对应状态的过程噪声，如式(24)所示，其中表示加速度计量测白噪声，/>表示陀螺仪量测白噪声，和/>分别表示陀螺仪和加速度计的零偏噪声；G为过程噪声转移矩阵，如式(25)所示。
(23)
(24)
(25)
其中，为加速度计的比力；/>为3×3的单位阵；/>为陀螺仪采样间隔；/>为加速度计采样间隔；w为过程噪声，/>为1×3的零矩阵；/>表示加速度计量测白噪声，/>表示陀螺仪量测白噪声，/>和/>分别表示陀螺仪和加速度计的零偏噪声。
为方便使用离散时间卡尔曼滤波，对系统状态变量误差微分方程进行离散化，构建离散时间系统状态方程，即状态传播模型：
(26)
式中：k表示第k个离散时间，为离散时间系统状态转移矩阵，当F在/>时间内变化不剧烈时，可简化为/>，/>为离散时间间隔；/>为离散化时间过程噪声。
为离散时间状态噪声协方差阵，可简化为梯形积分：
(27)
式中：q为IMU传感器功率谱密度，为/>过程噪声对应的协方差。基于/>，可计算系统预测协方差矩阵/>。
步骤5、基于IAE的多因子自适应弹性模型。当GNSS或UWB观测值检测到含有非视距误差时，基于IAE算法将GNSS和UWB量测协方差进行弹性调整，从而得到更加鲁棒的组合定位结果。
由于非视距距离误差与传播通道内障碍物数量、材质、厚度、几何形状等因素相关，目前缺乏有效的方法来精确估计非视距误差的大小，虽然加入IMU信息可以削弱非视距误差对定位结果的影响，但如果一直保持协方差不变，随着非视距误差持续时间的增加，IMU作用会越来越低，直至非视距误差把INS解算的结果拉偏为止。这里将IAE算法应用于GNSS/UWB/INS定位中，根据观测信息的可靠性自适应调节每个观测信息的权重，避免可靠的观测值可能会损失其使用效率而对于误差较大的观测值而得不到应有的控制的影响。
构造检验信息：
(28)
式中：为紧组合系统新息向量，为量测信息（量测的当前时刻误差状态变量）与预测信息（预测得到的当前时刻误差状态变量）的差值；/>为系统新息向量协方差矩阵，，/>为系统预测协方差矩阵。
构造等价权因子：
(29)
式中：c 1、 c 2为多因子阵检验阈值，其由视距环境下检验信息的取值规律得来。
令，/>为k时刻观测值的等价多因子阵，则系统量测噪声等价协方差矩阵为：
(30)
式中：为k时刻系统量测噪声等价协方差矩阵，并将其作为/>。
步骤6、进行多因子自适应后，将量测噪声等价协方差同步骤1得到的无电离层观测值和步骤2得到的时延补偿后的UWB测距信息输入EKF的量测更新部分，更新误差状态向量。
本实施例GNSS/UWB/INS融合EKF量测更新方程为：
(31)
式中：z k为量测信息；为量测矩阵；/>为量测噪声，符合高斯分布，根据量测噪声等价协方差矩阵/>得到。
量测更新根据当前历元观测值的不同情况分为GNSS/INS量测更新、UWB/INS量测更新。
（1）GNSS/INS量测更新。GNSS/INS量测更新部分可表示为:
(32)
式中：为GNSS量测信息，即更新后的无电离层观测值；m GNSS为GNSS无电离层观测；/>为INS惯导机械编排结果反算的无电离层观测；IF表示无电离层组合；/>和/>分别为IF载波相位和IF伪距测量，即P IF和L IF；ρ IF,INS为INS预测的IF几何范围；M w为湿映射函数；λ s,f为波长；/>为IF载波相位与接收机时钟相关的误差修正之和；/>为IF伪距与接收机时钟相关的误差修正之和；/>是IF载波相位的其他误差修正之和；/>是IF伪距的其他误差修正之和。
GNSS/INS组合的量测更新系数阵表示如下：
(33)
(34)
(35)
(36)
(37)
式中：为IF视距雅可比矩阵；/>是从导航系到地心地固坐标系的位置扰动误差的变换矩阵；/>值由式(13)给出：
(38)
(39)
（2）UWB/INS量测更新。由INS导航结果及杆臂测量值推导出UWB标签天线中心位置为：
(40)
式中：为INS解算位置；/>为将载体系b下的坐标转到导航系n下的旋转矩阵；/>为杆臂测量值；/>为UWB与IMU时间不同步造成的位置误差。
INS导航结果及杆臂测量值推导出的UWB标签天线中心位置与UWB基站位置之间的概略距离可表示为：
(41)
式中：、/>与/>分别为INS导航结果推导的e、n与u三方向上UWB标签天线中心位置，/>、/>与/>分别为基站i的e、n与u三方向上的位置。
UWB/INS量测更新部分可表示为：
(42)
式中：为UWB量测信息，即更新后的UWB测距信息；/>为时延补偿后的UWB测距信息。
UWB/INS组合的量测更新系数阵表示如下：
（43）
步骤7、进行时间更新和量测更新后，得到更新后的当前时刻误差状态变量，获得系统位置、速度、姿态误差以及陀螺仪、加速度计零偏误差，对惯导机械编排结果进行校正，从而获得最终的导航结果（定位结果）。
通过仿真和实测数据对比分析，结果表明：相较于传统方法，采用本实施例的方法可以有效削弱UWB时延偏差和非视距误差对室内外无缝导航定位的影响，实现连续可靠的室内外位置估计。
实施例二
本实施例提供了一种室内外无缝弹性融合定位系统，其具体包括：
GNSS模块，其被配置为：获取GNSS观测值，通过无电离层组合，得到无电离层观测值；
UWB模块，其被配置为：获取UWB测距信息，进行时延补偿，得到时延补偿后的UWB测距信息，并计算标签等效时延偏差；
INS模块，其被配置为：获取INS测得的角速度和加速度，进行惯导机械编排，得到惯导机械编排结果；
第一融合模块，其被配置为：基于标签等效时延偏差，计算得到量测的当前时刻误差状态变量，结合惯导机械编排结果，通过状态传播模型，预测得到下一时刻误差状态变量；
第二融合模块，其被配置为：基于所述量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值，计算视距环境下检验信息，并通过多因子自适应调节量测噪声等价协方差矩阵；
第三融合模块，其被配置为：基于量测噪声等价协方差矩阵，分别对无电离层观测值和时延补偿后的UWB测距信息进行更新，进而更新所述当前时刻误差状态变量，对惯导机械编排结果进行校正，得到定位结果。
此处需要说明的是，本实施例中的各个模块与实施例一中的各个步骤一一对应，其具体实施过程相同，此处不再累述。
实施例三
本实施例提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上述实施例一所述的一种室内外无缝弹性融合定位方法中的步骤。
实施例四
本实施例提供了一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述实施例一所述的一种室内外无缝弹性融合定位方法中的步骤。
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种室内外无缝弹性融合定位方法，其特征在于，包括：
获取GNSS观测值，通过无电离层组合，得到无电离层观测值；
获取UWB测距信息，进行时延补偿，得到时延补偿后的UWB测距信息，并计算标签等效时延偏差；
获取INS测得的角速度和加速度，进行惯导机械编排，得到惯导机械编排结果；
基于标签等效时延偏差，计算得到量测的当前时刻误差状态变量，结合惯导机械编排结果，通过状态传播模型，预测得到下一时刻误差状态变量；
基于所述量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值，计算视距环境下检验信息，并通过多因子自适应调节量测噪声等价协方差矩阵；
基于量测噪声等价协方差矩阵，分别对无电离层观测值和时延补偿后的UWB测距信息进行更新，进而更新所述当前时刻误差状态变量，对惯导机械编排结果进行校正，得到定位结果。
2.如权利要求1所述的一种室内外无缝弹性融合定位方法，其特征在于，所述标签等效时延偏差满足：
式中：为标签等效时延偏差，/>；/>为UWB测距信息；/>为基站i相对于主基站的时延偏差；r i为基站i与标签之间的几何距离；/>为噪声。
3.如权利要求2所述的一种室内外无缝弹性融合定位方法，其特征在于，所述基站相对于主基站的时延偏差为，基站的时延偏差与主基站的时延偏差的差值。
4.如权利要求1所述的一种室内外无缝弹性融合定位方法，其特征在于，所述时延补偿后的UWB测距信息为，UWB测距信息减去基站相对于主基站的时延偏差。
5.如权利要求1所述的一种室内外无缝弹性融合定位方法，其特征在于，所述惯导机械编排结果包括惯导姿态、惯导速度和惯导位置。
6.如权利要求1所述的一种室内外无缝弹性融合定位方法，其特征在于，所述状态传播模型为：
式中：k表示第k个离散时间，为状态转移矩阵，/>为离散化时间过程噪声。
7.如权利要求1所述的一种室内外无缝弹性融合定位方法，其特征在于，所述误差状态变量包括：位置误差、速度误差、姿态误差、陀螺仪零偏误差、加速度计零偏误差以及所述标签等效时延偏差。
8.一种室内外无缝弹性融合定位系统，其特征在于，包括：
GNSS模块，其被配置为：获取GNSS观测，通过无电离层组合，得到无电离层观测值；
UWB模块，其被配置为：获取UWB测距信息，进行时延补偿，得到时延补偿后的UWB测距信息，并计算标签等效时延偏差；
INS模块，其被配置为：获取INS测得的角速度和加速度，进行惯导机械编排，得到惯导机械编排结果；
第一融合模块，其被配置为：基于标签等效时延偏差，计算得到量测的当前时刻误差状态变量，结合惯导机械编排结果，通过状态传播模型，预测得到下一时刻误差状态变量；
第二融合模块，其被配置为：基于所述量测的当前时刻误差状态变量与预测得到的当前时刻误差状态变量的差值，计算视距环境下检验信息，并通过多因子自适应调节量测噪声等价协方差矩阵；
第三融合模块，其被配置为：基于量测噪声等价协方差矩阵，分别对无电离层观测值和时延补偿后的UWB测距信息进行更新，进而更新所述当前时刻误差状态变量，对惯导机械编排结果进行校正，得到定位结果。
9.一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行，其特征在于，该程序被处理器执行时实现如权利要求1-7中任一项所述的一种室内外无缝弹性融合定位方法中的步骤。
10.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1-7中任一项所述的一种室内外无缝弹性融合定位方法中的步骤。
CN202311413863.7A 2023-10-30 2023-10-30 一种室内外无缝弹性融合定位方法、系统、介质及设备 Active CN117148406B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311413863.7A CN117148406B (zh) 2023-10-30 2023-10-30 一种室内外无缝弹性融合定位方法、系统、介质及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311413863.7A CN117148406B (zh) 2023-10-30 2023-10-30 一种室内外无缝弹性融合定位方法、系统、介质及设备

Publications (2)

Publication Number Publication Date
CN117148406A true CN117148406A (zh) 2023-12-01
CN117148406B CN117148406B (zh) 2024-01-30

Family

ID=88908396

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311413863.7A Active CN117148406B (zh) 2023-10-30 2023-10-30 一种室内外无缝弹性融合定位方法、系统、介质及设备

Country Status (1)

Country Link
CN (1) CN117148406B (zh)

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104833354A (zh) * 2015-05-25 2015-08-12 梁步阁 一种多基多模组网融合室内人员导航定位系统及其实施方法
CN106793060A (zh) * 2017-03-08 2017-05-31 哈尔滨工程大学 一种超宽带室内定位方法
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
CN109946731A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN110645979A (zh) * 2019-09-27 2020-01-03 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
WO2020062531A1 (zh) * 2018-09-28 2020-04-02 北斗天地股份有限公司 一种定位装置及其定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN114779307A (zh) * 2022-06-17 2022-07-22 武汉理工大学 面向港区的uwb/ins/gnss的无缝定位方法
CN115435782A (zh) * 2022-08-29 2022-12-06 卓宇智能科技有限公司 一种多源信息约束下的抗干扰位置估计方法及装置
US20220413161A1 (en) * 2020-03-04 2022-12-29 Magellan Systems Japan, Inc. Indoor-outdoor dual-use high precision positioning system
CN115824223A (zh) * 2022-12-19 2023-03-21 浙江光遇科技有限公司 一种基于多源融合的室内外无缝定位方法
CN115951378A (zh) * 2023-03-08 2023-04-11 北京大学 一种基于北斗星基增强信息的自适应信息融合定位方法
CN116086448A (zh) * 2023-04-12 2023-05-09 成都信息工程大学 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法
KR20230115027A (ko) * 2022-01-26 2023-08-02 부경대학교 산학협력단 Uwb 및 amcl 기반 모바일 로봇의 실내 위치 측위 정확도를 높이기 위한 장치 및 방법

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104833354A (zh) * 2015-05-25 2015-08-12 梁步阁 一种多基多模组网融合室内人员导航定位系统及其实施方法
CN106793060A (zh) * 2017-03-08 2017-05-31 哈尔滨工程大学 一种超宽带室内定位方法
WO2020062531A1 (zh) * 2018-09-28 2020-04-02 北斗天地股份有限公司 一种定位装置及其定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN109946731A (zh) * 2019-03-06 2019-06-28 东南大学 一种基于模糊自适应无迹卡尔曼滤波的车辆高可靠融合定位方法
CN109946730A (zh) * 2019-03-06 2019-06-28 东南大学 一种车路协同下基于超宽带的车辆高可靠融合定位方法
CN109931926A (zh) * 2019-04-04 2019-06-25 山东智翼航空科技有限公司 一种基于站心坐标系的小型无人机无缝自主式导航算法
CN110645979A (zh) * 2019-09-27 2020-01-03 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法
US20220413161A1 (en) * 2020-03-04 2022-12-29 Magellan Systems Japan, Inc. Indoor-outdoor dual-use high precision positioning system
KR20230115027A (ko) * 2022-01-26 2023-08-02 부경대학교 산학협력단 Uwb 및 amcl 기반 모바일 로봇의 실내 위치 측위 정확도를 높이기 위한 장치 및 방법
CN114779307A (zh) * 2022-06-17 2022-07-22 武汉理工大学 面向港区的uwb/ins/gnss的无缝定位方法
CN115435782A (zh) * 2022-08-29 2022-12-06 卓宇智能科技有限公司 一种多源信息约束下的抗干扰位置估计方法及装置
CN115824223A (zh) * 2022-12-19 2023-03-21 浙江光遇科技有限公司 一种基于多源融合的室内外无缝定位方法
CN115951378A (zh) * 2023-03-08 2023-04-11 北京大学 一种基于北斗星基增强信息的自适应信息融合定位方法
CN116086448A (zh) * 2023-04-12 2023-05-09 成都信息工程大学 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
LINGHAN YAO ET AL.: "GNSS/UWB/INS indoor and outdoor seamless positioning algorithm based on federal filtering", MEASUREMENT SCIENCE AND TECHNOLOGY, vol. 35, no. 1 *
代晓霁等: "复杂环境下的UWB/PDR 紧组合定位方法", 导航定位学报, vol. 10, no. 6 *
刘韬;徐爱功;隋心;: "超宽带与惯性导航组合的室内导航定位", 测绘科学, no. 12 *
曹琢健: "基于GNSS/INS/UWB组合的室内外无缝定位方法研究", 中国硕士论文电子期刊, no. 2 *
李玉峰;张佩;王宇鹏;: "一种变权重k最近邻的室内外无缝定位算法", 微处理机, no. 02 *
蔡赣飞;徐爱功;洪州;隋心;: "抗差容积卡尔曼滤波及其在UWB室内定位中的应用", 测绘科学, no. 12 *

Also Published As

Publication number Publication date
CN117148406B (zh) 2024-01-30

Similar Documents

Publication Publication Date Title
CN107710017B (zh) 用于在实时运动模式和相对定位模式之间切换的卫星导航接收器及方法
US8756001B2 (en) Method and apparatus for improved navigation of a moving platform
Chiang et al. Assessment for INS/GNSS/odometer/barometer integration in loosely-coupled and tightly-coupled scheme in a GNSS-degraded environment
CN110823217B (zh) 一种基于自适应联邦强跟踪滤波的组合导航容错方法
AU2008260578B2 (en) Distance dependant error mitigation in real-time kinematic (RTK) positioning
US8164514B1 (en) Method and apparatus for fusing referenced and self-contained displacement measurements for positioning and navigation
US7576690B2 (en) Position determination with reference data outage
US20150130664A1 (en) Position tracking system and method using radio signals and inertial sensing
EP1221586A2 (en) Position and heading error-correction method and apparatus for vehicle navigation systems
US9377309B2 (en) Global positioning system (GPS) self-calibrating lever arm function
CN102171583A (zh) 位置估计方法和设备
US11428822B2 (en) Methods and systems for location determination
CA2733032C (en) Method and apparatus for improved navigation of a moving platform
CN103542854A (zh) 基于星载处理器的自主定轨方法
US20130090858A1 (en) System for measuring coseismic movements or vibrations of structures based on global navigation satellite systems-gnss and/or pseudolites
KR102052364B1 (ko) 반송파 위상 gps를 이용한 정밀 위치 추정 시스템 및 방법
CN114894181A (zh) 一种实时自主组合导航定位方法及装置
US6704650B1 (en) Technique for accurate distance and velocity calculations using the global positioning system (GPS)
CN115683094A (zh) 一种复杂环境下车载双天线紧耦合定位方法及系统
CN115561796A (zh) 一种电网无人机巡检实时定位方法和系统
US20240085567A1 (en) System and method for correcting satellite observations
Jiang et al. An analysis of PPP-GPS-based decentralized train multi-sensor navigation system
CN117148406B (zh) 一种室内外无缝弹性融合定位方法、系统、介质及设备
CN115220078A (zh) 基于载波相位差分的gnss高精度定位方法及导航方法
Elisson et al. Low cost relative GNSS positioning with IMU integration

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant