Inertial Measurement Unit Simulator

Transcription

1 Inertal Measurement Unt Smulator Krl Alexev Abstract 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 drawback of these sensors s the nfluence of dfferent type of errors, leadng to an unavodable wrong poston and orentaton estmaton. In the paper a smulator of Inertal Measurement Unt s proposed. The smulator s a tool for assstance of trajectory set up and on the base of nput data t generates IMU output accordng gven error/nose parameters. It allows us to smulate dfferent types of IMUs based on pror knowledge of the IMU error s propertes. One of the man goals n developng of the smulator s to valdate new methods nvolvng nertal technology. Somethng more, the smulator s an excellent tool for tunng complex flterng procedures and enhancng navgaton accuracy. The smulaton of dfferent scenaros gves more nformaton to receve better understandng of the weght of dfferent sensor noses and errors on the fnal results. 1 Introducton Inertal Measurement Unt (IMU) conssts from one or more sensors, measurng the change of knematc energy of a movng body. The sensors are dvded n two groups: gyro sensors and accelerometers. Gyro sensor measures rotaton rate of the body. Accelerometer provdes nformaton about lnear acceleraton of the body. Usually descrpton of 3D moton of a body s gven by 3 orthogonally placed accelerometers gvng transton dynamc of the body and 3 orthogonally placed gyro sensors determnng the orentaton of the body. The axes of the both 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 bult on the scheme of the classcal gyroscope and t preserves one and the same (ntal) poston, remanng ndependent of body rotaton. In ths case the body orentaton s measured as a dfference between gyroscopes axes orentaton and the present orentaton of the body - ts roll, ptch and yaw. The second one, called also strapdown gyro sensor, s fxed tghtly on the body and provdes measurement of rate of rotaton of the body. For ths class of sensors, the body orentaton s receved through the ntegraton of gyro measurements n respect to a pror known body orentaton. Usually the strapdown sensors are produced as a MEM devce wth extremely hgh robustness and low power consumpton. In ths paper such a type of devces wll be consdered. The Inertal Navgaton System (INS) s a system that reles entrely on nertal measurements for determnaton of dynamcal body poston and orentaton. Today a wde range of

2 strapdown INS s avalable on the market. The smulator, presented n ths paper, emulates the behavour of standard MEM realzaton of an INS wth three lnear accelerometers and three angular rate sensors. It generates nertal sensor measurements n accordance wth the precson and accuracy specfcatons of partcular sensor sample. The mprovement n computers capablty allows the smulaton to become nstrumental n technology development [5]. The tool, presented here, wll be used to: Enhance our understandng of nertal technology; Smulate dfferent types of IMUs based on pror knowledge of ther specfcatons; Smulate a wde range of scenaros, even unrealstc ones; Test and valdate new navgaton algorthms; Study of dfferent error propagaton and estmaton of the error nfluence over system precson and accuracy; Estmate the requred hardware/sensor characterstcs for a gven applcaton; Laboratory test of nstalled systems to assure that they are workng properly before real test and to verfy system performance n crtcal/rare stuatons. A modular archtecture s used n desgn of the proposed smulator that allows you to modfy, mprove and replace the ndvdual modules wthout changng the overall archtecture. Smulator gves also flexablty n desgnng and research work and dramatcally reduce tme and money consumng feld experments. The system under test can be examned on dfferent moton and vbraton probatons through the computer generaton. The paper s organzed as follows. In the next chapter the mathematcal background for nertal sensor modelng and smulaton n navgaton s revealed. Thrd chapter s devoted on error propagaton for accelerometers and gyro sensors and a short overvew of dfferent error types s gven. The fourth chapter descrbes the structure of IMU smulator. Some results are descrbed n the next chapter. The concludng remarks are gven the last chapter. IMU based navgaton (mechanzaton equatons) The body moton n an nertal frame of reference can be descrbed as a result of smultaneous acton of two forces - gravtatonal F and specfc F : g sp Fsp Fg a asp g, (1) m m b where g s acceleraton,caused by gravtatonal force and a sp s the acceleraton caused by specfc force. Gravtatonal force s a functon, dependng on the dstance between body and the Earth: M emb Fg G r, () 11 where G s the gravtatonal constant G 6.674*1, r s the dstance between the nteractng 9 bodes, M e s the mass of the Earth and K GM e *1. To explan the specfc force we ntroduce three frames of reference - one assosated wth the movng body, denoted by subscrpt b, the second one s a geocentrc frame, rotatng wth the rate of rotaton of the Earth - t s assocated wth the subscrbt e and the last one s also geocetrc, but t s nertal and t s marked by subscrbt. Let now denote the rate of the Earth rotaton by. The last ntroducton note concerns the dfferental of a vector n absolute reference frame f t s presented n rotatng system: dea der ea, (3) Let now express the velocty n nertal reference frame, applyng expresson from (3): b

3 dre v re ve re, (4) The next step s to express acceleraton, applyng twce (3): dv d( ve) d( re ) dve dre ve r, (5) e frst term second term dv ae ve re, (6) Regardng the receved result as equal to specfc acceleraton and substtutng n () we receve: a ae ve re g, (7) The acceleraton ve s result of Corols force, and the term re corresponds to centrfugal acceleraton. Usually the last two terms of (7) are grouped together and replaced by so called local gravtatonal acceleraton or smply gravty: a ae ve gl ( h), (8) where h s the heght of the body above the Earth surface. The equaton (8) s regarded as fundamental navgatonal equaton. It s worth to estmate the sgnfcance of all terms. Let consder a moton wth velocty of 36 km/h on the Earth surface near to Equator. The appled force creates acceleraton equal to 1 m/s. For ths 3 example a e. 1g, v e 1.46 *1, re 3.4*1. For calculaton of g the Gelmert formula s appled: l g l ( h) 9.783(1.53 sn.7 sn ).14 h, (9) 3 where s northern lattude and * 1 s the Schuler frequency. The goal of navgaton s to fnd coordnates of a body and ts orentaton. In the case of IMU sensor the task s solved based on IMU measurements and ntegral equaton (1) and (11): r t t r gl a ( t) (1) where r s the ntal body atttude n nertal coordnate system at tme t=. The body space orentaton can be descrbed accordngly: t (11) where denotes the ntal body orentaton n nertal coordnate system at tme t, s the vector 3D rate turn, adjusted to nertal coordnate system. A smple algorthm for coordnate determnaton s presented below. The calculaton scheme s based on Euler angles. Let us denote the rotaton matrx, transformng a vector from the movng body to nertal coordnate system by C b t. Then an acceleraton vector a b t n the body coordnate system wll be transformed to nertal coordnate system by (3): Now the rotaton matrx where t C t a t C b t wll be represented by Euler angles []: ' ' ' C t C t C t C t b a b b (1), (13) z y x

4 C x t 1 cos sn cos t sn t t sn t, C t cos t y t 1 sn t cos t cos t sn t t sn t cos t C z 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 the measurement samplng rate s suffcently hgh (n other words satsfes Nyqust samplng rate, whch guarantees that you capture a sgnal properly because you sample t at least twce per cycle of the hghest frequency component t contans) the followng substtutons for an angle may be appled: cos 1 and sn. The product of small angles can be also approxmated by zero:. The fnal expresson for the change n rotaton matrx wll be: 1 Cb t 1 I (14) t 1 Fnally, the rotaton matrx s presented as a product of the rotaton matrx at t and calculated above rotaton matrx C b t nterval t :,, correspondng to small adonal rotatons, commtted n tme t t C t C t C t I b b b b (15) C Let now express the dervatve of rotaton matrx: C t t C t C t I C t C b b b b b t lm lm C b t lm Cb t, (16) t t t t t t z where y z x and x, y, z are the lastly receved measurements from gyro y x sensors on correspondng axs. The soluton of (16) s C bt Cb. The matrx exponent n soluton can be presented as an nfnte sum: k I k k! 1!! k! Takng nto account only the frst two terms (lnear approxmaton) we receve an approxmate formula for recurrent calculaton of rotaton matrx: Cb t t Cb t I (17) Let now calculate the exact expressons for angle dervatves. In the dfferental equaton (16) we substtute the rotaton matrx takng expresson n explct form from (13). The matrx equaton wll be resolved for matrx element (3,1) (3-rd row, 1-st column). The correspondng equaton looks lke:

5 Therefore: d( sn ) sn sn cos cos cos y z z y (18) cos sn (19) For matrx element (3,) n a smlar way we receve: x tg sn y cos z () To fnd the expresson for the equatons that contan have to be used. For example, f the element (1,1) s used: 1 sn y cos z (1) cos The equaton (19), () and (1) are most often used for calculaton of rotaton angles between two successve gyro measurements wth a lnear approxmaton only. The explaned above mathematcal model s mplemented n the smulator. 3 IMU errors The body atttude s calculated usng smultaneously the measurements of 6 sensors - 3 gyros and 3 accelerometers. Body orentaton s gven by ntegraton of gyro sensors measurements. Transton of the body s calculated by double ntegraton of accelerometers readngs, accordng current body 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 body cause bg 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 1: Accumulated Error due to Accelerometer Bas Error Grade Accel. Bas Error [mg] Horzontal Poston Error [m] 1 s 1 s 6 s 1 hr Navgaton Tactcal Industral Automotve Table : Accumulated Error due to Accelerometer Msalgnment Accelerometer Msalgnment [deg] Horzontal Poston Error [m] 1 s 1 s 6 s 1 hr

6 Table 3: Accumulated Error due to Gyro Angle Random Walk Horzontal Poston Error [m] Grade Gyro Angle Random Walk [deg/ hr] 1 s 1 s 6 s 1 hr Navgaton Tactcal Industral Automotve As t can be seen from Table 1, Table and Table 3, even small errors n gyro angle estmaton may dscre navgaton. The sensors are subject to dfferent types of errors due to sensor mperfectness, model naccuracy or computatonal errors. The man errors nfluencng on the atttude estmaton accuracy may be 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 between 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 by nonorthogonally placed accelerometer or gyro sensors on the chp or by lack of concdence between axes of correspondng accelerometer and gyro sensors. The last error more often s ntated by the frst one, but sometmes can exst alone. The sensors readngs are also contamnated by adve Gaussan nose. Temperature dependent errors. Temperature devaton affects output readngs. There s tme synchronzaton problem. Sensors readngs do not belong 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 Usually the model naccuracy s caused by nexact sensor approxmaton, ncorrect gravtatonal acceleraton estmate. The computatonal errors are caused by lmtatons of computer arthmetc, teratve procedures for optmzaton, calculatons of trgonometrc functons, loss of orthonormalty of matrces, etc. C. External sources of dsturbances (uncontrolled, unpredctable even unknown sources of dfferent type dsturbances) Platform vbraton. The vbraton counteracts to sensor accuracy. It depends of dfferent random factors, platform dynamcs, mass dstrbuton, swtchng on/off of dfferent devces, and etc. Others The Fg. 1 below dsplays the nfluence of dfferent types of errors on qualty of atttude estmaton. Let consder now errors n sensor measurements. The error propagaton for acceleraton sensors only looks lke: r t t glt glt at r a ( t) a r a ( t) () Here a denotes the error vector of acceleraton sensors. t t

7 The error propagaton for gyro sensors only looks lke: t t t t Here denotes the error vector of gyro sensors. The equatons () and (3) 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 obvous from (19), () and (1). As a consequence the error propagaton n (), for example, generates/nduces nonlnear errors n estmaton of acceleratons, leadng to quckly growng errors n estmated system poston. That s why () and (3) are used only to approxmate the order of generated errors and these equatons are not of practcal use. t (3) Fg. 1 Errors n an IMU In order to mnmze dfferent type of errors we have to estmate ther nfluence on the poston estmate. There are many well establshed methods for self-consstency 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 be equal to 1 and due to ther orthogonalty, ther scalar products have to be zero. In the cases of usng quaternons the normalzaton means that the sum of squares of quarternon elements has to be equal to 1. Ths normalzaton usually doesn t correct errors. Even f optmzaton procedure s started, the best receved result does not guarantee the error compensaton. Moreover, the normalzaton algorthm propagates the error over correct terms. That s why the precse error expresson s not of practcal use.

8 4 The structure of IMU smulator The smulator has modular structure, presented on fg.. Input Data Interface s an nteractve module wth functonalty to nsert, e, save and search user data. The problem of choce what knd of eor to be used for trajectory parameterzaton (graphcal eor or text eor) was resolved n favor of the text eor, whch, although beng unfrendly and more cumbersome, allows exact parameterzaton of the trajectores. Trajectory Generator uses knematc equatons to generate body trajectory. The module has drect output to graphcal nterface to check generated trajectores and correct them n the case of wrong nput data. Nose Generator adds dfferent type of noses and naccuraces. Ths module underwent several adjustments due to change of authors understandng of the nfluence of dfferent errors on fnal result. Inertal sensor model smulates naccuracy and mperfectness of the sensors lke sensor axes nonorthogonalty, bas nstablty and scale errors, lag n sensor data, and others. Navgaton model conssts of sut of tested algorthms. There are several classcal realzatons of navgatonal algorthms and ther modfcatons for mplementaton n moble devces. Graphcal output gves 3D presentaton of generated trajectores, nosed data and results of navgaton algorthms data processng. A specal form of presentaton of 3D body orentaton s ntroduced. Input data nterface Trajectory generator Inertal sensor model Navgaton model Graphcal output Nose generator Fg. The smulator structure 5 Results The smulator was tested n an example for both: smulated data and real hardware generated data (a platform wth MPU-65 strapdown nertal sensors). The experment on the fg 3 ncludes a smple body move followng contour of a quadrate n horzontal plane. The data flow from smulator and sensors (3 gyros and 3 accelerometers) were saved and dfferent types of navgaton algorthms were appled. The hardware gyro and accelerometer sgnals are shown on Fg. 4. The calculated platform trajectory receved by data processng from a navgaton algorthm s shown on Fg. 5.

9 Fg. 3 Smulator wth graphcal output of the reference trajectory. In the crcle on the rght sde the orentaton of the movng body s presented. Fg. 4 Gyro and accelerometer sensors raw sgnals (from hardware platform) Fg. 5 The output results of navgaton algorthm 6 Concluson The contemporary strapdown nertal MEMs are far behnd 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 proposed n ths artcle smulator of IMU shortened the road from dea generaton to desgn of real applcaton. It mproves desgn by executng comprehensve and exhaustve smulatons n the lab, mnmsng feld testng. Somethng more, the smulator allows optmzaton of the choce of nertal sensors for a partcular applcaton based on publshed sensors datasheets only, materalzng software n loop smulaton approach. The modular structure of smulator allows further enhancement and enrchment of sut of algorthms. One of the most nterestng drectons for further development of the smulaton tool s realzaton of hardware n loop smulaton [6] through approprate hardware nterface and software drvers. Acknowledgement: The research work reported n the paper s partly supported by the project AComIn "Advanced Computng for Innovaton", grant 31687, funded by the FP7 Capacty Programme (Research Potental of Convergence Regons) and by the project No DFNI I1/8 funded by the Bulgaran Scence Fund. All data, laboratory equpment were suppled by MM Solutons n the framework of the project Industral research for development of technology for mage enhancement and vdeo stablzaton usng nertal sensors, Contract BG161PO C1, Operatonal Program "Development of the Compettveness of the Bulgaran Economy".

10 References [1] [] Davd H. Ttterton, John L. Weston Navgaton Technology - nd Eon, The Insttuton of Electrcal Engneers, 4, ISBN [3] Grewal, M.S., Well L.R., Andrews A.P., Global Postonng Systems, Inertal Navgaton, and Integraton, John Wley & Sons, 1, ISBN [4] Olver J. Woodman, An ntroducton to nertal navgaton, Techncal Report UCAM-CL-TR-696, ISSN , 7. [5] [6] KIRIL ALEXIEV Insttute of Informaton and Communcaton Technologes Mathematcal Methods for Sensor Informaton Processng Sofa, 5A Acad. G. Bonchev Str. BULGARIA E-mal: alexev@bas.bg