[发明专利]一种移动机器人定位与建图的方法

说明书

一种移动机器人定位与建图的方法，包括移动机器人主体、单目相机、IMU传感器和轮速计传感器，单目相机镜头朝向所述移动机器人主体的正前方；处理单目相机获取的每帧图像，获得单目相机的处理值，对每两相邻帧图像间的IMU传感器测量值进行预积分，获得IMU传感器的处理值，对每两相邻帧图像间的轮速计传感器测量值进行预积分，获得轮速计传感器的处理值；将单目相机、IMU传感器和轮速计传感器的处理值加入滑动窗口；获取滑动窗口的处理值，对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子进行初始化；初始化成功后，构建因子图模型获得待估计状态值，对滑动窗口的处理值和待估计状态值进行非线性优化，得到所述移动机器人的位姿和地图。

技术领域

本发明涉及移动机器人领域，具体涉及一种移动机器人定位与建图的方法。

背景技术

同步定位与建图(SLAM，Simultaneous Localization and Mapping)技术是移动机器人的关键技术。该技术可以让机器人利用自身携带的传感器提取到环境信息和已有地图，从而进行定位和地图更新。目前SLAM技术大致可以分为两大类：激光SLAM和视觉SLAM。

激光SLAM使用的核心传感器之一是激光雷达，视觉SLAM使用的核心传感器之一是视觉传感器，但是同等级激光雷达的价格普遍高于同等级的视觉传感器。现有技术当中，视觉SLAM采用的单目相机和IMU传感器信息融合是主流方案之一，被称为单目视觉惯性里程计。因为该方案采用的单目相机和IMU传感器价格较为低廉，而且技术也较为成熟，所以应用较为广泛。该方案通过获取预积分后的IMU测量值和单目相机对环境中特征点的观测值，采用非线性优化方法对机器人的位姿、线速度、IMU线加速度偏差、角速度偏差以及特征点的三维坐标进行实时状态估计。

但是，该方案在某些情况下存在额外的不可观性。例如当机器人在平面上以固定的线加速度运动时，存在尺度因子不可观性；当机器人在平面上沿直线运动时，存在航向角、俯仰角和横滚角的不可观性。某个值存在不可观性，意味着该值可能有多个解。因此，该方案会造成移动机器人位姿估计不准确，进而无法构建高准确度的地图。

发明内容

为了克服单目视觉惯性里程计方案在某些情况下存在额外的不可观性造成移动机器人位姿估计不准确，进而无法构建高准确度地图的问题，本发明在单目视觉惯性里程计方案的基础上加入轮速计传感器和平面约束信息，提出一种移动机器人的定位与建图方法，可以提高移动机器人的定位精度和建图的准确性。

基于上述目的，本发明的技术方案如下：

提供一种移动机器人定位与建图的方法，所述移动机器人包括移动机器人主体、单目相机、IMU传感器和轮速计传感器，所述单目相机镜头朝向所述移动机器人主体的正前方，其中IMU传感器包括IMU加速度计和IMU陀螺仪；

处理单目相机获取的每帧图像，获得单目相机的处理值，对每两相邻帧图像间的IMU传感器测量值进行预积分，获得IMU传感器的处理值，对每两相邻帧图像间的轮速计传感器测量值进行预积分，获得轮速计传感器的处理值；

将所述单目相机、IMU传感器和轮速计传感器的处理值加入滑动窗口；获取滑动窗口的处理值，对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子进行初始化；

初始化成功后，构建因子图模型获得待估计状态值，对滑动窗口的处理值和待估计状态值进行非线性优化最小化残差项，得到所述移动机器人的位姿和地图。

其中，成功完成对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子的初始化的方法包括：对所述移动机器人的位姿、特征点的坐标、IMU陀螺仪偏差、线速度和尺度因子进行初始化，若初始化失败，对所述滑动窗口进行更新，之后再次获取所述滑动窗口的处理值，直至成功初始化。