CN117367427A - 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 - Google Patents
一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 Download PDFInfo
- Publication number
- CN117367427A CN117367427A CN202311294675.7A CN202311294675A CN117367427A CN 117367427 A CN117367427 A CN 117367427A CN 202311294675 A CN202311294675 A CN 202311294675A CN 117367427 A CN117367427 A CN 117367427A
- Authority
- CN
- China
- Prior art keywords
- points
- depth
- feature
- imu
- map
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 50
- 230000004927 fusion Effects 0.000 title claims description 15
- 230000000007 visual effect Effects 0.000 claims abstract description 22
- 230000008569 process Effects 0.000 claims abstract description 19
- 238000004364 calculation method Methods 0.000 claims abstract description 7
- 238000004422 calculation algorithm Methods 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 24
- 238000000605 extraction Methods 0.000 claims description 14
- 230000003287 optical effect Effects 0.000 claims description 9
- 238000001914 filtration Methods 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 5
- 230000011218 segmentation Effects 0.000 claims description 5
- 238000010276 construction Methods 0.000 claims description 4
- 238000010586 diagram Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 claims description 4
- 230000008901 benefit Effects 0.000 claims description 3
- 230000008859 change Effects 0.000 claims description 3
- 230000004807 localization Effects 0.000 claims description 2
- 238000013507 mapping Methods 0.000 claims description 2
- 230000035772 mutation Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 abstract description 5
- 230000015556 catabolic process Effects 0.000 description 7
- 238000006731 degradation reaction Methods 0.000 description 7
- 239000013598 vector Substances 0.000 description 5
- 238000011161 development Methods 0.000 description 3
- 230000018109 developmental process Effects 0.000 description 3
- 238000012546 transfer Methods 0.000 description 3
- 230000004044 response Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000004069 differentiation Effects 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000001737 promoting effect Effects 0.000 description 1
- 239000000779 smoke Substances 0.000 description 1
- 230000006641 stabilisation Effects 0.000 description 1
- 238000011105 stabilization Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- 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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/88—Lidar systems specially adapted for specific applications
- G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging
-
- 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/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- 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/269—Analysis of motion using gradient-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
-
- 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/10016—Video; Image sequence
-
- 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/30—Subject of image; Context of image processing
- G06T2207/30241—Trajectory
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种适用于仓储等室内环境的视觉辅助激光里程计的多模态slam技术，具体步骤为：首先，利用图像与激光点云估计系统运动，特征跟踪模块检测视觉特征点，深度图配准模块把局部的深度图和点云对齐得到视觉特征点的深度。帧到帧的运动估计模块利用特征点计算机体运动。激光雷达数据用于提取平面和边缘特征。以视觉模块得到的数据为先验，将提取的特征和IMU测量值输入到状态估计模块，在10Hz‑50Hz下进行状态估计。估计的姿态将特征点注册到全局框架中，并将它们与构建的特征点图合并。更新后的地图会在下一步中加入更多新的点。在IMU的状态估计模块中增加了反向传播的过程，并将反向传播所得的结果与正向传播所得结果一起输入到剩余计算中去。
Description
技术领域
本发明涉及视觉识别技术领域，具体为一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法。
背景技术
随着智慧化仓储的发展，越来越多的AGV被用做室内搬运，传统的激光雷达融合IMU的多模态SLAM(LIO)已经较为成熟，可以长时间稳定工作。但在一些退化场景下，这种体系就缺乏了有效的观测手段，导致位姿估计退化，甚至出现错误。激光雷达的测量数据在这样的退化环境中可能受到干扰，这使得观测数据不足，计算更复杂，使得机器人定位和地图构建产生误差。为了能够适应退化环境并更加鲁棒的工作，机器人需要输入有效的视觉新近，并且需要更高效的处理实现。这是目前在一般环境中鲁棒性较高的LIO系统无法在退化环境中发挥其正常的功能，同时，在室内复杂环境中，激光雷达点云和IMU数据的耦合常常耗费大量计算量。随着3D-SLAM技术的进步，物体检测、语义分割等视觉识别技术得到了较大的发展，LIO算法得以更快速的运行。这使得机器人可以更准确地感知环境中的物体，从而更好地完成自身任务，随着自动化、智能化的发展，人们对机器人和自主驾驶技术的需求不断提升。视觉辅助激光IMU里程计的slam方法解决了退化场景下的适应问题，可以使得仓储agv小车有更好的适应性、更好更高效的工作。
发明内容
针对上述现有问题，本发明提供一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法。本方法能够实现现有LIO方案在烟尘较大等退化的室内环境中因缺乏有效观测而难以适应的问题。
本发明是这样实现的，本发明提供一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法。第一步我们将雷达和摄像头置于agv小车上获取环境数据，按图像的帧率利用图像和激光点云估计系统的运动。第二步我们在这个部分特征跟踪模块在连续的图像帧中检测和匹配视觉特征点。第三步我们利用深度图配准模块把局部的深度图和点云对齐并得到视觉特征点的深度。第四步我们使用帧到帧的运动估计模块利用特征点计算机体运动。第五步我们将激光雷达输入数据输入到特征提取模块，以此来获得平面特征和边缘特征。第六步我们以视觉模块计算得到的数据为先验，将提取的特征和IMU测量值输入到状态估计模块，在10Hz-50Hz下进行状态估计。第七步我们将估计的姿态将特征点注册到全局框架中，并将它们与到目前为止构建的特征点图合并。更新后的地图会在下一步中加入更多新的点。最后我们在IMU的状态估计模块中增加了反向传播的过程，并将反向传播所得的结果与正向传播所得结果一起输入到剩余计算中去。
本发明解决技术问题采用如下技术方案：一种适用于室内环境的视觉扶助激光多模态slam技术，包括：
(1)前端输入
通过单目相机采集场景的图像连续帧，通过激光雷达采集点云信息，通过IMU采集加速度信息。
(2)数据处理
A.对图像输入采用特征跟踪算法在连续的图像帧中检测和匹配视觉特征点。利用视觉特征点和激光雷达点云进行深度图配，块把局部的深度图和点云对齐并得到视觉特征点的深度图。帧到帧的运动估计模块利用特征点计算机体运动。B.然后将激光雷达输入数据输入到特征提取模块，以此来获得平面特征和边缘特征。然后以视觉模块计算得到的数据为先验，将提取的特征和IMU测量值输入到状态估计模块，在10Hz-50Hz下进行状态估计。
(3)地图更新
估计的姿态将特征点注册到全局框架中，并将它们与到目前为止构建的特征点图合并。更新后的地图会在下一步中加入更多新的点。在IMU的状态估计模块中增加了反向传播的过程，并将反向传播所得的结果与正向传播所得结果一起输入到剩余计算中去。
与已有技术相比，本发明的有益效果体现在：本发明创新性的将视觉和IMU与传统激光雷达相结合，以实现在退化环境下，快速准确的获取自身和周围环境的相关数据。本发明创新性地将上述目标检测与光流法相结合，以实现快速提取检测数据的平面特征和边缘特征，并采用贝叶斯估计进行先验的状态估计，对姿态估计更快速和准确。本发明创新性地将上述数据处理与Fast LIO相结合，以实现对退化环境的高鲁棒性地图构建，极大的推动了退化环境下自动驾驶领域的发展
附图说明
图1为本方法流程图；
图2为本方法具体实施方式示例图；
图3为本发明示例中导数(梯度)计算出来的过程以及反向传播与参数更新过程。
具体实施方式
参考附图1和附图2，一种适用于仓储等室内环境的视觉辅助激光里程计的多模态slam技术，按以下步骤进一步描述：
步骤1：特征跟踪算法，其具体步骤：
首先做出如下假设：相邻帧之间的亮度恒定；相邻视频帧的取帧时间连续，或者，相邻帧之间物体的运动比较“微小”；同一子图像的像素点具有相同或相似的运动。然后基于Lucas-Kanade算法，利用两帧之间像素坐标的变化确定图像的光流输出。
步骤2：检测视觉特征点，实现空间尺度不变性，模仿实际生活中物体近大远小、近清晰远模糊的特点。具体实现流程如下：
步骤2.1：首先，构建Hessian(黑塞矩阵)，利用自动微分技术，可以在一次正向传递和一次反向传递中计算所有的一阶和二阶偏导数，从而构建黑塞矩阵。生成所有的兴趣点，用于特征的提取；构建Hessian矩阵的目的是为了生成图像稳定的边缘点(突变点)，为后续的特征提取做好基础。构建Hessian矩阵的过程对应于Sift算法中的高斯卷积过程。
其中，高斯卷积的具体过程为：
步骤2.1.1.生成高斯差分金字塔：对输入图像进行高斯滤波，得到不同尺度下的图像。然后在同一尺度下，对不同的高斯滤波结果进行相减，得到高斯差分图像。
步骤2.1.2.构建DoG金字塔对于每个尺度，用一个高斯核和一个比它更小的高斯核对图像进行卷积，得到两个高斯滤波结果，然后将它们相减，得到一幅DoG(Differenceof Gaussian)图像。不同尺度下的DoG图像构成DoG金字塔。
步骤2.1.3.寻找极值点在DoG金字塔中，寻找局部极值点，即比它周围8个点和上下两层的26个点都要大或者小的点。
步骤2.1.4.确定特征点的尺度和位置在局部极值点周围的DoG图像中，用二阶导数的哈里斯矩阵进行特征点的精确定位，同时确定特征点的尺度。
步骤2.1.5.消除低对比度的特征点对于低对比度的特征点，可以通过计算图像像素值的变化的幅值来剔除。
步骤2.1.6.消除边缘响应的特征点对于位于边缘的特征点，可以通过计算主曲率比来剔除。
步骤2.1.7.计算特征点的方向在特征点周围的图像区域内，计算梯度方向直方图，以确定特征点的主方向。
步骤2.2：构建尺度空间，构建SURF尺度空间的步骤如下：
步骤2.2.1：首先对图像进行尺度空间的构建，生成一系列不同尺度的图像。常用的方法是通过高斯金字塔或拉普拉斯金字塔来实现。
步骤2.2.2：对每个尺度的图像进行高斯差分，得到一组不同尺度的高斯差分图像。这里的高斯差分指的是在同一尺度下，使用不同的高斯核对图像进行滤波，再将不同尺度的图像相减得到的结果。
高斯差分公式的一般形式如下：
G(x,y)＝(1/(2*π*σ^2))*e^((-x^2-y^2)/(2*σ^2))
其中，G(x,y)表示高斯函数在(x,y)位置的值，σ为高斯函数的标准差，e为自然常数。这个公式用于生成一个高斯核，可以用于卷积图像。
通过改变σ的值，可以调整高斯核的大小和强度。较大的σ会产生较模糊的图像，而较小的σ会产生较锐利的图像。因此，高斯差分公式可以用于平滑和降噪图像，同时保留图像的边缘和细节。
步骤2.2.3：对每个高斯差分图像计算Hessian矩阵的行列式，得到每个像素点的尺度不变特征。Hessian矩阵是一个二阶导数矩阵，包含了图像局部的二阶信息。
步骤2.2.4：在不同尺度的图像中，对每个像素的尺度不变特征进行极值检测。极值点被定义为在该点的所有邻域中，该点像素值最大或最小的点。
步骤2.2.5：对于检测到的极值点，通过插值方法计算出其精确的位置和尺度，并计算出其主方向。主方向是在该点的邻域中，梯度方向上具有最大响应的方向。
假设我们有两个已知的点(x1,y1)和(x2,y2)，我们想要通过插值方法计算出另一个点(x3,y3)的精确位置和尺度。我们可以使用线性插值公式来计算：
(y3-x1)*(y2-y1)/(x2-x1)+y1
其中，y3表示我们想要计算的点的纵坐标，x3表示我们想要计算的点的横坐标，x1和y1表示已知的第一个点的横纵坐标，x2和y2表示已知的第二个点的横纵坐标。这个公式的意义是：将(x3,y3)点放在已知的两个点所在的直线上，按比例计算纵坐标y3。
步骤2.2.6：最后，根据每个特征点的位置、尺度和主方向，生成SURF描述符。描述符是一个向量，包含了该特征点周围区域的梯度信息，用于表示该特征点的特征。
同Sift一样，Surf的尺度空间也是由O组L成组成，不同的是，Sift中下一组图像的尺寸是上一组的一半，同一组间图像尺寸一样，但是所使用的高斯模糊系数逐渐增大；而在Surf中，不同组间图像的尺寸都是一致的，不同的是不同组间使用的盒式滤波器的模板尺寸逐渐增大，同一组间不同层间使用相同尺寸的滤波器，但是滤波器的模糊系数逐渐增大。最后分别进行特征点定位，特征点主方向分配，生成特征点描述，特征点匹配。
步骤3：视觉特征点与激光雷达点云的深度配准，解决特征跟踪误差，其中，distance表示特征在{S}系下的z值；右上标k表示图象帧；用I表示{S^k}图像特征的集合
S系下第k帧已知深度的特征i表示为：S系下第k帧未知深度的特征i表示为：/>其中/>最后的运动模型可以定义为:
如果第k帧特征的深度可用，我们可以直接把这个深度关联到K-1帧的特征深度。由于这里还没有计算k-1到k的位姿，所有我们还不能通过深度图或者三角化得到第k帧的特征深度。定义是不知道深度的/>特征点的深度，且/>对于k-1帧有深度，k帧没深度的特征点，用第三行来融入一二行:：第三行/>然后用第一行/>减；第三行然后用第二行减，这样就可以求出来k帧的z：
其中Rl和Tl，l∈{1,2,3},l∈{1,2,3}表示R和T的第l行。
对于k帧和k-1帧都没有深度的点:
利用上边两个公式得到每个点的权重，残差越大权重越小，当残差大于某个阈值认为是外点需要剔除。设置其权重为0；如果找到收敛或满足最大迭代次数，则优化将终止。
步骤4：根视觉特征点与激光雷达点云的深度配准，其中：
利用激光雷达的点云和视觉里程计得到的位姿对齐和维护了一个深度图。计算运动的时候，按照深度来源划分可以得到三种特征点：来自深度图的深度；利用先前的位姿三角化得到的深度；没有深度的特征点。
如果第k帧特征的深度可用，我们可以直接把这个深度关联到K-1帧的特征深度。在求解运动时利于所有的方程用六个未知数来表示运动估计问题。这个问题由LM解决。
首先，设非线性函数为F(x)，待求解的最小二乘问题为：
minx∈R^n(1/2)||F(x)||^2
其中，‖·‖表示欧几里得范数(即向量的长度)
Levenberg-Marquardt算法的迭代公式为：
其中，xk和xk+1分别表示第k和k+1次迭代的解向量，Jk是F(xk)的雅可比矩阵，rk是F(xk)的残差向量，I是单位矩阵，λk是控制步长的参数。当λk取较大的值时，算法类似于梯度下降法；当λk取较小的值时，算法类似于高斯-牛顿法。
迭代的终止条件可以是残差的大小达到某个阈值或者迭代次数达到一定值运动估计采用一个鲁棒的拟合框架来解决特征跟踪误差。
步骤5：生成视觉特征点的深度图，其中：
维护深度图的时候，只添加相机前边的雷达点，那些在一定时间前的点就被丢弃。深度图会进行降采样保证常数的密度，还要把这些点投影到已经得到位姿的k-1帧。使用距离和两个角度在球面坐标上表示深度图上的点。这些点基于两个角坐标存储在2D KD树中。当深度图中的点关联到特征点的时候，在深度图中刚找到3个和特征点最近的深度点组成一个平面的patch，利用相机中心到平面的视线插值得到特征点的深度。
假设相机与物体平面垂直，如果相机与物体平面不垂直，则需要进行透视校正。
假设相机中心的坐标为(Cx,Cy,Cz)，特征点在图像平面上的坐标为(u,v)，对应的三维坐标为(X,Y,Z)。则特征点的深度信息Z可以通过以下公式计算：
其中，f是相机的焦距，d是特征点在图像平面上的实际距离(即物理尺寸)，v-Cy是特征点在图像平面上的像素坐标与相机中心的像素坐标之差。
如果多次跟踪特征点在深度图中始终得不到深度，就用三角化得到深度。
假设已知两个点A和B，它们之间的距离为dAB，目标点为C，AC和BC与水平面的夹角分别为α和β，则目标点C的深度为：
depth(C)＝dAB*sin(β)/sin(α+β)
其中，sin(α+β)可以通过三角函数公式sin(α+β)＝sinαcosβ+cosαsinβ来计算。
步骤6：激光雷达输入的特征提取模块，其主要分为两个步骤：区域分割和特征提取。区域分割阶段主要完成特征模式的分类及识别确定，即确定特征属于哪类模式，如直线，圆弧等，并确定属于该特征模式的区域及区域内的激光数据点集。特征提取阶段主要完成各类特征模式参数的确定以及特征点的提取。
步骤7：地图更新，其特具体步骤：
根据估计的最优状态变量，把特征点全部投影到全局坐标系
假设我们有一组特征点，它们在相机坐标系下的坐标为pi＝(xi,yi,zi)T，其中i表示第i个特征点。
我们需要将这些特征点投影到全局坐标系下，假设相机的位姿为表示相机坐标系到全局坐标系的变换矩阵，那么特征点在全局坐标系下的坐标pi＝(xi,yi,zi)T可以表示为：
其中可以通过相机位姿估计算法得到。这样，我们就可以将所有特征点投影到全局坐标系下了。
步骤8：状态估计模块，其特具体步骤：
先对雷达输入进行预处理，再利用卡尔曼滤波来估计状态量x。其中还包括了IMU的反向传播(运动补偿)。使用KD树进行临近点搜索，然后进行迭代卡尔曼滤波的迭代更新。
预测步骤：
其中是在不考虑当前观测值的情况下对状态的预测值；f是状态转移函数，uk-1是外部输入；Pk|k-1是预测误差协方差矩阵；Fk-1是状态转移矩阵；Qk-1是过程噪声协方差矩阵。
更新步骤：
其中yk是测量残差；zk是观测值；h是测量函数；Sk是测量噪声协方差矩阵；Hk是测量矩阵；Rk是测量噪声协方差矩阵；Kk是卡尔曼增益；是对状态的最优估计；Pk|k是状态估计误差协方差矩阵。
在非线性系统中，f和h可能是非线性函数
步骤9：特征点注册到全局框架，其特具体步骤：
需要使用一个SLAM算法，即同时定位和建图算法。SLAM算法可以通过对相机的运动和场景的结构进行估计，将特征点从相机坐标系转换到全局坐标系中。
假设特征点在相机坐标系下的坐标为Pc＝[Xc,Yc,Zc,1]T，相机位姿在全局坐标系下的坐标为T＝[R|t]，其中R为3×3的旋转矩阵，t为3×1的平移矢量，那么特征点在全局坐标系下的坐标Pw＝[Xw,Yw,Zw,1]T可以通过以下公式计算得到：
Pw＝T·Pc
其中·表示矩阵乘法，即
T·Pc＝[R t01][Xc Yc Zc 1][RXc+tx RYc+ty RZc+tz 1]
其中tx,ty,tz分别为t的三个分量。
具体来说，SLAM算法会根据相邻帧之间的视觉差异和IMU等传感器信息，光流法估计相机的运动轨迹，并同时构建一个全局地图。
最基本的光流方程是：
I(x+u,y+v,t+1)＝I(x,y,t)
其中，I表示图像，x、y表示像素坐标，t表示时间，u、v表示像素在x和y方向上的位移。这个方程表示了在时间t到t+1之间，像素的亮度值不变，因为它们在运动。
根据泰勒级数展开，将上述方程转换为：
I(x+u,y+v,t+1)≈I(x,y,t)+uIx+vIy+It
其中，Ix、Iy表示图像在x和y方向上的梯度，It表示相邻帧之间的时间差。这个方程用于计算像素的运动。
根据最小化运动误差的原则，可以得到光流方程的解：
u＝(Ix*It+Ix*Iy*v)/(Ix^2+Iy^2)v＝(Iy*It+Ix*Iy*u)/(Ix^2+Iy^2)
当SLAM算法估计出相机的运动轨迹和全局地图后，就可以将特征点从相机坐标系转换到全局坐标系中，从而实现将特征点注册到全局框架的目的。
p_global＝T*p_local
其中，p_local是特征点在局部坐标系中的坐标，p_global是特征点在全局坐标系中的坐标，T是描述局部坐标系相对于全局坐标系的变换矩阵。
步骤10：特征点图合并，其特具体步骤：
我们需要将传感器获取的数据进行融合，以得到更加准确和完整的环境地图。在地图构建中，我们需要对每个传感器获取的数据进行姿态估计，以便将其注册到全局框架中。
假设机器人的IMU可以测量三个轴向的加速度a和角速度w。则机器人的位移可以通过积分加速度得到：
位移＝∫∫a dt dt
机器人的姿态可以通过积分角速度得到：
姿态＝∫w dt
然后，我们可以将已经注册的特征点与之前构建的特征点图进行合并，从而构建一个更加完整和准确的地图。通过这种方式，我们可以利用多种传感器的优势，提高环境地图的精度和鲁棒性。
步骤11：IMU反向传播，其特具体步骤：
在IMU状态估计模块中，反向传播算法被用来计算损失函数对模型中每个参数的偏导数，以便更新这些参数，使模型的输出接近于实际的IMU状态。具体来说，反向传播算法通过将模型的输出与实际状态进行比较，计算损失函数的梯度，然后通过链式法则将梯度向后传递到每个网络层，最终更新每个参数的值。
具体的，本发明提供一个示例，求误差E对w5的导数。首先明确这是一个“链式求导”过程，要求误差E对w5的导数，需要先求误差E对out o1的导数，再求out o1对net o1的导数，最后再求net o1对w5的导数，经过这个链式法则，我们就可以求出误差E对w5的导数(偏导)，如附图3所示，显示了导数(梯度)计算出来的过程以及反向传播与参数更新过程。
Claims (10)
1.一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于包括以下步骤：
A.前端输入：通过单目相机采集场景的图像连续帧，通过激光雷达采集点云信息，通过IMU采集加速度信息；
B.数据处理：包括a.对图像输入采用特征跟踪算法在连续的图像帧中检测和匹配视觉特征点；利用视觉特征点和激光雷达点云进行深度图配，块把局部的深度图和点云对齐并得到视觉特征点的深度图，帧到帧的运动估计模块利用特征点计算机体运动；
b.将激光雷达输入数据输入到特征提取模块，以此来获得平面特征和边缘特征，然后以视觉模块计算得到的数据为先验，将提取的特征和IMU测量值输入到状态估计模块，在10Hz-50Hz下进行状态估计；
C.地图更新：估计的姿态将特征点注册到全局框架中，并将它们与到目前为止构建的特征点图合并，更新后的地图会在下一步中加入更多新的点，在IMU的状态估计模块中增加了反向传播的过程，并将反向传播所得的结果与正向传播所得结果一起输入到剩余计算中去。
2.根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于特征跟踪算法包括如下步骤：
首先做出如下假设：
(1)相邻帧之间的亮度恒定；
(2)相邻视频帧的取帧时间连续，或者，相邻帧之间物体的运动比较“微小”；
(3)同一子图像的像素点具有相同或相似的运动；
基于Lucas-Kanade算法，利用两帧之间像素坐标的变化确定图像的光流输出。
3.根据权利要求1所述根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于检测视觉特征点实现空间尺度不变性，模仿实际生活中物体近大远小、近清晰远模糊的特点，具体实现流程如下：
首先，构建Hessian(黑塞矩阵)，生成所有的兴趣点，用于特征的提取；构建Hessian矩阵的目的是为了生成图像稳定的边缘点(突变点)，为后续的特征提取做好基础，构建Hessian矩阵的过程对应于Sift算法中的高斯卷积过程，然后构建尺度空间，同Sift一样，Surf的尺度空间也是由O组L成组成，不同的是，Sift中下一组图像的尺寸是上一组的一半，同一组间图像尺寸一样，但是所使用的高斯模糊系数逐渐增大；而在Surf中，不同组间图像的尺寸都是一致的，不同的是不同组间使用的盒式滤波器的模板尺寸逐渐增大，同一组间不同层间使用相同尺寸的滤波器，但是滤波器的模糊系数逐渐增大；最后分别进行特征点定位，特征点主方向分配，生成特征点描述，特征点匹配。
4.根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于视觉特征点与激光雷达点云的深度配准的过程包括：
利用激光雷达的点云和视觉里程计得到的位姿对齐和维护了一个深度图；计算运动的时候，按照深度来源划分可以得到三种特征点：
来自深度图的深度、利用先前的位姿三角化得到的深度、没有深度的特征点；
如果第k帧特征的深度可用，我们可以直接把这个深度关联到K-1帧的特征深度，在求解运动时利于所有的方程用六个未知数来表示运动估计问题，这个问题由LM解决。运动估计采用一个鲁棒的拟合框架来解决特征跟踪误差。
5.根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于生成视觉特征点的深度图的过程如下：
维护深度图的时候，只添加相机前边的雷达点，那些在一定时间前的点就被丢弃；深度图会进行降采样保证常数的密度，还要把这些点投影到已经得到位姿的k-1帧，使用距离和两个角度在球面坐标上表示深度图上的点，这些点基于两个角坐标存储在2D KD树中，当深度图中的点关联到特征点的时候，在深度图中刚找到3个和特征点最近的深度点组成一个平面的patch，利用相机中心到平面的视线插值得到特征点的深度，如果多次跟踪特征点在深度图中始终得不到深度，就用三角化得到深度。
6.根据权利要求1所述根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于激光雷达输入的特征提取模块主要分为两个步骤：区域分割和特征提取；区域分割阶段主要完成特征模式的分类及识别确定，即确定特征属于哪类模式，如直线，圆弧等，并确定属于该特征模式的区域及区域内的激光数据点集；特征提取阶段主要完成各类特征模式参数的确定以及特征点的提取。
7.根据权利要求1所述根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于状态估计模块先对雷达输入进行预处理，再利用卡尔曼滤波来估计状态量x；其中还包括了IMU的反向传播(运动补偿)。使用KD树进行临近点搜索，然后进行迭代卡尔曼滤波的迭代更新。
8.根据权利要求1所述根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于特征点注册到全局框架的过程中使用一个SLAM算法，即同时定位和建图算法；SLAM算法可以通过对相机的运动和场景的结构进行估计，将特征点从相机坐标系转换到全局坐标系中；具体来说，SLAM算法会根据相邻帧之间的视觉差异和IMU等传感器信息，光流法估计相机的运动轨迹，并同时构建一个全局地图；
最基本的光流方程是：
I(x+u,y+v,t+1)＝I(x,y,t)
其中，I表示图像，x、y表示像素坐标，t表示时间，u、v表示像素在x和y方向上的位移，这个方程表示了在时间t到t+1之间，像素的亮度值不变，因为它们在运动；
根据泰勒级数展开，将上述方程转换为：
I(x+u,y+v,t+1)≈I(x,y,t)+uIx+vIy+It
其中，Ix、Iy表示图像在x和y方向上的梯度，It表示相邻帧之间的时间差；这个方程用于计算像素的运动；
根据最小化运动误差的原则，可以得到光流方程的解：
u＝(Ix*It+Ix*Iy*v)/(Ix^2+Iy^2)v＝(Iy*It+Ix*Iy*u)/(Ix^2+Iy^2)
当SLAM算法估计出相机的运动轨迹和全局地图后，就可以将特征点从相机坐标系转换到全局坐标系中，从而实现将特征点注册到全局框架的目的。
9.根据权利要求1所述根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于特征点图合并时需要将传感器获取的数据进行融合，以得到更加准确和完整的环境地图；在地图构建中，需要对每个传感器获取的数据进行姿态估计，以便将其注册到全局框架中；
假设机器人的IMU可以测量三个轴向的加速度a和角速度w，则机器人的位移可以通过积分加速度得到：
位移＝∫∫a dt dt
机器人的姿态可以通过积分角速度得到：
姿态＝∫w dt
然后，可以将已经注册的特征点与之前构建的特征点图进行合并，从而构建一个更加完整和准确的地图，通过这种方式，可以利用多种传感器的优势，提高环境地图的精度和鲁棒性。
10.根据权利要求1所述根据权利要求1所述的一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法，其特征在于：
在IMU状态估计模块中，反向传播算法被用来计算损失函数对模型中每个参数的偏导数，以便更新这些参数，使模型的输出接近于实际的IMU状态；具体来说，反向传播算法通过将模型的输出与实际状态进行比较，计算损失函数的梯度，然后通过链式法则将梯度向后传递到每个网络层，最终更新每个参数的值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311294675.7A CN117367427A (zh) | 2023-10-07 | 2023-10-07 | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311294675.7A CN117367427A (zh) | 2023-10-07 | 2023-10-07 | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117367427A true CN117367427A (zh) | 2024-01-09 |
Family
ID=89388411
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311294675.7A Pending CN117367427A (zh) | 2023-10-07 | 2023-10-07 | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117367427A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117949968A (zh) * | 2024-03-26 | 2024-04-30 | 深圳市其域创新科技有限公司 | 激光雷达slam定位方法、装置、计算机设备和存储介质 |
| CN117949942A (zh) * | 2024-03-26 | 2024-04-30 | 北京市计量检测科学研究院 | 基于雷达数据和视频数据融合的目标跟踪方法及系统 |
-
2023
- 2023-10-07 CN CN202311294675.7A patent/CN117367427A/zh active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117949968A (zh) * | 2024-03-26 | 2024-04-30 | 深圳市其域创新科技有限公司 | 激光雷达slam定位方法、装置、计算机设备和存储介质 |
| CN117949942A (zh) * | 2024-03-26 | 2024-04-30 | 北京市计量检测科学研究院 | 基于雷达数据和视频数据融合的目标跟踪方法及系统 |
| CN117949942B (zh) * | 2024-03-26 | 2024-06-07 | 北京市计量检测科学研究院 | 基于雷达数据和视频数据融合的目标跟踪方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110070615B (zh) | 一种基于多相机协同的全景视觉slam方法 | |
| CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
| CN109166149B (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与系统 | |
| CN109211241B (zh) | 基于视觉slam的无人机自主定位方法 | |
| CN109029433B (zh) | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 | |
| CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN104732518B (zh) | 一种基于智能机器人地面特征的ptam改进方法 | |
| CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
| CN110044354A (zh) | 一种双目视觉室内定位与建图方法及装置 | |
| CN109781092B (zh) | 一种危险化工事故中移动机器人定位与建图方法 | |
| CN113706626B (zh) | 一种基于多传感器融合及二维码校正的定位与建图方法 | |
| Liu et al. | Direct visual odometry for a fisheye-stereo camera | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| CN109579825A (zh) | 基于双目视觉和卷积神经网络的机器人定位系统及方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN112802096A (zh) | 实时定位和建图的实现装置和方法 | |
| CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
| CN112541423A (zh) | 一种同步定位与地图构建方法和系统 | |
| CN116128966A (zh) | 一种基于环境物体的语义定位方法 | |
| CN115147344A (zh) | 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN112945233A (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN117330052A (zh) | 基于红外视觉、毫米波雷达和imu融合的定位与建图方法及系统 | |
| Hu et al. | Efficient Visual-Inertial navigation with point-plane map | |
| Billy et al. | Adaptive SLAM with synthetic stereo dataset generation for real-time dense 3D reconstruction |
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 |