AnObject-OrientedFrameworkforSpatialMotionPlanningof MultibodySystems · 2013. 11. 13. ·...

An Object-Oriented Framework for Spatial Motion Planning ofMultibody Systems

Der Fakultät für Ingenieurwissenschaften, Abteilung Maschinenbau und

Verfahrenstechnik der

Universität Duisburg-Essen

zur Erlangung des akademischen Grades

eines

Doktors der Ingenieurwissenschaften

Dr.-Ing.

genehmigte Dissertation

von

Francisco Geu Flores

aus

Lima, Perú

Referent: Prof. Dr.-Ing. Andrés Kecskeméthy

Korreferent: o. Univ.-Prof. Dr.-Ing. habil. Hartmut Bremer

Tag der mündlichen Prüfung: 21. Dezember 2012

III

Vorwort

Die vorliegende Dissertation entstand während meiner Tätigkeit als wissenschaftlicher

Mitarbeiter am Lehrstuhl für Mechanik und Robotik der Universität Duisburg-Essen,

unter partieller Förderung der Stiftung Industrieforschung im Rahmen des Projektes

“IntelliSpline: Ein objektorientierter Software-Baukasten für die optimierte Bahnpla-

nung in industriellen Anlagen”.

Zunächst möchte ich Herrn Prof. Dr.-Ing. Andrés Kecskeméthy fü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 für seine

feinen Korrekturvorschläge und insbesondere für sein inspirierend elegant geschriebenes

Gutachten.

Mein besonder Dank gilt Herrn Dr.-Ing. Martin Tändl, der die Grundlagen dieser Ar-

beit in seiner Dissertation vorgelegt hat. Die langen und interessanten Besprechungen,

die wir an seiner Tafel geführt haben, legten die idealen Anfangsbedingungen für eine

gelungene Promotion.

Ich möchte ebenfalls Herrn Dr.-Ing. Martin Schneider fü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 Tätigkeit als

wissenschaftlicher Mitarbeiter in Duisburg begleitet haben. Es ist vor allem wegen

ihrer freundlichen Unterstützung und Kameradschaft, dass meine Zeit als Promovend

ein sehr geschä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 räumlichen Abstände sind sie

immer präsent.

Ich widme diese Arbeit meinem Vater, Santos Geu Rivera. Es ist meine Hoffnung, dass

seine Liebe für Genauigkeit und mathematische Schönheit in diesen Seiten zu spü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 ṡ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


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 Bé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


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 Jü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 Krö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 ã is used for the skew-symmetric matrix

ã =

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 = ã 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 Kecskeméthy (1993) as well as

the concept of a “curve joint” used by Tä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 Kecskemé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

tiṫi

wi

(b) spatial state object

Figure 2.2: State objects (from Kecskemé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 Kecskemé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

ϕ̇

ṡ

, (2.5)


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 ṡ 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

∆rS1ṫ1

m1, ΘS1

S1


∆rS1 , m, ΘS

K1

w1

(KS1)motion transm.

force transm.

(b) Block diagram

Figure 2.4: Mass element (from Kecskemé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)


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


K0

K1

K2

K3

K4

K5

KE

s

ψ

ϕ

m2, Θ2

m3, Θ3

m5, Θ5


{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 Kecskemé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 Kecskeméthy and Hiller (1994) are available.

Regard the general kinetostatic transmission element depicted in fig. 2.1. The velocity


KEK0wE

w0

{s}, {ψ}, {ϕ}K2, K3, K5Qs, Qψ, Qϕ

w2, w3, w5




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


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




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)


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


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.


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 Kecskemé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 ṡ , (2.21)

where Jg is the rigid-body Jacobian, and JP is the Jacobian mapping the path velocity

ṡ along the spatial path to the twist at the output frame K2.


PK0

K1

K2

s

r1

r2

∆r

∆R

R1

R2


{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 Tändl (2009))

The acceleration transmission can be written as[ω̇2

a2

]= Jg

[ω̇1

a1

]+

[0

2ω̃21∆RT∆r

]+ JP s̈ + JP

′ ṡ2 +

[2ω̃1 0

0 2 2ω̃1

]JP ṡ , (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


Tä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 Tä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 ṡ = 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.


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

Kecskeméthy and Tä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 Tä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.


K0

K1

K2

∆r

r1r2

C

s

exey

ezh(s)

Figure 2.12: Darboux-frame parametrization (from Tä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 Mü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


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

ũrelrrel

]ṡ , (2.29)

and the linear and angular accelerations as

[ω̇4

a4

]=

[2RT4 0

−2RT4 r̃rel 2RT4

][ω̇2

a2

]+

[0

4ω̃22 rrel

]+

[urel

ũrel rrel

]s̈ +

[u′rel

ũ′rel rrel + ũ2rel rrel

]ṡ2 +

[4ω̃2 urel

24ω̃2 ũrelrrel

]ṡ , (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 −rTrelũrel

[τ 4

f4

]. (2.32)


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 ṡ along the spatial path to the twist at


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.


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 (û) =

(k

j

)(1− û)k−j (û)j . (3.12)

This curves are also known as Bé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

AnObject-OrientedFrameworkforSpatialMotionPlanningof MultibodySystems · 2013. 11. 13. ·...

Documents

Transcript of AnObject-OrientedFrameworkforSpatialMotionPlanningof MultibodySystems · 2013. 11. 13. ·...