MOTION ESTIMATION BY INTEGRATED LOW COST SYSTEM (VISION AND MEMS) FOR POSITIONING OF A SCOOTER VESPA

Archves of Phoogrammery, Carography and Remoe Sensng, Vol. 22, 2011, pp. 147-158 ISSN 2083-2214 MOTION ESTIMATION BY INTEGRATED LOW COST SYSTEM (VISION AND MEMS) FOR POSITIONING OF A SCOOTER VESPA Albero Guarner 1, Ncola Mlan 2, Francesco Pro 3, Anono Veore 4 1, 2, 3, 4 CIRGEO Inerdep. Research Cener n Carography, Phoogrammery, Remoe Sensng and GIS, Unversy of Padua (albero.guarner,ncola.mlan,francesco.pro,anono.veore)@unpd. KEY WORDS: Moble Mappng, Orenaon Sensor, Image Transformaon, IMU, Compuer Vson ABSTRACT: In he auomove secor, especally n hese las decade, a growng number of nvesgaons have aken no accoun elecronc sysems o check and correc he behavour of drvers, ncreasng road safey. The possbly o denfy wh hgh accuracy he vehcle poson n a mappng reference frame for drvng drecons and bes-roue analyss s also anoher opc whch aracs lo of neres from he research and developmen secor. To reach he objecve of accurae vehcle posonng and negrae response evens, s necessary o esmae me by me he poson, orenaon and velocy of he sysem. To hs am low cos GPS and MEMS (sensors can be used. In comparson o a four wheel vehcle, he dynamcs of a wo wheel vehcle (e.g. a scooer) feaure a hgher level of complexy. Indeed more degrees of freedom mus be aken no accoun o descrbe he moon of he laer. For example a scooer can ws sdeways, hus generang a roll angle. A slgh pch angle has o be consdered as well, snce wheel suspensons have a hgher degree of moon wh respec o four wheel vehcles. In hs paper we presen a mehod for he accurae reconsrucon of he rajecory of a moorcycle ( Vespa scooer), whch can be used as alernave o he classcal approach based on he negraon of GPS and INS sensors. Poson and orenaon of he scooer are derved from MEMS daa and mages acqured by on-board dgal camera. A Bayesan fler provdes he means for negrang he daa from MEMS-based orenaon sensor and he GPS recever. 1. INTRODUCTION Deermnng he poson and orenaon of movng vehcles n real me hase become a very mporan research opc n hese las decade, wh parcular aenon o he developmen of elecronc sysems for road safey purposes. Two man resuls of he echnologcal progress n hs feld are represened by he Elecronc Sably Program (ESP), an evoluon of he An-Blockng Sysem (ABS), and saelle posonng of vehcles. In he auomove secor, due o lmed budges and szes, navgaon sensors rely on he negraon beween a low cos GPS recever and an Ineral Measuremen Un (IMU) based on MEMS(Mcro-Elecro-Mechancal Sysem) echnology. Such negraon s commonly realzed hrough an exended Kalman fler (Q and Moore, ), whch provdes opmal resuls for offses, drfs and scale facors of employed sensors. However he applcaon of hs fler o moorcycle dynamcs does no perform smlarly. Unlke cars, moorcycles are able o roae around s own longudnal axs (roll), bendng on he lef and on he rgh. Therefore he yaw angular velocy s no measured by jus one sensor, 147

Albero Guarner, Ncola Mlan, Francesco Pro, Anono Veore raher s he resul of he measuremens of all hree angular sensors, whch conrbue dfferenly n me accordng o he curren lng of he moorcycle. Consequenly, an error on he esmae of he roll angle a me wll affec he esmae of he pch and yaw angles a nex me +1, as well. In hs paper we consder he problem of deecng he poson and orenaon of a popular Ialan scooer, Vespa, usng a low cos POS (Posonng and Orenaon Sysem) and mages acqured by an on-board dgal vdeo camera. The esmae of he parameers (poson n space and orenaon angles) of he dynamc model of he scooer s acheved by negrang n a Bayesan parcle fler he measuremens acqured wh a MEMS-based navgaon sensor and a double frequency GPS recever. In order o furher mprove he accuracy of orenaon daa, roll and pch angles provded by he MEMS sensor are pre-flered n a Kalman fler wh hose compued wh he applcaon of he cumulaed Hough ransform o he dgal mages capured by a vdeocamera. The paper s organzed as follows. In he nex Secon we presen an overvew of he Whpple model (Whpple, 1899), whch consues he mahemacal bass of he dynamc model of he moorcycle. Then n Secon 3 we show how he roll angle can be esmaed from he mages recorded by he vdeo-camera usng he he cumulaed Hough ransform, as dscussed n Frezza and Veore (2001) and smlarly n Nor and Frezza (2003). In Secon 4 we dscuss he use of a Bayesan fler model o negrae MEMS sensor daa wh GPS measuremens, whle n Secon 5 we provde a shor descrpon of he sysem componens. Fnally n Secon 6 we presen some expermenal resuls and draw he conclusons. 2. THE WHIPPLE MODEL The Whpple model essenally consss n a nverse pendulum fxed n a frame movng along a lne wh deal wheels whch are consdered o be dscs wh no wdh (fgure 1). The vehcle s enre mass m s assumed o be concenraed a s mass cener, whch s locaed a hegh h above he ground and dsance b from he rear wheel, along he x axs. The parameer δ denoes he seerng angle, ψ he yaw angle, ϕ he roll angle and w s he dsance beween he wo wheels. In hs model he moorcycle moon s assumed o be consraned so ha no laeral slp of he res s allowed (non-holonomc consran). Furhermore doesn ake no accoun he movemen of a drver neher he oscllaon of he scooer s wheel suspensons. The moon equaons are herefore descrbed by: x& = v cosψ cosθ y& = v snψ cosθ z& = v snθ (1) where x, y and z represen he real-me vehcle posons n he spaal frame, v s he forward velocy, and θ s he pch angle (no shown n fgure 1). From he geomery of he sysem he rae of change (.e. frs dervave) of he yaw angle s defned as follows: 148

Moon esmaon by negraed low cos sysem (vson and MEMS) for posonng of a scooer an δ v ψ& = v = = σ v w cosϕ R (2) where σ s he sananeous curvaure of he pah followed by he moorcycle n he xy plane and R s he sananeous curvaure ray (σ = R -1 ). Fg. 1: The nvered pendulum moorcycle model (couresy of Lmebeer & Sharp, 2006) Accordng o he nvered pendulum dynamcs, he roll angle sasfes he followng equaon: h&& g h v b&& 2 ϕ = sn ϕ (1 + σ sn ϕ ) σ + ψ cosϕ (3) where g s he acceleraon due o gravy. The erm hσ snϕ can be rewren as a funcon of he seerng angle δ and he roll angle ϕ: h hσ sn ϕ = an δ an ϕ w (4) and gven ha angles δ and ϕ do no smulaneously assume hgh values, he erm hδ snϕ can be negleced. Therefore, akng no accoun also equaon (2), equaon (3) becomes: h&& g v b v& v & 2 ϕ = sn ϕ σ + ( σ + σ ) cosϕ (5) Assumng ha we can measure he roll angle ϕ(), he pch angle θ and he velocy v(), equaon (5) could be used o esmae he curvaure σ. Indeed, by negrang equaon (5) we can compue he sananeous curvaure σ(), provded ha an nal condon σ(0) s gven. Smlarly, knowng he profle σ, f we negrae he nonholonomc knemac model (1) from an nal poson (x(0), y(0), z(0)) he pah followed by he moorcycle can be 149

Albero Guarner, Ncola Mlan, Francesco Pro, Anono Veore fully reconsruced. In nex secons we wll dscuss how we esmae he parameers φ, θ and v, whose knowledge represens he key pon of he proposed mehod. 3. ESTIMATING THE ROLL & PITCH ANGLES Assumng ha he camera s srongly fxed o he moorcycle, roll and pch angles can be esmaed by deecng he poson n he mage (slope and dsance from he mage orgn) of he horzon lne. I can be proved ha usng he perspecve projecon camera model, he horzon lne projeced ono he mage plane can be descrbed n erms of roll and pch angles as follows (see Nor and Frezza, 2003, for deals): cosθ cosϕ V sn ϕ U = snθ cosϕ where (U,V) denoe he mage plane coordnaes of a pon P wh coordnaes [x,y,z] n he camera frame Σ c. Therefore, he pch and roll angles θ and ϕ can be deermned knowng he poson of he horzon lne n he mage. Despe ha he horzon canno be easly deermned due o occlusons frequenly occurrng n he scene, roll and pch raes can be robusly esmaed by comparng wo consecuve mages. Indeed, gven he horzon lne n he frame a me, I( ), n he nex frame a me +1, I( +1), he horzon s descrbed by he followng relaonshp: cos ( θ + θ ) cos ( ϕ + ϕ ) V sn ( ϕ + ϕ ) U = sn ( θ + θ ) cos ( ϕ + ϕ ) (6) Lnearzng (6) abou θ() and ϕ(), neglecng erms of order hgher han one n and assumng small pch angles (θ 0), we oban sn ϕ ϕ V + cosϕ ϕ U = θ cosϕ (7) Equaon (7) shows ha n wo successve frames, he horzon roaes by ϕ and ranslaes by θ cosϕ. These wo quanes ( ϕ, θ) can be measured by compung he Hough ransform on a regon of neres cenered around a neghborhood of he curren esmaon of he horzon lne. The Hough ransform (Duda and Har, 1972) s a feaure exracon echnque used n mage analyss, compuer vson, and dgal mage processng, whose purpose s o fnd mperfec nsances of objecs whn a ceran class of shapes by a vong procedure. Ths vong procedure s carred ou n a parameer space, from whch objec canddaes are obaned as local maxma n a so-called accumulaor space ha s explcly consruced by he algorhm for compung he Hough ransform. In hs case hs ransform s used o deermne he horzon lne n he mages acqured by he scooer s on.board vdeo-camera. To hs am he parameer space s defned by he polar coordnaes (ρ,α), whch are relaed o he mage coor-dnaes (U,V) as follows (fgure 2): ρ = U cosα + V sn α (8) 150

Moon esmaon by negraed low cos sysem (vson and MEMS) for posonng of a scooer Fg. 2: The parameer space (ρ,α) of he Hough ransform adoped for lne deecon. Gven hs paramerzaon, pons n parameer space (ρ, α) correspond o lnes n he mage space, whle pons n he mage space correspond o snusods n parameer space, and vceversa (fgure 3). The Hough ransform allows herefore o deermne a lne (e.g. he horzon) n he mage as nersecon, n parameer space, of snusods correspondng o a se of co-lnear mage pons. Such pons can be obaned by applyng an edge deecon algorhm. Fg. 3: Image pons mapped no he parameer space. The seps needed o compue he raes ( ϕ, θ) can be summarzed as follows: 1) apply an edge deecon o a predefned regon of neres of he mage; 2) perform a dscrezaon he parameer space (ρ, α) by subdvdng n a se of cells (bns); 3) consderng ha each edge canddae s an nfnesmal lne segmen of polar coordnaes (ρ, α), he number of edges fallng n each bn s couned; 4) hrough hs accumulaon an hsogram of an mage n coordnaes (ρ, α) s generaed, whose nensy values are proporonal o he number of edges fallng n each bn. Ths hsogram represens he Hough ransform H(ρ, α) of he mage. 151

Albero Guarner, Ncola Mlan, Francesco Pro, Anono Veore 5) from each hsogram he correspondng cumulaed Hough ransform s derved. Ths ransform s a modfcaon of he Hough ransform and s defned as follows: H E ( α ) = H ( ρ, α ) (9) for he roll angle (α = ϕ), whle for he pch angle (α = θ) becomes: H E ρ E ( ρ ) = H ( ρ, α ) (10) An example of he cumulaed Hough ransform s shown n fgures 4 and 5. 6) I can be proved ha f he same edges are vsble a me and +, hen for he roll angle (and smlarly for he pch angle) holds ha α E [ ) H E ( + ) ( ϕ) = H E ( ) ( ϕ + ϕ( )) ϕ 0, π (11) In presence of nose and consderng ha no all edges vsble a me reman vsble a me +, a good esmaon of ϕ( ) can be obaned mnmzng he Eucldean dsance beween each of he cumulaed ransforms a me and + : ( ) ( ) ϕ( ) = arg mn H ρ, α α dρ H ρ, α d ρ (12) α + Smlarly, he esmae of he ncremen of he roll angle θ s compued as follows: θ 1 + dα (13) cosϕ ( ) = arg mn H ( ρ ρ, α ) dα H ( ρ, α ) Afer hese seps, he esmaes of he roll and pch angles are compued by me negraon of he raes ϕ and θ. 152 (a) (b) Fg. 4: Image acqured form he on-board camera (a); edge deecon of he horzon lne (b).

Moon esmaon by negraed low cos sysem (vson and MEMS) for posonng of a scooer (a) (b) Fg. 5: Hough ransform obaned from he se of edges n fgure 4b (a); correspondng cumulaed Hough ransform (b). 4. THE BAYESIAN PARTICLE FILTER The key pon of all navgaon and rackng applcaons s he moon model o whch bayesan recursve flers (as he parcle fler) can be appled. Models whch are lnear n he sae dynamcs and non-lnear n he measuremens can be descrbed as follows: x = Ax + B u + B f +1 u f y = h(x ) + e (14) where x s he sae vecor a me, u he npu, f he error model, y he measuremens and e he measuremen error. In hs model, ndpenden dsrbuons are assumed for f, e and he nal sae x 0, wh known probably denses p e, p f and p x0, respecvely, bu no necessarly Gaussan. We denoe he se of avalable observaons a me as Y = { y 0,..., y } (15) The Bayesan soluon o equaons (14) deals wh he compung of he a pror dsrbuon p(x +1 Y ), gven pas dsrbuon p(x Y ). In case nose pdfs (probably densy funcons) are ndpenden, whe and gaussan wh zero mean, and h(x ) s a lnear funcon, hen he opmal soluon s provded by he Kalman fler. Should be hs condon no sasfed, an approxmaon of he a pror dsrbuon p(x +1 Y ) can be sll provded usng a Bayesan parcle fler. Ths fler s an erave process by whch a collecon of parcles, each one represenng a possble arge sae, approxmaes he a pror probably dsrbuon, whch descrbes he possble saes of he arge. Each parcle s assgned a wegh w, whose value wll ncrease as closer o rue value he relaed sample wll be. When a new observaon arrves, he parcles are me updaed o reflec he me of he observaon. Then, a lkelhood funcon s used o updaed he weghs of he parcles based on he new nformaon conaned n he observaon. Fnally, resamplng s performed o replace low wegh parcles wh randomly perurbed copes of hgh wegh parcles. More deals 153

Albero Guarner, Ncola Mlan, Francesco Pro, Anono Veore abou he Bayesan parcle fler can be found n (Gusafsson e al., 2001). A block dagram of he parcle fler s presened n fgure 6. Snce he compuaonal cos of a parcle fler s que hgh, only an adequae mnmum number of varables has been ncluded n he dynamc model of he scooer. I was herefore chosen o neglec any movemen along he z axs (e.g. bouncng of suspensons), and o accoun for poson varables x and y, speed v, he hree angles needed for modellng he orenaon (φ,θ,ψ) and he flered verson of he curvaure δ. In order o furher mprove he accuracy of orenaon daa, roll and pch angles provded by he MEMS sensor are combned and pre-flered n a Kalman fler wh hose compued usng he cumulaed Hough ransform appled o he dgal mages capured by a vdeo-camera. Assumng ha he sysem s now represened as a collecon of N parcles, he dynamcs of he generc parcle s (.e. a possble sysem sae) s descrbed by he followng model: where T s he samplng nerval; 2 x + 1 = x + v cos( ψ ) cos( θ ) T + N (0, x ) 2 y + 1 = y + v sn( ψ ) cos( θ ) T + N (0, y ) 2 v + 1 = v + ( a g cos( θ ) T + N (0, v ) 2 σ f v ϕ + 1 = (1 γ )( ϕ + & r ϕ ) + γ arcan T r g θ + 1 = θ + θ& T 2 ψ + 1 = ψ + ψ& T + N (0, ψ ) ψ σ f = (1 γ ) σ + γ + 1 s f s v w P ( p ) w + 1 = N j ( ) w j = 1 P p N(0, x 2 ) represens he measuremen nose of he X coordnae, modelled as a Gaussan funcon wh a zero mean and sandard devaon x. Smlar assumpon holds for measuremen noses N(0, y 2 ), N(0, v 2 ) and N(0, ψ 2 ); σ s he weghed combnaon of he curvaure esmaed a prevous me f +1 (σ ) and f he curren npu ψ v, beng γ s he weghng erm (γ s = 1/10); w s wegh of he -h parcle; P( s ) s he mporance funcon,.e. he lkelhood funcon hrough whch he weghs are updaed accordng o he followng relaonshp: (16) = w + 1 w P( y x ) (17) 154

Moon esmaon by negraed low cos sysem (vson and MEMS) for posonng of a scooer γ s a coeffcen whch dynamcally changes n order o gve more wegh o mnmal r curvaures and roll angular veloces as denoed by: ( )( ) γ σ σ & ϕ & ϕ σ < σ & ϕ < & m l f l f f ϕ l and l γ r = 0 oherwse where σ l and ϕ& l are he hresholds for he maxmum curvaure and roll angular velocy respecvely. We se γ m = 1/500, σ l = 1/100 m -1 and ϕ& l = 30 /s. (18) Fg. 6: Block dagram of he Bayesan parcle fler. In he model equaons (16) we used dfferen formulas for he dervaves of he orenaon angles ϕ&, & θ and ψ&. Ths s due o he fac ha he angular veloces (ω x, ω y, ω z ) measured by he MEMS sensor are relaed o he body frame (.e. he coordnae sysem fxed wh he scooer) whle orenaon angles (φ,θ,ψ) are deermned n a world reference frame (e.g. he GPS coordnae sysem, WGS-84). A frame ransformaon from he body o he world frame s herefore needed, whch leads o dfferen equaons for he orenaon angles. The componens of he sae vecor a me are hen compued as weghed average of he varables esmaed by he fler, usng he weghs w of all parcles s : x y v ϕ = θ ψ σ N = 1 x y v w ϕ θ ψ σ f (19) 155

Albero Guarner, Ncola Mlan, Francesco Pro, Anono Veore In order o lm he compuaonal effor of he fler, he updae of he parcle weghs w s no performed a every sep of he algorhm, bu raher when he GPS daa are avalable from he recever. 5. SYSTEM COMPONENTS The mehod for he moon esmaon of a moorcycle descrbed n prevous secons has been esed on an alan scooer, Vespa, whch was equpped wh a se of navgaon sensors as shown n fgure 7. The sysem consss of a Novael DL-4 double frequency GPS recever, an XSens MT-G MEMS-based neral sensor and a 1.3 Megapxel SONY Progressve Scan color CCD camera. Daa acquson and sensor snchronzaon was handled by a Noebook PC (Acer Travelmae) provded wh 1024 MB of RAM and a CPU processng speed of 1.66 GHz. (a) (b) Fg. 7: Sde vews of he Vespa scooer showng he daa acquson sensors. The dgal vdeo camera was placed on he rgh boom sde of he moorcycle (a), whle baery and navgaon sensors on he back rack (b). (a) Fg. 8: The Hough ransform (a) of an mage acqured durng a drve es (b). (b) 156

Moon esmaon by negraed low cos sysem (vson and MEMS) for posonng of a scooer 6. RESULTS AND CONCLUSIONS Three drve ess were carred ou on he same rack n order o evaluae he measuremen repeaably, whose resuls for he rol angel are shwon n fgure 9. A slgh dfference can be observed for es 3 where he speed was slower han for he oher ess. Roll angle ( ) -40-20 0 20 40 Tes 1 Tes 2 Tes 3 0 100 200 300 400 Tme (s) Fg. 9: Roll angle profles compued over hree ess. The reconsrucon from he daa receved durng navgaon n he es rack and measures done n real me allowed he recordng of roll and pch angles whch are coheren wh each oher. Employng he measuremens generaed by a smulaor, opmal reconsrucon can be acheved due o he absence of nose comng from vbraons, offses and scale facors. The smulaon of hese error sources s no necessary as he pre-fler sage of he mehod wll remove hem and hus smulang hem would no have been of neres. Oher more neresng sources of error whch have o be esed are wrong nal condons and noses of he roll and pch angles. Of neres was he roll angle, whch was brough o more han 20 o es he performance of he fler. The algorhm was able o converge, slowly, owards he real angle. Fg. 10: Trajecory esmaed wh he Bayesan parcle fler (doed lne) overmposed ono he dfferenally correced GPS reference rack (sold lne). 157

Albero Guarner, Ncola Mlan, Francesco Pro, Anono Veore Developmens of he proposed mehod wll deal wh he encodng of he Bayesan fler nsde an negraed sysem whch can be used o equp he Vespa scooer. Ths can lead n he fuure o provde even moorcycles wh racon conrol sysems. Furher developmens wll be he ncluson n he dynamc model of he suspensons moon along he Z axs, and also he sudy of he nfluence of he seerng angle (δ) on he esmaon of he roll angle. These wo parameers are ndeed relaed by he followng relaonshp, whch can be easly derved from equaon (2): anδ ϕ = arc cos Wσ (20) REFERENCES Bevly, D.M. Cobb S. 2010. GNSS for Vehcle Conrol. Arech House, Boson USA. Duda R.O., Har P. E. 1972. Use of he Hough Transformaon o Deec Lnes and Curves n Pcures. Communcaons of he ACM, 15, pp. 11 15. El-Shemy N., Schwarz K.P. 1994. Inegrang dfferenal GPS recevers wh an INS and CCD cameras for moble GIS daa collecon. In: proceedngs of Commsson II Symposum, 6-10 June, Oawa Canada, pp. 241-248. Frezza R., Veore A. 2001. Moon esmaon by vson for moble mappng wh a moorcycle. In: 3 rd Inernaonal Symposum on Moble Mappng Technology, Caro, Egyp, 3-5 January. Gusafsson F., Gunnarsson F., Bergman N., Forssell U., Jansson J., Karlsson R., Nordlund P. J. 2001. Parcle Flers for Posonng, Navgaon and Trackng. In: IEEE Transacons on Sgnal Processng, vol. 50, no. 2, pp. 425-437. Nor F., Frezza R. 2003. Accurae reconsrucon of he pah followed by a moorcycle from on board camera mages. In: IEEE Inellgen Vehcles Symposum, pp. 260-264. Lmebeer D.J.N., Sharp R.S. 2006. Bcycles, moorcycles and models. Conrol Sysems Magazne IEEE 26(5), pp. 34-61. Q H., Moore J. B. 2002. Drec Kalman Flerng Approach for GPS/INS Inegraon. In : IEEE Transacons on Aerospace and Elecronc Sysems, vol. 38, no. 2, pp. 687-693 Whpple, F.J.W. 1899. The sably of he moon of a bcycle. Quarerly Journal of Pure and Appled Mahemacs, 30 pp. 312-348. 158