Vision and Inertial Navigation Combined-Based Pose Measurement Method of Cantilever Roadheader

Abstract

Pose measurement of coal mine excavation equipment is an important part of roadway excavation. However, in the underground mining roadway of coal mine, there are some influencing factors such as low illumination, high dust and interference from multiple equipment, which lead to the difficulty in the position and pose measurement of roadheader with low measurement accuracy and poor stability. A combination positioning method based on machine vision and optical fiber inertial navigation is proposed to realize the position and pose measurement of roadheader and improve the accuracy and stability of the position and pose measurement. The visual measurement model of arm roadheader is established, and the optical fiber inertial navigation technology and the spatial coordinate transformation method are used. Finally, the Kalman filter fusion algorithm is used to fuse the two kinds of data to get the accurate roadheader pose data, and the inertia is compensated and corrected. Underground coal mine experiments are designed to verify the performance of the proposed method. The results show that the positioning error of the roadheader body using this method is within 40 mm, which meets the positioning accuracy requirements of roadway construction. This method compensates for the shortcomings of low accuracy and poor reliability of single vision measurement, single inertial navigation measurement and single odometer measurement.

1. Introduction

With the continuous development of science and technology, domestic and foreign coal mining is developing towards a safe, green, low-carbon and efficient direction [1,2,3]. In recent years, the pace of intelligent development of coal mines in China has accelerated [4]. Due to the complex excavation process of underground roadway excavation face, the intelligent level of excavation equipment is lower than that of coal mining equipment [5]. Pose measurement and precise positioning of tunneling equipment is the key to automation and intelligence, and it is also the basis of intelligent perception, automatic driving, autonomous navigation and trajectory correction [6,7,8].

At present, the pose measurement methods for coal mine tunneling equipment include laser positioning, UWB(ultrawide band), INS, visual measurement and combined positioning. Based on the laser positioning method, the receiving target on the fuselage is used to receive the laser emitted by the laser emitter on the roof of the roadway, and the position information of the target is obtained using the model calculation [9,10]. However, the laser line is prone to the influence of coal dust and water mist interference and is prone to going off-target, resulting in no continuous measurement. Wireless positioning is also widely used in roadway excavation. UWB (ultrawide band) positioning technology requires multiple base stations to complete the pose measurement of the roadheader [11,12]. However, the roadway space is narrow, and the positioning base station is difficult to install in the best way, resulting in reduced positioning accuracy. At the same time, as the distance increases, the antenna sensitivity requirements increase [13,14]. The inertial navigation technology is not affected by the environment and has strong autonomy. It can provide a high-precision attitude angle, but the integral operation over time leads to a large position drift [15,16]. Visual measurement technology has been widely used in the detection of coal mine equipment [17,18,19]. The feature image is collected using an explosion-proof industrial camera, and the pose information of the roadheader is obtained by combining the calculation model. However, the phenomenon of high dust and low illumination in the roadway poses a challenge to visual feature extraction.

Due to the complex working environment of underground coal mines, there are limiting factors of low illumination, high dust and water mist, vibration noise, narrow space and multiple equipment interference, which lead to difficult posture detection for boring machines. It is difficult to adapt to the complex underground environment using a single measurement method. Chen Hongyue et al. [20] proposed a multi-sensor information fusion method for position and attitude measurement of roadheader, which is composed of a cross-laser emitter, multiple photoelectric sensors and an inclination sensor to realize position and attitude measurement of roadheader. Yan Changqing et al. [21] integrated the data of inertial system, total station and cylinder sensor to realize the position measurement and attitude detection of roadheader and establish the positioning model of roadheader and cutting machine. However, the laser emitted by the total station is relatively weak. When the dust concentration in the roadway is large, the laser line cannot penetrate, resulting in the total station unable to continue working. Cui Yuming et al. [22] used the method of inertial navigation and odometer fusion to locate the unmanned automatic vehicles in the underground coal mine, so as to realize the real-time positioning of the vehicles in the underground coal mine roadway. Li Zhenxing et al. [23] proposed a six-degree-of-freedom measurement system combining monocular vision and inclination sensor, which is applied to tunnel TBM. However, the coal mine roadheader has high flexibility and strong operability, so this method is difficult to apply to the pose detection of coal mine roadheader.

In the preliminary research of the team, we proposed the monocular visual positioning method of the coal mine roadheader with the base laser point–laser line characteristic target. The nonlinear distortion caused by the refraction of explosion-proof glass (above 15 mm) of the explosion-proof camera in an underground coal mine is studied, and the nonuniform fuzzy model and correction theory of the explosion-proof camera based on explosion-proof glass are established [24,25]. In order to improve the pose measurement accuracy of cantilever roadheader in complex environment and the discontinuity of pose output when the roadheader’s visual features are blocked for a short time, a combined positioning method based on the fusion of vision and fiber optic inertial navigation data is proposed. According to the point characteristics formed by the target of the three-laser pointing instrument targe, the visual measurement model of the roadheader’s pose is constructed, and the pose information of the roadheader in the roadway coordinate system is obtained by combining the spatial coordinate transformation. Then, using the Kalman filter algorithm, the optical fiber inertial navigation data and the pose data of the visual measurement system are fused to realize the precise positioning of the roadheader and provide data basis for the intelligent cutting of the roadheader.

2. Combined Positioning Scheme of Cantilever Roadheader Based on Vision and Inertial Navigation

2.1. System Scheme

The composition of the combined positioning system is shown in Figure 1, including the cantilever roadheader, the explosion-proof industrial camera, the three-laser pointing instrument target, the optical fiber inertial navigation and the explosion-proof computer installed in the navigation control box. The three-laser pointing instrument target was fixedly connected by the mounting frame to form a triangle and was installed on the roof of the roadway behind the roadheader to the direction of roadway excavation. The explosion-proof industrial camera and the roadheader body were rigidly connected to the rear, and the three-laser pointing instrument target was photographed. The fiber optic inertial navigation was installed in the explosion-proof electric control box to obtain the position of the roadheader relative to the geographic coordinate system. Explosion-proof calculation was fixed in the explosion-proof shell for image processing and data calculation.

2.2. Combined Positioning Principle

The combined positioning principle of cantilever roadheader is shown in Figure 2. The visual measurement system adopted the monocular vision measurement method and used the explosion-proof industrial camera fixed on the fuselage to collect three-laser pointing instrument target images. Using image preprocessing, morphological transformation and feature point extraction, the center coordinates of the three laser spots in the camera coordinate system were obtained. The pose measurement model of the roadheader based on the three-point feature was established. Combined with the external parameter calibration results of the mine total station, the absolute pose coordinates of the roadheader fuselage were obtained. The fiber optic inertial navigation obtained the angular velocity component and gravity acceleration component of the earth rotation collected by the gyroscope and accelerometer in real time through its own mathematical solution platform. Taking the inertial coordinate system as the reference coordinate system, the angular motion and linear motion of the carrier were measured in the inertial coordinate system, as well as the direction change of the gravity acceleration. After the initial calibration and alignment, the initial attitude matrix of the carrier coordinate system relative to the navigation coordinate system was accurately estimated. After completing the heading and attitude calculation, the three-axis velocity and position of the roadheader were obtained by integral operation. The Kalman filter algorithm was used to fuse the two data, and the precise pose information of the roadheader was output in real time to realize the precise positioning and orientation of the roadheader, and the inertial navigation was corrected by the fused pose of the roadheader.

3. Roadheader Pose Measurement Technology Based on Machine Vision

3.1. System Coordinate System

In order to realize the pose measurement of roadheader, this paper establishes the coordinate system shown in Figure 3.

• Navigation coordinates $o_n{x}_n{y}_n{z}_n$. With the center of gravity of the roadheader as the origin $o_n$ of the navigation coordinate system, the $x_n$ axis points to the east of the geography, the $y_n$ axis points to the north of the geography, and the $z_n$ axis forms a right-handed coordinate system with the $x_n$ and $y_n$ axes. Navigation coordinate system used to represent the results of inertial navigation coordinate system is also geographical coordinates.
• Carrier coordinate system $o_b{x}_b{y}_b{z}_b$. With the center of gravity of the roadheader as the origin $o_b$, the $x_b$ axis points to the left of the roadheader, the $y_b$ axis points to the front of the roadheader, and the $z_b$ axis points to the sky and forms a right-handed coordinate system with the $x_b$ and $y_b$ axes.
• The camera coordinate system $o_c{x}_c{y}_c{z}_c$. With the camera optical center as the origin $o_c$, the $x_c$ axis points to the right side of the camera, the $y_c$ axis points to the bottom of the camera, the $z_c$ axis points to the front of the camera.
• The roadway coordinate system $o_h{x}_h{y}_h{z}_h$. Taking the starting point of the roadway center line as the coordinate origin $o_h$, the $x_h$ axis points to the right side of the roadway excavation direction, the $y_h$ axis coincides with the roadway center line and points to the roadway excavation direction, and the $z_h$ axis and $x_h$, $y_h$ vertically point to the roadway roof direction. In order to better characterize the motion state of the roadheader, this paper converts the pose information of the roadheader into the roadway coordinate system.

3.2. Pose Measurement Method of Roadheader

3.2.1. Central Positioning of the Laser Spots

The traditional spot center extraction algorithms include the gray centroid method [26] and Hough transform method [27]. These algorithms can effectively extract the spot center in a specific environment, but the coal mine heading face has low illumination, high dust, water mist and nonstationary noise, accompanied by miner’s lamp, roadway lighting lamp, ground measurement laser pointer and other interference light sources. The environment is changeable and very different, and it is difficult to extract the laser spot center stably and accurately. Considering the influence of complex interference factors and the luminous color of laser pointing instrument, the color explosion-proof industrial camera was selected for image acquisition. After image processing, the center of laser spot was determined using Gaussian surface fitting. According to the HSV color space of the laser pointer spot, namely chromaticity H, saturation S, and brightness V, as color space components, by setting the parameter range, other color stray lights such as mine lights and lighting lights were effectively filtered to distinguish the pixel information of the laser spot. The closed operation was used to fill the area boundary of the spot on the image to the center of the spot, increase the boundary area so that the spot boundary continued and mark the spot position. The Gaussian surface fitting method was used to obtain the spot center of each region on the image, and then the residual stray light inconsistent with the laser spot characteristics were removed by calculating the distance and proportion between the spots. The laser spot center positioning process is shown in Figure 4.

The specific spot center positioning steps are as follows:

• After the color spot image collected by the camera is preprocessed by distortion correction, Gaussian filtering, etc., all pixel information of the image is traversed by setting the HSV parameters of the image. The range of H is [0, 180], the range of S is [0, 255], and the range of V is [0, 255]. All spot ${\displaystyle \sum h\left(i,j\right)}$ with chroma H is [60, 120], saturation S at [150, 255], and brightness V at [30, 180] can be obtained.
• Contour extraction of all spot ${\displaystyle \sum h\left(i,j\right)}$, remove the stray light contour greater than the laser spot contour, and the remaining area ${\displaystyle \sum g\left(i,j\right)}$ including the laser spot.
• The closed operation of all remaining spots makes the spot area continuous and smooth.
• The center position of all spots in the image area is obtained using the Gaussian surface fitting method.
• The distance D and the ratio S between each spot are obtained, and the spot coordinates satisfying the given distance range $\left(d_{\min }-d_{\max }\right)$ and the proportional range $\left(s_{\min }-s_{\max }\right)$ are saved, that is, the spot center coordinates $O_{p1}\left(i,j\right)$, $O_{p2}\left(i,j\right)$, and $O_{p3}\left(i,j\right)$ of the laser pointer. Figure 5 shows the effect picture of laser spot extracted under stray light background.

It can be seen from the figure that this method has outstanding advantages in extraction efficiency and can meet the needs of fast positioning of the laser spot center, thus ensuring the real-time calculation requirements of the roadheader body pose.

3.2.2. Positioning Model of Roadheader Based on Three Laser Points

As shown in Figure 6, the pose calculation model of the cantilever roadheader is established. The three feature points A, B and C in the known space constitute a spatial triangle, and the projections in the camera image plane are a, b, c, and O are the center of the camera optical axis.

There is a correspondence between the triangles in the figure: $\mathsf{\Delta }O{P}_1{P}_2-\mathsf{\Delta }O{p}_1{p}_2$, $\mathsf{\Delta }O{P}_1{P}_3-\mathsf{\Delta }O{p}_1{p}_3$, $\mathsf{\Delta }O{P}_2{P}_3-\mathsf{\Delta }O{p}_2{p}_3$, using the cosine theorem:

$$\left\{\begin{array}{l}O{P}_1{}^2+O{P}_2{}^2-2O{P}_1·O{P}_2·\cos \alpha =P_1{P}_2{}^2\\ O{P}_1{}^2+O{P}_3{}^2-2O{P}_1·O{P}_3·\cos \beta =P_1{P}_3{}^2\\ O{P}_2{}^2+O{P}_3{}^2-2O{P}_2·O{P}_3·\cos \gamma =P_2{P}_3{}^2\end{array}\right.$$

(1)

In order to simplify the calculation, divide by O on both sides of Formula (1) and assume:

$$\left\{\begin{array}{l}x=O{P}_1/O{P}_3\\ y=O{P}_2/O{P}_3\\ a=P_1{P}_2{}^2/O{P}_3{}^2\\ ab=P_1{P}_3{}^2/O{P}_3{}^2\\ ac=P_2{P}_3{}^2/O{P}_3{}^2\end{array}\right.$$

(2)

Formula (1) is simplified to obtain:

$$\left\{\begin{array}{l}\left(1-b\right)y^2-b{x}^2-y·\cos \beta +2bxy·\cos \alpha +1=0\\ \left(1-c\right)x^2-c{y}^2-x·\cos \gamma +2cxy·\cos \alpha +1=0\end{array}\right.$$

(3)

In the formula, $\cos \alpha$, $\cos \beta$ and $\cos \lambda$ can be calculated by the image coordinates of laser points, and b, c can be calculated by the coordinates $P_{wi}={\left[x_{wi},y_{wi},z_{wi}\right]}^T\left(i=1,2,3\right)$ of $P_1$, $P_2$ and $P_3$ space points in the roadway coordinate system obtained by the calibration of the mine total station. As unknowns, x and y change with the pose of the camera. Therefore, Formula (3) is solved by using the Wu elimination method. The equation can get four solutions $(x_g,y_g)$, $g=1,2,3,4$ at most, and four sets of coordinates $P_{ci}={\left[x_i^m,y_i^m,z_i^m\right]}^T\left(i=1,2,3;m=1,2,3,4\right)$ of $P_1$, $P_2$ and $P_3$ in camera coordinate system can be obtained.

According to the camera perspective projection model [28], the rotation matrix R and translation vector t between camera coordinate system and tunnel coordinate system can be expressed as:

$$P_{ci}=R{P}_{xi}+t$$

(4)

According to rotation matrix R and translation vector t, the position, heading angle, pitch angle and roll angle of the explosion-proof camera in the tunnel coordinate system are obtained. However, only one set of attitude solutions is the optimal solution. Seeking the optimal solution to transform it into the problem of minimizing the reprojection error, the function of minimizing the reprojection error is established as:

$$\sum _{i=1}^n\sum _{j=1}^m{‖p_i-\frac{1}{s}K\left(R_j{P}_i+t_i\right)‖}^2$$

(5)

In Formula (5), $p_i$ is the image coordinate of the $i$ laser point in the image coordinate system; s is the coefficient of transforming the projection pixel point coordinates into homogeneous coordinates; $K$ is the internal parameter matrix of the camera; $R_j$ and $t_j$ are the J-rotation matrix and translation matrix obtained using Formula (4). Finally, combined with the pose transformation relationship between the calibrated camera coordinate system and the body coordinate system, the pose information of the roadheader body in the tunnel coordinate system is obtained.

3.3. Positioning Method of Roadheader Based on Fiber Inertial Navigation

The fiber optic inertial navigation is fixedly installed on the roadheader body. When the roadheader works, the attitude, speed and position information are constantly updated. The internal mathematical solution platform of the fiber optic inertial navigation can obtain the pose information of the roadheader relative to the navigation coordinate system in real time according to the three-axis angular velocity and linear velocity.

3.3.1. Attitude Angle Update

The commonly used rotation matrix representation methods are Euler angle method, quaternion method, direction cosine matrix method and equivalent rotation vector method. In order to facilitate the calculation, this paper uses the quaternion method [29] to calculate the attitude angle of roadheader. Quaternions are defined as:

$$Q=q_0+q_{1}i+q_{2}j+q_{3}k$$

(6)

In the formula, $q_0,q_1,q_2,q_3$ are real numbers, $i,j,k$ are mutually orthogonal unit vectors.

When the attitude update period is $T_k=t_{k+1}-t_k$, the attitude update can be expressed as:

$$Q_o^b\left(t_{k+1}\right)=\frac{1}{2}{Q}_o^b\left(t_k\right)\otimes Q_o^b\left(T_k\right)$$

(7)

After obtaining the attitude transformation matrix, the attitude angle of the roadheader in the geographic coordinate system can be solved:

$$\begin{array}{c}\alpha =\arcsin \left[2\left(q_2{q}_3-q_0{q}_1\right)\right]\\ \beta =\arctan \left[-\frac{2\left(q_2{q}_3-q_0{q}_1\right)}{q_0^2-q_1^2-q_2^2+q_3^2}\right]\\ \gamma =\arctan \left[-\frac{2\left(q_1{q}_3+q_0{q}_2\right)}{q_0^2-q_1^2+q_2^2-q_3^2}\right]\end{array}$$

(8)

3.3.2. Velocity Update

The speed update equation of roadheader can be obtained by projecting the acceleration of roadheader into the navigation coordinate system:

$${\dot{V}}^n=C_o^n{f}^o-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times V^n+g^n$$

(9)

In Formula (9): The attitude transformation matrix $C_o^n$ from the carrier coordinate system to the geographic coordinate system.

$f^o$ is the measured value of the accelerometer, ${\omega }_{ie}^n$ is the rotation angular velocity of the earth coordinate system relative to the geocentric coordinate system; ${\omega }_{en}^n$ is the rotation angular velocity of the navigation coordinate system relative to the earth coordinate system; $g^n$ is the acceleration of gravity in the navigation coordinate system; $V^n=\left[V_E,V_N,V_Z\right]$, $V_E,V_N,V_Z$ are the speed of east, north and sky; $L$ and $h$ are geographic latitude and altitude respectively. Remove harmful acceleration $\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times V^n+g^n$ to obtain the acceleration of the roadheader.

3.3.3. Location Update

According to the velocity equation, the position differential equation of inertial navigation is obtained and then integrated to obtain the position of roadheader:

$$\left\{\begin{array}{l}{L}_{s+1}=L_s+{\displaystyle {\int }_{t_s}^{t_{s+1}}\frac{V_N}{R_M+h}dt}\hfill \\ {\lambda }_{s+1}={\lambda }_s+{\displaystyle {\int }_{t_s}^{t_{s+1}}\frac{V_E}{\left(R_N+h\right)\cos L}dt}\hfill \\ h_{s+1}=h_s+{\displaystyle {\int }_{t_s}^{t_{s+1}}{V}_{Z}dt}\hfill \end{array}\right.$$

(10)

In Formula (10), $L_{s+1},{\lambda }_{s+1},h_{s+1}$ are longitude, latitude and height of roadheader in geographic coordinate system at $t_{s+1}$; $L_s,{\lambda }_s,h_s$ are the longitude, latitude and height of the roadheader in the geographic coordinate system at $t_s$ time; $R_M,R_N$ are the radius of curvature of the earth’s circle and meridian circle, respectively. The pose of the roadheader obtained by the inertial navigation solution is the pose in the navigation coordinate system, and the pose representation of the roadheader is usually in the roadway coordinate system. The pose of the roadheader in the navigation coordinate system is expressed as $M_n$. Assuming that the transformation matrix between the navigation coordinate system and the roadway coordinate system is $M_{nh}$, the pose of the roadheader in the roadway coordinate system can be expressed as:

$$M_h=M_{nh}\cdot M_n$$

(11)

4. Combined Positioning Model of Roadheader

In this paper, the Kalman filter algorithm is used to fuse the fiber optic inertial navigation and visual measurement pose data to improve the measurement accuracy and stability of the system. According to the principle of the Kalman filter, the difference between the position and attitude of the inertial navigation and the position and attitude of the visual measurement is used as the observation value of the Kalman filter, and the output of the Kalman filter is used as the compensation of the inertial navigation. Finally, the position data of the roadheader is obtained.

4.1. Real-Time Time Registration of Inertial and Visual Measurement Data

The sampling frequency and calculation speed of the two measurement methods of visual measurement and inertial navigation measurement are different (the output frequency of inertial navigation measurement is greater than the output frequency of visual measurement). In data fusion, the time synchronization of the two measurement results should be considered. In this paper, Lagrange three-point interpolation is used to complete the registration of the two data in time. In order to reduce the error, the starting position of the interpolation point is selected to interpolate when the visual output is normal and stable. According to the interpolation principle, it is assumed that the sensor measurement data at $t_{i-1}$, $t_i$ and $t_{i+1}$ are $x_{i-1}$, $x_i$ and $x_{i+1}$, respectively. Because the movement distance of the roadheader is not large during the sensor sampling time, the $t_{i-1}$, $t_i$, and $t_{i+1}$ time can be regarded as equal interval time, so $t_i-t_{i-1}=T$. If interpolated at time $t=t_k+aT$, the measured value at time t can be calculated by using Lagrange three-point interpolation:

$$\begin{array}{c}\begin{array}{l}{x}_t=\frac{\left(t-t_i\right)\left(t-t_{i-1}\right)}{\left(t_{i-1}-t_i\right)\left(t_{i-1}-t_{i+1}\right)}{x}_{i-1}\\ +\frac{\left(t-t_{i-1}\right)\left(t-t_{i+1}\right)}{\left(t_i-t_{i-1}\right)\left(t_i-t_{i+1}\right)}{x}_i\end{array}\\ +\frac{\left(t-t_{i-1}\right)\left(t-t_i\right)}{\left(t_{i+1}-t_{i-1}\right)\left(t_{i+1}-t_i\right)}{x}_{i+1}\end{array}$$

(12)

4.2. Error Equation

The pose error of roadheader provided using visual measurement is mostly caused by laser spot extraction error and calibration measurement error. It is often difficult to model in practical use. Therefore, this paper only considers the error of inertial navigation.

Inertial attitude error is

$$\dot{\varphi }=\varphi \times {\omega }_{in}^n+\delta {\omega }_{in}^n-{\epsilon }^n$$

(13)

In formula: $\varphi$ is attitude error; ${\omega }_{in}^n$ is the rotation angular velocity of the navigation coordinate system relative to the geocentric coordinate system; $\delta {\omega }_{in}^n$ is the error of ${\omega }_{in}^n$; ${\epsilon }^n$ is the gyroscope bias error.

The velocity error equation is

$$\delta {\dot{V}}^n=-\varphi \times f^n-\left(2{\omega }_{ie}^n+{\omega }_{en}^n\right)\times \delta V^n-\left(2\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n\right)\times V^n+{\nabla }^n$$

(14)

where $\delta V^n$ is the velocity error; $f^n$ is the specific force measured by accelerometer in navigation coordinate system; $V^n$ is the speed of roadheader in navigation coordinate system obtained by inertial navigation; ${\nabla }^n$ is the bias error of accelerometer.

The position error equation is

$$\begin{array}{c}\delta \dot{L}=\frac{\delta V_N}{R_M+h}-\delta h\frac{V_N}{{\left(R_M+h\right)}^2}\\ \delta \dot{\lambda }=\frac{\delta V_E}{R_N+h}\sec L+\delta L\frac{V_E}{\left(R_N+h\right)}tanLsecL-\delta h\frac{V_E\sec L}{{\left(R_N+h\right)}^2}\\ \delta \dot{h}=\delta V_Z\end{array}$$

(15)

where $\delta L$, $\delta \lambda$, $\delta h$ are latitude, longitude, height error; $\delta V_N$, $\delta V_E$, $V_Z$ are north, east, sky speed error.

4.3. Design of Kalman Filter

The Kalman filter is used to fuse the inertial navigation data and the visual measurement data. The state vector of the system is expressed as

$$X={\left[{\varphi }_N,{\varphi }_E,{\varphi }_Z,\delta v_N,\delta v_E,\delta v_Z,\delta L,\delta \lambda ,\delta h,{\epsilon }_N,{\epsilon }_E,{\epsilon }_Z,{\nabla }_N,{\nabla }_E,{\nabla }_Z\right]}^T$$

(16)

In the formula: ${\varphi }_N$, ${\varphi }_E$, ${\varphi }_Z$ are attitude angle errors; $\delta v_N$, $\delta v_E$, $\delta v_Z$ are velocity errors; $\delta L$, $\delta \lambda$ and $\delta h$ represent the error of latitude, longitude and height respectively; ${\epsilon }_N$, ${\epsilon }_E$, ${\epsilon }_Z$ are three axial gyro drift; ${\nabla }_N$, ${\nabla }_E$, ${\nabla }_Z$ are three axial acceleration drift.

The difference between the position and attitude data of the roadheader calculated by visual measurement and inertial navigation is taken as the observation of the system:

$$Z=\left[\begin{array}{l}{\varphi }_{INS}-{\varphi }_C\\ P_{INS}-P_C\end{array}\right]$$

(17)

The system Kalman filter state equation and observation equation are

$$\left\{\begin{array}{l}{X}_k=A{X}_{k-1}+\Gamma W_{k-1}\\ Z_k=H{X}_k+V_k\end{array}\right.$$

(18)

where $X_k$ is the state quantity at time $k$; $A$ is the state transition matrix; $X_{k-1}$ is the state variable at time $k-1$; $\Gamma$ is the noise driving matrix; $W_k$ is the input noise at time $k-1$; $H$ is the measurement matrix; $Z_k$ is the observed value at time $k$; $V_k$ is the observed noise at time $V_k$.

With Kalman filter state and observation equations known, the state at the next moment can be predicted:

$$\begin{array}{l}{\widehat{X}}^{-}{}_k=A{\widehat{X}}^{}{}_{k-1}\\ P_k^{-}=A{P}_{K-1}{A}^T+Q\end{array}$$

(19)

In the formula: ${\widehat{X}}^{-}{}_k$ is the prior estimate (predicted value) at time $k$; ${\widehat{X}}^{-}{}_{k-1}$ is the posteriori estimate (optimal estimate) at $k-1$; $P_k^{-}$ is the prediction error covariance matrix at time $k$; $P_{K-1}$ is the posteriori-estimated covariance matrix at time $k-1$; $Q$ is the noise covariance matrix.

Update Kalman filter status:

$$\begin{array}{l}{K}_k=P_k^{-}{H}^T{\left(H{P}_k^{-}{H}^T+R\right)}^{-1}\\ {\widehat{X}}_k={\widehat{X}}_k^{-}+K_k\left(Z_k-H{\widehat{X}}_k^{-}\right)\\ P_k=\left(I-K_{k}H\right)P_k^{-}\end{array}$$

(20)

where $K_k$ is Kalman filter gain; $R$ is the observation noise covariance matrix; ${\widehat{X}}_k$ is the posteriori estimate at time $k$; $P_k$ is the updated mean square error; $I$ is the unit matrix.

5. Experimental Verification

In order to verify the function and performance of the combined positioning method of the cantilever roadheader, an experimental platform for visual measurement of the pose of the cantilever roadheader is built in the laboratory environment to verify the accuracy of the visual measurement results at different distances. Since the fiber-optic inertial navigation is installed in the navigation control box of the roadheader, the combined positioning method of the cantilever roadheader is experimentally verified under the actual working conditions of the coal mine.

5.1. Vision Positioning Experiment of Roadheader Based on Three-Laser Pointing Instrument

As shown in Figure 7, the experimental platform for pose measurement of cantilever roadheader was built. The experimental platform includes a mobile crawler robot model, three-laser pointing instrument target, explosion-proof industrial camera, explosion-proof computer, digital total station and smoke machine.

In the process of the experiment, the unified global coordinate system of the total station was first constructed (the X axis points to the right, the Y axis points to the up, and the Z axis points to the forward direction of the tracked robot), and the three-laser pointing instrument target was measured and calibrated. The tracked robot was moved in the corridor, and the three-laser pointing instrument target image was collected using the explosion-proof camera. The initial distance between the robot and the three-laser target was 10 m, and then it increased by 20 m, 30 m, 40 m, 50 m and 60 m, respectively. No less than 30 laser target images were collected at each position, and image processing and laser spot center positioning were completed in the computer. The pose of the roadheader body was calculated, and the real pose of the crawler robot measured by the total station was compared to verify the accuracy of the vision measurement system of the roadheader. The visual measurement results of the tracked robot and the real values measured by the total station during the movement of the corridor are shown in

Table 1. Visual pose measurement results at different distances.

Item	Distance/m	X/mm	Y/mm	Y/mm	Yaw/(°)	Roll/(°)	Pitch/(°)
Measured value	10	−566	549	10,009	1.59	3.00	4.71
	20	−475	546	19,993	0.88	2.84	4.41
	30	−537	566	30,033	−1.56	2.47	4.52
	40	−560	512	39,982	−0.18	2.73	4.13
	50	−466	544	50,024	0.9	2.03	4.26
	60	−545	486	59,977	−2.26	2.79	4.15
True value	10	−562	558	10,002	1.76	2.84	4.86
	20	−480	562	20,005	0.66	2.67	4.62
	30	−525	548	30,002	−1.23	2.61	4.75
	40	−537	545	40,010	0.24	2.58	4.51
	50	−439	513	50,007	0.56	1.89	4.71
	60	−516	516	60,002	−1.84	2.57	4.62

, and the experimental errors are shown in

Table 2. Visual pose measurement results at different distances.

Distance/m	X/mm	Y/mm	Y/mm	Yaw/(°)	Roll/(°)	Pitch/(°)
10	−4	−9	7	−0.17	0.16	−0.15
20	5	−16	−12	0.22	0.17	−0.21
30	−12	18	31	−0.33	−0.14	−0.23
40	−23	−33	−28	−0.42	0.15	−0.38
50	−27	27	17	0.34	0.14	−0.45
60	−29	−30	−25	−0.42	0.22	−0.47

.

The experimental results show that when the distance is within 60 m, the position errors in the X-axis, Y-axis and Z-axis directions are within 35 mm, and the attitude angle errors are within 0.5°.

5.2. Visual and Inertial Navigation Combined Positioning Experimental Verification

In order to verify the combined positioning method, the fuselage-combined positioning experiment was carried out in the excavation face of belt transport roadway in a mine. The experimental platform includes an EBZ260 cantilever roadheader, explosion-proof industrial camera, optical fiber inertial navigation, explosion-proof computer and three-laser pointing instrument target. The system hardware and installation are shown in Figure 8.

In the underground experiment, in order to ensure the function of the combined positioning method, according to the size of the roadway and the installation position of each equipment, the three-laser pointing instrument target was installed on the roof of the roadway and pointed to the direction of the roadway excavation. The explosion-proof camera was installed on the right side of the tail of the roadheader to ensure that the laser pointer could be photographed. The fiber inertial navigation was fixed on the special shock absorber and installed in the navigation explosion-proof control box. When the combined positioning system was running, the total station was used to track and measure the position of the roadheader. The output pose of the combined system was compared with the total station data to analyze the measurement error of the combined positioning system. Due to the space limitation of underground roadway and the interference of supporting facilities, the roadheader could not move a long distance. Therefore, in the experiment, the roadheader travelled a total of 20 m along the middle line of the roadway. At the beginning, the total station was used to measure once, and then the roadheader travelled forward for a certain distance. After that, the total station was used to measure, and a total of five distances were traveled. At the same time, at least 100 sets of pose data of each measurement point-combined positioning system were guaranteed. The measured value of the total station was taken as the real value of the position of the roadheader, and the output of the combination positioning system was taken as the measured value. The field measurement error was determined by comparing the measurement results of the two. Because the real attitude angle of the underground roadheader was difficult to measure, only the attitude angle information output by the combination positioning system is provided.

Table 3. Experimental results of combined positioning part of cantilever roadheader in under-ground coal mine.

Item	Group	X/mm	Y/mm	Z/mm	Yaw/(°)	Roll/(°)	Pitch/(°)
Measured value	1	−256	582,781	1563	−0.2	−0.907	0.835
	2	−152	586,805	1518	−1.231	−0.845	0.535
	3	59	592,343	1548	0.187	−0.859	−1.135
	4	239	597,603	1538	−0.279	−0.815	0.535
	5	176	602,928	1508	0.152	−0.807	1.535
True value	1	−231	582,758	1546	-	-	-
	2	−135	586,825	1539	-	-	-
	3	82	592,351	1536	-	-	-
	4	224	597,582	1529	-	-	-
	5	157	602,956	1532	-	-	-

is the real value and measured value of the underground experiment of the roadheader, and Figure 9 is the measurement error of the roadheader.

It can be seen from Figure 9 that the combined positioning error of the roadheader in the X-axis direction, the Y-axis direction and the Z-axis direction is within ±40 mm, which meets the accuracy requirements of roadway excavation. After nearly two months of underground industrial experiments, the system runs stably and can provide accurate and reliable pose data for intelligent control of roadheader.

6. Conclusions

Taking into account the many interference factors present in the complex working conditions of coal mines, as well as the poor adaptability and stability of a single positioning method, a combined positioning method for cantilever roadheaders was presented that is based upon vision and optical fiber inertial navigation. Based on the principle of monocular vision measurement, the pose measurement model of the cantilever roadheader was established by using the three-laser pointing instrument as the feature target, and the laser spot feature was extracted and located to realize the pose measurement of the roadheader. According to the working principle of optical fiber inertial navigation, the pose update equation and error equation were derived. The Kalman filter method was used to fuse the two data and compensate the inertial navigation position data to improve the positioning accuracy of the roadheader. At the same time, the positioning of the roadheader when the visual data shows short-term failure was solved, and the system stability is improved. In a coal mine, an industrial experiment was conducted to measure the pose of a cantilever roadheader. The experiments demonstrated that the position error of the roadheader obtained by the combined positioning method was within 40 mm. Additionally, the data output can be sustained and stable, meeting the accuracy requirements for intelligent roadheader cutting data and roadway excavation construction.