Ultra-wideband pose detection system for boom-type roadheader based on Caffery transform and Taylor series expansion

To realize unmanned pose detection of a coalmine boom-type roadheader, an ultra-wideband (UWB) pose detection system (UPDS) for a roadheader is designed, which consists of four UWB positioning base stations and three roadheader positioning nodes. The positioning base stations are used in turn to locate the positioning nodes of the roadheader fuselage. Using 12 sets of distance measurement information, a time-of-arrival (TOA) positioning model is established to calculate the 3D coordinates of three positioning nodes of the roadheader fuselage, and the three attitude angles (heading, pitch, and roll angles) of the roadheader fuselage are solved. A range accuracy experiment of a UWB P440 module was carried out in a narrow and closed tunnel, and the experiment data show that the mean error and standard deviation of the module can reach below 2 cm. Based on the TOA positioning model of the UPDS, we propose a fusion-positioning algorithm based on a Caffery transform and Taylor series expansion (CTFPA). We derived the complete calculation process, designed a flowchart, and carried out a simulation of CTFPA in MATLAB, comparing 1000 simulated positioning nodes of CTFPA and the Caffery positioning algorithm (CPA) for a 95 m long tunnel. The positioning error field of the tunnel was established, and the influence of the spatial variation on the positioning accuracy of CPA and CTFPA was analysed. The simulation results show that, compared with CPA, the positioning accuracy of CTFPA is clearly improved, and the accuracy of each axis can reach more than 5 mm. The accuracy of the X-axis is higher than that of the Y- and Z-axes. In section X-Y of the tunnel, the root mean square error (RMSE) contours of CTFPA are clear and orderly, and with an increase in the measuring distance, RMSE increases linearly. In section X-Z, the RMSE contours are concentric circles, and the variation ratio is nonlinear.


Introduction
Coal mining is an important energy source to support the development of our society [1]. The total length of newly built tunnels in China's large and medium-sized coalmines per year is more than 8000 km [2]. A roadheader is the core equipment used for the tunnelling of an underground coalmine [3]. The production conditions of the working surface for coalmine tunnelling are very particular, with a narrow working space, appalling visual environment, and considerable dust and noise pollution. Some mines also suffer from high temperatures, high humidity, and roof collapses. Under such a working environment, the work efficiency of the miners is low, and the miners may be prone to errors or experience frequent accidents [4]. Therefore, unmanned mining equipment need to be developed urgently.
In many engineering field, the coordinates and posture angles of the target are important physical quantities that need to be determined. In [5], a novel and cost-effective fusion methodology is proposed and applied in the Inertial Navigation System (INS)/Global Positioning System (GPS)/ compass integrated positioning system to realize accurate and reliable positioning for land vehicles during GPS outages. In [6], a new enhanced tightly coupled cooperative positioning technique is presented by adding UWB based inter-vehicular range measurements. Reference [7] describes an experimental study into the detection and location of underground cables using magnetic field measurements. Reference [8] presents a comprehensive literature review of current progress in the application of state-of-the-art indoor positioning systems for telecare and telehealth monitoring. Reference [9] presents an integrity monitoring methodology for a proposed system, which includes Real-time kinematic Global Navigation Satellite System integrated with low-cost Inertial measurement unit and automotive speedometer to ensure trustworthy precise positioning for advanced vehicle positioning. A new underground localization algorithm using a re-measurement of the sequence of the magnetic field and pose graph simultaneous localization and mapping for directional drilling is proposed in [10].
Pose detection for roadheader is the key to realising unmanned tunnelling. In the traditional tunnelling process of underground mining, the route of the roadheader is pre-set using a surveyor. A point laser transmitter is installed at the back roof of the tunnel, which emits a laser to the front coal wall of the tunnel. The roadheader driver controls the tunnelling direction of the roadheader according to the position of the laser spot. Under appalling working environment, not only is this tunnelling method inefficient, it also has limited accuracy. In addition, the installation, disassembly, and calibration of the laser transmitter are very troublesome, and greatly increase the intensity and amount of work for the miners. In order to solve the existing technical problems, a number of remote pose detection techniques for roadheader are developed in recent years.
According to a previous study [11,12], INS is used to measure the pose parameters of the roadheader, but the method cannot establish the relationship between the roadheader and the roadway, and the system error increases rapidly with the increase of the running time of the system. Laser technology has been used to measure the posture parameters of roadheader [13,14], but there are many limitations in this method: there is a lot of dust in the underground environment, which seriously affect the transmission of infrared, laser and visible light, resulting in low measurement accuracy.
A method based on machine vision for roadheader pose detection that was tested in simulated tunnels was also proposed [15]. This method uses a laser target as the information source. By analysing the characteristics of the cross laser imaging, the pose angle of the roadheader and the offset information of the X-and Z-axes can be calculated. The method combines machine vision with laser detection technology, which significantly increases the complexity and uncertainty of the system. In addition, the method cannot be used to obtain distance information in the Y-axis direction of the tunnel, that is, the absolute position of the roadheader relative to the tunnel coordinate system cannot be determined, and a major flaw exists in the path planning of the roadheader. Furthermore, a large amount of dust in the coal tunnel will significantly affect the accuracy of the system, making it difficult to meet the actual work requirements.
A roadheader positioning method based on an indoor GPS was also proposed [16]. Two rotating fan laser transmitters emit four laser signals to the laser receiver on the roadheader fuselage. The time difference in the received signal is computed as four-angle information, which is collected to obtain the three-dimensional (3D) coordinates of the positioning node of the fuselage. However, the system has high complexity, high cost, and a low measuring range. The positioning accuracy of 7 m achieved in a simulated tunnel environment is far lower than the actual accuracy required. Therefore, the method is not suitable for a narrow and closed tunnel environment. Several of these techniques mostly stay have remained mainly in the theoretical design stage, with the development of a less mature system and cannot solve the actual technical problems.
In this study, an ultra-wideband (UWB) pose detection system (UPDS) of a boom-type roadheader for a coal tunnel is introduced. Three nodes of the roadheader fuselage are measured by four UWB base stations located behind the tunnel through UWB ranging technique. Based on the 12 sets of distance information, the position coordinates and posture parameters of the roadheader are obtained. The factors affecting the UPDS positioning error are analysed, and an accuracy and precision experiment carried out using a UWB P440 module in a narrow tunnel is described. In view of the time-of-arrival (TOA) positioning model of the UPDS, we propose a Fusion Positioning Algorithm based on a Caffery transform and Taylor series (CTFPA), where the CTFPA for the complete calculation process is deduced, a flowchart of the CTFPA is designed, the accuracy of the CTFPA is analysed using MATLAB, the UPDS positioning error field (PEF) in a narrow space is established, and the influence of spatial change on the positioning accuracy in a long narrow tunnel is determined. After analysis, UPDS can meet the needs of remote pose detection of roadheader, 3D coordinate and posture parameters of roadheader can be detected in real time, which provides an important basis for unmanned tunnelling.

Principle of UPDS
With the UPDS, the key problem is to realize the positioning of the nodes of the roadheader fuselage, which is achieved by obtaining the absolute position of the positioning nodes in the reference coordinate system of the tunnel. The attitude information (heading, pitch, and roll angles) of the roadheader can be calculated according to the coordinates of the three positioning nodes of the fuselage. As shown in figure 1, to realize the positioning of the roadheader nodes, the physical parameters (amplitude, frequency, phase, and time) between the fuselage positioning node and the tunnel coordinate system should first be obtained. Next, the original physical parameters are processed to obtain the positioning parameters (wave direction, distance, distance difference, and height). Within a particular 3D space, the locus of a point that maintains a constant positional parameter is a surface, which is called the position plane [17]. A set of observation equations were determined according to a plurality of positioning parameters, and the intersection point of the position plane can be obtained by solving these equations. The coordinates of the target location of the fuselage are obtained. Classic localization models include the received signal strength, TOA, angle-of-arrival, and time difference of arrival [18][19][20][21][22][23].
Generally speaking, the accuracy of the original positioning parameters directly affects the accuracy of the position and attitude calculations. An underground environment is also dark, and the accuracy of the machine-vision positioning technology is severely limited. Tunnel dust is also a serious problem, and directly interferes with the propagation of visible and infrared light, thereby affecting the reliability of the system. Inertial navigation technology is an independent measurement technology that cannot establish a proper relationship with the actual tunnel conditions. With an increase in the tunnelling time, the error drift is serious. Moreover, because of the limitations of the waveform and frequency, the accuracy of acoustic positioning technology is not high.
A UWB signal is a non-sinusoidal pulse radio signal. Its frequency band is very wide, and its signal pulse is quite steep.
It has a high time resolution and anti-interference capability. In addition, the two-way flight of time (TW-TOF) ranging method has high accuracy. By measuring the TW-TOF of the signal between the two UWB modules, the relative distance between them is calculated. This method can make full use of the high time resolution characteristics of UWB signals, and avoid errors caused by clock asynchrony between two modules, and ranging information can therefore be accurately computed.

UPDS scheme
Based on the TW-TOF ranging method of UWB signals, a pose detection system for a roadheader is designed in this study. By measuring the distance between three fuselage nodes and four base stations, a set of TOA positioning equations is established. By calculating the focus of the four spherical position planes, the 3D coordinates of the fuselage nodes of the roadheader are obtained, and the attitude angles of the roadheader are determined according to the coordinates of the three nodes of the fuselage.
The system is shown in figure 2. The UWB ranging modules are installed in the three fuselage nodes (E, F, and G) and four positioning base stations (A, B, C, and D) that are located in the rear of the tunnel. The coordinates of the base stations are calibrated by a surveyor prior to tunnelling. The base  stations measure the distance of the fuselage nodes separately. According to the distance measurement information, a set of position equations is established, and the 3D coordinates of the three fuselage nodes are calculated.
The coordinates of base station i (i = A, B, C, and D) are ( x i , y i , and z i ). The position coordinates of the fuselage node E are ( x E , y E , and z E ). The distance between base station i and the fuselage node E is d iE (i = A, B, C, and D). The TOA positioning observation equations between base stations A, B, C, and D and the fuselage node E are therefore By solving equation (1), the 3D coordinates of node E can be obtained. Similarly, the coordinates of the fuselage node F ( x F , y F , and z F ) and point G ( x G , y G , and z G ) can be obtained. As shown in figure 3, which is a vertical view of the system, L 1 and L 2 are the known distances of the roadheader fuselage. The heading angle α of the roadheader can be calculated according to equation (2): The front view of the system is shown in figure 4, and the pitch angle of the roadheader is solved according to equation (3): A side view of the system is shown in figure 5, and the roll angle of the roadheader is solved according to equation (4):

Fusion positioning algorithm based on Caffery transform and Taylor series expansion
Equation (1) is a nonlinear equation for x E , y E , and z E . It is not easy to directly solve nonlinear equations in an engineering project. Moreover, a UWB ranging error of the measurement is inevitable and the introduction of which will cause a spherical surface position to not intersect at a single point. Therefore, the positioning problem of a roadheader is converted into a nonlinear optimal estimation problem. For the TOA positioning model, the most efficient methods for a nonlinear optimal estimation are the Caffery algorithm and Taylor series method [17]. The Caffery algorithm replaces the original spherical position surface with the plane position surface [24]. The original nonlinear equations are then transformed into linear equations and the least square method is then used to estimate the position of the target.
The core idea of the Taylor series positioning algorithm [25] is expanding the initial estimate of the target position, in which the high power terms are ignored, converting the nonlinear equations into linear equations, and using the least square method to estimate the offset of each axis. The initial coordinate is then corrected using the estimated offset value, and the estimated target position is approximated as a real position through cyclic iterations, and thus the optimal estimation of the target position is finally obtained.

Caffery positioning algorithm
The Caffery positioning algorithm (CPA) for a roadheader is as follows: In equation (1), the Nth equations are subtracted from the (N + 1)th equations, and the linear equation of x E , y E , and z E are obtained: Equation (5) are transformed into a matrix form (6): Based on equation (6), The least square solution (7) of the 3D coordinates of fuselage node E is obtained using rank (A) = 3.

Taylor series expansion positioning algorithm
CPA and Taylor series expansion positioning algorithm (TPA) have their own advantages and disadvantages. CPA can obtain an initial point coordinate through a direct calculation, which uses a concise process. However, since the CPA is a non-iterative algorithm, the original ranging error is linearly related to the positioning error. When the original range error is very large, the positioning accuracy is not high. TPA has a higher positioning accuracy, but it requires an initial point to carry out a Taylor expansion; in addition, the selection of the initial point will affect the astringency of the algorithm iterations.
In this section, CPA and TPA are fused. After the initial point coordinates are computed by CPA, TPA is adopted to carry out the cyclic iterations to eliminate any errors. Thus, more accurate 3D coordinates of the roadheader nodes are obtained.
The steps of TPA are as follows: (1) The x E , y E , and z E values calculated in equation (7) are used as the initial values of the fuselage node E of the roadheader, and the error is set as δ X , δ Y , and δ Z for the true values ( x E0 , y E0 , and z E0 ), as shown in (8): (2) Equation (1) is expanded using a Taylor series in x E , y E , and z E , and the two powers and the higher term are ignored, as shown in (9):

DE
(9) Convert equation (9) into equation (10): Equation (10) is transformed into a matrix form in (11): In (11), The least square solution of δ is (3) Determine whether ε = |δ x | + |δ y | + |δ z | is less than the given threshold ε 0 , and set ε 0 = 0.1. If ε is greater than the threshold ε 0 ., then (13) is used instead of the last estimate of the target, and steps 2 and 3 are repeated (4) If ε is less than the threshold, the iteration is stopped, and the final estimate of the position of node E(

Positioning error analysis for UPDS
To analyse the position error of the system, in (5) (14), In addition, from (14), When ∆d obeys a Gaussian distribution of zero mean, the mean value of ∆X is If each range parameter is independent and the variance is the same as δ 2 d , then the covariance of the ranging error is In equation (16), 'I' is the unit matrix. The covariance of the position error is (17) In equation (17), The analysis results show that there are three main factors affecting the positioning accuracy of the system: the original range error, the position of the target, and the method for calculating the target position. As shown in figure 6(a), range error will directly affect the positioning accuracy. With an increase in the range error, the intersection area of the position surface increases, and the target coordinate estimation becomes less accurate.
As shown in figure 6(b), the covariance of the position error is not only related to the covariance of the ranging information, but also to the position of the target. As the distance between nodes and stations increases, the intersection area of the position surface increases.
As shown in figure 6(c), when there is no nonlinear treatment on the positioning observation equations, the rate of change in the Z-axis error will be far greater than that of the X-and Y-axes, and thus the main factors of the positioning calculation methods also affect the positioning accuracy.

UWB module P440 ranging experiment
Measurement precision is defined as the consistency of a group of observations about the mean value. However, the mean value may be biased; accuracy is denoted as the closeness of the measurements to the true value [26]; accuracy and precision reflect the degree of exactness of the measurement. To analyse the positioning accuracy and precision (AP) of the system in a narrow and closed tunnel, the distance measurement AP of the system should first be determined. The UWB P440 module of the American Time domain company was chosen as the ranging sensor for the UPDS, which has a bandwidth of 3.1-5.3 GHz, and a centre frequency of 4.3 GHz.
In addition, the TW-TOF ranging method was adopted. The P440 module is shown in figure 7. The broadcast antenna is connected to the module using Port A.
The experiment procedures are as follows: (1) the P440-1 is connected to a PC, (2) the RangeNet time domain software is run, (3) the software is tuned into the Range Tab, as shown in figure 8, (4) the P440-1 module is set to the base station, (5) the P440-2 module is connected to the PC, (6) the software is run, (7) set P440-2 to a mobile node, and (8) the P440-2 module is then disconnected from the PC.
The P440-1 and P440-2 modules are placed on two tripods using a heavy punch calibration to ensure that the two antenna phases are centred at the same height and are along the same line. The broadcast antenna is connected to the module, and a mobile power source is suspended below the tripod to ensure that the module has power. The system structure used in the ranging experiment is shown in figure 9.
The distance between the phase centres of the two modules is calibrated using a laser range finder and recorded. The P440-2 module is moved forward, and the above measurements are repeated at regular intervals to verify the ranging performance of the module in a narrow closed tunnel. The actual object used in the ranging measurement is shown in figure 10.
The experiment was carried out in a similar environment as the long and narrow enclosed space of an underground mine. The parameters of the experiment site are shown in figure 11.
Mobile node P440-2 is set every 10 m for measurement. Each node position is measured 1000 times. The signal interval is set to 100 ms, as shown in the ranging experiment data listed in table 1: The UWB measurement data in table 1 are interpolated to obtain the error distribution curve based on the distance. As shown in figure 12, the module has an error peak at 30-50 m, which is due to changes in the environment at that location.   However, the AP of the module in a normal narrow and closed environment can meet the requirements, and the mean error (accuracy) and standard deviation (precision) are both below 0.02 m.
To investigate the error distribution of ranging data of the P440 module, the ranging data of nodes placed at 20, 35, 57, and 90 m were selected for a statistical analysis. A frequency histogram is shown in figure 13. The analysis shows that the measured ranging data of the UWB module basically have a normal distribution, and the mean error is no more than 5 mm. In the simulation analysis of the positioning accuracy, the ranging error of the P440 module is taken as a random variable with an approximately normal distribution with a mean value of zero and a standard deviation of 0.02.

Positioning simulations
In this section, the positioning accuracy of CPA and CTFPA are analysed. The positioning error field (PEF) in a narrow tunnel is established.

Positioning accuracy
In order to investigate the positioning accuracy of CPA and CTFPA, the coordinate positioning process of the system is simulated by 'Matlab'. The node in the tunnel (0,95,0) is selected as the reference point, and the system's ranging error is assumed to obey normal distribution. The range error of standard deviation is 0.02, and the two algorithms are simulated 1000 times, and the coordinate estimation value of the system is obtained. Figure 14 shows the flow chart of the simulation program for the CTFPA.

Positioning error field
To compare the positioning accuracy of CPA and CTFPA in a narrow tunnel, and explore the influence of a change in space in the tunnel on the system positioning accuracy, the system PEF of the X-Y and X-Z sections in the tunnel is established.
The base station layout of the system for the X-Y section in the tunnel is shown in figure 15. The end of the tunnel is set as the origin of the tunnel coordinate system, where A (0,0,1), B (2,5,1), C (−2,5,1), and D (0,0,4) are the positions of the base stations, which are fixed behind the tunnel, and E is the fuselage node of the roadheader.
Using CPA and the CTFPA to calculate the root mean square error matrix, a contour map is drawn to obtain the PEF of the two algorithms in the X-Y section.
In the 10-100 m range of the Y-axis, a set of nodes is selected every 1 m. Each node interval is 0.1 m, and thus, a 61 * 91 matrix can be obtained. Each node in the matrix is simulated using CPA and CTFPA. The node E estimation coordinate and the node E truth coordinate are brought into equation (18), and the RMSE of node E is calculated, the result of which is used as the evaluation index of the positioning algorithm accuracy Figure 11. Experiment site.     In equation (18), x E1 , y E1 , and z E1 , and x t , y t , and z t are the estimated and truth values of node E, respectively.
The base station layout of the system for the X-Z section of the tunnel is shown in figure 16. The Y-axis is 100 m. Within the −3 to 3 m range of the X-axis, a set of nodes is selected every 0.1 m. Each node interval is 0.1 m, and thus a 61 * 91 matrix can be obtained. Similarly, using the CPA and CTFPA to calculate the RMSE matrix, a contour map is drawn to obtain the PEF of the two algorithms in the X-Z section.

Results and discussion
The data distribution for 1000 simulations of node (0, 95, 0) based on the CPA is shown in figure 17. The maximum error of the three axes is about 0.05 m, and the simulation data basically show a normal distribution. The closer the location is to true value, the more intensive the simulation nodes are.
The 100 times simulation value was selected, and a threeaxes error distribution curve was drawn, as shown in figure 18. The simulation results show that the three-axis error of the CPA is approximately 5 cm within the 95 m tunnel, and the accuracy of each axis is not significantly different.
The data distribution of the 1000 simulations of node (0, 95, 0) based on CTFPA is shown in figure 19. Compared with CPA, the positioning accuracy of CTFPA has been significantly improved, and the accuracy of each axis can reach the millimetre level.
The 100 values from simulation data was selected, and an error distribution curve of the three axes was drawn, as shown in figure 20.
The simulation results show that the X-axis error of the 95 m node simulation coordinate calculated using CTFPA is clearly smaller than the other two axes, and the errors of the Y-and Z-axes are not significantly different, reaching the millimetre scale.
The X-Y section PEF based on CPA is shown in figure 21. The line colour is used to characterize the RMSE of the roadheader fuselage node at different positions within the narrow and closed tunnel space.
The analysis shows that the distribution characteristics of the RMSE contours calculated using CPA for the X-Y section are scattered, varying greatly within the local range, and showing a certain amount of randomness. Overall, however,     within the 100 m range of the tunnel space, when the driving distance (Y-axis) increases, the RMSE line density and brightness increase; in other words, the positioning accuracy of the system is reduced. The X-axis positioning accuracy within the range of −3 to 3 m shows no obvious change.
The X-Y section PEF based on CPA is shown in figure 22. Compared with CPA, CTFPA has a higher accuracy. The RMSE contour distribution based on CTFPA is concise, regular, and orderly. With the increase in tunnelling distance (Y-axis), the RMSE of the system increases linearly, and the accuracy in the X-axis is almost unchanged.
The PEF of the X-Y section based on CPA is shown in figure 23. The analysis shows that the RMSE of CPA has a certain amount of randomness in the X-Z section of the tunnel within 100 m along the Y-axis, but there is no clear change in the positioning accuracy in any direction. The contour lines, density, and line brightness of the RMSE are evenly distributed.
The X-Z section PEF based on CTFPA is shown in figure 24. Compared with CPA, CTFPA has higher accuracy. The range of RMSE variation decreases, and the contour distribution is concise, regular, and orderly. The contour of the RMSE is a concentric circle with the original point of the tunnel. With the increase in the radius of the concentric circle, the error variance ratio increases nonlinearly.

Conclusions
(1) In view of the actual working conditions of a boom-type roadheader, a UPDS was designed in this study. By detecting the distance between the base stations and the fuselage nodes of the roadheader, all position and attitude parameters (coordinates and heading, pitch, and roll angles) of the roadheader fuselage can be calculated. (2) In view of the TOA positioning model of the system, CTFPA was proposed. The principle of the algorithm was analysed, and the complete calculation process of the algorithm was derived. A flow chart of the simulation program was also provided. (3) A UWB ranging experiment using a P440 module was carried out. The experiment data show that the mean error and standard deviation of the module are below 0.02 m within a narrow and closed environment, and that the ranging data follow a normal distribution. (4) CPA and CTFPA were simulated and analysed using MATLAB. The PEF of the tunnel section was established,     and the RMSE of the positioning was taken as the evaluation index of the positioning accuracy. The simulation results show that, compared with CPA, the positioning accuracy of CTFPA is significantly improved, and the accuracy of each axis can reach the millimetre scale, in which the X-axis accuracy is better than that of the Y-and Z-axes.
In a long and narrow tunnel, the RMSE contour distribution of CTFPA is concise and orderly. With an increase in the measurement distance, the RMSE increases linearly in the Yaxis. In the X-Z section, the RMSE contour distribution shows multiple concentric circles, and the change in RMSE ratio increases nonlinearly. The working environment of boom-type roadheader is a narrow and narrow confined space, generally located several hundred meters to thousands meters underground, and the GPS signals cannot be accepted. Therefore, in order to realize the remote pose detection of roadheader, the establishment of local positioning system is necessary. After analysis, the UPDS can effectively solve the existing technical problems, and realize the remote pose detection and autonomous cruise of roadheader. This research can also be extended to other similar engineering environment, such as tunnel boring machine, coal winning machine, undersea mining vehicle, underwater robot, lunar rover and other equipment's pose detection and autonomous cruise.