CN105469405A - 基于视觉测程的同时定位与地图构建方法 - Google Patents

Abstract

本发明提出一种基于视觉测程的同时定位与地图构建方法，包括：采集双目图像并校正，得到无失真的双目图像；对无失真的双目图像进行特征点提取，生成特征点描述子；建立双目图像的特征点匹配关系；根据该匹配关系得到匹配特征点的水平视差，并结合双目图像捕获系统的参数计算空间真实深度；计算当前帧的特征点与世界地图中特征点匹配结果；去除其中错误匹配的特征点，得到成功匹配的特征点；计算成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐标的变换矩阵，根据变换矩阵得到双目图像捕获系统相对初始位置的位姿变化估计值；建立及更新世界地图。本发明具有较低的计算复杂度，厘米级的定位精度及对位置估计的无偏特性。

Description

基于视觉测程的同时定位与地图构建方法

技术领域

本发明涉及计算机及电子信息技术领域，特别涉及一种基于视觉测程的同时定位与地图构建方法。

背景技术

近年来，移动工作平台如无人机，移动机器人等成为研究的热点之一。这些设备具有很高的灵活性，广泛应用于灾难救助、地质勘测等场景中。机器人自主导航系统的主要技术包括：构建空间环境三维地图、自身定位、路线规划以及障碍躲避等。其中，构建环境空间的三维地图与自身定位是问题的核心，而路线规划、障碍躲避依托于无人机对环境的认知以及定位的结果。

目前室外无人机已经有成熟的导航系统，例如全球定位系统以及惯性导航系统。在室内导航中，复杂的闭合空间环境对无线信号有屏蔽和干扰，因此，依赖于卫星的定位系统在室内失效；同时，惯性导航系统的累计误差和对初值敏感性使得它不适用于高精度的室内定位。主动传感器如激光扫描仪，声纳等可以测量得到一维距离及偏角用于位置估计，但主动传感器通常重量沉、价格昂贵且功耗较大，不适用于小型移动平台；同时，单一的数据类型和低数据率限制了室内定位精度的提升。

近年来随着视觉传感器价格逐渐降低，其被更广泛的应用于视觉导航任务中。视觉信息对于生物而言是更直观的定位方式，且视觉传感器可以提供更丰富的数据信息，不仅可以实现机器人自身位置和姿态的估计，也可以实现对空间的认知。正确且稠密的空间认知信息可以被有效的应用到路线规划及障碍躲避等后续问题中。其中双目相机系统可以在不提供环境先验信息的情况下获取真实深度实现定位，这扩大了移动平台在未知环境中的应用范围。

视觉测程是机器人导航领域的基础算法。它的主要思想是，通过计算前后帧相同特征点的坐标变换关系获得帧间运动信息，通过逐帧累积的积分思想获得机器人相对初始位置的位姿估计。但实际实验表明，基于视觉测程的定位算法虽然提供了复杂度较低的位姿估计方法，但其逐帧累积的方式使得位置估计随时间发生漂移，也即产生累计误差。

同时定位与地图构建算法(SimultaneousLocalizationandMapping，SLAM)是近年来机器人导航领域的研究热点。它是指在未知环境中移动的机器人通过传感器测量数据恢复出自身的运动轨迹，同时构建出关于运动空间的一张增量式地图。在该过程中，机器人自身位置与空间地标具有双重不确定性，通过建立概率学模型解决该问题。基于扩展卡尔曼滤波的同时定位与地图构建算法被证明为目前收敛性最好的SLAM算法，但其明显缺陷是计算复杂性很高，与世界地图所包含的特征点个数的平方成正比。平方项的计算复杂度一方面限制了世界地图的规模，也即限制了所处环境中的特征数目和环境面积；另一方面，制约了定位导航算法的计算速度。考虑室内移动机器人低负载、低功耗的特性，在移动平台商实现传统的SLAM系统并达到实时处理速度受到制约。

发明内容

本发明旨在至少在一定程度上解决上述相关技术中的技术问题之一。

为此，本发明的目的在于提出一种基于视觉测程的同时定位与地图构建方法，该方法仅依靠双目视觉导航，无需融合其他传感器数据，无需对环境有先验知识，具有较低的计算复杂度，厘米级的定位精度及对位置估计的无偏特性。

为了实现上述目的，本发明的实施例提出了一种基于视觉测程的同时定位与地图构建方法，包括以下步骤：S1：通过双目图像捕获系统同步采集双目图像，并对所述双目图像进行校正，以得到无失真的双目图像；S2：对所述无失真的双目图像进行特征点提取，并根据提取的特征点生成特征点描述子；S3：根据所述特征点描述子建立所述双目图像的特征点匹配关系；S4：根据所述双目图像的特征点匹配关系得到匹配特征点的水平视差，并根据所述匹配特征点的水平视差和双目图像捕获系统的参数计算所述匹配特征点的空间真实深度；S5：获取当前帧双目图像的特征点，并将所述当前帧双目图像的特征点与世界地图中的特征点进行匹配；S6：对所述当前帧双目图像的特征点与世界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点；S7：计算所述成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐标的变换矩阵，根据所述变换矩阵得到所述双目图像捕获系统相对初始位置的位姿变化估计值；S8：在首帧计算时，初始化所述世界地图，并在非首帧计算时，根据所述变换矩阵更新世界地图中与当前帧成功匹配的特征点的三维坐标与特征描述子，并向世界地图中加入当前帧新观察到的特征点及其特征描述子。

另外，根据本发明上述实施例的基于视觉测程的同时定位与地图构建方法还可以具有如下附加的技术特征：

在一些示例中，所述S1进一步包括：提取所述双目图像捕获系统的成像参数，并根据所述成像参数对所述双目图像进行校正，以得到无失真的双目图像。

在一些示例中，在所述S2中，通过尺度不变特征转换方法对所述无失真的双目图像进行特征点提取。

在一些示例中，在所述S3中，通过判断所述双目图像的特征点描述子间的向量夹角是否小于特定阈值建立所述双目图像的特征点间的匹配关系。

在一些示例中，所述图像捕获系统包括所述左相机和右相机，所述S4进一步包括：分别获取所述匹配特征点在左相机和右相机中的成像点的像素坐标；根据所述相机的中心点的坐标计算所述左相机中的成像点和右相机中的成像点的水平视差；根据所述左相机中的成像点和右相机中的成像点的水平视差及空间几何关系得到所述匹配特征点的空间真实深度，其中，所述空间真实深度的计算公式如下：

$Z_p=\frac{B_L\×f_C}{Disparity}$

其中，Z_p为所述匹配特征点的空间真实深度，B_L为基线，f_C为焦距，Disparity为水平视差。

在一些示例中，所述S5进一步包括：将当前帧特征点的描述子按行存储在第一矩阵中，并将所述世界地图中的特征点的描述子按行存储在第二矩阵中；遍历所述世界地图中的全部特征点，从所述世界地图中分别提取与所述当前帧中的每个特征点相似度最高的特征点；根据所述相似度最高的特征点建立从所述当前帧特征点到所述世界地图的特征点映射关系表。

在一些示例中，所述特征点的相似度根据所述特征点描述子向量间的夹角确定，其中，当所述特征点描述子向量间的夹角最小时，所述特征点的相似度最高。

在一些示例中，在所述S6中，通过随机抽样一致方法对所述当前帧的特征点与世界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点，具体包括：S61：随机从N组成功匹配的特征点中选取m组匹配特征点，并建立全局配准方程，根据所述全局配准方程计算所述m组匹配特征点的坐标从世界坐标系变换到当前相机参考系的变换矩阵；S62：将未选取的N-m组匹配特征点的当前相机参考系下的坐标根据所述变换矩阵逆映射到世界坐标系，得到坐标逆映射结果CoorTemp，并计算所述CoorTemp与特征点在世界坐标系下的实际观察坐标CoorGlobal的欧氏距离；S63：判断所述欧式距离是否大于预设阈值，如果所述欧氏距离大于预设阈值，则判定对应的特征点为局外点，如果所述欧式距离小于所述预设阈值，则判定对应的特征点为局内点，并统计局内点的个数；S64：迭代执行所述S61至S63，并记录每次迭代过程中局内点的个数及坐标变换矩阵；S65：从多次迭代过程中选取包含局内点个数最多的一次迭代，并将该次迭代中的局内点作为成功匹配的特征点，将局外点作为错误匹配的特征点，并去除所述错误匹配的特征点。

在一些示例中，所述S7进一步包括：获取所述成功匹配的特征点在世界坐标系下的坐标CoorGlobal及在当前图像捕获系统参考系下的坐标CoorCamera；计算所述成功匹配的特征点在三维坐标间的旋转矩阵R和平移矩阵T，根据所述旋转矩阵R和平移矩阵T得到所述双目图像捕获系统相对初始位置的位姿变化估计值，其中，所述旋转矩阵R和平移矩阵T满足如下关系式：

$\underset{R,T}{\min }\underset{inliers}{\Σ}\left|\right|CoorCamera-T-R\×CoorGlobsl|{|}^2.$

在一些示例中，所述S8进一步包括：在首帧计算时，初始化所述世界地图，将首帧的图像捕获系统参考系确定为本次定位的世界坐标系，将首帧全部特征点的三维坐标与特征描述子加入世界地图；在非首帧计算时，根据所述变换矩阵将当前帧成功匹配的特征点的三维坐标CoorCamera逆映射到世界坐标系CoorGlobal’，其中，转换公式为：

CoorGlobal′＝inv(R)×(CoorCamera-T)；

将所述成功匹配特征点在世界坐标系下的坐标更新为多次观察的均值，其中，当第n+1次观察到该特征点，其世界地图中的坐标更新公式为：

$CoorGlbal=\frac{CoorGlobal\×n+{\mathrm{CoorGlobal}}^{\′}}{n+1};$

将所述成功匹配特征点的特征点描述子更新为最近一次观察的特征点描述子，并将当前帧新观察到的特征点三维坐标与特征点描述子加入所述世界地图。

根据本发明实施例的基于视觉测程的同时定位与地图构建方法，通过双目相机定位系统，可以在无先验知识的室内环境中恢复空间特征点的真实深度；采用视觉测程方法进行位姿估计，有效的降低了同时定位与地图构建算法的计算复杂度，使得计算速度与世界地图规模得到提升，适用于实时定位移动平台，且可以工作在更大范围的室内空间中；同时，通过随机抽样一致方法有效的消除了错误匹配的特征点，避免为位姿估计引入误差信息；此外，通过建立并更新世界地图有效消除了测量误差的累计，保证了室内定位精度在厘米级别且不随时间发生漂移；此外，所构建的世界地图包含稠密的空间特征信息，可用于后续障碍躲避，路径规划工作。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是根据本发明一个实施例的基于视觉测程的同时定位与地图构建方法的流程图；

图2是根据本发明一个实施例的三维重建空间几何原理示意图；

图3是根据本发明一个实施例的随机抽样一致方法的流程图；

图4是根据本发明一个实施例的世界坐标系与相机坐标系关系示意图；

图5是根据本发明一个实施例的室内定位运动轨迹及世界地图示意图；

图6是根据本发明一个实施例的室内无偏测试结果示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

以下结合附图描述根据本发明实施例的基于视觉测程的同时定位与地图构建方法。

图1是根据本发明一个实施例的基于视觉测程的同时定位与地图构建方法的流程图。如图1所示，该方法包括以下步骤：

步骤S1：通过双目图像捕获系统采集双目图像，并对双目图像进行校正，以得到无失真的双目图像。

在具体示例中，双目图像捕获系统为双目相机，其由两个平行放置的摄像头组成，两摄像头间相距一定距离，该距离即为基线。具体地，双目摄像头受相同信号触发，保证所拍摄图片的时刻相同。在具体示例中，双目图像捕获系统在移动平台运动中以每秒30帧的速度捕捉一系列照片流，以作为定位系统的输入。

进一步地，电荷耦合器件(ChargeCoupledDevice，CCD)相机会在成像过程中产生由透镜成像造成的失真，如径向失真，倾斜失真等。对于双目图像捕获系统，每个单相机都存在光学畸变，同时两个相机摆放角度不完全一致也会给后续图像数据处理带来困难。更为具体地，两个相机参考系间存在旋转和平移的关系，该关系使得同一空间中的特征点在两个相机中的像素坐标各自不同，从而增加了三维重建恢复真实深度的计算复杂度。因此，在本发明的一个实施例中，例如通过matlab提供的双目相机校正工具箱提取双目相机的成像参数，并根据该成像参数对所拍摄到的双目图像进行预先校正，使得校正后的图片不存在光学畸变，且相同特征点在双目图像中的像素坐标具有相同的纵坐标，只在横坐标方向存在差异，即得到无失真的双目图像。

步骤S2：对无失真的双目图像进行旋转、伸缩、光照不变的特征点提取，并根据提取的特征点生成特征点描述子。

具体地，该步骤从校正后的双目图像中分别进行特征点检测，获得特征点在二维图像中的像素坐标，并提取特征点的描述子。特征提取可能包括尺度不变特征(scale-invariantfeaturetransform，SIFT)和加速稳健特征(SpeededUpRobustFeatures，SURF)等。

在本发明的一个实施例中，例如采用尺度不变特征转换方法对无失真的双目图像进行特征点提取。尺度不变特征转换方法是进行特征点提取并建立特征点间匹配关系的算法。这种特征匹配算法在图像发生旋转、尺度缩放以及亮度改变时保持相对稳定。考虑到室内属于近场环境，随着拍摄平台的移动空间物体的尺度缩放效应较为明显；同时室内存在复杂的光照条件。因此，该算法适用于室内双目图像、帧间图像的特征点提取与匹配。相同的特征点可从不同的拍摄条件和拍摄视角下提取出并建立特征匹配关系。在尺度不变特征转换中，每个特征点描述子用128维的向量描述，两个特征点的匹配关系是通过判断其特征点描述子的向量夹角是否小于特定阈值所决定。需要说明的是，本发明的实施例不仅限于采用尺度不变特征转换方法对无失真的双目图像进行特征点提取，也可以为其它特征点提取方法，此处仅是示例性的展示。

步骤S3：根据特征点描述子建立双目图像的特征点匹配关系。该步骤即双目匹配步骤，其中，在双目匹配中成功匹配的特征点可以根据三维重建步骤恢复其空间深度，而未成功建立匹配的特征点在这一步骤淘汰。

进一步地，双目匹配步骤通过判断特征点描述子间的向量夹角是否小于特定阈值来确定特征点间的匹配关系，其具体操作步骤与之后的步骤S5类似。

具体地，假设左图提取到的特征点的描述子按行存储在矩阵L([DscpL₁，DscpL₂，...，DscpL_m]^T)中，右图中的特征点的描述子存储在矩阵R([DscpR₁，DscpR₂，...，DscpR_n]^T)中。对于左图中的每个特征点，遍历右图的全部特征点，寻找与其最相似的特征点建立从左图到右图的特征点映射关系。进一步地，相似性的度量由特征点描述子向量间的夹角给出：向量间的夹角越小，两向量相似度越高；两特征点特征描述子的相似度越高，特征点的匹配置信度越大。

步骤S4：根据双目图像的特征点匹配关系得到匹配特征点的水平视差，并根据匹配特征点的水平视差和双目图像捕获系统(双目相机)的参数计算匹配特征点的空间真实深度。其中，双目相机的参数例如包括：双目相机的焦距、基线及中心点等。

在本发明的一个实施例中，图像捕获系统包括左相机和右相机，步骤S4进一步包括：分别获取匹配特征点在左相机和右相机中的成像点的像素坐标；根据相机中心点的坐标计算左相机中的成像点和右相机中的成像点的水平视差；根据左相机中的成像点和右相机中的成像点的水平视差及空间几何关系得到匹配特征点的空间真实深度，其中，空间真实深度的计算公式如下：

$Z_p=\frac{B_L\×f_C}{Disparity}.$

其中，Z_p为匹配特征点的空间真实深度，B_L为基线，f_C为焦距，Disparity为水平视差。

具体地说，在以往的工作中，计算特征点的真实深度往往需要对环境有一定先验知识，例如：场景中已知深度的物体，置入环境的特征标识等等。这限制了算法在未知环境中的应用范围。在本发明的一个实施例中，无需预先获知空间中物体的深度或提前干涉环境，通过双目相机的成像特点对空间中捕获到的特征点进行三维重建工作，可获得其相对于拍摄时刻的相机参考系的空间三维坐标。

进一步地，参照图2所示，给出了三维重建空间几何原理示意图(俯视)。校正后的双目相机的相机参考系由(x₁，y₁，z₁)和(x₂，y₂，z₂)表示，其具有相同的成像平面。空间中点P在左相机中的成像点为P_L，在右相机中的成像点为P_R。在步骤S2中可以获得双目相机参数如焦距(f_C)，基线(B_L)，中心点等。特别地，对于双目图像中匹配的特征点，由于双目图像已经过预先校正，其成像点P_L与P_R只在水平方向上存在像素差值，在竖直方向上像素坐标相同。具体地，通过相机成像分辨率及中心点的坐标计算得到左相机中的成像点和右相机中的成像点所对应的实际空间中的距离，即两个成像点的水平视差(Disparity)。进一步地，根据空间几何关系计算得到匹配特征点(如点P)的空间真实深度Z_p，其中，空间真实深度的计算公式如下：

$Z_p=\frac{B_L\×f_L}{Disparity}.$

进一步地，根据相机小孔成像原理，可由空间真实深度Z_p，像素坐标(x_p，y_p)及相机焦距f_C计算出特征点在左相机参考系(后续计算以左相机为基准相机)中的三维坐标(X_p，Y_p，Z_p)，具体计算方法为：

$\{\begin{array}{c}{X}_p=x_p\frac{Z_p}{f_C}\\ Y_p=y_p\frac{Z_p}{f_C}\end{array}.$

步骤S5：获取当前帧双目图像的特征点，并将当前帧双目图像的特征点与世界地图中的特征点进行匹配。换言之，该步骤即世界地图匹配步骤，指在当前帧中提取到的特征点与世界地图中所包含的特征点间建立匹配关系。

在本发明的一个实施例中，该步骤S5进一步包括：将当前帧特征点的描述子按行存储在第一矩阵中，并将世界地图中的特征点的描述子按行存储在第二矩阵中；遍历世界地图中的全部特征点，从世界地图中分别提取与当前帧中的每个特征点相似度最高的特征点；根据相似度最高的特征点建立从当前帧特征点到世界地图的特征点映射关系表。

具体地说，假设当前帧提取到的特征点的描述子按行存储在第一矩阵A([DscpC₁，DscpC₂，...，DscpC_m]^T)中，世界地图中包含的全部特征点的描述子存储在第二矩阵B([DscpG₁，DscpG₂，...，DscpG_n]^T)中。对于当前帧的每个特征点，遍历世界地图中的全部特征点，寻找与其最相似的特征点建立起从当前帧到世界地图的特征点映射关系。具体地，特征点的相似度根据特征点描述子向量间的夹角确定，向量间的夹角越小，两向量相似度越高，当特征点描述子向量间的夹角最小时，特征点的相似度最高。

进一步地，世界地图匹配包含以下操作：第二矩阵B与特征点i的描述子列向量DscpC_i相乘，结果为n×1列向量，向量中的n个元素分别代表DscpC_i和DscpG_j(j＝1...n)的向量夹角的余弦值。进一步地，通过反余弦公式计算可得向量夹角，存储在n×1列向量Res_i中，其中元素Res_i(l)(l＝1...n)代表当前帧提取到的第i个特征点与世界地图中第l个特征点的特征点描述子的向量夹角。

进一步地，遍历Res_i中的每个元素，寻找其中最小的元素记作Res_i(l₁)，及第二小的元素记作Res_i(l₂)，其中，l₁和l₂表示最小及第二小元素在Res_i中的位置(即在世界地图所包含特征点中的位置)。进一步地，当且仅当满足以下条件时当前帧的第i个特征点与世界地图中第l₁个特征点成功匹配：

Res_i(l₁)＜0.8Res_i(l₂)。

进一步地，依次将世界地图特征点描述子存储矩阵B与当前帧每个特征点描述子DscpC_i相乘，相乘的过程可以通过矩阵运算加速。具体地，通过矩阵乘法B×A^T，可以由一条指令计算出当前帧全部特征点描述子与世界地图中各个特征点描述子的向量夹角的余弦矩阵。进一步地，反余弦的计算例如可以通过麦克劳林展开式近似为一系列矩阵乘法、矩阵加法运算。通过相关的矩阵运算库如MKL(IntelMathKernelLibrary，英特尔数学核心函数库)，cuBLAS(NVIDIACudaBasicLinearAlgebraSubroutines，英伟达基础线性代数子程序库)可实现基于CPU(CentralProcessingUnit，中央处理器)及GPU(GraphicProcessingUnit，图形处理器)的高性能矩阵运算。考虑到世界地图维数较大，因此，该方法有效提升了世界地图匹配及双目匹配步骤在软件端的算法运行效率，提高了算法处理图像的帧率，使移动平台的实时定位成为可能。

步骤S6：对当前帧双目图像的特征点与世界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点。具体地，世界地图匹配中会形成一些错误匹配的特征点，这些错误匹配的特征点具有相似的特征点描述子，但并不属于同一像点。错误匹配的特征点会给位姿估计引入误差，使得定位结果不准确。因此，在本发明的一个实施例中，采用随机抽样一致方法对当前帧双目图像的特征点与世界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点。具体地说，参照图3所示，给出了随机抽样一致方法的流程图。随机抽样一致方法假设正确匹配的特征点(局内点)在世界系下的坐标与当前相机参考系中的坐标间的坐标变换关系可由统一的模型描述，而错误匹配的特征点(局外点)不符合该坐标变换模型。同时，错误匹配的特征点属于测量中的异常，在整个特征点集中属小概率事件。

具体地，在本发明的一个实施例中，通过随机抽样一致方法去除错误匹配特征点，包括以下步骤：

S61：随机从N组成功匹配的特征点中选取m组匹配特征点，并建立全局配准方程，根据全局配准方程计算所述m组匹配特征点的坐标从世界坐标系变换到当前相机参考系的变换矩阵。

S62：将步骤S61中未选取的N-m组匹配特征点在当前相机参考系下的坐标根据步骤S61中计算的变换矩阵逆映射到世界坐标系，得到坐标逆映射结果CoorTemp，并计算CoorTemp与特征点在世界坐标系下的实际观察坐标CoorGlobal的欧氏距离，称为匹配代价。

S63：判断步骤S62中欧式距离(匹配代价)是否大于预设阈值，以及如果该欧氏距离大于预设阈值，则判定对应的特征点为局外点，即不符合步骤S61中的坐标变换矩阵；如果该欧式距离小于预设阈值，则判定对应的特征点为局内点，并统计本次计算中局内点的个数。其中，该预设阈值既可以通过在环境中预先设定实验测量，也可以通过经验给出，并且在设置预设阈值时要综合考虑实验环境与测量的过程噪声。

S64：迭代执行S61至S63(即重复执行步骤S61至S63)，并记录每次迭代过程中局内点的个数及坐标变换矩阵；

S65：从多次迭代过程中选取包含局内点个数最多的一次迭代，并将该次迭代中的局内点作为成功匹配的特征点，将局外点作为错误匹配的特征点，在后续计算中去除错误匹配的特征点(局外点)。

步骤S7：计算成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐标的变换矩阵，根据变换矩阵得到双目图像捕获系统(双目相机)相对初始位置的位姿变化估计值。

具体地，参照图4所示，给出了世界坐标系与相机坐标系关系示意图。世界坐标系确定为图像拍摄首帧时刻的相机坐标系，在后续计算中不再发生改变。相机参考系随着载有双目图像捕获系统的移动平台的运动，相对世界坐标系发生旋转与平移，位姿估计的目标是获得两个坐标系间的变换关系，即为移动平台相对初始位置所发生的运动。进一步地，通过步骤S1至步骤S6，获得成功匹配的特征点在世界坐标系下的坐标及其在相机参考系中的坐标。根据几何原理，两坐标系的坐标系变换与同一点在两个坐标系下的坐标变换互为逆过程，这是进行位姿估计的基础。

进一步地，获取成功匹配的特征点在世界坐标系下的坐标CoorGlobal及其在当前图像捕获系统参考系下的坐标CoorCamera。三维坐标间的变换关系可以用旋转矩阵R和平移矩阵T来描述，因此，可以计算成功匹配的特征点在三维坐标间的旋转矩阵R和平移矩阵T，即得到双目图像捕获系统相对初始位置的位姿变化估计值。具体地，位姿估计的过程为找到一组坐标变换关系R和T，使得通过该组坐标变换关系将匹配的特征点集从世界坐标系转换到相机参考系后，与其在当前帧相机参考系下的实际观察坐标的均方误差之和最小，也即旋转矩阵R和平移矩阵T满足如下关系式：

$\underset{R,T}{\min }\underset{inliers}{\Σ}\left|\right|CoorCamera-T-R\×CoorGlobsl|{|}^2.$

步骤S8：在首帧计算时，初始化世界地图，并在非首帧计算时，根据步骤S7中计算的变换矩阵更新世界地图中与当前帧成功匹配的特征点的三维坐标与特征描述子，并向世界地图中加入当前帧新观察到的特征点及其特征描述子。其中，世界地图包含已拍摄区域提取出的特征点在世界坐标系下的坐标以及特征点的特征描述子。在对当前帧计算时，成功匹配的特征点的世界坐标将被更新，新观察到的特征点将被加入世界地图。世界地图的使用与维护，一方面综合了各个帧的历史信息得到特征点在世界坐标系下三维坐标的准确估计，另一方面通过世界地图匹配步骤成功建立当前帧与历史帧的数据关联，通过误差抵偿效应有效减小了测量误差和累计误差。此外，所构建的世界地图包含稠密的空间特征，可用于后续障碍躲避，路径规划工作。

具体地，在首帧计算时，初始化所述世界地图，将首帧的图像捕获系统参考系确定为本次定位的世界坐标系，将首帧观测到的全部特征点的三维坐标与特征描述子加入世界地图中，作为世界地图的初始化构建。世界坐标系一经确定在后续计算中不再发生改变。

进一步地，在当前帧提取的特征点中，一部分与世界地图成功建立了匹配关系，根据步骤S7中的位姿估计得到坐标变换关系R和T，将特征点在当前相机参考系下的三维坐标逆投影回世界坐标系，作为特征点的一次新的观察结果CoorGlobal′，转换公式为：

CoorGlobal′＝inv(R)×(CoorCamera-T)。

进一步地，将成功匹配的特征点在世界坐标系下的三维坐标更新为多次观察结果的均值。具体地，世界地图中某个特征点的坐标是第n次观察到该特征点后的均值，在当前帧第n+1次观察到该特征点并计算得到观察结果CoorGlobal′，更新该点世界地图中三维坐标的公式为：

$CoorGlbal=\frac{CoorGlobal\×n+{\mathrm{CoorGlobal}}^{\′}}{n+1}.$

具体地，根据中心极限定理，测量误差近似符合以真值为均值的正态分布。通过对特征点的各次观察结果取算术平均，测量误差通过误差抵偿效应有效减小，世界地图中特征点的三维坐标更接近真值。

进一步地，成功匹配的特征点在世界地图中的特征点描述子由最近一帧图像中提取到的特征描述子更新。考虑到时间与空间的连续性，最近一帧图像与后续拍摄图像具有更相近的成像条件，使用最近一次的特征点描述子与后续图像中提取的特征点描述子建立成功匹配的几率更大。

进一步地，当前帧中未与世界地图成功匹配的特征点被认为是新加入的特征点。其最可能出现在视角变换后新的观察区域，也可能是对重叠观察区域特征点的补充提取。其在相机参考系下的三维坐标同样被逆投影到世界坐标系，并被加入到世界地图中，其特征点描述子也被加入世界地图。

进一步地，本发明的实施例具有相比扩展卡尔曼滤波同时定位与地图构建算法(EKF-SLAM)更简单的的计算复杂度。

具体地，本发明实施例的方法与基于扩展卡尔曼滤波同时定位与地图构建方法均包含步骤S5中所述的世界地图匹配，其计算复杂度为：设世界地图包含n个特征点，每个特征点的特征描述子为p维列向量，当前帧中有m个特征点参与与世界地图的匹配计算，在这一步骤矩阵乘法的计算复杂度为O(m×n×p)。在本发明的一个具体实施例中，设当前帧中有m_c个特征点与世界地图建立了匹配关系，步骤S6中的随机抽样一致方法与步骤S7中的位姿估计的计算复杂度约为O(3×m_c×k)，其中k代表随机抽样一致方法中的迭代次数。对于基于扩展卡尔曼滤波同时定位与地图构建方法，其位姿估计步骤需要维护世界地图的协方差矩阵，该步骤计算复杂度约为O(n×n)。考虑到迭代次数k及当前帧与世界地图匹配的特征点个数m_c远远小于世界地图中的特征点个数n，因此，本发明实施例方法相比基于扩展卡尔曼滤波同时定位与地图构建方法具有更简单的计算复杂度，考虑到移动平台的低计算能力，低功耗特性，本发明方法更适用于小型移动平台的实时定位任务。

为了验证本发明实施例所提出的基于视觉测程的同时定位与地图构建方法的性能，本发明的实施例通过双目图像捕获系统拍摄室内真实场景，并在MATLABR2014b实施了该方法的软件验证。其中，双目图像捕获系统安装在水平滑轨上以保证移动平台的直线运动。如表1所示，给出了双目图像捕获系统运动不同的距离、旋转不同的角度时本发明实施例中的方法给出的位姿估计值，其中X/Y/Z代表双目图像捕获系统在世界坐标系中的位置坐标，Pitch/Yaw/Roll代表双目图像捕获系统在世界坐标系中的俯仰角/偏航角/滚转角。测量真值是通过水平轨道的刻度尺示数及量角器所得，当双目图像捕获系统处于静止状态时，正负值表示对运动估计的标准差；当双目图像捕获系统发生运动时，表1中给出的估计结果是十次运动估计的平均值。从表1中可以看出，在该实施例中，运动估计误差小于两厘米，这与基于扩展卡尔曼滤波的同时定位与地图构建算法的定位精度相近。进一步地，计算静止状态下运动估计的误差，这指示了本发明的实施例对运动估计结果的漂移情况：

${\mathrm{Error}}_T=\sqrt{mean(X^2+Y^2+Z^2)}=1.23cm,$

表1

进一步地，如图5所示，给出了本发明一个实施例的室内定位运动轨迹及世界地图示意图。在该实施例中，双目图像捕获系统被放置在装配滑轮的小车上。推动小车以5cm/s的速度向前移动约80cm，逆时针旋转约65°，再沿该方向继续向前运动。可以看出，本发明提出的位姿估计方法给出了准确的位姿估计结果。同时，构建的世界地图中包含大量空间特征点的三维信息，可用于后续障碍躲避，路径规划等工作。

进一步地，如图6所示，给出了本发明一个实施例的室内无偏测试结果示意图。在该实施例中，双目图像捕获系统在水平滑轨上沿同一段距离往复运动20次，深度方向的距离估计在多次计算中没有发生单向的漂移。这说明本发明提出的位置估计方法不随时间推移而发生累计误差进而逐渐漂离真值。通过世界地图建立的数据关联，有效减小了对空间中闭环运动估计的累计误差。

综上，根据本发明实施例的基于视觉测程的同时定位与地图构建方法，通过双目相机定位系统，可以在无先验知识的室内环境中恢复空间特征点的真实深度；采用视觉测程方法进行位姿估计，有效的降低了同时定位与地图构建算法的计算复杂度，使得计算速度与世界地图规模得到提升，适用于实时定位移动平台，且可以工作在更大范围的室内空间中；同时，通过随机抽样一致方法有效的消除了错误匹配的特征点，避免为位姿估计引入误差信息；此外，通过建立并更新世界地图有效消除了测量误差的累计，保证了室内定位精度在厘米级别且不随时间发生漂移；此外，所构建的世界地图包含稠密的空间特征信息，可用于后续障碍躲避，路径规划工作。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”、“顺时针”、“逆时针”、“轴向”、“径向”、“周向”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”、“固定”等术语应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

在本发明中，除非另有明确的规定和限定，第一特征在第二特征“上”或“下”可以是第一和第二特征直接接触，或第一和第二特征通过中间媒介间接接触。而且，第一特征在第二特征“之上”、“上方”和“上面”可是第一特征在第二特征正上方或斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”可以是第一特征在第二特征正下方或斜下方，或仅仅表示第一特征水平高度小于第二特征。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种基于视觉测程的同时定位与地图构建方法，其特征在于，包括以下步骤：

S1：通过双目图像捕获系统同步采集双目图像，并对所述双目图像进行校正，以得到无失真的双目图像；

S2：对所述无失真的双目图像进行特征点提取，并根据提取的特征点生成特征点描述子；

S3：根据所述特征点描述子建立所述双目图像的特征点匹配关系；

S4：根据所述双目图像的特征点匹配关系得到匹配特征点的水平视差，并根据所述匹配特征点的水平视差和双目图像捕获系统的参数计算所述匹配特征点的空间真实深度；

S5：获取当前帧双目图像的特征点，并将所述当前帧双目图像的特征点与世界地图中的特征点进行匹配；

S6：对所述当前帧双目图像的特征点与世界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点；

S7：计算所述成功匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐标的变换矩阵，根据所述变换矩阵得到所述双目图像捕获系统相对初始位置的位姿变化估计值；

S8：在首帧计算时，初始化所述世界地图，并在非首帧计算时，根据所述变换矩阵更新世界地图中与当前帧成功匹配的特征点的三维坐标与特征描述子，并向世界地图中加入当前帧新观察到的特征点及其特征描述子。

2.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述S1进一步包括：

提取所述双目图像捕获系统的成像参数，并根据所述成像参数对所述双目图像进行校正，以得到无失真的双目图像。

3.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，在所述S2中，通过尺度不变特征转换方法对所述无失真的双目图像进行特征点提取。

4.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，在所述S3中，通过判断所述双目图像的特征点描述子间的向量夹角是否小于特定阈值建立所述双目图像的特征点间的匹配关系。

5.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，其中，所述双目图像捕获系统包括左相机和右相机，所述S4进一步包括：

分别获取所述匹配特征点在左相机和右相机中的成像点的像素坐标；

根据相机成像分辨率及所述中心点的坐标计算所述左相机中的成像点和右相机中的成像点的水平视差；

根据所述左相机中的成像点和右相机中的成像点的水平视差及空间几何关系得到所述匹配特征点的空间真实深度，其中，所述空间真实深度的计算公式如下：

$Z_p=\frac{B_L\×f_C}{Disparity}$

其中，Z_p为所述匹配特征点的空间真实深度，B_L为基线，f_C为焦距，Disparity为水平视差。

6.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述S5进一步包括：

将当前帧特征点的描述子按行存储在第一矩阵中，并将所述世界地图中的特征点的描述子按行存储在第二矩阵中；

遍历所述世界地图中的全部特征点，从所述世界地图中分别提取与所述当前帧中的每个特征点相似度最高的特征点；

根据所述相似度最高的特征点建立从所述当前帧特征点到所述世界地图的特征点映射关系表。

7.根据权利要求6所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述特征点的相似度根据所述特征点描述子向量间的夹角确定，其中，当所述特征点描述子向量间的夹角最小时，所述特征点的相似度最高。

8.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，在所述S6中，通过随机抽样一致方法对所述当前帧的特征点与世界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点，具体包括：

S61：随机从N组成功匹配的特征点中选取m组匹配特征点，并建立全局配准方程，根据所述全局配准方程计算所述m组匹配特征点的坐标从世界坐标系变换到当前相机参考系的变换矩阵；

S62：将未选取的N-m组匹配特征点的当前图像捕获系统参考系下的坐标根据所述变换矩阵逆映射到世界坐标系，得到坐标逆映射结果CoorTemp，并计算所述CoorTemp与特征点在世界坐标系下的实际观察坐标CoorGlobal的欧氏距离；

S63：判断所述欧式距离是否大于预设阈值，如果所述欧氏距离大于预设阈值，则判定对应的特征点为局外点，如果所述欧式距离小于所述预设阈值，则判定对应的特征点为局内点，并统计局内点的个数；

S64：迭代执行所述S61至S63，并记录每次迭代过程中局内点的个数及坐标变换矩阵；

S65：从多次迭代过程中选取包含局内点个数最多的一次迭代，并将该次迭代中的局内点作为成功匹配的特征点，将局外点作为错误匹配的特征点，并去除所述错误匹配的特征点。

9.根据权利要求1所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述S7进一步包括：

获取所述成功匹配的特征点在世界坐标系下的坐标CoorGlobal及在当前图像捕获系统参考系下的坐标CoorCamera；

计算所述成功匹配的特征点在三维坐标间的旋转矩阵R和平移矩阵T，根据所述旋转矩阵R和平移矩阵T得到所述双目图像捕获系统相对初始位置的位姿变化估计值，其中，所述旋转矩阵R和平移矩阵T满足如下关系式：

$\underset{R,T}{\min }\underset{inliers}{\Σ}\left|\right|CoorCamera-T-R\×CoorGlobal|{|}^2.$

10.根据权利要求9所述的基于视觉测程的同时定位与地图构建方法，其特征在于，所述S8进一步包括：

在首帧计算时，初始化所述世界地图，将首帧的图像捕获系统参考系确定为本次定位的世界坐标系，将首帧全部特征点的三维坐标与特征描述子加入世界地图；

在非首帧计算时，根据所述变换矩阵将当前帧成功匹配的特征点的三维坐标CoorCamera逆映射到世界坐标系CoorGlobal’，其中，转换公式为：

CoorGlobal′＝inv(R)×(CoorCamera-T)；

将所述成功匹配的特征点在世界坐标系下的坐标更新为多次观察的均值，其中，当第n+1次观察到该特征点，其世界地图中的坐标更新公式为：

$CoorGlobal=\frac{CoorGlobal\×n+{\mathrm{CoorGlobal}}^{\′}}{n+1};$

将所述成功匹配特征点的特征点描述子更新为最近一次观察的特征点描述子，并将当前帧新观察到的特征点三维坐标与特征点描述子加入所述世界地图。