基于旋转雷达和IMU的巷道三维重建系统及方法

基于旋转雷达和imu的巷道三维重建系统及方法
技术领域
1.本发明涉及三维空间立体建模技术领域，尤其涉及一种基于旋转雷达和imu的巷道三维重建系统及方法。


背景技术：

2.三维空间立体建模技术、适用于煤矿行业地下矿井掘进、地铁等巷道等场景，而现有的巷道等地下环境的三维重建方式使用激光雷达、imu、gps等几种传感器，大多为非在线的方式，且创建的三维模型、位姿精度较低。
3.非在线的巷道三维重建方式无法为煤矿井下巷道掘进的少人、无人化作业提供环境信息。同时精度较低的三维重建方式会给巷道掘进提供错误的环境信息，最终导致巷道成型误差大，甚至精度达不到工程要求。


技术实现要素：

4.本发明要解决的技术问题是针对上述现有技术的不足，提供一种基于旋转雷达和imu的巷道三维重建系统及方法。
5.为解决上述技术问题，本发明所采取的技术方案是：一种基于旋转雷达和imu的巷道三维重建系统，系统包括：
6.移动载体机器、履带轮、轮速计、惯性测量系统imu、旋转雷达、工控机和电源；
7.所述轮速计安装于履带轮上，所述惯性测量系统imu、旋转雷达和工控机安装于移动载体机器上；
8.所述轮速计、惯性测量系统imu、旋转雷达分别与工控机电连接；
9.所述电源为轮速计、惯性测量系统imu、旋转雷达以及工控机供电。
10.所述工控机包括处理器和存储器；所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现巷道的三维重建。
11.所述旋转雷达包括激光雷达、旋转云台和旋转轴；所述旋转轴垂直于地面，并以恒定的速度旋转；所述激光雷达与旋转轴固定连接，并使激光雷达的扫描平面垂直于地面；固定连接的激光雷达与旋转轴随着旋转云台一起旋转。
12.另一方面，本发明还提供一种采用上述基于旋转雷达和imu的巷道三维重建系统进行三维重建的方法，包括如下步骤：
13.步骤1：基于旋转雷达和imu的巷道三维重建系统建立坐标系，并对旋转雷达进行标定，过程如下：
14.步骤1.1：根据移动载体机器建立移动载体坐标系te；
15.步骤1.2：根据惯性测量系统imu建立imu坐标系ti；
16.步骤1.3：根据旋转雷达建立旋转雷达坐标系t
l
；
17.步骤1.4：建立世界坐标系tw，并初始化与移动载体坐标系te重合；
18.步骤1.5：建立世界坐标系2t
w2
，并初始化与imu坐标系ti重合；
19.步骤1.6：激光雷达采集连续两帧点云，使用calidar calibration方法，获得激光雷达坐标系到云台坐标系的标定矩阵。
20.步骤2：静态的获取旋转雷达采集的三维点云数据，赋予点云中每一个点一个置信度，并去除置信度为零的点，过程如下：
21.步骤2.1：在移动载体机器静止时，静态的采用激光雷达获取巷道静态的二维点云数据和旋转云台的转角；
22.步骤2.2：根据静态的二维点云数据、旋转云台的转角以及步骤1.6得到的标定矩阵，获得旋转雷达坐标系下的三维点云数据；
23.步骤2.3：根据旋转雷达坐标系下的三维点云数据以及旋转雷达坐标系与移动载体坐标系的坐标变换矩阵，获得移动载体坐标系下的三维点云数据；
24.步骤2.4：测试不同距离下雷达精度，获得不同距离下雷达的精度变化情况，为点云中每一个点设置置信度，过程如下：
25.步骤2.4.1：获取激光雷达点云数据中每个测距点到雷达的距离；
26.步骤2.4.2：测试不同距离下雷达精度，并获得不同距离下雷达的精度变化情况；
27.步骤2.4.3：根据精度测试结果，为点云中每一个点设置一个0～1范围内的置信度，数值越大，点云置信度越高；
28.步骤2.5：判断点云中点的置信度是否为零，若为零，则剔除该点。
29.步骤3：通过滤波处理，对三维点云数据进行去除离群点、去除移动载体机器机身点云和体素化操作，过程如下：
30.步骤3.1：利用统计滤波器对三维点云数据进行去除离群点操作，得到去除离群点后的点云数据，过程如下：
31.步骤3.1.1：遍历点云，计算每个点与其最近的k个邻居点之间的平均距离；
32.步骤3.1.2：计算所有平均距离的均值μ与标准差σ，则距离阈值d
max
表示为d
max
＝μ a
×
σ，其中a为比例系数；
33.步骤3.1.3：再次遍历点云，剔除与k个邻居点的平均距离大于d
max
的点；
34.步骤3.2：利用条件滤波器对所述三维点云数据进行条件滤波，得到去除移动载体机器机身的点云数据，过程如下：
35.步骤3.2.1：根据移动载体机器大小与移动载体机器在移动载体坐标系下的位置，设置条件滤波参数；
36.步骤3.2.2：根据条件滤波参数将移动载体机器所在长方体内的全部点云去除，获得移动载体坐标系下去除移动载体机器机身点云后的三维点云数据；
37.步骤3.3：利用体素网格滤波器对点云数据进行滤波降采样，得到体素化后的三维点云数据，过程如下：
38.步骤3.3.1：设置网格大小，并将三维点云数据划分为多个网格，计算每一网格的重心；
39.步骤3.3.2：以重心数据代替相应网格中所有的点，获得移动载体坐标系下体素化后的三维点云数据。
40.步骤4：获取imu的惯性数据、轮速计的速度数据，通过卡尔曼算法融合获取移动载体机器位移数据，过程如下：
41.步骤4.1：获取惯性测量系统imu的惯性数据，包括imu坐标系下惯性测量系统imu的三轴加速度数据世界坐标系2下imu旋转角的四元数数据
42.步骤4.2：根据世界坐标系2下imu旋转角的四元数数据，获得对应的旋转矩阵r
i,w2
；
43.步骤4.3：根据imu坐标系下imu的三轴加速度数据，获得世界坐标系2下imu的加速度：
[0044][0045]
步骤4.4：获取两个轮速计的速度数据，通过运动学分析，获得移动载体坐标系下移动载体机器的速度数据
[0046][0047]
其中，为x轴速度，为y轴速度，为绕z轴角速度；步骤4.5：应用刚体平面运动的运动学原理，使用基点法由移动载体机器的速度数据求得移动载体坐标系下imu的速度数据其中，z方向速度为零；
[0048]
步骤4.6：根据移动载体坐标系下imu的速度数据获得世界坐标系2下imu的速度：
[0049][0050]
其中，r
i,e
为移动载体坐标系到imu坐标系的旋转矩阵；
[0051]
步骤4.7：将世界坐标系2下imu的速度和imu测得的imu加速度作为卡尔曼滤波的观测值，对系统状态进行最优估计，估计世界坐标系2下imu的位移、速度、加速度；
[0052]
步骤4.8：根据世界坐标系2下imu的加速度和旋转角的四元数数据，获得世界坐标系2下imu的位姿矩阵，再进行坐标转换得到世界坐标系下移动载体机器的位姿矩阵。
[0053]
步骤5：对前后两帧点云数据进行配准，获取高精度点云位姿，过程如下：
[0054]
步骤5.1：当旋转雷达开始获取点云时，同时储存世界坐标系移动载体机器位姿矩阵；
[0055]
步骤5.2：获取当前帧与上一帧点云对应的移动载体机器位姿矩阵，同时获取当前帧与上一帧点云对应的移动载体机器位姿变换矩阵；
[0056]
步骤5.3：将移动载体机器位姿变换矩阵作为初值，对当前帧与上一帧点云进行icp配准，获得精度更高的移动载体机器位姿变化矩阵；
[0057]
步骤5.4：使用移动载体机器位姿变换矩阵修正所述当前帧移动载体机器位姿矩阵，获得更高精度的移动载体机器位姿矩阵；
[0058]
步骤5.5：，根据步骤5.4得到的移动载体机器位姿矩阵将当前帧三维点云数据由移动载体坐标系转换到世界坐标系下，最终获得世界坐标系下的单帧三维点云数据。
[0059]
步骤6：去除历史帧的点云数据中的工作面；
[0060]
步骤7：基于置信度进行点云融合，获得三维巷道模型，过程如下：
[0061]
步骤7.1：根据世界坐标系下三维点云数据中每一个点所在体素计算点云中每个点的索引；
[0062]
步骤7.2：将第一帧点云的所有点的索引放入索引容器中；
[0063]
步骤7.3：当新的点云输入时，依次判断此点云中每个点的索引是否在索引容器中已经存在；
[0064]
若不存在，将此点的索引放入索引容器中，此点保留到整体点云中，然后继续判断下一个点；
[0065]
若存在，保留两个点中置信度较大的点，舍弃置信度较小的点，置信度相同则保留两个点的中点，然后继续判断下一个点；
[0066]
步骤7.4：直至点云中所有点判断完毕，继续等待下一帧点云输入，实现索引容器中对索引的动态去除，只保留可能会被影响体素的索引；
[0067]
步骤7.5：将整体点云进行表面重建，在线的获得巷道的三维重建模型。
[0068]
采用上述技术方案所产生的有益效果在于：
[0069]
1、本发明提供的系统中设计了一种旋转雷达，实现了三维场景的无盲区的检测；
[0070]
2、本发明提供的方法静态的获得三维场景点云，避免了运动导致的点云畸变；
[0071]
3、本发明提供的方法提出了置信度的概念，并通过合理的置信度赋值以及基于置信度的点云融合，获得了更为精准的巷道场景点云；
[0072]
4、本发明提供的方法使用卡尔曼滤波算法处理imu获取的移动载体机器加速度和轮速计与采集移动载体机器速度获得移动载体机器位移，结合imu获取的移动载体机器旋转角，得到移动载体机器位姿信息；
[0073]
5、本发明提供的方法根据上一帧点云数据采集时移动载体机器位姿、移动载体机器挖掘巷道的可达空间，计算巷道的工作面，利用条件滤波去除上一帧点云数据中的工作面，避免了已被挖掘的巷道墙体仍保留到巷道点云中；
[0074]
6、本发明提供的方法对巷道点云进行了表面重建，使得地面监控人员更直观的观测到巷道环境。
[0075]
7、本发明适应煤矿井下地质环境、能够实现在线的、高效的、安全的、高精度的巷道三维重建，加快了我国煤矿智能化进程速度，推进了我国矿业工业化进程。
附图说明
[0076]
图1为本发明实施例中基于旋转雷达和imu的巷道三维重建系统的结构图；
[0077]
图2为本发明实施例中采用基于旋转雷达和imu的巷道三维重建系统进行巷道三维重建方法的流程图；
[0078]
图3为本发明实施例中巷道三维重建系统中建立坐标系的示意图；
[0079]
图4为本发明实施例中数据处理流程图。
具体实施方式
[0080]
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
[0081]
如图1所示，本实施例中基于旋转雷达和imu的巷道三维重建系统如下所述。
[0082]
系统包括：移动载体机器7、履带轮1、轮速计2、惯性测量系统imu4、旋转雷达5、工控机6和电源3；
[0083]
所述轮速计2安装于履带轮1上，所述惯性测量系统imu4、旋转雷达5和工控机6安装于移动载体机器7上；
[0084]
所述轮速计2、惯性测量系统imu4、旋转雷达5分别与工控机6电连接；
[0085]
所述电源为轮速计2、惯性测量系统imu4、旋转雷达5以及工控机6供电。
[0086]
所述工控机6包括处理器和存储器；所述存储器上存储有计算机程序，所述计算机程序被所述处理器执行时，实现巷道的三维重建。
[0087]
所述旋转雷达5包括激光雷达、旋转云台和旋转轴；所述旋转轴垂直于地面，并以恒定的速度旋转；所述激光雷达与旋转轴固定连接，并使激光雷达的扫描平面垂直于地面；固定连接的激光雷达与旋转轴随着旋转云台一起旋转。
[0088]
本实施例中，移动载体机器7为掘进机，在掘进机的两侧履带轮1上左右对称安装两个轮速计2。
[0089]
所述轮速计2用于采集履带轮1的速度数据，并将所述速度数据发送至工控机6的存储器；
[0090]
所述惯性测量系统imu4用于采集移动载体机器的惯性数据，并将所述惯性数据发送至工控机6的存储器；
[0091]
所述旋转雷达5用于采集三维点云数据，并将所述三维点云数据发送至工控机6的存储器；
[0092]
所述存储器接收到采集回来的数据对数据进行存储，当处理器工作时同时调用存储器中的计算机程序和数据，并对数据进行处理，得到巷道的三维重建模型。
[0093]
另一方面，本发明还提供一种采用上述基于旋转雷达和imu的巷道三维重建系统进行三维重建的方法，其流程如图2所示，包括如下步骤：
[0094]
步骤1：基于旋转雷达和imu的巷道三维重建系统建立坐标系，并对旋转雷达进行标定，过程如下：
[0095]
步骤1.1：根据移动载体机器建立移动载体坐标系te，如图3所示；
[0096]
步骤1.2：根据惯性测量系统imu建立imu坐标系ti，如图3所示；
[0097]
步骤1.3：根据旋转雷达建立旋转雷达坐标系t
l
，如图3所示；
[0098]
步骤1.4：建立世界坐标系tw，如图3所示，并初始化与移动载体坐标系te重合；
[0099]
步骤1.5：建立世界坐标系2t
w2
，如图3所示，并初始化与imu坐标系ti重合；
[0100]
步骤1.6：激光雷达采集连续两帧点云，使用calidar calibration方法，获得激光雷达坐标系到云台坐标系的标定矩阵。
[0101]
步骤2：静态的获取旋转雷达采集的三维点云数据，赋予点云中每一个点一个置信度，并去除置信度为零的点，过程如下：
[0102]
步骤2.1：在移动载体机器静止时，静态的采用激光雷达获取巷道静态的二维点云数据和旋转云台的转角；
[0103]
步骤2.2：根据静态的二维点云数据、旋转云台的转角以及步骤1.6得到的标定矩阵，获得旋转雷达坐标系下的三维点云数据；
[0104]
步骤2.3：根据旋转雷达坐标系下的三维点云数据以及旋转雷达坐标系与移动载体坐标系的坐标变换矩阵，获得移动载体坐标系下的三维点云数据；
[0105]
步骤2.4：测试不同距离下雷达精度，获得不同距离下雷达的精度变化情况，为点
云中每一个点设置置信度，过程如下：
[0106]
步骤2.4.1：获取激光雷达点云数据中每个测距点到雷达的距离；
[0107]
步骤2.4.2：测试不同距离下雷达精度，并获得不同距离下雷达的精度变化情况；
[0108]
步骤2.4.3：根据精度测试结果，为点云中每一个点设置一个0～1范围内的置信度，数值越大，点云置信度越高；
[0109]
步骤2.5：判断点云中点的置信度是否为零，若为零，则剔除该点。
[0110]
步骤3：通过滤波处理，对三维点云数据进行去除离群点、去除移动载体机器机身点云和体素化操作，过程如下：
[0111]
步骤3.1：利用统计滤波器对三维点云数据进行去除离群点操作，得到去除离群点后的点云数据，过程如下：
[0112]
步骤3.1.1：遍历点云，计算每个点与其最近的k个邻居点之间的平均距离；
[0113]
步骤3.1.2：计算所有平均距离的均值μ与标准差σ，则距离阈值d
max
表示为d
max
＝μ a
×
σ，其中a为比例系数；
[0114]
步骤3.1.3：再次遍历点云，剔除与k个邻居点的平均距离大于d
max
的点；
[0115]
步骤3.2：利用条件滤波器对所述三维点云数据进行条件滤波，得到去除移动载体机器机身的点云数据，过程如下：
[0116]
步骤3.2.1：根据移动载体机器大小与移动载体机器在移动载体坐标系下的位置，设置条件滤波参数；
[0117]
步骤3.2.2：根据条件滤波参数将移动载体机器所在长方体内的全部点云去除，获得移动载体坐标系下去除移动载体机器机身点云后的三维点云数据；
[0118]
步骤3.3：利用体素网格滤波器对点云数据进行滤波降采样，得到体素化后的三维点云数据，过程如下：
[0119]
步骤3.3.1：设置网格大小，并将三维点云数据划分为多个网格，计算每一网格的重心；
[0120]
步骤3.3.2：以重心数据代替相应网格中所有的点，获得移动载体坐标系下体素化后的三维点云数据。
[0121]
步骤4：获取imu的惯性数据、轮速计的速度数据，通过卡尔曼算法融合获取移动载体机器位移数据，过程如下：
[0122]
步骤4.1：获取惯性测量系统imu的惯性数据，包括imu坐标系下惯性测量系统imu的三轴加速度数据世界坐标系2下imu旋转角的四元数数据
[0123]
步骤4.2：根据世界坐标系2下imu旋转角的四元数数据，获得对应的旋转矩阵r
i,w2
；
[0124]
步骤4.3：根据imu坐标系下imu的三轴加速度数据，获得世界坐标系2下imu的加速度：
[0125][0126]
步骤4.4：获取两个轮速计的速度数据，通过运动学分析，获得移动载体坐标系下移动载体机器的速度数据
[0127][0128]
其中，为x轴速度，为y轴速度，为绕z轴角速度；步骤4.5：应用刚体平面运动的运动学原理，使用基点法由移动载体机器的速度数据求得移动载体坐标系下imu的速度数据其中，z方向速度为零；
[0129]
步骤4.6：根据移动载体坐标系下imu的速度数据获得世界坐标系2下imu的速度：
[0130][0131]
其中，r
i,e
为移动载体坐标系到imu坐标系的旋转矩阵；
[0132]
步骤4.7：将世界坐标系2下imu的速度和imu测得的imu加速度作为卡尔曼滤波的观测值，对系统状态进行最优估计，估计世界坐标系2下imu的位移、速度、加速度；
[0133]
步骤4.8：根据世界坐标系2下imu的加速度和旋转角的四元数数据，获得世界坐标系2下imu的位姿矩阵，再进行坐标转换得到世界坐标系下移动载体机器的位姿矩阵。
[0134]
步骤5：对前后两帧点云数据进行配准，获取高精度点云位姿，过程如下：
[0135]
步骤5.1：当旋转雷达开始获取点云时，同时储存世界坐标系移动载体机器位姿矩阵；
[0136]
步骤5.2：获取当前帧与上一帧点云对应的移动载体机器位姿矩阵，同时获取当前帧与上一帧点云对应的移动载体机器位姿变换矩阵；
[0137]
步骤5.3：将移动载体机器位姿变换矩阵作为初值，对当前帧与上一帧点云进行icp配准，获得精度更高的移动载体机器位姿变化矩阵；
[0138]
步骤5.4：使用移动载体机器位姿变换矩阵修正所述当前帧移动载体机器位姿矩阵，获得更高精度的移动载体机器位姿矩阵；
[0139]
步骤5.5：，根据步骤5.4得到的移动载体机器位姿矩阵将当前帧三维点云数据由移动载体坐标系转换到世界坐标系下，最终获得世界坐标系下的单帧三维点云数据。
[0140]
步骤6：去除历史帧的点云数据中的工作面，过程如下：
[0141]
步骤6.1：根据当前帧的点云所对应的移动载体机器位姿，找到此时移动载体机器挖掘巷道的可达空间；
[0142]
步骤6.2：找到当前位置移动载体机器可以加工的最大范围，找到移动载体机器能对历史帧点云能够产生影响的最大范围；
[0143]
步骤6.3：使用条件滤波的方式去除世界坐标系下历史帧点云中在此范围内的所有点。
[0144]
步骤7：基于置信度进行点云融合，获得三维巷道模型，过程如下：
[0145]
步骤7.1：根据世界坐标系下三维点云数据中每一个点所在体素计算点云中每个点的索引；
[0146]
步骤7.2：将第一帧点云的所有点的索引放入索引容器中；
[0147]
步骤7.3：当新的点云输入时，依次判断此点云中每个点的索引是否在索引容器中已经存在；
[0148]
若不存在，将此点的索引放入索引容器中，此点保留到整体点云中，然后继续判断下一个点；
[0149]
若存在，保留两个点中置信度较大的点，舍弃置信度较小的点，置信度相同则保留两个点的中点，然后继续判断下一个点；
[0150]
步骤7.4：直至点云中所有点判断完毕，继续等待下一帧点云输入，实现索引容器中对索引的动态去除，只保留可能会被影响体素的索引；
[0151]
步骤7.5：将整体点云进行表面重建，在线的获得巷道的三维重建模型。
[0152]
本实施例中，方法的具体数据处理流程如图4所示。