Research on a Rapid and Accurate Reconstruction Method for Underground Mine Borehole Trajectories Based on a Novel Robot

Abstract

A vast number of boreholes in underground mining operations are often plagued by deviation issues, which severely impact both production efficiency and safety. The accurate and rapid acquisition of borehole trajectories is fundamental for subsequent deviation control and correction. However, existing inclinometers are limited by their operational efficiency and estimation accuracy, making them inadequate for large-scale measurement demands. To address this, this paper proposes a novel method for the rapid and accurate reconstruction of underground mine borehole trajectories using a robotic system. We employ a custom-designed robot equipped with an Inertial Measurement Unit (IMU) and a displacement sensor, which travels stably while collecting real-time attitude and depth information. Algorithmically, a complementary filter is used to fuse data from the gyroscope with that from the accelerometer and magnetometer, overcoming both integration drift and environmental disturbances. A cubic spline interpolation algorithm is then utilized to time-register the low-sampling-rate displacement data with the high-frequency attitude data, creating a time-synchronized sequence of ‘attitude–displacement increment’ pairs. Finally, the 3D borehole trajectory is accurately reconstructed by mapping the attitude quaternions to direction vectors and recursively accumulating the displacement increments. Comparative experiments demonstrate that the proposed method significantly improves efficiency. On a complex trajectory, the maximum and mean errors were reduced to 0.38 m and 0.18 m, respectively. This level of accuracy is far superior to that of the conventional static point-by-point measurement mode and effectively suppresses the accumulation of dynamic errors. This work provides a new solution for routine borehole trajectory surveying in mining operations.

1. Introduction

China has a vast number of underground metal mines, approximately 12,000, which account for 38% of the country’s total mines. In operations such as tunneling and stopping within these mines, drilling and blasting remains the predominant rock-breaking technology [1]. The ‘drill-first, blast-later’ process is highly dependent on precise borehole networks. Combined with continuous production modes, this leads to the massive spatio-temporal accumulation of drilling tasks across various working faces, ultimately resulting in a massive annual volume of hundreds of millions of boreholes. During the actual drilling process, the actual trajectory of the borehole often deviates from its designed axis, resulting in borehole deviation [2,3] (as shown in Figure 1). Excessive borehole deviation not only causes the spacing at the borehole bottom to be either too wide or too narrow, thereby increasing the oversize rock ratio, but can also lead to a loss of control over stope boundaries. This ultimately results in low ore recovery rates, increased dilution, and diminished economic benefits [4,5]. Therefore, by accurately reconstructing the 3D borehole trajectories and quantifying their deviation prior to detonation, it is possible to mitigate problems caused by borehole deviation—such as uneven blast energy distribution, over-break, and under-break—by adjusting the charge structure and quantity and optimizing the initiation network and delay sequence [6]. Evidently, as a technical prerequisite for transitioning from empirical to precision blasting, conducting in-depth research on borehole trajectory measurement is of great significance.

Figure 1. Schematic of Borehole Deviation in Tunneling.

Continuous academic research and engineering practice have driven the constant optimization of the structure and performance of borehole survey instruments. In the early stages of technological development, static inclination survey methods, typified by siphon-type and photographic inclinometers, were predominantly used. However, the siphon-type inclinometer was prone to subjective errors due to its reliance on visual readings, while the photographic method involved a cumbersome process of film developing and manual interpretation, leading to significant feedback delays. Consequently, both methods suffered from fundamental limitations in accuracy and reliability and have since been rendered obsolete [7]. These have been superseded by modern survey technologies based on electronic sensors. Currently, survey instruments for mining applications typically feature a measurement system composed of a tri-axial accelerometer and a tri-axial magnetometer. This system determines the instrument’s spatial attitude at a specific survey point by sensing the components of the gravitational and geomagnetic field vectors within the instrument’s body frame (b-frame) [8]. However, this measurement system heavily relies on the geomagnetic field as its azimuth reference. In complex downhole environments, hard iron interference from ferromagnetic materials such as drill strings and casings distorts the local geomagnetic field, leading to errors in azimuth measurements [9]. In this context, based on the Poisson model for magnetic interference, Wang et al. [10] utilized an ellipsoid fitting algorithm to calibrate the magnetic deviation of a triaxial magnetometer. Although error modeling and compensation can improve measurement accuracy, its effectiveness is fundamentally limited by magnetic interference. As the demand for trajectory precision increases across various drilling projects, researchers have introduced navigation and positioning technologies into survey systems, leading to the development of gyroscope-based survey instruments. Early gyroscopic inclinometers primarily utilized mechanical gyroscopes, which, however, exhibited poor vibration resistance, making them ill-suited for complex mining environments. To address this issue, researchers introduced high-precision Fiber-Optic Gyroscopes (FOGs) into borehole surveying [11,12]. Owing to their all-solid-state structure with no moving parts, FOGs significantly enhanced the accuracy and reliability of inclination surveys. Nevertheless, their high cost has substantially restricted their large-scale application.

In recent years, Micro-Electro-Mechanical System (MEMS) sensors have found widespread application in numerous fields, leveraging advantages such as small size, low power consumption, and a high level of integration [13,14,15]. This trend has also driven the development of measurement equipment towards smaller dimensions and lower costs [16,17]. However, consumer-grade Inertial Measurement Units (IMUs) built with these sensors exhibit low bias stability and large random noise compared to their navigation-grade counterparts [18,19]. Consequently, in pure inertial dead reckoning, their inherent errors continuously accumulate through integration, ultimately leading to a rapid drift in the position solution. Therefore, in the absence of external aiding information, it is not feasible to obtain accurate borehole depth information by solely integrating the IMU’s acceleration [20,21]. To address this, in trenchless applications for surface or shallow pipeline trajectory measurement, the three-dimensional position error from the pure inertial solution is corrected by fusing continuous distance measurements from an odometer and incorporating absolute position information from GPS or vision systems at the endpoints as external constraints [22,23,24]. However, due to signal shielding by rock formations and borehole diameter constraints, borehole surveying in mining cannot utilize external information sources like GPS and odometers. The borehole depth information was acquired through a deterministic external measurement method, detailed in reference [8], which involved point-by-point, stop-and-go measurements using a marked cable or drill rod as a benchmark. To further enhance efficiency and automation, Xu Guoming et al. [25] employed a wheeled odometer to achieve automatic depth recording at fixed increments. However, their measurement paradigm remained essentially a ‘stop-and-go’ process in nature and did not represent a fundamental breakthrough.

To address the aforementioned issues, this paper proposes and validates a novel robot-based method for the rapid and accurate reconstruction of borehole trajectories. The main contributions of this study are as follows:

• In terms of the measurement mode, a novel paradigm for borehole trajectory surveying is proposed based on the continuous motion of a robot. Using a robot as the measurement platform, this approach completes data acquisition in a single, continuous traverse, replacing the traditional and inefficient ‘stop-and-go’ operational method. This results in a significant enhancement of measurement efficiency.
• For attitude estimation, a complementary filter algorithm is employed to combine the high-frequency dynamic response of the gyroscope with the long-term stability of the accelerometer and magnetometer. This approach enhances the accuracy and robustness of the attitude solution under dynamic conditions.
• For trajectory reconstruction, an independent, drift-free draw-wire displacement sensor is introduced to provide external odometry observations. By fusing the precise attitude with the drift-free displacement increments at a high frequency, long-term position drift is effectively suppressed.

The remainder of this paper is organized as follows. Section 2 elaborates on the proposed trajectory reconstruction method. Section 3 validates the superiority of our method in terms of accuracy and efficiency through comparative experiments. Finally, Section 4 concludes the paper.

2. Methods

To achieve rapid and accurate reconstruction of borehole trajectories, this paper establishes a complete reconstruction pipeline, which comprises five key steps (as illustrated in Figure 2): (1) Data Acquisition; (2) Data Preprocessing; (3) Attitude Estimation; (4) Temporal Alignment of Displacement and Attitude Data; and (5) Trajectory Reconstruction.

Figure 2. Overall workflow of the proposed borehole trajectory reconstruction method.

First, in the data acquisition stage, an IMU is used to measure the carrier’s tri-axial acceleration, tri-axial angular velocity, and tri-axial magnetic field strength. Simultaneously, a draw-wire displacement sensor is utilized to acquire the carrier’s travel displacement, thereby obtaining depth information along the borehole axis. Next, during the data preprocessing stage, the random error terms of the IMU are first accurately identified using the Allan variance method. Based on these results, a static feedforward compensation strategy is designed for the gyroscope. Subsequently, to filter out high-frequency noise, a third-order Butterworth low-pass filter is applied to smooth the error-compensated tri-axial gyroscope data and the raw tri-axial accelerometer data. The process then proceeds to the attitude update stage. Following an initial alignment, a complementary filter algorithm is designed to achieve high-precision attitude estimation. This algorithm fuses the high-frequency dynamic response of the gyroscope with the low-frequency attitude reference from the accelerometer. Through a Proportional-Integral (PI) feedback controller, the integration drift of the gyroscope is corrected online using the attitude error calculated from the accelerometer. After obtaining the real-time attitude sequence, the process moves to the temporal alignment stage. To resolve data fusion issues caused by mismatched sampling frequencies and unsynchronized start/stop times of the sensors, cubic spline interpolation is used to interpolate the low-frequency, discrete displacement data. This allows each attitude data point, $Q_i$, to be registered with its corresponding displacement increment, $∆L_i$, ensuring the temporal consistency of the data used for subsequent trajectory calculations. Finally, in the trajectory reconstruction stage, the quaternion in each data pair is converted into a Direction Cosine Matrix (DCM). The unit vector of the carrier’s Y-axis, which is aligned with the borehole axis, is transformed by this matrix into a pointing vector in the global coordinate frame. From this, the inclination and azimuth angles that define the trajectory’s geometry can be calculated. Ultimately, by combining these three elements—inclination, azimuth, and the corresponding depth increment—the complete three-dimensional borehole trajectory is accurately reconstructed using the Average Angle Method in a point-by-point recursive manner.

2.1. Data Acquisition

To synchronously acquire the attitude and depth information of the robot within the borehole, the data acquisition system developed in this study integrates two types of sensors. Attitude information is provided by an HWT9073-485 nine-axis attitude sensor manufactured by Wit-Motion. This module integrates a tri-axial accelerometer, a tri-axial gyroscope, and a tri-axial magnetometer. The accelerometer features a measurement range of $\pm 2 \mathrm{g}$ and a thermal drift of $\pm 0.1 \mathrm{m}\mathrm{g}/°\mathrm{C}$; this range was selected to balance the high resolution required for static measurements with the dynamic adaptability needed in this application. The gyroscope’s measurement range is asymmetrically configured: the XY-axes (pitch/roll) have a range of $\pm 2000°/\mathrm{s}$ to accommodate high-dynamic angular motion, while the Z-axis (yaw) has a range of $\pm 400°/\mathrm{s}$ to ensure azimuth precision. The magnetometer has a measurement range of $\pm 800 \mathrm{\mu }\mathrm{T}$ and, under a relatively stable magnetic field or after proper calibration, can provide a crucial external reference for heading angle determination. The module supports a maximum output rate of 200 Hz; however, to balance dynamic response precision with data processing efficiency, the sampling frequency was set to 50 Hz for this study.

Depth information is measured by an RS485-type draw-wire displacement sensor, which operates based on potentiometer signal conversion. This sensor directly translates mechanical motion into a measurable electrical signal via a steel cable, enabling the real-time provision of the carrier’s displacement. Compared to methods reliant on inertial integration, this direct measurement approach fundamentally avoids the cumulative errors that arise from the double integration of acceleration. Regarding its specific parameters, the sensor has a measurement range of 15 m and a hardware response frequency of 25 Hz. To obtain the highest possible resolution for displacement data while ensuring stable communication, its sampling frequency was ultimately set to 20 Hz in this study.

2.2. Data Preprocessing

To suppress measurement errors in the raw IMU data, effective preprocessing is essential prior to attitude estimation. Typically, IMU errors are classified into two main categories: deterministic and random errors. Since the sensor used in this study was calibrated by the manufacturer prior to delivery and high-precision equipment such as a turntable was unavailable for on-site re-calibration, the preprocessing workflow primarily focuses on addressing random errors, which exert a cumulative effect on the accuracy of the final solution.

To effectively suppress these random errors, they must first be quantitatively characterized. This study employs the Allan variance method, which effectively separates different types of error components from time-series data by quantifying the sensor’s noise characteristics across various time scales. This allows for the identification of the parameters for each of the IMU’s random error terms. To mitigate the effects of temperature fluctuations, which can cause thermal drift and gain variations in the gyroscope, the IMU was first powered on and kept stationary to warm up for half an hour. Subsequently, static data was continuously collected for two hours at a sampling rate of 50 Hz.

By applying the Allan variance method to the gyroscope and accelerometer data, the complete Allan standard deviation curve over the entire measurement period is obtained, as illustrated in Figure 3.

Figure 3. Allan standard deviation plots for the IMU. (a) Tri-axial accelerometer data. (b) Tri-axial gyroscope data. The plots show the Allan deviation (Sigma) as a function of averaging time.

A fitting analysis of the Allan variance curve, as shown in Figure 3, allows for the quantitative evaluation of the dominant random error sources. The identified error coefficients for each axis of the gyroscope and accelerometer are summarized in Table 1.

Table 1. Identification results for the IMU error terms.

	$\mathbf{QN} (°/\mathit{h})$	$\mathbf{ARW} (°/{\mathit{h}}^{1/2})$	$\mathbf{BI} (°/\mathit{h})$
Gyro-X	0.091282	0.218799	12.067933
Gyro-Y	0.058105	0.202909	4.399149
Gyro-Z	0.091359	0.165631	6.979242
acc-X	0.005237	0.000599	0.424069
acc-Y	0.004904	0.001367	0.018365
acc-Z	0.000437	0.001137	0.004857

The identification results indicate that the static random errors of the gyroscope are an order of magnitude larger than those of the accelerometer, and its error composition is predominantly characterized by bias instability. A two-stage preprocessing procedure is therefore established. First, the static averaging method is employed to explicitly compensate for the low-frequency drift of the gyroscope. Subsequently, based on this, a Butterworth low-pass filter is globally applied to the six-axis IMU data to filter out the remaining high-frequency random noise, thereby further improving the signal quality.

To implement the first stage of static compensation, the robot is held stationary at the borehole entrance for several minutes before commencing operation. Under these static conditions, the gyroscope output, ${\omega }^b\left(k\right)$, can be approximated as the composite error. The arithmetic mean of this static data sequence is then calculated to obtain an optimal estimate of the initial bias, as shown in the following equation:

$${\widehat{b}}_g={\displaystyle \frac{1}{M}}\sum _{k=1}^M{\omega }^b\left(k\right)$$

(1)

Here, $M$ is the number of gyroscope data samples, ${\omega }^b\left(k\right)$ is the $k$-th sample of the gyroscope’s angular velocity output, and ${\widehat{b}}_g=\left[\begin{array}{ccc}{\widehat{b}}_{gx}& {\widehat{b}}_{gy}& {\widehat{b}}_{gz}\end{array}\right]$ is the estimated bias of the tri-axial gyroscope.

The static averaging method, by leveraging the significant differences among various random error terms in the frequency domain, filters out high-frequency zero-mean interference while completely preserving the low-frequency bias components within the static period, thereby achieving the explicit estimation and extraction of bias instability, with the resulting ${\widehat{b}}_g$ used for feed-forward compensation of the gyroscope’s raw output data.

When executing the second stage of low-pass filtering to remove the high-frequency noise caused by angular random walk and quantization noise from the compensated data, this paper designs and applies a third-order Butterworth low-pass filter, to strike a balance between effective signal smoothing and the preservation of the true motion trend, and to avoid aliasing in accordance with the 50 Hz sampling rate, the filter’s cutoff frequency is set to 10 Hz to perform smoothing filtration on the data from the IMU’s tri-axial accelerometer and tri-axial gyroscope, with the comparison before and after filtering shown in Figure 4.

Figure 4. Comparison of IMU data before and after applying the Butterworth low-pass filter. The left column shows the raw signals, while the right column shows the corresponding signals after filtering. The top row displays data from the tri-axial gyroscope, and the bottom row displays data from the tri-axial accelerometer.

As demonstrated by the changes in the data curves before and after filtering, the filter effectively removes high-frequency noise. This is achieved without introducing significant phase delay or distortion, thereby preserving the time-domain characteristics of the data. Consequently, the filtered signal is considerably smoother, leading to an improved signal-to-noise ratio.

2.3. Attitude Estimation

In the absence of external constraints, the robot’s attitude estimation relies entirely on the IMU, obtaining its initial attitude relative to the global coordinate system using a method based on the IMU’s internal measurements can significantly reduce the cumulative effect of initial attitude errors. To provide a clear mathematical framework, this paper defines a body coordinate system (hereafter referred to as the b-frame) fixed to the IMU, with its axes defined as Forward–Right–Down (Y-X-Z), and adopts a navigation coordinate system (hereafter referred to as the n-frame) defined as East–North–Up (ENU) as the global reference. Based on this framework, the initial attitude is determined through a static alignment process. Under static conditions, the ideal output of the accelerometer is the local gravitational acceleration, and its tri-axial measurement is essentially the projection of the standard gravity vector in the body coordinate system. By calculating the rotation required to rotate this measurement vector back into alignment with the theoretical gravity vector in the navigation coordinate system, the initial roll angle $\varphi$ and pitch angle $\theta$ can be determined, thereby obtaining the tilt attitude of the body relative to the horizontal plane. With the tilt attitude known, the initial heading angle $\psi$ can then be determined by establishing a rotational constraint between the magnetometer’s measurement and the geomagnetic reference vector, completing the alignment process. To avoid the inherent singularity of Euler angles during the dynamic update process, the computed initial angles are mapped to an equivalent quaternion, $Q_0$, to serve as the initial condition for subsequent dynamic attitude updates.

Following the initial alignment, the dynamic attitude update stage commences. This paper employs a fusion algorithm based on a complementary filter, which corrects the gyroscope’s angular velocity output using error vectors derived from the accelerometer and magnetometer. A PI control mechanism is adopted to perform a weighted fusion of the high-frequency information from the gyroscope with the low-frequency information measured by the accelerometer and magnetometer. The proportional (P) term facilitates a rapid response to the current error, while the integral (I) term eliminates long-term accumulated errors. This approach achieves complementary advantages and effective error suppression.

The algorithm is implemented in two main stages: prediction and correction. The former utilizes the angular velocity data from the gyroscope to predict the current attitude by integrating the attitude from the previous time step via the quaternion differential equation. The latter stage achieves real-time correction of the gyroscope’s integration error based on the error vector between the predicted gravity and geomagnetic vectors (at the current attitude) and the real-time sensor observations, thereby improving the accuracy of the attitude solution. Figure 5 presents a flowchart of the attitude update process based on this complementary filter algorithm. The corresponding specific implementation is divided into the following key steps.

Figure 5. Flowchart of the complementary filter-based attitude update algorithm.

• Correction Stage.

First, the observation vectors from the accelerometer and magnetometer are converted into unit vectors that solely represent directional information. The calculation formula is as follows:

$${acc}_f^b=\left[\begin{array}{c}{a}_{fx}\\ a_{fy}\\ a_{fz}\end{array}\right]={\displaystyle \frac{1}{\sqrt[2]{a_{fx}^2+a_{fy}^2+a_{fz}^2}}}\left[\begin{array}{c}{a}_{fx}\\ a_{fy}\\ a_{fz}\end{array}\right]$$

(2)

$$m_b=\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right]={\displaystyle \frac{1}{\sqrt[2]{m_x^2+m_y^2+m_z^2}}}\left[\begin{array}{c}{m}_x\\ m_y\\ m_z\end{array}\right]$$

(3)

where ${acc}_f^b$ is the filtered three-dimensional acceleration vector measured in the body coordinate system, and $m_b$ is the three-dimensional magnetic field vector measured in the body coordinate system.

The attitude error is determined by comparing the normalized observation vectors with the theoretical reference vectors. First, the theoretical gravity reference is obtained by transforming the standard gravity vector from the geographic coordinate system into the body coordinate system using the current quaternion, with its expression as follows:

$$v_b=\left[\begin{array}{c}{v}_x\\ v_y\\ v_z\end{array}\right]=\left[\begin{array}{c}2\left(q0q2+q1q3\right)\\ 2\left(q2q3-q0q1\right)\\ 1-2\left(q1q1+q2q2\right)\end{array}\right]$$

(4)

Correspondingly, the calculation of the theoretical geomagnetic reference is intended to construct a tilt-compensated heading reference. The process begins by transforming the real-time magnetic field measurement from the body coordinate system to the geographic coordinate system, using the current attitude:

$$\left[\begin{array}{c}{b}_x\\ 0\\ b_z\end{array}\right]=\left[\begin{array}{c}\sqrt{h_x^2+h_y^2}\\ 0\\ 2m_x\left(q1q3-q0q2\right)+2m_y\left(q2q3+q0q1\right)+m_z\left(1-2q1q1-2q2q2\right)\end{array}\right]$$

(5)

where ${\left[\begin{array}{ccc}{b}_x& 0& b_z\end{array}\right]}^T$ is the reference magnetic field vector in the n-frame, and ${\left[\begin{array}{ccc}{h}_x& h_y& h_z\end{array}\right]}^T$ is the measured magnetic vector after being transformed from the b-frame to the n-frame using the current attitude quaternion.

The theoretical geomagnetic vector obtained above is then transformed into the body coordinate system:

$$w_b=\left[\begin{array}{c}2b_x\ast \left(0.5-q2\ast q2-q3\ast q3\right)+2b_z\ast \left(q1\ast q3-q0\ast q2\right)\\ 2b_x\ast \left(q1\ast q2-q0\ast q3\right)+2b_z\ast \left(q0\ast q1+q2\ast q3\right)\\ 2b_z\ast \left(0.5-q1\ast q1-q2\ast q2\right)+2b_x\ast \left(q0\ast q2+q1\ast q3\right)\end{array}\right]$$

(6)

Subsequently, the attitude error terms, $e_a$ and $e_m$, are determined through the cross-product operation between the observation vectors and the reference vectors:

$$e_a=v_b\times {acc}_f^b=\left[\begin{array}{c}{a}_{fz}\ast v_y-a_{fy}\ast v_z\\ a_{fx}\ast v_z-a_{fz}\ast v_x\\ a_{fy}\ast v_x-a_{fx}\ast v_y\end{array}\right]$$

(7)

$$e_m=m_b\times w_b=\left[\begin{array}{c}{m}_y\ast w_z-m_z\ast w_y\\ m_z\ast w_x-m_x\ast w_z\\ m_x\ast w_y-m_y\ast w_x\end{array}\right]$$

(8)

Finally, the attitude error terms $e_a$ and $e_m$ are used to correct the gyroscope’s raw angular velocity through a PI controller. The calculation formula is as follows:

$$\acute{{\omega }_b}=\left[\begin{array}{c}\acute{{\omega }_x}\\ \acute{{\omega }_y}\\ \acute{{\omega }_z}\end{array}\right]=\left[\begin{array}{c}{\omega }_{fx}\\ {\omega }_{fy}\\ {\omega }_{fz}\end{array}\right]+k_p\left(e_a+e_m\right)+k_i\int \left(e_a+e_m\right)$$

(9)

where $\acute{{\omega }_b}$ is the angular velocity after being compensated by the accelerometer and magnetometer data, $k_p$ is the proportional gain, and $k_i$ is the integral gain. The selection of parameters $k_p$ and $k_i$ is a process of balancing the system’s dynamic response and static stability, in this paper, they are determined through an empirical tuning method, with the final adopted parameters being $k_p=0.13$ and $k_i=0.005$.

b.

Prediction Stage.

This stage utilizes the corrected angular velocity, $\acute{{\omega }_b}$, to predict the attitude at the next time step by integrating the quaternion kinematic differential equation. The calculation formula is as follows:

$${}_b{}^n\dot{Q}={\displaystyle \frac{1}{2}}{}_b{}^{n}Q ⨂ \left[\begin{array}{c}0\\ \acute{{\omega }_b}\end{array}\right]$$

(10)

In discrete-time systems, a first-order numerical integration is typically used for the solution, and its update equation is as follows:

$${}_b{}^n{Q}_t={}_b{}^n{Q}_{t-1}+{\displaystyle \frac{1}{2}}{}_b{}^n{Q}_{t-1} ⨂\left[\begin{array}{c}0\\ \acute{{\omega }_b}\end{array}\right]∆t$$

(11)

where $\acute{{\omega }_b}$ is the compensated and filtered angular velocity from the gyroscope, $⨂$ denotes the quaternion multiplication operator, ${}_b{}^n{Q}_t$ is the attitude quaternion representing the body’s orientation at time t, ${}_b{}^n{Q}_{t-1}$ is the attitude quaternion at the previous time step, t − 1, and $∆t$ is the time interval.

Numerical integration causes the quaternion to lose its unit property, necessitating the following normalization operation.

$${}_b{}^n{Q}_t={\displaystyle \frac{{}_b{}^n{Q}_t}{‖{}_b{}^n{Q}_t‖}}$$

(12)

Through the iterative “feedback–correction–prediction” closed-loop process described above, the algorithm can effectively combine the dynamic response capability of the gyroscope with the long-term stability of the accelerometer and magnetometer, thereby significantly suppressing the gyroscope’s integration drift and continuously outputting high-precision attitude estimates.

2.4. Temporal Alignment of Displacement and Attitude Data

During blast-hole trajectory measurement, the IMU’s high sampling rate meets the high-dynamic response requirements for attitude changes, providing high-resolution attitude data over time. However, the draw-wire sensor has a slower response speed and an insufficient sampling frequency, which leads to position data lag and poor continuity under highly dynamic conditions. Figure 6 illustrates the attitude and position data plotted against time, revealing that the sensors are not synchronized in their start-up and shut-down times. Specifically, with $T_0<t_0$ and $t_n<T_N$, the vehicle’s attitude and position information do not always correspond to the same observation instants. Consequently, the multi-source data cannot be effectively fused within a unified time frame.

Figure 6. Illustration of temporal asynchrony in multi-source sensor data. The left panel depicts the high-frequency attitude measurements from the IMU, contrasted with the sparsely sampled, lagging position data from the draw-wire sensor on the right. This temporal misalignment, characterized by disparate sampling rates and non-concurrent acquisition periods, precludes direct sensor fusion within a common time frame. In the left panel, the yellow ellipsoids represent the IMU sensor at different time instances, with the blue arrows indicating the body frame and the black arrows indicating the navigation frame. In the right panel, the blue dots represent the discrete position samples from the draw-wire sensor.

Timestamp-based data alignment and registration is the process of synchronizing heterogeneous measurement information from a common platform to the same time instant. For sensor data with a low sampling rate, cubic spline interpolation is employed to infer unknown displacement data points from known discrete displacement data. This method resolves the issue of an inability to precisely match displacement with attitude information, which is caused by the insufficient sampling rate of the displacement sensor. Combining the robot’s smooth motion characteristics with its sensor sampling pattern, the displacement over time can be regarded as a smooth function. Therefore, cubic spline interpolation can approximate the true displacement with high precision, achieving precise temporal alignment of the attitude and displacement data.

Given a set of N + 1 time knots, $T_0\le T_1\cdots \le T_N$, and their corresponding displacement values, $l_0,l_1,\cdots l_N$, we construct a cubic spline interpolation function $P\left(T\right)$ that satisfies the condition $P\left(T_i\right)=l_i,i=0,1,\cdots ,N$. The function $P\left(T\right)$ is defined as a piecewise polynomial, where on each subinterval $\left[\begin{array}{cc}{T}_i& T_{i+1}\end{array}\right]$, it takes the form of $P_i\left(T\right)$:

$$P_i\left(T\right)=a_i{T}^3+b_i{T}^2+c_{i}T+d_i$$

(13)

To determine the parameters of the functions $P_0\left(T\right),P_2\left(T\right),\cdots P_{N-1}\left(T\right)$, a total of 4N conditions are required. Among these, each piecewise function $P_i\left(T\right)$ must pass through the two endpoints of its interval $\left[\begin{array}{cc}{T}_i& T_{i+1}\end{array}\right]$, i.e., $P_i\left(T_i\right)=l_i$ and $P_i\left(T_{i+1}\right)=l_{i+1}$. This constraint provides 2N conditions for the N function segments. Furthermore, the continuity of the first and second derivatives provides an additional 2N − 2 conditions.

$$\dot{P_i}\left(T_i-0\right)=\dot{P_i}\left(T_i+0\right)$$

(14)

$$\ddot{P_i}\left(T_i-0\right)=\ddot{P_i}\left(T_i+0\right)$$

(15)

To ensure a unique solution, two additional conditions are required. The system of equations is closed by imposing the constraint of third-derivative continuity at two internal nodes, namely at $T_1$ and $T_{N-1}$, which yields $a_0=a_1$ and $a_{N-2}=a_{N-1}$. With these two final conditions, the parameters of the interpolation functions $P_i\left(T\right)$ (for $i=0, 1,\cdots N-1$) can be uniquely determined.

Using the displacement data timestamp sequence, ${\left\{T_i\right\}}_{i=1}^N$, as the reference time base, the attitude quaternion data, ${\left\{Q_i\right\}}_{i=1}^n$, is time-aligned. This alignment enables the determination of a corresponding displacement value, $L_i$, for each attitude measurement instant, $t_i$. Subsequently, the displacement increment over each consecutive time interval, $\left[\begin{array}{cc}{t}_{i-1}& t_i\end{array}\right]$, is calculated as $∆L_i=L_i-L_{i-1}$. A key aspect is that this displacement increment is synchronized with the attitude, $Q_i$, at the time instant $t_i$. As illustrated in Figure 7, by constructing joint data pairs of $\left(Q_i, ∆L_i\right)$, a time-series mapping from attitude to displacement change is established, which provides the foundational data for the borehole trajectory reconstruction.

Figure 7. Attitude and Displacement Data Alignment and Fusion Based on the Displacement Time Base. The green squares represent the discrete sampling instances T_i of the draw-wire sensor, while the yellow triangles represent the high-frequency sampling instances t_i of the IMU. The horizontal arrows between the P_i(T) labels represent the piecewise polynomial interpolation process between displacement samples. The vertical black arrows indicate the mapping of IMU attitude data Q_i to the corresponding displacement increments ∆L_i, forming the fused data pairs shown in the blue box.

2.5. Trajectory Reconstruction

The borehole trajectory is a spatial curve characterized by continuously varying azimuth, inclination, and length. The robot advances along the borehole axis, supported by the borehole wall. Its direction of motion aligns with the borehole’s axial direction. Therefore, changes in the orientation of a specific IMU axis (the Y-axis) are correlated with the variations in the azimuth and inclination of the borehole path. The quaternion, $Q_i$, which is updated in real-time using a complementary filter algorithm, maps the orientation of the Y-axis from the b-frame to the n-frame. The calculation is performed as follows:

$$C_b^n\left(k\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]$$

(16)

$$\mu ={\left[\begin{array}{ccc}0& 1& 0\end{array}\right]}^T$$

(17)

$$v^n\left(k\right)=\left[\begin{array}{c}{v}_x\left(k\right)\\ v_y\left(k\right)\\ v_z\left(k\right)\end{array}\right]=C_b^n\left(k\right)\mu$$

(18)

where $C_b^n\left(k\right)$ is the DCM at time instant k, representing the rotation from the b-frame to the n-frame; $\mu$ is the orientation vector of the Y-axis in the b-frame; and $v^n\left(k\right)$ is the borehole orientation expressed in the global frame at time instant $k$.

Inclination, azimuth, and Displacement Increment are the three essential elements for updating the borehole trajectory, as illustrated in Figure 8. Inclination describes the deviation of the borehole axis from the horizontal plane. It is defined as the angle between the vector $v^n\left(k\right)$ and the X-Y plane, with positive values indicating an upward direction. The value ranges from −90° to +90°. Azimuth is defined as the angle between the projection of the borehole axis onto the horizontal plane and the direction of the True North direction (the n-frame’s Y-axis). It is considered positive in the clockwise direction, with a value range of −180° to +180°. Displacement Increment refers to the length of the borehole segment between two consecutive measurement points.

Figure 8. Geometric relationship for the calculation of borehole inclination and azimuth. The vector vⁿ(k) represents the orientation vector in the navigation frame. The inclination is the angle between the vector and the z_n-axis, while the azimuth is the angle between the vector’s projection onto the x_ny_n-plane and the y_n-axis.

The inclination $\alpha \left(k\right)$ and azimuth $\beta \left(k\right)$ at time instant k are determined from the orientation vector $v^n\left(k\right)$ given in Equation (27). The transformation formulas are as follows:

$$\alpha (k)=\arcsin \left({\displaystyle \frac{v_z\left(k\right)}{\sqrt{{v_x\left(k\right)}^2+{v_y\left(k\right)}^2+{v_z\left(k\right)}^2}}}\right)$$

(19)

$$\beta \left(k\right)=\mathrm{a}\mathrm{t}\mathrm{a}\mathrm{n}2(v_x\left(k\right),v_y(k\left)\right)$$

(20)

Upon acquiring the inclination angle, azimuth angle, and the corresponding length increment at each time step, this data is used to construct the 3D borehole trajectory. With an attitude update interval of less than 50 ms, the robot’s displacement along the borehole axis between consecutive attitude samples is in the order of millimeters to centimeters. Within this small displacement range, the inclination and azimuth angles of the borehole axis remain virtually constant. Therefore, the borehole trajectory can be modeled as a curve composed of a series of short, straight-line segments with constant angular parameters, connected sequentially. To achieve 2D or 3D visualization of the blasthole trajectory, the Average Angle method is used to resolve the three survey elements at each survey point into plottable parameters within a global coordinate system.

The Average Angle method assumes that the inclination and azimuth of the straight-line segment between two adjacent survey points are the arithmetic mean of the angles measured at these two points. As shown in Figure 9, let A and B be two adjacent survey points on the borehole axis, and A’ and B’ be their respective projections onto the horizontal plane. The formulas for calculating the increments in the three spatial coordinates for this borehole segment are as follows:

$$∆x=∆Lcos\left({\displaystyle \frac{{\alpha }_A+{\alpha }_B}{2}}\right)\sin \left({\displaystyle \frac{{\beta }_A+{\beta }_B}{2}}\right)$$

(21)

$$∆y=∆Lcos\left({\displaystyle \frac{{\alpha }_A+{\alpha }_B}{2}}\right)\cos \left({\displaystyle \frac{{\beta }_A+{\beta }_B}{2}}\right)$$

(22)

$$∆z=∆L\sin \left({\displaystyle \frac{{\mathsf{\alpha }}_A+{\alpha }_B}{2}}\right)$$

(23)

where $∆x$, $∆y$, and $∆z$ represent the incremental changes in the $x$, $y$, and $z$ coordinates between the two survey points; ${\alpha }_A$ and ${\alpha }_B$ are the inclination angles at points $A$ and $B$, respectively; ${\beta }_A$ and ${\beta }_B$ are the corresponding azimuth angles; and $∆L$ is the measured length of the borehole segment. the iterative formulas for calculating the coordinates of each survey point are as follows:

$$X_i=∆L_{i}cos\left({\displaystyle \frac{{\alpha }_i+{\alpha }_{i-1}}{2}}\right)\sin \left({\displaystyle \frac{{\beta }_i+{\beta }_{i-1}}{2}}\right)+X_{i-1}$$

(24)

$$Y_i=∆L_{i}cos\left({\displaystyle \frac{{\alpha }_i+{\alpha }_{i-1}}{2}}\right)\cos \left({\displaystyle \frac{{\beta }_i+{\beta }_{i-1}}{2}}\right)+Y_{i-1}$$

(25)

$$Z_i=∆L_i\sin \left({\displaystyle \frac{{\alpha }_i+{\alpha }_{i-1}}{2}}\right)+Z_{i-1}$$

(26)

where ($X_i$, $Y_i$, $Z_i$) and ($X_{i-1}$, $Y_{i-1}$, $Z_{i-1}$) are the coordinates of the $i$-th and $i-1$-th survey points on the borehole axis in the global coordinate system, respectively. By sequentially calculating and connecting all the survey point coordinates, the complete borehole trajectory can be constructed.

Figure 9. Schematic of the Average Angle Method for Borehole Trajectory Calculation. The dashed lines represent the projection of the borehole segment AB onto the coordinate axes and the horizontal plane to illustrate the geometric derivation of the coordinate increments ($∆x,∆y,∆z$).

3. Experiment and Discussion

3.1. Experimental Setup

To validate the advantages of the proposed rapid continuous trajectory reconstruction method in terms of measurement efficiency and accuracy, we designed and constructed an experimental platform to simulate underground boreholes. A comparative study was then conducted against the conventional static point-by-point measurement method commonly used in mining.

First, regarding the construction of the experimental platform, it is primarily composed of three main components: a custom-designed borehole surveying robot, a simulated borehole pipe, and a data acquisition system. As shown in Figure 10a, the robot utilizes a wheeled structure with multi-point elastic support. This design allows the robot to adaptively maintain its body centered within a certain range of pipe diameters through the passive extension and retraction of its support wheels, and to provide stable wall contact pressure for the high-friction-coefficient driving wheels, enabling it to travel inside the pipe at a stable speed. Its core sensing unit is the aforementioned nine-axis MEMS-IMU module, which is rigidly fixed at the robot’s geometric center with its Y-axis aligned with the robot’s forward direction. As shown in Figure 10b, two PVC pipes, with lengths of 10 m and 12 m, respectively, were set up to simulate underground mine boreholes. Their geometric curved shapes were preset to mimic realistic borehole trajectories. Discrete points were marked along the pipeline’s axis, and their 3D spatial coordinates were extracted to construct the reference trajectory. A displacement sensor was mounted at the pipe entrance, with the end of its draw-wire rigidly connected to the robot’s rear. It was used for real-time measurement of the robot’s traveled distance along the pipe’s axis. To ensure measurement accuracy, the wire outlet was kept collinear with the pipe’s axis, and the angle between the wire and the pipe inlet plane was controlled within 0° to 2°. This setup is designed to minimize friction and lateral force components. To ensure communication reliability in potentially complex electromagnetic environments and over long-distance transmission, all sensors are connected to the data acquisition computer via the RS-485 bus, which supports long-distance transmission, and communicate using the industry-standard Modbus RTU protocol, thereby guaranteeing the packet-loss-free reception of the high-frequency data stream and timestamp synchronization.

Figure 10. Experimental platform for borehole trajectory measurement. (a) The custom-designed wheeled borehole surveying robot; (b) the experimental environment with PVC pipes used to simulate borehole trajectories; (c) the robot positioned at the entrance of the simulated borehole.

Based on the experimental platform described above, the continuous measurement method proposed in this paper aims to achieve rapid and accurate reconstruction of the borehole trajectory. First, during the measurement preparation stage, the measurement unit is kept stationary at the pipe entrance to collect an initial set of IMU data. This data is used not only to calculate the gyroscope’s static bias but also to solve for the initial attitude quaternion, $Q_0$. Subsequently, during the trajectory measurement stage, the robot is operated to travel through the entire pipeline in a single, complete pass at a nearly constant velocity (approx. 0.2 m/s). Throughout this process, both the IMU and displacement data are recorded simultaneously. Finally, in the data processing and trajectory reconstruction stage, a two-step pre-processing is performed. First, the calculated static bias is used to compensate the gyroscope’s angular velocity data. Subsequently, a low-pass filter is applied to suppress high-frequency noise in the data. Following the data pre-processing, the displacement data is interpolated using a cubic spline method. Based on the robot’s smooth motion and the extremely short 50 ms sampling interval, this interpolation can generate with high precision bore depth values that are precisely synchronized with the IMU data at each moment, rendering the introduced error negligible. Simultaneously, the attitude quaternion sequence, $Q\left(k\right)$, is computed from the IMU data via a complementary filter algorithm. Each quaternion in this sequence is then converted into a DCM, from which the pipeline’s inclination and azimuth angles at the current measurement point are resolved. Finally, the complete borehole trajectory is reconstructed through an iterative process based on the Mean Angle Tangential method. This involves calculating the 3D coordinate increment in the global frame at each timestamp using the attitude angles and the corresponding depth increment. By continuously accumulating these small displacement increments, the full trajectory is formed. A key advantage of this method is its ability to complete the survey in a single, continuous pass, which significantly enhances measurement efficiency.

Under the same experimental environment and sensor configuration, a control experiment was designed based on the operational principle of common mining inclinometers. This experiment employs a static, point-by-point discrete measurement mode, hereinafter referred to as the ‘discrete inclinometry method’. In this control experiment, the robot advances along the pipe’s axis in a step-wise manner. Whenever the displacement sensor reading increases by a fixed step length (set to ∆L = 10 cm in this study), the robot is brought to a complete stop. During this stationary period, a set of accelerometer and magnetometer readings is collected and averaged to mitigate sensor noise. Subsequently, the attitude angles at the current measurement point are calculated using analytical formulas derived for the specific coordinate systems defined in this paper, which represents a common approach for mining inclinometers [8]. The equations are as follows:

The roll angle ($\varphi$) and pitch angle ($\theta$) can be determined from the tri-axial accelerometer outputs $(a_x,a_y,a_z)$ using the following equations:

$$\varphi =\arctan 2\left(a_y,a_z\right)$$

(27)

$$\theta =\arcsin \left({\displaystyle \frac{a_x}{\sqrt{a_x^2+a_y^2+a_z^2}}}\right)$$

(28)

By combining the roll ($\varphi$) and pitch ($\theta$) angles obtained from the accelerometer with the tri-axial magnetometer outputs $\left(m_x,m_y,m_z\right)$, the yaw angle ($\psi$) can be calculated as follows:

$$\psi =\arctan 2\left(m_{x}cos\theta +m_{y}sin\varphi sin\theta +m_{z}cos\varphi sin\theta ,m_{y}cos\varphi -m_{z}sin\varphi \right)$$

(29)

Finally, at each measurement point, the calculated attitude angles are used to resolve the fixed step increment (∆L) into its 3D coordinate components in the global frame. The full borehole trajectory is then constructed by iteratively accumulating these coordinate increments in a piece-wise manner.

To systematically validate the effectiveness and feasibility of the method proposed in this paper, a comparative analysis of the two algorithmic approaches will be conducted. This comparison will focus on several key aspects, including attitude estimation accuracy, trajectory continuity, and the ability to suppress error accumulation.

3.2. Trajectory Error Analysis

Due to the combined effects of sensor noise, system coupling errors, and external disturbances, the calculated borehole trajectory inevitably deviates from its true path. This deviation manifests as spatial position offsets, local oscillations, or trend drift. To evaluate the accuracy of the proposed algorithm, it is necessary to quantitatively analyze the 3D trajectory error from multiple perspectives, including its spatial distribution, statistical characteristics, and global deviation trends.

First, a segment-wise calibration method was employed to establish a reference trajectory. Using a ruler, the 3D coordinate sequence of the red markers on the pipe, $P_i={\left[\begin{array}{ccc}{x}_i& y_i& z_i\end{array}\right]}^T (i=1, 2, 3,\cdots \cdots M_1)$, was acquired. This set of points ${\left\{P_i\right\}}_{i=1}^{M_1}$ constitutes the discrete reference trajectory, describing the actual geometric path of the pipeline. The coordinate sequence of the calculated trajectory is denoted as $\acute{P_j}={\left[\begin{array}{ccc}\acute{x_j}& \acute{y_j}& \acute{z_j}\end{array}\right]}^T (j=1, 2, 3,\cdots \cdots M_2)$. To ensure a comparable sampling resolution along the pipe’s primary axis, cubic spline interpolation was used to perform curve fitting and resampling on both trajectories, such that $M_1\approx M_2$.

To establish a unified longitudinal interval for error measurement and to avoid comparison errors arising from inconsistent endpoints, the initial orientation of the pipe (the Y-axis) was used as a reference. The upper bound for comparison was set to the minimum of the two trajectories’ final endpoint projections onto the Y-axis. The length of the error calculation interval is thus defined as:

$$L=\min \left\{y_{M1},\acute{y_{M2}}\right\}$$

(30)

Starting from the same initial point, a total of $N=L/∆L$ error measurement points are defined along the Y-axis at fixed intervals of ∆L. The longitudinal position of each sampling point is given by:

$$y^{\left(k\right)}=k·∆L, k=1,2,\cdots ,N$$

(31)

For each longitudinal position $y^{\left(k\right)}$, a corresponding point is found on both the reference and the calculated trajectories whose Y-axis projection is closest to this value:

$$P^{\left(k\right)}=\arg \underset{P_i}{\min }\left|y_i-y^{\left(k\right)}\right|$$

(32)

$${\acute{P}}^{\left(k\right)}=\arg \underset{{\acute{P}}_j}{\min }\left|y_j-y^{\left(k\right)}\right|$$

(33)

The 3D Euclidean error, ${\delta }^{\left(k\right)}$, at the k-th measurement position is then defined as:

$${\delta }^{\left(k\right)}={‖P^{\left(k\right)}-{\acute{P}}^{\left(k\right)}‖}_2=\sqrt{{\left(x_k-\acute{x_k}\right)}^2+{\left(y_k-\acute{y_k}\right)}^2+{\left(z_k-\acute{z_k}\right)}^2}$$

(34)

Finally, the trajectory error is quantitatively evaluated from different perspectives using three metrics: ${\sigma }_{max}$ for the maximum deviation, ${\sigma }_{mean}$ for the overall average deviation, and ${\sigma }_{MSE}$ for the error fluctuation. Their formulas are as follows:

$${\sigma }_{max}=\underset{k=1,2,\cdots N}{\max }{\delta }^{\left(k\right)}$$

(35)

$${\sigma }_{mean}={\displaystyle \frac{1}{N}}\sum _{k=1}^N{\delta }^{\left(k\right)}$$

(36)

$${\sigma }_{MSE}={\displaystyle \frac{1}{N}}\sum _{k=1}^N{\left({\delta }^{\left(k\right)}-{\sigma }_{mean}\right)}^2$$

(37)

3.3. Comparison of Attitude Estimation Algorithms

To evaluate the accuracy of the Complementary Filter algorithm in attitude estimation, a quantitative comparison was conducted with a static solution algorithm and the Extended Kalman Filter algorithm. The experiment was designed with two test conditions: static and dynamic. The static experiment serves to evaluate the algorithm’s output accuracy and drift suppression capability without external motion disturbances, while the dynamic experiment, through the simulation of vigorous motion, assesses the algorithm’s dynamic response, tracking accuracy, and robustness.

As shown in Figure 11a, under static conditions, the estimation curve of the Complementary Filter algorithm (green curve) is smooth and continuous, effectively filtering out the high-frequency noise from the accelerometer and magnetometer. However, over long-term observation, a slight linear drift, caused by the integration of the gyroscope’s residual bias, can still be observed in the pitch and roll angles. The Extended Kalman Filter curve (magenta curve) highly coincides with the reference angles, and its output shows no significant drift. This is attributable to the online estimation and compensation mechanism for gyroscope bias within its algorithm model, which effectively suppresses long-term integration error. In contrast, the pitch and roll angles output by the static solution method exhibit significant step-like fluctuations, while its yaw angle output shows large-amplitude random noise, because in a static horizontal state, the accelerometer cannot provide any yaw information, and the yaw estimation relies entirely on the magnetometer, which is susceptible to environmental interference.

Figure 11. Performance comparison of attitude estimation algorithms under static and dynamic tests. (a) Attitude estimation results under static conditions. (b) Attitude estimation results under dynamic conditions.

To further examine the algorithm’s robustness under complex motion conditions, the experimental results under dynamic conditions are presented in Figure 11b. Based on the complementarity of the three sensor types in the frequency domain, the Complementary Filter algorithm enables the attitude estimation curve to remain smooth and stable while the carrier is undergoing large-amplitude motion. It not only significantly suppresses the Euler angle drift problem caused by gyroscope integration error but also reduces the instantaneous angular “spikes” or “oscillations” that occur when the accelerometer is disturbed. The EKF also demonstrates optimal dynamic tracking performance, with its estimates highly coinciding with the reference angles. As an optimal estimation algorithm, the EKF achieves precise tracking of the true attitude by dynamically updating the Kalman gain to adaptively balance the prediction model against external measurements. However, the static solution method (blue curve), which relies solely on the accelerometer and magnetometer, reveals its inherent limitations. During rapid rotation or abrupt attitude changes of the carrier, the accelerometer’s output data fluctuates violently, manifesting as “jumps” in the pitch and roll angles. Meanwhile, its yaw angle estimation also deviates significantly from the reference yaw angle because it directly depends on the magnetometer, which is highly sensitive to external magnetic interference.

From this, it can be concluded that, compared to the static solution method, the EKF exhibits the best performance in both static drift suppression and dynamic tracking by virtue of its optimal estimation theory. The Complementary Filter, on the other hand, achieves comparable dynamic performance with extremely low computational complexity, demonstrating high practical value in resource-constrained systems.

3.4. Trajectory Reconstruction Results and Performance Discussion

To validate the effectiveness of the borehole trajectory reconstruction method proposed in this paper, a comparison was made between the experimentally obtained trajectories and the reference trajectory, as shown in Figure 12. The red curve represents the reference trajectory of Pipe 1. The blue and black curves depict the trajectories obtained using the proposed complementary filter-based continuous reconstruction method and the control discrete inclinometry method, respectively. The blue curve accurately resolves the geometric profile of the pipe, with its trend showing high consistency with the reference path. In contrast, the discrete inclinometry method exhibits an azimuth deviation from the very beginning of the measurement. As the measurement distance increases, its issues with error accumulation and susceptibility to environmental disturbances become more pronounced.

Figure 12. Three-dimensional comparison of reconstructed and reference trajectories for Pipe 1.

This is particularly evident in the middle section of the pipe (approximately the 5–8 m segment), where the trajectory displays irregular lateral drift and path tortuosity, which completely contradicts the smooth profile of the pipe. The root cause is that the discrete inclinometry method directly relies on quasi-static, deterministic observations from the accelerometer and magnetometer to calculate the azimuth angle. In the complex experimental environment, the magnetometer’s readings are corrupted by a combination of its own high-frequency random noise and local, time-varying magnetic field distortions from the surroundings. Lacking an effective filtering and fusion mechanism, this method incorrectly interprets these significant error components as heading changes, while the attitude error accumulates through a path with no feedback, ultimately leading to severe geometric distortion of the trajectory.

Figure 13 further presents the projections of the reconstructed trajectories onto the XY (horizontal) and YZ (vertical) planes. This 2D perspective allows for a deconstruction of the 3D error characteristics and path deviation trends.

Figure 13. Comparison of projections of the reconstructed and ground truth trajectories for Pipe 1 onto the horizontal and vertical planes. (a) Projection onto the horizontal plane (XY view). (b) Projection onto the vertical plane (YZ view).

In the XY-plane projection (Figure 13a), which illustrates the lateral continuity and deviation of the trajectories, the path reconstructed by the complementary filter fused with high-frequency displacement data is notably smoother, showing no abrupt changes or sharp angles. In contrast, the trajectory from the discrete inclinometry method exhibits a large lateral deviation from the very beginning. As the path extends, this issue is compounded by multiple “misalignments” and “offsets” at the pipe connection points, leading to continuous error accumulation.

Figure 13b highlights the differences between the two methods in the vertical direction. The projection curve generated by the proposed method maintains a similar undulation rhythm to the reference path. Although a progressive vertical deviation appears in the latter half of the trajectory, its overall smoothness is well-preserved. The YZ-plane projection of the discrete method, however, suffers from two major issues. First, due to its significant lateral tortuosity in the XY-plane, its effective forward travel distance along the Y-axis is severely shortened. This directly results in its trajectory in the YZ projection failing to reach the endpoint, making it shorter than both the proposed method’s and the reference path’s trajectories. Furthermore, in terms of morphology, its path is not a smooth curve but rather a polyline composed of discrete measurement points, characterized by irregular serrations and severe distortion.

To transform the preceding qualitative morphological comparison into a precise quantitative evaluation, Figure 14 illustrates the evolution of the X-direction error, Z-direction error, and the cumulative spatial error for different methods as a function of the Y-axis coordinate during the trajectory reconstruction process.

Figure 14. Component and Total Errors of the Proposed and Control Methods along the Y-axis.

In the lateral direction (X-direction), the error of the proposed method (blue curve) remains consistently within 0.2 m, exhibiting almost no significant drift. This result is in complete agreement with its smooth trajectory that closely follows the reference path in the XY projection. In contrast, the lateral error of the discrete inclinometry method (red curve) increases rapidly at the initial stage of the measurement, exceeding 0.5 m at Y = 1 m. It then drops sharply before embarking on a continuous accumulation process after Y = 2.3 m, accompanied by large oscillations. The maximum deviation exceeds 2 m, eventually stabilizing at 1.6 m at the end of the measurement.

In the longitudinal direction (Z-direction), the errors of both methods show a cumulative trend with distance. The longitudinal error generated by the proposed method grows very gently and smoothly, reaching a final error of 0.25 m. In comparison, the final error for the discrete inclinometry method is 0.31 m. Although this value is not substantially different from that of the proposed method, its error curve does not exhibit smooth growth; instead, it is characterized by noticeable irregular fluctuations throughout the overall accumulation process.

The cumulative error curve reveals that the proposed method maintains a low level of accumulated error throughout the entire range, without showing a clear distance-dependent cumulative drift. The cumulative error curve of the discrete inclinometry method closely resembles its lateral error curve, indicating that the lateral error component, induced by incorrect heading estimation, is dominant. This error exhibits an approximately linear or even super-linear accumulation trend, reaching a maximum value of 2.064 m at Y = 7.5 m.

To provide a more concise and objective evaluation of the overall algorithm performance from a statistical perspective, Figure 15 presents a visual comparison of key statistical metrics for the directional errors of the two methods using a bar chart. The results show that the proposed method, which fuses a complementary filter with high-frequency displacement data, comprehensively outperforms the traditional discrete inclinometry method across all metrics.

Figure 15. Comparative Analysis of Statistical Metrics for Trajectory Reconstruction Error.

Specifically, the proposed algorithm significantly enhances the system’s robustness by reducing the total maximum error from 2.078 m to 0.308 m, effectively suppressing the occurrence of extreme errors. In terms of accuracy, the total mean error is reduced from 1.028 m to 0.153 m. Furthermore, regarding the root mean square error (RMSE), which reflects the error’s dispersion and overall stability, our algorithm’s total RMSE is only 0.079 m, far lower than the control algorithm’s 0.625 m. This indicates that its output not only has a smaller deviation but also exhibits lower volatility. These findings provide strong evidence that the proposed fusion strategy can effectively overcome the drift issues of the traditional method, achieving a comprehensive performance improvement in positioning accuracy, stability, and robustness.

Building upon the measurement accuracy previously demonstrated in horizontal boreholes, this experiment was designed to further validate the adaptability and robustness of our method for long-distance, complex trajectories with large angle variations. For this purpose, Pipe 2 was used, which is longer and features a significant spatial geometry change—transitioning from a horizontal section to a steeply inclined ascent. This setup imposes more stringent requirements on the accuracy and stability of the trajectory reconstruction algorithm.

Figure 16 presents the trajectory reconstruction results for Pipe 2 from both the proposed complementary filter-based fusion method and the traditional discrete inclinometry method. As shown in the figure, the trajectory reconstructed by our proposed method (blue curve) exhibits a high degree of consistency with the reference trajectory (red curve) in its overall trend. Whether in the initial horizontal segment or the subsequent inclined ascending segment that undergoes a large spatial bend, our method’s trajectory accurately reflects the 3D orientation and morphological features of the pipe. This indicates that the precise attitude angles obtained via the complementary filter provide a reliable directional reference for the displacement vectors in 3D space. Concurrently, the high-frequency displacement fusion strategy ensures that these vectors are accurately accumulated in the global coordinate system, effectively suppressing error divergence.

Figure 16. Three-dimensional comparison of reconstructed and ground truth trajectories for Pipe 2.

In contrast, the discrete inclinometry method, although showing relatively high accuracy in the initial stage, reveals its inherent limitations as the path length increases. Specifically, during long-distance dynamic operation, the complex magnetic environment and dynamic disturbances cause a continuous cumulative drift in the azimuth angle. Furthermore, the motion-induced acceleration itself becomes a major source of interference for the inclination measurement, thereby significantly degrading its estimation accuracy. As depicted by the black curve in Figure 16, after the travel distance exceeds 1 m, the black curve progressively deviates from the reference trajectory, with errors accumulating rapidly. The trajectory’s azimuth and inclination undergo drastic changes, causing it to exhibit large-amplitude, irregular oscillations in 3D space and completely lose its expected smoothness.

To conduct a more detailed qualitative analysis of the aforementioned error characteristics from different perspectives, Figure 17 provides a comparison of the trajectory projections from the two algorithms onto the XY and YZ planes for the Pipe 2 experiment.

Figure 17. Horizontal and vertical projections of the reconstructed trajectories against the ground truth for Pipe 2. (a) Top-down view showing lateral deviation. (b) Side view illustrating the vertical path.

The trajectory reconstructed by the proposed algorithm exhibits a smooth and continuous curve in both orthogonal projection planes, free of any serrated or abrupt changes. It demonstrates an exceptionally high responsiveness to changes in the path’s geometry. When the reference trajectory undergoes a lateral turn at Y = 6 m and an upward incline at Y = 8 m, the algorithm achieves synchronous and precise tracking, with only minor deviations throughout the process, strongly demonstrating that the robot platform’s multi-point elastic support structure effectively absorbed the minor vibrations at the pipe joints, while the complementary filter algorithm accurately tracked the rapid angular changes, and the millimeter-level spatial sampling density ensured the high-fidelity reconstruction of the sharp bend’s geometry.

Conversely, the discrete inclinometry method suffers from rapid accumulation of lateral errors due to its azimuth estimation inaccuracies, causing its trajectory to fail to match the direction of the reference path and exhibit large-scale, irregular oscillations. Although the trajectory shows an upward-inclining trend to some extent, thanks to the use of the accelerometer for inclination measurement, its projection on the YZ-plane presents the same issue observed in the Pipe 1 experiment. Specifically, its effective forward travel distance along the Y-axis is severely shortened. This results in the reconstructed trajectory in the YZ projection being significantly shorter than those of the proposed method and the reference path, failing to extend to the final endpoint.

To further refine the preceding analysis on geometric deviations and to investigate the distribution and characteristics of errors in different dimensions, Figure 18 provides a quantitative analysis of the reconstruction errors from both algorithms in the Pipe 2 experiment. It presents the evolution of the X-direction, Z-direction, and total errors.

Figure 18. Component and Total Errors of the Proposed and Control Methods for Pipe 2 along the Y-axis.

The error of the proposed algorithm demonstrates stability across all dimensions. Although its lateral (X-direction) error shows an extremely slow linear growth trend with travel distance, reaching 0.368 m at the end of the path, it does not exhibit uncontrolled non-linear divergence like the control method. The vertical (Z-direction) error increases slowly with distance, peaks at 0.168 m at Y = 8 m, and then flattens out with a slight decrease, reducing to 0.06 m at Y = 9.3 m. The growth of the total error is gentle and controllable, remaining within an acceptable range.

In stark contrast, the lateral (X-direction) error of the discrete inclinometry method shows a distinct point of abrupt change at Y = 1.65 m, from which the error begins to grow rapidly in a non-linear fashion, eventually stabilizing above 3 m. Its vertical error exhibits a slow growth trend before Y = 6.7 m, after which it follows a nearly linear, monotonically increasing trend with distance, with the final error exceeding 0.7 m. The total error curve closely resembles the lateral error curve, also increasing sharply at Y = 1.6 m and reaching a final error of nearly 3.5 m.

To accurately assess the overall error and stability of the two algorithms from a macroscopic perspective, Figure 19 further summarizes and compares their key error statistical metrics in each direction. First, regarding the maximum error metric, the proposed algorithm’s errors in the lateral, vertical, and total components are 0.368 m, 0.168 m, and 0.373 m, respectively. The error peaks are constrained to within 0.4 m. In contrast, the maximum lateral and total errors of the control method are as high as 3.466 m and 3.469 m, respectively, confirming that its estimation system lacks the necessary robustness for long-distance dynamic scenarios.

Figure 19. Comparative Analysis of Statistical Metrics for Trajectory Reconstruction Error on Pipe 2.

Second, concerning the mean error metric, which reflects the overall error level, the total mean error of our algorithm is only 0.180 m and remains on the same order of magnitude as its maximum error. The discrete inclinometry method, however, has a mean error of 2.089 m, indicating that its reconstructed trajectory exhibits a severe overall deviation.

Furthermore, for the root mean square error (RMSE), an indicator of error dispersion, our algorithm’s total RMSE is 0.121 m, a value close to its mean error. This suggests the absence of extreme outliers that could significantly impact the overall trajectory accuracy. The traditional method’s RMSE, in contrast, is 1.115 m. This quantitative comparison of key metrics—from maximum deviation and overall level to error distribution—comprehensively and forcefully demonstrates the advantages of our proposed algorithm in terms of estimation accuracy, stability, and robustness.

In addition to its significant advantages in measurement accuracy and robustness, the method proposed in this paper also achieves a substantial improvement in surveying efficiency. The following section provides a comparative estimation of the time cost for the data acquisition phase of both methods, based on the experimental parameters.

The proposed method employs a continuous measurement mode, completing the data acquisition in a single, uninterrupted pass. According to the experimental setup, the robot travels at an average velocity of approximately 0.2 m/s. Therefore, the theoretical time required to survey the 10-meter-long Pipe 1 is about 50 s, and for the 12-meter-long Pipe 2, it is about 60 s. In contrast, the time cost of the traditional discrete inclinometry method consists of the cumulative cycles of ‘travel-stop-static measurement’. For Pipe 1, with a measurement step of 10 cm, a total of 100 measurements are required. Based on experimental records, the average time for each cycle is approximately 3.5 s, resulting in a total measurement time of up to 350 s.

This quantitative estimation reveals that, for the same surveying task, the proposed method significantly reduces the time cost. Furthermore, it lowers the need for on-site manual intervention, thereby reducing labor costs and the potential for operational errors.

4. Conclusions

This paper, targeting technical bottlenecks in current mine borehole trajectory measurement methods, such as low operational efficiency, insufficient measurement accuracy, and poor environmental adaptability, proposes a robot-based method for the rapid and precise reconstruction of underground mine borehole trajectories. The method uses a robot as the measurement carrier, replacing the traditional “point-by-point stop-and-go” operation with a single continuous traversal. It utilizes a Complementary Filter to acquire precise attitude in dynamic environments, subsequently achieves temporal synchronization of attitude and external displacement data through cubic spline interpolation, and finally, based on the Mean Angle and Full Range method, performs recursive accumulation on the synchronized “attitude–displacement increment” data pairs, thereby reconstructing the complete three-dimensional trajectory. The experimental results show that the proposed method completes borehole data acquisition in a single, uninterrupted pass, which fundamentally eliminates the substantial non-measurement auxiliary time inherent in the traditional stop-and-go mode, thereby achieving a significant improvement in operational efficiency. Furthermore, compared to the severe trajectory deviation and geometric distortion caused by error accumulation in traditional methods, the fusion strategy of this paper achieves a remarkable enhancement in measurement accuracy. In conclusion, this study not only validates the feasibility of using industrial-grade MEMS sensors and robotic technology to achieve the high-precision, rapid measurement of borehole trajectories but also provides a novel and efficient technical pathway for the automation of routine borehole trajectory measurement in mines. Future work will focus on translating this research from a laboratory prototype to an industrial-grade application, and will proceed along three dimensions: algorithm enhancement, physical platform optimization, and application expansion. At the algorithm level, the focus will be on enhancing the data preprocessing capability to enable online, real-time estimation and correction of the sensor’s random drift, and on identifying and compensating for complex non-linear errors by constructing data-driven models. Building on this, the robot platform itself will also be enhanced through methods such as introducing an articulated structure and optimizing the drive system to improve its adaptability to real physical environments like high-curvature and rough borehole walls. Simultaneously, based on the currently achieved single-agent autonomy, the exploration of a robot cluster system that supports parallel surveying will be pursued, laying a solid foundation for the realization of large-scale, high-efficiency borehole trajectory measurement in future smart mines.