[发明专利]一种基于RGBD传感器和IMU传感器的定位方法

说明书

本发明公开了一种基于RGBD传感器和IMU传感器的定位方法，过程如下：首先，移动机器人搭载RGBD传感器和IMU传感器采集数据；其次，采用视觉SLAM算法对RGBD传感器数据进行初始化获得其位姿信息，同时对IMU传感器数据进行预积分，然后对移动机器人初始化获取其初始状态信息；基于重投影误差优化跟踪移动机器人的位姿信息、速度和IMU传感器偏置；同时，采用基于滑动窗口的非线性优化算法，实时优化位姿信息和地图；加入回环检测，避免零偏情况的出现。该定位方法综合运用RGBD传感器和IMU传感器，克服纯视觉SLAM算法在纯旋转以及背景单一时容易跟踪失败和IMU传感器在长时间运动中产生零偏现象的缺点。

技术领域

本发明涉及视觉SLAM以及传感器融合技术领域，具体涉及一种基于RGBD传感器和IMU传感器的定位方法。

背景技术

同时定位与地图构建(Simultaneous Localization and Mapping)简称SLAM，最先是由SmithSelf和Cheeseman在1988年提出来的，被认为是实现真正全自主移动机器人的关键，SLAM问题可以描述为：机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图。只通过相机获取环境信息的SLAM被称为视觉SLAM，由于相机具有价格低、功耗低、重量轻、体积小、图像信息丰富等特点，所以视觉SLAM受到了国内外广大学者和专家的研究。

IMU(Inertial Measurement unit)为惯性测量单元，一个IMU传感器内会装有三轴的陀螺仪和三个方向的加速度计，来测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。IMU传感器常用在需要进行运动控制的设备，例如汽车和机器人上。

视觉SLAM算法虽然可以通过相机传感器数据估计自己在三维空间中的位姿信息，但在快速运动或纯旋转时容易造成特征点跟踪丢失，同时视觉SLAM一般采取第一帧作为世界坐标系，估计出来的位姿信息是相对于第一帧图像的位姿信息，而不是如东北天这样的世界坐标系，而IMU传感器可以提供各个时刻的位姿信息，但在长时间运动中存在零漂问题，导致估计的位姿信息产生偏差，而视觉SLAM中的回环能很好地解决这一问题。因此视觉SLAM和IMU传感器的融合能更好的估计机器人在未知空间中的位姿信息。

发明内容

本发明的目的是为了解决现有技术中的上述问题，提供一种基于RGBD传感器和IMU传感器的定位方法。

本发明的目的可以通过采取如下技术方案达到：

一种基于RGBD传感器和IMU传感器的定位方法，应用于搭载RGBD传感器和IMU传感器的移动机器人，实现对移动机器人的定位，所述的定位方法包括以下步骤：

S1、传感器数据采集：移动机器人搭载RGBD传感器和IMU传感器在三维空间中自由移动，采集RGBD传感器数据和IMU传感器数据；

S2、IMU预积分：采用视觉SLAM算法根据RGBD传感器不同帧的图像数据质量选定关键帧，基于选定的关键帧，采用IMU预积分方程计算关键帧之间的IMU传感器数据的预积分值；

S3、移动机器人初始化：采用视觉SLAM算法对RGBD传感器初始化得到RGBD传感器的位姿信息，然后根据RGBD传感器的位姿信息和对应关键帧的IMU预积分对移动机器人初始化获取移动机器人的初始化状态信息，其中，所述的移动机器人的状态信息包括移动机器人的位姿信息、速度和IMU传感器的偏置；

S4、移动机器人状态的跟踪优化：采用基于重投影误差的优化算法对估计的移动机器人的状态信息进行优化跟踪，然后采用基于滑动窗口的优化算法对移动机器人的状态进行非线性优化；

S5、回环检测：根据RGBD传感器数据，采用基于词袋模型的回环优化算法对RGBD传感器的位姿信息和特征点进行回环优化，然后采用基于滑动窗口的优化算法对IMU传感器误差项进行优化。