CN115984463A - 一种适用于狭窄巷道的三维重建方法和系统 - Google Patents

Abstract

本申请公开了一种适用于狭窄巷道的三维重建方法和系统，其中，方法包括：利用机械激光雷达和固态激光雷达进行点云累积，得到机械激光点云数据和固态激光点云数据；利用IMU进行前向传播，预测机器人的姿态，得到IMU估计的位姿变换；利用机械激光雷达从机械激光点云数据中筛选出特征点，根据IMU估计的位姿变换对机械激光点云数据和固态激光点云数据进行投影，形成点云帧；对点云帧和IMU估计的位姿变换进行滤波优化，得到最优位姿估计；利用最优位姿估计更新机械激光局部地图，生成并更新固态激光全局地图，形成巷道三维重建模型。本发明以固态激光雷达为主体辅以机械激光雷达和IMU作为约束，可以获得更多的巷道细节。

Description

一种适用于狭窄巷道的三维重建方法和系统

技术领域

本申请涉及三维重建领域，具体而言涉及一种基于机械式激光雷达、固态激光雷达和IMU(IMU)融合的三维重建方法，用于狭窄巷道的三维重建。

背景技术

巷道是在地表与矿体之间钻凿出的各种通路，用来运矿、通风、排水、行人以及为冶金设备采出矿石新开凿的各种必要准备工程等。由于巷道一般处于地下，会受到岩土体的挤压，并且在矿山开采期间容易受到影响从而导致安全问题，因此对巷道进行测绘非常重要。巷道建图技术来源于矿山测量学中，传统方法使用全站仪和三维激光扫描仪来获得巷道的环境信息。但传统的巷道建图过程耗时长，人员调动过多，更新地图慢，且全站仪和高精度的三维扫描仪价格不菲。随着近些年传感器精度的提高和种类的增加，传感器融合SLAM逐渐被应用到了地下巷道的建图技术中。根据目前所查阅的文献，发现一般在建图时只会使用到一个激光雷达，配合其他传感器融合使用。激光雷达分为机械激光雷达和固态激光雷达，机械激光雷达具有水平视角大的优点，可以提供更多的环境特征信息，但扫描所得的点云较稀疏，而固态激光雷达则是可以获得非常稠密的点云，但视场较小，捕捉到的环境特征信息较少，容易在建图时发生漂移。此外，由于地下巷道无GPS信号，因此使用IMU作为姿态估计传感器常被使用作为改善定位信息的方法。

考虑到巷道的三维重建所需要得到的是一个无漂移且稠密的点云模型，因此本发明将机械激光雷达、固态激光雷达和IMU融合，将其搭载在移动机器人底盘上，形成了一种新的紧耦合多传感器融合SLAM的巷道三维重建方法。此方法相较于单激光雷达SLAM的三维重建得到的三维模型要更加鲁棒，更加稠密，相较于使用传统的全站仪和三维扫描仪的方法，成本更低且耗费的人力要少很多。

发明内容

本申请公开了一种适用于巷道的三维重建方法，通过将机械激光雷达、固态激光雷达和IMU融合，将其搭载在移动机器人底盘上，形成了一种新的紧耦合多传感器融合SLAM的巷道三维重建方法。

为达到上述目的，本申请提供了以下方案：

一种适用于狭窄巷道的三维重建方法和系统，方法包括：

S1、分别利用机械激光雷达和固态激光雷达进行点云累积，得到机械激光点云数据和固态激光点云数据；

S2、利用IMU进行前向传播，预测机器人的姿态，得到IMU估计的位姿变换；

S3、利用所述机械激光雷达从所述机械激光点云数据中筛选出平面特征点和边缘特征点，根据所述IMU估计的位姿变换对所述机械激光点云数据和所述固态激光点云数据进行投影，形成点云帧；

S4、对所述点云帧和所述IMU估计的位姿变换进行滤波优化，得到最优位姿估计；

S5、利用所述最优位姿估计更新机械激光局部地图，生成并更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

优选的，得到所述机械激光点云数据和所述固态激光点云数据的过程具体包括：

对所述机械激光雷达和所述固态激光雷达的高频采样累计预设时间内的数据进行处理，得到所述机械激光点云数据和所述固态激光点云数据。

优选的，得到IMU估计的位姿变换具体过程包括：

开启所述IMU，利用所述IMU进行前向传播，预测机器人不同时刻的姿态，得到IMU估计的位姿变换。

优选的，所述IMU前向传播的过程具体包括：

六轴IMU的数据进行前向传播实时的线加速度和角加速度，利用IMU的数据进行前向传播，估计机器人实时位姿变化；得到IMU前向传播的公式为：

其中，x_i即上面定义的机器人的状态，u_i为输入，Δt为IMU两帧的间隔，0是指过程噪声设置为0。

优选的，所述筛选出平面特征点和边缘特征点具体过程包括：

从原始雷达数据中心，筛选用于后续状态估计环节的特征点，采用所述点云数据中随机一点前后5个点作为一组，计算曲率：

其中，c为曲率，S为随机一点的集合，i为当前为中心的点，k为第k帧的点，L为在机械激光雷达坐标系下，

为机械激光雷达坐标系下点的坐标。

优选的，进行滤波优化的过程具体包括：

采用扩展卡尔曼滤波器优化位姿估计方程，包括预测模型和矫正模型；

预测模型具体包括：先验估计，公式为：

先验误差的协方差矩阵，公式为：

其中，

为预测出来的机器人姿态，f(·)为状态方程，x_k-1为上一时刻确定的机器人状态，u_k-1为输入，0表示我们把过程噪声仍然看做0，Q为符合高斯分布的噪声协方差，

矫正模型包括：卡尔曼增益，公式为：

后验估计，公式为：

预测误差的协方差矩阵，公式为：

其中，

h(·)为输出方程。

优选的，所述形成稠密的巷道三维重建模型具体包括：

根据所述扩展卡尔曼滤波器优化位姿估计方程优化后的位姿估计，对所述机械激光点云数据和所述固态激光点云数据得到的不同时刻的点云帧按位姿变换方程进行投影，更新机械激光局部地图、生成和更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

一种适用于狭窄巷道的三维重建方法还包括一种适用于狭窄巷道的三维重建系统，包括点云累积模块、前向传播模块、筛选模块、滤波模块和模型建立模块；

点云累积模块用于分别利用机械激光雷达和固态激光雷达进行点云累积，得到机械激光点云数据和固态激光点云数据；

前向传播模块用于利用IMU进行前向传播，预测机器人的姿态，得到IMU估计的位姿变换；

筛选模块用于利用所述机械激光雷达从所述机械激光点云数据中筛选出平面特征点和边缘特征点，根据所述IMU估计的位姿变换对所述机械激光点云数据和所述固态激光点云数据进行投影，形成点云帧；

滤波模块用于对所述点云帧和所述IMU估计的位姿变换进行滤波优化，得到最优位姿估计；

模型建立模块利用所述最优位姿估计更新机械激光局部地图，生成并更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

本申请的有益效果为：

本申请公开了一种适用于巷道的三维重建方法，本申请的基于机械式激光雷达、固态激光雷达和IMU的测绘方法的优点在于：

(1)以固态激光雷达为主体的三维重建，其点云更加稠密，可以获得更多的巷道细节；

(2)辅以机械激光雷达和IMU作为约束，提高了三维重建的鲁棒性；

(3)相较于使用全站仪测绘成本大大降低；

(4)相较于人工测量的方法自动化程度更高；

(5)形成的紧耦合的基于机械激光雷达、固态激光雷达和IMU的三维重建方法，结合了二者的优点，克服了二者的缺点，课获得稳定性高、稠密的巷道三维重建模型。

附图说明

为了更清楚地说明本申请的技术方案，下面对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本申请实施例一一种适用于巷道的三维重建方法的方法步骤图；

图2为本申请实施例一一种适用于巷道的三维重建方法的流程图；

图3为本申请实施例二一种适用于巷道的三维重建系统的系统结构图；

图4为本实施例二一种适用于巷道的三维重建系统的实物图。

具体实施方式：

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

为使本申请的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本申请作进一步详细的说明。

实施例一

在本实施例一中，如图1-2所示，一种适用于巷道的三维重建方法，具体包括：

各设备(雷达和IMU)开启后，激光雷达开始进行点云累积，而IMU则直接开始了前向传播以预测机器人的姿态。接着机械激光雷达会从原始雷达数据中筛选出平面特征点和边缘特征点，记作{Lj}，用于后续状态估计等环节。由于之前接收到的激光点云都来自不同时刻，所以我们需要通过IMU估计的位姿变换来将不同时刻的点云投影为一帧一帧的点云帧，经过运动补偿后的特征点记作{Lk}。但是，之前提取到的特征因为是处于不同时刻的，因此它们与地图还是会有误差的，所以接下来会进入迭代卡尔曼滤波环节，用以优化位姿估计。当滤波器收敛后，就得到了最优位姿估计，也即得到了机械激光里程计，它将加入到IMU的前向传播以纠正IMU的预测位姿变换。而最优位姿估计还会被用于更新机械局部地图，此地图一方面被用于重新投入滤波环节以继续优化位姿估计，另一方面则是会加入到固态激光雷达的滤波环节。固态激光雷达能够提取到周边的环境特征较少，所以此处直接省去了固态激光雷达提取特征的环节，直接进行了反向传播进行运动补偿，并将固态激光雷达的点云帧和机械激光雷达的局部地图做残差计算(激光雷达的开启时间是一样的，工作频率也是一样的，所以在任意时刻，机械激光局部地图都将会包含固态激光雷达的点云帧)以得到固态激光雷达部分的位姿估计(若设备固定好或无较大较急的转弯时，位姿估计以及里程计应当和机械激光雷达部分得到的结果相同)，然后通过优化后的姿态估计可以将点云帧注册到全局地图中去，最终形成的固态激光全局地图就是我们所需要的巷道三维重建模型。

接下来将会对各环节进行讲解，其中，由于两激光雷达的处理过程有部分重合，因此将不会重复讲解。

具体如下：

(一)点云累积：由于激光雷达原始数据的采样频率很高，为200khz，所以不可能对采样到的每个点都直接进行实时处理，而是需要先累积一定时间内的数据点再一次性处理，累积时间选取为20ms，也就是50hz的频率，也正是按这个频率来输出里程计和地图的。

(二)特征提取：从诸多原始雷达数据中心，筛选出可用于后续状态估计等环节的特征点(平面特征点和边缘特征点){L_j}。具体的特征提取方式参考了LOAM的特征提取方法，即用某个点的前后各5个点作为一组，计算其曲率，计算公式如下：

其中，c为曲率，S为选取的点的集合，i为表示当前为中心的点，k为第k帧的点，L为在机械激光雷达坐标系下。

(三)IMU前向传播：六轴IMU可以获得机器人实时的线加速度和角加速度，因此可以利用IMU的数据进行前向传播以估计机器人实时位姿变换。此处需参照Fastlio中定义的

和

具体定义如下：

其中，R、R₁、R₂∈SO(3)，a,

指数映射为：

同时，机器人的位姿x可以定义如下，M是指流形：

其中，

是为MU在全局框架中的姿态和位置，

为此时的运动速度，

和

为IMU偏差，

为重力矢量，其微分动态模型如下：

(p_GI)′＝v_GI

(v_GI)′＝R_GI(a_m-b_a-n_a)+g_G

(b_ω)′＝n_bω

(b_a)′＝n_ba

其中，n_bω、n_ba为高斯噪声，符合(·)′为求导，符号∧为斜对称矩阵。通过微分方程逆推积分公式可以获得当前IMU推导出的机器人位姿变换。

于是可以得到IMU前向传播的公式如下：

其中，x_i为上面定义的机器人的状态，u_i为输入，Δt为IMU两帧的间隔，0为过程噪声设置为0，方程f的定义如下：

(四)反向传播：由于之前的点都是在不同时刻测量得到的，所以它们之间存在相对运动，那么此时就需要进行反向传播来进行运动补偿了。第k帧中第i个点的线性插值公式为：

由此结合旋转矩阵R(也就是IMU前向传播得到的R_GI)就可以求得投影的公式，即：

经此变换，就可以将不同时刻的点云转变成一帧一帧的点云帧了。(五)扩展卡尔曼滤波模块：由于之前提取的特征点以及通过IMU进行预测得到的估计总会存在一些偏差，因此需要用滤波模块优化位姿估计以得到最优的机器人位姿估计，此处我们使用扩展卡尔曼滤波器来进行此项工作。扩展卡尔曼滤波的应用与卡尔曼滤波总体相似，但扩展卡尔曼滤波适用于非线性系统，其分为预测和校正两个步骤。

预测部分如下：

①先验估计：

②先验误差的协方差矩阵：

其中，

为预测出来的机器人姿态，f(·)为状态方程，x_k-1为上一时刻确定的机器人状态，u_k-1为输入，0为我们把过程噪声仍然看做0，Q为符合高斯分布的噪声协方差，

校正部分如下：

①卡尔曼增益：

②后验估计：

③预测误差的协方差矩阵：

其中，

h(·)表示输出方程。

由此，我们可以通过得出当前机器人的最优位姿估计。

具体的，地图注册：包含机械局部地图的形成和固态全局地图的形成。根据扩展卡尔曼滤波优化后的位姿变换方程，只需要将扫描得到不同时刻的点云帧按位姿变换方程投影后即可拼接为局部/全局地图。

里程计：包含机械激光里程计和固态激光里程计。即通过扩展卡尔曼滤波器得出的位姿信息。

值得一提的是，本申请融合了机械激光雷达和固态激光雷达，固态激光雷达生成的点云是非常稠密的，但是固态激光雷达的视野范围较小，只有机械激光雷达的1/5左右，而机械激光雷达的视野范围较广，可以达到360度，能够提取到更多的特征。根据流程图可以发现我们在固态激光雷达部分并没有用到特征提取的过程，而且是将固态激光雷达的点云帧和机械激光雷达的局部地图进行滤波来优化姿态，即我们信任机械激光雷达提取的周围环境特征，让它帮助固态激光雷达所创建的稠密的地图更加稳定。

实施例二：

一种适用于狭窄巷道的三维重建系统，如图3-4所示，包括点云累积模块、前向传播模块、筛选模块、滤波模块和模型建立模块；

点云累积模块用于分别利用机械激光雷达和固态激光雷达进行点云累积，得到机械激光点云数据和固态激光点云数据；

前向传播模块用于利用IMU进行前向传播，预测机器人的姿态，得到IMU估计的位姿变换；

筛选模块用于利用所述机械激光雷达从所述机械激光点云数据中筛选出平面特征点和边缘特征点，根据所述IMU估计的位姿变换对所述机械激光点云数据和所述固态激光点云数据进行投影，形成点云帧；

滤波模块用于对所述点云帧和所述IMU估计的位姿变换进行滤波优化，得到最优位姿估计；

模型建立模块利用所述最优位姿估计更新机械激光局部地图，生成并更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

以上所述的实施例仅是对本申请优选方式进行的描述，并非对本申请的范围进行限定，在不脱离本申请设计精神的前提下，本领域普通技术人员对本申请的技术方案做出的各种变形和改进，均应落入本申请权利要求书确定的保护范围内。

Claims (8)

1.一种适用于狭窄巷道的三维重建方法，其特征在于，方法包括：

S1、分别利用机械激光雷达和固态激光雷达进行点云累积，得到机械激光点云数据和固态激光点云数据；

S2、利用IMU进行前向传播，预测机器人的姿态，得到IMU估计的位姿变换；

S3、利用所述机械激光雷达从所述机械激光点云数据中筛选出平面特征点和边缘特征点，根据所述IMU估计的位姿变换对所述机械激光点云数据和所述固态激光点云数据进行投影，形成点云帧；

S4、对所述点云帧和所述IMU估计的位姿变换进行滤波优化，得到最优位姿估计；

S5、利用所述最优位姿估计更新机械激光局部地图，生成并更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

2.根据权利要求1所述的适用于狭窄巷道的三维重建方法，其特征在于，得到所述机械激光点云数据和所述固态激光点云数据的过程具体包括：

对所述机械激光雷达和所述固态激光雷达的高频采样累计预设时间内的数据进行处理，得到所述机械激光点云数据和所述固态激光点云数据。

3.根据权利要求1所述的适用于狭窄巷道的三维重建方法，其特征在于，得到IMU估计的位姿变换具体过程包括：

开启所述IMU，利用所述IMU进行前向传播，预测机器人不同时刻的姿态，得到IMU估计的位姿变换。

4.根据权利要求3所述的适用于狭窄巷道的三维重建方法，其特征在于，所述IMU前向传播的过程具体包括：

六轴IMU的数据进行前向传播实时的线加速度和角加速度，利用IMU的数据进行前向传播，估计机器人实时位姿变化；得到IMU前向传播的公式为：

其中，x_i即上面定义的机器人的状态，u_i为输入，Δt为IMU两帧的间隔，0是指过程噪声设置为0。

5.根据权利要求1所述的适用于狭窄巷道的三维重建方法，其特征在于，所述筛选出平面特征点和边缘特征点具体过程包括：

从原始雷达数据中心，筛选用于后续状态估计环节的特征点，采用所述点云数据中随机一点前后5个点作为一组，计算曲率：

其中，c为曲率，S为随机一点的集合，i为当前为中心的点，k为第k帧的点，L为在机械激光雷达坐标系下，

为机械激光雷达坐标系下点的坐标。

6.根据权利要求1所述的适用于狭窄巷道的三维重建方法，其特征在于，进行滤波优化的过程具体包括：

采用扩展卡尔曼滤波器优化位姿估计方程，包括预测模型和矫正模型；

预测模型具体包括：先验估计，公式为：

先验误差的协方差矩阵，公式为：

其中，

为预测出来的机器人姿态，f(·)为状态方程，x_k-1为上一时刻确定的机器人状态，u_k-1为输入，0表示我们把过程噪声仍然看做0，Q为符合高斯分布的噪声协方差，

矫正模型包括：卡尔曼增益，公式为：

后验估计，公式为：

预测误差的协方差矩阵，公式为：

其中，

h(·)为输出方程。

7.根据权利要求6所述的适用于狭窄巷道的三维重建方法，其特征在于，所述形成稠密的巷道三维重建模型具体包括：

根据所述扩展卡尔曼滤波器优化位姿估计方程优化后的位姿估计，对所述机械激光点云数据和所述固态激光点云数据得到的不同时刻的点云帧按位姿变换方程进行投影，更新机械激光局部地图、生成和更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

8.一种适用于狭窄巷道的三维重建系统，其特征在于，包括点云累积模块、前向传播模块、筛选模块、滤波模块和模型建立模块；

点云累积模块用于分别利用机械激光雷达和固态激光雷达进行点云累积，得到机械激光点云数据和固态激光点云数据；

前向传播模块用于利用IMU进行前向传播，预测机器人的姿态，得到IMU估计的位姿变换；

筛选模块用于利用所述机械激光雷达从所述机械激光点云数据中筛选出平面特征点和边缘特征点，根据所述IMU估计的位姿变换对所述机械激光点云数据和所述固态激光点云数据进行投影，形成点云帧；

滤波模块用于对所述点云帧和所述IMU估计的位姿变换进行滤波优化，得到最优位姿估计；

模型建立模块利用所述最优位姿估计更新机械激光局部地图，生成并更新固态激光全局地图，最终形成稠密的巷道三维重建模型。

Images