b -sp lin e b ase d f ilter s for mu lti-p oin t t rajec...

6
B-Spline Based Filters for Multi-Point Trajectories Planning Luigi Biagiotti and Claudio Melchiorri Abstract—In this paper, the relation between B-splines and FIR (Finite Impulse Response) filters is demonstrated and exploited to design a digital filter for trajectory planning, com- bining the very simple structure and computational efficiency of FIR filters with the flexibility of splines. In particular, the trajectory generator consists of two main elements. The former is devoted to the solution of an optimization problem that, given a set of points to be interpolated (or approximated), provides the control points defining the spline. The latter, a cascade of moving average filters, gives the trajectory profile at each sampling time on the basis of such points. The proposed method has been applied to several robotic and industrial applications, and in this paper two case studies are reported as examples: an industrial robot performing a welding operation and a mobile robot moving in an environment with obstacles. With respect to these tasks, the main features of the trajectory generator are shown: the possibility of planning trajectories with high degree of smoothness (continuity of the derivatives), the possibility of easily changing the duration of the trajectory (and therefore the velocity, acceleration, jerk, etc. of the trajectory) maintaining the same geometric path, the possibility of locally modifying the pre-planned path. I. I NTRODUCTION Among many other applications, spline functions are ex- tensively used in planning trajectories for robots because of their flexibility. As a matter of fact, the tasks demanded to robots (mobile robots, industrial robots, humanoid robots, etc.) often require position profiles with complex shapes which are usually defined by means of a number of via- points. These via-points are then interpolated or approxi- mated with smooth functions to by optimized in order to comply with the constraints imposed by the specific robot application, i.e. kinematic constraints (such as limit values of velocity, acceleration, jerk, etc.) or dynamic constraints on the maximum torque available. In general, such interpolation tasks are performed by means of cubic splines since they assure the continuity of velocity and acceleration and prevent large oscillations of the trajectory that can result with high order polynomials, [1]. Therefore, cubic splines have been used to minimize the total traveling time of robot trajectories subject to constraints of velocity acceleration and jerk [2], or to globally minimize some quantities, such as acceleration [3] or jerk [4]. Some authors prefer the adoption of splines in the so-called B-form, i.e. B-splines, because they are much simpler from the computational point of view [5], [6] and because a local modification can be made quickly L. Biagiotti is with the Department of Information Engineer- ing, University of Modena and Reggio Emilia, 41100 Modena, Italy [email protected] C. Melchiorri is with the Department of Electronics, Informatics and Systems, University of Bologna, 40136 Bologna,Italy [email protected] and easily without recomputing the entire trajectory [7]. Also in this case cubic B-splines are in general considered. Splines of order higher than three, able to guarantee the continuity of jerk and higher derivatives, have been proposed but they are based on the classical polynomial formulation [8]. Indeed, B-spline are very suitable to generate trajectories with continuous derivatives up to a generic order n, since the interpolation/approximation of a given set of points does not depends on the particular order of the B-spline [1]. On the other hand, despite the clear geometrical meaning of B- splines and their computational superiority with respect to the other (equivalent) spline formulations, their use in robotics is still limited (e.g. in robotics textbooks only polynomial splines are general considered, see [9], [10], [11] among many others) probably because their evaluation is based on a recursive procedure rather than on a closed form expression. Aim of this paper is to provide a more simple formulation of B-spline based trajectories, combining the advantages of these functions with the simplicity and the low computational complexity of FIR (Finite Impulse Response) filters. II. B- SPLINES AND B- SPLINES BASIS FUNCTIONS Since the basic theory of B-splines is well known, we only give a brief summary of the concepts and notations. More details can be found e.g. in [1], [12], [13]. A B-spline of degree p is a parametric curve s : [t min ,t max ] R d defined as linear combination of B-spline basis functions of degree p, B p j (t): s(t)= n j=0 p j B p j (t), t min t t max . (1) The vectorial coefficients p j ,j =0,..., m, called control points, determine the shape of the curve and are computed by imposing approximation/interpolation conditions on a given set of data points. Let t = {t 0 ,...,t m1 } be a vector of real numbers (called knots), with t j t j+1 . The j -th B- spline basis function of degree p is defined, in a recursive manner, as B p j (t)= t - t j t j+p - t j B p1 j (t)+ t j+p+1 - t t j+p+1 - t j+1 B p1 j+1 (t) (2) with B 0 j (t)= 1, if t j t<t j+1 0, otherwise. A particular case of B-splines is represented by uniform B-splines, that are defined for an equally-spaced distribution of the knots, i.e. t j+1 - t j = T, j =0,...m - 2. In this 2010 IEEE International Conference on Robotics and Automation Anchorage Convention District May 3-8, 2010, Anchorage, Alaska, USA 978-1-4244-5040-4/10/$26.00 ©2010 IEEE 3065

Upload: others

Post on 23-May-2020

1 views

Category:

Documents


0 download

TRANSCRIPT

Page 1: B -Sp lin e B ase d F ilter s for Mu lti-P oin t T rajec ...vigir.missouri.edu/~gdesouza/Research/Conference... · A pa rticul ca s e of B - li ni t d by un if m B -splines, that

B-Spline Based Filters for Multi-Point Trajectories Planning

Luigi Biagiotti and Claudio Melchiorri

Abstract— In this paper, the relation between B-splines andFIR (Finite Impulse Response) filters is demonstrated andexploited to design a digital filter for trajectory planning, com-bining the very simple structure and computational efficiencyof FIR filters with the flexibility of splines. In particular, thetrajectory generator consists of two main elements. The formeris devoted to the solution of an optimization problem that, givena set of points to be interpolated (or approximated), providesthe control points defining the spline. The latter, a cascadeof moving average filters, gives the trajectory profile at eachsampling time on the basis of such points. The proposed methodhas been applied to several robotic and industrial applications,and in this paper two case studies are reported as examples: anindustrial robot performing a welding operation and a mobilerobot moving in an environment with obstacles. With respectto these tasks, the main features of the trajectory generator areshown: the possibility of planning trajectories with high degreeof smoothness (continuity of the derivatives), the possibility ofeasily changing the duration of the trajectory (and therefore thevelocity, acceleration, jerk, etc. of the trajectory) maintainingthe same geometric path, the possibility of locally modifyingthe pre-planned path.

I. INTRODUCTION

Among many other applications, spline functions are ex-

tensively used in planning trajectories for robots because of

their flexibility. As a matter of fact, the tasks demanded to

robots (mobile robots, industrial robots, humanoid robots,

etc.) often require position profiles with complex shapes

which are usually defined by means of a number of via-

points. These via-points are then interpolated or approxi-

mated with smooth functions to by optimized in order to

comply with the constraints imposed by the specific robot

application, i.e. kinematic constraints (such as limit values

of velocity, acceleration, jerk, etc.) or dynamic constraints on

the maximum torque available. In general, such interpolation

tasks are performed by means of cubic splines since they

assure the continuity of velocity and acceleration and prevent

large oscillations of the trajectory that can result with high

order polynomials, [1]. Therefore, cubic splines have been

used to minimize the total traveling time of robot trajectories

subject to constraints of velocity acceleration and jerk [2],

or to globally minimize some quantities, such as acceleration

[3] or jerk [4]. Some authors prefer the adoption of splines

in the so-called B-form, i.e. B-splines, because they are

much simpler from the computational point of view [5],

[6] and because a local modification can be made quickly

L. Biagiotti is with the Department of Information Engineer-ing, University of Modena and Reggio Emilia, 41100 Modena, [email protected]

C. Melchiorri is with the Department of Electronics, Informaticsand Systems, University of Bologna, 40136 Bologna,[email protected]

and easily without recomputing the entire trajectory [7].

Also in this case cubic B-splines are in general considered.

Splines of order higher than three, able to guarantee the

continuity of jerk and higher derivatives, have been proposed

but they are based on the classical polynomial formulation

[8]. Indeed, B-spline are very suitable to generate trajectories

with continuous derivatives up to a generic order n, since

the interpolation/approximation of a given set of points does

not depends on the particular order of the B-spline [1]. On

the other hand, despite the clear geometrical meaning of B-

splines and their computational superiority with respect to the

other (equivalent) spline formulations, their use in robotics

is still limited (e.g. in robotics textbooks only polynomial

splines are general considered, see [9], [10], [11] among

many others) probably because their evaluation is based on a

recursive procedure rather than on a closed form expression.

Aim of this paper is to provide a more simple formulation

of B-spline based trajectories, combining the advantages of

these functions with the simplicity and the low computational

complexity of FIR (Finite Impulse Response) filters.

II. B-SPLINES AND B-SPLINES BASIS FUNCTIONS

Since the basic theory of B-splines is well known, we only

give a brief summary of the concepts and notations. More

details can be found e.g. in [1], [12], [13].

A B-spline of degree p is a parametric curve s :[tmin, tmax] → R

d defined as linear combination of B-spline

basis functions of degree p, Bpj (t):

s(t) =

n∑

j=0

pjBpj (t), tmin ≤ t ≤ tmax. (1)

The vectorial coefficients pj , j = 0, . . . , m, called control

points, determine the shape of the curve and are computed by

imposing approximation/interpolation conditions on a given

set of data points. Let t = {t0, . . . , tm−1} be a vector of

real numbers (called knots), with tj ≤ tj+1. The j-th B-

spline basis function of degree p is defined, in a recursive

manner, as

Bpj (t) =

t − tjtj+p − tj

Bp−1j (t) +

tj+p+1 − t

tj+p+1 − tj+1Bp−1

j+1 (t) (2)

with

B0j (t) =

{1, if tj ≤ t < tj+1

0, otherwise.

A particular case of B-splines is represented by uniform

B-splines, that are defined for an equally-spaced distribution

of the knots, i.e. tj+1 − tj = T, j = 0, . . . m − 2. In this

2010 IEEE International Conference on Robotics and AutomationAnchorage Convention DistrictMay 3-8, 2010, Anchorage, Alaska, USA

978-1-4244-5040-4/10/$26.00 ©2010 IEEE 3065

Page 2: B -Sp lin e B ase d F ilter s for Mu lti-P oin t T rajec ...vigir.missouri.edu/~gdesouza/Research/Conference... · A pa rticul ca s e of B - li ni t d by un if m B -splines, that

case, the basis functions for a given degree p are consistent

under shifts:

Bpj+1(t) = Bp

j (t − T ), j = 0, . . . ,m − 2.

Therefore, for uniform B-splines it is possible to express the

(j+1)-th basis function Bpj in terms of the first basis function

Bp0 , hereafter simply denoted by Bp:

Bpj (t) = Bp(t − jT ), j = 0, . . . ,m − 1

and the B-spline can be rewritten as

su(t) =n∑

j=0

pjBp(t − jT ), 0 ≤ t ≤ (m − 1)T. (3)

Moreover, for uniform B-splines, the definition (2) of the

basis function Bp(t) of degree p is equivalent to

Bp(t) =1

TBp−1 ∗ B0

=1

TB0 ∗

1

TB0 ∗ . . . ∗

1

TB0

︸ ︷︷ ︸

p times

∗B0, (4)

where ∗ denotes the convolution product and

B0(t) =

{1, if 0 ≤ t < T

0, otherwise.

By Laplace transforming the general expression of the uni-

form B-spline (3) and substituting (4) one obtains

Su(s) =

n∑

j=0

L

{

pjB0 ∗

1

TB0 ∗

1

TB0 ∗ . . . ∗

1

TB0

}

e−jsT .

Exploiting the linearity of the above expression and the fact

that 1T

B0 is not a function of the index j, the B-spline

1

0 T 2T 3T 4T

1

0 T 2T 3T 4T

1

0 T 2T 3T 4T

1

0 T 2T 3T 4T

B0(t

)B

1(t

)B

2(t

)B

3(t

)

Fig. 1. B-spline basis functions Bp(t) obtained for different values of thedegree p.

expression becomes

Su(s) =n∑

j=0

L{pjB

0}

e−jsT · M(s) · M(s) · . . . · M(s)

=L

n∑

j=0

pjB0(t − jT )

·M(s)·M(s) ·. . .·M(s).

where M(s) = L{

1T

B0(t)}

= 1T

1−e−sT

sperforms the mean

of the input function over an interval of duration T . This

expression suggests that a uniform B-spline can be evaluated

by feeding the cascade of p filters M(s) with a piecewise

constant function

p(t) =

n∑

j=0

pjB0(t − jT )

that in the generic interval jT ≤ t < (j + 1)T assumes the

constant value pj . Note that, in multidimensional B-splines,

the control points pj are multi-dimensional and the function

p(t) is multidimensional as well. In this case, it is necessary

to consider the different components and filter each of them

with a separate chain of mean filters.

III. DISCRETE B-SPLINES

A unified transformation to convert analytic B-splines

in the discrete domain does not exist. In particular, with

reference to cardinal B-splines (uniform B-splines defined

over the integers) which are mainly used for signal analysis,

interpolation and image processing, it is possible to find

in the literature different techniques to obtains discrete B-

splines. In general, they are defined by directly sampling

analytic B-splines with Z-transform, bilinear transform, etc.

[14], [15], [16], [17], [18]. For this purpose, the first step

consists in discretizing the basis functions. By exploiting the

relationship

Bp(t) =1

TBp−1(t) ∗ B0(t)

=1

T

∫∞

−∞

Bp−1(t − τ) B0(τ)dτ

the value of the B-spline basis function at the discrete time

instants t = kTs (where Ts is the sampling time) results

Bpk =

1

T

∫∞

−∞

Bp−1(kTs − τ) B0(τ)dτ

≈1

T

n=∞∑

n=−∞

Bp−1(kTs − nTs) B0(nTs)Ts

=1

N

n=∞∑

n=−∞

Bp−1k−n B0

n =1

NBp−1

k ∗ B0k

where Bpk = Bp(kTs), N = T/Ts is the number of samples

in each knot span, and ∗ is the discrete convolution product.

Finally, the discrete basis function Bpk of degree p, can be

written as

Bpk =

1

NB0

k ∗1

NB0

k ∗ . . . ∗1

NB0

k

︸ ︷︷ ︸

p times

∗B0k (5)

3066

Page 3: B -Sp lin e B ase d F ilter s for Mu lti-P oin t T rajec ...vigir.missouri.edu/~gdesouza/Research/Conference... · A pa rticul ca s e of B - li ni t d by un if m B -splines, that

︸ ︷︷ ︸

p blocks

B0k Bp

k1N

1−z−N

1−z−1

1N

1−z−N

1−z−1

1N

1−z−N

1−z−1

Fig. 2. System composed by p filters for the computation of the discreteB-spline basis function B

p

kof degree p.

with

B0k =

{1, if 0 ≤ k < N − 1

0, otherwise.

Note that (5) is only an approximation of the analytic B-

spline basis function and does not have the same values at

discrete points. Nevertheless, it is possible to prove that the

staircase curve obtained in this way tend to the B-spline basis

function in the sense of root mean square (RMS) as Ts goes

to zero [17]. The performed approximation leads to a very

simple definition of discrete basis functions and discrete B-

splines. As a matter of fact, the discrete function (5) can be

seen as a cascade of p mean filters (moving average filters),

whose transfer functions are

M(z) =1

N

1 − z−N

1 − z−1(6)

=1

N

(

1 + z−1 + z−2 + . . . + z−(N−1))

with the input B0k, see Fig. 2. Like analytic splines, discrete

uniform B-splines are defined as a linear combination of the

discrete basis function properly time-shifted:

sk =

n∑

j=0

pjBpk−jN (7)

By applying the Z-transform to (7) one obtains

S(z)=

n∑

j=0

Z

{

pj

1

NB

0k ∗

1

NB

0k ∗ . . . ∗

1

NB

0k ∗ B

0k

}

z−jN

(8)

0 T 2T 3T 4T 5T 6T 7T 8T 9T 10T

p0 p1 p2

p3

p4

p5

p6

p7 p8 p9

pk

(a)

0 T 2T 3T 4T 5T 6T 7T 8T 9T

q0 q1 q2 q3

q4

q5

q6

q7

q8

q9

sk

(b)

Fig. 3. Samples of the piecewise constant function pk (a) generating thespline profile sk that interpolates the given points qj (b).

and then, because of the linearity of (8) and the fact that1N

B0 is not a function of the index j,

S(z) =n∑

j=0

Z{pjB

0k

}z−jN · M(z) · M(z) · . . . · M(z)

= Z

n∑

j=0

pjB0k−jN

· M(z) · M(z) · . . . · M(z).

Therefore, the discrete uniform B-spline of degree p is

defined as the output of a chain of p mean filters feeded

with the piecewise constant function

pk =

n∑

j=0

pjB0k−jN (9)

shown in Fig. 3(a).

IV. CHOICE OF THE CONTROL POINTS

The proposed trajectory planner, composed by a generator

of sequences of constant values pj and by a cascade of pmean filters, enjoys the same properties of analytic uniform

B-splines. Therefore, in order to find the control points

which define the piecewise constant function pk, one can

exploit classical techniques derived by B-spline interpola-

tion/approximation methods.

For example, if one considers the interpolation of a set of l+1points {q0, q1, q2, . . . , ql−1, ql} it is necessary to impose the

conditions

s(ti) = qi, i = 0, . . . , l (10)

where ti is the time instant at which the spline s(t) crosses

the given point qi.

The first step consists in selecting the degree p of the

spline according to the desired degree of smoothness. Strictly

related to p is the choice of time instants ti:

• if p is odd, the ti are assumed coincident with the knots,

ti = iT ;

• if p is even, the time instants ti should be selected in

the midpoint of each knot span, ti = 2i+12 T .

Once the interpolation time instants ti have been chosen it is

possible to make the system of equations (10) explicit with

the substitution of the values of basis functions at ti in the

spline definition (1). For uniform B-splines it is possible to

find a closed form expression which provides the value of

the basis function Bp at a generic time instant t [16]:

Bp(t) =1

p!

p+1∑

k=0

(−1)k

(p + 1

k

)(t

T− k

)p

+

where ! denote the factorial function,(ab

)= a!

b!(a−b)! and

(x)p+ is the truncated power function ((x)p

+ = xp if x ≥ 0,

(x)p+ = 0 otherwise). The values of Bp for p odd and p even,

computed at points ti = iT and ti = 2i+12 T respectively, are

reported in Tab. I. Note that, because of the choice of the

interpolation time instants, the values of Bp do not depend

on T , but only on the index i and on the degree p.

In order to obtain a system of equations well conditioned

3067

Page 4: B -Sp lin e B ase d F ilter s for Mu lti-P oin t T rajec ...vigir.missouri.edu/~gdesouza/Research/Conference... · A pa rticul ca s e of B - li ni t d by un if m B -splines, that

from a mathematical point of view it is necessary to consider

symmetrical B-splines ss(t), i.e. uniform B-splines whose

basis function βp(t) is symmetric with respect the origin.

The function βp(t) can be deduced from Bp(t) with a simple

time shift, βp(t) = Bp(t + p+1

2 T), and as a consequence

symmetrical B-splines are related to standard uniform B-

splines by

ss(t) =

n∑

j=0

pjβp(t − jT )

=

n∑

j=0

pjBp(t + p+1

2 T − jT ) = su(t + p+12 T ),

that is, given the control points, uniform B-splines are equal

to symmetrical B-splines delayed by p+12 T . Obviously, the

theory of Sec. II and Sec. III could be based on symmetrical

B-splines but this would imply the presence of a temporal

anticipation leading to noncausal filters for the evaluation of

B-splines.

For each point to be interpolated, with the only exception of

the first and last points, the equation (10) becomes

ss(ti) =

n∑

j=0

pjBp(ti − jT + p+1

2 T ) = qi (11)

where the unknowns are the control point pj . The interpo-

lation of the first and last points, with zero velocity and

acceleration, is achieved by exploiting the characteristics of

the dynamic system of Fig. 2 used to generate the spline.

Since all the filters have unitary static gain, the output of the

filters cascade will reach and maintain the desired value q0

or ql iff the same value is applied to the input pT seconds

before. In other words, in order to smoothly start from q0

and end to ql, the first/last p control points must be equal to

q0/ql. The l − 1 internal control points are then computed

by solving the system of equations obtained by stacking (11)

0 T 2T 3T 4T 5T 6T 7T 8T

p = 1 0 1 0

p = 3 0 1

6

4

6

1

60

p = 5 0 1

120

26

120

66

120

26

120

1

1200

p = 7 0 1

5040

120

5040

1191

5040

2416

5040

1191

5040

120

5040

1

50400

(a)

1

2T 1

2T 3

2T 5

2T 7

2T 9

2T 11

2T

p = 2 1

8

6

8

1

80

p = 4 1

384

76

384

230

384

76

384

1

3840

p = 6 1

46080

722

46080

10543

46080

23548

46080

10543

46080

722

46080

1

46080

(b)

TABLE I

B-SPLINE BASIS FUNCTION Bp(t) FOR p ODD AT POINTS ti = iT (a),

AND FOR p EVEN AT POINTS ti = 2i+1

2T (b).

for i = 1, . . . , l − 1 and the piecewise constant function (9)

can be finally built.

A. Cubic B-splines

The adoption of the B-splines of degree p = 3, i.e.cubic B-splines, guarantees the continuity of velocity andacceleration. By substituting the values of Tab. I, the generalequation (11) becomes

ss(iT ) =1

6pi−1 +

4

6pi +

1

6pi+1 = qi, i = 1, . . . , l − 1 (12)

with the additional constraints p0 = q0 and pl = ql.After simple algebraic manipulations, (12) can be writtenin a matrix form as

4 1 0 · · · 01 4 1 0 · · · 00 1 4 1 0 · · · 0...

. . ....

0 · · · 0 1 4 1 00 · · · 0 1 4 10 · · · 0 1 4

p1

p2

p3

...pl−3

pl−2

pl−1

=

6q1 − q0

6q2

6q3

...6ql−3

6ql−2

6ql−1 − ql

(13)

Due to the tridiagonal structure of the system matrix, the

solution of (13) can be easily found. More generally, by

adopting B-splines of generic degree p, the systems to be

solved for obtaining the control points will be characterized

by banded matrices, whose inversion can be carried out in a

very efficient way.

Figure 4 shows the cubic B-spline trajectory obtained by

interpolating the points

{qj} = {5, 12, 3, 45, 23, 4,−3, 5,−3, 10, 10, 16, 19, 4, 23}

with period T = 1s. For the sake of simplicity a one-

dimensional case has been taken into account but the same

considerations are valid in the multi-dimensional case as

shown in the later Sec. VI. Note that the first and the last

tracts are of length 2T (and in the case of B-splines of

generic degree p of length p+12 T ). This is unavoidable but, on

the other hand it allows the trajectory to smoothly start/end,

i.e. with all the derivatives up to the order p−1 that are null

0 2 4 6 8 10 12 14 16 18−200

−100

0

100

200−40

−20

0

20

40

60−20

0

20

40

60

80

sk

sk

sk

Time (s)

Fig. 4. One-dimensional cubic B-spline sk interpolating a set of givenpoints: profiles of position (superimposed to the piecewise constant functionpk), velocity and acceleration.

3068

Page 5: B -Sp lin e B ase d F ilter s for Mu lti-P oin t T rajec ...vigir.missouri.edu/~gdesouza/Research/Conference... · A pa rticul ca s e of B - li ni t d by un if m B -splines, that

0 T 2T 3T 4T 5T 6T 7T 8T 9T

Interpolation/approximation

problem solverSequencer

Offline On-line (Ts)

pk

sk

pk sk{qj} {pj} 1

N

1 − z−N

1 − z−1

1

N

1 − z−N

1 − z−1

︸ ︷︷ ︸

p blocks

Fig. 5. Overall structure of the filter for B-spline trajectories planning.

at the start- and end- points. In the case of the cubic B-spline

this means that initial and final velocities and accelerations

are zero. By comparing the profiles of the function pk and

of the spline sk it is clear that the latter roughly reproduces

the shape of the former with a “delay” of p+12 T seconds, see

Fig. 4. Moreover, when the last control point is applied, the

output of the trajectory planner reaches such a value after

pT seconds.

V. STRUCTURE OF THE FILTER FOR TRAJECTORY

PLANNING

In Fig. 5 the structure of the trajectory generator is

illustrated. It is composed by two main elements:

1) an algorithm (described previously) that transforms the

desired points qj in the set of control points pj ;

2) a cascade of p moving average filters, where p is the

desired degree of the B-spline (the resulting trajectory

will be Cp−1).

Between them, the “Sequencer” produces the piecewise

constant function pk shown in Fig. 3(a) arranging in a line

the control points pj , each one for a duration of T seconds,

where T is the time distance between consecutive points.

While the algorithm for the control points computation is

performed offline, the system composed by the sequencer

and by the filters runs on-line with a sampling time Ts, that

in general is the same of the digital controller of the overall

robotic system. Since Ts is fixed, the number N of samples

considered in each FIR filter only depends on T , being

N = round( TTs

). Therefore, by changing N it is possible to

directly modify T .

In order to smoothly starts from the first desired point q0

it is necessary that internal states of all the FIR filters are

set to q0, while a smooth end at ql is guaranteed by the

Curve to be

followed

−0.4−0.2

00.2

0.4

−1.02

−1

−0.98

−0.96

−0.94−0.4

−0.3

−0.2

−0.1

0

0.1

0.2

0.3

0.4

xy

z

(a) (b)

Fig. 6. Welding of two cylindrical objects (a) and related geometric path(b).

application to the filters cascade input of the control point

ql for at least p intervals of duration T .

In robotic applications, the points qj are generally defined

in a d-dimensional space (d = 2, 3). In this case, the

trajectory generator must be simply replicated d times, one

for each component, and the desired B-spline is obtained if

the related sequences are synchronized.

VI. NUMERICAL EXAMPLES

Two case studies are now reported as examples of the flex-

ibility and simplicity offered by this technique for trajectory

planning.

A. 3D welding

Consider the case of an industrial robot manipulator weld-

ing the two cylinders in Fig. 6(a). The path that the robot

tool must track is reported in Fig. 6(b). Many CAD/CAM

systems directly provide the control points pj of the B-spline

to be followed. Differently, it is possible to “sample” the

analytic curve if given or, more generally, consider a certain

number of points distributed along the geometric path to be

followed. In the case of uniform B-splines, the distribution

of the path points is crucial for imposing a desired feed-

rate of the tool. As a matter of fact, it is not possible to

freely chose the duration of each spline segment (being Tequal for all the tracts), but on the contrary it is necessary

to select the distance between points qj with the purpose of

“shaping” the velocity, the acceleration, etc. of the trajectory.

For instance, in a welding application the velocity should be

constant along the entire path. For this reason, points qj have

been selected at a distance which is approximatively constant

(the mean value of the distance between consecutive points

is δ = 0.0944). From these via-points the control points pj

have been obtained as reported in Sec. IV. In order to prevent

oscillation and reduce tracking errors a quintic B-spline has

been considered (p = 5). In this way, velocity, acceleration,

jerk and even snap (i.e. the derivative of jerk) are continuous,

as shown in Fig. 7. The trajectory is evaluated on-line by a

chain of five FIR filters with a sampling time Ts = 0.001s.

Each filter calculates the average of the last N = 1000samples, therefore the time distance between the points qj is

T = NTs = 1s. As a consequence the speed, which is almost

constant, is |sk| ≈δT

= 0.094, see Fig. 7(a). By changing

N it is possible to modify the duration of each tract and

of the overall trajectory. Accordingly the derivatives of sk

3069

Page 6: B -Sp lin e B ase d F ilter s for Mu lti-P oin t T rajec ...vigir.missouri.edu/~gdesouza/Research/Conference... · A pa rticul ca s e of B - li ni t d by un if m B -splines, that

0 5 10 15 20 250

2

4

6

8x 10

−5

0

0.02

0.04

0.06

0.080

0.02

0.04

0.06

0.08

0.1

|sk|

|sk|

|... sk|

Time (s)0 1 2 3 4 5

0

0.002

0.004

0.006

0.008

0.010

0.5

1

1.5

20

0.1

0.2

0.3

0.4

0.5

|sk|

|sk|

|... sk|

Time (s)

(a) (b)

Fig. 7. Profiles of the norm of velocity, acceleration and jerk related tothe B-spline sk defining the curve of Fig. 6(b) for different values of T :T = 1s (a) and T = 0.2s (b).

(velocity, acceleration, jerk, etc.) are modified as follows

s′(i)k = αi

s(i)k

where α = NN ′

, being N ′ the new number of samples

considered in each FIR filter. In Fig. 7(b) the velocity,

acceleration and jerk of the B-spline computed with N =200 (thus T = 0.2s) is shown. Note that the velocity is scaled

by 5 times, the acceleration by 25, the jerk by 125, while the

shape of these profiles remains unchanged. Obviously also

the geometric path defined by the trajectory generator does

not change.

B. Mobile robots

Another interesting application of the proposed planner

concerns the navigation of mobile robots in an environment

with obstacles, as shown in Fig. 8(a) where a proper set

of points (black spots) are interpolated by means of a

second degree spline. The choice p = 2 guarantees only the

continuity of the velocity, but on the other hand it reduces

the delay between the application of the input pk to the

chain of two filters and the related output constituting the

trajectory. In this way it is possible to change the trajectory

pre-computed by simply changing the control points during

the realtime generation and tracking of the trajectory itself.As

a matter of fact, a B-spline trajectory has the property that

1 2 3 4 5 6 7 8−2

−1

0

1

2

3

4

5

x

y

1 2 3 4 5 6 7 8−2

−1

0

1

2

3

4

5

x

y

Fig. 8. Geometric path in a 2D environment with obstacles definedby means of a B-spline interpolating the given via-points (a) and localmodification obtained by changing the function pk .

a local modification can be made quickly and easily without

recomputing the entire trajectory and leaves the remaining

trajectory unchanged. For instance, in Fig. 8(b) in order to

avoid the first obstacle, whose position is changed, some

control points are moved (the small spots in the figure are

the control points pj and not the via-points qj).

VII. CONCLUSIONS

In this paper a digital filter for trajectory planning is pro-

posed. This trajectory generator is based on B-spline func-

tions and therefore shares the same characteristics: multi-

point trajectories composed by properly joined polynomial

segments, degree of smoothness that can be freely selected,

efficient methods for interpolation/approximation of given

points, possibility of making local changes on the trajectory

without recomputing it. On the other hand, the proposed

structure composed by a chain of FIR filters allows a great

simplification of the procedures for B-spline evaluation and

on-line trajectory generation, making the proposed planner

suitable for a number of robotic applications.

REFERENCES

[1] L. Biagiotti and C. Melchiorri. Trajectory Planning for Automatic

Machines and Robots. Springer, Heidelberg, Germany, first edition,2008.

[2] C.-S. Lin, P.-R. Chang, and J.Y.S. Luh. Formulation and optimizationof cubic polynomial joint trajectories for industrial robots. IEEE

Transaction on Automatic Control, 28(12):1066–1074, 1983.[3] B. Cao, G.I. Dodds, and G.W. Irwin. Constrained time-efficient

and smooth cubic spline trajectory generation for industrial robots.Proceedings of IEE Conference on Control Theory and Applications,144:467–475, 1997.

[4] A. Piazzi and A. Visioli. Global minimum-jerk trajectory planningof robot manipulator. IEEE Transaction on Industrial Electronics,47(1):140–149, 2000.

[5] J. E. Bobrow. Optimal robot path planning using the minimum-timecriterion. IEEE Journal of Robotics and Automation, 4 (5):443–450,1988.

[6] C-H Wang and J-G Horng. Constrained minimum-time path planningfor robot manipulators via virtual knots of the cubic b-spline functions.IEEE transactions on automatic control, 35 (5):573–577, 1990.

[7] S. E. Thompson and R. V. Patel. Formulation of joint trajectoriesfor industrial robots using B-splines. IEEE Transactions on Industrial

Electronics, 34 (2):192–199, 1987.[8] A. Piazzi, C. G. Lo Bianco, and M. Romano. η3-splines for the

smooth path generation of wheeled mobile robots. IEEE Transactions

on Robotics, 23 (5):1089–1095, 2007.[9] M. W. Spong, S Hutchinson, and M. Vidyasagar. Robot Modeling and

Control. John Wiley & Sons, 1997.[10] J. Angeles. Fundamentals of Robotic Mechanical Systems. Springer,

2007.[11] B. Siciliano, L. Sciavicco, L.Villani, and G. Oriolo. Robotics:

Modelling, Planning and Control. Springer, 2009.[12] L. Piegl and W. Tiller. The Nurbs Book. Springer-Veralg, second

edition, 1997.[13] C. de Boor. A Practical Guide to Splines. Springer, 2001.[14] P.V. Sankar and L.A. Ferrari. Simple algorithms and architectures for

B-spline interpolation. IEEE Transactions on Pattern Analysis and

Machine Intelligence, 13 (3):277 – 285, 1991.[15] M. Unser, A. Aldroubi, and M. Eden. B-spline signal processing: Part

I and Part II. IEEE Transaction on Signal Processing, 41(2):821–848,1993.

[16] H. Olkkonen. Discrete binomial splines. Graphical Models and Image

Processing, 57 (2):101–106, 1995.[17] K. Ichige and M. Kamada. An approximation for discrete B-splines

in time domain. IEEE Signal Process. letters, 4:82–84, 1997.[18] S. Samadi, M.O. Ahmad, and M.N.S. Swamy. Characterization of

B-spline digital filters. IEEE Transactions on Circuits and Systems I:

Regular Papers, 51 (4):808– 816, 2004.

3070