AUV positioning based on Interactive Multiple Model

Transcription

1 AUV posiioning based on Ineracive Muliple Model H. Q. Liu ARL, Tropical Marine Science Insiue Naional Universiy of Singapore 18 Ken Ridge Road, Singapore Mandar Chire ARL, Tropical Marine Science Insiue Naional Universiy of Singapore 18 Ken Ridge Road, Singapore Gao Rui ARL, Tropical Marine Science Insiue Naional Universiy of Singapore 18 Ken Ridge Road, Singapore Absrac The research opic of auonomous underwaer vehicles (AUVs) has araced much aenion over years since hey provide marine researchers easy ways o access he ocean for surveying and sie invesigaion, ec. To accomplish hese applicaions, an AUV has o know is posiion accuraely. Therefore, AUV localizaion is very imporan problem. In his paper, we propose an ineracive muliple model (IMM)-based mehod for AUV localizaion because his mehod is capable of ackling complex behaviors of vehicles wih differen dynamic models. Several filering echniques, namely, Kalman filer (), paricle filer (PF) and modified PF (MPF), are invesigaed o esimae he posiion of he AUV. In developmen of he MPF, an l 1-norm is used o compue paricle s cos insead of heir weigh o allow us o operae he filer wihou he use of measuremen informaion. Those filers are running in parallel and he esimaes are inegraed by he IMM-based algorihm o obain he posiion of he AUV. The sensor uni onboard consiss of global posiioning sysem (GPS), Doppler velociy log (DVL), inerial measuremen uni (IMU) and a digial compass. Differen dynamic models are sudied o demonsrae he performance of he IMM-based mehods, namely, IMM-, IMM-PF and IMM- MPF. Field rials using he STARFISH AUV show he capabiliy of he algorihm. I. INTRODUCTION The research opic of auonomous underwaer vehicles (AUVs) has araced much aenion over years since hey provide marine researchers easy ways o access he ocean for surveying and sie invesigaion, ec [1]. To accomplish hose applicaions, AUV has o know is posiion all he ime. Therefore, AUV localizaion is very imporan issue. Several echniques have been developed using Doppler velociy log (DVL), inerial measuremen uni (IMU), global posiion sysem (GPS), acousic or opical sensors o esimae he posiion of he AUV. In [], an acousic-based localizaion echnique is developed by using freely floaing acousic buoys equipped wih GPS device. The problems is ha he floaing buoys mus be deployed in he area of ineres in advance o be able o make his work. This limiaion is eviden for a lo of applicaions since someimes we do know much abou he working area and we have o carry enough buoys every ime. Oher acousic-based localizaion echniques include he long baseline (LBL) [3] and shor baseline (SBL) [] sysems. For he LBL sysem, number of seafloor ransponders is deployed underwaer. Then, AUV can localize iself using disance informaion beween he AUV and ransponders. For he SBL sysem, using high-frequency direcional emier, a suppor ship can esimae he AUV posiion wih respec o he moher ship. The usage of addiional devices such as, ransponders and suppor ship, can really limi he applicaions of such sysems. To make AUV localizaion more independen, several echniques involving using onboard DVL, IMU, and GPS as well, are developed. In [5], an inegraed DVL/IMU navigaion sysem is presened based on exended, bu ranging aid is sill used in heir sysem. In [6], a paricle filering approach is developed o improve posiioning accuracy based on GPS/DVL/IMU measuremens. In his paper, we are ineresed in AUV posiioning using GPS/IMU/DVL daa. Since he movemen of he AUV is complicaed, he ineracive muliple model (IMM) mehod is presened o combine sae esimaes from individual filers. The IMM-based mehods have been used o improve he posiioning accuracy in navigaion sysems. In our work, IMMbased approaches combined wih several filering echniques such as, Kalman filer (), paricle filer (PF) and modified PF (MPF) are invesigaed. The is a opimal sae esimae of linear and Gaussian sae space models. and is varians [7] are mos commonly adoped filering mehods o esimae he sae. However, he performance of he s degrades as he sysem becomes nonlinear or non-gaussian [7]. In such siuaion, PF is a more suiable choice since i does no requires lineariy in sysem or Gaussianiy in noise. PF uses random samples, called paricles, o approximae he poserior funcion a every ime sep. The approximaed poserior funcion approaches he rue one when he number of paricles goes o infiniy [7], [8]. One problem in implemenaion of PF is ha he noise disribuion mus be needed o compue he imporance weigh of paricles. However, in realiy, his informaion is eiher no available or no accurae. Therefore, a more robus approach, called MPF, is developed wihou noise disribuion. In developmen of he MPF, l 1 -norm is used o calculae he paricles cos insead of heir imporance weighs. To cooperae wih he IMM, in his paper, hree dynamic models, namely, consan velociy (CV), consan acceleraion (CA) and consan urn (CT) are sudied o demonsrae he performance of he proposed mehods. The res of paper is organized as follows. In Secion II, he algorihm developmen including inroducion o Bayes filer,

2 , PF, as well as IMM mehod and hree differen dynamic models is presened. Secion III demonsraes he performance of he proposed approaches using field rial daa. In Secion IV, he conclusions are drawn. II. ALGORITHM DEVELOPMENT A. Bayes Filer-Concepual Soluion To define a racking sysem, consider he following dynamic sae space model: x = f (x 1, v 1 ) (1) z = h (x, n ) where f ( ) is a ransiion funcion, which describes he evoluion of he sae wih ime, h ( ) is a measuremen funcion, which defines he relaionship beween noisy observaions and he sae, v 1 is a independen and idenically disribued (i.i.d.) process noise, n is a i.i.d. measuremen noise, v and n are muually independen. In order o rack a arge under Bayesian framework, we need o calculae he poserior probabiliy disribuion funcion (PDF) of he sae, i.e., π(x z 1: ), where z 1: = {z 1, z,, z } denoes all he observaions up o he curren ime. Le he iniial densiy of he sae vecor be π(x )=π(x z ), where z means no measuremens. The PDF π(x z 1: ) is obained recursively in wo sages, namely, predicion and updae. Assuming ha a ime ( 1) he required PDF π(x 1 z 1: 1 ) is available, he predicion densiy of he sae a ime is obained by he following equaion [8], [9] π(x z 1: 1 )= p(x x 1 )π(x 1 z 1: 1 )dx 1 () A ime he observaion z becomes available, he updae sage is performed. Via he Bayes rule, an updae of he predicion densiy is given as π(x z 1: ) p(z x )π(x z 1: 1 ) (3) where p(x x 1 ) is ransiion disribuion defined by sae model, p(z x ) is he likelihood funcion defined by measuremen model. The recursive propagaion of he poserior densiy, using () and (3), is only a concepual soluion in he sense ha in general i canno be deermined analyically. In wha follows, wo famous filers, namely, and PF, o solve he above dynamic sysem are inroduced in deails. B. Kalman Filer The is an opimal soluion o he sysem (1) where i becomes linear and Gaussian. For such sysem, (1) can be rewrien as: x = F x 1 + v 1 () z = H x + n where F and H are known linear funcion, v 1 and n are Gaussian noise defined by covariance marix Q 1 and R, respecively. Anoher assumpion o use is ha he iniial densiy π(x ) is Gaussian disribued. The algorihm is given as follows: [1] Predicion: Updae: x 1 = F x 1 1 P 1 = Q 1 + F P 1 1 F H (5) x = x 1 + K (z H x 1 ) P = P 1 K H P 1 (6) K = P 1 H H (H P 1 H H + R ) 1 where K is called Kalman gain. Like we menion earlier, is only applicable o linear and Gaussian sysem. For nonlinear sysem, Taylor expansion is exploied o obain socalled exended (E). However, he E sill suffers from large performance loss when he sysem is severely nonlinear. In wha follows, a echnique called PF is inroduced o handle he nonlineariy and non-gaussian problems. C. Paricle Filer PF uses smarly generaed random sample o approximae he poserior funcion, which is an efficien way o solve nonlinear and/or non-gaussian problems [7]. The key idea behind PF is o represen he required poserior densiy funcion by a se of random samples wih associaed weighs and o compue esimaes based on hese samples and weighs. According o he law of large numbers, his Mone Carlo mehod becomes an equivalen represenaion of he usual funcional descripion, and he sequenial imporance sampling approaches he opimal Bayesian esimaor. Given he large se of N paricles {x (i) 1 }N i=1 and heir associaed weighs {w(i) 1 }N i=1.the poserior densiy a ime ( 1) is approximaed as π(x 1 z 1: 1 ) N i=1 w (i) 1 δ(x 1 x (i) 1 ) (7) where δ() is he Dirac dela funcion. Moreover, he new paricles {x (i) } N i=1 are generaed from he properly designed proposal funcion: x (i) q(x x (i) 1, z 1:), i =1,,N (8) While he imporance weigh w (i) w (i) w (i) 1 p(z x (i) is recursively updaed as )p(x (i) x (i) 1 ) q(x x (i) 1, z 1:) Based on he new paricles and heir associaed weighs, he minimum mean square error esimae is [7] N ˆx = E[x z 1: ]= x π(x z 1: )dx w (i) x (i) (1) i=1 where E denoes he expecaion operaor. In PFs afer a cerain number of recursive seps, all bu one paricle will have negligible weighs, leading o he degeneracy phenomenon. In order o avoid his problem, he resampling sep mus be aken. Resampling eliminaes samples wih low imporance weighs and muliplies samples wih high imporance weighs, and he (9)

3 deails can be found in [7]. One problem abou using PF is ha one has o know he likelihood funcion in (9). In pracice, i is very likely eiher we canno have his informaion or he likelihood funcion is inaccurae. Therefore, o overcome his problem, we propose a MPF in which he measuremen noise informaion is unnecessary. In he imporance weigh calculaion sep, we use l 1 -norm o compue he paricle cos, i.e., c (i) = z h (x i ) 1 w (i) =1/c (i) (11) Therefore, his sep replaces sep (9) for paricle imporance weigh updae in PF. D. Ineracive Muliple Model Single model is no sufficien enough o capure he movemen of he AUV, since is rajecory migh be very complex underwaer. Therefore, he IMM mehod is used o improve he posiioning accuracy. In he IMM approach, he sae esimaes from each filers are combined according o a Markovian model for he ransiion beween differen models. The IMM mehod is described in following four seps: Ineracion: The mixing probabiliies μ j i 1 1 for each model M i and M j are calculaed as n c j = p ij μ i 1 i=1 μ j i 1 1 = 1 c (1) p ij μ i 1 j where μ i 1 is he probabiliy of model M i in he ime sep ( 1), c j a normalizaion facor and p ij is he ransiion probabiliy from model M i o M j. Now we can compue he mixed inpus for each filer as x i 1 1 = j P i 1 1 = μ j i 1 1 {Pj j ( x i 1 1 ˆxj 1 1 ) ( xi 1 1 ˆxj 1 1 )T } μ j i 1 1ˆxj 1 1 (13) Filering: Now, for each model he filering is done by using /PF/MPF o compue he sae esimae ˆx i and covariance Pi according o he mixed inpus calculaed in ineracion sep. Model Probabiliy Updae: The probabiliies of each model M i a ime are calculaed as where Λ i is calculaed as c = Λ i c i i μ i = 1 (1) c Λi c i Λ i = N(v i ;, S i ) (15) where N( ) represens Gaussian disribuion, v i is he measuremen residual and is S i i s covariance. Combinaion: The combined sae esimae ˆx and is covariance P i are now calculaed as ˆx = μ i ˆxi i P = i μ i {Pi +(ˆx ˆx i 1 1 ) (ˆx ˆx i )T } (16) E. Dynamic Model Se To capure he movemen of he AUV underwaer more accuraely, hree differen dynamic models are sudied. The CV, CA and CT, which are described as follows, respecively. CV: The CV model is a nearly-consan-velociy one, since acceleraions along x and y direcions are modeled as small whie noise. The sae is defined as x =[x,y, ẋ, ẏ ] T and he ransiion model is F = 1 T 1 T 1 1 CA: To accoun he acceleraions in he movemen, he CA model is inroduced. The sae is defined as x = [x,y, ẋ, ẏ, ẍ, ÿ ] T and he ransiion model is F = 1 T T / 1 T T / 1 T 1 1 where T is he ime sep. CT: Someimes, he AUV may make urn, in order o capure his behavior, he CT model is invesigaed. The sae in CT model is he same as in CV model. The corresponding ransiion model is F = 1 sin(ωt)/ω (cos(ωt) 1)/ω 1 (1 cos(ωt))/ω sin(ωt) cos(ωt) sin(ωt)/ω sin(ωt) cos(ωt) where ω is he urn rae. Noe ha o use IMU measuremens, we have o consider he bias inroduced by he IMU. The bias is modeled as random walk for each direcion. Therefore, wo addiional saes [bx,by ] T for IMU bias in acceleraion are inroduced and he ransiion model for he bias is bx = bx 1 + w bx (17) by = by 1 + w by III. EXPERIMENTAL RESULTS In his secion, field underwaer rials are conduced o evaluae he performance of he proposed mehods from he STARFISH AUV [11]. In he es, he number of paricles

4 are 1, he urn rae for CT model is se o be.1g, where g is he acceleraion of graviy. A. GPS/DVL performance 15 1 Third segmen GPS daa The firs rial was conduced in Nov. 8 in Pandan reservoir in Singapore. In his es, he GPS and DVL measuremens were colleced for AUV posiioning. Two dynamic models, namely, CV and CT models, are used for he posiioning algorihm. The AUV surfaced hree imes during rial o obain GPS daa for ground ruh, as shown in Figure 1. In our sysem, GPS device direcly oupus he coordinaes of he AUV. We model hose measuremen equaion in GPS as noisy observaion of rue coordinaes, i.e., z x, = x + n x, (18) z y, = y + n y, where n x, and n y, areassumedobegaussian.thedvl gives he velociy of he AUV. We formulae hose measuremens in DVL as noisy observaions of rue velociy same as in GPS, herefore, we have: z vx, = v x, + n vx, (19) z vy, = v x, + n vy, The ransiion marix in IMM for his es is [ ].98. =..98 Figure 1 shows he racking resul obained by he proposed mehods. I is seen ha all filers have similar performance, which demonsraes he effeciveness of he MPF approach. The deailed figure 1 is presened in Figure. In Figure 3, he disance errors are ploed when he GPS daa is available. From he figure, i can be seen ha IMM-PF performs he bes and IMM- has gains over. In Figure, he esimaed model ransiion probabiliy is shown using. B. GPS/IMU performance The second rial was conduced in Jul. 9 in he same locaion. In his es, he GPS and IMU measuremens were colleced for AUV posiioning. All hree dynamic models are used for he posiioning algorihm. The same measuremen equaion for GPS is used as in es A. For IMU measuremen, he acceleraions along x and y direcions are formulaed as follows: z a = Cn,[a b x, a y, ] T + b a, + n a () where Cn, b is he direcion cosine marix, b a, is he bias erm inroduced by he IMU. The ransiion marix in IMM for his es is = The racking rajecory is shown in Figure 5 and disance errors when GPS daa is available are presened in Figure 6. y ccordiane (m) 5 5 Second segmen Firs segmen x coordinae (m) y ccordiane (m) Fig. 1. Trajecory racking resul. GPS daa x coordinae (m) Fig.. Trajecory racking resul in deail. Their deailed informaion are presened in Figure 7 and 8, respecively. From he figures, i is observed ha he IMM-PF gives he bes performance again. In Figure 9, he esimaed model ransiion probabiliy is shown using. IV. CONCLUSION In his paper, posiioning algorihms for use in an AUV are invesigaed. To improve he accuracy, he IMM-based posiioning mehods using, PF and MPF are presened. Several dynamic models, namely, CV, CA and CT, are also sudied. From he field rial resuls, i is seen ha he posiioning algorihms demonsrae heir effeciveness. ACKNOWLEDGMENT The auhors would like o hank STARFISH AUV eam in ARL for collecing he rial daa used in his paper. REFERENCES [1] Y. Zhang, A. B. Baggeroer, J. G. Bellingham, Specral-feaure classificaion of oceanographic processes using an auonomous underwaer vehicle, IEEE Journal of Oceanic Engineering, vol. 6, no., Oc. 1, pp

5 Firs segmen Second segmen Third segmen Firs segmen Second segmen Fig. 3. Disance esimaion error. Fig. 6. Disance esimaion error. Model probabiliy Firs model Second model y ccordiane (m) GPS daa Disance (m) x coordinae (m) Fig.. Model probabiliy ransiion using. Fig. 7. Trajecory racking resul in deail. y ccordiane (m) Firs segmen GPS daa Second segmen x coordinae (m) Firs segmen Second segmen Fig. 5. Trajecory racking resul. Fig. 8. Disance esimaion error in deail.

6 .9.8 Model probabiliy Firs model Second model hird model Disance (m) Fig. 9. Model probabiliy ransiion using. [] A. Caii, A. Garulli, F. Livide, and D. Praichizzo, Localizaion of auonomous underwaer vehicles by floaing acousic buoys: a semembership approach, IEEE Journal of Oceanic Engineering, vol. 3, no. 1, Jan. 5, pp [3] N. H. Kussa, C. D. Chadweell and R. Zimmeramn, Absolue posiioning of an auonomous underwaer vehicle using GPS and acousic measuremens, IEEE Journal of Oceanic Engineering, vol. 3, no. 1, Jan. 5, pp [] N. Sorkensen, J. Krisensen, A. Indreeide, J. Seim, and T. Glancy, Huginuuv for seabed survey, Sea Technol., 1998 [5] P.-M. Lee, B.-H. Jun, K. Kim, J. Lee, T. Aoki, and T. Hyakudome, Simulaion of an inerial acousic navigaion sysem wih range aiding for an auonomous underwaer vehicle, IEEE Journal of Oceanic Engineering, vol. 3, no., Apr. 7, pp [6] A. K. lammas, K. Sammu, F. He, Improving navigaional accuracy for AUVs using he mapr paricle filer, Proc. of Oceans 8, Quebec Ciy, Canada, Sep. 8 [7] B. Risic, A. Arulampalam and N. Gordon, Beyond he Kalman Filer- Paricle Filers for Tracking Applicaions, Arech House, Boson, [8] M. S. Arulampalam, S. Maskell, N. Gordon and T. Clapp, A uorial on paricle filers for online nonlinear/non-gaussian Bayesian racking, IEEE Trans. Signal Processing, vol. 5, no., Feb., pp [9] N. Bergman, Recursive Bayesian Esimaion: Navigaion and Tracking Applicaions, PhD Thesis, Linkoping Universiy, Sweden, [1] M. S. Grewal, Kalman Filering: Theory and Pracice using MATLAB, New York : John Wiley, 1. [11] STARFISH - small eam of auonomous roboic fish. [Online]. Available: hp://arl.nus.edu.sg/wiki/bin/view/arl/starfish