CN115235454A - 行人运动约束的视觉惯性融合定位与建图方法和装置 - Google Patents

行人运动约束的视觉惯性融合定位与建图方法和装置 Download PDF

Info

Publication number
CN115235454A
CN115235454A CN202211123604.6A CN202211123604A CN115235454A CN 115235454 A CN115235454 A CN 115235454A CN 202211123604 A CN202211123604 A CN 202211123604A CN 115235454 A CN115235454 A CN 115235454A
Authority
CN
China
Prior art keywords
imu
factor
foot
bound
visual
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
CN202211123604.6A
Other languages
English (en)
Other versions
CN115235454B (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.)
National University of Defense Technology
Original Assignee
National University of Defense Technology
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 National University of Defense Technology filed Critical National University of Defense Technology
Priority to CN202211123604.6A priority Critical patent/CN115235454B/zh
Publication of CN115235454A publication Critical patent/CN115235454A/zh
Application granted granted Critical
Publication of CN115235454B publication Critical patent/CN115235454B/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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3807—Creation or updating of map data characterised by the type of data
    • G01C21/3811—Point data, e.g. Point of Interest [POI]
    • 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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/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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Navigation (AREA)

Abstract

本申请涉及一种行人运动约束的视觉惯性融合定位与建图方法和装置。所述方法包括：对足绑式IMU进行因子图建模，得到因子图，根据足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子；根据视觉惯性里程计观测的位置信息与足绑式IMU观测的位置信息，构建视觉惯性里程计因子；将零速修正因子和视觉惯性里程计因子作为状态变量的约束条件，对因子图进行优化，得到优化因子图；根据视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据关键帧图像进行稠密重建，得到建图结果；对优化因子图和建图结果进行融合，实现行人运动的定位与建图。采用本方法能够复杂环境下的行人导航定位与建图。

Description

行人运动约束的视觉惯性融合定位与建图方法和装置
技术领域
本申请涉及行人导航技术领域，特别是涉及一种行人运动约束的视觉惯性融合定位与建图方法和装置。
背景技术
尽管卫星导航技术广泛应用于我们生产生活的各个领域，但是卫星导航易受遮挡而无法应用于室内、密林、隧道、地下等复杂场景，更不能全面满足行人导航需求。并且，行人运动模式更为灵活自由，动态模式更为复杂多变，导航装备精度要求高，体积成本要求低，因此与车辆、无人机等载体的导航模式有着很大的不同。行人导航也就成为了导航领域公认的难题之一，也是当前研究的热点问题之一。
为了解决行人自主导航定位问题，Hemin Zhang等人提出了一种基于航位推算的定位解算方法，其主要利用行人步态模型结合加速度计输出估计步长，结合陀螺与磁强计来进行航位推算。Skog等人足绑式MIMU上，提出了SHOE零速检测器来抑制低精度MIMU惯导的捷联惯性导航解算中的误差发散问题，从一定程度上提高了定位精度与稳定性。但是这两者方法由于主要依赖惯性信息来对行人进行定位，难以实现长航时的高精度定位。Andrei等人则提出了uNavchip结合零速检测与协同导航方法实现行人的导航定位。随着SLAM相关技术的发展，基于视觉的方法为解决行人导航问题提供了重要的思路。视觉传感器信息源包含了大量的关于运动场景的集合信息，视觉里程计或者惯性里程计通过利用这些场景中的几何信息，大大提高了行人导航定位的精度。如VINS、ORB-SLAM3等视觉SLAM方案利用视觉、惯性、深度等信息对相机进行位姿解算，将其用于行人导航可提高定位的可靠性与精度。陈泽等人则提出了利用足部惯性导航模块来修正视觉惯性里程计的行人全源导航算法，但是这种方法以视觉惯性模块为核心，易受到环境干扰而失效，鲁棒性相对不高。
视觉三维重建算法对于行人导航有着重要意义。行人导航过程中进行稠密三维重建有助于提高行人与环境的交互感知能力，是增强现实，语义分割，路径规划，导航避障等下游技术的基础。在视觉SLAM领域，RGB-D方法可以提供稠密的深度图和运动轨迹，因此也是三维重建领域的重要方法。Bylow等人和Kerl等人主要利用RGB-D相机在高精度轨迹估计反面展开了深入的研究。相较于RGB-D建图，单目方法不论是在建图还是在跟踪方面都具有许多挑战。Newcombe等人提出了DTAM算法使用一个单目相机，在GPU上实时对基于光流的代价体对相机位姿和稠密地图进行联合估计。最近有许多基于深度学习的方法相较于经典方法有了更好的表现。这些方法利用深度神经网络来构建出三维模型，这些3D模型涵盖了体积表示，3D点云表示以及深度图集表示等多种表示形式。Huang等人提出了DeepMVS算法根据输入图像预测的二维深度特征图直接计算单个体积。在接下来的工作中，Yao等人提出了MVSNet，利用一个循环网络来取代卷积神经网络进行深度预测。
发明内容
基于此，有必要针对上述技术问题，提供一种行人运动约束的视觉惯性融合定位与建图方法和装置。
一种行人运动约束的视觉惯性融合定位与建图方法，应用于行人导航中，其中足绑式IMU设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，所述方法包括：
对所述足绑式IMU进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和IMU因子节点，所述偏差因子节点连接相邻的偏差变量，所述IMU因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；
根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子；
根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子；
将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；
根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；
对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
在其中一个实施例中，还包括：构建坐标系方向为右-前-上的载体坐标系以及东-北-天的导航坐标系，通过对所述足绑式IMU进行初始化；
利用所述足绑式IMU静止时的三轴加速度测量得到横滚角和俯仰角；
根据所述横滚角和所述俯仰角，将足绑式IMU的测量数据从载体坐标系转化至导航坐标系，并计算得到在导航坐标系中的初始航向角。
在其中一个实施例中，还包括：通过足绑式IMU进行零速检测之后，进行足绑式IMU和所述视觉惯性里程计时间同步的步骤包括：
将足绑式IMU和所述视觉惯性里程计的传感器信息通过话题通讯机制上传至数据处理中心，并通过数据处理中心对所述传感器数据打上时间戳；
进行足绑式IMU和所述视觉惯性里程计空间上同步的步骤包括：
确定足绑式IMU和所述视觉惯性里程计在水平方向上初始航向角偏差值，根据所述初始航向角偏差值，对足绑式IMU和所述视觉惯性里程计进行空间对齐。
在其中一个实施例中，还包括：对所述视觉惯性里程计进行因子图建模，得到定位因子图；所述定位因子图中的因子包括：视觉因子和惯性因子；所述视觉因子是根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差确定的；所述惯性因子是通过预积分方式计算得到的；
对所述定位因子图进行计算，得到视觉惯性里程计的定位数据。
在其中一个实施例中，还包括：根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差，得到视觉因子为：
Figure 377626DEST_PATH_IMAGE001
其中，
Figure 970282DEST_PATH_IMAGE002
表示运动对象从第i个图像运动到第j个图像时，像素点在导航坐标系中位置，
Figure 661157DEST_PATH_IMAGE003
表示运动对象从第i个图像运动到第j个图像时，像素点在世界坐标系中位置；
其中，
Figure 604842DEST_PATH_IMAGE004
，
Figure 39366DEST_PATH_IMAGE005
表示投影方程；
Figure 752107DEST_PATH_IMAGE006
表示测量误差。
在其中一个实施例中，还包括：对所述足绑式IMU状态随时间变化的过程建模为：
Figure 348305DEST_PATH_IMAGE007
其中，
Figure 44865DEST_PATH_IMAGE008
表示导航状态变量，
Figure 17500DEST_PATH_IMAGE009
表示IMU的误差模型，
Figure 850327DEST_PATH_IMAGE010
与
Figure 617426DEST_PATH_IMAGE011
分别表示导航状态变量与误差的非线性模型；
定义
Figure 270124DEST_PATH_IMAGE012
与
Figure 312030DEST_PATH_IMAGE013
表示
Figure 733784DEST_PATH_IMAGE014
时刻的状态变量与误差变量，
Figure 937363DEST_PATH_IMAGE015
表示IMU测量值，离散化得到：
Figure 77357DEST_PATH_IMAGE016
其中，
Figure 657374DEST_PATH_IMAGE017
表示IMU因子节点与
Figure 402476DEST_PATH_IMAGE018
表示偏差因子节点。
在其中一个实施例中，还包括：根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子为：
Figure 901591DEST_PATH_IMAGE019
Figure 404248DEST_PATH_IMAGE020
表示载体坐标系下足绑式IMU的零速，
Figure 912589DEST_PATH_IMAGE021
表示载体坐标系下足绑式IMU的速度预测值，
Figure 184302DEST_PATH_IMAGE022
为系统的速度观测函数。
在其中一个实施例中，还包括：根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子为：
Figure 854318DEST_PATH_IMAGE023
其中，
Figure 844270DEST_PATH_IMAGE024
视觉惯性里程计提供的位置信息，
Figure 625144DEST_PATH_IMAGE025
为系统的位置观测函数，用于计算所述足绑式IMU观测的位置信息。
在其中一个实施例中，还包括：根据足绑式IMU的零速检测结果，剔除处于同一步态范围内的关键帧，得到不同步态中的关键帧；
将所述关键帧以及所述关键帧对应的全局位姿作为输入，输入预先构建的CVA-MVSNet模型进行分层深度估计，聚合所有关键帧的深度特征，得到深度估计结果；其中，聚合所有关键帧的深度特征包括：单阶段深度估计过程和分层深度估计过程；
所述单阶段深度估计过程为：采用自适应视图分级的方式构建成代价体为：
Figure 344839DEST_PATH_IMAGE026
。
其中，
Figure 61122DEST_PATH_IMAGE027
是大小为
Figure 928584DEST_PATH_IMAGE028
的视图聚合权重，
Figure 388515DEST_PATH_IMAGE029
是广播的元素级乘法；
将所述代价体输入3D U-Net模型进行正则化并最终经过softmax非线性层输出估计深度值为：
Figure 759454DEST_PATH_IMAGE030
其中，
Figure 974534DEST_PATH_IMAGE031
表示每个像素的深度假设。
一种行人运动约束的视觉惯性融合定位与建图装置，应用于行人导航中，其中足绑式IMU设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，所述装置包括：
因子图构建模块，用于对所述足绑式IMU进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和IMU因子节点，所述偏差因子节点连接相邻的偏差变量，所述IMU因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；
约束构建模块，用于根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子；根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子；
图优化模块，用于将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；
定位与建图模块，用于根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
上述行人运动约束的视觉惯性融合定位与建图方法和装置，首先，基于足绑式IMU进行因子图建模，然后基于行人上设置的视觉惯性里程计，构建了视觉惯性里程计因子，虽然两个传感器的设置在不同位置，但是在行动过程中，二者属于柔性连接，因此，通过找到足绑式IMU与视觉惯性里程计的运动关系，构建了视觉惯性里程计因子，另外还构建了零速修正因子，共同作为约束条件，对因子图进行优化，并且建图。从而，本发明可以不依赖于视觉数据，即使在复杂的环境，依然可以进行准确的行人导航与建图。
附图说明
图1为一个实施例中行人运动约束的视觉惯性融合定位与建图方法的流程示意图；
图2为一个实施例中行人运动约束的视觉惯性融合定位与建图方法的框架示意图；
图3为一个实施例中优化因子图的框架图；
图4为一个实施例中行人运动约束的视觉惯性融合定位与建图装置的结构框图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
在一个实施例中，如图1所示，提供了一种行人运动约束的视觉惯性融合定位与建图方法，包括以下步骤：
步骤102，对所述足绑式IMU进行因子图建模，得到因子图。
因子图中的因子节点包括：偏差因子节点和IMU因子节点，所述偏差因子节点连接相邻的偏差变量，IMU因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量。
步骤104，根据足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子。
步骤106，根据视觉惯性里程计观测的位置信息与足绑式IMU观测的位置信息，构建视觉惯性里程计因子。
步骤108，将零速修正因子和视觉惯性里程计因子作为状态变量的约束条件，对因子图进行优化，得到优化因子图。
步骤110，根据视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据关键帧图像进行稠密重建，得到建图结果。
步骤112，对优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
上述行人运动约束的视觉惯性融合定位与建图方法，框架图如图2所示，首先，基于足绑式IMU进行因子图建模，然后基于行人上设置的视觉惯性里程计，构建了视觉惯性里程计因子，虽然两个传感器的设置在不同位置，但是在行动过程中，二者属于柔性连接，因此，通过找到足绑式IMU与视觉惯性里程计的运动关系，构建了视觉惯性里程计因子，另外还构建了零速修正因子，共同作为约束条件，对因子图进行优化，并且建图。从而，本发明可以不依赖于视觉数据，即使在复杂的环境，依然可以进行准确的行人导航与建图。
在其中一个实施例中，构建坐标系方向为右-前-上的载体坐标系以及东-北-天的导航坐标系，通过对足绑式IMU进行初始化；利用足绑式IMU静止时的三轴加速度测量得到横滚角和俯仰角；根据横滚角和所述俯仰角，将足绑式IMU的测量数据从载体坐标系转化至导航坐标系，并计算得到在导航坐标系中的初始航向角。
具体的，在系统初始化部分，主要利用磁力计进行初始航向估计，以便于后续实现空间对齐。首采集磁传感器在各个方向上的数据，即不断旋转磁传感器以对各个方向的产生充分激励来对磁传感器进行校准。之后将包含了磁传感器的足绑式IMU安装于足部，之后静止20s采集静止数据。系统载体坐标系方向为右-前-上坐标系，导航坐标系为东-北-天坐标系。利用静止时的三轴加速度数据
Figure 673500DEST_PATH_IMAGE032
用来测量横滚角
Figure 327335DEST_PATH_IMAGE033
，俯仰角(pitch)：
Figure 428147DEST_PATH_IMAGE034
其中，
Figure 814129DEST_PATH_IMAGE035
表示重力的大小。
为了计算初始航向角
Figure 390603DEST_PATH_IMAGE036
，我们利用校准后的磁传感器数据
Figure 180619DEST_PATH_IMAGE037
，将其投影至导航坐标系得到：
Figure 526149DEST_PATH_IMAGE038
则初始航向角
Figure 20716DEST_PATH_IMAGE039
为：
Figure 818907DEST_PATH_IMAGE040
其中一个实施例中，足绑式IMU进行零速检测步骤包括：
在行人正常步态周期中，采用广义似然比检验的方式来对零速状态进行检测。首先将k时刻的足绑式IMU输出的比力表示为
Figure 424332DEST_PATH_IMAGE041
，其中包括k时刻的加速度计输出的角速率
Figure 562052DEST_PATH_IMAGE042
与陀螺的输出
Figure 352154DEST_PATH_IMAGE043
，即
Figure 778587DEST_PATH_IMAGE044
对于零速检测来说，主要的目的是检测出在时长为W的滑动时间窗口
Figure 46757DEST_PATH_IMAGE045
内，IMU是否处于零速状态。记
Figure 304563DEST_PATH_IMAGE046
。因此，可以将零速检测看作二元假设检验问题，原假设
Figure 140932DEST_PATH_IMAGE047
与备择假设
Figure 913716DEST_PATH_IMAGE048
分别定义如下：
Figure 860944DEST_PATH_IMAGE049
：IMU移动
Figure 35573DEST_PATH_IMAGE050
：IMU静止
将虚警概率表示为
Figure 777264DEST_PATH_IMAGE051
(即IMU在移动时被检测为静止的概率)，零速检测正确概率表示为
Figure 37344DEST_PATH_IMAGE052
。根据静止时陀螺与加速度计的输出，可以得到以下关系：
Figure 850580DEST_PATH_IMAGE053
其中
Figure 20661DEST_PATH_IMAGE054
表示时长为W的滑动时间窗口，对于静止时刻的
Figure 57887DEST_PATH_IMAGE055
，满足
Figure 8525DEST_PATH_IMAGE056
。即IMU静止时，
Figure 297555DEST_PATH_IMAGE057
为方向不确定的单位向量。
对于足绑式IMU，我们可以建模为
Figure 181198DEST_PATH_IMAGE058
其中
Figure 795850DEST_PATH_IMAGE059
，
Figure 764943DEST_PATH_IMAGE060
这里的
Figure 919981DEST_PATH_IMAGE061
，
Figure 799075DEST_PATH_IMAGE062
分别表示IMU比力与角速率的真实值。
Figure 443683DEST_PATH_IMAGE063
，
Figure 775438DEST_PATH_IMAGE064
分别表示加速度计与陀螺的噪声，二者独立同分布，可建模为零均值高斯噪声，协方差矩阵
Figure 530905DEST_PATH_IMAGE065
根据上述对静止时足绑式IMU的陀螺与加表输出我们可以得出
Figure 592402DEST_PATH_IMAGE066
。即在静止时刻下IMU的模型参数为方向未知的单位向量。
可以把加速度计与陀螺的概率密度函数(
Figure 283277DEST_PATH_IMAGE067
)写作：
Figure 492542DEST_PATH_IMAGE068
Figure 989382DEST_PATH_IMAGE069
当
Figure 374227DEST_PATH_IMAGE070
时，即IMU处于运动状态时，上式关于
Figure 563900DEST_PATH_IMAGE071
的最大似然估计为
Figure 870247DEST_PATH_IMAGE072
，带入上式可得
Figure 233096DEST_PATH_IMAGE073
当
Figure 675709DEST_PATH_IMAGE074
时，即IMU处于静止状态时，上式关于
Figure 833021DEST_PATH_IMAGE075
的最大似然估计为
Figure 954561DEST_PATH_IMAGE076
，带入上式可得：
Figure 465308DEST_PATH_IMAGE077
则
Figure 418220DEST_PATH_IMAGE078
其中，
Figure 621800DEST_PATH_IMAGE079
因此，将
Figure 965056DEST_PATH_IMAGE080
带回原式可得
Figure 669707DEST_PATH_IMAGE081
根据广义似然比检验条件我们可以得到
Figure 352493DEST_PATH_IMAGE082
Figure 851607DEST_PATH_IMAGE083
简化可得：
Figure 354264DEST_PATH_IMAGE084
其中
Figure 534709DEST_PATH_IMAGE085
。即当滑动时间窗口内加速度计与陀螺的输出满足上式时，将其判定为零速状态，否则不是零速状态。
在另一个实施例中，通过足绑式IMU进行零速检测之后，进行足绑式IMU和视觉惯性里程计时间同步的步骤包括：将足绑式IMU和视觉惯性里程计的传感器信息通过话题通讯机制上传至数据处理中心，并通过数据处理中心对传感器数据打上时间戳；进行足绑式IMU和视觉惯性里程计空间上同步的步骤包括：确定足绑式IMU和视觉惯性里程计在水平方向上初始航向角偏差值，根据初始航向角偏差值，对足绑式IMU和所述视觉惯性里程计进行空间对齐。
具体的，在将足绑式IMU数据进行零速状态检测后，由于足绑式IMU与视觉惯性里程计模块是不同的传感器源，借助ROS操作系统的话题通讯机制将不同的传感器信息实时上传至数据处理单元并将其附上时间戳，进而实现不同传感器源的时间同步。
在实际用用过程中，视觉惯性里程计位于胸前，而足绑式IMU位于脚步，二者之间初始状态在水平方向上往往会相差一个航向角
Figure 196635DEST_PATH_IMAGE086
。为了实现两个传感器的空间对准，需要将视觉定位模块的初始航向减去二者之间的航向角偏差，即
Figure 7596DEST_PATH_IMAGE087
其中
Figure 122182DEST_PATH_IMAGE088
表示足绑式IMU的初始航向，
Figure 106319DEST_PATH_IMAGE089
表示视觉惯性里程计模块的初始航向。从而实现了两个不同传感器的空间对准。
在其中一个实施例中，对视觉惯性里程计进行因子图建模，得到定位因子图；定位因子图中的因子包括：视觉因子和惯性因子；视觉因子是根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差确定的；惯性因子是通过预积分方式计算得到的；对定位因子图进行计算，得到视觉惯性里程计的定位数据。
具体的，视觉惯性里程计定位主要采用现有的视觉里程计定位模块。在本示例中，采用基于VINS-Mono方法的视觉惯性里程计。在该模块中，利用相机作为视觉前端，与固联的IMU模块紧耦合形成视觉惯性里程计。并利用因子图优化的方法来处理这个问题。
视觉惯性里程计的滑动窗口内待估计
Figure 498117DEST_PATH_IMAGE090
状态为:
Figure 339034DEST_PATH_IMAGE091
式中，
Figure 81862DEST_PATH_IMAGE092
是 MIMU 状态变量，由位置
Figure 666427DEST_PATH_IMAGE093
、姿态
Figure 240628DEST_PATH_IMAGE094
、速度
Figure 127813DEST_PATH_IMAGE095
、加速度计零偏
Figure 951412DEST_PATH_IMAGE096
和陀螺零偏
Figure 480614DEST_PATH_IMAGE097
组成，优化窗口内共有
Figure 706059DEST_PATH_IMAGE098
个；
Figure 92041DEST_PATH_IMAGE099
是图像中特征点的逆深度，优化窗口中共有
Figure 543882DEST_PATH_IMAGE100
个。
1、视觉因子
外部某个特征点在世界坐标系w系的坐标为
Figure 735829DEST_PATH_IMAGE101
，特征点直接通过相机测量在第
Figure 691146DEST_PATH_IMAGE103
张和第
Figure 513609DEST_PATH_IMAGE104
张图像中的像素坐标分别为
Figure 311801DEST_PATH_IMAGE105
和
Figure 917225DEST_PATH_IMAGE106
。视觉系统经过运动，第
Figure 117263DEST_PATH_IMAGE107
张图的特征点像素坐标通过转换到第
Figure 110626DEST_PATH_IMAGE108
张图的像素坐标为
Figure 5901DEST_PATH_IMAGE109
，
Figure 805230DEST_PATH_IMAGE110
与
Figure 735140DEST_PATH_IMAGE111
之间的误差称之为重投影误差。视觉因子代价函数
Figure 430563DEST_PATH_IMAGE112
为：
Figure 406610DEST_PATH_IMAGE113
Figure 353837DEST_PATH_IMAGE114
与
Figure 528466DEST_PATH_IMAGE115
的关系为：
Figure 535737DEST_PATH_IMAGE116
式中，
Figure 733500DEST_PATH_IMAGE117
表示投影方程；
Figure 609052DEST_PATH_IMAGE118
表示测量误差。特征点在第
Figure 525273DEST_PATH_IMAGE119
张图和第
Figure 828078DEST_PATH_IMAGE120
张图的视觉因子代价函数 可以扩展为：
Figure 778717DEST_PATH_IMAGE121
2、视觉惯性里程计中的惯性因子
视觉系统中，MIMU 的采样频率一般都在100Hz以上，远高于相机的采样频率。为了避免优化中对 MIMU 状态的重复计算，构建视觉惯性因子采用经典的预积分方法。MIMU 在b系中测量的加速度
Figure 67747DEST_PATH_IMAGE122
和角速度
Figure 216968DEST_PATH_IMAGE123
为：
Figure 300462DEST_PATH_IMAGE124
式中，
Figure 269555DEST_PATH_IMAGE125
和
Figure 690172DEST_PATH_IMAGE126
为实际运动的加速度和角速度；
Figure 303687DEST_PATH_IMAGE127
为
Figure 213874DEST_PATH_IMAGE128
系中的重力向量；
Figure 545630DEST_PATH_IMAGE129
、
Figure 504358DEST_PATH_IMAGE130
、
Figure 362593DEST_PATH_IMAGE131
和
Figure 319048DEST_PATH_IMAGE132
为高斯噪声。
从时间点
Figure 997154DEST_PATH_IMAGE133
到时间点
Figure 759573DEST_PATH_IMAGE134
区间中，MIMU 的预积分为：
Figure 347681DEST_PATH_IMAGE135
式中，
Figure 599670DEST_PATH_IMAGE136
代表
Figure 906018DEST_PATH_IMAGE137
时的b系；
Figure 206549DEST_PATH_IMAGE138
、
Figure 773797DEST_PATH_IMAGE139
和
Figure 806475DEST_PATH_IMAGE140
分别代表位置、速度和姿态预积分。
视觉惯性里程计中惯性因子代价函数
Figure 724752DEST_PATH_IMAGE141
为：
Figure 828974DEST_PATH_IMAGE142
式中，
Figure 391674DEST_PATH_IMAGE143
为
Figure 985466DEST_PATH_IMAGE144
系到w系四元数；
Figure 827DEST_PATH_IMAGE145
为四元数乘法。
因此，视觉惯性里程计的优化目标函数可以写为：
Figure 643161DEST_PATH_IMAGE146
之后我们可以用Ceressolver来优化这个因子图。
在其中一个实施例中，对足绑式IMU状态随时间变化的过程建模为：
Figure 185001DEST_PATH_IMAGE147
其中，
Figure 559481DEST_PATH_IMAGE148
表示导航状态变量，
Figure 452351DEST_PATH_IMAGE149
表示IMU的误差模型，
Figure 898376DEST_PATH_IMAGE150
与
Figure 435667DEST_PATH_IMAGE151
分别表示导航状态变量与误差的非线性模型；
定义
Figure 105683DEST_PATH_IMAGE152
与
Figure 95636DEST_PATH_IMAGE153
表示
Figure 142089DEST_PATH_IMAGE154
时刻的状态变量与误差变量，
Figure 861784DEST_PATH_IMAGE155
表示IMU测量值，离散化得到：
Figure 578067DEST_PATH_IMAGE156
其中，
Figure 179950DEST_PATH_IMAGE157
表示IMU因子节点与
Figure 639881DEST_PATH_IMAGE158
表示偏差因子节点。
IMU因子所表示的代价函数可以表示为：
Figure 214082DEST_PATH_IMAGE159
具体的，根据足绑式IMU特点，构建以足绑式IMU为核心的全源导航因子图框架
Figure 225900DEST_PATH_IMAGE160
。
Figure 190445DEST_PATH_IMAGE161
表示因子节点
Figure 985226DEST_PATH_IMAGE162
的集合，
Figure 476250DEST_PATH_IMAGE163
表示变量节点
Figure 534336DEST_PATH_IMAGE164
的集合，
Figure 845231DEST_PATH_IMAGE165
表示因子节点与变量节点之间的边
Figure 974861DEST_PATH_IMAGE166
。与因子
Figure 195758DEST_PATH_IMAGE167
相邻的变量节点集合记作
Figure 814958DEST_PATH_IMAGE168
，并且我们将对这个变量集合赋值写作
Figure 488516DEST_PATH_IMAGE169
。则该因子图可以表示为作用于全体变量函数
Figure 421837DEST_PATH_IMAGE170
的分解：
Figure 621875DEST_PATH_IMAGE171
即每一个因子节点都可以看做是作用于相邻变量节点的函数，因子图即为因子节点的乘积。
在行人导航中，需要对足绑式IMU的状态变量
Figure 287342DEST_PATH_IMAGE172
进行估计。其中
Figure 572830DEST_PATH_IMAGE173
由位置
Figure 981946DEST_PATH_IMAGE174
、速度
Figure 239752DEST_PATH_IMAGE175
与姿态
Figure 935175DEST_PATH_IMAGE176
组成。因子图优化的过程可以看做是给定观测量
Figure 848905DEST_PATH_IMAGE177
利用非线性优化方法求解关于变量
Figure 920766DEST_PATH_IMAGE178
的最大后验估计的过程。即
Figure 33078DEST_PATH_IMAGE179
在因子图中，对于给定的观测量，归一化项
Figure 40349DEST_PATH_IMAGE180
与最大后验估计无关，因此，不去显示的表示观测量。同时为了强调最大后验估计是关于变量
Figure 300429DEST_PATH_IMAGE181
的函数，可以用似然估计函数
Figure 51347DEST_PATH_IMAGE182
来表示相应的因子节点。即
Figure 814904DEST_PATH_IMAGE183
以下对各因子进行描述，
1、零速修正因子
根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子为：
Figure 320971DEST_PATH_IMAGE184
Figure 678135DEST_PATH_IMAGE185
表示载体坐标系下足绑式IMU的零速，
Figure 357378DEST_PATH_IMAGE186
表示载体坐标系下足绑式IMU的速度预测值，
Figure 116386DEST_PATH_IMAGE187
为系统的速度观测函数。
2、视觉惯性里程计因子
根据视觉惯性里程计观测的位置信息与足绑式IMU观测的位置信息，构建视觉惯性里程计因子为：
Figure 793355DEST_PATH_IMAGE188
其中，
Figure 762448DEST_PATH_IMAGE189
视觉惯性里程计提供的位置信息，
Figure 855169DEST_PATH_IMAGE190
为系统的位置观测函数，用于计算所述足绑式IMU观测的位置信息。
综上，优化因子图如图3所示，最大后验估计可以写作：
Figure 858897DEST_PATH_IMAGE191
Figure 378871DEST_PATH_IMAGE192
在其中一个实施例中，根据足绑式IMU的零速检测结果，剔除处于同一步态范围内的关键帧，得到不同步态中的关键帧；将关键帧以及所述关键帧对应的全局位姿作为输入，输入预先构建的CVA-MVSNet模型进行分层深度估计，聚合所有关键帧的深度特征，得到深度估计结果；其中，聚合所有关键帧的深度特征包括：单阶段深度估计过程和分层深度估计过程；单阶段深度估计过程为：采用自适应视图分级的方式构建成代价体为：
Figure 835260DEST_PATH_IMAGE193
。
其中，
Figure 793989DEST_PATH_IMAGE194
是大小为
Figure 527590DEST_PATH_IMAGE195
的视图聚合权重，
Figure 77520DEST_PATH_IMAGE196
是广播的元素级乘法；
将代价体输入3D U-Net模型进行正则化并最终经过softmax非线性层输出估计深度值为：
Figure 896571DEST_PATH_IMAGE197
其中，
Figure 455729DEST_PATH_IMAGE198
表示每个像素的深度假设。
具体的，由于相机图像频率相对较高，相邻两帧图像数据之间的重叠度往往较高，选取关键帧进行三维场景重建。首先根据行人运动约束，剔除掉处于同一个零速步态范围内的关键帧。即通过零速检测我们对足绑式IMU数据进行步态分割后，将剔除掉同一步内处于零速静止状态下的重复关键帧，增大不同关键帧之间的视差，提高建图效率。
在建图过程中，利用基于深度学习的方法对关键帧进行深度估计。这里神经网络的输入为关键帧的图像及其相应的全局位姿
Figure 297697DEST_PATH_IMAGE199
。其中，
Figure 284107DEST_PATH_IMAGE200
表示第一个关键帧大小为
Figure 652772DEST_PATH_IMAGE201
的图像，
Figure 625407DEST_PATH_IMAGE202
表示第
Figure 723813DEST_PATH_IMAGE203
帧图像的全局位姿。利用改进的CVA-MVSNet来进行深度估计，采用级联式的代价量来进行分层深度估计，之后用自适应视图聚合模块有效地聚合所有关键帧的深度特征，从而克服了深度MVS网络对内存的限制性要求，有效提高深度估计的性能。
关键帧的多尺度深度特征
Figure 756491DEST_PATH_IMAGE204
首先通过共享权重的2D U-Net网络来进行特征提取，其中
Figure 612451DEST_PATH_IMAGE205
是特征信息的空间纬度，
Figure 778991DEST_PATH_IMAGE206
是尺度信息。因此
Figure 341690DEST_PATH_IMAGE207
是大小为
Figure 669903DEST_PATH_IMAGE208
的矩阵，其中
Figure 13160DEST_PATH_IMAGE209
是尺度因子为
Figure 327598DEST_PATH_IMAGE210
的特征维度，
Figure 135017DEST_PATH_IMAGE211
，
Figure 509497DEST_PATH_IMAGE212
。参考帧的深度图则分为三个阶段进行分层估计，每个阶段将特征集
Figure 136788DEST_PATH_IMAGE213
作为输入并预测图像的深度值
Figure 582813DEST_PATH_IMAGE214
，其中
Figure 854525DEST_PATH_IMAGE215
的大小为
Figure 790120DEST_PATH_IMAGE216
。下面对单阶段估计和分层多阶段估计进行分别说明。
1、单阶段深度估计
算法在每个阶段都会利用深度特征
Figure 780073DEST_PATH_IMAGE217
构建匹配代价函数。对于参考帧的每个像素点，我们假设深度值为
Figure 826526DEST_PATH_IMAGE218
，得到一个大小为
Figure 280641DEST_PATH_IMAGE219
的。基于深度假设、相对位姿
Figure 996925DEST_PATH_IMAGE220
和相机内参，将每一帧图像的深度特征
Figure 864386DEST_PATH_IMAGE221
通过可微单应变换，扭曲至参考图像相机前的这些平行面上。每一帧图像的深度特征可以被表示为大小为
Figure 324318DEST_PATH_IMAGE222
的特征卷
Figure 898519DEST_PATH_IMAGE223
。为了将多视图特征体的信息聚合到一个代价体中，大多数以前的深度MVS方法都将不同的视图视为等同的，并使用基于差异的指标：
Figure 910337DEST_PATH_IMAGE224
其中，
Figure 140461DEST_PATH_IMAGE225
但是在滑窗SLAM系统中，关键帧在优化窗口中的分布并不均匀，甚至有的滑窗中并没有关键帧，通常新的关键帧之间的距离要小于旧的关键帧。这会引起阻塞和无重叠的图像帧。因此，基于变量的代价指标是不合适的，因为它对不同的视图具有同等的权重。为了解决这个问题，我们采用自适应视图分级的方式来构建成代价体：
Figure 263138DEST_PATH_IMAGE226
其中，
Figure 363949DEST_PATH_IMAGE227
是大小为
Figure 15510DEST_PATH_IMAGE228
的视图聚合权重，
Figure 591985DEST_PATH_IMAGE229
是广播的元素级乘法。以
Figure 393719DEST_PATH_IMAGE230
作为输入，通过浅层的卷积神经网络可以估计出相对于每个
Figure 942512DEST_PATH_IMAGE231
的视图权重
Figure 296133DEST_PATH_IMAGE232
。该聚合模块可以令网络自适应地降低错误信息的权重。
代价体
Figure 235270DEST_PATH_IMAGE233
通过3D U-Net进行正则化并最终经过
Figure 965329DEST_PATH_IMAGE234
非线性层来获取大小为
Figure 40732DEST_PATH_IMAGE235
的概率体，给定每个像素的深度假设
Figure 768517DEST_PATH_IMAGE236
，估计深度值为：
Figure 54004DEST_PATH_IMAGE237
2、分层深度估计
网络利用先前阶段
Figure 463120DEST_PATH_IMAGE238
的深度估计来定义一个精确的深度假设张量
Figure 517664DEST_PATH_IMAGE239
。由于第一阶段之前没有阶段，深度图
Figure 681929DEST_PATH_IMAGE240
的每个像素具有相同的深度范围。之后的阶段深度
Figure 330079DEST_PATH_IMAGE241
经过上采样并作为先验来定义
Figure 401940DEST_PATH_IMAGE242
。特别的，对于位于
Figure 451936DEST_PATH_IMAGE243
处的像素，
Figure 318261DEST_PATH_IMAGE244
被定义为利用上采样的
Figure 453707DEST_PATH_IMAGE245
作为中心。然后，用预先定义的起点来围绕这个中心继续进行采样。这样，更高的阶段则需要更少的深度信息。之后，再使用适用于所有三个阶段的真实深度的L1损失函数来训练网络，并使用总和作为最终损失函数。从而获得关键帧图像的深度估计。
应该理解的是，虽然图1的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图1中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图4所示，提供了一种行人运动约束的视觉惯性融合定位与建图装置，应用于行人导航中，其中足绑式IMU设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，包括：因子图构建模块402、约束构建模块404、图优化模块406和定位与建图模块408，其中：
因子图构建模块，用于对所述足绑式IMU进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和IMU因子节点，所述偏差因子节点连接相邻的偏差变量，所述IMU因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；
约束构建模块，用于根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子；根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子；
图优化模块，用于将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；
定位与建图模块，用于根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
关于行人运动约束的视觉惯性融合定位与建图装置的具体限定可以参见上文中对于行人运动约束的视觉惯性融合定位与建图方法的限定，在此不再赘述。上述行人运动约束的视觉惯性融合定位与建图装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双数据率SDRAM（DDRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink） DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (10)

1.一种行人运动约束的视觉惯性融合定位与建图方法，其特征在于，应用于行人导航中，其中足绑式IMU设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，所述方法包括：
对所述足绑式IMU进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和IMU因子节点，所述偏差因子节点连接相邻的偏差变量，所述IMU因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；
根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子；
根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子；
将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；
根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；
对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
构建坐标系方向为右-前-上的载体坐标系以及东-北-天的导航坐标系，通过对所述足绑式IMU进行初始化；
利用所述足绑式IMU静止时的三轴加速度测量得到横滚角和俯仰角；
根据所述横滚角和所述俯仰角，将足绑式IMU的测量数据从载体坐标系转化至导航坐标系，并计算得到在导航坐标系中的初始航向角。
3.根据权利要求1所述的方法，其特征在于，所述方法还包括：
通过足绑式IMU进行零速检测之后，进行足绑式IMU和所述视觉惯性里程计时间同步的步骤包括：
将足绑式IMU和所述视觉惯性里程计的传感器信息通过话题通讯机制上传至数据处理中心，并通过数据处理中心对所述传感器数据打上时间戳；
进行足绑式IMU和所述视觉惯性里程计空间上同步的步骤包括：
确定足绑式IMU和所述视觉惯性里程计在水平方向上初始航向角偏差值，根据所述初始航向角偏差值，对足绑式IMU和所述视觉惯性里程计进行空间对齐。
4.根据权利要求1所述的方法，其特征在于，视觉惯性里程计进行定位的步骤包括：
对所述视觉惯性里程计进行因子图建模，得到定位因子图；所述定位因子图中的因子包括：视觉因子和惯性因子；所述视觉因子是根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差确定的；所述惯性因子是通过预积分方式计算得到的；
对所述定位因子图进行计算，得到视觉惯性里程计的定位数据。
5.根据权利要求4所述的方法，其特征在于，计算视觉因子的步骤包括：
根据视觉传感器对运动对象的像素点位置进行测量产生的测量位置误差，得到视觉因子为：
Figure 391567DEST_PATH_IMAGE001
其中，
Figure 715232DEST_PATH_IMAGE002
表示运动对象从第i个图像运动到第j个图像时，像素点在导航坐标系中位置，
Figure 229390DEST_PATH_IMAGE003
表示运动对象从第i个图像运动到第j个图像时，像素点在世界坐标系中位置；
其中，
Figure 155758DEST_PATH_IMAGE004
，
Figure 376655DEST_PATH_IMAGE005
表示投影方程；
Figure 995855DEST_PATH_IMAGE006
表示测量误差。
6.根据权利要求4所述的方法，其特征在于，所述对所述足绑式IMU进行因子图建模，得到因子图，包括：
对所述足绑式IMU状态随时间变化的过程建模为：
Figure 669413DEST_PATH_IMAGE007
其中，
Figure 399472DEST_PATH_IMAGE008
表示导航状态变量，
Figure 802771DEST_PATH_IMAGE009
表示IMU的误差模型，
Figure 202660DEST_PATH_IMAGE010
与
Figure 488148DEST_PATH_IMAGE011
分别表示导航状态变量与误差的非线性模型；
定义
Figure 897263DEST_PATH_IMAGE012
与
Figure 951807DEST_PATH_IMAGE013
表示
Figure 788176DEST_PATH_IMAGE014
时刻的状态变量与误差变量，
Figure 764222DEST_PATH_IMAGE015
表示IMU测量值，离散化得到：
Figure 570504DEST_PATH_IMAGE016
其中，
Figure 620500DEST_PATH_IMAGE017
表示IMU因子节点与
Figure 752404DEST_PATH_IMAGE018
表示偏差因子节点。
7.根据权利要求1至5任一项所述的方法，其特征在于，根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子，包括：
根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子为：
Figure 887850DEST_PATH_IMAGE019
Figure 497823DEST_PATH_IMAGE020
表示载体坐标系下足绑式IMU的零速，
Figure 730221DEST_PATH_IMAGE021
表示载体坐标系下足绑式IMU的速度预测值，
Figure 908393DEST_PATH_IMAGE022
为系统的速度观测函数。
8.根据权利要求1至5任一项所述的方法，其特征在于，根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子，包括：
根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子为：
Figure 655769DEST_PATH_IMAGE023
其中，
Figure 679220DEST_PATH_IMAGE024
视觉惯性里程计提供的位置信息，
Figure 828441DEST_PATH_IMAGE025
为系统的位置观测函数，用于计算所述足绑式IMU观测的位置信息。
9.根据权利要求1至5任一项所述的方法，其特征在于，根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果，包括：
根据足绑式IMU的零速检测结果，剔除处于同一步态范围内的关键帧，得到不同步态中的关键帧；
将所述关键帧以及所述关键帧对应的全局位姿作为输入，输入预先构建的CVA-MVSNet模型进行分层深度估计，聚合所有关键帧的深度特征，得到深度估计结果；其中，聚合所有关键帧的深度特征包括：单阶段深度估计过程和分层深度估计过程；
所述单阶段深度估计过程为：采用自适应视图分级的方式构建成代价体为：
Figure 911935DEST_PATH_IMAGE026
其中，
Figure 146607DEST_PATH_IMAGE027
是大小为
Figure 973749DEST_PATH_IMAGE028
的视图聚合权重，
Figure 977477DEST_PATH_IMAGE029
是广播的元素级乘法；
将所述代价体输入3D U-Net模型进行正则化并最终经过softmax非线性层输出估计深度值为：
Figure 497451DEST_PATH_IMAGE030
其中，
Figure 157103DEST_PATH_IMAGE031
表示每个像素的深度假设。
10.一种行人运动约束的视觉惯性融合定位与建图装置，其特征在于，应用于行人导航中，其中足绑式IMU设置在行人足部，视觉惯性里程计设置在行人腿部以上的躯干部分；所述视觉惯性里程计包括：视觉传感器和惯性传感器，所述装置包括：
因子图构建模块，用于对所述足绑式IMU进行因子图建模，得到因子图；其中，所述因子图中的因子节点包括：偏差因子节点和IMU因子节点，所述偏差因子节点连接相邻的偏差变量，所述IMU因子节点连接当前时刻的状态变量和偏差变量与下一时刻的状态变量；
约束构建模块，用于根据所述足绑式IMU的速度预测值以及零速之间的误差，构建零速修正因子；根据所述视觉惯性里程计观测的位置信息与所述足绑式IMU观测的位置信息，构建视觉惯性里程计因子；
图优化模块，用于将所述零速修正因子和所述视觉惯性里程计因子作为状态变量的约束条件，对所述因子图进行优化，得到优化因子图；
定位与建图模块，用于根据所述视觉传感器采集的图像数据以及通过足绑式IMU的零速检测结果，筛选关键帧图像，根据所述关键帧图像进行稠密重建，得到建图结果；对所述优化因子图和所述建图结果进行融合，实现行人运动的定位与建图。
CN202211123604.6A 2022-09-15 2022-09-15 行人运动约束的视觉惯性融合定位与建图方法和装置 Active CN115235454B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211123604.6A CN115235454B (zh) 2022-09-15 2022-09-15 行人运动约束的视觉惯性融合定位与建图方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211123604.6A CN115235454B (zh) 2022-09-15 2022-09-15 行人运动约束的视觉惯性融合定位与建图方法和装置

Publications (2)

Publication Number Publication Date
CN115235454A true CN115235454A (zh) 2022-10-25
CN115235454B CN115235454B (zh) 2022-12-30

Family

ID=83680606

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211123604.6A Active CN115235454B (zh) 2022-09-15 2022-09-15 行人运动约束的视觉惯性融合定位与建图方法和装置

Country Status (1)

Country Link
CN (1) CN115235454B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115685292A (zh) * 2023-01-05 2023-02-03 中国人民解放军国防科技大学 一种多源融合导航系统的导航方法和装置

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170227363A1 (en) * 2015-01-08 2017-08-10 Uti Limited Partnership Method and apparatus for enhanced pedestrian navigation based on wlan and mems sensors
US20190301871A1 (en) * 2018-03-27 2019-10-03 Artisense Corporation Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization
CN111024070A (zh) * 2019-12-23 2020-04-17 哈尔滨工程大学 一种基于航向自观测的惯性足绑式行人定位方法
CN112985392A (zh) * 2021-04-19 2021-06-18 中国人民解放军国防科技大学 基于图优化框架的行人惯性导航方法和装置
CN113091738A (zh) * 2021-04-09 2021-07-09 安徽工程大学 基于视觉惯导融合的移动机器人地图构建方法及相关设备
US20220018962A1 (en) * 2020-07-16 2022-01-20 Beijing Tusen Weilai Technology Co., Ltd. Positioning method and device based on multi-sensor fusion
WO2022079292A1 (en) * 2020-10-16 2022-04-21 SLAMcore Limited Initialising keyframes for visual-inertial localisation and/or mapping
CN114545472A (zh) * 2022-01-26 2022-05-27 中国人民解放军国防科技大学 一种gnss/ins组合系统的导航方法和装置
CN114608561A (zh) * 2022-03-22 2022-06-10 中国矿业大学 一种基于多传感器融合的定位与建图方法及系统
CN114966734A (zh) * 2022-04-28 2022-08-30 华中科技大学 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法
CN114964276A (zh) * 2022-05-26 2022-08-30 哈尔滨工业大学 一种融合惯导的动态视觉slam方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20170227363A1 (en) * 2015-01-08 2017-08-10 Uti Limited Partnership Method and apparatus for enhanced pedestrian navigation based on wlan and mems sensors
US20190301871A1 (en) * 2018-03-27 2019-10-03 Artisense Corporation Direct Sparse Visual-Inertial Odometry Using Dynamic Marginalization
CN111024070A (zh) * 2019-12-23 2020-04-17 哈尔滨工程大学 一种基于航向自观测的惯性足绑式行人定位方法
US20220018962A1 (en) * 2020-07-16 2022-01-20 Beijing Tusen Weilai Technology Co., Ltd. Positioning method and device based on multi-sensor fusion
WO2022079292A1 (en) * 2020-10-16 2022-04-21 SLAMcore Limited Initialising keyframes for visual-inertial localisation and/or mapping
CN113091738A (zh) * 2021-04-09 2021-07-09 安徽工程大学 基于视觉惯导融合的移动机器人地图构建方法及相关设备
CN112985392A (zh) * 2021-04-19 2021-06-18 中国人民解放军国防科技大学 基于图优化框架的行人惯性导航方法和装置
CN114545472A (zh) * 2022-01-26 2022-05-27 中国人民解放军国防科技大学 一种gnss/ins组合系统的导航方法和装置
CN114608561A (zh) * 2022-03-22 2022-06-10 中国矿业大学 一种基于多传感器融合的定位与建图方法及系统
CN114966734A (zh) * 2022-04-28 2022-08-30 华中科技大学 一种结合多线激光雷达的双向深度视觉惯性位姿估计方法
CN114964276A (zh) * 2022-05-26 2022-08-30 哈尔滨工业大学 一种融合惯导的动态视觉slam方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
MANG WANG 等: "An Optimal Cooperative Navigation Algorithm based on Factor Graph for Pedestrians", 《2021 3RD INTERNATIONAL CONFERENCE ON INTELLIGENT CONTROL, MEASUREMENT AND SIGNAL PROCESSING AND INTELLIGENT OIL FIELD (ICMSP)》 *
李倩 等: "基于因子图的INS/UWB室内行人紧组合定位技术", 《仪器仪表学报》 *
毛军 等: "惯性/视觉/激光雷达SLAM技术综述", 《导航定位与授时》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115685292A (zh) * 2023-01-05 2023-02-03 中国人民解放军国防科技大学 一种多源融合导航系统的导航方法和装置

Also Published As

Publication number Publication date
CN115235454B (zh) 2022-12-30

Similar Documents

Publication Publication Date Title
US11519729B2 (en) Vision-aided inertial navigation
Heo et al. Consistent EKF-based visual-inertial odometry on matrix Lie group
US10371529B2 (en) Computational budget estimation for vision-aided inertial navigation systems
Heo et al. EKF-based visual inertial navigation using sliding window nonlinear optimization
US10203209B2 (en) Resource-aware large-scale cooperative 3D mapping using multiple mobile devices
Mourikis et al. A multi-state constraint Kalman filter for vision-aided inertial navigation
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
US10254118B2 (en) Extrinsic parameter calibration of a vision-aided inertial navigation system
US11940277B2 (en) Vision-aided inertial navigation system for ground vehicle localization
Laidlow et al. Dense RGB-D-inertial SLAM with map deformations
CN112639502A (zh) 机器人位姿估计
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
Mourikis et al. A dual-layer estimator architecture for long-term localization
US20220051031A1 (en) Moving object tracking method and apparatus
Heo et al. Consistent EKF-based visual-inertial navigation using points and lines
CN111595334B (zh) 基于视觉点线特征与imu紧耦合的室内自主定位方法
Zhang et al. Vision-aided localization for ground robots
Tomažič et al. Fusion of visual odometry and inertial navigation system on a smartphone
CN111145251A (zh) 一种机器人及其同步定位与建图方法和计算机存储设备
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
Rahman et al. Contour based reconstruction of underwater structures using sonar, visual, inertial, and depth sensor
CN115235454B (zh) 行人运动约束的视觉惯性融合定位与建图方法和装置
Nguyen et al. Developing a cubature multi-state constraint Kalman filter for visual-inertial navigation system
Hong et al. Visual inertial odometry using coupled nonlinear optimization
Yang et al. An Indoor Navigation System Based on Stereo Camera and Inertial Sensors with Points and Lines

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