CN111354043A - 一种基于多传感器融合的三维姿态估计方法及装置 - Google Patents

一种基于多传感器融合的三维姿态估计方法及装置 Download PDF

Info

Publication number
CN111354043A
CN111354043A CN202010108437.2A CN202010108437A CN111354043A CN 111354043 A CN111354043 A CN 111354043A CN 202010108437 A CN202010108437 A CN 202010108437A CN 111354043 A CN111354043 A CN 111354043A
Authority
CN
China
Prior art keywords
objective function
depth image
point
imu
measured object
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.)
Pending
Application number
CN202010108437.2A
Other languages
English (en)
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.)
Jimei University
Original Assignee
Jimei University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jimei University filed Critical Jimei University
Priority to CN202010108437.2A priority Critical patent/CN111354043A/zh
Publication of CN111354043A publication Critical patent/CN111354043A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/269—Analysis of motion using gradient-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
    • G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
    • G06T7/337—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods involving reference images or patches
    • 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/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • 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/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Data Mining & Analysis (AREA)
  • Multimedia (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明提出了一种基于多传感器融合的三维姿态估计方法及装置，该方法包括：获取被测对象的RGB图像，并提取FAST特征点以及进行特征点的跟踪；采集被测对象的IMU测量值，并积分得到当前时刻的被测对象的位置、速度和旋转；获取被测对象的深度图像，对所述深度图像进行处理得到迭代最近点；基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。本方法基于多种传感器进行姿态估计，并构建了用于姿态估计的目标函数，该目标函数使用最小二乘法进行求解，该函数的输入值通过滑动窗口进行输入，设置了滑动窗口的具体数目，降低求解时间，提高了位姿的估计精度。

Description

一种基于多传感器融合的三维姿态估计方法及装置
技术领域
本发明涉及三维重建技术领域，具体涉及一种基于多传感器融合的三维姿态估计方法及装置。
背景技术
姿态估计问题就是确定某一三维目标物体的方位指向问题。姿态估计在机器人视觉、动作跟踪和单照相机定标等很多领域都有应用。在不同领域用于姿态估计的传感器是不一样的。
基于模型的方法通常利用物体的几何关系或者物体的特征点来估计。其基本思想是利用某种几何模型或结构来表示物体的结构和形状，并通过提取某些物体特征，在模型和图像之间建立起对应关系，然后通过几何或者其它方法实现物体空间姿态的估计。这里所使用的模型既可能是简单的几何形体，如平面、圆柱，也可能是某种几何结构，也可能是通过激光扫描或其它方法获得的三维模型。基于模型的姿态估计方法是通过比对真实图像和合成图像，进行相似度计算更新物体姿态。目前基于模型的方法为了避免在全局状态空间中进行优化搜索，一般都将优化问题先降解成多个局部特征的匹配问题，非常依赖于局部特征的准确检测。当噪声较大无法提取准确的局部特征的时候，该方法的鲁棒性受到很大影响。
基于学习的方法借助于机器学习(machine learning)方法，从事先获取的不同姿态下的训练样本中学习二维观测与三维姿态之间的对应关系，并将学习得到的决策规则或回归函数应用于样本，所得结果作为对样本的姿态估计。基于学习的方法一般采用全局观测特征，不需检测或识别物体的局部特征，具有较好的鲁棒性。其缺点是由于无法获取在高维空间中进行连续估计所需要的密集采样，因此无法保证姿态估计的精度与连续性。
发明内容
本发明针对上述现有技术中的缺陷，提出了如下技术方案。
一种基于多传感器融合的三维姿态估计方法，该方法包括：
RGB图像获取步骤，使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；
IMU测量值获取步骤，使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；
深度图像获取步骤，使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；
目标函数构建步骤，基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；
位姿估计步骤，使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。
更进一步地，所述方法还包括：回环检测步骤，检测当前帧是否发生回环，如果是，则执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差。
更进一步地，所述目标函数为：
min(R,t){PP+∑||D||2+∑||I||2+∑||V||2}；
通过最小二乘法求得一个最合适的运动参数：旋转参数
Figure BDA0002389161120000031
和位移参数
Figure BDA0002389161120000032
使四个残差项PP、∑||D||2、∑||I||2与∑||V||2的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差。
更进一步地，基于滑动窗口求解该最小二乘，滑动窗口内包含最小二乘法计算所需要的输入参数，每次最小二乘的除先验信息之外的误差项都由滑动窗口内的值提供。
更进一步地，基于以下条件确定RGB图像中的关键帧：
该帧的平均视差与上一帧关键帧的平均视差相比差值超过第一阈值；
或者，该帧跟踪的特征点数量小于第二阈值。
更进一步地，通过深度图像得到空间的几何结构，从而估计两帧之间的变换矩阵，迭代最近点(ICP)残差项D使用点到面的距离为：
D＝(T·Pj-Pi)·ni
Figure BDA0002389161120000033
其中，Pj和Pi是上述深度图像中已匹配的三维点，ni是上一帧对应的法向量，旋转参数
Figure BDA0002389161120000041
和位移参数
Figure BDA0002389161120000042
是两帧之间的运动参数；
重投影误差V作为视觉约束，并将其定义在单位球面的正切平面上：
Figure BDA0002389161120000043
其中，
Figure BDA0002389161120000044
是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，
Figure BDA0002389161120000045
和
Figure BDA0002389161120000046
是P对应的切平面的任意两个正交基。
本发明还提出了一种基于多传感器融合的三维姿态估计装置，该装置包括：
RGB图像获取单元，用于使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；
IMU测量值获取单元，用于使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；
深度图像获取单元，用于使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；
目标函数构建单元，用于基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；
位姿估计单元，用于使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。
更进一步地，回环检测单元，用于检测当前帧是否发生回环，如果是，则执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差。
更进一步地，所述目标函数为：
min(R,t){PP+∑||D||2+∑||I||2+∑||V||2}；
通过最小二乘法求得一个最合适的运动参数：旋转参数
Figure BDA0002389161120000051
和位移参数
Figure BDA0002389161120000052
使四个残差项PP、∑||D||2、∑||I||2与∑||V||2的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差。
更进一步地，基于滑动窗口求解该最小二乘，滑动窗口内包含最小二乘法计算所需要的输入参数，每次最小二乘的除先验信息之外的误差项都由滑动窗口内的值提供。
更进一步地，基于以下条件确定RGB图像中的关键帧：
该帧的平均视差与上一帧关键帧的平均视差相比差值超过第一阈值；
或者，该帧跟踪的特征点数量小于第二阈值。
更进一步地，通过深度图像得到空间的几何结构，从而估计两帧之间的变换矩阵，迭代最近点(ICP)残差项D使用点到面的距离为：
D＝(T·Pj-Pi)·ni
Figure BDA0002389161120000053
其中，Pj和Pi是上述深度图像中已匹配的三维点，ni是上一帧对应的法向量，旋转参数
Figure BDA0002389161120000054
和位移参数
Figure BDA0002389161120000055
是两帧之间的运动参数；
重投影误差V作为视觉约束，并将其定义在单位球面的正切平面上：
Figure BDA0002389161120000061
其中，
Figure BDA0002389161120000062
是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，
Figure BDA0002389161120000063
和
Figure BDA0002389161120000064
是P对应的切平面的任意两个正交基。
本发明的技术效果在于：本发明的一种基于多传感器融合的三维姿态估计方法，该方法包括：使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。本方法基于多种传感器采集运动对象的RGB图像、IMU测量值及深度图像进行姿态估计，并构建了用于姿态估计的目标函数，该目标函数使用最小二乘法进行求解，该函数的输入值通过滑动窗口进行输入，设置了滑动窗口的具体数目，降低求解时间，提高了位姿的估计精度，还设计了回环检测过程，在发生回环时执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差，并具体设计相应的残差函数。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显。
图1是根据本发明的实施例的一种基于多传感器融合的三维姿态估计方法的流程图。
图2是根据本发明的实施例的一种基于多传感器融合的三维姿态估计装置的结构图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
图1示出了本发明的一种基于多传感器融合的三维姿态估计方法，该方法包括：
RGB图像获取步骤S101，使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；所述图像传感器可以是摄像头、摄像机等等，其可以采集被测对象的多帧图像。
每当一帧新的RGB图像到来时，***将自动提取FAST特征点，然后使用KLT稀疏光流法进行特征点的跟踪。为了保证跟踪的准确性又能保持比较小的计算代价，可以设置特征点跟踪的数量范围在100到300之间，在进行特征点跟踪的同时会检测新的特征点以维持跟踪特征点数量始终保持在一个阈值内。
IMU测量值获取步骤S102，使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；IMU传感器又称为惯性传感器，通过对离散的IMU测量值进行积分就可以得到当前时刻的位置、速度和旋转，可以给出当前搭载传感器的对象进行运动估计。
深度图像获取步骤S103，使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；所述深度图像传感器可以是深度摄像机、深度摄像头，通过深度图像可以得到被测对象空间的几何结构，从而估计两帧之间的变换矩阵。
此外，***对于场景真实尺度的估计也通过深度图像完成。深度图像可以提供像素点相对于相机光心(Pinhole)的真实距离，由此便可通过针孔相机模型进行像素坐标和空间坐标的转化，从而恢复场景的真实尺度。
在S102中IMU测量值的获取频率虽高于S103中深度图像的获取频率，而通常情况下，由于IMU数据和图像数据需要对齐，在相邻两帧深度图像间隔的时间差内累计的IMU测量值不足以积分要求。本发明在连续四帧的图像采集间隔内积累IMU测量值，以避免由于IMU测量数据不够而导致的积分结果波动。而相隔四帧的深度图像处理得到迭代最近点具有更大的误差，本发明保留了相邻帧的迭代最近点计算结果，将连续四帧的迭代最近点计算结果进行合并作为相邻4帧的迭代最近点结果。
目标函数构建步骤S104，基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数。由于每种传感器都有其不适用的环境，但运动过程中的环境是无法人为控制的。为了提高在多场景下的鲁棒性，得到更精确的定位，本发明采用多传感器的进行数据的融合，提高位姿估计的精确度及鲁棒性。
本发明以紧耦合(Tightly-coupled)的方式综合利用RGB图像、深度图像、IMU测量值等三种传感器信息估计两帧之间最优的姿态估计，通过迭代的方式解决这个最小二乘问题。
具体地，所述目标函数为：
min(R,t){PP+∑||D||2+∑||I||2+∑||V||2}；
通过最小二乘法求得一个最合适的运动参数：旋转参数
Figure BDA0002389161120000091
和位移参数
Figure BDA0002389161120000092
使四个残差项PP、∑||D||2、∑||I||2与∑||V||2的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差，即PP、D、I、V也可以称为目标函数的四个约束，也就是四个残差项。
滑动窗口外的状态也能提供一定的约束信息，为了能够得到更真实的约束关系，所以滑动窗口外的状态也被考虑进来(但它不被优化)，***使用边缘化的技巧，将它的约束信息转化为待优化变量的先验分布，即最小二乘问题中的PP。
对于通过深度图像得到空间的几何结构，从而估计两帧之间的变换矩阵，迭代最近点(ICP)残差项D使用点到面的距离为：
D＝(T·Pj-Pi)·ni
Figure BDA0002389161120000101
其中，Pj和Pi是上述深度图像中已匹配的三维点，ni是上一帧对应的法向量，旋转参数
Figure BDA0002389161120000102
和位移参数
Figure BDA0002389161120000103
是两帧之间的运动参数。
根据迭代最近点的计算特征，***采用了GPU来提高迭代最近点的计算效率。因为两帧之间的深度图像相似度较高，为了避免不必要的迭代，所以限制迭代次数最高为5次。
对于IMU约束I，给定两帧的IMU测量值(加速度和角速度)，IMU预积分可以计算出位置、速度和旋转。所以***将IMU误差项定义为两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差。
对于重投影误差V，作为视觉约束，并将其定义在单位球面的正切平面上：
Figure BDA0002389161120000104
其中，
Figure BDA0002389161120000105
是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，
Figure BDA0002389161120000106
和
Figure BDA0002389161120000107
是P对应的切平面的任意两个正交基。对于投影与反投影
Figure BDA0002389161120000108
是根据针孔相机模型推导的从三维空间点投影到二维空间点的投影函数，定义为：
Figure BDA0002389161120000111
其中，(fx,fy)和(cx,cy)是相机内参，可以通过棋盘格标定法等相机内参标定方法得到。投影的逆操作就是反投影。
位姿估计步骤S105，使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。
将所有残差项加在一起便构成了目标函数，使用迭代法最小化目标函数就可以得到最优的位姿。迭代方向由目标函数的一阶导决定。为了保证***的实时性，求解位姿所用的最大迭代次数设置为8次。
在一个实施例中，在实施本方法前，通过基于滑动窗口的运动恢复结构(SfM)和视觉惯性矫正模块来粗略估计***所依赖的状态(重力向量、陀螺仪偏差和速度等)，为本方法提供初始值。
在一个实施例中，所述方法还包括：回环检测步骤S106，检测当前帧是否发生回环，如果是，则执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差。本方法采用基于词袋模型的回环检测，如果当前帧被认定为是一个关键帧，那么***会根据字典把RGB图像转换成一个向量。接着把这个向量提交给回环检测，回环检测判断当前帧是否发生回环并返回结果。如果检测到发生了回环，那么将会执行全局的位姿图(Pose-Graph)优化来矫正定位轨迹，消除累积误差。
在一个实施例中，基于滑动窗口求解该最小二乘，滑动窗口内包含最小二乘法计算所需要的输入参数，每次最小二乘的除先验信息之外的误差项都由滑动窗口内的值提供。即本发明采用了基于滑动窗口的形式实现该最小二乘问题。滑动窗口内包含最小二乘所需要的所有输入(像素点、三维空间点、状态等)，每次最小二乘的误差项都由滑动窗口内的值提供(除了先验信息)。为了限制该最小二乘问题的求解时间，将滑动窗口的数量设置为10个，已足够得出精确的两帧之间的位姿(也可以称为姿势)估计。
在一个实施例中，基于以下条件确定RGB图像中的关键帧：该帧的平均视差与上一帧关键帧的平均视差相比差值超过第一阈值(比如，第一阈值设置为1.0，在大部分场景下定位性能良好)；或者，该帧跟踪的特征点数量小于第二阈值，比如，第二阈值设置为10，既保证***的高鲁棒性又不至于产生过多的关键帧。
本方法基于多种传感器采集运动对象的RGB图像、IMU测量值及深度图像进行姿态估计，并构建了用于姿态估计的目标函数，该目标函数使用最小二乘法进行求解，该函数的输入值通过滑动窗口进行输入，设置了滑动窗口的具体数目，降低求解时间，提高了位姿的估计精度，还设计了回环检测过程，在发生回环时执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差，并具体设计相应的残差函数。
图2示出了本发明的本发明的一种基于多传感器融合的三维姿态估计装置，该装置包括：
RGB图像获取单元201，用于使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；所述图像传感器可以是摄像头、摄像机等等，其可以采集被测对象的多帧图像。
每当一帧新的RGB图像到来时，***将自动提取FAST特征点，然后使用KLT稀疏光流法进行特征点的跟踪。为了保证跟踪的准确性又能保持比较小的计算代价，可以设置特征点跟踪的数量范围在100到300之间，在进行特征点跟踪的同时会检测新的特征点以维持跟踪特征点数量始终保持在一个阈值内。
IMU测量值获取单元202，用于使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；IMU传感器又称为惯性传感器，通过对离散的IMU测量值进行积分就可以得到当前时刻的位置、速度和旋转，可以给出当前搭载传感器的对象进行运动估计。
深度图像获取单元203，用于使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；所述深度图像传感器可以是深度摄像机、深度摄像头，通过深度图像可以得到被测对象空间的几何结构，从而估计两帧之间的变换矩阵。
目标函数构建单元204，用于基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数。由于每种传感器都有其不适用的环境，但运动过程中的环境是无法人为控制的。为了提高在多场景下的鲁棒性，得到更精确的定位，本发明采用多传感器的进行数据的融合，提高位姿估计的精确度及鲁棒性。
本发明以紧耦合(Tightly-coupled)的方式综合利用RGB图像、深度图像、IMU测量值等三种传感器信息估计两帧之间最优的姿态估计，通过迭代的方式解决这个最小二乘问题。
具体地，所述目标函数为：
min(R,t){PP+∑||D||2+∑||I||2+∑||V||2}；
通过最小二乘法求得一个最合适的运动参数：旋转参数
Figure BDA0002389161120000141
和位移参数
Figure BDA0002389161120000142
使四个残差项PP、∑||D||2、∑||I||2与∑||V||2的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差，即PP、D、I、V也可以称为目标函数的四个约束，也就是四个残差项。
滑动窗口外的状态也能提供一定的约束信息，为了能够得到更真实的约束关系，所以滑动窗口外的状态也被考虑进来(但它不被优化)，***使用边缘化的技巧，将它的约束信息转化为待优化变量的先验分布，即最小二乘问题中的PP。
对于通过深度图像得到空间的几何结构，从而估计两帧之间的变换矩阵，迭代最近点(ICP)残差项D使用点到面的距离为：
D＝(T·Pj-Pi)·ni
Figure BDA0002389161120000151
其中，Pj和Pi是上述深度图像中已匹配的三维点，ni是上一帧对应的法向量，旋转参数
Figure BDA0002389161120000152
和位移参数
Figure BDA0002389161120000153
是两帧之间的运动参数。
根据迭代最近点的计算特征，***采用了GPU来提高迭代最近点的计算效率。因为两帧之间的深度图像相似度较高，为了避免不必要的迭代，所以限制迭代次数最高为5次。
对于IMU约束I，给定两帧的IMU测量值(加速度和角速度)，IMU预积分可以计算出位置、速度和旋转。所以***将IMU误差项定义为两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差。
对于重投影误差V，作为视觉约束，并将其定义在单位球面的正切平面上：
Figure BDA0002389161120000154
其中，
Figure BDA0002389161120000155
是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，
Figure BDA0002389161120000156
和
Figure BDA0002389161120000157
是P对应的切平面的任意两个正交基。对于投影与反投影
Figure BDA0002389161120000158
是根据针孔相机模型推导的从三维空间点投影到二维空间点的投影函数，定义为：
Figure BDA0002389161120000159
其中，(fx,fy)和(cx,cy)是相机内参，可以通过棋盘格标定法等相机内参标定方法得到。投影的逆操作就是反投影。
位姿估计单元205，用于使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。
将所有残差项加在一起便构成了目标函数，使用迭代法最小化目标函数就可以得到最优的位姿。迭代方向由目标函数的一阶导决定。为了保证***的实时性，求解位姿所用的最大迭代次数设置为8次。
在一个实施例中，在实施本装置前，通过基于滑动窗口的运动恢复结构(SfM)和视觉惯性矫正模块来粗略估计***所依赖的状态(重力向量、陀螺仪偏差和速度等)，为本装置提供初始值。
在一个实施例中，所述装置还包括：回环检测单元206，用于检测当前帧是否发生回环，如果是，则执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差。本装置采用基于词袋模型的回环检测，如果当前帧被认定为是一个关键帧，那么***会根据字典把RGB图像转换成一个向量。接着把这个向量提交给回环检测，回环检测判断当前帧是否发生回环并返回结果。如果检测到发生了回环，那么将会执行全局的位姿图(Pose-Graph)优化来矫正定位轨迹，消除累积误差。
在一个实施例中，基于滑动窗口求解该最小二乘，滑动窗口内包含最小二乘法计算所需要的输入参数，每次最小二乘的除先验信息之外的误差项都由滑动窗口内的值提供。即本发明采用了基于滑动窗口的形式实现该最小二乘问题。滑动窗口内包含最小二乘所需要的所有输入(像素点、三维空间点、状态等)，每次最小二乘的误差项都由滑动窗口内的值提供(除了先验信息)。为了限制该最小二乘问题的求解时间，将滑动窗口的数量设置为10个，已足够得出精确的两帧之间的位姿(也可以称为姿势)估计。
在一个实施例中，基于以下条件确定RGB图像中的关键帧：该帧的平均视差与上一帧关键帧的平均视差相比差值超过第一阈值(比如，第一阈值设置为1.0，在大部分场景下定位性能良好)；或者，该帧跟踪的特征点数量小于第二阈值，比如，第二阈值设置为10，既保证***的高鲁棒性又不至于产生过多的关键帧。
本装置基于多种传感器采集运动对象的RGB图像、IMU测量值及深度图像进行姿态估计，并构建了用于姿态估计的目标函数，该目标函数使用最小二乘法进行求解，该函数的输入值通过滑动窗口进行输入，设置了滑动窗口的具体数目，降低求解时间，提高了位姿的估计精度，还设计了回环检测过程，在发生回环时执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差，并具体设计相应的残差函数。
为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本申请时可以把各单元的功能在同一个或多个软件和/或硬件中实现。
通过以上的实施方式的描述可知，本领域的技术人员可以清楚地了解到本申请可借助软件加必需的通用硬件平台的方式来实现。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例或者实施例的某些部分所述的装置。
最后所应说明的是：以上实施例仅以说明而非限制本发明的技术方案，尽管参照上述实施例对本发明进行了详细说明，本领域的普通技术人员应当理解：依然可以对本发明进行修改或者等同替换，而不脱离本发明的精神和范围的任何修改或局部替换，其均应涵盖在本发明的权利要求范围当中。

Claims (12)

1.一种基于多传感器融合的三维姿态估计方法，其特征在于，该方法包括：
RGB图像获取步骤，使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；
IMU测量值获取步骤，使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；
深度图像获取步骤，使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；
目标函数构建步骤，基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；
位姿估计步骤，使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
回环检测步骤，检测当前帧是否发生回环，如果是，则执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差。
3.根据权利要求2所述的方法，其特征在于，所述目标函数为：
min(R，t){PP+∑||D||2+∑||I||2+∑||V||2}；
通过最小二乘法求得一个最合适的运动参数：旋转参数
Figure FDA0002389161110000011
和位移参数
Figure FDA0002389161110000012
使四个残差项PP、∑||D||2、∑||I||2与∑||V||2的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差。
4.根据权利要求3所述的方法，其特征在于，基于滑动窗口求解该最小二乘，滑动窗口内包含最小二乘法计算所需要的输入参数，每次最小二乘的除先验信息之外的误差项都由滑动窗口内的值提供。
5.根据权利要求4所述的方法，其特征在于，基于以下条件确定RGB图像中的关键帧：
该帧的平均视差与上一帧关键帧的平均视差相比差值超过第一阈值；
或者，该帧跟踪的特征点数量小于第二阈值。
6.根据权利要求3所述的方法，其特征在于，
通过深度图像得到空间的几何结构，从而估计两帧之间的变换矩阵，迭代最近点(ICP)残差项D使用点到面的距离为：
D＝(T·Pj-Pi)·ni
Figure FDA0002389161110000021
其中，Pj和Pi是上述深度图像中已匹配的三维点，ni是上一帧对应的法向量，旋转参数
Figure FDA0002389161110000022
和位移参数
Figure FDA0002389161110000023
是两帧之间的运动参数；
重投影误差V作为视觉约束，并将其定义在单位球面的正切平面上：
Figure FDA0002389161110000024
其中，
Figure FDA0002389161110000025
是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，
Figure FDA0002389161110000031
和
Figure FDA0002389161110000032
是P对应的切平面的任意两个正交基。
7.一种基于多传感器融合的三维姿态估计装置，其特征在于，该装置包括：
RGB图像获取单元，用于使用图像传感器获取被测对象的RGB图像，并提取所述RGB图像的FAST特征点以及使用KLT稀疏光流法进行特征点的跟踪；
IMU测量值获取单元，用于使用IMU传感器采集被测对象的IMU测量值，并对所述IMU测量值进行积分得到当前时刻的被测对象的位置、速度和旋转；
深度图像获取单元，用于使用深度图像传感器获取被测对象的深度图像，使用迭代最近点(ICP)算法对所述深度图像进行处理得到迭代最近点；
目标函数构建单元，用于基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；
位姿估计单元，用于使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。
8.根据权利要求7所述的装置，其特征在于，所述装置还包括：
回环检测单元，用于检测当前帧是否发生回环，如果是，则执行全局的位姿图优化来矫正所述对象的定位轨迹，以消除累积误差。
9.根据权利要求8所述的方法，其特征在于，所述目标函数为：
min(R，t){PP+∑||D||2+∑||I||2+∑||V||2}；
通过最小二乘法求得一个最合适的运动参数：旋转参数
Figure FDA0002389161110000033
和位移参数
Figure FDA0002389161110000041
使四个残差项PP、∑||D||2、∑||I||2与∑||V||2的和的值最低，其中，PP表示先验信息的先验分布、D表示迭代最近点残差项、I表示所述对象的两帧之间位置、速度、旋转、陀螺仪偏差和加速度偏差的变化量的差、V表示重投影误差。
10.根据权利要求9所述的方法，其特征在于，基于滑动窗口求解该最小二乘，滑动窗口内包含最小二乘法计算所需要的输入参数，每次最小二乘的除先验信息之外的误差项都由滑动窗口内的值提供。
11.根据权利要求10所述的方法，其特征在于，基于以下条件确定RGB图像中的关键帧：
该帧的平均视差与上一帧关键帧的平均视差相比差值超过第一阈值；
或者，该帧跟踪的特征点数量小于第二阈值。
12.根据权利要求9所述的方法，其特征在于，
通过深度图像得到空间的几何结构，从而估计两帧之间的变换矩阵，迭代最近点(ICP)残差项D使用点到面的距离为：
D＝(T·Pj-Pi)·ni
Figure FDA0002389161110000042
其中，Pj和Pi是上述深度图像中已匹配的三维点，ni是上一帧对应的法向量，旋转参数
Figure FDA0002389161110000043
和位移参数
Figure FDA0002389161110000044
是两帧之间的运动参数；
重投影误差V作为视觉约束，并将其定义在单位球面的正切平面上：
Figure FDA0002389161110000051
其中，
Figure FDA0002389161110000052
是特征点经过刚体变换后三维点的单位向量的预测值，P是特征点由像素坐标经针孔相机模型反投影得到的三维点，
Figure FDA0002389161110000053
和
Figure FDA0002389161110000054
是P对应的切平面的任意两个正交基。
CN202010108437.2A 2020-02-21 2020-02-21 一种基于多传感器融合的三维姿态估计方法及装置 Pending CN111354043A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010108437.2A CN111354043A (zh) 2020-02-21 2020-02-21 一种基于多传感器融合的三维姿态估计方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010108437.2A CN111354043A (zh) 2020-02-21 2020-02-21 一种基于多传感器融合的三维姿态估计方法及装置

Publications (1)

Publication Number Publication Date
CN111354043A true CN111354043A (zh) 2020-06-30

Family

ID=71194092

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010108437.2A Pending CN111354043A (zh) 2020-02-21 2020-02-21 一种基于多传感器融合的三维姿态估计方法及装置

Country Status (1)

Country Link
CN (1) CN111354043A (zh)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112097768A (zh) * 2020-11-17 2020-12-18 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN112230242A (zh) * 2020-09-30 2021-01-15 深兰人工智能(深圳)有限公司 位姿估计***和方法
CN112734765A (zh) * 2020-12-03 2021-04-30 华南理工大学 基于实例分割与多传感器融合的移动机器人定位方法、***及介质
CN113436254A (zh) * 2021-06-29 2021-09-24 杭州电子科技大学 一种级联解耦的位姿估计方法
CN113487674A (zh) * 2021-07-12 2021-10-08 北京未来天远科技开发有限公司 一种人***姿估计***和方法
CN113610149A (zh) * 2021-08-05 2021-11-05 上海氢枫能源技术有限公司 氢气压缩机的位姿实时显示方法及***
CN114554030A (zh) * 2020-11-20 2022-05-27 空客(北京)工程技术中心有限公司 设备检测***以及设备检测方法
CN114608569A (zh) * 2022-02-22 2022-06-10 杭州国辰机器人科技有限公司 三维位姿估计方法、***、计算机设备及存储介质
CN115131404A (zh) * 2022-07-01 2022-09-30 上海人工智能创新中心 基于运动估计深度的单目3d检测方法
CN116310083A (zh) * 2023-02-03 2023-06-23 杭州百世伽信息科技有限公司 人体姿态深度图像的生成方法与人体姿态识别***
CN116912948A (zh) * 2023-09-12 2023-10-20 南京硅基智能科技有限公司 一种数字人的训练方法、***及驱动***

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106934827A (zh) * 2015-12-31 2017-07-07 杭州华为数字技术有限公司 三维场景的重建方法和装置
US20170278231A1 (en) * 2016-03-25 2017-09-28 Samsung Electronics Co., Ltd. Device for and method of determining a pose of a camera
CN108876897A (zh) * 2018-04-20 2018-11-23 杭州电子科技大学 快速运动下的场景三维重建方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106934827A (zh) * 2015-12-31 2017-07-07 杭州华为数字技术有限公司 三维场景的重建方法和装置
US20170278231A1 (en) * 2016-03-25 2017-09-28 Samsung Electronics Co., Ltd. Device for and method of determining a pose of a camera
CN108876897A (zh) * 2018-04-20 2018-11-23 杭州电子科技大学 快速运动下的场景三维重建方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Cited By (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112230242A (zh) * 2020-09-30 2021-01-15 深兰人工智能(深圳)有限公司 位姿估计***和方法
CN112230242B (zh) * 2020-09-30 2023-04-25 深兰人工智能(深圳)有限公司 位姿估计***和方法
CN112097768A (zh) * 2020-11-17 2020-12-18 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN112097768B (zh) * 2020-11-17 2021-03-02 深圳市优必选科技股份有限公司 机器人位姿的确定方法、装置、机器人及存储介质
CN114554030B (zh) * 2020-11-20 2023-04-07 空客(北京)工程技术中心有限公司 设备检测***以及设备检测方法
CN114554030A (zh) * 2020-11-20 2022-05-27 空客(北京)工程技术中心有限公司 设备检测***以及设备检测方法
CN112734765A (zh) * 2020-12-03 2021-04-30 华南理工大学 基于实例分割与多传感器融合的移动机器人定位方法、***及介质
CN112734765B (zh) * 2020-12-03 2023-08-22 华南理工大学 基于实例分割与多传感器融合的移动机器人定位方法、***及介质
CN113436254A (zh) * 2021-06-29 2021-09-24 杭州电子科技大学 一种级联解耦的位姿估计方法
CN113436254B (zh) * 2021-06-29 2022-07-05 杭州电子科技大学 一种级联解耦的位姿估计方法
CN113487674A (zh) * 2021-07-12 2021-10-08 北京未来天远科技开发有限公司 一种人***姿估计***和方法
CN113487674B (zh) * 2021-07-12 2024-03-08 未来元宇数字科技(北京)有限公司 一种人***姿估计***和方法
CN113610149A (zh) * 2021-08-05 2021-11-05 上海氢枫能源技术有限公司 氢气压缩机的位姿实时显示方法及***
CN113610149B (zh) * 2021-08-05 2024-03-26 上海氢枫能源技术有限公司 氢气压缩机的位姿实时显示方法及***
CN114608569A (zh) * 2022-02-22 2022-06-10 杭州国辰机器人科技有限公司 三维位姿估计方法、***、计算机设备及存储介质
CN114608569B (zh) * 2022-02-22 2024-03-01 杭州国辰机器人科技有限公司 三维位姿估计方法、***、计算机设备及存储介质
CN115131404A (zh) * 2022-07-01 2022-09-30 上海人工智能创新中心 基于运动估计深度的单目3d检测方法
CN116310083B (zh) * 2023-02-03 2023-11-14 杭州百世伽信息科技有限公司 人体姿态深度图像的生成方法与人体姿态识别***
CN116310083A (zh) * 2023-02-03 2023-06-23 杭州百世伽信息科技有限公司 人体姿态深度图像的生成方法与人体姿态识别***
CN116912948B (zh) * 2023-09-12 2023-12-01 南京硅基智能科技有限公司 一种数字人的训练方法、***及驱动***
CN116912948A (zh) * 2023-09-12 2023-10-20 南京硅基智能科技有限公司 一种数字人的训练方法、***及驱动***

Similar Documents

Publication Publication Date Title
CN111354043A (zh) 一种基于多传感器融合的三维姿态估计方法及装置
Teed et al. Droid-slam: Deep visual slam for monocular, stereo, and rgb-d cameras
Jiao et al. Robust odometry and mapping for multi-lidar systems with online extrinsic calibration
Qin et al. Vins-mono: A robust and versatile monocular visual-inertial state estimator
US11138742B2 (en) Event-based feature tracking
US10225473B2 (en) Threshold determination in a RANSAC algorithm
EP1679657B1 (en) Estimation system, estimation method, and estimation program for estimating object state
CN101398934B (zh) 对图像中的对象进行定位的方法和***
EP2572319B1 (fr) Procede et systeme pour fusionner des donnees issues de capteurs d'images et de capteurs de mouvement ou de position
CN108051002A (zh) 基于惯性测量辅助视觉的运输车空间定位方法及***
CN108229416B (zh) 基于语义分割技术的机器人slam方法
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
US20070237359A1 (en) Method and apparatus for adaptive mean shift tracking
CN111932674A (zh) 一种线激光视觉惯性***的优化方法
Michot et al. Bi-objective bundle adjustment with application to multi-sensor slam
EP3633617A2 (en) Image processing device
Roberts et al. Learning general optical flow subspaces for egomotion estimation and detection of motion anomalies
US10229508B2 (en) Dynamic particle filter parameterization
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及***
CN114693754A (zh) 一种基于单目视觉惯导融合的无人机自主定位方法与***
White et al. An iterative pose estimation algorithm based on epipolar geometry with application to multi-target tracking
CN112179373A (zh) 一种视觉里程计的测量方法及视觉里程计
Dang et al. Stereo calibration in vehicles
Schill et al. Estimating ego-motion in panoramic image sequences with inertial measurements
Jiang et al. Icp stereo visual odometry for wheeled vehicles based on a 1dof motion prior

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