Homing and navigation using one transponder

Transcription

1 Homing and navigaion using one ransponder P. Baccou, B. Jouvenl, V. Creuze LIRMM 6, rue Ada 3439 Monpellier dex 5 Fran Absrac Homing and navigaion capabiliies are essenial in many Auonomous Underwaer Vehicle (AUV applicaions. This paper presens he boh problems wih respec o a single beacon. The difficulies of his approach are due o he fac ha a single range measuremen does no compleely consrain he beacon s posiion in he vehicle frame. In order o riangulae his posiion, he AUV need o maneuver while measuring is displamens beween ranges. In addiion, range measuremens are noisy and someimes spurious, speed bias and underwaer currens affec dead-reconing measuremens. Homing and navigaion produres are he same beginning. In a firs ime, we have an iniializaion phase which is nessary o obain an iniial esimae of he vehicle s absolue posiion (he beacon absolue posiion is nown onboard and of he disurbans affecing he qualiy of he dead reconed displamens (underwaer componens and speed bias. These iniials esimaes are refined during he acual displamen owards he beacon by means of a Kalman filer. Two inds of navigaion are used so as o maximize he informaion marix and o mainain an accurae absolue posiion.. INTRODUCTION Many AUVs are currenly operaed in he world, eiher as prooypes or more renly wih commercial purposes. The as consising of locaing he vehicle on he surfa afer compleion of is mission is someimes made difficul by sea sae and lighing condiions. The vehicle is hen generally fied wih one or several surfa relocaion sysems (srobeligh, radio beacon, radio ransmission of he GPS posiion. In some cases, more sophisicaed capabiliies such as homing and docing are required. During under-i operaions, for insan, he AUV has o be able o reurn o he launch hole for recovery. APL wih UARS [] and Seashule [], ISER wih Theseus [3], and MIT wih Odyssey [4] made under-i experimens where he vehicle was fied wih an acousic sysem providing he bearing angle of a beacon locaed near he launch hole. This measuremen combined wih he vehicle s heading was used o seer he AUV owards he beacon. For he long erm deploymen of AUVs (Auonomous Oan Sampling Newor conp, he vehicles mus be capable of reurning o heir moorings for recharging baery, daa downloading and mission programming. Many differen approaches have hen been implemened and esed based on acousic (Ulra Shor Base Line, Shor Base Line and Long Base Line sysem, elecromagneic or opical sensors [4][5][6]. Long baseline navigaion sysems have also been used renly for he navigaion of AUV [7][8][9][0]. Alhough, hese sysems can offer a good posiioning accuracy, provided he array is correcly calibraed, hey also have several drawbacs. The deploymen and he recovery of he ransponders as well as he calibraion of he array are shipime consuming and herefore expensive. Furhermore, he whole pross has o be repeaed each ime he array move o a differen pla. The overall cos of he sysem is also subsanial because of each beacon s cos. Therefore, a differen approach consising of using a single beacon is being sudied [][][3]. The advanage of such a soluion is ha he calibraion redus o he deerminaion of he beacon s locaion. Whereas he vehicle rac is disored by errors on he baseline calibraion in he classical long baseline approach, any beacon posiion calibraion error resuls in a shif of he AUV posiion equal o he calibraion error in he single beacon approach. This approach could appear very aracive a firs sigh. I has, however, an imporan disadvanage associaed wih he fac ha i is no possible o deermine he vehicle s posiion from a single. In classical long baseline navigaion, he replies o a single can be used o riangulae he AUV s posiion (provided he number of reurns is sufficien and he measured ranges are valid. Wih a single beacon, he AUV has o he beacon from differen plas in order o riangulae is posiion. The baselines beween ranges are hen creaed by he displamens of he AUV, which have o be measured which maximal accuracy. As opposed o he soluion presened in [3], which relies on an accurae dead reconing sysem o measure he displamens, our conrn is o provide a soluion for vehicles equipped wih low-cos sensors. The availabiliy of a Doppler velociy log is hen no an opion due o he sensor s cos, and we raher consider ha he vehicle s speed is approximaely nown by a priori calibraion of he vehicle s waer speed as a funcion of propeller s rpm. A his poin, however, we consider ha he vehicle has a reliable heading referen. The vehicle s speed, used in he compuaion of he displamens, is hen affed by a bias due o speed vs. rpm calibraion errors and by he unnown underwaer curren componens. These error sours will be esimaed ogeher wih he vehicle s posiion. Mos of he ime, he AUV now approximaely is own iniial absolue posiion even if i nows beacon s absolue posiion. This is he case if he AUV has o dive deep afer a surfa GPS fix, or if i has raveled for a long ime

2 underwaer before geing wihin range of he beacon. For his reason, we will assume ha he AUV has no nowledge of is iniial absolue posiion. We separae our posiioning algorihm in wo seps, he firs consising o compue an iniial esimae of he AUV s absolue locaion and perurbaions lie underwaer currens componens and speed bias. The second sep use a Kalman filer who permis o home he beacon or navigae in i area using informaion marix. The paper is organized as follows: secion II defines imporan referen frame. Secion III describes he iniializaion produre allowing o obain he iniial sae esimae and is error covarian marix used o iniialize he Kalman filer. Secion IV describes he Kalman filer in deails and paricularly how he vehicle s moion beween and reply is aen ino accoun. Secion V describes differen heading command for homing or navigaion hans o improve he informaion marix. Secion VI presens simulaion resuls.. DEFINITIONS The locaion of he beacon in he local absolue referen frame R 0 is (x b, y b, z b. This locaion can be nown or no by he AUV, i depends on he mission, if we wor in local referen, we use (0,0,z b locaion. The axes x 0 and y 0 respecively poin norh and eas, and z 0 poins down. The mobile frame R m is locaed a he ner of graviy of he vehicle wih is axes x m, y m, z m parallel o x 0, y 0, z 0. The nowledge of he beacon s deph allows o conver he 3D problem ino a D problem. The erminology range in he remaining of he paper should hen be undersood as x-y range (slan range corred for he deph differen beween he vehicle and he beacon. The acousic ravel imes are measured lie in a Long Base Line sysem: he vehicle s, he beacon replies upon repion afer a given imeou and he vehicle deecs he ime of arrival of he reply. Slan ranges are calculaed based on he nown average speed of sound. beween ranges i and n can hen be wrien: = n = n xi, n= CθCψ u ducθ Cψ v = i = i = n = n yi, n= CθSψ u ducθ Sψ v = i = i x 0 x b x R 0 R m y d n- d n d d beacon Fig.: Geomery of he iniializaion produre y b y 0 cn = n = = n = i i ( Le R m be he mobile frame a he ime corresponding o he end of he roaion wih he measure d n and (x,y be he vehicle s absolue posiion a ha ime. The i h range can be expressed as a funcion of he vehicle s posiion a he end of he roaion and of he displamen beween ranges i and n by: [( x ( ] / b-x xi,n yb-y yi,n di = (3 4 equaions 3 holes 3. INITIALIZATION 3.. Principle The iniializaion of he beacon s locaion consiss of commanding a 360 roaion o he AUV while ranging o he beacon and measuring he displamens beween s (Fig.. The vehicle would hus describe a circular pah in he absen of underwaer curren or a disored circle oherwise. In he presen of a speed bias (du and of an underwaer curren (v cn,v all assumed o be consan, he norh and eas componens of he vehicle s displamen over a sampling period can be modeled by: x= C θ C ( udu vcn ( y= CθSψ ψ ( udu v Where u is he vehicle s calibraed waer referend speed, θ is he pich and ψ is he heading. The displamens Crierion evoluion Crierion evoluion 6 equaions Convergen hole hole Convergen hole Fig. : Leas square evoluion of crierion The sysem of equaions (3 can be solved for (x,y,du,v cn,v by non-linear leas squares using he Levenberg-Marquard algorihm. However, because he problem can have local minima, we could no iniialize he search wih (0,0,0,0,0 wihou rising convergen o an erroneous soluion.

3 A sudy of leas square crierion shows he insabiliy of he soluion. Figure represens he minimal value of he leas square crierion wih respec o he locaion, he underwaer curren variaion is comprised beween /- m/s and he speed bias is no consider here. We can see ha: f(x P= X ( HPe H R f(x X (5 The number of equaions increases he soluion accuracy and a good convergen, The number of local minima decreases wih respec o he number of equaions. Therefore, in order o iniialize he algorihm closer o he correc soluion, he sysem wihou curren and speed bias is firs solved saring a (x,y=(0,0. The approximae vehicle posiion obained in ha case is hen used o iniiae he search in he presen of curren (he curren componens and he bias being sill iniialized a Range pre-prossing In our simulaions, we chose o use a selecion of 4 ranges for he esimaion of (x,y,du,v cn,v. Sin ranges can someimes be spurious because of noise or muli pah, he 4 ranges have o be seled carefully. The se of ranges is firs pre-filered as follows: he differen beween sucssive ranges is compued and a hreshold is applied. If he differen is greaer han he hreshold, furher esing is performed by looing a he ranges before and afer he considered pair. Depending on he resuls of hese addiional ess, eiher boh ranges are discarded or only one. A median mehod is hen applied o he remaining ranges: Selec 4 ranges randomly (depends on he noise and on he number of ranges, Compue he soluion o equaion (3, Chararize he soluion wih he median of he range residuals. Repea hese seps N imes and eep he se of 4 ranges providing he smalles median residual. N has o be chosen even larger ha he range measuremens are noisy. This is of course a he expense of a longer compuaion ime. For he simulaions, we used N=70 (i s because of he greaes of he ranges noises: 0% are spurious and 0% are affed by a Gaussian noise. We can use N=30 for 0% of spurious ranges and 5% wih a Gaussian noise bu we increase he probabiliy of erroneous soluion Unrainy on he iniial esimaion The sysem of equaions (3 can be expressed by: d = f( X, ψ, θ, u, ξ (4 Where d is he vecor of he seled ranges, X=(x,y,v cn,v,du, and ξ is a Gaussian noise on he measured ranges wih varian marix R. The non-linear leas squares opimizaion provides he covarian marix of X (Cramer-Rao lower bound: Where H is he Jacobian of he ranges wih respec o ψ, θ, u and P e is he covarian marix of ψ, θ, u. The resul of he opimizaion is used o iniialize he sae vecor of he Kalman filer described below, and he covarian marix P is used o iniialize he esimaion error covarian marix of he filer. 4. EXTENDED KALMAN FILTER 4.. Sae equaion The sae vecor is made up of he vehicle s posiion (x,y in R 0, he vehicle s deph (z, he componens of he underwaer curren (v cn,v and he speed bias (du. The inpus are he vehicle s heading ψ, is pich θ and is waerreferend speed u. The covarian marix of he inpus is he diagonal marix Cin= diag( σ ψ, σ θ, σu. The curren and he speed bias are modeled as consan. The sae noise vecor is v and is covarian marix Q. The sae equaion is hen expressed by: x v z y du x v z y = udu ( udu C C θ C v θsψ ψ v Sθ( udu cn cn du Observaion equaion cn v The observaion equaion expresses he acousic round-rip ime of fligh (T as a funcion of he vehicle s sae a he ime of he and a he ime he vehicle reives he reply from he beacon. The vehicle s moion beween he and he repion is hen aen ino accoun, and observaions are expressed in erms of all he sae variables. The measuremen noise on he ravel imes is represened by w. I varian is R. The speed of sound is noed c and he beacon urn around ime r. T= {[ ( xbx ( yb y ( zbz ] / c (7 / xbxrep yb yrep zbzrep r w [( ( ( ] } In general, a ime of fligh is buffered by he acousic ranging sysem and made available for prossing afer a prese imeou following he ime. The vehicle s esimaed sae a ha ime is X ˆ /. The predicion sep being applied firs, he vehicle predid sae is X ˆ /. Sin he sae predicion does no modify he curren componens and he speed bias esimaes, hese esimaes are consan from he ime o he ime of fligh prossing (6

4 ime. The vehicle s posiion is hen bac propagaed in ime using he curren sae and he inpus, which were memorized sin he. Sin he ime ( is nown and he repion ime ( rep can be calculaed by adding he ime of fligh o he ime, he posiion of he vehicle a he and a he repion of he reply can hen be calculaed by: x y z x y z rep = x = y = z rep rep / = x = y = z / / / / / / [ Cθ icψ(u i idu / vcn / ] / [ Cθ isψ(u i idu / v / ] / Sθ(u i idu / [ Cθ icψ(u i idu / vcn / ] = rep / [ Cθi Sψ(u i idu / v /] i / rep / Sθ(u i idu rep / 4.3. Equaions of he filer The filer equaions are differen from he classical equaions in ha inpus U =(ψ,θ,u can be found boh in he sae and he observaion equaions. Equaions (6 and (7 can be rewrien: X Z = f(x,u v = h(x,u w The filer proeds in wo seps. The firs sep is a predicion of he sae based on he inpus in U. I basically corresponds o dead reconing using he las esimaes of he underwaer curren. The sae is predid ogeher wih is predicion error covarian P / : X P / / = = f(x/,u F P/F JC J Q in (8 (9 (0 The second sep aes pla when a ime of fligh is available. The predid sae is corred based on he informaion carried by he new measuremen. The predicion error covarian marix P / is also compued: X P / / = X = P / / K K ( H ( Z h( X /, U P / S The following Jacobian maris have o be calculaed: ( f(x,u F = X h(x,u H = X XU = XU /, f(x,u J= U h(x,u D = XU = XU /, XU = XU /, U XU = XU /, ( S = J C in D is a correlaion erm accouning for he fac ha inpus are boh in he sae and he observaion equaions. Because of his correlaion, he expression of he Kalman gain is a lile more complicaed han usual: K = ( P /H S ( H P H D C D R H S S H / in 4.4. Time of fligh validaion (3 When a ime of fligh is available, is validiy is firs ched by means of he Mahalanobis disan. The innovaion and is varian are calculaed by: = T h(x /,U (4 ν Pν = H H P / S H S D H C in D R (5 The ime of fligh T is validaed and used o correc he predid sae if i passes he following es: ν P υ ν < γ 5. VEHICLE NAVIGATION wih γ ypically equal o 3. (6 A sudy of he informaion marix similar o ha described in [4] has showed ha he rajecory ha maximizes he informaion (or conversely minimize he esimaion error covarian consiss of leaving he beacon on eiher side of he vehicle so ha he vehicle s heading deviaes by 90 from he line joining he vehicle and he beacon (he vehicle describes a circle nered a he beacon. In order o increase he informaion, we use he following produre. A he end of he 360 roaion, he vehicle is commanded o move a 90 wih respec o he beacon. When his condiion is achieved, he volume of he predicion error covarian ellipsoid is calculaed by: C 0 = de(p (7 When a new ime of fligh is made available and used by he filer, he volume is calculaed for he new covarian marix: C = de(p/ (8 The homing is no saisfying sin he vehicle never

5 reaches he beacon. Going sraigh o he beacon is, however, neiher a desired configuraion because he range measuremens canno correc efficienly laeral errors along he vehicle rac, so ha he vehicle could acually miss he beacon. In he case of speed bias is considered, he heading command law described in [] do no saisfied he speed convergen even if we change he speed of he vehicle during homing. We noi ha he deviaion arrived quicly o 56. We inrodud he disan o he beacon ( range in he deviaion angle: π C ψ = ψ b exp * range* (9 C C 0 Where is a speed homing coefficien. I s very difficul for he filer o converge o he real speed bias when heading is changing all he ime (he speed bias is considered on he longiudinal vehicle axis. For his reason, we eep he same direcion during some measuremens before compue he new heading. The case of navigaion is simples, he vehicle eeps circling around he beacon unil he raion of C over C 0 is smaller ha a prese hreshold (funcion of N. The AUV hen reaches is firs waypoin and sars doing is survey. A his poin he vehicle sill uses he filer o esimae is posiion during he survey. 6. SIMULATION RESULTS The algorihm described above has been esed in simulaion using he simulaor of he orpedo-shaped AUV Taipan developed a LIRMM [5]. In he homing simulaion, he curren was se so as o have a 0. m/s magniude and a 60 direcion. The speed bias was se a 0. m/s. In order o simplify he visualizaion of he racs, he absolue posiion of he beacon is subrad from all he absolue posiions. The beacon hen appears o be locaed a (0,0 on he plos. During he iniializaion phase, he vehicle dead-recons is displamens beween ranges wihou any nowledge of is absolue iniial posiion. For he plos, however, he deadreconed posiion was iniialized by he vehicle s acual iniial posiion. Afer he iniializaion, he filer provides he esimaed posiion of he vehicle in R 0, which is used o seer he AUV so as o leave he beacon a 90. We presen resuls for homing and wo differen vehicle racs. The firs rac consiss of homing (Fig.3. The second rac consiss of parallel legs (Fig.4 wih a curren of 0.4m/s magniude and 97 direcion, he speed bias is 0.9m/s. The hird rac consiss of radial legs (Fig.5 wih a curren of 0.m/s magniude and 7 direcion, he speed bias is 0.5m/s. Fig.3 : homing rajecory (dar: esimaed, ligh: acual and perurbaions resuls Throughou he simulaion, he ranges (or imes of fligh were simulaed so ha 0% of hem would be spurious, 0% would be affed by a Gaussian noise wih a 0m sandard deviaion, and he remaining would be affed by a Gaussian noise wih an 0.5m sandard deviaion. When you realize homing, he difficuly is o redu he disan wihou deeriorae filer soluions. In figure 3, we can see he difficul speed bias convergen. This convergen is more precise when you bring nearer o he beacon. We hin ha his is he fac o dead-reconed and disan are in he same order. The AUV sars is 360 roaion a he square mar locaed a abou (-00,-00 wih an iniial heading of abou 35. The dead-reconed pah is a circle (dar line sin he vehicle is no aware of he curren. The acual vehicle rajecory is show by he disored circle (ligh line. Fig.4 : Parallel rajecory (dar: esimaed, ligh: acual and perurbaions resuls Fig.5 : Radial rajecory (dar: esimaed, ligh: acual and perurbaions resuls The esimaed posiion of he vehicle a he end of he iniializaion is shown by a cross a abou (-00,-50 in figure 4. I can be seen ha his esimaed posiion is very close o he acual posiion of he vehicle afer he disored circle. The esimaed posiion hen jumps from he end of he

6 circle o hese coordinaes. From here, he acual and esimaed rajecories are very close o each oher and remain so unil he end. The esimaes of he underwaer curren and speed bias componens along he runs are shown in figure 4 and 5 (he sraigh lines show he acual curren and speed bias componens. I can be seen ha he esimaes converge o he correc values during he 90 navigaion phase. The radial rac gives beer resuls principally because of he vehicle ravels a varying headings and during differen sraigh line. 7. CONCLUSION In addiion o is ineres as an alernae soluion o classical long baseline, we believe ha he range-only soluion is worh sudying in he more general framewor of muliple vehicle operaions. Navigaion wih respec o a beacon by range measuremens supposes ha he vehicle is able o esimae is posiion relaive o he beacon. If he beacon becomes mobile (carried by anoher AUV, hen he AUV could be able o deermine is relaive posiion exacly in he same way, provided ha he beacon can ransmi is deph and displamens o he AUV by acousic communicaions. Assuming cloc synchronizaion beween muliple AUVs, i is hen possible o consider having a maser vehicle ha would and ransmi is displamens and have he oher AUVs deermine heir posiion relaive o he maser AUV. The simulaion resuls presened in his paper were obained using he simulaor of he Taipan AUV developed a LIRMM (Fig.6. An acousic modem incorporaing a range measuremen capabiliy o a beacon has renly been inegraed in Taipan, wih he objecive of experimening he algorihm. In addiion o he modem, he vehicle is fied wih a pressure sensor, pich rae and yaw rae gyromeers, a DGPS, a magneic compass, a pich and roll inclinomeer. The vehicle speed is based on a priori propeller rpm / speed calibraion, so ha esimaion of he underwaer curren is criical. Oher fuure wors conrns movemen generaion and docing. Fig.6 : Taipan wih he acousic modem ransdur (lengh:.8m, diameer: 5 cm, weigh: 30 g REFERENCES [] R.E. Francois e al, Unmanned Arcic research submersible (UARS sysem developmen and es repor, Technical Repor, n APL-UW 79, Applied Physics Laboraory, Universiy of Washingon, 97. [] J. Osse e Russell Ligh, Line deploymen by a miniaure AUV under Arcic i, [3] B. Buler, Field rials of he Theseus AUV, 9 h Unmanned Unehered Submersible Technology, Durham NH, 5-7 sepembre 995. [4] J.G. Bellingham e al, AUV operaions in he Arcic, Sea I Mechanics and Arcic Modeling Worshop proedings, Anchorage, Alasa, USA, April 5-8, 995. [5] D.R. Yoerger e al, Sysem esing of he Auonomous Benhic Explorer, IARP 94 worshop, Monerey, 994. [6] S. Cowen e al, Underwaer docing of auonomous undersea vehicles using opical erminal guidan, MTS/IEEE Oans 97, Halifax, 6-9 Ocobre 997. [7] J.G. Bellingham, T.R. Consi, U. Tedrow and D. Di Massa, Hyperbolic acousic navigaion for underwaer vehicles: implemenaion and demonsraion, Proedings of he 99 symposium on Auonomous Underwaer Vehicle Technology, June -3, Washingon, DC, USA. [8] D.K. Awood, J.J. Leonard, J.G. Bellingham, and B. A. Moran, An acousic navigaion sysem for muliple vehicles, 9h In. Symp. on Unmanned Unehered Submersible Technology, pp.0-08, New Hampshire, Sepember 5-7, 995. [9] J. Vaganay, J.G. Bellingham and J.J. Leonard, Comparison of fix compuaion and filering for auonomous acousic navigaion, Inernaional Journal of Sysems Scien, vol.9, n 0, pp. -, 998. [0] A. Maos, N. Cruz, A. Marins and F.L. Pereira, Developmen and implemenaion of low-cos LBL navigaion sysem for an AUV, OCEANS 99 MTS/IEEE. Riding he Cres ino he s Cenury, volume, pages [] A.P. Scherbayu, The AUV posiioning using ranges from one ransponder LBL, OCEANS 95 MTS/IEEE. Challenges of Our changing Global Environmen. Conferen Proedings., Volume 3, pages [] J.Vaganay, P. Baccou, B. Jouvenl, Homing by acousic ranging o a single beacon, OCEANS 00 MTS/IEEE, Conferen and Exhibiion, Proedings Volume, pages , Sepember -4, Providen, RI, 000. [3] M.B. Larsen, Synheic long baseline navigaion of underwaer vehicles, OCEANS 00 MTS/IEEE, Conferen and Exhibiion, Proedings Volume 3, pages , Sepember -4, Providen, RI, 000. [4] J. S. Feder, J. J. Leonard, C. M. Smih, Adapive mobile robo navigaion and map, In. Journal of Roboics Research, Special Issue on Field and Servi Roboics, July 999. [5] J.Vaganay, B. Jouvenl, P. Lepinay, R. Zapaa, An AUV for very shallow waer applicaions, World Auomaion Congress, Anchorage, Alasa, USA, May 0-4, 998.