An Algorithm for Error Reducing in IMU

Transcription

1 An Algorthm for Error Reducng n IMU Krl Alexev Mathematcal Methods for Sensor Informaton Processng IIT - BAS Sofa, Bulgara alexev@as.g Iva Nkolova omputer Systems Department FS, Techncal Unversty of Sofa Sofa, Bulgara nn@tu-sofa.g Astract Durng the last few years mcromnaturzed nertal sensors were ntroduced n many applcatons. Ther small sze, low power consumpton, rugged constructon open doors to many areas of mplementaton. The man drawack of these sensors s gyro drft, leadng to an unavodale accumulaton of errors. In the paper an approach s proposed to dmnsh error accumulaton. Keywords - Inertal sensors, change detecton I. INTRODUTION Inertal Measurement Unt (IMU conssts from one or more sensors, measurng the change of knematc energy of a movng ody. The sensors are dvded n two groups: gyro sensors and accelerometers. Gyro sensor gves rotaton rate of the ody. Accelerometer provdes nformaton aout lnear acceleraton of the ody. Usually descrpton of 3D moton of a ody s gven y 3 orthogonally placed accelerometers gvng transton dynamc of the ody and 3 orthogonally placed gyro sensors determnng the orentaton of the ody. The axes of the oth types of sensors normally concde e.g. n a 3D orthogonal coordnate system there are sensors to measure lnear acceleratons on each of the axes and rotaton rate of the same axes. Thus the calculaton process s also smplfed. Two type of IMU were realzed n the years. The frst one s a classcal gyroscope, whch preserve one and the same (ntal poston, remanng ndependent of ody movement. The real orentaton of the ody s measured as a dfference etween gyroscopes axes orentaton and present orentaton of the ody - ts roll, ptch and yaw. The second one, called also strap-down gyro sensor, s fxed tghtly on the ody and provdes measurement of rate of rotaton of the ody. Usually the strapdown sensors are produced as a MEM devce wth extremely hgh roustness and low power consumpton. In ths work such a type of devces wll e consdered. The ody atttude s calculated usng smultaneously the measurements of 6 sensors. Body orentaton s gven y ntegraton of gyro sensors measurements. Transton of the ody s calculated y doule ntegraton of accelerometers readngs, accordng current ody orentaton. The ntegraton process quckly accumulates errors. Due to exstence of almost constant gravtatonal acceleraton even small errors n the estmates of orentaton of the ody cause g devaton n the decomposton of gravtatonal acceleraton on the axes, leadng to large scale of atttude errors. Due to the qualty of sensors IMU are dvded n four groups of class of accuracy [1]: TABLE I. AUMULATED ERROR DUE TO AELEROMETER BIAS ERROR Grade Accel. Bas Error Horzontal Poston Error [m] [mg] 1 s 1 s 6 s 1 hr Navgaton Tactcal Industral Automotve TABLE II. Accelerometer Msalgnment [deg] TABLE III. Grade AUMULATED ERROR DUE TO AELEROMETER MISALIGNMENT Horzontal Poston Error [m] 1 s 1 s 6 s 1 hr AUMULATED ERROR DUE TO GYRO ANGLE RANDOM WALK Gyro Angle Random Walk [deg/ hr] Horzontal Poston Error [m] 1 s 1 s 6 s 1 hr Navgaton Tactcal Industral Automotve As t can e seen from Tale I, Tale II and Tale III, even small errors n gyro angle estmaton may dscredt navgaton. II. PROBLEM DESRIPTION The ody moton n an nertal coordnate system can e descred y followng equaton: t t r r + ( g + a ( t dtdt ( /13/$ IEEE

2 where r denotes the vector of atttude coordnates n nertal coordnate system; g s gravtatonal acceleraton, regarded constant for the tme and space of the ody movement; a s acceleraton vector of forces, nfluencng the ody, adjusted to nertal coordnate system; r s the ntal ody atttude n nertal coordnate system at tme t. The ody space orentaton can e descred accordngly: ν ν + dt ω where ν denotes the ntal ody orentaton n nertal coordnate system at tme t, ω s the vector 3D rate turn, adjusted to nertal coordnate system. The goal of navgaton s to fnd coordnates of a ody and ts orentaton. In the case of IMU sensor the task s solved ased on IMU measurements and ntegral equaton (1 and (2. A smple algorthm for coordnate determnaton s presented elow. The calculaton scheme s ased on Euler angles. Let us denote the rotaton matrx, transformng a vector from the movng ody to nertal coordnate system y ( t. Then an acceleraton vector a ( t n the ody coordnate system wll e transformed to nertal coordnate system y (3: t ( t ( t a ( t (2 a (3 Now the rotaton matrx ( t wll e represented y Euler angles [2]: where x y z ' ' ' ( t ( t ( t ( t 1, (4 z ( t cos( ϕ( t sn( ϕ( t sn( ϕ( t cos( ϕ( t cos( θ ( t sn( θ ( t ( t 1 sn( θ ( t cos( θ ( t cos( ψ ( t sn( ψ ( t ( t sn( ψ ( t cos( ψ ( t y x,,, 1 are the rotaton matrxes that rotate vectors on angles ϕ ( t, θ ( t, ψ ( t on axes x, y and z. It s mportant to menton that the order of rotaton s mportant. If the angles of rotaton are suffcently small: δϕ δt δθ δψ or n other words the measurement samplng rate s suffcently hgh satsfes Nyqust samplng rate, whch guarantees that you capture a sgnal properly ecause you sample t at least twce per cycle of the hghest frequency component t contans, the followng susttutons for an angle α may e appled: cosδα 1 and sn δα δα. The product of small angles can e also approxmated y zero: δα δα. The fnal expresson for the change n rotaton matrx wll e: 1 δθ δψ δθ 1 ( δt δψ 1 δϕ δt δϕ 1 δψ δθ 1 + δψ δϕ I + Δ δθ δϕ 1 The fnal rotaton matrx can e presented as a product of the rotaton matrx at t and calculated aove rotaton matrx ( δ t, correspondng to small addtonal rotatons, commtted n tme nterval δ t : ( t + δt ( t ( δt ( t( I + Δ (5 (6 Let now express the dervatve of rotaton matrx: ( t lm ( t + δt ( t (( t I + Δ ( t lm δt δt δt ( t ( t Δ δt δt Δ lm, (7 δt ω z ω where Δ y ωz ω x and ω x, ωy, ω z are the ω y ωx lastly receved measurements of rotaton rates from gyro sensors on correspondng axs. Δ The soluton of (7 s ( ( t. The matrx exponent n soluton can e presented as an nfnte sum: 2 k Δ Δ Δ Δ Δ I k! 1! 2! k! k

3 Takng nto account only the frst two terms (lnear approxmaton we receve an approxmate formula for recurrent calculaton of rotaton matrx: ( t + t ( t( I + δ Δ (8 Let now calculate the exact expressons for angle dervatves. The dfferental equaton (7 wll e used, where the rotaton matrx from (4 n explct form wll e susttuted: cosψ snψ - snθ - cosϕsnψ + snϕsnθcosψ cosϕcosψ + snϕsnθsnψ snϕ snϕsnψ + cosϕsnθcosψ - snϕcosψ + cosϕsnθsnψ cosϕ (9 The matrx equaton wll e resolved for matrx element (3,1 (3-rd row, 1-st column. The correspondng equaton looks lke: d( snθ dt Therefore: ( snθ snϕ cosϕ (1 θ snϕ ω cosϕ z ω y y ω z ω z ω y θ cosϕ ω snϕ (11 For matrx element (3,2 we receve: cos ϕ ϕ snϕ snθ θ snθ ω + cosϕ (12 z ω x Susttutng θ from (11 nto (12 and expressng ϕ we receve: ( sn ϕ ω + cosϕ ϕ ω (13 x + tgθ y ωz To fnd the expresson for ψ the equatons have to e used for matrx elements that contan ψ. For example, f the element (1,1 s used: sn θ cosψ θ snψ ψ ( cos ϕ snψ + sn ϕ sn θ cosψ ω + z ( ϕ snψ + cosϕ snθ ψ ( + sn cos (14 ω y Usng (11 to susttute θ and smplfyng we receve: ( snϕ ω + cosϕ 1 ψ y ω z (15 The equaton (11, (13 and (15 are most often used for calculaton of rotaton angles etween two successve gyro measurements wth a lnear approxmaton only. Let now consder errors n sensor measurements. The error propagaton for acceleraton sensors only looks lke: 2 t t gt r r ( a ( t + a dtdt 2 2 t t gt ε at r a( t dtdt 2 2 Here ε a denotes the error vector of acceleraton sensors. The error propagaton for gyro sensors only looks lke: t ν ν + ω ( ω ( t + ε dt ν + ε t ( t ε ω ω + t dt (16 (17 Here ε ω denotes the error vector of gyro sensors. The equatons (16 and (17 gve error propagaton n the smplest case of ndependent errors. In practce there are many types of errors, nfluencng one to others. The nfluence of rotaton rate error measurements on angle determnaton s ovous from (11, (13 and (15. As a consequence the error propagaton n (17, for example, generates/nduces nonlnear errors n estmaton of acceleratons, leadng to quckly growng errors n estmated system poston. That s why (16 and (17 are used only to approxmate the order of generated errors and are not of practcal use. The sensors are suject to dfferent types of errors due to sensor mperfectness, model naccuracy or computatonal errors. The man errors nfluencng on the atttude estmaton accuracy may e grouped nto three categores [3, 4]: A. Sensors do not provde perfect and complete data. Bas errors produce constant or almost constant shft of sensor values from the true ones. The scale factor errors cause lack of correspondence etween real turn veloctes and real straght lnear acceleratons and output sensors readngs (gyro and accelerometer correspondngly. Errors due to manufacturng mperfectons n IMU. Usually they are caused y non-orthogonally placed

4 accelerometer or gyro sensors on the chp or y lack of concdence etween axes of correspondng accelerometer and gyro sensors. The last error more often s ntated y the frst one, ut sometmes can exst alone. The sensors readngs are also contamnated y addtve Gaussan nose. Temperature dependent errors. Temperature devaton affects output readngs. There s tme synchronzaton prolem. Sensors readngs do not elong to one and the same moment of tme. Dynamc error (lag of sensor reacton/response to force mplementaton. B. Imperfectness of the used models and computatonal arthmetc The model naccuracy usually s caused y nexact sensor approxmaton, ncorrect gravtatonal acceleraton estmate. The computatonal errors are caused y lmtatons of computer arthmetc, teratve procedures for optmzaton, calculatons of trgonometrc functons, loss of orthonormalty of matrces, etc.. External sources of dsturances (uncontrolled, unpredctale even unknown sources of dfferent type dsturances Platform vraton. The vraton counteracts to sensor accuracy. It depends of dfferent random factors, platform dynamcs, mass dstruton, swtchng on/off of dfferent devces, and etc. Others The Fg. 1 elow dsplays the nfluence of dfferent types of errors on qualty of atttude estmaton. Fg. 1. Errors n an IMU

5 In order to mnmze dfferent type of errors we have to estmate ther nfluence on the poston estmate. There are many well estalshed methods for selfconsstency check and normalzaton. One of them concerns the rows/columns of the rotaton matrx. The rotaton matrx s drecton cosne matrx, whch row/columns are projectons of unty vector onto orthogonal axes. That means, that the sum of squares of values n each row/column have to e equal to 1 and due to ther orthogonalty, ther scalar products have to e zero. In the cases of usng quaternons the normalzaton means that the sum of squares of quarternon elements has to e equal to 1. Ths normalzaton usually doesn t correct errors. Even f optmzaton procedure s started, the est receved result does not guarantee the error compensaton. Moreover, t usually propagates the error over correct terms. That s why the precse error expresson s not of practcal use. III. THE PROPOSED ALGORITHM FOR ERROR PROPAGATION MINIMIZATION The IMU sensor systems have unavodale sources of errors. Through normalzaton and orthogonalty checks the error propagaton can e only slghtly enhanced, f ever. The effect s most often dluton of the errors on all varales. To stop the process of error accumulaton we have to stop the process of ntegraton/doule ntegraton and rentalze calculatons. We cannot do permanently that ecause of need to estmate platform poston. To mnmze ntegraton tme we wll dscover moton of the platform and only when the moton s detected the ntegraton process wll e swtched on. When there s no moton detected, the ntegraton process wll e stopped and the platform orentaton and poston wll reman the same. Ths dea s not new one. For example, n the emedded software on newest MEMS an actvty threshold s nserted for acceleraton sensors. Only acceleratons, exceedng threshold, swtch on the flag Actvty. In spte of ts smplcty the realzaton of ths functonalty gves the system engneers knowledge when to ntalze the sensor or when to start recalraton procedure. In ths work we develop ths dea further. We realze more precse algorthm for actvty detecton, whch s less susceptle from the sensor sgnal as. Two dfferent types of change can e dstngushed. Arupt change s an nstantaneous change n the parameters of the system. Here nstantaneous change means that the transton from one to other state s commtted faster wth respect to the samplng perod of the measurements. The second type of change s slow change. We are nterested n detecton of oth types of change, ndependent on magntude value of the change. Moreover, IMU measurements characterze usually wth small and not necessarly fast changes. hange detecton may e statstcally formulated as a random process, whch statstcal parameters change sgnfcantly at a pont, called change pont. To estmate statstcal parameters, an nterval wth suffcent length has to e consdered. The change pont dvdes the tme-seres data of two parts wth dfferent dstruton characterstcs. The change detecton prolem may e resolved y model ased change detecton algorthms or y model-free algorthms. Typcally model-ased approaches decompose tme-seres data nto trend, perodcal data and resdual components. For navgaton purposes the model-ased approaches are not sutale due to ther complexty and lack of general models. The model-free algorthms deal wth data values drectly. One of the most popular representatves of ths type of algorthms s USUM [5]. Page, the author of USUM, examned a "qualty numer", y whch he denotes a parameter of the explored proalty dstruton; for example, the mean. The devsed recursve procedure calculates a cumulatve sum and compares t wth a threshold. To detect the exact pont of change, Page defned also the average run length as the expected numer of processed measurements efore acton s taken. Usually, model-free change detecton algorthms are computatonally smple, more roust n comparson wth model-ased ones. The appled n ths paper change detecton algorthm s ased on Shewhart control chart [6]. Due to many types of error sources, nfluencng on sensor data, t s assumed that the tme-sequences from nertal sensors can e represented as a sgnal dstured y addtve Gaussan dstruted nose. It can e consdered that any change n dynamc of the examned platform leads to a change n the output data of one or more strapdown nertal sensors. We are lookng for a change of the mean of a sample wth length equal to N y the followng suffcent statstc [6]: j + N j+ N μk+ 1 μk μk+ 1 μk + 1 μk 2 σ j+ 1 2 S j y 1 μ N( K 1 + k y N NK + 1, where The change s detected when the nequalty s fulflled: μ k+1 σ μk m N The tunng parameters of the procedure are m and N. The procedure s appled on the tme-seres from the all sx nertal sensors three gyros and three accelerometers. The output results are fused n a fnal tme-seres wth two states only and 1. Zero means to stop ntegraton of sensor output data and remans n the same state (the same poston, velocty and acceleraton and preserve the same orentaton of the platform n the space. When One appears for the frst tme the ntegraton process s restarted and a new state vector and a new orentaton of the platform are calculated. IV. EMPIRIAL RESULTS The algorthm was tested on a platform wth MPU-65 strapdown nertal sensors. The platform commts a smple move followng contours of a quadrate wth a sde, equal to 1 cm. The data flow from 3 gyros and 3 accelerometers were saved and two types of algorthm were appled. The calculated platform trajectory receved y standard navgaton algorthm (wthout actvty detecton s shown on Fg. 2.

6 Fg. 6. The reconstracted platform trajectory wth change detecton Fg. 2. The reconstracted platform trajectory wthout change detecton The gyro and accelerometer sgnals are shown on Fg. 3. Fg. 3. Accelerometer raw sgnals The change detecton algorthm s llustrated on Fg. 5. The frst graphc depcts one of accelerometer sgnals, where change detecton algorthm s appled on (second graphc. The fused (from all sx sensors result s shown on Fg. 6 and the reconstructed trajectory s dsplayed on (Fg.7. Fg. 4. Applcaton of change detecton algorthm on a raw sgnal V. ONLUSION The contemporary strapdown nertal MEMs are far ehnd n accuracy from the precse, very heavy and costly navgaton platforms. In spte of ths a lot of applcatons are watng for more precse nertal sensors. The mplementaton of change detecton algorthms enhances the accuracy of nertal MEMs and open door for realzaton of some of the deas. The man drawack of the proposed change detecton algorthm, whch has to e consdered, s the very small changes. They are neglected n the process. Or, nothng etter than one accurate sensor! AKNOLEDGEMENT The research work reported n the paper s partly supported y the project AomIn "Advanced omputng for Innovaton", grant 31687, funded y the FP7 apacty Programme (Research Potental of onvergence Regons and y the project No DFNI I1/8 funded y the Bulgaran Scence Fund. All data, laoratory equpment were suppled y MM Solutons n the framework of the project Industral research for development of technology for mage enhancement and vdeo stalzaton usng nertal sensors, ontract BG161PO , Operatonal Program "Development of the ompettveness of the Bulgaran Economy". REFERENES [1] [2] Davd H. Ttterton, John L. Weston Navgaton Technology - 2nd Edton, The Insttuton of Electrcal Engneers, 24, ISBN [3] Grewal, M.S., Well L.R., Andrews A.P., Gloal Postonng Systems, Inertal Navgaton, and Integraton, John Wley & Sons, 21, ISBN [4] Olver J. Woodman, An ntroducton to nertal navgaton, Techncal Report UAM-L-TR-696, ISSN , 27. [5] Page, E. S., ontnuous Inspecton Scheme, Bometrka 41 (1/2, pp , ud4&sd JSTOR 2333 [6] Mchele Bassevlle, Igor V. Nkforov, Detecton of Arupt hanges: Theory and Applcaton, Prentce-Hall, Inc, ISBN , Fg. 5. The fused result from change detecton algorthm