Optimized trajectory planning and control for marine robots

FACULDADE DE ENGENHARIA DA UNIVERSIDADE DO PORTO Op imized ajec o y planning and con ol o ma ine obo s Ma ga ida Ma ia Rosas Rebelo Co eia Mes ado In eg ado em Engenha ia Ele o écnica e de Compu ado es Supe iso : Aníbal Cas ilho Coimb a de Ma os July 23, 2013 c Ma ga ida Co eia, 2013 Resumo O con olo ó imo é um as o campo de es udo com um a iado leque de aplicações al como obó ica, ae onáu ica, economia, e c. Exis e uma g ande a iedade de Veículos Au ónomos Subma inos (AUVs). A maio pa e de- les gas a a ma io ia da sua ene gia pa a se mo imen a . Assim, um bom planeamen o de caminhos é undamen al pa a aumen a a sua au onomia e alcance. Es a disse ação inse e-se na Unidade de Robó ica e Sis emas In eligen es do INESC TEC. As suas ac i idades êm como inalidade esponde à c escen e p ocu a de soluções obó icas. Pa a além do es udo e amilia ização com os á ios ins umen os de op imização dinâmica, es e abalho p e ende a a do impac o do seu uso em di e en es aplicações de obó ica aquá ica, em e mos da complexidade da sua implemen ação e uncionamen o em empo eal. Além disso, p e ende-se melho a o desempenho dos acima mencionados obo s. O algo i mo desen ol ido e/ou écnica de op imização de e ia se implemen ado em uma ou mais das pla a o mas obó icas disponí eis. Em p imei o luga , ez-se um es udo sob e como é ei a a localização do Slocum Elec ic Glide . Es e AUV segue os pon os p ede inidos da missão, com um algo i mo de Dead Reckoning aliado a um sinal pe iódico de GPS. Os dados ecolhidos numa missão do OceanSYS, ao la go da cos a de Sesimb a, em maio de 2013, são analisados e ap esen a-se uma p e isão do caminho eal desc i o po ele na sua missão. Po im, p opõe-se um algo i mo pa a es ima as co en es da água. Seguidamen e, ap esen a-se um algo i mo de planeamen o de caminhos op imizado, a a és de uma aplicação exemplo da Toolbox de O imização do MATLAB Cen al, “Finding Op imal Pa h Using Op imiza ion Toolbox”. Nes e código, o caminho ó imo é de e minado a pa i dum pon o inicial a é um pon o objec i o, usando a unção do MATLAB mincon. A unção obje i o baseia-se no cálculo do empo de iagem, endo em con a um campo ec o ial, que ep esen a as co en es no espaço de ação do eículo. Es e algo i mo oi al e ado pa a se adap a à inalidade de obó ica aquá ica. Assim o campo ec o ial e e en e ao en o oi con e ido em co en es aquá icas, as o dens de g andeza das a iá eis adap adas e o eículo em ques ão al e ado de um a ião pa a um AUV. Além disso, o algo i mo oi melho ado a a és da inco po ação de ince eza num dado núme o de pon os de e i icação p ede inidos, de o ma a consegui o melho caminho possí el, mesmo se oco e um des io da o a p e iamen e p econizada. Nes a implemen ação, a ince eza ep esen a o conjun o de posições possí eis que o eículo pode oma se, po algum mo i o (po exemplo, mudança no mapa de co en es, e os de odome- ia, e c.), não segui os pon os ó imos p e is os. Numa simulação, pon os alea ó ios den o de uma á ea de ince eza ep esen am a posição que o eículo a inge em luga do pon o ó imo cal- culado o iginalmen e. Numa aplicação eal, is o se ia dado, po exemplo, a a és dum sinal GPS (como na acima mencionada missão do glide ), ou da dis ância a algumas balizas conhecidas. i ii Es a ince eza é ambém usada pa a calcula o no o mapa de co en es, uma ez que se con- side a que uma pa e signi ica i a dos des ios do AUV é causada po uma mudança no campo de co en es. T a ado o p oblema da o imização do planeamen o do caminho pon o po pon o, conside a-se o p oblema da explo ação de á eas, com a inalidade de pe mi i o na mais e icien e o p ocesso de ecolha de dados, com o mínimo de consumo de ene gia. Pa a isso, p e ende-se passa su icien e- men e pe o de odos os pon os, o que não signi ica que seja necessá io p op iamen e a a essá-los a odos, mas simplesmen e passa à meno dis ância possí el de odos eles. Fo am es ados á ios algo i mos, usando écnicas de pa ição ó ima de in e alos na di eção ho izon al (de u u o se ão conside as di e en es di eções e a sua in luência). Ob i e am-se alguns esul ados expe imen ais mas há ainda uma quan idade signi ica i a de abalho u u o e es es que de em se ealizados pa a melho a os mé odos desen ol idos, po o ma a o ná-los mais obus os. Além disso, a implemen ação des es algo i mos numa pla a o ma obó ica o nece ia a opo unidade de uma análise de dados eais. Abs ac Op imal con ol is a wide ield o s udy wi h a di e se ange o applica ions such as obo ics, ae onau ics, economics and so on. A wide a ie y o Au onomous Unde wa e Vehicles (AUVs) is a ailable. Mos o hem spend he majo i y o i s ene gy o mo e. The e o e, a good pa h planning pe o mance is c ucial o inc ease hei au onomy and ange. This Disse a ion alls wi hin he INESC TEC Robo ics and In elligen Sys em Uni . I s ac i - i ies aim o add ess he g owing demand o obo ics solu ions. Besides he s udy and amilia iza ion wi h he di e en dynamic op imiza ion ools, his wo k is in ended o assess he impac o hei use in di e en ma ine obo ics applica ions, in e ms o he complexi y o i s implemen a ion and ope a ion in eal ime. Mo eo e i is aimed a imp o ing he pe o mance o he a o emen ioned obo s. The de eloped algo i hm and/o op imiza ion echnique should be implemen ed in one o mo e o he a ailable obo ic pla o ms. Fi s , a s udy o he Slocum Elec ic Glide ’s localiza ion is pe o med. Wi h a Dead Reckon- ing algo i hm allied wi h a pe iodic GPS signal, his AUV ollows he p ese mission poin s. The da a collec ed in one o he OceanSYS’ mission, o he coas o Sesimb a, in May 2013, a e analyzed and a p e ision o he eal pa h desc ibed by i in i s mission is de e mined. A las , an algo i hm o es ima e he wa e cu en s is p oposed. A e wa ds, an op imized pa h planning algo i hm using a MATLAB Cen al’s Op imiza ion Toolbox applica ion example, “Finding Op imal Pa h Using Op imiza ion Toolbox” is shown. In his code, he op imal pa h om an ini ial poin o an objec i e one is ound using he MATLAB unc ion mincon. The objec i e unc ion is based on he compu a ion o he a el ime, aking in o accoun a ec o ield, which ep esen s he cu en s in he ehicle wo kspace. This algo i hm was al e ed o i he ma ine obo ics scope. Hence he wind ield was con e ed in o wa e cu en s, he a iables magni udes adap ed and he ehicle a s ake changed om an ai plane in o an AUV. Fu he mo e, he algo i hm was imp o ed by inco po a ing unce ain y in a numbe o p ese check poin s, in o de o ha e he bes possible pa h e en i a de ia ion om he o iginally in ended way occu s. In his implemen a ion, he unce ain y ep esen s he se o possible posi ions ha he ehicle can ake i , o some eason (e.g. change in he cu en s map, odome y e o s, e c.), i does no ollow he p edic ed op imal poin s. In a simula ion, andom poin s wi hin an unce ain y a ea ep esen he posi ion he ehicle eaches ins ead o he o iginally compu ed op imal poin . In a eal applica ion his would be gi en by, o example, a GPS signal (as in he a o emen ioned glide mission) o he dis ance o some known beacons. This unce ain y is also used o ecompu e he cu en s’ map, since a signi ican pa o he AUV’s de ia ions is conside ed o be caused by a change in i s cu en s’ ield. Ha ing ea ed he poin o poin pa h planning op imiza ion p oblem, an a ea scanning p ob- lem is add essed, aiming a enabling he da a collec ion p ocess o be mo e e icien , wi h minimum iii i ene gy consump ion. Fo ha , he main poin is o a el close enough o all poin s, which does no mean c ossing all o hem exac ly bu simply passing by wi h he sho es possible dis ance o all o hem. Se e al algo i hms we e es ed, using op imal in e al pa i ioning echniques in he ho izon al di ec ion (In he u u e di e en di ec ions will be conside ed and hei in luence s udied). Some expe imen al esul s we e a ained bu he e is a signi ican amoun o u u e wo k and es ing which could be pe o med o imp o e he de eloped me hods and make hem mo e obus . Fu he mo e, he implemen a ion o hese algo i hms in a obo ic pla o m would p o ide one he oppo uni y o eal da a analysis. Acknowledgmen s I would like o exp ess my g ea es g a i ude o he people who ha e helped and suppo ed me h oughou his disse a ion. I am g a e ul o my supe iso , P o esso Aníbal Ma os, o his pa ien guidance, con inuous suppo and use ul c i iques o his disse a ion wo k. I would also like o hank he OceanSYS’ lab, INESC TEC and he Elec ical Enginee ing Depa men o FEUP (DEEC) o p o iding he people and esou ces which allowed me o s udy and esea ch in such a special ield as ma ine obo ics. A special hank o mine goes o my iends And é, Elisabe e, G aça, Ped o, Rica do and all o he s who helped and encou aged me in comple ing he p ojec and exchanged in e es ing ideas, hough s and made he long wo k hou s happie . Finally, I wish o hank my pa en s o hei uncondi ional suppo and encou agemen h ough- ou all my s udy yea s. This wo k wouldn’ be possible wi hou all he oppo uni ies hey ga e me. Ma ga ida Co eia i In memo y o Ma ia Odília Soa es da Cos a Rosas da Sil a Mo ei a Rebelo ii xi LIST OF TABLES Abb e ia ions and Symbols AUV Au onomous Unde wa e Vehicles AUVG Au onomous Unde wa e Vehicles Glide s GPS Global Posi ioning Sys em GUI G aphical Use In e ace MARES Modula Au onomous Robo o En i onmen Sampling MPC Model P edic i e Con ol OceanSys Ocean Sys ems G oup DR Dead Reckoning CCon igu a ion Space ARobo qVec o called con igu a ion o s a e OC-obs acle, space wi hin he C-space o all obs acles OiObs acle i COiSubse o he C-obs acle space, gene a ed by obs acle i C ee Robo ’s ee space ωsVehicle’s speed h ough he wa e x No hwa d glide ’s eloci y y Eas wa d glide ’s eloci y ω x No h wa e eloci y componen ω y Eas wa e eloci y componen imeo day ( ) Time o he day in secs UTC ddep h in me e s oll Roll in ads θPi ch in ads hHeading angle in ads, ela i e o he geog aphic no h hspeed Ho izon al speed in m/s la i ude La i ude in deg ees (WGS84) longi ude Longi ude in deg ees (WGS84) gps_s a us GPS Flag (0 means ok) gps_accu acy Es ima ed accu acy in me e s o GPS ix ∆cxiNo h displacemen om he GPS signal o he DR compu ed posi ion ∆cyiEas displacemen om he GPS signal o he DR compu ed posi ion ∆dWeigh , ep esen ing how a one is om he des ina ion dis ance_ o_end Dis ance om he cu en alid GPS eading o he des ina ion o al_dis ance To al a el dis ance newField Upda ed cu en s’ map PGPS Valid GPS eading PDR Poin de e mined by he DR algo i hm x x i ABREVIATURAS E SÍMBOLOS PGPS −PDR Di e ence be ween he es ima ed posi ion (DR) and he eal one (GPS) oldField Cu en s’ map one wan s o upda e Objec i e unc ion xPa ame e ec o hi(x) = 0 Equali y cons ain s gi(x)≤0 Inequali y cons ain s TTo al a el Time X= (x,y)Pa ame e ec o x( )x componen o he ehicle’s speed y( )y componen o he ehicle’s speed ieldx( )x componen o he wa e cu en s’ speed ieldy( )y componen o he wa e cu en s’ speed ˙x( )x componen o he o al speed o e ime ˙y( )y componen o he o al speed o e ime Vmax Maximum ehicle’s speed PX0Ini ial poin o he pa h PX Final poin o he pa h q(s)Fi s wo e ms o Taylo app oxima ion o (x)a x NNeighbo hood o x L(x,λ,σ)Lag angian λ,σLag ange Mul iplie s d E o be ween he poin he ehicle supposes i would be and i s eal posi ion P and = (x andom,y andom)Random poin P1= (xop imal,yop imal)Op imal waypoin / chekpoin newPP New pa h poin s numWaypoin s+1 To al numbe o checkpoin s plus he des ina ion poin di i−1 a el ime om check poin i−1 o he des ina ion di iT a el ime om check poin i o he des ina ion TTo al a el ime ob ained by he di ec compu a ion me hod iTime o go om check poin i−1 o check poin i au AUV’s speed ield Vec o ield speed Pcheck Checkpoin coo dina es P and −Pcheck Di e ence be ween he es ima ed posi ion and he eal one, i.e. unce ain y nlNumbe o lines npNumbe o poin s PiPoin i HjLine j Chap e 1 In oduc ion This Disse a ion alls wi hin he INESC TEC Robo ics and In elligen Sys em Uni . I s ac i - i ies aim o add ess he g owing demand o obo ics solu ions. Besides he s udy and amilia iza ion wi h he di e en dynamic op imiza ion ools, his wo k is in ended o assess he impac o hei use in di e en ma ine obo ics applica ions, in e ms o he complexi y o i s implemen a ion and ope a ion in eal ime. Mo eo e i is aimed a imp o ing he pe o mance o he a o emen ioned obo s. Finally, he de eloped algo i hm and/o op imiza ion echnique should be implemen ed in one o he a ailable obo ic pla o ms. 1.1 Goals The main goals o his disse a ion a e: •De elopmen , implemen a ion and es o ajec o y planning and con ol algo i hms o ma ine obo s. •S udy and simula ion o op imiza ion algo i hms o ele an pa ame e s such as: –goals accomplished –minimiza ion o ime o ene ge ic consump ion –maximiza ion o acqui ed da a 1.2 P oblem desc ip ion The add essed p oblem can be di ided in o wo di e en pa s co esponding o he main phases o he p ojec . The i s one co esponds o he de elopmen o a pa h planning algo i hm o an AUV (Au- onomous Unde wa e Vehicle) capable o de e mining a ou e om a s a o an end posi ion, a oiding any collision wi h obs acles, h ough an op imiza ion law. 1 2In oduc ion In a second s age, he p oblem will be he implemen a ion o he abo e men ioned algo i hm in an AUV such as a Glide o he OceanSys’ MARES. 1.2.1 P oblem cha ac e iza ion Since his is a complex p ojec , i is a good s a egy o spli i in o smalle p oblems. In he nex opics, a small desc ip ion o hose subp oblems is p esen ed: En i onmen model: Cha ac e iza ion o he ma ine en i onmen ea u es o de ine i s model and de e mine i s in luence in he ehicle’s mo emen . AUV model: Cha ac e iza ion o he cinema ic and/o dynamic ehicle ea u es o de ine i s model. T ajec o y planning algo i hm: A e inding he en i onmen and ehicle’s models he nex phase will be o de ine he ajec o y planning me hod o be used as well as he op i- miza ion unc ions ( ime, dis ance, ene gy consump ion, obs acles...). Op imiza ion p oblem: The planning algo i hm is expec ed o be based in an op imiza ion s a egy such as Model P edic i e Con ol. Fu he mo e, i will be necessa y o o mula e and sol e he esul ing op imiza ion p oblem hough me hods such as he Pon yagin Mini- mum/Maximum P inciple, he Two Poin Bounda y Value o he Hamil on-Jacobi-Bellman equa ions. 1.3 Me hodology Fo his disse a ion he ollowing me hodology was conside ed: 1. Resea ch o he exis ing me hods o obo s pa h planning 2. Resea ch o he exis ing me hods o ma ine obo ’s pa h planning and en i onmen models 3. De elopmen o an algo i hm o ajec o y planning in ma ine obo s using op imiza ion echniques 4. Implemen a ion and es ing o he men ioned algo i hm in MATLAB 5. Implemen a ion and es ing o he men ioned algo i hm in ei he he Slocum Elec ic Glide o he AUV MARES 6. Analysis o he collec ed da a 7. W i ing o he inal epo and p esen a ion o he a ained esul s 1.4 Disse a ion s uc u e 3 1.4 Disse a ion s uc u e In addi ion o he In oduc ion his Disse a ion epo has i e mo e chap e s. In chap e 2, i is desc ibed he s a e o he a o gene al pa h planning and some speci ic algo i hms o obo s in he ma ine en i onmen . Also i is p esen ed wo k de eloped wi hin he men ioned ields and inally, he ma ine obo ics’ pla o ms om he OceanSys’ g oup in which he algo i hm could be implemen ed on a e b ie ly desc ibed. In chap e 3, he da a om a Slocum Elec ic Glide mission o he coas o Sesimb a in May 2013 will be analyzed and he AUV eal pa h es ima ed. Fo ha , a combina ion o he pe iodic GPS signal wi h a Dead Reckoning algo i hm is pe o med and he esul s in e p e ed. Finally, he in luence o wa e cu en s in he Dead Reckoning algo i hm is s udied and a ough es ima e o his ec o ield is ob ained. In chap e 4, he i s implemen ed algo i hm (op imized pa h planning o an AUV, conside - ing unce ain y) is explained and he esul s o i s implemen a ion a e shown. Some conclusions a e d awn and u u e wo k is p esen ed. The nex chap e , 5, e e s o an op imized a ea scanning algo i hm, in ended o enable an AUV o scan a ce ain a ea wi h a de ined numbe o poin s o in e es . Finally, in chap e 6, some conclusions a e d awn om he wo k de eloped du ing his Disse - a ion and a discussion abou he possible u u e wo k ela ed o i is p esen ed. 4In oduc ion Chap e 2 S a e o he a In his chap e i is p esen ed he li e a u e e iew which was conside ed o be ele an o sol e he ma ine pa h planning p oblem unde s udy in his disse a ion. In sec ion 2.1 i is desc ibed he gene al pa h planning p oblem in obo ics, wi h i s global dimension - ajec o y planning, and i s local one - obs acles a oidance. In he ollowing sec ion 2.2 i is p esen ed some o he de eloped me hods o pa h planning in a ma ine en i onmen , as well as some possible en i onmen ’s models. A e wa ds, in sec ion 2.3, he wo ehicles a ailable in he OceanSys’ lab which could be used o pe o m eal es s o he de eloped algo i hms a e p esen ed and b ie ly compa ed. Finally, in sec ion 2.4, a summa y o he chap e is made and some conclusions a e d awn. 2.1 Robo ics pa h planning me hods 2.1.1 P oblem desc ip ion In obo ics he pa h planning p oblem consis s in inding a way wi hin he obo ’s wo kspace om a s a ing poin o an ending one, a oiding collisions wi h obs acles. Compu ing a solu ion o his p oblem di ec ly in he obo ’s wo kspace can become a e y complex ask since one needs o conside a high numbe o pa ame e s, such as he obo ’s size, shape and deg ees o eedom. Fo his eason, au ho s like [1] and [2] ecommend he use o he Con igu a ion Space o C-Space. This space ep esen s all he possible kinema ic s a es o a obo . I has one dimension o each deg ee o eedom o he obo , including he cen e o mass and all he posi ions o join s o o he componen s which ela i e posi ion can be independen ly de e mined [3]. In he C-Space (designa ed by C) a obo Ais ep esen ed me ely by a ec o called con ig- u a ion o s a e q. In his con igu a ion A’s physical s a e is ep esen ed wi h espec o a ixed en i onmen al ame. Obs acles in his en i onmen can es ic he se o possible con igu a ions and a ise he C- Obs acle O, he space o all obs acles in he C-Space. Being Oian obs acle i, i p ohibi s a ce ain con igu a ion o Aand gene a es a subse o C-Obs acle COias s a ed in equa ion 2.1: 5 6S a e o he a COi={q∈C|A(q)∩Oi6=/0}(2.1) whe e A(q)is he space occupied by obo Awhen i is in con igu a ion q. The p esence o obs acles in he obo ’s wo kspace allied wi h he physical cons uc ion o he obo may p ohibi some con igu a ions and ansi ions be ween con igu a ions. The union o all C-Obs acles (COi)is known as he C-Obs acle egion (CO). The in e sec ion o his egion wi h he A(q)gi es a o mal de ini ion o he obo ’s ee space (C ee), as s a ed in equa ion 2.2: C ee ={q∈C|A(q)∩CO =/0}(2.2) Acco ding o [2] he i s s ep in any pa h planning algo i hm is he disc e iza ion o he wo kspace map. The same au ho sugges s h ee di e en app oaches o do so: •Road map: iden i y possible pa hs in he obo ’s ee space (C ee), being his ee space as de ined in equa ion 2.2. •Cell decomposi ion: disc e iza ion o he space in cells. Each cell deno es i he space is ee (i i allows he obo ’s ci cula ion) o occupied (i i does no ). A e ha he ee cells a e connec ed and a pa h is ound in he esul ing g aph om he beginning poin o he inal posi ion. •Po en ial ield: impose a ma hema ical unc ion upon he space (po en ial ield, g adien ...). The ini ial posi ion will be ep esen ed as a epulsi e o ce whils he inal one as an a ac- i e o ce. The obo will mo e acco dingly o he applied ield, hus he sum o hese o ces and he obo ’s i sel will de ine he obo ’s ajec o y. Acco ding o [4], he nex s ep will be o ind a pa h in he de ined map, i.e. in he disc e ized obo ’s wo kspace. And inally, he hi d s ep will be o send he mo emen ’s commands o he obo ’s con olle . When a pa h planning algo i hm gene a es a solu ion om he ini ial poin o he goal posi ion, a oiding collisions wi h obs acles, i is said o be a comple e algo i hm [1]. Fu he mo e, i he achie ed ajec o y minimizes a se o pa ame e s such as dis ance, ime o ene gy, he algo i hm is also conside ed o be op imal. Howe e , ollowing hese me hods will no always esul in a solu ion o he p oblem. 2.1.2 Me hods The Pa h Planning p oblem can be ea ed in wo di e en le els: globally, de e mining he ajec o y i sel and locally, a oiding obs acles. In his sec ion se e al me hods o bo h le els will be b ie ly desc ibed. 2.1 Robo ics pa h planning me hods 7 2.1.2.1 Global pa h planning Op imal Con ol Sol ing a pa h planning p oblem h ough op imal con ol consis s o inding a con ol law o a gi en sys em, ollowing an op imiza ion c i e ion. Fo ha , he minimiza ion o maximiza ion o a unc ion called cos is pe o med. This unc ion depends on he s a e a iables and on he con ol and is es ic ed by hem [5]. As all me hods, Op imal Con ol has some limi a ions. Fo ins ance, he solu ion may be a local maximum o minimum, ins ead o he global one as in ended. Also, usually he complexi y o his kind o p oblems is e y high and some imes e en compu a ionally impossible o sol e [4]. Howe e , an op imal con ol app oach p o ides a sys ema ic design amewo k, i is applicable o nonlinea p oblems and can deal wi h cons ain s. The e a e di e en app oaches o sol e an Op imal Con ol p oblem such as Dynamic P o- g amming, Pon yagin Minimum (o Maximum) P inciple, Hamil on-Jacobi-Bellman Equa ion o Model P edic i e Con ol [5]. In igu e 2.1 he e is he solu ion o pa h planning p oblem, sol ed by an Op imal Con ol me hod. Figu e 2.1: Ilus a ion o a solu ion o an Op imal Con ol p oblem.1 G aph Sea ch Suppose he space was disc e ized h ough a g aph echnique as he one desc ibed in 2.1.1. The nex s ep in a pa h planning algo i hm would be o ind a pa h om he ini ial node o he inal poin in he g aph, using an op imiza ion c i e ion. These me hods which p o ide he connec ions be ween g aph’s nodes a e called G aph Sea ch Me hods [2]. 1Figu e om h p://www.p ince on.edu/~s engel/Rosenb ock.jpg [6] 14 S a e o he a Chap e 3 Fusing Dead Reckoning and GPS da a o es ima e a Glide ajec o y In sec ion 2.3.2, he Slocum Elec ic Glide owned by he OceanSys’ g oup was p esen ed. In his chap e , he da a ela ed o i s mo emen , collec ed in one o i s missions will be ea ed in o de o de e mine an es ima e o he eal pa h he glide desc ibed. Also, he in luence o GPS accu acy and cu en s’ knowledge on he pa h es ima ion will be analyzed. Acco ding o [20], he Slocum Elec ic Glide has a saw- oo h like mo emen , eme ging pe i- odically. While i is unde nea h wa e , he AUV uses a Dead Reckoning (DR) algo i hm o es ima e i s posi ion and a el o i s p ese waypoin s and when i comes o he su ace, he posi ion is up- da ed o he one gi en by a GPS signal, mo e likely o inc ease accu acy, hus educing he e o induced by he use o Dead Reckoning. In his chap e in sec ion 3.1, he Glide localiza ion p ocess is p esen ed, ollowed by he analysis and manipula ion o he mission da a in sec ions 3.2 and 3.3, wi h di e en app oaches. Finally in sec ion 3.4 conclusions a e d awn and u u e wo k on his chap e opic p esen ed. 3.1 Slocum Elec ic Glide localiza ion The Slocum Elec ic Glide is an AUV which na iga es wi hou p opulsion using only he a i- a ion o i s buoyancy (spends a ound 20 % o i s ene gy o do so) and a pai o wings, ho izon ally assembled, o mo e o wa d. While on su ace, his ehicle ecei es a GPS signal, which allows i o acknowledge i s po- si ion (la i ude and longi ude). Bu when i goes deepe in o he wa e , he e is no GPS signal a ailable and i s loca ion is gi en by a Dead Reckoning algo i hm. Also known as Deduced Reckoning o Pa h In eg a ion, a Dead Reckoning algo i hm consis s o an es ima e o he ma ine ehicle’s posi ion based on i s las known loca ion and cu en eloci y o speed o e a ime in e al. This ype o me hod is subjec o cumula i e e o s and may p oduce inaccu a e es ima es, specially i i is no ecei ing a GPS signal o a long ime. Also he lack o cu en s’ in o ma ion, imp ecise senso s eadings and o he e ec s agg a a e he posi ion e o s. 15 16 Fusing Dead Reckoning and GPS da a o es ima e a Glide ajec o y The e o e an accu a e GPS signal ob ained pe iodically allied wi h ei he mo e senso s o da a (eg cu en s’ ela ed) a e e y bene icial add ons. Acco ding o [20], he DR algo i hm implemen ed in he Slocum Elec ic Gide compu es he AUV’s posi ion a e e y ou second con ol cycle. I uses he in o ma ion gi en by wo senso s p essu e, o dep h (d) de e mina ion, and a i ude, o pi ch (θ), oll and heading (h) measu emen . The ollowing equa ions p esen he basic s eps o he algo i hm: ωs=−∆d anθ (3.1) x = (ωs∗cosh)+ ω x (3.2) y = (ωs∗sinh)+ ω y (3.3) ∆x= x ∗∆ ⇒xi+1=xi+ xi∗( i+1− i)(3.4) ∆y= y ∗∆ ⇒yi+1=yi+ yi∗( i+1− i)(3.5) whe e ωs: ehicle’s speed h ough he wa e x: no hwa d glide ’s eloci y y: eas wa d glide ’s eloci y ω x: no h wa e eloci y componen (op ional) ω y: eas wa e eloci y componen (op ional) 3.2 Mission da a analysis The da a which will be analyzed in his sec ion is he esul o app oxima ely ou hou (14418 s) deploymen o Sesimb a’s coas , in May 2013. In o de o de e mine he desc ibed pa h, some da a is needed as, o ins ance, he GPS coo di- na es in se e al poin s o mission, AUV’s speed, heading, pi ch, e c. Below a lis o he a ailable a iables and i s uni s is p esen ed: • imeo day ( ): secs UTC •dep h (d): me e s • oll: ads •pi ch (θ): ads •heading (h): ads ( ela i e o he geog aphic no h) 3.2 Mission da a analysis 17 •hspeed: ho izon al speed in m/s •la i ude: deg ees (WGS84) •longi ude: deg ees (WGS84) •gps_s a us: lag (0 means ok) •gps_accu acy: es ima ed accu acy in me e s o GPS ix As explained in sec ion 3.1, he Slocum Elec ic Glide localiza ion is pe o med by combining a Dead Reckoning algo i hm wi h pe iodic GPS eadings. The DR me hod gene a es Ca esian coo dina es (x,y). So, in o de o compa e hem wi h he GPS signal, he i s s ep will be o con e he GPS (la i ude,longi ude)pai in o a mo e use ul (no h,eas ). Fo ha , he unc ion ll_di .m1was used in MATLAB. I akes a ec o wi h la i udes and longi udes in deg ees and con e s i in no h and eas di ec ions, in me e s, om an ini ial pai o (la 1,long1). Ha ing hese GPS coo dina es in a con enien o ma one can compa e hem wi h he posi ions gi en by he Dead Reckoning algo i hm. In sec ion 3.1, he Glide DR equa ions we e p esen ed. Howe e , wi h he a ailable da a, he compu a ions a e sligh ly di e en since he hspeed ec o ep esen s al eady he ehicle’s ho izon al speed in m/s. Hence, equa ion 3.1 can be igno ed. Also, a i s , he wa e cu en s’ e ec won’ be included in he compu a ions. The e o e, he DR algo i hm is as ollows: x = (hspeed ∗cosh)(3.6) y = (hspeed ∗sinh)(3.7) ∆x= x ∗∆ ⇒xi+1=xi+ xi∗( i+1− i)(3.8) ∆y= y ∗∆ ⇒yi+1=yi+ yi∗( i+1− i)(3.9) In igu e 3.1, one can see in blue a plo o he pa h poin s in eas ×no h coo dina es, compu ed wi h he a o emen ioned Dead Reckoning algo i hm. In g een, he e is a ep esen a ion o he GPS ix. The glide has access o a alid GPS signal e e y ime i comes close enough o he su ace. In ec o gps_s a us a lag equal o ze o means he AUV is comple ely eme ged (i i is close bu no ye he e he lag will be equal o 7). Also, ela ed o i s eadings, ec o gps_accu acy gi es a measu e o how accu a e is he measu emen : as smalle he alue in his ec o is, he highe he accu acy o he GPS signal in he co esponding ins an . The ole ance chosen o he gps_accu acy is 5 m since by doing so one only loses 9.4 % o he alid GPS da a. I a highe alue was conside ed, he GPS esul s would no be e y accu a e and only a small pe cen age o alid GPS da a would be los . Fu he mo e, he less o en a alid 1This unc ion belongs o P o esso Aníbal Ma os and is da ed Augus 1998. 18 Fusing Dead Reckoning and GPS da a o es ima e a Glide ajec o y Figu e 3.1: Pa h Poin s compu ed simply wi h he Dead Reckoning algo i hm (in blue) and GPS alid da a, wi h an accu acy <5m(in g een). GPS alue is conside ed, he less imes he DR alue will be co ec ed, hus i s cumula i e e o s will g ow highe . As one can see, he di e ence be ween he compu ed posi ion and he eal one gi en by he GPS signal inc eases in ime, since he Dead Reckoning posi ions a e no upda ed o he eal ones a any poin o he algo i hm. Hence, he nex s ep o ha e a be e pe cep ion o he Glide eal pa h is o inco po a e he GPS eading and co ec he DR localiza ion. Fo ha , when he glide is a su ace (i.e. gps_s a us == 0) and he GPS accu acy is smalle han a ce ain ole ance, hen he cu en Pa h Poin is upda ed o he (no h, eas ) coo dina es a ha ins an . Fu he mo e, he pa h un il his poin should be adap ed acco dingly. In igu e 3.2, a schema ic o how he Pa h Poin s could be ecompu ed is shown: The di e ence be ween he pa h poin co esponding o he ins an when he e is a alid GPS signal (P1) and he GPS eading i sel (Pgps) is aken (d in he igu e). Then a ac ion o his dis ance (d i) is applied o all he poin s since he las alid GPS signal. The ec o d has componen s in bo h no h (dn) and eas (de) di ec ions. Also,in o de o compu e he eal pa h one has o spli he pa hPoin s ec o in smalle ac ions (di), co esponding o each pa hPoin Pi. Howe e , he a ailable pa hPoin s, ep esen ed in blue in igu e 3.2, we e compu ed wi h he Dead Reckoning algo i hm. The e o e, in o de o educe he compu a ion e o , one shall conside he a ailable ime da a ins ead. The e e ed s eps can be ollowed in equa ions 3.10 o 3.13: 3.2 Mission da a analysis 19 d di d i newPP pa hPoin s dPPi P0 P1 Pgps Pi Figu e 3.2: New Pa h Poin s compu a ion. dn=no hgps −x1;de=eas gps −y1(3.10) d = [dnde](3.11) d = 1− las gps_ok (3.12) n=dn d ; e=de d (3.13) To ob ain he new pa h poin s, newPP, one simply sums he co esponding ac ion o he d displacemen o each poin i,d i, o all pa h poin s om he las alid gps_sa us un il he cu en ime. The e o e, a each ime s ep i he new pa h poin (newPP(i)) will be gi en by: "newPPn(i) newPPe(i)#="pa hPoin sn(i) pa hPoin se(i)#+" n e#∗( (i)− las gps_ok )(3.14) In igu e 3.3, in blue one can see he glide pa h, compu ed wi h he Dead Reckoning algo i hm. A e e y alid GPS eading, ep esen ed as g een do s, hese pa h poin s a e displaced o he (no h,eas ) coo dina es gi en by he GPS signal. Also, an es ima e o he eal pa h a eled om each GPS eading o he nex is made based on his displacemen (in ed in he igu e). An in e es ing obse a ion can be made when plo ing he gps_accu acy h oughou he mis- sion ime: when he signal is less accu a e he de ia ion be ween he pa h poin s and he new pa h poin s is highe . In igu e 3.4 he men ioned plo is shown and one can see ha he highe peaks, co esponding o he wo s case scena ios, occu a he beginning, un il a ound 4.7 s. I one looks back a igu e 3.3, one can con i m ha he wo s pe o mance o he DR algo i hm occu s in he i s pa o he pa h. A summa y o he desc ibed s eps can be seen in Algo i hm 1. 20 Fusing Dead Reckoning and GPS da a o es ima e a Glide ajec o y Figu e 3.3: Pa h Poin s compu ed wi h he Dead Reckoning algo i hm, wi h he GPS co ec ion (in blue). In ed, he es ima e o he eal pa h is p esen ed and, in g een, GPS alid da a, wi h an accu acy <5. 4.4 4.6 4.8 5 5.2 5.4 5.6 5.8 6 x 10 4 0 5 10 15 20 25 30 35 40 Figu e 3.4: GPS accu acy o e imeo day. Finally, in igu e 3.5 a 3D plo illus a es he eal pa h he glide a e sed. In blue, he cha ac- e is ic saw- oo h like pa h is shown while in ed one can see he ajec o y desc ibed in no h×eas coo dina es. 3.2 Mission da a analysis 21 Algo i hm 1 Glide Localiza ion 1: p ocedu e LOCALIZATION WITH DR AND GPS 2: Con e (la i ude,longi ude)⇒(no h,eas ) 3: o i=1:All Pa hPoin s do 4: i gpss a us == /0 || gpsaccu acy < ole ance hen 5: dn =no h(i)−Pa hPoin s(i,1) 6: de =eas (i)−Pa hPoin s(i,2) 7: d = (i)− (las gpsok ) 8: n=dn d 9: e=de d 10: o j=las gpsok :ido 11: Pa hPoin s(j,1) = Pa hPoin s(j,1)+ n∗ (j)− (las gpsok ) 12: Pa hPoin s(j,2) = Pa hPoin s(j,2)+ e∗ (j)− (las gpsok ) 13: end o 14: Pa hPoin s(i+1,:) = [no h(i)eas (i)] 15: else 16: Pa hPoin s(i+1,:) = DeadReckoning(Pa hPoin s(i)) 17: end i 18: end o 19: end p ocedu e Figu e 3.5: Schema ic o he Glide es ima ed ajec o y in Sesimb a’s mission. 22 Fusing Dead Reckoning and GPS da a o es ima e a Glide ajec o y 3.3 Es ima ion o wa e cu en s’ and i s e ec s on Dead Reckoning The e a e se e al possible causes o he di e ence be ween he esul o he Dead Reckoning compu a ion and he GPS signal. The DR cumula i e e o s and he senso s’ e o s, amongs o he s, ypically p esen alues wi hin a ce ain ange o which one can pe o m some co ec ions. Howe e wa e cu en s p esen mo e unp edic able beha io and e o s. Also o he e o s end o be much highe when compa ed o he a o emen ioned and, o hei andomness, ha de o p edic [21] [22]. The e o e, in his sec ion he cu en s’ map change is conside ed o be he main cause o he men ioned di e ence and a p oposal o how one could es ima e he cu en s’ map a each alid GPS poin is pe o med. Upda ing he cu en s’ map is pa icula ly impo an in his p oblem, since hey can be inco - po a ed in he DR algo i hm compu a ions and enhance i s esul s. 3.3.1 De e mining he eal pa h wi h he help o cu en s’ in o ma ion Acco ding o [20], he Dead Reckoning algo i hm p esen s wo op ional pa ame e s: w x (added o equa ion 3.15) and w y (added o equa ion 3.16). These pa ame e s ep esen he wa e cu en s’ speed componen s ela ed, espec i ely, o he eas wa d and no hwa d coo dina es. x = (hspeed ∗cosh)+w x (3.15) y = (hspeed ∗sinh)+w y (3.16) Since he e is a lack o da a abou his ield and assuming he main cause o he di e ence be ween he compu ed poin a he GPS eading and he alid GPS signal a his poin is he a ia ion o wa e cu en s, we shall conside ins ead o he cu en s’ speed i s displacemen and add i o he DR no h (∆cx) and eas (∆cy) coo dina es compu a ion: xi+1=xi+ xi∗( i+1− i) +∆cxi(3.17) yi+1=yi+ yi∗( i+1− i) +∆cyi(3.18) whe e ∆cxi: no h displacemen om he GPS signal o he DR compu ed posi ion ∆cyi: eas displacemen om he GPS signal o he DR compu ed posi ion 3.4 Conclusions and u u e wo k 23 3.3.2 Es ima ing wa e cu en h oughou he ime In p e ious sec ions he glide ’s eal pa h was ob ained based on i s Dead Reckoning algo i hm and pe iodic GPS eadings. A e e y alid GPS eading he di e ence be ween he posi ion he AUV es ima ed and i s eal one is aken. In o de o ob ain he cu en s ield a weigh ed a e age is pe o med, as shown in he ollowing equa ions: ∆d=dis ance_ o_end o al_dis ance (3.19) newField =∆d∗(PGPS −PDR)+oldField (3.20) whe e ∆d: weigh , ep esen ing how a one is om he des ina ion dis ance_ o_end: dis ance om he cu en alid GPS eading o he des ina ion o al_dis ance: o al a el dis ance newField: upda ed cu en s’ map PGPS −PDR: di e ence be ween he es ima ed posi ion (DR) and he eal one (GPS) oldField: cu en s’ map one wan s o upda e Close o he s a ing poin , he unce ain y should be bigge since one doesn’ know i he ield es ima e is co ec o no . The e o e, he unce ain y weigh is highe and he ec o ield unde goes a bigge al e a ion. As one app oaches he des ina ion, hence ha e a mo e accu a e and upda ed map, he unce ain y weigh dec eases and he ec o ield’s inc eases, being he upda e almos null. A simila app oach will be explo ed in chap e 4 o upda e also i s cu en s’ map. 3.3.3 Implemen a ion and esul s Un o una ely due o ime es ic ions i was no possible o implemen he desc ibed algo- i hm. Howe e in he u u e his ask will be comple ed and esul s will be a ailable o analysis. 3.4 Conclusions and u u e wo k In his chap e he da a collec ed in he OceanSys’ Slocum Elec ic Glide ’s mission o he coas o Sesimb a in May 2013 was analyzed. The Dead Reckoning algo i hm compu es ai ly good esul s bu i is much mo e accu a e when allied wi h he pe iodic GPS signal upda e. 30 Op imized pa h planning o an AUV conside ing unce ain y This unc ion allows he use o choose be ween ou nonlinea p og amming me hods o sol e he op imiza ion p oblem: •T us Region Re lec i e [34] [35]: also known as es ic ed s ep me hods, he us egion algo i hms ake he ollowing app oxima ion o he main minimiza ion p oblem: min s{q(s)such ha s∈N}(4.11) o a ce ain x.q(s)1is a quad a ic app oxima ion o he objec i e unc ion (x)in he neighbo hood No x. This neighbo hood is called us egion and ep esen s a subse o he objec i e unc ion space in which one belie es he minimum lies on. Sol ing he subp oblem one ge s he alue o s, minimum alue o q(s), which will be called s ep. A e wa ds, i (x+s)< (x) he cu en poin xis upda ed o x+sand N, he us ed egion is expanded. O he wise he cu en poin emains he same, Nis con ac ed and one should ecompu e he s ep s. These s eps a e epea ed un il he me hod con e ges.2 •Ac i e Se [34] [36]: also known as p ojec ion me hod, i is mos e ec i e wi h small o medium-scale p oblems and alls wi hin he scope o quad a ic p og amming (op imiza ion p oblem wi h a quad a ic objec i e unc ion and linea cons ain s). In an op imiza ion p oblem, a easible egion is he se o all poin s xwhe e he op imal solu ion migh be. These poin s a e de ined by he p oblem’s cons ain s 4.1 (equali ies and inequali ies). Gi en an xpoin in he easible egion, a cons ain gi(x)⩾0 is conside ed o be ac i e i gi(x) = 0 (all equali y cons ain s a e ac i e) and inac i e i o he wise. Hence he ac i e se a xis he g oup o all he ac i e op imiza ion p oblem’s cons ain s. The main s eps o an Ac i e-Se me hod a e hen3: Algo i hm 2 Ac i e Se Me hod 1: p ocedu e ACTIVE SET METHOD 2: Find a easible s a ing poin x 3: while no "op imal enough" do 4: Sol e gi(x) = 0 5: Compu e λio he ac i e se 4 6: Remo e a subse wi h λi<0 7: Sea ch o in easible cons ain s 8: end while 9: end p ocedu e 1In MATLAB’s implemen a ion o his me hod q(s)co esponds o he i s wo e ms o he Taylo app oxima ion o (x)a x. 2This is he me hod used by de aul in MATLAB’s mincon unc ion. 3This is he chosen algo i hm o sol e he p oblem unde s udy by MATLAB’s sc ip . 4λis ands o he Lag angian Mul iplie s 4.3 Inco po a ing unce ain y in o he o iginal sc ip 31 •In e io Poin [34] [36]: also known as ba ie me hods, MATLAB’s implemen a ion o his me hod is a a ian o Meh o a’s p edic o -co ec o algo i hm, a p imal-dual in e io -poin me hod [34]. This me hod uses a ba ie unc ion which encodes he con ex se . I eaches an op imal solu ion a e c ossing he easible egion. This me hod can be less accu a e han o he s since he in e nally compu ed ba ie unc ion keeps inequali y cons ain s away. The e o e, i was no chosen o sol e he p oblem unde s udy. •SQP (Sequen ial Quad a ic P og amming) [34] [36]: i is an i e a i e me hod o sol e nonlinea op imiza ion p oblems wi h wice con inuously di e en iable objec i e unc ions. These algo i hms op imize a quad a ic model o he objec i e unc ion subjec o a linea iza- ion o he cons ain s. Using he gene al de ini ion o he nonlinea p og amming p oblem 4.1, one de ine he p ob- lem’s Lag angian as: L(x,λ,σ) = (x)−λTg(x)−σTh(x)(4.12) whe e λand σa e he Lag ange Mul iplie s [37]. A each i e a ion xk, will y o sol e he quad a ic p og amming p oblem in he di ec ion dk: min d (xk)+∇ (xk)Td+1 2dT∇2 xxL(xk,λk,σk)d(4.13) s. . g(xk) = ∇g(xk)Td≥0 (4.14) h(xk) = ∇h(xk)Td=0 (4.15) The base sc ip uses he algo i hm op ion Ac i e Se , wi h a maximum i e a ions numbe o 2000. The mincon unc ion e u ns he coo dina es o he waypoin s which will gi e he op imal pa h a e being in e pola ed. The esul o MATLAB’s op imiza ion sc ip o i e waypoin s can be seen in igu e 4.3. The ime was imp o ed om he 10 h 58.8 min o he s aigh line pa h o 10 h 17.8 min, hus almos one hou . 4.3 Inco po a ing unce ain y in o he o iginal sc ip In he las sec ion, 4.2, he o iginal sc ip was desc ibed: a pa h planning algo i hm de e mines he bes pa h om a s a ing poin o he des ina ion o an ai plane. 32 Op imized pa h planning o an AUV conside ing unce ain y 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [km] Tailwind (km/h) Headwind -200 -150 -100 -50 0 50 100 150 200 Figu e 4.3: O iginal algo i hm’s esul s. As i was men ioned be o e, since his wo k alls wi hin he ma ine obo ics scope, he o iginal algo i hm was adap ed o i he aim o his disse a ion. The e o e one shall conside he wind ield as a wa e cu en and ins ead o an ai plane he ehicle unde s udy will be an AUV. Also, he magni ude o he p oblem’s a iables has o be modi ied. The a ailable OceanSys’ AUVs ha e a ange o app oxima ely 40 km wi h an a e age speed o 1 m/s and he Glide has a ange o 1500 km wi h an a e age speed o 0.4 m/s. The e o e, o es ing pu poses, he o al a el dis ance will be 5 km ins ead o he o iginal 5000 km, he AUV a e age speed will be 1 m/s ins ead o he ai plane’s 500 km/h and he wa e cu en s will ha e a maximum alue o 0.5 m/s, ins ead o 200 km/h. The e was a discussion abou wha else should be adap ed and imp o ed in MATLAB’s im- plemen a ion. Mul iple hypo hesis we e conside ed: •Enable he sc ip esponsible o he wa e cu en s’ de ini ion o be cus omizable, hus allowing i o be al e ed o he speci ic condi ions o a mission day. Fo example, make i able o ead a ex ile wi h some pa ame e s and con e hem in o a ec o ial ield. •Change he op imiza ion p ocedu e and me hod ( he sc ip uses mincon wi h he op ion "ac i e-se " bu he e a e mo e op imiza ion unc ions o wi hin mincon o he me hods). •The cu en model op imizes he ajec o y wi hou conside ing any unce ain y. So, i would be a good imp o emen o inco po a e i and ecompu e he pa h a ec ed by i . •Rela ed o he las hypo hesis, change he cu en s’ map acco dingly o he compu ed unce - ain y, since his is one o he majo causes o he di e ence be ween he posi ion p edic ion and he eal a ained one. •Change he sc ip in o de o sol e a 3D p oblem, since he wa e cu en s a e dep h depen- den . 4.3 Inco po a ing unce ain y in o he o iginal sc ip 33 F om he imp o emen s p esen ed abo e, he p io i y was o inco po a e he unce ain y in he pa h planning p oblem. A s ong eason o ha choice was he ac ha he ini ially s udied p ob- lem, om MATLAB’s example, akes place along hund eds o kilome e s. Wi h such dis ances he e is no gua an ee he ini ial condi ions will be he same h oughou he whole pa h. Besides he ehicles odome y’s e o s, he es ima ed cu en migh no be equal o he eal one o change du ing he a eling, e c. Fo hose easons he waypoin s, used in he o iginal implemen a ion as he op imiza ion poin s, which ep esen he spo s whe e he op imal pa h should go h ough, would now ha e an addi ional ea u e. These poin s would ep esen check poin s, whe e he AUV’s es ima ed op- imal posi ion would be compa ed wi h i s eal one, gi en by, o ins ance, a GPS signal o he dis ance o a known beacon. This di e ence would ep esen he unce ain y up o ha ins an . In o de o simula e he unce ain y in he sc ip a ci cle wi h uni a y adius was d awn a ound each waypoin and a andom poin chosen inside ha a ea. In igu e 4.4, he blue ci cle ep esen s he unce ain y a ea delimi e while he ed poin is he a o emen ioned andom poin . 12 13 14 15 16 17 18 13 13.5 14 14.5 15 15.5 16 Uni s = 100 [km] Fo wa d cu en (km/h) Coun e cu en -200 -150 -100 -50 0 50 100 150 200 Figu e 4.4: Simula ion o he unce ain y. The nex s ep is o compu e he new op imal pa h om he new s a ing poin ( he ed poin in he simula ion), since by being in a di e en egion o he wa e cu en he op imal pa h migh be di e en om he p e iously compu ed.5 In igu e 4.5, he new pa h, esul ing om he new second op imiza ion, is ep esen ed in g ay, wi h ou waypoin s, less one han in he i s s ep. Also, an es ima e o wha should ha e been he ajec o y desc ibed by he ehicle is compu ed and ep esen ed in igu e 4.5 in blue. In o de o ob ain he new pa h poin s ( he blue ajec o y), he ini ial op imal pa h was di ided in small subin e als, one o each o iginal pa h poin . Then, o he ec o di om he s a ing 5This new op imal pa h will ha e one less waypoin . 34 Op imized pa h planning o an AUV conside ing unce ain y 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [km] Fo wa d cu en (km/h) Coun e cu en -200 -150 -100 -50 0 50 100 150 200 Figu e 4.5: Rep esen a ion o he ecompu ing o he op imal pa h. waypoin P0 o he o iginal pa h poin Pi, a ac ion o he inal displacemen d , he g een ec o dPPi, is added, as shown in igu e 4.6: d di d i newPP pa hPoin s dPPi P0 P1 P and Pi Figu e 4.6: New pa h poin s compu a ion diag am. The o iginal pa h poin s (pa hPoin s in he diag am) a e ep esen ed in g ay while he es i- ma ed new pa h poin s a e in blue (newPP in he diag am), as in he p e iously p esen ed simula- ion plo . P0and P1, he big black do s, a e waypoin s while P and, in ed, ep esen s he andom sample o he unce ain y a ea. The e o e d is he e o be ween whe e he ehicle hough i would be a and he ac ually eached posi ion, as s a ed in equa ion 4.16: d =q(x andom −xop imal)2+(y andom −yop imal)2(4.16) whe e P and = (x andom,y andom)and P1= (xop imal,yop imal). 4.3 Inco po a ing unce ain y in o he o iginal sc ip 35 An es ima e o he eal a e sed pa h can hen be gi en by he sum o he pa hPoin s ec o wi h he displacemen d : newPP =pa hPoin s +d (4.17) Fu he mo e, by spli ing he pa hPoin s ec o in o smalle ec o s di, as explained be o e, he newPP ec o can be ob ained by summing up all he dPPi: newPP =∑dPPi=∑di+d i(4.18) whe e d i=di d ep esen he ac ion o he inal de ia ion om P1 o P and. By epea ing he a o emen ioned s eps un il he e a e no mo e waypoin s le , one eaches he des ina ion and ge s an es ima e o he a e sed pa h, as shown in igu e 4.7: 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [m] Fo wa d cu en (m/s) Coun e cu en -0.4 0 0.6 Figu e 4.7: Resul s wi h 5 waypoin s wi h he o iginal and he al e ed algo i hms. Finally, ga he ing all he da a compu ed abo e one should be able o de e mine he o al a el ime. Fo ha , wo di e en me hods we e implemen ed: he di e en ial ime and he di ec compu a ion. In he o iginal sc ip , he a el ime was compu ed om he s a ing poin o he a ge as explained in sec ion 4.2. The e o e, he i s way o compu ing he o al a el ime consis s o using he o iginal ge TimeF omPa h unc ion and simply al e he ini ial poin o he waypoin one is in. A e ha , o know how long i ook o go om waypoin i−1 o waypoin i, he di e ence be ween he a el ime om i−1 o he a ge and he a el ime om i o he a ge is aken, hence he name di e en ial ime. Equa ion 4.19 ep esen s he compu a ion o he o al a el ime Tdi : Tdi = numWaypoin s+1 ∑ i=1 di i−1− di i(4.19) 36 Op imized pa h planning o an AUV conside ing unce ain y whe e numWaypoin s+1: o al numbe o checkpoin s plus he des ina ion poin di i−1: a el ime om check poin i−1 o he des ina ion di i: a el ime om check poin i o he des ina ion The di ec compu a ion consis s o calcula ing di ec ly he ime om each s a ing poin o he nex check poin and summing up his subin e al’s imes, as s a ed in equa ion 4.20: T= numWaypoin s+1 ∑ i=1 i(4.20) whe e T: o al a el ime ob ained by he di ec compu a ion me hod i: ime o go om check poin i−1 o check poin i 4.3.1 Resul s and discussion In his sec ion he es s pe o med o con i m he algo i hm’s implemen a ion a e desc ibed and he esul s p esen ed and discussed. The ehicle speed was se o 1 m/s and he wa e cu en s can ake alues om -0.5 m/s up o 0.5 m/s ( he signal ep esen s i s di ec ion). The o al a el dis ance is 5 km. A e se ing hese pa ame e s he i s es pe o med consis ed on applying a cons an ec o ield in he di ec ion o he in ended mo emen o e i y i s beha io . I was expec ed o ha e a s aigh line om s a o inish, wi h some small de ia ions due o he unce ain y simula ion. In igu e 4.8, a s ong ield was gene a ed o ep esen an in ense o wa d cu en and, as expec ed, in e e y check poin he ecompu ed pa h con e ged owa ds he des ina ion poin . 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [m] Fo wa d cu en (m/s) Coun e cu en Figu e 4.8: Applica ion o a cons an ec o ield in he di ec o he mo emen . 4.3 Inco po a ing unce ain y in o he o iginal sc ip 37 Ha ing se he cu en o 1 m/s, he expec ed alue o a s aigh line pa h would be: T=d au + ield ⇒T=5000 1+1=2500 s (4.21) This alue is equi alen o 41 min 40 s, simila o he ob ained 41 min 55 sec, om he di e en ial ime compu a ion and 41 min 57 sec, om he di ec ly compu ed ime. A e ha ing his con i ma ion, a andom pa h was de ined and applied o he p oblem wi h di e en numbe s o checkpoin s. Al hough a andom ec o ield was gene a ed, he same one was used o es he algo i hm wi h di e en numbe s o checkpoin s so ha one could assess hei in luence in he a el ime. Fi s , a s aigh line was gene a ed wi h he ini ially se condi ions 4.3.1, as one can see in igu e 4.9. A s aigh line co esponds o he sho es pa h om s a o end. So, in o de o compa e he ime compu a ion esul ing o he op imiza ion, he i s s ep was o ob ain he s aigh line a el du a ion, which was 1 hou 26 min 58 sec. 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [m] Fo wa d cu en (m/s) Coun e cu en -0.4 0 0.6 Figu e 4.9: S aigh line: he sho es pa h one can ake om s a ing poin o he des ina ion. A e wa ds, he algo i hm was es ed wi h 5, 10, 15 and 20 checkpoin s and he espec i e imes ob ained. The esul s o hese simula ions can be seen in igu es 4.7,4.10,4.11 and 4.12, espec i ely. In able 4.1, one can see he esul ing a el imes o he a o emen ioned es s. Numbe waypoin s O iginal sc ip ime Di ec ly compu ed ime Di e en ial ime 5 1 h 25 min 25 sec 1 h 29 min 3 sec 1 h 18 min 30 sec 10 1 h 25 min 19 sec 1 h 23 min 48 sec 1 h 27 min 11 sec 15 1 h 25 min 28 sec 1 h 25 min 55 sec 1 h 23 min 26 sec 20 1 h 25 min 30 sec 1 h 23 min 27 sec 1 h 27 min 29 sec Table 4.1: Compa ison be ween he esul s a ained wi h he o iginal sc ip and he al e ed one. 38 Op imized pa h planning o an AUV conside ing unce ain y 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [m] Fo wa d cu en (m/s) Coun e cu en -0.4 0 0.6 Figu e 4.10: Resul s wi h 10 waypoin s wi h he o iginal and he al e ed algo i hms. 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [m] Fo wa d cu en (m/s) Coun e cu en -0.4 0 0.6 Figu e 4.11: Resul s wi h 15 waypoin s wi h he o iginal and he al e ed algo i hms. 0 5 10 15 20 25 30 35 40 45 50 0 5 10 15 20 25 Uni s = 100 [m] Fo wa d cu en (m/s) Coun e cu en -0.4 0 0.6 Figu e 4.12: Resul s wi h 20 waypoin s wi h he o iginal and he al e ed algo i hms. Looking a able 4.1 i is e i ied ha despi e he o iginal ime emains almos cons an wi h 4.4 Upda e o he cu en s’ map based in he unce ain y 39 any numbe o waypoin s, bo h he di ec ly compu ed ime and he di e en ial ime p esen a ia- ions o di e en numbe s o hese poin s. Compa ing he ob ained alues wi h he s aigh line a el ime (1 h 26 min 58 sec), he o ig- inal algo i hm wins o e he implemen ed one only o a smalle numbe o poin s (5 poin s). The e o e, one can conclude he eop imiza ion equi es mo e waypoin s in o de o p esen e ec- i e esul s. 4.4 Upda e o he cu en s’ map based in he unce ain y The e a e se e al possible causes o las chap e ’s unce ain y. The ehicle’s odome y and he senso s’ e o s, amongs o he s, a e ypically well known e o s and p esen alues wi hin a ce ain ange o which one can pe o m some co ec ions. Howe e wa e cu en s p esen mo e unp edic able beha io . Also e o s end o be much highe when compa ed o he a o emen ioned and, o hei andomness, ha de o con ol. The e o e, in his sec ion he cu en s’ map change is conside ed o be he main cause o he compu ed unce ain y. A p oposal o how one could b oaden he applica ion o he p e iously compu ed unce ain y o upda e he cu en s’ map a each check poin h oughou he pa h is pe - o med. Upda ing he cu en s’ map is pa icula ly impo an in his p oblem esolu ion, since he op- imiza ion objec i e unc ion ep esen s he o al a el ime and his one is dependen on he cu en s’ speed. 4.4.1 Base Concep In p e ious sec ions he op imiza ion algo i hm was explained and he unce ain y inco po a ed in i . A e e y check poin he op imiza ion algo i hm akes he di e ence be ween he posi ion he AUV es ima ed o ha e eached and i s eal posi ion. This di e ence is called unce ain y. In o de o ob ain he new cu en ield a weigh ed a e age is pe o med, as shown in he ollowing equa ions: ∆d=dis ance_ o_end o al_dis ance (4.22) newField =∆d∗(P and −Pcheck) +oldField (4.23) whe e ∆d: weigh , ep esen ing how a one is om he des ina ion dis ance_ o_end: dis ance om he cu en check poin o he des ina ion o al_dis ance: o al a el dis ance newField: upda ed cu en s’ map 46 Op imized a ea scanning i x y 1 0.1 0.5 2 0.5 0.8 3 0.2 0.3 4 0.0 0.0 5 1.0 1.0 6 0.6 0.15 7 0.8 0.6 Table 5.1: Example o poin s o in e es in he no malized scanning a ea. 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Figu e 5.2: Ho izon al a ea pa i ioning wi h upda e o he pa i ion o he Fu hes Poin . Th esh- old=0.1 Poin s Fu hes Poin ; h eshold=0.1 Fu hes Poin ; h eshold=0.2 1 0.00 0.00 2 0.15 0.05 3 0.00 0.20 4 0.10 0.10 5 0.00 0.00 6 0.20 0.20 7 0.00 0.00 Table 5.2: Compa ison o he Fu hes Poin me hod esul s wi h h eshold=0.1 and h eshold=0.2. The i e a ions numbe was, espec i ely, 2001 and 3. The eason o ha is exac ly o a oid he p oblem ela ed o he Fu hes Poin algo i hm. In he case one has poin s in ex eme posi ions, by placing he lines be ween he u hes ones, hey 5.2 Ho izon al pa i ioning 47 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Figu e 5.3: Ho izon al a ea pa i ioning wi h upda e o he pa i ion o he Fu hes Poin . Th esh- old=0.2 will be close o all he poin s. The p oblem ela ed o his app oach is ha i he u hes poin s a e always he same, he lines will no mo e anymo e, a oiding he algo i hm o con e ge o a alid solu ion. In igu es 5.4 and 5.5, on he le side he o iginal posi ion o bo h poin s and lines is shown, whe eas on he igh one can see he esul s wi h, espec i ely, a h eshold o 0.1 and 0.2. 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Figu e 5.4: Ho izon al a ea pa i ioning wi h upda e o he pa i ion Be ween he Fu hes Poin s. Th eshold=0.1 In able 5.3, one can see he esul s o he p esen me hod o a h eshold o 0.1 and 0.2. Fo he i s one, he me hod did no con e ge while o he highe alue i con e ged in 11 i e a ions. This esul is wo s han wi h he Fu hes Poin algo i hm. Al hough, since only a se o poin s was es ed, one can no ush o conclusions. 48 Op imized a ea scanning 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 0 0.2 0.4 0.6 0.8 1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 1 Figu e 5.5: Ho izon al a ea pa i ioning wi h upda e o he pa i ion Be ween he Fu hes Poin s. Th eshold=0.2 Also, o a h eshold o 0.1, besides he goal o ha ing a line close o a ha same dis ance no being me , he e a e also wo poin s o in e es (p3and p7) which a e no co e ed, one mo e han wi h he las me hod. Bu again, i is p uden no ush o conclusions. Mo e es ing should be pe o med in o de o con i m o no his in o ma ion. Poin s Be ween Fu hes Poin s; h eshold=0.1 Be ween Fu hes Poin s; h eshold=0.2 1 0.00 0.00 2 0.15 0.15 3 0.25 0.10 4 0.05 0.05 5 0.05 0.05 6 0.15 0.00 7 0.35 0.20 Table 5.3: Compa ison o he Be ween Fu hes Poin s app oach esul s wi h h eshold=0.1 and h eshold=0.2. The i e a ions numbe was, espec i ely, 2001 and 11. 5.2.3 Fu hes Poin and Line The nex discussed me hod di e s only in a small de ail om he Be ween Fu hes Poin s me hod: ins ead o conside ing he wo closes poin s o he u hes line, one shall mo e he line o he a e age dis ance om he u hes line and he co esponding poin . Since he lines a e mo ing in e e y i e a ion, his me hod should sol e he p oblem o he p e ious one o eaching a local op imum, in alid solu ion, ins ead o a global one. 5.3 Ve ical pa i ioning 49 A he ime o he w i ing he implemen a ion was no concluded. None heless i will be comple ed in he sho u u e and used o compa ison wi h o he esul s. 5.2.4 S ep Me hod Simila o he T us egion algo i hm p esen ed in chap e 4, he S ep Me hod s a s o dis ibu e he lines uni o mly spaced. A e wa ds, a small s ep Siis aken up o down he line and he objec i e unc ion FOiis compu ed o ha a ia ion. The hi d s ep is o di ide he objec i e unc ion alue by he y a ia ions: ∆FO ∆yi (5.2) The new lines should be gi en by: yi=h+k∆FO ∆yi (5.3) whe e k ep esen s he di ec ion o he minimiza ion. To de e mine he ac ual lines one has o de e mine kand hence one should pe o m he ollow- ing minimiza ion: min ih+k∆FO ∆yi(5.4) Finally, de e mined kand he new lines, as in he o he s me hods one should con i m i all he poin s ha e a leas a line close enough, in a dis ance less o equal o he h eshold alue. Again, a he ime o he w i ing he implemen a ion was no concluded. None heless i will be comple ed in he sho u u e and used o compa ison wi h o he esul s. This me hod is expec ed o be he one wi h he bes esul s, howe e ha will be assessed when he implemen a ion is inished. 5.3 Ve ical pa i ioning The same algo i hms p esen ed in sec ion 5.2 will be implemen ed in he u u e o he e ical di ec ion, in o de o assess he in luence o he o ien a ion o he selec ed lines in o he esul s. 5.4 Conclusions and u u e wo k In his chap e , se e al app oaches o sol e an a ea scanning p oblem we e discussed. Al- hough a signi ican amoun o wo k is s ill o be done, in he sho u u e esul s will be a ailable o mo e obus conclusions o be d awn. 50 Op imized a ea scanning In he sho u u e, he implemen a ion o he me hods will be inished and besides he p e- sen ed es s explained in his chap e , i is in ended o analyze he algo i hms beha io s owa ds di e ence se s o poin s, bo h chosen, bo h andom. Fu he mo e, u u e wo k may include explo ing al e na i e me hods. Fo ins ance pa i ioning he scanning a ea in o solids, o elying in a g eedy algo i hm o de e mine he op imal app oach o explo ing he majo i y o poin s wi h he lowes coas . Chap e 6 Conclusions and u u e wo k A e ca e ul s udy o he di e en pa h planning me hods, an Op imiza ion echnique was chosen o be used o de e mine he bes possible pa h. Al hough an op imal con ol p oblem migh be di icul o o mula e and de e mining a sui able con olle a ha d p ocess, his ype o me hods p o ide a sys ema ic design amewo k. Also hey a e applicable o nonlinea p oblems and can deal wi h cons ain s, which is e y impo an in o de o inco po a e, o ins ance, he ma ine cu en s in o he p oblem. Fu he mo e an Op imiza ion algo i hm g an s he conside a ion o he ma ine cu en s’ e ec s in he de e mina ion o he ehicle’s ajec o y since in he p oblem de ini ion i sel one has o include he so called es ic ions. Mos o AUVs spend he majo i y o hei a ailable ene gy in hei mo ion. The e o e, de e - mining he bes possible pa h, hence op imal, o ul ill a mission will allow a maximum educ ion o ene gy and ime consump ion. This con ibu es he a ionale behind choosing an Op imiza ion echnique o pe o m pa h planning in his Disse a ion. Two obo ic pla o ms om he OceanSys g oup we e po en ial candida es o s udy in his Disse a ion: AUV MARES and he Slocum Elec ic Glide we e selec ed. A e analyzing bo h ehicles, i was concluded hey bo h we e adequa e o es ing an Op imized Pa h Planning algo- i hm, al hough he Glide is be e sui ed o long ange missions. A e choosing he ehicles he da a collec ed in he OceanSys’ Slocum Elec ic Glide ’s mis- sion o he coas o Sesimb a in May 2013 was analyzed. The Dead Reckoning algo i hm com- pu es ai ly good esul s bu i is much mo e accu a e when allied wi h he pe iodic GPS signal upda e. An es ima e o he eal pa h ha he glide mus ha e ollowed was compu ed and he in luence o he GPS accu acy s udied (be e esul s come wi h a highe GPS accu acy). Also, an algo i hm o es ima ing he wa e cu en s’ was p oposed as well as i s inco po a ion in he DR algo i hm. The nex s ep was o de elop and implemen a pa h planning algo i hm o ma ine obo ics ough an op imiza ion echnique. This me hod has he pa icula i y o p edic ing he possibili y o some de ia ion om he compu ed pa h. A each check poin he unce ain y is quan i ied and a 51 52 Conclusions and u u e wo k new op imal pa h is compu ed o he emaining way poin s, allowing one o ha e he bes possible pa h om each mission check poin o he des ina ion. The o al a el ime was compu ed in wo di e en ways. The mos accu a e one, he di ec compu a ion o ime o e each segmen , be ween check poin s, equi es a highe numbe o poin s o e ine he in e pola ion. The e o e, al hough he di e en ial ime compu a ion is no as accu a e as he op imal ime p ocedu e i is close enough o he eal alue and much less compu a ionally expensi e. Also a p edic ion o he ehicle’s ue pa h was p oposed, o gi e some in ui ion abou he de ia ions e ec s on he mo emen . An ex ended abs ac on he subjec o chap e 4was submi ed o he Ocean’s 2013 con e - ence, in San Diego, and i was accep ed. The e o e a pape will be published in he men ioned con e ence on his opic. Finally, a e pe o ming poin o poin pa h planning, a scanning a ea algo i hm was p oposed. This one aimed a making his common mission ac i i y mo e e icien , in e ms o ime and ene gy consump ion. 6.1 Ful illmen o he de ined objec i es The main goals o his disse a ion we e ul illed since a pa h planning algo i hm based in an op imiza ion echnique was de eloped, implemen ed and es ed. As in ended, he a el ime was educed and he e o e he ene gy spen h oughou a mission consequen ly dec eased. Wi h he op imal pa i ioning algo i hm, an e icien me hod o scanning o a ce ain a ea wi h a de e mined numbe o a ge poin s was sugges ed. The e o e, one could say his algo i hm goes owa ds he maximiza ion o acqui ed da a in an e icien way. Fo he easons p esen ed abo e, one can conclude he objec i es o his disse a ion we e me , al hough he e is s ill some imp o emen s which could be pe o med. Thus in he nex sec ion u u e wo k will be p oposed. 6.2 Fu u e wo k The e a e se e al de elopmen s and u u e implemen a ions which one could pe o m, ela ed o he di e en algo i hms p esen ed in his disse a ion. Fu u e wo k migh be es ing and implemen ing he sugges ed algo i hms ela ed o he cu - en s’ map es ima ion. Also, he in luence o he GPS accu acy in he pa h compu a ion could be mo e ho oughly s udied as well as he cu en s’ in luence in he pa h. Rega ding he op imized pa h planning algo i hm conside ing unce ain y, he sc ip esponsi- ble o he wa e cu en s’ de ini ion could be cus omizable (e.g. eading a ex ile and con e ing i in a ec o ial ield). Also o he op imiza ion p ocedu es could be es ed and he “ac i e-se ” me hod subs i u ed o ano he o e en he mincon unc ion could be eplaced. 6.2 Fu u e wo k 53 S ill ega ding MATLAB’s adap ed sc ip , one could con e he cu en wo k in o de o pe o m a 3D op imiza ion, since wa e cu en s a y wi h dep h. Finally, he de eloped algo i hms should be es ed in a eal en i onmen and mission in o de o allow one o p o e and measu e he imp o emen s hey can p oduce. 54 Conclusions and u u e wo k Re e ences [1] Emili He nàndez Bes and Dipòsi Gi. Pa h planning wi h homo opic cons ain s o au- onomous unde wa e ehicles. 2012. [2] R. Siegwa and I.R. Nou bakhsh. Au onomous mobile obo s. Massachuse s Ins i u e o Technology, 2004. [3] G ego y Dudek and Michael Jenkin. Compu a ional p inciples o mobile obo ics. Cam- b idge uni e si y p ess, 2010. [4] R. Siegwa . Lec u e 11 Planning and Na iga ion, 2011. [5] Jönsson,Ul and T ygge ,Claes and Ög en,Pe e . Op imal Con ol, Lec u e no es o Op i- miza ion and Sys ems Theo y, 2011. [6] URL: h p://www.p ince on.edu/~s engel/Rosenb ock.jpg. [7] URL: h p://home.pos ech.ac.k /~pos man/Pa hPlanning.jpg. [8] URL: h p://www.p ism.ga ech.edu/~ejones7/images/ igu e_11.jpg. [9] Feb ua y 2013. URL: h p://3.bp.blogspo .com/_-u6ZJlBFOL0/ Suaw o90b6I/AAAAAAAAAF4/dIddhRs1KLY/s320/ORM.png. [10] Pie e F J Le musiaux, Thesis Supe iso , and Da id E Ha d . Pa h Planning Me hods o Au onomous Unde wa e Vehicles. 2011. [11] Clémen Pê ès, Yan Pailhas, Ped o Pa ón, Y an Pe illo , Jona han E ans, and Da id Lane. Pa h Planning o Au onomous Unde wa e Vehicles. 23(2):9–13, 2007. [12] Ba olome Ga au and Albe o Al a ez. Pa h Planning o Au onomous Unde wa e Vehicles in Cu en Fields wi h Complex Spa ial Va iabili y : an A *. (Ap il):194–198, 2005. [13] R.N. Smi h and M. Dunbabin. Con olled d i : An in es iga ion in o he con ollabili y o unde wa e ehicles wi h minimal ac ua ion. In P oceedings o he Aus alasian Con e ence on Robo ics and Au oma ion 2011, pages 1–10. Aus alian Robo ics & Au oma ion Associa- ion, 2011. [14] T Lolla, M P Uecke mann, K Yi, P J Haley J , and P F J Le musiaux. Pa h Planning in Time Dependen Flow Fields using Le el Se Me hods. 2012. [15] Jonas Wi , Ma hew Dunbabin, Csi o I C T Cen e, and P O Box. Go wi h he Flow : Op imal AUV Pa h Planning in Coas al En i onmen s †Au onomous Sys ems. 2008. 55