AnObject-OrientedFrameworkforSpatialMotionPlanningof MultibodySystems · 2013. 11. 13. ·...
Transcript of AnObject-OrientedFrameworkforSpatialMotionPlanningof MultibodySystems · 2013. 11. 13. ·...
-
An Object-Oriented Framework for Spatial Motion Planning ofMultibody Systems
Der Fakultät für Ingenieurwissenschaften, Abteilung Maschinenbau und
Verfahrenstechnik der
Universität Duisburg-Essen
zur Erlangung des akademischen Grades
eines
Doktors der Ingenieurwissenschaften
Dr.-Ing.
genehmigte Dissertation
von
Francisco Geu Flores
aus
Lima, Perú
Referent: Prof. Dr.-Ing. Andrés Kecskeméthy
Korreferent: o. Univ.-Prof. Dr.-Ing. habil. Hartmut Bremer
Tag der mündlichen Prüfung: 21. Dezember 2012
-
III
Vorwort
Die vorliegende Dissertation entstand während meiner Tätigkeit als wissenschaftlicher
Mitarbeiter am Lehrstuhl für Mechanik und Robotik der Universität Duisburg-Essen,
unter partieller Förderung der Stiftung Industrieforschung im Rahmen des Projektes
“IntelliSpline: Ein objektorientierter Software-Baukasten für die optimierte Bahnpla-
nung in industriellen Anlagen”.
Zunächst möchte ich Herrn Prof. Dr.-Ing. Andrés Kecskeméthy für die Anregung und
Betreuung dieser Arbeit danken. Sein Fachwissen und vor allem seine Leidenschaft
zur Wissenschaft haben nicht nur wesentlich zum Gelingen dieser Arbeit beigetragen,
sondern auch eine wertvolle Spur in meiner wissenschaftlichen Ausbildung hinterlassen.
Ich danke auch Herrn o. Univ.-Prof. Dr.-Ing. habil. Hartmut Bremer für seine
feinen Korrekturvorschläge und insbesondere für sein inspirierend elegant geschriebenes
Gutachten.
Mein besonder Dank gilt Herrn Dr.-Ing. Martin Tändl, der die Grundlagen dieser Ar-
beit in seiner Dissertation vorgelegt hat. Die langen und interessanten Besprechungen,
die wir an seiner Tafel geführt haben, legten die idealen Anfangsbedingungen für eine
gelungene Promotion.
Ich möchte ebenfalls Herrn Dr.-Ing. Martin Schneider für seinen freundlichen Beistand
danken. Seine vorbildlich systematische Arbeitsweise hat meine eigene Arbeitsweise
wesentlich beeinflußt.
Weiterhin danke ich allen Kolleginnen und Kollegen, die mich in meiner Tätigkeit als
wissenschaftlicher Mitarbeiter in Duisburg begleitet haben. Es ist vor allem wegen
ihrer freundlichen Unterstützung und Kameradschaft, dass meine Zeit als Promovend
ein sehr geschätzer Abschnitt meines Lebens geworden ist.
Abschließend bedanke ich mich bei meiner Familie und meinen Freunden. Sie wecken
in mir den Willen, jeden Tag besser zu werden. Trotz räumlichen Abstände sind sie
immer präsent.
Ich widme diese Arbeit meinem Vater, Santos Geu Rivera. Es ist meine Hoffnung, dass
seine Liebe für Genauigkeit und mathematische Schönheit in diesen Seiten zu spüren
ist.
Duisburg, im August 2013 Francisco Geu Flores
-
V
Contents
1 Introduction 1
1.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1
1.2 State of the art . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4
1.2.1 Calculus of variations and optimal control . . . . . . . . . . . . 4
1.2.2 Time-optimal control along a given spatial path . . . . . . . . . 6
1.2.3 Motion interpolation with splines . . . . . . . . . . . . . . . . . 8
1.2.4 Optimal motion planning . . . . . . . . . . . . . . . . . . . . . . 9
1.3 Objective and overview . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
1.4 Notation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
2 Description of one-parametric motion as a kinetostatic transmission 15
2.1 The concept of kinetostatic transmission element . . . . . . . . . . . . 15
2.1.1 Multibody systems as concatenations of kinetostatic transmis-
sion elements . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19
2.1.2 Computation of the transmission Jacobians . . . . . . . . . . . . 20
2.1.3 Generation of the equations of motion in minimal form . . . . . 21
2.2 Spatial paths as kinetostatic transmission elements . . . . . . . . . . . 23
2.2.1 Spatial curves in IR3 and spatial paths in SE(3) . . . . . . . . . 23
2.2.2 The concept of a curve joint . . . . . . . . . . . . . . . . . . . . 26
2.2.3 Tangent frame parametrization . . . . . . . . . . . . . . . . . . 27
2.2.4 Orientation parametrization with respect to a tangent frame . . 30
2.2.5 General frame parametrization . . . . . . . . . . . . . . . . . . . 32
2.3 Joint-motion parametrization as a kinetostatic transmission element . . 34
-
VI Contents
3 Motion interpolation using B-splines 35
3.1 Fundamentals of spline generation with B-splines . . . . . . . . . . . . 35
3.2 Re-parametrization of B-splines using curve fitting . . . . . . . . . . . . 40
3.3 Path length of a three-dimensional B-spline . . . . . . . . . . . . . . . 45
3.4 Spatial path generation using splines . . . . . . . . . . . . . . . . . . . 47
3.5 Spatial path parametrization in optimal motion planning problems . . . 48
4 Time-optimal motion along a prescribed spatial path 53
4.1 General problem formulation . . . . . . . . . . . . . . . . . . . . . . . . 53
4.2 Computation of the dynamic constraints . . . . . . . . . . . . . . . . . 57
4.3 Formulation as an optimal control problem . . . . . . . . . . . . . . . . 58
4.4 Solution of the optimal control problem . . . . . . . . . . . . . . . . . . 60
4.4.1 Systems with acceleration constraints linear in ṡ2 . . . . . . . . 61
4.4.2 General time-optimal problems . . . . . . . . . . . . . . . . . . 64
4.5 Application examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . 65
4.5.1 Loading cycles of backhoe excavators . . . . . . . . . . . . . . . 65
4.5.2 Waiter-motion problem for a given spatial path . . . . . . . . . 67
4.5.3 Generalized rocket-car problem . . . . . . . . . . . . . . . . . . 72
5 The object-oriented framework “IntelliSpline” for motion planning 74
5.1 Optimal motion planning as a nonlinear optimization problem . . . . . 74
5.1.1 Solution strategy . . . . . . . . . . . . . . . . . . . . . . . . . . 77
5.2 Interfaces with existing optimization routines . . . . . . . . . . . . . . . 78
5.2.1 NAG e04unc . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 79
5.2.2 MINPACK lmdif . . . . . . . . . . . . . . . . . . . . . . . . . . 80
-
Contents VII
5.3 Application examples . . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
5.3.1 Brachistochrone . . . . . . . . . . . . . . . . . . . . . . . . . . . 81
5.3.2 Roller-coaster design . . . . . . . . . . . . . . . . . . . . . . . . 91
5.3.3 General waiter-motion problem . . . . . . . . . . . . . . . . . . 102
6 Summary and outlook 117
Bibliography 118
-
1
1 Introduction
1.1 Motivation
Optimal motion control of mechanical systems is one of the broadest areas of research
in multibody dynamics and robotics. The general optimal motion problem consists in
controlling a mechanical system such that it follows an optimal spatial motion, subject
to geometric, kinematic and dynamic constraints. The quality of the resulting motion
is usually defined in terms of the total cycle time, consumed energy, jerkiness, deviation
from given target dynamics, or a combination of all of them. The problem is typically
divided into two main tasks, as shown in fig. 1.1: (1) the offline computation of an
optimal target motion subject to geometric, kinematic and dynamic constraints, and
(2) the control of the actual mechanical system along the target motion computed in
(1). This work addresses the first of these tasks, referred to hereafter as “optimal
motion planning”.
optimization
problem 1a
optimization
problem 1bcontroller
mechanical
systemsensorics
initial
guess
modelled
geometric
constraints
modelled
geometric,
kinematic,
and dynamic
constraints
disturbances
actual
geometric,
kinematic,
and dynamic
constraints
1) offline motion planning 2) motion controller
Figure 1.1: Classical motion planning and control
Optimal motion planning can be further divided into two subtasks: (1a) the compu-
tation of a feasible initial motion, subject to a set of geometric constraints, and (1b)
the optimization of the motion computed in (1a), subject to additional kinematic and
dynamic constraints.
The solution of optimal motion planning problems requires the use of motion inter-
polation tools, multibody dynamics methods, numerical integration, and nonlinear
-
2 1 Introduction
programming routines. These methods have reached in the last decades, indepen-
dently of each other, high levels of sophistication, and are available under comfortable
user interfaces, such as MATLAB. However, their composition to a functioning whole for
general applications is still an open problem, as the interactions between them are non-
trivial and the simple interlinking of modules typically yields unflexible and unwieldy
programs, hindering the interaction of the user with each of the modules.
The present work aims at combining geometric methods for spatial motion interpolation
with methods of multibody dynamics and optimal control, in order to provide a library
for the handling of a broad spectrum of applications, ranging from optimal roller-coaster
design, over robot palletizing operations with vacuum grippers, up to minimal-cycle-
time excavator operations under power constraints. In order to achieve this goal, the
work follows two main ideas not yet investigated in the current literature:
a) A general path planning library should allow for the description of spatial path
variations in the Euclidean space SE(3) of rigid-body motion. This is essential for
guided motions, as is the case of roller-coaster tracks, and of clear advantage for
motions whose representation in SE(3) is much simpler than the corresponding
representation in configuration space. Fig. 1.2 shows, for instance, a six-degrees-
of-freedom manipulator following two different end-effector planar paths. In tar-
get coordinates, both paths can be parametrized in terms of four parameters,
namely, the two coordinates of a via-point and two boundary tangent angles (see
sec. 3.2). In configuration space, the corresponding paths are more involved and,
thus, require a larger number of description parameters.
b) In general optimal motion planning problems, it is very difficult and often im-
possible to design an optimization algorithm which is able to find the global
optimum starting at an arbitrary initial guess, since practical problems are often
non-convex and filled with singularities and local valleys which may trap the opti-
mizer or even lead it to impossible configurations. In this setting, it is important
to provide an intuitive user interface through which the user can communicate
with the optimizer and guide it, for instance, by using physical path correction
subspace limitations or selecting locations at which the spatial track should be
modified. These requirements can be fulfilled by parametrizing variations of spa-
tial paths in terms of physical variations of via-poses.
In order to combine state-of-the-art multibody models with general optimal motion
planning algorithms, multibody motion is decomposed in two main components, namely,
-
1.1 Motivation 3
-2
3
0 0.2 0.4 0.6 0.8 1
-3
-1
0
1
2
a) target coordinates b) configuration coordinates
motion parameter u
axisan
gles
inrad
axis 1axis 2axis 3
axis 4axis 5axis 6
u0 = 0.0
uf = 1.0
t0
tf
multibody
system
end-effector
spatial pathx0
y0
z0
Figure 1.2: Target coordinates
the spatial path followed by the multibody system and the one-dimensional motion of
the system along this path. The geometry of the spatial path is parametrized using
curve fitting with B-splines, allowing for variations of the path’s via-points and bound-
ary conditions to be used as the design parameters of the optimization problem. The
motion along the path is then computed by integrating the projected equations of mo-
tion or by solving the optimal problem along the path, and the simulated kinematic
and dynamic time histories – measured at different poses on the multibody system –
are passed to state-of-the-art gradient-based optimization routines in the form of cost
and constraint functions.
Hereby, the use of via-poses and boundary conditions to parametrize a spatial path
offers an appropriate interface for the numerical optimization of the spatial path ge-
ometry, since it decomposes the variations of the spatial path in variations of a set of
design parameters with a qualitatively well-defined correlation to the variations of the
differential geometry of the path. In this way, high-level expert user input is made
possible, which helps to tackle the main problems encountered in motion planning:
a) Due to the inherent nonlinearities of the equations governing the kinematics and
dynamics of multibody systems, there is no known algorithm which can guarantee
the global optimality of a solution. The existing routines yield – if they converge
– only local optima, which are highly dependent on the initial guess supplied by
the user.
-
4 1 Introduction
b) Since local optima often lie on constraint boundaries, motions computed by stan-
dard optimization routines tend to be very sensitive to small disturbances and
inaccuracies in the real system. Thus, the optimal target motion computed by
an offline motion planning algorithm does not necessarily yield the optimal mo-
tion for the real system, in which case further iterations between offline motion
planning and tests on the actual system are necessary.
c) There is no general agreement on how to evaluate the “quality” of a given motion.
Usual motion quality measurements are computed as a weighted combination of
the motion cycle time, jerkiness, and consumed energy. The best choice for the
relative weights of these criteria, however, is highly problem-dependent and must
be tested, in general, by trial and error.
d) The convergence of existing numerical methods for optimal motion planning is
very sensitive to the choice of the optimization parameters as well as the formu-
lation of the cost and constraint functions.
1.2 State of the art
A complete survey of all the available literature and recent research findings in motion
planning is by far too extensive to be completed within the scope of this work. The
present section thus aims to give, instead, a broad overview of the different ideas which
have given shape to the latest approaches for motion planning and, in particular, to
the main goals of this dissertation.
1.2.1 Calculus of variations and optimal control
The search for optimality by mathematical means can be traced back to the late 17th
century, when calculus opened new possibilities to study real functions. Leibniz and
Newton, among others, studied the necessary conditions for the extrema of real func-
tions, pointing out that the tangent of a function must be horizontal at its extrema
and that the sign of the rate of change of the slope of the tangent at these extrema
distinguishes minima from maxima. Not many years later, these ideas were used in an
attempt to tackle a much more difficult problem: the problem of finding the function
which minimizes a given functional. This new type of mathematical problem arouse
in January 1697 when Johann Bernoulli proposed to determine the path between
two given points along which a point mass falls in the shortest time. Two different
-
1.2 State of the art 5
approaches to the solution were published in the same year by Johann Bernoulli
himself and his brother Jacob, respectively. Even though none of these solutions is
entirely satisfactory (Troutman, 1983), they both arrive to the right answer and can
be regarded as the beginning of the calculus of variations (Bliss, 1925).
In the hereon following centuries Euler, du Bois-Reymond, Legendre, Jacobi,
Weierstrass and Hilbert, among others, worked on formulating necessary and
sufficient conditions for free variational problems with continuous, piecewise-smooth
admissible functions. Their findings gave great insight in the nature of the solution
to these kind of problems, but lacked direct applicability in the emerging field of en-
gineering, since they relied on the smoothness of the admissible functions. Not until
the 20th century, a solid theory emerged capable to deal with functions which are
constrained not only at their boundaries but also inside their domain. This theory,
based on the work of McShane (1939), Bellman (1957), and Pontryagin et al. (1962),
regarded afterwards as “optimal control”, is a generalization of the classical calculus
of variations (McShane, 1989). It essentially fills in the gaps between the theory of
variations and modern engineering, since it allows for piecewise-continuous admissible
functions, which are inherent to most of engineering applications.
In the last decades, a great amount of resources has been invested in the develop-
ment of numerical methods for the solution of optimal control problems. Research has
continued in two directions: methods which attempt to solve general optimal control
problems, and methods which address only certain families of problems, taking ad-
vantage of their special form. The general methods can be divided into (1) indirect
methods, (2) direct methods and (3) dynamic programming. The indirect methods
correspond to the so-called “first optimize then discretize” approach. They are based
on the solution of the ordinary differential equations given by Pontryagin’s maximum
principle (Pontryagin et al., 1962). This is typically done by solving a boundary-value
problem by means of multiple shooting or collocation, as in the work by Bulirsch
(1971) and Dickmanns and Well (1975), or by minimizing the Hamiltonian function
using gradient-methods, as done by Miele (1980). The direct methods correspond to
the so-called “first discretize then optimize” approach. They are based on the dis-
cretization of the optimal control problem into a nonlinear finite-dimensional problem,
and can be classified as (2a) direct shooting methods, in which only control variables
or state variables are discretized and the equations of motion are fulfilled by numeri-
cal integration, as done by Chou (1978) and Kraft (1990), and (2b) direct collocation
methods, where both, control and state variables, are discretized and the equations
of motion are fulfilled only at some set of states called collocation points, as in the
-
6 1 Introduction
work of Renes (1978) and von Stryk (1993). The direct methods have become, by far,
the most widespread and successful techniques in optimal control, since they can deal
with inequality constraints by means of active set changes, available in nonlinear op-
timization routines such as the ones described by Fletcher (1987) (Diehl et al., 2009),
and lead to more intuitive user-interfaces which require less skills in the mathematical
theory of optimal control (von Stryk, 1995). Dynamic programming methods are based
on the application of Bellman’s principle of optimality, developed by Bellman (1957).
They are typically easy to implement, but consume large amounts of computational
resources, as they suffer from the so called “curse of dimensionality”, i. e. require very
small discretization grids (Luus, 1990).
The special methods rely on the form of the differential and algebraic equations defining
the problem to be solved and can therefore be applied to certain families of problems
only, as in the work of Graichen and Petit (2009). Of particular interest in this work
is the method for solving the time-optimal control of a multibody system along a pre-
scribed end-effector path, whose special form allows for a straight forward computation
of the control, as described in sec. 1.2.2.
1.2.2 Time-optimal control along a given spatial path
The problem of computing the time-optimal motion of a mechanism along a prescribed
spatial path for given initial and final states as well as maximally allowed actuator forces
was solved independently by Bobrow et al. (1985) and Shin and McKay (1985). Both
groups proposed to formulate the problem in terms of one single parameter by map-
ping the multibody equations of motion and the force constraints to a one-dimensional
motion along the prescribed spatial path. This allows for the representation of all ad-
missible states in a one-dimensional phase plane, where the boundaries of the admissible
regions in the plane as well as the maximal acceleration and maximal deceleration of
every admissible state are defined by the actuator force constraints. The time-optimal
solution is computed as a concatenation of motions with maximal acceleration and
maximal deceleration, such that its representation in the phase plane encloses the
largest possible area and, hence, lies tangent to the limits imposed by the actuator
force constraints. Bobrow et al. (1985) proved that such a motion is time-optimal, and
proposed an algorithm to find the solution for the case of constant actuator limits,
based on successive forwards and backwards integrations of the one-dimensional equa-
tions of motion. Shin and McKay (1985) proposed a very similar algorithm as well as
an extension for the case of velocity-proportional actuator forces. They also proved
-
1.2 State of the art 7
that if the parametric equations describing the spatial path are piecewise analytical,
their algorithm finds an optimum in a finite number of steps. Both contributions had
a great impact. Several research groups started working on the subject with the main
goals of perfecting the solution algorithm, extending it to a broader type of dynamic
constraints, and using it as an intermediate step for time-optimal path planning.
The most important modifications to the algorithm where proposed only years later.
Pfeiffer and Johanni (1987) noticed that the states on the boundaries of the admissible
regions behave in the phase plane either as sinks or as sources, and that the reduced
mass coefficients at the actuators’ degrees of freedom can become singular. Shiller
and Lu (1990) noticed that these singularities appear at states at which the velocity
constraints are active. They pointed out that this can happen not only at isolated
points on the boundaries but also at whole segments (singular arcs) and proposed a
modified algorithm, which is robust at these singularities. Slotine and Yang (1989)
proposed an algorithm based on the observations of Pfeiffer and Johanni (1987), in
which the boundaries of the admissible regions need not to be computed explicitly.
The algorithm has been applied in different contexts, including the computation of
the time-optimal motion of cooperating robots (Cho et al., 1990) and the computation
of CNC machines feed rate functions (Timar et al., 2005). Recently, the extension of
the solution algorithm to problems including higher derivatives constraints has been
become of great interest, since it would allow for the computation of ready-to-use
time-optimal controls. Unfortunately, this extension leads to much higher levels of
complexity. Constantinescu and Croft (2000) proposed to parametrize the description
of the motion in the one-dimensional phase plane by means of cubic splines in order
to optimize the total time subject to high-derivative constraints using the spline coeffi-
cients as optimization parameters, and a fitted approximation of the solution computed
by the traditional method – without the high-derivative constraints – as initial guess.
Messner et al. (2011) proposed to reformulate the optimal control problem by extending
the state vector to higher derivatives and choosing the highest derivative as the control
variable. If singular arcs are ignored, their method allows for an efficient computation
of smooth controls, which can be implemented in real-time. Nevertheless, ignoring the
singular arcs disregards constraints at the state variables, calling the mathematical
optimality of the resulting solution into question, especially if the acceleration along
the spatial path is part of the state vector and not a control variable.
Some alternative algorithms have been proposed which avoid searching for the switch-
ing points, including dynamic programming (Shin and McKay, 1986; Johanni, 1988)
-
8 1 Introduction
and convex optimization (Verscheure et al., 2009), which nevertheless exhibit some
limitations. The dynamic programming approach has been shown to be considerably
slower than the original method (Johanni, 1988). The convex optimization approach
can only handle dynamic constraints which can be formulated as a linear set of equa-
tions in terms of the acceleration and the square of the velocity along the spatial path.
1.2.3 Motion interpolation with splines
The problem of constructing a continuous function from given discrete data has been
known since very early times. Meijering (2002) has been able to trace the first attempts
to interpolate data back to the Uruk and Babylon culture three centuries B.C. Never-
theless, the first scientist to work out a solid mathematical theory of interpolation was
Newton, who referred to the problem of “describing a geometrical curve which shall
pass through any given points” as “one of the prettiest problems” that he could “ever
hope to solve”.
The first approaches to interpolation – including Newton’s – consisted in comput-
ing the unique polynomial that interpolates the given data by describing it using a
given basis of independent polynomials and computing the corresponding coefficients.
Different bases would yield different numerical performances but always the same in-
terpolant. The degree of the interpolating polynomial would be directly proportional
to the size of the interpolated data, limiting the applicability of these approaches to
very small sets of data.
The need for smoother interpolants led, on the late 1800’s, to the development of so-
called osculatory interpolation techniques. These techniques made use of piecewise
polynomials which join smoothly together – as “though kissing” –, allowing for suffi-
ciently smooth interpolation even for larger amounts of data. In 1946, Schoenberg
proposed the use of the term “spline curve of order k + 1” to name a curve of class
Ck−1 composed of polynomial arcs of degree k, called later basis splines or B-splines.
Only some years later, robust methods to perform computations with basis splines were
published, which made the use of splines very convenient. In 1959 de Casteljau de-
veloped a robust and efficient algorithm for handling a special type of basis splines
called Bézier curves. In 1972 de Boor and Cox published efficient algorithms for
calculating with general basis splines. These algorithms are still broadly used today.
On the late 1990’s, the fast development of computers turned the use of splines to a
standard in several areas of engineering, especially for curve fitting and computer aided
-
1.2 State of the art 9
geometric design. Two main reasons make splines so popular: (a) they offer the pos-
sibility to explicitly impose constraints such as periodicity, convexity, or smoothness,
which makes them a very flexible choice for fitting large amounts of data, and (b) their
parameters can be attributed geometric significance, which makes the editing of spline
geometries very intuitive and straight forward (Hoschek and Lasser, 1989). Impor-
tant contributions in these years include the curve-fitting routines by Dierckx (1993),
which are still broadly used for parameter estimation, data smoothing, functional rep-
resentation and data reduction, as well as the methods for three-dimensional motion
interpolation by Shoemake (1985) to interpolate Euler parameters on a curved three-
dimensional space, by Ge and Ravani (1994) to interpolate rotations in an Euclidean
hyperspace with a subsequently normalization of the interpolant, and by Jüttler (1994)
to interpolate spatial poses by using dual Quaternions.
Splines can also be designed to parametrize the solution of optimal control problems.
Egerstedt and Martin (2001), for instance, proposed the use of a spline with exponential
basis functions which are, by definition, solutions to linear optimal-control problems
under state and control variable constraints. Unfortunately, their work considers only
linear constraints and cannot be directly applied to the problems presented in this
work. An extension to nonlinear problems is very difficult and still an open issue.
1.2.4 Optimal motion planning
The most general optimal motion planning problem consists in finding the optimal
target motion of a mechanism performing a given task subject to geometric constraints
– such as given initial and final configurations as well as obstacles or joint limits – kine-
matic constraints – such as maximal joint velocities and accelerations – and dynamic
constraints – such as limits in the joint forces and moments as well as overall power
consumption. The quality of the mechanism’s motion is typically defined in terms of
the total cycle time, consumed energy, jerkiness of the motion, the deviation from given
target kinematics, or a combination of all of them.
Optimal motion planning problems are most commonly tackled using direct methods,
since the differential equations resulting from Pontryagin’s maximum principle are
very difficult to integrate numerically. These methods reformulate the optimal control
problem by transforming it into a nonlinear optimization problem with a finite number
of optimization parameters. To this end, the state variables, the input variables, or
both sets of variables are discretized, and the constraints and cost functionals are
-
10 1 Introduction
reformulated in terms of the discretized functions, allowing for the solution to be sought
in the neighborhood of a suitable initial guess by standard nonlinear optimization
routines.
Since the late 1970’s, polygons and splines have been a natural choice for the dis-
cretization of optimal control problems. Chou (1978), for instance, used B-splines with
constant uniform knot distribution to discretize simple problems with fixed cycle times
and solve them using gradient methods, with time as the B-spline parameter and the
spline coefficients as optimization parameters. He analyzed the computational effi-
ciency and accuracy of two different discretizations: the parametrization of the control
variables and the parametrization of the state variables, showing that their performance
with respect to each other depends on the type of problem to be solved.
The direct discretization of optimal path-planning problems with splines can be roughly
divided into two main approaches, depending on the domain of the basis splines. In
the first approach, the B-splines domain is a linear mapping of the cycle time and
the optimization parameters are the time intervals between the inner spline knots. All
time functions are computed by differentiating or integrating the B-splines analytically,
and evaluating the equations governing the kinematics of the system. This approach
is not suitable for problems where dynamic effects such as friction occur. In the sec-
ond approach, the B-spline parameter is either the path length of a reference spatial
curve or a general dimensionless parameter decoupled from time, and the optimization
parameters are either the spline coefficients, or the set of control points from which
the spline coefficients are computed. All time functions are determined by integrating
the projection of the equations of motion on the spline parameter or by computing an
optimal motion law along the trajectory. In both approaches, the discretization is done
either in joint coordinates or in target coordinates.
The method proposed by Wang and J.G. (1990) to compute the time-optimal motion
of serial manipulators between two given configurations is an example of the first
approach. They proposed to discretize the motion of each of the joints and to seek
for the optimal time at which each configuration is reached. The method was recently
extended by Gasparetto and Zanotto (2010) to include the minimization of the joint
jerks along the motion, allowing for the generation of sufficiently smooth, time-efficient
manipulator motions.
The method proposed by Bobrow (1988) and Johanni (1988) to compute the time-
optimal motion of serial manipulators between two given end-effector poses considering
-
1.3 Objective and overview 11
possible obstacles is an example of the second approach. They proposed to parametrize
the spatial path followed by the manipulator using B-splines, compute the time-optimal
motion along the given path using the methods mentioned in sec. 1.2.2, and then seek
for the feasible spatial path which would yield the smallest optimal cycle time by means
of a general purpose nonlinear constrained optimization program. They showed some
results for three-degrees-of-freedom planar serial manipulators and emphasized that
their method would only search for a local optimum in the neighborhood of a feasible
initial guess. Other examples of the second approach include methods based on genetic
algorithms, which aim to search for the optimum further away from the initial guess.
Yamamoto et al. (1994), for instance, proposed to formulate the optimal motion prob-
lem in configuration coordinates, compute multiple feasible initial guesses with genetic
algorithms and then search for local optima in the vicinity of these guesses with gra-
dient methods in order to select the best solution as a possible global optimum. Rana
and Zalzala (1996) proposed a similar method though using exclusively evolutionary
programs. Both works showed results only for simple two and three-degrees-of-freedom
planar serial manipulators.
A very different approach to path planning is the online generation of spatial path
segments as inputs for low-level motion controllers, such as in the work of Kröger and
Wahl (2010). Online algorithms focus on the local optimality of the target path at
every time step instead of seeking for the local or global optimality of the complete
motion. The quality of the motion as well as the kinematic and dynamic constraints
are recomputed at every sampling time as a function of a set of sensor data. This allows
for the controller to be able to switch between path-segment-following control, sensor-
guided control and sensor-guarded control within one framework, in order to react to
external disturbances and changes in the environment. Though these algorithms can
be used to tackle some of the practical problems addressed in this work, they have
different goals than offline algorithms and cannot be compared to them in a general
sense.
1.3 Objective and overview
The goal of this work is the development of an object-oriented library for handling opti-
mal motion planning problems using direct methods which integrates motion interpola-
tion tools, multibody dynamics methods, as well as nonlinear programming routines in
a computer-aided-design framework. The library should provide a geometry-oriented
-
12 1 Introduction
user interface with an intuitive visualization of the differential geometric properties of
the motion to be optimized, aiming to improve the condition of the resulting nonlinear
optimization problems by engaging the user in the creative aspects of the optimization
process.
The work is organized as follows: Chapter 2 reviews the main concepts underlying the
object-oriented modeling of mechanical systems as chains of kinetostatic transmission
elements, showing, in particular, how the motion of a multibody system can be mod-
eled as a kinetostatic transmission element mapping one single motion coordinate to
the motion of the whole system. This allows for the projection of the equations of
motion along a predefined spatial path within a multibody simulation setting. Chap-
ter 3 reviews the fundamentals of spatial-path interpolation using B-splines and curve
fitting algorithms. Chapter 4 describes state-of-the-art algorithms for the computa-
tion of time-optimal motion of multibody systems along given spatial paths subject to
kinematic and dynamic constraints. The algorithms are extended to arbitrary multi-
loop systems with general rigid-body motion with kinematic and dynamic constraints
including sticking friction and global power limits. Chapter 5 describes the object-
oriented library “IntelliSpline” for optimal motion planning. The main goal of this
library is to enable the use of the geometry of spatial paths as the main interface
between user and optimal motion planning algorithms, allowing for (a) an intuitive
editing of spatial path geometries with the help of motion interpolation tools, (b) the
prescription and editing of cost and constraint functions in terms of kinematic and
dynamic quantities for given spatial path geometries, (c) the computation of different
kinds of cost and constraint functions in terms of the system kinematics or dynamics,
by means of virtual measurements, and (d) the automatic optimization of spatial path
geometries with gradient-based algorithms. Chapter 6 summarizes the main points and
results of the work.
1.4 Notation
The use of some terms in the context of control theory and optimal control differs
slightly from its use in classical mechanics. In mechanics, for instance, the word “tra-
jectory” describes the path which a moving object follows through space as a function
of time. In control theory, the same word refers to a time-ordered set of states of a
dynamical system, which usually corresponds to the solution of a set of differential
equations for given initial conditions. In this work, the terminology used in control
-
1.4 Notation 13
theory is preferred over the terminology used in classical mechanics. In order to strive
for unambiguousness, some definitions are listed below.
- states of a multibody system: a minimal set of coordinates and their first time
derivatives, describing the configuration and velocities of a multibody system.
- state objects: scalar variables or spatial frames containing information about the
position, velocity, acceleration and generalized forces of an object in a multibody
framework.
- spatial curve: set of points C ∈ IR3 describing the three-dimensional trace of themotion of one point.
- spatial path: set of poses P ∈ SE(3) describing the three-dimensional trace ofthe motion of one rigid body or spatial reference frame.
- trajectory: time-ordered set of states of a dynamical system
- arc: subset of a trajectory
- parameterization: description of a function in terms of a set of parameters. Two
types of parametrizations are addressed in this work: (1) the parametrization of
spatial motions, which refers to the choice of the set of real parameters mapping
elements of SE(3) in IRn, with n ≥ 6 and (2) the parametrization of scalar orvectorial functions using one single scalar parameter, such as a spline parameter
u or a path length s, and a set of control vectors, such as spline coefficients or
via-points.
In this work, t denotes time and u represents a general, dimensionless curve parameter.
The symbol s is referred to as “motion coordinate” and corresponds either to (a)
the path length of a spatial curve C ∈ IR3 with units of length, or to (b) a generaldimensionless variable parametrizing the motion of a multibody system, for instance,
its joint coordinates. The time derivatives d(·)/dt are denoted as ˙(·), and the derivativesd(·)/ds with respect to the motion coordinate s are denoted as (·)′.
General vectors are assumed to be decomposed in the target frame. For other decom-
positions, the notation ki bj is used, where k denotes the frame of decomposition, i the
frame with respect to which the motion is measured, and j the target frame. The
index i is omitted for motions measured with respect to frame K0, and the index k isomitted for decompositions in the target frame. Furthermore, Ri denotes the rotation
-
14 1 Introduction
matrix transforming coordinates with respect to frame Ki to coordinates with respectto frame K0, and ã is used for the skew-symmetric matrix
ã =
0 −az ayaz 0 −ax−ay ax 0
(1.1)
generated by a 3-dimensional vector a = [ax ay az]T. With this skew-symmetric matrix,
the cross product c = a × b of two 3-dimensional vectors a and b can be expressed asthe matrix multiplication c = ã b.
-
15
2 Description of one-parametric motion as a
kinetostatic transmission
The present chapter reviews the main concepts underlying the object-oriented modeling
of mechanical systems by chains of kinetostatic transmission elements. It shows, in
particular, how multibody motion along a given spatial path can be modeled as a
kinetostatic transmission element mapping the motion of a single parameter to the
motion of the whole system. Most of the chapter is based on the framework for object-
oriented modeling of multibody systems developed by Kecskeméthy (1993) as well as
the concept of a “curve joint” used by Tändl (2009) to model roller-coaster motion.
These ideas are extended to any one-parametric description of motion, which allows
for the projection of the equations of motion along a predefined spatial path within a
multibody simulation setting.
2.1 The concept of kinetostatic transmission element
A mechanical system can be regarded as a concatenation of kinetostatic transmission
elements mapping motion and forces from one set of input state objects to a set of
output state objects. State objects are, in this context, spatial reference frames and/or
scalar variables, collecting positions and orientations as well as associated velocities,
accelerations and generalized forces.
Let the input and output state objects of an ideal kinetostatic transmission element
be collected in vectors qin
and qout
respectively. This allows for the element to be
represented as the block diagram shown in fig. 2.1.
qin
q̇in
q̈in
Qin
qout
q̇out
q̈out
Qout
position
velocity
acceleration
force
motion transmission
force transmission
Figure 2.1: Elementary kinetostatic transmission element (from Kecskeméthy (1993))
The motion transmission behavior described by the element comprises three sub-
-
16 2 Description of one-parametric motion as a kinetostatic transmission
operations
position: qout
= ϕ ( qin)
velocity: q̇out
= Jϕ q̇inacceleration: q̈
out= Jϕ q̈in + J̇ϕ q̇in
(2.1)
where Jϕ = ∂ϕ/∂qin represents the Jacobian of the transmission element. Furthermore,
since the transmission element is ideal, the virtual work at the input and output of the
element should be the same, i. e.
δqTinQ
in= δqT
outQ
out.
Substituting δqout
= Jϕ δqin and noting that this condition must hold for all δqin ∈ IRn,
yields the force transmission function
force: Qin
= JϕT Q
out. (2.2)
As shown by equation eq. 2.2, the force transmission takes place in opposite direc-
tion to the motion transmission and can be computed using the transposed velocity
Jacobian. This relationship is known as the “duality of velocity and force” and holds
independently of the complexity and nature of the motion transmission function ϕ (qin).
The state objects can be either scalar – e. g., the angle of a revolute joint or the distance
between a point and a plane – or spatial, if they parametrize the kinematical state of
a rigid body in space (see fig. 2.2). Scalar state objects contain a scalar variable β,
its time derivatives β̇ and β̈, and a corresponding generalized force Qβ. Spatial state
objects contain the position ri, orientation0Ri, linear velocity vi, angular velocity ωi,
linear acceleration ai, and angular acceleration ω̇i of a reference frame Ki with respectto an inertial frame K0, as well as the force f i and torque τ i acting on frame Ki.
{β} =
β
β̇
β̈
Qβ
(a) scalar state object
K0
Kiri
0Ri
tiṫi
wi
(b) spatial state object
Figure 2.2: State objects (from Kecskeméthy (1993))
-
2.1 The concept of kinetostatic transmission element 17
In this work, the linear and angular velocity vectors as well as the force and torque are
collected in the vectors
ti =
[ωi
vi
]and wi =
[τ i
fi
], (2.3)
referred to as “twist” and “wrench” respectively. Similarly, the kinematical quantities
are collected in expressions of the form {β} for scalar quantities, or simply Ki forspatial quantities. All elements of a spatial state object are given in coordinates of the
target frame Ki unless otherwise explicitly specified.
As an example of a kinetostatic transmission element, the model of a cylindrical joint
is displayed in fig. 2.3. It describes the absolute motion of the output frame K2, as afunction of the absolute motion of an input frame K1 as well as a relative translations and a relative rotation ϕ with respect to K1 along a given unit vector t fixed to theinput frame K1.
K1
K2
sϕ
t
(a) Mechanical model
{s}, {ϕ}K1
Qs, Qϕ
w1
K2
w2
t
motion transm.
force transm.
(b) Block diagram
Figure 2.3: Elementary joint (from Kecskeméthy (1993))
The position and orientation of the output frame K2 with respect to the inertially fixedframe K0 is computed from the equations
0R2 =0R1∆R , with ∆R = Rot [ t, ϕ ] , and
r2 = ∆RT (r1 +∆r) , with ∆r = s t .
(2.4)
The velocity transmission takes the form
[ω2
v2
]=
[∆RT 0 t 0
−∆RT ∆̃r ∆RT 0 t
]
ω1
v1
ϕ̇
ṡ
, (2.5)
-
18 2 Description of one-parametric motion as a kinetostatic transmission
and the acceleration transmission can be written as
[ω̇2
a2
]=
[∆RT 0 t 0
−∆RT ∆̃r ∆RT 0 t
]
ω̇1
a1
ϕ̈
s̈
+
[ϕ̇ ω̃2 t
∆RT ω̃1 (s ω̃2 t + 2 ṡ t)
]. (2.6)
Eq. 2.5 eq. 2.2 yield for the force transmission
τ 2
f2
Qϕ
Qs
=
∆R ∆̃r∆R
0 ∆R
tT 0T
0T tT
[τ 1
f1
]. (2.7)
Applied forces – which in general are functions of the system’s state – and inertia forces
acting on a given frame may also be regarded as kinetostatic transmission elements.
They constitute a special type of transmission elements often referred to as “leaf ele-
ments”, since their output state objects cannot be used as input state objects of further
elements. They do not transmit motion in the general sense, but use the information
contained in the input state objects to compute the wrench which is to be transmitted
to their output state objects.
K1
∆rS1ṫ1
m1, ΘS1
S1
(a) Mechanical model
∆rS1 , m, ΘS
K1
w1
(KS1)motion transm.
force transm.
(b) Block diagram
Figure 2.4: Mass element (from Kecskeméthy (1993))
Fig. 2.4 shows this for the case of a mass element with the mass m1 and the inertia
tensor ΘS1 attached to a frame K1 with an offset ∆rS1 . The inertial or d’Alembertforce transmitted to frame K1 is computed from the linear acceleration a1, angularvelocity ω1 and angular acceleration ω̇1 of frame K1 as
[τ 1
f1
]=
[− (ΘS1 ω̇1 + ω1 ×ΘS1 ω1) + ∆rS1 × f1
−m1(a1 + ω̇1 ×∆rS1 + ω1 ×
(ω1 ×∆rS1
))]. (2.8)
-
2.1 The concept of kinetostatic transmission element 19
Not only typical mechanical components but also more general mappings such as scalar
metrics and geometrical constraints can be regarded as kinetostatic transmission ele-
ments. Any passive physical or mathematical object which maps a set of input state
objects to a set of output state objects can be regarded as an element of the form
described by eq. 2.1 and eq. 2.2.
2.1.1 Multibody systems as concatenations of kinetostatic transmission
elements
One of the most powerful features of the concept described in sec. 2.1 is that a con-
catenation of kinetostatic transmission elements itself can be regarded as one global
kinetostatic transmission element mapping global input state objects into global out-
put state objects, which allows for different levels of abstraction and analysis. In the
case of a whole mechanism, the global input state objects correspond to the general-
ized coordinates of the system and the output state objects can be set to be e. g. the
end-effector frame as well as the frames on which the external and inertia forces act.
This allows for an efficient computation of Jacobian matrices as well as the projection
of applied forces and inertial properties on the generalized coordinates.
Fig. 2.5 shows a serial mechanism consisting of four bodies, two revolute joints defined
by the angular variables ψ and φ, and one prismatic joint defined by the linear variable
s, with K0 being the inertial frame and frame KE, the end-effector frame. As shown infig. 2.5, the manipulator can be regarded as two concatenated kinetostatic transmis-
sion elements. The first element corresponds to a kinetostatical skeleton mapping the
scalar kinematical states {ψ}, {ϕ}, and {s} to the spatial kinematical states KE andK2, K3, and K5. The second element corresponds to a set of mass and force elementstransmitting the applied and inertial forces into the system. Traversing these elements
by evaluating the motion transmission for given state objects at the generalized coor-
dinates {ψ}, {ϕ}, and {s}, and subsequently evaluating the force transmission for agiven external wrench at KE and inertia properties at K2, K3, and K5 yields the inversedynamics of the mechanism, i. e. the generalized forces −Qψ, −Qϕ, and −Qs neededfor static equilibrium.
A representation of the same mechanism with inverted kinetostatical skeleton is shown
in fig. 2.6. The inverted transmission behavior can be obtained by either inverting
the transmission equations of the kinetostatical skeleton of fig. 2.5, or by formulating
and solving the corresponding closure conditions with the aid of special kinetostatical
-
20 2 Description of one-parametric motion as a kinetostatic transmission
K0
K1
K2
K3
K4
K5
KE
s
ψ
ϕ
m2, Θ2
m3, Θ3
m5, Θ5
(a) Mechanical model
{s}, {ψ}, {ϕ}K0
Qs, Qψ, Qϕ
w0
KEK2, K3, K5wE
w2, w3, w5
motion transm.motion transm.
force transm.force transm.
kinetostatical skeleton applied/inertia forces
(b) Block diagram (direct kinematics, inverse dynamics)
Figure 2.5: A mechanism as a kinetostatic transmission element
transmission elements, as shown by Kecskeméthy and Hiller (1994). Traversing the
resulting kinetostatic chain yields the direct dynamics of the mechanism.
2.1.2 Computation of the transmission Jacobians
Computations in object-oriented multibody libraries are typically based on a Jacobian-
free formulation, since this simplifies considerably the implementation of new elements
and the expansion of the library. In order to compute the Jacobians, the following
derivative-free methods proposed by Kecskeméthy and Hiller (1994) are available.
Regard the general kinetostatic transmission element depicted in fig. 2.1. The velocity
-
2.1 The concept of kinetostatic transmission element 21
KEK0wE
w0
{s}, {ψ}, {ϕ}K2, K3, K5Qs, Qψ, Qϕ
w2, w3, w5
motion transm.motion transm.
force transm.force transm.
kinetostatical skeleton applied/inertia forces
Figure 2.6: Inverse kinematics and direct dynamics for mechanism of fig. 2.5
Jacobian Jϕ can be computed by using (a) the element’s velocity transmission or (b)
the element’s force transmission.
a) Velocity-based determination of Jacobians
Setting all velocity components at the input of the transmission element besides
the jth-one equal to zero, and the jth-one equal to one, yields an output velocity
vector which is identical to the jth-column of the Jacobian, i. e.
jth-column of Jϕ = q̇out
∣∣∣∣q̇(i)in =
{1 for i = j
0 otherwise
. (2.9)
b) Force-based determination of Jacobians
Setting all force components at the output of the transmission element besides the
jth-one equal to zero, and the jth-one equal to one, yields a vector of generalized
forces at the input the transmission element which is identical to the jth-column
of the transposed Jacobian, thus to its jth-row, i. e.
jth-row of Jϕ = Qin
∣∣∣∣Q
(j)out =
{1 for i = j
0 otherwise
. (2.10)
2.1.3 Generation of the equations of motion in minimal form
The inverse dynamics ϕD−1
S of a general mechanism with n degrees of freedom – with
serial or closed kinematical topology – can be regarded as a function mapping the
generalized coordinates q ∈ IRn and their time derivatives q̇, q̈ to a set of generalized
-
22 2 Description of one-parametric motion as a kinetostatic transmission
forces Q at the input of the kinetostatical skeleton. Traversing this function yields
Q = ϕD−1
S ( q , q̇ , q̈ ) = −M (q) q̈ − b (q, q̇) +Qe (q, q̇ ) +QG(q) , (2.11)
where M is the n × n mass matrix of the manipulator, b(q, q̇) is the n-dimensionalvector containing the centripetal and Coriolis terms, Q
G(q) is an n-dimensional vector
containing the projection of the gravitational forces on the generalized coordinates,
Qe(q, q̇) is an n-dimensional vector containing the projection of general external forces,
and Q is an n-dimensional vector collecting the generalized actuator forces. The forces
Q are usually referred to as “residual forces” and correspond to the generalized forces
required for static equilibrium.
{q}K0Q
w0
motion transm.motion transm.
force transm.force transm.
kinetostatical skeleton applied/inertia forces
inverse dynamics ϕD−1
S
Figure 2.7: Model of the inverse dynamics of a multibody system
The residual forces Q can be used to generate M(q) and Q̂ at every state [q, q̇]T, where
Q̂ = −b(q, q̇) +Qe(q, q̇) +Q
G(q), by the following procedure:
a) Computation of Q̂
Evaluate ϕD−1
S by setting the generalized accelerations to q̈ = 0. The resulting
vector Q of residual forces is exactly Q̂.
b) Computation of M
Evaluate ϕD−1
S by “switching off” the forces Q̂ and setting the all input acceler-
ations besides the jth-one to zero, and the jth-one to one. The resulting vector
Q of residual forces corresponds to the jth-column of mass matrix M, i. e.
jth-column of M = −Q∣∣∣∣q̈i =
{1 for i = j
0 otherwise
. (2.12)
-
2.2 Spatial paths as kinetostatic transmission elements 23
The equations of motion in minimal form can then be written as
M(q)q̈ + b(q, q̇)−Qe(q, q̇)−Q
G(q)−Q = 0 , (2.13)
where Q is the vector of applied generalized forces acting at the input state objects.
The generation of the equations of motion by this approach requires n + 1 traversals
of the inverse dynamics for one set of equations.
2.2 Spatial paths as kinetostatic transmission elements
2.2.1 Spatial curves in IR3 and spatial paths in SE(3)
A spatial curve is the image of an open, closed, half open, finite or infinite interval
Du ⊂ IR under a continuous, locally injective mapping into IR3 (Hoschek and Lasser,1989). It can be considered as a set of vectorial values ∆r ∈ IR3 of a locally one-to-onevector-valued function ∆r = ∆r(u), with u ∈ Du. In this work, the interval Du isalways a closed interval Du = [u0, uf], and the vector ∆r(u) is interpreted as the vector
defining the position of one point on the curve with respect to a reference frame K1(see fig. 2.8).
K1K1x1x1 y1y1
z1z1
s
∆r(u)∆r(u0)
∆r(uf)
u = u0
u = uf
t(u)
n(u)b(u)
C(u)C(u)
a) point coordinates b) intrinsic directions
Figure 2.8: Differential geometry of a spatial curve
The length of the curve is defined as
ℓ =
∫ ufu0
∣∣∣∣∣∣∣∣d∆r(u)
du
∣∣∣∣∣∣∣∣ , (2.14)
-
24 2 Description of one-parametric motion as a kinetostatic transmission
and the vector
t(u) =
d∆r(u)
du∣∣∣∣∣∣∣∣d∆r(u)
du
∣∣∣∣∣∣∣∣
(2.15)
is tangent to the curve.
In particular, if
∣∣∣∣∣∣∣∣d∆r(u)
du
∣∣∣∣∣∣∣∣ = 1, then the parameter u is called path length of the curve.
If the curve is at least twice continuously differentiable, its deviation from a straight
line can be measured by the real number
κ(u) =
∣∣∣∣∣∣∣∣d∆r(u)
du× d
2∆r(u)
du2
∣∣∣∣∣∣∣∣
∣∣∣∣∣∣∣∣d∆r(u)
du
∣∣∣∣∣∣∣∣3 , (2.16)
called the curvature of the curve.
If the curve is at least three times continuously differentiable, its deviation from being
planar can be measured by the real number
τ(u) =
[d∆r(u)
du× d
2∆r(u)
du2
]· d
3∆r(u)
du3∣∣∣∣∣∣∣∣d∆r(u)
du× d
2∆r(u)
du2
∣∣∣∣∣∣∣∣2 , (2.17)
called the torsion of the curve.
It can be shown that the real-valued functions κ(u) and τ(u) form a complete invariant
system and hence define uniquely a spatial curve up to rigid-body transformation.
A parametrization ∆r(u) can be transformed into a new parametrization ∆r(s) with
domain Ds = [s0, sf] via a parameter transformation s = u(s), provided that u(s)
is an invertible function. In particular, if s is the path length of the curve, i. e. if∣∣∣∣∣∣∣∣d∆r(s)
ds
∣∣∣∣∣∣∣∣ = 1, equations eq. 2.17, eq. 2.16 and eq. 2.15 yield the intrinsic directions
t(s) = ∆r′(s) ,
n(s) =∆r′′(s)
κ(s), with κ(s) = ‖∆r′′(s) ‖ ,
b(s) = t(s)× n(s) , (2.18)
called tangent, normal and binormal vectors, respectively. Deriving these equations
-
2.2 Spatial paths as kinetostatic transmission elements 25
with respect to the path length s yields the well-known Frenet-Serret equations
t′(s) = κ(s) n(s)
n′(s) = −κ(s) t(s) + τ(s) b(s)b′(s) = −τ(s) n(s) . (2.19)
A spatial path P(u) is the image of the closed interval Du ⊂ IR under a continuous,locally injective mapping into the 3 dimensional special Euclidean group SE(3). It
consists of a spatial curve C(u) describing the translation of the origin of a givenreference frame K2 with respect to a given reference frame K1, and a rotation ∆R(u)describing its orientation at every point ∆r(u) along the curve C with respect to K1 asa function of the parameter u.
K1
K2
x1 y1
z1
∆R(u)∆r(u)∆r(u0)
∆r(uf)
u = u0
u = uf
C(u)
P(u)
Figure 2.9: Representation of a spatial path
There are three main approaches for defining the rotation matrix ∆R(u):
a) Tangent-frame parametrization:
Also referred to as “framing” of the curve (Bishop, 1975). The rotation ∆R(u) is
prescribed as a function of the differential geometry of the curve C, with one of theaxis of the traveling frameK2 always tangent to the curve C. This parametrizationis particularly useful for guided rigid body motion, where the rigid body slides
along a physical track or in applications where the tangent to the curve is of
technical relevance.
-
26 2 Description of one-parametric motion as a kinetostatic transmission
b) Orientation parametrization relative to a tangent frame:
The rotation ∆R(u) is given as a concatenation of two rotations, the first one
yielding a tangent frame, the second one representing additional elementary rota-
tions relative to the tangent frame. This parametrization is particularly useful in
applications where the information contained by the tangent frame is of technical
relevance although a general orientation is to be prescribed.
c) General orientation parametrization:
The rotation ∆R(u) is defined as an explicit function of the parameter u using
one set of rotation parameters such as Euler parameters (unit Quaternions)
or Euler-angles. The proper choice of these rotation parameters depends on
the nature of the path to be parametrized. A comparison between the different
possibilities exceeds the scope of this work. For a summary on the properties
of rigid body motion depending on the choice of the orientation parametrization
refer to the work of Kecskeméthy (2004).
These approaches and their implementation in the framework of object-oriented multi-
body modeling are discussed in detail in the following section.
2.2.2 The concept of a curve joint
Let a general spatial path be given by the pose of an output frame K2 = P(s) ∈ SE(3),with the translation part C(s) ∈ IR3 of P(s) described by vector ∆r(s) and the rotationpart parametrized by a rotation matrix ∆R(s), both measured with respect to a basis
frame K1. Let the coordinate s be the path length of C(s), as shown in fig. 2.10.
The pose of K2 can be computed as a function of the pose of the basis frame K1 andthe path coordinate s as
0R2 =
0R1∆R , with ∆R = ∆R(s) , and
r2 = ∆RT (r1 +∆r) , with ∆r = ∆r(s) .
(2.20)
The velocity transmission takes then the form[ω2
v2
]=
[∆RT 0
−∆RT∆̃r ∆RT
]
︸ ︷︷ ︸Jg
[ω1
v1
]+ JP ṡ , (2.21)
where Jg is the rigid-body Jacobian, and JP is the Jacobian mapping the path velocity
ṡ along the spatial path to the twist at the output frame K2.
-
2.2 Spatial paths as kinetostatic transmission elements 27
PK0
K1
K2
s
r1
r2
∆r
∆R
R1
R2
(a) Mechanical model
{s}K1Qs
w1
K2
w2
spatial path P(s)
motion transm.
force transm.
(b) Block diagram
Figure 2.10: Curve joint as a kinetostatic transmission element (from Tändl (2009))
The acceleration transmission can be written as[ω̇2
a2
]= Jg
[ω̇1
a1
]+
[0
2ω̃21∆RT∆r
]+ JP s̈ + JP
′ ṡ2 +
[2ω̃1 0
0 2 2ω̃1
]JP ṡ , (2.22)
where (.)′ denotes the derivative with respect to the path coordinate s (sec. 1.4).
According to eq. 2.2 the force transmission yieldsτ 1
f1
Qs
=
[JTg
JPT
][τ 2
f2
]. (2.23)
The functions ∆r(s), ∆R(s), and JP depend on the description of the spatial path P(s).Typically, the parametrization ∆r(s) of the spatial curve is computed by interpolating
or approximating given via positions of the output frame K2 with respect to the inputframe K1. This is explored in detail in sec. 3. The rotation matrix ∆R(s) is thenprescribed as a function of the differential geometry of the curve or, in the most general
case, with respect to the input frame K1.
2.2.3 Tangent frame parametrization
For some applications, it is convenient to parametrize the orientation ∆R(s) of the
guided frame K2 as a function of the differential geometry of the curve, as done by
-
28 2 Description of one-parametric motion as a kinetostatic transmission
Tändl et al. (2007) to model roller-coaster motion. In these cases, the first axis of K2is chosen tangent to the curve, and ∆R(s) is referred to as the “framing” of the curve
described by ∆r(s).
K0
K1
K2
t
n
b
∆r′′
∆r
r1
r2
C
s
Figure 2.11: Frenet-frame parametrization (from Tändl (2009))
The most natural way to frame a curve is to make use of the directions defined by
eq. 2.18 and choose the tangent t(s), normal n(s), and binormal b(s) vectors as the
axes of the guided frame K2. The orientation of K2 with respect to K1 becomes
∆R =[1t(s) 1n(s) 1b(s)
]. (2.24)
The Jacobian JP and its first derivative JP′ with respect to the path coordinate s are
computed as
JP =
[u
t
], u =
t′′zκ
0
κ
, t =
1
0
0
JP′ =
[u′
t′
], u′ =
t′′′zκ
−2 t′′y t
′′z
κ2
0
t′′y
, t
′ =
0
t′y
t′z
, (2.25)
where u is the direction of the angular velocity of frame K2 for ṡ = 1, t is the tangentvector, and t′′ = [t′′x t
′′y t
′′z ] and t
′′′ = [t′′′x t′′′y t
′′′z ] are the second and third derivatives of t
with respect to s, all decomposed in K2.
-
2.2 Spatial paths as kinetostatic transmission elements 29
Unfortunately, equations eq. 2.24 and eq. 2.25 become singular at inflexion points
and straight-line segments, i. e. at points where the curvature κ(s) vanishes. These
first-order singularities can be treated efficiently by constructing a smooth vector n̄(s)
which is parallel to n(s) only at points far away from the inflection points, as shown by
Kecskeméthy and Tändl (2006). The resulting framing ∆R =[1t(s) 1n̄(s) 1b̄(s)
]does
not yield the exact Frenet-frame, but does contain a smooth representation of the
natural directions of the curve ∆r(s), which turns out to be very useful in practice.
Bishop (1975) proposed another way of framing a curve which consists in constructing
the second and third directions of K2 such that the relative angular velocity of theframe along the tangent is zero at every point on the curve. This requires numerical
integration of a set of ordinary differential equations and, as shown by Tändl (2009),
is not well suited for efficient kinematic analysis at arbitrary, non-consecutive points
along the curve.
A further option, calledDarboux-frame parametrization, consists in generating ∆R(s)
such that the second axis of the guided frame K2 is always perpendicular to a givenvector field h(s). This field plays the role of a vector normal to the horizon spanned
by the first and second axes of the traveling frame K2.
In this case, setting t as the first axis yields for the second axis the term h(s)×t(s)/λ(s),with λ(s) = ‖ h(s)× t(s) ‖, and the rotation matrix ∆R becomes
∆R =
1t(s)
1h̃(s)1t(s)
λ(s)
1t̃(1h̃(s)1t(s)
)
λ(s)
. (2.26)
The Jacobian JP and its first derivative JP′ can be written as
JP =
[u
t
], JP
′ =
[u′
t′
], u =
hxt′y − h′yλ
−t′zt′y
,
u′ =
−λh′′y + 2λh′xt′y + λhxt′′y + t′zt′yλ2 + 2λ′h′y − 2λ′hxt′yλ2
−t′′zλ+ 2t
′zh
′z − 2hx(t′z)2 − 2t′zλ′ + t′yh′y − hx(t′y)2
λ2h′zt
′y − t′yhxt′z − 2t′yλ′ + t′′yλ− h′yt′z
λ,
, (2.27)
with λ′ = h′z − hxt′z.
-
30 2 Description of one-parametric motion as a kinetostatic transmission
K0
K1
K2
∆r
r1r2
C
s
exey
ezh(s)
Figure 2.12: Darboux-frame parametrization (from Tändl (2009))
TheDarboux-frame parametrization is the most general of curve framings and is well-
suited for applications where the function h(s) can be given a physical or geometrical
meaning. It has been used, for instance, by Müller et al. (2004) for orienting frames
moving along a curve contained on a surface with respect to the surface normal, and
by Scholz et al. (2012) for a singularity-free framing of planar trajectories.
It should be noted that this parametrization becomes singular at points along the curve
where vector h(s) is tangent to the curve. Hence, h(s) is usually chosen such that the
angle between h(s) and t(s) does not become too small.
2.2.4 Orientation parametrization with respect to a tangent frame
Even in applications requiring a more general motion parametrization, it may be con-
venient to define the orientation of the guided frame with respect to the natural direc-
tions given by a tangent-frame parametrization. When possible, this offers an intuitive
understanding of the nonlinear relationship between the rotation parameters and the
resulting spatial motion.
Fig. 2.13 shows one particularly convenient parametrization which consists in append-
ing three additional consecutive transformations to a tangent frame, namely, a rotation
β(s) about the tangential axis – hereafter referred to as “banking” –, a rotation ϕ(s)
about the rotated transversal axis – hereafter referred to as “leaning” –, and a constant
displacement ℓ along the rotated vertical direction. The guided frame is denoted here
-
2.2 Spatial paths as kinetostatic transmission elements 31
as K4 and the motion of the tangent frame K2 is supposed to be given by one of thetangential parametrizations presented in sec. 2.2.3.
K1
K2
K4
x2y2
z2y3
ℓ
β(s)
ϕ(s)
Figure 2.13: Superposition of “banking” and “leaning” angles with respect to a tangent
frame
The rotation matrix of the guided frame K4 takes the form0R4 = ∆R · 2R4 , with 2R4 = Rot [ x, β(s) ] · Rot [ y, ϕ(s) ]r4 =
2RT4 r2 + rrel , with rrel = [ 0 0 −ℓ ]T ,(2.28)
where the angles β(s) and ϕ(s) are prescribed as a function of the path coordinate s.
The linear and angular velocities of frame K4 can be computed as[ω4
v4
]=
[2RT4 0
−2RT4 r̃rel 2RT4
][ω2
v2
]+
[urel
ũrelrrel
]ṡ , (2.29)
and the linear and angular accelerations as
[ω̇4
a4
]=
[2RT4 0
−2RT4 r̃rel 2RT4
][ω̇2
a2
]+
[0
4ω̃22 rrel
]+
[urel
ũrel rrel
]s̈ +
[u′rel
ũ′rel rrel + ũ2rel rrel
]ṡ2 +
[4ω̃2 urel
24ω̃2 ũrelrrel
]ṡ , (2.30)
with
urel =
β ′ cosϕ
ϕ′
β ′ sinϕ
, u′rel =
β ′′ cosϕ− β ′ϕ′ sinϕϕ′′
−β ′′ sinϕ− β ′ϕ′ cosϕ
. (2.31)
Assuming an ideal transmission, the force transmission can be written asτ 1
f1
Qs
=
[JTg 0
JPT 1
]
2R4 r̃rel2R4
0 2R4
uTrel −rTrelũrel
[τ 4
f4
]. (2.32)
-
32 2 Description of one-parametric motion as a kinetostatic transmission
2.2.5 General frame parametrization
Some applications require a general spatial path parametrization P(s) with the ori-entation component also prescribed with respect to the input frame K1. This can beachieved by choosing a set of rotation parameters describing the orientation ∆R(s) of
the output frame K2 with respect to frame K1 and prescribing them as functions ofthe path coordinate s. There are some standard rotation parametrizations available
such as Euler angles, Bryant angles, Euler parameters and Rodrigues parame-
ters. For most applications, the use of Euler parameters is the best choice, since they
allow for smooth interpolation in a singularity-free framework.
In this work, general orientation parametrization is carried out using Euler param-
eters, which are usually represented by the four components of the unit Quaternion
P =[p0, p
T]T ∈ IR4, with
‖P ‖2 = p20 + p2 = 1 (2.33)
where p0 ∈ IR and p ∈ IR3 are the real and vectorial part of the Quaternion respectively.Eq. 2.33 is the normalizing condition and must be fulfilled in order for the components
of the Quaternion P to represent Euler parameters, i. e. a general vectorial function
P (s) ∈ IR4 which describes a spatial rotation must fulfill eq. 2.33 for all values ofs ∈ Ds. There are two main approaches to generate feasible functions P (s): (a) togenerate spherical functions contained in the four-dimensional unit sphere intrinsically
fulfilling eq. 2.33, or (b) to generate a general four dimensional vectorial function
P̂ (s) ∈ IR4 and normalize it by computing
P (s) =P̂ (s)
‖ P̂ (s) ‖. (2.34)
Consider a general function P̂ (s) ∈ IR4 whose projection P (s) on the four-dimensionalunit sphere represents Euler parameters describing the orientation of the output
frame K2 with respect to the input frame K1. The corresponding rotation matrix canbe computed as
∆R = I+2
‖ P̂ ‖2[p̂0 ˜̂p + ˜̂p
2]. (2.35)
The Jacobian JP mapping the path velocity ṡ along the spatial path to the twist at
-
2.2 Spatial paths as kinetostatic transmission elements 33
K0
K1
KE
r1
rE s
C
∆r
∆R(P̂ (s))
Figure 2.14: Quaternion parametrization
the output frame K2 is
JP =
[u
t
], with u = EP̂
′,
E =
−E1 E0 −E3 E2−E2 E3 E0 −E1−E3 −E2 E1 E0
,
Ei =2p̂i
‖ P̂ ‖2, ∀i = 0, · · · , 4 , (2.36)
where t is the tangent vector decomposed in K2.
The derivative of the Jacobian JP with respect to the path coordinate s becomes
J′P =
[u′
t′
], with u′ = EP̂
′′ −ETP̂ ′u ,
E =[E0 E1 E2 E3
]T. (2.37)
Note that these equations are also valid for the particular case that P̂ (s) is a unit
Quaternion with ‖ P̂ (s) ‖ = 1 for all s ∈ Ds.
-
34 2 Description of one-parametric motion as a kinetostatic transmission
2.3 Joint-motion parametrization as a kinetostatic transmis-
sion element
Let vector q collect a set of n joint coordinates defined by a continuous, locally injective
function q = η(s) ∈ IRn in the domain s ∈ Ds, with Ds = [s0, sf] ⊂ IR. The nonlinearmapping between s and q is called in this work “joint-motion parametrization” and
can be interpreted as a kinetostatic transmission element mapping motion and forces,
as shown in fig. 2.15. The coordinate s represents the progression of the joint-motion
q along the parametrization η(s) and is dimensionless.
{s}K1Qs
w1
q
Q
joint-motion
parametrization η(s)
motion transm.
force transm.
Figure 2.15: Joint-motion parametrization as a kinetostatic transmission element
In sec. 4, either the concept of joint-motion parametrization (fig. 2.15) or the concept
of a curve joint (fig. 2.10) will be used to rewrite the direct or inverse kinetostatics of a
multibody system as a function of one single parameter s. This parameter is referred to
as the “motion coordinate” since it decouples the geometry of the spatial path followed
by the multibody system and the time dependency of its motion along it. In this work,
the curve joint approach is generally preferred since it allows for a direct rendering of
the resulting spatial paths. Joint-motion parametrization is used mainly in applications
where the end-effector does not need to fulfill any geometrical constraints along the
given spatial path and, in particular, in the special case of multibody systems with
limited degrees of freedom.
-
35
3 Motion interpolation using B-splines
Splines are smooth piecewise-polynomial functions widely used in several areas of nu-
merical analysis, including data interpolation, data smoothing, computer aided geo-
metric design, and numerical methods for solving differential and integral equations.
In this work, splines are used to parametrize the spatial paths followed by mechanical
systems by interpolating or approximating either given joint configurations or given
end-effector poses. This yields path geometries which are uniquely defined by a finite
set of discrete parameters, allowing for their editing either by hand, e. g. with the help
of a suited graphical interface, or by optimization routines, e. g. nonlinear programming
algorithms.
The present chapter reviews the fundamentals of spline generation, focusing on methods
which are particularly relevant for spatial motion interpolation.
3.1 Fundamentals of spline generation with B-splines
A function f(u) ∈ IRn, defined on a finite interval Du = [u0, uf] ⊂ IR is called a splinefunction of degree k > 0 (of order k+ 1) with the strictly increasing sequence of knots
λ0 = u0 < λ1 < · · · < λg+1 = uf, where g is referred to as number of internal knots, if
1) on each knot interval [λj, λj+1], the function f(u) is given by a polynomial of
degree k at the most, and
2) the function f(u) and its derivatives up to order k − 1 are all continuous on Du.
Even though every polynomial on Du of degree smaller or equal to k is also a spline
function of degree k on Du, a spline f(u) of degree k is generally given by a different
polynomial of degree smaller or equal to k in each of the knot intervals [λj , λj+1]. Thus,
a spline f(u) of degree k might show discontinuities in its kth-order derivative at the
internal knots λ1, λ2, . . . , λg.
-
36 3 Motion interpolation using B-splines
In particular, a function Ni,k+1(u) defined by the recursive formulae (De Boor, 1978)
Ni,ℓ+1(u) = ωi,ℓ+1(u)Ni,ℓ(u) + [1− ωi+1,ℓ+1(u)] Ni+1,ℓ(u) ,
ωi,ℓ+1(u) =
u− λiλi+ℓ − λi
, if λi < λi+ℓ ,
0 , otherwise ,
Ni,1(u) =
{1 , if u ∈ [λi, λi+1[ ,0 , otherwise
(3.1)
is a spline of degree k in the interval [λi, λi+k+1] for the particular case of a strictly
increasing sequence of knots λi < λi+1 < · · · < λi+k+1. This becomes evident whenwriting Ni,k+1(u) explicitely as
Ni,k+1(u) = (λi+k+1 − λi)k+1∑
j=0
(λi+j − u)k+k+1∏
ℓ=0, ℓ 6=j
(λi+j − λi+ℓ), (3.2)
with the truncated power function (u− c)k+ being defined by
(u− c)k+ ={
(u− c)k , if u ≥ c ,0 , if u < c .
(3.3)
Furthermore, the k + g + 1 independent functions Ni,k+1(u) defined by eq. 3.1 for the
sequence of knots λ−k, λ−k+1, . . . , λg+k+1, with
λ−k ≤ λ−k+1 ≤ . . . ≤ λ−1 ≤ λ0 = u0 , called left boundary knots,u0 < λ1 < . . . < λg < uf , called internal knots, and
uf = λg+1 ≤ λg+2 ≤ . . . ≤ λg+k ≤ λg+k+1 , called right boundary knots,(3.4)
form a full linear basis which spans the spline function space of degree k with knots
λ0 = u0 < λ1 < · · · < λg+1 = uf (Hoschek and Lasser, 1989). Hence, they are calledbasis splines or simply B-splines. Fig. 3.1 shows the basis splines of degree k = 3 for
g = 3 internal knots and coincident boundary knots in the interval Du = [0, 10].
B-splines have, among others, the following properties (Dierckx, 1993):
a) Positiveness:
Ni,k+1(u) > 0 , for all u ∈ IR . (3.5)
-
3.1 Fundamentals of spline generation with B-splines 37
0
0.2
0.4
0.6
0.8
1
u
Ni,k+1
λ0 = 0 λ1 = 2 λ2 = 5 λ3 = 7 λ4 = 10
N−3,k+1
N−2,k+1
N−3,k+1
N0,k+1 N1,k+1N2,k+1
N3,k+1
Figure 3.1: B-spline basis of degree k = 3 and g = 3 internal knots in the interval
Du = [0, 10] with coincident boundary knots λ−3 = λ−2 = λ−1 = λ0 = 0, λ4 = λ5 =
λ6 = λ7 = 10 and internal knots λ1 = 2, λ2 = 5, and λ3 = 7
b) Local support:
Ni,k+1(u) = 0 , if u /∈ [λi, λi+k+1] . (3.6)
c) Partition of unity:
g∑
i=−k
Ni,k+1(u) = 1 , for all u ∈ Du . (3.7)
d) Vanishing boundary values:
If q left boundary knots are coincident, with λi = λi+1 = · · · = λi+q−1 < λi+q,and r right boundary knots are coincident, with λk+1−r < λk+2−r = · · · = λi+k+1,it holds
dℓNi,k+1duℓ
(λi) = 0 , for all ℓ = 0, 1, · · · , k − q , and (3.8)
dℓNi,k+1duℓ
(λi+k+1) = 0 , for all ℓ = 0, 1, · · · , k − r . (3.9)
Every spline with knots λ0 = u0, λ1, . . . , λg+1 = uf can be described as a linear
combination of B-splines
f(u) =
g∑
i=−k
ciNi,k+1(u) , for all u ∈ Du . (3.10)
-
38 3 Motion interpolation using B-splines
The expression eq. 3.10 is called B-spline representation of f(u) or simply a B-spline
curve. The k + g + 1 vector coefficients ci ∈ IRn are called B-spline coefficients, orde Boor points, and the broken line which joins all de Boor points is called de
Boor polygon or simply control polygon. The control polygon of a B-spline curve has
important properties. If k = 1, its graph coincides with the graph of the given linear
spline. If k > 1, it encloses the form of the underlying spline function (see fig. 3.2).
In particular, the B-spline basis with no internal knots (g = 0) and thus only coincident
boundary knots λ−k = λ−k+1 = · · · = λ0 = u0 and λ1 = λ2 = · · · = λk+1 = uf yieldbasis splines of the form
N⋆i,k+1(u) = Bkk+i
(u− u0uf − u0
), (3.11)
where Bkk+i(u) are the so-called Bernstein basis of degree k
Bkj (û) =
(k
j
)(1− û)k−j (û)j . (3.12)
This curves are also known as Bézier-curves and are very popular in computer graph-
ics, in particular because of the advantageous properties of their control polygons
(Hoschek and Lasser, 1989).
Furthermore, the ν-th derivative of a B-spline curve of degree k is a B-spline curve of
degree k − ν of the form
d(ν)f
du(ν)(u) =
ν∏
i=1
(k + 1− i)g∑
i=−(k−ν)
c(ν)i Ni,k+1−ν(u) , for all u ∈ Du , (3.13)
with the B-spline coefficients defined by the recursive formula
c(i)j =
cj if i = 0 ,
c(i−1)j − c
(i−1)j−1
λj+k+1−i − λjif i > 0 .
(3.14)
In this work, splines are used to parametrize the spatial paths followed by mechanical
systems by interpolating or approximating either given joint configurations or given
end-effector poses. This yields path geometries which are uniquely defined by a finite
set of discrete parameters, allowing for their editing either by hand, e. g. with help of
a suited graphical interface, or by optimization routines, e. g. nonlinear programming
algorithms. If a spatial function is represented using B-splines, the finite set of discrete
parameters describing the function consists of
-
3.1 Fundamentals of spline generation with B-splines 39
a) the degree k of the spline,
b) the number of internal knots g,
c) the g + 2k + 2 knots λj, with j = −k,−k + 1, · · · , g + k + 1, as well as
d) the g + k + 1 B-spline coefficients ci, with i = −k,−k + 1, · · · , g.
The degree k of the B-spline curve and its number of internal knots g define the com-
plexity of the B-spline parametrization, while the knots λj and the B-spline coefficients
ci determine the geometry of the resulting spline. Hence, the degree k and the number
of internal knots g are typically set to constant values, while the knots λj and the
B-spline coefficients ci are given free for editing. The choice of the degree k usually de-
pends on the number of continuous derivatives the spline is required to have, while the
choice for the number of internal knots g usually depends on the number of segments
the control polygon is required to have.
K1
∆r(u)
∆r(u)
∆r(u0)
∆r(uf)
c−3c−2
c−1
c0
c1
c2 c3
control polygon
f(u; c, λ)u
λ−3 , λ−2 , . . . λ3 , . . . λ7
c−3 , c−2 , . . . c3
Figure 3.2: B-spline representation of a three-dimensio