PreciseShearerPositioningTechnologyUsingShearerMotion … · 1 day ago · precise positioning...

Research ArticlePrecise Shearer Positioning Technology Using Shearer MotionConstraint and Magnetometer Aided SINS

Ming Yan and Zengcai Wang

School of Mechanical Engineering Shandong University Jinan 250061 China

Correspondence should be addressed to Zengcai Wang wangzcsdueducn

Received 24 August 2020 Revised 29 January 2021 Accepted 28 February 2021 Published 8 March 2021

Academic Editor Ramon Sancibrian

Copyright copy 2021 Ming Yan and Zengcai Wang is is an open access article distributed under the Creative CommonsAttribution License which permits unrestricted use distribution and reproduction in anymedium provided the original work isproperly cited

e key technology to realize intelligent unmanned coal mining is the strapdown inertial navigation system (SINS) however thegradual increase of cumulative error during the working process needs to be solved On the basis of an SINSodometer (OD)-integrated navigation system this paper adds magnetometer (MAG)-aided positioning and proposes an SINSODMAG-in-tegrated shearer navigation system e velocity observation equation is obtained from the speed constraints during shearermovement and the yaw angle observation equation is obtained from the magnetometer output e position information of theSINS output is calibrated using these two observations In order to improve the fault tolerance of the integrated navigation systeman adaptive federated Kalman filter is established to complete the data fusion of the SINS Experimental results show that thepositioning accuracy of the SINSODMAG-integrated navigation system is 7564 and 7401 higher in the east and northdirections respectively than the SINSOD-integrated navigation system

1 Introduction

Coal is Chinarsquos main energy source Since coal mines inChina are mostly subterranean the probability of a miningaccident is highemost effective way to solve this problemis to increase the degree of automation of mining equipmentto realize intelligent mining and keep miners away fromdangerous mine faces [1] e shearer is core equipment forthe fully mechanized coal mining face Obtaining accurateshearer position and attitude information and the runningtrack of shearer is key to realizing coal mine automation andunmanned mining [2] GPS is a commonly used positioningsystem for terrestrial equipment However satellite posi-tioning signals cannot be received in subterranean coalmines erefore GPS cannot be used to position shearersand a more autonomous positioning method must be used[3 4] e strapdown inertial navigation system (SINS) isdirectly mounted to the body of the shearer e angularvelocity and acceleration information of the shearer aremeasured in real-time using an internal gyroscope andaccelerometer and the attitude and position information of

the shearer are solved by integration No external infor-mation input is required during this process e SINS hasthe characteristics of autonomy suitability for concealmentand strong anti-interference ability making it suitable forsubterranean use and has thus become the mainstreamdirection for shearer positioning research [5] e SINS canachieve a high positioning accuracy in a short time but itgenerates a large cumulative error with the passage of timeis problem is the focus of the current research [6]

e Commonwealth Scientific and Industrial ResearchOrganization of Australia (CSIRO) has achieved preciseshearer positioning by using high-precision fiber optic gy-roscopes and accelerometers [7] but the severe vibration ofshearers seriously interferes with the operation of fiber opticgyroscopes and their cost is high making them difficult topromote erefore a MEMS inertial measurement unit(MIMU) with lower cost and higher reliability is used in thispaper and low-cost advantage of MIMU is conducive to thepromotion of shearer positioning technology Howevercompared with optical gyroscopes the precision of MIMU islower and the cumulative error generated is greater

HindawiMathematical Problems in EngineeringVolume 2021 Article ID 1679014 12 pageshttpsdoiorg10115520211679014

erefore it is necessary to rely on other sensor informationto calibrate the SINS Using MIMU with low precision andhigh reliability to realize the accurate positioning of sheareris the research focus at this stage

In order to reduce the influence of the cumulative errorof SINS on measurement accuracy a variety of SINS-basedintegrated navigation systems have appeared Chengminget al [8] proposed using a wireless sensor network (WSN) todetermine the position of the shearer On this basis Fan et al[9] proposed an integrated SINSWSN navigation systemmobile nodes were installed on the shearer and anchor nodeswere installed on the top beam of the hydraulic support andthe duration and angle of wireless signal transmission be-tween mobile node and anchor node is used to realize real-time positioning of the shearer However the frequentmovement of the hydraulic support makes it difficult toobtain accurate reference coordinate values which seriouslyaffects the positioning accuracy Dunn et al [10] and othersproposed the use of Doppler radar to measure the relativeground speed of the shearer so as to correct the velocityerror of SINS However Doppler radar is susceptible to thesurrounding environment when it is used undergroundthus it is difficult to obtain an accurate speede integratednavigation system employing SINS and an odometer (OD) isa widely used and highly reliable e axial encoder is mostcommonly used as an odometer and is installed in thewalking part of the shearer to measure the driving speed agyroscope is installed on the body of the shearer to measurethe attitude angle of the shearer and dead reckoning (DR) isused to obtain position information [11] is methodavoids the quadratic integration of acceleration when SINS isused to solve for the position and effectively improves thepositioning accuracy but this method could not suppress theaccumulated errors of attitude angle of SINS and the cal-culated position information of shearer would still producelarge errors over time On this basis combined with thespeed and position constraints of a shearer in motionscholars have put forward a variety of calibration methodse authors in [12 13] employ zero velocity correctiontechnology to reduce SINS errors based on the motionconstraints of shearer the lateral and longitudinal velocitiesof SINS output in the carrier coordinate system of theshearer are taken as error values and a linear Kalman filter isestablished based on the measured values so as to improvethe measurement accuracy of the attitude angle In [14] thestroke displacement of the hydraulic support pushing jack isused to predict the position of the scraper conveyor when thenext coal cutting is performed the difference between thepredicted value of the scraper conveyor position and themeasured value of the next coal cutting position is taken asthe observation measurement and experiment shows thatthe positioning error of each cutting cycle can be effectivelyreduced within 1 h e authors [15] combined the abovetwo methods compared with the pure inertial navigationsystem the positioning accuracy of the shearer in the eastand north directions was improved by 54 and 56 re-spectively In the research it is found that if the error of thefirst cutter of the shearer can be limited then the error of thesubsequent several cutters will not diverge erefore the

focus of future research should be on improving the posi-tioning accuracy of the first coal cutter On the whole thepositioning accuracy of the DR method depends heavily onthe attitude angles especially the accuracy of the yaw anglebut the existing algorithms lack the observation of yaw angle

e magnetometer can measure intensity informationand the geomagnetic intensity information can be used tocalculate the magnetic yaw angle without the cumulativeerror when the carrier pitch and roll angles are known Basedon this principle most measurement-while-drilling (MWD)systems in coal mines obtain attitude angles through ac-celerometers and magnetometers [16 17] Considering thecharacteristics of magnetometer a magnetometer is intro-duced into the SINS of shearer and the yaw angle calculatedby the magnetometer is used to calibrate the yaw anglecalculated by SINS so as to improve the positioning ac-curacy of shearer Although the magnetometer itself doesnot produce cumulative errors it needs to be calibratedbefore use and is easily disturbed by changes in the externalmagnetic field Li et al [18] proposed a 12-parameter fittingmethod which can better compensate the soft magnetic fieldinterference of the surrounding environmente study [19]used a support vector regression machine to realize asoftware-based antimagnetic interference method forMWD e simulation results show that the predicted az-imuthal error is less than 1deg which proves the feasibility ofmagnetometer for applications in subterranean coal mineenvironments

e integrated navigation system generally uses Kalmanfilters to complete data fusion and most of which arecentralized Kalman filters (CKFs) at is the state esti-mation and measurement information from all sensors areprocessed in one filter us CKF is not fault tolerant If theoperation of any sensor in the system is affected by inter-ference [20] due to the large number of sensors in themultisensor integrated navigation system the probability ofsensor failure or interference is higher and CKF is notsuitable for such systems e distributed Kalman filter isused to group sensors into subsystems and each subsystemcan estimate the state independently which can effectivelyreduce the amount of calculation [21 22] Federated Kalmanfilter (FKF) is a distributed Kalman filter based on infor-mation distribution principle proposed by Carlson [23]which has the function of fault diagnosis and isolationWhen a navigation subsystem fails FKF can identify thefault subsystem and shield the fault subsystem throughinformation distribution coefficient so as to improve thefault tolerance of the multisensor-integrated navigationsystem [24 25]

In order to improve the positioning accuracy of theshearer for the first coal cutting a magnetometer is added tocalibrate the yaw angle based on the SINSOD-integratednavigation system and the SINSODMAG-integratednavigation system is proposed Compared with otherstudies the main contributions of this paper are as follows(1) the shearer motion constraint and magnetometer areused to assist the SINS of the shearer so as to improve thepositioning accuracy e state equation and observationequation of the SINSODMAG-integrated navigation

2 Mathematical Problems in Engineering

system are established (2) e mathematical model of theadaptive federated Kalman filter (AFKF) is established as theSINSODMAG data fusion algorithm e SINSOD andSINSMAG are regarded as two independent subsystemsAFKF can suppress the influence of the magnetic yaw anglecalculation error on the positioning accuracy of the inte-grated navigation system

e rest of this paper is as follows In Section 2 the SINSerror equation is simplified and the state equation isestablished according to the operation characteristics ofshearer e velocity observation equation and yaw angleobservation equation are established by using the motionconstraint of shearer and the magnetic declination anglecalculated by magnetometer in Section 3 e mathematicalmodel of the AFKF is established in Section 4 In Section 5an experimental platform is established to simulate the coalcutting process of the shearer and the performance of theproposed SINSODMAG-integrated navigation system isverified

2 State Equation of Shearer SINS

e principle of the shearer positioning system is shown inFigure 1 e SINS is installed on the shearer body and anaxial encoder is installed on the walking part of the shearer asan odometer e East-North-Up coordinate system is usedas the navigation coordinate system (n system) of theshearer the carrier coordinate system (b system) takes theshearerrsquos center of gravity as the origin and the shearerrsquosforward direction as the yb axis and coal mining ehorizontal axis of themachine body is the xb axis to the rightand the vertical axis is the zb axis

e equation of state of shearerrsquos SINS is derived fromthe error equation of SINSe attitude error velocity errorand position error equations are respectively

_φ minus ωnin times φ + δωn

in minus Cnbε

bg

δ _vn

fntimes φ minus 2ωn

ie + ωnen( 1113857 times δvn

+ vntimes 2δωn

ie + δωnen( 1113857

+ Cnbε

ba

δ _P δζvn+ ζδvn

(1)

where δ is the error of each physical quantityφ ϕE ϕN ϕU1113858 1113859

T is the misalignment angle vector vn

vnE vn

N vnU1113858 1113859

T is the shearer velocity vector in the navigationcoordinate system P L λ h1113858 1113859

T is the latitude longitudeand altitude information of the shearer εb

g is the gyroscopeerror vector in the carrier coordinate system and εb

a is theaccelerometer error vector in the carrier coordinate systemCn

b is the transformation matrix from the carrier coordinatesystem to the navigation coordinate system ωn

ie is the an-gular velocity of the Earth ωn

en is the projection of theangular velocity of the navigation coordinate system relativeto the Earth coordinate system in the navigation coordinatesystem fn fE fN fU1113858 1113859

T is the acceleration valuemeasured by the accelerometer in the carrier coordinatesystem ζ is the conversion matrix between the carrier linearvelocity and the latitude and longitude angular velocity

Since the shearerrsquos moving speed is slow its position willnot change significantly in a short time so the speed errorand position error in the error equation are ignored and thesimplified SINS equation of the shearer is expanded asfollows

_ϕE ωie sin L +v

nE tan L

RN + h1113888 1113889ϕN minus ωie cos L +

vnE

RN + h1113888 1113889ϕU minus εn

gE

_ϕN minus ωie sin L +v

nE tan L

RN + h1113888 1113889ϕE minus

vnN

RM + hϕU minus εn

gN

_ϕU ωie cos L +v

nE

RN + h1113888 1113889ϕE +

vnN

RM + hϕN minus εn

gU

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

(2)

δ _vnE minus fUϕN + fNϕU + εn

aE

δ _vnN fUϕE minus fEϕU + εn

aN

δ _vnU minus fNϕE + fEϕN + εn

aU

⎧⎪⎪⎨

⎪⎪⎩(3)

δ _L 1

RM + hδv

nN

δ _λ sec L

RN + hδv

nE

δ _h δvnU

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(4)

Mathematical Problems in Engineering 3

where RM is the curvature radius of the local meridian andRN is the curvature radius of the local unitary circle

e attitude angle error velocity error position errorgyroscope error and accelerometer error of the shearerrsquosstrapdown inertial navigation system are taken as statevariables e state vector is

X φT δvnT δPT εbTg εbT

a1113960 1113961T (5)

e equation of state of SINS is_X FSINSX + w (6)

where FSINS is the state transition matrix which can beobtained by equations (2)ndash(4) w is the system noise vectorwhich conforms to Gaussian white noise

3 Establishment of Observation Equation

31 Velocity Observation Equation e shearer travels onthe tracks of the scraper conveyor and thus the shearer onlyhas speed in the longitudinal direction due to the con-strained movement erefore the speed in the xb-axis andzb-axis directions is zero and the output of the axial encoderis the speed vb

y in theyb-axis direction of the shearer and thespeed vector of the shearer measured by the axial encoderOD in the carrier coordinate system is

vbod 0 vby 01113876 1113877 (7)

Considering the misalignment angle error and scaleerror of the axial encoder the speed of the shearer measuredby the axial encoder in the navigation coordinate system is

1113954vnod I3times3 minus (φtimes)1113858 1113859Cnb(1 + δk)vb

od (8)

where (φtimes) is the antisymmetric matrix of the misalignmentangle vectors and δk is the error factor for the axial encoderAfter expanding equation (13) we obtain

1113954vnod vn

od + δkvnod + vn

od times φ minus δkvnod times φ (9)

Ignoring the small second-order term δkvnod times φ in

formula (14) the error formula for axial encoder speed

measurement in the navigation coordinate system isobtained

δvod 1113954vnod minus vn

od δkvnod + vn

od times φ (10)

e closing error Zv between the velocity calculated bySINS and the velocity measured by the axial encoder is takenas an observation measurement by the filter After the realvalue is offset the observed measurement is the differencebetween this value and the error value

Zv 1113954vnSINS minus 1113954vn

od δvnminus vn

od times φ minus δkvnod (11)

e state quantity of the system is defined as

X1k φT δvnT δPT εbTg εbT

a δk1113960 1113961T (12)

us the system state equation is thus

_X1k F1kX1k + G1kw1k (13)

wherew1k is the system noise matrixG1k is the system noisedriving matrix and F1k is the state transition matrix of theSINSOD subsystem which can be expressed as

F1k FSINS 0

0 01113890 1113891

16times16 (14)

e observation quantity of the system is as follows

Z1k ZTv (15)

e measurement equation is as follows

Z1k H1kX1k + v1k (16)

where v1k is the observation noise vector of the axial en-coder H1k is the observation matrix and its value is

H1k minus vno dtimes( 1113857 I3 03times6 minus vn

od1113858 1113859 (17)

32 Yaw Angle Observation Equation With the advance ofpitch angle θ and roll angle c the magnetic yaw angle can becalculated from the magnetometer measured value eformula used for calculation is

zbxb

yb

Strapdown inertialnavigation

Axial encoder

Figure 1 Principle of the shearer positioning system


ψMAG arctanH

bz sin c cos θ + H

by sin c sin θ + H

bx cos c

Hby cos c minus H

bz sin c

(18)

where Hbx Hb

y and Hbz are the measured values of the three-

axis magnetometer in the carrier coordinate systemere isan error between the magnetic yaw angle and the carrier yawangle e carrier yaw angle is calculated as

ψ ψmag + δψmag (19)

where δψmag is the magnetic declination which is related tothe latitude and longitude of the initial carrier positionAlthough there is an estimated error of magnetic declina-tion the longitude and latitude of the shearer will not changesignificantly due to the slow speed in the operation processso the estimated error of magnetic declination can beignored



a1113960 1113961T (20)

us the system error state equation is_X2k F2kX2k + G2kw2k (21)

wherew2k is the system noise vectorG2k is the system noisedriving matrix and F2k is the system state matrix of theSINSMAG subsystem and it is

F2k FSINS (22)

e closure error between the yaw angle obtained by themagnetometer and the yaw angle calculated by SINS is takenas another observation measurement which cancels out thereal value in the process of subtraction erefore theclosure error Zψ is actually the closure error between the yawangle error of SINS and the measurement error of themagnetometer Since the estimation error of the magneticdeclination angle of the magnetometer is ignored the clo-sure error is

Zψ ψSINS minus ψmag ϕU (23)

e observation vector is defined as

Z2k Zψ (24)


Z2k H2kX2 + v2k (25)

where v2k is the corresponding observation noise vector andH2k is the observation matrix and the value of which is

H2k 01times2 1 01times121113858 1113859 (26)

4 Design of Adaptive Federated Kalman Filter

41 Principle of Adaptive Federated Kalman Filter eworkflow of the proposed SINSODMAG-integrated nav-igation system is shown in Figure 2 In the proposed

integrated navigation system SINS measures the accelera-tion and angular velocity information of the shearer at time kunder the b coordinate system and updates the attitudeangle velocity and position Using SINSOD as subsystem 1the y-component of the shearer speed is measured by theaxial encoder thereby obtaining the speed vector of theshearer in the b coordinate system and converting it to the ncoordinate system e closing difference between the speedcomponent 1113954vn

od of the shearer in the n coordinate systemmeasured by the shaft encoder and the speed component1113954vnSINS of the shearer in the n coordinate system calculated bySINS is used as the observation vector Z1 Using SINSMAGas subsystem 2 the yaw angle of the shearer is calculated bythe magnetometer according to the roll and pitch anglesmeasured by SINS and the closing error of the yaw angleψSINS calculated by SINS and the yaw angle ψmag measuredby the magnetometer is taken as the observation vector Z2e optimal error estimation value is obtained by inputtingthe error state and observation into the federated Kalmanfilter e optimal error estimation value is returned to theSINS to correct the position information of the SINS outputto improve the shearer positioning accuracy

e adaptive federated Kalman filter presented in thispaper is shown in Figure 3 e FKF is a two-stage filter inwhich SINS is the common reference system and SINSODand SINSMAG are local filters 1 and 2 respectively e Xk

of the SINS output is both used as the measurement value ofthe subfilter and applied to the main filter Each local filtercan calculate the local optimal estimation value 1113954Xi and thecovariance matrix Pi ese values are input into the mainfilter and fused to obtain the global optimal estimation value1113954Xg and the global covariance matrix Pg βi (i 1 2) is theinformation distribution coefficient which is determinedaccording to the covariance matrix and is key to the in-formation fusion process

42 Local Optimal Estimation of Subfilters e stateequations of the two subfilters of SINSOD and SINSMAG are

Xik Φi(kkminus 1)Xikminus 1 + Γikminus 1Wikminus 1

Zik HikXik + Vik1113896 (i 1 2) (27)

When i 1 the equation of state represents the SINSOD subsystem and the values of X1k Φ1(kkminus 1) Γ1kminus 1 W1kZ1k H1k and V1k can be obtained from Section 31 wheni 2 the equation of state represents the SINSMAG sub-system and the values of X2k Φ2(kkminus 1) Γ2kminus 1 W2k Z2kH2k and V2k can be obtained from Section 32

e local filter updates are divided into time andmeasurement updates e time update process of eachsubfilter is

1113954Xikkminus 1 Φi(kkminus 1)1113954Xikminus 1 (i 1 2)

Pikkminus 1 Φi(kkminus 1)Pikminus 1ΦTi(kkminus 1) + Γikminus 1Qikminus 1Γ

Tikminus 1 (i 1 2)

(28)

e measurement update process of the subfilter is


1113954Xik 1113954Xi(k(kminus 1)) + Kikεik (i 1 2) (29)

where εik is the innovation and K is the filter gain matrixe formulas used for calculation are

εik Zik minus Hik1113954Xi(k(kminus 1)) (i 1 2)

Kik Pi(k(kminus 1))HTik HikPi(k(kminus 1))H

Tik + Rik1113872 1113873

minus 1 (i 1 2)

(30)

e formula for calculating the state error covariancematrix is

Pk I minus KikHik1113872 1113873Pi(k(kminus 1)) I minus KikHik1113872 1113873T

+ KikRikKTik (i 1 2)

(31)

During the update of the Kalman filter the system noisevariance Qik and the observation noise variance Rik de-termine the degree of utilization of the observations for the

optimal estimate e system noise variance depends on therandom error of the sensor but the setting of the observationnoise variance is complex and related to the operating stateof the shearer In this paper we assume thatQik is constantand we use the innovation εik to estimate Rik

Rlowastik 1113936

kkminus N+1 eike

Tik

N (32)

where Rlowastik is the estimated observation noise covariance toreplace Rik and N is the number of samples

43 Adaptive Information Allocation and Global Optimiza-tion Estimation e information distribution coefficient βi

is related to the eigenvalue of the estimation error covariancematrix Pi e eigenvalue of Pi is the variance of the cor-responding state quantity or the linear combination of thecorresponding state quantities e large eigenvalue of Pi

proves that the variance of the state quantity estimation islarge indicating that the effectiveness of the filter is poor

SINS

Odometer

FederatedKalman

filter

Magnetometer

Yaw angle

Yaw angle

Velocity

Velocity

+ndash

ndash+

Velocityerror

Yaw angleerror

Estimated error

Error statequantity

Corrected position

Figure 2 Workflow of the SINSODMAG-integrated navigation system

SINS

Odometer

MAG

SINSOD

SINSMAG

Primary filter

Time update

Optimal fusion

Adaptive channel allocation

Z1 P1

P2

Z2X2 P2

X1 P1

Xg βndash 2

1Pg

Xg Pg

Xg βndash 1

1Pg

β1 β2

Figure 3 Structure of adaptive federated Kalman filter


and the accuracy of the subsystem is low so βi should as-sume a larger value On the contrary a large eigenvalue of Pi

indicates that the effectives of the filter is improved and thesubsystem accuracy is high so βi should assume a smallvalue e distribution of adaptive information is an im-portant feature resulting in high fault tolerance of theadaptive federated Kalman filter e calculation formula ofβi is

βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)

where tr represents the trace of the corresponding matrixthat is the sum of all the eigenvalues of the matrix

It can be seen from Figure 3 that 1113954Xg and Pg providefeedback 1113954Xg is used to reset the local estimation of thesubfilter and Pg is used to reset feedback Pi after amplifi-cation which is expressed by the formula

1113954Xi 1113954Xg (i 1 2)

Pi βminus 1i Pg (i 1 2)

(34)

emain filter of the AFKF presented in this paper doesnot filter so the estimated value of the main filter is theglobal optimal estimate and the calculation formula of theglobal covariance matrix and the global optimal estimate arerespectively

Pg Pminus 11 + Pminus 1

21113872 1113873minus 1

1113954Xg Pg Pminus 11

1113954X1 + Pminus 12

1113954X21113872 1113873(35)

5 Experimental Verification

51 Description of Experimental Equipment In order to testthe positioning accuracy of the integrated shearer navigationsystem a ground test platform was built employing a vehicleas a carrier to simulate the operation state of the shearerunderground e experimental equipment is shown inFigure 4 and the experimental equipment includes an in-ertial sensor (XsensMTi-30) a wheel speed sensor CAN bustest equipment and a GPS sensor e MTi-30 sensor is amicroinertial heading and attitude measurement sensorwhich integrates a three-axis gyroscope three-axis accel-erometer and three-axis magnetometer e MTi-30 sensorcan output raw data from each sensor and can also calculatethe carrier attitude angle through the internal algorithmbased on the data of the three internal sensors which is usedas the reference value e performance specifications of theMTi-30 sensor are shown in Table 1 e principle of thewheel speed sensor of the car is the same as that of the shaftencoder of the shearer which is used to measure the runningspeed of the carrier e CAN bus test equipment includesCANNoe CANcope and CANStressDR which are used toobtain vehicle wheel speed sensor data GPS sensors are usedto measure the actual running track of the carrier and serveas a reference for judging the positioning accuracy of theintegrated navigation system

Axial encoders are installed on the front wheels as theodometer carrier and the running speed of the carrier istaken as the average output value of the left and right axialencoders In order to reduce interference of the internalcomponents of the vehicle on the magnetometer theMTi-30sensor and GPS sensor are installed on the middle of theoutside of the roof e two-point method [26] was used toeliminate the installation error angle of the sensor andcompensate for acceleration error caused by the lever armeffect [27]

52 Performance Test of Integrated Navigation SystemAccording to the coal cutting process of the shearer thevehicle is driven at low speed to simulates the shearer to do astraight-line coal cutting movement and then executes in-clined sumping and the running track will be in the shape ofa broken line Four different navigation systems were used tocalculate the carrier movement path and the results werecompared and analyzed e path solution results are shownin Figure 5 e blue line is the carrier movement pathmeasured by GPS and takes it as reference value e greencurve is the carrier movement path solved by SINS and dueto the low accuracy of the MIMU sensor used in this projectthe path solved by this method has a large deviation eblack curve is the path solved by the SINSOD-integratednavigation system because the integrated navigation systemadopts the shearer motion constraint assistance and thecalculated path does not appear obvious divergence butthere are still obvious errors compared with the pathmeasured by GPS e red curve is the path solved by theSINSODMAG-integrated navigation system Comparedwith the SINSOD-integrated navigation system the path

Figure 4 Schematic diagram of experimental platform andequipment

Table 1 Performance index of MTi-30

Bias instability Noise density NonlinearityGyroscope 18degh 01degsradicHz 003FSAccelerometer 40 μg 80 μgradicHz 003FSMagnetometer 05mG 200 μGradicHz 01FS


ndash60 ndash50 ndash40 ndash30 ndash20 ndash10 0

ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)

Figure 5 Comparison of SINS SINSOD and SINSODMAG solution paths

SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)

Figure 6 e attitude angle of the carrier (a) Pitch angle of the carrier (b) roll angle of the carrier (c) yaw angle of the carrier


SINS errorSINODMAG errorSINSOD error

0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)

Figure 7 e position error of the carrier (a) East position error (b) North position error

Table 2 Positioning error of different navigation modes

Navigation method SINS SINSOD Error-reduction rate () SINSODMAG Error-reduction rate ()East error variation (m) 39664 09012 7728 02195 9447North error variation (m) 62906 12491 8014 03246 9494

ndash90 ndash80 ndash60ndash70 ndash50 ndash40 ndash30 ndash20 ndash10 0

ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD

Centralized Kalman filterFederated Kalman filter

Nor

th (m

)

Figure 8 Result of path calculation under interference of magnetometer


solved by the SINSODMAG-integrated navigation systemis more consistent with the path measured by GPS whichproves that using magnetometer to calibrate yaw angle caneffectively improve the positioning accuracy of shearer

Figure 6 shows the attitude angle of the carrier in theprocess of movinge attitude angle calculated by SINS willdiverge rapidly with time resulting a large error e pitchangle and roll angle calculated by SINSOD- and SINSODMAG-integrated navigation systems are basically the sameand there is no obvious divergencee yaw angle calculatedby the SINSOD-integrated navigation system divergesobviously in the early stage of linear movement which doesnot conform to the linear movement of the carrier and thedivergence of yaw angle is restrained in the stage of inclined

sumping e yaw angle calculated by the SINSODMAG-integrated navigation system fluctuates in the range of1238255deg to 1250927deg in the initial stage of linear motionbut there is no divergence and there is no divergence orfluctuation in the subsequent time which is more in linewith the actual trajectory of the carrier

Figure 7 shows the time varying positioning error of theintegrated navigation system Since the path error of SINS islarge this paper focuses on the comparison of the posi-tioning error changes of the SINSODMAG- and SINSOD-integrated navigation systems in the east and north direc-tions e positioning errors of different navigation modesare shown in Table 2 in the east direction the maximumerror of the SINSOD-integrated navigation system is

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)

Figure 9 Attitude angle of carrier undermagnetic interference (a) Pitch angle of the carrier (b) roll angle of the carrier and (c) yaw angle ofthe carrier


09012m which is 7728 lower than that of SINS themaximum error of the SINSODMAG-integrated naviga-tion system is 02195m which is 9447 lower than that ofSINS After adding magnetometer calibration on the basis ofthe SINSOD-integrated navigation system the positioningaccuracy of the east direction is improved by 7564 In thenorth direction the maximum error of SINSOD is12491m which is 8014 lower than that of SINS themaximum error of SINSODMAG in the north direction is03246m which is 9484 lower than that of SINS thus thenorth positioning accuracy of the SINSODMAG-inte-grated navigation system is 7401 higher than that of theSINSOD-integrated navigation system By analyzing thepositioning error change curve of Figure 7 it can be seen thatcompared with the SINSOD-integrated navigation systemerror the SINSODMAG-integrated navigation systemerror has been effectively suppressed and the divergencedegree has been significantly reduced

In order to verify that the adaptive FKF proposed in thispaper has better fault tolerance than the centralized Kalmanfilter the path of magnetometer subjected to magnetic in-terference is analyzed Figure 8 shows the paths solved byFKF and CKF respectively e solution path of CKF showsa large deviation due to the magnetic interference and themaximum errors in the east and north directions are38268m and 19420m respectively By comparison theFKF proposed in this paper has a small path deviation andthe maximum errors in the east and north directions arereduced to 15452m and 19420m respectively It can beseen from Figure 8 that the path solved by the FKF is similarto that calculated by the CKF in the early stage and the erroris gradually reduced in the later stages At the end of theexperiment the error in the east and north directions de-creased to 09866m and 12015m respectively

Figure 9 shows the attitude angle of the carrier undermagnetic interference As shown in the figure there is noobvious difference between CKF and FKF in the calculationof pitch angle and roll angle It can be seen from the curve ofyaw angle that the magnetometer is seriously disturbed inthe range of 10sim30 s and there is a serious deviation in theyaw angle calculation However when the magnetometer isdisturbed the yaw angle error calculated by the FKF issmaller than that by CKF e experimental results showthat the FKF has better fault tolerance and can effectivelyreduce the impact of a single system fault on the wholesystem when a sensor in the SINSODMAG-integratednavigation system fails or is susceptible to interference

6 Conclusion

In order to solve the problem of low positioning accuracy ofthe shearer navigation system using low-cost IMU thispaper presents an integrated navigation system based onsharer motion constraint and magnetometer assisted SINSe main contributions of this paper are as follows

(1) On the basis of the SINSOD-integrated navigationsystem the magnetometer was added to correct theyaw angle and the SINSODMAG-integrated

navigation system was established e experimentalresults show that compared with the SINSOD-in-tegrated navigation system the positioning accuracyof the SINSODMAG-integrated navigation systemis improved by 7564 and 7401 in the east andnorth directions respectively which can effectivelyimprove the positioning accuracy of shearer

(2) e federated Kalman filter is designed as a datafusion algorithm to improve the robustness andreliability of the integrated navigation systemCombined with the analysis of experimental resultsFKF can effectively reduce the impact of faults on theintegrated navigation system when the magnetom-eter experiences interference

Although the federated Kalman filter can effectivelyimprove the antimagnetic interference ability of the SINSODMAG-integrated navigation system the problem thatthe magnetometer is vulnerable to interference has not beenfundamentally solved We plan to implement software an-timagnetic research in the form of neural networks to themagnetometer to further improve the positioning accuracyof the SINSODMAG-integrated navigation system

Data Availability

e data used to support the findings of this study areavailable from the corresponding author upon request

Conflicts of Interest

e authors declare that there are no conflicts of interestregarding the publication of this paper

References

[1] G Wu X Fang L Zhang M Liang J Lv and Z QuanldquoPositioning accuracy of the shearer based on a strapdowninertial navigation system in underground coal miningrdquoApplied Sciences vol 10 no 6 p 2176 2020

[2] J C Ralston C O Hargrave and M T Dunn ldquoLongwallautomation trends challenges and opportunitiesrdquo Interna-tional Journal of Mining Science and Technology vol 27 no 5pp 733ndash739 2017

[3] H Yang W Li C Luo J Zhang and Z Si ldquoResearch on errorcompensation property of strapdown inertial navigationsystem using dynamic model of shearerrdquo IEEE Access vol 4pp 2045ndash2055 2016

[4] C Zhang X Zhao C Pang T Li and L Zhang ldquoAdaptivefault isolation and system reconfiguration method for GNSSINS integrationrdquo IEEE Access vol 8 pp 17121ndash17133 2020

[5] Y Hai L Wei L Chengming et al ldquoExperimental study onposition and attitude technique for shearer using SINSmeasurementrdquo Journal of China Coal Society vol 39pp 2550ndash2556 2014

[6] H Liu Z Wang S Fang and C Li ldquoMEMS based SINSODfilter for land vehiclesrsquo applicationsrdquo Mathematical Problemsin Engineering vol 2017 pp 1ndash9 2017

[7] B R Peter Real World Automation New Capabilities forunder Ground Longwall Mining CSIRO Mining TechnologyCanberra Australia 2013


[8] L Chengming L Wei F Qigao F Mengbao and Y HaildquoPositioning accuracy of shearer in mobile wireless sensornetworksrdquo Journal of Central South University vol 45 no 2pp 428ndash434 2014

[9] Q Fan W Li J Hui et al ldquoIntegrated positioning for coalmining machinery in enclosed underground mine based onSINSWSNrdquo Scientific World Journal vol 2014 Article ID460415 12 pages 2014

[10] M Dunn D Reid and J Ralston Control of Automated MiningMachinery using Aided Inertial Navigation inMachine Vision andMechatronics in Practice Spriner Berlin Germany 2015

[11] L I Sen ldquoMeasurement amp control and localisation for fully-mechanized working face alignment based on inertial navi-gationrdquo Coal Science and Technology vol 47 pp 169ndash1742019

[12] W Shijia W Shibo Z Boyuan et al ldquoDynamic zero-velocityupdate technology to shearer inertial navigation positioningrdquoJournal of China Coal Society vol 43 pp 578ndash583 2018

[13] Y Hai ldquoResearch on the strategy of motion constraint-aidedZUPT for the SINS positioning system of a shearerrdquoMicromachines vol 8 no 11 p 340 2017

[14] W Shibo Z Boyuan W Shijia and G Shirong ldquoDynamicprecise positioning method of shearer based on closing pathoptimal estimation modelrdquo IEEE Transactions on AutomationScience and Engineering vol 16 pp 1468ndash1475 2018

[15] S Wang and S Wang ldquoImproving the shearer positioningaccuracy using the shearer motion constraints in longwallpanelsrdquo IEEE Access vol 8 pp 52466ndash52474 2020

[16] X Qilong W Ruihe S Feng H Leilei and H LaijuldquoContinuous measurement-while-drilling utilizing strap-down multi-model surveying systemrdquo IEEE Transactions onInstrumentation and Measurement vol 63 no 3 pp 650ndash657 2014

[17] A S Jurkov J Cloutier E Pecht and M P MintchevldquoExperimental feasibility of the in-drilling alignment methodfor inertial navigation in measurement-while-drillingrdquo IEEETransactions on Instrumentation and Measurement vol 60no 3 pp 1080ndash1090 2011

[18] C Li Z Wang and H Liu ldquoNovel method to integrateMARG and an odometer into AHRS for moving vehiclesrdquoAdvances in Mechanical Engineering vol 9 no 9 Article ID168781401772797 2017

[19] L Zongwei and S Jiancheng ldquoA low-cost calibration strategyfor measurement-while-drilling systemrdquo IEEE Transactionson Industrial Electronics vol 65 no 4 pp 3559ndash3567 2018

[20] B Anderson J B Moore and M Eslami ldquoOptimal filteringrdquoIEEE Transactions on Systems Man amp Cybernetics vol 122pp 235-236 2007

[21] A Mustafa and H Modares ldquoAnalysis and detection of cyber-physical attacks in distributed sensor networksrdquo in Pro-ceedings of the 2018 56th Annual Allerton Conference onCommunication Control and Computing (Allerton) IEEEMonticello IL USA October 2018

[22] A Mustafa H Modares and M Hamidreza ldquoSecure event-triggered distributed kalman filters for state estimationrdquo 2020httpsarxivorgabs190106746

[23] N A Carlson ldquoFederated filter for computer-efficient near-optimal GPS integrationrdquo in Proceedings of the PositionLocation And Navigation Symposium-Plansrsquo96 pp 22ndash25Atlanta GA USA April 1996

[24] Y Yang X Liu W Zhang X Liu and Y Guo ldquoA nonlineardouble model for multisensor-integrated navigation using thefederated EKF algorithm for small UAVsrdquo Sensors vol 20no 10 p 2974 2020

[25] K Shen M Wang M Fu Y Yang and Z Yin ldquoObservabilityanalysis and adaptive information fusion for integratednavigation of unmanned ground vehiclesrdquo IEEE Transactionson Industrial Electronics vol 67 no 9 pp 7659ndash7668 2020

[26] B Zhang W Shibo and G Shirong ldquoEffects of initialalignment error and installation noncoincidence on theshearer positioning accuracy and calibration methodrdquo Jour-nal of China Coal Society vol 42 pp 789ndash795 2017

[27] S Zhuoyin L Wei and T Junling ldquoResearch on lever armeffect error compensation method for strap-down inertialnavigation system of shearerrdquo Industry and Mine Automa-tion vol 45 no 5 pp 56ndash61 2019


erefore it is necessary to rely on other sensor informationto calibrate the SINS Using MIMU with low precision andhigh reliability to realize the accurate positioning of sheareris the research focus at this stage

In order to reduce the influence of the cumulative errorof SINS on measurement accuracy a variety of SINS-basedintegrated navigation systems have appeared Chengminget al [8] proposed using a wireless sensor network (WSN) todetermine the position of the shearer On this basis Fan et al[9] proposed an integrated SINSWSN navigation systemmobile nodes were installed on the shearer and anchor nodeswere installed on the top beam of the hydraulic support andthe duration and angle of wireless signal transmission be-tween mobile node and anchor node is used to realize real-time positioning of the shearer However the frequentmovement of the hydraulic support makes it difficult toobtain accurate reference coordinate values which seriouslyaffects the positioning accuracy Dunn et al [10] and othersproposed the use of Doppler radar to measure the relativeground speed of the shearer so as to correct the velocityerror of SINS However Doppler radar is susceptible to thesurrounding environment when it is used undergroundthus it is difficult to obtain an accurate speede integratednavigation system employing SINS and an odometer (OD) isa widely used and highly reliable e axial encoder is mostcommonly used as an odometer and is installed in thewalking part of the shearer to measure the driving speed agyroscope is installed on the body of the shearer to measurethe attitude angle of the shearer and dead reckoning (DR) isused to obtain position information [11] is methodavoids the quadratic integration of acceleration when SINS isused to solve for the position and effectively improves thepositioning accuracy but this method could not suppress theaccumulated errors of attitude angle of SINS and the cal-culated position information of shearer would still producelarge errors over time On this basis combined with thespeed and position constraints of a shearer in motionscholars have put forward a variety of calibration methodse authors in [12 13] employ zero velocity correctiontechnology to reduce SINS errors based on the motionconstraints of shearer the lateral and longitudinal velocitiesof SINS output in the carrier coordinate system of theshearer are taken as error values and a linear Kalman filter isestablished based on the measured values so as to improvethe measurement accuracy of the attitude angle In [14] thestroke displacement of the hydraulic support pushing jack isused to predict the position of the scraper conveyor when thenext coal cutting is performed the difference between thepredicted value of the scraper conveyor position and themeasured value of the next coal cutting position is taken asthe observation measurement and experiment shows thatthe positioning error of each cutting cycle can be effectivelyreduced within 1 h e authors [15] combined the abovetwo methods compared with the pure inertial navigationsystem the positioning accuracy of the shearer in the eastand north directions was improved by 54 and 56 re-spectively In the research it is found that if the error of thefirst cutter of the shearer can be limited then the error of thesubsequent several cutters will not diverge erefore the

focus of future research should be on improving the posi-tioning accuracy of the first coal cutter On the whole thepositioning accuracy of the DR method depends heavily onthe attitude angles especially the accuracy of the yaw anglebut the existing algorithms lack the observation of yaw angle

e magnetometer can measure intensity informationand the geomagnetic intensity information can be used tocalculate the magnetic yaw angle without the cumulativeerror when the carrier pitch and roll angles are known Basedon this principle most measurement-while-drilling (MWD)systems in coal mines obtain attitude angles through ac-celerometers and magnetometers [16 17] Considering thecharacteristics of magnetometer a magnetometer is intro-duced into the SINS of shearer and the yaw angle calculatedby the magnetometer is used to calibrate the yaw anglecalculated by SINS so as to improve the positioning ac-curacy of shearer Although the magnetometer itself doesnot produce cumulative errors it needs to be calibratedbefore use and is easily disturbed by changes in the externalmagnetic field Li et al [18] proposed a 12-parameter fittingmethod which can better compensate the soft magnetic fieldinterference of the surrounding environmente study [19]used a support vector regression machine to realize asoftware-based antimagnetic interference method forMWD e simulation results show that the predicted az-imuthal error is less than 1deg which proves the feasibility ofmagnetometer for applications in subterranean coal mineenvironments

e integrated navigation system generally uses Kalmanfilters to complete data fusion and most of which arecentralized Kalman filters (CKFs) at is the state esti-mation and measurement information from all sensors areprocessed in one filter us CKF is not fault tolerant If theoperation of any sensor in the system is affected by inter-ference [20] due to the large number of sensors in themultisensor integrated navigation system the probability ofsensor failure or interference is higher and CKF is notsuitable for such systems e distributed Kalman filter isused to group sensors into subsystems and each subsystemcan estimate the state independently which can effectivelyreduce the amount of calculation [21 22] Federated Kalmanfilter (FKF) is a distributed Kalman filter based on infor-mation distribution principle proposed by Carlson [23]which has the function of fault diagnosis and isolationWhen a navigation subsystem fails FKF can identify thefault subsystem and shield the fault subsystem throughinformation distribution coefficient so as to improve thefault tolerance of the multisensor-integrated navigationsystem [24 25]

In order to improve the positioning accuracy of theshearer for the first coal cutting a magnetometer is added tocalibrate the yaw angle based on the SINSOD-integratednavigation system and the SINSODMAG-integratednavigation system is proposed Compared with otherstudies the main contributions of this paper are as follows(1) the shearer motion constraint and magnetometer areused to assist the SINS of the shearer so as to improve thepositioning accuracy e state equation and observationequation of the SINSODMAG-integrated navigation








in minus Cnbε

bg

δ _vn



+ vntimes 2δωn

ie + δωnen( 1113857

+ Cnbε

ba


(1)



vnE vn

N vnU1113858 1113859










_ϕE ωie sin L +v

nE tan L


vnE

RN + h1113888 1113889ϕU minus εn

gE


nE tan L

RN + h1113888 1113889ϕE minus

vnN

RM + hϕU minus εn

gN

_ϕU ωie cos L +v

nE

RN + h1113888 1113889ϕE +

vnN

RM + hϕN minus εn

gU

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

(2)


aE


aN


aU

⎧⎪⎪⎨

⎪⎪⎩(3)

δ _L 1

RM + hδv

nN

δ _λ sec L

RN + hδv

nE

δ _h δvnU

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(4)





a1113960 1113961T (5)






vbod 0 vby 01113876 1113877 (7)



od (8)


1113954vnod vn

od + δkvnod + vn






od δkvnod + vn

od times φ (10)



od δvnminus vn




a δk1113960 1113961T (12)




F1k FSINS 0

0 01113890 1113891

16times16 (14)


Z1k ZTv (15)


Z1k H1kX1k + v1k (16)



od1113858 1113859 (17)


zbxb

yb


Axial encoder



ψMAG arctanH

bz sin c cos θ + H

by sin c sin θ + H

bx cos c

Hby cos c minus H

bz sin c

(18)

where Hbx Hb







a1113960 1113961T (20)



F2k FSINS (22)




Z2k Zψ (24)


Z2k H2kX2 + v2k (25)


H2k 01times2 1 01times121113858 1113859 (26)









Zik HikXik + Vik1113896 (i 1 2) (27)





Tikminus 1 (i 1 2)

(28)







Tik + Rik1113872 1113873

minus 1 (i 1 2)

(30)




(31)



Rlowastik 1113936

kkminus N+1 eike

Tik

N (32)





SINS

Odometer

FederatedKalman

filter

Magnetometer

Yaw angle

Yaw angle

Velocity

Velocity

+ndash

ndash+

Velocityerror

Yaw angleerror

Estimated error

Error statequantity

Corrected position


SINS

Odometer

MAG

SINSOD

SINSMAG

Primary filter

Time update

Optimal fusion


Z1 P1

P2

Z2X2 P2

X1 P1

Xg βndash 2

1Pg

Xg Pg

Xg βndash 1

1Pg

β1 β2





βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)



1113954Xi 1113954Xg (i 1 2)


(34)



21113872 1113873minus 1


1113954X1 + Pminus 12

1113954X21113872 1113873(35)










ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References




































in minus Cnbε

bg

δ _vn



+ vntimes 2δωn

ie + δωnen( 1113857

+ Cnbε

ba


(1)



vnE vn

N vnU1113858 1113859










_ϕE ωie sin L +v

nE tan L


vnE

RN + h1113888 1113889ϕU minus εn

gE


nE tan L

RN + h1113888 1113889ϕE minus

vnN

RM + hϕU minus εn

gN

_ϕU ωie cos L +v

nE

RN + h1113888 1113889ϕE +

vnN

RM + hϕN minus εn

gU

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎩

(2)


aE


aN


aU

⎧⎪⎪⎨

⎪⎪⎩(3)

δ _L 1

RM + hδv

nN

δ _λ sec L

RN + hδv

nE

δ _h δvnU

⎧⎪⎪⎪⎪⎪⎪⎪⎪⎨

⎪⎪⎪⎪⎪⎪⎪⎪⎩

(4)





a1113960 1113961T (5)






vbod 0 vby 01113876 1113877 (7)



od (8)


1113954vnod vn

od + δkvnod + vn






od δkvnod + vn

od times φ (10)



od δvnminus vn




a δk1113960 1113961T (12)




F1k FSINS 0

0 01113890 1113891

16times16 (14)


Z1k ZTv (15)


Z1k H1kX1k + v1k (16)



od1113858 1113859 (17)


zbxb

yb


Axial encoder



ψMAG arctanH

bz sin c cos θ + H

by sin c sin θ + H

bx cos c

Hby cos c minus H

bz sin c

(18)

where Hbx Hb







a1113960 1113961T (20)



F2k FSINS (22)




Z2k Zψ (24)


Z2k H2kX2 + v2k (25)


H2k 01times2 1 01times121113858 1113859 (26)









Zik HikXik + Vik1113896 (i 1 2) (27)





Tikminus 1 (i 1 2)

(28)







Tik + Rik1113872 1113873

minus 1 (i 1 2)

(30)




(31)



Rlowastik 1113936

kkminus N+1 eike

Tik

N (32)





SINS

Odometer

FederatedKalman

filter

Magnetometer

Yaw angle

Yaw angle

Velocity

Velocity

+ndash

ndash+

Velocityerror

Yaw angleerror

Estimated error

Error statequantity

Corrected position


SINS

Odometer

MAG

SINSOD

SINSMAG

Primary filter

Time update

Optimal fusion


Z1 P1

P2

Z2X2 P2

X1 P1

Xg βndash 2

1Pg

Xg Pg

Xg βndash 1

1Pg

β1 β2





βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)



1113954Xi 1113954Xg (i 1 2)


(34)



21113872 1113873minus 1


1113954X1 + Pminus 12

1113954X21113872 1113873(35)










ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References

































a1113960 1113961T (5)






vbod 0 vby 01113876 1113877 (7)



od (8)


1113954vnod vn

od + δkvnod + vn






od δkvnod + vn

od times φ (10)



od δvnminus vn




a δk1113960 1113961T (12)




F1k FSINS 0

0 01113890 1113891

16times16 (14)


Z1k ZTv (15)


Z1k H1kX1k + v1k (16)



od1113858 1113859 (17)


zbxb

yb


Axial encoder



ψMAG arctanH

bz sin c cos θ + H

by sin c sin θ + H

bx cos c

Hby cos c minus H

bz sin c

(18)

where Hbx Hb







a1113960 1113961T (20)



F2k FSINS (22)




Z2k Zψ (24)


Z2k H2kX2 + v2k (25)


H2k 01times2 1 01times121113858 1113859 (26)









Zik HikXik + Vik1113896 (i 1 2) (27)





Tikminus 1 (i 1 2)

(28)







Tik + Rik1113872 1113873

minus 1 (i 1 2)

(30)




(31)



Rlowastik 1113936

kkminus N+1 eike

Tik

N (32)





SINS

Odometer

FederatedKalman

filter

Magnetometer

Yaw angle

Yaw angle

Velocity

Velocity

+ndash

ndash+

Velocityerror

Yaw angleerror

Estimated error

Error statequantity

Corrected position


SINS

Odometer

MAG

SINSOD

SINSMAG

Primary filter

Time update

Optimal fusion


Z1 P1

P2

Z2X2 P2

X1 P1

Xg βndash 2

1Pg

Xg Pg

Xg βndash 1

1Pg

β1 β2





βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)



1113954Xi 1113954Xg (i 1 2)


(34)



21113872 1113873minus 1


1113954X1 + Pminus 12

1113954X21113872 1113873(35)










ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References






























ψMAG arctanH

bz sin c cos θ + H

by sin c sin θ + H

bx cos c

Hby cos c minus H

bz sin c

(18)

where Hbx Hb







a1113960 1113961T (20)



F2k FSINS (22)




Z2k Zψ (24)


Z2k H2kX2 + v2k (25)


H2k 01times2 1 01times121113858 1113859 (26)









Zik HikXik + Vik1113896 (i 1 2) (27)





Tikminus 1 (i 1 2)

(28)







Tik + Rik1113872 1113873

minus 1 (i 1 2)

(30)




(31)



Rlowastik 1113936

kkminus N+1 eike

Tik

N (32)





SINS

Odometer

FederatedKalman

filter

Magnetometer

Yaw angle

Yaw angle

Velocity

Velocity

+ndash

ndash+

Velocityerror

Yaw angleerror

Estimated error

Error statequantity

Corrected position


SINS

Odometer

MAG

SINSOD

SINSMAG

Primary filter

Time update

Optimal fusion


Z1 P1

P2

Z2X2 P2

X1 P1

Xg βndash 2

1Pg

Xg Pg

Xg βndash 1

1Pg

β1 β2





βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)



1113954Xi 1113954Xg (i 1 2)


(34)



21113872 1113873minus 1


1113954X1 + Pminus 12

1113954X21113872 1113873(35)










ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References


































Tik + Rik1113872 1113873

minus 1 (i 1 2)

(30)




(31)



Rlowastik 1113936

kkminus N+1 eike

Tik

N (32)





SINS

Odometer

FederatedKalman

filter

Magnetometer

Yaw angle

Yaw angle

Velocity

Velocity

+ndash

ndash+

Velocityerror

Yaw angleerror

Estimated error

Error statequantity

Corrected position


SINS

Odometer

MAG

SINSOD

SINSMAG

Primary filter

Time update

Optimal fusion


Z1 P1

P2

Z2X2 P2

X1 P1

Xg βndash 2

1Pg

Xg Pg

Xg βndash 1

1Pg

β1 β2





βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)



1113954Xi 1113954Xg (i 1 2)


(34)



21113872 1113873minus 1


1113954X1 + Pminus 12

1113954X21113872 1113873(35)










ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References
































βi tr Pi( 1113857

tr P1( 1113857 + tr P2( 1113857 (i 1 2) (33)



1113954Xi 1113954Xg (i 1 2)


(34)



21113872 1113873minus 1


1113954X1 + Pminus 12

1113954X21113872 1113873(35)










ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References































ndash35

ndash25

ndash15

ndash5

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINS

SINSODSINSODMAG

Nor

th (m

)


SINS

SINSOD

SINSODMAG

3

2

1

0

ndash3

ndash2

ndash1

ndash4

Pit

ch a

ngle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(a)

SINS

SINSOD

SINSODMAG

1

0

ndash3

ndash2

ndash1

Roll

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(b)

SINS

SINSOD

SINSODMAG

130

125

115

120

Yaw

angle

(deg

)

0 5 10 15 20 25 30 35 40 45

Time (s)

(c)




0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References































0 5 10 15 20 25 30 35 40 45Time (s)

1

0

ndash1

ndash2

ndash3

ndash4

Erro

r in

the e

ast (

m)

(a)


0 5 10 15 20 25 30 35 40 45Time (s)

45678

3210

ndash1

Erro

r in

the e

ast (

m)

(b)





ndash60

ndash70

ndash50

ndash40

ndash30

ndash20

ndash10

0

East (m)

GPSSINSOD


Nor

th (m

)







SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References


































SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

4

ndash4

2

ndash20

ndash6

Pitc

h an

gle (

deg)

(a)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

3210

ndash3ndash2ndash1

ndash4ndash5ndash6

Roll

angl

e (de

g)

(b)

SINSSINSOD


0 10 20 30 40 50 60 70Time (s)

Yaw

angl

e (de

g)

130

135

140

125

115

110

120

(c)






6 Conclusion






Data Availability




References

































6 Conclusion






Data Availability




References






























PreciseShearerPositioningTechnologyUsingShearerMotion … · 1 day ago · precise positioning...

Documents

Transcript of PreciseShearerPositioningTechnologyUsingShearerMotion … · 1 day ago · precise positioning...