基于视觉测程的同时定位与地图构建方法
【技术领域】
[0001] 本发明涉及计算机及电子信息技术领域，特别涉及一种基于视觉测程的同时定位 与地图构建方法。
【背景技术】
[0002] 近年来，移动工作平台如无人机，移动机器人等成为研究的热点之一。这些设备具 有很高的灵活性，广泛应用于灾难救助、地质勘测等场景中。机器人自主导航系统的主要技 术包括:构建空间环境三维地图、自身定位、路线规划以及障碍躲避等。其中，构建环境空间 的三维地图与自身定位是问题的核心，而路线规划、障碍躲避依托于无人机对环境的认知 以及定位的结果。
[0003] 目前室外无人机已经有成熟的导航系统，例如全球定位系统以及惯性导航系统。 在室内导航中，复杂的闭合空间环境对无线信号有屏蔽和干扰，因此，依赖于卫星的定位系 统在室内失效；同时，惯性导航系统的累计误差和对初值敏感性使得它不适用于高精度的 室内定位。主动传感器如激光扫描仪，声纳等可以测量得到一维距离及偏角用于位置估计， 但主动传感器通常重量沉、价格昂贵且功耗较大，不适用于小型移动平台；同时，单一的数 据类型和低数据率限制了室内定位精度的提升。
[0004] 近年来随着视觉传感器价格逐渐降低，其被更广泛的应用于视觉导航任务中。视 觉信息对于生物而言是更直观的定位方式，且视觉传感器可以提供更丰富的数据信息，不 仅可以实现机器人自身位置和姿态的估计，也可以实现对空间的认知。正确且稠密的空间 认知信息可以被有效的应用到路线规划及障碍躲避等后续问题中。其中双目相机系统可以 在不提供环境先验信息的情况下获取真实深度实现定位，这扩大了移动平台在未知环境中 的应用范围。
[0005] 视觉测程是机器人导航领域的基础算法。它的主要思想是，通过计算前后帧相同 特征点的坐标变换关系获得帧间运动信息，通过逐帧累积的积分思想获得机器人相对初始 位置的位姿估计。但实际实验表明，基于视觉测程的定位算法虽然提供了复杂度较低的位 姿估计方法，但其逐帧累积的方式使得位置估计随时间发生漂移，也即产生累计误差。
[0006] 同时定位与地图构建算法（Simultaneous Localizationand Mapping，SLAM)是近 年来机器人导航领域的研究热点。它是指在未知环境中移动的机器人通过传感器测量数据 恢复出自身的运动轨迹，同时构建出关于运动空间的一张增量式地图。在该过程中，机器人 自身位置与空间地标具有双重不确定性，通过建立概率学模型解决该问题。基于扩展卡尔 曼滤波的同时定位与地图构建算法被证明为目前收敛性最好的SLAM算法，但其明显缺陷是 计算复杂性很高，与世界地图所包含的特征点个数的平方成正比。平方项的计算复杂度一 方面限制了世界地图的规模，也即限制了所处环境中的特征数目和环境面积;另一方面，制 约了定位导航算法的计算速度。考虑室内移动机器人低负载、低功耗的特性，在移动平台商 实现传统的SLAM系统并达到实时处理速度受到制约。
【发明内容】
[0007] 本发明旨在至少在一定程度上解决上述相关技术中的技术问题之一。
[0008] 为此，本发明的目的在于提出一种基于视觉测程的同时定位与地图构建方法，该 方法仅依靠双目视觉导航，无需融合其他传感器数据，无需对环境有先验知识，具有较低的 计算复杂度，厘米级的定位精度及对位置估计的无偏特性。
[0009] 为了实现上述目的，本发明的实施例提出了一种基于视觉测程的同时定位与地图 构建方法，包括以下步骤:S1:通过双目图像捕获系统同步采集双目图像，并对所述双目图 像进行校正，以得到无失真的双目图像;S2:对所述无失真的双目图像进行特征点提取，并 根据提取的特征点生成特征点描述子;S3:根据所述特征点描述子建立所述双目图像的特 征点匹配关系;S4:根据所述双目图像的特征点匹配关系得到匹配特征点的水平视差，并根 据所述匹配特征点的水平视差和双目图像捕获系统的参数计算所述匹配特征点的空间真 实深度;S5:获取当前帧双目图像的特征点，并将所述当前帧双目图像的特征点与世界地图 中的特征点进行匹配;S6:对所述当前帧双目图像的特征点与世界地图中的特征点的匹配 结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的特征点；S7:计算所述成功 匹配的特征点在世界坐标系下的坐标与当前参考坐标系下的三维坐标的变换矩阵，根据所 述变换矩阵得到所述双目图像捕获系统相对初始位置的位姿变化估计值;S8:在首帧计算 时，初始化所述世界地图，并在非首帧计算时，根据所述变换矩阵更新世界地图中与当前帧 成功匹配的特征点的三维坐标与特征描述子，并向世界地图中加入当前帧新观察到的特征 点及其特征描述子。
[0010] 另外，根据本发明上述实施例的基于视觉测程的同时定位与地图构建方法还可以 具有如下附加的技术特征：
[0011] 在一些示例中，所述S1进一步包括:提取所述双目图像捕获系统的成像参数，并根 据所述成像参数对所述双目图像进行校正，以得到无失真的双目图像。
[0012] 在一些示例中，在所述S2中，通过尺度不变特征转换方法对所述无失真的双目图 像进行特征点提取。
[0013] 在一些示例中，在所述S3中，通过判断所述双目图像的特征点描述子间的向量夹 角是否小于特定阈值建立所述双目图像的特征点间的匹配关系。
[0014] 在一些示例中，所述图像捕获系统包括所述左相机和右相机，所述S4进一步包括： 分别获取所述匹配特征点在左相机和右相机中的成像点的像素坐标;根据所述相机的中心 点的坐标计算所述左相机中的成像点和右相机中的成像点的水平视差;根据所述左相机中 的成像点和右相机中的成像点的水平视差及空间几何关系得到所述匹配特征点的空间真 实深度，其中，所述空间真实深度的计算公式如下：
[0015]
[0016] a |的空间真实深度，Bl为基线，fc为焦距，Disparity为水 平视差。
[0017] 在一些示例中，所述S5进一步包括:将当前帧特征点的描述子按行存储在第一矩 阵中，并将所述世界地图中的特征点的描述子按行存储在第二矩阵中；遍历所述世界地图 中的全部特征点，从所述世界地图中分别提取与所述当前帧中的每个特征点相似度最高的 特征点;根据所述相似度最高的特征点建立从所述当前帧特征点到所述世界地图的特征点 映射关系表。
[0018] 在一些示例中，所述特征点的相似度根据所述特征点描述子向量间的夹角确定， 其中，当所述特征点描述子向量间的夹角最小时，所述特征点的相似度最高。
[0019] 在一些示例中，在所述S6中，通过随机抽样一致方法对所述当前帧的特征点与世 界地图中的特征点的匹配结果进行分析，以去除其中错误匹配的特征点，得到成功匹配的 特征点，具体包括:S61:随机从N组成功匹配的特征点中选取m组匹配特征点，并建立全局配 准方程，根据所述全局配准方程计算所述m组匹配特征点的坐标从世界坐标系变换到当前 相机参考系的变换矩阵；S62:将未选取的N-m组匹配特征点的当前相机参考系下的坐标根 据所述变换矩阵逆映射到世界坐标系，得到坐标逆映射结果CoorTemp，并计算所述 CoorTemp与特征点在世界坐标系下的实际观察坐标CoorGlobal的欧氏距离;S63:判断所述 欧式距离是否大于预设阈值，如果所述欧氏距离大于预设阈值，则判定对应的特征点为局 外点，如果所述欧式距离小于所述预设阈值，则判定对应的特征点为局内点，并统计局内点 的个数；S64:迭代执行所述S61至S63，并记录每次迭代过程中局内点的个数及坐标变换矩 阵；S65:从多次迭代过程中选取包含局内点个数最多的一次迭代，并将该次迭代中的局内 点作为成功匹配的特征点，将局外点作为错误匹配的特征点，并去除所述错误匹配的特征 点。
[0020] 在一些示例中，所述S7进一步包括:获取所述成功匹配的特征点在世界坐标系下 的坐标CoorGlobal及在当前图像捕获系统参考系下的坐标CoorCamera;计算所述成功匹配 的特征点在三维坐标间的旋转矩阵R和平移矩阵T，根据所述旋转矩阵R和平移矩阵T得到所 述双目图像捕获系统相对初始位置的位姿变化估计值，其中，所述旋转矩阵R和平移矩阵T 满足如下关系式：
[0021；
[0022] 在一些示例中，所述S8进一步包括:在首帧计算时，初始化所述世界地图，将首帧 的图像捕获系统参考系确定为本次定位的世界