A Roadheader Positioning Method Based on Multi-Sensor Fusion

Abstract

In coal mines, accurate positioning is vital for roadheader equipment. However, most roadheaders use a standalone strapdown inertial navigation system (SINS) which faces challenges like error accumulation, drift, initial alignment needs, temperature sensitivity, and the demand for high-quality sensors. In this paper, a roadheader Visual–Inertial Odometry (VIO) system is proposed, combining SINS and stereo visual odometry to adjust to coal mine environments. Given the inherently dimly lit conditions of coal mines, our system includes an image-enhancement module to preprocess images, aiding in feature matching for stereo visual odometry. Additionally, a Kalman filter merges the positional data from SINS and stereo visual odometry. When tested against three other methods on the KITTI and EuRoC datasets, our approach showed notable precision on the EBZ160M-2 Roadheader, with attitude errors less than 0.2751° and position discrepancies within 0.0328 m, proving its advantages over SINS.

1. Introduction

In China, coal remains the predominant energy source, with rapid coal mine roadway excavation being essential for maintaining consistent and optimal production levels. As depicted in Figure 1, the roadheader is the principal machinery used for tunneling in coal mines, with its operation primarily being manual [1]. Such tunneling involves challenging work conditions that pose significant risks to the well-being and safety of frontline workers. Drawing from extensive experience, it is evident that the modernization of traditional excavation techniques necessitates the creation of advanced, efficient, reliable, and intelligent excavation equipment [2,3,4,5,6].

The strapdown inertial navigation system(SINS) is a self-sufficient navigation mechanism that functions autonomously, devoid of any external interventions [7]. Within the SINS framework, the gyroscope quantifies the angular acceleration, while the accelerometer gauges the carrier’s linear acceleration. Through the amalgamation of these linear and angular accelerations, one can ascertain the carrier’s relative velocity and angular velocity. Typically, the SINS finds its application in swift, short-duration, and expansive-range aircraft positioning and navigation. Conversely, roadheaders predominantly function in a modus operandi characterized by slow speeds, prolonged durations, and limited ranges [8]. At the same time, the SINS is prone to inherent challenges. Over time, its continuous integration of measurements leads to accumulating errors, significantly affecting position accuracy. Gyroscope drift can further distort orientation readings. The system’s initial alignment is crucial; any misalignment can introduce notable errors. Moreover, the SINS is sensitive to temperature changes, demanding either controlled operational environments or compensation mechanisms. Ensuring sustained accuracy requires the use of high-end, and often expensive, sensors like accelerometers and gyroscopes.

To deal with the limitations of the SINS, many research efforts have introduced Visual–Inertial Odometry (VIO) to enhance the two aspects of the odometry. VIO combines the strengths of both visual systems (like cameras) and inertial systems (like the SINS) to estimate a robot’s or vehicle’s path or change in position over time. This synergistically combines visual systems, such as cameras, with inertial systems, like the SINS, to refine position estimates. The visual component offers absolute position updates, mitigating the accumulated drift inherent in purely inertial systems. Cameras track environmental visual features, aiding in motion determination. While inertial data provide rapid position updates, the visual system, despite its typically lower frequency, validates these, ensuring enhanced navigation accuracy. Unlike the heavy reliance of SINS on initial alignment, VIO uses visual cues for better initialization, reducing susceptibility to alignment errors. Furthermore, incorporating cameras, which are generally more economical than high-end inertial sensors, makes VIO a cost-effective solution that doesn’t compromise on accuracy, capitalizing on the combined strengths of both systems [9].

Stereo Camera–Inertial VIO Odometry harnesses the depth perception of stereo cameras, which are paired with rapid position updates from inertial systems. The stereo cameras capture 3D environmental features, enhancing motion determination through depth perception. These visual inputs not only correct accumulated drift from the inertial system but also provide robust initialization, reducing alignment dependency. The inertial system, on the other hand, offers high-frequency position updates. Together, they ensure a richer, more accurate, and cost-effective navigation solution, leveraging the depth data from stereo vision and the dynamic feedback from inertial sensors [10,11]. However, in coal mines, low-quality images significantly affect SLAM (Simultaneous Localization and Mapping) feature matching in computer vision. Dark segments in these images obscure details essential for accurate feature detection, affecting tasks like pose estimation. In coal mines, coal dust exacerbates the issue by reducing image contrast and the signal-to-noise ratio, which are crucial for computer vision. This results in fewer detectable feature points, hindering effective feature extraction in SLAM. Consequently, the accuracy and reliability of SLAM-based systems can be compromised without clear, high-quality images.

In this study, we present a roadheader Visual–Inertial Odometry (VIO) tailored for coal mine environments, integrating the strapdown inertial navigation system(SINS) and stereo sensors. Given the low-light conditions inherent in coal mines, our approach incorporates an image-enhancement module designed to preprocess images, facilitating feature matching for stereo visual odometry under dim lighting. Furthermore, to synergize the position data from the SINS with stereo visual odometry, we employ a Kalman filter as the fusion strategy for these two modalities of positional information. Tests on the KITTI and EuRoC datasets compared our method to three alternatives. Experiments on the EBZ160M-2 Roadheader showed attitude errors below 0.2751° and position errors within 0.0328 m, demonstrating our system’s superior accuracy over the SINS.

2. Related Work

The positioning method of a roadheader can be divided into three categories: photoelectric navigation, inertial navigation, and multi-sensor fusion navigation [8]. Fu [12,13] designed an ultra-wideband (UWB) position and attitude perception system which realizes the position and attitude perception of the roadheader in narrow environments. Roman [14] designed an attitude perception system based on an active illumination infrared sensor which realizes the attitude perception of a roadheader under light-free conditions. Du [15] applied machine vision to the attitude perception of a roadheader, and built an attitude perception system for a roadheader. Based on the principle of indoor global positioning system (iGPS) positioning, Tao [16] designed a single-station, multi-point, and time-sharing attitude perception system which calculates the deflection angle, offset distance, and position of the roadheader by measuring the position of the receivers fixed at different positions on the roadheader’s body.

Tian [17] proposed an inertial navigation positioning method for a roadheader based on zero velocity correction, which can correct the measurement error. Shen [18] established a roadheader dynamics model based on cutting loads, and then proposed an error compensation method based on the vibration characteristics of the roadheader to reduce the cone and paddle error of the inertial navigation system.

The multi-sensor fusion positioning method can make full use of the advantages of various sensors and make up for the shortcomings of individual sensors. In order to solve the problem that the error of the SINS accumulates over time, and given that the single odometer limits position perception ability, Wang [19] introduced a differential odometer as an auxiliary positioning system on the basis of the SINS, which effectively improves the position and attitude perception ability of the inertial navigation system. Meanwhile, the development of machine vision and visual odometry provides new ideas for the positioning of a roadheader. Yang [20] proposed a roadway environment modeling and roadheader-positioning method based on self-coupling HectorSLAM, which realized the modeling of the roadway and the positioning of the roadheader. Based on the principle of monocular vision measurement, Wang [21] used the least-squares method to align SINS data with monocular camera data and fused both datasets using an extended Kalman filter to achieve a highly accurate perception of the roadheader’s position. The SLAM method based on an RGB-D camera, as adopted by Zhang [22], not only realizes the position perception of a roadheader, but also realizes the construction of the roadway map.

The above studies cover the main methods of position and attitude perception for a roadheader. Among them, positioning solutions based on machine vision are widely used for the positioning of roadheaders due to the advantages of non-contact measurement and rich and intuitive information. And the SINS is also suitable for the positioning of a roadheader because of the system’s advantages of adaptability, autonomy, and passivity.

Therefore, this paper proposes a positioning method for a roadheader based on stereo visual odometry and the SINS. The method improves the front end of the stereo visual odometry system by adding an image-enhancement module to suppress complex light sources, account for coal dust, and enhance the image’s details to improve the quality of images and system robustness, and, in the back-end part, a Kalman filter with a 15-dimensional state quantity is designed to fuse the position and attitude data of the SINS and stereo visual odometry.

3. Framework of Proposed Method

Figure 2 illustrates the structure of the proposed methodology. The system is segmented into three primary components: the position and attitude detection system SINS, another position and attitude detection system utilizing stereo visual odometry, and a data fusion mechanism based on the Kalman filter. As for the detail, the provided image portrays a schematic representation of a VIO system. At its core, two primary subsystems are distinctly illustrated: the SINS representing the inertial segment, and the stereo visual odometry system symbolizing the visual element. The SINS component is marked with various interconnected nodes, suggesting sensors such as gyroscopes and accelerometers. Opposite, the stereo visual odometry system showcases twin camera icons, indicative of its dual optical capture capability. Both subsystems converge towards a central module, likely indicating a data fusion mechanism.

The inertial component is driven by the SINS. This provides rapid, high-frequency positional updates. In contrast, the visual segment, underpinned by the stereo visual odometry system, captures 3D spatial information from the surroundings using its twin cameras. This captures the depth and relative motion within the environment, offering absolute position updates that can counteract the drift inherent in standalone inertial systems. When combined, these systems offer a robust navigation solution. The SINS gives a continuous stream of movement data, while the stereo visual odometry periodically refines and corrects this data using captured visual cues. Central to this integration is a data fusion mechanism, typically based on algorithms such as the Kalman filter, which synergistically processes data from both sources to deliver accurate and real-time positional and orientation information.

In this paper, the coordinate systems used to describe the relative relationship between a roadheader and an integrated positioning system are as follows.

• Geocentric Coordinate System: This system has its origin at the Earth’s center. The x-axis is directed towards the vernal equinox, while the y-axis aligns with the Earth’s rotational axis, pointing to the North Pole. The z-axis, in conjunction with the other axes, creates a right-handed coordinate system. Notably, the outputs from both the gyroscope and accelerometer rely on this system.
• Earth Coordinate System: Centered at the Earth’s core, the x and y axes of this system lie on the Earth’s equatorial plane. Specifically, the x-axis indicates the juncture of the prime meridian and the Equator, whereas the y-axis extends towards the North Pole in alignment with the Earth’s rotational motion.
• Navigation Coordinate System: With the roadheader’s center as its origin, the x-axis of this system is directed towards the geographic east. The y-axis aligns with the geographic north. Complementing the other two axes, the z-axis helps to constitute a right-handed system. This system serves as the referential framework for showcasing the integrated positioning system’s final results.
• Carrier Coordinate System: This system also uses the roadheader’s center as its starting point. Here, the x-axis looks to the right side, and the y-axis faces the roadheader’s front. Meanwhile, the z-axis rises upwards, and together, these axes form a right-handed coordinate system.
• Camera Coordinate System: Rooted at the left camera’s optical center, the x-axis of this system runs rightward along the camera’s baseline. Conversely, the y-axis descends, while the z-axis, combined with the other two, establishes a right-handed coordinate system. This orientation follows the camera’s optical path.

As shown in Figure 3, in the three-dimensional space, the attitude of the roadheader can be described by a Euler angle (yaw angle, roll angle, pitch angle), which is determined by the angular position relationship between the carrier coordinate system and the navigation coordinate system.

4. Position and Attitude Perception System Based on Improved Stereo Visual Odometry

4.1. Improved Stereo Visual Odometry Front End: Image Pre-Processing with Image Enhancement Module

Within coal mine tunnels, intricate lighting conditions, combined with dust accumulation, predominantly contribute to the degradation in image quality. Two salient characteristics of such low-quality images are the prevalence of numerous shadowed regions juxtaposed with sporadic overexposed areas. These darkened segments often blur image contours, resulting in the loss of intricate details and subpar visual appeal. The presence of coal dust exacerbates the situation by diminishing both image contrast and the signal-to-noise ratio, consequently reducing the number of image feature points, and thereby hampering effective feature extraction [23,24,25]. To combat this challenge, it becomes imperative to deploy specific image-enhancement techniques aimed at ameliorating the overall quality of these mine-based visuals.

Implementing the divide-and-conquer strategy [26,27,28], an image-enhancement module was devised to increase the quality of the images and thus bolster the accuracy of stereo visual odometry. After assessing the latest progress in image-enhancement methods [29,30], the current research approach was adopted with a consistent 424 × 240 pixel-size resolution. Results from side-by-side tests highlighted the effectiveness of chosen resizing, as depicted in Figure 4 [31,32,33]. Our enhancement approach, grounded on a weighted fusion strategy, predominantly encompasses two stages: the extraction of the reflection component and image dehazing. Separate models are established to accentuate each image, followed by the amalgamation of these enhanced sub-images through a straightforward, yet efficacious, weighted fusion mechanism. This consolidated approach facilitates the enhancement of previously degraded images.

4.1.1. Extraction of the Reflection Component of the Mine Image

As shown in Figure 5, according to the retinex [34] theory, the image information S observed by the observer is determined by the illumination component L and the reflection component R which reflects the details of object, and their relationship can be expressed as follows:

$$S=R·L.$$

(1)

In order to extract the reflection component, the above formula is usually converted to the logarithmic domain to make it more consistent with the form of the human eye’s perception [35].

$$\log R=\log S-\log L.$$

(2)

Then the illumination component can be estimated [36], and subsequently, the image signal S removes the light component to isolate the reflection component R.

$$\left\{\begin{array}{l}L=S\ast G\\ \log R=\log S-\log (S\ast G)\end{array}\right.$$

(3)

where $G$ is the Gaussian surround function and $\ast$ is a convolution operation.

4.1.2. Mine-Image Dehaze

When there is coal dust in the roadway, the image information S can be regarded as the superposition of the reflection component P and a scattering component Q [37].

$$S=P\omega +Q(1-\omega )$$

(4)

where $Q$ represents the scattering component, and $\omega$ is the weight coefficient. $P$ represents the reflection component of the position under sufficient and uniform illumination, carrying the texture information of the object.

Suppose that the mean pixel grayscale value of an image in a fixed small-sized window centered at pixel point $S(x,y)$ is $\mu (x,y)$ and the standard deviation is $\sigma (x,y)$. Within that window, the scattering component can be assumed to be a constant, so the variance ${\sigma }^2(x,y)$ satisfies:

$${\sigma }^2(x,y)={\omega }^2(x,y){\sigma }_P^2(x,y)$$

(5)

where ${\sigma }_P^2(x,y)$ is the standard deviation of the reflection component within that window. Assuming that $\omega (x,y)$ is 1 in the window where the maximum of ${\sigma }^2(x,y)$ is located, the weight coefficient $\omega (x,y)$ can be obtained as follows:

$$\omega (x,y)=\frac{\sigma (x,y)·\max (u(x,y))}{u(x,y)\max (\sigma (x,y))}.$$

(6)

According to the dark channel theory [38], the gray level at the minimum reflection component in each window is close to 0, that is:

$$\left\{\begin{array}{l}P{(x,y)}_{\min }=0\\ S{(x,y)}_{\min }=(1-\omega (x,y))Q(x,y)\end{array}\right..$$

(7)

Finally, the reflection component can be derived based on Formulas (4)–(7).

$$P(x,y)=(S(x,y)-P{(x,y)}_{\min })\frac{\max (\sigma (x,y))}{\sigma (x,y)}.$$

(8)

By performing above operation for each pixel of the image, the coal-dust-free image can be restored. And the final image is a linear weighted fusion of two enhanced images, R and P.

$$\left\{\begin{array}{l}{S}_{out}=W_{1}R+W_{2}P\\ W_1=\frac{{\mu }_1}{{\mu }_1+{\mu }_2}\\ W_1+W_2=1\end{array}\right.$$

(9)

where ${\mu }_1$ is the average gray value of component R and ${\mu }_2$ is the average gray value of component P.

4.1.3. Image Enhancement Experiment

The images were resized to 424 × 240 pixels and the enhancement results of each method were determined. We selected four datasets (DCIM [39], LIME [40], NPEA [41], and VV [42]) to verify the effectiveness of the proposed module. In the working file, we built a mine scene dataset, and the dataset contained four scenarios: underground work environment with personnel (Scenario 1), tunnel scene with multiple light sources (Scenario 2), scene with reflective stripes (Scenario 3), and coal mining equipment scene (Scenario 4).

Analyzing the data in

Table 1. Comparison of average NIQE for public datasets. Lower scores indicate higher image quality for image restoration. For each case, the best result is in red, whereas the second-best one is in blue.

Methods	DICM	LIME	NPEA	VV
LIESD	3.82	4.38	3.54	3.09
KinD	3.33	4.77	3.51	3.37
Mbllen	3.72	4.51	3.94	4.18
Enlight-GAN	3.57	3.72	4.11	2.58
Ours	3.47	3.64	3.73	3.28

, we used two metrics: the natural image quality evaluator (NIQE) [43] and the no-reference image quality metric for contrast distortion (NIQMC) [44]. The ‘KinD’ [45] method outperformed according to the NIQE metric with a high of 4.77 on the LIME dataset. In contrast, ‘Enlight-GAN’ [46] had the lowest NIQE score, 3.57, for the DICM. For the NIQMC metric, ‘Mbllen’ [47] scored the highest, with 4.18 on the VV dataset. Our proposed method showcased a consistent performance, with scores between 3.47 and 3.73 across all datasets. While not always leading, our method’s steady results across various datasets highlight its adaptability and reliability, making it a competitive choice against other image-enhancement methods in diverse scenarios.

In

Table 2. Comparison of average NIQMC for public datasets. Higher scores indicate higher image quality for image restoration. For each case, the best result is in red, whereas the second-best one is in blue.

Methods	DICM	LIME	NPEA	VV
Retinex_net	5.20	4.83	5.20	5.30
KinD	5.15	4.95	5.01	5.44
Mbllen	5.31	5.13	5.19	5.38
Enlight-GAN	5.13	4.88	5.15	5.45
Ours	5.47	5.41	5.13	5.49

, using the NIQMC metric, our method notably outperforms most of the competing approaches across various datasets. Specifically, it achieves the highest scores on DICM (5.47), LIME (5.41), and VV (5.49), indicating superior image restoration quality in these scenarios. While the ‘KinD’ method scored marginally higher on the NPEA dataset (5.01) compared to ours (5.13), our method consistently retained better overall naturalness in the images across the majority of tested datasets.

As is shown in Figure 6, the ‘Retinex-Net’ [48] introduces noise and artifacts in dark areas, leading to noticeable color distortions and loss of contrast. On the other hand, the ‘KinD’ significantly improves the quality of the enhancement, but it tends to oversaturate the bright areas in the enhanced image. ‘Mbllen’ exhibits poorer contrast-adjustment results in both dark and high-intensity regions. ‘Enlight-GAN’ is capable of restoring low-quality images to high-quality ones but fails to remove noise.

As is shown in Figure 7, the proposed module can maintain higher quality, minimizing distortion and retaining intricate details. The naturalness of the image is well maintained after enhancement, and, meanwhile, the effects of complex light sources and coal dust are eliminated. In addition, the module demonstrates the ability to balance the relationship between light sources and dark environments, reducing the impact of dust on image-enhancement results. It also achieves a balance between contrast and saturation, resulting in more realistic colors, compared to other methods.

In the realm of visual odometry, feature matching quality critically influences pose-estimation accuracy. Through our study, we probed the impact of image enhancement on feature-point extraction and matching efficacy. Utilizing the Scale-Invariant Feature Transform (SIFT) for feature point extraction and matching, our data was derived from a meticulously chosen video sequence of 1000 frames shot with a stereo camera in a mining roadway. Figure 8 displays histograms illustrating the contrast in correctly matched feature points for raw versus enhanced images. Here, ‘correctly matched’ refers to points retained after employing the RANSAC algorithm to eliminate initial matching errors. Notably, raw images average near 150 correctly matched points, while enhanced ones rise significantly, to about 180. This improvement underscores the pivotal role of image enhancement in optimizing feature-point extraction and matching.

Figure 9 provides an insightful visual representation of a frame extracted from the referenced video sequence for a more granulated analysis. In the untreated image, the concentration of extracted feature points is markedly denser than its counterpart in the enhanced rendition. Such density indicates that, in the absence of image enhancement, the motion-estimation predominantly identifies feature points within a confined depth spectrum. Conversely, when augmented through image enhancement, the motion estimation can discern feature points across an extended depth continuum. An important corollary to note is that a heightened density of feature points often predicates a heightened susceptibility to errors during the matching phase. Consequently, the introduction of the image-enhancement module stands out as a substantial advancement, elevating the precision and robustness of feature matching, particularly in the challenging visual environments of underground coal mines.

4.2. Stereo Visual Odometry Back End: Position and Attitude Perception for a Roadheader Based on Motion Estimation

Considering the inherent challenges in simultaneously estimating position and attitude using a monocular camera, and the superior measurement accuracy and broader measurement range of a stereo camera, this method employs a stereo camera to estimate the position and attitude of the roadheader.

As is shown in Figure 10, in the initialization phase, the feature points of the image captured by the left-eye camera are first extracted and then matched with the feature points of the image captured by the right-eye camera. After calibration, the stereo camera can obtain its baseline b and focal length f. According to the similar-triangle principle, the depth information of matching points can be solved by the following equation:

$$\left\{\begin{array}{l}{z}_c=\frac{fb}{d}\\ d=x_l-x_r\end{array}\right.$$

(10)

where $x_l$ and $x_r$ are the coordinates of the feature point p in the left and right camera coordinate systems, respectively. With the scale information, the position $(x_c,y_c,z_c)$ of the feature point in the camera coordinate system can be derived based on the pixel coordinates $(u,v)$ of the feature point in the left-eye camera.

$$\left[\begin{array}{l}u\\ v\\ 1\end{array}\right]=\frac{1}{z_c}\mathit{K}\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]$$

(11)

where $\mathit{K}$ is the internal parameter matrix of the camera. Since the camera is fixed on the roadheader’s body, it is necessary to clarify the conversion relationship between the navigation coordinate system and the camera coordinate system in order to solve for the position and attitude of the roadheader. At a certain time, the relationship between the coordinates of feature points in the navigation coordinate system and camera coordinate system can be expressed as follows:

$$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\\ 1\end{array}\right]={\mathit{T}}_k\left[\begin{array}{c}{x}_n\\ y_n\\ z_n\\ 1\end{array}\right]$$

(12)

where ${\mathit{T}}_k=\left[\begin{array}{cc}{\mathit{R}}_k& {\mathit{t}}_k\\ 0& 1\end{array}\right]$ is the external parameter matrix of the camera. The position and attitude of the roadheader can be determined based on the external reference matrix ${\mathit{T}}_k$:

$${\mathit{\phi }}_C^n=\left[\begin{array}{c}\gamma \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{c}\arctan \left(-\frac{R_{k 13}}{R_{k 33}}\right)\\ \arcsin \left(R_{k 23}\right)\\ \arctan \left(\frac{R_{k 21}}{R_{k 22}}\right)\end{array}\right]$$

(13)

where $R_{k\begin{array}{l}\hfill \end{array}ij}$ is the element of the i-th row and j-th column of the matrix ${\mathit{R}}_k$. The position of the roadheader in the navigation coordinate system can be expressed as follows:

$${\mathit{P}}_c^n=-{\mathit{R}}_k^{-1}\mathit{t}.$$

(14)

The external parameter matrix of the camera is different from the internal parameter matrix, which is unchanged after calibration. However, the external parameter matrix changes as the camera moves, so the essence of position and attitude perception is the update of the external parameter matrix ${\mathit{T}}_k$.

After the stereo camera captures the image of the most recent frame, the feature points in the previous frame are tracked using the feature-point matching method to establish correspondences between the feature points.

Descriptors [49] can describe the Hamming distance between feature points where the Hamming distance of correctly matched feature points is smaller than that of mismatches. When an object is obstructed, there will be a large number of mismatched points in the occluded area of the image, but correctly matched points still exist in the unobstructed area. Therefore, by setting a threshold, the portion of the feature points with a Hamming distance greater than the threshold are judged to be mismatched points and eliminated, while the remaining available feature points can still be used for pose estimation.

For matched point sets $P=\{p_1,\cdots ,p_n\}$ and $P^{\prime }=\{p_1{}^{\prime },\cdots ,p_n{}^{\prime }\}$, the following least-squares problem needs to be constructed to find the ${\mathit{R}}_{k+1,k}$ and ${\mathit{t}}_{k+1,k}$ that make $J$ reach the minimum.

$$J=\underset{R,t}{\min }\frac{1}{2}{\displaystyle \sum _{i=1}^n\left|\right|p_i-{\mathit{R}}_{k+1,k}{p}_i{}^{\prime }-{\mathit{t}}_{k+1,k}|{|}_2^2}.$$

(15)

After getting ${\mathit{R}}_{k+1,k}$ and ${\mathit{t}}_{k+1,k}$, the external parameter matrix is updated by the following formula. The position and attitude of the roadheader at the current moment can be solved by Formulas (13) and (14).

$${\mathit{T}}_{k+1}={\mathit{T}}_{k+1,k}{\mathit{T}}_k.$$

(16)

5. Position and Attitude Perception for a Roadheader Based on the SINS

5.1. Update of Roadheader Attitude

Given the continual movement of the roadheader, its position and orientation constantly vary. Consequently, the integrated positioning system is responsible for ascertaining the roadheader’s precise location and orientation within the navigation coordinate system. Therefore, it is necessary to project the data of the SINS in the carrier coordinate system to the navigation coordinate system through the attitude transformation matrix ${\mathit{C}}_b^n$.

The spatial angular position relationship between two coordinate systems can be viewed as the rotation of a rigid body around a fixed point, and this rotation relationship can be expressed by the normalized quaternion $Q=q_0+q_{1}i+q_{2}j+q_{3}k$ ($q_0$, $q_1$, $q_2$, $q_3$ are real numbers, $i$, $j$, $k$ are imaginary units).

$${\mathit{C}}_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

(17)

where $C_{ij}$ is the element of the i-th row and j-th column of the transfer matrix ${\mathit{C}}_b^n$. The crux of updating the attitude transformation matrix lies in the quaternion update, accomplished through the equivalent rotation vector method [50].

$$Q_{k+1}=Q_k\otimes Q_{k,k+1}$$

(18)

where $Q_k$ is the attitude quaternion at time k, $\otimes$ is the multiplication of quaternions, and $Q_{k+1,k}$ is the attitude change quaternion from time k to k + 1,which can be calculated by the rotational angular velocity of the roadheader, as measured by gyroscope.

Once the attitude transformation matrix is acquired, the orientation angle of the roadheader in the navigation coordinate system can be determined using the following formula:

$${\mathit{\phi }}_I^n=\left[\begin{array}{c}\gamma \\ \theta \\ \psi \end{array}\right]=\left[\begin{array}{c}\arctan \left(-\frac{C_{31}}{C_{33}}\right)\\ \arcsin \left(C_{32}\right)\\ \arctan \left(\frac{C_{12}}{C_{22}}\right)\end{array}\right].$$

(19)

5.2. Update of Roadheader Velocity and Position

The differential equation for the velocity from the SINS in the navigation coordinate system can be expressed as follows [51]:

$${\dot{\mathit{V}}}_I^n={\mathit{C}}_b^n{\mathit{f}}^b-(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n)\times {\mathit{V}}_I^n+{\mathit{g}}^n$$

(20)

where ${\mathit{V}}_I^n$ is the velocity of the roadheader within the navigation coordinate system, as obtained by the SINS; ${\mathit{f}}^b$ is the specific force measured by the accelerometer in the carrier coordinate system; ${\mathit{\omega }}_{ie}^n$ is the rotational angular velocity of the earth coordinate system relative to the geocentric coordinate system; ${\mathit{\omega }}_{en}^n$ is the rotational angular velocity of the navigation coordinate system relative to the earth coordinate system; ${\mathit{g}}^n$ is gravitational acceleration.

The differential equation for the position of the roadheader can be expressed as follows [52]:

$${\dot{\mathit{P}}}_I^n=\left[\begin{array}{c}{\dot{L}}_I\\ {\dot{\lambda }}_I\\ {\dot{h}}_I\end{array}\right]=\left[\begin{array}{ccc}0& \frac{1}{R_M+h_I}& 0\\ \frac{\sec L_I}{R_N+h_I}& 0& 0\\ 0& 0& 1\end{array}\right]{\mathit{V}}_I^n$$

(21)

where ${\mathit{P}}_I^n$ is the geographic coordinate of the roadheader obtained by the SINS, and L_I, λ_I, h_I are latitude, longitude, and height, respectively. $R_M$ and $R_N$ are the curvature radius of the meridian circle and the curvature radius of the unit circle, respectively.

5.3. Error Equation of the SINS

The velocity-error equation of the SINS is expressed as follows [53]:

$$\Delta {\dot{\mathit{V}}}_I^n=-{\mathit{\varphi }}_I^n\times ({\mathit{C}}_b^n{\mathit{f}}^b)+{\mathit{C}}_b^n\left(\Delta {\mathit{K}}_A+\Delta {\mathit{\alpha }}_A\right){\mathit{f}}^b+\Delta {\mathit{V}}_I^n\times \left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)+{\mathit{V}}_I^n\times \left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)+{\nabla }^n.$$

(22)

$\Delta {\mathit{V}}_I^n$ is velocity error; ${\mathit{\varphi }}_I^n$ is attitude error, $\Delta {\mathit{K}}_A$ and $\Delta {\mathit{\alpha }}_A$ are respectively the scale factor error and installation angle error of the accelerometer; ${\nabla }^n$ is the bias error of the accelerometer. The position equation of the SINS can be expressed as follows:

$$\Delta {\dot{\mathit{P}}}_I^n=\left[\begin{array}{c}\Delta {\dot{L}}_I\\ \Delta {\dot{\lambda }}_I\\ \Delta {\dot{h}}_I\end{array}\right]=\left[\begin{array}{c}\frac{\Delta V_N}{R_M+h_I}-\Delta h_I\frac{V_N}{{\left(R_M+h_I\right)}^2}\\ \frac{\Delta V_E}{R_N+h_I}\sec L_I+\Delta L_I\frac{V_E}{R_N+h_I}\tan L_I\sec L_I-\Delta h_I\frac{V_E\sec L_I}{{\left(R_N+h_I\right)}^2}\\ \Delta V_U\end{array}\right]$$

(23)

where $\Delta L_I$, $\Delta {\lambda }_I$, and $\Delta h_I$ are latitude, longitude, and height error, respectively; $\Delta V_N$, $\Delta V_E$, and $\Delta V_U$ are north, east, and sky velocity errors, respectively. The attitude error equation can be written as follows:

$${\dot{\mathit{\varphi }}}_I^n={\mathit{\varphi }}_I^n\times {\mathit{\omega }}_{in}^n+\Delta {\mathit{\omega }}_{in}^n-{\mathit{C}}_b^n\left(\Delta {\mathit{K}}_G+\Delta {\mathit{\alpha }}_G\right){\mathit{\omega }}_{ib}^b-{\mathit{\epsilon }}^n$$

(24)

where ${\mathit{\omega }}_{in}^n$ is the rotational angular velocity of the navigation coordinate system relative to the geocentric coordinate system, $\Delta {\mathit{K}}_G$ and $\Delta {\mathit{\alpha }}_G$ are, respectively, the calibration coefficient error and installation angle error of the gyroscope; ${\mathit{\omega }}_{ib}^b$ is the rotational angular velocity of the roadheader relative to the geocentric coordinate system in the carrier coordinate system; ${\mathit{\epsilon }}^n$ is gyroscope bias error.

5.4. Design of Kalman Filter

The Kalman filter accurately updates the mean square error of the system, allowing for an optimal estimation of the system’s state. Based on the Kalman filter principle, the disparity between the position and attitude data of the SINS and those of the stereo visual odometry is employed as the observed value of the Kalman filter, while the Kalman filter’s output value is used to rectify the error of the SINS.

On the basis of the analysis of error equation of the SINS, 15-dimensional vector $\mathit{x}$ related to position and attitude is selected as the state quantity:

$$\mathit{x}={\left[\begin{array}{ccccc}{({\mathit{\varphi }}_I^n)}^T& {\left(\Delta {\mathit{V}}_I^n\right)}^T& {\left(\Delta {\mathit{P}}_I^n\right)}^T& {({\mathit{\epsilon }}^n)}^T& {({\nabla }^n)}^T\end{array}\right]}^T$$

(25)

The difference between the data of the SINS and of the stereo visual odometry is used as the observation quantity of the Kalman filter:

$$\mathit{Z}=\left[\begin{array}{l}{\mathit{P}}_I^n\\ {\mathit{\phi }}_I^n\end{array}\right]-\left[\begin{array}{l}{\mathit{P}}_C^n\\ {\mathit{\phi }}_C^n\end{array}\right]$$

(26)

The state equation and the observation equation of the Kalman filter are as follows:

$$\left\{\begin{array}{l}{\mathit{x}}_k=\mathit{A}{\mathit{x}}_{k-1}+{\mathit{w}}_{k-1}\\ {\mathit{Z}}_k=\mathit{H}{\mathit{x}}_k+{\mathit{D}}_k\end{array}\right.$$

(27)

where $\mathit{A}$ is the state transfer matrix; ${\mathit{w}}_{k-1}$ is the process noise of the system; ${\mathit{Z}}_k$ is the observed value; $\mathit{H}$ is the measurement matrix; ${\mathit{D}}_{\mathit{k}}$ is the measurement white noise.

The Kalman filter updates state quantity by following these steps:

• Predict the state quantity ${\widehat{\mathit{x}}}_k^{-}$:

  $${\widehat{\mathit{x}}}_k^{-}=\mathit{A}{\widehat{\mathit{x}}}_{k-1}$$

  (28)

  where ${\widehat{\mathit{x}}}_{k-1}$ is the a posteriori estimate of $\mathit{x}$ at time k − 1.
• Predict the state covariance matrix

  $${\mathit{P}}_k^{-}=\mathit{A}{\mathit{P}}_{k-1}{\mathit{A}}^T+{\mathit{Q}}_{k-1}$$

  (29)

  where ${\mathit{P}}_{k-1}$ is the state covariance matrix at time k − 1 and ${\mathit{Q}}_{k-1}$ is the system noise covariance matrix at time k − 1.
• Update Kalman filter gain

  $${\mathit{K}}_k={\mathit{P}}_k^{-}{\mathit{H}}^T{(\mathit{H}{\mathit{P}}_k^{-}{\mathit{H}}^T+{\mathit{R}}_k)}^{-1}$$

  (30)

  where ${\mathit{R}}_k$ is the covariance matrix of the measurement noise at time k.
• Update the state variable quantity ${\widehat{\mathit{x}}}_k$:

  $${\widehat{\mathit{x}}}_k={\widehat{\mathit{x}}}_k^{-}+{\mathit{K}}_k({\mathit{Z}}_k-\mathit{H}{\widehat{\mathit{x}}}_k^{-}).$$

  (31)
• Update the state covariance matrix:

  $${\mathit{P}}_k=(\mathit{I}-{\mathit{K}}_k\mathit{H}){\mathit{P}}_k^{-}.$$

  (32)

The posterior state variable quantity ${\widehat{\mathit{x}}}_k$ is used as the optimal estimate of the state quantity, on the basis of which the SINS performs error correction and then updates the position and attitude of the roadheader.

6. Experiment and Discussion

6.1. Dataset

We tested the proposed method on the KITTI dataset [54] and the EuRoC dataset [55] and compared the proposed methods with three other methods. The absolute trajectory error (ATE) was used to compute the average translation error (m) and rotation error (°).

The ATE values on KITTI (00 to 03) and EuRoC (MH01-MH03) are respectively shown in

Table 3. Comparisons of accuracy using the KITTI dataset.

Seq.	IMU		Visio		ORB-SLAM2		Proposed
	t	r	t	r	t	r	t	r
00	5.82	1.45	5.43	1.75	3.49	1.47	2.75	1.32
01	9.73	3.29	8.22	3.17	7.52	2.92	6.17	2.84
02	10.72	3.26	7.02	5.11	6.69	4.73	5.52	2.57
03	5.18	1.25	4.19	1.63	3.07	1.27	2.51	1.08

and

Table 4. Comparisons of accuracy using the EuRoc dataset.

Seq.	IMU		Visio		ORB-SLAM2		Proposed
	t	r	t	r	t	r	t	r
MH_01	0.45	1.53	0.49	1.65	0.34	1.62	0.26	1.37
MH_02	0.63	1.59	0.61	1.70	0.35	1.71	0.30	1.09
MH_03	0.59	1.54	0.22	1.65	0.24	1.01	0.17	0.97
MH_04	0.51	1.38	0.72	1.53	0.38	1.27	0.21	1.12

, with Figure 11 and Figure 12, respectively, showing the trajectory of KITTI seq. 00 and EuRoc MH_03 in the xz plane.

From these results, we can summarize:

• Visio struggles with accurately estimating attitude, but it is relatively consistent on overall translation.
• With IMU, the position estimation from triple integration of accelerometer and gyroscope measurements drifts rapidly over time. Also, the IMU results have lower orientation error than Vision; this implies that the majority of IMU drifts are the result of integrating noisier accelerometer measurements, leading to increasingly inaccurate velocity estimates.
• Comparing to ORBSLAM2 [56], our method obtains lower error on all sequences, which can be attributed to the image-enhancement module and the ability of the Kalman filter to fuse the data of IMU and Visio.

The proposed method has, overall, lower rates of error than other methods on all sequences and it corrects the accumulated errors in translation and rotations, significantly improving position estimates.

6.2. Integrated Positioning Experiment

The experimental platform of integrated positioning for the roadheader mainly includes: EBZ160M-2 Roadheader (SANY Heavy Industry Co., Ltd., Beijing, China), FSON II nine-axis strapdown inertial navigation system (China Aerospace Science & Industry Corp, Beijing, China), TS60 total station (Leica Geosystems, St. Gallen, Switzerland), CH110 low-precision six-axis strapdown inertial navigation system (Beijing Hipnuc Electronic Technology Co., Ltd., Beijing, China), and a Realsense D455 camera (Intel Corporation, Santa Clara, CA, USA). The resolution of the images taken by the camera is 640 × 480 pixels, but all images need to be resized to 424 × 240 pixels before processing. And our system operates at a speed of 30 frames per second for image processing. The experiments were conducted in an extendable simulated roadway measuring 25 m in length, 4 m in width, and 5.5 m in height. The roadheader has a height of 2.5 m, a length of 10.4 m, and a track spacing of 1.1 m.

The CH110 is a six-axis strapdown inertial navigation system with moderate precision, equipped with a three-axis gyroscope and a three-axis accelerometer for measuring angular velocity and linear acceleration. Due to the absence of a built-in magnetometer, the CH110 is unable to conduct self-alignment to acquire the initial attitude. FSON II is a high-precision nine-axis strapdown inertial navigation system which can perform fine alignment and use the attitude after alignment as the initial attitude of the integrated positioning system. During the positioning experiment, the position data from the total station and the attitude data from FSON II will serve as the actual position and attitude data of the roadheader, validating the precision of the integrated positioning method. The parameters of CH110 and FSON II are shown in

Table 5. Parameters of CH110 and FSON II.

Parameters	CH110	FSON II
Gyroscope zero bias/${(}^{\circ }·h^{-1})$	≤3.5	≤0.005
Alignment accuracy/(°)	0.1	0.01
Heading accuracy/(°)	≤0.4	≤0.02

.

The equipment is installed as shown in Figure 13. CH110 is installed above the roadheader, near the center of gravity. And the D455 and prism are installed next to the CH110 to reduce the pole-arm error. If the installation error angle of the CH110 is not a small angle, the CH110 needs to be calibrated with FSON II to eliminate the error and output the CH110 initial attitude transformation matrix before the integrated positioning system starts working.

The operator controls its movement along the target path through a remote-control platform. The strap-down inertial navigation system (SINS) initiates the updating process for the machine’s posture, velocity, and position. Simultaneously, stereo visual odometry undertakes pose updates. The navigation computer utilizes a Kalman filter to amalgamate the data from both systems, rectifying and offsetting errors in the SINS, and concurrently recording the real-time position and attitude data of the machine. The experiment is repeated multiple times under the same road conditions, and a set of experimental data is selected for analysis.

In the positioning experiment, the position and attitude of roadheader was estimated by CH110 and the integrated positioning method, respectively. As shown in the figure, three different curves represent the results of three different positioning methods. The displacements of the roadheader in the x, y, and z axes are shown in Figure 14a–c. Figure 14a represents the displacement of the roadheader along the x-axis in the navigation coordinate system. Figure 14b represents the displacement along the y-axis, and Figure 14c represents the displacement along the z-axis. The attitude of the roadheader, as solved by a different positioning method, is shown in Figure 14d–f. Figure 14d–f, respectively, represent the variations of the roadheader’s roll angle, yaw angle, and pitch angle over time. As can be seen from the figure, the position and attitude data calculated by CH110 fluctuates widely, indicating that there is a large amount of noise in the gyroscope and accelerometer data.

The results of the quantitative analysis of the positioning results are shown in

Table 6. Results of the quantitative analysis of the positioning data. It contains three evaluation indicators: maximum error, average error, and standard deviation.

Positioning Method	Assessment Indicators	x/m	y/m	z/m	Roll/°	Yaw/°	Pitch/°
Proposed method	Maximum error	0.0360	0.1172	0.0150	0.1129	1.0616	0.9759
	Average error	−0.0012	−0.0615	0.0073	−0.0343	−0.477	−0.5367
	Standard deviation	0.0150	0.0328	0.0035	0.0358	0.2751	0.2413
SINS	Maximum error	0.1148	0.3942	0.0581	0.2203	1.3589	1.0110
	Average error	−0.0036	−0.2061	0.0314	−0.0631	−0.1227	−0.5782
	Standard deviation	0.0492	0.1143	0.0133	0.0928	0.6188	0.2401

; the maximum errors of roll, yaw, and pitch are 0.1129°, 1.3589°, and 0.9759°, respectively. The maximum errors of displacement in the x, y, and z axis are 0.0360 m, 0.1172 m, and 0.0150 m, respectively. Additionally, the average errors and standard deviation errors of the integrated positioning method are all smaller for the SINS, which means that integrated positioning system has higher accuracy and better reliability.

6.3. Discussions

It can be seen from Figure 14 that the error of the SINS would accumulate and tend to disperse over time, and that the integrated positioning method proposed in this paper can effectively eliminate the accumulated error of the SINS. In addition, the deviation of the position and attitude curves of CH110 gradually increases over time, indicating that the errors of gyroscope and accelerometer data accumulate over time and lead to a sharp decrease in the accuracy of attitude perception, while the integrated positioning method can effectively eliminate the effects of accumulated errors. Comparing the displacement curves in different directions, we find that the average error and standard deviation in the y-axis direction are larger than those in the other two axes, because the roadheader moves a longer distance in the y-axis direction, and similarly, since the yaw angle varies more than the roll angle and pitch angle, the mean error and mean square error of the yaw angle are larger. The turning action of the roadheader at 75 s and 180 s makes the error of the SINS increase significantly, while the integrated positioning method can effectively limit the error fluctuation.

By comparing the position and attitude data calculated by the integrated positioning method and the SINS with real position and attitude data, it can be concluded that the integrated positioning method can effectively eliminate the influence of the accumulated errors of the SINS and realize the highly accurate positioning of the roadheader.

The quantitative analysis of the experimental data shows that the maximum errors of position and attitude of the integrated positioning method are reduced relative to the SINS error. The integrated positioning method proposed in this paper can realize the highly accurate positioning of the roadheader.

In this paper, we propose an integrated positioning method for a roadheader. By leveraging multi-sensor fusion, the accuracy of the positioning system can be enhanced without relying on costly sensors. Furthermore, this method demonstrates high applicability in harsh environments such as coal mines.

This method is not limited to roadheaders, but can also be used for other mine equipment that works in similar environments, such as bolt-drilling trucks and trackless rubber-tired vehicles. But inevitably, the positioning accuracy of various sensors would be greatly discounted owing to the vibration, etc. In particular, the vibration caused by cutting work not only reduces the positioning accuracy, but also damages the sensors. Therefore, it is necessary to reduce the impact of vibration through algorithm optimization and technology optimization in future research [57]. Furthermore, this method does not provide the roadheader’s position relative to the surrounding roadway. Therefore, integrating positioning technology with digital roadway technology becomes imperative, as it can facilitate the advancement of roadheader automation.

6.4. Limitation

In the domain of image processing, particularly within stereo visual odometry, the computation of both “attitude” and “position” is not instantaneous; it mandates a quantifiable duration. Leveraging state-of-the-art hardware, our system is adept at processing images at a brisk pace of 30 frames per second, which is meticulously calibrated to align with the roadheader’s kinetic speed. Nevertheless, it is pivotal to note that, while our system’s swiftness is attributed to its robust hardware components, we have yet to innovate a methodology that mitigates computational overheads, marking a discernible limitation in our research.

Diving deeper into the nuances of coal-mine roadways, one is confronted with the inherent unpredictability of rock distribution. The subterranean geological landscapes within coal mines are emblematic of intricate and dynamic environments. Termed as ‘mixed and transitioning terrains’, these roadways predominantly traverse sedimentary formations, including mudstone, siltstone, sandstone, and occasionally, coal seams [58]. Notably, the chromatic properties of coal and associated rock strata exhibit marked similarities, often making them indistinguishable, given their minimal localized variations [59].

In our future endeavors, we aim to further refine the Visual–Inertial Odometry (VIO) tailored for coal mining environments. Key areas of emphasis include optimizing computational overhead to enhance real-time responsiveness and advancing our research in image quality enhancement techniques specifically tailored for mining scenarios, such as dehazing and denoising. Through these improvements, we hope to develop a VIO system that is robust and efficient under the challenging conditions inherent in coal mines.

7. Conclusions

(1)

With the goal of achieving the precise positioning and attitude perception of the roadheader, we propose an integrated positioning method based on the fusion of the strapdown inertial navigation system (SINS) and stereo visual odometry. The fundamental concept of this method is to utilize the strapdown inertial navigation system (SINS) as the reference system and incorporate stereo visual odometry as the supplementary system. The data from both systems are combined using the Kalman filter.

(2)

To eliminate the influence of low-quality images on the accuracy of stereo visual odometry, we designed an image-enhancement module to preprocess the images. We tested our image-enhancement module on both public and self-built datasets and conducted comparative experiments with other image-enhancement methods. The results show that our image-enhancement module can effectively improve image quality, increase the number of features extracted from images, and improve the accuracy of feature matching.

(3)

We tested the proposed integrated positioning method on the KITTI dataset and the EuRoC dataset and compared proposed methods with three other methods. In addition, we designed and conducted integrated positioning experiments on a simulated roadway, with the roadheader as the experimental object. The experimental results demonstrated that the maximum errors for roll, yaw, and pitch were 0.1129°, 1.3589°, and 0.9759°, respectively. Additionally, the maximum errors for displacement along the x, y, and z axes were 0.0360 m, 0.1172 m, and 0.0150 m, respectively.