[发明专利]一种双目视觉室内定位与建图方法及装置

说明书

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

技术领域

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

背景技术

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

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

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

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

发明内容

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

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

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

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

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

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

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

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