CN110260861A - 位姿确定方法及装置、里程计 - Google Patents

位姿确定方法及装置、里程计 Download PDF

Info

Publication number
CN110260861A
CN110260861A CN201910510820.8A CN201910510820A CN110260861A CN 110260861 A CN110260861 A CN 110260861A CN 201910510820 A CN201910510820 A CN 201910510820A CN 110260861 A CN110260861 A CN 110260861A
Authority
CN
China
Prior art keywords
frame
pose
updated
error
interframe
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
CN201910510820.8A
Other languages
English (en)
Other versions
CN110260861B (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.)
Beijing HJIMI Technology Co Ltd
Original Assignee
Beijing HJIMI 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 Beijing HJIMI Technology Co Ltd filed Critical Beijing HJIMI Technology Co Ltd
Priority to CN201910510820.8A priority Critical patent/CN110260861B/zh
Publication of CN110260861A publication Critical patent/CN110260861A/zh
Application granted granted Critical
Publication of CN110260861B publication Critical patent/CN110260861B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/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/18—Stabilised platforms, e.g. by gyroscope
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G01C22/02—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers by conversion into electric waveforms and subsequent integration, e.g. using tachometer generator
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Studio Devices (AREA)

Abstract

本发明提供了一种位姿确定方法及装置、里程计，通过获取图像帧序列中的N个连续的图像帧，最小化预设的目标函数，得到位姿。目标函数包括第一项和第二项，第一项为图像帧中相邻帧间的光度测量残差项，第二项为相邻帧间的状态参数测量残差项，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到。可见，本申请提供的技术方案，将视觉数据(图像帧)与IMU测量数据(状态参数测量残差)融合，确定位姿，因此，具有更高的准确性。同时，目标函数的第一项采用的是两帧间的像素点的光度误差，而不是采用的重投影误差，故避免了重投影误差中重投影后带来的几何位置的误差，因此能够进一步提高位姿的准确性。

Description

位姿确定方法及装置、里程计
技术领域
本发明涉及计算机视觉技术领域，特别涉及一种位姿确定方法及装置、里程计。
背景技术
科技的发展日新月异，作为科技发展前沿产品的自主机器人逐渐走进人们的视野中。里程计作为自主机器人相对定位的有效传感器，为自主机器人提供了实时的位姿信息，确保自主机器人能够实时获悉周围的场景结构。定位与地图构建(simultaneouslocalization and mapping,SLAM)技术作为里程计中的核心技术，能够在一个未知的环境中实时重建环境的三维结构并同时对机器人自身进行定位。在SLAM技术中，SLAM技术的具体目的是为了同时恢复出每帧图像对应的相机运动参数以及场景三维结构。其中，相机运动参数中所包含的3×3的旋转矩阵和三维位置变量就是里程计所需确定的位姿信息。
现有的里程计在位姿估计的过程中，首先提取相机采集到的相邻图像帧间的特征点，通过特征点匹配法将帧间的特征点关联，并通过重投影的方法使关联的特征点构成重投影对，最后最小化帧间特征点的重投影误差得到位姿估计结果。由于帧间特征点匹配过程会产生较大的误差，而且重投影误差中包含重投影后带来的额外的几何位置的误差，所以最小化帧间特征点的重投影误差并不能准确的反映帧间特征点的运动变化，故现有的VIO得到位姿估计结果准确度较低。
发明内容
有鉴于此，本发明实施例提供了一种位姿确定方法及装置、里程计，目的在于解决如何提高测量位姿的准确性的问题。
为实现上述目的，本发明实施例提供如下技术方案：
一种位姿确定方法，包括：
获取图像帧序列中的N个图像帧；
最小化预设的目标函数，得到位姿；
其中，所述目标函数至少包括第一项与第二项，所述第一项为所述图像帧中相邻帧间的光度测量残差，所述第二项为所述图像帧中相邻帧间的状态参数测量残差，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到，所述光度误差至少依据该两帧的位姿确定，所述状态参数至少包括位姿。
上述的方法，可选的，所述获取图像帧序列中的N个图像帧包括：
获取待更新图像帧集合，所述待更新图像帧集合中包括N个图像帧；
在采集到图像帧的情况下，从所述待更新图像帧集合中删除目标帧，并将采集到的图像帧加入所述待更新图像帧集合，得到更新后的图像帧集合，所述更新后的图像帧集合中包括所述图像帧序列中的N个图像帧；
其中，在所述待更新图像帧中的采集时刻最晚的帧为关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最早的帧，在所述待更新图像帧中的采集时刻最晚的帧不是关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最晚的帧。
上述的方法，可选的，所述目标函数还包括第三项，所述第三项为边缘化的先验信息，所述边缘化的先验信息为所述目标帧的位姿以及状态参数。
上述的方法，可选的，最小化目标函数的初始值的确定过程，包括：
计算获得所述待更新图像帧集合中每个图像帧的位姿以及状态参数；
依据所述图像帧的位姿以及所述相邻帧间的状态参数进行对齐计算，得到所述目标函数的初始化参数。
上述的方法，可选的，获取所述目标函数的第一项的过程，包括：
计算两帧间的像素点的光度误差，所述两帧间的像素点的光度误差依据相邻两帧中第一帧的像素点的光度、误差系数、以及所述第一帧中的像素点投影到第二帧得到的投影像素点的光度确定；其中，所述投影像素点依据所述第一帧的位姿确定；
对所述两帧间的像素点的光度误差进行最小化计算，得到所述目标函数的第一项。
上述的方法，可选的，用于计算所述目标函数的第二项的状态参数包括：旋转量、平移量、速度量、陀螺仪偏移以及加速度计偏移，所述旋转量和平移量构成所述位姿。
一种位姿确定装置，包括：
获取单元，用于获取图像帧序列中的N个图像帧；
优化单元，用于最小化预设的目标函数，得到位姿；
其中，所述目标函数至少包括第一项与第二项，所述第一项为所述图像帧中相邻帧间的光度测量残差，所述第二项为所述图像帧中相邻帧间的状态参数测量残差，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到，所述光度误差至少依据该两帧的位姿确定，所述状态参数至少包括位姿。
上述的装置，可选的，所述获取单元，用于获取图像帧序列中的N个图像帧，包括：
所述获取单元具体用于获取待更新图像帧集合，所述待更新图像帧集合中包括N个图像帧；在采集到图像帧的情况下，从所述待更新图像帧集合中删除目标帧，并将采集到的图像帧加入所述待更新图像帧集合，得到更新后的图像帧集合，所述更新后的图像帧集合中包括所述图像帧序列中的N个图像帧；其中，在所述待更新图像帧中的采集时刻最晚的帧为关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最早的帧，在所述待更新图像帧中的采集时刻最晚的帧不是关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最晚的帧。
上述的装置，可选的，所述优化单元在最小化的目标函数的过程中，所述目标函数还包括第三项，所述第三项为边缘化的先验信息，所述边缘化的先验信息为所述目标帧的位姿以及状态参数。
上述的装置，可选的，所述优化单元还用于：
计算获得所述待更新图像帧集合中每个图像帧的位姿以及状态参数；
依据所述图像帧的位姿以及所述状态参数进行对齐计算，得到所述目标函数的初始化参数。
上述的装置，可选的，所述优化单元还用于：
计算两帧间的像素点的光度误差，所述两帧间的像素点的光度误差依据相邻两帧中第一帧的像素点的光度、误差系数、以及所述第一帧中的像素点投影到第二帧得到的投影像素点的光度确定；其中，所述投影像素点依据所述第一帧的位姿确定；对所述两帧间的像素点的光度误差进行最小化计算，得到所述目标函数的第一项。
上述的装置，可选的，用于计算所述目标函数的第二项的状态参数包括：旋转量、平移量、速度量、陀螺仪偏移以及加速度计偏移，所述旋转量和平移量构成所述位姿。
一种视觉惯性里程计，包括处理器和存储器，所述存储器用于存储程序，所述处理器用于运行程序，以执行上述的位姿确定方法。
本发明提供的位姿确定方法及装置、里程计，通过获取图像帧序列中的N个连续的图像帧，最小化预设的目标函数，得到位姿。其中，目标函数包括第一项和第二项，第一项为图像帧中相邻帧间的光度测量残差项，第二项为相邻帧间的状态参数测量残差项，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到。可见，本申请提供的技术方案，将视觉数据(图像帧)与IMU测量数据(状态参数测量残差)融合，确定位姿，因此，具有更高的准确性。同时，目标函数的第一项采用的是两帧间的像素点的光度误差，而不是现有技术中采用的重投影误差，由于像素点的光度是像素点本身的一个参数，故光度误差避免了重投影误差中重投影后带来的几何位置的误差，因此能够进一步提高位姿的准确性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据提供的附图获得其他的附图。
图1为本申请实施例提供的位姿确定方法的流程图；
图2为本申请实施例提供的更新滑动窗口中的图像帧的流程图；
图3为本申请实施例提供的获取目标函数中的相邻帧间的光度测量残差项的流程图；
图4为本申请实施例提供的位姿确定方法的整体流程图；
图5为本发明提供的位姿确定装置的结构示意图；
图6为本发明提供的视觉惯性里程计的结构示意图。
具体实施方式
本申请实施例公开的位姿确定方法及装置，用于测量相机的实时位姿。
本申请实施例公开的位姿确定装置可以应用在具有测量位姿功能的设备上，可以包括但不限于机器人和无人机。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
图1为本实施公开的一种位姿定位方法，包括以下步骤：
S101：获取图像帧序列中的N个图像帧作为待优化的图像帧。
本实施例中，预先设定滑动窗口，待优化的N个图像帧为滑动窗口在图像帧序列中滑动过程中，被滑动窗口覆盖的图像帧，即通过设定滑动窗口限定待优化的图像帧的数量，以减少优化的计算复杂程度。
可选的，滑动窗中的N个图像帧组成一个图像帧集合，该图像帧集合是实时更新的，即滑动窗口中的图像帧是实时更新的，相机每采集到新的图像帧的情况下，需从滑动窗口中删除一个图像帧，并将该新的图像帧加入到滑动窗口中，具体的更新滑动窗中的图像帧的过程可以参考图2所示的流程。
S102：获取目标函数。
本实施例中，目标函数用于优化N个图像帧中所有帧的变量，该变量可以是位姿，速度以及零偏。其中，目标函数包括第一项和第二项，第一项为滑动窗口中相邻帧间的光度测量残差，第二项为滑动窗口中相邻帧间的状态参数测量残差。其中，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到。
当目标函数包括第一项和第二项时，目标函数如公式(1)所示。
Ftotal＝Fphoto+Finertial (1)
其中，Fphoto为相邻帧间的光度测量残差，Finertial为相邻帧间的状态参数测量残差。获取相邻帧间的光度测量残差的具体过程可以参考图3所示的流程。
相邻帧间的状态参数测量残差是滑动窗口中相邻两帧的状态参数的差值。可以对在任意一帧图像的采集时刻采集的惯性测量数据(包括陀螺仪数据和加速度传感器数据)进行预积分，得到该帧图像对应的状态参数。
任意一帧的状态参数xi如公式(2)所示。
其中，i表示第i帧图像帧，Ri为旋转量，pi为平移量，vi为速度，为陀螺仪偏移，为加速度计偏移。
因此，两帧间的状态参数测量残差也包括上述参数，具体计算相邻帧间的状态参数测量残差的方法可以参考现有技术，此处不再赘述。
可选的，虽然删除了滑动窗口中的图像帧，但删除的图像帧的信息，可以保留，以进一步提高位姿估计结果的准确性。因此，目标函数还包括第三项，第三项为边缘化的先验信息，该边缘化的先验信息是滑动窗口中被删除的图像帧的位姿和状态参数。当目标函数还包括第三项时，目标函数如公式(3)所示：
Ftotal＝Fphoto+Finertial+Fmarginalization (3)
其中，Fmarginalization为边缘化的先验信息。
S103：最小化目标函数，得到位姿。
本实施例中，可以采用基于滑动窗口的非线性最小二乘法求解目标函数，其具体的求解过程可以参考现有技术。通过求解最小化目标函数，对待优化的N个图像帧进行优化求解，最终得到求解结果中即包括当前时刻的位姿。当前时刻为N个图像帧的采集时刻中最晚的时刻，因此，在使用图1所示的流程处理实时获得的图像帧的情况下，能够得到实时的位姿估计结果。
本实施例提供的方法，获取图像帧序列中的N个的图像帧作为待优化的图像帧，并通过最小化目标函数，对N个的图像帧进行优化求解，得到当前时刻的位姿。目标函数包括相邻帧间的光度测量残差项以及相邻帧间的状态参数测量残差项，其中，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到。本实施例中，采用像素点的光度误差代替现有技术中采用像素点的重投影误差作为优化的目标方程，避免了重投影误差中重投影后带来附加的几何位置的误差。并且，目标函数同时包含了相邻帧间的光度测量残差和相邻帧间的状态参数测量残差，邻帧间的状态参数测量残差项增加了相邻帧间的运动约束，使图像帧的位姿数据与惯性预积分数据相融合，使最小化目标函数更能准确的反应帧间像素点的运动变化，所以最小化目标函数得到位姿估计准确度较高。
并且，与现有技术中使用逐特征点的描述子匹配计算位姿的方式相比，因为直接使用了光度差，所以，能够减小运算量，从而提高计算的实时性。
图2为上述实施例中更新滑动窗口中的图像帧的一种实施方式，具体可以包括：
S201：采集到新的图像帧后，判断滑动窗口中的最后一帧是否为关键帧。
本实施例中，滑动窗口中的最后一帧为滑动窗口中的时间戳最晚的图像帧，即采集时刻最晚的图像帧。为了对滑动窗口进行实时更新，每来一帧新的图像帧后，判断滑动窗口中的最后一帧是否为关键帧。
可选的，判断滑动窗口中的最后一帧是否为关键帧可依据该最后一帧与新的图像帧的运动距离的大小进行判断，如果运动距离不大于预设的阈值，说明该最后一帧与新的图像帧的运动距离较短，则判断该最后一帧不是关键帧，反之，则判断该最后一帧为关键帧。预设的阈值可以依据需求和经验值设定。两帧之间的运动距离的计算方式可以参见现有技术，这里不再赘述。
S202：如果最后一帧为关键帧，则删除滑动窗口中的第一帧，并将新的图像帧加入到滑动窗口中。
本实施例中，滑动窗口中的第一帧为滑动窗口中的时间戳最早的图像帧，即滑动窗口中的采集时刻最早的图像帧。
S203：如果最后一帧不是关键帧，则删除滑动窗口中的最后一帧，并将新的图像帧加入到滑动窗口中。
本实施例中提供的方法，采用滑动窗口法控制待优化的状态量在一定的范围，对滑动窗口进行实时的更新，将需要进行优化的图像帧放入滑动窗口中，而不需要优化的图像帧从滑动窗口删除，保证了待优化的状态量的稳定性。
图3为上述实施例中获取目标函数中的相邻帧间的光度测量残差项的一种实施方式，具体可以包括：
S301：获取相邻帧中第一图像帧的目标像素点。
本实施例中，任意相邻两帧中第一图像帧的目标像素点为像素梯度值大于预设的像素梯度阈值的像素点。获取任意一图像帧的目标像素点的过程可以是：
在该图像划分出L个素梯度阈值的区域。并将图像分成M个图像块，计算M个图像块中每个图像块中像素点的像素梯度值，并将图像块中的像素点的梯度值分别与L个不同像素梯度阈值的区域中的每个区域进行比较，选出在该图像块中像素梯度值大于任意一个像素梯度阈值的像素点。
选出像素点后，判断选取到的像素点的总数量是否等于预设的总数，如果不是，再将图像分成K个图像块，并选出在K个图像块中大于预设的像素梯度阈值的像素点，直到选出的像素点的总数等于预设的总数。
可选的，可设为L＝32*32，M＝d*d，K＝2d*2d，其中d<32。
S302：获取目标像素点在相邻帧的第二图像帧中对应的像素点。
本实施例中，任意一个目标像素点对应的像素点为该目标像素点在第二图像帧中投影得到的像素点。具体的，将在第一图像帧获取到的目标点像素点反投影到3D空间，得到3D空间中的点，再将投影得到的3D空间中的点投影到第二图像帧，投影在第二图像帧的像素点即为目标像素点在第二图像帧中对应的像素点。查找对应像素点的具体实现过程，可以参见现有依据特征点和描述子的匹配方法，这里不再赘述。
S303：计算相邻帧间像素点间的光度误差。
本实施例中，任意两个相邻帧间的像素点的光度误差为相邻帧间目标像素点和与目标像素点对应的像素点之间的光度误差。在计算相邻帧间像素点间的光度误差前，需先计算像素点的光度，并设定误差系数。其中，像素点的光度计算可以参考现有技术。计算相邻帧间像素点间的光度误差如公式(4)所示。
其中，Ii[p]为第i个图像帧中像素点P的光度，Ij[p']为第j个图像帧像素点p'的光度，i,j为相邻两个图像帧的帧序号，p'是像素点P在第j个图像帧的逆深度投影点，p'可通过公式(5)计算得到，为误差系数，NP为像素块，像素块由中心像素点与其周围的像素点组成，ti是图像帧Ii曝光时间，tj是图像帧Ij曝光时间，i,j为相邻两个图像帧的帧序号，a和b都是光度标定参数，||·||γ是Huber范数。公式(5)表示如下：
其中，Πc()为像素点P的投影，为像素点P的反投影，c为相机参考系，R为旋转矩阵，t为平移矩阵，像素梯度权重wp的计算公式如公式(6)所示。
其中，为i帧图像的像素点P的像素梯度，c为常量系数，为2的范数。
S304：最小化光度误差，得到相邻帧间的光度测量残差。
本实施例中，对相邻帧间的光度误差进行最小化计算，得到相邻帧间的视觉残差，该相邻帧间的视觉残差即为目标函数的第一项。最小化的光度误差如公式(7)所示。
本实施例中，通过获取相邻帧间的像素点，计算相邻帧间像素点的光度误差，最后最小化光度误差得到相邻帧间的视觉残差，由于获取相邻帧间像素点是根据像素点的像素点的像素梯度值获取的，无需提取图像的特征点，从而可以提高优化的速度，另外，目标像素点对应的像素点是通过投影的方式确定的，避免了现有技术中采用匹配的方法确定图像特征点产生的匹配误差。同时，采用像素点的光度误差代替现有技术中采用像素点的重投影误差作为目标方程，避免了重投影误差中重投影后带来额外的几何位置的误差，提高了优化的准确度。
综上所述，本申请实施例提供的方法可通过图4公开的位姿确定的整体流程图实现。
S401：对滑动窗口中的图像帧进行数据预处理。
具体的数据预处理包括：
采用直接法对相机采集到的图像进行直接图像对齐，得到每帧图像的位姿。
对IMU采集得到的每帧图像对应(对应即采集时间相同)的惯性数据进行预积分计算，得到每帧图像对应的状态参数。
S402：对滑动窗口中的图像帧进行初始化计算。
本实施例中，对图像帧进行预处理得到的位姿是无尺度的相对运动的估计，相邻帧间的惯性预积分是有尺度的实际运动的估计。因为每个图像帧的位姿估计以及IMU预积分得到帧间的运动约束(即状态参数)都有各自的时间戳，所以可以根据时间戳对视觉数据与状态参数进行同步对齐，实现相机坐标系与IMU坐标系的统一，从而得到里程计的初始化参数，该初始化参数包括：陀螺仪偏移的估计、加速度计偏移的估计、尺度的估计、重力向量的修正以及初始位姿。其中，陀螺仪和加速度计为IMU中的组成部件。
S403：判断初始化的结果是否收敛。
本实施例中，判断初始化的计算是否收敛可以通过判断每相邻帧间的状态参数误差是否达到预设的误差精度，如果达到，则认为计算收敛。其中，状态参数可以是每相邻帧间的位姿参数。
通过检验初始化的计算是否收敛，提高初始化得到的初始化参数的准确性，从而最终提高了位姿估计的准确度。
S404：如果初始化的计算不收敛，更新滑动窗口的图像帧，再次进行初始化计算，直到初始化的计算收敛。
S405：如果收敛，判断滑动窗口中的最后一帧是否为关键帧。
S406：如果该最后一帧为关键帧，则将滑动窗口中的第一帧删除，并将最新的图像帧加入到滑动窗口中。
S407：如果该最后一帧不是关键帧，则将该最后一帧删除，并将最新的图像帧加入到滑动窗口中。
S408：通过最小化目标方程，优化滑动窗口中的所有帧的变量，得到当前时刻的位姿。
本实施例提供的方法，通过实施更新滑动窗口，并最小化目标方程以优化滑动窗口中所有图像帧的变量，最后得到当前时刻的位姿。因为目标方程包括了相邻帧间的光度误差、状态参数测量误差以及边缘化的先验信息，故得到的优化结果准确性高，即得到当前时刻的位姿准确性高。
与图1的方法相对应，本发明实施例还提供了位姿确定的装置，用于对图1方法的具体实现，其结构示意图如图5所示，具体包括：
获取单元501，用于获取图像帧序列中的N个图像帧；
优化单元502，用于最小化预设的目标函数，得到位姿；
其中，目标函数至少包括第一项与第二项，第一项为图像帧中相邻帧间的光度测量残差，第二项为图像帧中相邻帧间的状态参数测量残差，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到，光度误差至少依据该两帧的位姿确定，状态参数至少包括位姿。
其中，获取单元501，获取图像帧序列中的N个图像帧的具体实现方式为：在采集到图像帧的情况下，从待更新图像帧集合中删除目标帧，并将采集到的图像帧加入待更新图像帧集合，得到更新后的图像帧集合，更新后的图像帧集合中包括图像帧序列中的N个图像帧；其中，在待更新图像帧中的采集时刻最晚的帧为关键帧的情况下，目标帧为待更新图像帧中的采集时刻最早的帧，在待更新图像帧中的采集时刻最晚的帧不是关键帧的情况下，目标帧为待更新图像帧中的采集时刻最晚的帧。
其中，优化单元502在最小化目标函数的过程中所使用的目标函还包括第三项，第三项为边缘化的先验信息，边缘化的先验信息为目标帧的位姿以及状态参数。
其中，优化单元502在确定最小化目标函数的初始值的具体实现方式为：计算获得待更新图像帧集合中每个图像帧的位姿数据以及状态参数，依据图像帧的位姿数据以及状态参数进行对齐计算，得到初始化参数。
其中，优化单元502在最小化目标函数的过程中，获取目标函数的第一项过程的具体实现方式为：计算两帧间的像素点的光度误差，所述两帧间的像素点的光度误差依据相邻两帧中第一帧的像素点的光度、误差系数、以及所述第一帧中的像素点投影到第二帧得到的投影像素点的光度确定；其中，所述投影像素点依据所述第一帧的位姿确定；对所述两帧间的像素点的光度误差进行最小化计算，得到所述目标函数的第一项。
其中，优化单元502最小化目标函数中用于计算所使用的目标函的第二项的状态参数包括：旋转量、平移量、速度量、陀螺仪偏移以及加速度计偏移，旋转量和平移量构成位姿。
本发明实施例提供的装置，获取图像帧序列中的N个的图像帧作为待优化的图像帧，并通过最小化目标函数，对N个的图像帧进行优化求解，得到当前时刻的位姿。目标函数包括相邻帧间的光度测量残差项以及相邻帧间的状态参数测量残差项，其中，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到。本实施例中，采用像素点的光度误差代替现有技术中采用像素点的重投影误差作为优化的目标方程，避免了重投影误差中重投影后带来附加的几何位置的误差。并且，目标函数同时包含了相邻帧间的光度测量残差和相邻帧间的状态参数测量残差，邻帧间的状态参数测量残差项增加了相邻帧间的运动约束，使图像帧的位姿数据与惯性预积分数据相融合，使最小化目标函数更能准确的反应帧间像素点的运动变化，所以最小化目标函数得到位姿估计准确度较高。
本发明实施例还提供了一种视觉惯性里程计，包括处理器601和存储器602，存储器用于存储程序，处理器用于运行程序，以实现本发明实施例中位姿确定方法。
在本申请中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (13)

1.一种位姿确定方法，其特征在于，包括：
获取图像帧序列中的N个图像帧；
最小化预设的目标函数，得到位姿；
其中，所述目标函数至少包括第一项与第二项，所述第一项为所述图像帧中相邻帧间的光度测量残差，所述第二项为所述图像帧中相邻帧间的状态参数测量残差，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到，所述光度误差至少依据该两帧的位姿确定，所述状态参数至少包括位姿。
2.根据权利要求1所述的方法，其特征在于，所述获取图像帧序列中的N个图像帧包括：
获取待更新图像帧集合，所述待更新图像帧集合中包括N个图像帧；
在采集到图像帧的情况下，从所述待更新图像帧集合中删除目标帧，并将采集到的图像帧加入所述待更新图像帧集合，得到更新后的图像帧集合，所述更新后的图像帧集合中包括所述图像帧序列中的N个图像帧；
其中，在所述待更新图像帧中的采集时刻最晚的帧为关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最早的帧，在所述待更新图像帧中的采集时刻最晚的帧不是关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最晚的帧。
3.根据权利要求2所述的方法，其特征在于，所述目标函数还包括第三项，所述第三项为边缘化的先验信息，所述边缘化的先验信息为所述目标帧的位姿以及状态参数。
4.根据权利要求2所述的方法，其特征在于，最小化目标函数的初始值的确定过程，包括：
计算获得所述待更新图像帧集合中每个图像帧的位姿以及状态参数；
依据所述图像帧的位姿以及所述相邻帧间的状态参数进行对齐计算，得到所述目标函数的初始化参数。
5.根据权利要求1所述的方法，其特征在于，获取所述目标函数的第一项的过程，包括：
计算两帧间的像素点的光度误差，所述两帧间的像素点的光度误差依据相邻两帧中第一帧的像素点的光度、误差系数、以及所述第一帧中的像素点投影到第二帧得到的投影像素点的光度确定；其中，所述投影像素点依据所述第一帧的位姿确定；
对所述两帧间的像素点的光度误差进行最小化计算，得到所述目标函数的第一项。
6.根据权利要求1所述的方法，其特征在于，用于计算所述目标函数的第二项的状态参数包括：旋转量、平移量、速度量、陀螺仪偏移以及加速度计偏移，所述旋转量和平移量构成所述位姿。
7.一种位姿确定装置，其特征在于，包括：
获取单元，用于获取图像帧序列中的N个图像帧；
优化单元，用于最小化预设的目标函数，得到位姿；
其中，所述目标函数至少包括第一项与第二项，所述第一项为所述图像帧中相邻帧间的光度测量残差，所述第二项为所述图像帧中相邻帧间的状态参数测量残差，任意两个相邻帧间的光度测量残差通过最小化该两帧间的像素点的光度误差得到，所述光度误差至少依据该两帧的位姿确定，所述状态参数至少包括位姿。
8.根据权利要求7所述的装置，其特征在于，所述获取单元，用于获取图像帧序列中的N个图像帧，包括：
所述获取单元具体用于获取待更新图像帧集合，所述待更新图像帧集合中包括N个图像帧；在采集到图像帧的情况下，从所述待更新图像帧集合中删除目标帧，并将采集到的图像帧加入所述待更新图像帧集合，得到更新后的图像帧集合，所述更新后的图像帧集合中包括所述图像帧序列中的N个图像帧；其中，在所述待更新图像帧中的采集时刻最晚的帧为关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最早的帧，在所述待更新图像帧中的采集时刻最晚的帧不是关键帧的情况下，所述目标帧为所述待更新图像帧中的采集时刻最晚的帧。
9.根据权利要求8所述的装置，其特征在于，所述优化单元在最小化的目标函数的过程中，所述目标函数还包括第三项，所述第三项为边缘化的先验信息，所述边缘化的先验信息为所述目标帧的位姿以及状态参数。
10.根据权利要求8所述的装置，其特征在于，所述优化单元还用于：
计算获得所述待更新图像帧集合中每个图像帧的位姿以及状态参数；
依据所述图像帧的位姿以及所述状态参数进行对齐计算，得到所述目标函数的初始化参数。
11.根据权利要求8所述的装置，其特征在于，所述优化单元还用于：
计算两帧间的像素点的光度误差，所述两帧间的像素点的光度误差依据相邻两帧中第一帧的像素点的光度、误差系数、以及所述第一帧中的像素点投影到第二帧得到的投影像素点的光度确定；其中，所述投影像素点依据所述第一帧的位姿确定；对所述两帧间的像素点的光度误差进行最小化计算，得到所述目标函数的第一项。
12.根据权利要求7所述的装置，其特征在于，用于计算所述目标函数的第二项的状态参数包括：旋转量、平移量、速度量、陀螺仪偏移以及加速度计偏移，所述旋转量和平移量构成所述位姿。
13.一种视觉惯性里程计，其特征在于，包括处理器和存储器，所述存储器用于存储程序，所述处理器用于运行程序，以执行权利要求1-6中任一项所述的位姿确定方法。
CN201910510820.8A 2019-06-13 2019-06-13 位姿确定方法及装置、里程计 Active CN110260861B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910510820.8A CN110260861B (zh) 2019-06-13 2019-06-13 位姿确定方法及装置、里程计

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910510820.8A CN110260861B (zh) 2019-06-13 2019-06-13 位姿确定方法及装置、里程计

Publications (2)

Publication Number Publication Date
CN110260861A true CN110260861A (zh) 2019-09-20
CN110260861B CN110260861B (zh) 2021-07-27

Family

ID=67918030

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910510820.8A Active CN110260861B (zh) 2019-06-13 2019-06-13 位姿确定方法及装置、里程计

Country Status (1)

Country Link
CN (1) CN110260861B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111145255A (zh) * 2019-12-27 2020-05-12 浙江省北大信息技术高等研究院 一种结合深度学习和几何优化的位姿计算方法和系统
CN111829522A (zh) * 2020-07-02 2020-10-27 浙江大华技术股份有限公司 即时定位与地图构建方法、计算机设备以及装置
CN112241983A (zh) * 2020-10-19 2021-01-19 深圳市目心智能科技有限公司 一种基于主动双目视觉的感知系统及机器人
CN113723379A (zh) * 2021-11-02 2021-11-30 深圳市普渡科技有限公司 人工智能设备、视觉定位方法、装置及可读存储介质
CN114018236A (zh) * 2021-09-30 2022-02-08 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN113706633B (zh) * 2021-09-02 2023-08-29 广州小鹏自动驾驶科技有限公司 一种目标对象的三维信息确定方法和装置
CN117689711A (zh) * 2023-08-16 2024-03-12 荣耀终端有限公司 位姿测量方法和电子设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102109348A (zh) * 2009-12-25 2011-06-29 财团法人工业技术研究院 定位载体、估测载体姿态与建地图的系统与方法
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法
CN108564600A (zh) * 2018-04-19 2018-09-21 北京华捷艾米科技有限公司 运动物体姿态跟踪方法及装置
WO2018182524A1 (en) * 2017-03-29 2018-10-04 Agency For Science, Technology And Research Real time robust localization via visual inertial odometry
CN108827315A (zh) * 2018-08-17 2018-11-16 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN109764880A (zh) * 2019-02-19 2019-05-17 中国科学院自动化研究所 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102109348A (zh) * 2009-12-25 2011-06-29 财团法人工业技术研究院 定位载体、估测载体姿态与建地图的系统与方法
WO2018182524A1 (en) * 2017-03-29 2018-10-04 Agency For Science, Technology And Research Real time robust localization via visual inertial odometry
CN108036785A (zh) * 2017-11-24 2018-05-15 浙江大学 一种基于直接法与惯导融合的飞行器位姿估计方法
CN108564600A (zh) * 2018-04-19 2018-09-21 北京华捷艾米科技有限公司 运动物体姿态跟踪方法及装置
CN108827315A (zh) * 2018-08-17 2018-11-16 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN109764880A (zh) * 2019-02-19 2019-05-17 中国科学院自动化研究所 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
VON STUMBERG L , USENKO V , CREMERS D: "Direct Sparse Visual-Inertial Odometry using Dynamic Marginalization", 《2018 IEEE INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION,(ICRA), IEEE》 *

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111145255A (zh) * 2019-12-27 2020-05-12 浙江省北大信息技术高等研究院 一种结合深度学习和几何优化的位姿计算方法和系统
CN111145255B (zh) * 2019-12-27 2022-08-09 浙江省北大信息技术高等研究院 一种结合深度学习和几何优化的位姿计算方法和系统
CN111829522A (zh) * 2020-07-02 2020-10-27 浙江大华技术股份有限公司 即时定位与地图构建方法、计算机设备以及装置
CN111829522B (zh) * 2020-07-02 2022-07-12 浙江大华技术股份有限公司 即时定位与地图构建方法、计算机设备以及装置
CN112241983A (zh) * 2020-10-19 2021-01-19 深圳市目心智能科技有限公司 一种基于主动双目视觉的感知系统及机器人
CN113706633B (zh) * 2021-09-02 2023-08-29 广州小鹏自动驾驶科技有限公司 一种目标对象的三维信息确定方法和装置
CN114018236A (zh) * 2021-09-30 2022-02-08 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN114018236B (zh) * 2021-09-30 2023-11-03 哈尔滨工程大学 一种基于自适应因子图的激光视觉强耦合slam方法
CN113723379A (zh) * 2021-11-02 2021-11-30 深圳市普渡科技有限公司 人工智能设备、视觉定位方法、装置及可读存储介质
CN117689711A (zh) * 2023-08-16 2024-03-12 荣耀终端有限公司 位姿测量方法和电子设备

Also Published As

Publication number Publication date
CN110260861B (zh) 2021-07-27

Similar Documents

Publication Publication Date Title
CN110260861A (zh) 位姿确定方法及装置、里程计
CN106679648B (zh) 一种基于遗传算法的视觉惯性组合的slam方法
JP5987823B2 (ja) 画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム
CN109540126A (zh) 一种基于光流法的惯性视觉组合导航方法
JP6025830B2 (ja) ３ｄ走査用のセンサ位置決め
CN110118554A (zh) 基于视觉惯性的slam方法、装置、存储介质和设备
CN110617814A (zh) 单目视觉和惯性传感器融合的远距离测距系统及方法
CN107888828A (zh) 空间定位方法及装置、电子设备、以及存储介质
CN111750853B (zh) 一种地图建立方法、装置及存储介质
CN109461208B (zh) 三维地图处理方法、装置、介质和计算设备
KR20220028042A (ko) 포즈 결정 방법, 장치, 전자 기기, 저장 매체 및 프로그램
CN111156998A (zh) 一种基于rgb-d相机与imu信息融合的移动机器人定位方法
CN111091587B (zh) 一种基于视觉标志物的低成本动作捕捉方法
RU2572637C2 (ru) Параллельное или выполняемое последовательно в онлайн- и оффлайн- режимах формирование реконструкций для трехмерного обмера помещения
JP2015532077A (ja) 少なくとも１つの画像を撮影する撮影装置に関連する装置の位置及び方向の決定方法
CN110969665B (zh) 一种外参标定方法、装置、系统及机器人
CN110118556A (zh) 一种基于协方差交叉融合slam的机器人定位方法及装置
CN109520476B (zh) 基于惯性测量单元的后方交会动态位姿测量系统及方法
CN110375739A (zh) 一种移动端视觉融合定位方法、系统及电子设备
KR20120099952A (ko) 센서 시스템, 이를 이용하는 환경 지도 작성 시스템 및 방법
CN113820735A (zh) 位置信息的确定方法、位置测量设备、终端及存储介质
CN110243390A (zh) 位姿的确定方法、装置及里程计
TWM560099U (zh) 運用擴增實境技術之室內精確導航系統
CN116772844A (zh) 一种基于动态环境下的视觉惯性室内机器人的导航方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping

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