Path Following for Underactuated Marine Vessels

10th IFAC Symposium on Nonlinear Control Systems August 23-25, 2016. Monterey, California, USA 10th IFAC Symposium on Control Systems August 23-25, 2016. Monterey, California, USA 10th IFAC Symposium on Nonlinear Nonlinear Control Systems Available August 23-25, 2016. Monterey, California, USA August 23-25, 2016. Monterey, California, USA online at www.sciencedirect.com

ScienceDirect Path Path Path Path Path

Following Underactuated Marine IFAC-PapersOnLinefor 49-18 (2016) 588–593 Following for Underactuated Marine Following for Underactuated Marine Vessels Following Underactuated Vessels Following for for Underactuated Marine Marine Vessels ∗ Vessels ∗ D.J.W. Belleter ∗ Vessels C. Paliotta M. Maggiore ∗∗ D.J.W. Belleter C. Paliotta ∗ M. Maggiore ∗∗ ∗

∗ K.Y. D.J.W. Belleter C. Pettersen Paliotta ∗∗ ∗M. Maggiore ∗∗ K.Y. Pettersen ∗ ∗∗ D.J.W. Belleter C. Paliotta ∗ K.Y. D.J.W. Belleter C. Pettersen Paliotta ∗ ∗∗M. M. Maggiore Maggiore ∗∗ ∗ K.Y. Pettersen ∗ Marine Operations and Systems (NTNU ∗ Centre for Autonomous K.Y. Pettersen Centre for Autonomous Marine Operations and Systems (NTNU ∗ AMOS) and the Department of Engineering Cybernetics, Centreand for the Autonomous Marine OperationsCybernetics, and SystemsNorwegian (NTNU AMOS) Department of Engineering Norwegian ∗ Centre for Autonomous Marine Operations and Systems (NTNU ∗ University of Science and Technology, NO7491 Trondheim, Norway AMOS) the Department of Engineering Cybernetics, Centreand for Autonomous Marine Operations and SystemsNorwegian (NTNU University of Science and Technology, NO7491 Trondheim, Norway AMOS) and the Department of Engineering Cybernetics, Norwegian {dennis.belleter,claudio.paliotta,kristin.y.pettersen}@itk.ntnu.no University of Science and Technology, NO7491 Trondheim, Norway AMOS) and the Department of Engineering Cybernetics, Norwegian {dennis.belleter,claudio.paliotta,kristin.y.pettersen}@itk.ntnu.no ∗∗ University of Science and Technology, NO7491 Trondheim, Norway Department of Electrical and Computer Engineering, University ∗∗ {dennis.belleter,claudio.paliotta,kristin.y.pettersen}@itk.ntnu.no University of Science and Technology, NO7491 Trondheim, Norwayof Department of Electrical and Computer Engineering, University of ∗∗ Toronto, 10 King’s College rd., Toronto, ON, M5S3G4, Canada, {dennis.belleter,claudio.paliotta,kristin.y.pettersen}@itk.ntnu.no Department of Electrical and Computer Engineering, University of {dennis.belleter,claudio.paliotta,kristin.y.pettersen}@itk.ntnu.no ∗∗ Toronto, 10 King’s College rd., Toronto, ON, M5S3G4, Canada, Department of and Computer University ∗∗ Toronto, [email protected] 10 King’s College rd., Toronto,Engineering, ON, M5S3G4, Canada, of Department of Electrical Electrical and Computer Engineering, University of [email protected] Toronto, College [email protected] Toronto, 10 10 King’s King’s College rd., rd., Toronto, Toronto, ON, ON, M5S3G4, M5S3G4, Canada, Canada, [email protected] [email protected] Abstract: This paper investigates the problem of making an underactuated marine vessel follow Abstract: This paper investigates the problem of making an underactuated marine vessel follow an arbitrary differentiable Jordan curve. A solution is proposed which relies on a vessel hierarchical Abstract: This paper investigates problem of making an underactuated marine follow an arbitraryThis differentiable Jordan the curve. A solution is proposed which relies on a vessel hierarchical Abstract: paper investigates the problem of making an underactuated marine follow control methodology involving the simultaneous stabilization of two nested sets, and results in an arbitrary differentiable Jordan curve. A solution is proposed which relies on a hierarchical Abstract: This paper investigates the problem of making an underactuated marine vessel follow control methodology involving the curve. simultaneous stabilization of two nested sets, and results in arbitrary differentiable Jordan A solution is proposed which relies on a hierarchical aan smooth, static, and time-invariant feedback. The methodology in question effectively reduces control methodology involving the curve. simultaneous stabilization of two nested results in arbitrary differentiable Jordan A solution is proposed which reliessets, on and a hierarchical aan smooth, static, and time-invariant feedback. The methodology in question effectively reduces control methodology involving the following simultaneous stabilization of two two nested sets, and results in the control problem one of path forThe a stabilization kinematic point-mass. It is sets, shown that as long a smooth, static, andto time-invariant feedback. methodology in question effectively reduces control methodology involving the simultaneous of nested and results in the control problem to one of path following for a kinematic point-mass. It is shown that as long a smooth, static, and time-invariant feedback. methodology in question effectively reduces as the curvature of the path is smaller thanforaThe quantity dependent on the mass and damping the control problem to one of path following a kinematic point-mass. It is shown that as long a smooth, static, and time-invariant feedback. The methodology in question effectively reduces as the curvature of to theone path is smaller thanfora aquantity dependent on the mass and damping the control problem of path following kinematic point-mass. It shown that as parameters of theofship, path isthan achieved with uniformly bounded speed. as the curvature theone path is smaller dependent on the mass and the control problem to of following path following fora aquantity kinematic point-mass. It is issway shown thatdamping as long long parameters of the ship, path following is achieved with uniformly bounded sway speed. as the curvature of the path is smaller than a quantity dependent on the mass and damping parameters of theofship, achieved with uniformly bounded as the curvature the path path following is smalleristhan a quantity dependent on the sway mass speed. and damping parameters of the following is with uniformly bounded sway © 2016,1.IFAC Federation of Automatic Control) Hosting by Elsevier Ltd. All speed. rights reserved. of straightparameters of(International the ship, ship, path path following is achieved achieved with uniformly bounded sway speed. INTRODUCTION The papers listed above consider path following 1. INTRODUCTION The papers listed above consider path following of straightline paths or path-following/trajectory-tracking curved 1. INTRODUCTION The papersorlisted above consider path following ofof line paths path-following/trajectory-tracking ofstraightcurved 1. INTRODUCTION The papers listed above consider path following of straightpaths that are parametrized by time or a path variable. This paper presents a control methodology for underline paths orlisted path-following/trajectory-tracking curved 1. INTRODUCTION papers above consider path following ofofvariable. straightpaths that are parametrized by time or a path This paper presents a control methodology for under- The line the paths orare path-following/trajectory-tracking ofvariable. curved To best of our knowledge, the orcontext marine actuated marine vessels with two control inputs (thrust paths that parametrized by in time a pathof paths or path-following/trajectory-tracking of curved This paper presents a control methodology for (thrust under- line To the best of our knowledge, in the context of marine actuated marine vessels with two control inputs paths that are parametrized by time or aastatic, path variable. vessels, the problem of finding a smooth, and time This paper presents a control methodology for underand torque) and three degrees-of-freedom (position and To the best of our knowledge, in the context of marine paths that are parametrized by time or path variable. actuated marine vessels with two control inputs (thrust This torque) paper presents control methodology for underthe problem of finding ainsmooth, static,ofand time and and vessels threea degrees-of-freedom (position and vessels, To the best of our knowledge, the context marine invariant feedback solving the path following problem for actuated marine with two control inputs (thrust rotation). The control specification is path following: make vessels, the problem of finding ainsmooth, static, time the best of oursolving knowledge, the context ofand marine and torque) and threespecification degrees-of-freedom (position and To actuated marine vessels with two is control inputs (thrust invariant feedback the path following problem for rotation). The control path following: make vessels, the problem of finding a smooth, static, and time general unparametrized paths remains open. In this paper, and torque) and three degrees-of-freedom (position and the ship approach a path and follow it with nonzero speed invariant feedback theremains path following for the problemsolving of finding a smooth, static, andpaper, time rotation). Theand control specification isitpath following: make and torque) three degrees-of-freedom (positionspeed and vessels, general unparametrized paths open. Inproblem this the ship approach a path and follow with nonzero invariant feedback solving the path following problem for we makeunparametrized an initial step towards its solution. approach rotation). The specification make without requiring any time parametrization. While in the invariant general paths open. Our Inproblem this paper, feedback solving theremains path following for the ship requiring approach a path and followis withfollowing: nonzero speed rotation). The control control specification isitpath path following: make we make an initial step towards its solution. Our approach without any time parametrization. While in the general unparametrized paths remains open. In this paper, leverages the hierarchical control methodology presented the ship approach aa problem path and follow it with speed trajectory trackingany one would seeknonzero to make the we make an initial step towards its solution. Our approach general unparametrized paths remains open. In this paper, without requiring time parametrization. While in the the ship approach path and follow it with nonzero speed theinitial hierarchical control methodology presented trajectory trackingany problem one would seek While to make the leverages we make towards its solution. Our approach El-Hawwary andstep Maggiore (2013), a methodology which without requiring time in the ship follow a moving reference point, in path following one leverages theinitial hierarchical control we make an an step towards its methodology solution. Our presented approach trajectory tracking problem one would seek following to make without requiring any time parametrization. parametrization. While in one the in in El-Hawwary and Maggiore (2013), a methodology which ship follow a moving reference point, in path leverages the hierarchical control methodology presented has been used in Roza and Maggiore (2014) to derive trajectory tracking problem one would seek to make the wants to stabilize a suitable controlled-invariant subset in El-Hawwary and Maggiore (2013), a methodology which control methodology ship follow a movingaproblem reference point, in path one trajectory tracking one would seek following to make the leverages has been the usedhierarchical in Roza and Maggiore (2014) presented to derive wants to stabilize suitable controlled-invariant subset in El-Hawwary and Maggiore (2013), a methodology which almost global position controllers for underactuated flying ship follow a moving reference point, in path following one of the state space (see Nielsen et al. (2010)), and no has been used in Roza and Maggiore (2014) to derive in El-Hawwary and Maggiore (2013), a methodology which wants to stabilize a(see suitable controlled-invariant subset ship follow a moving reference point, in path following one almost global position controllers for underactuated flying of the state space Nielsen et al. (2010)), and no has been used in Roza and Maggiore (2014) to derive vehicles. The idea is to first design a path following control wants to stabilize aa(see suitable controlled-invariant subset exogenous signal drives the control loop. almost global position controllers for underactuated flying has been used in Roza and Maggiore (2014) to derive of the state space Nielsen et al. (2010)), and no wants to stabilize suitable controlled-invariant subset vehicles. The idea is to first design a path following control exogenous signal drives the control loop. almost position controllers for underactuated flying law for global aThe kinematic point-mass. Then from this feedback of the state space (see Nielsen et al. (2010)), and no vehicles. idea is to first design a path following control almost global position controllers for underactuated flying exogenous signal drives the control loop. of the state space (see Nielsen et al. (2010)), and no law for a kinematic point-mass. Then from this feedback The path following and trajectory tracking problems have extract vehicles. The idea is to first design a path following control a desired heading angle, and view it as a reference exogenous signal drives the control loop. The path following and trajectory tracking problems have law for a kinematic point-mass. Then from this feedback vehicles. The idea is to first design a path following control exogenous signal drives the control loop. extract a desired heading angle, and view it as a reference been the subject ofand significant research in problems the context of for afortorque The path followingof trajectory tracking have kinematic point-mass. Then from this feedback controller. Carrying outview these two separate been the subject significant research in problems the context of law extract aa desired heading angle, and it as a reference law kinematic point-mass. Then from this feedback for afortorque controller. Carrying outview these two separate The path following and trajectory have marine vessels. Weofmention someresearch oftracking the relevant references. been the subject significant in the context of extract a desired heading angle, and it as a reference The path following and trajectory tracking problems have design steps corresponds to the simultaneous stabilization marine vessels. We mention some of the relevant references. for a torque controller. Carrying out these two separate extract a desired heading angle, and view it as a reference design steps corresponds to the simultaneous stabilization been the subject of significant research in the context of Straight-line/waypoint path following for underactuated marine vessels. Weofmention some of the relevant for aa torque controller. out these been the subject significant research in underactuated the references. context of of two nested subsets Carrying of the simultaneous state space,two andseparate the a Straight-line/waypoint path following for design steps corresponds to the stabilization for torque controller. Carrying out these two separate of two nested subsets of the state space, and the a marine We some of references. vessels is considered in path Fredriksen andrelevant Pettersen (2006), reduction Straight-line/waypoint following for underactuated design steps corresponds to the simultaneous stabilization marine vessels. vessels. We mention mention some of the the relevant references. theorem from El-Hawwary and Maggiore (2013) vessels is considered in Fredriksen and Pettersen (2006), of two nested subsets of the state space, and the a design steps corresponds to the simultaneous stabilization reduction theorem from El-Hawwary and Maggiore (2013) Straight-line/waypoint path following for underactuated Børhaug et al. (2008), Oh and Sun (2010), and (2006), Aguiar of vessels is et considered in path Fredriksen and Pettersen two subsets of the space, the Straight-line/waypoint following for underactuated is used tonested show overall stability. Instate particular, weand show thata Børhaug al. (2008), Oh and Sun (2010), and Aguiar reduction theorem from El-Hawwary and Maggiore (2013) of two nested subsets of the state space, and the used to show overall stability. In particular, we show thata vessels is considered in Fredriksen and Pettersen (2006), and Pascoal (2007). Path following for curved paths is is Børhaug et al. (2008), Oh and Sun (2010), and Aguiar reduction theorem from El-Hawwary and Maggiore (2013) vessels is considered in Fredriksen and Pettersen (2006), if the curvature of the path is not too large in relation to a and Pascoal (2007). Path following for curved paths is is used to show overall stability. In particular, we show that reduction theorem from El-Hawwary and Maggiore (2013) the curvature of the stability. path is not too large inwe relation to a Børhaug et al. (2008), Oh and Sun (2010), and Aguiar considered in Do and Panfollowing (2006) where the path is and Pascoal (2007). Path curved is if is used to show overall In particular, show that Børhaug et in al. (2008), Oh and Sun for (2010), andpaths Aguiar constant that depends on the ship’s parameters, then thea considered Do and Pan (2006) where the path is if the of the stability. path is not large inwe relation to is usedcurvature to that showdepends overall In too particular, show that constant on the ship’s parameters, then thea and Pascoal (2007). Path following for curved paths is parametrized by a path-variable that propagates along considered in Do and Pan (2006) where the path is if the curvature of the path is not too large in relation to and Pascoal (2007). Path following forpropagates curved paths sideways velocity is uniformly bounded. parametrized by a path-variable that along constant that depends on the ship’s parameters, then thea if the curvature of the path is not too large in relation to sideways velocity is uniformly bounded. considered in Do and Pan (2006) where the path is the path with aDo velocity dependent onwhere the desired vessel parametrized a and path-variable that propagates along that depends on the ship’s parameters, then the considered in by Pan (2006) the path is constant the path with a velocity dependent on the desired vessel sideways velocity is uniformly bounded. constant that depends on the ship’s parameters, then challenge in is solving the path following problem the for parametrized by aa path-variable propagates along velocity. The papers Aguiar and that Hespanha (2007)vessel and The the path with a velocity dependent on the desired sideways velocity uniformly bounded. parametrized by path-variable that propagates along The challenge in solving the path following problem for velocity. The papers Aguiar and Hespanha (2007)vessel and sideways velocity isthat, uniformly bounded. marine vessels is due to the presence of sideways the path with a velocity dependent on the desired Skjetne et al. (2005) investigate the trajectory tracking The challenge in solving the path following problem for velocity. papers Aguiar andthe Hespanha (2007) and marine vessels is that, due to the presence of sideways the pathetThe with a(2005) velocity dependent ontrajectory the desired vessel Skjetne al. investigate tracking The challenge in solving the path following problem for motion, in order to stay on a curved path the ship cannot velocity. The papers Aguiar and Hespanha (2007) and problem. Path following of curved paths for underactuated marine vessels is that, due to the presence of sideways The challenge in solving the path following problem for Skjetne et al. (2005) investigate the trajectory tracking velocity. Path The papers Aguiar andpaths Hespanha (2007) and motion, in order to stay on a curved path the ship cannot problem. following of curved for underactuated marine vessels is that, due to the presence of sideways head tangent to it, and its angle of attack relative to the Skjetne et al. (2005) investigate the trajectory tracking vessels using a Serret-Frenet path frame is considered in Li marine motion, in order to stay on a curved path the ship cannot vessels is that, due to the presence of sideways problem. Path following of curved paths for underactuated Skjetne et al. (2005) investigate the trajectory tracking tangent to it, and its angle of attack relative to the vessels using a Serret-Frenet path frame is considered in Li head motion, in to on aa curved path ship tangent sway speed. problem. Path following of paths for underactuated et al. (2009); Moe et al. (2014) and Lapierre and Soetanto head tangent todepends it, and on its angle of attack relative to the motion, in order order to stay stay on the curved path the the ship cannot cannot vessels using aMoe Serret-Frenet path frame is in Li path’s problem. Path following of curved curved paths forconsidered underactuated path’s tangent depends on the sway speed. et al. (2009); et al. (2014) and Lapierre and Soetanto head tangent to it, and its angle of attack relative vessels using Serret-Frenet path frame is in (2007). path’stangent tangenttodepends sway speed. relative to it, and on its the angle of attack to the the et al. (2009); et al. (2014) and Lapierre and Soetanto vessels using aaMoe Serret-Frenet path frame is considered considered in Li Li head (2007). path’s tangent depends on the sway speed. et al. (2009); Moe et al. (2014) and Lapierre and Soetanto path’s tangent depends on the sway speed. (2007). etM. al. Maggiore (2009); Moe et al. (2014) and Lapierre and Soetanto 1 was supported by the Natural Sciences and Engi2. PRELIMINARIES AND NOTATION (2007). 1 M. Maggiore was supported by the Natural Sciences and Engi(2007). 2. PRELIMINARIES AND NOTATION neering Researchwas Council of Canada (NSERC). This research was 1 M. Maggiore supported by the Natural Sciences and Engi2. PRELIMINARIES AND NOTATION neering Research Council of Canada (NSERC). This research was 1 performed while was M. Maggiore was on sabbatical leave at and the Labosupported by the Natural Engi1 M. 2. AND neering Research Council of Canada This research was M. Maggiore Maggiore supported by on the(NSERC). Natural Sciences Sciences Engiperformed while was M. Maggiore was sabbatical leave at and the LaboIn this paper we adopt the following notation. We denote 2. PRELIMINARIES PRELIMINARIES AND NOTATION NOTATION ratoire des Signaux et Syst` emes, Gif sur Yvette, France. neering Research Council of Canada (NSERC). This research was In this paper we adopt the following notation. We denote performed while M. Maggiore was on sabbatical leave at the Labo1 neering Research Council of Canada (NSERC). This research was ratoire des Signaux et Syst` e mes, Gif sur Yvette, France. 2 D.J.W. Belleter, C. Paliotta, and K.Y. Pettersen were supported by S the set of real numbers modulo 2π,We with the 1 In this paper we adopt the following notation. denote performed while M. Maggiore was on sabbatical leave at the 2 D.J.W. by S1 paper the set of realthe numbers modulo 2π,We with the ratoire desBelleter, Signaux et Syst` emes, Gif sur Yvette, France. performed while M.C. Maggiore was onK.Y. sabbatical leave at supported the LaboLaboPaliotta, and Pettersen were In this we adopt following notation. denote differentiable manifold structure making it diffeomorphic by the Research Council of Norway through its Centres of Excellence 2 D.J.W. by S the set of real numbers modulo 2π, with the ratoire des Signaux et Syst` e mes, Gif sur Yvette, France. In this paper we adopt the following notation. We denote Belleter, C. Paliotta, and K.Y. Pettersen were supported differentiable manifold structure making it diffeomorphic ratoire des Signaux et Syst` e mes, Gif sur Yvette, France. 1 by the Research Council of Norway through its Centres of Excellence 1 2 D.J.W. by S the set of modulo 2π, with the 1 unit to the circle. If real ψ ∈structure Snumbers , Rψ ismaking the rotation matrix funding scheme, project No. 223254 AMOS. Belleter, C. Paliotta, and K.Y. Pettersen were supported 1 differentiable manifold it diffeomorphic 2 D.J.W. by S the set of real numbers modulo 2π, with by the Research Council of Norway through its Centres of Excellence Belleter, C. Paliotta, and K.Y. Pettersen were supported to the unit circle. If ψ ∈structure S1 , Rψ ismaking the rotation matrix the funding scheme, project No. 223254 AMOS. differentiable manifold it diffeomorphic by the Research Council of Norway through its Centres of Excellence to the unit circle. If ψ ∈structure S1 , Rψ ismaking the rotation matrix funding scheme, Council project of No. 223254through AMOS.its Centres of Excellence differentiable manifold it diffeomorphic by the Research Norway is the rotation matrix funding Copyright © 2016project IFAC No. 600 to to the the unit unit circle. circle. If If ψ ψ∈ ∈S S1 ,, R Rψ funding scheme, scheme, project No. 223254 223254 AMOS. AMOS. ψ is the rotation matrix

Copyright © 2016 IFAC 600 2405-8963 © IFAC (International Federation of Automatic Control) Copyright © 2016, 2016 IFAC 600Hosting by Elsevier Ltd. All rights reserved. Copyright 2016 IFAC 600 Peer review© of International Federation of Automatic Copyright ©under 2016 responsibility IFAC 600Control. 10.1016/j.ifacol.2016.10.229

IFAC NOLCOS 2016 August 23-25, 2016. Monterey, California, USA D.J.W. Belleter et al. / IFAC-PapersOnLine 49-18 (2016) 588–593

Rψ =



589

 cos(ψ) − sin(ψ) . sin(ψ) cos(ψ)

px

If f (x, y) is a differentiable function of two scalar variables, we denote by ∂x f , ∂y f the partial derivatives with respect 2 to x and y, respectively. Similarly, we define ∂xy f := ∂x ∂y f , and similarly for the other second-order partial derivatives. If f : Rn → Rm is a differentiable vector function and p ∈ Rn , dfp is the m × n Jacobian matrix of f at p. If Γ is a closed subset of a metric space (M, d) and x ∈ M , then we denote by �x�M the point-to-set distance of x to M , �x�M = inf y∈M d(x − y). The following stability definitions are taken from ElHawwary and Maggiore (2013). Let Σ : χ˙ = f (χ) be a smooth dynamical system with state space a Riemannian manifold X with associated metric d. Let φ(t, χ0 ) denote the local phase flow generated by Σ, and let Bδ (x) denote the ball of radius δ centred at x ∈ M . Consider a closed set Γ ⊂ X which is positively invariant for Σ, i.e., for all χ0 ∈ Γ, φ(t, χ0 ) ∈ Γ for all t > 0 for which φ(t, χ0 ) is defined. Then we have the following stability definitions taken from El-Hawwary and Maggiore (2013). Definition 1. The set Γ is stable for Σ if for any ε > 0, there exists a neighborhood N (Γ) ⊂ X such that, for all χ0 ∈ N (Γ), φ(t, χ0 ) ∈ Bε (Γ), for all t > 0 for which φ(t, χ0 ) is defined. The set Γ is attractive for Σ if there exists a neighborhood N (Γ) ⊂ X such that for all χ0 ∈ N (Γ), limt→∞ �φ(t, χ0 )�Γ = 0. The domain of attraction of Γ is the set {χ0 ∈ X : limt→∞ �φ(t, χ0 )�Γ = 0}. The set Γ is globally attractive for Σ if it is attractive with domain of attraction X . The set Γ is locally asymptotically stable (LAS) for Σ if it is stable and attractive. The set Γ is globally asymptotically stable for Σ if it is stable and globally attractive. If Γ1 ⊂ Γ2 are two closed positively invariant sets, then Γ1 is asymptotically stable relative to Γ2 if Γ1 is asymptotically stable for the restriction of Σ to Γ2 . System Σ is locally uniformly bounded (LUB) near Γ if for each x ∈ Γ there exist positive scalars λ and m such that φ(R+ , Bλ (x)) ⊂ Bm (x). △

py

u

v y

x

ψ p˙

Fig. 1. Illustration of the ship’s kinematic variables. the plane (towards the sea bottom). We represent the velocity vector p˙ in body frame coordinates as (u, v), where u, the longitudinal component of the velocity vector, is called the surge speed, while v, the lateral component, is called the sway speed. Finally, the control inputs of the vessel are the surge trust Tu and the rudder angle Tr . In terms of these variables, the model derived in Fossen (2011) is   Rψ 0 ν η˙ = 0 1 (1) M ν˙ + C(ν)ν + Dν = Bf with η  [p, ψ]⊤ , ν  [u, v, r]⊤ , and f  [Tu , Tr ]⊤ . The matrices M , D, and B are given by      m11 0 0  d11 0 0 b11 0 M  0 m22 m23 , D  0 d22 d23 , B  0 b22 0

m23 m33

0 d32 d33

0 b32

⊤

3. THE PROBLEM

with M = M > 0 the symmetric positive definite inertia matrix including added mass, D > 0 is the hydrodynamic damping matrix, and B is the actuator configuration matrix. The matrix C(ν) is the matrix of Coriolis and centripetal forces and can be obtained from M (see Fossen (2011)). We place the origin of the body frame at a point on the center-line of the vessel with distance ǫ from the centre of mass. Following Fredriksen and Pettersen (2006), assuming that the vessel is starboard symmetric, there exists ǫ such that the resulting dynamics have mass and damping matrices satisfying this relation: M −1 Bf = [τu , 0, τr ]⊤ . Thus, with this choice of origin of the body frame, the sway dynamics become decoupled from the rudder control input, making it easier to analyze the stability properties of the sway dynamics. Using this convention, the model of the marine vessel (1) can be represented as   u p˙ = Rψ v     d11 u˙ Fu (v, r) − m u + τ u 11 = (2) v˙ X(u)r + Y (u)v ψ˙ = r

Consider the 3-degrees-of-freedom vessel depicted in Figure 1, which may describe an autonomous surface vessel (ASV) or an autonomous underwater vehicle (AUV) moving in the horizontal plane. We denote by p ∈ R2 the position of the vessel on the plane and ψ ∈ S1 its heading (or yaw) angle. The yaw rate ψ˙ is denoted by r.

r˙ = Fr (u, v, r) + τr . The functions X(u) and Y (u) are linear. Their expressions are given in Appendix A together with those of Fu and Fr . Denoting by χ := (p, u, v, ψ, r) the state of the vessel, the state space is X := R2 × R × R × S1 × R. Assumption 1. We assume that Y (u) < 0 ∀u ∈ [0, Vmax ].

We attach at the point p of the vessel a body frame aligned with the main axes of the vessel, as depicted in the figure, with the standard convention that the z-axis points into

This is a realistic assumption, since Y (¯ u) ≥ 0 would imply that the sway dynamics are undamped or unstable when the yaw rate r is zero.

The following result is key in the development of this paper. Theorem 1. (El-Hawwary and Maggiore (2013)). Let Γ1 , Γ2 , Γ1 ⊂ Γ2 ⊂ X , be two closed sets that are positively invariant for Σ and suppose that Γ1 is not compact. If (i) Γ1 is asymptotically stable relative to Γ2 , (ii) Γ2 is asymptotically stable, and (iii) Σ is LUB near Γ1 , then Γ1 is asymptotically stable for Σ.

601

IFAC NOLCOS 2016 590 D.J.W. Belleter et al. / IFAC-PapersOnLine 49-18 (2016) 588–593 August 23-25, 2016. Monterey, California, USA

Assumption 2. The ocean current is zero. This assumption is made to simplify the exposition of the ideas. The results of this paper can be adapted to handle unknown constant current. Consider a planar Jordan 3 curve γ expressed in implicit form as γ = {p : h(p) = 0}, where h is a C 1 function whose gradient never vanishes on γ. We assume that h : R2 → R is a proper function, i.e., all its sublevel sets {p : h(p) ≤ c}, c ∈ R, are compact. Since γ is assumed to be compact, there is no loss of generality in this assumption. Path Following Problem (PFP). Design a smooth timeinvariant feedback such that, for suitable initial conditions, the position vector p(t) → {p : h(p) = 0}, and the speed �p(t)� ˙ satisfies 0 < �p(t)� ˙ ≤ supt �p(t)� ˙ < ∞. In other words, we want to make the position of the ship converge to the path, travel along it without stopping, while guaranteeing that its speed is bounded. Geometric objects. Associated with the implicit representation h(p) = 0 of γ there are three geometric objects: the unit tangent and normal vectors, and the signed curvature. The unit normal vector at p is N (p) := dh⊤ p /�dhp �. The unit tangent vector at p is the counterclockwise rotation of N (p) by π/2, T (p) := Rπ/2 N (p). Finally, the signed curvature κ(p) is defined as 2 2 2 (∂y h)2 ∂xx h − 2∂xy h ∂x h ∂y h + ∂yy h (∂x h)2 κ(p) = − .  (3/2) (∂x h)2 + (∂y h)2 (3) The quantities N (p), T (p), κ(p) are defined not just on γ, but at all points p such that dhp �= [0 0]. If p0 �∈ γ, then N (p0 ), T (p0 ), κ(p0 ) are the normal vector, tangent vector, and curvature at p0 of the curve {p : h(p) = p0 }. 4. HIERARCHICAL CONTROL APPROACH The idea of the proposed solution is hierarchical in nature. (1) We regulate the surge speed u to a desired constant u ¯ > 0. (2) We consider the kinematic point-mass system p˙ = µ, and we solve the PFP with the constraint that �µ� = (¯ u2 + v 2 )(1/2) . The result of this design is a function µ(p, v). (3) Having found µ(p, v), we find the desired heading angle ψd (p, v) such that   u¯ = µ. Rψ d v This equation has a solution because, by construction, �µ� = (¯ u2 + v 2 )(1/2) . Intuitively, when ψ = ψd and u = u ¯, the marine vessel behaves like a kinematic point-mass subject to a path following control law. (4) Having found ψd (p, v), we define the output function e = ψ−ψd and we show that, under certain conditions 3 A curve is said to be Jordan if it is closed and has no selfintersections.

602

on u ¯ (possibly any u¯ > 0), the system with input τr and output e has relative degree 2. We define a controller τr (χ) that stabilizes the set where e = e˙ = 0. (5) We show that, if the curvature of the path is not too large, then the sway speed v remains bounded. We use Theorem 1 to prove that the hierarchical approach described above does indeed solve the PFP if the curvature of the path is not too large. 5. CONTROL DESIGN In this section we carry out the design steps 1-4 outlined above. The stability analysis of step 5 is carried out in the next section. Step 1: regulation of surge speed. This step is trivial, we choose the feedback linearizing control law d11 τu = −Fu (v, r) + u − Ku (u − u ¯), Ku > 0. (4) m11 Step 2: solution of the PFP for a kinematic pointmass. Consider the kinematic point-mass system p˙ = µ, (5) where the velocity vector µ ∈ R2 is the control input. We are to design µ such that �µ� = (¯ u2 + v 2 )(1/2) and the set {h(p)} is asymptotically stable. To this end, consider the output z = h(p). The derivative is z˙ = dhp µ = �dhp �N (p)⊤ µ.

(6)

  µ(p, v) := − u ¯σ(h(p)) N (p) + w(p, v)T (p).

(7)

Define This control input is composed of two terms. The first term is orthogonal to all level sets of h (in particular, to γ) and is responsible for making z → 0, as we shall see in a moment. The second term is tangent to the level sets of h and it will be designed to guarantee that �µ� = (¯ u2 + 2 (1/2) . The function σ : R → (−a, a), a ∈ (0, 1), is a v ) saturation function, chosen to be smooth, monotonically increasing, zero in zero, and such that lim|z|→∞ |σ(z)| = a. The positive scalar a is a design parameter. Since {T (p), N (p)} is an orthonormal frame, substitution of (7) into (6) gives z˙ = −�dhp �¯ uσ(z). Since, by assumption, �dhp � �= 0 on γ, by continuity of h we have that �dhp � �= 0 in a neighborhood of γ. Therefore, for any u ¯ > 0, the set {p : h(p) = 0} is asymptotically stable. Next we design w(p, v) such that �µ(p, v)� = (¯ u2 +v 2 )(1/2) . Referring to the identity (7), since {T (p), N (p)} form an orthonormal frame, we have �µ�2 = u ¯2 σ 2 (h(p)) + w2 (p, v). Setting (1/2)  2 , (8) w(p, v) := u ¯ (1 − σ 2 (h(p))) + v 2

we have �µ(p, v)� = (¯ u2 + v 2 )(1/2) , as required. Note that the above expression of w(p, v) is well-defined and smooth because, by construction, |σ| < a ≤ 1. In conclusion, we have the following result.

IFAC NOLCOS 2016 August 23-25, 2016. Monterey, California, USA D.J.W. Belleter et al. / IFAC-PapersOnLine 49-18 (2016) 588–593

Lemma 1. The feedback µ(p, v) defined in (7) and (8) makes the set {p ∈ R2 : h(p) = 0} asymptotically stable for the kinematic point-mass system (5). Step 3: definition of ψd . We need to find a smooth function ψd (p, v) such that   u¯ = µ(p, v). Rψ d v The vector on the left-hand side of the identity above has norm (¯ u2 +v 2 )(1/2) and, by construction, so does the vector on the right-hand side. Thus ψd is just the phase of the vector µ, (9) ψd (p, v) := atan2(µ2 (p, v), µ1 (p, v)), where atan2 is the four-quadrant arctangent function such that atan2(sin(θ), cos(θ)) = θmod2π. Step 4: regulation of ψ to ψd . We define the output function e = ψ − ψd . Then e˙ = g(p, u, v)r + f (p, u, v, ψ), (10) where   g(p, u, v) = 1 − ∂v ψd (p, v) X(u),     u − ∂v ψd Y (u)v. f (p, u, v, ψ) = − ∂p ψd (p, v) Rψ v

Taking one more time derivative along (2) we get   e¨ = g(p, u, v) Fr (v, r) + τr + g(χ)r ˙ + f˙(χ). Lemma 2. The following identity holds:   σ(h(p))v u ¯ , 1+ ∂v ψd = − 2 u ¯ + v2 w(p, v)

(11)

where w(p, v) is given in (8). Suppose that u ¯|X(¯ u)| 1− 2 >0 (12) u ¯ + v2 for all v ∈ R. Then, the parameter a ∈ (0, 1] in the saturation σ can be chosen small enough that system (2) with input τr and output e = ψ − ψd (p, v) has relative degree 2 at any point χ = (p, u, v, ψ, r) such that u = u ¯. The proof of the lemma is omitted due to space limitations. Remark 1. Condition (12) is met for all u¯, for the ship parameters in Fredriksen and Pettersen (2004) that are used in our simulations. Assuming that (12) holds, we define the smooth feedback linearizing control law  1 τr = −Fr (v, r) + − f˙(χ) − g(χ)r ˙ g(p, u, v)  (13) − Kp sin(ψ − ψd (p, v)) − Kd (r − ψ˙ d (χ)) ,

where dot on a function denotes the time derivative of the function along the vector field (2) with τu as in (4). With the feedback above, we obtain e¨ + Kp sin(e) + Kd e˙ = 0. This is the equation of a pendulum with friction. Thus the equilibrium (e, e) ˙ = (0, 0) is almost globally asymptotically stable. This implies that the set {χ ∈ X : ψ = ψd (p, v), r = ψ˙ d (χ)} is stable. Moreover, this set is also asymptotically stable if the original system (2) with the chosen feedbacks τu and τr has no finite escape times. The absence of finite escape times will be proved in the next section. 603

591

Summary of feedback design. We have designed the following feedback control law d11 τu = −Fu (v, r) + u − Ku (u − u ¯), m11  1 (14) − f˙(χ) − g(χ)r ˙ τr = −Fr (v, r) + g(p, u, v)  − Kp sin(ψ − ψd (p, v)) − Kd (r − ψ˙ d (χ)) ,

where u ¯, Ku , Kp , Kd > 0 are design parameters and ψd (p, v) = atan2(µ2 (p, v), µ1 (p, v)),   µ(p, v) = − u ¯σ(h(p)) N (p) (1/2)  2 T (p). + u ¯ (1 − σ 2 (h(p))) + v 2 Finally, σ(z) is any smooth, monotonically increasing function such that σ(0) = 0 and lim|z|→∞ |σ(z)| = a, where a ∈ (0, 1] is sufficiently small as in Lemma 2. For instance, σ(z) = a tanh(Kz), K > 0, has the desired properties. As we discussed, in the absence of finite escape times the feedback above asymptotically stabilizes the set Γ2 := {χ ∈ X : u = u ¯, ψ = ψd (p, v), r − ψ˙ d (p, u, v, r) = 0}. In Theorem 2 below we show that it solves the PFP. 6. STABILITY ANALYSIS As we shall see in a moment, the control design procedure developed in the previous section amounts to the simultaneous stabilization of the two nested closed sets Γ1 ⊂ Γ2 Γ2 = {χ ∈ X : u = u ¯, ψ = ψd (p, v), r = ψ˙ d (χ)}, Γ1 = {χ ∈ Γ2 : h(p) = 0}. On Γ2 , the ship behaves like a kinematic point-mass subject to a path following control law. On Γ1 , the ship is on the path with a desired surge speed u¯. Showing that the feedback (14) solves the PFP amounts to showing that Γ1 is asymptotically stable. To prove this property, we will use Theorem 1. To begin, we observe that, by design, Γ2 is stable, and asymptotically stable if solutions starting in a neighborhood of Γ2 have no finite escape times. Assume for a moment that this is the case. On Γ2 , we have   u ¯ . p˙ = Rψd v By the construction in step 2,   u ¯ = µ(p, v), Rψd v and thus p˙ = µ(p, v). By Lemma 1, the set {h(p) = 0} is asymptotically stable for the above dynamics. In the absence of finite escape times, this implies that Γ1 is asymptotically stable relative to Γ2 . Therefore, in order to prove asymptotic stability of Γ1 , we will prove that the closed-loop system has no finite escape times near Γ2 and, in addition, property (iii) of Theorem 1 holds. This is done in the next lemma. Lemma 3. Consider system (2) with the feedbacks defined in (14), and suppose Assumptions 1 and 2 hold. Suppose further that the desired surge speed u ¯ ∈ [0, Vmax ] is such that 1+¯ uX(¯ u)/(¯ u2 +v 2 ) �= 0. Then for any initial condition in a neighborhood of Γ2 , the solution is defined for all

IFAC NOLCOS 2016 592 D.J.W. Belleter et al. / IFAC-PapersOnLine 49-18 (2016) 588–593 August 23-25, 2016. Monterey, California, USA

Using the result of Lemma 3 and of Theorem 1, we get the following result. Theorem 2. Consider system (2) with the feedbacks defined in (14), suppose that Assumptions 1 and 2 hold, and assume that the desired surge speed u ¯ ∈ [0, Vmax ] is chosen such that condition (12) holds. If the curvature κ of γ |Y (¯ u)| satisfies the bound maxp∈γ |κ(p)| < |X(¯ u)| , then Γ1 and Γ2 are asymptotically stable, implying that feedback (14) solves the PFP. Remark 2. It is interesting to note that in (Moe et al., 2014, Theorem 1), the authors present a stability result for a path following control law with a similar, but more restrictive, curvature bound, max |κ| < (1/3)|Y (¯ u)/X(¯ u)| compared to that in Theorem 2. 7. SIMULATION RESULTS In this section two case studies are presented to verify the proposed path following strategy. For this purpose we consider a supply vessel described by the model (2) with the function descriptions and model parameters given in Appendix A. In the first case study we consider the case of following of following a straight-line path. Note that in the proof we assume the curves are Jordan, which the straightline is not since it is not closed. However, the straight-line is a common test case and serves as a good proof of concept of the control strategy. The second case study considers following of a Cassini oval. Both the simulations presented in the following are based on the model parameters given in Fredriksen and Pettersen (2004). 7.1 Case 1: Straight-Line Path In this case study the goal is to follow a straight-line path aligned with the inertial x-axis. Hence, h(p)  −py and the implicit representation of the path is given by γ = {p : −py = 0}. This assures that the unit normal vector, N (p), points in the negative y-direction and the unit tangent vector, T (p), points in the positive x-direction. The desired velocity is chosen as u¯ = 2 [m/s] and the saturation function is set to σ(h(p)) = 2/π tan−1 (h(p)). The initial conditions are given by χ0 := ([0, 100], 0, 0, π/2, 0) and the controller gains from (14) are given by Ku = 0.5, Kp = 0.4, and Kd = 2. The trajectory of the ship in the x-y plane can be seen in Figure 2, where the ship icons superimposed on the path give the orientation of the ship at those points. 604

100 0

−100 −200

0

100 200 300 400

500 600 700

x [m]

800 1000

Fig. 2. Path of the ship (the ship is not to scale). 150 100

Error [m]

The proof of the lemma is omitted due to space limitations. The idea is roughly this. Using the fact that, by design, Γ2 is stable, one can show that the path following error z = h(p) is uniformly bounded. Using this fact, one can show that Γ1 is stable. In a neighborhood of Γ1 , the vsubsystem describing the sway dynamics of the ship can be viewed as a perturbation of the “nominal” dynamics. The assumption 1 + u ¯X(¯ u)/(¯ u2 + v 2 ) �= 0 guarantees that the equilibrium v = 0 is exponentially stable for the nominal dynamics, and that v remains uniformly bounded in the presence of the perturbation, so that the system is LUB near Γ1 .

200

y [m]

t ≥ 0. Moreover, if the curvature κ of γ satisfies the |Y (¯ u)| bound maxp∈γ |κ(p)| < |X(¯ u)| , then the closed-loop system is LUB near Γ1 .

50 0

−50 0

50 100 150 200 250 300 350 400 450 500 Time [s]

Fig. 3. Path following error of the ship. From Figure 2 it can be seen that the trajectory converges to the x-axis and that the ship travels in the direction of the unit tangent vector T (p). The positional error of the ship w.r.t. the path, i.e. py , can be seen in Figure 3, from which it can clearly be seen that the error converges to zero. 7.2 Case 2: Cassini Oval In this case study the goal is to follow a Cassini oval. This implies that h(p)  (p2x + p2y )2 − 2a2 (p2x − p2y ) + a4 − b4 and that the path is implicitly described by γ = {p : (p2x + p2y )2 − 2a2 (p2x − p2y ) + a4 − b4 = 0}. where in this case study a = 22.5 [m] and b = 24.9 [m]. This results in a curve for which the maximum curvature ¯ = maxp∈γ |κ(p)| = 0.0785 and with a desired velocity u 2 [m/s] the ratio |Y (¯ u)|/|X(¯ u)| = 0.2483. Note that this curve satisfies the curvature condition of Theorem 2 showing that this is not a very restrictive condition, since it allows a ship with a length of approximately 83 meters to follow a curve whose diameter (the maximum distance between any two of its points) is approximately 70 metres. The saturation function is set to σ(h(p)) = 2/π tan−1 (αh(p)), where α is a parameter that can be used to tune the slope of the saturation function. In this case the magnitude of h(p) is large, therefore α needs to be small to make the saturation effective close to the path and we choose α = 10−4 . The initial conditions are given by χ0 := ([15, 45], 0, 0, −2/3π, 0) and the controller gains from (14) are given by Ku = 1, Kp = 30, and Kd = 5. The trajectory of the ship and the desired oval can be seen in Figure 4. From Figure 4 we can clearly see convergence to the desired oval and from the superimposed ships it can be seen that the heading of the vessel is not tangent to the oval. Its velocity vector, on the other hand, is tangent to the path. From the plot of the sway velocity in Figure 5 it can be seen that this motion induces quite large sway velocities relative to the desired surge velocity u ¯ = 2 [m/s]. The value of h(p) is plotted in Figure 6 which shows that h(p) is driven to zero as the ship converges to the path,

IFAC NOLCOS 2016 August 23-25, 2016. Monterey, California, USA D.J.W. Belleter et al. / IFAC-PapersOnLine 49-18 (2016) 588–593

593

REFERENCES 30 20

x [m]

10 0

−10 −20 −30 0

−20 −10

10

20

30

y [m]

40

50

60

Fig. 4. Ship’s path and the Cassini oval (ship not in scale). v [m/s]

1.5 1

0.5 0

−0.5 0

10 20 30 40 50 60 70 80 90 100 Time [s]

Fig. 5. Sway velocity of the ship. h(p) [m4 ]

6 8×10 6 4 2 0 −2 0 10 20 30 40 50 60 70 80 Time [s]

90 100

Fig. 6. Magnitude of h(p) as the ship converges to the path. showing that the ship is able to track the specified Cassini oval in accordance with the theoretical analysis. 8. CONCLUSIONS AND FUTURE WORK In this paper we presented a methodology to design path following controllers for a class of underactuated marine vessels. This methodology allows one to migrate a path following controller designed for a point-mass to one that is guaranteed to work for the underactuated vessel. As we mentioned in the introduction, the proposed solution is an initial step. For simplicity, we assumed the curve to be Jordan and the ocean current to be absent. We will remove these assumptions in future work. Appendix A. FUNCTIONS USED IN THE MODEL The functions Fu , X(u), Y (u), and Fr are given by: X(u) 

1 m11 (m22 v + m23 r)r, m223 −m11 m33 m23 −d23 m33 u + d33 , m22 m33 −m223 m22 m33 −m223

Y (u) 

(m22 −m11 )m23 u m22 m33 −m223

Fu 

Fr (u, v, r) 

−

d22 m33 −d32 m23 , m22 m33 −m223

m23 d22 −m22 (d32 +(m22 −m11 )u) v m22 m33 −m223

+

m23 (d23 +m11 u)−m22 (d33 +m23 u) r. m22 m33 −m223

605

Aguiar, A.P. and Hespanha, J.P. (2007). Trajectorytracking and path-following of underactuated autonomous vehicles with parametric modeling uncertainty. IEEE Transactions on Automatic Control, 52(8), 1362–1379. Aguiar, A.P. and Pascoal, A.M. (2007). Dynamic positioning and way-point tracking of underactuated AUVs in the presence of ocean currents. International Journal of Control, 80(7), 1092–1108. Børhaug, E., Pavlov, A., and Pettersen, K.Y. (2008). Integral LOS control for path following of underactuated marine surface vessels in the presence of ocean currents. In Proc. of the 47th IEEE Conference on Decision and Control, 4984–4991. Do, K.D. and Pan, J. (2006). Global robust adaptive path following of underactuated ships. Automatica, 42(10), 1713–1722. El-Hawwary, M.I. and Maggiore, M. (2013). Reduction theorems for stability of closed sets with application to backstepping control design. Automatica, 49(1), 214– 222. Fossen, T.I. (2011). Handbook of Marine Craft Hydrodynamics and Motion Control. Wiley. Fredriksen, E. and Pettersen, K.Y. (2004). Global κexponential way-point manoeuvering of ships. In Decision and Control, 2004. CDC. 43rd IEEE Conference on, volume 5, 5360–5367. IEEE. Fredriksen, E. and Pettersen, K.Y. (2006). Global κexponential way-point maneuvering of ships: Theory and experiments. Automatica, 42(4), 677–687. Lapierre, L. and Soetanto, D. (2007). Nonlinear pathfollowing control of an AUV. Ocean engineering, 34(11), 1734–1744. Li, Z., Sun, J., and Oh, S. (2009). Design, analysis and experimental validation of a robust nonlinear path following controller for marine surface vessels. Automatica, 45(7), 1649–1658. Moe, S., Caharija, W., Pettersen, K.Y., and Schjølberg, I. (2014). Path following of underactuated marine surface vessels in the presence of unknown ocean currents. In American Control Conference (ACC), 2014, 3856–3861. IEEE. Nielsen, C., Fulford, C., and Maggiore, M. (2010). Path following using transverse feedback linearization: Application to a maglev positioning system. Automatica, 46(3), 585–590. Oh, S.R. and Sun, J. (2010). Path following of underactuated marine surface vessels using line-of-sight based model predictive control. Ocean Engineering, 37(2), 289–295. Roza, A. and Maggiore, M. (2014). A class of position controllers for underactuated VTOL vehicles. IEEE Transactions on Automatic Control, 59(9), 2580–2585. Skjetne, R., Fossen, T.I., and Kokotovi´c, P.V. (2005). Adaptive maneuvering, with experiments, for a model ship in a marine control laboratory. Automatica, 41(2), 289–298.