基于因子图优化融合视觉与惯导的掘进机组合定位方法

A combined positioning method for the roadheader based on factor graph optimization fusing vision and inertial navigation

  • 摘要: 煤矿井下感知退化环境限制了视觉测量技术的应用,同时掘进机长时低速的作业特性使惯导系统的误差随时间累积放大。针对上述问题,提出了一种基于因子图优化融合视觉与惯导的掘进机组合定位方法。首先,以人工设定红外标靶作为特征目标,构建基于红外标靶的掘进机位姿视觉测量系统,防爆红外双目相机采集红外标靶图像,通过特征点的提取并结合双目视觉位姿观测模型,得到掘进机的视觉位姿观测量。其次,安装在掘进机机身的防爆惯性导航仪实时获得掘进机的角速度和加速度,通过惯性预积分对惯导数据进行预处理,得到惯性预积分观测量。最后,利用因子图优化方法融合视觉与惯导数据,基于残差函数构建视觉因子和惯性因子,通过非线性优化迭代求解目标函数,得到掘进机位姿的最优估计。地面试验结果表明所提方法在x、y、z方向的位置绝对平均误差分别为0.004m、0.027m和0.005m,横滚角、航向角和俯仰角绝对平均误差分别为0.04°、0.78°和0.39°,整体优于KF和EKF融合结果。井下试验结果表明掘进机在x轴、y轴、z轴的平均误差分别为0.015m,0.015 m和0.014m,能够满足巷道掘进作业对定位精度与稳定性的要求。

     

    Abstract: The perception of degraded environment in coal mine limits the application of visual measurement technology. At the same time, the long-term and low-speed operation characteristics of roadheader make the error of inertial navigation system accumulate and enlarge with time. Aiming at the above problems, an integrated positioning method of roadheader based on factor graph optimization fusion vision and inertial navigation is proposed. Firstly, the infrared target is manually set as the feature target, and the visual pose measurement system of the roadheader based on the infrared target is constructed. The explosion-proof infrared binocular camera collects the infrared target image. Through the extraction of the feature points and the binocular vision pose observation model, the visual pose observation of the roadheader is obtained. Secondly, the explosion-proof inertial navigator installed on the body of the roadheader obtains the angular velocity and acceleration of the roadheader in real time. The inertial pre-integration is used to preprocess the inertial navigation data to obtain the inertial pre-integration observation. Finally, the factor graph optimization method is used to fuse the visual and inertial navigation data, and the visual factor and inertial factor are constructed based on the residual function. The objective function is solved by nonlinear optimization iteration to obtain the optimal estimation of the roadheader pose. The ground test results show that the absolute average position errors of the proposed method in x, y and z directions are 0.004m, 0.027m and 0.005m, respectively, and the absolute average errors of roll angle, heading angle and pitch angle are 0.04°, 0.78° and 0.39°, respectively, which are better than the fusion results of KF and EKF. The underground test results show that the average errors of the roadheader in the x-axis, y-axis and z-axis are 0.015m, 0.015m and 0.014m respectively, which can meet the requirements of the positioning accuracy and stability of the roadway excavation operation.

     

/

返回文章
返回