本发明涉及机器人定位与导航技术领域，尤其涉及一种视觉里程计的测量方法及视觉里程计。
背景技术：
本部分旨在为权利要求书中陈述的本发明的具体实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。
随着机器人技术的不断发展，传统的机器人构建环境地图并确定位姿的方法难以满足机器人计算速度及精准度的需求。MSCKF是一种非常经典的单目视觉与IMU(Inertial measurement unit，惯性测量单元)融合的算法，其通过利用卡尔曼滤波器(KF，Kalman Filter)的方法进行多传感器融合。MSCKF把视觉中的图像特征信息加入系统需要估计的特征向量之中。
这种基于滤波器的方法只能在小的场景的情况下使用，对于大的地图场景会因为特征向量的维度过大导致计算时间的急剧上升，从而不能保证实时性，更致命的是，对于长时间的运动，该方法由于误差的累积会导致状态不确定度的上升，从而也就导致了该方法的准确性与鲁棒性均不强。
技术实现要素：
有鉴于此，本发明提供一种有利于提高计算准确性、快速性及鲁棒性的视觉里程计的测量方法，本发明还提供一种视觉里程计。
一种视觉里程计的测量方法，包括以下步骤：
获取拍摄对象的图像序列；
测量载体的角速度与加速度；
采用ORB算法，从所述图像序列中选取多个关键帧；及
采用非线性数值优化方法，对多个关键帧之间的重投影误差以及载体的运动模型误差进行优化操作，计算得到所述重投影误差及所述运动模型误差之和为最小值对应的载体的姿态信息，并进行实时的三维重建。
进一步地，所述图像序列中的每帧图像包括彩色图像及深度图像，所述采用ORB算法，从所述图像序列中选取多个关键帧，包括：
在所述图像序列中选取一关键帧；
对每帧图像中的彩色图像进行灰度化处理并得到对应的灰度图像；
采用ORB算法提取在前关键帧及当前帧的灰度图像及深度图像中的特征点并进行特征匹配；及
采用ICP算法，根据匹配好的特征点计算得到所述在前关键帧与所述当前帧之间的齐次变换矩阵及图像相似度值，若所述图像相似度值位于预设范围内，则确定所述当前帧为关键帧。
进一步地，根据相邻关键帧之间的齐次变换矩阵估计对应的重投影误差；
通过对所述关键帧对应的角速度与加速度数据进行预积分得到载体的运动模型误差。
进一步地，所述非线性数值优化方法为列文伯格-马尔夸特非线性拟合法。
进一步地，视觉里程计的测量方法还包括系统初始化，所述系统初始化包括以下步骤：
获取拍摄对象的图像序列；
测量载体的角速度与加速度；
采用ORB算法，对相邻两帧图像进行特征提取与匹配；
对所述相邻两帧图像之间的重投影误差、以及载体的运动模型误差进行优化操作，估算角速度与加速度的偏差、重力向量在载体坐标系下的分量以及各个帧图像对应的载体运动速度，以采用所述非线性数值优化方法，对多个关键帧之间的重投影误差以及载体的运动模型误差进行优化操作。
进一步地，所述采用ORB算法，对相邻两帧图像进行特征提取与匹配之后，包括：
采用ICP算法，根据匹配好的特征点计算得到所述相邻两帧图像之间的齐次变换矩阵。
进一步地，根据所述相邻两帧图像之间的齐次变换矩阵计算得到对应的重投影误差；
通过对所述角速度与加速度数据进行预积分得到载体的运动模型误差。
进一步地，所述优化操作利用非线性数值拟合法。
一种视觉里程计，包括：
图像采集装置，用于获取拍摄对象的图像序列；
IMU，测量载体的角速度与加速度；
特征检测单元，采用ORB算法，对所述彩色和深度图像进行特征提取与匹配，以获得关键帧；及
优化器，采用非线性数值优化方法，对所述关键帧之间的重投影误差、以及所述IMU的运动模型误差进行优化操作，计算得到所述重投影误差及所述运动模型误差之和为最小值对应的载体的姿态信息，并进行实时的三维重建。
进一步地，所述图像采集装置为RGB-D相机。
本发明提供的视觉里程计的测量方法及视觉里程计，采用非线性数值优化方法对系统运动状态进行优化，有利于准确的估计出载体自身的位姿，并能够实时进行场景三维地图的重建。
附图说明
为了更清楚地说明本发明实施例/方式技术方案，下面将对实施例/方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例/方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施方式提供的视觉里程计的测量方法流程图。
图2为图1所示的系统初始化流程图。
图3为图2所示的采用非线性数值优化方法的系统初始化过程中IMU偏差随时间的曲线。
图4为为图1所示的采用ORB算法从图像序列中并选取多个关键帧的方法流程图。
图5为本发明实施方式提供的三维重建的结果。
如下具体实施方式将结合上述附图进一步说明本发明。
具体实施方式
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施例对本发明进行详细描述。需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，所描述的实施例仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。
本发明提供一种视觉里程计的测量方法及一种视觉里程计，其采用非线性数值优化方法，对多个关键帧之间的重投影误差以及载体的运动模型误差进行优化操作，有利于实时进行场景三维地图的重建，同时准确的估计出机器人自身的位姿。
本发明实施方式中提供的视觉里程计包括图像采集装置、IMU、特征检测单元及优化器。其中，所述图像采集装置用于获取拍摄对象的图像序列；所述IMU测量载体的角速度与加速度；所述特征检测单元采用ORB算法，对所述彩色和深度图像进行特征提取与匹配，以获得关键帧；所述优化器采用非线性数值优化方法，对所述关键帧之间的重投影误差、以及所述IMU的运动模型误差进行优化操作，计算得到所述重投影误差及所述运动模型误差之和为最小值对应的载体的姿态信息，并进行实时的三维重建。
具体地，本发明实施方式中提供的图像采集装置用于采集对象，特别是环境的彩色与深度图像。在一种实施方式中，图像采集装置为RGB-D相机，比如微软的Kinect、Kinect v2英特尔的RealSense、奥比中光的Orbbec Astra等。在一种优选的实施方式中，RGB-D相机采用深度轮廓估计方法得到深度轮廓图像。具体地，首先得到低分辨率的深度边缘信息；然后通过相机标定和图像校准操作，得到深度边缘的高分辨率散点图，并进行边缘插值得到高分辨率的连续的深度轮廓；最后，在彩色图像边缘的引导和约束下，对深度轮廓进行矫正优化，生成最终的深度图像。
在一种实施方式中，所述图像采集装置为双目相机，通过双目摄像头可分别得到左右目图像，使用立体视觉技术，通过左右目视差计算像素的距离得到深度图像，该深度图像包含所有像素点的三维世界坐标信息。
所述IMU是测量物体三轴的角速率以及线性加速度的装置。一般的，一个IMU包含了三个单轴的加速度计(accelerometer)和三个单轴的陀螺仪(gyroscope)，本发明实施方式中，所述IMU还包括三个单轴的磁强计。加速度计能够检测物体在载体坐标系统独立三轴的加速度信号，而陀螺仪检测载体相对于载体坐标系的角速度信号，磁强计是检测物体在载体坐标系下地球磁场的三个分量，与加速度计和陀螺仪一起测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。在机器人的导航与控制之中有着很重要的应用价值。
在一种实施方式中，IMU设备包括一用于与一数据校准设备电连接的IMU，所述IMU包括一用于检测不同温度环境下高度信息的第一传感器、一用于检测不同温度环境下角加速度信息的第二传感器和一用于接收校准信息的存储器；所述第一传感器、所述第二传感器和所述存储器均与该数据校准设备电连接。所述IMU设备中自带存储器，可直接将IMU设备置于多种不同温度环境下进行数据校验，而无需将包括所述IMU设备的机器人置于各种温度环境下进行校验，使操作更方便。
请参阅图1，为本发明实施方式提供的视觉里程计的测量方法流程图。如图1所示，所述视觉里程计的测量方法包括以下步骤：
S0：系统初始化。在一种实施方式中，所述视觉里程计已知所述IMU的角速度与加速度偏差，重力向量在载体坐标系下的分量以及各个帧图像对应的载体运动速度，即可省略系统初始化的步骤。在本发明实施方式中，所述视觉里程计通过系统初始化的步骤估计所述IMU的角速度与加速度偏差、重力向量在载体坐标系下的分量以及各个帧图像对应的载体运动速度。
请参阅图2，为为图1所示的系统初始化流程图。所述系统初始化的步骤具体包括：
S01：获取拍摄对象的图像序列。所述图像序列包括在不同时间、不同方位对目标依序连续获取的系列图像。本发明实施方式中，所述图像序列中的图像均由RGB-D相机获取，所述RGB-D相机装设于载体上，在一种优选的实施方式中，所述载体为机器人，比如无人机。
S02：测量载体的角速度与加速度。本发明实施方式中，所述IMU用于测量所述角速度与所述加速度。
S03：采用ORB算法，对相邻两帧图像进行特征提取与匹配。
在所述视觉里程计中，一共会使用四个坐标系：像素坐标系、相机坐标系、IMU坐标系以及世界坐标系。所述系统初始化的步骤中，设置世界坐标系为第一个相机帧的相机坐标系，而IMU坐标系与相机坐标系由于都是固连在载体之上的，所以二者之间有一个不会变化齐次变换矩阵，所述相机坐标系与像素坐标系之间同样具有一个不会变化的齐次变换矩阵。
在像素坐标系中，利用BRIEF描述子对所述特征点进行特征匹配，从而得到一组像素坐标系中的匹配好的特征点；
将像素坐标系中匹配好的特征点反投影至相机坐标系中，以得到相机坐标系中的匹配好的特征点。那么就拥有了一组匹配好的相机坐标系下的三维特征点。
S04：采用ICP算法，根据匹配好的特征点计算得到所述相邻两帧图像之间的齐次变换矩阵。
S05：对所述相邻两帧图像之间的重投影误差、以及载体的运动模型误差进行优化操作，估算角速度与加速度的偏差、重力向量在载体坐标系下的分量以及各个帧图像对应的载体运动速度，以采用所述非线性数值优化方法，对多个关键帧之间的重投影误差以及载体的运动模型误差进行优化操作。
根据所述相邻两帧图像之间的齐次变换矩阵以及载体的角速度与加速度，估计重力向量在载体坐标系下的分量、每帧图像对应的运动速度及运动模型误差。根据所述相邻两帧图像之间的齐次变换矩阵计算得到对应的重投影误差；通过对所述角速度与加速度数据进行预积分得到载体的运动模型误差。
在一种实施方式中，所述优化操作利用非线性数值优化方法，非线性数值优化方法通过有限的迭代计算求出优化目标函数的极小值，非线性指的是优化的目标函数不是线性，因此在每一步迭代计算中都会存在着泰勒公式近似的步骤。进一步地，在一种优选的实施方式中，所述非线性数值优化方法为列文伯格-马尔夸特非线性拟合法。
如图3所示，所述系统初始化的过程中，采用非线性数值优化方法，所述IMU中的加速度计及陀螺仪的偏差在5s内可以估计出来。采用非线性数值优化方法，有利于缩短所述视觉里程计的初始化时间，并提高的初始化步骤的鲁棒性。可以理解的是，所述系统初始化步骤中的优化操作中还可以采用线性拟合法或其他非线性拟合法，在这里不做限制。
所述系统初始化完成后，所述视觉里程计的测量方法还包括以下步骤：
S1：获取拍摄对象的图像序列。所述图像序列包括在不同时间、不同方位对目标依序连续获取的系列图像。本发明实施方式中，所述图像序列中的图像均由RGB-D相机获取，所述RGB-D相机装设于载体上，在一种优选的实施方式中，所述载体为机器人，比如无人机。
S2：测量载体的角速度与加速度。本发明实施方式中，所述IMU用于测量所述角速度与所述加速度。
S3：采用ORB算法，从所述图像序列中选取多个关键帧。
特征检测单元用于采用ORB算法，对所述彩色和深度图像进行特征提取与匹配，以获得关键帧。
请参阅图4，为图1所示的采用ORB算法从所述图像序列中并选取多个关键帧的方法流程图。本发明实施方式中，所述采用ORB算法，从所述图像序列中并选取多个关键帧，包括以下步骤：
S31：在所述图像序列中选取一关键帧。所述选取的关键帧，可以为所述图像采集装置采集的第一帧图像，或是所述图像序列中的任意一帧图像。
S32：对每帧图像中的彩色图像进行灰度化处理并得到对应的灰度图像；
S33：采用ORB算法提取在前关键帧及当前帧的灰度图像及深度图像中的特征点并进行特征匹配。具体地，在像素坐标系中，利用BRIEF描述子对所述特征点进行特征匹配，从而得到一组像素坐标系中的匹配好的特征点；将像素坐标系中匹配好的特征点反投影至相机坐标系中，以得到相机坐标系中的匹配好的特征点。
S34：采用ICP算法，根据匹配好的特征点计算得到所述在前关键帧与所述当前帧之间的齐次变换矩阵及图像相似度值，若所述图像相似度值位于预设范围内，则确定所述当前帧为关键帧。
S4：采用非线性数值优化方法，对多个关键帧之间的重投影误差以及载体的运动模型误差进行优化操作，计算得到所述重投影误差及所述运动模型误差之和为最小值对应的载体的姿态信息，并进行实时的三维重建。
根据相邻关键帧之间的齐次变换矩阵估计对应的重投影误差，通过对所述关键帧对应的角速度与加速度数据进行预积分得到载体的运动模型误差。
所述优化器以在前关键帧作为输入，在所述优化器之中优化所述关键帧之间的重投影误差与运动模型误差，即将所述关键帧之间的齐次变换矩阵，以及所述IMU的预计分的结果作为误差函数加入到所述优化器之中，采用列文伯格-马尔夸特的非线性数值优化方法进行数值求解，解出使得误差函数最小的系统状态。可以理解的是，非线性数值优化方法还可以采用其他的算法，并不以此为限。
在得到误差函数最小的系统状态(包含所述深度相机的位置、姿态、速度)之后就可以进行实时的三维重建，利用所述深度相机提供的彩色图与深度图，可以将像素坐标系下的点转换为相机坐标系下的坐标，进一步根据相机坐标系到世界坐标系的齐次变换矩阵，将相机坐标系下的坐标转换为世界坐标系。在相机的实时运动之中，每一个采集到的特征点都可以转换到唯一的世界坐标系中，也就完成了实时的三维重建。图5为本发明实施方式提供的三维重建的结果。
本发明提供的视觉里程计的测量方法及视觉里程计，采用非线性数值优化方法对系统运动状态进行优化，通过有限的迭代计算求出优化目标函数的极小值，在每一步迭代计算中都会存在着泰勒公式近似的步骤有利于准确的估计出载体自身的位姿，并能够实时进行场景三维地图的重建。
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个装置也可以由同一个装置或系统通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。
最后应说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或等同替换，而不脱离本发明技术方案的精神和范围。