# Precise Shearer Positioning Technology Using Shearer Motion Constraint and Magnetometer Aided SINS

Authors: Ming Yan; Zengcai Wang
Journal: Mathematical Problems in Engineering
 (2021)

---

## Abstract

The key technology to realize intelligent unmanned coal mining is the strapdown inertial navigation system (SINS); however, the gradual increase of cumulative error during the working process needs to be solved. On the basis of an SINS/odometer (OD)-integrated navigation system, this paper adds magnetometer (MAG)-aided positioning and proposes an SINS/OD/MAG-integrated shearer navigation system. The velocity observation equation is obtained from the speed constraints during shearer movement, and the yaw angle observation equation is obtained from the magnetometer output. The position information of the SINS output is calibrated using these two observations. In order to improve the fault tolerance of the integrated navigation system, an adaptive federated Kalman filter is established to complete the data fusion of the SINS. Experimental results show that the positioning accuracy of the SINS/OD/MAG-integrated navigation system is 75.64% and 74.01% higher in the east and north directions, respectively, than the SINS/OD-integrated navigation system.


---

## Body

## 1. Introduction

Coal is China’s main energy source. Since coal mines in China are mostly subterranean, the probability of a mining accident is high. The most effective way to solve this problem is to increase the degree of automation of mining equipment, to realize intelligent mining and keep miners away from dangerous mine faces [1]. The shearer is core equipment for the fully mechanized coal mining face. Obtaining accurate shearer position and attitude information and the running track of shearer is key to realizing coal mine automation and unmanned mining [2]. GPS is a commonly used positioning system for terrestrial equipment. However, satellite positioning signals cannot be received in subterranean coal mines. Therefore, GPS cannot be used to position shearers, and a more autonomous positioning method must be used [3, 4]. The strapdown inertial navigation system (SINS) is directly mounted to the body of the shearer. The angular velocity and acceleration information of the shearer are measured in real-time using an internal gyroscope and accelerometer, and the attitude and position information of the shearer are solved by integration. No external information input is required during this process. The SINS has the characteristics of autonomy, suitability for concealment, and strong anti-interference ability, making it suitable for subterranean use, and has thus become the mainstream direction for shearer positioning research [5]. The SINS can achieve a high positioning accuracy in a short time, but it generates a large cumulative error with the passage of time. This problem is the focus of the current research [6].The Commonwealth Scientific and Industrial Research Organization of Australia (CSIRO) has achieved precise shearer positioning by using high-precision fiber optic gyroscopes and accelerometers [7], but the severe vibration of shearers seriously interferes with the operation of fiber optic gyroscopes, and their cost is high making them difficult to promote. Therefore, a MEMS inertial measurement unit (MIMU) with lower cost and higher reliability is used in this paper, and low-cost advantage of MIMU is conducive to the promotion of shearer positioning technology. However, compared with optical gyroscopes, the precision of MIMU is lower and the cumulative error generated is greater. Therefore, it is necessary to rely on other sensor information to calibrate the SINS. Using MIMU with low precision and high reliability to realize the accurate positioning of shearer is the research focus at this stage.In order to reduce the influence of the cumulative error of SINS on measurement accuracy, a variety of SINS-based integrated navigation systems have appeared. Chengming et al. [8] proposed using a wireless sensor network (WSN) to determine the position of the shearer. On this basis, Fan et al. [9] proposed an integrated SINS/WSN navigation system, mobile nodes were installed on the shearer and anchor nodes were installed on the top beam of the hydraulic support, and the duration and angle of wireless signal transmission between mobile node and anchor node is used to realize real-time positioning of the shearer. However, the frequent movement of the hydraulic support makes it difficult to obtain accurate reference coordinate values, which seriously affects the positioning accuracy. Dunn et al. [10] and others proposed the use of Doppler radar to measure the relative ground speed of the shearer, so as to correct the velocity error of SINS. However, Doppler radar is susceptible to the surrounding environment when it is used underground; thus, it is difficult to obtain an accurate speed. The integrated navigation system employing SINS and an odometer (OD) is a widely used and highly reliable. The axial encoder is most commonly used as an odometer and is installed in the walking part of the shearer to measure the driving speed, a gyroscope is installed on the body of the shearer to measure the attitude angle of the shearer, and dead reckoning (DR) is used to obtain position information [11]. This method avoids the quadratic integration of acceleration when SINS is used to solve for the position and effectively improves the positioning accuracy, but this method could not suppress the accumulated errors of attitude angle of SINS, and the calculated position information of shearer would still produce large errors over time. On this basis, combined with the speed and position constraints of a shearer in motion, scholars have put forward a variety of calibration methods. The authors in [12, 13] employ zero velocity correction technology to reduce SINS errors based on the motion constraints of shearer, the lateral and longitudinal velocities of SINS output in the carrier coordinate system of the shearer are taken as error values, and a linear Kalman filter is established based on the measured values, so as to improve the measurement accuracy of the attitude angle. In [14], the stroke displacement of the hydraulic support pushing jack is used to predict the position of the scraper conveyor when the next coal cutting is performed, the difference between the predicted value of the scraper conveyor position and the measured value of the next coal cutting position is taken as the observation measurement, and experiment shows that the positioning error of each cutting cycle can be effectively reduced within 1 h. The authors [15] combined the above two methods; compared with the pure inertial navigation system, the positioning accuracy of the shearer in the east and north directions was improved by 54% and 56%, respectively. In the research, it is found that if the error of the first cutter of the shearer can be limited, then the error of the subsequent several cutters will not diverge. Therefore, the focus of future research should be on improving the positioning accuracy of the first coal cutter. On the whole, the positioning accuracy of the DR method depends heavily on the attitude angles, especially the accuracy of the yaw angle, but the existing algorithms lack the observation of yaw angle.The magnetometer can measure intensity information, and the geomagnetic intensity information can be used to calculate the magnetic yaw angle without the cumulative error when the carrier pitch and roll angles are known. Based on this principle, most measurement-while-drilling (MWD) systems in coal mines obtain attitude angles through accelerometers and magnetometers [16, 17]. Considering the characteristics of magnetometer, a magnetometer is introduced into the SINS of shearer, and the yaw angle calculated by the magnetometer is used to calibrate the yaw angle calculated by SINS, so as to improve the positioning accuracy of shearer. Although the magnetometer itself does not produce cumulative errors, it needs to be calibrated before use and is easily disturbed by changes in the external magnetic field. Li et al. [18] proposed a 12-parameter fitting method, which can better compensate the soft magnetic field interference of the surrounding environment. The study [19] used a support vector regression machine to realize a software-based antimagnetic interference method for MWD. The simulation results show that the predicted azimuthal error is less than 1°, which proves the feasibility of magnetometer for applications in subterranean coal mine environments.The integrated navigation system generally uses Kalman filters to complete data fusion, and most of which are centralized Kalman filters (CKFs). That is, the state estimation and measurement information from all sensors are processed in one filter. Thus, CKF is not fault tolerant. If the operation of any sensor in the system is affected by interference [20], due to the large number of sensors in the multisensor integrated navigation system, the probability of sensor failure or interference is higher, and CKF is not suitable for such systems. The distributed Kalman filter is used to group sensors into subsystems, and each subsystem can estimate the state independently, which can effectively reduce the amount of calculation [21, 22]. Federated Kalman filter (FKF) is a distributed Kalman filter based on information distribution principle proposed by Carlson [23], which has the function of fault diagnosis and isolation. When a navigation subsystem fails, FKF can identify the fault subsystem and shield the fault subsystem through information distribution coefficient, so as to improve the fault tolerance of the multisensor-integrated navigation system [24, 25].In order to improve the positioning accuracy of the shearer for the first coal cutting, a magnetometer is added to calibrate the yaw angle based on the SINS/OD-integrated navigation system, and the SINS/OD/MAG-integrated navigation system is proposed. Compared with other studies, the main contributions of this paper are as follows: (1) the shearer motion constraint and magnetometer are used to assist the SINS of the shearer, so as to improve the positioning accuracy. The state equation and observation equation of the SINS/OD/MAG-integrated navigation system are established. (2) The mathematical model of the adaptive federated Kalman filter (AFKF) is established as the SINS/OD/MAG data fusion algorithm. The SINS/OD and SINS/MAG are regarded as two independent subsystems. AFKF can suppress the influence of the magnetic yaw angle calculation error on the positioning accuracy of the integrated navigation system.The rest of this paper is as follows. In Section2, the SINS error equation is simplified and the state equation is established according to the operation characteristics of shearer. The velocity observation equation and yaw angle observation equation are established by using the motion constraint of shearer and the magnetic declination angle calculated by magnetometer in Section 3. The mathematical model of the AFKF is established in Section 4. In Section 5, an experimental platform is established to simulate the coal cutting process of the shearer, and the performance of the proposed SINS/OD/MAG-integrated navigation system is verified.


## 2. State Equation of Shearer SINS

The principle of the shearer positioning system is shown in Figure1. The SINS is installed on the shearer body, and an axial encoder is installed on the walking part of the shearer as an odometer. The East-North-Up coordinate system is used as the navigation coordinate system (n system) of the shearer; the carrier coordinate system (b system) takes the shearer’s center of gravity as the origin and the shearer’s forward direction as the yb axis, and coal mining. The horizontal axis of the machine body is the xb axis to the right, and the vertical axis is the zb axis.Figure 1
Principle of the shearer positioning system.The equation of state of shearer’s SINS is derived from the error equation of SINS. The attitude error, velocity error, and position error equations are, respectively,(1)φ˙=−ωinn×φ+δωinn−Cbnεgb,δv˙n=fn×φ−2ωien+ωenn×δvn+vn×2δωien+δωenn+Cbnεab,δP˙=δζvn+ζδvn,where δ is the error of each physical quantity. φ=ϕEϕNϕUT is the misalignment angle vector. vn=vEnvNnvUnT is the shearer velocity vector in the navigation coordinate system. P=LλhT is the latitude, longitude, and altitude information of the shearer. εgb is the gyroscope error vector in the carrier coordinate system, and εab is the accelerometer error vector in the carrier coordinate system. Cbn is the transformation matrix from the carrier coordinate system to the navigation coordinate system. ωien is the angular velocity of the Earth. ωenn is the projection of the angular velocity of the navigation coordinate system relative to the Earth coordinate system in the navigation coordinate system. fn=fEfNfUT is the acceleration value measured by the accelerometer in the carrier coordinate system. ζ is the conversion matrix between the carrier linear velocity and the latitude and longitude angular velocity.Since the shearer’s moving speed is slow, its position will not change significantly in a short time, so the speed error and position error in the error equation are ignored, and the simplified SINS equation of the shearer is expanded as follows:(2)ϕ˙E=ωiesinL+vEntanLRN+hϕN−ωiecosL+vEnRN+hϕU−εgEn,ϕ˙N=−ωiesinL+vEntanLRN+hϕE−vNnRM+hϕU−εgNn,ϕ˙U=ωiecosL+vEnRN+hϕE+vNnRM+hϕN−εgUn,(3)δv˙En=−fUϕN+fNϕU+εaEn,δv˙Nn=fUϕE−fEϕU+εaNn,δv˙Un=−fNϕE+fEϕN+εaUn,(4)δL˙=1RM+hδvNn,δλ˙=secLRN+hδvEn,δh˙=δvUn,where RM is the curvature radius of the local meridian and RN is the curvature radius of the local unitary circle.The attitude angle error, velocity error, position error, gyroscope error, and accelerometer error of the shearer’s strapdown inertial navigation system are taken as state variables. The state vector is(5)X=φTδvnTδPTεgbTεabTT.The equation of state of SINS is(6)X˙=FSINSX+w,where FSINS is the state transition matrix, which can be obtained by equations (2)–(4). w is the system noise vector, which conforms to Gaussian white noise.


## 3. Establishment of Observation Equation

### 3.1. Velocity Observation Equation

The shearer travels on the tracks of the scraper conveyor, and thus the shearer only has speed in the longitudinal direction due to the constrained movement. Therefore, the speed in thexb-axis and zb-axis directions is zero, and the output of the axial encoder is the speed vyb in theyb-axis direction of the shearer, and the speed vector of the shearer measured by the axial encoder OD in the carrier coordinate system is(7)vodb=0vyb0.Considering the misalignment angle error and scale error of the axial encoder, the speed of the shearer measured by the axial encoder in the navigation coordinate system is(8)v^odn=I3×3−φ×Cbn1+δkvodb,where φ× is the antisymmetric matrix of the misalignment angle vectors, and δk is the error factor for the axial encoder. After expanding equation (13), we obtain(9)v^odn=vodn+δkvodn+vodn×φ−δkvodn×φ.Ignoring the small second-order termδkvodn×φ in formula (14), the error formula for axial encoder speed measurement in the navigation coordinate system is obtained:(10)δvod=v^odn−vodn=δkvodn+vodn×φ.The closing errorZv between the velocity calculated by SINS and the velocity measured by the axial encoder is taken as an observation measurement by the filter. After the real value is offset, the observed measurement is the difference between this value and the error value:(11)Zv=v^SINSn−v^odn=δvn−vodn×φ−δkvodn.The state quantity of the system is defined as(12)X1,k=φTδvnTδPTεgbTεabTδkT.Thus, the system state equation is thus(13)X˙1,k=F1,kX1,k+G1,kw1,k,where w1,k is the system noise matrix, G1,k is the system noise driving matrix, and F1,k is the state transition matrix of the SINS/OD subsystem, which can be expressed as(14)F1,k=FSINS00016×16.The observation quantity of the system is as follows:(15)Z1,k=ZvT.The measurement equation is as follows:(16)Z1,k=H1,kX1,k+v1,k,where v1,k is the observation noise vector of the axial encoder, H1,k is the observation matrix, and its value is(17)H1,k=−vodn×I303×6−vodn.


### 3.2. Yaw Angle Observation Equation

With the advance of pitch angleθ and roll angle γ, the magnetic yaw angle can be calculated from the magnetometer measured value. The formula used for calculation is(18)ψMAG=arctanHzbsinγcosθ+Hybsinγsinθ+HxbcosγHybcosγ−Hzbsinγ,where Hxb, Hyb, and Hzb are the measured values of the three-axis magnetometer in the carrier coordinate system. There is an error between the magnetic yaw angle and the carrier yaw angle. The carrier yaw angle is calculated as(19)ψ=ψmag+δψmag,where δψmag is the magnetic declination, which is related to the latitude and longitude of the initial carrier position. Although there is an estimated error of magnetic declination, the longitude and latitude of the shearer will not change significantly due to the slow speed in the operation process, so the estimated error of magnetic declination can be ignored.The state quantity of the system is defined as(20)X2,k=φTδvnTδPTεgbTεabTT.Thus, the system error state equation is(21)X˙2,k=F2,kX2,k+G2,kw2,k,where w2,k is the system noise vector, G2,k is the system noise driving matrix, and F2,k is the system state matrix of the SINS/MAG subsystem, and it is(22)F2,k=FSINS.The closure error between the yaw angle obtained by the magnetometer and the yaw angle calculated by SINS is taken as another observation measurement, which cancels out the real value in the process of subtraction. Therefore, the closure errorZψ is actually the closure error between the yaw angle error of SINS and the measurement error of the magnetometer. Since the estimation error of the magnetic declination angle of the magnetometer is ignored, the closure error is(23)Zψ=ψSINS−ψmag=ϕU.The observation vector is defined as(24)Z2,k=Zψ.The measurement equation is as follows:(25)Z2,k=H2,kX2+v2,k,where v2,k is the corresponding observation noise vector and H2,k is the observation matrix, and the value of which is(26)H2,k=01×2101×12.


## 3.1. Velocity Observation Equation

The shearer travels on the tracks of the scraper conveyor, and thus the shearer only has speed in the longitudinal direction due to the constrained movement. Therefore, the speed in thexb-axis and zb-axis directions is zero, and the output of the axial encoder is the speed vyb in theyb-axis direction of the shearer, and the speed vector of the shearer measured by the axial encoder OD in the carrier coordinate system is(7)vodb=0vyb0.Considering the misalignment angle error and scale error of the axial encoder, the speed of the shearer measured by the axial encoder in the navigation coordinate system is(8)v^odn=I3×3−φ×Cbn1+δkvodb,where φ× is the antisymmetric matrix of the misalignment angle vectors, and δk is the error factor for the axial encoder. After expanding equation (13), we obtain(9)v^odn=vodn+δkvodn+vodn×φ−δkvodn×φ.Ignoring the small second-order termδkvodn×φ in formula (14), the error formula for axial encoder speed measurement in the navigation coordinate system is obtained:(10)δvod=v^odn−vodn=δkvodn+vodn×φ.The closing errorZv between the velocity calculated by SINS and the velocity measured by the axial encoder is taken as an observation measurement by the filter. After the real value is offset, the observed measurement is the difference between this value and the error value:(11)Zv=v^SINSn−v^odn=δvn−vodn×φ−δkvodn.The state quantity of the system is defined as(12)X1,k=φTδvnTδPTεgbTεabTδkT.Thus, the system state equation is thus(13)X˙1,k=F1,kX1,k+G1,kw1,k,where w1,k is the system noise matrix, G1,k is the system noise driving matrix, and F1,k is the state transition matrix of the SINS/OD subsystem, which can be expressed as(14)F1,k=FSINS00016×16.The observation quantity of the system is as follows:(15)Z1,k=ZvT.The measurement equation is as follows:(16)Z1,k=H1,kX1,k+v1,k,where v1,k is the observation noise vector of the axial encoder, H1,k is the observation matrix, and its value is(17)H1,k=−vodn×I303×6−vodn.


## 3.2. Yaw Angle Observation Equation

With the advance of pitch angleθ and roll angle γ, the magnetic yaw angle can be calculated from the magnetometer measured value. The formula used for calculation is(18)ψMAG=arctanHzbsinγcosθ+Hybsinγsinθ+HxbcosγHybcosγ−Hzbsinγ,where Hxb, Hyb, and Hzb are the measured values of the three-axis magnetometer in the carrier coordinate system. There is an error between the magnetic yaw angle and the carrier yaw angle. The carrier yaw angle is calculated as(19)ψ=ψmag+δψmag,where δψmag is the magnetic declination, which is related to the latitude and longitude of the initial carrier position. Although there is an estimated error of magnetic declination, the longitude and latitude of the shearer will not change significantly due to the slow speed in the operation process, so the estimated error of magnetic declination can be ignored.The state quantity of the system is defined as(20)X2,k=φTδvnTδPTεgbTεabTT.Thus, the system error state equation is(21)X˙2,k=F2,kX2,k+G2,kw2,k,where w2,k is the system noise vector, G2,k is the system noise driving matrix, and F2,k is the system state matrix of the SINS/MAG subsystem, and it is(22)F2,k=FSINS.The closure error between the yaw angle obtained by the magnetometer and the yaw angle calculated by SINS is taken as another observation measurement, which cancels out the real value in the process of subtraction. Therefore, the closure errorZψ is actually the closure error between the yaw angle error of SINS and the measurement error of the magnetometer. Since the estimation error of the magnetic declination angle of the magnetometer is ignored, the closure error is(23)Zψ=ψSINS−ψmag=ϕU.The observation vector is defined as(24)Z2,k=Zψ.The measurement equation is as follows:(25)Z2,k=H2,kX2+v2,k,where v2,k is the corresponding observation noise vector and H2,k is the observation matrix, and the value of which is(26)H2,k=01×2101×12.


## 4. Design of Adaptive Federated Kalman Filter

### 4.1. Principle of Adaptive Federated Kalman Filter

The workflow of the proposed SINS/OD/MAG-integrated navigation system is shown in Figure2. In the proposed integrated navigation system, SINS measures the acceleration and angular velocity information of the shearer at time k under the b coordinate system and updates the attitude angle, velocity, and position. Using SINS/OD as subsystem 1, the y-component of the shearer speed is measured by the axial encoder, thereby obtaining the speed vector of the shearer in the b coordinate system and converting it to the n coordinate system. The closing difference between the speed component v^odn of the shearer in the n coordinate system measured by the shaft encoder and the speed component v^SINSn of the shearer in the n coordinate system calculated by SINS is used as the observation vector Z1. Using SINS/MAG as subsystem 2, the yaw angle of the shearer is calculated by the magnetometer according to the roll and pitch angles measured by SINS, and the closing error of the yaw angle ψSINS calculated by SINS and the yaw angle ψmag measured by the magnetometer is taken as the observation vector Z2. The optimal error estimation value is obtained by inputting the error state and observation into the federated Kalman filter. The optimal error estimation value is returned to the SINS to correct the position information of the SINS output to improve the shearer positioning accuracy.Figure 2
Workflow of the SINS/OD/MAG-integrated navigation system.The adaptive federated Kalman filter presented in this paper is shown in Figure3. The FKF is a two-stage filter, in which SINS is the common reference system, and SINS/OD and SINS/MAG are local filters 1 and 2, respectively. The Xk of the SINS output is both used as the measurement value of the subfilter and applied to the main filter. Each local filter can calculate the local optimal estimation value X^i and the covariance matrix Pi. These values are input into the main filter and fused to obtain the global optimal estimation value X^g and the global covariance matrix Pg. βi,i=1,2 is the information distribution coefficient, which is determined according to the covariance matrix and is key to the information fusion process.Figure 3
Structure of adaptive federated Kalman filter.


### 4.2. Local Optimal Estimation of Subfilters

The state equations of the two subfilters of SINS/OD and SINS/MAG are(27)Xi,k=Φi,k,k−1Xi,k−1+Γi,k−1Wi,k−1,Zi,k=Hi,kXi,k+Vi,k,i=1,2.Wheni=1, the equation of state represents the SINS/OD subsystem and the values of X1,k, Φ1,k,k−1, Γ1,k−1, W1,k, Z1,k, H1,k, and V1,k can be obtained from Section 3.1; when i = 2, the equation of state represents the SINS/MAG subsystem and the values of X2,k, Φ2,k,k−1, Γ2,k−1, W2,k, Z2,k, H2,k, and V2,k can be obtained from Section 3.2.The local filter updates are divided into time and measurement updates. The time update process of each subfilter is(28)X^i,k/k−1=Φi,k,k−1X^i,k−1,i=1,2,Pi,k/k−1=Φi,k,k−1Pi,k−1Φi,k,k−1T+Γi,k−1Qi,k−1Γi,k−1T,i=1,2.The measurement update process of the subfilter is(29)X^i,k=X^i,k/k−1+Ki,kεi,k,i=1,2,where εi,k is the innovation and K is the filter gain matrix. The formulas used for calculation are(30)εi,k=Zi,k−Hi,kX^i,k/k−1,i=1,2,Ki,k=Pi,k/k−1Hi,kTHi,kPi,k/k−1Hi,kT+Ri,k−1,i=1,2.The formula for calculating the state error covariance matrix is(31)Pk=I−Ki,kHi,kPi,k/k−1I−Ki,kHi,kT+Ki,kRi,kKi,kT,i=1,2.During the update of the Kalman filter, the system noise varianceQi,k and the observation noise variance Ri,k determine the degree of utilization of the observations for the optimal estimate. The system noise variance depends on the random error of the sensor, but the setting of the observation noise variance is complex and related to the operating state of the shearer. In this paper, we assume that Qi,k is constant, and we use the innovation εi,k to estimate Ri,k:(32)Ri,k∗=∑k−N+1kei,kei,kTN,where Ri,k∗ is the estimated observation noise covariance to replace Ri,k and N is the number of samples.


### 4.3. Adaptive Information Allocation and Global Optimization Estimation

The information distribution coefficientβi is related to the eigenvalue of the estimation error covariance matrix Pi. The eigenvalue of Pi is the variance of the corresponding state quantity or the linear combination of the corresponding state quantities. The large eigenvalue of Pi proves that the variance of the state quantity estimation is large, indicating that the effectiveness of the filter is poor, and the accuracy of the subsystem is low, so βi should assume a larger value. On the contrary, a large eigenvalue of Pi indicates that the effectives of the filter is improved, and the subsystem accuracy is high, so βi should assume a small value. The distribution of adaptive information is an important feature resulting in high fault tolerance of the adaptive federated Kalman filter. The calculation formula of βi is(33)βi=trPitrP1+trP2,i=1,2,where tr represents the trace of the corresponding matrix, that is, the sum of all the eigenvalues of the matrix.It can be seen from Figure3 that X^g and Pg provide feedback, X^g is used to reset the local estimation of the subfilter, and Pg is used to reset feedback Pi after amplification, which is expressed by the formula(34)X^i=X^g,i=1,2,Pi=βi−1Pg,i=1,2.The main filter of the AFKF presented in this paper does not filter, so the estimated value of the main filter is the global optimal estimate, and the calculation formula of the global covariance matrix and the global optimal estimate are, respectively,(35)Pg=P1−1+P2−1−1,X^g=PgP1−1X^1+P2−1X^2.


## 4.1. Principle of Adaptive Federated Kalman Filter

The workflow of the proposed SINS/OD/MAG-integrated navigation system is shown in Figure2. In the proposed integrated navigation system, SINS measures the acceleration and angular velocity information of the shearer at time k under the b coordinate system and updates the attitude angle, velocity, and position. Using SINS/OD as subsystem 1, the y-component of the shearer speed is measured by the axial encoder, thereby obtaining the speed vector of the shearer in the b coordinate system and converting it to the n coordinate system. The closing difference between the speed component v^odn of the shearer in the n coordinate system measured by the shaft encoder and the speed component v^SINSn of the shearer in the n coordinate system calculated by SINS is used as the observation vector Z1. Using SINS/MAG as subsystem 2, the yaw angle of the shearer is calculated by the magnetometer according to the roll and pitch angles measured by SINS, and the closing error of the yaw angle ψSINS calculated by SINS and the yaw angle ψmag measured by the magnetometer is taken as the observation vector Z2. The optimal error estimation value is obtained by inputting the error state and observation into the federated Kalman filter. The optimal error estimation value is returned to the SINS to correct the position information of the SINS output to improve the shearer positioning accuracy.Figure 2
Workflow of the SINS/OD/MAG-integrated navigation system.The adaptive federated Kalman filter presented in this paper is shown in Figure3. The FKF is a two-stage filter, in which SINS is the common reference system, and SINS/OD and SINS/MAG are local filters 1 and 2, respectively. The Xk of the SINS output is both used as the measurement value of the subfilter and applied to the main filter. Each local filter can calculate the local optimal estimation value X^i and the covariance matrix Pi. These values are input into the main filter and fused to obtain the global optimal estimation value X^g and the global covariance matrix Pg. βi,i=1,2 is the information distribution coefficient, which is determined according to the covariance matrix and is key to the information fusion process.Figure 3
Structure of adaptive federated Kalman filter.


## 4.2. Local Optimal Estimation of Subfilters

The state equations of the two subfilters of SINS/OD and SINS/MAG are(27)Xi,k=Φi,k,k−1Xi,k−1+Γi,k−1Wi,k−1,Zi,k=Hi,kXi,k+Vi,k,i=1,2.Wheni=1, the equation of state represents the SINS/OD subsystem and the values of X1,k, Φ1,k,k−1, Γ1,k−1, W1,k, Z1,k, H1,k, and V1,k can be obtained from Section 3.1; when i = 2, the equation of state represents the SINS/MAG subsystem and the values of X2,k, Φ2,k,k−1, Γ2,k−1, W2,k, Z2,k, H2,k, and V2,k can be obtained from Section 3.2.The local filter updates are divided into time and measurement updates. The time update process of each subfilter is(28)X^i,k/k−1=Φi,k,k−1X^i,k−1,i=1,2,Pi,k/k−1=Φi,k,k−1Pi,k−1Φi,k,k−1T+Γi,k−1Qi,k−1Γi,k−1T,i=1,2.The measurement update process of the subfilter is(29)X^i,k=X^i,k/k−1+Ki,kεi,k,i=1,2,where εi,k is the innovation and K is the filter gain matrix. The formulas used for calculation are(30)εi,k=Zi,k−Hi,kX^i,k/k−1,i=1,2,Ki,k=Pi,k/k−1Hi,kTHi,kPi,k/k−1Hi,kT+Ri,k−1,i=1,2.The formula for calculating the state error covariance matrix is(31)Pk=I−Ki,kHi,kPi,k/k−1I−Ki,kHi,kT+Ki,kRi,kKi,kT,i=1,2.During the update of the Kalman filter, the system noise varianceQi,k and the observation noise variance Ri,k determine the degree of utilization of the observations for the optimal estimate. The system noise variance depends on the random error of the sensor, but the setting of the observation noise variance is complex and related to the operating state of the shearer. In this paper, we assume that Qi,k is constant, and we use the innovation εi,k to estimate Ri,k:(32)Ri,k∗=∑k−N+1kei,kei,kTN,where Ri,k∗ is the estimated observation noise covariance to replace Ri,k and N is the number of samples.


## 4.3. Adaptive Information Allocation and Global Optimization Estimation

The information distribution coefficientβi is related to the eigenvalue of the estimation error covariance matrix Pi. The eigenvalue of Pi is the variance of the corresponding state quantity or the linear combination of the corresponding state quantities. The large eigenvalue of Pi proves that the variance of the state quantity estimation is large, indicating that the effectiveness of the filter is poor, and the accuracy of the subsystem is low, so βi should assume a larger value. On the contrary, a large eigenvalue of Pi indicates that the effectives of the filter is improved, and the subsystem accuracy is high, so βi should assume a small value. The distribution of adaptive information is an important feature resulting in high fault tolerance of the adaptive federated Kalman filter. The calculation formula of βi is(33)βi=trPitrP1+trP2,i=1,2,where tr represents the trace of the corresponding matrix, that is, the sum of all the eigenvalues of the matrix.It can be seen from Figure3 that X^g and Pg provide feedback, X^g is used to reset the local estimation of the subfilter, and Pg is used to reset feedback Pi after amplification, which is expressed by the formula(34)X^i=X^g,i=1,2,Pi=βi−1Pg,i=1,2.The main filter of the AFKF presented in this paper does not filter, so the estimated value of the main filter is the global optimal estimate, and the calculation formula of the global covariance matrix and the global optimal estimate are, respectively,(35)Pg=P1−1+P2−1−1,X^g=PgP1−1X^1+P2−1X^2.


## 5. Experimental Verification

### 5.1. Description of Experimental Equipment

In order to test the positioning accuracy of the integrated shearer navigation system, a ground test platform was built employing a vehicle as a carrier to simulate the operation state of the shearer underground. The experimental equipment is shown in Figure4, and the experimental equipment includes an inertial sensor (Xsens MTi-30), a wheel speed sensor, CAN bus test equipment, and a GPS sensor. The MTi-30 sensor is a microinertial heading and attitude measurement sensor, which integrates a three-axis gyroscope, three-axis accelerometer, and three-axis magnetometer. The MTi-30 sensor can output raw data from each sensor and can also calculate the carrier attitude angle through the internal algorithm based on the data of the three internal sensors, which is used as the reference value. The performance specifications of the MTi-30 sensor are shown in Table 1. The principle of the wheel speed sensor of the car is the same as that of the shaft encoder of the shearer, which is used to measure the running speed of the carrier. The CAN bus test equipment includes CANNoe, CANcope, and CANStressDR, which are used to obtain vehicle wheel speed sensor data. GPS sensors are used to measure the actual running track of the carrier and serve as a reference for judging the positioning accuracy of the integrated navigation system.Figure 4
Schematic diagram of experimental platform and equipment.Table 1
Performance index of MTi-30.
Bias instabilityNoise densityNonlinearityGyroscope18°/h0.1°/s/√Hz0.03%FSAccelerometer40μg80μg√Hz0.03%FSMagnetometer0.5 mG200μG/√Hz0.1%FSAxial encoders are installed on the front wheels as the odometer carrier, and the running speed of the carrier is taken as the average output value of the left and right axial encoders. In order to reduce interference of the internal components of the vehicle on the magnetometer, the MTi-30 sensor and GPS sensor are installed on the middle of the outside of the roof. The two-point method [26] was used to eliminate the installation error angle of the sensor and compensate for acceleration error caused by the lever arm effect [27].


### 5.2. Performance Test of Integrated Navigation System

According to the coal cutting process of the shearer, the vehicle is driven at low speed to simulates the shearer to do a straight-line coal cutting movement and then executes inclined sumping, and the running track will be in the shape of a broken line. Four different navigation systems were used to calculate the carrier movement path, and the results were compared and analyzed. The path solution results are shown in Figure5. The blue line is the carrier movement path measured by GPS and takes it as reference value. The green curve is the carrier movement path solved by SINS, and due to the low accuracy of the MIMU sensor used in this project, the path solved by this method has a large deviation. The black curve is the path solved by the SINS/OD-integrated navigation system, because the integrated navigation system adopts the shearer motion constraint assistance, and the calculated path does not appear obvious divergence, but there are still obvious errors compared with the path measured by GPS. The red curve is the path solved by the SINS/OD/MAG-integrated navigation system. Compared with the SINS/OD-integrated navigation system, the path solved by the SINS/OD/MAG-integrated navigation system is more consistent with the path measured by GPS, which proves that using magnetometer to calibrate yaw angle can effectively improve the positioning accuracy of shearer.Figure 5
Comparison of SINS, SINS/OD, and SINS/OD/MAG solution paths.Figure6 shows the attitude angle of the carrier in the process of moving. The attitude angle calculated by SINS will diverge rapidly with time, resulting a large error. The pitch angle and roll angle calculated by SINS/OD- and SINS/OD/MAG-integrated navigation systems are basically the same, and there is no obvious divergence. The yaw angle calculated by the SINS/OD-integrated navigation system diverges obviously in the early stage of linear movement, which does not conform to the linear movement of the carrier, and the divergence of yaw angle is restrained in the stage of inclined sumping. The yaw angle calculated by the SINS/OD/MAG-integrated navigation system fluctuates in the range of 123.8255° to 125.0927° in the initial stage of linear motion, but there is no divergence, and there is no divergence or fluctuation in the subsequent time, which is more in line with the actual trajectory of the carrier.Figure 6
The attitude angle of the carrier. (a) Pitch angle of the carrier; (b) roll angle of the carrier; (c) yaw angle of the carrier.
(a)(b)(c)Figure7 shows the time varying positioning error of the integrated navigation system. Since the path error of SINS is large, this paper focuses on the comparison of the positioning error changes of the SINS/OD/MAG- and SINS/OD-integrated navigation systems in the east and north directions. The positioning errors of different navigation modes are shown in Table 2; in the east direction, the maximum error of the SINS/OD-integrated navigation system is 0.9012 m, which is 77.28% lower than that of SINS; the maximum error of the SINS/OD/MAG-integrated navigation system is 0.2195 m, which is 94.47% lower than that of SINS. After adding magnetometer calibration on the basis of the SINS/OD-integrated navigation system, the positioning accuracy of the east direction is improved by 75.64%. In the north direction, the maximum error of SINS/OD is 1.2491 m, which is 80.14% lower than that of SINS; the maximum error of SINS/OD/MAG in the north direction is 0.3246 m, which is 94.84% lower than that of SINS; thus, the north positioning accuracy of the SINS/OD/MAG-integrated navigation system is 74.01% higher than that of the SINS/OD-integrated navigation system. By analyzing the positioning error change curve of Figure 7, it can be seen that compared with the SINS/OD-integrated navigation system error, the SINS/OD/MAG-integrated navigation system error has been effectively suppressed, and the divergence degree has been significantly reduced.Figure 7
The position error of the carrier. (a) East position error. (b) North position error.
(a)(b)Table 2
Positioning error of different navigation modes.
Navigation methodSINSSINS/ODError-reduction rate (%)SINS/OD/MAGError-reduction rate (%)East error variation (m)3.96640.901277.280.219594.47North error variation (m)6.29061.249180.140.324694.94In order to verify that the adaptive FKF proposed in this paper has better fault tolerance than the centralized Kalman filter, the path of magnetometer subjected to magnetic interference is analyzed. Figure8 shows the paths solved by FKF and CKF, respectively. The solution path of CKF shows a large deviation due to the magnetic interference, and the maximum errors in the east and north directions are 3.8268 m and 1.9420 m, respectively. By comparison, the FKF proposed in this paper has a small path deviation, and the maximum errors in the east and north directions are reduced to 1.5452 m and 1.9420 m, respectively. It can be seen from Figure 8 that the path solved by the FKF is similar to that calculated by the CKF in the early stage, and the error is gradually reduced in the later stages. At the end of the experiment, the error in the east and north directions decreased to 0.9866 m and 1.2015 m, respectively.Figure 8
Result of path calculation under interference of magnetometer.Figure9 shows the attitude angle of the carrier under magnetic interference. As shown in the figure, there is no obvious difference between CKF and FKF in the calculation of pitch angle and roll angle. It can be seen from the curve of yaw angle that the magnetometer is seriously disturbed in the range of 10∼30 s, and there is a serious deviation in the yaw angle calculation. However, when the magnetometer is disturbed, the yaw angle error calculated by the FKF is smaller than that by CKF. The experimental results show that the FKF has better fault tolerance and can effectively reduce the impact of a single system fault on the whole system when a sensor in the SINS/OD/MAG-integrated navigation system fails or is susceptible to interference.Figure 9
Attitude angle of carrier under magnetic interference. (a) Pitch angle of the carrier, (b) roll angle of the carrier, and (c) yaw angle of the carrier.
(a)(b)(c)


## 5.1. Description of Experimental Equipment

In order to test the positioning accuracy of the integrated shearer navigation system, a ground test platform was built employing a vehicle as a carrier to simulate the operation state of the shearer underground. The experimental equipment is shown in Figure4, and the experimental equipment includes an inertial sensor (Xsens MTi-30), a wheel speed sensor, CAN bus test equipment, and a GPS sensor. The MTi-30 sensor is a microinertial heading and attitude measurement sensor, which integrates a three-axis gyroscope, three-axis accelerometer, and three-axis magnetometer. The MTi-30 sensor can output raw data from each sensor and can also calculate the carrier attitude angle through the internal algorithm based on the data of the three internal sensors, which is used as the reference value. The performance specifications of the MTi-30 sensor are shown in Table 1. The principle of the wheel speed sensor of the car is the same as that of the shaft encoder of the shearer, which is used to measure the running speed of the carrier. The CAN bus test equipment includes CANNoe, CANcope, and CANStressDR, which are used to obtain vehicle wheel speed sensor data. GPS sensors are used to measure the actual running track of the carrier and serve as a reference for judging the positioning accuracy of the integrated navigation system.Figure 4
Schematic diagram of experimental platform and equipment.Table 1
Performance index of MTi-30.
Bias instabilityNoise densityNonlinearityGyroscope18°/h0.1°/s/√Hz0.03%FSAccelerometer40μg80μg√Hz0.03%FSMagnetometer0.5 mG200μG/√Hz0.1%FSAxial encoders are installed on the front wheels as the odometer carrier, and the running speed of the carrier is taken as the average output value of the left and right axial encoders. In order to reduce interference of the internal components of the vehicle on the magnetometer, the MTi-30 sensor and GPS sensor are installed on the middle of the outside of the roof. The two-point method [26] was used to eliminate the installation error angle of the sensor and compensate for acceleration error caused by the lever arm effect [27].


## 5.2. Performance Test of Integrated Navigation System

According to the coal cutting process of the shearer, the vehicle is driven at low speed to simulates the shearer to do a straight-line coal cutting movement and then executes inclined sumping, and the running track will be in the shape of a broken line. Four different navigation systems were used to calculate the carrier movement path, and the results were compared and analyzed. The path solution results are shown in Figure5. The blue line is the carrier movement path measured by GPS and takes it as reference value. The green curve is the carrier movement path solved by SINS, and due to the low accuracy of the MIMU sensor used in this project, the path solved by this method has a large deviation. The black curve is the path solved by the SINS/OD-integrated navigation system, because the integrated navigation system adopts the shearer motion constraint assistance, and the calculated path does not appear obvious divergence, but there are still obvious errors compared with the path measured by GPS. The red curve is the path solved by the SINS/OD/MAG-integrated navigation system. Compared with the SINS/OD-integrated navigation system, the path solved by the SINS/OD/MAG-integrated navigation system is more consistent with the path measured by GPS, which proves that using magnetometer to calibrate yaw angle can effectively improve the positioning accuracy of shearer.Figure 5
Comparison of SINS, SINS/OD, and SINS/OD/MAG solution paths.Figure6 shows the attitude angle of the carrier in the process of moving. The attitude angle calculated by SINS will diverge rapidly with time, resulting a large error. The pitch angle and roll angle calculated by SINS/OD- and SINS/OD/MAG-integrated navigation systems are basically the same, and there is no obvious divergence. The yaw angle calculated by the SINS/OD-integrated navigation system diverges obviously in the early stage of linear movement, which does not conform to the linear movement of the carrier, and the divergence of yaw angle is restrained in the stage of inclined sumping. The yaw angle calculated by the SINS/OD/MAG-integrated navigation system fluctuates in the range of 123.8255° to 125.0927° in the initial stage of linear motion, but there is no divergence, and there is no divergence or fluctuation in the subsequent time, which is more in line with the actual trajectory of the carrier.Figure 6
The attitude angle of the carrier. (a) Pitch angle of the carrier; (b) roll angle of the carrier; (c) yaw angle of the carrier.
(a)(b)(c)Figure7 shows the time varying positioning error of the integrated navigation system. Since the path error of SINS is large, this paper focuses on the comparison of the positioning error changes of the SINS/OD/MAG- and SINS/OD-integrated navigation systems in the east and north directions. The positioning errors of different navigation modes are shown in Table 2; in the east direction, the maximum error of the SINS/OD-integrated navigation system is 0.9012 m, which is 77.28% lower than that of SINS; the maximum error of the SINS/OD/MAG-integrated navigation system is 0.2195 m, which is 94.47% lower than that of SINS. After adding magnetometer calibration on the basis of the SINS/OD-integrated navigation system, the positioning accuracy of the east direction is improved by 75.64%. In the north direction, the maximum error of SINS/OD is 1.2491 m, which is 80.14% lower than that of SINS; the maximum error of SINS/OD/MAG in the north direction is 0.3246 m, which is 94.84% lower than that of SINS; thus, the north positioning accuracy of the SINS/OD/MAG-integrated navigation system is 74.01% higher than that of the SINS/OD-integrated navigation system. By analyzing the positioning error change curve of Figure 7, it can be seen that compared with the SINS/OD-integrated navigation system error, the SINS/OD/MAG-integrated navigation system error has been effectively suppressed, and the divergence degree has been significantly reduced.Figure 7
The position error of the carrier. (a) East position error. (b) North position error.
(a)(b)Table 2
Positioning error of different navigation modes.
Navigation methodSINSSINS/ODError-reduction rate (%)SINS/OD/MAGError-reduction rate (%)East error variation (m)3.96640.901277.280.219594.47North error variation (m)6.29061.249180.140.324694.94In order to verify that the adaptive FKF proposed in this paper has better fault tolerance than the centralized Kalman filter, the path of magnetometer subjected to magnetic interference is analyzed. Figure8 shows the paths solved by FKF and CKF, respectively. The solution path of CKF shows a large deviation due to the magnetic interference, and the maximum errors in the east and north directions are 3.8268 m and 1.9420 m, respectively. By comparison, the FKF proposed in this paper has a small path deviation, and the maximum errors in the east and north directions are reduced to 1.5452 m and 1.9420 m, respectively. It can be seen from Figure 8 that the path solved by the FKF is similar to that calculated by the CKF in the early stage, and the error is gradually reduced in the later stages. At the end of the experiment, the error in the east and north directions decreased to 0.9866 m and 1.2015 m, respectively.Figure 8
Result of path calculation under interference of magnetometer.Figure9 shows the attitude angle of the carrier under magnetic interference. As shown in the figure, there is no obvious difference between CKF and FKF in the calculation of pitch angle and roll angle. It can be seen from the curve of yaw angle that the magnetometer is seriously disturbed in the range of 10∼30 s, and there is a serious deviation in the yaw angle calculation. However, when the magnetometer is disturbed, the yaw angle error calculated by the FKF is smaller than that by CKF. The experimental results show that the FKF has better fault tolerance and can effectively reduce the impact of a single system fault on the whole system when a sensor in the SINS/OD/MAG-integrated navigation system fails or is susceptible to interference.Figure 9
Attitude angle of carrier under magnetic interference. (a) Pitch angle of the carrier, (b) roll angle of the carrier, and (c) yaw angle of the carrier.
(a)(b)(c)


## 6. Conclusion

In order to solve the problem of low positioning accuracy of the shearer navigation system using low-cost IMU, this paper presents an integrated navigation system based on sharer motion constraint and magnetometer assisted SINS. The main contributions of this paper are as follows:(1)
On the basis of the SINS/OD-integrated navigation system, the magnetometer was added to correct the yaw angle, and the SINS/OD/MAG-integrated navigation system was established. The experimental results show that compared with the SINS/OD-integrated navigation system, the positioning accuracy of the SINS/OD/MAG-integrated navigation system is improved by 75.64% and 74.01% in the east and north directions respectively, which can effectively improve the positioning accuracy of shearer.(2)
The federated Kalman filter is designed as a data fusion algorithm to improve the robustness and reliability of the integrated navigation system. Combined with the analysis of experimental results, FKF can effectively reduce the impact of faults on the integrated navigation system when the magnetometer experiences interference.Although the federated Kalman filter can effectively improve the antimagnetic interference ability of the SINS/OD/MAG-integrated navigation system, the problem that the magnetometer is vulnerable to interference has not been fundamentally solved. We plan to implement software antimagnetic research in the form of neural networks to the magnetometer to further improve the positioning accuracy of the SINS/OD/MAG-integrated navigation system.



---

*Source: 1679014-2021-03-08.xml*