CN110044354B - 一种双目视觉室内定位与建图方法及装置 - Google Patents

Abstract

本发明公开了一种双目视觉室内定位与建图方法及装置，包括：实时采集左右图像，并计算出相机的初始位姿；实时采集角速度信息和加速度信息，并预积分得到惯性测量单元状态量；构建一个包含若干图像帧的滑动窗口，以图像帧与帧之间的视觉误差项和惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，得到优化后的相机位姿及惯性测量单元测量值；构建词袋模型进行回环检测，校正优化后的相机位姿；提取左右图像特征后转换为单词并与离线地图的词袋进行匹配，成功则优化求解得到优化后的相机位姿，否则重新采集左右图像并进行词袋匹配。本发明可在未知环境中实现定位与建图，以及在已建图场景中的定位功能，具有较好的精度和鲁棒性。

Description

一种双目视觉室内定位与建图方法及装置

技术领域

本发明涉及一种双目视觉室内定位与建图方法及装置，属于定位导航技术领域。

背景技术

当前，室内机器人越来越多地应用于大型商场、仓库以及家庭等场景，如商场导购机器人、智能仓储机器人、家庭扫地机等。在这些应用领域中，机器人需要完成自主导航，而完成自主导航首先需要机器人具备室内定位的功能，也就是说，机器人需要知道自己当前在场景中的位置信息以及去往目的地的位置信息。

目前尚无成熟的室内高精度定位方案。GPS(Global Positioning System,全球定位系统)方案在室内定位误差大；依赖于在场景中粘贴二维码标签来完成室内定位的方案，标签易被损坏，维护成本过高；纯单目视觉定位方案缺乏场景的尺度信息；深度视觉的测量深度有限；激光雷达扫码的方案场景信息不丰富、重定位能力较差。

定位与地图构建(Simultaneous Localization And Mapping，简称SLAM)，通常是指在机器人或者其他载体上，通过对各种传感器数据进行采集和计算，生成对其自身位置姿态的定位和场景地图信息的系统。SLAM技术对于机器人或其他智能体的行动和交互能力至为关键，代表了上述能力的基础：知道在哪里，知道周围环境如何，进而知道下一步如何自主行动。在自动驾驶、服务型机器人、无人机、AR/VR等领域有着广泛的应用。

考虑到依靠单一传感器来实现SLAM存在固有的缺陷，例如对场景纹理性要求较高、重定位能力差等问题。因此，针对现有技术中室内导航方式导航效果较差的问题，目前尚未提出有效的解决方案。

发明内容

本发明所要解决的技术问题在于，解决现有室内导航方式场景纹理性要求较高、重定位能力差、导航效果较差的问题，提供一种双目视觉室内定位与建图方法及装置，通过视觉与惯性导航单元融合的室内定位方法，可以在复杂场景下进行定位、建图。

本发明具体采用以下技术方案解决上述技术问题：

一种双目视觉室内定位与建图方法，包括以下步骤：

步骤1、通过双目视觉传感器实时采集左右图像，并根据左右图像计算出相机的初始位姿；

步骤2、通过惯性测量单元实时采集角速度信息和加速度信息，并根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

步骤3、构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

步骤4、构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

步骤5、提取步骤1采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。

进一步地，作为本发明的一种优选技术方案，所述步骤1根据左右图像计算出相机的初始位姿，具体为：

对左右图像采用自适应直方图均衡化处理；

对构建的滑动窗口内左右图像分别提取特征点，为所述特征点找到对应双目匹配的特征点，并剔除误匹配点对；通过双目匹配获得特征点对应三维空间点的深度值，利用3D-2D点对之间的几何关系求解相机的初始位姿；

利用所述三维空间点和滑动窗口内其他特征点计算滑动窗口内其他图像帧位姿及特征点的三维坐标，以得到局部地图。

进一步地，作为本发明的一种优选技术方案，所述步骤1中为特征点找到对应双目匹配的特征点，包括：

通过光流法跟踪得到当前帧特征点在下一图像帧中的特征点位置，若跟踪到的特征点数量不满足阈值，则提取新的特征点。

进一步地，作为本发明的一种优选技术方案，所述步骤1中剔除误匹配点对，具体为：

在双目视觉左、右图像之间做双目匹配；

利用光流法对左图像上一帧特征点进行跟踪，得到左图像当前帧的特征点，并在两帧之间做RANSAC算法剔除误匹配；对右图像上一帧及当前帧直接做RANSAC算法剔除误匹配。

进一步地，作为本发明的一种优选技术方案，所述步骤2预积分得到的惯性测量单元状态量包括位移、速度、旋转和零偏值。

进一步地，作为本发明的一种优选技术方案，所述步骤3还包括判断最新获取的图像帧是否为关键帧，若是关键帧，则丢弃滑动窗口内时间戳最小的图像帧；若不是关键帧，则丢弃此图像帧且保留惯性测量单元测量值。

进一步地，作为本发明的一种优选技术方案，所述步骤3中判断最新获取的图像帧是否为关键帧的原则为：计算当前帧和上一关键帧之间跟踪点的平均视差，若超过设定阈值，则视当前帧为关键帧；或如果跟踪到的特征点数目超过一定阈值，则视该帧为关键帧。

本发明提出的一种双目视觉室内定位与建图装置，包括：

双目视觉传感器，用于实时采集左右图像；

惯性测量单元，用于实时采集角速度信息和加速度信息；

初始化模块，用于根据左右图像计算出相机的初始位姿，及根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

优化模块，用于构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

回环检测模块，用于构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

重定位模块，用于提取采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。

本发明采用上述技术方案，能产生如下技术效果：

本发明的方法及装置，通过双目视觉传感器获取左、右帧图像，在特征点匹配及剔除误匹配后通过最小化重投影误差获得相机当前位姿。利用前若干帧图像初始化惯性测量单元的参数，图像帧与帧之间被惯性测量单元测量值预积分所约束。通过构建词袋模型从而进行回环检测，即全局优化相机位姿。

本发明在视觉SLAM中融合了惯性测量单元的数据，利用惯性测量单元的加速度和角速度信息提高了视觉定位的精度及鲁棒性。通过建立滑动窗口解决双目视觉计算量超载的问题。在重新启动时可以利用之前建好的离线地图进行重定位来获取初始位姿，可以在未知位置环境中实现定位与建图，以及在已建图场景中的定位功能，具有较好的精度和鲁棒性。

附图说明

图1为本发明双目视觉室内定位与建图方法的原理示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

如图1所示，本发明设计了一种双目视觉室内定位与建图方法，可以进行多传感器融合定位与建图，实现了复杂场景下的定位、建图以及自主导航的功能，本方法包括以下步骤：

步骤1、通过双目视觉传感器实时采集左右图像，并根据左右图像计算出相机的初始位姿。

首先，针对双目视觉传感器获得的左右图像，若图像亮度过高或过低，则采用自适应直方图均衡化对其进行处理；然后分别提取构建的滑动窗口内特征点，为所述特征点找到对应双目匹配的特征点，例如FAST角点，通过光流法跟踪可得到特征点在下一帧图像中的位置信息，若跟踪到的特征点数量不满足阈值，则提取新的特征点，此时，在已跟踪到的特征像素点周围设置标志区域，在标志区域内不提取新的特征点，以保证特征点在图像中分布均匀。

然后，为所述特征点找到对应双目匹配的特征点，剔除误匹配点对，通过双目匹配获得特征点对应三维空间点的深度值，利用3D-2D点对的几何关系求解相机的初始位姿。其中，光流跟踪是基于图像灰度不变的假设，假设当前帧图像任一区域块和上一帧的区域块平均灰度不变，取区域内多对特征点，求解超定方程得到相机的初始位姿。

最后，对特征点进行三角化得到三维空间点，利用所述三维空间点和滑动窗口内其他特征点计算窗口内其他图像帧位姿及特征点的三维坐标，以得到局部地图。

其中，所述剔除误匹配特征点对，包括：在双目视觉左、右图像之间做双目匹配；利用光流法对左图像上一帧特征点进行跟踪，得到左图像当前帧的特征点，两帧之间做RANSAC算法剔除误匹配；对右图像上一帧及当前帧做RANSAC算法剔除误匹配，此时不需要进行特征点跟踪，双目匹配的结果可以直接应用在RANSAC算法中。

步骤2、通过惯性测量单元实时采集角速度信息和加速度信息，并根据角速度信息和加速度信息预积分得到惯性测量单元状态量，该状态量包括：位移、速度、旋转和零偏值。

IMU一般由三个单轴加速度计和陀螺仪组成，解算主要是通过加速度计测得的载体加速度和陀螺测得的载体相对于导航坐标系的角速度来对载体的位置、姿态及速度进行解算。在初始化阶段，可以利用相机若干帧来初始化IMU的零偏值，该若干帧例如是指系统启动后的最开始的20帧，用来估计IMU的零偏值。估计零偏值可以分为两个步骤：在不考虑加速度计零偏值的情况，利用惯性测量单元的陀螺仪测得的旋转量及滑动窗口图像测量的旋转量来初始化陀螺仪的零偏值，然后再估计加速度计的零偏值、重力加速度及尺度；对重力方向进行优化。只有惯性测量单元在初始化的时候得到充分的激励才能够初始化成功，即需要保证平移加速度的标准差超过一定的阈值。

预积分是指通过重新参数化，将状态量从世界坐标系转换到惯性坐标系，把关键帧之间的惯性测量单元测量值积分成相对运动的约束。避免了因为初始条件变化造成的重复积分，在下一个关键帧到来之前先对惯性测量单元数据积分。

步骤3、构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，即重投影误差，由图像帧与上一帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项，包括旋转量误差、速度量误差、平移量误差、陀螺仪偏置误差、加速度计偏执误差，以上述两个误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差使两项误差和最小，得到局部优化后的相机位姿及惯性测量单元测量值。

在同时定位与建图中，常见的几种约束条件包括：三维点到二维特征的映射关系（通过投影矩阵）、位姿和位姿之间的变换关系（通过三维刚体变换）、二维特征到二维特征的匹配关系（通过基础变换矩阵）。

由于位姿跟踪和惯性测量单元积分值存在一定的累积误差，将视觉的重投影误差和惯性测量单元的状态量误差值作为约束进行联合优化，利用非线性优化求解出更准确的相机位姿及惯性测量单元状态量。

随着双目图像的不断增加，为了保证优化时的计算实时性，本发明实施例采用了滑动窗口的形式，一个滑动窗口包含若干图像帧，首先，判断视觉传感器最新获取的图像帧是否为关键帧，若是关键帧，则丢弃滑动窗口内时间戳最小的图像帧；若不是关键帧，则丢弃此图像帧，保留惯性测量单元测量值，以此避免滑动窗口内图像帧数量过大，导致计算量超载，保证在局部优化时维持一定数量帧的窗口大小，对窗口内的图像帧位姿、三维坐标点及惯性测量单元测量值进行优化。

对关键帧与关键帧之间的惯性测量单元测量值预积分，例如对陀螺仪所测的角速度积分得到旋转量，对加速度计所测的加速度积分得到速度及平移量。

其中，所述判断视觉传感器最新获取的图片是否为关键帧的原则，包括关键帧的选择：一是计算当前帧和上一关键帧之间跟踪点的平均视差，若超过设定阈值，则视当前帧为关键帧。若因为视觉传感器纯旋转导致特征点无法三角化即无法计算跟踪点的视差时，短时间内采用惯性测量单元的陀螺仪观测值进行补偿，从而计算出视差。这种通过陀螺仪补偿的方式在本方法中只适用于关键帧的选择。二是如果跟踪到的特征点数目超过一定阈值，就视该帧为关键帧，避免特征点完全丢失。

步骤4、构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

回环检测又称闭环检测，是指机器人识别曾经到达场景的能力。例如利用词袋模型，将图像特征转换为单词，通过词袋匹配进行回环检测，利用全局优化校正当前位姿以及其他所有关键帧的位姿，根据校正后的位姿更新关键帧对应的地图点。如果检测成功，可以显著地减小累积误差。回环检测实质上是一种图像相似性检测地算法，对于视觉定位与建图而言，可以把图像中的视觉特征聚类，然后建立词典，进而可以寻找每个图像中含有哪些单词word。

本发明中通过构建词袋模型从而进行回环检测，即全局优化相机位姿，同时，具有重定位功能。包括：提取图像特征，将特征转换为单词，构建词袋模型；对当前帧进行图像相似性检测，若检测出相同场景，则进行全局优化，消除累积误差；当相机位姿完全丢失时，若检测到回环，可通过之前所建地图恢复出当前位姿。其中，闭环检测过程可找出一个确定的与当前图像帧最相似的一个帧作为闭环帧。

步骤5、利用视觉离线地图进行重定位，具体如下：

利用离线地图进行重定位，上述视觉离线地图包含关键帧和地图点两部分。

首先加载上述所建的地图，通过BOW词袋匹配完成在离线地图中的定位。在通过双目摄像头获取图像后，提取左右图像特征并转换为单词，与离线地图的词袋进行匹配。

如果词袋匹配成功，且通过了连续性检测，即筛选出闭环帧，计算sim3变换。当检测到闭环时，闭环连接的两个关键帧的位姿需要通过Sim3优化，以使得其尺度一致，优化求解两帧之间的相似变换矩阵，使得二维对应点的投影误差最小。求解出当前帧与闭环帧之间的位姿变换，进行后端优化得到优化位姿，即重定位成功。若重定位成功，则利用重定位后的若干帧用于初始化惯性测量单元的零偏值，随后进入视步骤3进行优化，继续执行定位与建图。

若词袋匹配不成功，即重定位失败，则不断从双目摄像头模块获取左右图像进行词袋匹配。

本发明还提出一种双目视觉室内定位与建图装置，该装置可以运用于上述方法中，该装置具体包括：

双目视觉传感器，用于实时采集左右图像；

惯性测量单元，用于实时采集角速度信息和加速度信息；

初始化模块，用于根据左右图像计算出相机的初始位姿，及根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

优化模块，用于构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

回环检测模块，用于构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

重定位模块，用于提取采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。

所述装置可以用于本发明的方法，通过双目视觉传感器获取左、右帧图像，在特征点匹配及剔除误匹配后通过最小化重投影误差获得相机当前位姿。利用前若干图像帧初始化惯性测量单元的参数，图像帧与帧之间被惯性测量单元测量值预积分所约束。通过构建词袋模型从而进行回环检测，即全局优化相机位姿。

综上，本发明的方法及装置，可在重新启动时可以利用之前建好的离线地图进行重定位来获取初始位姿，可以在未知环境中实现定位与建图，以及在已建图场景中的定位功能，具有较好的精度和鲁棒性。

上面结合附图对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (8)

1.一种双目视觉室内定位与建图方法，其特征在于，包括以下步骤：

步骤1、通过双目视觉传感器实时采集左右图像，并根据左右图像计算出相机的初始位姿；

步骤2、通过惯性测量单元实时采集角速度信息和加速度信息，并根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

步骤3、构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

步骤4、构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

步骤5、提取步骤1采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。

2.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤1根据左右图像计算出相机的初始位姿，具体为：

对左右图像采用自适应直方图均衡化处理；

对构建的滑动窗口内左右图像分别提取特征点，为所述特征点找到对应双目匹配的特征点，并剔除误匹配点对；通过双目匹配获得特征点对应三维空间点的深度值，利用3D-2D点对之间的几何关系求解相机的初始位姿；

利用所述三维空间点和滑动窗口内其他特征点计算滑动窗口内其他图像帧位姿及特征点的三维坐标，以得到局部地图。

3.根据权利要求2所述双目视觉室内定位与建图方法，其特征在于：所述步骤1中为特征点找到对应双目匹配的特征点，包括：

通过光流法跟踪得到当前帧特征点在下一图像帧中的特征点位置，若跟踪到的特征点数量不满足阈值，则提取新的特征点。

4.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤1中剔除误匹配点对，具体为：

在双目视觉左、右图像之间做双目匹配；

利用光流法对左图像上一帧特征点进行跟踪，得到左图像当前帧的特征点，并在两帧之间做RANSAC算法剔除误匹配；对右图像上一帧及当前帧直接做RANSAC算法剔除误匹配。

5.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤2预积分得到的惯性测量单元状态量包括位移、速度、旋转和零偏值。

6.根据权利要求1所述双目视觉室内定位与建图方法，其特征在于，所述步骤3还包括判断最新获取的图像帧是否为关键帧，若是关键帧，则丢弃滑动窗口内时间戳最小的图像帧；若不是关键帧，则丢弃此图像帧且保留惯性测量单元测量值。

7.根据权利要求6所述双目视觉室内定位与建图方法，其特征在于，所述步骤3中判断最新获取的图像帧是否为关键帧的原则为：计算当前帧和上一关键帧之间跟踪点的平均视差，若超过设定阈值，则视当前帧为关键帧；或如果跟踪到的特征点数目超过设定阈值，则视该帧为关键帧。

8.一种双目视觉室内定位与建图装置，其特征在于，包括：

双目视觉传感器，用于实时采集左右图像；

惯性测量单元，用于实时采集角速度信息和加速度信息；

初始化模块，用于根据左右图像计算出相机的初始位姿，及根据角速度信息和加速度信息预积分得到惯性测量单元状态量；

优化模块，用于构建一个包含若干图像帧的滑动窗口，以滑动窗口内三维点在当前图像帧下的投影位置与当前图像帧实际像素观测值之间产生的视觉误差项，和由图像帧与上一图像帧之间预积分项的值作为的测量值与估计值之间差值产生的惯性测量单元测量值的误差项为约束，对相机的初始位姿进行非线性优化，通过最小化误差得到优化后的相机位姿及惯性测量单元测量值；

回环检测模块，用于构建词袋模型进行回环检测，通过全局优化校正优化后的相机位姿，且根据校正后的相机位姿更新关键帧对应的地图点；

重定位模块，用于提取采集的左右图像特征后转换为单词，并与离线地图的词袋进行匹配，如果词袋匹配成功，优化求解当前帧与闭环帧之间的相似变换矩阵，使得二维对应点的投影误差最小以得到优化后的相机位姿，获得重定位后的若干图像帧用于初始化惯性测量单元的零偏值；如果词袋匹配不成功，则通过双目视觉传感器重新采集左右图像并进行词袋匹配。

Images