Real-time 3D mapping method of fully mechanized working face based on laser SLAM
-
摘要: 移动式建图方法依赖高精度的光纤惯导和里程计进行位姿计算，而在实际工程实践中里程计精度难以满足应用需求，导致获取的工作面三维激光点云不完整。针对该问题，提出了一种基于激光SLAM的综采工作面实时三维建图方法。该方法主要包括激光点云去畸变、特征提取、位姿估计、优化建图等步骤。通过惯导数据消除激光点云的畸变，根据点云中每个点的时间戳检索惯导数据，获得对应每个点的姿态角，如果没有检索到对应姿态角，则采用四元数法进行插补。采用主成分分析法提取点云的几何张量特征，先求解点集的协方差矩阵，再进行特征值分解，得到几何张量特征。计算相邻2帧中特征点之间的距离，构建目标函数，采用Levenberg−Marquardt算法求解目标函数，获取变换矩阵，从而实现位姿估计。采用增量式优化算法，使用GTSAM优化库对历史关键帧与当前关键帧进行联合优化，将获得的所有关键帧点云叠加到一起，即为全局实时三维地图。井下工业性试验结果表明，该方法能实时、完整、高精度地构建全工作面范围的三维地图，最大绝对误差均值为0.19 m，满足综采工作面监控及刮板输送机找直精度需求。Abstract: The mobile mapping method relies on fiber optic inertial navigation with high precision and odometer to calculate the position and attitude. But in the actual engineering practice, the precision of odometer is difficult to meet the application requirements, resulting in incomplete 3D laser point cloud of working face. In order to solve this problem, a real-time 3D mapping method of fully mechanized working face based on laser SLAM is proposed. The method mainly comprises the steps of distortion removal of laser point cloud, feature extraction , position and attitude estimation and optimization mapping. The distortion of laser point cloud is eliminated through the inertial navigation data. The inertial navigation data is retrieved according to the time stamp of each point in the point cloud to obtain the attitude angle corresponding to each point. If the corresponding attitude angle is not retrieved, the quaternion method is adopted for interpolation. The geometric tensor feature of the point cloud is extracted by principal component analysis. Firstly, the covariance matrix of the point set is solved. Secondly, the eigenvalue decomposition is performed to obtain the geometric tensor feature. The distance between the feature points in two adjacent frames is calculated to construct an objective function. The Levenberg-Marquardt algorithm is used to solve the objective function and obtain the transformation matrix, so as to realize position and attitude estimation. The incremental optimization algorithm is adopted. The GTSAM optimization library is used for carrying out joint optimization on the historical keyframe and the current keyframe. All obtained keyframe point clouds are superposed together to obtain the global 3D real-time map. The results of the underground industrial test show that this method can construct the 3D map of the whole working face in real-time, completely and accurately. The maximum mean absolute error is 0.19 m, which meets the precision requirements of monitoring of fully mechanized working face and straightening of the scraper conveyor.
-
0. 引言
煤矿井下环境的复杂扰动和回采过程中煤层顶板、底板的不规则变化，造成液压支架支护位姿发生变化，可能造成支护失效，导致安全事故发生[1-3]。迅速且精确地获取液压支架支护位姿信息是当前综采工作面智能化进程中亟待攻克的关键技术挑战之一[4-5]。
在液压支架姿态感知研究方面，郭周斌[6]采用压力传感器、行程传感器和倾角传感器对液压支架位姿进行感知，但未考虑到井下恶劣环境导致传感器产生误差，未采取有效的滤波手段，缺乏多传感器间的数据融合。谢明明[7]采用双轴倾角传感器对液压支架构件的倾角数据进行采集，实现了对液压支架关键构件姿态角度的计算。王忠乐[8]利用测高传感器配合倾角传感器对液压支架高度、倾角变化情况进行分析，实现了支架姿态实时监测，但仅在底座上安装1个倾角传感器，可能导致测得的姿态与真实姿态存在较大误差。谢嘉成[9]将双轴倾角传感器布置在液压支架关键构件处，实现了对液压支架的位姿监测，但在复杂工况下监测误差可能较大。张坤等[10]利用超声波传感器与九轴姿态传感器实现了对超前液压支架姿态的感知，但仅采用卡尔曼滤波算法易陷入局部极小值点，寻优结果存在误差。崔宽宽[11]将倾角传感器部署于液压支架顶梁、底座与后连杆中，通过卡尔曼滤波算法提升位姿监测的准确性，但卡尔曼滤波算法常用于求解线性问题，而液压支架位姿感知属于非线性系统状态估计问题，可能导致估计误差偏大。Chen Ningning等[12]将研制的光纤光栅倾角传感器和光纤光栅压力传感器安装于液压支架上，实现了对液压支架位姿数据和压力数据的实时监测，虽然光纤光栅传感器具有高精度和抗电磁干扰的优点，但传感器受到噪声、振动等因素的干扰，未采用滤波算法进一步提升精度。Chen Hongyue等[13]采用广角超声波传感器与倾角传感器协同监测，通过融合算法对传感器数据进行融合，实现恶劣井下环境下液压支架的姿态感知，但仅能感知得到偏航角与横滚角数据，未涉及俯仰角数据。Gao Kuidong等[14]将拉线位移传感器、视觉传感器、惯性测量单元应用于液压支架位姿感知，并引入粒子群优化算法，但视觉传感器在井下恶劣环境中易受煤炭粉尘、外界振动等因素的影响，导致位姿感知精度不高，同时所需成本较高。
为了精确感知扰动环境下液压支架位姿，本文提出一种基于多传感器融合的液压支架位姿精确感知方法。在液压支架顶梁、掩护梁、后连杆和底座4个构件上部署九轴姿态传感器采集数据，并解算出其所在构件的横滚角、俯仰角和偏航角等位姿数据；通过无迹卡尔曼滤波（Unscented Kalman Filter，UKF）算法和改进梯度下降（Improved Gradient Descent，IGD）算法对位姿数据进行滤波处理；采用自适应加权融合算法对滤波处理后的液压支架顶梁和底座的偏航角和横滚角数据进行融合处理，实现液压支架位姿精确感知。
1. 液压支架支护位姿精确感知流程
液压支架支护位姿精确感知流程如图1所示。通过在液压支架各关键构件中部署九轴姿态传感器，解算出其所在构件的横滚角、俯仰角和偏航角等位姿数据，并通过IGD−UKF算法进行滤波处理；为消除外界振动、噪声等因素引起的液压支架顶梁和底座传感器数据误差，利用自适应加权融合算法对滤波处理后的液压支架顶梁与底座横滚角和偏航角位姿数据进行融合，输出精确的液压支架位姿数据。
![]()
图 1 液压支架支护位姿精确感知流程Figure 1. Precise perception process for position and posture of hydraulic supports2. 液压支架关键构件位姿数据解算
建立地理惯性坐标系[15]，定义正东方向为xn轴正方向，正北方向为yn轴正方向，根据右手正交坐标系定则，定义垂直于大地内部指向天空为zn轴正方向；建立液压支架顶梁坐标系，定义xb轴正方向为液压支架随采煤机推移方向，zb轴正方向垂直于液压支架顶梁向上，yb轴正方向指向液压支架一侧且与xb轴、zb轴组成右手正交坐标系。将九轴姿态传感器分别布置于液压支架顶梁、掩护梁、后连杆和底座4个构件上，如图2所示。九轴姿态传感器由三轴陀螺仪、三轴加速度计、三轴磁力计组成，分别采集所安装构件的相关数据，经解算得到所安装构件的俯仰角、横滚角和偏航角等位姿数据[16-18]，并用四元数法[19]表示。
3. 基于IGD−UKF的液压支架关键构件位姿数据滤波
在液压支架支护过程中，由于液压支架工作循环中各构件间相互运动，受摩擦、振动、噪声等扰动因素影响，若仅依靠在液压支架关键构件上九轴姿态传感器的输出数据来判断液压支架位姿，会出现较大的计算误差。因此采用IGD−UKF滤波算法[20]对液压支架关键构件的位姿数据进行滤波处理。IGD−UKF算法流程如图3所示，通过状态预测和观测预测得到变量和误差协方差矩阵，并计算增益矩阵，在目标函数的迭代过程中添加扰动因子，在状态更新中利用IGD优化UKF算法的噪声协方差，输出最优后验估计，提高复杂环境下状态估计的精度。
4. 基于自适应加权融合算法的液压支架横滚角和偏航角数据融合
液压支架随采煤机向前推进过程中，因截割量存在差别会导致底板出现凹凸不平。为使液压支架保持有效支护，即液压支架顶梁与顶板贴合、底座与底板贴合，在这种情况下可能会导致液压支架顶梁与底座发生不同角度的俯仰；但对于横滚角和偏航角来说，液压支架在支护中由于自身无法发生扭转，理论上液压支架顶梁和底座的横滚角和偏航角是相同的；但在实际工作过程中，因为振动、噪声等其他外界因素的影响，液压支架顶梁和底座的横滚角和偏航角也会出现偏差。为消除偏差，采用自适应加权融合算法对权重进行自适应分配，将多个传感器数据进行融合[21]。
自适应加权融合算法通过数学优化方法在约束条件下求解最优权重，确保融合结果满足总方差最小原则。自适应加权融合算法流程如图4所示。对经IGD−UKF滤波算法处理的液压支架顶梁和底座的横滚角和偏航角数据点进行方差求解，判断是否满足总方差最小原则，若满足则根据数据点方差大小对数据点权重进行自适应分配，以保证输出最终位姿融合结果具有较高的准确性。
最终获得的融合数据可表示为
$$ x' = \sum\limits_{i = 1}^j {{\omega _i}\bar{{x_i}} \left( k \right)} $$ (1) 式中：$ \bar{{x_i}} \left( k \right) $为传感器i采集k组数据得到的均值；$ {\omega _i} $为传感器i测得数据的权值；j为传感器个数。
融合数据的方差为
$$ {\bar \sigma ^2} = \frac{1}{j}\sum\limits_{i = 1}^j {{\omega _i}\mu _i^2} $$ (2) 式中$ \mu _i^{} $为传感器i的测量值与真实值间误差。
随着权重分配的动态调整，方差趋于减小，从而提高数据精度。
5. 液压支架支护位姿精确感知实验
搭建液压支架位姿感知实验平台，如图5所示。通过顶板控制油缸带动顶板运动，以模拟顶板对液压支架顶梁施加的压力，同时顶板和底板均可前后左右倾斜，以模拟开采中顶板和底板的变化；将HWT905−CAN九轴姿态传感器分别安装在液压支架顶梁、掩护梁、后连杆和底座上，并通过RS485−USB模块建立九轴姿态传感器与数据采集平台之间的通信；通过实验控制台控制液压支架与顶板运动，液压泵站为各液压部件的伸缩提供动力；九轴姿态传感器采集液压支架在顶梁低头和抬头、底座低头和抬头、液压支架左倾和右倾、液压支架左偏和右偏等情况下的位姿数据，采集频率为10 Hz。采集数据时对液压支架顶梁进行手动敲击，以模拟振动干扰。
![]()
图 5 液压支架位姿感知实验平台Figure 5. Experimental platform for position and posture perception of hydraulic supports5.1 IGD−UKF算法滤波效果
选取九轴姿态传感器采集并解算出的底座和顶梁各600组包含俯仰角、横滚角、偏航角的数据导入Matlab软件中，分别利用UKF算法和IGD−UKF算法进行滤波处理，效果如图6所示。可看出相对于UKF算法，IGD−UKF算法处理后的数据曲线波动趋于平缓，在抑制振荡、减小振幅上的效果明显。
![]()
图 6 UKF与IGD−UKF滤波效果对比Figure 6. Comparison of filtering effects between UKF and IGD-UKF5.2 自适应加权融合算法的融合效果
将经过IGD−UKF算法处理后的液压支架顶梁与底座的横滚角、偏航角数据导入自适应加权融合算法模型中，得到的融合结果与仅经过IGD−UKF算法处理后的液压支架顶梁与底座的横滚角、偏航角进行对比，如图7所示，可看出自适应加权融合算法抑制了顶梁与底座横滚角、俯仰角数据波动。利用Matlab软件标记出图7的最小误差与最大误差，通过提取各样本点数据的绝对误差并求解平均值，得到平均绝对误差，结果见表1，可看出自适应加权融合算法提升了液压支架位姿的感知精度。
![]()
图 7 液压支架顶梁与底座姿态角融合前后效果对比Figure 7. Comparison of effects before and after fusion of attitude angles of top beam and base of hydraulic supports表 1 自适应加权融合算法融合位姿数据误差结果Table 1. Error results of position and posture data fused by adaptive weighted fusion algorithm（°） 液压支架姿态角 最小误差 最大误差 平均绝对误差 偏航角 0.001 8 0.025 1 0.004 8 横滚角 0.001 4 0.028 1 0.004 7 6. 结论
1） 将九轴姿态传感器安装于液压支架顶梁、掩护梁、后连杆和底座进行数据采集并解算出所在构件的位姿数据，利用IGD−UKF算法对位姿数据进行滤波处理，抑制了数据振荡。通过自适应加权融合算法对滤波处理后不同构件的相同位姿数据进行融合，以减小振动等对液压支架位姿感知精度的干扰。
2） 搭建了液压支架位姿感知实验平台，开展了IGD−UKF算法滤波实验和自适应加权融合算法融合实验，得出液压支架偏航角误差为0.001 8～0.025 1°，平均绝对误差为0.004 8°，横滚角误差为0.001 4～0.028 1°，平均绝对误差为0.004 7°，实现了液压支架位姿的精确感知。
-
![]()
图 1 综采工作面三维激光扫描硬件
Figure 1. Hardware for three dimensional laser scanning in fully mechanized working face
![]()
图 4 综采工作面实时三维重建效果
Figure 4. Real time 3D reconstruction effect of fully mechanized working face
表 1 激光点云标记点坐标测量误差分析结果
Table 1 The error analysis result of marked points coordinate of laser point cloud
m 标记点 实测值 测量值1 测量值2 测量值3 1 (12.76，0.51，1.21) (12.77，0.42，1.27) (12.65，0.45，1.31) (12.69，0.49，1.31) 2 (30.30，0.81，1.31) (30.41，0.78，1.42) (30.55，0.89，1.50) (30.23，0.75，1.41) 3 (47.52， 1.51，1.88) (47.60， 1.32，1.83) (47.78， 1.41，1.71) (47.63， 1.41，1.92) 4 (65.77，2.52，1.23) (65.90，2.48，1.01) (65.88，2.31，1.24) (65.57，2.43，1.38) 5 (83.27，2.21，1.84) (83.42，2.41，1.69) (83.57，2.61，1.54) (83.97，2.41，1.74) 6 (101.98，2.75，1.83) (101.89，2.73，1.79) (101.92，2.77，1.79) (101.79，2.63，1.89) 7 (112.03，2.67，1.92) (112.04，2.69，1.96) (112.05，2.65，1.94) (112.06，2.68，1.95) 8 (129.53，2.69，1.90) (129.41，2.68，1.89) (129.61，2.67，1.87) (129.51，2.67，1.84) 绝对误差均值 0.18 0.19 0.14 
下载: 导出CSV
-
[1] 李森,王峰,刘帅,等. 综采工作面巡检机器人关键技术研究[J]. 煤炭科学技术,2020,48(7):218-225. DOI: 10.13199/j.cnki.cst.2020.07.022 LI Sen,WANG Feng,LIU Shuai,et al. Study on key technology of patrol robots for fully-mechanized mining face[J]. Coal Science and Technology,2020,48(7):218-225. DOI: 10.13199/j.cnki.cst.2020.07.022
[2] 王国法,范京道,徐亚军,等. 煤炭智能化开采关键技术创新进展与展望[J]. 工矿自动化,2018,44(2):5-12. DOI: 10.13272/j.issn.1671-251x.17307 WANG Guofa,FAN Jingdao,XU Yajun,et al. Innovation progress and prospect on key technologies of intelligent coal mining[J]. Industry and Mine Automation,2018,44(2):5-12. DOI: 10.13272/j.issn.1671-251x.17307
[3] 李首滨,李森,张守祥,等. 综采工作面智能感知与智能控制关键技术与应用[J]. 煤炭科学技术,2021,49(4):28-39. DOI: 10.13199/j.cnki.cst.2021.04.004 LI Shoubin,LI Sen,ZHANG Shouxiang,et al. Key technology and application of intelligent perception and intelligent control in fully mechanized mining face[J]. Coal Science and Technology,2021,49(4):28-39. DOI: 10.13199/j.cnki.cst.2021.04.004
[4] 黄曾华,王峰,张守祥. 智能化采煤系统架构及关键技术研究[J]. 煤炭学报,2020,45(6):1959-1972. DOI: 10.13225/j.cnki.jccs.zn20.0348 HUANG Zenghua,WANG Feng,ZHANG Shouxiang. Research on the architecture and key technologies of intelligent coal mining system[J]. Journal of China Coal Society,2020,45(6):1959-1972. DOI: 10.13225/j.cnki.jccs.zn20.0348
[5] 王海军,刘再斌,雷晓荣,等. 煤矿巷道三维激光扫描关键技术及工程实践[J]. 煤田地质与勘探,2022,50(1):109-117. DOI: 10.12363/issn.1001-1986.21.10.0589 WANG Haijun,LIU Zaibin,LEI Xiaorong,et al. Key technologies and engineering practice of 3D laser scanning in coal mine roadways[J]. Coal Geology & Exploration,2022,50(1):109-117. DOI: 10.12363/issn.1001-1986.21.10.0589
[6] 李首滨. 煤炭智能化无人开采的现状与展望[J]. 中国煤炭,2019,45(4):5-12. DOI: 10.3969/j.issn.1006-530X.2019.04.001 LI Shoubin. Present situation and prospect on intelligent unmanned mining at work face[J]. China Coal,2019,45(4):5-12. DOI: 10.3969/j.issn.1006-530X.2019.04.001
[7] CADENA C,CARLONE L,CARRILLO H,et al. Past,present,and future of simultaneous localization and mapping:toward the robust-perception age[J]. IEEE Transactions on Robotics,2016,32(6):1309-1332. DOI: 10.1109/TRO.2016.2624754
[8] BOSSE M,ZLOT R,FLICK P. Zebedee:design of a spring-mounted 3-D range sensor with application to mobile mapping[J]. IEEE Transactions on Robotics,2012,28(5):1104-1119. DOI: 10.1109/TRO.2012.2200990
[9] BOSSE M, ZLOT R. Continuous 3D scan-matching with a spinning 2D laser[C]. IEEE International Conference on Robotics and Automation, Kobe, 2009: 4312-4319.
[10] PALIERI M,MORRELL B,THAKUR A,et al. LOCUS:a multi-sensor lidar-centric solution for high-precision odometry and 3D mapping in real-time[J]. IEEE Robotics and Automation Letters,2021,6(2):421-428. DOI: 10.1109/LRA.2020.3044864
[11] KAESS M,RANGANATHAN A,DELLAERT F. iSAM:incremental smoothing and mapping[J]. IEEE Transactions on Robotics,2008,24(6):1365-1378. DOI: 10.1109/TRO.2008.2006706
[12] HACKEL T,WEGNER J D,SCHINDLER K. Fast semantic segmentation of 3d point clouds with strongly varying density[J]. ISPRS Annals of the Photogrammetry,Remote Sensing and Spatial Information Sciences,2016,3:177-184.
[13] CAI Zhipeng,CHIN T-J,BUSTOS A P,et al. Practical optimal registration of terrestrial LiDAR scan pairs[J]. ISPRS Journal of Photogrammetry and Remote Sensing,2019,147:118-131. DOI: 10.1016/j.isprsjprs.2018.11.016
[14] SEGAL A V, HAEHNEL D, THRUN S. Generalized-ICP[EB/OL]. [2022-05-10]. https://www.robots.ox.ac.uk/~avsegal/resources/papers/Generalized_ICP.pdf.
[15] RAMEZANI M, KHOSOUSSI K, CATT G, et al. Wildcat: online continuous-time 3D lidar-inertial SLAM[EB/OL]. [2022-05-10]. https://arxiv.org/abs/2205.12595.
[16] GIAMMARINO L D, ALOISE I, STACHNISS C, et al. Visual place recognition using LiDAR intensity information[EB/OL]. [2022-05-10]. https://arxiv.org/abs/2103.09605.
[17] MORDOHAI P, MEDIONI G. Tensor voting: a perceptual organization approach to computer vision and machine learning[M]. Cham: Springer, 2006.
[18] PAN Yue, XIAO Pengchuan, HE Yujie, et al. MULLS: versatile LiDAR SLAM via multi-metric linear least square[C]. IEEE International Conference on Robotics and Automation, Xi'an, 2021.
[19] KANATANI K. 3D rotations: parameter computation and lie algebra-based optimization[M]. Boca Raton: CRC Press, 2020.
[20] HUBER P J, RONCHETTI E M. Robust statistics[M]. 2nd Edition. New York: Wiley, 2009.
[21] GLADUNOVA O P,RODIONOV E D,SLAVSKII V V. Harmonic tensors on three-dimensional Lie groups with left-invariant Lorentz metric[J]. Doklady Mathematics,2009,80:755-758. DOI: 10.1134/S1064562409050329
[22] OHTSUKA T,FUJII H A. Real-time optimization algorithm for nonlinear receding-horizon control[J]. Automatica,1997,33(6):1147-1154. DOI: 10.1016/S0005-1098(97)00005-8
[23] KHOSOUSSI K, SUKHATME G S, HUANG S, et al. Designing sparse reliable pose-graph SLAM: a graph-theoretic approach[M]//GOLDBERG K, ABBEEL P, BEKRIS K, et al. Algorithmic foundations of robotics XII. Cham: Springer, 2020: 17-32.
计量
- 文章访问数:
- HTML全文浏览量:
- PDF下载量:
