[发明专利]一种基于视觉-IMU-轮速计融合的语义SLAM方法

说明书

本发明涉及计算机视觉技术领域，公开了一种基于视觉‑IMU‑轮速计融合的语义SLAM方法，将视觉测量、IMU测量和轮速计测量融合，并使用非线性优化方法来最大化后验概率，以求解最优的状态估计。然后将估计出的每帧图像位姿、对应得深度图以及2D语义分割结果输入到3D建图模块中，以构建全局3D语义网格地图。路面状况信息通过2D语义分割结果传递给SLAM前端的定位模块，并根据小车前方路面状况、IMU与轮速计预测位置误差以及IMU与轮速计测量值之间的对准误差来判断当前小车的运动是否异常，如异常，则主动从状态估计方程中删除当前帧对应的轮速计的预积分测量值，以提高复杂场景中姿态估计的鲁棒性。

技术领域

本发明涉及计算机视觉技术领域，具体涉及一种基于视觉-IMU-轮速计融合的语义SLAM方法。

背景技术

尽管可以使用融合惯性传感器和视觉数据来补偿纯视觉姿势估计方法固有的比例尺不确定性和较差的快速运动跟踪，但组合方法在视觉传感器无法获得可用信息的无纹理或弱光环境中无效。在这种情况下，视觉惯性方法将退化为仅基于惯性导航的航位推测法，并且姿势误差会随着时间迅速增加。对于带有轮速传感器的移动机器人，可以融合摄像头，惯性传感器和轮速传感器，以提高复杂场景中姿态估计的鲁棒性。

地面机器人在室内航行时经常受到限制的运动(近似平面，并且大部分以恒定速度或加速度沿着弧线或直线运动)会改变VINS的可观性并使某些额外的自由度变得不可观。当局部加速度不变时，无法将真实IMU加速度的大小与加速度计偏置的大小区分开，因为它们至少是暂时恒定的。所以，真实IMU加速度的大小可以是任意的，从而导致尺度模糊。当没有旋转运动时，无法将局部重力加速度的方向与加速度计偏置的方向区分开，因为它们至少是暂时恒定的。所以，翻滚角和俯仰角变得模糊。在这两种不可观察的情况中，第二种情况的不可观可以通过允许机器人偏离其直线路径来轻松消除，但要使尺度可观察是非常具有挑战性的，因为这将要求机器人不断更改其加速度，这将增加其移动系统的磨损。因此，我们通过扩展VINS并结合机器人的轮速计提供的测量值来解决此问题并确保VINS的可观察性。

但轮速计仅在小车处于平面运动且无运动异常时有正确的测量，即如果机器人在倾斜的环境中移动(例如路面不平，斜坡，路面有小坑，减速带等等)或车轮打滑时，错误的轮速计测量会使得SLAM算法将无法正确估计机器人的姿势(尺度估计不准)，并且定位可能会失败。

发明内容

发明目的：本发明提供一种基于视觉-IMU-轮速计融合的语义SLAM方法，解决现有技术中存在的问题。

技术方案：本发明提供了一种基于视觉-IMU-轮速计融合的语义SLAM方法，包括如下步骤：

S1：将深度相机固定安装在小车上，小车上安装有轮速计，采集前一时刻和当前时刻的彩色图和深度图、IMU测量值以及轮速计读数；

S2：对S1中的IMU测量值以及轮速计读数计算IMU和轮速计的预积分；

S3：计算相邻两帧间的视觉重投影误差、IMU预积分误差、轮速计预积分误差；

S4：位姿估计，对S3中的视觉重投影误差、IMU预积分误差、轮速计预积分误差进行非线性优化，求解出滑动窗口中所有帧的位姿；

S5：使用卷积神经网络对路面经行2D语义分割，分割出路面不平的区域，得到2D路面语义分割结果图；

S6：对S4中的每一帧的位姿、S5中的2D路面语义分割结果图、S1中的深度图输入到全局3D语义建图模块中，以构建带有语义标签的全局3D语义网格地图；

S7：对小车运动异常进行检测；

S8：位姿优化，当检测到小车异常移动时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。

进一步地，所述S2中计算IMU和轮速计的预积分具体为：