A waypoint guidance strategy for underwater snake robots

Transcription

1 A waypoit guidace strategy for uderwater sake robots E. Kelasidi, K. Y. Petterse ad J. T. Gravdahl Abstract I this paper, a waypoit guidace strategy is proposed for a uderwater sake robot. The robot is directed to follow a path which is derived usig path plaig techiques. A first versio of the path is derived by the path plaer by usig the artificial potetial field method for obstacle avoidace. Afterwards, by subsamplig the derived path, a set of waypoits are chose alog the path. The path is the defied by itercoectig these waypoits by straight lies. Secodly, a straight lie path followig cotroller is proposed, to make the uderwater sake robot follow the desired path. Simulatio results are preseted, illustratig the performace of the proposed guidace cotrol strategy for both lateral udulatio ad eel-like motio. I. INTRODUCTION For ceturies, egieers ad scietists have gaied ispiratio from the atural world, while searchig for ideal solutios to techical problems. More recetly, this process has bee termed as biomimetics. Every biological orgaism livig i a aquatic eviromet, swims by geeratig a propulsive force through the iteractio betwee the body ad the surroudig fluid that is created through a rhythmic body movemet. Geerally, studies of hyper-redudat mechaisms (HRMs), also kow as sake robots, have largely restricted themselves to lad-based studies, while several models for sake robots have bee proposed. Empirical ad aalytic studies of sake locomotio were reported by Gray i, while, amog the first attempts to develop a sake prototype, the work of Hirose 3 is essetial. Recetly, HRMs are preseted that are suited for aquatic propulsio as well, 5,. Motio plaig, i.e. a path coverig a certai area or movig the robot towards a desired locatio-goal takig ito accout eergy cosumptio 7, is a challegig task for uderwater sake robot locomotio. Oe importat feature of motio plaig is obstacle avoidace. Algorithms for obstacle avoidace for robots appeared as early as mid-98s 8, 9. proposes the use of vector field histograms to steer the robot towards the directio of low obstacle desity areas. I, the idea for potetials creatio from obstacles that repel the robot ad potetials from the target that attracts it, is preseted. presets a solutio for collisio ad obstacle free formatio flight ad recofiguratio of groups of autoomous helicopters. Artificial Potetial Fields (APF) E. Kelasidi, ad J. T. Gravdahl are with the Departmet of Egieerig Cyberetics at NTNU, NO-79 Trodheim, Norway. {Elei.Kelasidi,Tommy.Gravdahl}@itk.tu.o K. Y. Petterse is with the Cetre for Autoomous Marie Operatios ad Systems, Dept. of Egieerig Cyberetics at NTNU, NO-79 Trodheim, Norway. Kristi.Y.Petterse@itk.tu.o This work was partly supported by the Research Coucil of Norway through project o. 5 ad its Cetres of Excellece fudig scheme, project o. 35 have sice bee utilised i various applicatios as they offer a fast ad simple method for obstacle avoidace 3. Also, this method is used by i the case of a fish robot. I this paper we will use artificial potetial fields as the first step i the motio plaer, creatig a first rough descriptio of the desired path. Give the desired path that the robot should follow, the ext task is to develop a path followig cotroller. Regardig cotrol for uderwater sake robots, several types of cotrollers have bee proposed i the literature, 5,, 7. However, it is worth metioig that the emphasis so far has maily bee o achievig forward ad turig locomotio. The ext step would be ot oly to achieve forward locomotio, but also to make the sake robot follow a desired path, i.e. solvig the path followig cotrol problem, ad this paper presets prelimiary results towards this ed. Research o robotic fish ad uderwater sake robots (also referred to as eel-like robots) are related sice these mechaisms have importat similarities. The works of, 8 ad 9 sythesize gaits for traslatioal ad rotatioal motio of various fish-like mechaisms ad propose cotrollers for trackig straight ad curved trajectories. The work of 5 demostrates the use of cetral patter geerators (CPGs) for crawlig ad swimmig i a real robot. Eel-like motio is cosidered i ad, where cotrollers for trackig straight ad curved trajectories are proposed. I this paper we are motivated by recet results for lad-based sake robots. I particular, proposes a waypoit guidace strategy for steerig a lad-based sake robot alog a path, defied by straight lies of itercoected poits. Waypoit guidace strategy, where the waypoits are defied a priori, is proposed for a Caragiform swimmer i 7. Path followig cotrol for uderwater sake robots, however, still remais a ope problem. I this paper we combie the use of a artificial potetial fields-based path plaer with a ew waypoit guidace strategy for a uderwater sake robot. I particular, the first cotributio of this paper is to describe how a approach for straight lie path followig, that previously has bee preseted for a groud sake robot i, ca be exteded ad used for path followig cotrol of uderwater sake robots. The paper presets prelimiary results towards solvig the path followig cotrol problem for uderwater sake robots, by proposig a waypoit guidace cotrol strategy. Simulatios are preseted, ad these are see to support the hypothesis that the proposed cotrol strategy ca solve the path followig cotrol problem. The paper is thus a first step towards solvig the cotrol problem, ad lays the foudatio for a full systems aalysis of the closed-loop system. A full

2 systems aalysis is challegig, sice the kiematics ad dyamics of uderwater sake robots are complex, ad is a topic of future work. I additio, as opposed to 7 ad where the waypoits are chose a priori, we propose i this waypoit guidace strategy to istead derive the waypoits usig a path plaer based o the artificial potetial field method i order to also address the obstacle avoidace problems. Simulatio results that illustrate the performace of the proposed guidace strategy both for lateral udulatio ad eel-like motios are preseted. The paper is orgaized as follows. Sectio II presets the dyamic model of a uderwater sake robot, while the motio plaig algorith is outlied i Sectio III. A straight lie path followig cotroller for uderwater sake robot is preseted i Sectio IV, followed by simulatio results i Sectio V. Fially, coclusios ad suggestios for further research are give i Sectio VI. II. DYNAMIC MODELLING OF UNDERWATER SNAKE ROBOT This sectio briefly presets the kiematics ad dyamics of a uderwater sake robot movig i a virtual horizotal plae. A extesive presetatio of the kiematics ad dyamics of the robot ca be foud i. A. Notatios ad defied symbols The robot cosists of rigid liks of equal legth l itercoected by joits. The liks are assumed to have the same mass m ad momet of iertia J = 3 ml. The mass of each lik is uiformly distributed so that the lik CM (ceter of mass) is located at its ceter poit (at legth l from the joit at each side). The total mass of the sake robot is therefore m. I the followig sectios, the kiematics ad dyamics of the uderwater sake robot will be described i terms of the mathematical symbols described i Table I ad illustrated i Fig.. The followig vectors ad matrices are used i the subsequet sectios: A =......, D = where A,D R ( ). Furthermore, e =... T R, E = e e, R, si = si... si T R, S = diag(si) R, cos = cos... cos T R, C = diag(cos) R sg = T sg... sg R =... T R, J = JI, L = li, M = mi K = A T ( DD T ) D, V = A T ( DD T ) A B. Kiematics of the uderwater sake robot The sake robot is assumed to move i a virtual horizotal plae, fully immersed i water, ad has + degrees of freedom ( liks agles ad the x-y positio of the robot). The lik agle of each lik i,..., of the sake robot is deoted by i R, while the joit agle of joit i,..., is give by φ i = i i. The headig (or orietatio) R of the sake is defied as the average of the liks agles similar to lad-based sake robots i = i. () i= ( x, y) l ( x, y) -h x,i- u i- y lik,3 x lik,3 ( x 3, y3 ) ( x-, y-) Body frame F B y global x global Iertial frame F I (a) Kiematic parameters -h y,i- ( x, y ) i i i t i y global u i x global ( x, y ) h y,i + f y,i - h x,i + f x,i (b) Forces ad torques actig o each lik Fig. : Uderwater sake robot TABLE I: Defiitio of mathematical terms Symbol Descriptio Vector The umber of liks l The half legth of a lik m Mass of each lik J Momet of iertia of each lik i Agle betwee lik i ad the global x axis R φ i Agle of joit i φ R (x i,y i) Global coordiates of the CM of lik i X,Y R (, ) Global coordiates of the CM of the robot p CM R u i Actuator torque of joit betwee lik i ad lik i + u R u i Actuator torque of joit betwee lik i ad lik i u R ( f x,i, f y,i) Fluid force o lik i f x,f y R τ i Fluid torque o lik i τ R (h x,i,h y,i) Joit costrait force o lik i from lik i + h x,h y R (h x,i,h y,i ) Joit costrait force o lik i from lik i h x,h y R The global frame positio p CM R of the CM (ceter of mass) of the robot is give by px p CM = = m i= mx i p y m i= my = e T X i e T, () Y where (x i,y i ) are the global frame coordiates of the CM of lik i, X= x,...,x T R ad Y= y,...,y T R. The liks are costraied by the joits accordig to DX + lacos =, DY + lasi =. (3) It is show i that the positio of the idividual liks as a fuctio of the CM positio ad the lik agles of the robot is give by X = lk T cos + e, Y = lk T si + e, () where K = A T ( DD T ) D R, ad where DD T is osigular ad thereby ivertible. The liear velocities of the liks are give by Ẋ = lk T S + eṗ x, Ẏ = lk T C + eṗ y. (5) The liear acceleratios of the liks are foud by differetiatig the velocity of the idividual liks (5) with respect to time, which gives ( ) Ẍ = lk T C + S ( ) + e, Ÿ = lk T S C + e. () Note that i this paper () has bee adjusted compared to the correspodig expressio preseted i i order to express the acceleratio of the liks i a more proper way, by also takig ito accout the acceleratio of the CM. C. Hydrodyamic modellig The hydrodyamic modelig approach, that cosidered i this paper, takes ito accout both the liear ad the oliear drag forces (resistive fluid forces), the added mass effect (reactive fluid forces), the fluid momets ad curret

3 effect (see, e.g. ). I particular, i, it is show that the fluid forces o all liks i vector form ca be expressed as fx fax f I f = = fy f + Dx f II Ay f I D + x D y f II. (7) D y The vectors f Ax ad f Ay represet the effects from added mass forces ad are expressed as fax µ (S = ) µ S C f Ay µ S C µ (C ) ẌŸ µ S C µ (S ) V a x µ (C ) µ S C V a, y where V a x = diag(v x,,...,v x, ) R, V a y = diag(v y,,...,v y, ) R ad V x,i,v y,i T is the curret velocity expressed i iertial frame coordiates. The vectors f I D x, f I D y ad f II D x, f II D y preset the effects from the liear (9) ad oliear drag forces (), respectively, where the relative velocities are give from the Eq. (). f I Dx f I D y f II D x f II D y = ct (C ) + c (S ) (c t c )S C (c t c )S C c t (S ) + c (C ) ct C = c S Vrx sg( c t S c C V ry Vrx C S = Ẋ Vx V ry S C Ẏ V y ) Vrx V ry Ẋ Vx (8) Ẏ V y (9) () () I additio, the fluid torques o all liks i matrix form are τ = Λ Λ Λ 3, () where Λ = λ I, Λ = λ I ad Λ 3 = λ 3 I. The coefficiets c t, c, λ, λ 3 represet the drag forces parameters due to the pressure differece betwee the two sides of the body, ad the parameters µ, λ stad for the added mass of fluid carried by the movig body. D. Equatios of motio This subsectio presets the equatios of motio for the robot, i terms of the acceleratio of the liks agles,, ad the acceleratio of the CM of the robot, p CM. These coordiates describe all + DOFs of the robot. I, it is show that the force balace equatios for all liks may be expressed i matrix form as mẍ = D T h x + f x, mÿ = D T h y + f y. (3) Note that the lik acceleratios may also be expressed by differetiatig (3) twice with respect to time. This gives DẌ = la ( C + S ), DŸ = la ( S C ). () We obtai the acceleratio of the CM by differetiatig () twice with respect to time, isertig (3), ad otig that the costrait forces h x ad h y, are cacelled out whe the lik acceleratios are summed. This gives px = e T Ẍ e T = e T Ÿ m e T f (5) By isertig (), (7) ad (8) ito (5) the acceleratio of the CM may be expressed as px e = M T µ S e T µ S C lk T (C + S ) p e M T µ S C p e T µ C where M p = e T µ S C e T µ S e T µ S C e T µ C V a x V a y lk T (S C ) + M p e T f Dx e T f Dy m m m + e = T µ S e et µ S C e m m e T µ S C e m + e T µ C e (). (7) ad f Dx = f I D x +f II D x ad f Dy = f I D y +f II D y are the drag forces i x ad y directios. Additioally, it is easily verifiable that the determiat m + mµ + µ i= j=i+ (si( i j )) is ozero for ad m. The torque balace equatios for all liks is expressed i matrix form as J = D T u ls A T h x + lc A T h y + τ, (8) where τ is give from (). What ow remais is to remove the costrait forces from (8). By premultiplyig (3) by D ad solvig for h x ad h y, we ca write the expressio for the joit costrait forces as h x = (DD T ) D(mẌ + µ (S ) Ẍ µ S C Ÿ µ S C V a x µ (S ) V a y f I D x f II D x ) h y = (DD T ) D(mŸ µ S C Ẍ + µ (C ) Ÿ + µ (C ) V a x + µ S C V a y f I D y f II D y ). (9) Isertig i (8) the joit costraits forces (9) ad also replacig DẌ, DŸ with (), Ẍ,Ÿ with () ad, with (), ad solvig for, we ca fially express the model of a uderwater sake robot as M + W + V + Λ 3 + K Dx f Dx + K Dy f Dy = D T u, () where M, W, V, K Dx ad K Dy are defied as M = J + ml S VS + ml C VC + Λ + l µ K K T S + l µ K K T C () W = ml S VC ml C VS + l µ K K T C l µ K K T S () V = Λ lµ K V a x lµ K V a y (3) K Dx = lµ m A ee T lµ m A ee T ls K () K Dy = lµ m A ee T lµ m A ee T + lc K (5) where K = A + µ A ee T (m S C m S ) µ A ee T (m S C m S ), K = A µ A ee T (m S C m C ) + µ A ee T (m S C m C ), A = S KS + C KS C, A = S KS C + C KC. Remark : The model (,) has bee adjusted compared to the model i by redefiig the expressio of the lik acceleratios as i () i order to avoid a sigularity issue of the model preseted i. I summary, the equatios of motio for the uderwater sake robot are give by () ad (). By itroducig the state variable x = T, pcm T, T T, ṗcm T R +, we ca rewrite the model of the robot compactly i state space form as ẋ = T, ṗ T CM, T, p T CM T = F(x,u) () where the elemets of F(x,u) are foud by solvig () ad () for p CM ad, respectively. III. MOTION PLANNING ALGORITHM A. Artificial Potetial Field I this subsectio, the artificial potetial field (APF) method is applied to the obstacle avoidace problem of a uderwater sake robot. The desig of the path is a importat step for the cotrol of the robot. The objective is to determie the optimal path that should be followed i order to reach the goal of the missio while avoidig obstacles. Geerally, path plaers are divided ito two categories; local path plaers ad global path plaers. I this study the artificial potetial field method, which is itroduced by, is used to derive the path for the uderwater sake robot. I APF the robot moves i a area with a potetial created by the obstacles ad the target,. The obstacles repel the robot, while the target attracts it. The potetial fuctios are chose so that the target positio

4 is a global miimum while the obstacles are maxima of the fuctio. Deotig the repulsive potetial from the i th obstacle by Ur, i i =,...,o, where o is the umber of the obstacles, ad the attractive potetial from the goal positio by U a, the potetial of the area ca be expressed as o U = U a + Ur. i (7) i= It should be oted that, geerally, i APF theory the obstacles are cosidered as poits, without specific volume. However, it is obvious that the volume of the obstacles ad the volume of the robot should be cosidered i order to derive the fuctio for the potetial. I this paper, we follow a similar approach as described i, where the volume of the obstacles ad the robot is set as the volume of a circle aroud the obstacles. Hece, the repulsive potetial fuctio is defied as Ur i = K rep do i N a N, (8) r where K rep is a positive gai costat which determies the itesity of the attractive potetial, the parameter do i deotes the distace to the i th obstacle, ad a r is the radius of the circle aroud the obstacle takig ito accout also the dimesios of the robot. The radius of the circle that takes ito accout the dimesio of the robot should be at least twice the amplitude of the siusoidal motio of the robot. The parameter N is chose equal to ad K rep is chose close to i order to take ito accout oly the local ifluece of the repulsive potetial. The attractive potetial fuctio is defied as U a = K att d N. (9) where K att is a positive gai costat which determies the itesity of the attractive potetial, the parameter d deotes the distace to the goal ad N is chose equal to i this study. It is worth to metio that icreasig the parameter N will lead to a steeper curve of the attractive potetial fuctio. A gradiet-based optimizatio method is used to derive the path. I additio, i this paper, we decide to icrease the radius of the circle aroud the robot with a small value, i order to take ito accout the joit costraits of the robot, cf.. Whe followig the resultig path, the robot is movig towards the global miimum of the potetial fuctio (7). A simulatio result of the described motio plaig method is preseted below for a space (8m 8m) with obstacles (obstacle positios : {(,) (3, ) (,) (,) (,5)}) ad a selected target (target positio: (7, 5)). The obstacles are rectagular shapes with dimesios (.3.). The simulatio results are performed with the parameter K rep = givig the radius of the circumscribed circle aroud the obstacles ad K att =. Fig. a shows the described sceario, while i Fig. b the resultig potetial is show. Fig. c shows the resultig path that is created based o the obstacles ad target sceario preseted i Fig. a. Remark : It should be oted that the disadvatage of this method is that there is a risk that the robot i some cases ca be stuck at a local miimum. This problem ca be solved usig harmoic potetial field as preseted i, but i this study the typical o-harmoic potetial method is chose to avoid addig more complexity i our system. B. Waypoit guidace Future applicatios of sake robots will geerally ivolve motio i challegig ad ustructured eviromets. It is worth to ote that compared to traditioal sake robots the uderwater sake robots have the advatage of adaptability to aquatic eviromets. The most recet fields of iterest iclude the itegratio of sake robots ito uderwater exploratio, moitorig, ad surveillace. This brigs the eed for steerig the robot to a specific target locatio(s) avoidig obstacles i the plae of motio. I these situatios, it is geerally less importat to follow a exact curved path as log as the robot reaches the target(s) withi a reasoable amout of time, avoidig obstacles i the eviromet. Waypoit Guidace (WPG) is the most widely used scheme i the field of autoomous uderwater vehicles (AUVs) (see e.g. 7), but has, to the authors best kowledge, ot bee cosidered for motio cotrol of uderwater sake robots. It was show i how waypoit guidace ca be used for groud sake robots, ad based o these results we here propose a waypoit guidace strategy for uderwater sake robots. The first versio of the desired path is derived by the AFP as described i Sectio III.A. Waypoit guidace ca the be achieved betwee the start ad ed poits of this path, x d (t o ),y d (t o ) ad x d (t f ),y d (t f ), by splittig the path betwee the poits ito a umber of waypoits x d (k),y d (k) for k =,,...,N w, with N w beig the umber of the waypoits. The waypoit guidace system switches from oe waypoit to the ext whe the uderwater sake robot reaches the viciity of the curret waypoit, i.e. whe it comes withi a circle of acceptace with radius r accept of the curret waypoit x d (k),y d (k) (see e.g. 7). The circle of acceptace is typically chose as two times the legth of the vehicle 7. Note that the distace betwee the waypoits o the path derived by the path plaer usig APF should be sufficietly small i order to keep the straight lie path approximatio as close to the origial path that the collisio avoidace properties are kept. A disadvatage of the waypoit guidace method is that udesirable cotrol eergy cosumptio due to overshoot ca occur durig the chage from oe straight lie path to the ext. Therefore, selectio of the referece path to follow is importat to reduce the overshoot width of path ad thus to decrease the cotrol eergy cosumptio. employs turig simulatios to determie modified waypoits to avoid overshoot. Waypoit guidace, ad specifically Lie-of-Sight (LOS) guidace is a key feature i the majority of guidace systems for marie vehicles 7, ad we believe that this is a promisig approach also for swimmig sake robots. I this paper, a acceptace regio is used, istead of the commo approach of acceptace circle (see e.g. ). By this defiitio, it is guarateed that the robot will reach the acceptace regio of the curret waypoit o matter how the waypoits are defied. The reaso is that usig a acceptace circle istead gives the risk that the robot misses a waypoit which is placed too close to the previous waypoit.

5 y y x x (a) Space with obstacles ad target (b) The potetial of the defied space (c) Potetial of the space ad the derived path Fig. : Derived path for obstacle avoidace purposes usig artificial potetial field IV. PATH FOLLOWING CONTROL ALONG STRAIGHT LINES I this sectio a path followig cotroller is proposed for lateral udulatio ad eel-like motio. The path followig cotrol objective that is cosidered for the uderwater sake robot is to make the robot coverge to the desired straight lie path ad subsequetly progress alog the path at some ozero forward velocity. The authors cosider it as less importat to accurately cotrol the forward velocity of the robot. The path followig cotroller of the robot, which is proposed i this paper, cosists of two mai compoets 3. The first compoet is the gait patter cotroller, which propels the robot forward accordig to the gait patter lateral udulatio or eel-like motio. The secod compoet is the headig cotroller, which steers the sake robot towards ad subsequetly alog the desired path. A ier-loop PD cotroller is used to compesate the effects of the sake s dyamics, while a outer loop cotroller is used for the formatio of the referece joit agles by trackig the desired headig (Fig. 3). The two compoets of the path followig cotroller are ow preseted. Lieof-Sight Outer Loop Cotroller ref Headig Cotrol Gait Patter Geerator Ier Loop Cotroller * Joit Cotrol u Uderwater Sake Robot Fig. 3: Cotroller Structure A. Lateral udulatio The gait patter lateral udulatio is the fastest ad most commo form of sake locomotio. I order to achieve lateral udulatio, the sake is commaded to follow the serpeoid curve 3. The proposed lateral udulatio is realized by cotrollig each joit of the sake robot accordig to the siusoidal referece φi = α si(ωt + (i )β) + γ, i =,...,, (3) where the parameters α ad ω correspod to the amplitude ad agular frequecy of the siusoidal joit motio, β determies the phase shift betwee the sequetial joits, ad γ is the joit offset. B. Eel-like motio Eel-like motio is achieved by propagatig lateral axial udulatios with icreasig amplitude from ose to tail. A simple equatio is derived for the eel-like motio by cotrollig each joit of the sake robot accordig to the referece sigal φi = α ( i + ) si(ωt + (i )β) + γ, i =,...,, (3) where the parameter α( i)/( + ) correspods to the icreasig amplitude, from ose to tail. C. Outer-Loop Cotroller It is well-kow that the parameters α ad β are typically fixed ad the parameters ω,γ are used to cotrol the speed ad the directio of the sake robot 3. I this paper, the same idea will be used i order to steer the uderwater sake robot to a desired orietatio. I particular, the outer-loop cotroller will be resposible for geeratig the referece joits agles i order to esure that the desired orietatio is achieved. The orietatio of the robot is give by Eq. (). Motivated by, we propose to defie the referece orietatio usig the followig LOS guidace law py ) ref = arcta(, (3) where is the cross-track error (i.e., the positio of the robot alog the global y axis), ad is a desig parameter referred to as the look-ahead distace. Note that LOS guidace law is much used i practice for path followig cotrol of marie surface vessels 7, 5. Remark 3: The look-ahead distace is a importat desig parameter that directly iflueces the trasiet motio of the uderwater sake robot. Choosig large results i a welldamped trasiet motio, but the rate of covergece to the path will be slow. O the other had, choosig too small results i poor performace or eve istability. A rule of thumb is to choose larger tha twice the legth of the robot (see e.g. 7). As we already metioed, usig the parameter γ, it is possible to cotrol the directio of the locomotio of the sake robot. To steer the headig accordig to the LOS agle i (3), we set the joit agle offset accordig to γ = k ( ref ), (33) where k > is a cotrol gai. Remark : It should be oted that this traditioal LOS guidace is ot desiged to hadle the curret effect or ay other evirometal disturbaces such as wid or waves. If the curret effect has a compoet actig i the directio perpedicular to the path, the uderwater sake robot, ca ot stay idetically o the path with zero headig. This is a drawback of traditioal LOS guidace laws that results i deviatio problems whe usig LOS cotrol i the presece of curret. Oe approach that has give good results for marie vehicles, is to allow the vehicle to side-slip such that a compoet of the forward velocity of the robot ca couteract the effect of the curret effect 7.

6 D. Ier-Loop Cotroller I order to make the joit agle φ i follow its referece sigal φi, a PD cotroller is used: u i = φ i + k d ( φ i φ i ) + k p (φi φ i ), i =,...,, (3) where k p > ad k d > are the gais of the cotroller. This cotroller expoetially stabilizes the origi of the joit error dyamics. V. SIMULATION STUDY This sectio presets simulatio results i order to ivestigate the performace of the proposed waypoit guidace strategy described i Sectio III-IV. The dyamics was calculated usig the ode3tb solver i Matlab Rb with a relative ad absolute error tolerace of. A. Implemetatio of guidace strategy The efficacy of the proposed scheme has bee examied through three differet simulatio studies; case the straight lie path followig cotroller, case the waypoit guidace strategy for a priori defied poits ad case 3 the waypoit guidace strategy with obstacle avoidace. A uderwater sake robot was cosidered with = liks, each oe havig legth l =. m ad mass m =.597 kg. The iitial values of all states of the robot were set to zero for case, while the value for the iitial positio of the CM is selected as p CM () =, i case ad p CM () =, i case 3. The hydrodyamic related parameters are set to c t =.39, c = 8., µ =.3958, λ =.988 7, λ =.33 ad λ 3 =.9 5. A extesive discussio about the values of the fluid parameters ca be foud i. We do ot take ito accout currets i the simulatio, as hadlig currets will be a topic of future work. I additio, the radius of the acceptace circle (i.e. acceptace circle beig a subset of the acceptace regio) eclosig each waypoit is r accept =.8. The joit PD cotroller (3) is used for each joit with parameters k p =, k d = 5, ad lateral udulatio or eel-like motio are achieved by movig the joits accordig to (3) or (3), respectively, with gait parameters a = 3 o, β = o ad ω = 5 o /s. Furthermore, the parameter k =.3 ad the parameter =. m. B. Simulatio results ) Case : The straight lie path followig cotroller: First, we tested the straight lie path followig cotroller proposed i Sectio IV. I Fig. we see that the joit agles coverge expoetially to their referece values. Furthermore, we see i Fig. 5a ad a that (33) together with (3) ad (3), respectively, makes the headig agle coverge to the desired headig agle give by (3). Moreover, Fig. 5b ad b show that the hypothesis that the LOS guidace law (3) will make the cross track error oscillate aroud zero is verified. A xy-plot illustratig the behavior of the uderwater sake robot for lateral udulatio ad eel-like motio is show i Fig. 5c ad c, respectively. ) Case : The waypoit guidace strategy for a priori defied waypoits: The proposed guidace strategy is the tested whe havig chose a set of waypoits a priori. The chose waypoits i global frame coordiates are ( ), (3 ), ( 3), ( 8) ad ( 8), respectively. The headig of the robot is preseted i the Fig. 7a, Fig. 8a ad Fig. 7b, Fig. 8b show l φ φ l φ φ l φ φ l φ φ = l φ () φ().9t l φ () φ() (a) Lateral Udulatio l φ φ l φ φ = l φ () φ().3t l φ () φ() (b) Eel-like motio Fig. : Covergece of joit agles the cross-track error. From Fig. 7c ad Fig. 8c we see that the robot achieves a smooth path towards each waypoit, ad from Fig. 7a ad Fig. 8a we ca see that the robot is able to follow the directio of the desired path. Fig. 7b ad Fig. 8b show the trasiet behavior discussed i Sectio IV.C, ad show that the cross-track error oscillates aroud zero after each waypoit switchig both for lateral udulatio ad eellike motio, respectively. Fig. 7c ad Fig. 8c show the motio of the CM of the uderwater sake robot. 3) Case 3: The waypoit guidace strategy for obstacle avoidace usig APF: Fially, the proposed cotrol strategy is tested i the case of a obstacle avoidace sceario described i Sectio III.A. The headig of the robot is preseted i the Fig. 9a, a, a ad a. It may be oted that the cross-track error does ot coverge to zero (Figs 9b, b, b ad b) i all cases, because the waypoits are really close to each other, so there is ot eough time to achieve zero covergece. I Fig. 9c, Fig. c, Fig. c ad Fig. c we ca see that the robot maages to follow the desired path, derived usig artificial potetial field for obstacle avoidace purposes. As we ca see from Fig. 9b, b ad Fig. b, b, with larger subsamplig step, we achieve better covergece results i cross-track error. This illustrates, as discussed i Sectio III.B., that a larger step i the subsamplig process ca give better results i crosstrack error, but as the step is chose larger the deviatio betwee the origial path ad the straight lie path becomes larger, ad thus the risk for the robot to collide with a object icreases. The importat poit is that, eve if the cross-track error does ot coverge exactly to zero, the headig of the robot oscillates aroud zero (Fig. 9a, a, a ad a) ad the robot maages to follow, with almost zero error, the desired path without collidig with ay obstacle. VI. CONCLUSIONS I this paper prelimiary results for path followig cotrol of uderwater sake robots were preseted. I particular, a waypoit guidace cotrol strategy was proposed: A motio

7 ref deg cm Fig. 5: Straight lie path followig cotroller for lateral udulatio ref deg cm Fig. : Straight lie path followig cotroller for eel-like motio ref deg cm Fig. 7: Path followig of uderwater sake robot durig waypoit guidace for lateral udulatio ref deg cm Fig. 8: Path followig of uderwater sake robot durig waypoit guidace for eel-like motio ref 5 5 deg cm Fig. 9: Path followig of uderwater sake robot durig waypoit guidace for obstacle avoidace for lateral udulatio: subsamplig every poits waypoits distace approximately. m ref 5 5 deg cm Fig. : Path followig of uderwater sake robot durig waypoit guidace for obstacle avoidace for eel-like motio: subsamplig every poits waypoits distace approximately. m

8 ref 5 5 deg cm Fig. : Path followig of uderwater sake robot durig waypoit guidace for obstacle avoidace for lateral udulatio: subsamplig every poits waypoits distace approximately m ref 5 5 deg cm Fig. : Path followig of uderwater sake robot durig waypoit guidace for obstacle avoidace for eel-like motio: subsamplig every poits waypoits distace approximately m plaig algorithm was proposed for derivig referece paths based o APF techiques, i order to achieve collisio free paths. The path was the split ito liear path segmets defied by waypoits. Fially, a LOS guidace law was proposed to make the uderwater sake robot coverge to the straight lie paths. Simulatio results were preseted, illustratig the successful itegratio of the aforemetioed strategy. It remais a topic of future work to ivestigate the stability properties of the proposed path followig cotroller. REFERENCES P. Liljebäck, K. Y. Petterse, Ø. Stavdahl, ad J. T. Gravdahl, Sake Robots: Modellig, Mechatroics, ad Cotrol. Spriger-Verlag, Advaces i Idustrial Cotrol, 3. J. Gray, Studies i aimal locomotio, Joural of Experimetal Biology, vol., o., pp. 88, S. Hirose, Biologically Ispired Robots: Sake-Like Locomotors ad Maipulators. Oxford: Oxford Uiversity Press, 993. K. McIsaac ad J. Ostrowski, Motio plaig for aguilliform locomotio, IEEE Trasactios o Robotics ad Automatio, vol. 9, o., pp. 37 5, 3. 5 F. Boyer, M. Porez, ad W. Khalil, Macro-cotiuous computed torque algorithm for a three-dimesioal eel-like robot, IEEE Trasactios o Robotics, vol., o., pp ,. E. Kelasidi, K. Y. Petterse, J. T. Gravdahl, ad P. Liljebäck, Modellig of uderwater sake robots, i Proc. IEEE Iteratioal Coferece o Robotics ad Automatio (ICRA), Hog Kog, Chia, May 3 - Jue 7, accepted. Olie. Available: 7 T. I. Fosse, Hadbook of Marie Craft Hydrodyamics ad Motio Cotrol. Joh Wiley & Sos, Ltd,. 8 S. LaValle, Plaig Algorithms, Cambridge, Ed. UK: Cambridge Uiversity Press,. 9 H. Choset, K. M. Lych, S. Hutchiso, G. Kator, W. Burgard, L. E. Kavraki, ad S. Thru, Priciples of Robot Motio: Theory, Algorithms, ad Implemetatios. U.K. Cambridge, MIT Press, 5. J. Borestei ad Y. Kore, The vector field histogram-fast obstacle avoidace for mobile robots, IEEE Trasactios o Robotics ad Automatio, vol. 7, o. 3, pp , 99. O. Khatib, Real-time obstacle avoidace for maipulators ad mobile robots, i Proc. IEEE Iteratioal Coferece o Robotics ad Automatio (ICRA), vol., 985. T. Paul, T. Krogstad, ad J. Gravdahl, UAV formatio flight usig 3D potetial field, i Proc. th Mediterraea Coferece o Cotrol ad Automatio (MED), Ajaccio-Corsica, Frace, Jue H. Igarashi ad M. Kakikura, Path ad posture plaig for walkig robots by artificial potetial field method, i Proc. IEEE Iteratioal Coferece o Robotics ad Automatio (ICRA), vol. 3, New Orleas, LA, USA, April -May. M. Porez, V. Lebastard, A. Ijspeert, ad F. Boyer, Multi-physics model of a electric fish-like robot: Numerical aspects ad applicatio to obstacle avoidace, i Proc. IEEE/RSJ Iteratioal Coferece o Itelliget Robots ad Systems (IROS), Sa Fracisco, Califoria, Sept A. Crespi ad A. J. Ijspeert, AmphiBot II: A Amphibious Sake Robot that Crawls ad Swims usig a Cetral Patter Geerator, i Proc. of the 9th Iteratioal Coferece o Climbig ad Walkig Robots (CLAWAR ), Brussels, Belgium, Sept. -. K. A. McIsaac ad J. P. Ostrowski, A framework for steerig dyamic robotic locomotio systems, It. J. Robot. Res., vol., o., pp , February 3. 7 J. Guo, A waypoit-trackig cotroller for a biomimetic autoomous uderwater vehicle, Ocea Egieerig, vol. 33, pp ,. 8 P. A. Vela, K. A. Morgase, ad J. W. Burdick, Uderwater locomotio from oscillatory shape deformatios, i Proc. IEEE Cof. Decisio ad Cotrol, vol., Las Vegas, NV, USA, Dec K. Morgase, B. Triplett, ad D. Klei, Geometric methods for modelig ad cotrol of free-swimmig fi-actuated uderwater vehicles, IEEE Trasactios o Robotics, vol. 3, o., pp. 8 99, 7. P. Liljebäck ad K. Y. Petterse, Waypoit guidace cotrol of sake robots, i Proc. IEEE Iteratioal Coferece o Robotics ad Automatio (ICRA), Shaghai, Chia, May 9-3. I. Arvaitakis ad A. Tzes, Trajectory optimizatio satisfyig the robot s kiodyamic costraits for obstacle avoidace, i Proc. th Mediterraea Coferece o Cotrol Automatio (MED), Barceloa, Spai, July 3-. D. Yeo, Desig of AUV trackig system usig the slidig mode cotrol ad the optimal cotrol theory, Ph.D. dissertatio, MS Thesis, Departmet of Naval Architecture ad Ocea Egieerig, Seoul Natioal Uiversity, Korea, E. Kelasidi ad A. Tzes, Serpetie motio cotrol of sake robots for curvature ad headig based trajectory - parameterizatio, i Proc. th Mediterraea Coferece o Cotrol Automatio (MED), Barceloa, Spai, July 3-. P. Liljebäck, I. U. Haugstue, ad K. Y. Petterse, Path followig cotrol of plaar sake robots usig a cascaded approach, IEEE Trasactios o Cotrol Systems Techology, vol., o., pp.,. 5 E. Fredrikse ad K. Y. Petterse, Global κ-expoetial way-poit maeuverig of ships: Theory ad experimets, Automatica, vol., pp ,.