Research on Visual Positioning of a Roadheader and Construction of an Environment Map

Abstract

The autonomous positioning of tunneling equipment is the key to intellectualization and robotization of a tunneling face. In this paper, a method based on simultaneous localization and mapping (SLAM) to estimate the body pose of a roadheader and build a navigation map of a roadway is presented. In terms of pose estimation, an RGB-D camera is used to collect images, and a pose calculation model of a roadheader is established based on random sample consensus (RANSAC) and iterative closest point (ICP); constructing a pose graph optimization model with closed-loop constraints. An iterative equation based on Levenberg–Marquadt is derived, which can achieve the optimal estimation of the body pose. In terms of mapping, LiDAR is used to experimentally construct the grid map based on open-source algorithms, such as Gmapping, Cartographer, Karto, and Hector. A point cloud map, octree map, and compound map are experimentally constructed based on the open-source library RTAB-MAP. By setting parameters, such as the expansion radius of an obstacle and the updating frequency of the map, a cost map for the navigation of a roadheader is established. Combined with algorithms, such as Dijskra and timed-elastic-band, simulation experiments show that the combination of octree map and cost map can support global path planning and local obstacle avoidance.

1. Introduction

With the rapid development of the industrial Internet, big data, and artificial intelligence, the intellectualization and robotization of equipment have become an inevitable trend in industrial development. Based on literature data from the Web of Science database, a cluster analysis of hotspots in intelligent coal mine research, from 2010 to 2020, obtained using VOSviewer software, is shown in Figure 1. It reveals that the in-depth integration of intelligent technology and the development of the coal industry is inevitable. For China’s coal industry, the degree of automation and intelligence of fully mechanized mining faces has been significantly improved over years of innovative development, and has been applied practically in many mining areas [1,2]. However, the intelligent development of fully mechanized excavation faces is still in a bottleneck period due to the poor working environments and complex excavation processes; the imbalance between mining and tunneling seriously restricts the safety and efficiency of coal mine production. Therefore, it is urgent to carry out research on the intelligence of tunneling equipment and systems for a fully mechanized excavation face [3,4].

As the main equipment for coal roadway driving, autonomous positioning of roadheaders is a key link to realize intelligent tunneling, and many scholars have done a great deal of original work in this area. An indoor global position system (IGPS)-based pose measurement method for roadheaders was proposed by Tao et al. [5]. The position coordinates of a receiver can be obtained from a single transmitter station, which realized the autonomous measurement of the body pose in a multi-point time-sharing manner. A remote-controlled system for roadheaders was developed by Roman et al. [6]. The system had built-in active lighting infrared sensors, which could assist in the positioning of a roadheader without visual control by workers. Du et al. [7] studied the pose measurement method of roadheaders based on machine vision. A laser target was fixed on a roadheader to receive the laser beam. After the imaging features were collected using a monocular camera, the roadheader pose can accurately be solved by solving the pose model and performing error compensation. Mao et al. [8] explored a multi-source fusion measurement method based on ultrasound, inertial navigation, a laser, and geomagnetic information. The method could overcome the shortcoming of excessive accumulated errors in a single inertial navigation measurement and effectively monitor the roadheader pose. Fu et al. [9,10] developed a set of roadheader pose measurement systems based on an ultra-wideband. A time-of-arrival model was built after the distance between the positioning node of a roadheader and ultra-wideband (UWB) positioning base station was measured. After the positioning algorithm was solved, the roadheader’s pose information could be obtained. Yang et al. [11] presented a vision measurement method for the pose of roadheader’s cutting head, based on infrared LEDs. A 16-point infrared target was fixed on the roadheader, and then the target image was collected and processed using a monocular camera; the pose of the cutting head could then be estimated. Simulation and experimental results showed that the accuracy of pose measurements could meet the requirements of underground roadway construction. Yan et al. [12] investigated a roadheader pose measurement method based on multi-source information, such as a gyroscope, a total station, and an inertial measurement system, and built a positioning model. Experimental results showed that this method could meet precision requirements. Xue et al. [13] put forward a positioning method for roadheaders based on binocular vision technology. By collecting information with a binocular vision sensor, the coordinate model between a roadheader and the roadway space was established, and then the pose was calculated. Simulations and experiments showed that the accuracy of the calculated positioning parameters was good and the established positioning model was reasonable. Yang et al. [14] proposed a vision measurement method for the pose of a roadheader body based on laser beams. The laser beams were used for the image feature, and a measurement system of roadheader body pose, based on monocular vision, was established. Experimental results showed that this method could effectively solve body pose information of roadheaders. A pose measurement method for a roadheader, combining total station and strapdown inertial navigation, was proposed by Zhang et al. [15]. Total station and inertial navigation technology were used to measure the pose of roadheaders, respectively, and the pose was determined using fusion data, which reduced the influence of dust in the mine.

The above research covers the main schemes of present pose measurement of roadheaders. Total station has high installation flexibility, but its light path is easily blocked, and its real-time performance is poor and cannot adapt to the dynamic working conditions of a roadheader. An inertial sensor is generally used as an auxiliary measurement method because of its large cumulative error. UWB technology has a strong real-time performance, but its signal is greatly affected by the complex electromagnetic environment of coal mines. IGPS technology has a large measurement range, but the cost of installation and construction is high. Visual measurements based on target imaging are susceptible to the influence of light beam scattering, and the installation of a light source emitter is limited by complicated roadway structures. The composition of the combined measurement system is complex, and the multi-source data are difficult to synchronize accurately in time and space; thus, the data reliability is not high. State perception, trajectory planning, and motion control are the three keys to realize intelligent and robotized roadheaders. As a core technology of autonomous positioning and navigation for mobile devices, simultaneous localization and mapping (SLAM) is widely used in mobile robot navigation, unmanned driving, and in other fields [16,17,18]. For instance, the open source library, Real-Time Appearance-Based Mapping (RTAB-MAP), provides a positioning and mapping solution that is independent of time and scale, which can be applied to various setups [19]. According to different sensor types, SLAM technology is mainly divided into laser-based and vision-based [20]. The point cloud obtained by LiDAR is sparse, a created map lacks semantic information, and the linearization error is large. In contrast, vision sensors can obtain far more status information than LiDARs. Furthermore, the RGB-D (red, green, blue, and depth) camera in a vision sensor is based on active ranging technology, and it has the advantages of not being affected by ambient lighting changes and textures compared with monocular and binocular cameras.

In summary, SLAM technology is a better solution to realize autonomous positioning and navigation of roadheaders and roadway mapping in a GPS-free roadway environment. At present, there are no reports on the application of visual SLAM technology to coal roadway roadheaders, and research on this technology is still lacking. Therefore, the positioning requirements of roadheaders and the objective conditions, such as tunnel environment and sensor attributes, are comprehensively considered, a method of body pose estimation for roadheaders, based on visual SLAM, is proposed, and multiple schemes of environmental map construction based on laser and visual SLAM are tested. These can provide information for pose correction, navigation of roadheaders, and three-dimensional reconstruction of roadways.

2. Positioning Foundation of the Roadheader and Modeling

2.1. Definition of Coordinate System and Pose

The positioning problem of a roadheader is transformed into a state estimation problem in this paper; that is, to solve the optimal pose. To accurately describe the pose of a roadheader in three-dimensional (3D) space, a series of coordinate systems need to be established, an earth coordinate system O_eX_eY_eZ_e, roadway coordinate system O_tX_tY_tZ_t, world coordinate system O_wX_wY_wZ_w, and camera coordinate system O_cX_cY_cZ_c. The O_e system is fixed on the earth, which is the basis of the design of the roadway; the O_t system is the absolute reference of the location in the roadway, and its position is determined by the structure of the roadway; the O_w system is the reference frame for the pose description of an airborne camera when it is moving, and the O_c and O_w systems are related to rotation and translation.

As the input device of visual SLAM, an RGB-D camera can be installed in the sensing system of a roadheader through intrinsic safety technology. Considering that the camera is connected to the roadheader body, the pose of the roadheader’s body is expressed by the pose of the RGB-D camera relative to the O_w system. If the absolute pose of the roadheader body in the O_t system or the O_e system is required, it can be obtained using Euclidean coordinate transformation. In addition, the pose of the cutting head of a cantilever roadheader can be solved according to the Denavit–Hartenberg model and forward kinematics in robotics [21]. In sum, the pose acquisition of a roadheader body in the O_w system is the basis for realizing other functions.

At the initial moment, it is stipulated that the O_c and O_w systems are coincident. The X_w axis of the O_w system is located in the horizontal plane and is perpendicular to the roadway midline, pointing to the right of the heading direction. The Y_w axis is located in the horizontal plane and is parallel to the roadway midline, pointing to the heading direction; the Z_w axis is perpendicular to the horizontal plane and points to the roadway roof. After the roadheader moves, the pose of the O_c system changes relative to the O_w system, and the pose of the roadheader body (camera) is described by axis deflection angles α, β, and γ, and axis displacements t_x, t_y, and t_z, as shown in Figure 2. α is defined as the included angle between the projection of the axis in the tunneling direction on the O_wX_wY_w plane and the Y_w axis, namely the yaw angle (Figure 2a). β is defined as the included angle between the projection of the axis in the tunneling direction on the O_wY_wZ_w plane and the Y_w axis, namely the pitch angle (Figure 2b). γ is defined as the included angle between the projection of the transverse axis of the roadheader on the O_wX_wZ_w plane and the X_w axis, namely the roll angle (Figure 2c); t_x, t_y, and t_z are, respectively, the displacement of the roadheader body in the direction of X_w, Y_w, and Z_w axes (Figure 2d–f).

According to rigid body kinematics, the motion of a roadheader can be decomposed into rotation and translation. The rotation of the body (camera) in the O_w system is described by rotation matrix R; R_cw means the rotation transformation from the O_w system to the O_c system. Specifically, point P_w(x_w, y_w, z_w) in the O_w system is rotated and transformed to point P_c(x_c, y_c, z_c) in the O_c system, which can be described as: (1) rotating at angle α along the Z_w axis, as shown in Equation (1); (2) rotating at angle β along the X_w axis, as shown in Equation (2); and (3) rotating at angle γ along the Y_w axis, as shown in Equation (3). R has a definite transformation relationship with the α, β, and γ, as shown in Equation (4) [22].

$$\left[\begin{array}{c}{x}_{\mathrm{c}}\\ y_{\mathrm{c}}\\ z_{\mathrm{c}}\end{array}\right]=\left[\begin{array}{ccc}\cos \alpha & -\sin \alpha & 0\\ \sin \alpha & \cos \alpha & 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{w}}\\ y_{\mathrm{w}}\\ z_{\mathrm{w}}\end{array}\right]$$

(1)

$$\left[\begin{array}{c}{x}_{\mathrm{c}}\\ y_{\mathrm{c}}\\ z_{\mathrm{c}}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \beta & \sin \beta \\ 0& -\sin \beta & \cos \beta \end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{w}}\\ y_{\mathrm{w}}\\ z_{\mathrm{w}}\end{array}\right]$$

(2)

$$\left[\begin{array}{c}{x}_{\mathrm{c}}\\ y_{\mathrm{c}}\\ z_{\mathrm{c}}\end{array}\right]=\left[\begin{array}{ccc}\cos \gamma & 0& -\sin \gamma \\ 0& 1& 0\\ \sin \gamma & 0& \cos \gamma \end{array}\right]\left[\begin{array}{c}{x}_{\mathrm{w}}\\ y_{\mathrm{w}}\\ z_{\mathrm{w}}\end{array}\right]$$

(3)

$$\begin{array}{l}{\mathit{R}}_{\mathrm{cw}}=\mathrm{Rot}\left({\mathrm{Z}}_{\mathrm{w}},\alpha \right)\cdot \mathrm{Rot}\left({\mathrm{X}}_{\mathrm{w}},\beta \right)\cdot \mathrm{Rot}\left({\mathrm{Y}}_{\mathrm{w}},\gamma \right)\\ =\left[\begin{array}{ccc}\cos \alpha \cos \gamma -\sin \alpha \sin \beta \sin \gamma & -\sin \alpha \cos \beta & \cos \alpha \sin \gamma +\sin \alpha \sin \beta \cos \gamma \\ \sin \alpha \cos \gamma +\cos \alpha \sin \beta \sin \gamma & \cos \alpha \cos \beta & \sin \alpha \sin \gamma -\cos \alpha \sin \beta \cos \gamma \\ -\cos \beta \sin \gamma & \sin \beta & \cos \beta \cos \gamma \end{array}\right]\end{array}$$

(4)

The translation of the body (camera) is described by 3D vector t, and t_cw represents the translation transformation from the O_w system to the O_c system, as shown in Equation (5). After homogeneous transformation, R_cw and t_cw can be unified into transformation matrix T_cw, as shown in Equation (6) [22]. In the absence of ambiguity, R_cw, t_cw, and T_cw below are referred to as R, t, and T without subscripts.

$${\mathit{t}}_{\mathrm{c}\mathrm{w}}={\left[\begin{array}{ccc}{t}_{\mathrm{x}}& t_{\mathrm{y}}& t_{\mathrm{z}}\end{array}\right]}^{\mathrm{T}}$$

(5)

$${\mathit{T}}_{\mathrm{c}\mathrm{w}}=\left[\begin{array}{cc}{\mathit{R}}_{\mathrm{c}\mathrm{w}}& {\mathit{t}}_{\mathrm{c}\mathrm{w}}\\ 0^{\mathrm{T}}& 1\end{array}\right]$$

(6)

2.2. SLAM Mathematical Model of Roadheader

In the moving process of the roadheader, the airborne camera observes the environmental signposts and estimates its own motion. As shown in Figure 3, the SLAM process of a roadheader equipped with a camera can be visualized, and the pose state and signposts are represented by nodes, and processes of camera movement and observation are represented by edges. Further, at time i, let the pose variable of the airborne camera be x_i, the position variable of waypoint is y_j, the motion input is u_i, and the observation data of airborne camera are z_i,j; then, the mathematical model of visual SLAM for the roadheader is shown as Equation (7).

$$\{\begin{array}{cc}{x}_i=f_{\mathrm{p}}(x_{i-1},u_i,w_i)& i\in \left(1,\dots ,m\right)\\ z_{i,j}=g_{\mathrm{o}}\left(y_j,x_i,v_{i,j}\right)& j\in \left(1,\dots ,n\right)\end{array}$$

(7)

where f_p and go are the process function and observation function, respectively; w_i and v_i,j are the motion noise and observation noise at time i, respectively.

3. Pose Estimation and Optimization of the Roadheader

3.1. Pose Estimation Scheme

The Pose estimation scheme of the roadheader (camera) is shown in Figure 4. Specifically, (1) the images of the roadway environment are collected by the airborne RGB-D camera and depth information is obtained; (2) feature extraction and matching are performed on the color images, and 3D matching pairs are formed after depth mapping; (3) combined with the random sample consensus (RANSAC) strategy and the iterative closest point (ICP) strategy, the pose of the roadheader is estimated; (4) the pose graph, based on keyframe and closed-loop constraints, is constructed, and the global pose optimization is carried out to obtain the optimal pose estimation of the roadheader.

3.2. Image Feature Extraction and Matching

Images collected by the RGB-D camera need feature calculations and data associations, and commonly used methods include the direct method, optical flow method, and feature point method [23]. Both the direct method and the optical flow method rely too heavily on the assumption of a constant gray level, so it is difficult for them to be directly used in a roadway environment; the feature point method, however, is less affected by motion and illumination and is relatively stable. In coal mine roadways, the forming quality of a section is affected by the real-time and accurate measurement of the roadheader pose. Compared with other features, ORB (oriented fast and rotated brief) features perform better in terms of real-time performance and accuracy, and ORB features are not sensitive to rotation and translation [24,25]. Consequently, ORB features are used for extraction and matching in this paper.

To verify the effect of ORB feature extraction and matching, two images from the public dataset, TUM-RGBD, were selected for the experiment. The experimental results are shown in Figure 5. Figure 5a shows the effect obtained after using the Brute-Force matcher and Figure 5b shows the effect after matching filtering, and the filtering basis is that the Hamming distance is less than twice the minimum distance.

3.3. Pose Solution

Based on image feature extraction and matching, an initial motion transform with a high confidence is iteratively solved using the RANSAC method, which aims to provide reliable initial values for pose estimation based on ICP. The process is as follows: (1) According to the matched 3D point set, the initial model is generated by randomly sampling some point pairs; (2) the model is evaluated according to the given threshold by calculating the point-pair error; and (3) through calculating the number of interior point pairs and the average value of the error, the model is updated and iterated based on the given threshold. After several iterations, a relatively optimal motion transformation can be obtained [26,27].

After depth mapping and RANSAC iteration, a set of 3D point sets with exact matching is obtained, i.e., P = {p₁, p₂, …, p_n}, Q = {q₁, q₂, …, q_n}, the body pose transformation between the i-th pair of 3D matching points in the point sets can be written as:

$$p_i=\mathit{R}{q}_i+\mathit{t}$$

(8)

According to the SLAM mathematical model of a roadheader (Equation (7)), there are noises in the motion equation and observation equation of the airborne camera; thus, the state estimation is uncertain. The inter-frame error function, e_(3D-3D)i, is positioned as [28]:

$$e_{\left(3\mathrm{D}\text{-}3\mathrm{D}\right)i}=p_i-\left(\mathit{R}{q}_i+\mathit{t}\right)$$

(9)

For n pairs of 3D matching points in the point set, the least squares expression of the objective function, F(R, t), can be expressed as [29,30]:

$$F\left(\mathit{R},\mathit{t}\right)=\arg \underset{\mathit{R},\mathit{t}}{\min }\frac{1}{2}{\displaystyle \sum _{i=1}^n{\Vert p_i-\left(\mathit{R}{q}_i+\mathit{t}\right)\Vert }^2}$$

(10)

To solve Equation (10), let the de-centroid coordinates of point sets P and Q be $p_i^{\prime }$ and $q_i^{\prime }$, respectively, that is:

$$\{\begin{array}{c}{p}_i^{\prime }=p_i-\frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{p}_i}\\ q_i^{\prime }=q_i-\frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{q}_i}\end{array}$$

(11)

Combining Equations (10) and (11), the de-centroid form can be obtained as:

$$F\left(\mathit{R},\mathit{t}\right)=\arg \underset{\mathit{R},\mathit{t}}{\min }\frac{1}{2}{\displaystyle \sum _{i=1}^n\left({\Vert p_i^{\prime }-\mathit{R}{q}_i^{\prime }\Vert }^2+{\Vert \frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{p}_i-}\left(\mathit{R}\cdot \frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{q}_i}+\mathit{t}\right)\Vert }^2\right)}$$

(12)

Equation (12) can be decomposed into two functions, f₁(R) and f₂(t), namely:

$$f_1\left(\mathit{R}\right)=\arg \underset{\mathit{R}}{\min }\frac{1}{2}{\displaystyle \sum _{i=1}^n{\Vert p_i^{\prime }-\mathit{R}{q}_i^{\prime }\Vert }^2}$$

(13)

$$f_2\left(\mathit{t}\right)=\arg \underset{\mathit{t}}{\min }\frac{1}{2}{\displaystyle \sum _{i=1}^n{\Vert \frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{p}_i-\mathit{R}\cdot \frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{q}_i}-t}\Vert }^2}$$

(14)

Further, Equation (13) can be expanded and simplified as:

$$g_1\left(\mathit{R}\right)=\arg \underset{\mathit{R}}{\min }\left[-\mathrm{t}\mathrm{r}\left(\mathit{R}{\displaystyle \sum _{i=1}^n{q}_i^{\prime }{\left(p_i^{\prime }\right)}^{\mathrm{T}}}\right)\right]$$

(15)

where, tr() is the trace operator of matrix.

To understand the R contained in Equation (15), a method based on singular value decomposition is adopted, that is, let [31,32]:

$${\displaystyle \sum _{i=1}^n{q}_i^{\prime }{\left(p_i^{\prime }\right)}^{\mathrm{T}}}=\mathit{U}\mathit{D}{\mathit{V}}^{\mathrm{T}}$$

(16)

where D, U, and V are all diagonal matrices, and matrix D is composed of singular values.

According to the principle of Cauchy–Schwarz’s inequality, when the objective function g₁(R) in Equation (15) takes the minimum value and DUV^T is full rank, R can be obtained, the expression is:

$$\mathit{R}=\mathit{U}{\mathit{V}}^{\mathrm{T}}$$

(17)

After R is solved, let f₂(t) in Equation (14) be zero, and t can be solved, i.e.:

$$\mathit{t}=\frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{p}_i-}\mathit{R}\cdot \frac{1}{\mathrm{n}}{\displaystyle \sum _{i=1}^n{q}_i}$$

(18)

So far, through the construction of the least squares problem of pose solution and singular value decomposition, the pose parameters, R and t, of the roadheader body can be solved.

3.4. Pose Optimization

Pose optimization must be carried out because of the accumulated errors in pose solutions. Waypoints are regarded as constraints of pose optimization rather than as optimization variables; the optimization model of a body pose graph is established by selecting the image key frame. Additionally, closed-loop constraints are added to carry out global pose optimization, so as to ensure the effectiveness and real-time performance of pose estimation, and then the deviation correction is completed in time.

3.4.1. Pose Graph Construction and Closed-Loop Detection

The selection principle of key frames is based on the evaluation of the distance between frames. After R and t are given different weights, the motion transformation index representing the distance between frames can be calculated. If the index is less than a given threshold, the current frame is determined to be a key frame.

After selecting the key frame, the pose graph of the motion transformation constraint can be constructed. To further modify the computed pose, closed-loop detection is needed and closed-loop constraints are added to the pose graph. The key of closed-loop detection lies in the judgment of the similarity between two frames of images, i.e., if the similarity between the current frame and the historical key frame exceeds a certain value of similarity between the current frame and the previous-moment key frame, and the consistency of space and time is satisfied, a closed loop can be determined.

Based on the SLAM motion model of the roadheader, shown in Figure 3, the pose graph optimization model, which eliminates the waypoint variables and adds closed-loop constraints, is shown in Figure 6. The figure shows two processes of motion transformation and closed-loop detection, T_i(i−1) is the motion transformation constraint between pose nodes x_i−1 and x_i, and T_(i+1)n is the closed-loop constraint between pose nodes x_i+1 and x_n.

3.4.2. Pose Graph Optimization

According to the pose graph optimization model shown in Figure 6, let the inter-frame transformation matrix obtained in the pose solution stage or closed-loop detection be T_ij, and the pose estimation error be e_ij; then, e_ij can be expressed as [33]:

$$e_{ij}=\ln {\left({\mathit{T}}_{ij}^{-1}{\mathit{T}}_i^{-1}{\mathit{T}}_j\right)}^{\vee }$$

(19)

where the sign ∨ means to transform the matrix into a vector.

To optimize the global pose graph, the least squares expression of the objective function, G(T), based on the error term is constructed, that is [34]:

$$G\left(\mathit{T}\right)=\arg \underset{T}{\min }{\displaystyle {\sum }_{ij}{e}_{ij}^{\mathit{T}}{\mathsf{\Sigma }}_{ij}^{-1}{e}_{ij}}$$

(20)

where Σ_ij is the covariance matrix of the pose measurement, which is used to represent the uncertainty of the measurement.

Equation (20) is nonlinear and is solved using an iterative method in this paper. Let ∆x be the iterative increment, x* be the initial value of the pose estimation, and then the Taylor expansion form of the error, e_ij, at x* can be expressed as:

$$e_{ij}\left(x^{\ast }+\Delta x\right)=e_{ij}+{\mathit{J}}_{ij}\Delta x$$

(21)

where J_ij is the Jacobian matrix of e_ij at x*.

Combined with Equations (20) and (21), Equation (22) can be obtained after approximate processing, that is:

$$\{\begin{array}{c}\mathit{H}\Delta x=-\mathit{b}\\ \mathit{H}={\displaystyle \sum {\mathit{J}}_{ij}^T}{\mathsf{\Sigma }}_{ij}^{-1}{\mathit{J}}_{ij}\\ \mathit{b}={\displaystyle {\sum }_{ij}{e}_{ij}^T{\mathsf{\Sigma }}_{ij}^{-1}{\mathit{J}}_{ij}}\end{array}$$

(22)

By introducing Lagrange multiplier ρ on the basis of Equation (22), the increment equation based on the Levenberg–Marquadt method is shown in Equation (23):

$$\left(\mathit{H}+\rho \mathit{{\rm I}}\right)\Delta x=-\mathit{b}$$

(23)

where I is the identity matrix.

After multiple iterations of Equation (23), when the termination condition of the iteration is satisfied, the optimized pose estimation can be obtained. Subsequently, a closed-loop control and deviation correction can be carried out for the traveling of the roadheader according to the optimized body pose, and trajectory planning of the cutting arm can be performed according to robotics to ensure the forming quality of the roadway section.

4. Map Construction and Test

4.1. Overview of Mapping Scheme

Although the acquired pose can provide a priori information for the moving of the roadheader, its navigation also depends on the environment map. Based on SLAM technology, a roadway map is established, which can help the roadheader identify obstacles and judge roadway edge shapes, and provide support for remote man–machine interactions, roadheader travel prediction, section forming control, and so on.

Common mapping schemes are shown in

Table 1. Common mapping schemes of laser/visual SLAM [19].

Mapping Algorithm	Input						Output
	Camera			Laser Radar		Odometer	Grid Map		Feature/Point Cloud Map
	Monocular	Binocular	RGB-D	2D	3D		2D	3D	Sparse	Dense
Gmapping [35]				▲		▲	▲
Cartographe [36]				▲	▲	▲	▲
Karto [37]				▲		▲	▲
Hector [38]				▲			▲
ORB-SLAM1 [39]	▲								▲
ORB-SLAM2 [40]	▲	▲	▲						▲
RTAB-MAP [19]		▲	▲	▲	▲	▲	▲	▲		▲

, including laser-based and vision-based schemes, and the real triangle symbol indicates that the algorithm supports the relevant interface. It can be seen from Table 1 that the RTAB-MAP algorithm can provide a variety of sensor input interfaces, and can output a dense 3D point cloud map, which is currently an excellent application-based mapping solution.

Comprehensively considering the underground environment of a coal mine as well as the existing conditions of a laboratory, a long corridor is adopted to simulate an underground roadway, and some mapping algorithms, listed in Table 1, are tested. The long corridor used for test, as shown in Figure 7, is as close as possible to the dimly lit, narrow and closed roadway environment.

4.2. Construction of Grid Map and Test

4.2.1. Principle of Laser Mapping

To construct an occupancy grid map based on laser SLAM, the key is to use the prior information to calculate the posterior probability of the map, and then identify the grid state, i.e., unknown, idle, or occupied [41,42]. The grid state schemas formed after two-dimensional (2D) LiDAR scanning are shown in Figure 8; O_gX_gY_g and O_bX_bY_b are the grid coordinate system and body coordinate system, respectively. The white area in the grid is idle, the black area in the grid is occupied, and the gray area in the grid is unknown, which is the target area for further exploration using SLAM technology.

The measured data after the laser beam is emitted by the 2D LiDAR is denoted as z^L, and is as shown in Equation (24). According to probability theory, if s = 1 that means that the grid is occupied, and if s = 0 that means that the grid is idle; then, the updated rule of state S for any grid is shown in Equation (25).

$$z^L={\left[\begin{array}{cc}d& \theta \end{array}\right]}^T$$

(24)

where d represents the distance of the measuring point, and θ represents the included angle between the laser beam and the body orientation.

$$S_i=S_{i-1}+\log \frac{\mathrm{p}\left(z|\mathrm{s}=1\right)}{\mathrm{p}\left(z|\mathrm{s}=0\right)}$$

(25)

where p() is the probability calculation expression.

The map constructed using laser SLAM can provide a priori information for navigation and obstacle avoidance of the roadheader, but it is not safe to move in the occupancy grid map using only Boolean values [42]. To realize global navigation of the roadheader and to avoid dynamic obstacles in real time, an obstacle map layer and expansion layer should be added, based on the static map layer constructed using SLAM; that is, a cost map for navigation should be constructed. The value principle of a cost map is shown in Figure 9, where each grid state is represented by a gray value from 0 to 255. The dashed line frame in the figure represents the contours of the roadheader, and the grid value of 254 indicates that the obstacles coincide with the center of the body, which will inevitably lead to a collision. The single dotted circle represents the inner tangent circle of the body contour line, the grid value in the inner tangent circle is 253, and collision is also inevitable in this area. The double-dot dotted circle represents the outer tangential circle of the body contour line, the grid value range in the outer tangential circle is 128–252, and collisions may occur in this area; the three-point scribbled circle represents the buffer area, and the grid value range is 1–128, which should be avoided. The grid value in the three-point double-dashed circle is 0, and the area is allowed free passage. The gray block with a grid value of 255 represents an unknown area, which needs to be updated according to the sensor information [43].

4.2.2. Mapping Test Based on Laser SLAM

Laser SLAM is divided into filtering-based and optimization-based, the Gmapping algorithm [35] in Table 1 belongs to the former, and the algorithms, Cartographer [36], Karto [37], and Hector [38], belong to the latter. In this paper, the four abovementioned laser SLAM algorithms are used to carry out mapping tests of the simulated roadway along the long corridor shown in Figure 7.

The test results of the four algorithms are shown in Figure 10. Figure 10a shows a map of simulated roadway, built based on the Gmapping algorithm, and the Gmapping algorithm relies on the Rao–Blackwellized particle filter, which takes laser data and odometer data as inputs; the map constructed based on the Cartographer algorithm is shown in Figure 10b. The Cartographer algorithm consists of two phases of local and global SLAM, with the first phase establishing and maintaining grid submaps, and the second phase eliminating cumulative errors and carrying out global optimization. Figure 10c shows a map built based on the Karto algorithm, which is based on the graph optimization theory, and uses the graph mean to represent the map, and the front-end and back-end are carried out by a single thread. The map constructed based on the Hector algorithm is shown in Figure 10d; the Hector algorithm only creates a grid map by matching laser data without the need for odometer data.

The Gmapping algorithm based on the filtering framework has a lower computational complexity, but it relies too heavily on odometer data. Although the Hector algorithm is not limited to the odometer, it requires a high laser frequency. In addition, the Cartographer algorithm is relatively resource intensive. Consequently, based on the 2D occupy grid map constructed using the Karto algorithm, the cost map for the navigation of the roadheader is constructed by introducing parameters, such as the expansion radius of obstacles and the updating frequency of map. The simulation test results are shown in Figure 11, the red arrow represents the body pose.

4.3. Construction of 3D Map and Test

4.3.1. Principle of Vision Mapping

According to Table 1, the map constructed using the ORB-SLAM1/2 algorithm is a sparse point cloud map, which cannot be used for navigation and obstacle avoidance of the roadheader. In addition, the algorithm can easily miss a target when the airborne camera rotates and it is sensitive to noise. Considering the complex environment of underground roadways and the requirements of tunneling work, the performance of running and being in real-time, the accuracy of algorithm should be given more priority. The RTAB-MAP framework provides a map construction scheme that is independent of scale and time. As shown in Figure 12 [19], its internal memory management is implemented through three memory nodes, i.e., WM (working memory), LTM (long-term memory), and STM (short-term memory). The weight of the registration point is updated in STM, and the closed-loop detection of the registration point is completed in WM. When the number of registration points exceeds a certain threshold, the registration point with the longest storage time is placed in WM from STM. To ensure real-time Bayesian estimation of the number of registration points, two functional nodes, namely fetch and transfer, are defined between WM and LTM during closed-loop detection in WM. When the closed loop is detected in the WM, the adjacent location points in the closed loop will be retrieved from the LTM and placed in the WM. When the processing time for closed-loop detection exceeds a certain threshold, the anchor point with the lowest weight is transferred to the LTM [19]. Based on the above memory management mechanism, the RTAB-MAP framework avoids the problem of having map data that are too large, which can meet the engineering needs of long-term, real-time, and online mapping in a large-scale environment.

The RTAB-MAP algorithm can build a dense point cloud map. Considering the disadvantage of a large amount of data in a dense point cloud, a portable stored octree map can be formed after octree compression [44,45], which can be used for stereoscopic navigation of the roadheader. The data structure of the octree is shown in Figure 13; each parent node contains eight child nodes, among which the leaf layer node corresponds to the map with the highest resolution. In addition, each node points to a cube voxel, and the probability of the n-th node corresponding to the voxel being occupied is calculated according to Bayesian theory, as shown in Equation (26) [45].

$$p(n|z_{1:t}^c)={\left[1+\frac{1-p(n|z_t^c)}{p(n|z_{1:t}^c)}\cdot \frac{1-p(n|z_{1:t-1}^c)}{p(n|z_{1:t-1}^c)}\cdot \frac{1-p(n)}{p(n)}\right]}^{-1}$$

(26)

where z^c represents the observation data of the airborne camera at the corresponding subscript time.

4.3.2. Mapping Test Based on Vision SLAM

Based on RTAB-MAP framework, the image data collected by RGB-D camera are input, the dense 3D point cloud map is constructed by mapping the test of the simulated roadway (shown in Figure 14), and the octree map is constructed via octree compression and is shown in Figure 15.

Octree map is convenient for navigation of the roadheader because of its portable characteristics. Based on the octree map shown in Figure 15, the multi-layer cost map for navigation is constructed by introducing parameters, such as the expansion radius of obstacles and the frequency of map updates, as shown in Figure 16. The red arrow in the figure represents the body pose, the darkly colored expansion area around the body is a local cost map, which is used for local path planning; the rest of the area is a global cost map, which is used for global path planning. In this test, the Dijskra algorithm is used for global path planning, and the red lines in the figure are the trajectory of the global path planning. The TEB (timed-elastic-band) algorithm is used for local path planning, and the blue line is the trajectory of local path planning.

As can be seen from Figure 16, when an obstacle is temporarily added in the navigation test, the dynamic obstacle expansion region can be marked by updating the cost map, and the obstacle avoidance based on local path planning is successfully realized.

4.3.3. Mapping Test Based on Fusion of Laser and Vision

Although the mapping accuracy of laser SLAM is high and the computational complexity is low, the map constructed is a sparse point cloud, lacks semantic information, and has large linearization errors. In contrast, the environmental information obtained by the camera is much more than that obtained by LiDAR, and the semantic information is rich. Moreover, the RGB-D camera has the advantage of not being affected by the change in ambient light and texture, but its mapping accuracy is affected by the cumulative error. Based on the above considerations, the two mapping schemes are complementary. Specifically, in the dynamic environment of a coal mine roadway with strong interference, the mapping scheme based on the laser and vision fusion has a stronger robustness.

The RTAB-MAP framework provides an interface between the camera and LiDAR [19], as shown in Figure 17. First, the laser and visual data are entered, then the multi-source information is initialized and aligned, and finally a map of the environment is output.

The mapping result of the simulated roadway, based on the fusion of laser and visual data, is shown in Figure 18. The map is in a composite form, and includes a dense 3D point cloud and a 2D occupied grid. Compared with Figure 14, it can be seen that the SLAM mapping scheme that integrates laser and vision has more advantages in detail processing of the point cloud, and the map is more refined. Additionally, the point cloud can be used for 3D reconstruction of the environment after gridding and texture mapping in the later stage, and the 2D occupancy grid map can be used for roadheader navigation after being converted into a cost map.

5. Conclusions

In this paper, according to the development requirements of intelligent and robotization of fully mechanized excavation faces, the method of estimating roadheader poses and constructing navigation maps by using vision and laser SLAM technology is proposed. The main conclusions are as follows:

(1)

The positioning method based on visual SLAM technology is proposed, the environment data are collected by an airborne RGB-D camera, and the RANSAC+ICP model is constructed to solve and realize the autonomous measurement of pose by establishing and solving the optimization model of pose graph with closed-loop constraints. The global consistent pose of the roadheader can be obtained, which can effectively solve the difficult positioning and orientation problem of roadheaders in a restricted space.

(2)

A series of open-source algorithms, such as Gmapping, Cartographer, Karto, Hector, and RTAB-MAP, are applied to construct simulated roadway maps, including 2D occupied grid, 3D point cloud, and octree, based on a static map created by SLAM. The cost map for roadheader navigation is further constructed by setting parameters, such as expansion radius of obstacles and the updating frequency of map. Furthermore, obstacle avoidance tests based on the Dijskra+TEB algorithm show that a combination of octree map and cost map can support global path planning and local dynamic obstacle avoidance.

(3)

Based on the open-source library RTAB-MAP framework, by fusing laser and vision data, a composite map composed of a dense 3D point cloud and a 2D occupancy grid is constructed, which could, not only be used for plane navigation of a roadheader, but also provide support for 3D reconstruction of an environment.