Keywords

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

authors]Ober-Blöbaum, S. authors]Flaßkamp, K.

Planning problems arise in many technical applications and typically one is interested in an optimal solution of the problem. Taking into account the dynamics of the technical system, e.g. an industrial robot, there has to be found a solution trajectory to the dynamic optimal control problem fulfilling in addition required start and final configurations (cf. e.g. [1]). Furthermore, the dynamics of a complex technical system has to be modeled by an interaction of continuous time and discrete event dynamics, thus by a hybrid system.

Optimal Control. An optimal control problem for a mechanical system with configuration manifold Q and states \(x(t) = (q(t),\dot{q}(t)) \in TQ\) is defined by a cost functional J(x, u) = ∫ 0 T C(x(t), u(t)) dt, that has to be minimized. Constraints are given by the system’s dynamics, e.g. the Euler-Lagrange equations

$$\displaystyle{\frac{\mathrm{\partial }L} {\mathrm{\partial }q} (q,\dot{q}) - \frac{\mathrm{d}} {\mathrm{d}t} \frac{\mathrm{\partial }L} {\mathrm{\partial }\dot{q}} (q,\dot{q}) + f(q,\dot{q},u) = 0}$$

with Lagrangian L and forces f depending on continuous time control inputs u(t) and on \((q,\dot{q})\), by boundary conditions and typically further constraints on the states and controls. There exists a number of approaches for numerically solving optimal control problems (cf. e.g. [2] for an overview). For our computational example, we use DMOC (Discrete Mechanics and Optimal Control, [3]), a method that directly discretizes the problem such that a high dimensional constrained nonlinear optimization problem is obtained which can be solved e.g. by sequential quadratic programming (SQP, cf. e.g. [4]). Since these methods compute local optima only, it is necessary to provide good initial guesses for the optimal control method and it is beneficial to combine the method with global, e.g. planning techniques [5–7]. In [5], Frazzoli et al. present the approach for motion planning with motion primitives (cf. Sect. 2).

Hybrid Dynamics. The dynamic behavior of technical systems is typically modeled by systems of continuous time differential equations. However, for an appropriate description of complex behavior and interactions, discrete effects have to be additionally accounted for, leading to the general framework of hybrid systems. Considering mechanical systems, there is a number of origins for hybrid effects: a changing environment as well as varying internal parameters change the system’s dynamics, obstacles lead to impacts or (de)coupling processes cause changes of the system’s topology. Formally, a hybrid system can be defined by a finite family of continuous subsystems \(\dot{x} = f_{i}(x,u),\,i = 1,\ldots,N\) (the vector fields origin from different Lagrangian L i ) defined on subsets \(\mathcal{X}_{i}\) (domains) of a common state space and with the same control inputs. Switching between the subsystems is usually restricted by guards and reset maps (cf. e.g. [8]). Then, a hybrid trajectory consists of the continuous variables plus a discrete mode \(d(t) \in \{ 1,\ldots,N\}\) that defines which subsystem is active. The optimal control of hybrid systems is of great interest, since it includes an optimization of discrete and continuous variables leading to mixed-integer programming problems (cf. e.g. [9]).

The remainder of this paper is structured as follows: in Sect. 2, we introduce the different kinds of motion primitives and sketch the idea of motion planning with primitives. Extensions for an application to hybrid mechanical systems are presented in Sect. 3. Finally, in Sect. 4, the method is illustrated by an academic model of an open-chain jointed robot.

2 Motion Planning with Motion Primitives

The basic idea of motion planning with primitives (introduced in [5]) lies in exploiting the inherent symmetries of a system. Mechanical systems often inhabit symmetries, for example if they are invariant with respect to translations or rotations. Formally, this means that there exists a Lie group G with a left-action Φ g TQ: TQ → TQ, g ∈ G on the state space which leaves the Lagrangian invariant, L ∘Φ g TQ = L for all g ∈ G. Then, we call two trajectories equivalent, if they are equal except for a symmetry transformation and a time shift. Symmetry helps to reduce the complexity of the motion planning library since it is sufficient to store one representative, a motion primitive, for all equivalent trajectories. Solving a motion planning problem corresponds to a search for the optimal sequence in this library represented by a maneuver automaton (cf. [5, 6]).

Trim Primitives. A special kind of primitives is given by trim primitives, which are motions along the group orbits of G with a constant control value. Thus, the trajectories can be simply described by \((q,\dot{q})(t) {=\varPhi }^{TQ}(\exp (\xi t),x_{0})\), u(t) ≡ u 0 with ξ being an element of the Lie algebra corresponding to G, with the exponential map exp( ⋅ ) and some initial value x 0 (cf. [5, 7] for details). In mechanical systems, trims are also known as relative equilibria and they are closely related to the conservation of momentum maps, the Noether theorem, and to symmetry reduction procedures (see [7]). For a spherical pendulum (Fig. 1), trims are horizontal rotations with constant velocity. In the lower half sphere, uncontrolled trims exist. A constant additive control can be chosen to create trims with arbitrary rotational velocities at any height.

Fig. 1
figure 1

For a simple spherical pendulum, trim primitives are horizontal rotations, but (un)stable manifolds belongs to purely vertical motions. Thus, connecting maneuvers as motion primitives are required such that sequences of primitives can be found

Full size image

Trajectories on (Un)stable Manifolds. The natural, i.e. uncontrolled dynamics of a mechanical system provide motions that can be of great interest when searching for energy efficient control maneuvers. In particular, trajectories on stable manifolds of hyperbolic unstable fixed points are promising candidates since a stable manifold contains all motions which tend to the corresponding equilibrium point (cf. e.g. [10]). The unstable manifold, in contrast, shows the direction of expansion from the equilibrium and is attractive. Formally, assuming \(\bar{x} = (\bar{q},0)\) is an equilibrium of the system and F L (x, t) denotes the flow of the autonomous system defined by the Lagrangian L, the local stable manifold is given by

$$\displaystyle{W_{\mathrm{loc}}^{s}(\bar{x}) =\{ x \in U\,\vert \,F_{ L}(x,t) \rightarrow \bar{ x}\text{ for }t \rightarrow \infty \text{ and }F_{L}(x,t) \in U\,\forall t \geq 0\}.}$$

The global stable manifold W s can be governed by the preimages of the flow on \(W_{\mathrm{loc}}^{s}(\bar{x})\). (For the unstable manifold W u, the same holds in backward time (t ≤ 0).) To compute such manifolds numerically, we use the method GAIO (Global Analysis of Invariant Objects, [11]), see Figs. 2 and 3 for single pendulum (cf. Fig. 1) and double pendulum (cf. Fig. 4) manifold examples. In [7], it is explained in detail how trajectories on manifolds can be chosen.

Fig. 2
figure 2

Unstable manifold of the up-up equilibrium of a double pendulum (restricted to vertical motions). Motion primitives are generated by choosing trajectories with different time durations on the manifold

Full size image
Fig. 3
figure 3

The locked double pendulum (restricted to vertical motions) is a one degree of freedom system with a one-dimensional unstable manifold. For the numerical computations, the locking angle is set to 0. 25π

Full size image
Fig. 4
figure 4

Model of a double spherical pendulum with four degrees of freedom and chosen coordinates \((\phi _{1},\phi _{2},\theta _{1},\theta _{2})\). Actuation in both joints is assumed

Full size image

Connecting Maneuvers and Sequencing. Motion primitives of a third kind have to be computed to build up the motion planning library, namely short controlled maneuvers that connect trims with each other and trims to manifolds. This can be done for example by the optimal control method DMOC (see [3]). In Fig. 1, all three types of primitives for a simple spherical pendulum are sketched (we refer to [7] for a detailed description).

3 Motion Planning for Hybrid Mechanical Systems

In the motion planning framework, the hybrid properties of the system have to be accounted for: in the first time, when computing the primitives (restricted domains, limited time between switches), but also when searching for the optimal sequence in the library. In general, there is a need for hybrid control maneuvers, which connect primitives of the different continuous subsystems. For their computation additional constraints on the state space due to the guards have to be considered and an optimization of switching time has to be included (cf. Sect. 4 for illustrating examples).

Symmetries also occur in hybrid systems (cf. e.g. [12]). In the following, we restrict to a very specific case and assume that for two continuous subsystems, switching back and forth is allowed and the subsystems inhabit the same symmetry group G. We call a tuple of two pairs (ξ 1, u 1) and (ξ 2, u 2) a hybrid trim, if both are trims in their state spaces and if it holds that \(x({t}^{-}) = x({t}^{+})\), i.e. the state, in particular the velocity before and after switching is the same. In Fig. 5, an example is shown of a hybrid trim for a spherical pendulum which switches at the “pick” and “place” locations between two different modes (cf. Sect. 4 for a more detailed discussion.) By a hybrid control with switched constant control values, it is possible to generate a hybrid trim trajectory with constant horizontal velocity \(\dot{\theta }\), i.e. in this example, we have ξ 1 = ξ 2 but u 1 ≠ u 2.

Fig. 5
figure 5

Illustration and simulation of a hybrid trim for the pick and place scenario. The rotational velocity \(\dot{\theta }\) is kept constant by the discontinuous, but piecewise constant control u

Full size image

4 Example: An Academic Motion Planning Problem for an Industrial Robot

An open chain jointed robot as used e.g. in production facilities can be modeled—in an academic fashion allowing high simplifications on technical details—as a spherical pendulum. Thus, to illustrate the presented motion planning approach, we consider a double spherical pendulum with two-dimensional controls in both joints (cf. Fig. 4, m 1 = 20 kg, m 2 = 8 kg, l 1 = 1 m, l 2 = 0. 5 m, \(g = 9.81\,\mathrm{m}/\mathrm{{s}}^{2}\)). The Lagrangian and the equations of motion can be found e.g. in [7]; as the cost functional we chose the control effort modeled as \(J(u) =\int _{ 0}^{T}{u}^{2}(t)dt\). The starting point for the scenario is the up-up position. The final condition is a periodic motion of the outstretched locked double pendulum, which is motivated by a pick and place scenario (a hybrid trim) assuming that m 2 is changed to 12 kg while the picked object is moved (cf. Fig. 5). Before heading to the final condition, the robot has to change the tool in the down-down equilibrium. Another kind of hybrid effect is brought into the problem by defining a safety region for \(\phi _{1} \in [\pm \pi /4,\pm \pi /2]\), where the second link has to be locked (cf. Fig. 3). To compute energy efficient control sequences, uncontrolled trajectories on the unstable manifolds (cf. Figs. 2 and 3) are used together with connecting control maneuvers. Thus, there are four different subsystems (labeled by their different vector fields for shortness): a double spherical pendulum (f 1), a locked double pendulum (f 2), and an outstretched locked pendulum with m 2 = 8 kg (f 3) or \(\overline{m}_{2} = 12\,\mathrm{kg}\) (f 4). Figure 6 shows an example solution sequence for the motion planning problem.

Fig. 6
figure 6

Example solution sequence for the industrial robot scenario consisting of motion primitives. The scenario starts at the up-up position (subsystem f 1 active), is pushed to the unstable manifold (maneuver for f 1), than switches to a locked mode (f 2) and uses the corresponding unstable manifold to go downwards; after a short maneuver leaving the safety region (f 1), it rests at the down-down position to change the tool and finally steers (maneuver for f 1) to the rotational pick and place motion, which is a hybrid trim (switching between f 3 and f 4). The solution sequence is given in cartesian coordinates (cf. Fig. 4), red dots mark the switching between primitives

Full size image

In conclusion, this example shows that the motion planning with motion primitives method is particularly suited for an extension to hybrid systems: the flexibility of the method allows for incorporating motion primitives from each continuous subsystem, the computational effort of deriving an optimal hybrid solution is reduced by the motion planning library to finding a hybrid optimal sequence, and the method exploits dynamical properties which are present in hybrid as well as in ordinary mechanical systems. In future work, the approach has to be evaluated further by larger examples with more or different kinds of hybrid effects. Then, searching in the motion planning library will have to be performed by appropriate methods, e.g. sampling based road map algorithms (cf. e.g. [6]).