CN114018236B - 一种基于自适应因子图的激光视觉强耦合slam方法 - Google Patents

一种基于自适应因子图的激光视觉强耦合slam方法 Download PDF

Info

Publication number
CN114018236B
CN114018236B CN202111168147.8A CN202111168147A CN114018236B CN 114018236 B CN114018236 B CN 114018236B CN 202111168147 A CN202111168147 A CN 202111168147A CN 114018236 B CN114018236 B CN 114018236B
Authority
CN
China
Prior art keywords
robot
scene
laser
point
point cloud
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
CN202111168147.8A
Other languages
English (en)
Other versions
CN114018236A (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.)
Harbin Juche Technology Co ltd
Harbin Engineering University
Original Assignee
Harbin Juche Technology Co ltd
Harbin Engineering University
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 Harbin Juche Technology Co ltd, Harbin Engineering University filed Critical Harbin Juche Technology Co ltd
Priority to CN202111168147.8A priority Critical patent/CN114018236B/zh
Publication of CN114018236A publication Critical patent/CN114018236A/zh
Application granted granted Critical
Publication of CN114018236B publication Critical patent/CN114018236B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
    • 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/08—Systems determining position data of a target for measuring distance only
    • 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
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • G06F18/251—Fusion techniques of input or preprocessed data
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Electromagnetism (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Evolutionary Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

一种基于自适应因子图的激光视觉强耦合SLAM方法，涉及激光视觉强耦合领域。目前视觉系统与雷达系统结合不够紧密，无法动态的调整多传感器数据的融合方式和特征信息的利用程度低的问题。本发明提出的方法，包括：定义四种机器人场景，并根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景；通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿；激光里程计模块根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿；单目相机模块采集特征点信息，进行重投影误差计算。本发明适用于机器人在无GPS信息下的室内室外融合场景下的自主定位与环境感知。

Description

一种基于自适应因子图的激光视觉强耦合SLAM方法
技术领域
本发明涉及激光视觉强耦合领域，尤其涉及一种基于自适应因子图的激光视觉强耦合SLAM方法领域。
背景技术
现有基于激光雷达与IMU融合的SLAM方法由于激光雷达传感器采集距离信息精度高且距离远，在结合IMU提供机器人的旋转信息，在室外场景中定位准确、建图质量高。可是，激光雷达传感器在一些退化场景下表现不佳，如草地、运动场等缺乏几何特征的空旷室外场景无法准确的提取特征点，导致定位不准确。并且，在室内等较小场景中，激光雷达采集到的点可能会集中在一些距离激光雷达较近的物体上，导致无法完整的收集场景中的信息，引起系统定位失效。
相比之下，基于视觉惯导的VIO算法在室内光照稳定的情况下效果较好。但是视觉SLAM系统易受环境光照强度改变以及纹理信息的影响，导致其在室外场景中系统容易崩溃。
综上所述，最近提出了一些基于激光雷达与视觉相融合的SLAM方法。这些方法的核心思想是利用视觉信息提取有用的特征点，从而帮助激光雷达惯导系统的运行。但是现有的激光雷达与视觉融合SLAM方法有如下缺点：
1.系统与雷达系统结合不够紧密，没有构建一个统一的模型来解决视觉与激光雷达数据融合的位姿估计问题；
2.由于缺少对周围环境的感知，无法动态的调整多传感器数据的融合方式，如可能错误的在室内场景中过多的依赖不可靠的激光雷达信息而不是更准确的视觉信息；
3.无法针对不同的场景采取不同的特征提取方式，对于特征信息的利用程度不高。
发明内容
本发明解决了视觉系统与雷达系统结合不够紧密，无法动态的调整多传感器数据的融合方式和特征信息的利用程度低的问题。
一种基于自适应因子图的激光视觉强耦合SLAM方法，包括：
激光雷达模块，单目相机模块，IMU模块，激光回环检测单元，因子图优化器单元，单目回环检测单元和地图存储模块；
所述激光雷达模块包含激光里程计单元和场景检测单元；
所述单目相机模块包含场景检测单元和重投影误差计算；
所述IMU模块包含IMU预积分单元；
定义四种机器人场景，并根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景；
通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿；
激光里程计单元根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿；
所述单目相机模块采集特征点信息，进行重投影误差计算。
进一步的，所述的定义四种机器人场景为：室内场景，室外场景，结构信息丰富场景，结构信息匮乏场景。
进一步的，所述的根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景，包括：
根据采集的三维激光雷达数据转换为高度图；
将三维点云在X-Y平面栅格地图上投影，对于同一栅格内存在多个点的情况，保留集合Z值最高的点作为该栅格的值，保留的栅格点通过RANSAC方法拟合一个二次曲线；
删除不包含投影点的栅格；
计算所述拟合的二次曲线所包围的空间面积S：
如果空间面积S小于阈值，则判定机器人处于室内场景，如果空间面积S大于阈值，则判定机器人处于室外场景；同时，该阈值存储在一个长度固定的队列中，计算该阈值与队列中数值平均数差距是否大于阈值，若大于则标记系统处于场景变化状态中。
进一步的，所述机器人处于室外场景时，确定场景中是否存在足够的结构信息来保证特征点提取的稳定性：
点云数据进行物体分割处理；
任意两个相邻的激光点云数据打在同一物体表面上，则其两点构成线段的切线方向指向激光雷达，其夹角β：
其中点A，点B为相邻的两个激光点云，点O为激光雷达；
若夹角β大于设定的阈值，则相邻的两个激光点云数据处于同一物体上；
若夹角β小于设定的阈值，则相邻的两个激光点云数据处于两个物体上；
扫描点云中所有相邻点云数据，据类分割点云数据：对于提取物体，若打在该物体上的激光点云的数目小于设定的阈值，则认为该物体为噪声，将其剔除。
进一步的，所述的通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿，包括：
在IMU预积分单元中，对IMU模块中的角速度和线加速度通过流形进行积分；
通过在李群三维旋转群SO3上对IMU的角速度进行积分，计算出两帧激光雷达数据之间机器人的相对位姿，其构建的残差方程为：
其中，rp代表位置、rq代表旋转、rv代表速度、rba代表IMU加速度偏置，rbg代表陀螺仪偏置；代表i到j时刻内位置预积分处理的结果、/>代表i到j时刻内速度预积分处理的结果、/>代表i到j时刻内旋转预积分处理的结果；/>代表了世界坐标系到body坐标系j时刻的相对旋转四元数；/>代表body坐标系i时刻到body坐标系j时刻的相对旋转四元数，/>代表了i时刻IMU在世界坐标系中的速度，/>代表了j时刻IMU在世界坐标系中的速度Δt是两帧之间的时间间隔，gw为世界坐标系中的重力加速度；/>代表i时刻的加速度偏置，代表i时刻的陀螺仪偏置。
进一步的，所述的激光里程计单元根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿，包括：
当机器人所处室内场景或者结构信息匮乏场景中时，机器人通过NDT方法进行帧间里程计，其残差函数Score(T1)为：
其中，T为两帧点云之间的转换矩阵，p为落入的栅格确定该点对应的概率，xi为第i个点云位置坐标；
当机器人处在室外场景，或结构信息丰富场景时，使用基于特征点的帧间里程计，其残差函数Score(T2)为：
其中，pik表示当前帧点云中第i个物体的第k个特征点，pi′k表示上一帧点云中第i个物体第k个特征点。
进一步的，所述单目相机模块采集特征点信息，进行重投影误差计算，包括：
在特征点信息丰富时，通过PnP方法对所有的特征点进行重投影误差计算；在特征点信息匮乏时，对所有的像素点进行重投影误差计算。
进一步的，所述PnP方法的残差方程为：
其中，ui为投影后的像素坐标，K为相机的内参矩阵，T为两帧图像之间机器人的位姿；i在特征点信息丰富时为所有特征点的数量和，i在特征点信息匮乏时是所有像素点的和。
本发明还提供一种计算机可读存储介质，所述计算机可读存储介质用于储存计算机程序，所述计算机程序执行如上文任一项所述的一种基于自适应因子图的激光视觉强耦合SLAM方法。
本发明还提供一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，当所述处理器运行所述存储器存储的计算机程序时，所述处理器执行根据如上文任一项中所述的一种基于自适应因子图的激光视觉强耦合SLAM方法。
本发明的有益效果为：
本发明解决了视觉系统与雷达系统结合不够紧密，无法动态的调整多传感器数据的融合方式和特征信息的利用程度低的问题。
本发明的优势在于：
1.能供在室内室外融合场景下提供高精度的定位建图能力。
2.系统具有强鲁棒性，当视觉系统或激光雷达系统中某一系统失效时仍能够正常运行。
3.系统能够根据场景特征点不同选择不同的特征提取方式，可以更有效地利用特征信息，提升定位建图精度以及稳定性。
附图说明
图1为一种基于自适应因子图的激光视觉强耦合SLAM方法的系统流程框图；
图2为点云分割算法，其中，图2(a)为点A和点B的局部放大图，图2(b)为地面分割算法示意图；
图3为点云聚类效果图；
图4为两帧间点云匹配效果图；
图5为因子图模型；
图6为建图结果。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。
实施例一、参照图1说明本实施例。本实施例所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，包括：
激光雷达模块，单目相机模块，IMU模块，激光回环检测单元，因子图优化器单元，单目回环检测单元和地图存储模块；
所述激光雷达模块包含激光里程计单元和场景检测单元；
所述单目相机模块包含场景检测单元和重投影误差计算；
所述IMU模块包含IMU预积分单元；
定义四种机器人场景，并根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景；
通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿；
激光里程计单元根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿；
所述单目相机模块采集特征点信息，进行重投影误差计算。
其中，所述的定义四种机器人场景为：室内场景，室外场景，结构信息丰富场景，结构信息匮乏场景。
本实施例中提出了一种基于激光雷达与单目摄像头的场景感知方法，能供判断机器人所处的工作场景；提出了一种基于自适应因子图的多传感器融合算法，能够根据机器人所处的工作场景自适应的更改融合方式。
实施例二、参见图1说明本实施例。本实施例是对实施例一所述的一种基于自适应因子图的激光视觉强耦合SLAM方法的进一步限定，本实施例中，所述的根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景，包括：
根据采集的三维激光雷达数据转换为高度图；
将三维点云在X-Y平面栅格地图上做投影，对于同一栅格内存在多个点的情况，保留集合Z值最高的点作为该栅格的值，保留的栅格点通过RANSAC方法拟合一个二次曲线；
删除不包含投影点的栅格；
计算所述拟合的二次曲线所包围的空间面积S：
如果空间面积S小于阈值，则判定机器人处于室内场景，如果空间面积S大于阈值，则判定机器人处于室外场景；同时，该阈值存储在一个长度固定的队列中，计算该阈值与队列中数值平均数差距是否大于阈值，若大于则标记系统处于场景变化状态中。
本实施例中保留的栅格点通过RANSAC方法拟合一个二次曲线，使得该曲线经过的栅格数量变多，便于计算计算所述拟合的二次曲线所包围的空间面积S，进而判定机器人所处场景。
实施例三、参见图2和图3说明本实施例。本实施例是对实施例二所述的一种基于自适应因子图的激光视觉强耦合SLAM方法的进一步限定，本实施例中，所述机器人处于室外场景时，确定场景中是否存在足够的结构信息来保证特征点提取的稳定性：
点云数据进行物体分割处理；
任意两个相邻的激光点云数据打在同一物体表面上，则其两点构成线段的切线方向应指向激光雷达，其夹角β：
其中点A，点B为相邻的两个激光点云，点O为激光雷达；
若夹角β大于设定的阈值，则相邻的两个激光点云数据处于同一物体上；
若夹角β小于设定的阈值，则相邻的两个激光点云数据处于两个物体上；
扫描点云中所有相邻点云数据，将点云数据进行据类分割：对于提取物体，若打在该物体上的激光点云的数目小于设定的阈值，则认为该物体为噪声，将其剔除。
本实施例中通过对点云数据的聚类分割，计算出从当前点云中提取的物体数量，若数量小于某一个阈值时则认为处在结构信息匮乏场景中；进一步，对单目相机采集到的图像维护一个滑动窗口，该窗口记录一定数量的连续帧。对采集到的图像进行基于ORB的特征点检测，并判断该特征点是否在滑动窗口中稳定的存在，如果稳定存在的特征点数量N超过阈值，则系统处于特征点稳定状态中，反之则处于特征点缺失状态中。
实施例四、参见图1说明本实施例。本实施例是对实施例一所述的一种基于自适应因子图的激光视觉强耦合SLAM方法的进一步限定，本实施例中，所述的通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿，包括：
在IMU预积分单元中，对IMU模块中的角速度和线加速度通过流形进行积分；
通过在李群三维旋转群SO3上对IMU的角速度进行积分，计算出两帧激光雷达数据之间机器人的相对位姿，其构建的残差方程为：
其中，rp代表位置、rq代表旋转、rv代表速度、rba代表IMU加速度偏置，rbg代表陀螺仪偏置；i到j时刻内位置预积分处理的结果、/>代表i到j时刻内速度预积分处理的结果、/>代表i到j时刻内旋转预积分处理的结果；/>代表了世界坐标系到body坐标系j时刻的相对旋转四元数；/>代表body坐标系i时刻到body坐标系j时刻的相对旋转四元数，代表了i时刻IMU在世界坐标系中的速度，/>代表了j时刻IMU在世界坐标系中的速度Δt是两帧之间的时间间隔，gw为世界坐标系中的重力加速度；/>代表i时刻的加速度偏置，/>代表i时刻的陀螺仪偏置。
本实施例中，通过在一段时间内IMU构建的预积分量作为测量值，对两时刻之间的状态量进行约束。
实施例五、参见图4说明本实施例。本实施例是对实施例一所述的一种基于自适应因子图的激光视觉强耦合SLAM方法的进一步限定，本实施例中，所述的激光里程计单元根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿，包括：
当机器人所处室内场景或者结构信息匮乏场景中时，机器人通过NDT方法进行帧间里程计。首先将上一帧点云数据通过正态分布变换将三维点云数据转换为三维高斯分布集合。对三维空间划分为不同的栅格，栅格划分点云为不同集合，对每一个集合中的点计算网格中点集的均值μ，计算网格中点集的协方差矩阵Σ，网格中的观测到点x的概率p(x)服从正态分布N(μ,Σ)。其均值和方差计算方法如下所示：
其中，为栅格中点k，m为该栅格中包含点的数量。通过正态分布变换将每一个栅格转换为一个高斯正态分布。进一步，对当前点云数据中的每一个点将其根据位姿信息投影到上一帧对应坐标系中，并根据其落入的栅格确定该点对应的概率p，p的计算公式如下所述：
其中，x为投影后该点在上一帧点云中的坐标、μ,Σ为该栅格所服从的正态分布的均值和方差。由此构建其残差函数Score(T1)为：：
其中，T为两帧点云之间的转换矩阵，p为落入的栅格确定该点对应的概率，xi为第i个点云位置坐标；
当机器人处在室外场景，或结构信息丰富场景时，使用基于特征点的帧间里程计。首先根据场景检测模块中提取的聚类信息将点云进行分类。在对每一类中的点云数据通过体素滤波器进行滤波，再按照点云数据在空间中曲度的改变量，可以将点云按照曲度进行排序，并将其分为点特征点和面特征点。对t时刻的点云中一个点ri，在点ri的沿着同一激光束方向找其前后相邻的点云rj,j∈(0,n)组成集合S，其中j为选择点云数量。
根据集合S计算曲度(平滑度)的公式如下所示：
其中，|S|为集合中点的数量，|ri|代表该点云的模二范数(到原点的欧式距离)。根据上述计算出来的点云，设定平滑度阈值cth：
c>cth：点特征点
c<cth：面特征点
根据上述规则，可以将点云分解为点特征点和面特征点。更进一步的，将特征信息与据类信息进行融合。对每一个据类出的物体，我们对点特征点和面特征点个10+/-5个特征点。由此可以得到残差函数为：
其中，pik表示当前帧点云中第i个物体的第k个特征点，p′ik表示上一帧点云中第i个物体第k个特征点。
本实施例中，当机器人处在室内场景或者结构信息匮乏场景中时，机器人使用较为鲁棒但运算量大的NDT方法。
为了鲁棒性的求解位姿估计问题，使用具有自适应性的因子图模型来求解机器人的位姿。
该模型如图5所示。图中黑色实心圆圈代表变量之间的约束条件，黑色空心圆圈代表目标优化变量。其中目标优化变量表示为：
xi＝Ri+ti
其中，R为旋转矩阵、t为平移向量。在已知测量值(残差方程形式表现)的前提下，求满足最大后验估计的机器人状态量X，可以将该最大后验估计等价转换为最小二乘问题，该最小二乘问题的目标函数为：
S(xi+1)＝λ1Score(T)+λ2Rpnp(T)
xi+1＝Txi
其中，Score(T)为由激光雷达引入的残差方程，Rpnp(T)为由单目相机引入的残差方程。其中T的初始值由IMU预积分提供。引入的残差方程形式以及优化权重λ1，λ2由系统所述的场景情况决定。其计算公式为：
其中，s为步骤一中计算出来的二次曲线所包围的空间面积，smin和smin分别表示处于有效空间的最小面积以及最大面积。nkey为图像中提取的特征点数量，smax为图像标准提取的特征点数量。通过使用高斯牛顿法或L-M方法求解该最小二乘问题即可估计机器人的相对位姿。并根据计算出的位姿，将通过体素滤波后的配准点云数据加入到地图中。
实施例六、参见图6说明本实施例。本实施例是对实施例一所述的一种基于自适应因子图的激光视觉强耦合SLAM方法的进一步限定，本实施例中，所述单目相机模块采集特征点信息，进行重投影误差计算，包括：
在特征点信息丰富时，通过PnP方法对所有的特征点进行重投影误差计算；在特征点信息匮乏时，对所有的像素点进行重投影误差计算。
所述PnP方法的残差方程为：
其中，ui为投影后的像素坐标，K为相机的内参矩阵，T为两帧图像之间机器人的位姿；i在特征点信息丰富时为所有特征点的数量和，i在特征点信息匮乏时是所有像素点的和。
本实施例中，通过使用高斯牛顿法或L-M方法求解该最小二乘问题即可估计机器人的相对位姿。并根据计算出的位姿，将通过体素滤波后的配准点云数据加入到地图中。
为了消除累计误差，当机器人移动到曾经经过的场景时提供额外的回环检测约束。首先分别使用基于DBOW的回环检测方法、基于几何信息的激光点云回环检测方法获得机器人在地图中的近似位姿，再从地图中提取周围10平方米的点云数据与当前雷达点云数据使用ICP方式进行配准，获得额外的优化约束。
实施例七、一种计算机可读存储介质，所述计算机可读存储介质用于储存计算机程序，所述计算机程序执行如上文任一项所述的一种基于自适应因子图的激光视觉强耦合SLAM方法。
实施例八、一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，当所述处理器运行所述存储器存储的计算机程序时，所述处理器执行根据如上文任一项中所述的一种基于自适应因子图的激光视觉强耦合SLAM方法。

Claims (9)

1.一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，包括：
激光雷达模块，单目相机模块，IMU模块，激光回环检测单元，因子图优化器单元，单目回环检测单元和地图存储模块；
所述激光雷达模块包含激光里程计单元和场景检测单元；
所述单目相机模块包含场景检测单元和重投影误差计算；
所述IMU模块包含IMU预积分单元；
定义四种机器人场景，并根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景；
通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿；
激光里程计单元根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿；
所述单目相机模块采集特征点信息，进行重投影误差计算；
所述激光里程计单元根据机器人所处的场景，使用不同的帧间匹配方式，获得两帧之间的机器人位姿，包括：
当机器人所处室内场景或者结构信息匮乏场景中时，机器人通过NDT方法进行帧间里程计，其残差函数Score(T1)为：
其中，T为两帧点云之间的转换矩阵，p为落入的栅格确定该点对应的概率，xi为第i个点云位置坐标；
当机器人处在室外场景，或结构信息丰富场景时，使用基于特征点的帧间里程计，其残差函数Score(T2)为：
其中，pik表示当前帧点云中第i个物体的第k个特征点，pi′k表示上一帧点云中第i个物体第k个特征点。
2.根据权利要求1所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，所述的定义四种机器人场景为：室内场景，室外场景，结构信息丰富场景，结构信息匮乏场景。
3.根据权利要求1所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，所述的根据采集的激光雷达与单目相机数据通过场景检测模块判断当前机器人所处场景，包括：
根据采集的三维激光雷达数据转换为高度图；
将三维点云在X-Y平面栅格地图上投影，对于同一栅格内存在多个点的情况，保留集合Z值最高的点作为该栅格的值，保留的栅格点通过RANSAC方法拟合一个二次曲线；
删除不包含投影点的栅格；
计算所述拟合的二次曲线所包围的空间面积S：
如果空间面积S小于阈值，则判定机器人处于室内场景，如果空间面积S大于阈值，则判定机器人处于室外场景；同时，该阈值存储在一个长度固定的队列中，计算该阈值与队列中数值平均数差距是否大于阈值，若大于则标记系统处于场景变化状态中。
4.根据权利要求3所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，所述机器人处于室外场景时，确定场景中是否存在足够的结构信息来保证特征点提取的稳定性：
点云数据进行物体分割处理；
任意两个相邻的激光点云数据打在同一物体表面上，则其两点构成线段的切线方向指向激光雷达，其夹角β：
其中点A，点B为相邻的两个激光点云，点O为激光雷达；
若夹角β大于设定的阈值，则相邻的两个激光点云数据处于同一物体上；
若夹角β小于设定的阈值，则相邻的两个激光点云数据处于两个物体上；
扫描点云中所有相邻点云数据，据类分割点云数据：对于提取物体，若打在该物体上的激光点云的数目小于设定的阈值，则认为该物体为噪声，将其剔除。
5.根据权利要求1所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，所述通过机器人所处场景预处理IMU接收到的数据，计算出两帧激光雷达数据之间机器人的相对位姿，包括：
在IMU预积分单元中，对IMU模块中的角速度和线加速度通过流形进行积分；
通过在李群三维旋转群SO3上对IMU的角速度进行积分，计算出两帧激光雷达数据之间机器人的相对位姿，其构建的残差方程为：
其中，rp代表位置、rq代表旋转、rv代表速度、rba代表IMU加速度偏置，rbg代表陀螺仪偏置；代表i到j时刻内位置预积分处理的结果、/>代表i到j时刻内速度预积分处理的结果、/>代表i到j时刻内旋转预积分处理的结果；/>代表了世界坐标系到body坐标系j时刻的相对旋转四元数；/>代表body坐标系i时刻到body坐标系j时刻的相对旋转四元数，/>代表了i时刻IMU在世界坐标系中的速度，/>代表了j时刻IMU在世界坐标系中的速度Δt是两帧之间的时间间隔，gw为世界坐标系中的重力加速度；/>代表i时刻的加速度偏置，/>代表i时刻的陀螺仪偏置。
6.根据权利要求1所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，所述单目相机模块采集特征点信息，进行重投影误差计算，包括：
在特征点信息丰富时，通过PnP方法对所有的特征点进行重投影误差计算；在特征点信息匮乏时，对所有的像素点进行重投影误差计算。
7.根据权利要求6所述的一种基于自适应因子图的激光视觉强耦合SLAM方法，其特征在于，所述PnP方法的残差方程为：
其中，ui为投影后的像素坐标，K为相机的内参矩阵，T为两帧图像之间机器人的位姿；i在特征点信息丰富时为所有特征点的数量和，i在特征点信息匮乏时是所有像素点的和。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质用于储存计算机程序，所述计算机程序执行权利要求1-7任一项所述的一种基于自适应因子图的激光视觉强耦合SLAM方法。
9.一种计算机设备，其特征在于：包括存储器和处理器，所述存储器中存储有计算机程序，当所述处理器运行所述存储器存储的计算机程序时，所述处理器执行根据权利要求1-7中任一项中所述的一种基于自适应因子图的激光视觉强耦合SLAM方法。
CN202111168147.8A 2021-09-30 2021-09-30 一种基于自适应因子图的激光视觉强耦合slam方法 Active CN114018236B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111168147.8A CN114018236B (zh) 2021-09-30 2021-09-30 一种基于自适应因子图的激光视觉强耦合slam方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111168147.8A CN114018236B (zh) 2021-09-30 2021-09-30 一种基于自适应因子图的激光视觉强耦合slam方法

Publications (2)

Publication Number Publication Date
CN114018236A CN114018236A (zh) 2022-02-08
CN114018236B true CN114018236B (zh) 2023-11-03

Family

ID=80055333

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111168147.8A Active CN114018236B (zh) 2021-09-30 2021-09-30 一种基于自适应因子图的激光视觉强耦合slam方法

Country Status (1)

Country Link
CN (1) CN114018236B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114563000B (zh) * 2022-02-23 2024-05-07 南京理工大学 一种基于改进型激光雷达里程计的室内外slam方法
CN114571450A (zh) * 2022-02-23 2022-06-03 达闼机器人股份有限公司 机器人控制方法、装置及存储介质
CN115183778A (zh) * 2022-07-01 2022-10-14 北京斯年智驾科技有限公司 一种基于码头石墩的建图方法、装置、设备以及介质
CN115265561A (zh) * 2022-09-27 2022-11-01 小米汽车科技有限公司 车辆定位方法、装置、车辆及介质
CN115683170B (zh) * 2023-01-04 2023-03-14 成都西物信安智能系统有限公司 基于雷达点云数据融合误差的校准方法
CN116222543B (zh) * 2023-04-26 2023-07-28 齐鲁工业大学(山东省科学院) 用于机器人环境感知的多传感器融合地图构建方法及系统
CN117470230A (zh) * 2023-10-23 2024-01-30 广州创源机器人有限公司 基于深度学习的视觉激光传感器融合定位算法

Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN110260861A (zh) * 2019-06-13 2019-09-20 北京华捷艾米科技有限公司 位姿确定方法及装置、里程计
CN110428467A (zh) * 2019-07-30 2019-11-08 四川大学 一种相机、imu和激光雷达联合的机器人定位方法
CN110487286A (zh) * 2019-08-09 2019-11-22 上海电器科学研究所（集团）有限公司 一种基于点特征投影与激光点云融合的机器人位姿判断法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
WO2020104423A1 (en) * 2018-11-20 2020-05-28 Volkswagen Aktiengesellschaft Method and apparatus for data fusion of lidar data and image data
CN111427047A (zh) * 2020-03-30 2020-07-17 哈尔滨工程大学 一种大场景下自主移动机器人slam方法
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN112634451A (zh) * 2021-01-11 2021-04-09 福州大学 一种融合多传感器的室外大场景三维建图方法
CN112767485A (zh) * 2021-01-26 2021-05-07 哈尔滨工业大学（深圳） 一种基于静态语义信息的点云地图创建与场景辨识方法
CN112785702A (zh) * 2020-12-31 2021-05-11 华南理工大学 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN113052903A (zh) * 2021-03-17 2021-06-29 浙江大学 一种用于移动机器人的视觉与雷达融合定位方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113074725A (zh) * 2021-05-11 2021-07-06 哈尔滨工程大学 一种基于多源信息融合的小型水下多机器人协同定位方法及系统
CN113253297A (zh) * 2021-06-21 2021-08-13 中国人民解放军国防科技大学 融合激光雷达和深度相机的地图构建方法及装置
CN113436260A (zh) * 2021-06-24 2021-09-24 华中科技大学 基于多传感器紧耦合的移动机器人位姿估计方法和系统

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108663677A (zh) * 2018-03-29 2018-10-16 上海智瞳通科技有限公司 一种多传感器深度融合提高目标检测能力的方法
CN109297510B (zh) * 2018-09-27 2021-01-01 百度在线网络技术（北京）有限公司 相对位姿标定方法、装置、设备及介质

Patent Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
WO2020104423A1 (en) * 2018-11-20 2020-05-28 Volkswagen Aktiengesellschaft Method and apparatus for data fusion of lidar data and image data
WO2020155616A1 (zh) * 2019-01-29 2020-08-06 浙江省北大信息技术高等研究院 一种基于数字视网膜的拍摄装置的定位方法
CN110260861A (zh) * 2019-06-13 2019-09-20 北京华捷艾米科技有限公司 位姿确定方法及装置、里程计
CN110428467A (zh) * 2019-07-30 2019-11-08 四川大学 一种相机、imu和激光雷达联合的机器人定位方法
CN110487286A (zh) * 2019-08-09 2019-11-22 上海电器科学研究所（集团）有限公司 一种基于点特征投影与激光点云融合的机器人位姿判断法
CN111045017A (zh) * 2019-12-20 2020-04-21 成都理工大学 一种激光和视觉融合的巡检机器人变电站地图构建方法
CN111427047A (zh) * 2020-03-30 2020-07-17 哈尔滨工程大学 一种大场景下自主移动机器人slam方法
CN112785702A (zh) * 2020-12-31 2021-05-11 华南理工大学 一种基于2d激光雷达和双目相机紧耦合的slam方法
CN112634451A (zh) * 2021-01-11 2021-04-09 福州大学 一种融合多传感器的室外大场景三维建图方法
CN112767485A (zh) * 2021-01-26 2021-05-07 哈尔滨工业大学（深圳） 一种基于静态语义信息的点云地图创建与场景辨识方法
CN113052903A (zh) * 2021-03-17 2021-06-29 浙江大学 一种用于移动机器人的视觉与雷达融合定位方法
CN113066105A (zh) * 2021-04-02 2021-07-02 北京理工大学 激光雷达和惯性测量单元融合的定位与建图方法及系统
CN113074725A (zh) * 2021-05-11 2021-07-06 哈尔滨工程大学 一种基于多源信息融合的小型水下多机器人协同定位方法及系统
CN113253297A (zh) * 2021-06-21 2021-08-13 中国人民解放军国防科技大学 融合激光雷达和深度相机的地图构建方法及装置
CN113436260A (zh) * 2021-06-24 2021-09-24 华中科技大学 基于多传感器紧耦合的移动机器人位姿估计方法和系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
一种室内高动态环境的机器人定位方法;黄山;电子科技大学学报;全文 *
基于启发式的多机器人SLAM 地图融合方法研究;王桐;系统仿真学报;全文 *
基于激光雷达与双目视觉的移动机器人SLAM研究;王消为;贺利乐;赵涛;;传感技术学报(第03期);全文 *
多传感器信息融合的车载定位方法的研究;赵静;高山;;数字通信(第04期);全文 *

Also Published As

Publication number Publication date
CN114018236A (zh) 2022-02-08

Similar Documents

Publication Publication Date Title
CN114018236B (zh) 一种基于自适应因子图的激光视觉强耦合slam方法
CN111258313B (zh) 多传感器融合slam系统及机器人
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN110070615B (zh) 一种基于多相机协同的全景视觉slam方法
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
CN111275763B (zh) 闭环检测系统、多传感器融合slam系统及机器人
CN108052103B (zh) 基于深度惯性里程计的巡检机器人地下空间同时定位和地图构建方法
CN113436260B (zh) 基于多传感器紧耦合的移动机器人位姿估计方法和系统
CN109084732A (zh) 定位与导航方法、装置及处理设备
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
WO2021212477A1 (zh) 校正点云数据的方法和相关装置
WO2021017211A1 (zh) 一种基于视觉的车辆定位方法、装置及车载终端
CN109978919A (zh) 一种基于单目相机的车辆定位方法及系统
CN111898552B (zh) 一种判别人员关注目标物的方法、装置及计算机设备
CN116147618B (zh) 一种适用动态环境的实时状态感知方法及系统
CN112907633A (zh) 动态特征点识别方法及其应用
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN113720323B (zh) 基于点线特征融合的单目视觉惯导slam方法及装置
CN116203976A (zh) 变电站室内巡检方法、装置、无人机和存储介质
CN114862953A (zh) 一种基于视觉特征和3d激光的移动机器人重定位方法及装置
CN112146647A (zh) 一种地面纹理的双目视觉定位方法及芯片
Xu et al. A Multi-source Information Fusion Method for Mobile Robot Visual-inertial Navigation
CN116226298B (zh) 一种地图质量的自动化评估方法
CN117671022B (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