# A Scheme of MEMS-SINS Initial Alignment Aided by Laser Spot Perception System for the Boom-Type Roadheader

Authors: Yang Shen; Shichen Fu; Pengjiang Wang; Rui Li; Yu Lan; Xiaodong Ji; Chao Liu; Miao Wu
Journal: Mathematical Problems in Engineering
 (2020)

---

## 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.


---

## Body

## 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: Section2 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 Figure1. 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.Figure 1
Schematic diagram of LSPS: 1: boom-type roadheader, 2: fan-shaped laser beam, 3: laser receivers, and 4: fan-shaped laser transmitter.


### 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 Figure2, 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.Figure 2
The overall structure of laser receiver: 1: explosion-proof enclosure, 2: circuit boards, 3: linear laser spot, and 4: photosensitive elements.


### 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 Figure3.Figure 3
The overall structure of fan-shaped laser transmitter: 1: transmitter holder, 2: fan-shaped laser transmitter, 3: transmitter attitude adjusting mechanism, and 4: fan-shaped laser beam.


### 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 Figure4.Figure 4
Schematic diagram of the absolute heading angle calculation: 1: laser receivers, 2: fan-shaped laser transmitter, 3: boom-type roadheader, and 4: fan-shaped laser beam.The calculation model of heading angle of roadheader by LSPS is shown as follows:(1)ψl=arctanO1P1−O2P2O1O2,where O is the center of the body of the roadheader; O1 is the midpoint of the laser receiver; O2 is the midpoint of another laser receiver; P1 is the laser spot on the receiver; P2 is the laser spot on another laser receiver.Sinceψl is the absolute heading angle of the roadheader relative to the middle line of the roadway, the heading angle in the navigation coordinate system is(2)ψL=ψl+ψB,where ψB 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 Figure5. 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 O. At this time, a straight line is drawn backward along the crawler belt edge on the same side of the body. Three distances of L, S1, and S2 are marked on two straight lines. Finally, the heading angle of the roadheader on the ground is calculated by the following formula:(3)α=arctanS1−S2L.Figure 5
Schematic diagram of the ground marking method.In the experiment, the roadheader rotated to the left four times at the center pointO 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.Figure 6
Experimental results of the ground marking method.Table 1
Experimental statistics of LSPS.
S1 (mm)S2 (mm)L (mm)Tanαα (°)ψl (°)Difference (°)2923535000.126.846.770.076717845000.2212.4112.350.061752005000.052.862.89−0.031341515000.042.292.270.02405476500−0.13−7.41−7.36−0.05596675500−0.17−9.65−9.730.089131025500−0.23−12.95−13.060.11241302500−0.12−6.84−6.80−0.04Based 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.


## 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 Figure2, 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.Figure 2
The overall structure of laser receiver: 1: explosion-proof enclosure, 2: circuit boards, 3: linear laser spot, and 4: photosensitive elements.


## 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 Figure3.Figure 3
The overall structure of fan-shaped laser transmitter: 1: transmitter holder, 2: fan-shaped laser transmitter, 3: transmitter attitude adjusting mechanism, and 4: fan-shaped laser beam.


## 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 Figure4.Figure 4
Schematic diagram of the absolute heading angle calculation: 1: laser receivers, 2: fan-shaped laser transmitter, 3: boom-type roadheader, and 4: fan-shaped laser beam.The calculation model of heading angle of roadheader by LSPS is shown as follows:(1)ψl=arctanO1P1−O2P2O1O2,where O is the center of the body of the roadheader; O1 is the midpoint of the laser receiver; O2 is the midpoint of another laser receiver; P1 is the laser spot on the receiver; P2 is the laser spot on another laser receiver.Sinceψl is the absolute heading angle of the roadheader relative to the middle line of the roadway, the heading angle in the navigation coordinate system is(2)ψL=ψl+ψB,where ψB 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 Figure5. 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 O. At this time, a straight line is drawn backward along the crawler belt edge on the same side of the body. Three distances of L, S1, and S2 are marked on two straight lines. Finally, the heading angle of the roadheader on the ground is calculated by the following formula:(3)α=arctanS1−S2L.Figure 5
Schematic diagram of the ground marking method.In the experiment, the roadheader rotated to the left four times at the center pointO 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.Figure 6
Experimental results of the ground marking method.Table 1
Experimental statistics of LSPS.
S1 (mm)S2 (mm)L (mm)Tanαα (°)ψl (°)Difference (°)2923535000.126.846.770.076717845000.2212.4112.350.061752005000.052.862.89−0.031341515000.042.292.270.02405476500−0.13−7.41−7.36−0.05596675500−0.17−9.65−9.730.089131025500−0.23−12.95−13.060.11241302500−0.12−6.84−6.80−0.04Based 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 Cnb between the navigation coordinate system n and the body coordinate system b. 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 n′ and the ideal navigation coordinate system n. 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 velocityvE,vN,vU and horizontal specific force fE,fN are zero, vertical specific force fU is g, and L is the geographic latitude. This paper uses the nonlinear SINS error model proposed in [31](4)δv˙n=Cn′n−Ifn−2δωien+δωenn×vn−2ωien+ωenn×δvn+Cbn′∇b+Cbnwab,(5)ϕ˙=Cw−1I−Cnn′ωinn−Cbn′εb+Cbn′wgb,(6)∇˙b=0,(7)ε˙b=0,where δv is the velocity error, Cn′n is the transformation matrix from the actual navigation coordinate system to ideal navigation coordinate system and Cnn′=Cn′nT, I is identity matrix, fn is the specific force in the navigation coordinate system, ωien is the angular velocity of the Earth rotation, ωenn is the angular velocity in the navigation coordinate system, vn is the velocity in the navigation coordinate system, Cbn′ is the transformation matrix from the body coordinate system to the actual navigation coordinate system, ∇b is the constant drift noise of the accelerometer, Cbn is the transformation matrix from the body coordinate system to the ideal navigation coordinate system, wab is the random drift noise of the accelerometer, ϕ is platform misalignment angle, Cw−1 is an antisymmetric matrix, ωinn is the angular velocity of the navigation coordinate system relative to the inertial coordinate system and ωinn=ωien+ωenn, εb is the constant drift noise of gyro, and wgb 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 as(8)Δψ=ψ−ψL,where ψL 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 ϕE,ϕN,ϕU is given by(9)Δθ=sinψ⋅ϕE+cosψ⋅ϕN,Δζ=−sinψ⋅ϕE+cosψ⋅ϕNcosθ,(10)Δψ=−tanθsinψ⋅ϕN+cosψ⋅ϕE+ϕU.Differentiating (10), the heading error differential equation can be yielded:(11)Δψ˙=tanθsinψωiesinLϕN−tanθcosψωiesinL+ωiesinLϕE+tanθsinψωiecosLϕU−tanθcosψεxb+tanθsinψεyb+εzb.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(12)x=δvEδvNϕEϕNϕUΔψ∇xb∇ybεxbεybεzb.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:(13)z=δvEδvNΔψ.The state-space model can be established:(14)x˙=fx+Γw,z=Hx+r,where the expressions of fx and gx are obtained by formulas (4)–(7) and (11); w and r are uncorrelated zero-mean Gaussian white noise sequences for system process and observed noise, respectively; Γ is the noise allocation matrix; H is the observation matrix.


## 3.1. SINS Error Dynamics Model

In the initial alignment of the static base, the roadheader keeps motionless, its velocityvE,vN,vU and horizontal specific force fE,fN are zero, vertical specific force fU is g, and L is the geographic latitude. This paper uses the nonlinear SINS error model proposed in [31](4)δv˙n=Cn′n−Ifn−2δωien+δωenn×vn−2ωien+ωenn×δvn+Cbn′∇b+Cbnwab,(5)ϕ˙=Cw−1I−Cnn′ωinn−Cbn′εb+Cbn′wgb,(6)∇˙b=0,(7)ε˙b=0,where δv is the velocity error, Cn′n is the transformation matrix from the actual navigation coordinate system to ideal navigation coordinate system and Cnn′=Cn′nT, I is identity matrix, fn is the specific force in the navigation coordinate system, ωien is the angular velocity of the Earth rotation, ωenn is the angular velocity in the navigation coordinate system, vn is the velocity in the navigation coordinate system, Cbn′ is the transformation matrix from the body coordinate system to the actual navigation coordinate system, ∇b is the constant drift noise of the accelerometer, Cbn is the transformation matrix from the body coordinate system to the ideal navigation coordinate system, wab is the random drift noise of the accelerometer, ϕ is platform misalignment angle, Cw−1 is an antisymmetric matrix, ωinn is the angular velocity of the navigation coordinate system relative to the inertial coordinate system and ωinn=ωien+ωenn, εb is the constant drift noise of gyro, and wgb 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 as(8)Δψ=ψ−ψL,where ψL 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 ϕE,ϕN,ϕU is given by(9)Δθ=sinψ⋅ϕE+cosψ⋅ϕN,Δζ=−sinψ⋅ϕE+cosψ⋅ϕNcosθ,(10)Δψ=−tanθsinψ⋅ϕN+cosψ⋅ϕE+ϕU.Differentiating (10), the heading error differential equation can be yielded:(11)Δψ˙=tanθsinψωiesinLϕN−tanθcosψωiesinL+ωiesinLϕE+tanθsinψωiecosLϕU−tanθcosψεxb+tanθsinψεyb+εzb.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(12)x=δvEδvNϕEϕNϕUΔψ∇xb∇ybεxbεybεzb.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:(13)z=δvEδvNΔψ.The state-space model can be established:(14)x˙=fx+Γw,z=Hx+r,where the expressions of fx and gx are obtained by formulas (4)–(7) and (11); w and r are uncorrelated zero-mean Gaussian white noise sequences for system process and observed noise, respectively; Γ is the noise allocation matrix; H 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:(15)Exk+1−x^k+1xk+1−x^k+1T=min,(16)Eγk+1+jγk+1T=0,k=0,1,...;j=1,2,...,,where γk+1 is the residual at time of k+1.


### 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 k=0, select x0 and P0, and calculate the weights of the sample points:(17)W0m=μY+μ,W0c=μY+μ+1−ε2+σ,Wim=Wic=μ2Y+μ,i=1,2,…,2Y,where μ is a scaling parameter, determined by μ=ε2Y+κ−Y; Y is the dimension of the state; ε is the control parameter of the distribution state of the sampling point (ε=0.01); κ=0 ; σ is a nonnegative weight coefficient (σ=2).Step 2. Perform UT. Calculate the Sigma point set from xk and Pk (the symmetric sampling strategy is used in this paper):(18)χi=xkxki+Y+μPkixki−Y+μPki,i=1∼Y,where Pki denotes the i−th column of the square root of the matrix.Step 3. Calculate the state prediction x^k+1/k and the state prediction covariance matrix Pk+1/k:(19)x^k+1/k=∑i=02YWimχi∗,Pk+1/k=∑i=02YaWiacχi∗−x^k+1/kχi∗−x^k+1/kT+ΓQkΓT,where Ya is the dimension of the state vector except for the noise state; if the dimension of the noise vector is Yw, then Y=Ya+Yw; Wiac is the weights of the sample points except for the noise state; χi∗=fχi; Qk is the system noise variance matrix.Step 4. Calculate the residual γk+1, the measurement prediction covariance Pzz,k+1, and the cross covariance Pxz,k+1 after introducing the fading factor λk+1.The normal strong tracking algorithm needs to perform the UT again to calculatez^k+1/k after the state prediction covariance matrix Pk+1/k after the introduction of the fading factor γk+1 was obtained, and the change of the state prediction covariance matrix Pk+1/k does not affect the estimation of the observed measurement z^k+1/k when the observation equation is linear.Proof:χi,k+1/k∗ is the Sigma sampling point sets obtained from x^k+1/k and Pk+1/k. When zk+1=Hk+1xk+1+v,(20)ηi,k+1/k∗=Hk+1χi,k+1/k∗,z^k+1/k=Hk+1∑i=02YWicχi,k+1/k∗=Hk+1x^k+1/k.It can be concluded thatz^k+1/k only depends on Hk+1 and x^k+1/k and not on Pk+1/k.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λk+1, this paper introduces the fading factor λk+1 directly in the calculation of the measurement prediction covariance Pzz,k+1 and the cross covariance Pxz,k+1, which can be obtained:(21)γk+1=zk+1−z^k+1/k,Pzz,k+1=λk+1∑i=02YWciηi∗−z^k+1/kηi∗−z^k+1/kT+Rk+1=λk+1∑i=02YWciHk+1χi∗−x^k+1/kHk+1χi∗−x^k+1/kT+Rk+1,where Rk+1 is the measurement noise variance matrix.One has [34](22)∑i=02Yawiacχi,k+1/k∗−x^k+1/kχi,k+1/k∗−x^k+1/kT+ΓkQkΓkT=∑i=02YWicχi,k+1/k∗−x^k+1/kχi,k+1/k∗−x^k+1/kT=Pk+1/k.It can be obtained that(23)Pzz,k+1=λk+1Hk+1Pk+1/kHk+1T+Rk+1,Pxz,k+1=λk+1∑i=02YWicχi∗−x^k+1/kηi∗−z^k+1/kT=λk+1Pk+1/kHk+1T.Step 5. Measurement updating: calculate the gain matrix Kk+1, the state estimate x^k+1, and the estimated error covariance matrix Pk+1 to complete the entire filtering process:(24)Kk+1=Pxz,k+1Pzz,k+1−1,x^k+1=x^k+1/k+Kk+1zk+1−z^k+1/k,Pk+1=λk+1Pk+1/k−Kk+1Pzz,k+1Kk+1T.


### 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.Define(25)Nk+1=Vγ,k+1−βRk+1−Hk+1ΓQkΓTHk+1T,Mk+1=Hk+1∑i=02YWicχi,k+1/k−x^k+1/k⋅χi,k+1/k−x^k+1/kTHk+1T,where β is the weakening factor and β≥1; Vγ,k+1 is the residual covariance matrix, and the calculation process of it is unknown. It can be estimated by(26)Vγ,k+1=γ1γT1,k=0,ρVγ,k+γk+1γTk+11+ρ,k≥1,where ρ is a forgetting factor and 0<ρ≤1.According to the principle of orthogonalization, it can be obtained that(27)λk+1Mk+1=Nk+1.Tracing both sides of (27) to obtain the suboptimal fading factor λk+1,(28)λk+1=λ0,λ0>1,1,λ0≤1,where λ0=trNk+1/trMk+1 and tr⋅ is the tracing operator.


## 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 k=0, select x0 and P0, and calculate the weights of the sample points:(17)W0m=μY+μ,W0c=μY+μ+1−ε2+σ,Wim=Wic=μ2Y+μ,i=1,2,…,2Y,where μ is a scaling parameter, determined by μ=ε2Y+κ−Y; Y is the dimension of the state; ε is the control parameter of the distribution state of the sampling point (ε=0.01); κ=0 ; σ is a nonnegative weight coefficient (σ=2).Step 2. Perform UT. Calculate the Sigma point set from xk and Pk (the symmetric sampling strategy is used in this paper):(18)χi=xkxki+Y+μPkixki−Y+μPki,i=1∼Y,where Pki denotes the i−th column of the square root of the matrix.Step 3. Calculate the state prediction x^k+1/k and the state prediction covariance matrix Pk+1/k:(19)x^k+1/k=∑i=02YWimχi∗,Pk+1/k=∑i=02YaWiacχi∗−x^k+1/kχi∗−x^k+1/kT+ΓQkΓT,where Ya is the dimension of the state vector except for the noise state; if the dimension of the noise vector is Yw, then Y=Ya+Yw; Wiac is the weights of the sample points except for the noise state; χi∗=fχi; Qk is the system noise variance matrix.Step 4. Calculate the residual γk+1, the measurement prediction covariance Pzz,k+1, and the cross covariance Pxz,k+1 after introducing the fading factor λk+1.The normal strong tracking algorithm needs to perform the UT again to calculatez^k+1/k after the state prediction covariance matrix Pk+1/k after the introduction of the fading factor γk+1 was obtained, and the change of the state prediction covariance matrix Pk+1/k does not affect the estimation of the observed measurement z^k+1/k when the observation equation is linear.Proof:χi,k+1/k∗ is the Sigma sampling point sets obtained from x^k+1/k and Pk+1/k. When zk+1=Hk+1xk+1+v,(20)ηi,k+1/k∗=Hk+1χi,k+1/k∗,z^k+1/k=Hk+1∑i=02YWicχi,k+1/k∗=Hk+1x^k+1/k.It can be concluded thatz^k+1/k only depends on Hk+1 and x^k+1/k and not on Pk+1/k.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λk+1, this paper introduces the fading factor λk+1 directly in the calculation of the measurement prediction covariance Pzz,k+1 and the cross covariance Pxz,k+1, which can be obtained:(21)γk+1=zk+1−z^k+1/k,Pzz,k+1=λk+1∑i=02YWciηi∗−z^k+1/kηi∗−z^k+1/kT+Rk+1=λk+1∑i=02YWciHk+1χi∗−x^k+1/kHk+1χi∗−x^k+1/kT+Rk+1,where Rk+1 is the measurement noise variance matrix.One has [34](22)∑i=02Yawiacχi,k+1/k∗−x^k+1/kχi,k+1/k∗−x^k+1/kT+ΓkQkΓkT=∑i=02YWicχi,k+1/k∗−x^k+1/kχi,k+1/k∗−x^k+1/kT=Pk+1/k.It can be obtained that(23)Pzz,k+1=λk+1Hk+1Pk+1/kHk+1T+Rk+1,Pxz,k+1=λk+1∑i=02YWicχi∗−x^k+1/kηi∗−z^k+1/kT=λk+1Pk+1/kHk+1T.Step 5. Measurement updating: calculate the gain matrix Kk+1, the state estimate x^k+1, and the estimated error covariance matrix Pk+1 to complete the entire filtering process:(24)Kk+1=Pxz,k+1Pzz,k+1−1,x^k+1=x^k+1/k+Kk+1zk+1−z^k+1/k,Pk+1=λk+1Pk+1/k−Kk+1Pzz,k+1Kk+1T.


## 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.Define(25)Nk+1=Vγ,k+1−βRk+1−Hk+1ΓQkΓTHk+1T,Mk+1=Hk+1∑i=02YWicχi,k+1/k−x^k+1/k⋅χi,k+1/k−x^k+1/kTHk+1T,where β is the weakening factor and β≥1; Vγ,k+1 is the residual covariance matrix, and the calculation process of it is unknown. It can be estimated by(26)Vγ,k+1=γ1γT1,k=0,ρVγ,k+γk+1γTk+11+ρ,k≥1,where ρ is a forgetting factor and 0<ρ≤1.According to the principle of orthogonalization, it can be obtained that(27)λk+1Mk+1=Nk+1.Tracing both sides of (27) to obtain the suboptimal fading factor λk+1,(28)λk+1=λ0,λ0>1,1,λ0≤1,where λ0=trNk+1/trMk+1 and tr⋅ 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 (g=9.8m2/s), and the random noise is 0.5 mg; the velocity error noise is 0.1 m/s; the ideal attitude transformation matrix Cnb 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φ0=1°1°15°T. The simulation results of alignment error are shown in Figure 7.Figure 7
The alignment error of the alignment model with heading error aided or not: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)Figure7 shows that when the initial misalignment angle is φ0=1°1°15°T, 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.Table 2
The statistical properties of the simulation results.
Normal model (°)Heading error aided model (°)MeanPitch error0.03240.0162Roll error0.04520.0228Heading error1.05420.7243VariancePitch error0.06340.0326Roll error0.07230.0533Heading error2.14201.2713


#### 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 are10×1mgσ, 10×0.05°/sσ, and 10×0.028°σ , but the system noise variance in the filtering algorithm remains the same. The initial misalignment angle is φ0=1°1°15°T. The simulation results are shown in Figure 8.Figure 8
The alignment error of two filtering algorithms: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)As can be seen from Figure8, 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.Table 3
The statistical properties of the simulation results.
Mean (°)Variance (°)Pitch errorRoll errorHeading errorPitch errorRoll errorHeading errorUKF0.05230.07251.84390.08420.09233.4327SST-UKF0.11260.15821.06320.03260.04612.4459


#### 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φ0=1°1°30°T, φ0=1°1°45°T, and φ0=1°1°60°T, respectively. Figure 9 shows the simulation results.Figure 9
The alignment error of different large azimuth misalignment angles.Figure9 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.


## 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 (g=9.8m2/s), and the random noise is 0.5 mg; the velocity error noise is 0.1 m/s; the ideal attitude transformation matrix Cnb 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φ0=1°1°15°T. The simulation results of alignment error are shown in Figure 7.Figure 7
The alignment error of the alignment model with heading error aided or not: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)Figure7 shows that when the initial misalignment angle is φ0=1°1°15°T, 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.Table 2
The statistical properties of the simulation results.
Normal model (°)Heading error aided model (°)MeanPitch error0.03240.0162Roll error0.04520.0228Heading error1.05420.7243VariancePitch error0.06340.0326Roll error0.07230.0533Heading error2.14201.2713


### 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 are10×1mgσ, 10×0.05°/sσ, and 10×0.028°σ , but the system noise variance in the filtering algorithm remains the same. The initial misalignment angle is φ0=1°1°15°T. The simulation results are shown in Figure 8.Figure 8
The alignment error of two filtering algorithms: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)As can be seen from Figure8, 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.Table 3
The statistical properties of the simulation results.
Mean (°)Variance (°)Pitch errorRoll errorHeading errorPitch errorRoll errorHeading errorUKF0.05230.07251.84390.08420.09233.4327SST-UKF0.11260.15821.06320.03260.04612.4459


### 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φ0=1°1°30°T, φ0=1°1°45°T, and φ0=1°1°60°T, respectively. Figure 9 shows the simulation results.Figure 9
The alignment error of different large azimuth misalignment angles.Figure9 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.


## 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φ0=1°1°15°T. The simulation results of alignment error are shown in Figure 7.Figure 7
The alignment error of the alignment model with heading error aided or not: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)Figure7 shows that when the initial misalignment angle is φ0=1°1°15°T, 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.Table 2
The statistical properties of the simulation results.
Normal model (°)Heading error aided model (°)MeanPitch error0.03240.0162Roll error0.04520.0228Heading error1.05420.7243VariancePitch error0.06340.0326Roll error0.07230.0533Heading error2.14201.2713


## 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 are10×1mgσ, 10×0.05°/sσ, and 10×0.028°σ , but the system noise variance in the filtering algorithm remains the same. The initial misalignment angle is φ0=1°1°15°T. The simulation results are shown in Figure 8.Figure 8
The alignment error of two filtering algorithms: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)As can be seen from Figure8, 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.Table 3
The statistical properties of the simulation results.
Mean (°)Variance (°)Pitch errorRoll errorHeading errorPitch errorRoll errorHeading errorUKF0.05230.07251.84390.08420.09233.4327SST-UKF0.11260.15821.06320.03260.04612.4459


## 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φ0=1°1°30°T, φ0=1°1°45°T, and φ0=1°1°60°T, respectively. Figure 9 shows the simulation results.Figure 9
The alignment error of different large azimuth misalignment angles.Figure9 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 Figure1. 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.Figure 10
Onboard experimental system.Table 4
Specifications of SINS.
Gyroscope (°/s)Accelerometer (mg)Constant drift0.12Random drift0.550.5Table 5
Specifications of LSPS.
Laser receiverFan-shaped laser transmitterPhotosensitive wavelength600–1000 nmOutput power100 mvEffective irradiance10–60 W/m2Operating wavelength650 nm


### 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 is0°0°15°T. 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.Figure 11
The alignment error of onboard experiment: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)As can be seen from Figure11, 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.Table 6
The statistical properties of the experiment results.
Mean (°)Variance (°)Pitch errorRoll errorHeading errorPitch errorRoll errorHeading errorUKF0.06320.64531.40630.11580.10432.7352SST-UKF0.03250.28420.97450.06430.05381.7436


## 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 Figure1. 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.Figure 10
Onboard experimental system.Table 4
Specifications of SINS.
Gyroscope (°/s)Accelerometer (mg)Constant drift0.12Random drift0.550.5Table 5
Specifications of LSPS.
Laser receiverFan-shaped laser transmitterPhotosensitive wavelength600–1000 nmOutput power100 mvEffective irradiance10–60 W/m2Operating wavelength650 nm


## 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 is0°0°15°T. 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.Figure 11
The alignment error of onboard experiment: (a) pitch errors; (b) roll errors; (c) heading errors.
(a)(b)(c)As can be seen from Figure11, 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.Table 6
The statistical properties of the experiment results.
Mean (°)Variance (°)Pitch errorRoll errorHeading errorPitch errorRoll errorHeading errorUKF0.06320.64531.40630.11580.10432.7352SST-UKF0.03250.28420.97450.06430.05381.7436


## 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.



---

*Source: 3101203-2020-06-17.xml*