CN113052855B - 一种基于视觉-imu-轮速计融合的语义slam方法 - Google Patents

一种基于视觉-imu-轮速计融合的语义slam方法 Download PDF

Info

Publication number
CN113052855B
CN113052855B CN202110218495.5A CN202110218495A CN113052855B CN 113052855 B CN113052855 B CN 113052855B CN 202110218495 A CN202110218495 A CN 202110218495A CN 113052855 B CN113052855 B CN 113052855B
Authority
CN
China
Prior art keywords
imu
wheel speed
speed meter
semantic
trolley
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110218495.5A
Other languages
English (en)
Other versions
CN113052855A (zh
Inventor
李威
李晓馨
朴松昊
陈立国
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Suzhou Maisijie Intelligent Technology Co ltd
Original Assignee
Suzhou Maisijie Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Suzhou Maisijie Intelligent Technology Co ltd filed Critical Suzhou Maisijie Intelligent Technology Co ltd
Priority to CN202110218495.5A priority Critical patent/CN113052855B/zh
Publication of CN113052855A publication Critical patent/CN113052855A/zh
Application granted granted Critical
Publication of CN113052855B publication Critical patent/CN113052855B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G06T7/11—Region-based segmentation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/04—Architecture, e.g. interconnection topology
    • G06N3/045—Combinations of networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N3/00—Computing arrangements based on biological models
    • G06N3/02—Neural networks
    • G06N3/08—Learning methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
    • G06N7/00—Computing arrangements based on specific mathematical models
    • G06N7/01—Probabilistic graphical models, e.g. probabilistic networks
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/215—Motion-based segmentation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20081—Training; Learning
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20084—Artificial neural networks [ANN]
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30248—Vehicle exterior or interior
    • G06T2207/30252—Vehicle exterior; Vicinity of vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Evolutionary Computation (AREA)
  • Mathematical Physics (AREA)
  • Artificial Intelligence (AREA)
  • Computing Systems (AREA)
  • Computational Linguistics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • General Health & Medical Sciences (AREA)
  • Biophysics (AREA)
  • Biomedical Technology (AREA)
  • Molecular Biology (AREA)
  • Health & Medical Sciences (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Probability & Statistics with Applications (AREA)
  • Mathematical Optimization (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Multimedia (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

Abstract

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

Description

一种基于视觉-IMU-轮速计融合的语义SLAM方法
技术领域
本发明涉及计算机视觉技术领域，具体涉及一种基于视觉-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和轮速计的预积分具体为：
Figure GDA0003283994320000021
Figure GDA0003283994320000022
Figure GDA0003283994320000023
Figure GDA0003283994320000024
Figure GDA0003283994320000025
Figure GDA0003283994320000026
其中，i和j分别表示相机拍摄第k张图片和第k+1张图片的时刻；l和l+1是i和j之间的两个时刻；
Figure GDA0003283994320000027
是预积分阶段IMU和轮速计之间的外参
Figure GDA0003283994320000028
的值；
Figure GDA0003283994320000029
和
Figure GDA00032839943200000210
分别为加速度计、陀螺仪和轮速计的读数；δtl是时刻l和l+1之间的时间间隔；
Figure GDA00032839943200000211
和
Figure GDA00032839943200000212
分别表示在某个时刻t的加速度计和陀螺仪偏置；
Figure GDA00032839943200000213
是预积分结果，可以视为IMU的位移、速度变化和相对旋转；
Figure GDA00032839943200000214
表示轮速计的位移。
进一步地，所述S3中觉重投影误差、IMU预积分误差、轮速计预积分误差计算方法为：
Figure GDA0003283994320000031
其中，gw是世界坐标系下的重力加速度，Δtk表示图像帧k和k+1之间的时间间隔；
Figure GDA0003283994320000032
Figure GDA0003283994320000033
和
Figure GDA0003283994320000034
是被IMU偏差的变化所补偿
Figure GDA0003283994320000035
和
Figure GDA0003283994320000036
以及IMU-轮速计外参
Figure GDA0003283994320000037
补偿后的预积分结果。
进一步地，所述S4中位姿估计方法为：
Figure GDA0003283994320000038
Figure GDA0003283994320000039
Figure GDA00032839943200000310
其中，c(x)表示代价函数，l表示地图点的索引，Bl是在其上出现地图点l的图像帧集；K是滑动窗口中的帧数，
Figure GDA00032839943200000311
表示重投影误差，
Figure GDA00032839943200000312
表示IMU-轮速计误差，em表示边缘化残差，Wr是所有重投影项的信息矩阵，待优化量x包含每帧状态量xk，每个地图点的逆深度λl，相机到IMU的旋转
Figure GDA00032839943200000313
和平移
Figure GDA00032839943200000314
以及轮速计到IMU的旋转
Figure GDA00032839943200000315
和平移
Figure GDA00032839943200000316
其中，每帧状态量xk包括在每一帧对应时刻下的IMU到世界坐标系的位移
Figure GDA00032839943200000317
速度
Figure GDA00032839943200000318
和旋转
Figure GDA00032839943200000319
以及IMU的加速度偏置
Figure GDA00032839943200000320
和角速度偏置
Figure GDA00032839943200000321
进一步地，所述S6中构建三维地图还包括：从RGBD数据中生成三维点云，然后在每一关键帧上使用光线投射方法得到TSDF，从中使用marching cube算法提取网格；具体为：
1)输入每个关键帧带有语义标签的2D路面语义分割结果，然后将标签附加到每个三维点上；
2)在光线投射时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量；
3)仅在TSDF截断距离，即接近表面内传播该信息，以节省计算时间；
4)使用贝叶斯方法，更新每个体素的标签概率；
5)在语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签；
6)对5)中概率最大的标签利用marching cubes算法提取语义网格。
进一步地，所述4)中基于贝叶斯方法的体素标签概率更新公式为：
Figure GDA0003283994320000041
其中，k为观测到的图像帧的序号，I为观测到的图像帧，i为语义类别的序号，P()则为第i个分类的概率分布；u(s,k)表示第k帧中第s个点元的像素坐标；O为n元的向量，n代表类别个数，其中取第i元，得到一张li类的概率分布图，取其中u的位置的概率值，来乘以原来的概率进行一次更新。
进一步地，所述S7中小车运动异常检测包括：
1)基于路面语义分割的小车运动异常检测：小车前方检测到有路面不平的路况，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；
2)基于惯性导航和轮速计一致性的小车运动异常检测：从状态估计器中获取IMU在最后一次接收相机帧时在世界坐标系中的位置、速度和姿态，具有已知的起始姿态和速度，基于惯性导航的IMU航位推算算法在没有重力加速度的情况下，基于IMU测量，在短时间内预测小车的实时姿态和速度；
轮速计预积分算法根据前一帧的运动状态预测小车的实时姿态和速度，根据轮速计预积分所获得的位置协方差，从IMU或轮速计的预先点推算出机器人的位置，通过两种方法计算出的马尔可夫距离均大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态；
3)基于轮速计和IMU对准的小车运动异常检测：直接将IMU预积分与轮速计预积分进行线性对准，然后在最新帧中计算IMU预设点和轮速计预设点之间的偏差，如果马氏距离的偏差大于1.5，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
有益效果：
1、本发明将轮速计测量融合入视觉-IMU里程计中，解决了小车平地运动时，VINS初始化尺度模糊的问题，提高了定位的鲁棒性。
2、本发明构建语义地图，利用贝叶斯增量的方法更新每个地图节点所属类别的概率分布，解决了同一个空间点不断被观测的过程中类型概率不一致的问题。
3、本发明利用语义的方法检测小车的运动异常，避免了错误的轮速计测量对机器人的姿态估计的不利影响。
附图说明
图1为本发明***流程图。
具体实施方式
下面结合附图对本发明作进一步描述。以下实施例仅用于更加清楚地说明本发明的技术方案，而不能以此来限制本发明的保护范围。
本发明在传统的视觉惯导SLAM的基础上，使用预积分和基于优化的方法，融合了轮速计测量，对关键帧进行路面语义分割，构建全局三维语义地图。结合路面语义信息，进行小车运动异常检测，当检测到小车运动异常时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。
图1示出了根据本发明一个实施例的基于视觉-IMU-轮速计融合的语义SLAM方法流程图。如1所示，该方法包括以下步骤：
S1：将深度相机固定安装在小车上，小车上安装有轮速计，采集前一时刻和当前时刻的彩色图和深度图、IMU测量值以及轮速计读数。
S2：对S1中的IMU测量值以及轮速计读数计算IMU和轮速计的预积分。
计算IMU和轮速计的预积分具体为：
Figure GDA0003283994320000051
Figure GDA0003283994320000052
Figure GDA0003283994320000053
Figure GDA0003283994320000054
Figure GDA0003283994320000055
Figure GDA0003283994320000056
其中，i和j分别表示相机拍摄第k张图片和第k+1张图片的时刻；l和l+1是i和j之间的两个时刻；
Figure GDA0003283994320000057
是预积分阶段IMU和轮速计之间的外参
Figure GDA0003283994320000058
的值；
Figure GDA0003283994320000059
和
Figure GDA00032839943200000510
分别为加速度计、陀螺仪和轮速计的读数；δtl是时刻l和l+1之间的时间间隔；
Figure GDA00032839943200000511
和
Figure GDA00032839943200000512
分别表示在某个时刻t的加速度计和陀螺仪偏置；
Figure GDA00032839943200000513
是预积分结果，可以视为IMU的位移、速度变化和相对旋转；
Figure GDA00032839943200000514
表示轮速计的位移。
S3：计算相邻两帧间的视觉重投影误差、IMU预积分误差、轮速计预积分误差。
视觉重投影误差、IMU预积分误差、轮速计预积分误差计算方法为：
Figure GDA0003283994320000061
其中，gw是世界坐标系下的重力加速度，Δtk表示图像帧k和k+1之间的时间间隔；
Figure GDA0003283994320000062
Figure GDA0003283994320000063
和
Figure GDA0003283994320000064
是被IMU偏差的变化所补偿
Figure GDA0003283994320000065
和
Figure GDA0003283994320000066
以及IMU-轮速计外参
Figure GDA0003283994320000067
补偿后的预积分结果。
S4：位姿估计，对S3中的视觉重投影误差、IMU预积分误差、轮速计预积分误差进行非线性优化，求解出滑动窗口中所有帧的位姿。
位姿估计方法为：
Figure GDA0003283994320000068
Figure GDA0003283994320000069
Figure GDA00032839943200000610
其中，c(x)表示代价函数，l表示地图点的索引，Bl是在其上出现地图点l的图像帧集；K是滑动窗口中的帧数，
Figure GDA00032839943200000611
表示重投影误差，
Figure GDA00032839943200000612
表示IMU-轮速计误差，em表示边缘化残差，Wr是所有重投影项的信息矩阵，待优化量x包含每帧状态量xk，每个地图点的逆深度λl，相机到IMU的旋转
Figure GDA00032839943200000613
和平移
Figure GDA00032839943200000614
以及轮速计到IMU的旋转
Figure GDA00032839943200000615
和平移
Figure GDA00032839943200000616
其中，每帧状态量xk包括在每一帧对应时刻下的IMU到世界坐标系的位移
Figure GDA00032839943200000617
速度
Figure GDA00032839943200000618
和旋转
Figure GDA00032839943200000619
以及IMU的加速度偏置
Figure GDA00032839943200000620
和角速度偏置
Figure GDA00032839943200000621
S5：使用卷积神经网络对路面经行2D语义分割，分割出路面不平的区域，得到2D路面语义分割结果图。
S6：对S4中的每一帧的位姿、S5中的2D路面语义分割结果图、S1中的深度图输入到全局3D语义建图模块中，以构建带有语义标签的全局3D语义网格地图。
构建三维地图还包括：从RGBD数据中生成三维点云，然后在每一关键帧上使用光线投射方法得到TSDF，从中使用marching cube算法提取网格；具体为：
1)输入每个关键帧带有语义标签的2D路面语义分割结果，然后将标签附加到每个三维点上。
2)在光线投射时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量。
3)仅在TSDF截断距离，即接近表面内传播该信息，以节省计算时间。
4)使用贝叶斯方法，更新每个体素的标签概率。
基于贝叶斯方法的体素标签概率更新公式为：
Figure GDA0003283994320000071
其中，k为观测到的图像帧的序号，I为观测到的图像帧，i为语义类别的序号，P()则为第i个分类的概率分布；u(s,k)表示第k帧中第s个点元的像素坐标；O为n元的向量，n代表类别个数，其中取第i元，得到一张li类的概率分布图，取其中u的位置的概率值，来乘以原来的概率进行一次更新。
5)在语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签。
6)对5)中概率最大的标签利用marching cubes算法提取语义网格。
S7：对小车运动异常进行检测。
小车运动异常检测包括：
1)基于路面语义分割的小车运动异常检测：小车前方检测到有路面不平的路况，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
2)基于惯性导航和轮速计一致性的小车运动异常检测：从状态估计器中获取IMU在最后一次接收相机帧时在世界坐标系中的位置、速度和姿态，具有已知的起始姿态和速度，基于惯性导航的IMU航位推算算法在没有重力加速度的情况下，基于IMU测量，在短时间内预测小车的实时姿态和速度。
轮速计预积分算法还可以根据前一帧的运动状态预测小车的实时姿态和速度，根据轮速计预积分所获得的位置协方差，可以从IMU或轮速计的预先点推算出机器人的位置，如果通过两种方法计算出的马尔可夫距离均大于1.5(相应的概率约为13.4％)，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
3)基于轮速计和IMU对准的小车运动异常检测：直接将IMU预积分与轮速计预积分进行线性对准，然后在最新帧中计算IMU预设点和轮速计预设点之间的偏差，如果马氏距离的偏差大于1.5(相应的概率约为13.4％)，则轮速计的预积分结果被视为异常，并且小车处于异常运动状态。
S8：位姿优化，当检测到小车异常移动时，主动地从状态估计方程中删除当前帧的轮速计预积分测量值。
上述实施方式只为说明本发明的技术构思及特点，其目的在于让熟悉此项技术的人能够了解本发明的内容并据以实施，并不能以此限制本发明的保护范围。凡根据本发明精神实质所做的等效变换或修饰，都应涵盖在本发明的保护范围之内。

Claims (6)

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和轮速计的预积分具体为：
Figure FDA0003283994310000021
Figure FDA0003283994310000022
Figure FDA0003283994310000023
Figure FDA0003283994310000024
Figure FDA0003283994310000025
Figure FDA0003283994310000026
其中，i和j分别表示相机拍摄第k张图片和第k+1张图片的时刻；l和l+1是i和j之间的两个时刻；
Figure FDA0003283994310000027
是预积分阶段IMU和轮速计之间的外参
Figure FDA0003283994310000028
的值；
Figure FDA0003283994310000029
Figure FDA00032839943100000210
和
Figure FDA00032839943100000211
分别为加速度计、陀螺仪和轮速计的读数；δtl是时刻l和l+1之间的时间间隔；
Figure FDA00032839943100000212
和
Figure FDA00032839943100000213
分别表示在某个时刻t的加速度计和陀螺仪偏置；
Figure FDA00032839943100000214
是预积分结果，为IMU的位移、速度变化和相对旋转；
Figure FDA00032839943100000215
表示轮速计的位移。
3.根据权利要求1所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述S3中视觉重投影误差、IMU预积分误差、轮速计预积分误差计算方法为：
Figure FDA00032839943100000216
其中，gw是世界坐标系下的重力加速度，Δtk表示图像帧k和k+1之间的时间间隔；
Figure FDA00032839943100000217
和
Figure FDA00032839943100000218
是被IMU偏差的变化所补偿
Figure FDA00032839943100000219
和
Figure FDA00032839943100000220
以及IMU-轮速计外参
Figure FDA00032839943100000221
补偿后的预积分结果，在每一帧对应时刻下的IMU到世界坐标系的位移
Figure FDA00032839943100000222
和旋转
Figure FDA00032839943100000223
轮速计到IMU的平移
Figure FDA00032839943100000224
4.根据权利要求1所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述S4中位姿估计方法为：
Figure FDA0003283994310000031
Figure FDA0003283994310000032
Figure FDA0003283994310000033
其中，c(x)表示代价函数，l表示地图点的索引，Bl是在其上出现地图点l的图像帧集；K是滑动窗口中的帧数，
Figure FDA0003283994310000034
表示重投影误差，
Figure FDA0003283994310000035
表示IMU-轮速计误差，em表示边缘化残差，Wr是所有重投影项的信息矩阵，待优化量x包含每帧状态量xk，每个地图点的逆深度λl，相机到IMU的旋转
Figure FDA0003283994310000036
和平移
Figure FDA0003283994310000037
以及轮速计到IMU的旋转
Figure FDA0003283994310000038
和平移
Figure FDA0003283994310000039
其中，每帧状态量xk包括在每一帧对应时刻下的IMU到世界坐标系的位移
Figure FDA00032839943100000310
速度
Figure FDA00032839943100000311
和旋转
Figure FDA00032839943100000312
以及IMU的加速度偏置
Figure FDA00032839943100000313
和角速度偏置
Figure FDA00032839943100000314
5.根据权利要求1所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述S6中构建三维地图还包括：从RGBD数据中生成三维点云，然后在每一关键帧上使用光线投射方法得到TSDF，从中使用marching cube算法提取网格；具体为：
1)输入每个关键帧带有语义标签的2D路面语义分割结果，然后将标签附加到每个三维点上；
2)在光线投射时还投射了语义标签，对于光线投射中的每束光线，根据束中观察到的标签的频率建立一个标签概率向量；
3)仅在TSDF截断距离，即接近表面内传播信息，以节省计算时间；
4)使用贝叶斯方法，更新每个体素的标签概率；
5)在语义光线投射之后，每个体素都有一个标签概率向量，从中选择概率最大的标签；
6)对5)中概率最大的标签利用marching cubes算法提取语义网格。
6.根据权利要求5所述的基于视觉-IMU-轮速计融合的语义SLAM方法，其特征在于，所述4)中基于贝叶斯方法的体素标签概率更新公式为：
Figure FDA0003283994310000041
其中，k为观测到的图像帧的序号，I为观测到的图像帧，i为语义类别的序号，P()则为第i个分类的概率分布；u(s,k)表示第k帧中第s个点元的像素坐标；O为n元的向量，n代表类别个数，其中取第i元，得到一张li类的概率分布图，取其中u的位置的概率值，来乘以原来的概率进行一次更新。
CN202110218495.5A 2021-02-26 2021-02-26 一种基于视觉-imu-轮速计融合的语义slam方法 Active CN113052855B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110218495.5A CN113052855B (zh) 2021-02-26 2021-02-26 一种基于视觉-imu-轮速计融合的语义slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110218495.5A CN113052855B (zh) 2021-02-26 2021-02-26 一种基于视觉-imu-轮速计融合的语义slam方法

Publications (2)

Publication Number Publication Date
CN113052855A CN113052855A (zh) 2021-06-29
CN113052855B true CN113052855B (zh) 2021-11-02

Family

ID=76509163

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110218495.5A Active CN113052855B (zh) 2021-02-26 2021-02-26 一种基于视觉-imu-轮速计融合的语义slam方法

Country Status (1)

Country Link
CN (1) CN113052855B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113793381A (zh) * 2021-07-27 2021-12-14 武汉中海庭数据技术有限公司 单目视觉信息和轮速信息融合的定位方法及***
CN114485653A (zh) * 2022-02-23 2022-05-13 广州高新兴机器人有限公司 基于视觉与轮式里程计融合的定位方法、装置、介质及设备
CN115164918B (zh) * 2022-09-06 2023-02-03 联友智连科技有限公司 语义点云地图构建方法、装置及电子设备

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106054896A (zh) * 2016-07-13 2016-10-26 武汉大学 一种智能导航机器人小车***
CN106274308A (zh) * 2015-06-24 2017-01-04 通用汽车环球科技运作有限责任公司 集成感测单元以及用于确定车辆车轮速度和轮胎压力的方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN111854770A (zh) * 2019-04-30 2020-10-30 北京初速度科技有限公司 一种车辆的定位***和方法
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建***及方法
CN112197770A (zh) * 2020-12-02 2021-01-08 北京欣奕华数字科技有限公司 一种机器人的定位方法及其定位装置
CN112219087A (zh) * 2019-08-30 2021-01-12 深圳市大疆创新科技有限公司 位姿预测方法、地图构建方法、可移动平台及存储介质
CN112344923A (zh) * 2021-01-11 2021-02-09 浙江欣奕华智能科技有限公司 一种机器人的定位方法及其定位装置
CN112734852A (zh) * 2021-03-31 2021-04-30 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN113223161A (zh) * 2021-04-07 2021-08-06 武汉大学 一种基于imu和轮速计紧耦合的鲁棒全景slam***和方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7970512B2 (en) * 2006-08-30 2011-06-28 Ford Global Technologies Integrated control system for stability control of yaw, roll and lateral motion of a driving vehicle using an integrated sensing system with pitch information
CN101344391B (zh) * 2008-07-18 2011-05-11 北京工业大学 基于全功能太阳罗盘的月球车位姿自主确定方法
CN111882607B (zh) * 2020-07-14 2021-05-04 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106274308A (zh) * 2015-06-24 2017-01-04 通用汽车环球科技运作有限责任公司 集成感测单元以及用于确定车辆车轮速度和轮胎压力的方法
CN106054896A (zh) * 2016-07-13 2016-10-26 武汉大学 一种智能导航机器人小车***
CN111854770A (zh) * 2019-04-30 2020-10-30 北京初速度科技有限公司 一种车辆的定位***和方法
CN110345944A (zh) * 2019-05-27 2019-10-18 浙江工业大学 融合视觉特征和imu信息的机器人定位方法
CN112219087A (zh) * 2019-08-30 2021-01-12 深圳市大疆创新科技有限公司 位姿预测方法、地图构建方法、可移动平台及存储介质
CN111968129A (zh) * 2020-07-15 2020-11-20 上海交通大学 具有语义感知的即时定位与地图构建***及方法
CN112197770A (zh) * 2020-12-02 2021-01-08 北京欣奕华数字科技有限公司 一种机器人的定位方法及其定位装置
CN112344923A (zh) * 2021-01-11 2021-02-09 浙江欣奕华智能科技有限公司 一种机器人的定位方法及其定位装置
CN112734852A (zh) * 2021-03-31 2021-04-30 浙江欣奕华智能科技有限公司 一种机器人建图方法、装置及计算设备
CN113223161A (zh) * 2021-04-07 2021-08-06 武汉大学 一种基于imu和轮速计紧耦合的鲁棒全景slam***和方法

Non-Patent Citations (6)

* Cited by examiner, † Cited by third party
Title
A Comparative Study of SLAM Algorithms for Indoor Navigation of Autonomous Wheelchairs;Abhayjeet Juneja等;《2019 IEEE International Conference on Cyborg and Bionic Systems (CBS)》;20200611;第261-266页 *
基于IMU/ODO预积分的多传感器即插即用因子图融合方法;白师宇等;《中国惯性技术学报》;20201015;第28卷(第5期);第624-628+637页 *
基于视觉的惯性导航误差在线修正;张超等;《导航定位与授时》;20180507;第5卷(第3期);第104-110页 *
多位姿信息融合的双目视觉惯性里程计研究;王延东;《中国博士学位论文全文数据库 信息科技辑》;20190815(第08期);第I138-95页 *
融合IMU与RGB-D相机的室内移动机器人地图创建算法研究;林鹏;《中国优秀硕士学位论文全文数据库 信息科技辑》;20200715(第07期);第I138-679页 *
视觉SLAM综述;朴松昊等;《智能***学报》;20170111;第11卷(第6期);第768-776页 *

Also Published As

Publication number Publication date
CN113052855A (zh) 2021-06-29

Similar Documents

Publication Publication Date Title
CN113052855B (zh) 一种基于视觉-imu-轮速计融合的语义slam方法
US20220082386A1 (en) Vision-aided inertial navigation
CN110243358B (zh) 多源融合的无人车室内外定位方法及***
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
CN107941217B (zh) 一种机器人定位方法、电子设备、存储介质、装置
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN107909614B (zh) 一种gps失效环境下巡检机器人定位方法
CN111795686A (zh) 一种移动机器人定位与建图的方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN113503873B (zh) 一种多传感器融合的视觉定位方法
Liu et al. Bidirectional trajectory computation for odometer-aided visual-inertial SLAM
CN117739972B (zh) 一种无全球卫星定位***的无人机进近阶段定位方法
CN115453599A (zh) 一种多传感器协同的管道机器人精准定位方法
CN117710476A (zh) 基于单目视觉的无人机位姿估计与稠密建图方法
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及***
CN112923934A (zh) 一种适用于非结构化场景中结合惯导的激光slam技术
CN116202509A (zh) 一种面向室内多层建筑的可通行地图生成方法
Chen et al. River: A Tightly-coupled Radar-inertial Velocity Estimator Based on Continuous-time Optimization
Reina et al. Iterative path reconstruction for large-scale inertial navigation on smartphones
CN113298796B (zh) 一种基于最大后验imu的线特征slam初始化方法
WO2022179047A1 (zh) 一种状态信息估计方法及装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant