Abstract
The initial alignment is one of the difficult problems of the strapdown inertial navigation system based on the microelectromechanical systems (MEMS-SINS) applied to the navigation of boom-type roadheader under a coalmine. To overcome the complex environment of the underground coalmine and the large noise of the MEMS gyroscope, the laser spot perception system (LSPS) was developed to provide the heading information of the roadheader to aid the initial alignment of the MEMS-SINS. During the process of initial alignment, the differential equation of heading error is derived, the heading error is extended as a state variable, and a nonlinear initial alignment model aided by heading error is built up. To cope with the time-varying noise statistics of MEMS-SINS in the working face of the coal mine roadway, a simplified strong tracking Unscented Kalman Filter (SST-UKF) algorithm is proposed by combining covariance matching technology with UKF. In the calculation of the measurement prediction covariance and the cross covariance, the fading factor is introduced, respectively, to avoid the contradiction between the residuals before and after the introduction; according to the characteristics of the observation equation being a linear equation, it proves that the state prediction covariance matrix change does not affect the observation measurement and uses unscented transform (UT) only once in the state estimation and variance prediction; thus, the computational burden of the algorithm is reduced and the real-time performance is improved. The simulation and onboard experiment results show that the proposed scheme can achieve horizontal alignment within 40 s and convergence azimuth misalignment angle to 0.9° within 450 s, which fully meets the requirements of MEMS-SINS initial alignment for underground coalmine roadheader.
1. Introduction
The coal is still an important energy resource in today’s society [1]. Roadway tunneling is one of the most difficult, least efficient, and most dangerous parts in coal mining operations [2]. It is an urgent need to realize the unmanned tunneling surface. Ensuring the directional tunneling of the roadway is a basic requirement of the roadway construction task. Deviations in the direction of the roadway during coal roadway excavation will cause safety accidents in the roadway and endanger the personal safety of all construction workers at the working face. The boom-type roadheader is the core equipment for coal mine roadway tunneling, and realizing its autonomous navigation is the key foundation of automated and unmanned tunneling face [3, 4].
The conditions of the working face of the coal mine roadway are harsh; on the one hand, the space is narrow and closed, the geological conditions are complicated, and the natural disasters such as gas, water, fire, ground pressure, and ground temperature are serious; on the other hand, the roadheader produces high-concentration dust during the cutting process; at the same time, the body will generate strong vibrations, and the whole machine will slip, which will make it difficult for the machine to achieve accurate autonomous navigation. The existing navigation and positioning technologies for roadheaders include, based on total station [5], indoor-GPS technology [6], laser guidance technology [7], visual measurement [8], ultra-wideband ranging technology [9], and other methods. The above methods rely on external devices to provide a reference, which reduces the autonomous navigation performance of the roadheader. At the same time, it is difficult to meet the special requirements of an underground coal mine for electrical equipment, and its application in the aforementioned harsh environments has been greatly limited.
According to the inertia principle, SINS can dynamically output the position, speed, and attitude information of carrier in real time, and there is no need for information interaction with the external environment [10, 11]. It is suitable for the narrow, closed space, complex, and harsh environment of the underground coal mine. Initial alignment is extremely important for SINS subsequent navigation work. However, for the gyroscope in low-cost MEMS-SINS, the signal-to-noise ratio is low, the angular rate of the Earth’s rotation will be overwhelmed by the gyroscope noise, and the alignment method of high-precision SINS cannot be used in its initial alignment directly [12]. Also, the harsh environment of coal mines makes it difficult to obtain accurate noise statistics. Therefore, external auxiliary information and a strong robust filtering algorithm are needed to improve the accuracy of initial alignment and shorten the alignment time.
Many scholars have researched the initial alignment of SINS assisted methods. In [13, 14], using GPS-aided SINS for initial alignment achieved good results. But there are no GPS signals in the closed space under the coal mine; in [15, 16], Doppler Velocity Log (DVL) is adopted to obtain the three-dimensional velocity information of the carrier to calibrate SINS. This aided method is mainly used for navigation of ships and submarines. In the coal mine tunneling face, space is narrow and the equipment is numerous, which leads to serious interference of acoustic wave propagation, and it is difficult to measure the velocity accurately; the authors in [17, 18] show that by using geomagnetic information to obtain the heading information of the carrier for assisted alignment. When the cutting system of roadheader is working, the high-power motor will generate a powerful changing magnetic field, which will cause huge interference to the magnetometer, and the magnetic deflection angle obtained will be greatly deviated. Aiming at the above problems, this paper has developed a kind of LSPS that can provide the heading information of the roadheader, which can overcome the unfavorable factors such as dim light, high concentration of dust, and obstruction of sight in the underground coal mine. It has higher reliability and lower risk and is fully adapted to the harsh operating conditions underground.
The initial alignment of SINS is usually divided into two stages: coarse alignment and fine alignment [19]. In the stage of coarse alignment, both the accelerometer and gyro drift are small for high-precision SINS, and the initial attitude matrix can be calculated directly from the output of the accelerometer and gyroscope [20]. Regarding MEMS-SINS, the accuracy of the microsilicon accelerometer is generally better than 1 mg, which can meet the requirements for horizontal attitude self-alignment. However, the gyroscope can drift up to several to tens of degrees per hour and cannot be sensitive to the angular velocity of Earth’s rotation, so that azimuth self-alignment cannot be completed [21]. Therefore, this paper proposes to couple the heading information of LSPS with the attitude information of the acceleration to complete the initial coarse alignment. In the fine alignment stage, since the error characteristics of the gyroscope are extremely unstable and susceptible to the external environment, the bias repeatability error and random drift are large, resulting in a large azimuth misalignment angle, and the state equation is seriously nonlinear [22]. In this paper, the nonlinear error model of a large azimuth misalignment angle is adopted, the differential equation of heading error is derived, and the heading error is extended as a state variable, which further improves the initial alignment accuracy.
The strongly robust filtering algorithm is also one of the important issues in the initial alignment of the MEMS-SINS [23]. In the nonlinear error model of large azimuth misalignment angle, the nonlinear estimation method is used to estimate the misalignment angle. The Extended Kalman Filter (EKF) is widely used in nonlinear system filtering. It linearizes general nonlinear systems and expands the nonlinear function into a Taylor series and omits the second order and above. But it is only applicable to systems where the nonlinearity is weak and the Gaussian noise assumption is met [24, 25]. The UKF uses a deterministic sampling method to approach the posterior probability density of the state and has a higher estimation accuracy for the statistic of the nonlinear system. Compared to EKF, there is no need to calculate the Jacobian matrix, and accuracy and robustness are also significantly higher [26, 27]. However, in the initial alignment of MEMS-SINS, the uncertainties such as model error, noise, and interference are relatively large; accurate noise statistics are difficult to obtain, which leads to serious degradation of UKF filtering accuracy and robustness.
In recent years, covariance matching technology has received increasing attention in solving the problem of unknown noise statistics in the initial alignment of mobile equipment in coal mines. Its principle is to introduce fading factors into the prediction covariance matrix, adjust the gain matrix online in real time, and force the filter residual sequence to remain orthogonal to each other [28]. The UKF based on covariance matching technology can make filtering residuals consistent with the theoretical covariances in the condition of unknown statistical noise characteristics and can effectively solve problems such as poor robustness and filtering divergence when the model is uncertain. The strong tracking filter algorithm proposed in [29] requires three times of UT for each filtering, and the calculation amount is huge, which makes it very limited in practical applications. In this paper, according to the characteristic of the observation equation being a linear equation in the MEMS-SINS nonlinear state-space model, it is proved that the change of state prediction covariance matrix when the observation equation is linear does not affect the estimation of observation measurement, and only UT is used once in the state estimation and variance prediction, simplifying the filtering process of UKF. At the same time, to avoid the contradiction between the residuals before and after the fading factor is introduced, the fading factor is introduced into the calculation of the measured prediction covariance and the cross covariance, respectively. An SST-UKF algorithm is formed, which reduces the computation load, shortens the alignment time of the initial alignment, improves the real time and accuracy of the algorithm, and expands the practical application range of MEM-SINS.
The rest of this paper is organized as follows: Section 2 introduces the principle of LSPS and performs the performance verification experiment. A nonlinear initial alignment model aided by heading error is built up in Section 3. In Section 4, the filtering algorithm of SST-UKF is proposed and the algorithm process is given. Section 5 simulates the proposed SST-UKF initial alignment algorithm and analyzes it in different situations. Section 6 carries out the onboard experiment and analyzes results. The conclusions are drawn in Section 7.
2. Laser Spot Perception System (LSPS)
LSPS is a set of systems independently developed by the author’s team, which can measure the absolute heading angle of the roadheader in real time. It is mainly composed of one fan-shaped laser transmitter and two laser receivers [30]. It can overcome the unfavorable factors such as dim light, high-concentration dust, and line-of-sight obstruction in coal mines. It has higher reliability and less risk and is fully adapted to the harsh operating conditions in the mine. It is feasible to use LSPS to aid the initial alignment of MEMS-SINS.
According to the requirements of the roadway design, the personnel of the surveying and mapping department accurately installed the fan-shaped laser transmitter on the centerline of the roof of the roadway behind the roadheader, and two laser receivers were installed in parallel at the known position of the roadheader fuselage and perpendicular to the longitudinal axis of the fuselage, as shown in Figure 1. At any time, the fan-shaped laser beam transmitted by fan-shaped laser transmitter forms linear laser spots on the two laser receivers, respectively. The photosensitive elements in the laser receiver can perceive the position of the laser spot. By solving the relative position of the two laser spots, the absolute heading angle of the roadheader relative to the midline of the roadway can be obtained.
2.1. Laser Receivers
The function of the laser receivers is to perceive the linear laser spot formed by the fan-shaped laser beam, convert the laser signal into an electrical signal to identify the position of the laser spot, and calculate the absolute heading angle of the roadheader through the internal circuit and processor. As shown in Figure 2, the photosensitive area of the laser receiver is composed of 3 rows of closely arranged photosensitive elements, which only allow the light of a certain frequency range (the frequency of the fan-shaped laser beam) to pass through. It has a strong identification ability to the laser spot and high reliability in the complex environment of the coalmine.
2.2. Fan-Shaped Laser Transmitter
Fan-shaped laser transmitter is mainly responsible for transmitting a fan-shaped laser beam and forming a linear laser spot with a certain brightness on the laser receiver within a certain distance of the roadway. By adding a set of Powell prism to the front of the original beam transmitted by the semiconductor laser transmitter, the line laser beam is scattered into a fan-shaped laser beam with a certain angle by using convex edge on the prism. To ensure that the laser target can reliably perceive the signal source, infrared light with a power of 100 mw and a working wavelength of 650 nm is used as the semiconductor laser source.
The installation position of the fan-shaped laser transmitter is accurately determined by the surveying and mapping department. Therefore, the fan-shaped laser beam transmitted by a fan-shaped laser transmitter contains the information of the roadway coordinates, which is the benchmark for heading determination of the roadheader. The overall structure is shown in Figure 3.
2.3. Calculation Model
The principle of LSPS is to calculate the heading angle by the relative position of the laser spot formed on the two laser receivers. To express the principle of calculating the absolute heading angle of the roadheader relative to the midline of the roadway, the whole system is simplified, as shown in Figure 4.
The calculation model of heading angle of roadheader by LSPS is shown as follows:where is the center of the body of the roadheader; is the midpoint of the laser receiver; is the midpoint of another laser receiver; is the laser spot on the receiver; is the laser spot on another laser receiver.
Since is the absolute heading angle of the roadheader relative to the middle line of the roadway, the heading angle in the navigation coordinate system iswhere is the angle between the midline of the roadway and the north direction, given by the surveying and mapping department.
2.4. LSPS Experiment
In order to evaluate the feasibility of LSPS and the accuracy of heading angle measurement and to prepare for the following aided alignment work, the onboard experiment of LSPS was carried out.
This paper uses the ground marking method to measure the heading angle of the roadheader as a reference, as shown in Figure 5. The layout of LSPS is shown in Figure 1. First, the roadheader is parked at the designated position and a straight line is drawn backward along the crawler belt edge on one side of its body. The fan-shaped laser beam of the fan-shaped laser transmitter remains stationary, and then the roadheader rotates at a certain angle with its center point . At this time, a straight line is drawn backward along the crawler belt edge on the same side of the body. Three distances of , , and are marked on two straight lines. Finally, the heading angle of the roadheader on the ground is calculated by the following formula:
In the experiment, the roadheader rotated to the left four times at the center point of the body, then rotated back to the original position, and then rotated to the right four times at random. After each rotation, the laser receiver output data is recorded and the length of lines is measured. The experimental results are shown in Figure 6, and the experimental statistics are shown in Table 1.
Based on the above experimental data, it can be calculated that the mean and variance of the absolute heading error obtained by LSPS are 0.05° and 0.028°. It can be confirmed that the use of LSPS to obtain the absolute heading angle of the roadheader is a practical technical solution, and the measurement accuracy of the system can ensure the aided initial alignment of MEMS-SINS.
3. Initial Alignment Model
Initial alignment can be divided into two stages: coarse alignment and fine alignment [19]. The purpose of coarse alignment is to calculate the attitude transformation matrix between the navigation coordinate system and the body coordinate system . Due to the large error of the attitude matrix determined by the coarse alignment, there is a deviation angle between the actual navigation coordinate system and the ideal navigation coordinate system . This deviation angle is the misalignment angle. The function of fine alignment is to estimate the misalignment angle and further improve the accuracy of alignment.
For the initial alignment of low-cost MEMS-SINS, the gyroscope noise is large, and the noise statistics change over time in the harsh environment of the underground, which leads to the traditional SINS error model which is unsuitable for the initial alignment of the roadheader. Therefore, the SINS nonlinear error model with a large azimuth misalignment angle is adopted in this paper, the differential equation of heading angle error is deduced, the heading error is extended as a state variable, and a nonlinear initial alignment model aided by heading error is built up.
3.1. SINS Error Dynamics Model
In the initial alignment of the static base, the roadheader keeps motionless, its velocity and horizontal specific force are zero, vertical specific force is , and is the geographic latitude. This paper uses the nonlinear SINS error model proposed in [31]where is the velocity error, is the transformation matrix from the actual navigation coordinate system to ideal navigation coordinate system and , is identity matrix, is the specific force in the navigation coordinate system, is the angular velocity of the Earth rotation, is the angular velocity in the navigation coordinate system, is the velocity in the navigation coordinate system, is the transformation matrix from the body coordinate system to the actual navigation coordinate system, is the constant drift noise of the accelerometer, is the transformation matrix from the body coordinate system to the ideal navigation coordinate system, is the random drift noise of the accelerometer, is platform misalignment angle, is an antisymmetric matrix, is the angular velocity of the navigation coordinate system relative to the inertial coordinate system and , is the constant drift noise of gyro, and is the random drift noise of gyro.
The fine alignment scheme proposed in this paper is to derive the differential equation of the heading error by the relationship between the heading error and other state error variables so that the heading error is extended as a state variable.
The heading error is defined aswhere is the heading angle obtained by LSPS, as the heading reference, and is the heading angle calculated by the SINS attitude matrix updating. The relationship between the horizontal attitude error angle and heading error angle and misalignment angle is given by
Differentiating (10), the heading error differential equation can be yielded:
By the coupling relationship between the differential of the heading error and the misalignment angle in (11), the heading error can be added to the SINS error equation, which can theoretically enhance the observability of the alignment system model, improve the accuracy of alignment estimation, shorten the alignment convergence time, and enhance the stability of the system.
3.2. State Space Model
According to the error differential equations listed above, the differential form of heading error is extended to the state variable. Take the state variables as
The velocity of the roadheader at stationary state is the velocity error, and the horizontal velocity error and heading error are taken as the observed variables:
The state-space model can be established:where the expressions of and are obtained by formulas (4)–(7) and (11); and are uncorrelated zero-mean Gaussian white noise sequences for system process and observed noise, respectively; is the noise allocation matrix; is the observation matrix.
4. Simplified Strong Tracking UKF (SST-UKF) Algorithm
The performance of traditional UKF filters depends to a large extent on the process noise covariance matrix and the measured noise covariance matrix. For the initial alignment of the roadheader, accurate noise statistics and dynamic process models are difficult to determine, and the noise statistics information of MEMS is likely changing with time due to the complex downhole environment. In this paper, the covariance matching technology is adopted to solve the above problems. The principle is to adjust the gain matrix in real time by introducing a fading factor into the state prediction covariance matrix to force the output residual sequences to remain mutually orthogonal [32, 33]. The filtering algorithm combining covariance matching technology with UT improves the robustness and adaptive ability of the UKF alignment algorithm, but it also increases the computational burden and complexity of the algorithm. To simplify the process of the strong tracking UKF algorithm, reduce the calculation amount, and improve the real-time performance of the algorithm, according to the characteristics of the system observation equation being a linear equation, this paper proves that the measurement prediction is only related to the state transition matrix and the state prediction and has nothing to do with the state prediction covariance; only UT is used once in the state estimation and variance prediction. Besides, to avoid the contradiction between the residuals before and after the fading factor is introduced, the fading factor is introduced into the calculation of the measured prediction covariance and the cross covariance, respectively.
The SST-UKF algorithm also needs to satisfy the condition that the estimated error variance is minimum and the residual sequence at different times remains orthogonal, as the following formula:where is the residual at time of .
4.1. Algorithm Process
The specific steps of the simplified SST-UKF initial alignment filtering algorithm used in this paper are as follows: Step 1. At time , select and , and calculate the weights of the sample points:where is a scaling parameter, determined by ; is the dimension of the state; is the control parameter of the distribution state of the sampling point (); ; is a nonnegative weight coefficient (). Step 2. Perform UT. Calculate the Sigma point set from and (the symmetric sampling strategy is used in this paper):where denotes the column of the square root of the matrix. Step 3. Calculate the state prediction and the state prediction covariance matrix :where is the dimension of the state vector except for the noise state; if the dimension of the noise vector is , then ; is the weights of the sample points except for the noise state; ; is the system noise variance matrix. Step 4. Calculate the residual , the measurement prediction covariance , and the cross covariance after introducing the fading factor .
The normal strong tracking algorithm needs to perform the UT again to calculate after the state prediction covariance matrix after the introduction of the fading factor was obtained, and the change of the state prediction covariance matrix does not affect the estimation of the observed measurement when the observation equation is linear. Proof:
is the Sigma sampling point sets obtained from and . When ,
It can be concluded that only depends on and and not on .
Based on the above conclusions, this paper uses only one-time UT to complete state estimation and variance prediction, which simplifies the calculation process and reduces the amount of calculation. Also, to avoid the inconsistency of the residual inconsistency before and after the introduction of the fading factor , this paper introduces the fading factor directly in the calculation of the measurement prediction covariance and the cross covariance , which can be obtained:where is the measurement noise variance matrix.
One has [34]It can be obtained that
Step 5. Measurement updating: calculate the gain matrix , the state estimate , and the estimated error covariance matrix to complete the entire filtering process:
4.2. Calculation of the Fading Factor
According to the principle of the fading factor, the state estimation needs to keep the observed prediction residual sequences orthogonal to each other in real time.
Definewhere is the weakening factor and ; is the residual covariance matrix, and the calculation process of it is unknown. It can be estimated bywhere is a forgetting factor and .
According to the principle of orthogonalization, it can be obtained that
Tracing both sides of (27) to obtain the suboptimal fading factor ,where and is the tracing operator.
5. Simulation and Analysis
To evaluate the performance of the proposed MEMS-SINS initial alignment scheme, the filtering simulation experiments were performed in the case of the alignment model with heading error aided or not, the actual system noise mismatching with the given system noise variance seriously, and different large azimuth misalignment angles. All three simulations were implemented in MATLAB R2018a.
5.1. Simulation Conditions
Under the condition of MEMS-SINS at static base, the geographical position is 45° north latitude and 120° east longitude; the gyroscope constant drift is 0.01°/s and the random drift is 0.05°/s; the acceleration zero bias is 1 mg (), and the random noise is 0.5 mg; the velocity error noise is 0.1 m/s; the ideal attitude transformation matrix is a unit matrix; the sampling frequency is 100 Hz. The error variance of LSPS is 0.028° and sampling frequency is 10 Hz. The simulation time is 1500 s.
5.2. Simulation Analysis
5.2.1. Simulation 1: Heading Error Aided Initial Alignment Simulation
As the gyroscope noise in MEMS-SINS is too large, the attitude error after coarse alignment is too large to complete the initial alignment alone; this paper proposes a nonlinear state-space model of initial alignment aided by the heading error obtained by LSPS. In the stage of fine alignment, the heading error aided fine alignment model is derived. To verify the superiority of the proposed method, the normal state-space model without heading angle error aided and the state-space model with heading angle error aided were used, respectively. Simulation analysis was performed based on the SST-UKF filtering algorithm. In both situations, the initial misalignment angle is chosen as . The simulation results of alignment error are shown in Figure 7.
(a)
(b)
(c)
Figure 7 shows that when the initial misalignment angle is , both the alignment models with heading error aided or not require less than 50 s to reach the steady-state value of the horizontal alignment; for azimuth misalignment, the normal alignment model needs 530 s and the heading error aided alignment model needs 460 s. It can be seen from Table 2 that the alignment accuracy using the heading error aided model is also higher than the normal alignment model. Therefore, the heading error aided alignment model proposed in this paper is superior to the normal alignment model.
5.2.2. Simulation 2: The Actual System Noise Mismatching with the Given System Noise Variance Seriously
It is difficult to obtain accurate noise statistics in the harsh environment of coal mines, which requires the filtering algorithm to have strong self-adaptation ability to cope with the time-varying noise covariance. To verify the stability and robustness of the SST-UKF algorithm proposed in this paper when the statistical noise of the system is not accurate, we adopt the traditional UKF algorithm and SST-UKF algorithm to perform simulation under the condition of the actual system noise mismatching with the given system noise variance seriously, respectively. The variance of random noise of accelerometer, gyroscope, and LSPS is enlarged by 10 times, respectively, that are , , and , but the system noise variance in the filtering algorithm remains the same. The initial misalignment angle is . The simulation results are shown in Figure 8.
(a)
(b)
(c)
As can be seen from Figure 8, there is no significant difference in the dynamic characteristics of error curves between the two algorithms for horizontal alignment, and both have good effects. For azimuth alignment, the fluctuation of the alignment error curve of the UKF is significantly more severe than the SST-UKF, and the convergence time is much longer than the SST-UKF. The time required for UKF to converge to a steady-state value is 550 s, and the SST-UKF is 420 s. The azimuth error curve of the SST-UKF in this simulation is not different from that in Simulation 1. It shows that SST-UKF can track residual changes well in the case of serious mismatch of system noise variance, with strong robustness and good adaptive ability. And, as we can see from Table 3, the mean and variance of heading error obtained by SST-UKF are much smaller than those obtained by UKF.
5.2.3. Simulation 3: Under Different Large Azimuth Misalignment Angles
To deal with the problem that azimuth misalignment angles are too large after coarse alignment, a large azimuth misalignment error model is proposed in this paper, to evaluate the alignment ability of the SST-UKF algorithm under different large azimuth misalignment angles and perform simulation when the initial misalignment angles are , , and , respectively. Figure 9 shows the simulation results.
Figure 9 shows the error curves of the initial azimuth misalignment angle at 30°, 45°, and 60°, respectively. All three error curves converge with time, and the time required for alignment increases with the increase of the initial azimuth misalignment angle. As the initial azimuth misalignment angle increases, the convergence accuracy also decreases. The azimuth misalignment angle error can be guaranteed to converge to the steady-state error value within 900 s, and the convergence accuracy of azimuth misalignment angle at 30°, 45°, and 60° is 1.0521°, 1.5534°, and 1.9842°, respectively.
6. Onboard Experiments
To evaluate the performance of the initial alignment scheme of MEMS-SINS aided by LSPS proposed in this paper, an initial alignment experiment system was set up in the workshop of coalmine machinery manufacturer to simulate the actual working condition of the road header in the coalmine, and the onboard experiment of initial alignment was carried out.
6.1. Experimental Scheme
The equipment required for the experimental system mainly includes boom-type roadheader, SINS-integrated three-axis quartz accelerometer and three-axis MEMS gyroscope, LSPS consisting of two laser receivers and one fan-shaped laser transmitter, and the navigation computer. The two laser receivers are mounted parallel to the known position on the upper surface of the roadheader body and are perpendicular to the centerline of the body; the fan-shaped laser transmitter is suspended 50 m behind the roadheader; the layout of LSPS is shown in Figure 1. SINS is installed in the explosion-proof electric control box of the roadheader; the output data of the SINS and LSPS are transmitted to the navigation computer through the RS422 bus for data fusion and filtering, and then the navigation parameters are output. Since the update frequency of SINS is much higher than LSPS, the measurement update is only performed when LSPS is effective. The cutting system of the roadheader was in operation during the experiment. The geographical location of the experiment site is 38.87° north latitude and 114.64° east longitude. The experimental system is shown in Figure 10. The specifications of the sensors used in the experiment are shown in Tables 4 and 5.
6.2. Experimental Results and Analysis
In the experiment, the roadheader is parked on the horizontal ground, and the fuselage is turned to the direction of 15° east by north by the total station measuring. The total station is a TM30 automatic total station with an angular measurement accuracy (standard deviation) of 0.5′′. It can be considered that the theoretical attitude angle of the roadheader is . The performance of two alignment algorithms based on UKF and SST-UKF was compared in onboard experiments. The experiment took 1500 s. The misalignment angle estimation error curve is shown in Figure 11.
(a)
(b)
(c)
As can be seen from Figure 11, when using the UKF, the horizontal alignment time is less than 50 s, and the azimuth alignment time is about 520 s. Using the SST-UKF, the horizontal alignment time is 40 s, and the azimuth alignment time is 450 s. The horizontal alignment accuracy of the SST-UKF is about 0.03°, and the azimuth alignment accuracy is about 0.9°; the horizontal alignment accuracy of UKF is about 0.06°, and the azimuth alignment accuracy is about 1.4°. The SST-UKF algorithm is better than the UKF algorithm in both convergence speed and accuracy. The statistical properties of the experiment results are shown in Table 6.
7. Conclusion
This paper has presented an LSPS-aided MEMS-SINS initial alignment scheme; this scheme overcome the harsh working conditions of the underground coal mining face to obtain the heading information of the roadheader through the self-developed LSPS; derived the differential equation of heading error and extended the heading error to the system state; and proposed an initial alignment model of nonlinear large azimuth misalignment angle aided by heading angle error. Besides, by combining the covariance matching technique with the UKF algorithm and taking advantage of the characteristic that the observation equation is linear, a SST-UKF algorithm was obtained. The simulation results showed that the proposed scheme can converge to the required accuracy in a certain time under different conditions and has good robustness and stability. Through the onboard experiment, this scheme can complete the horizontal alignment within 40 s and converge the azimuth misalignment angle to 0.9° within 450 s, which is completely applicable to the initial alignment of underground coal mine roadheader based on MEMS-SINS.
Data Availability
The data used to support the findings of this study are included within the article.
Conflicts of Interest
The authors declare that they have no conflicts of interest.
Acknowledgments
This research was supported by the National Natural Science Foundation of China, Grant no. 51874308.