CN112923919B - 基于图优化的行人定位方法及系统 - Google Patents

基于图优化的行人定位方法及系统 Download PDF

Info

Publication number
CN112923919B
CN112923919B CN202110082996.5A CN202110082996A CN112923919B CN 112923919 B CN112923919 B CN 112923919B CN 202110082996 A CN202110082996 A CN 202110082996A CN 112923919 B CN112923919 B CN 112923919B
Authority
CN
China
Prior art keywords
data
optimization
positioning
time
ekf
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
CN202110082996.5A
Other languages
English (en)
Other versions
CN112923919A (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.)
Hunan Yunjiangna Micro Information Technology Co ltd
Original Assignee
Hunan Yunjiangna Micro Information Technology Co ltd
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 Hunan Yunjiangna Micro Information Technology Co ltd filed Critical Hunan Yunjiangna Micro Information Technology Co ltd
Priority to CN202110082996.5A priority Critical patent/CN112923919B/zh
Publication of CN112923919A publication Critical patent/CN112923919A/zh
Application granted granted Critical
Publication of CN112923919B publication Critical patent/CN112923919B/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
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开一种基于图优化的行人定位方法及系统，该方法步骤包括：S1.实时接收对被测行人测量得到的微惯导定位数据、气压高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中；S2.实时对存入数据缓存中的数据分为两路进行融合处理，一路使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果，另一路根据数据缓存中历史数据使用图优化方法对前端EKF滤波融合进行后端循环优化修正。本发明具有实现方法简单、计算量小、实时性以及精度高且适用范围广等优点。

Description

基于图优化的行人定位方法及系统
技术领域
本发明涉及行人定位技术领域，尤其涉及一种基于图优化的行人定位方法及系统。
背景技术
随着信息化、智能化时代的到来，基于位置的服务已广泛应用于大众生产、生活中的各个方面，对行人定位导航的需求也日益迫切，而对定位的便捷性、准确性、连续性和可靠性的要求也越来越高，特别是在消防等特殊场合。全球导航卫星系统(GlobalNavigation Satellite System，缩写为GNSS，泛指GPS、GLONASS与BDS等)的定位精度高、信号获取便捷，而惯性传感器定位不受环境干扰影响，因此以卫星定位与惯性传感器定位为主，联合应用多种位置姿态感知传感器，是当前解决行人定位问题最佳选择。在空旷或遮挡不太严重的室外环境下，通过融合卫星导航和惯性传感器数据，对行人的定位已经能很好的解决，但是在林地、城市峡谷和室外与室内衔接部分等环境下，由于卫星信号受多径和非视距传播效应的影响，卫星定位精度会出现显著降低，导致卫导与惯性融合定位精度出现严重退化，而对于从室外进入室内的定位，则会明显影响室内的纯惯导位置和航向精度，如当行人走入狭巷、林地及靠近高大建筑时，由于卫导信号遮挡、多径等影响，定位稳定性、精度都会下降。因此在卫星信号受遮挡的丛林、城市峡谷、室内等卫星信号受遮挡影响的环境下如何高精度、稳定的定位，是当前行人定位导航领域最大的难题。
在卫星信号受限情况下，由于环境的多样性，以及各种定位技术固有的缺陷，单一定位手段往往实现不了定位中同时对高精度和高稳定性的要求，因此需要采用多种定位技术进行融合，以利用各类定位方式进行优劣互补，从而实现长时间、高精度及稳定的定位。现有的技术方案中，对于纯粹的室内定位解决方案，应用于对物定位的(如AGV、机器人等)，通常以视觉传感器、IMU和里程计为主要感知器，通过视觉SLAM或滤波算法对数据进行融合，或以激光雷达、IMU和里程计为主要感知器，通过激光SLAM或滤波算法对数据进行融合两种方案为主来实现定位定姿态，其它还有分别以磁条或二维码等为主的技术方案，也往往通过滤波技术融合里程计、IMU等设备共同实现定位。
而针对于人的定位(如工厂工人定位、商城顾客定位等)，则主要是以超宽带(UWB)、IMU为主要感知器，以服务器为处理平台，通过滤波算法进行数据融合实现定位，以及以手机为定位终端，蓝牙、IMU为主要感知设备，通过滤波算法进行数据融合实现定位两种方案应用最为普遍，该两种方案在成本、定位精度上各有优劣，能够在一定程度上满足不同定位精度的定位需求，但是共同的缺陷在于需要预先在环境中部署设备，无法满足及时性、快速反应的需求。对于以实现室外室内无缝定位为目标的定位解决方案，无论是对于物的定位而是对于人的定位，都是在以上技术方案中，融合人来自卫星定位的数据，在融合方法上主要以滤波方法为主。
在针对于行人的定位中，目前通常是采用基于以超宽带技术为主和以蓝牙为主的多源数据融合定位方案，该类方法需预先部署设备，不能满足及时性、快速响应的需求，且即使融合IMU、地磁等数据，也往往因为超宽带或蓝牙信号受环境影响严重，而最终导致定位结果满足不了精度和稳定性要求。在数据融合方法上，目前典型的是使用KF、EKF、UKF及粒子滤波等集中式滤波方法，或者对每一种定位数据均分别使用一个局部滤波器，每个局部滤波器中定位传感器的组合可以是集中式或级联式的，再设计一个主滤波对局部滤波器的输出进行组合，形成一个定位输出的联邦滤波方法。上述两种滤波方案的交叉融合又可以得到多种不同实现形式的滤波方式，其中基于滤波的方法由于对系统做了一阶马尔科夫假设，抛弃掉了将在复杂定位环境下可以有效的帮助进行当前状态估计的历史信息，因而计算量小、实时性高，但是不能充分利用历史信息。
如传统的EKF(扩展卡尔曼滤波)假设遵循一阶马尔可夫链，导致不能完全利用历史资料，它严重依赖于前一个时刻的状态和当前时刻的测量值，因此基于EKF的传感器融合不能充分利用已有的信息。从数学角度看，由于马尔可夫假设，EKF只在一个时间步(单次迭代)计算雅可比矩阵以实现其递归形式，因而没有保持足够的预先测量(冗余信息)来抵抗异常值，这会使得当测量值出现异常值时，会使得EKF误差增大甚至滤波发散，而这对于需要精确定位服务的应用是不可接受的，例如无人机(UAV)、自动驾驶车辆(ADV)和高精度行人定位等。然而，异常值并不是全球导航卫星系统在城市峡谷中定位的偶然情况，如进入室内则卫星系统定位完全失效，气压高度计也会显著受气流影响。
而不同于滤波方式，图优化融合方法将所有的历史信息和当前信息一起构建成一个位姿图，基于位姿图构建优化代价函数，将所有历史信息和当前信息一起优化，能够充分利用历史信息估计当前状态，但是图优化融合方法的计算量大、实时性低。因此亟需提供一种针对行人的定位方法，以实现快速、高效的行人定位，使得能够兼顾行人定位的计算复杂度、定位效率以及定位精度。
发明内容
本发明要解决的技术问题就在于：针对现有技术存在的技术问题，本发明提供一种实现方法简单、能够充分利用历史信息，使得可以适用于卫星信号受限的场景中，同时能够兼顾计算量、实时性以及定位精度的基于图优化的行人定位方法及系统。
为解决上述技术问题，本发明提出的技术方案为：
一种基于图优化的行人定位方法，步骤包括：
S1.数据缓存：实时接收对被测行人测量得到的微惯导定位数据、气压计高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
S2.图优化-EKF融合滤波：实时对存入所述数据缓存中的数据分为两路进行融合处理，一路使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果，另一路根据所述数据缓存中历史数据对所述前端EKF滤波融合使用图优化方法进行后端循环优化修正。
进一步的，所述步骤S2中使用图优化方法进行后端循环优化修正包括：每次优化时使用所述数据缓存中当前时刻前指定时长的历史数据构建优化问题，并对构建的所述优化问题进行求解，得到优化结果；使用所述优化结果作为初始值构建EKF重置滤波器，使用构建的所述EKF重置滤波器对所述数据缓存中最近一次优化开始时刻到当前时刻的数据进行滤波融合处理；处理完成后使用所述EKF重置滤波器更新所述EKF滤波器以进行前端EKF滤波。
进一步的，所述步骤S2中使用图优化方法时还包括构建用于实现全局优化修正的全局优化器步骤，所述全局优化器包括用于代表待优化变量的图节点，以及用于表示运动模型约束的模型约束边、用于表示微惯导测量数据约束的微惯导量测约束边、用于表示GNSS测量数据约束的GNSS量测约束边和用于表示气压高度计量测约束的气压高度约束边，其中每两个相邻图节点之间都具有模型约束边，若k时刻接收到GNSS测量数据，则第k个图节点具有所述GNSS量测约束边；若第l时刻接收到气压高度数据，第l-τ时刻接收到上一帧气压高度数据，则第l个图节点与第l-τ个图节点之间具有气压高度约束边；若第m时刻接收到第m-τ时刻到m时刻微惯导设备测量的水平步长测量值和高度变化值，则第m-τ图节点与第m图节点之间具有微惯导量测约束边。
进一步的，每次优化时具体将当前次优化开始执行时及前面指定个时刻的待估计位姿向量作为图的顶点，将所有通过所述微惯导定位数据、GNSS定位数据、气压高度计测量得到的对各个时刻位姿的观测、以及根据运动模型的预测值，与对应时刻的位姿做差得到的误差项作为图的边。
进一步的，所述构建优化问题的具体步骤包括：
对于待优化变量集合{X0 X1 X2 … Xn}，其中
Figure BDA0002909770610000031
xn、yn、zn表示n时刻三轴位置，θn表示n时刻水平方向角，
Figure BDA0002909770610000041
分别表示n时刻水平步长变化率、高度变化率，ωn表示n时刻水平方向角变化率偏置，构建所述运动模型约束为：
Figure BDA0002909770610000042
其中，Δtn,n-1表示n-1时刻到n时刻的时间增量，sdθins为从n-1时刻到n时刻微惯导设备测量得到的方向角增量，
Figure BDA0002909770610000043
为运动模型高斯噪声协方差矩阵；
并转换为优化问题的运动模型误差项为：
Figure BDA0002909770610000044
构建所述微惯导测量数据约束为：
Figure BDA0002909770610000045
其中，m-τ表示上一个具有微惯导测量水平步长和高度变化值数据的时刻，Δtm,m-τ表示m-τ时刻到m时刻的时间增量，slins、shins分别为微惯导设备测量得到的m-τ时刻到m时刻的水平运动距离、高度变化，
Figure BDA0002909770610000046
为高斯噪声协方差矩阵：
并转换为优化问题的微惯导量测误差项为：
Figure BDA0002909770610000047
构建所述气压计高度约束为：
Figure BDA0002909770610000048
其中，l-τ表示上一个具有气压高度计测量数据的时刻，考虑观测噪声后转换为：
Figure BDA0002909770610000049
其中，
Figure BDA00029097706100000410
为高斯噪声协方差矩阵；
并转换为优化问题的气压高度计量测误差项为：
Figure BDA00029097706100000411
构建所述GNSS量测数据约束为：
Figure BDA0002909770610000051
考虑观测噪声后具体化为：
Figure BDA0002909770610000052
其中
Figure BDA0002909770610000053
分别为经纬高转换到本地坐标系下的坐标值，
Figure BDA0002909770610000054
为高斯噪声协方差矩阵；
并转换为优化问题的GNSS量测误差项为：
Figure BDA0002909770610000055
最终构建得到用于图优化全局修正的优化模型目标函数为：
Figure BDA0002909770610000056
其中，w为优化时间窗口长度，X*为待优化变量X＝[X0,X1,X2,...,Xn]优化后的值。
进一步的，所述步骤S2中进行前端EKF滤波融合时，使用被测行人的位置坐标参数、方位参数作为滤波状态量，使用所述微惯导定位数据中位移变化量、角度变化量进行状态预测，使用所述GNSS定位数据作为观测量。
一种基于图优化的行人定位方法，步骤包括：
S1.数据缓存：实时接收对被测行人测量得到的微惯导定位数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
S2.因子图优化滤波：实时对存入所述数据缓存中的数据使用因子图优化方法进行滤波融合，每次优化时从所述数据缓存中选取部分历史数据构建优化问题，并通过因子图方法对构建的所述优化问题进行求解，优化结束后将优化结果从本地坐标系转回地理坐标系下，得到最终定位结果。
一种基于图优化的行人定位系统，包括：
数据缓存模块，用于实时接收对被测行人测量得到的微惯导定位数据、气压计高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
EKF滤波器模块，用于实时对存入所述数据缓存中的数据，使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果；
图优化全局修正模块，用于实时根据所述数据缓存模块中历史数据使用图优化方式对所述EKF滤波器模块中EKF滤波融合结果进行后端循环优化修正。
进一步的，所述图优化全局修正模块每次优化时使用所述数据缓存中当前时刻前指定时长的历史数据构建优化问题，并对构建的所述优化问题进行求解，得到优化结果；使用所述优化结果作为初始值构建EKF重置滤波器，使用构建的所述EKF重置滤波器对所述数据缓存中最近一次优化开始时刻到当前时刻的数据进行滤波融合处理；处理完成后使用所述EKF重置滤波器更新所述EKF滤波器模块中EKF滤波器。
一种基于图优化的行人定位系统，包括：
数据缓存模块，用于实时接收对被测行人测量得到的微惯导定位数据、气压计高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
因子图优化滤波模块，用于实时对存入所述数据缓存中的数据使用因子图优化方法进行滤波融合，每次优化时从所述数据缓存中选取部分历史数据构建优化问题，并通过因子图方法对构建的所述优化问题进行求解，优化结束后将优化结果从本地坐标系转回地理坐标系下，得到最终定位结果。
与现有技术相比，本发明的优点在于：
1、本发明进行数据融合时，采用EKF滤波与图优化的融合方式，实时对数据缓存中数据进行前端EKF滤波，保证定位数据的实时性，同时后端使用图优化进行循环优化修正，利用有效的历史信息对前端位置数据进行低频修正，充分利用历史信息估计当前状态，降低测量值异常对融合定位结果的影响，提高行人定位精度和稳定性，能够显著提高林地、城市峡谷和室外与室内衔接部分等卫星信号受限环境下的融合定位精度，实现室外室内各种环境下行人高精度、稳定的定位。
2、本发明通过结合图优化方法实现对EKF滤波方法的优化修正，不仅能够利用图优化充分利用历史数据，使得可适用于卫星信号受限环境等各类环境中，且相比于传统直接使用图优化方法以及直接使用EKF滤波方法进行数据融合，还能够大大减少滤波计算量，提高定位实时性，从而实现与EKF滤波方法的优劣互补。
3、本发明前端利用EKF融合卫星导航数据和微惯导定位数据时，进一步由位置和航向角作为状态量，在预测过程中使用微惯导定位数据中的位移和角度变化量进行预测，GNSS定位结果为观测量，可提高复杂室外环境下的定位精度与稳定性，使得定位轨迹持续、平滑，并能够保证定位的实时性。
4、本发明后端循环执行图优化融合时，进一步通过优化结果修正前端的滤波估计位姿，每一次优化中通过求解优化问题，实现对前端滤波估计结果的全局修正，由该全局优化充分利用历史信息估计当前状态，能够降低测量值异常对融合定位结果的影响，进一步提高行人定位精度和稳定性。
5、本发明进一步综合利用卫星导航、微惯导、气压高度定位数据，实现各定位方法优劣互补，可实现复杂室内外环境中人员长时间持续的高精度实时定位，并且室内不需预先部署设备，使用方便，还可对定位需求实现快速响应。
附图说明
图1是本发明实施例1中基于图优化的行人定位方法实现流程示意图。
图2是本发明实施例1中使用的全局优化器的图结构示意图。
图3是本发明实施例2中基于图优化的行人定位方法实现流程示意图。
具体实施方式
以下结合说明书附图和具体优选的实施例对本发明作进一步描述，但并不因此而限制本发明的保护范围。
实施例1：
如图1所示，本实施例基于图优化的行人定位方法的步骤包括：
S1.数据缓存：实时接收对被测行人测量得到的微惯导定位数据、气压计高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
S2.图优化-EKF融合滤波：实时对存入数据缓存中的数据分为两路进行融合处理，一路使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果，另一路根据数据缓存中历史数据对前端EKF滤波融合使用图优化方法进行后端循环优化修正。
本实施例具体预先在被测行人配置卫星定位终端、气压高度计和微惯导设备(如加速度计、陀螺仪等微机电惯性导航设备)，由卫星定位终端实时获取GNSS定位数据，由气压高度计实时获取气压高度数据，微惯导设备可以为通过检测足部静止来修正积分误差的足部微机电惯性定位设备，也可以为携带于身体腰部等其它部位的微机电惯性定位设备，由该微惯导设备充分利用人员运动特征修正积分误差，如由微机电陀螺、微机电加速度计、磁力计等传感器组合，运行惯导解算算法(INS)或行人航迹推算法(PDR)，能够给出行人运动方向和步长的惯性定位设备。获取到微惯导定位数据、气压计高度数据以及GNSS定位数据后，将数据按照接收顺序进行时间同步后存入数据缓存中以进行数据融合。
本实施例进行数据融合时，采用EKF滤波与图优化的融合方式，实时对数据缓存中数据进行前端EKF滤波，保证定位数据的实时性，同时后端使用图优化进行循环优化修正，利用有效的历史信息对前端位置数据进行低频修正，充分利用历史信息估计当前状态，降低测量值异常对融合定位结果的影响，提高行人定位精度和稳定性，能够显著提高林地、城市峡谷和室外与室内衔接部分等卫星信号受限环境下的融合定位精度，不需预先在场景中部署设备，即可实现室外室内各种环境下行人高精度、稳定的定位，且可满足对定位快速响应的要求。
本实施例通过结合图优化方法实现对EKF滤波方法的优化修正，其前端进行实时滤波定位，后端利用历史数据进行优化计算修正前端结果，不仅能够利用图优化充分利用历史数据，使得可适用于卫星信号受限环境等各类环境中，且相比于传统直接使用图优化方法以及直接使用EKF滤波方法进行数据融合，还能够在不显著降低精度的情况下大大减少滤波计算量，有效节约计算资源，从而实现与EKF滤波方法的优劣互补。
本实施例步骤S1中获取GNSS定位数据前还包括：根据卫星导航模块输出的信息对GNSS定位数据进行筛选，将筛选后的定位数据转换至本地坐标系，得到所需的GNSS定位数据存入数据缓存中。
在具体应用实施例中，分别接收来自微惯导定位设备测量得到的本地坐标系下的三轴定位数据、卫星定位模块的地理坐标系下的经纬高数据，同时还接收来自气压高度计的气压高度测量值，并根据卫导定位模块给出的卫星数、dop值等对GNSS定位数据进行筛选，将筛选后的经纬高转化到本地坐标系下，然后根据各数据的到达时间，对数据进行时间同步后存入数据缓存。
本实施例步骤S2中进行前端EKF滤波融合时，具体根据行人的运动特征，使用被测行人的位置坐标参数、方位参数作为滤波状态量，使用微惯导定位数据中位移变化量、角度变化量进行状态预测，使用GNSS定位数据作为观测量，以使得能够适用于行人及类似的低速运动定位场景中。
本实施例中，EKF滤波器的系统模型具体为：
Figure BDA0002909770610000081
当使用GNSS数据作为量测时，量测模型为：
Figure BDA0002909770610000091
当使用气压高度计数据作为量测时，测量模型为：
Figure BDA0002909770610000092
即由三轴位置xn、yn、zn、方向角θn和方向角的偏置量ωn作为滤波状态量，sl、sh、sdθ分别为微惯导提供的水平位移、垂直位移和角度变化量。
上式中，
Figure BDA0002909770610000093
分别为GNSS定位数据转换为本地坐标系的坐标值；δx、δy、δz、δθ、δω别为对应状态传递过程中的零均值高斯系统噪声，具体与微惯导设备对步长和角增量的测量精度相关；
Figure BDA0002909770610000094
为GNSS量测噪声，
Figure BDA0002909770610000095
为气压高度计相关的量测噪声。
由于系统模型为非线性模型，则采用EKF线性化后的系统模型的矩阵形式可以表示为：
Xn＝An/n-1Xn-1+Wn (3)
Figure BDA0002909770610000096
Figure BDA0002909770610000097
Figure BDA0002909770610000098
其中Wn为系统噪声矩阵。
对于GNSS量测模型的矩阵形式表示为：
Figure BDA0002909770610000101
式中，
Figure BDA0002909770610000102
对于气压高度计量测模型的矩阵表示为：
Figure BDA0002909770610000103
式中，
Figure BDA0002909770610000104
其中，
Figure BDA0002909770610000105
为GNSS观测噪声矩阵，该观测噪声为零均值高斯噪声，与GNSS定位数据的精度相关，其协方差矩阵由下(23)式确定。状态量的初始方差可根据最近一次图优化结果中与之对应的优化变量残差确定。
本实施例前端利用EKF融合卫星导航数据和微惯导定位数据，由位置和航向角作为状态量，在预测过程中使用微惯导定位数据中的位移和角度变化量进行预测，GNSS定位结果为观测量，可提高复杂室外环境下的定位精度与稳定性，使得定位轨迹持续、平滑，并能够保证定位的实时性。
本实施例步骤S2中使用图优化方法进行后端循环优化修正包括：每次优化时使用数据缓存中当前时刻前指定时长的历史数据构建优化问题，并对构建的优化问题进行求解，得到优化结果；使用优化结果作为初始值构建EKF重置滤波器，使用构建的EKF重置滤波器对数据缓存中最近一次优化开始时刻到当前时刻的数据进行滤波融合处理；处理完成后使用EKF重置滤波器更新EKF滤波器以进行前端EKF滤波。
图优化即为将一个常规的优化问题以图(Graph)的形式来表述。图是由顶点(Vertex)和边(Edge)组成的结构，记一个图为G＝{V,E}，其中V为顶点集，E为边集。每一条边连接着若干个顶点，表示顶点之间的一种关系。边可以是有向的或是无向的，对应的图称为有向图或无向图。边也可以连接一个顶点(Unary Edge，一元边)、两个顶点(BinaryEdge，二元边)或多个顶点(Hyper Edge，多元边)。
本实施例步骤S2中使用图优化方法时还包括构建用于实现全局优化修正的全局优化器步骤，本实施例具体使用如图2所示的图结构实现全局优化修正，具体包括用于代表待优化变量的图节点，以及用于表示运动模型约束的模型约束边、用于表示微惯导测量数据约束的微惯导量测约束边、用于表示GNSS测量数据约束的GNSS量测约束边和用于表示气压高度计量约束的气压高度约束边，其中每两个图节点之间都具有模型约束边，若k时刻接收到GNSS测量数据，则第k个图节点具有GNSS量测约束边；若第l时刻接收到气压高度数据，第l-τ时刻接收到上一帧气压高度数据，则第l个图节点与第l-τ个图节点之间具有气压高度约束边；若第m时刻接收到第m-τ时刻到m时刻微惯导设备测量的水平步长测量值和高度变化值，则第m-τ图节点与第m图节点之间具有微惯导量测约束边。图结构中每一个节点都有模型约束边与之相连，且至少有一种其它约束边与之相连，同时每两个相邻节点之间的时间差是非固定，随着量测数据到达的时间间隔变化而变化。
本实施例中，每次优化时具体将当前次优化开始执行时及前面指定个时刻的待估计位姿向量作为图的顶点，将所有通过微惯导定位数据、GNSS定位数据、气压高度计测量得到的对各个时刻位姿的观测、以及根据运动模型得到的预测值，与对应时刻的位姿做差得到的误差项作为图的边。在后端循环执行图优化融合过程中，通过优化结果修正前端的滤波估计位姿，如上述，每一次优化中，将该次优化开始执行时及其前面的各个时刻的待估计位姿向量作为图的顶点，将所有通过惯导、卫导定位、气压高度计测量得到对各个时刻位姿的观测、以及根据运动模型得到的预测值，与对应时刻的位姿做差得到得误差项作为图的边后，求解优化问题，实现对前端滤波估计结果的全局修正，通过该全局优化充分利用历史信息估计当前状态，降低测量值异常对融合定位结果的影响，进一步提高行人定位精度和稳定性。
本实施例中，全局优化器构建优化问题的具体步骤包括：
对于待优化变量集合{X0 X1 X2 … Xn}，其中
Figure BDA0002909770610000111
xn、yn、zn表示n时刻三轴位置，θn表示n时刻水平方向角，
Figure BDA0002909770610000112
分别表示n时刻水平步长变化率、高度变化率，ωn表示n时刻水平方向角变化率偏置，构建运动模型约束为：
Figure BDA0002909770610000113
其中，Δtn,n-1表示n-1时刻到n时刻的时间增量，sdθins为从n-1时刻到n时刻微惯导设备测量得到的方向角增量，
Figure BDA0002909770610000121
为运动模型高斯噪声协方差矩阵如下：
Figure BDA0002909770610000122
其中，σ1,TF～σ3,TF为行人运动加减速引入的误差及微惯导方向角测量误差的标准差，σ5,TF,σ6,TF是行人运动状态变化引起误差标准差，σ4,TF是微惯导方向角测量误差的标准差，σ7,TF为微惯导陀螺偏置误差标准在方向角上的反映量。
并转换为优化问题的运动模型误差项为：
Figure BDA0002909770610000123
构建微惯导测量数据约束为：
Figure BDA0002909770610000124
其中，m-τ表示上一个具有微惯导量测水平步长和高度变化值数据的时刻，Δtm,m-τ表示m-τ时刻到m时刻的时间增量，slins、shins分别为微惯导设备测量得到的m-τ时刻到m时刻的水平运动距离、高度变化，m为大于零的整数，
Figure BDA0002909770610000125
为高斯噪声协方差矩阵：
Figure BDA0002909770610000126
其中σins,x、σins,y、σins,z为常量，具体与微惯导误差特性相关。
并转换为优化问题的微惯导量测误差项为：
Figure BDA0002909770610000127
构建气压计高度约束为：
Figure BDA0002909770610000128
其中，l-τ表示上一个具有气压高度测量的时刻，考虑观测噪声后具体化为：
Figure BDA0002909770610000129
其中，
Figure BDA00029097706100001210
为高斯噪声协方差矩阵：
Figure BDA0002909770610000131
并转换为优化问题的气压高度计量测误差项为：
Figure BDA0002909770610000132
构建GNSS量测数据约束为：
Figure BDA0002909770610000133
考虑观测噪声后具体化为：
Figure BDA0002909770610000134
其中
Figure BDA0002909770610000135
分别为经纬高转换到本地坐标系下的坐标值，
Figure BDA0002909770610000136
为高斯噪声协方差矩阵：
Figure BDA0002909770610000137
其中hdopk为GNSS定位器给出的位置水平精度因子，γ为与GNSS定位解类型有关的正系数，根据定位结果属于单点、伪距差分、浮点和固定解依次减小，η与η1为与GNSS定位器性能有关的正系数，由于GNSS定位结果高度上定位误差往往比水平误差更大，所有η1设为比η更大的值。
并转换为优化问题的运动模型误差项为：
Figure BDA0002909770610000138
根据式(13)、(16)、(20)、(24)，在图优化全局修正中，最终构建得到用于图优化全局修正的优化模型目标函数为：
Figure BDA0002909770610000139
其中，w为优化时间窗口长度，X*为待优化变量X＝[X0,X1,X2,...,Xn]优化后的值。
对上述图优化问题进行求解上主要包含求解稀疏线性方程和LM(列文伯格-马夸尔特方法)迭代(或高斯迭代等)两个过程，可直接选用ceres、g2o、gtsam等开源优化库进行求解。
本实施例通过依据行人运行特征，以位置坐标(x、y、z)、方向角θ、水平步长变化率vsl、高度变化率vsh和方向角等效零偏ω为待估状态变量，以GNSS定位数据、微惯导定位数据中得到的方向变化量、水平位移增量和垂直位移增量、以及气压计测量获得的高度增量为观测量，按照上述方法构建用于滤波和图优化的数学模型，仅需使用较少的待估变量，即可构建得到可以满足行人等低速运动定位的滤波模型。
如图1所示，本实施例实现行人定位时，首先分别接收来自微惯导定位模块的本地坐标系下的三轴定位数据、卫星定位模块的地理坐标系下的经纬高数据和气压高度计的气压高度测量值，并根据卫导定位模块给出的卫星数、dop值等对GNSS定位数据进行筛选，将筛选后的经纬高转化到本地坐标系下；然后根据各数据的到达时间，对数据进行时间同步后存入数据缓存，然后分两路对存入缓存的数据进行融合处理：一路为前端实时EKF滤波融合，通过一个EKF滤波器对存入缓存的最新数据进行滤波，融合得实时定位结果，再将该结果从本地坐标系转回地理坐标系下，得到最终定位结果；另一路进行图优化全局修正融合，每次优化选择缓存中从当前时刻开始往后固定时长内的历史数据构建优化问题，并对优化问题进行求解，优化结束后使用优化结果作为初始值构建EKF重置滤波器，对缓存中最近一次优化开始时刻到当前时刻的数据进行滤波处理，处理完成使用EKF重置滤波器替代前端实时EKF滤波器进行实时滤波融合，之后循环进行下一次优化。
本实施例综合利用卫星导航、微惯导、气压高度定位数据，结合图优化与EKF滤波融合的处理方法，实现了各定位方法优劣互补，可实现复杂室内外环境中人员长时间持续的高精度实时定位，并且室内不需预先部署设备，使用方便，且可对定位需求的快速响应。
为实现上述方法，本实施例还提供基于图优化的行人定位系统包括：
数据缓存模块，用于实时接收对被测行人测量得到的微惯导定位数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
EKF滤波器模块，用于实时对存入数据缓存中的数据，使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果；
图优化全局修正模块，用于实时根据数据缓存模块中历史数据使用图优化方式对EKF滤波器模块中EKF滤波进行后端循环优化修正。
本实施例中，图优化全局修正模块每次优化时使用数据缓存中当前时刻前指定时长的历史数据构建优化问题，并对构建的优化问题进行求解，得到优化结果；使用优化结果作为初始值构建EKF重置滤波器，使用构建的EKF重置滤波器对数据缓存中最近一次优化开始时刻到当前时刻的数据进行滤波融合处理；处理完成后使用EKF重置滤波器更新EKF滤波器模块中EKF滤波器。
本实施例中，图优化全局修正模块包括用于实现全局优化修正的全局优化器，全局优化器包括用于代表待优化变量的图节点，以及用于表示运动模型约束的模型约束边、用于表示微惯导测量数据约束的微惯导量测约束边、用于表示GNSS测量数据约束的GNSS量测约束边和用于表示气压高度计量约束的气压高度约束边，其中每两个图节点之间都具有模型约束边，若k时刻接收到GNSS测量数据，则第k个图节点具有GNSS量测约束边；若第l时刻接收到气压高度数据，第l-τ时刻接收到上一帧气压高度数据，则第l个图节点与第l-τ个图节点之间具有气压高度约束边；若第m时刻接收到第m-τ时刻到m时刻微惯导设备测量的水平步长测量值和高度变化值，则第m-τ图节点与第m图节点之间具有微惯导量测约束边。
本实施例中各模块具体与上述基于图优化的行人定位方法中步骤对应，其中数据缓存模块与数据缓存步骤对应，EKF滤波器模块与EKF滤波步骤对应，图优化全局修正模块与图优化全局修正步骤对应，在此不再一一赘述。
实施例2：
本实施例采用与实施例1相似的原理，基于图优化实现行人定位，如图3所示，步骤包括：
S1.数据缓存：实时接收对被测行人测量得到的微惯导定位数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
S2.因子图优化滤波：实时对存入所述数据缓存中的数据使用因子图优化方法进行滤波融合，每次优化时从所述数据缓存中选取部分历史数据构建优化问题，并通过因子图方法对构建的所述优化问题进行求解，优化结束后将优化结果从本地坐标系转回地理坐标系下，得到最终定位结果。
本实施例与实施例1的原理类似，不同之处在于本实施例步骤S2中是单独使用全局图优化修正实现行人实时定位，全局图优化修正的原理、图结构与实施例1中基本相同，区别仅在于选择因子图优化方法求解图优化问题，可以适用于计算资源充足的情况下，以进一步减少实现复杂度。
本实施例步骤S2的详细步骤为：首先分别接收来自微惯导定位模块的本地坐标系下的三轴定位数据、卫星定位模块的地理坐标系下的经纬高数据和气压高度计的气压高度测量值，并根据卫导定位模块给出的卫星数、dop值等对GNSS定位数据进行筛选，将筛选后的经纬高转化到本地坐标系下，然后根据各数据的到达时间，对数据进行时间同步后存入数据缓存，然后每次优化选择缓存中从当前时刻开始往后固定窗口时长内的历史数据构建优化问题，并通过因子图方法对优化问题进行求解；优化结束后，将该结果从本地坐标系转回地理坐标系下，得到最终定位结果。
相应的，本实施例基于图优化的行人定位系统包括：
数据缓存模块，用于实时接收对被测行人测量得到的微惯导定位数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
因子图优化滤波模块，用于实时对存入所述数据缓存中的数据使用因子图优化方法进行滤波融合，每次优化时从所述数据缓存中选取部分历史数据构建优化问题，并通过因子图方法对构建的所述优化问题进行求解，优化结束后将优化结果从本地坐标系转回地理坐标系下，得到最终定位结果。
本实施例基于图优化的行人定位系统与上述基于图优化的行人定位方法对应。
上述只是本发明的较佳实施例，并非对本发明作任何形式上的限制。虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明。因此，凡是未脱离本发明技术方案的内容，依据本发明技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均应落在本发明技术方案保护的范围内。

Claims (6)

1.一种基于图优化的行人定位方法，其特征在于，步骤包括：
S1.数据缓存：实时接收对被测行人测量得到的微惯导定位数据、气压高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
S2.图优化-EKF融合滤波：实时对存入所述数据缓存中的数据分为两路进行融合处理，一路使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果，另一路根据所述数据缓存中历史数据对所述前端EKF滤波融合使用图优化方法进行后端循环优化修正；
所述步骤S2中使用图优化方法进行后端循环优化修正包括：每次优化时使用所述数据缓存中当前时刻前指定时长的历史数据构建优化问题，并对构建的所述优化问题进行求解，得到优化结果；使用所述优化结果作为初始值构建EKF重置滤波器，使用构建的所述EKF重置滤波器对所述数据缓存中最近一次优化开始时刻到当前时刻的数据进行滤波融合处理；处理完成后使用所述EKF重置滤波器更新所述EKF滤波器以进行前端EKF滤波融合。
2.根据权利要求1所述的基于图优化的行人定位方法，其特征在于：所述步骤S2中使用图优化方法时还包括构建用于实现全局优化修正的全局优化器步骤，所述全局优化器包括用于代表待优化变量的图节点，以及用于表示运动模型约束的模型约束边、用于表示微惯导测量数据约束的微惯导量测约束边、用于表示GNSS测量数据约束的GNSS量测约束边和用于表示气压高度计量测约束的气压高度约束边，其中每两个相邻图节点之间都具有所述模型约束边，若k时刻接收到GNSS测量数据，则第k个图节点具有所述GNSS量测约束边；若第l时刻接收到气压高度数据，第l-τ时刻接收到上一帧气压高度数据，则第l个图节点与第l-τ个图节点之间具有所述气压高度约束边；若第m时刻接收到第m-τ时刻到m时刻微惯导设备测量的水平步长测量值和高度变化值，则第m-τ图节点与第m图节点之间具有所述微惯导量测约束边。
3.根据权利要求2所述的基于图优化的行人定位方法，其特征在于：每次优化时具体将当前次优化开始执行时及前面指定个时刻的待估计位姿向量作为图的顶点，将所有通过所述微惯导定位数据、GNSS定位数据、气压高度计测量得到的对各个时刻位姿的观测、以及根据运动模型得到的预测值，与对应时刻的位姿做差得到的误差项作为图的边。
4.根据权利要求3所述的基于图优化的行人定位方法，其特征在于：所述构建优化问题的具体步骤包括：
对于待优化变量集合{X0 X1 X2…Xn}，其中
Figure FDA0004159872250000011
xn、yn、zn表示n时刻三轴位置，θn表示n时刻水平方向角，
Figure FDA0004159872250000021
分别表示n时刻水平步长变化率、高度变化率，ωn表示n时刻水平方向角变化率偏置，构建所述运动模型约束为：
Figure FDA0004159872250000022
其中，Δtn,n-1表示n-1时刻到n时刻的时间增量，sdθins为从n-1时刻到n时刻微惯导设备测量得到的方向角增量，
Figure FDA0004159872250000023
为运动模型高斯噪声协方差矩阵；
并转换为优化问题的运动模型误差项为：
Figure FDA0004159872250000024
构建所述微惯导测量数据约束为：
Figure FDA0004159872250000025
其中，m-τ表示上一个具有微惯导测量水平步长和高度变化值数据的时刻，Δtm,m-τ表示m-τ时刻到m时刻的时间增量，slins、shins分别为微惯导设备测量得到的m-τ时刻到m时刻的水平运动距离、高度变化，
Figure FDA0004159872250000026
为高斯噪声协方差矩阵：
并转换为优化问题的微惯导量测误差项为：
Figure FDA0004159872250000027
构建所述气压高度计量测约束为：
Figure FDA0004159872250000028
其中，l-τ表示上一个具有气压高度计测量数据的时刻，考虑观测噪声后具体化为：
Figure FDA0004159872250000029
其中，
Figure FDA00041598722500000210
为高斯噪声协方差矩阵；
并转换为优化问题的气压高度计量测误差项为：
Figure FDA00041598722500000211
构建所述GNSS测量数据约束为：
Figure FDA0004159872250000031
考虑观测噪声后具体化为：
Figure FDA0004159872250000032
其中
Figure FDA0004159872250000036
分别为经纬高转换到本地坐标系下的坐标值，
Figure FDA0004159872250000033
为高斯噪声协方差矩阵；
并转换为优化问题的GNSS量测误差项为：
Figure FDA0004159872250000034
最终构建得到用于图优化全局修正的优化模型目标函数为：
Figure FDA0004159872250000035
其中，w为优化时间窗口长度，X*为待优化变量X＝[X0,X1,X2,...,Xn]优化后的值。
5.根据权利要求1～4中任意一项所述的基于图优化的行人定位方法，其特征在于，所述步骤S2中进行前端EKF滤波融合时，使用被测行人的位置坐标参数、方位参数作为滤波状态量，使用所述微惯导定位数据中位移变化量、角度变化量进行状态预测，使用所述GNSS定位数据作为观测量。
6.一种基于图优化的行人定位系统，其特征在于，包括：
数据缓存模块，用于实时接收对被测行人测量得到的微惯导定位数据、气压高度数据以及GNSS定位数据，将接收到的各数据进行时间同步后存入数据缓存中，所述微惯导定位数据为由被测行人佩戴的微惯导设备测量得到的定位数据；
EKF滤波器模块，用于实时对存入所述数据缓存中的数据，使用EKF滤波器进行前端EKF滤波融合，得到实时定位结果；
图优化全局修正模块，用于实时根据所述数据缓存模块中历史数据使用图优化方式对所述EKF滤波器模块中EKF滤波器进行后端循环优化修正；所述图优化全局修正模块每次优化时使用所述数据缓存中当前时刻前指定时长的历史数据构建优化问题，并对构建的所述优化问题进行求解，得到优化结果；使用所述优化结果作为初始值构建EKF重置滤波器，使用构建的所述EKF重置滤波器对所述数据缓存中最近一次优化开始时刻到当前时刻的数据进行滤波融合处理；处理完成后使用所述EKF重置滤波器更新所述EKF滤波器模块中EKF滤波器。
CN202110082996.5A 2021-01-21 2021-01-21 基于图优化的行人定位方法及系统 Active CN112923919B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110082996.5A CN112923919B (zh) 2021-01-21 2021-01-21 基于图优化的行人定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110082996.5A CN112923919B (zh) 2021-01-21 2021-01-21 基于图优化的行人定位方法及系统

Publications (2)

Publication Number Publication Date
CN112923919A CN112923919A (zh) 2021-06-08
CN112923919B true CN112923919B (zh) 2023-05-16

Family

ID=76164177

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110082996.5A Active CN112923919B (zh) 2021-01-21 2021-01-21 基于图优化的行人定位方法及系统

Country Status (1)

Country Link
CN (1) CN112923919B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114222240A (zh) * 2021-10-29 2022-03-22 中国石油大学(华东) 一种基于粒子滤波的多源融合定位方法
CN114440895B (zh) * 2022-03-04 2023-11-24 杭州电子科技大学 基于因子图的气压辅助Wi-Fi/PDR室内定位方法
CN114915913A (zh) * 2022-05-17 2022-08-16 东南大学 一种基于滑窗因子图的uwb-imu组合室内定位方法
CN115711616B (zh) * 2022-06-09 2024-08-30 同济大学 一种室内室外穿越无人机的平顺定位方法及装置
CN115507849B (zh) * 2022-11-22 2023-03-10 北京理工大学前沿技术研究院 一种基于ins/gnss组合导航辅助的磁传感器校正方法和系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303710A (zh) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维激光雷达的无人机多场景定位建图方法
CN109631909A (zh) * 2019-02-01 2019-04-16 湖南格纳微信息科技有限公司 用于室内定位的基于重过点的微惯导轨迹修正方法
CN109855620A (zh) * 2018-12-26 2019-06-07 北京壹氢科技有限公司 一种基于自回溯算法的室内行人导航方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11536572B2 (en) * 2016-11-09 2022-12-27 The Texas A&M University System Method and system for accurate long term simultaneous localization and mapping with absolute orientation sensing
US10837773B2 (en) * 2016-12-30 2020-11-17 DeepMap Inc. Detection of vertical structures based on LiDAR scanner data for high-definition maps for autonomous vehicles
CN109813319B (zh) * 2019-03-07 2021-09-28 山东大学 一种基于slam建图的开环优化方法及系统
CN110118554B (zh) * 2019-05-16 2021-07-16 达闼机器人有限公司 基于视觉惯性的slam方法、装置、存储介质和设备
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108303710A (zh) * 2018-06-12 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维激光雷达的无人机多场景定位建图方法
CN109855620A (zh) * 2018-12-26 2019-06-07 北京壹氢科技有限公司 一种基于自回溯算法的室内行人导航方法
CN109631909A (zh) * 2019-02-01 2019-04-16 湖南格纳微信息科技有限公司 用于室内定位的基于重过点的微惯导轨迹修正方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于前后端图优化的RGB-D三维SLAM;邢科新;竺海光;林叶贵;张雪波;;浙江工业大学学报;第46卷(第06期);第616-621页 *

Also Published As

Publication number Publication date
CN112923919A (zh) 2021-06-08

Similar Documents

Publication Publication Date Title
CN112923919B (zh) 基于图优化的行人定位方法及系统
KR102463176B1 (ko) 위치 추정 장치 및 방법
US8315794B1 (en) Method and system for GPS-denied navigation of unmanned aerial vehicles
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN110706279A (zh) 基于全局地图与多传感器信息融合的全程位姿估计方法
CN111983660A (zh) Gnss拒止环境下四旋翼无人机定位系统和方法
US11681303B2 (en) Methods and systems for estimating the orientation of an object
CN116086448B (zh) 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法
CN116182873B (zh) 室内定位方法、系统及计算机可读介质
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
KR101141984B1 (ko) Ｄｒ／ｇｐｓ 데이터 융합 방법
Kong et al. An accurate and reliable positioning methodology for land vehicles in tunnels based on UWB/INS integration
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Afia et al. A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments
CN111089596A (zh) 一种基于激光视觉信号与惯性导航融合的自主定位系统及其方法
CN117320148A (zh) 多源数据融合定位方法、系统、电子设备及存储介质
Jung et al. Scalable and modular ultra-wideband aided inertial navigation
CN116907484A (zh) 一种基于自主导航与ZigBee约束的应急救援人员协同定位方法
Suzuki Attitude-Estimation-Free GNSS and IMU Integration
Elbes et al. Gyroscope drift correction based on TDoA technology in support of pedestrian dead reckoning
CN114894180A (zh) 基于相对导航信息的多源融合导航方法及系统
Gong et al. Application of improved robust adaptive algorithm in UWB/MEMS positioning system
CN117346768B (zh) 同时适用于室内外的多传感器融合感知定位方法
CN116193581B (zh) 一种基于集员滤波的室内无人机混合定位方法及系统
Sahmoudi et al. Analysis of a navigation system based on partially tight integration of IMU-visual odometry with loosely coupled GPS

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
CB02 Change of applicant information

Address after: 410205 1705, building 1b, Hunan military civilian integration science and Technology Innovation Industrial Park, No. 699, Qingshan Road, Changsha high tech Development Zone, Changsha, Hunan

Applicant after: Hunan yunjiangna micro Information Technology Co.,Ltd.

Address before: 410205 No.5, F4 building, luguyuyuan, No.27 Wenxuan Road, Changsha high tech Development Zone, Changsha City, Hunan Province

Applicant before: HUNAN GLONAVIN INFORMATION TECHNOLOGY CO.,LTD.

CB02 Change of applicant information
GR01 Patent grant
GR01 Patent grant