Curves Trajectories Anglais4(1)

Curves for Path Planning
Curves-Interpolation
Saïd MAMMAR
Univ. Evry Paris-Saclay
October 2023
S. MAMMAR (Institute) Curves 10/23 1 / 66

The objective is initially to …nd a polynomial form passing through (n + 1)
points. This form will be de…ned either over the entire domain or in pieces.
In addition to ensuring the continuity of the function, we will also impose,
as needed, the continuity of its derivative and successive derivatives.

Suppose we are given (n + 1) points de…ned by the pairs (xi , yi ), where
(i = 0, 1, ..., n). It can be proven that there exists one and only one
polynomial of degree n:
P (x ) = a0 + a1 x + a2 x 2 + ... + an x n (1)
verifying P (xi ) = yi
Preuve:Let’s suppose that there exist two polynomials, P (x ) and Q (x ),
satisfying this property. Then the polynomial with a degree at most equal
to n
R (x ) = P (x ) Q (x ) (2)
It vanishes at (n + 1) distinct points xi . However, the polynomial R (x ),
being of degree at most n, cannot have more than n distinct roots. We
can therefore conclude that R (x ) is zero, and thus, P (x ) = Q (x ).

Lagrange interpolation
Suppose we are given two points (x0 , y0 ) and (x1 , y1 ). The polynomial
interpolation of these two points is a polynomial of degree 1, which
corresponds to the line passing through these two points. This polynomial
is given by:
P (x ) = y0 + g (x )(y1 y0 ) (3)
with
x x0 0 si x = x0
g (x ) = = (4)
x x1 1 si x = x1
What is the polynomial in the general case where we have (n + 1) points
(n > 1)?

General case
Suppose we are given (n + 1)points de…ned by the pairs (xi , yi ), for

(i = 0, 1, ..., n). We start by de…ning (n + 1) polynomials of degree n,
li (x ) for (i = 0, 1, ..., n)n, l_{i}(x) for (i=0,1,...,n), satisfying
1 si i = j
li (xj ) = δij =
0 si i 6= j
It is easy to see that these polynomials are given by

n
x xj
li (x ) = ∏ xi xj
(5)
j =0,j 6=i
The (n + 1) polynomials li (x ) form a basis for the vector subspace of

polynomials of degree n.

In fact if
λ0 l0 (x ) + λ1 l1 (x ) + ... + λn ln (x ) = 0 (6)
then
λ0 l0 (xk ) + λ1 l1 (xk ) + ... + λk lk (xk ) + ... + λn ln (xk ) = 0 () λk lk (xk ) = 0

(7)
This results in
λk = 0 (k = 0, 1, 2, ...n) (8)

Example
In the case n = 2, one obtains
x x1 x x2
l0 (x ) = (9)
x0 x1 x0 x2
x x0 x x2
l1 (x ) = (10)
x1 x0 x1 x2
x x0 x x1
l2 (x ) = (11)
x2 x0 x2 x1
These polynomials appear on the …gure 5 for
x0 = 0, x1 = 1, x2 = 2
1 2 3 1 2 1
l0 = x x + 1, l1 = x 2 + 2x, l2 = x x
2 2 2 2

Polynômes de Lagrange pour 3 points
We then verify that the polynomial

n
P (x ) = ∑ yi li (x ) (12)
i =0
is the interpolation polynomial since it is of degree n and satis…es

P (xi ) = yi for (i = 0, 1, ..., n). This polynomial is called the Lagrange
interpolation polynomial.
Algorithm for calculating the coe¢ cients
It can be shown that the previous polynomial can be expressed simply as

n n n n
x xj
∑ yi ∏ = ∑ yi λi ∏
(n )
Pn (x ) = (x xj ) (13)
i =0 j =0,j 6=i
xi xj i =0 j =0,j 6=i
(n )
The coe¢ cients λi can be calculated recursively using the following
formula
(n +1 ) (n )
λi = λi /(xi xj ) (i = 0, 1, ..., n) (14)
Furthermore, we can demonstrate the following equality:
n
∑ λi
(n )
=0 (15)
i =0

(n )
The algorithm for calculating the λi coe¢ cients can be summarized as
follows.:
(0 )
Start λ0 =1
For k = 1to n
For i = 0 to k 1
(k ) (k 1 )
λi = λi / (x i x k )
end
(k ) (k )
λk = ∑ki =01 λi
end
The coe¢ cients ai of the polynomial Pn (x ) can then be derived from the
(n )
coe¢ cients λi .

Example
Figure 8 shows the polynomial interpolation of the function
x 1
f (x ) = log x 2 (16)
x
at points [1, 2, 4, 8, 10]
S. MAMMAR (Institute) PolynomialCurves

interpolation 10/23 11 / 66
Remark
When determining an interpolation polynomial, one hopes that the
polynomial still approximates the function outside the interpolation domain
(extrapolation). The plot of the preceding function and its interpolation
polynomial on the interval [0.5, 12] shows that this is not the case. There
is a rapid divergence of the polynomial compared to the function.
Interpolation / Extrapolation polynomiale

The Newton interpolation polynomial obtained for (n + 1) points
x0 , x1 , ..., xn has the form:
Pn (x ) = c0 + c1 (x x0 ) + c2 (x x0 )(x x1 ) + ... + cn (x x0 )(x

x1 )...(x
(17)
You should not forget that the Newton polynomial will be equal to the
Lagrange polynomial (the interpolation polynomial being unique). They
only di¤er in terms of their form and the algorithm for calculating the
coe¢ cients.
The coe¢ cients ci can be calculated by solving the system of linear
equations in lower triangular form. The solution is straightforward using
simple substitution, as shown in the equations below...
2
Pn (x0 ) = c0 = y0
6 Pn (x1 ) = c0 + c1 (x1 x0 )) = y1
6
4 ....
Pn (xn ) = c0 + c1 (xn x0 ) + c2 (xn x0 )(xn x1 ) + ... + cn (xn x0 )...(xn

In any case, another method allows for an iterative algorithm based on the
number of points. This is a signi…cant di¤erence from the Lagrange
polynomial, where if you add a point, you are required to recompute all
the coe¢ cients of the polynomial.
Let’s …rst note that the term ci depends only on the xk with (k = 0, ...., i ).
c0 = y0 (18)
y1 c0
c1 = (19)
x1 x0
y2 y0 c1 (x2 x0 )
c2 = (20)
(x2 x0 )(x2 x1 )
y2 y0 y1 y0
= (21)
(x2 x0 )(x2 x1 ) (x1 x0 )(x2 x1 )
(y2 y0 )(x1 x0 ) (y1 y0 )(x2 x0 )
= (22)
(x2 x0 )(x2 x1 )(x1 x0 )
y1 y0 y2 y1
x1 x0 x2 x1
= (23)
(x2 x0 )

The previous formula generalizes easily, so the Newton interpolation
polynomial can be calculated as follows by constructing the table:
x0 f (x0 )
f [x0 , x1 ]
x1 f (x1 ) f [x0 , x1 , x2 ]
f [x1 , x2 ] f [x0 , x1 , x2 , x3 ]
x2 f (x2 ) f [x1 , x2 , x3 ] : :
f [x2 , x3 ] : : : :
x3 f (x3 ) : : : : :
: : : : : :
: : : : f [xn 3 xn 2 , xn 1 , xn ]
xn 1 f (xn 1 ) : f [xn 2 , xn 1 , xn ]
f [xn 1 , xn ]
xn f (xn )
avec
f (xi ) = yi , ci = f [x0 , x1 , ..., xi ] (24)

S. MAMMAR (Institute) f (xj ) f (xi ) Curves f [xj , xk ] f [xi10/23
, xj ] 15 / 66
Soit une fonction dé…nie sur [a, b ] et (n + 1) points (xi , yi = f (xi )). Que
peut-on dire de la di¤érence f (x ) P (x ), où P (x ) est le polynôme
d’interpolation de f ? Nous avons aux points d’interpolation :
P (xi ) = f (xi ) = yi (26)
Cette di¤érence peut être faible ou grande comme le montre la …gure ??

Example
1
f (x ) = x x 2 [1, 3] (27)
1
x0 = 1, f (1) = 1 x1 = 3, f (3) = 3 (28)
alors
1 4
P (x ) = x+ (29)
3 3
In this case, one can say that the di¤erence is small, but it is worth noting
that the polynomial P (x ) is also an interpolation polynomial for any
function that satis…es condition 28, and the di¤erence can then be
signi…cant (…gure 2).
Di¤erence between the function and the interpolation polynomial

One can therefore say that the smoother (or more regular) the function is,
the better the approximation will be. The following theorem provides the
value of this error.
Theorem
If
f 2 C n +1 [a, b ]
(30)
a x0 x1 ... xn b
then
n
1
f (x ) P (x ) = ∏
(n + 1 ) ! i =0
(x xi )f (n +1 ) (ξ ) (31)
where
a min(x0 , x ) < ξ < max(x, xn ) b (32)
Preuve: Note developed here

Example
The application of this theorem in the case of linear interpolation with
x0 = a x1 = b (33)
then
x b x a
P (x ) = f (a ). + f (b ). (34)
a b b a
leads to
(x
a)(x b ) 00
f (x ) P (x ) = f (ξ ) (35)
2
Therefore, it is necessary to …nd an upper bound for jf 00 (ξ )j

According to the previous theorem,
n
f (n +1 ) ( ξ )
jf (x ) P (x )j max
x 2[a,b ]
max
(n + 1)! x 2[a,b ] ∏ (x xi ) (36)
i =0
The error results from two terms:

The term involving the (n + 1)th derivative depends on the function
to be interpolated, and therefore, nothing can be changed about it. It
will simply be smaller when the function is more regular.
On the other hand, the second term depends on the choice of xi
n
max
x 2[a,b ] i =0
∏ (x xi ) (37)
The problem is as follows: how to choose the xi to make this quantity

minimal? It can be shown that this quantity is minimal for the points
xi that nullify the Chebyshev polynomials, in other words
b a n i b+a
xi = cos π + (38)
2 n 2
Example
The …gure below illustrates the case of the Runge function with regularly
spaced points and points following a Chebyshev distribution. The degree-6
polynomial in dashed lines is obtained at the interpolation points
x0 = 5, x1 = 3, x2 = 1, x3 = 0, x4 = 1, x5 = 3, x6 = 5
The dashed line polynomial is obtained with the point distribution given by
the formula 38 for a = 5, b = 5 et n = 6. The point distribution is
shown in the …gure 7
x00 = 5, x10 = 4.3301, x20 = 2.5, x30 = 0, x40 = 2.5, x50 = 4.3301, x60 = 5

Runge function with uniform distribution and Chebychev distribution of
the points xi

When performing interpolation, one generally hopes that the interpolation
continues to work outside the points used to calculate the polynomial.
However, as mentioned earlier, polynomial interpolation rapidly diverges
outside the interpolation points. This is also true at the boundaries of the
domain, as in the case of the Runge function, where oscillations appear
when the number of points is greater than 10. A better distribution of
points (Chebyshev distribution) can reduce the error, but oscillations may
still persist. Additionally, the use of high-degree polynomials should be
avoided as it introduces numerical instabilities.
Spline interpolation, a piecewise polynomial interpolation, is often
preferred over traditional polynomial interpolation in such cases. Splines
can change their shape at speci…c points (xi , yi ) known as nodes or knots.

De…nition
Given (n + 1) points (xi , yi ) , the spline S (x ) is de…ned by
8
>
> S0 ( x ) , x 2 [x0 , x1 ]
<
S1 ( x ) , x 2 [x1 , x2 ]
S (x ) = (39)
>
>
:
Sn 1 ( x ) , x 2 [xn 1 , xn ]
De…nition
When the polynomials Si (x ) are of degree 1, it’s referred to as linear spline
interpolation. When they are of degree 2, it’s called quadratic spline
interpolation. If they are of degree 3, it’s referred to as cubic spline
interpolation.

Given (n + 1) points (xi , yi ), we search a linear spline S (x ) with the form
8
>
> S0 (x ) = a0 x + b0 , x 2 [x0 , x1 ]
<
S1 (x ) = a1 x + b1 , x 2 [x1 , x2 ]
S (x ) = (40)
>
>
:
Sn 1 (x ) = an 1 x + bn 1 , x 2 [xn 1 , xn ]
verifying the interpolation condition
S (xi ) = yi (41)

The …rst question that needs to be answered is: Is the spline S (x ) unique?
The determination of S (x ) requires the calculation of 2n coe¢ cients ai
and bi , (i = 0, 1, ..., n 1). For that, we have 2n equations :
(n + 1) equations of interpolation (i = 0, 1, ..., n)
Si (xi ) = yi (42)
yi = ai xi + bi (43)
(n 1) equations of continuity for the spline (i = 0, 1, ..., n 2)
Si (xi +1 ) = Si +1 (xi +1 ) (i = 0, 1, ..., n 2) (44)
ai xi +1 + bi = yi +1 (45)
We can thus uniquely deduce
yi +1 yi
ai = , (i = 0, 1, ..., n 1) (46)
xi +1 xi
bi = yi ai xi , (i = 0, 1, ..., n 1) (47)

We have seen previously that the linear spline only guarantees the
continuity of the function S (x ). If we want S (x ) to be continuously
di¤erentiable, we must choose polynomials Si (x ) with a degree of at least
2.
De…nition
Suppose we are given (n + 1) points de…ned by the pairs (xi , yi ), where
(i = 0, 1, ..., n). Quadratic spline interpolation is equivalent to
approximating on each interval [xi , xi +1 ] with a second-degree polynomial.
Continuity of the values of the polynomials and their derivatives is
imposed at di¤erent points.
8
>
> S0 (x ) = a0 x 2 + b0 x + c0 , x 2 [x0 , x1 ]
< 2
S1 (x ) = a1 x + b1 x + c1 , x 2 [x1 , x2 ]
S (x ) = (48)
>
>
:
Sn 1 (x ) = an 1 x 2 + bn 1 x + cn 1 , x 2 [xn 1 , xn ]

Example
The function 8
< x2 x 0
f (x ) = x2 0 x 1
:
1 2x x 1
is a quadratic spline. In fact:
It is de…ned on ] ∞, +∞[
It is continious on ] ∞, +∞[
Its derivative is continious on ] ∞, +∞[, and
* f 0 (0 ) = f 0 (0+ ) = 0
* f 0 (1 ) = f 0 (1+ ) = 2

Is the spline S (x ) unique? The determination of S(x) requires the
calculation of the 3n coe¢ cients ai and bi , where (i = 0, 1, ..., n 1). For
this, we have only (3n 1) equations:
(n + 1) interpolation equations (i = 0, 1, ..., n)
Si (xi ) = yi (49)
yi = ai xi2 + bi x + ci (50)
(n 1) equations of spline continuity
Si (xi +1 ) = Si +1 (xi +1 ) (i = 0, 1, ..., n 2) (51)
(n 1) equation of derivative continuity
Si0 (xi +1 ) = Si0+1 (xi +1 ) (i = 0, 1, ..., n 2) (52)

To solve the problem, it is necessary to introduce an additional condition.
For this purpose, let’s de…ne the auxiliary variable
zi = S 0 (xi ), (i = 0, 1, ..., n), and rewrite Si (x ) in the form
Si ( x ) = ai ( x xi )2 + bi (x xi ) + ci (53)
From the interpolation conditions, it is clear that, in this form, the ci are
uniquely determined
ci = Si (xi ) = yi (i = 0, 1, ..., n 2) (54)

In addition, by de…nition
Si0 (xi ) = bi = zi (55)
which determines the bi . By now writing the conditions of continuity of

the derivative, we obtain
2ai (xi +1 xi ) + bi = 2ai +1 (xi +1 xi +1 ) + bi +1 (i = 0, 1, ..., n 2)

(56)
By using equation 55, one obtains the coe¢ cients ai
zi +1 zi
ai = (57)
2 (xi +1 xi )
Si (x ) take thus the form

zi +1 zi
Si ( x ) = (x xi )2 + zi (x xi ) + yi (58)
2 (xi +1 xi )

Now we just need to calculate the zi . To do that, we write the continuity
equations of the spline at the point xi +1
zi +1 zi
Si (xi +1 ) = (xi +1 xi )2 + zi (xi +1 xi ) + yi
2 (xi +1 xi )
zi +1 zi
= (xi +1 xi ) + zi (xi +1 xi ) + yi
2
= yi +1 (59)
We deduce that
zi +1 = 2 xyii ++11 yi
xi zi (i = 0, 1, ..., n 1) (60)
And we naturally see the additional condition that will be an initialization
condition for the recurrence on the zi . Therefore, we need to choose...
z0 = S00 (x0 ) (61)
Remark
On peut donner n’importe quelle valeur à z0 , mais généralement on choisit
z0 = 0. Notons qu’au lieu de …xer z0 , on aurait pu …xer zn et faire une
récurrence descendante.
Figures 7 and 7 show the results obtained for the Runge function at points
[ 5, 3, 1, 1, 3, 5] and[ 5, 3, 0, 1, 1, 3, 5], respectively. It can be
observed that adding the point 0 improves the results around 0 but
degrades the results in the interval [2, 5]. Indeed, there are signi…cant
amplitude oscillations.
Quadratic splines at points [ 5, 3, 1, 1, 3, 5]

Interpolation with cubic splines (degree-3 polynomials) leads to the
continuity of the spline, its …rst derivative, and its second derivative. Let’s
de…ne yi00 = S 00 (xi ), and write Si (x ) in the form...
Si ( x ) = ai ( x xi )3 + bi (x xi )2 + ci (x xi ) + di (62)
With 4n coe¢ cients to determine, this time we have only (4n 1)

equations available: the (3n 1) conditions identical to those of the
quadratic spline and the (n 1) additional conditions for the continuity of
the second derivative. Therefore, two additional conditions need to be
speci…ed. It is common to impose the values of the second derivatives at
the points x0 and xn .
y000 = S0 (x0 ) (63)
and
yn00 = Sn 1 (xn ) (64)

The interpolation conditions at the points xi give
di = yi (65)
The continuity conditions of the …rst and second derivative lead to

1 1 00
ai = (y 00 yi00 ), bi = y (66)
6hi i +1 2 i
1 1
ci = (yi +1 yi ) hi (yi00+1 + 2yi00 ) (67)
hi 6
with hi = xi +1 xi
It is now su¢ cient to calculate the yi00 , (i = 0, 1, ..., n 2).
Writing the continuity of the spline using the previously mentioned
equalities results in the system of equations with (n 2) unknowns to
obtain the yi00 , (i = 1, 1, ..., n 2).

2
2(h0 + h1 ) h1 0 . . . 0
6 h1 2(h1 + h2 ) h2 0 . . .
6
6 0 h2 2(h2 + h3 ) h3 0 . .
6
6 0 0 h3 . . . .
6
6 . . . . .
6
6 . . . hn 3
6
4 0 0 . . . hn 3 2(hn 3 + hn 2 )
0 0 0 . . . hn 2 2(h
2 3 2 6 6 00 3
y 00 (1) h 0 (y1 y0 ) + h1 (y2 y1 ) h0 y0
6 y (2) 7 6 6
y1 ) + h62 (y3 y2 ) 7
h 1 (y2
00
6 7 6 6 6 6
7
7
6 y (3) 7
00 ( y y ) + ( y y )
6 7 6 6 h2 3 2 h3 4 3 7
7
6 . 7 6 . 7
6 7=6 7
6 . 7 6 . 7
6 7 6 7
6 7
6 . 7 6 6
. 7
7
4 y 00 (n 2) 5 4 6
( yn 2 y n 3 ) + 6
( y n 1 yn 2 ) 5
hn 3 hn 2
y 00 (n 1) 6
h n 2 (yn 1
6
yn 2 ) + hn 1 (yn yn 1 ) hn 1 yn 00

The …gure 4 shows an example of a cubic spline obtained for the Runge
function at the interpolation points...
x0 = 5, x1 = 3, x2 = 1, x3 = 0, x4 = 1, x5 = 3, x6 = 5
It should be noted that in this case, the oscillations are greatly reduced
compared to polynomial interpolation.
Remark
Quand on choisit
S. MAMMAR (Institute) y000 =Curves
yn00 = 0 10/23 (68)
37 / 66
Theorem (Optimality of the natural cubic spline)
Given f a C 2 function of the interval [a, b ] , and (n + 1) points
a = x0 , x1 , ..., xn = b (69)
with
f (a) = y0 , y1 , ..., yn = f (b ) (70)
then the natural cubic spline S (x ) interpolating the function f at points
(xi , yi ) veri…es
Z b Z b
00 2 2
S (x ) dx f 00 (x ) dx (71)
a a
In other words, the average curvature of S is lower than that of f .

Parametric Curves I
Path planning is a fundamental aspect of autonomous UAS guidance and

deployment and an essential component for reaching autonomy level 4 and
higher levels.
The inputs of this path planning algorithm are
the initial con…guration and velocity: xi , yi , zi , γi , χi , Vi
the …nal con…guration and velocity: xf , yf , zf , γf , χf , Vf .
Dubins curves, Cartesian polynomials, β splines, Pythagorean
hodographs, η splines

Cartesian polynomials I
Lowest degree polynomial curves that allow in‡ection points (where

curvature is zero) so they are suitable for the composition of curves
with second order geometric continuity.
Interpolating via Cartesian polynomials versus 0 s 1
x (s ) = s 3 xf (s 1)3 xi + αx s 2 (s 1) + βx s (s 1)2
y (s ) = s 3 yf (s 1)3 yi + αy s 2 (s 1) + βy s (s 1)2 (72)
z (s ) = s 3 zf (s 1)3 zi + αz s 2 (s 1) + βz s (s 1)2

Cartesian polynomials II
it is also necessary to impose the additional boundary conditions :
x 0 (0) = cos γi cos χi x 0 (1) = cos γf cos χf
y (0) = cos γi sin χi and y 0 (1) = cos γf sin χf Deriving once the
0
z 0 (0) = sin γi z 0 (1) = sin γf

equations ((72)), the following relations are obtained :
x 02 xf 3(s 1)2 xi + αx s (3s 2) + βx (3s 1)(s 1)

y 02 yf 3(s 1)2 yi + αy s (3s 2) + βy (3s 1)(s 1) (73)
z 02 zf 3(s 1)2 zi + αz s (3s 2) + βz (3s 1)(s 1)
and deriving twice the equations (72),
x 00 (s ) = 6sxf 6(s 1)xi + αx (6s 2) + βx (6s 2)

y 00 (s ) = 6syf 6(s 1)yi + αy (3s 2) + βy (6s 2) (74)
z 00 (s ) = 6szf 6(s 1)zi + αz (6s 2) + βz (6s 2)
Geometric inputs that drive the aircraft along the Cartesian path
Cartesian polynomials III
With the boundary conditions,

αx = cos γf cos χf 3xf βx = cos γi cos χi + 3xi
αy = cos γf sin χf 3yf and βy = cos γi sin χi + 3yi
αz = sin γf 3zf βz = sin γi + 3zi
dz /ds
Flight path angle γ = arcsin V (s )
dx dy
Heading angle χ = arctan2 ds , ds + kπ; k = 0, 1
(k = 0) or backward (k = 1). If the initial orientation is assigned,
only one of the choices for k is correct.
This approach can be easily generalizable to a fourth or …fth
polynomial. However the main drawback is a complicated formulation
of the curvature and the torsion making control of smoothness (twists
and cusps) a di¢ cult task.

Clothoids I
The curvature of a clothoid curve is proportional to the length of the

path as a curve element κ (s ) = σc s, where σc is a real constant
called the sharpness of the Clothoid. In the xy plane, this is a well
known path in mobile robotics: clothoid curve. An elementary path is
considered as a concatenation of two equal piecewise clothoids.
h i
κ (s ) = σ c s 8s 2 0, 2`
h i (75)
κ (s ) = σc (` s ) 8s 2 2` , `

Clothoids II
Furthermore, two con…gurations in the plane can be obtained with
two di¤erent elementary paths to form a bi-elementary path. In
particular to link two successive way-points
Pi = (xi , yi , χi )T , Pf = (xf , yf , χf )T , it is necessary to calculate the
split con…gurations Ps = (xs , ys , χs )T which is a symmetric
con…gurations with respect to the start and end con…gurations. For
symmetrical con…gurations, the following relations are veri…ed
κ (s ) = 4ζs
` `
κ 2 = κ 2 χ(s ) = 2ζs 2
`
=χ ` ζ Rs
χ 2 2 = 2 ) x (s ) = sin(2ζu 2 )du (76)
R
0.5 0
D (ζ ) = 2 sin(2ζu 2 )du Rs
0 y (s ) = cos(2ζu 2 )du
0
1 1
for 2 s 2.

Clothoids III
For non symmetrical con…gurations, one intermediate con…guration

symmetrical to both initial and …nal con…gurations must be found. It
belongs to a circle. Then two portions of clothoids must be joined.
For a real clothoid, the following relations are used
1 homothetie ` = d
D (ζ )
2 rotation χ(s ) ! χ(s ) + ς
3 translation x (s ) ! x (s ) + x0 and y (s ) ! y (s ) + y0
The loci of split con…gurations joining the start and end con…gurations
with a bi-elementary path is a circle. Therefore, it exists an in…nite
set of solutions ( bi-elementary paths with di¤erent lengths and
curvatures) joining start and end con…gurations. In particular, the
shortest bounded curvature path satisfying κ κ max is of interest.

Cubic Spirals I
For symmetrical con…gurations
R
0.5
3
D (ζ ) = 2 cos ζu 2 2u 2 du
0
d
`= D (ζ )
6ζ `2
κ (s ) = `3 4 s2
χ (s ) 6ζ
= `3 `2 s3 (77)
4 3
Rs
x (s ) = cos(χ(u ))du
0
Rs
y (s ) = sin(χ(u ))du
0
The locus of intermediate con…gurations for non symmetrical initial

and …nal con…gurations are on

Cubic Spirals II
1 if Pi //Pf , parallel con…gurations then it is on the line
(x xi )(y yf ) = (x xf )(y yi )
2 it not, the loci is a circle whose center is given by
xi + xf + c (yi + yf ) yi + yf + c (xi + xf )
Pc = ,
2 2
χi + χf 1
where c = tan 2

Pythagorean Hodograph I
Pythagorean Hodograph have a polynomial arc length function.

Double Pythagorean Hodograph if the Frenet frame, the curvature
and the torsion have all a rational dependence on the curve parameter.
2 D Pythagorean Hodograph: for some polynomial σ(t )
ẋ 2 (t ) + ẏ 2 (t ) = σ2 (t ) (78)
Path length s and parametric speed ṡ of a parametric curve
Zt2 Zt2 q
s= kṙ (t )k dt = ẋ 2 (t ) + ẏ 2 (t )dt
t1 t1
If the sum of square of the tangents ẋ (t ),ẏ (t ) could be represented by

perfect square of a single polynomial, it leads to two advantages:

Pythagorean Hodograph II
ẋ (t ) = w (t ) u 2 (t ) v 2 (t )
Hodograph of the curve r (t ) : or
ẏ (t ) = 2w (t )u (t )v (t )
p
ẋ 2 (t ) + ẏ 2 (t ) = σ(t ) = w (t ) u 2 (t ) + v 2 (t ) , u (t ), v (t ), w (t ) and
σ(t ) are non zero real polynomials, satisfying gcd (u (t ), v (t )) = 1
Zt2
s= σ(t )dt (79)
t1
Depending on the order of the polynomials u (t ), v (t ), w (t ), the

Pythagorean Hodograph curve can be either cubic, quartic or quintic.
Unit tangent T , unit normal N, curvature κ, O¤-set curve r0 (t ) of the
curve r (t ) at a distance d
(u 2 v 2 ,2uv ) (2uv ,v 2 u2 )
T = u 2 +v 2
N= u 2 +v 2
2 (u v̇ v u̇ ) (80)
κ= (u 2 +v 2 )
r0 (t ) = r (t ) dN (t )

Pythagorean Hodograph III
3 D Pythagorean Hodograph, it is necessary and su¢ cient that
ẋ (t ) = u 2 (t ) + v 2 (t ) p 2 (t ) q 2 (t )
ẏ (t ) = 2 (u (t )q (t ) + v (t )p (t ))
(81)
ż (t ) = 2 (v (t )q (t ) u (t )p (t ))
σ (t ) = u 2 (t ) + v 2 (t ) + p 2 (t ) + q 2 (t )
If u (t ), v (t ), p (t ), q (t ) are all constants, the hodograph is a single

point specifying a uniformly parameterized straight line.
Cubic Pythagorean Hodograph correspond to segments of non circular
helices with a constant τκ , characterized by geometrical constraints on
these Bezier control polygons.
3
3
r (t ) = ∑ bk k
(1 t )3 k k
t t 2 [0, 1] (82)
k =1

Pythagorean Hodograph IV
To guarantee su¢ cient shape ‡exibility, …fth order Pythagorean

Hodograph curve may be employed, in Bernstein-Bezier form
5
5
r (t ) = ∑ bk k
(1 t )5 k k
t t 2 [0, 1] (83)
k =1
where bk = (xk , yk , zk ) are control points, whose vertices de…ne the

control polygon or Bezier polygon, t is a parameter and k = 0...5. As
Z n +1
n 1
n + 1 i =∑
(1 t )n k k
t dt = (1 t )n +1 i i
t (84)
k
k +1
and
n n!
= (85)
k k!(n k )!

Pythagorean Hodograph V
For Pythagorean Hodograph …fth orders, jr 0 r 00 j = σ2 ι and

2 2
ι = (up 0 u 0 p + vq 0 v 0 q ) + (uq 0 u 0 q + v 0 p vp 0 ) . For
Pythagorean Hodograph …fth order, (r 0 r 00 ) .r 000 is of degree 6 while
σ, ι are both in degree 4 in t. For a Pythagorean Hodograph curve with
ι = ω 2 , the Frenet-Serret frame vectors and the curvature and torsion
functions are given by the rational expressions:
r0 σr 00 σ0 r 0 r0 r 00
T = N= B= (86)
σ σω σω
ω (r 0 r 00 ) .r 000
κ= τ= (87)
σ2 σ2 ω 2
Hence, the Pythagorean Hodograph curves may be regarded as
the complete set of polynomial curves that have rational Frenet
frame.

Pythagorean Hodograph VI
The four control points of the Bezier polygon are calculated by …rst
order Hermite interpolation
b0 = (xi , yi , zi )
b5 = (xf , yf , zf )
(88)
b1 = b0 + 51 (cos χi cos γi , sin χi cos γi , sin γi , )
b4 = b5 51 (cos χf cos γf , sin χf cos γf , sin γf , )
Find the control points b_2, b_3. Polynomials curves are
u (t ) = u0 (1 t )2 + 2u1 (1 t ) + u2 t 2
v (t ) = v0 (1 t )2 + 2v1 (1 t ) + v2 t 2
(89)
p (t ) = p0 (1 t )2 + 2p1 (1 t ) + p2 t 2
q (t ) = q0 (1 t )2 + 2q1 (1 t ) + q2 t 2

Pythagorean Hodograph VII
Pythagorean hodograph
1 Inputs : Endpoints P0 , P1 and endpoint derivatives (velocity

vectors) V0 , V1 . All these data are considered as complex
numbers

Pythagorean Hodograph VIII
2 Outputs : PH quintic p (τ ) de…ned over the interval [0, 1]
and interpolating the input
3 Transform the data to a certain canonical position
V0 V1
Ṽ0 = Ṽ1 = (90)
P1 P0 P1 P0
4 Compute the control points of the so-called preimage

p +
w0 = Ṽ0
p +
w2 = Ṽ1
q +
1
w1 = 4 3(w0 + w2 ) + 120 15(Ṽ0 + Ṽ1 ) + 10w0 w2
(91)
p+
where denotes the square root with the positive real
part.

Pythagorean Hodograph IX
Pythagorean hodograph

Pythagorean Hodograph X
5 Compute the control points of the hodograph i.e. the …rst
derivative and transform it back to the original position
h0 = w02 (P1 P0 )
h1 = w0 w1 (P1 P0 )
h2 = ( 23 w12 + 13 w0 w2 )(P1 P0 ) (92)
h3 = w2 w1 (P1 P0 )
h4 = w22 (P1 P0 )
6 Compute the control points of the PH interpolant

1
p0 = P0 pi = pi 1 + hi 1 (93)
5
and return the PH curve in Bernstein-Bezier representation
5
5
p (τ ) = ∑ pi i
τ i (1 τ )5 i
(94)
i =0

The set of equations to be solved for the control points b2 , b3 results
in four solutions. Among these four paths, only one has an acceptable
shape that is without twists and cusps.
if u (t ) = u0 (1 t ) + u1 t, and similarly for v (t ), p (t ), q (t ) , the
control points of spatial PH cubics are of the form
0 2 1
u + v02 p02 q02
1@ 0
P1 = P0 + 2 (u0 q0 + v0 p0 ) A (95)
3
2 (v0 q0 + u0 p0 )
0 1
u0 u1 + v0 v1 p0 p1 q0 q1
1
P2 = P1 + @ u0 q1 + q0 u1 + v0 p1 + p0 v1 A (96)
3
v0 q1 + q0 v1 u0 p1 p0 u1
0 2 1
u1 + v12 p12 q12
1
P3 = P2 + @ 2 (u1 q1 + v1 p1 ) A (97)
3
2 (v1 q1 + u1 p1 )

The point corresponding to the integration constants, being freely
chosen. Degree-n spatial PH curves possess 2n + 5 degrees of
freedom. However, ten of these freedoms correspond to a choice of
origin and orientation of the coordinate axes in R3 . The pseudo code
of this approach is presented in algorithm ?? in page ??.
Its parametric speed is a polynomial, possibly piecewise
p 0 (τ ) = kw (τ )k2 jp1 p0 j (98)
where
w ( τ ) = w0 ( 1 τ )2 + 2w1 τ (1 τ ) + w2 τ 2 (99)
is the so-called pre-image.

β Splines I
Cubic splines are degree three polynomials that connect to adjoining

spline polynomials, provide smoothness and continuity by ensuring
that the value of the function, its …rst and second derivative match
with those of neighboring splines.
It can assume a collision-free path sphere volume as a safety radius of
a selectable radius, or insert other geometries into spline segments.
A univariate polynomial spline is de…ned as a piecewise polynomial
function. A polynomial spline S : [a, b ] ! R consists of polynomial
pieces Pi : [τ i , τ i +1 ] ! R where a = τ 0 < τ 1 < ... < τ k 1 = b that
is
S (τ ) = P0 (τ ) τ0 τ τ1
S (τ ) = P1 (τ ) τ1 τ τ2
(100)
...
S (τ ) = Pk 2 (τ ) τk 2 τ τk 1

β Splines II
τ = [τ 0 , ..., τ k 1] is a knot vector for the spline.

A cubic spline is applied piecewise for each segment. For each
segment k and each degrees of freedom i = [x, y , z ]
Si ,k (t ) = ai ,k + bi ,k (τ τ k ) + ci ,k (τ τ k )2 + di ,k (τ τ k )3 (101)
Si ,k (t ) represents the spline for the dimension i in segment k for a

speci…c τ. The n waypoints Pj (x, y , z ), j = 1, ..., n are the support
points of the spline interpolation. A number of n points causes
k = (n 1) segments. Each segment contains three spline functions
including four parameters to be determined by four relations. The
following requirements for each segment boundary are de…ned:
Consecutive spline segments must be connected with each other.
The …rst and second derivative must be continuous

β Splines III
Q (s ) = (1 s )2 P0 + 2s (1 s )P1 + s 2 P2 0 s 1 (102)
The total range is divided into N subintervals each of length ∆R.

(N+1) data points and (N-1 ) interior points exist. Let a cubic spline
on the i th interval Ri R Ri +1 , be de…ned as
Hi (R ) = ai R 3 + bi R 2 + ci R + di (103)
which expresses one of the degrees of freedom (x,y or z) in that

interval. h1 , h2 , ..., hN 1 at the knot points are the optimizing
parameters; h0 and hN and the gradients at R1 and RN 1 .
Aircraft constraints are enforced not only at the knot points
but at intermediate points along the spline also.

β Splines IV
Spline continuity and smoothness constraints
Hi 1 (Ri ) = Hi (Ri ) (104)
dHi 1 (R ) dHi (R )
jR i = jR i (105)
dR dR
d 2 Hi 1 ( R ) d 2 Hi ( R )
j R = jR i (106)
dR 2 i
dR 2
Boundary conditions of the …rst and last splines
H0 (0) = h0
(107)
HN 1 (RN ) = hN
dH0 (R )
j R 1 = S1 (108)
dR

β Splines V
dHN 1 (R )
j R N 1 = SN 1 (109)
dR
4N constraints exist to compute the N splines; the splines having 4
coe¢ cients each. The optimization problem is to minimize
Z RF
J= e 2 dR (110)
0
where RF is the total range value (the total distance to be travelled).

The error e has to be positive.
dh dh/dt
Slope slope = dR = V with slopemin slope slopemax .
dslope d h/dt 2
2
Curvature κ = dR = V2
with the constraint κ min κ κ max .
dκ
Kink p = dR with pmin p pmax .

β Splines VI
A sequential quadratic programming (SQP) can be used to solve this
problem. At each major iteration, an approximation of the Hessian of
the Lagrangian function is made which is then used to form a search
direction for a line search procedure. Each Quadratic Programming
(QP) suproblem minimizes a quadratic approximation of the
constraints. The approximations are carried out using Taylor series
expansion.
The Quadratic Programming QP subproblem is to minimize the
quadratic objective function;
1 T
F (h ) = h̄ H h̄ + ȳ h̄ (111)
2
subject to linear equality and inequality constraints:
Ah̄ = a
(112)
B h̄ b
β Splines VII
where h̄ = (h1 , ..., hN 1 )T denotes the optimizing parameter vector, y

is the gradient vector, H is the positive de…nite Hessian matrix and A,
B, a, b are matrices of appropriate dimensions de…ning the
constraints.

Curves Trajectories Anglais4(1)

Uploaded by

Copyright:

Available Formats

Curves Trajectories Anglais4(1)

Uploaded by

Document Information

Copyright

Available Formats

Share this document

Share or Embed Document

Sharing Options

Did you find this document useful?

Is this content inappropriate?

Copyright:

Available Formats

Curves Trajectories Anglais4(1)

Uploaded by

Copyright:

Available Formats

Curves for Path Planning

Univ. Evry Paris-Saclay

S. MAMMAR (Institute) Curves 10/23 1 / 66

S. MAMMAR (Institute) Curves 10/23 2 / 66

S. MAMMAR (Institute) Curves 10/23 3 / 66

S. MAMMAR (Institute) Curves 10/23 4 / 66

Suppose we are given (n + 1)points de…ned by the pairs (xi , yi ), for

It is easy to see that these polynomials are given by

The (n + 1) polynomials li (x ) form a basis for the vector subspace of

S. MAMMAR (Institute) Curves 10/23 5 / 66

λ0 l0 (xk ) + λ1 l1 (xk ) + ... + λk lk (xk ) + ... + λn ln (xk ) = 0 () λk lk (xk ) = 0

S. MAMMAR (Institute) Curves 10/23 6 / 66

S. MAMMAR (Institute) Curves 10/23 7 / 66

We then verify that the polynomial

is the interpolation polynomial since it is of degree n and satis…es

It can be shown that the previous polynomial can be expressed simply as

S. MAMMAR (Institute) Curves 10/23 9 / 66

S. MAMMAR (Institute) Curves 10/23 10 / 66

S. MAMMAR (Institute) PolynomialCurves

Interpolation / Extrapolation polynomiale

Pn (x ) = c0 + c1 (x x0 ) + c2 (x x0 )(x x1 ) + ... + cn (x x0 )(x

S. MAMMAR (Institute) Curves 10/23 13 / 66

S. MAMMAR (Institute) Curves 10/23 14 / 66

f (xi ) = yi , ci = f [x0 , x1 , ..., xi ] (24)

P (xi ) = f (xi ) = yi (26)

Cette di¤érence peut être faible ou grande comme le montre la …gure ??

S. MAMMAR (Institute) Curves 10/23 17 / 66

S. MAMMAR (Institute) Curves 10/23 18 / 66

S. MAMMAR (Institute) Curves 10/23 19 / 66

The error results from two terms:

The problem is as follows: how to choose the xi to make this quantity

S. MAMMAR (Institute) Curves 10/23 21 / 66

S. MAMMAR (Institute) Curves 10/23 22 / 66

S. MAMMAR (Institute) Curves 10/23 23 / 66

S. MAMMAR (Institute) Curves 10/23 24 / 66

verifying the interpolation condition

S. MAMMAR (Institute) Curves 10/23 25 / 66

Si (xi +1 ) = Si +1 (xi +1 ) (i = 0, 1, ..., n 2) (44)

S. MAMMAR (Institute) Curves 10/23 26 / 66

S. MAMMAR (Institute) Curves 10/23 27 / 66

S. MAMMAR (Institute) Curves 10/23 28 / 66

Si (xi +1 ) = Si +1 (xi +1 ) (i = 0, 1, ..., n 2) (51)

(n 1) equation of derivative continuity

Si0 (xi +1 ) = Si0+1 (xi +1 ) (i = 0, 1, ..., n 2) (52)

S. MAMMAR (Institute) Curves 10/23 29 / 66

ci = Si (xi ) = yi (i = 0, 1, ..., n 2) (54)

S. MAMMAR (Institute) Curves 10/23 30 / 66

Si0 (xi ) = bi = zi (55)

which determines the bi . By now writing the conditions of continuity of

2ai (xi +1 xi ) + bi = 2ai +1 (xi +1 xi +1 ) + bi +1 (i = 0, 1, ..., n 2)

Si (x ) take thus the form

S. MAMMAR (Institute) Curves 10/23 31 / 66

Quadratic splines at points [ 5, 3, 1, 1, 3, 5]

S. MAMMAR (Institute) Curves 10/23 33 / 66

With 4n coe¢ cients to determine, this time we have only (4n 1)

S. MAMMAR (Institute) Curves 10/23 34 / 66

The continuity conditions of the …rst and second derivative lead to

S. MAMMAR (Institute) Curves 10/23 35 / 66

S. MAMMAR (Institute) Curves 10/23 36 / 66

In other words, the average curvature of S is lower than that of f .