Content uploaded by Enrico Bertolazzi

Author content

All content in this area was uploaded by Enrico Bertolazzi

Content may be subject to copyright.

Fast and accurate G1ﬁtting of clothoid curves

ENRICO BERTOLAZZI and MARCO FREGO

University of Trento, Italy

A new eﬀective solution to the problem of Hermite G1interpolation with a clothoid curve is

here proposed, that is a clothoid that interpolates two given points in a plane with assigned unit

tangent vectors. The interpolation problem is a system of three nonlinear equations with multiple

solutions which is diﬃcult to solve also numerically. Here the solution of this system is reduced

to the computation of the zeros of one single function in one variable. The location of the zero

associated to the relevant solution is studied analytically: the interval containing the zero where

the solution is proved to exists and to be unique is provided. A simple guess function allows to

ﬁnd that zero with very few iterations in all possible conﬁgurations.

The computation of the clothoid curves and the solution algorithm call for the evaluation of

Fresnel related integrals. Such integrals need asymptotic expansions near critical values to avoid

loss of precision. This is necessary when, for example, the solution of the interpolation problem is

close to a straight line or an arc of circle. A simple algorithm is presented for eﬃcient computation

of the asymptotic expansion.

The reduction of the problem to a single nonlinear function in one variable and the use of

asymptotic expansions make the present solution algorithm fast and robust. In particular a

comparison with algorithms present in literature shows that the present algorithm requires less

iterations. Moreover accuracy is maintained in all possible conﬁgurations while other algorithms

have a loss of accuracy near the transition zones.

Categories and Subject Descriptors: G.1.2 [Numerical Analysis]: Approximation

General Terms: Algorithms, Performance

Additional Key Words and Phrases: Clothoid ﬁtting, Fresnel integrals, Hermite G1interpolation,

Newton–Raphson

1. INTRODUCTION

There are several curves proposed for Computer Aided Design either [Farin 2002;

Baran et al. 2010; De Boor 1978], for trajectories planning of robots and vehicles

and for geometric roads layout [De Cecco et al. 2007; Scheuer and Fraichard 1997].

The most important curves are the clothoids (also known as Euler’s or Cornu’s

spirals), the clothoids splines (a planar curve consisting in clothoid segments), cir-

cles and straight lines,[Davis 1999; Meek and Walton 1992; Meek and Walton 2004;

2009; Walton and Meek 2009], the generalized clothoids or Bezier spirals [Walton

and Meek 1996]. Pythagorean Hodograph [Walton and Meek 2007; Farouki and

Neﬀ 1995], bi-arcs and conic curves are also widely used [Pavlidis 1983]. It is well

known that clothoids are extremely useful despite their transcendental form.

The procedure that allows a curve to interpolate two given points in a plane

with assigned unit tangent vectors is called G1Hermite interpolation, while if the

curvatures are given at the two points, then this is called G2Hermite interpolation

[McCrae and Singh 2009]. A single clothoid segment is not enough to ensure G2

Hermite interpolation, because of the insuﬃcient degrees of freedom. For example

the interpolation problem is solved by using composite clothoid segments in [Shin

and Singh 1990] and using cubic spirals in [Kanayama and Hartman 1989]. However,

arXiv:1305.6644v2 [math.NA] 5 Jun 2013

2·E.Bertolazzi and M.Frego

in some applications is enough the cost-eﬀectiveness of a G1Hermite interpolation

[Walton and Meek 2009; Bertolazzi et al. 2006] especially when the discontinuity

of the curvature is acceptable.

The purpose of this paper is to describe a new method for G1Hermite interpolation

with a single clothoid segment; this method, which does not require to split the

problem in mutually exclusive cases (as in Meek and Walton 2009), is fully eﬀective

even in case of Hermite data like straight lines or circles (see Figure 1 - right). These

are of course limiting cases but are naturally treated in the present approach. In

previous works the limiting cases are treated separately introducing thresholds.

The decomposition in mutually exclusive states is just a geometrical fact that helps

to understand the problem but introduces instabilities and inaccuracies which are

absent in the present formulation, as we show in the section of numerical tests.

Finally, the problem of the G1Hermite interpolation is reduced to the compu-

tation of a zero of a unique nonlinear equation. The Newton–Rasphson iterative

algorithm is used to accurately compute the zero and a good initial guess is also

derived so that few iterations (less than four) suﬃce.

The article is structured as follows. In section 2 we deﬁne the interpolation

problem, in section 3 we describe the passages to reformulate it such that from three

equations in three unknowns it reduces to one nonlinear equation in one unknown.

Section 4 describes the theoretical aspects of the present algorithm. Here it is

proved the existence of the solution and how to select a valid solution among the

inﬁnite possibilities. A bounded range where this solution exists and is unique is

provided. Section 5 is devoted to the discussion of a good starting point for the

Newton–Raphson method, so that using that guess, quick convergence is achieved.

Section 6 introduces the Fresnel related integrals, e.g. the momenta of the Fresnel

integrals. Section 6 analyses the stability of the computation of the clothoid and

ad hoc expressions for critical cases are provided. Section 7 is devoted to numerical

tests and comparisons with other methods present in literature. In the Appendix a

pseudo-code complete the presented algorithm for the accurate computation of the

Fresnel related integrals.

2. THE FITTING PROBLEM

Consider the curve which satisﬁes the following diﬀerential equations:

x0(s) = cos ϑ(s), x(0) = x0,

y0(s) = sin ϑ(s), y(0) = y0,

ϑ0(s) = K(s), ϑ(0) = ϑ0,

(1)

where sis the arc parameter of the curve, ϑ(s) is the direction of the tangent

(x0(s), y0(s)) and K(s) is the curvature at the point (x(s), y(s)). When K(s):=

κ0s+κ, i.e. when the curvature changes linearly, the curve is called Clothoid. As

a special case, when κ0= 0 the curve has constant curvature, i.e. is a circle and

when both κ=κ0= 0 the curve is a straight line. The solution of ODE (1) is given

in the next deﬁnition:

Definition 2.1 Clothoid curve. The general parametric form of a clothoid

Fast and accurate clothoid ﬁtting ·3

spiral curve is the following

x(s) = x0+Zs

0

cos 1

2κ0τ2+κτ +ϑ0dτ,

y(s) = y0+Zs

0

sin 1

2κ0τ2+κτ +ϑ0dτ.

(2)

Notice that 1

2κ0s2+κs+ϑ0and κ0s+κare, respectively, the angle and the curvature

at the abscissa s.

The computation of the integrals (2) is here recast as a combination (discussed

in Section 6) of Fresnel sine S(t) and cosine C(t) functions. Among the various

possible deﬁnitions, we choose the following one, Abramowitz and Stegun 1964.

Definition 2.2 Fresnel integral functions.

C(t) = Zt

0

cos π

2τ2dτ, S(t) = Zt

0

sin π

2τ2dτ. (3)

Remark 2.3. The literature reports diﬀerent deﬁnitions, such as:

e

C(t) = Zt

0

cos(τ2) dτ, e

S(t) = Zt

0

sin(τ2) dτ,

or

b

C(θ) = 1

√2πZθ

0

cos u

√udu, b

S(θ) = 1

√2πZθ

0

sin u

√udu. (4)

The identities (5) allow to switch among these deﬁnitions:

C(t) = Z√2

√πt

0

cos τ2dτ=sign(t)

√2πZπ

2t2

0

cos u

√udu

S(t) = Z√2

√πt

0

sin τ2dτ=sign(t)

√2πZπ

2t2

0

sin u

√udu.

(5)

Thus, the problem considered in this paper is stated next.

Problem 1 Clothoid Hermite interpolation. Given two points (x0, y0)and

(x1, y1)and two angles ϑ0and ϑ1, ﬁnd a clothoid segment of the form (2) which

satisﬁes:

x(0) = x0, y(0) = y0,(x0(0) , y0(0)) = (cos ϑ0,sin ϑ0),

x(L) = x1, y(L) = y1,(x0(L), y0(L)) = (cos ϑ1,sin ϑ1),

where L > 0is the length of the curve segment.

The general scheme is showed in Figure 1 - left.

Remark 2.4. Notice that Problem 1 admits an inﬁnite number of solutions. In

fact, given ϑ(s), the angle of a clothoid which solves Problem 1, satisﬁes ϑ(0) =

ϑ0+ 2kπ and ϑ(L) = ϑ1+ 2`π with k, ` ∈Z: diﬀerent values of kcorrespond

to diﬀerent interpolant curves that loop around the initial and the ﬁnal point.

Figure 1- right shows possible solutions derived from the same Hermite data.

The solution of Problem 1 is a zero of the following nonlinear system involving the

unknowns L,κ,κ0:

4·E.Bertolazzi and M.Frego

y

x

(x0, y0)

(x1, y1)

ϕ

φ0

φ1

ϑ0

ϑ1

y

x

ϕ

φ0

φ1

Fig. 1. Left: G1Hermite interpolation schema and notation. Right: some possible solutions.

F(L, κ, κ0) =

x1−x0−RL

0cos 1

2κ0s2+κs +ϑ0ds

y1−y0−RL

0sin 1

2κ0s2+κs +ϑ0ds

ϑ1−1

2κ0L2+κL +ϑ0

.(6)

In Section 3 the nonlinear system (6) is reduced to a single non linear equation that

allows for the eﬃcient solution Problem 1.

3. RECASTING THE INTERPOLATION PROBLEM

The nonlinear system (6) is recasted in an equivalent form by introducing the

parametrization s=τL so that the involved integrals have extrema which do not

dependent on L:

FL, B

L,2A

L2=

∆x−LR1

0cos Aτ2+Bτ +ϑ0dτ

∆y−LR1

0sin Aτ2+Bτ +ϑ0dτ

ϑ1−(A+B+ϑ0)

,(7)

where A=1

2κ0L2,B=L κ, ∆x=x1−x0, ∆y=y1−y0. It is convenient to

introduce the following functions whose properties are studied in Section 6:

X(a, b, c) = Z1

0

cos a

2τ2+bτ +cdτ,

Y(a, b, c) = Z1

0

sin a

2τ2+bτ +cdτ.

(8)

Combining (8) with (7) becomes:

FL, B

L,2A

L2=

∆x−L·X(2A, B, ϑ0)

∆y−L·Y(2A, B, ϑ0)

ϑ1−(A+B+ϑ0)

.(9)

The third equation in (7) is linear so that solve it with respect to B,

B=δ−A, δ =ϑ1−ϑ0=φ1−φ0,(10)

Fast and accurate clothoid ﬁtting ·5

and the solution of the nonlinear system (9) is reduced to the solution of the non-

linear system of two equations in two unknowns, namely Land A:

G(L, A) = ∆x−L·X(2A, δ −A, ϑ0)

∆y−L·Y(2A, δ −A, ϑ0),(11)

followed by the computation of Bby (10). We can perform one further simpliﬁcation

using polar coordinates to represent (∆x, ∆y), namely

∆x=rcos ϕ,

∆y=rsin ϕ. (12)

From (12) and L > 0 we deﬁne two nonlinear functions f(L, A) and g(A), where

g(A) does not depend on L, as follows:

f(L, A) = G(L, A)·cos ϕ

sin ϕ, g(A) = 1

LG(L, A)·sin ϕ

−cos ϕ.

Taking advantage of the trigonometric identities

sin(α−β) = sin αcos β−cos αsin β,

cos(α−β) = cos αcos β+ sin αsin β, (13)

the functions g(A) and f(L, A) are simpliﬁed in:

g(A) = Y(2A, δ −A, φ0),

f(L, A) = p∆x2+ ∆y2−L X(2A, δ −A, φ0),(14)

where φ0=ϑ0−ϕ.

Supposing to ﬁnd Asuch that g(A) = 0, then from f(L, A) = 0 we compute Land

Busing equations (14) and (10) respectively. This yields

L=p∆x2+ ∆y2

X(2A, δ −A, φ0), B =δ−A.

Thus, the solutions of the nonlinear system (9) are known if the solutions of the

single nonlinear function g(A) of equation (14) are determined. The solution of

Problem 1 is recapitulated in the following steps:

1. Solve g(A) = 0 where g(A):=Y(2A, δ −A, φ0);

2. Compute L=p∆x2+ ∆y2/h(A) where h(A):=X(2A, δ −A, φ0);

3. Compute κ= (δ−A)/L and κ0= 2A/L2.

This algorithm prompts the following issues.

•How to compute the roots of g(A) and to select the one which appropriately

solves Problem 1.

•Check that the length Lis well deﬁned and positive.

These issues are discussed in Section 4. Unlike the work of Meek and Walton 2009,

this method does not require to split the problem in mutually exclusive cases, i.e.,

straight lines and circles are treated naturally.

6·E.Bertolazzi and M.Frego

4. THEORETICAL DEVELOPMENT

In this section the existence and selection of the appropriate solution are discussed

in detail. The computation of Lrequires only to verify that for A?such that

g(A?) = 0 then h(A?) = X(2A?, δ −A?, φ0)6= 0. This does not ensure that the

computed Lis positive; but positivity is obtained by an appropriate choice of A?.

4.1 Symmetries of the roots of g(A)

The general analysis of the zeros of g(A) requires the angles φ0and φ1to be in the

range (−π, π). It is possible to restrict the domain of search stating the following

auxiliary problems:

The reversed problem. The clothoid joining (x1, y1) to (x0, y0) with angles ϑR

0=

−ϑ1and ϑR

1=−ϑ0is a curve with support a clothoid that solves Problem 1 but

running in the opposite direction (with the same length L). Let δR=ϑR

1−ϑR

0=

−ϑ0+ϑ1=δ, it follows that gR(A):=Y(2A, δ −A, −φ1) is the function whose

zeros give the solution of the reversed interpolation problem.

The mirrored problem. The curve obtained connecting (x0, y0) to (x1, y1) with

angle ϑM

0=ϕ−φ0and ϑM

1=ϕ−φ1is a curve with support a curve solving

the same problem but mirrored along the line connecting the points (x0, y0) and

(x1, y1) (with the same length L). Let δM=ϑM

1−ϑM

0=−φ1+φ0=−δ, it follows

that gM(A):=Y(2A, −δ−A, −φ0) is the function whose zeros are the solution of

the mirrored interpolation problem.

Lemma (4.1) shows that it is possible to reduce the search of the roots in the

domain |φ0|< φ1≤π. The special cases φ0±φ1= 0 are considered separately.

Lemma 4.1. Let g(A):=Y(2A, δ −A, φ0)and h(A):=X(2A, δ −A, φ0)with

gR(A):=Y(2A, δ −A, −φ1), gM(A):=Y(2A, −δ−A, −φ0),

hR(A):=X(2A, δ −A, −φ1), hM(A):=X(2A, −δ−A, −φ0),

then

g(A) = −gR(−A), g(A) = −gM(−A), h(A) = hR(−A) = hM(−A).

Thus, g(A)has the same roots of gR(A),gM(A)with opposite sign.

Proof.

g(A) = Y(2A, δ −A, φ0) = Z1

0

sin(Aτ(τ−1) + δτ +φ0) dτ,

=−Z1

0

sin(−Aτ(τ−1) −δτ −φ0) dτ,

=−Z1

0

sin(A(1 −z)z−δ(1 −z)−φ0) dz, [z= 1 −τ]

=−Z1

0

sin(−Az(z−1) + δz −φ1) dz,

=−Y(−2A, δ +A, −φ1),

Fast and accurate clothoid ﬁtting ·7

we have g(A) = −gR(−A).

Using again g(A) with the following manipulation:

g(−A) = Y(−2A, δ +A, φ0) = Z1

0

sin(−Aτ2+ (δ+A)τ+φ0) dτ,

=−Z1

0

sin(Aτ2+ (−δ−A)τ−φ0) dτ,

=−Y(2A, −δ−A, −φ0) = −gM(A),

we have g(A) = −gM(−A). The thesis for h(A) is obtained in the same way.

Figure 2 shows the domain |φ0|< φ1≤πwith the mirrored and reversed problem.

Reﬂecting and mirroring allows to assume the constraints for the angles described

in Assumption (4.2).

Assumption 4.2 Angle domains. The angles φ0and φ1satisfy the restric-

tion: |φ0| ≤ φ1≤πwith ambiguous cases |φ0|=φ1=πexcluded (see Figure 2).

This ordering ensures that the curvature of the ﬁtting curve is increasing, i.e. κ0>0.

Notice that if Ais the solution of nonlinear system (14) then κ0= 2A/L2, i.e. the

sign of Ais the sign of κ0and thus Amust be positive. Finally, δ=φ1−φ0>0.

This assumption is not a limitation because any interpolation problem can be refor-

mulated as a problem satisfying Assumption 4.2 while the analysis for the special

case φ0+φ1= 0 and φ0−φ1= 0 are performed apart.

Lemma 4.3. The (continuous) functions g(A):=Y(2A, δ −A, φ0)and h(A):=

X(2A, δ −A, φ0)for A > 0, when φ0and φ1satisfy assumption 4.2, can be written

as

g(A) = √2π

√A

p(δ−A)2

4A0< A ≤δ,

q(δ−A)2

4AA≥δ;

h(A) = √2π

√A

p(δ−A)2

4A0< A ≤δ,

q(δ−A)2

4AA≥δ,

φ1

φ0

O

φ0=φ1

φ0+φ1= 0

π

π

φ1>|φ0|

φ1

φ0

O

π

π

reversed

mirrored

mirrored

and reversed

Fig. 2. Left: the domain φ1>|φ0|with the special cases φ1=φ0and φ1+φ0= 0. Right: the

domain mirrored and reversed.

8·E.Bertolazzi and M.Frego

where

p(θ) = Zθ+δ

θ

sin(u+φ0−θ)

√udu, q(θ) = p(θ)+2Zθ

0

sin(u+φ0−θ)

√udu,

p(θ) = Zθ+δ

θ

cos(u+φ0−θ)

√udu, q(θ) = p(θ)+2Zθ

0

cos(u+φ0−θ)

√udu.

(15)

Proof. With standard trigonometric passages and A > 0 we deduce the follow-

ing expression for g(A) and h(A):

√A g(A) = √2πh(C(ω+)− C(ω−)) sin η+ (S(ω+)− S(ω−)) cos ηi,

√A h(A) = √2πh(C(ω+)− C(ω−)) cos η−(S(ω+)− S(ω−)) sin ηi,

where

ω−=δ−A

√2πA , ω+=δ+A

√2πA , η =φ0−(δ−A)2

4A.

Combining equivalence (5) and the parity properties of sin xand cos x,g(A) and

h(A) take the form:

√A g(A)=∆b

Csin (φ0−θ)+∆b

Scos (φ0−θ),

√A h(A)=∆b

Ccos (φ0−θ)−∆b

Ssin (φ0−θ),(16)

where

∆b

C=b

C(θ+δ)−σ−b

C(θ),∆b

S=b

S(θ+δ)−σ−b

S(θ),

and b

C,b

Sare deﬁned in (4); moreover

θ=(δ−A)2

4A, θ +δ=(δ+A)2

4A, σ−= sign(δ−A).

By using identities (13) equation (16) becomes:

ˆg(θ) = √A

√2πg(A) = Zθ+δ

0

sin(u+φ0−θ)

√udu−σ−Zθ

0

sin(u+φ0−θ)

√udu,

ˆ

h(θ) = √A

√2πh(A) = Zθ+δ

0

cos(u+φ0−θ)

√udu−σ−Zθ

0

cos(u+φ0−θ)

√udu.

It is recalled that Amust be positive, so that when Ato 0 < A < δ then σ−= 1,

otherwise, when A>δthen σ−=−1. In case A=δthen θ= 0 and the second

integral is 0 and thus g(δ) = p(0) = q(0) and h(δ) = p(0) = q(0).

4.2 Localization of the roots of g(A)

The problem g(A) = 0 has in general inﬁnite solutions. The next Theorems show

the existence of a unique solution in a prescribed range, they are in part new and in

part taken from [Walton and Meek 2009] here reported without proofs and notation

slightly changed to better match the notation. By appropriate transformations we

use these Theorems to select the suitable solution and ﬁnd the interval where the

solution is unique. The Theorems characterize the zeros of the functions (15) ﬁnding

intervals where the solution exists and is unique.

Fast and accurate clothoid ﬁtting ·9

Theorem 4.4 Meek-Walton th.2. Let 0<−φ0< φ1< π. If p(0) >0then

p(θ)=0has no root for θ≥0. If p(0) ≤0then p(θ)=0has exactly one root for

θ≥0. Moreover, the root occurs in the interval [0, θ?]where

θ?=λ2

1−λ2(φ1−φ0)>0 0 < λ =1−cos φ0

1−cos φ1

<1.(17)

Theorem 4.5 Meek-Walton th.3. Let −π < −φ1< φ0<0and q(0) >0

then q(θ)=0has exactly one root in the interval [0, π/2 + φ0]. If q(0) <0then

q(θ)=0has no roots in the interval [0, π/2 + φ0].

Theorem 4.6 Meek-walton th.4. Let φ0∈[0, π)and φ1∈(0, π], then q(θ) =

0has exactly one root in [0, π/2 + φ0], moreover, the root occurs in [φ0, π/2 + φ0].

The following additional Lemmata are necessary to complete the list of properties

of p(θ) and q(θ):

Lemma 4.7. Let p(θ)and q(θ)as deﬁned in equation (15), then

(a)if 0≤φ0≤φ1≤πthen

◦if φ1> φ0then p(θ)>0for all θ≥0otherwise p(θ)=0 for all θ≥0;

◦q(θ)=0for θ∈[φ0, π/2+φ0]and the root is unique in the interval [0, π/2+φ0];

(b)if −π≤ −φ1< φ0<0

◦if p(0) = q(0) ≤0then

•p(θ) = 0 has a unique root θthat satisﬁes 0≤θ≤θ0with θ0deﬁned in (17).

•q(θ)=0has no roots in the interval [0, π/2 + φ0];

◦if p(0) = q(0) >0then

•p(θ)>0for all θ≥0;

•q(θ)=0has a unique root in the interval [0, π/2 + φ0]

(c)if φ0≤ −π/2then p(0) = q(0) <0.

Proof. A straightforward application of Theorems 4.4, 4.5 and 4.6. For point

(c), from (16):

p(0) = q(0) = √δ g(δ) = ∆ b

Csin φ0+ ∆ b

Scos φ0,

in addiction, since −π≤φ0≤ −π/2, b oth sin φ0≤0 and cos φ0≤0 resulting in

p(0) = q(0) <0.

The combination of Lemma 4.1 together with reversed and mirrored problems,

proves that the interpolation problem satisﬁes Assumption 4.2.

With this Assumption and Lemma 4.3 prove a Theorem that states the existence

and uniqueness in a speciﬁed range. We show the special case of φ0+φ1= 0 in a

separated Lemma, the case of φ0−φ1= 0 follows from the application of Theorem

4.6 for positive angles, because Assumption 4.2 forces φ1≥0 and excludes the case

of equal negative angles.

Lemma 4.8. Let φ0+φ1= 0 and φ0∈(−π, π), then g(A) = 0 has the unique

solution A= 0 in the interval (−2π, 2π).

10 ·E.Bertolazzi and M.Frego

Proof. For φ0+φ1= 0 we have δ=−2φ0and

g(A) = Y(2A, −2φ0−A, φ0),

=Z1

0

sin(Aτ(τ−1) + φ0(1 −2τ)) dτ,

=Z1

−1

sin(A(z2−1)/4−zφ0)dz

2,[τ= (z+ 1)/2]

=Z1

−1

sin(A(z2−1)/4) cos(zφ0)dz

2−Z1

−1

cos(A(z2−1)/4) sin(zφ0)dz

2.

Using properties of odd and even functions the rightmost integral of the previous

line vanishes yielding

g(A) = Z1

0

sin(A(z2−1)/4) cos(zφ0) dz.

From this last equality, if A= 0 then g(A) = 0. If 0 <|A|<4π, the sign of the

quantity sin(A(z2−1)/4) is constant; if |φ0|< π/2, then cos(zφ0)>0 and thus

g(A) has no roots. For the remaining values of φ0, i.e. π/2≤ |φ0|< π, we have:

Zπ/(2|φ0|)

0

cos(zφ0) dz=1

|φ0|,Z1

π/(2|φ0|)|cos(zφ0)|dz=1−sin |φ0|

|φ0|<1

|φ0|.

If in addition, 0 <|A|<2πthen sin(A(z2−1)/4)is positive and monotone

decreasing so that:

Zπ/(2|φ0|)

0

sin A

4(z2−1)cos(zφ0) dz≥C

|φ0|,

Z1

π/(2|φ0|)

sin A

4(z2−1)cos(zφ0) dz<C

|φ0|,

where

C=sin A

16 |φ0|2π2−4|φ0|2>0,

and thus g(A)6= 0 for 0 <|A|<2πand |φ0|< π.

We state now the main Theorem of this study.

Theorem 4.9 Existence and uniqueness of solution for system (14).

The function g(A), when angles φ0and φ1satisfy assumption 4.2, admits a unique

solution for A∈(0, Amax], where

Amax =δ+ 2θmax 1 + p1 + δ/θmax , θmax = max n0, π/2 + φ0o.

Moreover h(A)>0where g(A) = 0.

Proof. The special cases φ0+φ1= 0 and φ0=φ1were previously considered

and in Lemma 4.8. The other cases are discussed next. From Lemma 4.7 it follows

that the two equations

p(θ)=0, θ ≥0,

q(θ)=0, θ ∈[0, θmax],

Fast and accurate clothoid ﬁtting ·11

g(θ), h(θ)

θ

θ?= 14.75θ= 7.00

h(θ)>0

g(θ)=0

z

φ1=π

φ0=−3π

4

φ0+π

φ0+ 2π

Fig. 3. Left: functions gand h, when gvanishes, his strictly positive. Right: the plot of

cos z−wsin z. In both ﬁgures φ0=−(3/4)πand φ1=π.

cannot be satisﬁed by the same θin the speciﬁed range, so that they are mutually

exclusive although one of the two is satisﬁed. Thus g(A) = 0 has a unique solution.

To ﬁnd the equivalent range of Awe select the correct solution of (δ−A)2= 4Aθmax.

The two roots are:

A1= 2θmax +δ−2pθ2

max +θmaxδ=δ2

2θmax +δ+ 2pθ2

max +θmaxδ≤δ

A2= 2θmax +δ+ 2pθ2

max +θmaxδ≥δ,

and thus A2is used to compute Amax. We are now interested to check if h(A)>0

when g(A) = 0. According to Lemma 4.3,

h(A) =

p(δ−A)2

4A=Zφ1

φ0

cos z

√z+θ−φ0

dz, 0< A ≤δ,

q(δ−A)2

4A, A ≥δ.

Suppose we set the root θof g(by doing so, we set also the value of A), we have to

show that if A≤δthen p(θ)>0 and if A≥θthen q(θ)>0.

We begin analysing A≤δfor |φ0|< φ1≤π

2. In this case we have that the cosine

in the numerator is always positive, and so is the square root at the denominator,

thus the integral p(θ) is strictly positive. When −π < φ0<−π

2then φ1≥π

2, we

can write, for all w∈R,

Zφ1

φ0

cos z

√z+θ−φ0

dz=Zφ1

φ0

cos z−wsin z

√z+θ−φ0

dz. (18)

In particular we can choose w=cos φ0

sin φ0>0 positive (which, incidentally is always

positive because −π < φ0<−π

2) so that the integrand function vanishes for the

three values z=φ0,φ0+π,φ0+ 2π. Moreover cos z−wsin zis strictly positive

for z∈(φ0, φ0+π), see Figure 3. Thus we can bound integral (18) as

Zφ1

φ0

cos z−wsin z

√z+θ−φ0

dz > Zφ0+2π

φ0

cos z−wsin z

√z+θ−φ0

dz,

≥Rφ0+2π

φ0cos z−wsin zdz

p(φ0+π) + θ−φ0

= 0.

12 ·E.Bertolazzi and M.Frego

The last subcase of A≤δoccurs when both φ0, φ1>π

2, but it is not necessary

because Lemma 4.7 at point (a) states that p(θ)>0 does not vanish. We discuss

now the second case, when A≥δ, it is recalled that from (16)

h(A) = ∆ b

Ccos (φ0−θ)−∆b

Ssin (φ0−θ),

with ∆ b

C,∆b

S>0 and for θ∈[0,π

2+φ0] we can write −π

2≤φ0−θ≤0, thus

the cosine is positive and the sine is negative, hence the whole quantity is strictly

positive.

Corollary 4.10. All the solutions of the nonlinear system (11) are given by

L=p∆x2+ ∆y2

X(2A, δ −A, φ0), κ =δ−A

L, κ0=2A

L2,

where Ais any root of g(A):=Y(2A, δ −A, φ0)provided that the corresponding

h(A):=X(2A, δ −A, φ0)>0.

Corollary 4.11. If the angles φ0and φ1are in the range [−π, π], with the

exclusion of the points φ0=−φ1=±π, the solution exists and is unique for

−Amax ≤A≤Amax where

Amax =|φ1−φ0|+ 2θmax 1 + p1 + |φ1−φ0|/θmax,

θmax = max n0, π/2 + sign(φ1)φ0o.

Notice that the solution of the nonlinear system (11) is reduced to the solution of

g(A) = 0. Hence the interpolation problem is reduced to a single nonlinear equation

that can be solved numerically with Newton–Raphson Method; also, Corollary 4.11

ensures that L > 0 for Ain the speciﬁed range. It can also be proved that in general

for all Asuch that g(A) = 0 then h(A)6= 0. In the next section, a technique to

select a meaningful initial point is described.

5. COMPUTING THE INITIAL GUESS FOR ITERATIVE SOLUTION

The zeros of function g(A) are used to solve the interpolation problem and are

approximated by the Newton-Raphson scheme. This algorithm needs “a guess

point” to converge to the appropriate solution. Notice that there is an inﬁnite

number of solutions of Problem 1 and we need criteria for the selection of a solution.

By Theorem 4.9 we reduce the problem to standard angles and search the unique

solution in the appropriate range.

Denote with A(φ0, φ1) the selected zero of g(A):=Y(2A, δ −A, φ0) as a function

of φ0and φ1. Figure 4 shows that A(φ0, φ1) is approximated by a plane. A simple

approximation of A(φ0, φ1) is obtained by sinx≈xin Y(2A, δ −A, φ0) and thus,

g(A) = Y(2A, δ −A, φ0)≈Z1

0

Aτ2+ (δ−A)τ+φ0dτ=φ0+φ1

2−A

6,

and solving for A

A(φ0, φ1)≈3(φ0+φ1),(19)

This approximation is a ﬁne initial point for Newton-Raphson, however better ap-

proximation for A(φ0, φ1) are obtained by least squares method. Invoking reﬂection

Fast and accurate clothoid ﬁtting ·13

and mirroring properties, the functional form of the approximation is simpliﬁed and

results in the two following expressions for A(φ0, φ1):

A(φ0, φ1)≈(φ0+φ1)c1+c2φ0φ1+c3(φ2

0+φ2

1),(20a)

A(φ0, φ1)≈(φ0+φ1)d1+φ0φ1(d2+d3φ0φ1) +

(φ2

0+φ2

1)(d4+d5φ0φ1) + d6(φ4

0+φ4

1),(20b)

where φ0=φ0/π,φ1=φ1/π and

1 2 3 4 5 6

c3.070645 0.947923 -0.673029

d2.989696 0.71622 -0.458969 -0.502821 0.26106 -0.045854

Using (19), (20a) or (20b) as the starting point for Newton-Raphson, the solution

for Problem 1 is found in very few iterations. Computing the solution with Newton-

Raphson starting with the proposed guesses in a 1024 ×1024 grid for φ0and φ1

ranging in [−0.9999π, 0.9999π] with a tolerance of 10−10, results in the following

distribution of iterations:

#iter 1 2 3 4 5

#instances 1025 6882 238424 662268 142026 Guess (19)

0.1% 0.7% 22.7% 63.0% 13.5%

#instances 1025 10710 702534 336356 Guess (20a)

0.1% 1.0% 66.9% 32.0%

#instances 1025 34124 1015074 402 Guess (20b)

0.1% 3.2% 96.6% 0.1%

The complete algorithm for the clothoid computation is written in the function

buildClothoid of Table I. This function solves equation (14) and builds the coef-

ﬁcients of the interpolating clothoid.

−3 −2 −1 0123

−3

−2

−1

0

1

2

3

−40

−20

0

20

40

−3 −2 −1 0123

−3

−2

−1

0

1

2

3

0

1

2

3

4

Fig. 4. Left: the function A(φ0, φ1). Notice that A(φ0, φ1) is approximately a plane. Right:

values of the length Lof the computed clothoid as a function of φ0and φ1. Notice that when

angles satisfy φ0=π, φ1=−πor φ0=−π, φ1=πthe length goes to inﬁnity. The angles range

in [−π, π].

14 ·E.Bertolazzi and M.Frego

Table I. The ﬁtting algorithm

Function normalizeAngle(φ)

1while φ > +πdo φ←φ−2π;

2while φ < −πdo φ←φ+ 2π;

3return φ;

Function buildClothoid(x0,y0,ϑ0,x1,y1,ϑ1,)

1∆x←x1−x0; ∆y←y1−y0;

2Compute rand ϕfrom rcos ϕ= ∆xand rsin ϕ= ∆y;

3φ0←normalizeAngle(ϑ0−ϕ); φ1←normalizeAngle(ϑ1−ϕ);

4Deﬁne gas g(A) := Y(2A, (φ1−φ0)−A, φ0);

5Set A←3(φ1+φ0); // In alternative use (20a) or (20b)

6while |g(A)|> do A←A−g(A)/g0(A) ;

7L←r/X(2A, δ −A, φ0); κ←(δ−A)/L;κ0←(2 A)/L2;

8return κ,κ0

,L

The algorithm is extremely compact and was successfully tested in any possible

situation. The accurate computation of the clothoid needs an equally accurate

computation of g(A) and g0(A) and thus the accurate computation of Fresnel re-

lated functions X(a, b, c) and Y(a, b, c) with associated derivatives. These functions

are a combination of Fresnel and Fresnel momenta integrals which are precise for

large aand small momenta. For the computation, only the ﬁrst two momenta are

necessary so that the inaccuracy for large momenta does not pose any problem. A

diﬀerent problem is the computation of these integrals for small values of |a|. In

this case, laborious expansion are used to compute the integrals with high accuracy.

The expansions are discussed in section 6.

6. ACCURATE COMPUTATION OF FRESNEL MOMENTA

Here it is assumed that the standard Fresnel integrals can be computed with high

accuracy. For this task one can use algorithms described in Snyder 1993, Smith

2011 and Thompson 1997 or using continued fraction expansion as in Backeljauw

and Cuyt 2009 or simply use the available software [Press et al. 2002]. It is possible

to reduce the integrals (2) to a linear combination of this standard Fresnel integrals

(3). However simpler expressions are obtained using also the momenta of the Fresnel

integrals:

Ck(t) = Zt

0

τkcos π

2τ2dτ, Sk(t) = Zt

0

τksin π

2τ2dτ. (21)

Notice that C(t):=C0(t) and S(t):=S0(t). Closed forms via the exponential

integral or the Gamma function are also possible, however we prefer to express

them as a recurrence. Integrating by parts, the following recurrence is obtained:

Ck+1(t) = 1

πtksin π

2t2−kSk−1(t),

Sk+1(t) = 1

πkCk−1(t)−tkcos π

2t2.

(22)

Fast and accurate clothoid ﬁtting ·15

Recurrence is started by computing standard Fresnel integrals (3) and (changing

z=τ2) the following values are obtained:

C1(t) = 1

πsin π

2t2,S1(t) = 1

π1−cos π

2t2.

Also C2(t) and S2(t) are readily obtained:

C2(t) = 1

πtsin π

2t2− S0(t),S2(t) = 1

πC0(t)−tcos π

2t2.

Notice that from recurrence (22) it follows that Ck(t) and Sk(t) with kodd do not

contain Fresnel integrals (3) and are combination of elementary functions.

The computation of clothoids relies most on the evaluation of integrals of kind

(8) with their derivatives. The reduction is possible via a change of variable and

integration by parts. It is suﬃcient to consider two integrals, that cover all possible

cases:

Xk(a, b, c) = Z1

0

τkcos a

2τ2+bτ +cdτ,

Yk(a, b, c) = Z1

0

τksin a

2τ2+bτ +cdτ,

(23)

so that

g(A) = Y0(2A, δ −A, φ0),

g0(A) = X1(2A, δ −A, φ0)−X2(2A, δ −A, φ0),

and ﬁnally, equation (2) can be evaluated as

x(s) = x0+s X0(κ0s2, κs, ϑ0),

y(s) = y0+s Y0(κ0s2, κs, ϑ0).

From the trigonometric identities (13), integrals (23) are rewritten as

Xk(a, b, c) = Xk(a, b, 0) cos c−Yk(a, b, 0) sin c,

Yk(a, b, c) = Xk(a, b, 0) sin c+Yk(a, b, 0) cos c.

Deﬁning Xk(a, b):=Xk(a, b, 0) and Yk(a, b):=Yk(a, b, 0) the computation of (23)

is reduced to the computation of Xk(a, b) and Yk(a, b). It is convenient to introduce

the following quantities

σ= sign(a), z =ω+−ω−=σp|a|

√π, ω−=b

pπ|a|, η =−b2

2a,

so that it is possible to rewrite the argument of the trigonometric functions of

Xk(a, b) and Yk(a, b) as

a

2τ2+bτ =π

2σ τσp|a|

√π+b

pπ|a|!2

−b2

2a=π

2στz +ω−2+η.

By using the change of variable ξ=τ z +ω−with inverse τ=z−1(ξ−ω−) for

16 ·E.Bertolazzi and M.Frego

Xk(a, b) and the identity (13) we have:

Xk(a, b) = z−1Zω+

ω−

z−k(ξ−ω−)kcos σπ

2ξ2+ηdξ,

=z−k−1Zω+

ω−

k

X

j=0 k

jξj(−ω−)k−jcos π

2ξ2+σηdξ,

=z−k−1

k

X

j=0 k

j(−ω−)k−j[cos η∆Cj−σsin η∆Sj],

=cos η

zk+1 k

X

j=0 k

j(−ω−)k−j∆Cj−σsin η

zk+1 k

X

j=0 k

j(−ω−)k−j∆Sj

(24)

where

∆Cj=Cj(ω+)− Cj(ω−),∆Sj=Sj(ω+)− Sj(ω−),

are the evaluation of the momenta of the Fresnel integrals as deﬁned in (21). Anal-

ogously for Yk(a, b) we have:

Yk(a, b) = sin η

zk+1 k

X

j=0 k

j(−ω−)k−j∆Cj+σcos η

zk+1 k

X

j=0 k

j(−ω−)k−j∆Sj.(25)

This computation is inaccurate when |a|is small: in fact zappears in the denomi-

nator of several fractions. For this reason, for small values of |a|we substitute (24)

and (25) with asymptotic expansions. Notice that the recurrence (22) is unstable

so that it produces inaccurate results for large k, but we need only the ﬁrst two

terms so this is not a problem for the computation of g(A) and g0(A).

6.1 Accurate computation with small parameters

When the parameter ais small, we use identity (13) to derive series expansion:

Xk(a, b) = Z1

0

τkcos a

2τ2+bτdτ,

=Z1

0

τkhcos a

2τ2cos(bτ)−sin a

2τ2sin(bτ)idτ,

=

∞

X

n=0

(−1)n

(2n)! a

22nX4n+k(0, b)−

∞

X

n=0

(−1)n

(2n+ 1)! a

22n+1 Y4n+2+k(0, b),

=

∞

X

n=0

(−1)n

(2n)! a

22nX4n+k(0, b)−a Y4n+2+k(0, b)

2(2n+ 1) ,

(26)

and analogously using again identity (13) we have the series expansion

Yk(a, b) = Z1

0

τksin a

2τ2+bτdτ(27)

=

∞

X

n=0

(−1)n

(2n)! a

22nY4n+k(0, b) + a X4n+2+k(0, b)

2(2n+ 1) .

Fast and accurate clothoid ﬁtting ·17

From the inequalities:

|Xk| ≤ Z1

0|τk|dτ=1

k+ 1,|Yk| ≤ Z1

0|τk|dτ=1

k+ 1,

we estimate the remainder for the series of Xk:

Rp,k =

∞

X

n=p

(−1)n

(2n)! a

22nX4n+k(0, b)−a Y4n+2+k(0, b)

2(2n+ 1)

≤

∞

X

n=p

1

(2n)! a

22n1

4n+ 1 +|a|

2(2n+ 1)(4n+ 3)

≤a

22p∞

X

n=p

1

(2(n−p))! a

22(n−p)

≤a

22p∞

X

n=0

1

(2n)! a

22n=a

22pcosh(a).

The same estimate is obtained for the series of Yk

Remark 6.1. Both series (26) and (27) converge fast. For example, if |a|<10−4

and p= 2, the error is less than 6.26 ·10−18 while if p= 3 the error is less than

1.6·10−26.

It is possible to compute Xk(0, b) and Yk(0, b) as follows:

X0(0, b) = b−1sin b,

Y0(0, b) = b−1(1 −cos b),

Xk(0, b) = b−1sin b−k Yk−1(0, b), k = 1,2, . . .

Yk(0, b) = b−1k Xk−1(0, b)−cos b, k = 1,2, . . .

This recurrence permits the computation of Xk(0, b) and Yk(0, b) when b6= 0, how-

ever, it is readily seen that this recurrence formula is not stable. As an alternative,

an explicit formula based on Lommel function sµ,ν(z) can be used [Shirley and

Chang 2003]. The explicit formula is, for k= 1,2,3, . . .:

Xk(0, b) = k sk+1

2,3

2(b) sin b+f(b)sk+3

2,1

2(b)

(1 + k)bk+1

2

+cos b

1 + k,

Yk(0, b) = k sk+3

2,3

2(b) sin b+g(b)sk+1

2,1

2(b)

(2 + k)bk+1

2

+sin b

2 + k,

(28)

where f(b) = b−1sin b−cos band g(b) = f(b)(2 + k). The Lommel function has the

following expansion (see http://dlmf.nist.gov/11.9 or reference [Watson 1944])

sµ,ν (z) = zµ+1

∞

X

n=0

(−z2)n

αn+1(µ, ν ), αn(µ, ν) =

n

Y

m=1

((µ+ 2m−1)2−ν2),(29)

and using this expansion in (28) results in the following explicit formula for k=

1,2,3, . . .:

Xk(0, b) = A(b)wk+1

2,3

2(b) + B(b)wk+3

2,1

2(b) + cos b

1 + k,

Yk(0, b) = C(b)wk+3

2,3

2(b) + D(b)wk+1

2,1

2(b) + sin b

2 + k,

18 ·E.Bertolazzi and M.Frego

where

wµ,ν (b) =

∞

X

n=0

(−b2)n

αn+1(µ, ν ), A(b) = k b sin b

1 + k, B(b) = (sin b−bcos b)b

1 + k,

C(b) = −b2sin b

2 + k, D(b) = sin b−bcos b.

7. NUMERICAL TESTS

The algorithm was implemented and tested in MATLAB (2009 version). For the

Fresnel integrals computation we use the script of Telasula [2005]. The ﬁrst six

tests are taken form Walton and Meek [2008] and a MATLAB implementation of

the algorithm described in the reference is used for comparison.

Test 1. (x0, y0) = (5,4), (x1, y1) = (5,6), ϑ0=π/3, ϑ1= 7π/6;

Test 2. (x0, y0) = (3,5), (x1, y1) = (6,5), ϑ0= 2.14676, ϑ1= 2.86234;

Test 3. (x0, y0) = (3,6), (x1, y1) = (6,6), ϑ0= 3.05433, ϑ1= 3.14159;

Test 4. (x0, y0) = (3,6), (x1, y1) = (6,6), ϑ0= 0.08727, ϑ1= 3.05433;

Test 5. (x0, y0) = (5,4), (x1, y1) = (4,5), ϑ0= 0.34907, ϑ1= 4.48550;

Test 6. (x0, y0) = (4,4), (x1, y1) = (5,5), ϑ0= 0.52360, ϑ1= 4.66003.

The accuracy of ﬁt as in Walton and Meek [2008] is determined by comparing the

ending point as computed by both methods, with the given ending point. For all

the tests, both methods have an error which does not exceed 10−15 or less. Also

iterations are comparable and are reported in the following table.

#test 1 2 3 4 5 6

#iter present method 3 3 3 3 3 3

#iter Walton and Meek [2008] 5 4 4 4 5 4

The diﬀerence of the present method compared with the algorithm of Walton and

Meek [2008] are in the transition zone where the solution is close to be a circle arc

or a segment. In fact in this situation the present method performs better without

loosing accuracy or increasing in iterations as for the Meek and Walton algorithm.

The following tests highlight the diﬀerences:

Test 7. (x0, y0) = (0,0), (x1, y1) = (100,0), ϑ0= 0.01 ·2−k,ϑ1=−0.02 ·2−k;

for k= 1,2,...,10;

Test 8. (x0, y0) = (0,−100), (x1, y1)=(−100,0),

ϑ0= 0.00011 ·2−k,ϑ1=3

2π−0.0001 ·2−kfor k= 1,2,...,10.

Table II collects the results. The error is computed as the maximum of the norm

of the diﬀerences at the ending point as computed by the algorithm with the given

ending point. The tolerance used for the Newton iterative solver for both the algo-

rithms is 1 ·10−12 . Improving the tolerance does not improve the error for both the

methods. Notice that the proposed algorithm computes the solution with constant

accuracy and few iterations while Meek and Walton algorithm loose precision and

uses more iterations. The ∞symbol for iterations in Table II means that Newton

method do not reach the required accuracy and the solution is computed using the

last computed values. The iteration limit used was 100, increasing this limit to 1000

Fast and accurate clothoid ﬁtting ·19

Table II. Test #7 and #8 results

Test #7 Test #8

Proposed method Meek & Walton Proposed method Meek & Walton

kiter Error iter Error iter Error iter Error

1 2 2.6 ·10−16 30 1.83 ·10−63 3.18 ·10−14 ∞3.76 ·10−9

2 2 1.42 ·10−14 29 1.85 ·10−63 2.01 ·10−14 ∞1.45 ·10−8

3 3 0 28 1.38 ·10−62 2.01 ·10−14 ∞7.47 ·10−8

4 2 4.33 ·10−17 27 9.83 ·10−72 2.84 ·10−14 ∞3.47 ·10−8

5 2 5.42 ·10−18 26 6.96 ·10−72 0 ∞1.07 ·10−9

6 2 0 25 4.92 ·10−72 1.42 ·10−14 ∞5.53 ·10−9

7 2 1.35 ·10−18 24 3.48 ·10−72 5.12 ·10−14 ∞2.43 ·10−7

8 2 0 23 2.46 ·10−72 0 ∞3.09 ·10−6

9 2 0 22 1.74 ·10−72 0 ∞3.25 ·10−6

10 2 0 21 1.23 ·10−72 5.12 ·10−14 ∞4.84 ·10−7

did not change the results. In Table II, the algorithm of Walton and Meek [2008]

has a large number of iteration respect to the proposed method. To understand

this behavior, it is recalled that in solving a general nonlinear function f(θ) = 0

3 4 5 6 7

4

4.5

5

5.5

6Test N.1

3 3.5 4 4.5 5 5.5 6

4.2

4.4

4.6

4.8

5

5.2

5.4

5.6

Test N.2

2.5 3 3.5 4 4.5 5 5.5 6 6.5

5

5.5

6

6.5

7Test N.3

3 3.5 4 4.5 5 5.5 6

4.5

5

5.5

6

Test N.4

3.5 4 4.5 5 5.5 6

4

4.5

5

5.5

Test N.5

3.5 4 4.5 5 5.5

4

4.2

4.4

4.6

4.8

5Test N.6

Fig. 5. Results of test N.1 up to test N.6.

20 ·E.Bertolazzi and M.Frego

using Newton-Raphson method, the error k=θk−αnear the root θ=αsatisﬁes:

k+1 ≈C2

k, C =−f00(α)

2f0(α).

showing the quadratic behavior of Newton-Raphson method; large values of C

reﬂects a slow convergence. If f(θ) is the function used by Walton and Meek [2009]

for computing the clothoid curve and g(A) is the function of the proposed method

then

f(θ(A)) √A

√2π=g(A), θ(A) = (δ−A)2

4A,

and the error ek=Ak−A?near the root θ(A?) = αsatisfy:

ek+1 ≈C?e2

k, C?=−g00 (A?)

2g0(A?).

If g(A?) = 0 and A?>0 but small (i.e. A?≈0) then θ(A)≈δ2/(4A) near A?,

moreover

C?≈2(A?)2

δ2

f00(α)

f0(α),⇒C≈ − δ2

4(A?)2C?,

thus if the ratio δ/A?is large then |C| |C?|and thus Walton and Meek [2008]

algorithm is slower than the proposed one. This is veriﬁed in Table II. Notice that

Test #7 has constant Clower than Test #7 and this is reﬂected in the iteration

counts. When the ratio δ/A?is small then |C| |C?|and the algorithm of Meek

& Walton should be faster. An estimation based on Taylor expansion shows that

when δ/A?is small then C?is small too. Thus the proposed algorithm do not suﬀer

of slow convergence as veriﬁed experimentally.

8. CONCLUSIONS

An eﬀective solution to the problem of Hermite G1interpolation with a clothoid

curve was presented. We solve the G1Hermite interpolation problem in a new and

complete way.

In our approach we do not need the decomposition in mutually exclusive states,

that, numerically, introduces instabilities and inaccuracies as showed in section 7.

The solution of the interpolation problem is uniformly accurate even when close

to a straight line or an arc of circle, as pointed out in section 7, this was not

the case of algorithms found in literature. In fact, even in domains where other

algorithms solve the problem, we perform better in terms of accuracy and number

of iterations. The interpolation problem was reduced to one single function in one

variable making the present algorithm fast and robust. Existence and uniqueness

of the problem was discussed and proved in section 4. A guess functions which

allows to ﬁnd that zero with very few iterations in all possible conﬁgurations was

provided. Asymptotic expansions near critical values for Fresnel related integrals

are provided to keep uniform accuracy. Implementation details of the proposed

algorithm are given in the appendix using pseudocode and can be easily translated

in any programming language.

Fast and accurate clothoid ﬁtting ·21

A. ALGORITHMS FOR THE COMPUTATION OF FRESNEL RELATED INTEGRALS

We present here the algorithmic version of the analytical expression we derived

in Section 6 and 7. These algorithms are necessary for the computation of the

main function buildClothoid of Section 5 which takes the input data (x0,y0,

ϑ0,x1,y1,ϑ1) and returns the parameters (κ,κ0,L) that solve the problem as

expressed in equation (2). Function evalXY computes the generalized Fresnel inte-

grals (23). It distinguishes the cases of alarger or smaller than a threshold ε. The

value of εis discussed in Section 7, see for example Remark 6.1. Formulas (24)-

(25), used to compute Xk(a, b) and Yk(a, b) at arbitrary precision when |a| ≥ ε,

are implemented in function evalXYaLarge. Formulas (26)-(27), used to compute

Xk(a, b) and Yk(a, b) at arbitrary precision when |a|< ε, are implemented in func-

tion evalXYaSmall. This function requires computation of (28) implemented in

function evalXYaZero which needs (reduced) Lommel function (29) implemented

in function rLommel.

Function evalXY(a,b,c,k)

1if |a|< ε then ˆ

X, ˆ

Y←evalXYaSmall(a,b,k,5) else

ˆ

X, ˆ

Y←evalXYaLarge(a,b,k)for j= 0,1,...,k−1do Xj←ˆ

Xjcos c−ˆ

Yjsin c;

Yj←ˆ

Xjsin c+ˆ

Yjcos creturn X,Y

Function evalFresnelMomenta(t,k)

1C0← C(t); S0← S (t); z←π(t2/2) c←cos z s ←sin z;

2if k > 1then C1←s/π;S1←(1 −c)/π if k > 2then C2←(t s −S0)/π;

S2←(C0−t c)/π return C,S

Function evalXYaLarge(a,b,k)

1s←a

|a|;z←p|a|/π;`←s b

z π ;γ← − sb2

2|a|;sγ←sin γ;cγ←cos γ;

2C+, S+←evalFresnelMomenta(`+z, k ); C−, S−←evalFresnelMomenta(z, k);

3∆C ← C+−C−; ∆S ← S+−S−;

4X0←z−1(cγ∆C0−s sγ∆S0); Y0←z−1(sγ∆C0+s cγ∆S0);

5if k > 1then

6dc←∆C1−`∆C0;ds←∆S1−`∆S0;

7X1←z−2(cγdc−s sγds); Y1←z−2(sγdc+s cγds);

8end if

9if k > 1then

10 dc←∆C2+`(`∆C0−2∆C1); ds←∆S2+`(`∆S0−2∆S1);

11 X2←z−3(cγdc−s sγds); Y2←z−3(sγdc+s cγds);

12 end if

13 return X,Y

22 ·E.Bertolazzi and M.Frego

Function evalXYaZero(b,k)

1if |b|< ε then

2X0←1−(b2/6) 1−b2/20;Y0←(b2/2) 1−(b2/6) 1−b2/30;

3else

4X0←(sin b)/b;Y0←(1 −cos b)/b;

5end if

6A←bsin b;D←sin b−bcos b;B←bD;C← −b2sin b;

7for k= 0,1,...,k−1do

8Xk+1←1

1 + kkA rLommelk+1

2,3

2, b+BrLommelk+3

2,1

2, b+ cos b;

9Yk+1 ←1

2 + kCrLommelk+3

2,3

2, b+ sin b+DrLommelk+1

2,1

2, b;

10 end for

11 return X,Y

Function evalXYaSmall(a,b,k,p)

1ˆ

X, ˆ

Y←evalXYaZero(b, k + 4p+ 2); t←1;

2for j= 0,1,...,k−1do Xj←X0

j−a

2Y0

j+2;Yj←Y0

j+a

2X0

j+2 for

n= 1,2,...,p do

3t←(−t a2)/(16n(2n−1)); s←a/(4n+ 2);

4for j= 0,1,...,k−1do

5Xj←Xj+t(ˆ

X4n+j−sˆ

Y4n+j+2);

6Yj←Yj+t(ˆ

Y4n+j+sˆ

X4n+j+2);

7end for

8end for

9return X,Y

Function rLommel(µ,ν,b)

1t←(µ+ν+ 1)−1(µ−ν+ 1)−1;r←t;n←1; ε←10−50;

2while |t|> ε |r|do t←t(−b)

2n+µ−ν+ 1

b

2n+µ+ν+ 1 ;r←r+t;n←n+ 1

return r

REFERENCES

Abramowitz, M. and Stegun, I. A. 1964. Handbook of Mathematical Functions with Formu-

las, Graphs, and Mathematical Tables. Number 55 in National Bureau of Standards Applied

Mathematics Series. U.S. Government Printing Oﬃce, Washington, D.C.

Backeljauw, F. and Cuyt, A. 2009. Algorithm 895: A continued fractions package for special

functions. ACM Trans. Math. Softw. 36, 3 (July), 15:1–15:20.

Baran, I.,Lehtinen, J.,and Popovi´

c, J. 2010. Sketching clothoid splines using shortest paths.

Computer Graphics Forum 29, 2 (May), 655–664.

Bertolazzi, E.,Biral, F.,and Lio, M. D. 2006. Symbolic-numeric eﬃcient solution of optimal

control problems for multibody systems. Journal of Computational and Applied Mathemat-

ics 185, 2, 404–421. Special Issue: International Workshop on the Technological Aspects of

Mathematics.

Davis, T. 1999. Total least-squares spiral curve ﬁtting. Journal of Surveying Engineering-

Asce 125, 4 (NOV), 159–176.

Fast and accurate clothoid ﬁtting ·23

De Boor, C. 1978. A Practical Guide to Splines. Number v. 27 in Applied Mathematical Sciences.

Springer-Verlag.

De Cecco, M.,Bertolazzi, E.,Miori, G.,Oboe, R.,and Baglivo, L. 2007. Pc-sliding for

vehicles path planning and control - design and evaluation of robustness to parameters change

and measurement uncertainty. In ICINCO-RA (2)’2007. 11–18.

Farin, G. 2002. Curves and surfaces for CAGD: a practical guide , 5th ed. Morgan Kaufmann

Publishers Inc., San Francisco, CA, USA.

Farouki, R. T. and Neff, C. A. 1995. Hermite interpolation by pythagorean hodograph quintics.

Math. Comput. 64, 212 (Oct.), 1589–1609.

Kanayama, Y. and Hartman, B. 1989. Smooth local path planning for autonomous vehicles. In

Robotics and Automation, 1989. Proceedings., 1989 IEEE International Conference on. 1265

–1270 vol.3.

McCrae, J. and Singh, K. 2009. Sketching piecewise clothoid curves. Computers & Graph-

ics 33, 4 (June), 452–461.

Meek, D. S. and Walton, D. J. 1992. Clothoid spline transition spirals. Mathematics of

Computation 59, 117–133.

Meek, D. S. and Walton, D. J. 2004. A note on ﬁnding clothoids. J. Comput. Appl. Math. 170, 2

(Sept.), 433–453.

Meek, D. S. and Walton, D. J. 2009. A two-point g1 hermite interpolating family of spirals. J.

Comput. Appl. Math. 223, 1 (Jan.), 97–113.

Pavlidis, T. 1983. Curve ﬁtting with conic splines. ACM Trans. Graph. 2, 1 (Jan.), 1–31.

Press, W. H.,Vetterling, W. T.,Teukolsky, S. A.,and Flannery, B. P. 2002. Numerical

Recipes in C++: the art of scientiﬁc computing, 2nd ed. Cambridge University Press, New

York, NY, USA.

Scheuer, A. and Fraichard, T. 1997. Continuous-curvature path planning for car-like vehi-

cles. In Intelligent Robots and Systems, 1997. IROS ’97., Proceedings of the 1997 IEEE/RSJ

International Conference on. Vol. 2. Intelligent Robots and Systems. IROS ’97.,, 997–1003.

Shin, D. H. and Singh, S. 1990. Path generation for robot vehicles using composite clothoid

segments. Tech. Rep. CMU-RI-TR-90-31, Robotics Institute, Pittsburgh, PA. December.

Shirley, E. L. and Chang, E. K. 2003. Accurate eﬃcient evaluation of lommel functions for

arbitrarily large arguments. Metrologia 40, 1, S5.

Smith, D. M. 2011. Algorithm 911: Multiple-precision exponential integral and related functions.

ACM Trans. Math. Softw. 37, 4 (Feb.), 46:1–46:16.

Snyder, W. V. 1993. Algorithm 723: Fresnel integrals. ACM Transactions on Mathematical

Software 19, 4 (Dec.), 452–456.

Telasula, V. 2005. Fresnel cosine and sine integral function. http://www.mathworks.it/

matlabcentral.

Thompson, W. J. 1997. Atlas for Computing Mathematical Functions: An Illustrated Guide for

Practitioners with Programs in C and Mathematica with Cdrom, 1st ed. John Wiley & Sons,

Inc., New York, NY, USA.

Walton, D. and Meek, D. 1996. A planar cubic bezier spiral. Journal of Computational and

Applied Mathematics 72, 1, 85–100.

Walton, D. and Meek, D. 2008. An improved euler spiral algorithm for shape completion. In

Computer and Robot Vision, 2008. CRV ’08. Canadian Conference on. IEEE, 237–244.

Walton, D. and Meek, D. 2009. Interpolation with a single cornu spiral segment. Journal of

Computational and Applied Mathematics 223, 1, 86–96.

Walton, D. J. and Meek, D. S. 2007. G2curve design with a pair of pythagorean hodograph

quintic spiral segments. Comput. Aided Geom. Des. 24, 5 (July), 267–285.

Watson, G. N. 1944. A Treatise on the Theory of Bessel Functions. Cambridge University

Press, Cambridge, England.