CN115479598A - 基于多传感器融合的定位与建图方法及紧耦合系统 - Google Patents

基于多传感器融合的定位与建图方法及紧耦合系统 Download PDF

Info

Publication number
CN115479598A
CN115479598A CN202211011477.0A CN202211011477A CN115479598A CN 115479598 A CN115479598 A CN 115479598A CN 202211011477 A CN202211011477 A CN 202211011477A CN 115479598 A CN115479598 A CN 115479598A
Authority
CN
China
Prior art keywords
odometer
data
point cloud
laser radar
imu
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.)
Withdrawn
Application number
CN202211011477.0A
Other languages
English (en)
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.)
Changchun University of Technology
Original Assignee
Changchun University of Technology
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 Changchun University of Technology filed Critical Changchun University of Technology
Priority to CN202211011477.0A priority Critical patent/CN115479598A/zh
Publication of CN115479598A publication Critical patent/CN115479598A/zh
Withdrawn legal-status Critical Current

Links

Images

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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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

Abstract

本发明公开了一种基于多传感器融合的定位与建图方法及紧耦合系统，应用于移动机器人实时定位与建图技术领域。所述方法包括：获取激光雷达数据、IMU惯性测量数据、轮式里程计数据和GPS数据后，通过构建紧耦合的激光惯性里程计，实现将不同传感器的相对测量数据和绝对测量数据进行融合，再对上述的融合结果进行滑动窗口优化、回环检测和全局位姿优化，进而完成地图的定位与构建。其中，上述构建的紧耦合的激光惯性里程计极大提高移动机器人在复杂环境的适应能力、数据运算的稳定性以及系统的鲁棒性；并且通过多传感器的融合提升回环检测的精度和速度，解决了原有方案不能在复杂场景下实时定位与建图的鲁棒性与精度的问题。

Description

基于多传感器融合的定位与建图方法及紧耦合系统
技术领域
本发明涉及移动机器人实时定位与建图领域，具体涉及一种基于多传感器融合的定位与建图方法及紧耦合系统。
背景技术
同时定位与地图构建(simultaneous localization and mapping,SLAM)技术所关注的问题是载有传感器的机器人如何在未知环境中定位并构建出环境地图,是机器人估计自身状态和感知外部环境的关键技术。目前移动机器人的使用场景日益复杂,使得基于单一传感器的SLAM算法难以在复杂环境下鲁棒运行,例如激光雷达只能扫描平面信息,无法完整还原场景信息；惯性测量单元IMU存在零偏,采用积分的方式求解位姿会产生累积误差，轮式里程计位姿估计上相当于只有运动先验信息，地面颠簸时，也会造成位姿估计不准。
激光雷达SLAM技术一般与惯性测量单元IMU联合使用，通过IMU测量的数据提供激光雷达点云去畸变的初值，尽管激光雷达与IMU融合可以在一定程度上提升建图的精度，但是在复杂场景下,高效鲁棒地完成定位和建图的目标任务是亟需解决的问题。
针对SLAM问题中，在复杂场景下定位与建图的鲁棒性的问题，目前也有相应的解决方案：文献(Zhang J,Kaess M,Singh S.On degeneracy of optimization-based stateestimation problems.[C]//2016IEEE(ICRA).IEEE,2016.)文献提出了LOAM算法。LOAM定义了逐帧跟踪的边缘与平面3D特征点,使用高频率的IMU测量对两帧激光雷达之间的运动进行插值,该运动作为先验信息用于特征间的精准匹配,从而实现高精度里程计以及利用匀速运动假设来消除激光点云的运动畸变，取得了低漂移和低计算成本的平衡。但是，LOAM并没有闭环检测使得系统的鲁棒性并不好。文献(Shan T,Englot B,Meyers D,et al.LIO-SAM:Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping[J].2020.)通过引入局部扫描匹配提出的LIO-SAM(lidar inertial odometry viasmoothing and mapping),使用IMU预积分对激光雷达点云做运动补偿并为激光点云的配准提供初值,此外,该系统还可以加入闭环与GPS信息来消除漂移,但在复杂场景下，长时间的导航会导致系统漂移,鲁棒性不足。文献(Shan T,Englot B,Meyers D,et al.Lego-loam:Lightweight and ground-optimized lidar odometry and mapping on variableterrain[C]//2018IEEE/RSJ(IROS).IEEE,2018:4758-4765.)文献中提出的LEGO_LOAM算法针对地面无人平台对LOAM算法进行了改进，提出了利用点云地面分割技术提取出地面，并基于图像的分割方法将距离图像分为很多个聚类，去除了微小事物的噪声点，降低了计算量，并有效的得到在距离图像中的行和列的索引以及传感器的距离值，再利用剩余的特征点进行点云配准，同时加入了回环检测部分，使得点云地图更加精确。但是没有完全解决复杂环境下激光SLAM算法高度漂移的问题。
发明内容
本发明的目的提供一种基于多传感器融合定位与建图的方法及系统，可有效提升在复杂场景下实时定位与建图的鲁棒性与精度。
为实现上述目的，本发明提供了如下方案：
基于多传感器融合定位与建图的方法，包括：
采集目标区域的激光雷达，IMU/轮速里程计和GPS数据；
将IMU和轮速里程计的数据进行预积分处理，构建IMU/轮速里程计的预积分模型，得到IMU/轮速里程计的预积分处理后的数据；
利用当前帧起止时刻之间的IMU/轮速里程计的预积分数据为激光雷达点云去畸变提供初值，得到去畸变激光雷达数据；
对上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，构建激光雷达里程计，并根据激光里程计约束关系得到激光雷达里程计因子；
根据上述激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息；
将上述的位姿信息进行关键帧处理，并进行滑动窗口优化，得到局部的位姿点云数据；
将上述得到的数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子；
引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS因子；
以因子图为后端，采用增量平滑式更新(iSAM2)，构建紧耦合的激光雷达惯性里程计，将激光雷达里程计因子，IMU/轮速里程计预积分因子，回环检测因子，GPS因子一同加入因子图中进行优化，实时估计载体的位姿信息，完成所述区域的定位与建图；
可选地，所述根据上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，构建激光雷达里程计，并根据激光里程计约束关系得到激光雷达里程计因子，具体包括：
当新的激光雷达点云到达时，我们首先对点云进行特征点提取，获得五种类别的特征点集以及对应的稀疏特征点集合。通过构造一幅局部地图来执行帧到地图的匹配。当前帧的稀疏特征点集合作为多度量线性最小二乘ICP的源点云，局部地图的特征点集合作为多度量最小二乘ICP的目标点云，局部地图是通过近邻关键帧集合构建的。当某一关键帧的特征点集添加到局部地图中时，那些远离局部地图中同类点集的特征点被去除，将当前关键帧与局部地图的姿态改变量，使用激光雷达与IMU的外参变换到IMU系下，作为激光里程计的帧间约束得到激光雷达里程计因子并添加到因子图中。
可选地，所述根据上述激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息，具体包括：
利用所述的激光雷达里程计因子与预积分约束因子构建激光惯性里程计的误差模型；
将采集的激光雷达数据和所述IMU/轮式里程计的数据输入激光惯性里程计中，并将由所述激光惯性里程计的误差模型计算的最小误差时对应的位姿关系，确定为位姿信息。
一种基于多传感器融合定位与建图系统，包括：
数据采集模块，用于获取采集的激光雷达数据、IMU/轮速里程计测量数据和GPS数据；
数据处理模块，将采集到的IMU/轮速里程计数据预积分，得到预积分处理后的数据，与激光雷达数据一起进行点云去畸变处理，得到去畸变激光雷达数据；
里程计获取模块，对上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，得到激光雷达里程计，根据激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计；
局部地图生成模块，将多个关键帧生成局部点云地图，进行滑动窗口优化，得到优化后局部的位姿点云数据；
回环检测模块，将优化后局部的位姿点云数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子；
定位与建图模块，采用因子图优化的方式，在因子图上制定激光雷达惯性里程计，将上述的不同的传感器的相对测量值作为因子合并到系统中，进行全局优化，得到全局优化位姿，完成所属区域的定位与建图。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明公开了一种基于多传感器融合的定位与建图方法及紧耦合系统，在获取激光雷达数据、IMU惯性测量数据、轮式里程计数据和GPS数据后，通过构建紧耦合的激光惯性里程计，实现将不同传感器的相对测量数据和绝对测量数据进行融合，再对上述的融合结果进行滑动窗口优化、回环检测和全局位姿优化，进而完成地图的定位与构建。其中，上述构建的紧耦合的激光惯性里程计极大提高在复杂环境的适应能力、数据运算的稳定性以及系统的鲁棒性；并且通过激光雷达、IMU、轮式里程计以及GPS数据的融合提升了回环检测的精度和速度，并且极大提高了实时定位的精度，解决了原有方案不能在复杂场景下实时定位与建图的鲁棒性与精度的问题。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的基于多传感器融合的定位与建图方法的流程示意图；
图2为本发明实施例提供的基于多传感器融合的定位与建图方法的逻辑示意图；
图3为本发明实施例提供的基于多传感器融合的定位与建图紧耦合系统的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于多传感器融合的定位与建图方法及系统，通过将多传感器数据进行融合处理，有效提升在在复杂场景下实时定位与建图的鲁棒性与精度的能力。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
如图1-图2所示，本发明实施例提供的基于多传感器融合的定位与建图方法包括：
步骤一：采集目标区域的激光雷达，IMU/轮速里程计和GPS数据；
步骤二：将IMU和轮速里程计的数据进行预积分处理，构建IMU/轮速里程计的预积分模型，得到IMU/轮速里程计的预积分处理后的数据；
步骤三：利用当前帧起止时刻之间的IMU/轮速里程计的预积分数据为激光雷达点云去畸变提供初值，得到去畸变激光雷达数据；
步骤四：对上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，构建激光雷达里程计，并根据激光里程计约束关系得到激光雷达里程计因子；
步骤五：根据上述激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息；
步骤六：将上述的位姿信息进行关键帧处理，并进行滑动窗口优化，得到局部的位姿点云数据；
步骤七：将上述得到的数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子；
步骤八：引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS因子；
步骤九：以因子图为后端，采用增量平滑式更新(iSAM2)，构建紧耦合的激光雷达惯性里程计，将激光雷达里程计因子，IMU/轮速里程计预积分因子，回环检测因子，GPS因子一同加入因子图中进行优化，实时估计载体的位姿信息，完成所述区域的定位与建图；
其中在步骤一中，将激光雷达置于载体外表面，且将IMU惯性测量传感器置于载体外表面或内部，轮式里程计安装在车轮上，GPS置于载体外表面，通过所述载体上的各传感器分别获取目标区域的激光雷达数据、IMU惯性测量数据、轮式里程计测量数据和GPS测量数据。所述激光雷达数据是指安置在运动载体上的多线程激光雷达通过运动采集到的3D点云数据，所述IMU惯性测量数据和轮式里程计数据是根据实时动态变化的数据，所述GPS数据也是根据载体实时动态变化的数据。
在上述第二步，步骤二中，且由于IMU惯性测量传感器和轮速里程计的频率相比于激光雷达的频率高很多，为了使得IMU惯性测量数据对点云图像关键帧形成约束，需要对IMU与轮速里程计进行联合预积分处理，如下列公式所示。
其中IMU系记为b，轮速里程计系记为o。导航坐标系采用东北天坐标系，记为w。IMU/编码器状态传播方程被建模为如下形式：
Figure BDA0003811074300000061
其中，
Figure BDA0003811074300000062
式(1)中，
Figure BDA0003811074300000063
和
Figure BDA0003811074300000064
分别为tk和tk+1时刻机体在导航坐标系下的位置向量，
Figure BDA0003811074300000065
和
Figure BDA0003811074300000066
分别为tk和tk+1时刻机体在导航坐标系下的速度向量，
Figure BDA0003811074300000067
和
Figure BDA0003811074300000068
为tk和tk+1时刻机体系相对于导航系的四元数。Δtk为时间间隔，
Figure BDA0003811074300000069
为t时刻从机体系到导航系的旋转矩阵,gw为导航系下的重力向量。at和wt分别为t时刻加速度计与陀螺仪输出，
Figure BDA00038110743000000610
和bwt为t时刻加速度计与陀螺的零偏，na和nw分别为加速度计和陀螺的噪声。
Figure BDA00038110743000000611
和
Figure BDA00038110743000000612
分别为tk和tk+1时刻轮速里程计在机体系下的位置向量，
Figure BDA00038110743000000613
为t时刻轮速里程计系到机体系的旋转矩阵，
Figure BDA00038110743000000614
为t时刻轮速里程计的标度因数，ns为轮速里程计的噪声。将式(1)中的状态传播模型投影到tk时刻的机体系bk下，可得：
Figure BDA00038110743000000615
其中，
Figure BDA0003811074300000071
式(3)中，
Figure BDA0003811074300000072
为tk+1时刻的机体相对于bk系的预积分位置变化量，
Figure BDA0003811074300000073
为tk+1时刻的机体相对于bk系的预积分速度变化量，
Figure BDA0003811074300000074
为tk+1时刻的机体相对于bk的预积分旋转变化量，
Figure BDA0003811074300000075
为tk+1时刻的机体相对于bk系的预积分位置变化量。
式(4)的离散形式可以写为：
Figure BDA0003811074300000076
其中在步骤三得到去畸变激光雷达数据中，点云畸变去除基于匀速运动假设，对应的公式(6)为：
Figure BDA0003811074300000077
其中
Figure BDA0003811074300000078
表示当前帧最后一个扫描点到第一个扫描点的位姿。使用高频的IMU来预测
Figure BDA0003811074300000079
Figure BDA00038110743000000710
中的旋转部分是通过欧拉积分确定的，即将当前帧采样间隔内的IMU测量缓存，处理好时间戳关系后进行欧拉积分；平移部分则是来自IMU和轮速里程计的预积分模块预测的高频位姿。在获取上述数据后，记录数据在采集周期内IMU惯性测量传感器和轮速里程计的运动轨迹；IMU缓存时保证缓存队列首帧IMU数据的时间戳小于等于点云起始时刻，缓存队列尾帧IMU数据的时间戳大于等于点云的终止时刻，这样可以利用相邻两帧IMU数据来线性插值激光点云起点和终点时刻的IMU测量，如此可以精确积分出激光雷达采样间隔内的姿态变化量。利用所述时间戳和IMU惯性测量传感器的运动轨迹得到在采集各个所述点云时激光雷达的位置和姿态；通过外参将得到的每帧点云的坐标值转换至该帧点云中第一个3D点采集时刻的激光雷达坐标系下，即实现了点云去畸变。
在步骤四中，对上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，构建激光雷达里程计，并根据激光里程计约束关系得到激光雷达里程计因子，具体分为以下四个步骤，如下列公式所示。
第一步，点云地面分割，为了应对移动机器人可以在室内外更多的场景，采取双阈值地面滤波的方式来提取地面点云，将预处理后的点云投影到上一帧的地面点拟合的平面，并按照3×3大小的栅格进行划分，一个栅格内的点可能都是地面点，也可能皆为非地面点，还可能既有地面点又包含非地面点。对于栅格内点数小于10的栅格过滤不用，对其他的每个栅格ci内激光点的最小高度记为
Figure BDA0003811074300000081
与相邻的3×3的栅格ci内激光点的最小高度记为
Figure BDA0003811074300000082
我们使用两个阈值δh1和δh2来粗略地判定栅格分别记录每个栅格ci内每个激光点pk是地面点还是非地面点，具体判别公式如下。
Figure BDA0003811074300000083
接下来对地面点进行提炼，对每个栅格中粗略判定的地面点，使用RANSAC算法去拟合平面方程，公式如下。
Ax+By+Cz+D＝0 (8)
A,B,C为拟合平面的法向量，x,y,z为拟合平面的的坐标。内点作为最终的地面点，拟合平面的法向量(A,B,C)作为地面点的方向向量，外点则改正为非地面点。
第二步，点云特征提取，点云划分为地面点、竖直线点集、水平线点集、外立面点集、单点点集五种类型，对非地面点云中的每个点首先进行K-R近邻搜索，即在距离当前激光点以R为半径的球内选取距离当前激光点最近的K个激光点，公式如下。
Figure BDA0003811074300000084
Figure BDA0003811074300000085
是局部表面的中心，对矩阵进行特征值分解，可以分解成v,m,n三个特征向量，v,n代表局部表面的主线方向和法线方向。
Figure BDA0003811074300000086
其中，δ1D局部线性度、δ2D局部平面度以及δC局部曲率，根据局部线性度、局部平面度、局部曲率以及局部表面的主线方向、法线方向对非地面点云进行特征点提取。
点云配准采用多度量线性最小二乘ICP算法，算法具体如下所示。
1)对目标点云中的地面点集、外立面点集、竖直线点集、水平线点集以及单点点集分别建立Kd-Tree。对于每一次迭代，对五种类别的激光点通过Kd-Tree进行近邻搜索来建立源点云和目标点云中最近点的对应关系。之后对平面点集(地面点集和外立面点集)和线性点集(竖直线点集和水平线点集)这四类几何特征点分别进行法向量n和主线向量v的方向一致性检测，未通过检测(方向矢量夹角大于设定阈值)的激光匹配点对则从最近点对应关系中移除。
2)对平面点集建立点-面约束，对线性点集建立点-线约束，对单点点集建立点-点约束。假定是pi和qi源点云与目标点云中对应的最近匹配点对，源点云经过特定的旋转R与平移变换t后之间的误差ri有如下的定义：
ri＝qi-p′i＝qi-(Rpi+t) (11)
计算点-面、点-线、点-点距离，公式如下。
Figure BDA0003811074300000091
Figure BDA0003811074300000092
为点-面距离，
Figure BDA0003811074300000093
为点-线距离，
Figure BDA0003811074300000094
为点-点距离。得到源点云到目标点云的最优姿态变换矩阵{R*,t*}，公式如下。
Figure BDA0003811074300000095
wi是每对匹配点的权重。
在小角度运动的假设下，旋转矩阵可被线性化为:
Figure BDA0003811074300000096
其中α，β和γ分别是x-y-z旋转次序的欧拉角，将优化变量{R,t}参数化为列向量
ξ＝[α β γ tx ty tz]T，转换为加权线性最小二乘，公式如下所示。
Figure BDA0003811074300000097
其中P＝diag(w1,...,wn)是权重矩阵，A为约束矩阵，b为观测向量。
对于平面点集(地面点集和外立面点集)，其建立的点-面距离约束可应用公式线性展开
Figure BDA0003811074300000101
同理，线性点集(竖直线点集和水平线点集)点-线约束应用公式线性展开为；
Figure BDA0003811074300000102
单点点集建立的点-点距离约束应用公式线性展开后可以获得：
Figure BDA0003811074300000103
在多度量线性最小二乘ICP收敛后，点云配准的后验标准差
Figure BDA0003811074300000104
以及信息矩阵η为：
Figure BDA0003811074300000105
第三步，关键帧选择，使用一种简单而有效的启发式方法：与之前的状态相比，当载体姿态变化超过用户定义的阈值时，我们选择激光雷达帧作为关键帧。在因子图中，新保存的关键帧与一个新的载体状态节点关联。两个关键帧之间的激光雷达帧被丢弃。增加一个新的关键帧的位置和旋转变化阈值被选择为1m和10°。
第四步，激光雷达里程计因子生成步骤如下，根据当前激光帧的初始位姿，我们提取i个几何距离最近的关键帧，这个集合称之为近邻关键帧集合{Fn...Fm}，根据近邻关键帧集合{Fn...Fm}对应的关键帧位姿{Tn...Tm}帧点云投影到Fm帧下，投影过后的近邻关键帧点云合并成一张局部地图Mi。当某一关键帧的特征点集添加到局部地图中时，那些远离局部地图中同类点集的特征点被去除，将当前关键帧与局部地图的姿态改变量
Figure BDA0003811074300000106
使用激光雷达与IMU的外参变换到IMU系下，作为激光里程计的帧间约束添加到因子图中。
Figure BDA0003811074300000107
在步骤五中，根据上述激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息。
在步骤六中，将上述的位姿信息进行关键帧处理，并进行滑动窗口优化，得到局部的位姿点云数据，采用了SLAM中常用的关键帧策略，在因子图中，两个关键帧之间的激光、图像帧将被丢弃，新保存的关键帧与姿态节点相关联。以关键帧的方式处理激光雷达数据，可以实现点云地图密度与计算机内存之间的平衡，并有助于保持一个相对稀疏的因子图，这更有利于实现因子图的实时更新。
在步骤七中，将上述得到的数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子，具体包括以下四个步骤。
第一步，首先将点云转换为鸟瞰图，对每个面元内的点采用一些特征提取方法，如高度、距离、反射、环等。使用八位二进制代码对同一个面元内的所有点进行编码。可以获得8位二进制编码。
第二步，雷达虹膜算法将激光雷达的鸟瞰图扩展为图像条，图像的像素强度为每个面元的8位二进制数所转换的十进制数。下面通过傅立叶变换使得描述子具有平移不变性。公式如下所示。
I1，I2为两个雷达虹膜图像，两个图像之间仅差(δx,δy)，使得I1(x,y)＝I2(x-δx,y-δy)，则I1，I2的傅里叶变换如下列公式所示。
Figure BDA0003811074300000111
归一化交叉功率谱由下式给出。
Figure BDA0003811074300000112
其中*表示复共轭。
第三步，使用LoG-Gabor滤波器从图像中深入提取特征:LoG-Gabor滤波器可用于将图像区域中的数据分解为以不同分辨率出现的分量，它的优势在于允许频率数据局部化，允许在相同位置和分辨率进行特征匹配。为了确保实时性，仅使用1D LoG Gabor滤波器。公式如下。
Figure BDA0003811074300000113
其中f0和σ是滤波器的参数。f0将给出滤波器的中心频率。σ影响滤波器的带宽。在频率参数变化时保持相同的形状很有用。为此，比率σ/f0应保持不变。
第四步，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子。
在步骤八中，引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS因子，加入到因子图中。
在步骤九中，以因子图为后端，采用增量平滑式更新(iSAM2)，构建紧耦合的激光雷达惯性里程计，将激光雷达里程计因子，IMU/轮速里程计预积分因子，回环检测因子，GPS因子一同加入因子图中进行优化，实时估计载体的位姿信息，完成所述区域的定位与建图。
在本实施例中，将IMU惯性测量传感器的坐标系为载体坐标系，将激光雷达和轮式里程计进行标定，使其他两个坐标系的数据转换至所述IMU惯性测量传感器的坐标系下，再进行数据融合计算。
本实施例针对当前激光SLAM及多传感器融合的研究现状，采用基于3D激光雷达、轮式里程计，IMU惯性测量传感器和GPS传感器四类传感器融合的方式进行SLAM方案的研究。本实施例设计以激光雷达和IMU为主的里程计，这种紧耦合的激光雷达惯性里程计可以在复杂环境下高精度定位，提高本SLAM方案对环境的适应性和鲁棒性。为了获得全局一致的高精度地图，设计SLAM方案利用因子图优化的方式对四种数据进行融合。将IMU惯性测量数据和轮式里程计的测量数据的联合预积分项结合激光雷达去畸变数据组成激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息。本实施例中的SLAM方案满足基于激光雷达虹膜的回环检测进行闭环优化，可有效提升回环检测的准确度与实时性，大大增加系统定位的鲁棒性和精度，能够适用于移动机器人室内外无缝定位与建图系统。
如图3所示，本发明实施例提供的基于多传感器融合的定位与建图紧耦合系统，包括：数据采集模块、数据处理模块、里程计获取模块、局部地图生成模块、回环检测模块、定位与建图模块。
具体地，数据采集模块用于获取目标区域的激光雷达数据、IMU惯性测量数据和轮式里程计测量数据，数据处理模块用于对所述激光雷达数据进行去畸变处理，得到去畸变激光雷达数据，对所述IMU惯性测量数据和轮式里程计测量数据进行联合预积分处理，得到预积分处理后的数据。里程计获取模块采用多度量线性最小二乘ICP算法，得到激光雷达里程计，根据激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述激光雷达数据和所述惯性测量数据通过所述激光惯性里程计进行状态估计，得到位姿信息。局部地图生成模块用于对上述多个位姿关键帧进行滑动窗口优化，得到局部优化位姿数据。回环检测模块，将优化后局部的位姿点云数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子，定位与建图模块，采用因子图优化的方式，在因子图上制定激光雷达惯性里程计，将上述的不同的传感器的相对测量值作为因子合并到系统中，进行全局优化，得到全局优化位姿，完成所属区域的定位与建图。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (4)

1.一种基于多传感器融合的定位与建图方法，其特征在于，该方法包括如下步骤：
步骤一：采集目标区域的激光雷达，IMU/轮速里程计和GPS数据；
步骤二：将IMU和轮速里程计的数据进行预积分处理，构建IMU/轮速里程计的预积分模型，得到IMU/轮速里程计的预积分处理后的数据；
步骤三：利用当前帧起止时刻之间的IMU/轮速里程计的预积分数据为激光雷达点云去畸变提供初值，得到去畸变激光雷达数据；
步骤四：对上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，构建激光雷达里程计，并根据激光里程计约束关系得到激光雷达里程计因子；
步骤五：根据上述激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息；
步骤六：将上述的位姿信息进行关键帧处理，并进行滑动窗口优化，得到局部的位姿点云数据；
步骤七：将上述得到的数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子；
步骤八：引入GPS传感器的绝对测量值消除传感器的漂移，根据GPS绝对测量值的约束关系得到GPS因子；
步骤九：以因子图为后端，采用增量平滑式更新(iSAM2)，构建紧耦合的激光雷达惯性里程计，将激光雷达里程计因子，IMU/轮速里程计预积分因子，回环检测因子，GPS因子一同加入因子图中进行优化，实时估计载体的位姿信息，完成所述区域的定位与建图。
2.根据权利要求1所述的基于多传感器融合的定位与建图方法，其特征在于所述根据上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，构建激光雷达里程计，并根据激光里程计约束关系得到激光雷达里程计因子，具体包括：
当新的激光雷达点云到达时，我们首先对点云进行特征点提取，获得地面点云、竖直线点云、水平线点云、外立面点云、单点点云五种类别的特征点集以及对应的稀疏特征点集合。通过构造一幅局部地图来执行帧到地图的匹配。当前帧的稀疏特征点集合作为多度量线性最小二乘ICP的源点云，局部地图的特征点集合作为多度量最小二乘ICP的目标点云，局部地图是通过近邻关键帧集合构建的。当某一关键帧的特征点集添加到局部地图中时，那些远离局部地图中同类点集的特征点被去除，将当前关键帧与局部地图的姿态改变量，使用激光雷达与IMU的外参变换到IMU系下，作为激光里程计的帧间约束得到激光雷达里程计因子并添加到因子图中。
3.根据权利要求1所述的基于多传感器融合的定位与建图方法，其特征在于，所述根据上述激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计，并将所述数据经过激光惯性里程计进行状态估计，得到位姿信息，具体包括：
利用所述的激光雷达里程计因子与预积分约束因子构建激光惯性里程计的误差模型；
将采集的激光雷达数据和所述IMU/轮式里程计的数据输入激光惯性里程计中，并将由所述激光惯性里程计的误差模型计算的最小误差时对应的位姿关系，确定为位姿信息。
4.一种基于多传感器融合定位与建图紧耦合系统，包括：
数据采集模块，用于获取采集的激光雷达数据、IMU/轮速里程计测量数据和GPS数据；
数据处理模块，将采集到的IMU/轮速里程计数据预积分，得到预积分处理后的数据，与激光雷达数据一起进行点云去畸变处理，得到去畸变激光雷达数据；
里程计获取模块，对上述去畸变激光雷达数据采用多度量线性最小二乘ICP算法，得到激光雷达里程计，根据激光雷达点云去畸变数据和上述IMU/轮速里程计预积分约束因子构建激光惯性里程计；
局部地图生成模块，将多个关键帧生成局部点云地图，进行滑动窗口优化，得到优化后局部的位姿点云数据；
回环检测模块，将优化后局部的位姿点云数据进行回环检测，将位姿点云数据里的雷达点云经过Gabor过滤和阈值操作生成雷达虹膜二进制特征图，保存之前所有关键帧获取的二进制特征图，并生成历史数据库，将当前关键帧和历史关键帧的二进制特征图之间的距离用汉明距离计算，若获得的汉明距离小于阈值，则视为闭环，根据闭环约束关系得到回环检测因子；
定位与建图模块，采用因子图优化的方式，在因子图上制定激光雷达惯性里程计，将上述的不同的传感器的相对测量值作为因子合并到系统中，进行全局优化，得到全局优化位姿，完成所属区域的定位与建图。
CN202211011477.0A 2022-08-23 2022-08-23 基于多传感器融合的定位与建图方法及紧耦合系统 Withdrawn CN115479598A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211011477.0A CN115479598A (zh) 2022-08-23 2022-08-23 基于多传感器融合的定位与建图方法及紧耦合系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211011477.0A CN115479598A (zh) 2022-08-23 2022-08-23 基于多传感器融合的定位与建图方法及紧耦合系统

Publications (1)

Publication Number Publication Date
CN115479598A true CN115479598A (zh) 2022-12-16

Family

ID=84422899

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211011477.0A Withdrawn CN115479598A (zh) 2022-08-23 2022-08-23 基于多传感器融合的定位与建图方法及紧耦合系统

Country Status (1)

Country Link
CN (1) CN115479598A (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115797425A (zh) * 2023-01-19 2023-03-14 中国科学技术大学 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法
CN116222543A (zh) * 2023-04-26 2023-06-06 齐鲁工业大学(山东省科学院) 用于机器人环境感知的多传感器融合地图构建方法及系统
CN116772828A (zh) * 2023-08-24 2023-09-19 长春工业大学 一种基于图优化的多传感器融合定位与建图方法
CN117475092A (zh) * 2023-12-27 2024-01-30 安徽蔚来智驾科技有限公司 位姿优化方法、设备、智能设备和介质

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115797425A (zh) * 2023-01-19 2023-03-14 中国科学技术大学 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法
CN116222543A (zh) * 2023-04-26 2023-06-06 齐鲁工业大学(山东省科学院) 用于机器人环境感知的多传感器融合地图构建方法及系统
CN116772828A (zh) * 2023-08-24 2023-09-19 长春工业大学 一种基于图优化的多传感器融合定位与建图方法
CN116772828B (zh) * 2023-08-24 2023-12-19 长春工业大学 一种基于图优化的多传感器融合定位与建图方法
CN117475092A (zh) * 2023-12-27 2024-01-30 安徽蔚来智驾科技有限公司 位姿优化方法、设备、智能设备和介质
CN117475092B (zh) * 2023-12-27 2024-03-19 安徽蔚来智驾科技有限公司 位姿优化方法、设备、智能设备和介质

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN115479598A (zh) 基于多传感器融合的定位与建图方法及紧耦合系统
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN113066105A (zh) 激光雷达和惯性测量单元融合的定位与建图方法及系统
JP2009193240A (ja) 移動ロボット及び環境地図の生成方法
CN114018248B (zh) 一种融合码盘和激光雷达的里程计方法与建图方法
CN114526745A (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及系统
Fang et al. A real-time 3d perception and reconstruction system based on a 2d laser scanner
CN113674412B (zh) 基于位姿融合优化的室内地图构建方法、系统及存储介质
CN112254729A (zh) 一种基于多传感器融合的移动机器人定位方法
CN112833892B (zh) 一种基于轨迹对齐的语义建图方法
JP2023021098A (ja) マップ構築方法、装置及び記憶媒体
Qian et al. Robust visual-lidar simultaneous localization and mapping system for UAV
CN115272596A (zh) 一种面向单调无纹理大场景的多传感器融合slam方法
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
Wen et al. TM 3 Loc: Tightly-coupled monocular map matching for high precision vehicle localization
CN116429116A (zh) 一种机器人定位方法及设备
CN113639722B (zh) 连续激光扫描配准辅助惯性定位定姿方法
Karam et al. Integrating a low-cost mems imu into a laser-based slam for indoor mobile mapping
CN114674311A (zh) 一种室内定位与建图方法及系统
Khoshelham et al. Vehicle positioning in the absence of GNSS signals: Potential of visual-inertial odometry
CN112580683A (zh) 一种基于互相关的多传感器数据时间对齐系统及其方法
CN115355904A (zh) 一种针对地面移动机器人的Lidar-IMU融合的slam方法
CN115930948A (zh) 一种果园机器人融合定位方法
CN115984463A (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
WW01 Invention patent application withdrawn after publication

Application publication date: 20221216

WW01 Invention patent application withdrawn after publication