CN106056664B - 一种基于惯性和深度视觉的实时三维场景重构系统及方法 - Google Patents

Abstract

本发明公开了一种基于惯性和深度视觉的实时三维场景重构系统及方法，利用IMU传感器在短时间内通过积分计算所得的相对位移和相对姿态变化相对准确的特点，将IMU的ΔP^IMU可以作为一个近似真实值与ΔP^RGB和ΔP^D比较。如果ΔP^RGB与ΔP^IMU相差超过一定的阈值，则可认为RGB相机通过跟踪连续帧上的特征所计算的相对位置和姿态变化不够精确，同理，如果ΔP^D与ΔP^IMU相差超过一定的阈值，则可认为通过ICP算法对对连续帧的点云匹配所求得的相对位置和姿态变化不够精确，再根据比较结果进行数据融合，实时估计设备的相对位置和姿态变化，从而提高三维场景重构的精确度，提高设备的容错性。

Description

一种基于惯性和深度视觉的实时三维场景重构系统及方法

技术领域

本发明涉及计算机视觉和多传感器融合导航绘图领域，具体地，涉及一种基于惯性传感器和深度视觉传感器的实时三维场景重构的系统及方法。

背景技术

在2010年前后，以色列PrimeSense公司在基于结构光的深度传感器的小型化和模组化技术上取得了突破，并与微软公司合作开发了Kienct传感器。Kienect传感器上集成了一个彩色RGB相机和一个深度D相机，可以快速获得周围0.5米到4米内物体表面的点云数据，是一种用于捕捉游戏者动作变化的体感设备。该技术一推向市场，就引起了工业界和学术界的注意。在过去几年里，陆续有类似的传感器推出，比如美国Occipital公司的Structure Sensor，Intel公司的RealSense传感器，Google公司的Tango项目等。所有这些设备都可以简称为RGB-D传感器，一种深度视觉传感器系统。

在过去几年里，随着计算机视觉和机器人领域的快速发展，尤其是同时绘图并导航技术(SLAM－Simultaneous Localization And Mapping)的发展，学术界的研究热点是如何通过移动RGB相机实时重建三维场景。基于单目相机，双目相机和全景相机的实时三维场景重构系统都被广泛研究。目前，学术界主流的看法是基于双目相机系统，并采用视觉里程计技术(Visual Odometry)，实时重构三维场景的稳定性和效果是最好的。然而，当拍摄光线不好和周围物体表面缺乏纹理的情况下，无论哪种相机系统都无法正常工作。与之相比较，RGB-D传感器，可以同时获取周围环境的彩色影像和深度信息。深度信息是不依赖于拍摄时的光线条件和物体表面纹理的。这就使得RGB-D传感器与相机系统相比有了明显的优势。自然地，工业界和学术界开始关注如何使用RGB-D传感器来实时重构三维环境。一个典型的RGB-D实时三维绘图系统应该包括三个步骤，

1.基于连续帧影像的特征匹配来计算相机的相对运动，即相机跟踪。

2.基于第一步计算所得的相对位置和姿态，正确显示深度D传感所获得的点云数据，产生周围环境的三维场景。

3.实时检测闭环，并通过基于图的闭合算法来纠正累积的误差。

这类算法是否成功的关键就在于是否能够精确地估计出RGB-D传感器的相对运动，这是在上述的第一步和第二步中完成的。第一步是基于视觉里程计技术来估计RGB-D的相对运动，这会受到光线和物体表面纹理的影响。第二步是用第一步提供的相对位置和姿态作为初始条件来做点云的形状匹配，比如ICP算法(已知算法)，从而提高RGB-D的相对运动估计。如果周围物体的形状变化过多也会导致算法的失败。

当前，基于RGB-D的实时三维环境重建系统在周围环境特征信息丰富的情况下，基于视觉里程计技术和ICP技术，可以达到较好的三维重建效果，然而，在周围环境条件变化时，比如光线变化，周围环境特征变化等，RGB-D的实时三维环境重建就会遇到困难。一旦实时三维环境重构由于周围环境条件变化而中断，就很难再继续工作，使整个系统的鲁棒性降低。

惯性传感器，可以高频率地(比如100Hz)测量三个方向的加速度和三个方向的角速度，通过加速度的积分可以获得速度变化，通过对速度的积分可以获得相对位移；同样的，对角速度积分可以获得姿态的相对变化。这类传感器在短时间内可以提供较高精度的相对位移和姿态变化，而且完全不受环境条件的影响。

发明内容

为了克服现有技术中基于RGB-D传感器的实时三维环境重建的缺点，本发明提供一种基于惯性和深度视觉的实时三维场景重构系统及方法，利用IMU传感器在短时间内通过积分计算所得的相对位移和相对姿态变化相对准确的特点，通过IMU传感器与RGB-D传感器进行数据融合，实时估计设备的相对位置和姿态变化，从而提高三维场景重构的精确度，提高设备的容错性。

本发明为解决上述技术问题所采用的技术方案是：

一种基于惯性和深度视觉的实时三维场景重构系统，包括惯性传感器算法子模块，RGB相机算法子模块，深度D相机算法子模块，比较及数据融合模块和三维点云生成模块；所述惯性传感器算法子模块、RGB相机算法子模块、深度D相机算法子模块分别与比较及数据融合模块电连接，所述比较及数据融合模块与三维点云生成模块电连接；

所述惯性传感器算法子模块、RGB相机算法子模块、深度D相机算法子模块分别用于采集k及k+1时刻系统的相对位置及姿态，并发送给比较及数据融合模块；

所述比较及数据融合模块用于进行惯性传感器算法子模块与RGB相机算法子模块、惯性传感器算法子模块与深度D相机算法子模块所采集相对位置及姿态同预设阈值的比较，并根据比较结果进行数据融合，校正系统相对位置及姿态的偏差并发送给三维点云生成模块；

所述三维点云生成模块用于根据比较及数据融合模块得到的系统相对位置及姿态信息，利用Kinect Fusion算法生成三维点云，进行实时三维环境重建。

为了提高运算的实时性，算法实现采用了模块化设计，可以同时支持多个模块的并行运算。IMU惯性传感器算法子模块是基于100Hz的加速度和角速度数据，计算出IMU传感器的相对位置和姿态变化，记作ΔP^IMU。RGB相机算法子模块采用特征跟踪的视觉里程计技术按30帧/秒的频率计算RGB传感器的相对位置和姿态变化，记作ΔP^RGB。深度D相机算法子模块，采用ICP算法(已知算法)，以30帧/秒的频率计算D传感器的相对位置和姿态变化，记作ΔP^D。三个传感器之间的几何关系都是已知的，因此可以把它们各自的相对位置变化和姿态变化转换到同一个坐标系下，比如都转换到深度D相机坐标系下。

一种基于惯性和深度视觉的实时三维场景重构方法，该算法包括以下步骤：

步骤1：对设备进行初始化，标定IMU传感器、深度D相机和RGB相机的相对位置关系；同时在设备完全静止状态下，设定设备的初始位置和初始姿态P0；初始位置默认为局部三维坐标系的原点；

步骤2：计算当前时刻与上一时刻相比，IMU传感器的位置和姿态变化值ΔP^IMU与深度D相机的位置和姿态变化值ΔP^D的差值，并与预设阈值进行比较；若差值小于预设阈值，则进行IMU传感器与深度D相机的数据融合，并将融合数据作为下一时刻IMU的位置和姿态；若差值大于预设阈值，则表示数据融合失败，执行步骤3；

步骤3：计算当前时刻与上一时刻相比，IMU传感器的位置和姿态变化值ΔP^IMU与RGB相机的位置和姿态变化值ΔP^RGB的差值，并与预设阈值进行比较；若差值小于预设阈值，进行IMU传感器与RGB相机的数据融合，并将融合数据作为下一时刻IMU的位置和姿态；若差值大于预设阈值，则表示数据融合失败，通过IMU的速度和角速度积分求得下一时刻的位置与姿态；

步骤4：将设备位置与姿态数据带入Kinect Fusion算法，实时产生周围环境的三维点云。

作为优选，还包括步骤5：若连续出现多次通过IMU的速度和角速度积分求得下一时刻的位置与姿态的情况则表示设备出现严重错误，需对设备进行重新初始化。

作为优选，步骤2包括以下具体步骤：

步骤201：分别采集k和k+1时刻IMU传感器的位置和姿态P_k、P’_k+1，由此得到IMU传感器的相对位置和姿态变化值ΔP^IMU；

步骤202：利用ICP算法根据深度D相机产生的k和k+1时刻的点云对，计算深度D相机相对位置和姿态变化值ΔP^D；

步骤203：计算ΔP^IMU与ΔP^D的差值，并与预设阈值进行比较，若差值小于阈值则执行步骤204，否则进行IMU传感器与深度D相机的数据融合；

步骤204：计算k+1时刻IMU的位置和姿态：P_k+1＝P_k+ΔP^D；

步骤205：将步骤204中计算得到的k+1时刻的位置和姿态作为扩展卡尔曼滤波中的外部观测值引入基于IMU导航微分方程建立的观测方程中，从而精确估计出IMU的状态向量。

作为优选，步骤3包括以下具体步骤：

步骤301：分别采集k和k+1时刻IMU传感器的位置和姿态P_k、P’_k+1，由此得到IMU传感器的相对位置和姿态变化值ΔP^IMU；

步骤302：采集RGB相机从k至k+1时刻的连续影像，根据视觉历程计技术，计算RGB相机的相对位置和姿态变化值ΔP^RGB；

步骤303：计算ΔP^IMU与ΔP^RGB的差值，并与预设阈值进行比较，若差值小于阈值则执行步骤304，否则k+1时刻IMU传感器的位置和姿态P_k+1＝P_k+ΔP^IMU；

步骤304：计算k+1时刻IMU的位置和姿态：P_k+1＝P_k+ΔP^RGB；

步骤305：将步骤304中计算得到的k+1时刻的位置和姿态作为扩展卡尔曼滤波中的外部观测值引入基于IMU导航微分方程建立的观测方程中，从而精确估计出IMU的状态向量。

本发明的有益效果是：

IMU传感器在短时间内通过积分计算所得的相对位移和相对姿态变化是很准确的，因此IMU的ΔP^IMU可以作为一个近似真实值与ΔP^RGB和ΔP^D比较。如果ΔP^RGB与ΔP^IMU相差超过一定的阈值，则可认为RGB相机通过跟踪连续帧上的特征所计算的相对位置和姿态变化不够精确，这往往与拍摄的光线条件和环境里特征贫乏有关。同理，如果ΔP^D与ΔP^IMU相差超过一定的阈值，则可认为通过ICP算法对对连续帧的点云匹配所求得的相对位置和姿态变化不够精确，这往往是由于连续帧的点云数据在形状上相似度不够造成的。

本发明的创新之处在于扩展了仅基于深度D传感器的Kienct Fusion算法(已知算法)。本发明仅保留了Kinect Fusion算法中连续帧点云逐步叠加的三维场景重构的算法。在新的设计下，IMU传感器，RGB相机和深度D相机的数据都被用于实时估计设备的相对位置和姿态变化。这比Kinect Fusion算法仅通过单一的深度相机数据来估计相对运动，要精确很多。

为了提高实现实时三维场景重构的鲁棒性，算法上采用了递进处理的方式。对每一时刻，首先比较ΔP^IMU与ΔP^D，如果差别在阈值以内，则采用深度D相机计算的相对位置和姿态，并通过松耦合的卡尔曼滤波对IMU传感器的误差项加以纠正。如果差别较大，则放弃当前时刻深度D传感器的相对运动估计结果。再比较ΔP^IMU与ΔP^RGB，如果差别在阈值以内，则采用RGB相机子模块的相对运动估计，并通过松耦合的卡尔曼滤波对IMU的误差项加以纠正。如果差别依然超出阈值，则采用IMU传感器子模块的相对运动估计。

当设备的相对运动确定之后，就可以直接采用Kinect Fusion算法来完成点云数据的累积匹配，实时产生周围环境的三维点云。

本发明扩展了Kinect Fusion算法，通过IMU传感器，RGB相机和深度D相机的传感器组合技术大大提高了设备相对运动估计的精度，从而进一步提高了三维场景重构的效率。引入了IMU传感器，也大大提高了整个实时三维重建系统的应用范围，鲁棒性高，容错性强。

附图说明

图1为本发明提供的基于惯性和深度视觉的实时三维场景重构系统结构图

图2为本发明提供的基于惯性和深度视觉的实时三维场景重构方法流程图

图3为IMU传感器和深度D相机的组合算法流程图

图4为IMU传感器和RGB相机的组合算法流程图

具体实施方式

下面结合附图及实施例对本发明作进一步说明。以下的实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出，本发明所涉及的传感器组合仅只是本发明的一种特例，本发明不局限于IMU，RGB和深度D传感器。基于本发明的构思，还可以做若干变形和改进，比如改用双目视觉系统取代单目相机，又比如采用精度更高的惯导设备IMU等。计算机视觉和多传感器融合领域的技术人员在本发明的算法思路的启发下，还可以做出改进，比如用更好的特征跟踪算法来加强视觉里程计估计相对运动的精度等。这些基于本发明的改进，扩展和提高都属于本发明的保护范围。

如图1所示，一种基于惯性和深度视觉的实时三维场景重构系统，包括IMU惯性传感器算法子模块1，RGB相机算法子模块2，深度D相机算法子模块3，比较及数据融合模块4和三维点云生成模块5；所述IMU惯性传感器算法子模块1、RGB相机算法子模块2、深度D相机算法子模块3分别与比较及数据融合模块4电连接，所述比较及数据融合模块4与三维点云生成模块5电连接；

所述IMU惯性传感器算法子模块1、RGB相机算法子模块2、深度D相机算法子模块3分别用于采集k及k+1时刻系统的相对位置及姿态，并发送给比较及数据融合模块4；

所述比较及数据融合模块4用于进行IMU惯性传感器算法子模块1与RGB相机算法子模块2、IMU惯性传感器算法子模块1与深度D相机算法子模块3所采集相对位置及姿态同预设阈值的比较，并根据比较结果进行数据融合，校正系统相对位置及姿态的偏差并发送给三维点云生成模块5；

所述三维点云生成模块5用于根据比较及数据融合模块4得到的系统相对位置及姿态信息，利用Kinect Fusion算法生成三维点云，进行实时三维环境重建。

图2给出了本发明算法上的总体流程。在使用设备实时三维场景重建之前，设备需要在完全静止的状态下完成初始化。初始化的主要目的是设定设备的初始位置和姿态。初始位置默认为局部三维坐标系的原点，即(0，0，0)。初始姿态，用基于IMU的ZUPT算法(已知算法)来计算IMU传感器的初始姿态(滚动角和翻滚角)。

接下来，执行IMU传感器和深度相机D做数据融合，如图3所示，算法的输入是k时刻的位置和姿态P_k。通过对IMU的测量值积分，可以得到k时刻到k+1时刻，设备的位置和姿态的相对变化，并记作ΔP^IMU。深度D相机计算模块会对k时刻和k+1时刻的点云对进行ICP匹配。ICP算法是点云匹配的经典算法，目的是求出两个点云坐标系间的变换矩阵，一般采用放射变换，比例尺因子一般为1，即两个点云坐标系之间的旋转和平移关系。ICP算法通过多次迭代不断逼近最佳的变换矩阵，一般当两个点云间所有匹配点对距离都小于一个阈值的时候，停止迭代，并获得最佳的变换矩阵。这样，通过ICP算法就求得了k到k+1时刻的位置和姿态的相对变化ΔP^D。如果ΔP^IMU与ΔP^D的差异在给定的阈值以内，说明ICP算法得到了比较精确的相对运动估计，从而计算出k+1时刻的位置和姿态，即P_k+1＝P_k+ΔP^D。

基于IMU传感器的加速度和角速度测量值，可以建立导航微分方程。本实例中，IMU的状态向量包含了位置向量，速度向量，旋转四元数向量(quaternion)，加速度计偏置误差(accelerometer bias)和角速度计偏置误差(gyro bias)。当IMU传感器和深度D相机融合成功时，k+1时刻的位置和姿态可以作为扩展卡尔曼滤波中的外部观测值引入基于IMU导航微分方程建立的观测方程中，从而精确估计出IMU的状态向量。

如果IMU和深度D相机的数据融合失败，则转向IMU和RGB相机的数据融合。

图4所示的是IMU和RGB相机的数据融合算法。

IMU的处理方法一样。对RGB相机k到k+1时刻连续帧上的特征跟踪，视觉里程计技术，计算出相机的位置和姿态的相对变化。同样，比较ΔP^IMU与ΔP^RGB的差异，如果在给定的阈值以内，则说明通过视觉里程计技术所求得的相对运动估计比较精确，可以直接计算出P_k+1＝P_k+ΔP^RGB。同样的道理，这个P_k+1可以被用作扩展卡尔曼滤波里的外部观测值引入基于IMU的观测方程中，去精确估计IMU的状态向量。

如果IMU和RGB相机的数据融合失败，则k+1时刻的位置和姿态是P_k+1＝P_k+ΔP^IMU。当深度D相机和彩色RGB相机工作正常的情况下，k+1时刻的位置和姿态一般都是精确的，同时IMU的偏置误差也能得到很好的纠正，从而保证IMU计算出的位置和姿态也保持精确。当深度D相机和RGB相机偶尔无法正常工作的情况下，IMU的位置和姿态也可以维持精度一小段时间。当深度D相机和RGB相机重新工作的时候，则又回归到正常的工作模式。

通过上述步骤得到每个时刻相对精确的设备位置和姿态，在采用KinectFusion算法进行实时三维环境重建时的效率和精度都会大大提高。

当IMU传感器和RGB相机及深度D相机的数据融合连续多次出现失败时，即连续出现多次通过IMU的速度和角速度积分求得下一时刻的位置与姿态的情况，则表示设备出现严重错误，需对设备进行重新初始化。

说明书中未阐述的部分均为现有技术或公知常识。本实施例仅用于说明该发明，而不用于限制本发明的范围，本领域技术人员对于本发明所做的等价置换等修改均认为是落入该发明权利要求书所保护范围内。

Claims (5)

1.一种基于惯性和深度视觉的实时三维场景重构系统，其特征在于：包括IMU惯性传感器算法子模块(1)，RGB相机算法子模块(2)，深度D相机算法子模块(3)，比较及数据融合模块(4)和三维点云生成模块(5)；所述IMU惯性传感器算法子模块(1)、RGB相机算法子模块(2)、深度D相机算法子模块(3)分别与比较及数据融合模块(4)电连接，所述比较及数据融合模块(4)与三维点云生成模块(5)电连接；

所述IMU惯性传感器算法子模块(1)、RGB相机算法子模块(2)、深度D相机算法子模块(3)分别用于采集k及k+1时刻系统的相对位置及姿态，并发送给比较及数据融合模块(4)；

所述比较及数据融合模块(4)用于进行IMU惯性传感器算法子模块(1)与RGB相机算法子模块(2)、IMU惯性传感器算法子模块(1)与深度D相机算法子模块(3)所采集相对位置及姿态同预设阈值的比较，并根据比较结果将IMU传感器与深度D的数据或者IMU传感器与RGB相机的数据进行数据融合，校正系统相对位置及姿态的偏差并发送给三维点云生成模块(5)；

所述三维点云生成模块(5)用于根据比较及数据融合模块(4)得到的系统相对位置及姿态信息，利用Kinect Fusion算法生成三维点云，进行实时三维环境重建。

2.一种基于惯性和深度视觉的实时三维场景重构方法，其特征在于：所述基于惯性和深度视觉的实时三维场景重构方法包括以下步骤：

步骤1：对设备进行初始化，标定IMU传感器、深度D相机和RGB相机的相对位置关系；同时在设备完全静止状态下，设定设备的初始位置和初始姿态P0；初始位置默认为局部三维坐标系的原点；

步骤2：计算当前时刻与上一时刻相比，IMU传感器的位置和姿态变化值△PIMU与深度D相机的位置和姿态变化值△PD的差值，并与预设阈值进行比较；若差值小于预设阈值，则进行IMU传感器与深度D相机的数据融合，并将融合数据作为下一时刻IMU的位置和姿态；若差值大于预设阈值，则表示数据融合失败，执行步骤3；

步骤3：计算当前时刻与上一时刻相比，IMU传感器的位置和姿态变化值△PIMU与RGB相机的位置和姿态变化值△PRGB的差值，并与预设阈值进行比较；若差值小于预设阈值，进行IMU传感器与RGB相机的数据融合，并将融合数据作为下一时刻IMU的位置和姿态；若差值大于预设阈值，则表示数据融合失败，通过IMU的速度和角速度积分求得下一时刻的位置与姿态；

步骤4：将设备位置与姿态数据带入Kinect Fusion算法，实时产生周围环境的三维点云。

3.根据权利要求2所述的一种基于惯性和深度视觉的实时三维场景重构方法，其特征在于：步骤2包括以下具体步骤：

步骤201：分别采集k和k+1时刻IMU传感器的位置和姿态Pk、P’k+1，由此得到IMU传感器的相对位置和姿态变化值△PIMU；

步骤202：利用ICP算法根据深度D相机产生的k和k+1时刻的点云对，计算深度D相机相对位置和姿态变化值△PD；

步骤203：计算△PIMU与△PD的差值，并与预设阈值进行比较，若差值小于阈值则执行步骤204，否则进行IMU传感器与RGB相机的数据融合；

步骤204：计算k+1时刻IMU的位置和姿态：Pk+1＝Pk+△PD；

步骤205：将步骤204中计算得到的k+1时刻的位置和姿态作为扩展卡尔曼滤波中的外部观测值引入基于IMU导航微分方程建立的观测方程中，从而精确估计出IMU的状态向量。

4.根据权利要求2所述的一种基于惯性和深度视觉的实时三维场景重构方法，其特征在于：步骤3包括以下具体步骤：

步骤301：分别采集k和k+1时刻IMU传感器的位置和姿态Pk、P’k+1，由此得到IMU传感器的相对位置和姿态变化值△PIMU；

步骤302：采集RGB相机从k至k+1时刻的连续影像，根据视觉里程计技术，计算RGB相机的相对位置和姿态变化值△PRGB；

步骤303：计算△PIMU与△PRGB的差值，并与预设阈值进行比较，若差值小于阈值则执行步骤304，否则k+1时刻IMU传感器的位置和姿态Pk+1＝Pk+△PIMU；

步骤304：计算k+1时刻IMU的位置和姿态：Pk+1＝Pk+△PRGB；

步骤305：将步骤304中计算得到的k+1时刻的位置和姿态作为扩展卡尔曼滤波中的外部观测值引入基于IMU导航微分方程建立的观测方程中，从而精确估计出IMU的状态向量。

5.根据权利要求2所述的一种基于惯性和深度视觉的实时三维场景重构方法，其特征在于：还包括步骤5：若连续出现多次通过IMU的速度和角速度积分求得下一时刻的位置与姿态的情况则表示设备出现严重错误，需对设备进行重新初始化。