CN115963508A - 激光雷达和imu的紧耦合slam方法及*** - Google Patents

Abstract

本发明公开一种激光雷达和IMU的紧耦合SLAM方法及***，属于矿山智能定位与导航技术领域，所述方法包括基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。本发明提高整个***在非结构化环境中的精度。

Description

激光雷达和IMU的紧耦合SLAM方法及***

技术领域

本发明涉及矿山智能定位与导航技术领域，具体涉及一种激光雷达和IMU的紧耦合SLAM方法及***。

背景技术

煤矿巷道、采掘工作面等作业区域具有典型的非结构化环境特征，且全球定位***(Global Positioning System，GPS)技术无法直接应用于井下，进而导致煤矿开采时矿难频发，急需机械化换人、自动化减人和提高矿山智能化水平。通过构建适用于煤矿机器人的自主定位***方案，可解决井下机器人精准定位、姿态感知等问题。如何快速突破惯导、激光等多信息融合的井下机器人精准感知与定位技术，是实现井下机器人局部自主的关键，故融合激光雷达和惯性测量单元(Inertial measurement unit，IMU)的同步定位与地图构建(Simultaneous Localization And Mapping，SLAM)方法非常重要。

相关技术中，申请公布号为CN113379910A的中国发明申请，公开了一种基于SLAM的移动机器人矿山场景重建方法及***，获取同步标定过的通过移动机器人测得的激光点云数据和视觉点云数据；对获取的激光点云数据和视觉点云数据进行融合；对融合后的点云数据进行点云运动畸变去除处理和点云滤波处理；结合处理后的点云数据，采用基于图优化的多约束因子图算法，将IMU预积分数据、点云关键帧数据以及GNSS数据加入约束子图，进行回环检测后得到重建后的三维地图；本公开能够有效实现矿山场景下的三维重建，并且最终获得带有颜色的点云地图，提高了矿山场景重建的精度。

但该重建方法将激光雷达的原始点云与IMU数据进行一同运算，提高整个***在非结构化环境中的精度，但会造成计算量的增加，无法兼顾***的准确率和实时性。

发明内容

本发明所要解决的技术问题在于如何平衡***的精度与计算量，保证***的准确率和实时性。

本发明通过以下技术手段实现解决上述技术问题的：

一方面，本发明提出了一种激光雷达和IMU的紧耦合SLAM方法，所述方法包括：

基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；

对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；

从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；

对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。

本发明使用激光雷达和IMU进行紧耦合的方法，将激光雷达的原始点云与IMU数据进行一同运算，提高整个***在非结构化环境中的精度；并且将激光里程计的数据作为优化因子，添加至因子图中，由于激光里程计数据可以去除畸变，进一步提高精度；同时针对激光雷达点云数据与IMU数据同时运算造成计算量增加的问题，本发明进行关键帧的匹配，使得精度与计算量之间取得平衡，保证了***的准确率和实时性。而且，本发明通过设置IMU里程计和激光里程计，并通过激光雷达和IMU进行紧耦合的方，无需依赖于全球导航卫星***(GlobalNavigation Satellite System，GNSS)，可在无卫星信号下使用，更具有通用性。

进一步地，所述基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据，包括：

接收所述激光里程计的数据，并保存所述激光里程计的当前时间戳；

计算IMU队列中起始时刻和终止时刻所对应IMU里程计之间的相对位姿变换，所述起始时刻为所述激光里程计的当前时间戳；

将IMU原始数据加入到IMU队列，并利用所述激光里程计的数据和所述相对位姿变换作为积分的起始值，对所述IMU队列中两帧之间的数据进行积分计算，得到所述IMU积分数据。

进一步地，所述从畸变校正之后的点云数据中提取点云特征信息，包括：

对于所述畸变校正之后的点云数据中的每一点云帧，取当前点云帧的前五个点和后五个点，并计算各点到当前点云帧中间点的距离差之和的平方，作为当前点云帧的曲率；

将每一点云帧分成若干段，并从每段提取设定数量的边缘点和平面点，以平滑度为指标对每段内的边缘点和平面点分别进行排序，得到第一排序结果和第二排序结果；

基于所述第一排序结果，按照平滑度从低到高，提取边缘点特征；

基于所述第二排序结果，按照平滑度从高到低，提取平面点特征；

将具有所述边缘点特征和所述平面点特征的点云数据进行封装，得到所述点云特征信息。

进一步地，所述激光里程计的数据的确定过程包括：

基于激光雷达的点云帧，确定关键帧，所述激光雷达的点云帧由平面特征和边缘特征组成；

通过滑动窗口方法构建包括一定数量扫描点云的点云地图，将关键帧注册到固定大小的先验子关键帧集合中，并采用变换关系将子关键帧集合转换到世界坐标系中，得到由子关键帧合并形成的地图；

通过扫描匹配将得到的激光雷达的当前关键帧加入所述地图；

计算所述边缘特征与对应的边缘块的第一距离，以及计算所述平面特征与对应的平面块之间的第二距离；

基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系，得到机器人相邻时刻状态节点的位姿变换关系作为所述激光里程计的数据。

进一步地，所述基于激光雷达的点云帧，确定关键帧，包括：

定义激光雷达在t时刻扫描点云中边缘特征

和平面特征

基于所述边缘特征和所述平面特征构成激光雷达点云帧

在满足机器人当前状态节点与上一时刻状态节点的差值超过所定义的阈值时，将当前状态节点对应的激光雷达点云帧确定为所述关键帧；

关联所述当前状态节点和所述关键帧。

进一步地，所述基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系，得到机器人相邻时刻状态节点的位姿变换关系作为所述激光里程计的数据，包括：

基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系为：

式中：d_ek为第一距离，d_pk为第二距离，

为机器人坐标系中的关键帧转换到世界坐标系中的边缘特征和平面特征，

为第k+1时刻所取边缘点，

为第k+1时刻所取平面点。

基于所述变换矩阵，得到状态节点X_k和X_k+1之间的变换关系ΔT_k,k+1：

式中：T_k为k时刻的旋转和平移，T_k+1为k+1时刻的旋转和平移。

进一步地，所述方法还包括：

当有新的状态节点添加到所述因子图中时，在欧几里得空间搜索所述因子图中最接近新的所述状态节点的先验状态节点；

将最近接的所述先验状态节点和下一时刻的状态节点的位姿变换关系作为闭环因子加入到所述因子图中。

进一步地，所述从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子，包括：

通过所述历史关键帧构造Kdtree，并且搜索匹配指定搜索半径的关键帧，将距离其最近且时间间隔满足指定范围的关键帧作为其匹配帧；

通过当前历史关键帧固有的边缘点、平面点构造当前历史关键帧的第一特征集，使用所述匹配帧前后相邻的共m个关键帧特征点构造所述匹配帧的第二特征集；

基于指定的迭代次数和最小迭代阈值，对第一特征点集和第二特征点集进行匹配，根据匹配的结果矫正当前历史关键帧的位姿，并保存当前历史关键帧与匹配帧之间的对应关系，作为闭环因子。

进一步地，所述方法还包括：

将所述IMU积分数据、所述畸变校正之后的点云数据、所述点云特征信息以及所述因子图的优化结果通过Rviz展示。

此外，本发明还提出了一种激光雷达和IMU的紧耦合SLAM***，所述***包括：

预积分模块，用于基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；

校正提取模块，用于对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；

闭环因子确定模块，用于从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；

优化模块，用于对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。

本发明的优点在于：

(1)本发明使用激光雷达和IMU进行紧耦合的方法，将激光雷达的原始点云与IMU数据进行一同运算，提高整个***在非结构化环境中的精度；并且将激光里程计的数据作为优化因子，添加至因子图中，由于激光里程计数据可以去除畸变，进一步提高精度；同时针对激光雷达点云数据与IMU数据同时运算造成计算量增加的问题，本发明进行关键帧的匹配，使得精度与计算量之间取得平衡，保证了***的准确率和实时性。而且，本发明通过设置IMU里程计和激光里程计，并通过激光雷达和IMU进行紧耦合的方，无需依赖于GNSS数据，可在无卫星信号下使用，更具有通用性。

(2)本发明中利用机器人的节点状态来定义关键帧，由于状态参数是多维的向量形式，相比较于距离这一单一数据，对环境的感知更加明显，因此本方中对关键帧的定义更加准确，进一步提高了***的精确性。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

图1是本发明第一实施中激光雷达和IMU的紧耦合SLAM方法的流程示意图；

图2是本发明第一实施例中激光雷达和IMU的紧耦合SLAM的原理框图；

图3是本发明第一实施例中IMU预积分数据传递示意图；

图4是本发明第一实施例中激光畸变矫正数据传递示意图；

图5是本发明第一实施例中点云特征提取数据传递示意图；

图6是本发明第一实施例中因子图优化数据传递示意图；

图7是本发明第二实施例中激光雷达和IMU的紧耦合SLAM***的结构示意图；

图8是本发明中激光雷达和IMU的紧耦合SLAM方法实验结果图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

如图1至图2所示，本发明第一实施例提出了一种激光雷达和IMU的紧耦合SLAM方法，所述方法包括以下步骤：

S10、基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；

S20、对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；

S30、从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；

S40、对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。

考虑到矿山井巷非结构化环境特征且GPS技术无法直接运用的因素，所以在一定情况下，激光雷达无法采集到足够的特征点云，但是IMU可以不依赖于外部信息，独立于其它传感器存在，故IMU可以在无足够的特征点云时进行预积分，弥补没有足够特征点云的缺陷，进而对激光雷达进行点云的特征提取，从而使整个***保持稳定。

本实施例使用激光雷达和IMU进行紧耦合的方法，将激光雷达的原始点云与IMU数据进行一同运算，提高整个***在非结构化环境中的精度；并且将激光里程计的数据作为优化因子，添加至因子图中，由于激光里程计数据可以去除畸变，进一步提高精度，但需要增加运算代价。本实施例针对激光雷达点云数据与IMU数据同时运算造成计算量增加的问题，进行关键帧的匹配，使得精度与计算量之间取得平衡，保证了***的准确率和实时性。

进一步地而言，本实施例通过设置IMU里程计和激光里程计，并通过激光雷达和IMU进行紧耦合的方，无需依赖于GNSS数据，可在无卫星信号下使用，更具有通用性。

在一实施例中，如图3所示，所述步骤S10，包括以下步骤：

S11、接收所述激光里程计的数据，并保存所述激光里程计的当前时间戳；

具体地，本实施例将接收的激光里程数据的数据统一为指定格式并将激光里程计的当前时间戳进行保存。

S12、计算IMU队列中起始时刻和终止时刻所对应IMU里程计之间的相对位姿变换，所述起始时刻为所述激光里程计的当前时间戳；

具体地，本实施例基于激光里程计的当前时间戳，将IMU队列中时间戳早于激光里程计当前时间戳的IMU增量数据移除；然后计算IMU队列中起始和终止时刻对应的IMU里程计之间的相对位姿变换；再将先前接收到激光里程计的数据和计算得出的相对位姿变换得到的数据进行结合并发布。

S13、将IMU原始数据加入到IMU队列，并利用所述激光里程计的数据和所述相对位姿变换作为积分的起始值，对所述IMU队列中两帧之间的数据进行积分计算，得到所述IMU积分数据。

进一步地，IMU积分数据即为观测数据，包括IMU的角速度和加速度，公式定义如下：

角速度：

加速度：

则有：t时刻下的IMU观测源数据为

和

并且

和

都会受到白噪声n_t和IMU零偏b_t的影响；矩阵

是世界坐标系到机器人坐标系的变换矩阵；重力常数向量g属于世界坐标系W；w_t为t时刻的角速度，

为t时刻IMU零偏角速度的噪声，

为t时刻IMU零偏角速度的白噪声，a_t为t时刻的加速度，

为t时刻IMU零偏角速度的噪声，

为t时刻IMU零偏角速度的白噪声。

进一步地，本实施例还包括：

每当接收到100帧的激光里程计的数据，优化器就进行一次重置。

具体地，初始化与重置不同，进行重置时，当前的速度与位姿和之前优化得到的速度与位姿保持一致，噪声模型采用之前优化后模型的边缘分布；添加IMU因子，进行IMU数据两帧之间的积分计算；添加先验因子，以当前接收到的激光里程计作为先验因子；对变量节点进行赋予初值，执行优化并且更新之前时刻的状态；使用当前优化后的状态对预积分器进行初始化，对当前帧之后的IMU数据进行计算。

进一步地，通过IMU增量对机器人的运动进行推断，机器人在t+Δt时刻的位置、速度和旋转计算公式如下：

其中

假设积分过程中机器人角速度和加速度都保持不变。

通过IMU预积分来获得不同时间戳之间的相对运动，预积分得到的第m时刻和第n时刻之间的位置变化Δp_mn、速度变化Δv_mn和旋转变化ΔR_mn的计算公式如下：

式中：R_n为n时刻的旋转量，R_m为m时刻的旋转量，p_n为n时刻的位置量，p_m为m时刻的位置量，v_n为n时刻的速度量，v_m为m时刻的速度量。

在一实施例中，如图4所示，所述步骤S20中，对激光雷达产生的点云数据进行运动畸变校正，包括以下步骤：

(1)订阅IMU采集到的原始数据和IMU增量，当接收到新的IMU数据时，通过IMU积分数据以及当前得到的新的IMU数据，预估当前时刻的位姿。

(2)订阅激光雷达所采集到的原始点云数据，默认在激光扫描一帧的时间内仅发生了旋转并没有发生平移，对齐IMU数据与时间戳，同时计算IMU队列中每帧包括RPY在内的IMU积分数据，再对激光帧中的每个扫描点进行线性插值。

(3)发布当前帧经过畸变校正之后的有效点云，发布当前帧经过畸变校正之后的点云数据、位姿角、初始位姿等信息。

在一实施例中，如图5所示，所述步骤S20中，从畸变校正之后的点云数据中提取点云特征信息，包括以下步骤：

(1)对于所述畸变校正之后的点云数据中的每一点云帧，取当前点云帧的前五个点和后五个点，并计算各点到当前点云帧中间点的距离差之和的平方，作为当前点云帧的曲率；

(2)将每一点云帧分成若干段，并从每段提取设定数量的边缘点和平面点，以平滑度为指标对每段内的边缘点和平面点分别进行排序，得到第一排序结果和第二排序结果；

具体地，将一帧的点云数据分成6段，每段提取20个边缘点和20个平面点，以平滑度为指标对段内的所有点进行排序。

(3)基于所述第一排序结果，按照平滑度从低到高，提取边缘点特征；基于所述第二排序结果，按照平滑度从高到低，提取平面点特征；

需要说明的是，一帧点云数据中余下的未处理的点归类为平面点。

(4)将具有所述边缘点特征和所述平面点特征的点云数据进行封装，得到所述点云特征信息。

需要说明的是，考虑到矿山井巷非结构化环境特征，一定情况下，激光雷达无法采集到足够的特征点云，IMU可以在无足够的特征点云时进行预积分，弥补没有足够特征点云的缺陷，使用激光雷达和IMU进行紧耦合的方法，将激光雷达的原始点云与IMU数据进行一同运算，提高整个***在非结构化环境中的精度。

在一实施例中，如图6所示，所述步骤S40中，激光里程计的数据的确定过程包括以下步骤：

(1)基于激光雷达的点云帧，确定关键帧，所述激光雷达的点云帧由平面特征和边缘特征组成。

具体包括：通过计算曲率来提取平面和边缘特征，曲率较小的点被划分为平面特征，曲率较大的点被划分为边缘特征；定义激光雷达在t时刻扫描点云中边缘特征

和平面特征

基于所述边缘特征和所述平面特征构成激光雷达点云帧

在满足机器人当前状态节点与上一时刻状态节点的差值超过所定义的阈值时，将当前状态节点对应的激光雷达点云帧确定为所述关键帧；关联所述当前状态节点和所述关键帧。

比如：当机器人此时的状态X_k+1与之前的状态X_k的差值超过了所定义的阈值时，则将激光雷达帧R_k+1当作关键帧，并关联机器人状态节点X_k+1和关键帧F_k+1。

需要说明的是，确定关键帧后，将两个关键帧之间的非关键帧丢弃。

应当理解的是，本实施例所述阈值为经过大量实验得出的用于与平移和旋转的状态量进行比较的数值，平移和旋转的变化阈值据图可选择1米和10度。

需要说明的是，利用机器人的节点状态来定义关键帧，由于状态参数是多维的向量形式，相比较于距离这一单一数据，对环境的感知更加明显，因此本方中对关键帧的定义更加准确，进一步提高了***的精确性。

(2)通过滑动窗口方法构建包括一定数量扫描点云的点云地图，将关键帧注册到固定大小的先验子关键帧集合中，并采用变换关系将子关键帧集合转换到世界坐标系中，得到由子关键帧合并形成的地图。

具体地，通过滑动窗口的方法去构建包括了一定数量的最近激光雷达的扫描点云的点云地图；采用变换关系{T_k-n,...,T_k}将相关联的子关键帧集合{F_k-n,...,F_k}转换到世界坐标系中，其中，T_k表示k时刻的旋转和平移，T_k-n表示k-n时刻的旋转和平移；转换后的子关键帧会被合并成一个地图M_k。

其中，地图M_k的公式表示为：

式中：

为边缘特征地图，

为平面特征地图，

是经过转换，在世界坐标系下的平面和边缘特征。

(3)通过扫描匹配将得到的激光雷达的当前关键帧加入所述地图，得到实际坐标位置。

比如：通过扫描匹配将最新得到的雷达关键帧

加入地图M_k中，进行坐标系转换，将机器人坐标系中的关键帧

转换到世界坐标系中

(4)计算所述边缘特征与对应的边缘块的第一距离，以及计算所述平面特征与对应的平面块之间的第二距离。

具体地，第一距离和第二距离的计算公式分别为：

式中：i,u,v,w分别为平面特征或边缘特征的集合中的索引号，

为第i索引中第k+1时刻所取边缘点，

为第v索引中第k时刻所取边缘点，

为第u索引中第k时刻所取边缘点，

为第i索引中第k+1时刻所取平面点，

为第v索引中第k时刻所取平面点，

为第u索引中第k时刻所取平面点。

(5)基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系，得到机器人相邻时刻状态节点的位姿变换关系作为所述激光里程计的数据。

具体地，基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系为：

式中：d_ek为第一距离，d_pk为第二距离，

为机器人坐标系中的关键帧转换到世界坐标系中的边缘特征和平面特征，

为k+1时刻选取的边缘点，

为k+1时刻选取的平面点。

需要说明的是，边缘点是特征明显的点，平面点是特征不明显的点。

基于所述变换矩阵，得到状态节点X_k和X_k+1之间的变换关系ΔT_k,k+1：

式中：T_k为k时刻的旋转和平移，T_k+1为k+1时刻的旋转和平移。

需要说明的是，并且将激光里程计的数据作为优化因子，添加至因子图中，由于激光里程计数据可以去除畸变，进一步提高精度。

在一实施例中，所述方法还包括：

当有新的状态节点添加到所述因子图中时，在欧几里得空间搜索所述因子图中最接近新的所述状态节点的先验状态节点；

将最近接的所述先验状态节点和下一时刻的状态节点的位姿变换关系作为闭环因子加入到所述因子图中。

具体地，当有新的状态X_k+1添加到因子图中时，首先搜索因子图并在欧几里得空间中找到接近X_k+1的先验状态。假设X₅是返回的候选状态之一，通过扫描匹配将F_k+1与子关键帧{F_5-m,...,F₅,...,F_5+m}进行匹配。扫描匹配之前，通过转换将F_k+1和之前的子关键帧转换到世界坐标系W中。将得到的ΔT_5,i+1作为闭环因子加入到因子图中。

进一步地，在因子图优化之后，本实施例还利用因子图优化后的历史关键帧位姿，矫正激光雷达之前所有的点云数据。

在一实施例中，所述方法还包括：将所述IMU积分数据、所述畸变校正之后的点云数据、所述点云特征信息以及所述因子图的优化结果通过Rviz展示。

在一实施例中，所述步骤S30，具体包括以下步骤：

S31、通过所述历史关键帧构造Kdtree，并且搜索匹配指定搜索半径的关键帧，将距离其最近且时间间隔满足指定范围的关键帧作为其匹配帧；

S32、通过当前历史关键帧固有的边缘点、平面点构造当前历史关键帧的第一特征集，使用所述匹配帧前后相邻的共m个关键帧特征点构造所述匹配帧的第二特征集；

S33、基于指定的迭代次数和最小迭代阈值，对第一特征点集和第二特征点集进行匹配，根据匹配的结果矫正当前历史关键帧的位姿，并保存当前历史关键帧与匹配帧之间的对应关系，作为闭环因子。

本实施例通过进行关键帧的匹配，降低激光雷达和IMU进行紧耦合的情况下的计算量，使得精度与计算量之间取得平衡，保证了***的准确率和实时性，得到的矿山井巷非结构化特征下的激光雷达和IMU的紧耦合SLAM方法实验结果图如图7所示。

此外，如图8所示，本发明第二实施例还提出了一种激光雷达和IMU的紧耦合SLAM***，所述***包括：

预积分模块10，用于基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；

校正提取模块20，用于对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；

闭环因子确定模块30，用于从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；

优化模块40，用于对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。

本实施例使用激光雷达和IMU进行紧耦合的方法，将激光雷达的原始点云与IMU数据进行一同运算，提高整个***在非结构化环境中的精度；并且将激光里程计的数据作为优化因子，添加至因子图中，由于激光里程计数据可以去除畸变，进一步提高精度，但需要增加运算代价。本实施例针对激光雷达点云数据与IMU数据同时运算造成计算量增加的问题，进行关键帧的匹配，使得精度与计算量之间取得平衡，保证了***的准确率和实时性。

进一步地而言，本实施例通过设置IMU里程计和激光里程计，并通过激光雷达和IMU进行紧耦合的方，无需依赖于GNSS数据，可在无卫星信号下使用，更具有通用性。

在一实施例中，所述预积分模块10，包括：

接收单元，用于接收所述激光里程计的数据，并保存所述激光里程计的当前时间戳；

具体地，本实施例将接收的激光里程数据的数据统一为指定格式并将激光里程计的当前时间戳进行保存。

相对位姿变换计算单元，用于计算IMU队列中起始时刻和终止时刻所对应IMU里程计之间的相对位姿变换，所述起始时刻为所述激光里程计的当前时间戳；

具体地，本实施例基于激光里程计的当前时间戳，将IMU队列中时间戳早于激光里程计当前时间戳的IMU增量数据移除；然后计算IMU队列中起始和终止时刻对应的IMU里程计之间的相对位姿变换；再将先前接收到激光里程计的数据和计算得出的相对位姿变换得到的数据进行结合并发布。

预积分单元，用于将IMU原始数据加入到IMU队列，并利用所述激光里程计的数据和所述相对位姿变换作为积分的起始值，对所述IMU队列中两帧之间的数据进行积分计算，得到所述IMU积分数据。

在一实施例中，所述校正提取模块20，包括：

畸变校正单元，用于对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正后的点云数据；

特征提取单元，用于从畸变校正之后的点云数据中提取点云特征信息，具体用于实现如下步骤：

(1)对于所述畸变校正之后的点云数据中的每一点云帧，取当前点云帧的前五个点和后五个点，并计算各点到当前点云帧中间点的距离差之和的平方，作为当前点云帧的曲率；

(2)将每一点云帧分成若干段，并从每段提取设定数量的边缘点和平面点，以平滑度为指标对每段内的边缘点和平面点分别进行排序，得到第一排序结果和第二排序结果；

具体地，将一帧的点云数据分成6段，每段提取20个边缘点和20个平面点，以平滑度为指标对段内的所有点进行排序。

(3)基于所述第一排序结果，按照平滑度从低到高，提取边缘点特征；基于所述第二排序结果，按照平滑度从高到低，提取平面点特征；

需要说明的是，一帧点云数据中余下的未处理的点归类为平面点。

(4)将具有所述边缘点特征和所述平面点特征的点云数据进行封装，得到所述点云特征信息。

在一实施例中，闭环因子确定模块30，包括：

搜索匹配单元，用于通过所述历史关键帧构造Kdtree，并且搜索匹配指定搜索半径的关键帧，将距离其最近且时间间隔满足指定范围的关键帧作为其匹配帧；

特征集构建单元，用于通过当前历史关键帧固有的边缘点、平面点构造当前历史关键帧的第一特征集，使用所述匹配帧前后相邻的共m个关键帧特征点构造所述匹配帧的第二特征集；

匹配单元，用于基于指定的迭代次数和最小迭代阈值，对第一特征点集和第二特征点集进行匹配，根据匹配的结果矫正当前历史关键帧的位姿，并保存当前历史关键帧与匹配帧之间的对应关系，作为闭环因子。

在一实施例中，所述优化模块40包括激光里程计数据因子添加单元，具体用于：

(1)基于激光雷达的点云帧，确定关键帧，所述激光雷达的点云帧由平面特征和边缘特征组成。

(2)通过滑动窗口方法构建包括一定数量扫描点云的点云地图，并采用变换关系将相关联的关键帧集合转换到世界坐标系中，得到由关键帧合并形成的地图。

(3)通过扫描匹配将得到的激光雷达的当前关键帧加入所述地图。

(4)计算所述边缘特征与对应的边缘块的第一距离，以及计算所述平面特征与对应的平面块之间的第二距离。

(5)基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系，得到机器人相邻时刻状态节点的位姿变换关系作为所述激光里程计的数据。

在一实施例中，所述优化模块40包括闭环因子添加单元，具体用于：

当有新的状态节点添加到所述因子图中时，在欧几里得空间搜索所述因子图中最接近新的所述状态节点的先验状态节点；

将最近接的所述先验状态节点和下一时刻的状态节点的位姿变换关系作为闭环因子加入到所述因子图中。

需要说明的是，本发明所述激光雷达和IMU的紧耦合SLAM***的其他实施例或具有实现方法可参照上述各方法实施例，此处不再赘余。

需要说明的是，在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行***、装置或设备(如基于计算机的***、包括处理器的***或其他可以从指令执行***、装置或设备取指令并执行指令的***)使用，或结合这些指令执行***、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行***、装置或设备或结合这些指令执行***、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行***执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (10)

1.一种激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述方法包括：

基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；

对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；

从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；

对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。

2.如权利要求1所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据，包括：

接收所述激光里程计的数据，并保存所述激光里程计的当前时间戳；

计算IMU队列中起始时刻和终止时刻所对应IMU里程计之间的相对位姿变换，所述起始时刻为所述激光里程计的当前时间戳；

将IMU原始数据加入到IMU队列，并利用所述激光里程计的数据和所述相对位姿变换作为积分的起始值，对所述IMU队列中两帧之间的数据进行积分计算，得到所述IMU积分数据。

3.如权利要求1所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述从畸变校正之后的点云数据中提取点云特征信息，包括：

对于所述畸变校正之后的点云数据中的每一点云帧，取当前点云帧的前五个点和后五个点，并计算各点到当前点云帧中间点的距离差之和的平方，作为当前点云帧的曲率；

将每一点云帧分成若干段，并从每段提取设定数量的边缘点和平面点，以平滑度为指标对每段内的边缘点和平面点分别进行排序，得到第一排序结果和第二排序结果；

基于所述第一排序结果，按照平滑度从低到高，提取边缘点特征；

基于所述第二排序结果，按照平滑度从高到低，提取平面点特征；

将具有所述边缘点特征和所述平面点特征的点云数据进行封装，得到所述点云特征信息。

4.如权利要求1所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述激光里程计的数据的确定过程包括：

基于激光雷达的点云帧，确定关键帧，所述激光雷达的点云帧由平面特征和边缘特征组成；

通过滑动窗口方法构建包括一定数量扫描点云的点云地图，将关键帧注册到固定大小的先验子关键帧集合中，并采用变换关系将子关键帧集合转换到世界坐标系中，得到由子关键帧合并形成的地图；

通过扫描匹配将得到的激光雷达的当前关键帧加入所述地图；

计算所述边缘特征与对应的边缘块的第一距离，以及计算所述平面特征与对应的平面块之间的第二距离；

基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系，得到机器人相邻时刻状态节点的位姿变换关系作为所述激光里程计的数据。

5.如权利要求4所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述基于激光雷达的点云帧，确定关键帧，包括：

定义激光雷达在t时刻扫描点云中边缘特征

和平面特征

基于所述边缘特征和所述平面特征构成激光雷达点云帧

在满足机器人当前状态节点与上一时刻状态节点的差值超过所定义的阈值时，将当前状态节点对应的激光雷达点云帧确定为所述关键帧；

关联所述当前状态节点和所述关键帧。

6.如权利要求4所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系，得到机器人相邻时刻状态节点的位姿变换关系作为所述激光里程计的数据，包括：

基于所述第一距离和所述第二距离，采用高斯牛顿法求解最优的变换矩阵关系为：

式中：d_ek为第一距离，d_pk为第二距离，

为机器人坐标系中的关键帧转换到世界坐标系中的边缘特征和平面特征，

为k+1时刻的边缘点，

为k+1时刻的平面点；

基于所述变换矩阵，得到状态节点X_k和X_k+1之间的变换关系ΔT_k,k+1：

式中：T_k为k时刻的旋转和平移，T_k+1为k+1时刻的旋转和平移。

7.如权利要求6所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述方法还包括：

当有新的状态节点添加到所述因子图中时，在欧几里得空间搜索所述因子图中最接近新的所述状态节点的先验状态节点；

将最近接的所述先验状态节点和下一时刻的状态节点的位姿变换关系作为闭环因子加入到所述因子图中。

8.如权利要求1所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子，包括：

通过所述历史关键帧构造Kdtree，并且搜索匹配指定搜索半径的关键帧，将距离其最近且时间间隔满足指定范围的关键帧作为其匹配帧；

通过当前历史关键帧固有的边缘点、平面点构造当前历史关键帧的第一特征集，使用所述匹配帧前后相邻的共m个关键帧特征点构造所述匹配帧的第二特征集；

基于指定的迭代次数和最小迭代阈值，对第一特征点集和第二特征点集进行匹配，根据匹配的结果矫正当前历史关键帧的位姿，并保存当前历史关键帧与匹配帧之间的对应关系，作为闭环因子。

9.如权利要求1所述的激光雷达和IMU的紧耦合SLAM方法，其特征在于，所述方法还包括：

将所述IMU积分数据、所述畸变校正之后的点云数据、所述点云特征信息以及所述因子图的优化结果通过Rviz展示。

10.一种激光雷达和IMU的紧耦合SLAM***，其特征在于，所述***包括：

预积分模块，用于基于激光里程计的数据和IMU里程计的数据，进行IMU预积分，得到IMU积分数据；

校正提取模块，用于对激光雷达产生的点云数据进行运动畸变校正，得到畸变校正之后的点云数据，并从畸变校正之后的点云数据中提取点云特征信息；

闭环因子确定模块，用于从历史关键帧中选择候选闭环匹配帧，进行所述激光雷达的扫描数据与地图的匹配，得到位姿变换，构建闭环因子；

优化模块，用于对于所述点云特征信息，将所述IMU积分数据、所述激光里程计的数据以及闭环因子作为图优化算法的因子，添加至因子图中，对所述因子图进行优化，更新所有关键帧的位姿，得到重建地图。

Images