一种基于视觉-IMU-轮速计融合的语义SLAM方法与流程

一种基于视觉
‑
imu
‑
轮速计融合的语义slam方法
技术领域
1.本发明涉及计算机视觉技术领域，具体涉及一种基于视觉
‑
imu
‑
轮速计融合的语义slam方法。


背景技术：

2.尽管可以使用融合惯性传感器和视觉数据来补偿纯视觉姿势估计方法固有的比例尺不确定性和较差的快速运动跟踪，但组合方法在视觉传感器无法获得可用信息的无纹理或弱光环境中无效。在这种情况下，视觉惯性方法将退化为仅基于惯性导航的航位推测法，并且姿势误差会随着时间迅速增加。对于带有轮速传感器的移动机器人，可以融合摄像头，惯性传感器和轮速传感器，以提高复杂场景中姿态估计的鲁棒性。
3.地面机器人在室内航行时经常受到限制的运动(近似平面，并且大部分以恒定速度或加速度沿着弧线或直线运动)会改变vins的可观性并使某些额外的自由度变得不可观。当局部加速度不变时，无法将真实imu加速度的大小与加速度计偏置的大小区分开，因为它们至少是暂时恒定的。所以，真实imu加速度的大小可以是任意的，从而导致尺度模糊。当没有旋转运动时，无法将局部重力加速度的方向与加速度计偏置的方向区分开，因为它们至少是暂时恒定的。所以，翻滚角和俯仰角变得模糊。在这两种不可观察的情况中，第二种情况的不可观可以通过允许机器人偏离其直线路径来轻松消除，但要使尺度可观察是非常具有挑战性的，因为这将要求机器人不断更改其加速度，这将增加其移动系统的磨损。因此，我们通过扩展vins并结合机器人的轮速计提供的测量值来解决此问题并确保vins的可观察性。
4.但轮速计仅在小车处于平面运动且无运动异常时有正确的测量，即如果机器人在倾斜的环境中移动(例如路面不平，斜坡，路面有小坑，减速带等等)或车轮打滑时，错误的轮速计测量会使得slam算法将无法正确估计机器人的姿势(尺度估计不准)，并且定位可能会失败。


技术实现要素：

5.发明目的：本发明提供一种基于视觉
‑
imu
‑
轮速计融合的语义slam方法，解决现有技术中存在的问题。
6.技术方案：本发明提供了一种基于视觉
‑
imu
‑
轮速计融合的语义slam方法，包括如下步骤：
7.s1：将深度相机固定安装在小车上，小车上安装有轮速计，采集前一时刻和当前时刻的彩色图和深度图、imu测量值以及轮速计读数；
8.s2：对s1中的imu测量值以及轮速计读数计算imu和轮速计的预积分；
9.s3：计算相邻两帧间的视觉重投影误差、imu预积分误差、轮速计预积分误差；
10.s4：位姿估计，对s3中的视觉重投影误差、imu预积分误差、轮速计预积分误差进行非线性优化，求解出滑动窗口中所有帧的位姿；
11.s5：使用卷积神经网络对路面经行2d语义分割，分割出路面不平的区域，得到2d路面语义分割结果图；
12.s6：对s4中的每一帧的位姿、s5中的2d路面语义分割结果图、s1中的深度图输入到全局3d语义建图模块中，以构建带有语义标签的全局3d语义网格地图；
13.s7：对小车运动异常进行检测；
14.s8：位姿优化，当检测到小车异常移动时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。
15.进一步地，所述s2中计算imu和轮速计的预积分具体为：
[0016][0017][0018][0019][0020][0021][0022]
其中，i和j分别表示相机拍摄第k张图片和第k 1张图片的时刻；l和l 1是i和j之间的两个时刻；是预积分阶段imu和轮速计之间的外参的值；和分别为加速度计、陀螺仪和轮速计的读数；δt
l
是时刻l和l 1之间的时间间隔；和分别表示在某个时刻t的加速度计和陀螺仪偏置；是预积分结果，可以视为imu的位移、速度变化和相对旋转；表示轮速计的位移。
[0023]
进一步地，所述s3中觉重投影误差、imu预积分误差、轮速计预积分误差计算方法为：
[0024][0025]
其中，g
w
是世界坐标系下的重力加速度，δt
k
表示图像帧k和k 1之间的时间间隔；表示图像帧k和k 1之间的时间间隔；和是被imu偏差的变化所补偿和以及imu
‑
轮速计外参补偿后的预积分结果。
[0026]
进一步地，所述s4中位姿估计方法为：
[0027][0028]
[0029][0030]
其中，c(x)表示代价函数，l表示地图点的索引，b
l
是在其上出现地图点l的图像帧集；k是滑动窗口中的帧数，表示重投影误差，表示imu
‑
轮速计误差，e
m
表示边缘化残差，w
r
是所有重投影项的信息矩阵，待优化量x包含每帧状态量x
k
，每个地图点的逆深度λ
l
，相机到imu的旋转和平移以及轮速计到imu的旋转和平移其中，每帧状态量x
k
包括在每一帧对应时刻下的imu到世界坐标系的位移速度和旋转以及imu的加速度偏置和角速度偏置
[0031]
进一步地，所述s6中构建三维地图还包括：从rgbd数据中生成三维点云，然后在每一关键帧上使用光线投射方法得到tsfd，从中使用marching cube算法提取网格；具体为：
[0032]
1)输入每个关键帧带有语义标签的2d路面语义分割结果，然后将标签附加到每个三维点上；
[0033]
2)在光线投射时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量；
[0034]
3)仅在tsdf截断距离，即接近表面内传播该信息，以节省计算时间；
[0035]
4)使用贝叶斯方法，更新每个体素的标签概率；
[0036]
5)在语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签；
[0037]
6)对5)中概率最大的标签利用marching cubes算法提取语义网格。
[0038]
进一步地，所述4)中基于贝叶斯方法的体素标签概率更新公式为：
[0039][0040]
其中，k为观测到的图像帧的序号，i为观测到的图像帧，i为语义类别的序号，p()则为第i个分类的概率分布；u
(s,k)
表示第k帧中第s个点元的像素坐标；o为n元的向量，n代表类别个数，其中取第i元，得到一张l
i
类的概率分布图，取其中u的位置的概率值，来乘以原来的概率进行一次更新。
[0041]
进一步地，所述s7中小车运动异常检测包括：
[0042]
1)基于路面语义分割的小车运动异常检测：小车前方检测到有路面不平的路况，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；
[0043]
2)基于惯性导航和轮速计一致性的小车运动异常检测：从状态估计器中获取imu在最后一次接收相机帧时在世界坐标系中的位置、速度和姿态，具有已知的起始姿态和速度，基于惯性导航的imu航位推算算法在没有重力加速度的情况下，基于imu测量，在短时间内预测小车的实时姿态和速度；
[0044]
轮速计预积分算法根据前一帧的运动状态预测小车的实时姿态和速度，根据轮速计预积分所获得的位置协方差，从imu或轮速计的预先点推算出机器人的位置，通过两种方法计算出的马尔可夫距离均大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；
[0045]
3)基于轮速计和imu对准的小车运动异常检测：直接将imu预积分与轮速计预积分
进行线性对准，然后在最新帧中计算imu预设点和轮速计预设点之间的偏差，如果马氏距离的偏差大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
[0046]
有益效果：
[0047]
1、本发明将轮速计测量融合入视觉
‑
imu里程计中，解决了小车平地运动时，vins初始化尺度模糊的问题，提高了定位的鲁棒性。
[0048]
2、本发明构建语义地图，利用贝叶斯增量的方法更新每个地图节点所属类别的概率分布，解决了同一个空间点不断被观测的过程中类型概率不一致的问题。
[0049]
3、本发明利用语义的方法检测小车的运动异常，避免了错误的轮速计测量对机器人的姿态估计的不利影响。
附图说明
[0050]
图1为本发明系统流程图。
具体实施方式
[0051]
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
[0052]
本发明在传统的视觉惯导slam的基础上，使用预积分和基于优化的方法，融合了轮速计测量，对关键帧进行路面语义分割，构建全局三维语义地图。结合路面语义信息，进行小车运动异常检测，当检测到小车运动异常时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。
[0053]
图1示出了根据本发明一个实施例的基于视觉
‑
imu
‑
轮速计融合的语义slam方法流程图。如1所示，该方法包括以下步骤：
[0054]
s1：将深度相机固定安装在小车上，小车上安装有轮速计，采集前一时刻和当前时刻的彩色图和深度图、imu测量值以及轮速计读数。
[0055]
s2：对s1中的imu测量值以及轮速计读数计算imu和轮速计的预积分。
[0056]
计算imu和轮速计的预积分具体为：
[0057][0058][0059][0060][0061][0062][0063]
其中，i和j分别表示相机拍摄第k张图片和第k 1张图片的时刻；l和l 1是i和j之间的两个时刻；是预积分阶段imu和轮速计之间的外参的值；和分别为加速度计、陀螺仪和轮速计的读数；δt
l
是时刻l和l 1之间的时间间隔；和分别表示在某个时刻t的加速度计和陀螺仪偏置；是预积分结果，可以视为imu的位移、速度变化和
相对旋转；表示轮速计的位移。
[0064]
s3：计算相邻两帧间的视觉重投影误差、imu预积分误差、轮速计预积分误差。
[0065]
视觉重投影误差、imu预积分误差、轮速计预积分误差计算方法为：
[0066][0067]
其中，g
w
是世界坐标系下的重力加速度，δt
k
表示图像帧k和k 1之间的时间间隔；表示图像帧k和k 1之间的时间间隔；和是被imu偏差的变化所补偿和以及imu
‑
轮速计外参补偿后的预积分结果。
[0068]
s4：位姿估计，对s3中的视觉重投影误差、imu预积分误差、轮速计预积分误差进行非线性优化，求解出滑动窗口中所有帧的位姿。
[0069]
位姿估计方法为：
[0070][0071][0072][0073]
其中，c(x)表示代价函数，l表示地图点的索引，b
l
是在其上出现地图点l的图像帧集；k是滑动窗口中的帧数，表示重投影误差，表示imu
‑
轮速计误差，e
m
表示边缘化残差，w
r
是所有重投影项的信息矩阵，待优化量x包含每帧状态量x
k
，每个地图点的逆深度λ
l
，相机到imu的旋转和平移以及轮速计到imu的旋转和平移其中，每帧状态量x
k
包括在每一帧对应时刻下的imu到世界坐标系的位移速度和旋转以及imu的加速度偏置和角速度偏置
[0074]
s5：使用卷积神经网络对路面经行2d语义分割，分割出路面不平的区域，得到2d路面语义分割结果图。
[0075]
s6：对s4中的每一帧的位姿、s5中的2d路面语义分割结果图、s1中的深度图输入到全局3d语义建图模块中，以构建带有语义标签的全局3d语义网格地图。
[0076]
构建三维地图还包括：从rgbd数据中生成三维点云，然后在每一关键帧上使用光线投射方法得到tsfd，从中使用marching cube算法提取网格；具体为：
[0077]
1)输入每个关键帧带有语义标签的2d路面语义分割结果，然后将标签附加到每个三维点上。
[0078]
2)在光线投射时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量。
[0079]
3)仅在tsdf截断距离，即接近表面内传播该信息，以节省计算时间。
[0080]
4)使用贝叶斯方法，更新每个体素的标签概率。
[0081]
基于贝叶斯方法的体素标签概率更新公式为：
[0082][0083]
其中，k为观测到的图像帧的序号，i为观测到的图像帧，i为语义类别的序号，p()则为第i个分类的概率分布；u
(s,k)
表示第k帧中第s个点元的像素坐标；o为n元的向量，n代表类别个数，其中取第i元，得到一张l
i
类的概率分布图，取其中u的位置的概率值，来乘以原来的概率进行一次更新。
[0084]
5)在语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签。
[0085]
6)对5)中概率最大的标签利用marching cubes算法提取语义网格。
[0086]
s7：对小车运动异常进行检测。
[0087]
小车运动异常检测包括：
[0088]
1)基于路面语义分割的小车运动异常检测：小车前方检测到有路面不平的路况，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
[0089]
2)基于惯性导航和轮速计一致性的小车运动异常检测：从状态估计器中获取imu在最后一次接收相机帧时在世界坐标系中的位置、速度和姿态，具有已知的起始姿态和速度，基于惯性导航的imu航位推算算法在没有重力加速度的情况下，基于imu测量，在短时间内预测小车的实时姿态和速度。
[0090]
轮速计预积分算法还可以根据前一帧的运动状态预测小车的实时姿态和速度，根据轮速计预积分所获得的位置协方差，可以从imu或轮速计的预先点推算出机器人的位置，如果通过两种方法计算出的马尔可夫距离均大于1.5(相应的概率约为13.4％)，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
[0091]
3)基于轮速计和imu对准的小车运动异常检测：直接将imu预积分与轮速计预积分进行线性对准，然后在最新帧中计算imu预设点和轮速计预设点之间的偏差，如果马氏距离的偏差大于1.5(相应的概率约为13.4％)，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
[0092]
s8：位姿优化，当检测到小车异常移动时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。
[0093]
上述实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所做的等效变换或修饰，都应涵盖在本发明的保护范围之内。

技术特征：
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：对小车运动异常进行检测；s8：位姿优化，当检测到小车异常移动时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。2.根据权利要求1所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征在于，所述s2中计算imu和轮速计的预积分具体为：imu和轮速计的预积分具体为：imu和轮速计的预积分具体为：imu和轮速计的预积分具体为：imu和轮速计的预积分具体为：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
‑
轮速计外参补偿后的预积分结果。4.根据权利要求1所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征在于，所述s4中位姿估计方法为：述s4中位姿估计方法为：述s4中位姿估计方法为：其中，c(x)表示代价函数，l表示地图点的索引，b
l
是在其上出现地图点l的图像帧集；k是滑动窗口中的帧数，表示重投影误差，表示imu
‑
轮速计误差，e
m
表示边缘化残差，w
r
是所有重投影项的信息矩阵，待优化量x包含每帧状态量x
k
，每个地图点的逆深度λ
l
，相机到imu的旋转和平移以及轮速计到imu的旋转和平移其中，每帧状态量x
k
包括在每一帧对应时刻下的imu到世界坐标系的位移速度和旋转以及imu的加速度偏置和角速度偏置5.根据权利要求1所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征在于，所述s6中构建三维地图还包括：从rgbd数据中生成三维点云，然后在每一关键帧上使用光线投射方法得到tsfd，从中使用marching cube算法提取网格；具体为：1)输入每个关键帧带有语义标签的2d路面语义分割结果，然后将标签附加到每个三维点上；2)在光线投射时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量；3)仅在tsdf截断距离，即接近表面内传播该信息，以节省计算时间；4)使用贝叶斯方法，更新每个体素的标签概率；5)在语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签；6)对5)中概率最大的标签利用marching cubes算法提取语义网格。6.根据权利要求5所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征在于，所述4)中基于贝叶斯方法的体素标签概率更新公式为：其中，k为观测到的图像帧的序号，i为观测到的图像帧，i为语义类别的序号，p()则为第i个分类的概率分布；u
(s,k)
表示第k帧中第s个点元的像素坐标；o为n元的向量，n代表类别个数，其中取第i元，得到一张l
i
类的概率分布图，取其中u的位置的概率值，来乘以原来的概率进行一次更新。7.根据权利要求1至6任一所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征
在于，所述s7中小车运动异常检测包括：基于路面语义分割的小车运动异常检测：小车前方检测到有路面不平的路况，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。8.根据权利要求1至6任一所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征在于，所述s7中小车运动异常检测还包括：基于惯性导航和轮速计一致性的小车运动异常检测：从状态估计器中获取imu在最后一次接收相机帧时在世界坐标系中的位置、速度和姿态，具有已知的起始姿态和速度，基于惯性导航的imu航位推算算法在没有重力加速度的情况下，基于imu测量，在短时间内预测小车的实时姿态和速度；轮速计预积分算法根据前一帧的运动状态预测小车的实时姿态和速度，根据轮速计预积分所获得的位置协方差，从imu或轮速计的预先点推算出机器人的位置，通过两种方法计算出的马尔可夫距离均大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。9.根据权利要求1至6任一所述的基于视觉
‑
imu
‑
轮速计融合的语义slam方法，其特征在于，所述s7中小车运动异常检测还包括：基于轮速计和imu对准的小车运动异常检测：直接将imu预积分与轮速计预积分进行线性对准，然后在最新帧中计算imu预设点和轮速计预设点之间的偏差，如果马氏距离的偏差大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
技术总结
本发明涉及计算机视觉技术领域，公开了一种基于视觉


技术研发人员：李威 李晓馨 朴松昊 陈立国
受保护的技术使用者：苏州迈思捷智能科技有限公司
技术研发日：2021.02.26
技术公布日：2021/6/29