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

权利要求书

1.一种基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，包括如下步骤：

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

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

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

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

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

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

S7：对小车运动异常进行检测，小车运动异常检测包括：

基于路面语义分割的小车运动异常检测：小车前方检测到有路面不平的路况，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；

基于惯性导航和轮速计一致性的小车运动异常检测：从状态估计器中获取IMU在最后一次接收相机帧时在世界坐标系中的位置、速度和姿态，具有已知的起始姿态和速度，基于惯性导航的IMU航位推算算法在没有重力加速度的情况下，基于IMU测量，在短时间内预测小车的实时姿态和速度；轮速计预积分算法根据前一帧的运动状态预测小车的实时姿态和速度，根据轮速计预积分所获得的位置协方差，从IMU或轮速计的预先点推算出机器人的位置，通过两种方法计算出的马尔可夫距离均大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；

基于轮速计和IMU对准的小车运动异常检测：直接将IMU预积分与轮速计预积分进行线性对准，然后在最新帧中计算IMU预设点和轮速计预设点之间的偏差，如果马氏距离的偏差大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；

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

2.根据权利要求1所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述S2中计算IMU和轮速计的预积分具体为：

其中，i和j分别表示相机拍摄第k张图片和第k+1张图片的时刻；l和l+1是i和j之间的两个时刻；是预积分阶段IMU和轮速计之间的外参的值；和分别为加速度计、陀螺仪和轮速计的读数；δt_l是时刻l和l+1之间的时间间隔；和分别表示在某个时刻t的加速度计和陀螺仪偏置；是预积分结果，为IMU的位移、速度变化和相对旋转；表示轮速计的位移。

3.根据权利要求1所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述S3中视觉重投影误差、IMU预积分误差、轮速计预积分误差计算方法为：

其中，g^w是世界坐标系下的重力加速度，Δt_k表示图像帧k和k+1之间的时间间隔；和是被IMU偏差的变化所补偿和以及IMU-轮速计外参补偿后的预积分结果，在每一帧对应时刻下的IMU到世界坐标系的位移和旋转轮速计到IMU的平移

4.根据权利要求1所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述S4中位姿估计方法为：

其中，c(x)表示代价函数，l表示地图点的索引，B_l是在其上出现地图点l的图像帧集；K是滑动窗口中的帧数，表示重投影误差，表示IMU-轮速计误差，e_m表示边缘化残差，W_r是所有重投影项的信息矩阵，待优化量x包含每帧状态量x_k，每个地图点的逆深度λ_l，相机到IMU的旋转和平移以及轮速计到IMU的旋转和平移其中，每帧状态量x_k包括在每一帧对应时刻下的IMU到世界坐标系的位移速度和旋转以及IMU的加速度偏置和角速度偏置