CN109655060B - 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及*** - Google Patents

基于kf/fir和ls-svm融合的ins/uwb组合导航算法及*** Download PDF

Info

Publication number
CN109655060B
CN109655060B CN201910122571.5A CN201910122571A CN109655060B CN 109655060 B CN109655060 B CN 109655060B CN 201910122571 A CN201910122571 A CN 201910122571A CN 109655060 B CN109655060 B CN 109655060B
Authority
CN
China
Prior art keywords
ins
uwb
east
navigation
fir filter
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.)
Active
Application number
CN201910122571.5A
Other languages
English (en)
Other versions
CN109655060A (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.)
University of Jinan
Original Assignee
University of Jinan
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 University of Jinan filed Critical University of Jinan
Priority to CN201910122571.5A priority Critical patent/CN109655060B/zh
Publication of CN109655060A publication Critical patent/CN109655060A/zh
Application granted granted Critical
Publication of CN109655060B publication Critical patent/CN109655060B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了基于KF/FIR和LS‑SVM融合的INS/UWB组合导航算法及***，当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估。与此同时，LS‑SVM算法处于培训模式，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS‑SVM算法的输入和培训目标，力求通过培训构造INS位置与位置误差的映射关系。一旦UWB信号不可用，LS‑SVM算法替代不能工作的KF/FIR滤波器，利用构建的映射关系对INS位置误差进行预估。本发明的提出，克服了传统KF/FIR滤波器在UWB失锁状态下不能正常工作的缺陷，实现了对导航信息的无缝预估。

Description

基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法及***
技术领域
本发明涉及复杂环境下组合定位技术领域，尤其涉及基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法及***。
背景技术
近年来，行人导航(Pedestrian Navigation,PN)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。
在现有的定位方式中，全球卫星导航***(Global Navigation SatelliteSystem,GNSS)是最为常用的一种方式。虽然GNSS能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，GNSS信号被严重遮挡，无法进行有效的工作。近年来，UWB(Ultra Wideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于UWB的目标跟踪应用于GNSS失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，UWB信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于UWB采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于UWB的目标跟踪在室内导航领域仍旧面临很多挑战。
在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种技术能够独立的完成导航定位。例如，需要UWB设备能够提供行人的导航信息，这就要求目标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低了组合导航模型的应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领域，紧组合模型直接将参与组合导航的子技术的原始传感器数据应用于最后的导航信息的解算，减少了子技术自行解算引入新误差的风险，提高了组合导航的精度，但是需要指出的是，现有紧组合导航模型均使用集中式模式，这一方式***容错能力差，并不利于日益精确复杂的组合导航模型。
发明内容
本发明的目的就是为了解决上述难题，提供了一种基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法及***。当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估。与此同时，LS-SVM算法处于培训模式，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，力求通过培训构造INS位置与位置误差的映射关系。一旦UWB信号不可用，LS-SVM算法替代不能工作的KF/FIR滤波器，利用构建的映射关系对INS位置误差进行预估。本发明的提出，克服了传统KF/FIR滤波器在UWB失锁状态下不能正常工作的缺陷，实现了对导航信息的无缝预估。
为实现上述目的，本发明的具体方案如下：
本发明的第一目的是公开一种基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法，包括：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用INS位置与位置误差的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。
进一步地，所述惯性导航器件IMU固定在目标行人足部，UWB参考节点预先布置在位置已知的导航环境中，UWB目标节点固定在目标行人身上。
进一步地，惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置的初值根据实际需要自行设定。
进一步地，所述KF/FIR滤波器的状态方程为：
Figure BDA0001972453650000031
其中，
Figure BDA0001972453650000032
分别为k时刻和k-1时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；
Figure BDA0001972453650000033
分别为k时刻和k-1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声；T为采样周期。
进一步地，所述KF/FIR滤波器的观测方程为：
Figure BDA0001972453650000034
其中，
Figure BDA0001972453650000035
为k时刻惯性器件IMU解算的东向和北向位置；
Figure BDA0001972453650000036
为k时刻UWB解算的东向和北向位置；
Figure BDA0001972453650000037
为k时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；
Figure BDA0001972453650000038
为k时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；vk为观测噪声；
Figure BDA0001972453650000039
为k时刻INS东向位置误差的观测值、
Figure BDA00019724536500000310
为k时刻INS北向位置误差的观测值。
进一步地，所述KF/FIR滤波器中KF算法的性能评估方法为：
计算信道质量评价指标Dk，判断Dk与门限door的关系，如果Dk<door，则信道质量正常，反之，则信道质量差，引入FIR滤波器。
本发明的第二目的是公开一种基于KF/FIR和LS-SVM融合的INS/UWB组合导航***，包括：服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用INS位置与位置误差的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。
本发明的第三目的是公开一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用INS位置与位置误差的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。
本发明的有益效果：
1、组合导航方法采用INS/UWB松组合模型，该模型以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；在此基础上，通过KF/FIR滤波将INS和UWB获取的导航信息进行数据融合，最终得到当前时刻最优的导航信息和对参考节点位置信息的预估。提高了数据融合滤波器的精度和鲁棒性。
2、当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估。与此同时，LS-SVM算法处于培训模式，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，力求通过培训构造INS位置与位置误差的映射关系。一旦UWB信号不可用，LS-SVM算法替代不能工作的KF/FIR滤波器，利用构建的映射关系对INS位置误差进行预估。
本发明的提出，克服了传统KF/FIR滤波器在UWB失锁状态下不能正常工作的缺陷，实现了对导航信息的无缝预估。
3、可用于室内环境下的行人中高精度定位。
附图说明
图1为UWB信号可用时的算法示意图；
图2为UWB信号不可用时的算法示意图。
具体实施方式：
下面结合附图对本发明进行详细说明：
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
为了解决背景技术提出的问题，本发明公开了基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法，包括：
(1)以惯性导航***INS的东向和北向的位置和速度误差作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；
组合导航***主要包括惯性导航***和UWB无线定位***两部分。其中，惯性导航***采用固定在目标行人足部的惯性测量单元(IMU)。UWB无线定位***主要包括目标节点和参考节点两部分，其中，参考节点预先布置在位置已知的导航环境中；目标节点则固定在目标行人身上。
(2)当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，与此同时，LS-SVM算法处于培训模式，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系，如图1所示；
其中，KF/FIR滤波器的状态方程为：
Figure BDA0001972453650000051
其中，
Figure BDA0001972453650000052
分别为k时刻和k-1时刻INS测量得到的导航坐标系(n系)下目标行人的东向和北向两个个方向的速度误差；
Figure BDA0001972453650000053
分别为k时刻和k+1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声，其协方差阵为Q；T为采样周期。
KF/FIR滤波器的观测方程为：
Figure BDA0001972453650000061
其中，
Figure BDA0001972453650000062
为k时刻惯性器件IMU解算的东向和北向位置；
Figure BDA0001972453650000063
为k时刻UWB解算的东向和北向位置；vk为观测噪声，其协方差阵为R。
KF/FIR滤波器中KF算法的迭代方程为：
Figure BDA0001972453650000064
Pk|k-1＝FkPk-1(Fk)T+Q
Kk＝Pk|k-1(Hk)T(HkPk|k-1(Hk)T+R)-1
Figure BDA0001972453650000065
Pk＝(I-KkHk)Pk|k-1
Figure BDA0001972453650000066
表示KF在k时刻预估的状态向量，
Figure BDA0001972453650000067
表示KF由k-1时刻到k时刻预估的状态向量，Fk表示表示k时刻的***矩阵，Pk|k-1表示KF由k-1时刻到k时刻的最小预测均方误差矩阵；Pk表示KF在k时刻的最小预测均方误差矩阵；Pk-1表示KF在k-1时刻的最小预测均方误差矩阵；Kk表示KF在k时刻的误差增益矩阵；I表示单位阵。
KF/FIR滤波器中KF算法的性能评估方法为：
首先计算信道质量评价指标：
Figure BDA0001972453650000068
在此基础上，判断Dk与门限door的关系，如果Dk<door，则信道质量正常，反之，则信道质量差，引入FIR滤波器。
进行KF算法性能评估的目的主要是看一下KF算法有没有发散，一旦发现其有发散的迹象，就引入FIR滤波器。
KF/FIR滤波器中FIR算法的迭代方程为：
首先，若当前采样时刻大于FIR预先设置的窗口N，定义
m＝k-N+1,s＝m+M-1
Figure BDA0001972453650000069
从第m+M时刻到k时刻，执行下面操作：
Figure BDA0001972453650000071
Figure BDA0001972453650000072
Figure BDA0001972453650000073
Figure BDA0001972453650000074
最终，
Figure BDA0001972453650000075
Figure BDA0001972453650000076
表示FIR在j时刻预估的状态向量，
Figure BDA0001972453650000077
表示FIR由j-1时刻到j时刻预估的状态向量，Fj表示表示j时刻的***矩阵，
Figure BDA0001972453650000079
表示FIR由j-1时刻到j时刻的最小预测均方误差矩阵；
Figure BDA0001972453650000078
表示FIR在j时刻的最小预测均方误差矩阵；K表示KF在j时刻的误差增益矩阵；I表示单位阵；
(4)当UWB信号不可用时，LS-SVM算法替代不能工作的KF/FIR滤波器，利用(3)中构建的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息，如图2所示。
本发明进一步公开了一种基于KF/FIR和LS-SVM融合的INS/UWB组合导航***，包括：服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用构建的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。
本发明进一步公开了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用构建的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。
上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。

Claims (6)

1.基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法，其特征在于，包括：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之误差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用INS位置与位置误差的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息；
所述KF/FIR滤波器中KF算法的性能评估方法为：计算信道质量评价指标Dk，判断Dk与门限door的关系，如果Dk＜door，则信道质量正常，反之，则信道质量差，引入FIR滤波器；
所述KF/FIR滤波器的观测方程为：
Figure FDA0002923635740000011
其中，
Figure FDA0002923635740000012
为k时刻惯性器件IMU解算的东向和北向位置；
Figure FDA0002923635740000013
为k时刻UWB解算的东向和北向位置；
Figure FDA0002923635740000014
为k时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；
Figure FDA0002923635740000015
为k时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；vk为观测噪声；
Figure FDA0002923635740000016
为k时刻INS东向位置误差的观测值、
Figure FDA0002923635740000017
为k时刻INS北向位置误差的观测值。
2.如权利要求1所述的基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法，其特征在于，所述惯性导航器件IMU固定在目标行人足部，UWB参考节点预先布置在位置已知的导航环境中，UWB目标节点固定在目标行人身上。
3.如权利要求1所述的基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法，其特征在于，惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置的初值根据实际需要自行设定。
4.如权利要求1所述的基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法，其特征在于，所述KF/FIR滤波器的状态方程为：
Figure FDA0002923635740000021
其中，
Figure FDA0002923635740000022
分别为k时刻和k-1时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；
Figure FDA0002923635740000023
分别为k时刻和k-1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声；T为采样周期。
5.基于KF/FIR和LS-SVM融合的INS/UWB组合导航***，其特征在于，包括：服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之误差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用INS位置与位置误差的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息；
所述KF/FIR滤波器中KF算法的性能评估方法为：计算信道质量评价指标Dk，判断Dk与门限door的关系，如果Dk＜door，则信道质量正常，反之，则信道质量差，引入FIR滤波器；
所述KF/FIR滤波器的观测方程为：
Figure FDA0002923635740000024
其中，
Figure FDA0002923635740000025
为k时刻惯性器件IMU解算的东向和北向位置；
Figure FDA0002923635740000026
为k时刻UWB解算的东向和北向位置；
Figure FDA0002923635740000027
为k时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；
Figure FDA0002923635740000031
为k时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；vk为观测噪声；
Figure FDA0002923635740000032
为k时刻INS东向位置误差的观测值、
Figure FDA0002923635740000033
为k时刻INS北向位置误差的观测值。
6.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时执行以下步骤：
以惯性导航器件IMU的误差向量和各UWB参考节点的东向和北向位置作为状态量，以惯性导航器件IMU测量得到的目标行人的东向和北向位置之误差作为***观测量构建INS/UWB松组合导航模型；
当UWB信号可用时，KF/FIR滤波器正常工作，为组合导航***提供导航信息的最优预估，同时，将INS解算的位置信息与KF/FIR滤波器给出的INS位置误差的最优预估分别作为LS-SVM算法的输入和培训目标，通过培训构造INS位置与位置误差的映射关系；
一旦UWB信号不可用，利用INS位置与位置误差的映射关系对INS位置误差进行预估，并对INS的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息；
所述KF/FIR滤波器中KF算法的性能评估方法为：计算信道质量评价指标Dk，判断Dk与门限door的关系，如果Dk＜door，则信道质量正常，反之，则信道质量差，引入FIR滤波器；
所述KF/FIR滤波器的观测方程为：
Figure FDA0002923635740000034
其中，
Figure FDA0002923635740000035
为k时刻惯性器件IMU解算的东向和北向位置；
Figure FDA0002923635740000036
为k时刻UWB解算的东向和北向位置；
Figure FDA0002923635740000037
为k时刻INS测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；
Figure FDA0002923635740000038
为k时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；vk为观测噪声；
Figure FDA0002923635740000039
为k时刻INS东向位置误差的观测值、
Figure FDA00029236357400000310
为k时刻INS北向位置误差的观测值。
CN201910122571.5A 2019-02-19 2019-02-19 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及*** Active CN109655060B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910122571.5A CN109655060B (zh) 2019-02-19 2019-02-19 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910122571.5A CN109655060B (zh) 2019-02-19 2019-02-19 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及***

Publications (2)

Publication Number Publication Date
CN109655060A CN109655060A (zh) 2019-04-19
CN109655060B true CN109655060B (zh) 2021-07-06

Family

ID=66123061

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910122571.5A Active CN109655060B (zh) 2019-02-19 2019-02-19 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及***

Country Status (1)

Country Link
CN (1) CN109655060B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110375730B (zh) * 2019-06-12 2021-07-27 深圳大学 基于imu和uwb融合的室内定位导航***
CN112781601A (zh) * 2021-01-18 2021-05-11 北京嘀嘀无限科技发展有限公司 导航方法和装置
CN113970331A (zh) * 2021-09-06 2022-01-25 济南大学 一种基于重构观测量的四旋翼定位方法及***

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102359787A (zh) * 2011-07-15 2012-02-22 东南大学 一种wsn/mins高精度实时组合导航信息融合方法
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航***及方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104316058B (zh) * 2014-11-04 2017-01-18 东南大学 一种采用交互多模型的移动机器人wsn/ins组合导航方法
CN104864865B (zh) * 2015-06-01 2017-09-22 济南大学 一种面向室内行人导航的ahrs/uwb无缝组合导航方法
CN105509739B (zh) * 2016-02-04 2018-04-06 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航***及方法
CN106871893A (zh) * 2017-03-03 2017-06-20 济南大学 分布式ins/uwb紧组合导航***及方法
CN107255795B (zh) * 2017-06-13 2020-01-07 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102359787A (zh) * 2011-07-15 2012-02-22 东南大学 一种wsn/mins高精度实时组合导航信息融合方法
CN106680765A (zh) * 2017-03-03 2017-05-17 济南大学 基于分布式组合滤波ins/uwb行人导航***及方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
《Indoor pedestrian tracking by combining recent INS and UWB measurements》;LIli Bu 等，;《2017 International Conference on Advanced Mechatronic Systems (ICAMechS)》;20171209;244-245页 *
《Indoor robot navigation by coupling IMU, UWB, and encode》;Siyuan， Ma 等;《2016 10th International Conference on Software, Knowledge, Information Management & Applications (SKIMA)》;20161217;全文 *
《Robust tightly-coupled INS/UWB-integrated human localization using UFIR filtering》;Lili Bu 等;《2018 IEEE International Instrumentation and Measurement Technology Conference (I2MTC)》;20180517;全文 *

Also Published As

Publication number Publication date
CN109655060A (zh) 2019-04-19

Similar Documents

Publication Publication Date Title
CN106680765B (zh) 基于分布式组合滤波ins/uwb行人导航***及方法
CN105928518B (zh) 采用伪距和位置信息的室内行人uwb/ins紧组合导航***及方法
CN109141413B (zh) 具有数据缺失uwb行人定位的efir滤波算法及***
CN107255795B (zh) 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN105509739B (zh) 采用固定区间crts平滑的ins/uwb紧组合导航***及方法
CN112268559B (zh) 复杂环境下融合slam技术的移动测量方法
CN109655060B (zh) 基于kf/fir和ls-svm融合的ins/uwb组合导航算法及***
CN110375739B (zh) 一种移动端视觉融合定位方法、***及电子设备
CN114018274B (zh) 车辆定位方法、装置及电子设备
CN110187375A (zh) 一种基于slam定位结果提高定位精度的方法及装置
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN109141412B (zh) 面向有数据缺失ins/uwb组合行人导航的ufir滤波算法及***
CN108759825B (zh) 面向有数据缺失INS/UWB行人导航的自适应预估Kalman滤波算法及***
CN111080682B (zh) 点云数据的配准方法及装置
CN112652062B (zh) 一种点云地图构建方法、装置、设备和存储介质
CN111964667B (zh) 一种基于粒子滤波算法的地磁_ins组合导航方法
CN109269498B (zh) 面向具有数据缺失uwb行人导航的自适应预估ekf滤波算法及***
CN205384029U (zh) 采用固定区间crts平滑的ins/uwb紧组合导航***
CN114440880B (zh) 基于自适应迭代ekf的施工现场控制点定位方法及***
CN113189541B (zh) 一种定位方法、装置及设备
CN111578939B (zh) 考虑采样周期随机变化的机器人紧组合导航方法及***
CN109916401B (zh) 采用ls-svm辅助ekf滤波方法的分布式无缝紧组合导航方法及***
CN103727947B (zh) 基于ukf滤波的bds与gis深耦合定位方法和***
CN109343013A (zh) 一种基于重启机制的空间配准方法和***
CN112824936B (zh) 一种地物高度确定方法、装置、电子设备和介质

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