[发明专利]基于视觉惯导融合的移动机器人地图构建方法及相关设备

说明书

本发明公开了基于视觉惯导融合的移动机器人地图构建方法及相关设备，该方法包括：S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；S2，构建情形评估函数实时分析姿态趋势；S3，确定融合状态局部期望值并输出该状态；S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。本发明保证了相机和IMU数据之间能准确融合，系统读取当前地图点位姿信息效率高、误差小。

技术领域

本发明属于同步定位与地图创建(Simultaneous Location And Mapping，SLAM)技术领域，涉及基于视觉惯导融合的移动机器人地图构建方法及相关设备。

背景技术

同步定位与地图创建(Simultaneous Location And Mapping，SLAM)的核心问题是，要求机器人在一个陌生的环境中，首先要探索环境从而了解环境(构建地图)，同步运用地图追踪机器人在该环境中的位置(定位)。对SLAM问题的传统解决方法主要是基于数学概率方法，其中，卡尔曼滤波器、粒子滤波器和极大期望算法等是机器人SLAM问题的基本解决方法。虽然这些传统的SLAM算法中仍使用激光测距和声呐测距采集信息，但是这些传感器采集信息，往往得到的数据不精确且使用有一定的局限性，更多情况下激光测距传感器已被视觉传感器所替代。

然而传统视觉SLAM算法仅依靠单一传感器已无法满足科研的需求，也无法在复杂快速运动的环境中及时捕捉机器人的运动信息并反馈给主控机进行运动补偿。科研工作者开始融合两种不同传感器的信息意图互补两者的不足，期望惯性测量单元(InertialMeasurement Unit，IMU)可以在快速运动时替代相机捕捉相机运动的物理信息提高视觉SLAM定位精度，而相机也可以弥补IMU在慢速运动下因使用时间过长使得累计误差漂移的问题。

但目前的融合方法在构建地图时，并未实现实时对运动的姿态趋势进行分析，不能确定惯导融合的局部实时最优状态，因此相机和IMU数据无法准确融合，导致读取当前地图点位姿信息效率低、误差大。同时具有后端计算量较大，运行时间长，处理误差实时性差等问题。

发明内容

本发明的目的在于提供基于视觉惯导融合的移动机器人地图构建方法，以解决现有技术中相机和IMU数据无法准确融合导致读取当前地图点位姿信息效率低、误差大以及后端处理误差实时性差的技术问题。

所述的基于视觉惯导融合的移动机器人地图构建方法，所述方法包括：

步骤S1，根据视觉姿态和惯性姿态融合状态确定融合达成度，完成视觉初始化、视觉惯性校准对齐以及惯性初始化；

步骤S2，构建情形评估函数实时分析姿态趋势；

步骤S3，确定融合状态局部期望值并输出该状态：通过评估函数值与阈值比较判断此时机器人的运动状态观测数据是否具有高确定性，同时增加时间阈值设定双重约束评估状态以确定是否终止视觉惯性联合初始化并输出当前状态；

步骤S4，根据公共地图点稀疏程度将关键帧分为强共视关键帧和弱共视关键帧；

步骤S5，根据分级结果将强共视关键帧送入滑动窗口并利用帧间位姿密集连续性优化强共视关键帧，借助观测地图点的联系对自感知地图进行绘制和校正。

优选的，所述步骤S1中，IMU测量值由真实值、随机游走偏差和白噪声三个部分组成，加速度计仍受到重力加速度的影响，将IMU运动状态变量利用中值法离散化以方便相机跟IMU进行状态融合，离散化的运动状态变量算式如下：