Post mission trajectory smoothing for the Isurus AUV

Pos Mission T ajec o y Smoo hing o he Isu us AUV Aníbal Ma os, Nuno C uz and Fe nando Lobo Pe ei a Faculdade de Engenha ia da Uni e sidade do Po o Ins i u o de Sis emas e Robó ica R. D . Robe o F ias 4200-465 Po o Po ugal {anibal, nac uz, lp}@ e.up.p Abs ac – In his pape we desc ibe an algo i hm ha p oduces a pos mission es ima e o he spa ial e olu ion o he Isu us AUV. To make his pos p ocessing possible, he na iga ion sys em eco ds on he ehicle logging sys em all he na iga ion da a ecei ed du ing he mission execu ion. The da a comp ise he dep h o he ehicle, he ou pu s o he il senso s and digi al compass, he angula eloci y o he p opelle , as well as acous ic ange measu emen s o a se o ansponde s. A e mission comple ion, he logged da a is hen p ocessed o p oduce he es ima e o he e olu ion o he ehicle. The algo i hm used o p ocess his da a is based on a ixed in e al nonlinea s ochas ic smoo hing scheme and p oduces an es ima e ha e ol es con inuously in ime. Fo each ins an o ime, he pos mission posi ion es ima e is based on all he in o ma ion collec ed du ing he mission, as opposed o eal ime es ima es ha can only ake in o accoun pas da a. I. INTRODUCTION Isu us is an AUV ope a ed and pa ially de eloped a he Unde wa e Sys ems and Technology Labo a o y (LSTS) om Po o Uni e si y. This ehicle has been pe o ming se e al missions o he las ew yea s in di e en ope a ional scena ios, mainly collec ing CTD and ba hyme ic da a. A e he execu ion o each mission, he collec ed da a is p ocessed acco ding o he inal pu pose o he mission. In almos all he cases, i is necessa y o loca e he collec ed da a bo h in ime and in space. I ime loca ion is a i ial ask ( he ehicle ca ies a eal ime clock), he spa ial loca ion equi es in o ma ion ela ed o he na iga ion sys em o he ehicle. The main unc ion o he na iga ion sys em o an AUV is o p o ide an es ima e o he ehicle posi ion in eal ime. In he case o Isu us, he na iga ion sys em ecei es dep h da a om a p essu e cell, a i ude da a om an in eg a ed se o il senso s and digi al compass, and eloci y da a om an encode coupled o he p opelle . Absolu e posi ioning da a is p o ided by ime o ligh based ange measu emen s o a se o acous ic ansponde s deployed in he ope a ion a ea. This sys em employs a Kalman il e based p ocedu e o ob ain a eal ime posi ion es ima e. This es ima e is compu ed by ime in eg a ing he ehicle eloci y and co ec ing he in eg a ed alue wi h ange measu emen s om he acous ic sys em. Al hough his es ima e is accu a e enough o he guidance and con ol o he ehicle, i is no well sui ed o spa ially loca ing he da a collec ed by he AUV. In ac , he disc e e na u e o he co ec ion mechanism gi es ise o discon inui ies in he e olu ion o he es ima e, which is a majo d awback o spa ially loca ing he collec ed da a. This pape p esen s an algo i hm ha p ocesses he da a collec ed by he ehicle na iga ion sys em du ing he execu ion o a mission and p oduces an es ima e o he ajec o y desc ibed by he ehicle mo e sui ed o spa ially loca ing he oceanog aphic da a ga he ed. This es ima e e ol es con inuously in ime and is compu ed using echniques based on a nonlinea s ochas ic smoo hing scheme. The pape is o ganized as ollows. In sec ion II we desc ibe he Isu us AUV. Then we desc ibe he Isu us na iga ion algo i hm ha uses in eal ime he dead eckoning da a oge he wi h absolu e posi ion in o ma ion. In sec ion IV we p esen he pos mission ajec o y smoo hing algo i hm. Finally, we p esen expe imen al esul s ha show ha he pos -mission smoo hing algo i hm can g ea ly imp o e he posi ion es ima e gi en by he on-boa d na iga ion sys em, mainly when he ehicle mo es in u ns o when ange measu emen s a e sca ce. II. THE ISURUS AUV Au onomous Unde wa e Vehicles cons i u e powe ul and e ec i e ools o unde wa e da a ga he ing. These ehicles ope a e wi h no ex e nal di ec con ol, ca ying a se o ele an senso s o cha ac e ize he unde wa e en i onmen and o loca e hemsel es. Isu us (Fig. 1) is a REMUS (Remo e En i onmen Measu ing Uni S) class AUV, buil by he Woods Hole Oceanog aphic Ins i u ion, MA, USA, in 1997. These ehicles a e low cos , ligh weigh AUVs specially designed o coas al wa e s moni o ing [1]. The educed weigh and dimensions makes hem ex emely easy o handle, equi ing no special equipmen o launching and eco e y. Fo he pas 6 yea s, he LSTS has been cus omizing Isu us, and pe o ming oceanog aphic and en i onmen al moni o ing missions wi h 1234 i in di e en ope a ional scena ios, including es ua ies, i e dams and coas al ocean wa e s [2,3]. Fig. 1 – The Isu us AUV a he su ace Isu us has a diame e o 20 cm and is abou 1.5 me e s long, weigh ing abou 35 kg in ai . Inside he hull, se e al subsys ems ha e been imp o ed o speci ically de eloped a LSTS, con ibu ing o he con inuous enhancemen o he ehicle pe o mance and eliabili y. The maximum o wa d speed o he ehicle is 4 kno s, howe e he bes ene gy e iciency is achie ed a abou 2 kno s. A his eloci y, he ene gy p o ided by a se o echa geable Li hium-Ion ba e ies may las o o e 20 hou s (i.e., o e 40 nau ical miles). Al hough small in size, his ehicle can accommoda e a wide ange o oceanog aphic senso s, such as CTD, al ime e , sidescan sona , and op ical backsca e . Each o hese senso s can be independen ly ins alled on boa d, enabling he use o he ehicle in di e en con igu a ions acco ding o he equi emen s o each mission. The na iga ion sys em o he Isu us AUV es ima es he posi ion o he ehicle based on da a p o ided om se e al de ices and sys ems. The e ical coo dina e is ob ained om a dep h cell ins alled on he ehicle. To es ima e he ehicle ho izon al posi ion, he na iga ion sys em uses oge he dead eckoning da a wi h absolu e posi ioning da a. The dead eckoning da a is composed by he ehicle a i ude, ob ained om a digi al compass and a se o il senso s, and also by he ehicle eloci y wi h espec o he wa e , ob ained om an encode ha measu es he p opelle o a ion speed. The absolu e posi ioning da a consis s in ange measu emen s o acous ic beacons deployed in he ope a ion a ea. These beacons (Fig. 2) a e mul i- equency ansponde s and we e de eloped a he LSTS [4]. Each beacon can be con igu ed o eply wi h a signal o gi en equency when in e oga ed by ano he signal o a possibly di e en equency. Fig. 2 – Mul i- equency ansponde To ob ain a ange measu emen o a pa icula beacon, he ehicle has o send an in e oga ion signal and hen wai o and de ec he beacon eply signal. The ange is compu ed om he o e all ime o ligh o he acous ic signals exchanged [5]. III. NAVIGATION ALGORITHM Since he ehicle dep h is ob ained di ec ly om he dep h cell, he majo ask o he na iga ion algo i hm is he usion o he dead eckoning da a wi h he ange measu emen s o es ima e he ho izon al posi ion. The ins an aneous eloci y wi h espec o he wa e is ob ained by measu ing he p opelle o a ion speed and he ehicle heading, pi ch and oll. Veloci y measu emen s a e used oge he wi h ange measu emen s by a Kalman il e based algo i hm [6], aking ad an age o he cha ac e is ics o each ype o da a. On one hand, he ehicle eloci y is a ailable a a high a e, bu i s in eg a ion leads o a d i in he es ima ed posi ion. On he o he , ange measu emen s, a ailable a a lowe a e, can be noisy bu do no d i o e ime. The algo i hm upda es he es ima e o he ehicle posi ion a he same a e he eloci y is measu ed, and co ec s i whene e a new ange measu emen is a ailable, gi ing he bes es ima ed posi ion in eal- ime. The ho izon al posi ion o he ehicle is de ined by he no h (x) and eas (y) de ia ions om a base poin de ined o each ope a ion. The il e ing algo i hm is based on a simple model ha ela es he p opelle speed (ω), he ehicle pi ch (θ) and heading (ψ) angles o he ime de i a i es o he x and y coo dina es, de ined by he ollowing equa ions cos( )cos( ) x x kw = ωθ ψ+  (1) cos( )sin( ) y yk w = ωθ ψ +  (2) This model oughly cha ac e izes he ehicle mo ion in a s aigh line a a cons an speed. In he abo e equa ions, wx and wy ep esen he no h and eas componen s o he wa e cu en eloci y, espec i ely, and k is a cons an o p opo ionali y ob ained om a linea iza ion o he ela ionship be ween p opelle o a ion speed and ehicle longi udinal eloci y in s eady s a e. The na iga ion algo i hm is based on a con inuous disc e e Kalman il e , wi h he ehicle mo ion cha ac e ized by he di e en ial equa ions (1) and (2). The il e has a s a e o dimension 4, co esponding o he es ima es o he no h and eas coo dina es o he ehicle posi ion and also o he no h and eas componen s o he wa e speed. To keep he no a ion simple he il e s a e will be ep esen ed by [] x y Exyww = . The il e also keeps a ma ix wi h he co a iance o he es ima ion e o (P) ha is used o assess he quali y o he posi ion es ima e. Be ween he ecep ions o wo consecu i e ange measu emen s, he e olu ion o E and P is uled by he di e en ial equa ions 1235 cos( )cos( ) cos( )sin( ) (,,,) 0 0 x x kw k EFE ωθ ψ+   ωθ ψ+  =ωθψ=     w   (3) T FF EE PPP ∂∂ ∂∂ =⋅+⋅ + Q (4) whe e Q is a, possibly ime a ying, symme ic semi-posi i e de ini e ma ix ha ep esen s he a e o inc ease o he es ima ion e o , due o he impe ec modeling o he ehicle mo ion. Whene e a new ange measu emen is ecei ed and is alida ed, he s a e E and he co a iance ma ix P a e co ec ed acco ding o he exp essions )( * KEE −⋅+= −+ (5) E P −−+ ⋅⋅−= P H K P P (6) whe e E − and P − a e he alues o he E and P be o e he co ec ion and E+ and P+ a e hei alues a e he co ec ion; is he measu ed ange and * is he expec ed ange o he ansponde . The ma ices H and K a e, espec i ely, he jacobian o he obse a ion wi h espec o he s a e, and he Kalman gain [5,6]. IV. TRAJECTORY SMOOTHING The na iga ion algo i hm p esen ed abo e p o ides a each momen an es ima e o he ehicle posi ion. This es ima e is upda ed by wo di e en mechanisms wi h qui e di e en ea u es. The i s depends on he dead eckoning da a and is desc ibed by he di e en ial equa ions (3) and (4). I is a sampled con inuous p ocess, due o he sampling o he dead eckoning da a (pe o med a 10 Hz). The o he upda ing mechanism ac s whene e a new ange measu emen is a ailable and upda es he posi ion es ima e acco ding o he co ec ion equa ions (5) and (6). I is, he e o e, a disc e e e en p ocess. Due o he disc e e ime na u e o he co ec ion mechanism, he posi ion es ima e gi en by he na iga ion algo i hm does no e ol e con inuously (e en i he dead eckoning we e pe o med a an in ini e sampling a e). I can ha e discon inui ies a he momen s o co ec ions mo i a ed by ecep ion o ange measu emen s. Such discon inui ies a e inhe en o he na iga ion algo i hm employed as i p o ides a each ins an he bes es ima e o he ehicle posi ion a ha ins an , based on all he in o ma ion collec ed up o ha momen . While hese discon inui ies in he es ima e o he ehicle posi ion a e accep able as a as na iga ion is conce ned, hey a e a majo d awback whene e he ehicle is collec ing da a ha ha e o be spa ially loca ed, as in he case o ba hyme y missions. To o e come such di icul y we implemen ed an algo i hm o pos p ocess he da a collec ed by he na iga ion sys em du ing he execu ion o he mission. The algo i hm is applied upon he execu ion o he mission and compu es he spa ial e olu ion o he ehicle. The new posi ion es ima es a e based on he in o ma ion collec ed du ing all he mission, as opposed o he eal- ime es ima es ha can only ake in o accoun pas da a. The smoo hing algo i hm, desc ibed below, is based on he Rauch-Tung-S iebel nonlinea smoo he [6]. The smoo he has he same s a e o he eal ime il e . Le { wi h i} i 0,...N = , be he ins an s o ime a which he dead eckoning da a we e collec ed du ing he mission. To simpli y he p esen a ion, we a e assuming he e ha he ange measu emen s can only occu a he ime ins an s i. The goal o he smoo hing algo i hm is o compu e he es ima e o he s a e E and o he e o co a iance ma ix P, a he ime ins an s i, based on all he da a collec ed. Such es ima es will be deno ed by and , espec i ely. These alues a e compu ed in wo s eps. The i s s ep is essen ially he same as he eal ime Kalman il e . This s ep compu es a sequence o he es ima es o he s a e E and o he e o co a iance ma ix P, a he ime ins an s |iN 1] |iN i, each one based only on pas da a. These es ima es, deno ed by Ei and Pi, espec i ely, a e ob ained sequen ially acco ding o he ollowing p ocedu e. The di e en ial equa ions (3) and (4) a e in eg a ed in he in e al [, ii + wi h ini ial da a Ei and Pi, espec i ely, o ob ain 1i E − + and 1i P − + . I he e is no new ange measu emen a he ins an 1i + , hen 1i E1i E − + + = and 1ii PP 1 − + = 1i + + . I he e is a new ange measu emen a he ins an , hen 11ii EE + + + = and 1i1i PP + + + =, whe e 1i E + + and a e compu ed acco ding o (5) and (6). 1i P+ + In he second s ep o he smoo hing algo i hm, and a e compu ed om and (and using he alues om he i s s ep), acco ding o he equa ions |iN E|iN P 1|iN E+1|iN P+ |1| ( iN i i i N i i i EEKE Eb +) = +− φ − (7) 1 ( iiiiii i KP P Q) − =φφφ+ TT (8) |1| ( iN i i i N i i i i i PPKP PQK + =+ −φφ− T ) T (9) The ini ial condi ions a e and |NN N EE=|NN N PP = . In he abo e equa ions, is he alue a o he solu ion o he di e en ial equa ion i b1i+ F E F ∂ ∂ =+ F E Ebb wi h he ini ial condi ion ∂  ∂ − () 0 i b = , 1 (, i i ii Q + ) ()(, ) i Q d = φτ ∫τφτ τ T, and 1 (, ii +) i φ =φ , whe e (,) φ ⋅⋅ is he ansi ion ma ix associa ed o he linea ized ehicle dynamics, i.e., F E zz ∂ ∂ = . V. EXPERIMENTAL RESULTS Bo h he na iga ion and he pos mission smoo hing algo i hms desc ibed he e ha e been ex ensi ely es ed di e en scena ios o ope a ion. The es ing p og am enabled 1236 us o ine une he pa ame e s o bo h algo i hms o op imal pe o mance. Cu en ly, hese algo i hms a e employed in i ually all he missions pe o med wi h Isu us. We p esen below some esul s illus a ing he beha io o bo h he eal ime na iga ion and he pos mission smoo hing algo i hms. The da a was collec ed du ing a 2 hou s mission ha ook place 2 km o he po uguese coas nea A ei o. In his mission, Isu us collec ed CTD and ba hyme ic da a in an a ea abou 100 m by 200 m a se e al dep hs. Fig. 3 shows he eal- ime es ima e o he ehicle posi ion compu ed by he na iga ion algo i hm ( ed line) as well as he desi ed ajec o y (black line). The discon inuous na u e o he es ima ed ajec o y can be eadily obse ed in his igu e, since he e we e la ge co ec ions o he es ima ed posi ion. Typically, hese la ge co ec ions a ise jus a e he ehicle being wi hou ecei ing ange measu emen s o a long ime ( ypically mo e han 10 seconds). -100 -50 050 100 -140 -120 -100 -80 -60 -40 -20 0 eas (m) no h (m) Fig. 3 – Real ime ajec o y es ima ion I should be e e ed he e ha he la e al o se be ween he wo ajec o ies in he eas -wes lines is no he esul o a poo pe o mance o he na iga ion algo i hm, bu is a di ec consequence o he s ong wa e cu en om he no h ha was p esen du ing he mission. In ac , he heading e e ence gene a ed by he ehicle guidance sys em does no in eg a e he o ack e o , gi ing ise o la ge o ack e o s when he e a e la ge wa e cu en s pe pendicula o he desi ed ajec o y. -100 -50 050 100 -140 -120 -100 -80 -60 -40 -20 0 eas (m) no h (m) Fig. 4 – Pos mission ajec o y es ima ion Figu e 4 p esen s he es ima ed ajec o y p oduced by he smoo hing algo i hm. As expec ed, his ajec o y does no p esen discon inui ies, since each co ec ion due o a ange measu emen is dis ibu ed along he mission in e al, and is no accoun ed o a a single ins an o ime. A de ail o he wo es ima es o he ehicle ajec o y is shown in ig. 5, we e he ed line co esponds o he eal ime es ima e and he blue one o he pos mission es ima e. The wo es ima es a e e y simila when he ehicle is going in a s aigh line o is u ning slowly. When he ehicle makes igh u ns o when he e a e la ge co ec ions in he eal ime es ima e, he wo es ima es can be a ew me e s away. Since he dynamic model used he e does no desc ibe wi h su icien accu acy he ehicle mo ion du ing u ns, la ge co ec ions in he eal ime es ima e a e expec ed when he ehicle is u ning. 55 60 65 70 75 80 85 90 95 100 105 110 115 -80 -75 -70 -65 -60 -55 -50 -45 -40 -35 eas (m) no h (m) Fig. 5 – Compa ison o he wo es ima es One ad an age o using s ochas ic il e ing echniques, like he Kalman il e used in he eal ime es ima ion o he smoo hing il e used he pos mission es ima ion, is he in o ma ion p o ided by he e o co a iance ma ix P. The 2×2 sub-ma ix o P, deno ed Pxy, and co esponding o he x and y componen s o he il e s a e E, can be used o measu e he quali y o he es ima e o he ehicle posi ion. The ma ix Pxy de ines an unce ain y ellipse in he plane ha cha ac e izes he e o o he posi ion es ima e. This ellipse in de ined by 3 pa ame e s: he leng hs o i s wo p incipal axes and he o ien a ion o i s majo axis. Figu es 6 and 7 show he e olu ion o he leng hs o he axes o he unce ain y ellipse along he mission o he eal ime and he pos mission es ima es, espec i ely. The highe alues o unce ain y occu ei he when he ehicle is u ning o when ange measu emen s a e sca ce. In he eal ime case he leng hs o he axes a e mos ly be ween 1 and 2 me e s, while in he pos mission case hese leng hs a e almos always below 0.8 me e s. This illus a es he supe io accu acy o he pos mission ajec o y es ima e, as expec ed. 1237 01000 2000 3000 4000 5000 6000 7000 0 1 2 3 4 5 6 7 ime (s) unce ain y (m) Fig. 6 – Real ime unce ain y ellipse axes 01000 2000 3000 4000 5000 6000 7000 0 0.4 0.8 1.2 1.6 2 ime (s) unce ain y (m) Fig. 7 – Pos mission unce ain y ellipse axes Finally, igu e 8 shows he e olu ion o he leng hs o he majo axes o he unce ain y ellipses o bo h he eal ime es ima e ( ed line) and he pos mission es ima e (blue line), du ing a sho pe iod o he mission. The eal ime unce ain y p esen s a ypical saw- oo h beha io : he unce ain y g ows as he dead eckoning da a is in eg a ed o e ime and is “ ese ” o a low alue whene e a new ange measu emen is ecei ed by he na iga ion algo i hm. On he con a y, he unce ain y o he pos mission es ima e a ies smoo hly, e lec ing he con inui y p ope ies o his es ima e. None heless, he wo unce ain ies e ol e in a co ela ed way, since hei highe alues and hei lowe alues occu a almos he same ins an s o ime. 6050 6100 6150 6200 6250 6300 0 0.5 1 1.5 2 2.5 3 3.5 ime (s) unce ain y (m) Fig. 8 – Compa ison o he wo unce ain ies VI. CONCLUSIONS Bo h he eal ime and he pos mission ajec o y es ima ion algo i hms desc ibed in his pape ha e been success ully implemen ed and es ed unde se e al condi ions. In a i s s age, he pe o mances o bo h algo i hms ha e been e alua ed wi h da a aken om p e ious missions. A e a ew adjus men s in he pa ame e s o he algo i hms, hey we e inally alida ed in eal applica ions. The esul s ob ained so a also show ha he pos mission ajec o y smoo hing algo i hm can g ea ly imp o e he posi ion es ima e gi en by he on-boa d na iga ion sys em, mainly when he ehicle mo es in u ns o when ange measu emen s a e sca ce. This is pa icula ly ele an in he cha ac e iza ion o unde wa e ea u es, as collec ed da a has o be spa ially loca ed wi h he maximum possible accu acy. Al hough Isu us is no equipped wi h sophis ica ed dead eckoning na iga ion sys ems such as ine ial measu emen uni s o Dopple based acous ic elocime e s, he pe o mance o he pos mission algo i hm seems o be e y good, e en when he ehicle is u ning. REFERENCES [1] C. on Al , B. Allen, T. Aus in, and R. S okey, “Remo e En i onmen al Measu ing Uni s”, P oceedings o he Au onomous Unde wa e Vehicle's 94 Con e ence, Camb idge, USA, 1994. [2] N. C uz, J. Sousa, F. L. Pe ei a, A. Ma os, E. Sil a, J. Coimb a, and E. Dias, “PISCIS: Mul iple Au onomous Unde wa e Vehicles o En i onmen al and Oceanog aphic Field S udies”, P oceedings En i onmen 2010 Con e ence, Po o, Po ugal, 2003. [3] N. C uz, A. Ma os, A. Ma ins, J. Sil a, D. San os, D. Bou o , D. Fe ei a and F. L. Pe ei a, “Es ua ine En i onmen S udies wi h Isu us, a REMUS class AUV”, P oceedings o he MTS/IEEE Oceans’99 Con e ence. Sea le, USA, 1999. [4] N. C uz, L. Madu ei a, A. Ma os, and F. Lobo Pe ei a, “A Ve sa ile Acous ic Beacon o Na iga ion and Remo e T acking o Mul iple Unde wa e Vehicles”, P oceedings o he MTS/IEEE Oceans’01 Con e ence, Honolulu, USA, 2001. [5] A. Ma os, N. C uz, A. Ma ins and F. Lobo Pe ei a, “De elopmen and Implemen a ion o a Low-Cos LBL Na iga ion Sys em o an AUV”, P oceedings o he MTS/IEEE Oceans’99 Con e ence, Sea le, USA, 1999. [6] A. Gelb, Applied Op imal Es ima ion, MIT P ess, 1989. 1238