Continuous-Curvature Path Generation Using Fermat's Spiral

Modeling, Identiﬁcation and Control, Vol. 34, No. 4, 2013, pp. 183–198, ISSN 1890–1328

Continuous-Curvature Path Generation Using Fermat’s Spiral

Anastasios M. Lekkas 1 Andreas R. Dahl 2 Morten Breivik 3 Thor I. Fossen 4

1Centre for Ships and Ocean Structures, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway. E-mail: [email protected]

2Department of Marine Technology, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway. E-mail: [email protected]

3Department of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway. E-mail: [email protected]

4Centre for Autonomous Marine Operations and Systems, Department of Engineering Cybernetics, Norwegian University of Science and Technology, NO-7491 Trondheim, Norway. E-mail: [email protected]

Abstract

This paper proposes a novel methodology, based on Fermat’s spiral (FS), for constructing curvature- continuous parametric paths in a plane. FS has a zero curvature at its origin, a property that allows it to be connected with a straight line smoothly, that is, without the curvature discontinuity which occurs at the transition point between a line and a circular arc when constructing Dubins paths. Furthermore, contrary to the computationally expensive clothoids, FS is described by very simple parametric equations that are trivial to compute. On the downside, computing the length of an FS arc involves a Gaussian hypergeometric function. However, this function is absolutely convergent and it is also shown that it poses no restrictions to the domain within which the length can be calculated. In addition, we present an alternative parametrization of FS which eliminates the parametric speed singularity at the origin, hence making the spiral suitable for path-tracking applications. A detailed description of how to construct curvature-continuous paths with FS is given.

Keywords: Path planning, Fermat’s spiral, continuous curvature, parametric curve, path tracking

1 Introduction vironmental forces. A thorough treatment of the subject can be found in (LaValle, 2006). Path-planning systems are of great significance when In the case of underactuated vehicles, especially in it comes to the performance and mission accomplish- path-following and path-tracking motion control sce- ment of practically every type of vehicle, as well as narios, the shape and properties of the path have a mechatronic devices such as computer numerical con- direct influence on the guidance system, which is re- trol (CNC) machines and robotic manipulators, to sponsible for generating reference trajectories to be fed name a few. Depending on the task demands, the to the autopilot. The reason for this is that the path path-planning algorithms generate appropriate paths curvature affects the heading angle commands and the or trajectories usually by taking into account both the parametric speed affects the velocity commands. In physical constraints of the system under consideration, these applications, as a rule, the path-planning algo- and the workspace constraints, such as obstacles or en- rithm will first define a number of ordered waypoints

doi:10.4173/mic.2013.4.3 c 2013 Norwegian Society of Automatic Control Modeling, Identification and Control on the map which will have to be connected sequen- dimensions, and is consequently also true for the tor- tially so as to form the path. Connecting the waypoints sion (Harary and Tal, 2012). Clothoids have also been can be achieved in many different ways, with each one used in formation control applications, see for instance having its own advantages and drawbacks. However (Shanmugavel et al., 2007a) and (Shanmugavel et al., two main categories can be distinguished: 2010). The drawback of clothoids is that their coordi- nates do not have a closed-form expression since they • Combining straight lines and arc segments. involve computation of the Fresnel integrals. • Using splines. The second category of methodologies pertains to connecting waypoints using splines. There is a vast Regarding the first category, a simple and intuitive literature on this subject, mostly due to the research way of getting continuous paths without corners is to stemming from the computer graphics community. inscribe a circle between two lines in order to form a Some approaches, such as the cubic Hermite spline in- curved path. In 1957, Dubins showed that for a parti- terpolation (CHSI), result in continuous velocity paths cle that moves forward with unity speed, the shortest with more tractable shape (without wiggling between possible path that meets a maximum curvature bound the waypoints) but have a discontinuous curvature at between a starting position with predefined orientation the waypoint locations. Others, like the natural cu- (starting pose) and a finishing position with predefined bic splines, give curvature-continuous paths but the orientation (finishing pose) consists of at most three resulting shape can be very impractical and ineffi- pieces, each of which is either a straight line or an arc cient. An interesting alternative is to use Pythagorean- of a circle of radius R > 0 (Dubins, 1957). Reeds and hodograph (PH) curves which are characterized by Shepp(1990) extended Dubins’ result for a car-like ve- the special property that their parametric speed is hicle by taking into account backward motion, hence a polynomial (or rational) function of the path pa- allowing to include cusps along the path. The Du- rameter (Farouki, 2008). The PH curves were pre- bins car was further extended to include altitude, hence sented for the first time in (Farouki and Sakkalis, 1990) leading to the Dubins airplane (Chitsaz and LaValle, and the idea occurred while the authors were investi- 2007). A discrete analogue of Dubins’ result using gating the existence of planar or spatial curves that polygonal paths was proved by Eriksson-Bique et al. have natural parametrizations (Farouki and Sakkalis, (2012), although the authors state that different defi- 1991). PH curves do not necessarily entail curvature- nitions of discrete curvature-constrained motion than continuous paths, this issue was addressed by Bruyn- the one they used might lead to shorter paths. Chang inckx and Reynaerts(1997). In addition, several other et al.(2012) augmented Dubins’ result by incorporat- researchers have employed PH curves for path plan- ing a directional-cost element in order to minimize the ning, see for instance (Toussaint et al., 2001), (Shanmu- costs associated with the construction of underground gavel et al., 2007b) and (Gajny et al., 2012). Interest- mines. In (Hota and Ghose, 2013), a modified Dubins ingly, PH curves of monotone curvature have also been problem was considered where a rectilinear path and used to connect straight lines in a way such that the direction, to which a vehicle should converge, was spec- overall curve is curvature continuous (Farouki, 1997). ified instead of a final point. The goal was to find the optimal 2D path for convergence to a rectilinear path Contrary to what might be the case in the field of for a vehicle that is initially located at any distance computer graphics, where often the goal is to design from the desired path. Moreover, Techy and Woolsey aesthetically pleasing curves (Farin and Sapidis, 1989), (2009), Bakolas and Tsiotras(2010) and Bakolas and in motion control scenarios it is preferable to assign Tsiotras(2013) deal with the problem of finding a Du- paths that consist of straight lines and arc segments bins path for a vehicle that moves in a constant drift rather than splines, mainly because the latter implies field. In this case, it is not always possible to find a that at least one of the control surfaces (a ship’s rudder, Dubins path. for instance) will always be active, due to the relent- The main disadvantage of the Dubins path is the cur- lessly varying curvature. In addition, using straight vature discontinuity which occurs at the meeting points lines and arc segments makes it more likely and easier of two consecutive path segments, for more details see to ensure that the path will not include wiggles or zig- (Tsourdos et al., 2011). This problem can be circum- zags between two waypoints. This property, which is vented by employing a clothoid arc between a straight also related to allowance (Dahl, 2013), can be critical line and a circular arc (Fraichard and Scheuer, 2004). when navigating in an area where obstacles are present. The clothoid (also known as Euler spiral, Cornu spiral Last but not least, moving on straight lines enables and spiro) is useful in path-planning applications due adaptive techniques to provide a faster and more ro- to its property of having its curvature change linearly bust estimate of unknown external disturbances, such with arc-length. This notion can be extended to three as ocean currents (Fossen and Lekkas, 2014).

184 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral”

This paper proposes an alternative way of design- curve segments, each single curve segment can be ex- ing curvature-continuous paths, which belongs to the pressed by the set (Haugen, 2010): first of the two aforementioned categories. More specif- 2 ically, Fermat’s spiral (FS) is employed as a means Pi := {pi ∈ R | (3) to connect successive straight lines in a plane. The pi = pi,p($)∀ $ ∈ Ii = [$i,0, $i,1] ⊂ R}, main motivation is that the curvature of FS is equal to zero at the origin, a property which makes it suitable and, consequently, the path can be written as a super- for connecting it with a straight line without induc- set of n curve segments: ing curvature discontinuities, as is the case with circu- n lar arcs. Moreover, contrary to clothoids, FS involves [ Ps = Pi. (4) very simple parametric equations that are easy to pro- i=1 gram and fast to compute. The use of FS for path- planning applications was initially studied by Dahl In this paper, we consider two-dimensional curves: (2013) and later implemented for building curvature- x ($) continuous and collision-free paths using Voronoi dia- p ($) = p , (5) p y ($) grams in (Candeloro et al., 2013). In this paper, we p investigate the properties of FS and show that it can from which, the path-tangential (or, alternatively, accomplish the task of generating curvature-continuous course) angle is computed as: paths and therefore be used in path-following applications. Furthermore, we extend the work done by Dahl 0 0 χp($) = atan2(yp($), xp($)), (6) (2013) and Candeloro et al.(2013) in three ways: a) we propose a different parametrization which removes where (·)0 denotes the first derivative w.r.t the path the speed singularity at the origin, hence making FS parameter $. In contrast, the derivation w.r.t. time is paths suitable also for path-tracking applications, b) denoted (·˙). The parametric path (5) is regular when: we present a more thorough analysis regarding the cal- q culation of the FS arc length, and c) we show that 0 0 2 0 2 |pp($)| = xp($) + yp($) 6= 0. (7) FS can be used successfully in combination with circular arcs in order to produce Dubins paths with FS arc The curvature of the regular path is given by the ex- transition. pression:

0 00 0 00 xp($)yp ($) − yp($)xp ($) κ = q . (8) 2 Preliminaries 3 0 2 0 2 xp($) + yp($)

2.1 Basic Definitions Naturally, a minimum requirement is that subse- Similarly to Breivik and Fossen(2009), a planar path is quent path segments should be connected. The level considered to be a one-dimensional manifold that can of connection can be described by the path smooth- n be expressed by the set ness. Parametric continuity is denoted C , where n is the degree of smoothness, and implies that the first 2 P := p ∈ R | p = pp($) ∀ $ ∈ R (1) n parametric derivatives match at the common point of two subsequent paths (Barsky and DeRose, 1989). n where $ is the path parameter and pp($) denotes the Similarly, geometric continuity is denoted G , and is a position of a point belonging to the path. The gen- relaxed form of parametric continuity, which does not eralized path parameter does not necessarily have any impose constraints on the parameter’s speed along the physical meaning. It can be the path length, but is path. For many applications (such as path following of usually more convenient to be defined within the unit underactuated vehicles) it is sufficient to have geomet- domain: ric continuity, while for others (such as path tracking) parametric continuity is required. For more informa- $ ∈ [0, 1] , (2) tion on path smoothness as well as other path evaluation criteria, the reader is referred to Dahl(2013) and because it can be difficult to construct an elegant basis Lekkas and Fossen(2013). for a general interval [$i, $i+1]. It is common prac- Given a number of ordered points on a plane, it is tice to implement p (·) as a piecewise-defined func- useful to distinguish the following two types of curves: tion, which reduces the function complexity but demands consideration at the transition points between • Interpolating curves, which pass through all the subpaths. For planar paths consisting of a number of given points.

185 Modeling, Identiﬁcation and Control

T 2D path where p0 = [N0,E0] is the starting point, L is the 30 length of the path, and χl is the path-tangential angle of the straight line. An example of a path consisting 20 only of straight lines can be seen in Fig.1. As it can

N (m) be observed in Fig.2, the path tangential angle is dis- 10 Computed path Line between wpts continuous and the curvature cannot be deﬁned at the Waypoints waypoint locations. Such a path cannot be followed by 0 0 10 20 30 40 50 60 70 80 90 an underactuated vehicle and should only be assigned E (m) to a fully-actuated (3-DOF in the planar case) vehicle, Figure 1: Piecewise linear path such as a wheeled mobile robot. Even in that case, though, the robot must stop at the waypoint locations

N Position and then change its heading so as to adjust its attitude 30 according to the direction of the next linear segment 20 before starting to move along it. This problem can (m) 10 be alleviated by using circular segments to smooth the path. 0

E Position 100 2.3 Circular Arcs

50 Constructing a path using linear and circular arc seg- (m) ments is an extensively studied and popular approach

0 which also leads to the shortest feasible path, according to Dubins’ result (Dubins, 1957). Two path variations Course tangential angle (χ) 180 are probably the most common ones in this case, and their differences occur due to the different locations 0 where the circular segments are placed. The first varia- (deg) tion defines an interpolating curve that passes through −180 all the waypoints (that is, the Dubins path), while the

Curvature (κ ) second deﬁnes an approximating curve that passes only 1 through the ﬁrst and last waypoint (also known as cir- 0.5 ) 1 cular smoothing path), see (Breivik and Fossen, 2004b) − 0

m 1 ( and (Fossen, 2011, Sec. 10.3). Both are G paths, but −0.5 the ﬁnal geometry is diﬀerent. A circular arc can be −1

0 parametrized as follows: 22 53 77 118 128 Path length (m) cN + R cos (α0 + $ (α1 − α0)) pcir ($) = , (10) Figure 2: Piecewise linear path geometric properties cE + R sin (α0 + $ (α1 − α0))

T where c = [cN , cE] and R are the center and radius of • Approximating curves, which do not pass through the circle which the arc is a segment of, while α0 and α1 all the given points. are the angles at which the arc starts and ends, respectively. Fig.3 shows a straight-line path smoothened This categorization helps describe more accurately by circular arcs. The circular arc radius R is chosen methods that use the same building blocks but have equal to that of the vehicle’s minimum turning circle diﬀerent properties. For example, both the Dubins (or, the sharpest turn achievable by the vehicle), which paths and the paths resulting from circular smooth- corresponds to the curvature constraint: ing (Fossen, 2011, Sec. 10.3.1) use straight lines and circular arcs, however they result in diﬀerent paths. 1 κmax = . (11) Rmin 2.2 Piecewise Linear Path It can be seen in Fig.4 that the course tangential angle The parametric form of a straight line connecting two is continuous but the curvature is discontinuous at the points in a plane is given by (Breivik and Fossen, 2009): locations where the transition between a straight line and a circular arc occurs, which indicates that the path 1 N0 + L$ cos (χl) is G . The curvature discontinuity entails a disconti- pline ($) = , (9) E0 + L$ sin (χl) nuity in the desired lateral acceleration of the vehicle

186 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral”

2D path 2D path 30 30

20 20

N (m) Computed path N (m) 10 10 Computed path Line between wpts Line between wpts Waypoints Waypoints 0 0 0 20 40 60 80 100 0 10 20 30 40 50 60 70 80 90 E (m) E (m) Figure 5: Clothoid smoothing path Figure 3: Circular smoothing path

N Position N Position 30 30 20 20 (m) 10 (m) 10 0 0 E Position E Position 100 100

50 50 (m) (m)

0 0 Course tangential angle (χ) Course tangential angle (χ) 180 180

0 (deg) (deg)

−180 −180 Curvature (κ ) Curvature (κ ) 0.6 0.4 0.4 )

0.2 1 − )

1 0.2 m −

0 ( m

( 0 −0.2 −0.2 −0.4 0 10 19 29 43 46 50 66 70 75 104 110 117 120 0 15 25 46 50 70 74 109 116 122 Path length (m) Path length (m) Figure 6: Clothoid smoothing path geometric proper- Figure 4: Circular smoothing path geometric properties ties

Its parametrization is because of the relationship (Tsourdos et al., 2011): R $ 1 2 2 x0 + 0 cos 2 cξ + κ0ξ + χ0 dξ |α| = |u| κ, (12) pcl ($) = R $ 1 2 , y0 + 0 sin 2 cξ + κ0ξ + χ0 dξ where α is the lateral acceleration vector and u the (13) T velocity vector. This will affect the input to the head- where p0 = [x0, y0] is the initial position, c is a sharp- ing controller and the vehicle performance in general. ness coefficient describing the increase in curvature, κ0 Finally, from Figs2 and4 it can be observed that the is the curvature at p0, χ0 is the course angle at p0 path using circular arcs is shorter. This agrees with and ξ is a dummy integration variable. Unfortunately, intuition since the circular arc operates as a shortcut the integrals of (13) have no analytic solution and a and avoids going all the way to the second waypoint. numerical approach must be employed. There are two ways clothoids can be used as tran- 2.4 Clothoid sition curves. The first one is to replace the circular arc with a clothoid. Since the clothoid has an increas- The clothoid is a curve with linear increase in curvature ing curvature w.r.t. the path length, it is necessary to along its length and thus an appealing transition curve. use two clothoid segments: one with starting point at

187 Modeling, Identiﬁcation and Control

1.5 2.5

1 2 0.5

0 1.5

−0.5 κ y (m)

−1 1

−1.5 0.5 −2

−2.5 −2 −1 0 1 2 3 0 0 1 2 3 4 5 6 7 x (m) θ

Figure 7: Fermat’s spiral, one complete rotation. The Figure 8: Fermat’s spiral curvature. The asterisk asterisk marks the point where the curve has marks the value of θ for which the curve has the maximum curvature the maximum curvature.

3 Fermat’s Spiral 3.1 Introduction the end of the first straight line and ending point at the middle of the curved segment (this is the part with The term Archimedean spirals refers to a family of increasing curvature), and one with starting point at curves described by the equation (Weisstein, 2013a): the middle of the curved segment and ending point at r = kθ1/n, (14) the beginning of the second straight line (this is the part with decreasing curvature). The second clothoid where r is the radial distance, θ is the polar angle, segment is actually a mirrored version of the first one, k is a scaling constant, and n determines how tightly therefore the computation needs to be done only once. the spiral is wrapped. The Fermat’s spiral (FS) is an We will refer to this as clothoid smoothing. The second Archimedean spiral with n = 2 and therefore given by approach is to use the clothoid as a transition curve the equation: between the linear segment and the circular arc. To √ achieve curvature continuity, that is G2 continuity, the r = k θ. (15) transition curve must have zero curvature at one end The spiral curvature is (Weisstein, 2013b): and R−1 at the other. Similarly to clothoid smoothing, a mirrored curve is necessary for the transition between √ 1 2 θ 3 + 4θ2 the circular arc and the second straight line. We will κ (θ) = 3 , (16) k 2 2 refer to this as circular smoothing with clothoid transi- (1 + 4θ ) tion. Figs5–6 show a clothoid smoothing path and its which gives κ (0) = 0 and κ (θ) > 0 for all θ > 0. Fig.7 geometrical properties, respectively. In this case, the shows a full rotation of (15), and Fig.8 shows a plot length of the path is even shorter, when compared to of the curvature for a full rotation with an asterisk in- the circular smoothing case. This is related to the dif- dicating the value of θ corresponding to the maximum ferent wheel over and pull out points of the arc in each curvature. The curve was first studied by Fermat in case. In accordance with what has been mentioned 1636 and published in (Fermat, 1679). earlier, this is an approximating path. 3.2 Cartesian Parametrization The computational cost of (13) is the main motivation for investigating alternative solutions. One such A Cartesian parametrization of (15) is (Dahl, 2013): alternative is Fermat’s spiral, and the contribution of √ x r cos(θ) k θ cos(θ) this article is the application of such a spiral for path = = √ . (17) smoothing. y r sin(θ) k θ sin(θ)

188 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral”

0.8 χ = −22.5◦ k = 0.5 0 ◦ 1.2 k χ0 = 0 = 1 0.7 ◦ k = 2 χ0 = 45 1 0.6

0.5 0.8

0.4 0.6 N (m) N (m)

0.3 0.4 0.2

0.2 0.1

0 0 0 0.2 0.4 0.6 0.8 0 0.2 0.4 0.6 0.8 1 1.2 1.4 E (m) E (m)

Figure 9: Fermat’s spiral segment with fixed scaling Figure 10: Fermat’s spiral with fixed initial tangent an- (k=1) and different initial tangent angles gle (χ0 = 0) and different scaling constants

T To allow for different initial positions p0 = [x0, y0] , 3.3 Parametrized Speed and Acceleration turning both left and right, and different initial tangent The first derivative of (18) is (Dahl, 2013): angles χ0, the following parametrization is proposed: √ d x0 + k θ cos (ρθ + χ0) pFS (θ) = pFS (θ) = √ , (18) dθ y0 + k θ sin (ρθ + χ0) k cos (ρθ + χ ) − 2ρθ sin (ρθ + χ ) √ 0 0 , (21) where 2 θ sin (ρθ + χ0) + 2ρθ cos (ρθ + χ0) ( 1 for an anti-clockwise turn, and and the second derivative is ρ = (19) −1 for a clockwise turn. d2 p (θ) = dθ2 FS The domain in (18) is θ ∈ [0, θ ], where θ is the end end −k 4θ2 + 1 cos (ρθ + χ ) + 4ρθ sin (ρθ + χ ) parameter corresponding to the spiral end point, yet 0 0 . 3 4θ2 + 1 sin (ρθ + χ ) − 4ρθ cos (ρθ + χ ) to be determined. This parametrization is thus neither 4θ 2 0 0 unit domain nor unit speed, but it can be made unit (22) domain by setting θ = $θ . Fig.9 shows how chang- end Similarly, for the mirrored curve (20), the first and ing χ affects the orientation of the spiral and Fig. 10 0 second derivatives are given by (34)–(35). Equations demonstrates the effect of the scaling constant k. (21)–(22) and (34)–(35) have a singularity at the begin- As was the case with the clothoid in Sec. 2.4, we ning of the path, i.e. θ = 0. This is a drawback since will distinguish between FS smoothing and circular the velocity and acceleration at the beginning and end smoothing with FS transition. This implies that a of the path are undefined, a property that makes the mirrored version of (18) is needed to describe a curve Fermat spiral segment initially appear unsuitable for of curvature decreasing to zero. The following is pro- path-tracking applications in its current form. How- posed: ever, this is a property of the parametrization (that is, the kinematics and not the geometry of the curve) and p (θ) = FS √ it is possible to remedy the problem by changing vari- x + k θ − θ cos (ρ (θ − θ ) + χ ) ables, as was discussed by Peters(2012). Given that end √ end end end , √ yend + k θend − θ sin (ρ (θ − θend) + χend) the problem is caused by the term θ in the denom- (20) inator and that the Cartesian parametrization of the FS resembles that of a circle, we change variables as T where pend = [xend, yend] is the position at the end follows: of the curve, i.e. pend = pFS (θend), and χend is the √ p course angle at that point. u := θ ⇒ 0 ≤ u ≤ θmax, (23)

189 Modeling, Identiﬁcation and Control

and rewrite (25) as: 0.8

FS 0.7 d θ pFS (u) = u du 0.6 2 p 2 2 sin ρu + χ0 − φ k 1 + (2ρu ) 2 , (29) cos ρu + χ0 − φ 0.5 2 4 0.4 with φ = atan2(1, 4ρ u ). For a regular curve it is N (m) required that: 0.3 d pFS (u) 6= 0. (30) 0.2 du

0.1 For (29) we have that:

0 √ −0.2 0 0.2 0.4 0.6 0.8 d p 2 4 pFS (u) = k 2 1 + 4ρ u , (31) E (m) du Figure 11: Illustration of the two different parametriza- which guarantees that (30) is satisfied. As a conse- tions. The geometry of the curve remains quence, the FS can be used for path-tracking appli- unaffected. cations. In that case, the derivative of the parameter w.r.t time has to be defined. According to Breivik and Fossen(2004a) and (Fossen, 2011, Sec. 10.4), for a and then (18) is written: desired vehicle speed Ud(t), the parameter derivative 2 w.r.t time is determined by: x0 + ku cos ρu + χ0 pFS (u) = 2 , (24) y0 + ku sin ρu + χ0 U (t) u˙(t) = d , (32) q 0 2 0 2 which gives the singularity-free expression for the ve- xp(u) + yp(u) locity: and from (31)–(32): d pFS (u) = U du u˙ = √ √ d , (33) cos ρu2 + χ − 2ρu2 sin ρu2 + χ k 2 1 + 4u4 k 0 0 . (25) sin ρu2 + χ + 2ρu2 cos ρu2 + χ 0 0 where we used that ρ2 = 1. Fig. 11 shows the image of an FS segment when each of the parameters θ and It is necessary to show that (24) results in a regular u is sampled at 10 equispaced points. curve. To this end, we employ the amplitude-phase expressions 3.4 Path Length p a sin (x) + b cos (x) = a2 + b2 sin (x + φ), (26) One, often important, consideration when designing p paths is the length of the path, an example can be a cos (x) + b sin (x) = a2 + b2 sin (x − φ), (27) found in (Jørgensen and Skjetne, 2012). Before mov- φ = atan2(b, a), (28) ing on, it should be noted that we assume no external

d k cos (ρ (θ − θend) + χ0) + 2ρ (θend − θ) sin (ρ (θ − θend) + χ0) pFS (θ) = − √ (34) dθ 2 θend − θ sin (ρ (θ − θend) + χ0) + 2ρ (θend − θ) cos (ρ (θ − θend) + χ0)

d2 p (θ) = dθ2 FS  2  k 4 (θ − θend) + 1 cos (ρ (θ − θend) + χ0) − 4ρ (θend − θ) sin (ρ (θ − θend) + χ0) − 3  2  (35) 2 4 (θend − θ) 4 (θ − θend) + 1 sin (ρ (θ − θend) + χ0) + 4ρ (θend − θ) cos (ρ (θ − θend) + χ0)

190 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral” disturbances acting on the vehicle, therefore the path atan2 (·, ·) length is discussed here from a geometric point of view. arctan(·) ) 180 As mentioned in Sec.1, the Dubins path is the shortest χ θ + arctan(2θ) one between two poses in a plane if we consider a unit 90 speed vehicle with bounded curvature (alternatively, turning radius). Calculating the total length of a Du- 0

bins path is trivial since the expressions for the length Tangent angle ( of straight lines and circular arcs are well known and −90 easy to compute. −180 Despite their complex Cartesian parametrization, 0 π Polar angle (θ) the clothoids have the nice property of a curvature linearly varying w.r.t. the path parameter. As a result, Figure 12: Course tangent angle. Only the expression there is a linear relationship between the curvature and θ + arctan(2θ) is continuous. the clothoid arc length, meaning that the clothoid arc length is easy to compute. Computing the length of an FS arc is not as triv- To conclude, the computation of the length of an ial as the aforementioned cases because (using the FS arc is not as straightforward as that of a circular, parametrization of Sec. 3.3) it is given by the integral: or even clothoid, arc. On the positive side, the FS length is given by a hypergeometric series that always √ Z umax p 4 LFS = k 2 1 + 4u du, (36) converges and is easy to compute. 0 which has no closed-form solution. The path length can be expressed as a Gaussian hypergeometric function: 3.5 Course Angle √ The course, or direction, along the curve can be de- 1 1 5 2 LFS(θ) = k θ 2F1 − , ; ; −4θ , (37) termined by (6). It is well-defined since the singular 2 4 4 terms of (21) cancel. Moreover (Dahl, 2013): see (Weisstein, 2013b). It should be noted that, using sin (θ) + 2θ cos (θ) the parametrization of Sec. 3.3,(37) is equivalent to: χ (θ) = arctan , (40) cos (θ) − 2θ sin (θ) 1 1 5 L (u) = ku F − , ; ; −4u4 . (38) FS 2 1 2 4 4 or, expressed with the two-argument arctangent function, According to (Abramowitz and Stegun, 1970, Ch. 15), the circle of convergence of the Gauss hypergeometric sin (θ) + 2θ cos (θ) , χ (θ) = arctan 2 . (41) series cos (θ) − 2θ sin (θ) ∞ n X (a)n(b)n z F (a, b; c; z) = (39) We rewrite (40) as follows: 2 1 (c) n! n=0 n Z d sin (θ) + 2θ cos (θ) is the unit circle |z| < 1, which corresponds to θ < 1/2, χ(θ) = arctan dθ, or a course change ∆χ < 73.6479◦. The behavior of dθ cos (θ) − 2θ sin (θ) Z 4θ2 + 3 the series (39) on its circle of convergence is absolute = dθ, (42) convergence when Re(c − a − b) > 0, which is true for 4θ2 + 1 (37)–(38) since 5/4 + 1/2 − 1/4 = 3/2. The closer to zero z is, the faster and more accurate the computation where, finally, the equations are continuous and equal of (39) becomes. to: The restriction mentioned above regarding the circle of convergence does not forbid us to compute the χ (θ) = θ + arctan (2θ) (43) length of an FS arc for course changes equal to and higher than 73.6479◦. As it can be seen in (Pearson, up to respectively χ = 90◦ and χ = 180◦. The three 2009, Sec. 4.6), for z ∈ R it is possible to use appro- expressions (40)–(41) and (43) are plotted in Fig. 12 for priate transformation formulae in order to map z to a comparison. In practice, a 180◦ course change is con- new variable w ∈ [0, 1/2], hence resulting in a fast and sidered sufficient for one curve because a full rotation accurate computation for all z, and thus θ. is achieved when combined with a mirrored curve.

191 Modeling, Identiﬁcation and Control

4 Fermat Spiral Path Design 4.3 Scaling Determination 4.1 Problem Description Once the domain of (18) is determined, the scaling constant k must be set such that the curvature constraint In this section we present a methodology for using the κmax is satisﬁed all over the curve. As given by (16), FS in order to construct curvature-continuous paths. the curvature reaches its maximum at: Given a piecewise linear path, the vertices can be s√ smoothened by two Fermat curves, one entering and 7 5 θ = − . (49) one exiting (mirrored) curve. The vertices then be- 2 4 come curvature continuous (G2) and respect a given curvature constraint κmax. In the case where the domain does not include this Without loss of generality, we assume the ini- point, the maximum curvature is at θend. Thus, the tial piecewise linear path consists of two straight parameter corresponding to the maximum curvature is lines connecting three sequential waypoints  s√  (wpti−1, wpti, wpti+1) on the plane, see Fig. 13. 7 5 θ = min θ , − . (50) Initially, the course change direction ρ must be κmax  end 2 4 determined, for this reason it is useful to deﬁne two normalized vectors parallel to the straight lines (see Fig. 14): Solving (16) for k yields that, in order for the maximum curvature along the curve to be the same as the cur- wpt − wpt vature constraint, i.e. κ (θ ) = κ , it is necessary v = i i−1 , (44) κmax max in with wpti − wpti−1 wpt − wpt p 2 i+1 i 1 2 θκ 3 + 4θ vout = . (45) k = max κmax . (51) wpt − wpt 3 i+1 i κmax 1 + 4θ2 2 κmax Then, the course change magnitude can be computed as follows: 4.4 Length and Height Calculations

|∆χ| = arccos (vin · vout) , (46) The projected length l1 of the FS curve along the initial course and the height h perpendicular to it are useful and the course change direction: for determining the initial position p0. Both are found by considering (18) with χ = 0. The length is then ρ = −sign (v v − v v ) . (47) 0 in,y out,x in,x out,y the x-component (see Fig. 15): p 4.2 Domain Determination l1 = k θend cos(θend), (52) The domain of θ in (18) must be set according to the and the height is the y-component (see Fig. 16): desired course change from the beginning to the end of the curve. Similarly, the scaling constant k must be p h = k θend sin(θend). (53) set to respect the curvature constraint κmax. The domain’s upper limit θend determines the course change. Unfortunately, (43) is not invertible, thus it is 4.5 Fermat Smoothing not possible to determine the θend needed to achieve a The initial and ﬁnal course angles, χ0 and χend, can be desired course χ analytically. determined in many ways. For instance, given tangent However, (43) is continuous and diﬀerentiable, even beyond χ = 180◦ and numerical solutions are therefore possible. This problem was investigated in (Dahl, 2013) where it was concluded that Halley’s method (or, equivalently, Householder’s method of order 2), given by:

0 2f(xn)f (xn) xn+1 = xn − 0 2 00 , (48) 2(f (xn)) − f(xn)f (xn) was the most efficient. More specifically, Halley’s method was sufficiently efficient for tolerance equal to Figure 13: Initial path of two straight lines connecting 10−3 after only one step. three sequential waypoints on the plane

192 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral”

vin

vout vout vin ∆ χ

Figure 14: Two normalized vectors parallel to the straight lines are deﬁned α l2

Figure 16: Starting point distance (magniﬁed)

Figure 17: Wheel over and pull out points l1

Figure 15: Starting point distance

Figure 18: Final path

193 Modeling, Identiﬁcation and Control

2D path where h is given by (53) and 30 180◦ − ∆χ α = , (58) 20 2 N (m) 10 Computed path as seen from Fig. 16. There is no danger for the angle Line between wpts α being equal to zero, and hence inducing a singularity Waypoints ◦ 0 at Eq. (57). This would imply a course change of 180 , 0 10 20 30 40 50 60 70 80 90 E (m) which will never occur for two successive waypoints. Figs 17–18 show the wheel over and pull out points, Figure 19: Fermat-smoothened path and the FS arc connecting them, respectively. Figs 19–20 show the Fermat smoothing path and its N Position 30 properties. Similarly to the clothoid, due to the numeric solution of θend, the apparent continuity is not 20 true: a gap exists at the transition between the Fermat (m) 10 curves. Still, any numeric precision can be achieved,

0 consequently making the discontinuity correspondingly small and practically neglectable. Except for this, the E Position 100 path is visibly both tangent and curvature continuous, thus G2. 50 (m) It should be noted that the path curvature is κmax at two points, unless the required course change is 0 ∆χ ≤ 43.83◦1. However, it is more reasonable to stay Course tangential angle (χ) at maximum curvature for as long as possible once 180 reaching it. Such a path is discussed in Sec. 4.6.

0 (deg) 4.6 Dubins Path with Fermat Transition −180 As mentioned earlier, in addition to using FS for con- Curvature (κ ) 0.6 necting straight paths, FS can be used as a transition

0.4 curve between a straight line and a circular arc. In ) 1

− 0.2 this way, the resulting path can be the same as a Du- m ( 0 bins path with the exception of the transitional FS seg-

−0.2 ment. A similar approach where the clothoid was used 0 13 20 27 44 47 51 68 72 76 106 112 118 121 as the transition curve was proposed in (Fraichard and Path length (m) Scheuer, 2004). A Dubins path with FS transition and Figure 20: Fermat smoothing path properties its properties can be seen in Figs 21–22. The curvature plot shows a more realistic and attractive behavior, compared to that of the previous section. Similarly to vectors: the original Dubins path case, this is an interpolating path.

χ0 = atan2 (vin,y, vin,x) , (54) χend = atan2 (vout,y, vout,x) . (55) 5 Fermat Spiral Path Evaluation

Finally, the initial and ﬁnal positions p0 and pend In (Lekkas and Fossen, 2013) and (Dahl, 2013), the must be determined such that the curves meet. Fig. 15 authors developed a number of path evaluation criteria shows that these must be at a distance that can be used for comparing paths depending on the application. Those criteria were: l = l1 + l2 (56) • Smoothness along the linear path from the waypoint. The distance q √ 1When θ > 7 − 5 = 0.27, the curvature κ is reached l1 is given by (52). From the ﬁgure it can be seen that end 2 4 max q √ 7 5 at θ = 2 − 4 both on the entering and exiting curve. q √ h 7 5 l2 = , (57) When θend ≤ 2 − 4 , the curvature κmax is reached only tan (α) at the transition between the curves.

194 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral”

• Length

2D path 30 • Allowance

20 • Tractability

N(m) 10 Computed path Line between wpts • Algorithm Complexity Waypoints 0 0 20 40 60 80 100 E(m) Regarding the FS smoothing path, we conclude the following: Figure 21: Dubins path with Fermat transition Smoothness: The path has a continuous curvature and, in addition, it was shown in Sec. 3.3 that a different parametrization results in parametric continuity as well. Table1 shows how the FS path compares with N Position 30 the other paths considered in this paper, when it comes

20 to geometric continuity. Note that this is true for both (m) 10 interpolating and approximating versions of the path.

E Position Length: The length of an FS arc can be computed by 100 a hypergeometric series which converges for any value 50 of θ, as it was shown in Sec. 3.4. Naturally, a path con- (m) 0 sisting of straight lines and FS segments will be longer −50 than a Dubins path.

Course tangential angle (χ) 180 Allowance: Allowance refers to how much the smoothing arc deviates from the initial piecewise linear 0

(deg) path. This can be useful when the vehicle navigates in an area with obstacles. In this case the allowance is −180 computed as: Curvature (κ ) 0.4

0.2 p ) 1 a = k θend sin (θend). (59) − 0 m ( −0.2

−0.4 Fig. 23 shows the allowance a for diﬀerent course 0 16 19 27 30 51 54 55 58 75 78 79 82 116 118 123 126 131 change magnitudes ∆χ. The comparison includes the Path length (m) FS, the clothoid and the circular arc. It can be ob- Figure 22: Dubins path with Fermat smoothing prop- served that the Fermat smoothing allowance is almost erties the same as that of the clothoid for course changes up to 30◦. Above that value, the FS allowance is smaller than the clothoid’s, and always larger than that of the circle.

6 Tractability: The eﬀect of moving a waypoint is the Fermat smoothing same as for the clothoid, resulting in local inﬂuence, 5 Clothoid smoothing

(m) which means that if one waypoint changes location only

a 4 Circular smoothing the neighboring path segments will be aﬀected. This is 3 a useful property when a vehicle navigates in an area 2 with obstacles and a waypoint has to be relocated dur-

Allowance ing operation, for instance, due to weather conditions. 1

0 0 20 40 60 80 100 120 140 Algorithm Complexity: The process of computing ∆ Course change χ (deg.) an FS is signiﬁcantly less computationally expensive Figure 23: Fermat, clothoid and circular smoothing al- than the clothoid. The exact computational times will lowance comparison vary (depending on the implementation platform), but FS involves only the numerical solution of (43), while the clothoid requires calculating the Fresnel integrals.

195 Modeling, Identiﬁcation and Control

Method G0/C0 G1 G2 wind case. In American Control Conference. IEEE, Baltimore, Maryland, USA, 2010. Piecewise linear Circular arc smoothing Bakolas, E. and Tsiotras, P. Optimal synthesis of the Clothoid smoothing Zermelo–Markov–Dubins problem in a constant drift Fermat smoothing ﬁeld. Journal of Optimization Theory and Appli- cations, 2013. 156(2):469–492. doi:10.1007/s10957- Table 1: Geometric continuity comparison 012-0128-0. Barsky, B. A. and DeRose, T. D. Geometric continuity 6 Conclusions of parametric curves: three equivalent characteriza- tions. IEEE Computer Graphics and Applications, This work has considered the development of a new 1989. 9(6):60–69. doi:10.1109/38.41470. methodology, based on Fermat’s spiral (FS), for gener- Breivik, M. and Fossen, T. I. Path following for marine ating curvature-continuous paths that can be used for surface vessels. In Proceedings of the OTO’04. Kobe, path-following and path-tracking applications for un- Japan, 2004a. doi:10.1109/OCEANS.2004.1406507. deractuated vehicles. The main motivation has been to construct paths of simple geometry (preferably consist- Breivik, M. and Fossen, T. I. Path following of straight ing of straight lines and arc segments) which avoid the lines and circles for marine surface vessels. In Pro- curvature discontinuity of Dubins paths and the com- ceedings of the IFAC Conference on Control Appli- putational intensity of the clothoids. The FS was con- cations in Marine Systems. Ancona, Italy, 2004b. sidered as a good candidate because it has a zero curvature at its origin, hence making it possible to connect it Breivik, M. and Fossen, T. I. Guidance laws for au- smoothly with straight lines. The properties of the FS tonomous underwater vehicles, chapter 4, pages 51– have been explored and an alternative parametrization 76. INTECH Education and Publishing, 2009. has been proposed for guaranteeing parametric speed continuity, an important constraint when it comes to Bruyninckx, H. and Reynaerts, D. Path plan- path-tracking applications. Despite the fact that the ning for mobile and hyper-redundant robots us- parameter end point (depending on the course change ing Pythagorean hodograph curves. In Proceed- between two sequential straight lines) can only be com- ings of the 8th International Conference on Ad- puted numerically, the computational load is much vanced Robotics. IEEE, Monterey, CA, USA, 1997. lighter than that of the clothoid. doi:10.1109/ICAR.1997.620243. Further work includes a generalization to 3D paths Candeloro, M., Lekkas, A. M., Sørensen, A. J., and and the design of parallel FS paths for formation con- Fossen, T. I. Continuous curvature path planning trol of multiple vehicles. using Voronoi diagrams and Fermat’s spirals. In 9th IFAC Conference on Control Applications in Marine Acknowledgments Systems. Osaka, Japan, 2013. Chang, A. J., Brazil, M., Rubinstein, J. H., and This work was supported by the Norwegian Research Thomas, D. A. Curvature-constrained directional- Council through the Centre for Ships and Ocean Struc- cost paths in the plane. Journal of Global Optimiza- tures (CeSOS, project number 146025) as well as the tion, 2012. 53(4):663–681. doi:10.1007/s10898-011- Centre for Autonomous Marine Operations and Sys- 9730-1. tems (AMOS, project number 223254) at NTNU, and through the MAROFF project “D2V” (project number Chitsaz, H. and LaValle, S. M. Time-optimal 210670). paths for a Dubins airplane. In Proceedings of the 46th IEEE Conference on Decision and Control. New Orleans, Louisiana, USA, 2007. References doi:10.1109/CDC.2007.4434966.

Abramowitz, M. and Stegun, I. Handbook of mathe- Dahl, A. R. Path Planning and Guidance for Marine matical functions. Dover Publishing Inc. New York, Surface Vessels. Master’s thesis, Norwegian Univer- 1970. sity of Science and Technology, 2013.

Bakolas, E. and Tsiotras, P. Time-optimal synthesis for Dubins, L. E. On curves of minimal length with a the Zermelo-Markov-Dubins problem: The constant constraint on average curvature, and with prescribed

196 Lekkas et al., “Continuous-Curvature Path Generation Using Fermat’s Spiral”

initial and terminal positions and tangents. Amer- Haugen, J. Guidance Algorithms for Planar Path-based ican Journal of Mathematics, 1957. 79(3):497–516. Motion Control Scenarios. Master’s thesis, Norwe- doi:10.2307/2372560. gian University of Science and Technology, 2010.

Eriksson-Bique, S., Kirkpatrick, D., and Polishchuk, Hota, S. and Ghose, D. Optimal trajectory genera- V. Discrete Dubins paths. arXiv preprint tion for convergence to a rectilinear path. Journal of arXiv:1211.2365, 2012. Intelligent & Robotic Systems, 2013. pages 1–20.

Farin, G. and Sapidis, N. Curvature and the Jørgensen, U. and Skjetne, R. Generating safe and fairness of curves and surfaces. IEEE Com- equally long trajectories for multiple unmanned puter Graphics and Applications, 1989. 9(2):52–57. agents. In 20th Mediterranean Conference on Con- doi:10.1109/38.19051. trol & Automation (MED). IEEE, Barcelona, Spain, 2012. doi:10.1109/MED.2012.6265862. Farouki, R. T. Pythagorean-hodograph quintic transition curves of monotone curvature. Computer-Aided LaValle, S. M. Planning algorithms. Cambridge univer- Design, 1997. 29(9):601–606. doi:10.1016/S0010- sity press, 2006. doi:10.1017/CBO9780511546877. 4485(97)00004-3. Lekkas, A. M. and Fossen, T. I. Line-of-Sight Guid- Farouki, R. T. Pythagorean—hodograph Curves. ance for Path Following of Marine Vehicles, chap- Springer, 2008. doi:10.1007/978-3-540-73398-0. ter 5, in Advanced in Marine Robotics, pages 63– 92. LAP LAMBERT Academic Publishing (O. Gal, Farouki, R. T. and Sakkalis, T. Pythagorean Ed.), 2013. hodographs. IBM Journal of Research and Develop- ment, 1990. 34(5):736–752. doi:10.1147/rd.345.0736. Pearson, J. W. Computation of hypergeometric functions. Master’s thesis, University of Oxford, 2009. Farouki, R. T. and Sakkalis, T. Real rational curves are not ‘unit speed’. Computer Aided Geomet- Peters, J. Changing variables. IEEE Computer ric Design, 1991. 8(2):151–157. doi:10.1016/0167- Graphics and Applications, 2012. 32(3):88–93. 8396(91)90040-I. doi:10.1109/MCG.2012.51.

Fermat, P. Ad locos planos et solidos isagoge. Varia Reeds, J. A. and Shepp, L. A. Optimal paths for a Opera Mathematica, 1679. car that goes both forwards and backwards. Pa- ciﬁc Journal of Mathematics, 1990. 145(2):367–393. Fossen, T. I. Handbook of Marine Craft Hydrodynam- doi:10.2140/pjm.1990.145.367. ics and Motion Control. John Wiley and Sons Ltd., 2011. doi:10.1002/9781119994138. Shanmugavel, M., Tsourdos, A., White, B., and Zbikowski, R. Co-operative path planning of mul- Fossen, T. I. and Lekkas, A. M. Direct and in- tiple UAVs using Dubins paths with clothoid arcs. direct adaptive integral line-of-sight path-following Control Engineering Practice, 2010. 18(9):1084– controllers for marine craft exposed to ocean cur- 1092. doi:10.1016/j.conengprac.2009.02.010. rents. International Journal of Adaptive Control and Signal Processing, 2014. (submitted). Shanmugavel, M., Tsourdos, A., Zbikowski, R., and White, B. Path planning of multiple UAVs with Fraichard, T. and Scheuer, A. From Reeds and clothoid curves in two dimensions. In Proceedings Shepp’s to continuous-curvature paths. IEEE of the 17th IFAC Symposium on Automatic Control Transactions on Robotics, 2004. 20(6):1025–1035. in Aerospace. Toulouse, France, 2007a. doi:10.1109/TRO.2004.833789. Shanmugavel, M., Tsourdos, A., Zbikowski, R., and Gajny, L., Bearee, R., Nyiri, E., and Gibaru, O. Path White, B. A. 3D path planning for multiple UAVs planning with PH G2 splines in R2. In Proceed- using Pythagorean hodograph curves. In AIAA ings of the 1st International Conference on Systems Guidance, Navigation, and Control Conference and and Computer Science. IEEE, Lille, France, 2012. Exhibit, Hilton Head, South Carolina. 2007b. doi:10.1109/IConSCS.2012.6502455. Techy, L. and Woolsey, C. A. Minimum-time path Harary, G. and Tal, A. 3D Euler spirals for 3D planning for unmanned aerial vehicles in steady uni- curve completion. Computational Geometry, 2012. form winds. Journal of Guidance, Control, and Dy- 45(3):115–126. doi:10.1016/j.comgeo.2011.10.001. namics, 2009. 32(6):1736–1746. doi:10.2514/1.44580.

197 Modeling, Identiﬁcation and Control

Toussaint, G. J., Basar, T., and Bullo, F. Motion plan- MathWorld–A Wolfram Web Resource (accessed ning for nonlinear underactuated vehicles using H∞ 03/12/2013). 2013a. URL http://mathworld. techniques. In Proceedings of the American Control wolfram.com/ArchimedeanSpiral.html. Conference. IEEE, Arlington, Virginia, USA, 2001. Weisstein, E. W. “Fermat’s Spiral.” From Tsourdos, A., White, B., and Shanmugavel, M. Co- MathWorld–A Wolfram Web Resource (accessed operative path planning of unmanned aerial vehicles. 19/12/2013). 2013b. URL http://mathworld. Wiley Online Library, 2011. wolfram.com/FermatsSpiral.html. Weisstein, E. W. “Archimedean spiral.” From

198