ArticlePDF Available

Abstract and Figures

A new effective solution to the problem of Hermite $G^1$ interpolation 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 difficult 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 find that zero with very few iterations in all possible configurations. 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 efficient 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 configurations while other algorithms have a loss of accuracy near the transition zones. (software implementing the algorithm is available at http://www.mathworks.com/matlabcentral/fileexchange/42113-g1-fitting-with-clothoids)
Content may be subject to copyright.
Fast and accurate G1fitting of clothoid curves
ENRICO BERTOLAZZI and MARCO FREGO
University of Trento, Italy
A new effective 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 difficult 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
find that zero with very few iterations in all possible configurations.
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 efficient 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 configurations 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 fitting, 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
Neff 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 insufficient 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-effectiveness 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 effective
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) suffice.
The article is structured as follows. In section 2 we define 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
infinite 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 satisfies the following differential 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 definition:
Definition 2.1 Clothoid curve. The general parametric form of a clothoid
Fast and accurate clothoid fitting ·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 definitions, 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 different definitions, 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 definitions:
C(t) = Z2
πt
0
cos τ2dτ=sign(t)
2πZπ
2t2
0
cos u
udu
S(t) = Z2
π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, find a clothoid segment of the form (2) which
satisfies:
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 infinite number of solutions. In
fact, given ϑ(s), the angle of a clothoid which solves Problem 1, satisfies ϑ(0) =
ϑ0+ 2and ϑ(L) = ϑ1+ 2with k, ` Z: different values of kcorrespond
to different interpolant curves that loop around the initial and the final 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) =
x1x0RL
0cos 1
2κ0s2+κs +ϑ0ds
y1y0RL
0sin 1
2κ0s2+κs +ϑ0ds
ϑ11
2κ0L2+κL +ϑ0
.(6)
In Section 3 the nonlinear system (6) is reduced to a single non linear equation that
allows for the efficient 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=
xLR1
0cos 2++ϑ0dτ
yLR1
0sin 2++ϑ0dτ
ϑ1(A+B+ϑ0)
,(7)
where A=1
2κ0L2,B=L κ, ∆x=x1x0, ∆y=y1y0. 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++cdτ,
Y(a, b, c) = Z1
0
sin a
2τ2++cdτ.
(8)
Combining (8) with (7) becomes:
FL, B
L,2A
L2=
xL·X(2A, B, ϑ0)
yL·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 fitting ·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) = xL·X(2A, δ A, ϑ0)
yL·Y(2A, δ A, ϑ0),(11)
followed by the computation of Bby (10). We can perform one further simplification
using polar coordinates to represent (∆x, y), namely
x=rcos ϕ,
y=rsin ϕ. (12)
From (12) and L > 0 we define 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 simplified in:
g(A) = Y(2A, δ A, φ0),
f(L, A) = px2+ ∆y2L X(2A, δ A, φ0),(14)
where φ0=ϑ0ϕ.
Supposing to find Asuch that g(A) = 0, then from f(L, A) = 0 we compute Land
Busing equations (14) and (10) respectively. This yields
L=px2+ ∆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=px2+ ∆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 defined 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((τ1) + δτ +φ0) dτ,
=Z1
0
sin((τ1) δτ φ0) dτ,
=Z1
0
sin(A(1 z)zδ(1 z)φ0) dz, [z= 1 τ]
=Z1
0
sin(Az(z1) + δz φ1) dz,
=Y(2A, δ +A, φ1),
Fast and accurate clothoid fitting ·7
we have g(A) = gR(A).
Using again g(A) with the following manipulation:
g(A) = Y(2A, δ +A, φ0) = Z1
0
sin(2+ (δ+A)τ+φ0) dτ,
=Z1
0
sin(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.
Reflecting 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 fitting 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 defined 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 infinite 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 find the interval where the
solution is unique. The Theorems characterize the zeros of the functions (15) finding
intervals where the solution exists and is unique.
Fast and accurate clothoid fitting ·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 < λ =1cos φ0
1cos φ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 defined 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 satisfies 0θθ0with θ0defined 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 φ00 and cos φ00 resulting in
p(0) = q(0) <0.
The combination of Lemma 4.1 together with reversed and mirrored problems,
proves that the interpolation problem satisfies Assumption 4.2.
With this Assumption and Lemma 4.3 prove a Theorem that states the existence
and uniqueness in a specified 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 φ10 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φ0A, φ0),
=Z1
0
sin((τ1) + φ0(1 2τ)) dτ,
=Z1
1
sin(A(z21)/40)dz
2,[τ= (z+ 1)/2]
=Z1
1
sin(A(z21)/4) cos(0)dz
2Z1
1
cos(A(z21)/4) sin(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(z21)/4) cos(0) dz.
From this last equality, if A= 0 then g(A) = 0. If 0 <|A|<4π, the sign of the
quantity sin(A(z21)/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(0) dz=1
|φ0|,Z1
π/(2|φ0|)|cos(0)|dz=1sin |φ0|
|φ0|<1
|φ0|.
If in addition, 0 <|A|<2πthen sin(A(z21)/4)is positive and monotone
decreasing so that:
Zπ/(2|φ0|)
0
sin A
4(z21)cos(0) dzC
|φ0|,
Z1
π/(2|φ0|)
sin A
4(z21)cos(0) dz<C
|φ0|,
where
C=sin A
16 |φ0|2π24|φ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 fitting ·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 zwsin z. In both figures φ0=(3/4)πand φ1=π.
cannot be satisfied by the same θin the specified range, so that they are mutually
exclusive although one of the two is satisfied. Thus g(A) = 0 has a unique solution.
To find the equivalent range of Awe select the correct solution of (δA)2= 4max.
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 wR,
Zφ1
φ0
cos z
z+θφ0
dz=Zφ1
φ0
cos zwsin 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 zwsin zis strictly positive
for z(φ0, φ0+π), see Figure 3. Thus we can bound integral (18) as
Zφ1
φ0
cos zwsin z
z+θφ0
dz > Zφ0+2π
φ0
cos zwsin z
z+θφ0
dz,
Rφ0+2π
φ0cos zwsin 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=px2+ ∆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 AAmax 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 specified 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 infinite
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 sinxxin Y(2A, δ A, φ0) and thus,
g(A) = Y(2A, δ A, φ0)Z1
0
2+ (δA)τ+φ0dτ=φ0+φ1
2A
6,
and solving for A
A(φ0, φ1)3(φ0+φ1),(19)
This approximation is a fine initial point for Newton-Raphson, however better ap-
proximation for A(φ0, φ1) are obtained by least squares method. Invoking reflection
Fast and accurate clothoid fitting ·13
and mirroring properties, the functional form of the approximation is simplified 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=φ1and
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 1010, 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-
ficients 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 infinity. The angles range
in [π, π].
14 ·E.Bertolazzi and M.Frego
Table I. The fitting algorithm
Function normalizeAngle(φ)
1while φ > +πdo φφ2π;
2while φ < πdo φφ+ 2π;
3return φ;
Function buildClothoid(x0,y0,ϑ0,x1,y1,ϑ1,)
1xx1x0; ∆yy1y0;
2Compute rand ϕfrom rcos ϕ= ∆xand rsin ϕ= ∆y;
3φ0normalizeAngle(ϑ0ϕ); φ1normalizeAngle(ϑ1ϕ);
4Define gas g(A) := Y(2A, (φ1φ0)A, φ0);
5Set A3(φ1+φ0); // In alternative use (20a) or (20b)
6while |g(A)|>  do AAg(A)/g0(A) ;
7Lr/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 first two momenta are
necessary so that the inaccuracy for large momenta does not pose any problem. A
different 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 π
2t2kSk1(t),
Sk+1(t) = 1
πkCk1(t)tkcos π
2t2.
(22)
Fast and accurate clothoid fitting ·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
π1cos π
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 sufficient to consider two integrals, that cover all possible
cases:
Xk(a, b, c) = Z1
0
τkcos a
2τ2++cdτ,
Yk(a, b, c) = Z1
0
τksin a
2τ2++cdτ,
(23)
so that
g(A) = Y0(2A, δ A, φ0),
g0(A) = X1(2A, δ A, φ0)X2(2A, δ A, φ0),
and finally, 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 cYk(a, b, 0) sin c,
Yk(a, b, c) = Xk(a, b, 0) sin c+Yk(a, b, 0) cos c.
Defining 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+=π
2σ τσp|a|
π+b
pπ|a|!2
b2
2a=π
2στz +ω2+η.
By using the change of variable ξ=τ z +ωwith inverse τ=z1(ξω) for
16 ·E.Bertolazzi and M.Frego
Xk(a, b) and the identity (13) we have:
Xk(a, b) = z1Zω+
ω
zk(ξω)kcos σπ
2ξ2+ηdξ,
=zk1Zω+
ω
k
X
j=0 k
jξj(ω)kjcos π
2ξ2+σηdξ,
=zk1
k
X
j=0 k
j(ω)kj[cos ηCjσsin ηSj],
=cos η
zk+1 k
X
j=0 k
j(ω)kjCjσsin η
zk+1 k
X
j=0 k
j(ω)kjSj
(24)
where
Cj=Cj(ω+)− Cj(ω),Sj=Sj(ω+)− Sj(ω),
are the evaluation of the momenta of the Fresnel integrals as defined in (21). Anal-
ogously for Yk(a, b) we have:
Yk(a, b) = sin η
zk+1 k
X
j=0 k
j(ω)kjCj+σcos η
zk+1 k
X
j=0 k
j(ω)kjSj.(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 first 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+dτ,
=Z1
0
τkhcos a
2τ2cos()sin a
2τ2sin()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+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 fitting ·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(np))! a
22(np)
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|<104
and p= 2, the error is less than 6.26 ·1018 while if p= 3 the error is less than
1.6·1026.
It is possible to compute Xk(0, b) and Yk(0, b) as follows:
X0(0, b) = b1sin b,
Y0(0, b) = b1(1 cos b),
Xk(0, b) = b1sin bk Yk1(0, b), k = 1,2, . . .
Yk(0, b) = b1k Xk1(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) = b1sin bcos 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
((µ+ 2m1)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 bbcos b)b
1 + k,
C(b) = b2sin b
2 + k, D(b) = sin bbcos 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 first 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 fit 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 1015 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 difference 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 differences:
Test 7. (x0, y0) = (0,0), (x1, y1) = (100,0), ϑ0= 0.01 ·2k,ϑ1=0.02 ·2k;
for k= 1,2,...,10;
Test 8. (x0, y0) = (0,100), (x1, y1)=(100,0),
ϑ0= 0.00011 ·2k,ϑ1=3
2π0.0001 ·2kfor k= 1,2,...,10.
Table II collects the results. The error is computed as the maximum of the norm
of the differences 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 ·1012 . 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 fitting ·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 ·1016 30 1.83 ·1063 3.18 ·1014 3.76 ·109
2 2 1.42 ·1014 29 1.85 ·1063 2.01 ·1014 1.45 ·108
3 3 0 28 1.38 ·1062 2.01 ·1014 7.47 ·108
4 2 4.33 ·1017 27 9.83 ·1072 2.84 ·1014 3.47 ·108
5 2 5.42 ·1018 26 6.96 ·1072 0 1.07 ·109
6 2 0 25 4.92 ·1072 1.42 ·1014 5.53 ·109
7 2 1.35 ·1018 24 3.48 ·1072 5.12 ·1014 2.43 ·107
8 2 0 23 2.46 ·1072 0 3.09 ·106
9 2 0 22 1.74 ·1072 0 3.25 ·106
10 2 0 21 1.23 ·1072 5.12 ·1014 4.84 ·107
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 θ=αsatisfies:
k+1 C2
k, C =f00(α)
2f0(α).
showing the quadratic behavior of Newton-Raphson method; large values of C
reflects 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=AkA?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 verified in Table II. Notice that
Test #7 has constant Clower than Test #7 and this is reflected 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 suffer
of slow convergence as verified experimentally.
8. CONCLUSIONS
An effective 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 find that zero with very few iterations in all possible configurations 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 fitting ·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, ˆ
YevalXYaSmall(a,b,k,5) else
ˆ
X, ˆ
YevalXYaLarge(a,b,k)for j= 0,1,...,k1do 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) ccos z s sin z;
2if k > 1then C1s/π;S1(1 c)if k > 2then C2(t s S0);
S2(C0t c)return C,S
Function evalXYaLarge(a,b,k)
1sa
|a|;zp|a|;`s b
z π ;γ← − sb2
2|a|;sγsin γ;cγcos γ;
2C+, S+evalFresnelMomenta(`+z, k ); C, SevalFresnelMomenta(z, k);
3C ← C+C; ∆S S+S;
4X0z1(cγC0s sγS0); Y0z1(sγC0+s cγS0);
5if k > 1then
6dcC1`C0;dsS1`S0;
7X1z2(cγdcs sγds); Y1z2(sγdc+s cγds);
8end if
9if k > 1then
10 dcC2+`(`C02∆C1); dsS2+`(`S02∆S1);
11 X2z3(cγdcs sγds); Y2z3(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
2X01(b2/6) 1b2/20;Y0(b2/2) 1(b2/6) 1b2/30;
3else
4X0(sin b)/b;Y0(1 cos b)/b;
5end if
6Absin b;Dsin bbcos b;BbD;C← −b2sin b;
7for k= 0,1,...,k1do
8Xk+11
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, ˆ
YevalXYaZero(b, k + 4p+ 2); t1;
2for j= 0,1,...,k1do XjX0
ja
2Y0
j+2;YjY0
j+a
2X0
j+2 for
n= 1,2,...,p do
3t(t a2)/(16n(2n1)); sa/(4n+ 2);
4for j= 0,1,...,k1do
5XjXj+t(ˆ
X4n+jsˆ
Y4n+j+2);
6YjYj+t(ˆ
Y4n+j+sˆ
X4n+j+2);
7end for
8end for
9return X,Y
Function rLommel(µ,ν,b)
1t(µ+ν+ 1)1(µν+ 1)1;rt;n1; ε1050;
2while |t|> ε |r|do tt(b)
2n+µν+ 1
b
2n+µ+ν+ 1 ;rr+t;nn+ 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 Office, 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 efficient 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 fitting. Journal of Surveying Engineering-
Asce 125, 4 (NOV), 159–176.
Fast and accurate clothoid fitting ·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 finding 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 fitting 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 scientific 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 efficient 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.
... The path generation step of the algorithm may accordingly be performed by means of clothoid fitting, as described in Bertolazzi and Frego [2013]. Linear clothoids feature a linearly variable continuous curvature that can be expressed as: ...
... The initial and final points of the clothoid -whose length is L c -are {x R (0), y R (0)} and {x R (L c ), y R (L c )}, respectively, while the corresponding path heading angles are θ 0c and θ Lc . The G 1 Hermite interpolation problem consists of finding a curve in the plane satisfying these initial and final conditions, and Bertolazzi and Frego [2013] propose to use a single clothoid segment to achieve this goal, which is equivalent to finding the parameters L c , κ 1 and κ 2 so as to uniquely define the clothoid. The G 2 Hermite interpolation problem involves instead the determination of a curve whose initial and final points are specified in terms of position, tangent vector and curvature, but for this purpose a single clothoid would not be adequate. ...
... The G 2 Hermite interpolation problem involves instead the determination of a curve whose initial and final points are specified in terms of position, tangent vector and curvature, but for this purpose a single clothoid would not be adequate. Following Bertolazzi and Frego [2013], a clothoid curve satisfies the differential equations (4.25): (4.25) with θ c representing the local tangent angle. Since the initial value of θ c is θ 0c , then it is θ c = 1 2 κ 2 s 2 + κ 1 s + θ 0c . ...
Thesis
Full-text available
The aim of this project is to present an advanced path planning and control framework that enables a self-driving racing vehicle to move along a given circuit, exploiting all the limits of handling. In order to achieve this target, a simulation environment is created, with a complex non linear 10 degrees of freedom multibody vehicle model adopted as driving simulator. The latter includes a Pacejka formulation of tire performance and an electrical powertrain model, with the purpose of accurately reproducing the behavior of a Formula SAE vehicle. A hierarchical motion planning framework is proposed, consisting of a high level planner (HLP) and a low level controller (LLC). The HLP uses a novel advanced kineto-dynamical vehicle model and it solves an optimal control problem (OCP) in receding horizon fashion, computing the optimal speed and steering angles profiles over a prediction window. An accurate identification of vehicle maximum performance (both in terms of G-G acceleration envelope and steering behavior) allows the HLP to produce solutions that are perfectly compatible with actuator limits. The output steering angle trajectory is directly fed to the vehicle, while the speed profile is tracked by a low level PID controller, which calculates the corresponding motor and brake actuation torques. Gain scheduling is employed to tune PID coefficients at different operating conditions, as a function of vehicle speed, while actuators saturation is taken into account with anti-windup techniques. The high level planner is also capable to deal with the presence of potential fixed obstacles, detected by a vehicle camera sensor model. An efficient obstacle avoidance technique is presented, which uses semi-analytical OCP solutions to generate a set of possible clothoid-based trajectories, for then choosing the one associated with the minimum time. The entire path planning and control framework is tested, in Matlab & Simulink environment, to drive the vehicle along two different circuits. Simulation results suggest that the proposed algorithms are suited for being implemented in a real online application. This Thesis is organized as follows. Chapter 1 illustrates the state of the art, with a particular attention to optimal control algorithms and their applicability to real-time automotive implementations. Chapter 2 presents the high-fidelity 10 degrees of freedom vehicle model in its entirety. The procedures enabling the identification of the vehicle maximum performance limits are outlined in Chapter 3. The high level planner and the optimal control formulation are discussed in Chapter 4, while the structure and design of the low level PID controller is illustrated in Chapter 5. Finally, Chapter 6 presents simulation results, to show the effectiveness of the entire framework.
... An off-line convex optimization is presented by [11]. Finally, a semi-analytical solution based on the clothoids curve is proposed in [12]. However, clothoid based approaches were never proven experimentally, since till now it was considered a computationally expensive method. ...
... Indeed, clothoids are the natural trajectory for a car-like vehicle at constant velocity, as discussed in [14]. The work in [12] introduces and prove an efficient strategy to solve the G 1 Hermite interpolation problem. The proposed control approach is based on the use of these curves to generate the steering control to follow a given path. ...
... As shown in Figure 6, the clothoid is generated between the pose of the vehicle (x r (s), y r (s), θ(s)) and the desired pose on the given path at lookahead distance s + L h : (x h (s + L h ), y h (s + L h ), θ h (s + L h )). With these quantities, the clothoid is uniquely defined as guaranteed by the algorithm provided in [12] (see theorem 11). The solution of the G 1 Hermite interpolation problem returns the necessary parameters to define the clothoid, which are κ,κ and L c ∈ R. The parameter L c is the length of the clothoid, that is different from L h . ...
Conference Paper
Real-time planning of feasible motion, and the accurate tracking of that motion are key aspects of autonomous vehicles. Various planning and lateral control algorithms have been proposed in the literature. However, no experimental comparison has been carried out to confront the performance of different algorithms. In this work some of the most popular algorithms have been experimentally tested to drive a scale vehicle model along a track. A new algorithm for path tracking based on clothoid curves is here proposed and experimentally validated. The same curves are also used in a new path planning algorithm proposed. This planning is compared to the off-line solution of an optimal control problem. The results of the comparison and the validations are then reported and discussed.
... For example, a driver do not need a computer to calculate or a lidar to detect the distance between vehicle and obstacle. In this paper we will follow the method of ENRICO BERTOLAZZI and MARCO FREGO [16]. More details will be described in Section V. ...
... Fig.5 demonstrates the ability of T esttoGoal procedure in accurately arriving at goal pose and smoothing path. It is a hard task to get to a goal pose accurately just through propagating dynamic model [16], like standard RRT * , while the fast clothoid fitting method make this possible and simplified. Moreover, because this procedure can reduce the amount of nodes on the path, therefore, the path with T esttoGoal looks smoother. ...
Conference Paper
Full-text available
We present an improved RRT * to blend lane information and avoid obstacles in this paper. Unlike most of other improved RRT * , this paper attach great importance to the convergent goal of RRT *. We consider the condition that there exists a reference path, maybe not the shortest path, but the environment requires the vehicle to follow, such as a lane center. Compared with standard RRT * applied to differential situation, we first add a T esttoGoal procedure to improve the convergent speed and also make sure the path can reach the goal pose but not the goal region to promise the safety of autonomous vehicle. One of the key characteristic of our improved algorithm is to employ a fast clothoid fitting method into RRT * to enable us to control the curvature. Another important modification is the heuristic sampling method that makes our algorithm can converge to lane center. We evaluate our algorithm with a real lane to demonstrate the effect of our modifications.
... The first is determined dynamically by exploiting a clothoid [38,39], a curve characterized by a continuous third-order curvature that ensures continuity in the jerk and so an optimal comfort level in the navigation. On top of that, the curve can be expressed analytically [40,41], providing as advantages a simple data management for control, HMI and AR, but also it facilitates the identification of obstacles and thus their avoidance. ...
Article
Full-text available
Any severe motor disability is a condition that limits the ability to interact with the environment, even the domestic one, caused by the loss of control over one’s mobility. This work presents RoboEYE, a power wheelchair designed to allow users to move easily and autonomously within their homes. To achieve this goal, an innovative, cost-effective and user-friendly control system was designed, in which a non-invasive eye tracker, a monitor, and a 3D camera represent some of the core elements. RoboEYE integrates functionalities from the mobile robotics field into a standard power wheelchair, with the main advantage of providing the user with two driving options and comfortable navigation. The most intuitive and direct modality foresees the continuous control of frontal and angular wheelchair velocities by gazing at different areas of the monitor. The second, semi-autonomous modality allows navigation toward a selected point in the environment by just pointing and activating the wished destination while the system autonomously plans and follows the trajectory that brings the wheelchair to that point. The purpose of this work was to develop the control structure and driving interface designs of the aforementioned driving modalities taking into account also uncertainties in gaze detection and other sources of uncertainty related to the components to ensure user safety. Furthermore, the driving modalities, in particular the semi-autonomous one, were modeled and qualified through numerical simulations and experimental verification by testing volunteers, who are regular users of standard electric wheelchairs, to verify the efficiency, reliability and safety of the proposed system for domestic use. RoboEYE resulted suitable for environments with narrow passages wider than 1 m, which is comparable with a standard domestic door and due to its properties with large commercialization potential.
... We base our approach on this assumption and utilize clothoids to provide a compact representation of the planned path. A single clothoid is defined by three parameters, where κ represents the curvature at the starting point, κ the change in curvature per unit of length, and L the total length of this trajectory representation [33]. The Local Path Generation part of ProMoD consists of two clothoids as visualized in Fig. 2. The first clothoid shown in dark grey is fitted to start in the center of gravity parallel to the velocity vector v of the vehicle, and ends on the global target trajectory with a short preview time P 1 = 250ms to provide essential information in order to neutralize lateral path deviation. ...
Preprint
Understanding and modeling human driver behavior is crucial for advanced vehicle development. However, unique driving styles, inconsistent behavior, and complex decision processes render it a challenging task, and existing approaches often lack variability or robustness. To approach this problem, we propose Probabilistic Modeling of Driver behavior (ProMoD), a modular framework which splits the task of driver behavior modeling into multiple modules. A global target trajectory distribution is learned with Probabilistic Movement Primitives, clothoids are utilized for local path generation, and the corresponding choice of actions is performed by a neural network. Experiments in a simulated car racing setting show considerable advantages in imitation accuracy and robustness compared to other imitation learning algorithms. The modular architecture of the proposed framework facilitates straightforward extensibility in driving line adaptation and sequencing of multiple movement primitives for future research.
... In order to accomplish these restrictions, Euler spirals are used for these motion primitives. These curves of continuous curvature are calculated offline [24] using the techniques described in [5] for achieving a reduced set. These motion primitives are then discretized, both in position and orientation, obtaining the list of 3D cells traversed by each motion, thus speeding up the online graph search. ...
Article
Full-text available
This paper presents an algorithm for finding a solution to the problem of planning a feasible path for a slender autonomous mobile robot in a large and cluttered environment. The presented approach is based on performing a graph search on a kinodynamic-feasible lattice state space of high resolution; however, the technique is applicable to many search algorithms. With the purpose of allowing the algorithm to consider paths that take the robot through narrow passes and close to obstacles, high resolutions are used for the lattice space and the control set. This introduces new challenges because one of the most computationally expensive parts of path search based planning algorithms is calculating the cost of each one of the actions or steps that could potentially be part of the trajectory. The reason for this is that the evaluation of each one of these actions involves convolving the robot’s footprint with a portion of a local map to evaluate the possibility of a collision, an operation that grows exponentially as the resolution is increased. The novel approach presented here reduces the need for these convolutions by using a set of offline precomputed maps that are updated, by means of a partial convolution, as new information arrives from sensors or other sources. Not only does this improve run-time performance, but it also provides support for dynamic search in changing environments. A set of alternative fast convolution methods are also proposed, depending on whether the environment is cluttered with obstacles or not. Finally, we provide both theoretical and experimental results from different experiments and applications.
... In this paper, we follow the method by ENRICO BERTO-LAZZI and MARCO FREGO, which provides a robust and fast computational method [17]. This new mehod does not need to split the problem in mutually exclusive cases, and also does not suffer in case of degenerate Hermite data like straight lines or circles. ...
Conference Paper
Full-text available
For its high efficiency and simpleness, Pure-Pursuit has been widely used in fields of automatic drive. This paper presents a modified robust controller based on Pure-Pursuit. The highlight of the method is that it replaces the circle in Pure-Pursuit with a clothoid C1 curve. Meantime, it applies the curvature information from the clothoid curve to tune the look-ahead distance. We compare the controller with improved Pure-Pursuit through experiments on the platform of autonomous vehicle, TuZhi, developed by Wuhan University. The results demonstrate that the proposed controller is more effective.
Conference Paper
Full-text available
In this paper we describe a system aimed at the generation and analysis of graffiti tags.We argue that the dynamics of the movement involved in generating tags is in large part - and at a higher degree with respect to many other visual art forms - determinant of their stylistic quality. To capture this notion computationally, we rely on a biophysically plausible model of handwriting gestures (the Sigma Lognormal Model proposed by Réjean Plamondon et al.) that permits the generation of curves which are aesthetically and kinetically similar to the ones made by a human hand when writing. We build upon this model and extend it in order to facilitate the interactive construction and manipulation of digital tags. We then describe a method that reconstructs any planar curve or a sequence of planar points with a set of corresponding model parameters. By doing so, we seek to recover plausible velocity and temporal information for a static trace. We present a number of applications of our system: (i) the interactive design of curves that closely resemble the ones typically observed in graffiti art; (ii) the stylisation and beautification of input point sequences via curves that evoke a smooth and rapidly executed movement; (iii) the generation of multiple instances of a synthetic tag from a single example. This last application is a step in the direction of our longer term plan of realising a system which is capable of automatically generating convincing images in the graffiti style space.
Article
Full-text available
The response of an autonomous vehicle in tracking a reference path depends partly on the nature of the path. The condition for paths that are intrinsically amenable to follow are briefly presented, and then a method for the generation of amenable paths is proposed in this paper. Previous path generation methods have sought to simplify a path by using arcs, superarcs, polynomial curves, and clothoid curves to round corners, which result from poly- line fits through a given sequence of points. The developed method consists of two steps: First, a sequence of postures is obtained using given points, then each pair of neighboring postures is connected with three clothoid curve segments. In the second step, a completely general method to connect a path of clothoid curves between two completely arbitrary postures was not envisioned and method for a pair of adjacent postures was developed. By virtue of the property of clothoid curves, a generated path is continuous with respect to position, tangent direction, and curvature, and is linear in curvature. Aside from the properties innate to clothoid curves, the generated paths transition smoothly into turns, pass through all the way points, and sweep outside the corners. For interpolating around obstacles that are commonly inside the corner, these properties are especially useful.
Article
The clothoid is a spiral that is used as transition curve in highway and railway route design. Although its defining formulas are transcendental functions, recent work has shown that it can be used fairly easily on small computers. A single nonlinear equation must be solved in each of four common arrangements of the clothoid. The purpose of this work is to give starting intervals for which Newton's root finding method applied to those equations is guaranteed to converge.
Article
A rapidly convergent algorithm for fitting clothoids to measured points is developed and tested. The second-order, reduced Hessian method, broadly applicable to the class of scalable, C 2 parametrizations, is orthogonal distance re- gression with four-parameter similarity transformations. The local parameters, or state variables, are implicitly eliminated, and second-order solutions are rigorously computed in the model parameter space (rank #4). The algorithm is further distin- guished from earlier works by the inclusion of approximation procedures that yield very good starting values. Additionally, a strong connection between the Helmert transformation and the total least-squares problem is established, and a fixed point method is suggested.
Article
The Pythagorean hodograph (PH) curves are polynomial parametric curves {x(t),y(t)} whose hodograph (derivative) components satisfy the Pythagorean condition x '2 (t)+y '2 (t)≡σ 2 (t) for some polynomial σ(t). Thus, unlike polynomial curves in general, PH curves have arc lengths and offset curves that admit exact rational representations. The lowest-order PH curves that are sufficiently flexible for general interpolation/approximation problems are the quintics. While the PH quintics are capable of matching arbitrary first-order Hermite data, the solution procedure is not straightforward and furthermore does not yield a unique result – there are always four distinct interpolants (of which only one, in general, has acceptable “shape” characteristics). We show that formulating PH quintics as complex-valued functions of a real parameter leads to a compact Hermite interpolation algorithm and facilitates an identification of the “good” interpolant (in terms of minimizing the absolute rotation number). This algorithm establishes the PH quintics as a viable medium for the design or approximation of free-form curves, and allows a one-for-one substitution of PH quintics in lieu of the widely-used “ordinary” cubics.
Article
A one-parameter family of spirals that can match planar, two-point G1 Hermite data is presented. These spirals can be used as an alternative to the biarc, which is also a one-parameter family of curves that can match two-point G1 Hermite data. Some suggestions on choosing the free parameter of the family of spirals is given. It is shown that there is a unique G1 Hermite interpolating spiral that passes through a given point in an allowable region. Three examples of the use of these spirals are given: curve completion with spirals, design with spirals, and approximation of the clothoid by spirals.
Article
Highway and railway designers use clothoid splines (planar {G^2} curves consisting of straight line segments, circular arcs, and clothoid segments) as center lines in route location. This paper considers the problem of finding a clothoid spline transition spiral which joins two given points and matches given curvatures and unit tangents at the two points. Conditions are given for the existence and uniqueness of the clothoid spline transition spirals, and algorithms for finding them are outlined.
Article
It can be difficult to evaluate Lommel functions Un(u,v) and Vn(u,v) when u and v are both large. Here, we present an accurate means to evaluate such functions using contour integration. The approach suggests the potential for developing analogous techniques in diffraction calculations for many optical systems.