Inertial Navigation System for Mobile Land Vehicles

Transcription

1 Inetil Nvigtion Sstem fo Mobile Lnd Vehicles Joge Lobo, Pulo Lucs, Joge Dis, A. Tç de Almeid ISR - Instituto de Sistems e Robótic Deptmento de Engenhi Electotécnic Univesidde de Coimb Lgo Mquês de Pombl 3 COIMBRA, PORTUGAL E-mil : joge@mecuio.uc.pt f : Abstct This ppe descibes pototpe of n inetil nvigtion sstem fo use in mobile lnd vehicles, such s cs o mobile obots. The complete sstem is composed b sensos, thei mechnicl mount nd cbling, these connect to PC cd with locl pocessing nd memo, bsed on Intel 8C196KC micocontolle. The sensos used wee pieoelectic vibting goscope, two silicon cceleometes, two clinometes nd lso mgnetic flugte compss cting s n independent souce fo dift compenstion. The woking pinciple of the vious sensos e pesented. The epeimentl setup used fo testing is bsed on obot m contolled in el time to move the sensos long known tjecto (velocit contol. 1. Intoduction Fom inetil mesuements we cn detemine n estimte fo line cceletions nd ngul velocities. B integting these quntities we detemine the velocit vecto nd the bod ttitude. Position cn be clculted b integtion of the velocit vecto. Inetil nvigtion is thus bsed on the ded-eckoning pinciple. Inetil nvigtion sstems hve been widel used in eospce pplictions but e onl now being seiousl eploited in obotics pplictions whee the hve consideble potentil. Recent development in solid-stte cceleometes nd gometes hs led to some new low-pice sensos. These sensos, though not suitble fo high pefomnce inetil sstems, cn povide esonble sstem suitble fo some pplictions. Unlike the tpicll high-cost sstems developed fo eospce, these new sstems e much moe pice-competetive nd thus suitble fo wide nge of pplictions. The needs of the utomotive indust hs mde these sstems incesingl commecill vilble. The lck the high ccuc of thei eospce countepts, but b ppopite modelling of the sstem the cn povide useful positioning infomtion fo mn pplictions. A theoeticl sstem using thee cceleometes nd thee goscopes ws pesented in [1]. Methods of etcting the motion nd oienttion of the sstem fom inetil infomtion e deived theoeticll but not diectl implemented in el sstem. In [] vibting bem goscope ws used fo shot nge oienttion estimtion fo outdoo vehicles, enbling wide spcing of nvigtion becons. In [3] n opticl fibe goscope ws used fo position estimtion, b combining the go dt with the wheel encodes dt. In [4] clibtion method is poposed fo gometes, bsed on the use of edundnt one. In this wok we developed pototpe of n inetil nvigtion sstem fo use in mobile lnd vehicles, such s cs o mobile obots. We hope to tke dvntge of the vehicles limited movements to build simplified inetil sstem. Unlike some sstems tht el on outdoo envionments to obtin dditionl infomtion, like GPS NvSt [5], o on knowledge of the vehicle's moving spce [6], we intend to build sstem tht is cpble of woking eithe indoos o outdoos in n unstuctued envionment. We ssume howeve tht g is constnt unifom 3- D vecto field. The following sections descibe the poposed sstem, the sensos used nd thei woking pinciple, how the sstem ws put togethe nd the epeimentl setup used to test it.. Poposed Inetil Nvigtion Sstem The complete sstem is composed of the sensos, thei mechnicl mount nd cbling, the dt cquisition hdwe nd the contolling softwe..1. The Hdwe The sensos used wee pieoelectic vibting goscope, two silicon cceleometes, two clinometes nd lso mgnetic flugte compss cting s n independent souce fo dift compenstion Gomete In ode to keep tck of ottion on the -is Gost ENV- 11D, built b Mut, ws used. The device is senso fo ngul velocit. Theefoe the oienttion must be clculted b integtion of the ngul velocit ove time. The device s output

2 is voltge popotionl to the ngul velocit nd eltive to efeence. The pinciple of the senso is outlined in Fig. 1. Inside the device thee is n equiltel tingle pism mde fom elstic invible metl, which is fied t two points. Thee pieoelectic cemic elements e ttched to the fces of pism, one on ech side. The pism is foced to vibte t bout 7 kh b one of the pieoelectic elements, whilst the othe two e used fo detection. When thee is no ottion the detect equll lge signls. When the pism is tuned it gets twisted, so the detectos eceive diffeent signls. The diffeence between the signls is pocessed b the intenl nlog cicuits to povide n output voltge popotionl to the ngul velocit [7]. esistos incese with downwd cceletion nd two decese with the sme foce. This povides tue DC esponse, unlike the pieoelectic cceleometes tht onl hve AC esponse. The full bidge design substntill educes off-is sensitivit. Since sidews cceletions cuse smmetic distubnces on ech side of the double cntileve stuctue, thei effect is cncelled out, povided the pieoesistos e well mtched. A tpicl fequenc esponse of the senso is shown in Fig.. +6dB db -3dB Pieoelectic cemic Oscilltion cicuit Diffeentil mplifiction Voltge popotionl to ngul speed Phse compenstion Snchonied detection DC mplifie Fig.1. Block digm of the pieoelectic vibting goscope. The Gost ENV-11D hs high pecision comped to othe vibtion goscopes. It is line to within ±.5% of mimum ngul velocit. Hs n ngul velocit nge of ±9 deg/sec nd no hsteesis. The output DC voltge is popotionl to ngul te b.±.65 mv/deg/sec scle fcto nd nges fom.5 V to 4.5 V. Simple models of the goscope built b Mut e known to hve self-heting poblems tht cuse offset dift [4]. Tests cied out with the ENV-11D showed it wsn t significntl ffected b this poblem. The dift is onl noticeble when integted ove long peiods of time. To ovecome this poblem, etenl efeences hve to be used, such s the flugte compss included in ou sstem nd descibed in following section..1.. Acceleometes Two pieoesistive cceleometes wee used to mesue cceletions long the nd is. The device used, model fom ICSensos, is double cntileve silicon cceleomete. The full scle output is ± volts bout.5 volt offset. The sensos used hd nge of ±g. Silicon senso development hs enbled the btch-fbiction of this integted cceleomete. It incopotes mplifiction, signl conditioning, tempetue compenstion nd intenl voltge egultion, ll in smll pckge. Pieoesistos e diffused onto the fleues of the bidge suppoting the sensing mss. The esultnt electicl stuctue is Whetstone bidge whee two -9dB 1H 1kH 1kH Fig.. Acceleomete fequenc esponse (fom ICSensos clibtion dt. As cn be seen fom the fequenc esponse, the sensos used wee slightl unde-dmped. When designing the cquisition filtes this ws tken into ccount, so s to void output stution nd distotion Clinometes To mesue tilt bout the nd is two AccuSt electonic clinometes, built b Lucs Sensing Sstems, wee used. The device hs line nge of ±45 deg. Coss-is inclintions up to 45 deg intoduce n eo of less thn 1%. The concept of the senso is bsed on dielectic fluid with n i bubble inside cpcitive senso. When ou tilt the senso, the bubble, moving unde the foce of gvit, chnges the cpcitnce of the senso elements. The esulting diffeentil genetes n output signl which eflects the eltive tilt in the sensing is s shown in Fig. 3. The clinometes hve deled esponse with time constnt of.3 seconds. This is not poblem if tken into ccount in the clcultions. Ce should lso be tken when cceletions othe thn gvit e pesent, since the tilt will be mesued eltive to the esultnt vecto. In ou sstem we Diffeentil One shot Filte mplifie time One shot time Astble time Senso Filte Buffe mplifie Anlog output popotionl to tilt Fig.3. Block digm of the cpcitive clinomete.

3 ssume tht, fom time to time, we know tht the vehicle is t est, nd cn theefoe keep tck of the veticl The Flugte Compss The compss used is C1 model fom KVH Industies. The flugte senso element is stuble ing coe, fee floting in n inet fluid within clindicl len housing. The pupose of the floting ing coe is to keep the sensing element hoiontl with espect to the eth. Hd mounted coes epeience significnt eos when not pefectl hoiontl (often moe thn thee degees of eo fo ech degee of tilt. The len housing is suounded b windings which electicll dive the coil into stution [8]. v v Fig.4. Flu-gte senso element. Pulses, whose mplitude is popotionl to the sensed hoiontl component of the eth's mgnetic filed, e detected b two second windings tht e t ight ngles, s cn be seen in Fig. 4. These povide dt on the nd hoiontl components of the eth's mgnetic field. These signls e conveted to DC level, digitied nd then sent to micopocesso tht clcultes the imuth ngle s 1 v φ= tn ( (1 v See [8] fo moe detils. The C1 compss cn compenste onl fo modete mgnetic intefeences. It is convenient to keep distnce of t lest 1 inches [9] fom ion, steel, mgnets, motos, nd othe mgnetic mteil. As this ws incomptible with ou ppliction, especill the Pum56 used in epeimentl setup, mgnetic house shield ws built. The mteil, the shpe nd the dimensions of the shield hd to be studied. As the compss ws clindicl, tht ws the chosen geomet fo the shield. A lge sphee would be theoeticll idel, but the clinde poved to be good solution. We wnted mteil tht hd good shield effectiveness fo ll mgnetic intefeences, but pemeble to the eth mgnetic field. The fct tht the lowe fequencies e hde to shield mens tht, in the end, the eth field gets though the shield. Sevel shielding effectiveness tbles wee consulted [1] [11]. These nlsed the pefomnce of vious mteils with diffeent fequencies nd souce loctions. Steel ws found to be the moe ppopite mteil since it hs good behviou ove wide nge of fequencies. Fom the diffeent steels we selected SAE 145 (AISI stndd, o CK45 (DIN stndd, fo its ecellent popeties (µ = 1 nd σ =.1, fo its vilbilit nd low cost. The dimensions of the shielding mteil hd to be smll enough so s not to mke the piece bulk nd hev, but big enough to povide esonble shielding. The effectiveness of the shield is detemined b tee fctos, the bsoption loss A, the eflection loss R nd the thin shield coection pmete B (fo multiple eflection of mgnetic fields. Being t the thickness of the shielding mteil nd f the mgnetic field fequenc we hve S = A + R + B, i.e. S = [ ] f σ 3,34 5,4 f µ σ log( + B [ db] t ( µ Fo low fequencies, whee the shield effectiveness is lowe, the coection pmete B nd the eflection loss of the selected steel e negligible, mking the shield effectiveness detemined onl b the bsoption loss. This is diectl popotionl to the thickness of the selected mteil, nd consequentl to its skin depth δ, given b δ [ mm] 66,4 = (3 f µ σ Fo t = 5δ, the mgnetic field educes to e t δ B =,67% B. So fo 6 mm thick steel nd n outside mgnetic field of s 3H, the mgnetic intefeence inside the shield is 149 times lowe. The clinde petues dsticll educe the shield effectiveness. To void this the clinde tops hve n L coss-section t the point of contct, nd the pssge petue fo the compss wies follows the sme citei. The built shield ws 8mm high hollow clinde with 6mm wlls nd 4mm dius. The compss ws plced inside, in w tht the geometic centes of the two components mtched. The shield pefomed well in the cied out tests. The pointed noth b the flugte compss chnges, s we epected, becuse the shield is mde of feomgnetic mteil tht cuses constnt intefeence in the compss, but the imuths in espect to the new noth e well computed. The tests lso showed tht the mgnetic intefeence of the suounding envionment hd educed much, even fo ve smll distnces. So, we could hve moe confidence in the vlues obtined b the flugte compss. The C1 compss hs its own electonics contolle bod. Sevel output modes e vilble. A seil pot is lso pesent to pefom compss configution. The RS-3 pot cn lso povide heding infomtion. Fo monitoing ngul motion long the is the sstem led hs goscope. But since the go edings e pone to dift, the compss is clled upon peiodicll to coect the dift. The nlog nd digitl diect outputs povided b the compss e fste, but fo ou ppliction, the seil pot did just fine Getting ll sensos togethe The sensos hd to be ssembled so s to be popel ligned eltive to ech othe nd esil ttched to the vehicle, o in the cse of the test pptus, the Pum56 obot m. The wee theefoe mounted inside cube with suitble geomet fo etcting the elevnt dt s cn be seen in Fig. 5. It is mde of 8mm thick clic, hving 1 mm inne side. The clic mteil ws light enough to be ttched to the end of the Pum56 obot m, nd igid enough fo holding the sensos t ppopite loctions.

4 db5 connecto cceleomete pum fitue db9 connecto Sensos Filtes A/D Intel 8C196KC µ Contolle 56 KB RAM Memo 8 KB EPROM ICSensos ICSensos Lucs AccuSt Lucs AccuSt cceleomete flugte electonics contolle bod clinomete goscope clinomete Encodes Incementl encode intefce out FIFO PC Bus in FIFO Fig.5. Flttened lout of the cube. Since the sensos sensing point couldn't ll coincide, the hd to be plced t convenient loctions. The goscope, povided it is coectl ligned with the -bod-is, will lws mesue the coect ngul speed. But the cceleometes, being w fom the cente of the cube, will be subject to centifugl cceletion. This cn be compensted fo, povided the senso is coectl ligned nd the distnce to the cente is known. The cube povided modul nd compct w of potecting the sensos, s well s housing ll the connecting cbles, while t the sme time plcing the sensos long the desied is. The flugte compss ws plced inside it's mgnetic shielding clinde nd ttched to the bottom of the cube, s cn be seen in Fig. 8. The compss electonics bod ws plced inside the cube. The cube is connected b multicoe cble to the contolle pcbod nd b RS-3 cble to the pesonl compute seil pot The pocessing hdwe Fig.6. Achitectue of the contolle pcbod. The sensos output e led in convenient fomt, needing onl nti-lising nlog filtes. An etenl A/D convete ws used, enbling fste nd highe pecision smpling thn the 8C196KC intenl A/D, which ws left fo mesuing less demnding signls. The mico-contolles ddess spce is onl 64KB, in ode to epnd it, windowing scheme ws used. The EPROM stoes the onbod softwe. To communicte with the PC two FIFOs wee used. This llowed the onbod pocessing to continue without hving to wit fo the pesonl compute. In development stges the softwe cn be downloded fom the pesonl compute to the onbod memo, using the EPROM to stoe onl simple monitoing pogm. Although not pt of the cuent sstem, n encode intefce ws included in the design. This ws done to etend the nge of pplictions wee the bod could be used. The flugte compss hs its own electonics bod with RS-3 pot tht diectl connects to the PC, s shown in Fig. 7. To hndle the dt cquisition, nd lso enble some pocessing, mico-contolle bsed cd ws built. The cd is designed to wok in pesonl compute nd hs n EISA bus intefce. The 16-bit 8C196KC mico-contolle ws chosen, since it povided the necess contol functions s well s esonbl fst dt pocessing to implement the lgoithms. The bod povides powe to the sensos nd cquies the elevnt dt, with ppopite filteing nd smpling. Figue 6 shows its chitectue. cceleomete cceleomete clinomete clinomete goscope flugte compss pcbod with filteing nd A/D Fig.7. Lout of the sstem. PCbus Sei1 pot PC. Positioning Algoithms The sstem is still unde development, nd the fist lgoithm tied out is simple solution fo D nvigtion. A pelimin clibtion is done b plcing the cube t levelled plne. The sensos' offsets e then mesued, poviding eo-point efeence fo futue edings. Initill the cube is t est nd the clinometes give the diection of g eltive to the bod. With α nd α being the sensed ngles b the nd -is clinometes, we hve

5 g g = sinα sinα 1 sin α sin α given in bod fme of efeence. Povided tht ll futue movements, until the net sstem eset, e done long D plne, we cn estimte the bod's position with the dt given b the cceleometes nd the goscope. The cceleomete will pick up some centifugl cceletion due to the ottion ω bout the bod -is. This cn be compensted fo, since ω nd the dius e known. This combintion is given b = + ω (5 bod sensed fo both nd -is cceleometes. To keep tck of the ottion ound the -is, the ngul velocit ω sensed b the goscope must be integted. Defining θ s the cuent -is ngle eltive to the oiginl oienttion we hve = T ω (4 θ dt (6 Ou cuent sstem onl dels with ottions bout the bod is. The initil diection of g is known fom (4. To keep tck of it, we must updte it to the otted bod fme of efeence. Assuming ottion ngle θ, g cosθ sinθ sinα = sinθ cosθ sinα (7 g 1 sin α sin α 1 The cceletion epeienced b the bod will be the sum of gvit with the tue cceletions, i.e. = g (8 tue sensed As long s no o -is ottions occu (limited to D plne, we cn septe the two, since the diection of g is known fom (7 nd g is constnt ( g = g 9.81m / s. The tue bod cceletions e then given b tue tue tue = = bod bod -g -g ( sinα cosθ + sinα sinθ ( sinα sinθ sinα cosθ = (ssumed (9 To convet to the nvigtion fme, defined b the bod's initil ttitude nd position (lst eset, we need to otte the tue cceletion vecto b θ. nv cos = sin ( θ sin( θ ( θ cos( θ 1 tue tue (1 To obtin bod position we onl need to integte (1. Assuming initil est conditions, nv = nv dt with = [ ] nd = [ ] nv t = T v. (11 We will theefoe hve the finl position nd ttitude of the bod given b nv = (, nd θ (1 eltive to initil position. Fo movements long plnes close to the level plne, the flugte compss cn be used to compenste the ccumulted dift in θ.when the diffeence between the two estimtes eceeds peset vlue, the compss eding is tken s the coect one. But if the compss eding shows n bupt chnge, we keep the vlue given b the goscope integtion, since the flugte cn be ffected b bump cceletions. An bsolute eset on the ccumulted dift eo is theefoe povided..3 Epeimentl Setup The cube ws ttched to the end of Pum56 obot m (see. Fig. 8, enbling the movement of the sensos long known tjecto (velocit contol. A softwe pckge ws mde to povide set of tools fo development nd testing of the inetil nvigtion sstem. The softwe (INERwokbench pocesses dt off-line, tht is, the tjecto contol nd dt cquisition is done in el time but the lgoithms e tested off-line, using the known tjecto to evlute thei ccuc. The INERwokbench llows the genetion of composed tjectoies, specifing the pmetes fo ech pth section, nd thei tiggeing nd contol in el time. nv Fig.8. Epeimentl setup. The Pum56 contol ws done using the el-time lte mode [1]. A pcbod pefoms low level communiction potocol with

6 the pum contolle. A RS-3/RS-4 intefce inteconnects the two. The cquied dt cn be viewed, stoed nd etieved, nd lgoithms tested b dding modules to the INERwokbench witten in the *.m mtlb fomt. 3. Conclusions The ticle pesented n inetil sstem fo lnd vehicles. The sensos, thei mechnicl mount nd the pocessing pcbod wee descibed. The chcteistics nd the functionlit of ech senso wee nled. Pticul emphsis ws given to the design of mgnetic shield fo the flugte compss. The lgoithm, unde testing, ws designed fo use in vehicles moving in D plne. This kind of nvigtion is well dpted fo mobile pltfoms. Futue impovements e plnned fo the sstem, such s the stud of its eos nd the impovement of the nvigtion lgoithm using klmn filteing techniques. [1] H. W. Ott, "Noise Reduction Techniques in Electonic Sstems", John Wile & sons, nd edition, pges 159-1, [11] D. R. J. White, "Electomgnetic Shielding Mteils nd Pefomnce", Don White Consultnts, nd edition, 198. [1] Unimtion Incopoted, "Pt 3 - Rel-time Pth Contol, Pogmming Mnul", Use's Guide to VAL II 398T1, Refeences [1] T. Viéville nd O. D. Fuges, "Computtion of Inetil Infomtion on Robot", Fifth Intentionl Smposium on Robotics Resech, Hiofumi Miu nd Suguu Aimoto edito, MIT-Pess, pges 57-65, [] B. Bshn, "Oienttion Estimte fo Mobile Robots using Goscopic Infomtion", Poc. IROS'94, pges , [3] K. Komoi nd E. Om, "Position Estimtion of Mobile Robot Using Opticl Fibe Goscope (OFG", Poc. IROS'94, pges , [4] K. Rintnen, I. Kuppi, K. Koskinen nd A. Koskinen, "Inetil Nvigtion fo Mobile Robots b Redundnt Low-cost Gometes". [5] S. Coope nd H. Dunt-Whte, "A Klmn Filte Model fo GPS Nvigtion of Lnd Vehicles", Poc. IROS'94, pges , [6] Y. Tonouchi, T. Tsubouchi nd S. Aimoto, "Fusion of Ded-eckoned Positions with Wokspce Model fo Mobile Robot b Besin Infeence", Poc. IROS'94, pges , [7] Mut MFG. Co, Ltd, "Pieoelectic Vibting Goscope Gost", Ct.No.S34E, [8] T. J. Petes, "Automobile Nvigtion Using Mgnetic Flu-Gte Compss",IEEE Tnsctions on Vehicul Technolog, pges 41-47, VOL. VT-35, No., M [9] KVH Industies, Inc, "C1 Compss Engine Technicl Mnul", KVH Pt No , Revision G, M 1993.