Passivity-Based Tracking Control of a Flexible Link Robot

Abstract

This contribution addresses modeling and control of highly complex nonlinear mechanical systems such as an articulated robot with two flexible links and three flexible joints. We employ the Projection Equation in subsystem formulation, a very efficient method for modeling repeating assemblies and beam elasticities and apply a Ritz expansion to obtain ordinary differential equations of motion. For model-based control design, the small elastic deformations of the beams are approximated with linear springs and dampers in a lumped element model. On this basis, a control design with two degrees of freedom is proposed: a flatness-based feed forward and a passivity-based feedback control technique of interconnection and damping assignment. Further, we deal with acceleration and angular rate measurements to compute all system states used in the feedback loop. Finally, the proposed strategies are validated by measurements from a fast straight line in space and a ball catching scenario.

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.

6.1 Introduction

Lightweight structures may offer a good trade-off between high acceleration (and thus short cycle times) on the one hand and good tracking accuracy on the other. However, reducing inertia parameters and the weight of robots may lead to a loss in stiffness, resulting in elastic deformations and vibrations at the tool center point (TCP). Therefore, rational model-based control design must avoid or compensate for this weakness.

In industrial setups, effective vibration suppression requires further measurements, in addition to motor position detection at each servo drive. For this purpose, we propose using acceleration and angular rate sensors, as they are inexpensive and easy to mount.

6.1.1 Modeling

A very comprehensive survey of the most commonly used modeling techniques and approaches for flexible robotic manipulators can be found in Ref. [1]. The Langrange formalism of the second kind and the Hamilton representation are well-established methods (Refs. [3, 4] provide excellent overviews). In contrast the Projection Equation is a very effective procedure by means of modeling the complex dynamics of highly nonlinear systems [2]. The Projection Equation is usually used for mechanical systems consisting of a large number of bodies with many degrees of freedom. The elastic links of the robot are assumed to be Euler-Bernoulli beams, and elasticities are modeled with Ritz Ansatz functions.

Conventionally, dealing with energy-based control concepts requires analytic procedures using energy functionals. As mentioned above, a reduced model in which the elasticities are approximated with lumped elements (LEM) is introduced, also called mass-spring-damper models or virtual spring-damper-elements, see Refs. [5] or [6].

6.1.2 Tracking Control

Flexible manipulator control has been the focus of extensive research (e.g., see Refs. [7, 8]). Some practical examples can be found in Refs. [9, 10], where linear robots with flexible beams were considered. In Ref. [11], robot similar to that considered in our work was discussed, although we use the flatness-based approach to solve the tracking problem, see Ref. [12]. In Ref. [13], a general algorithm for the inverse dynamics computation of robots with elastic links/joints was presented, but there are some uncertainties in the elastic system – which are discussed below – that prevent an exactly linearizing tracking control from achieving the desired results. In the present work, we therefore propose a control structure with two degrees of freedom (2DoF) for solving the tracking problem. Such an approach allows designing the feedforward part independently of the feedback part. The feedforward control is based on the flatness approach, while the feedback control of the remaining tracking error dynamics builds upon energy-based concepts. Many different approaches regarding this technique can be found in the literature. A general overview of passivity-based control can be found in Refs. [14, 15]. The fundamental idea of these approaches is energy preservation. Roughly speaking, energy ports of external inputs and collocated outputs are considered. These ports describe the power exchange between the system environment and the system dynamics. On this basis, we propose the interconnection and damping assignment – passivity based control (IDA-PBC) approach.

6.2 Setup

Figure 6.1 shows the mechanical setup under consideration. Fast trajectories and the influence of gravity result in two bending and torsional deflections of both arms. Triple-axis analog angular rate and acceleration sensors are mounted on the elbow and on the TCP to improve accuracy and minimize vibration. This sensor pack is called IMU (internal measurement unit) – a low-cost product that operates on a micro-electro-mechanical system (MEMS) principle. The three synchronous motors of the robot are powered by servo drives with a common cascaded motor joint control. Communication with a central computing unit and the servo drives is realized via a very fast Ethernet Powerlink bus.

Fig. 6.1

The elastic articulated robot is driven by three synchronous motors and Harmonic Drive gears for base, shoulder and elbow. The lightweight upper arm and forearm have square hollow cross-sections

6.3 System Dynamics

As mentioned in the introduction, various modeling procedures are used in this contribution. The Projection Equation and the canonical Hamilton Equations serve as the fundamental bases for a Ritz Model and a LEM.

6.3.1 Methods

The equations of motion in configuration space can be written as

$$ {\boldsymbol{M}\ddot{\boldsymbol{q}}} +{\boldsymbol{g}}({\boldsymbol{q}},{\dot{\boldsymbol{q}}}) = {\boldsymbol{Q}}, $$

(6.1)

with the mass matrix M, minimal coordinates q and their time derivatives, the vector of nonlinearities g, and the generalized force vector Q. In state space, this reads

$$ \begin{array}{llll} {\dot{\boldsymbol{x}}} & ={\boldsymbol{f}}({\boldsymbol{x}},{\boldsymbol{u}}) \\{\boldsymbol{y}} & ={\boldsymbol{h}}({\boldsymbol{x}})\end{array} $$

(6.2)

with the state vector ${{{\boldsymbol{x}}}^T} =({{{\boldsymbol{q}}}^T}\;\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits^T )$, the output vector y, and the input vector u.

6.3.1.1 Projection Equation

The Projection Equation for ${{N}_B}$ bodies is given by

$$ \sum\limits_{{i = 1}}^{{{{N}_B}}} \mathop{{\left[{\begin{array}{llll}{\mathop{{\left({\displaystyle\frac{{{{\partial}_R}{{{\boldsymbol{v}}}_c}}}{{\partial{\dot{\boldsymbol{q}}}}}} \right)}}\nolimits^T }{\mathop{{\left({\displaystyle\frac{{{{\partial }_R}{{\omega }_c}}}{{\partial{\dot{\boldsymbol{q}}}}}} \right)}}\nolimits^T } \\\end{array}}\right]}}\nolimits_i \mathop{{\left[ {\begin{array}{llll}{_R{\dot{\boldsymbol{p}}} + {{\:}_R}\mathop{{\tilde{\omega}}}\nolimits_{{IR}} {{\:}_R}{\boldsymbol{p}} -\:{{}_R}{{{\boldsymbol{f}}}^e}} \\{_R{\dot{\boldsymbol{L}}} + \:{{}_R}\mathop{{\tilde{\omega }}}\nolimits_{{IR}}{{\:}_R}{\boldsymbol{L}} - \:{{}_R}{{{\boldsymbol{M}}}^e}} \\\end{array}}\right]}}\nolimits_i = 0, $$

(6.3)

see Ref. [2] for details. This method projects the linear and angular momenta $_R{\boldsymbol{p}}$ and $_R{\boldsymbol{L}}$ respectively, the impressed forces $_R{{{\boldsymbol{f}}}^e}$ and impressed moments $_R{{{\boldsymbol{M}}}^e}$, formulated in an arbitrary reference frame R, using the Jacobian matrices ${{({{\partial }_R}{{{\boldsymbol{v}}}_c}/\partial{\dot{\boldsymbol{q}}})}_i}$ and${{({{\partial }_R}{{\omega }_c}/\partial{\dot{\boldsymbol{q}}})}_i}$ (in terms of the selected minimal velocities ${\dot{\boldsymbol{q}}}$) into the direction of unconstrained motion. Variables $_R{{{\boldsymbol{v}}}_c}$ and $_R{{\omega }_c}$ denote respectively the linear and angular velocities of the center of gravity for each body i. For repeating system modules as in our case (the upper arm and forearm, for instance), it is convenient to use the subsystem formulation of the Projection Equation:

$$ \sum\limits_{{n = 1}}^N \mathop{{\left( {\frac{{\partial\mathop{{{\dot{\boldsymbol{y}}}}}\nolimits_n}}{{\partial{\dot{\boldsymbol{q}}}}}} \right)}}\nolimits^T \left\{{{{{\boldsymbol{M}}}_n}\mathop{{{\ddot{\boldsymbol{y}}}}}\nolimits_n +{{{\boldsymbol{G}}}_n}\mathop{{{\dot{\boldsymbol{y}}}}}\nolimits_n -{\boldsymbol{Q}}_n^e} \right\} = 0, $$

(6.4)

combining similar segments of the rigid-elastic multibody system in a modular way. Each subsystem n is associated with describing velocities $\mathop{{{\dot{\boldsymbol{y}}}}}\nolimits_n$, a mass matrix ${{{\boldsymbol{M}}}_n}$, a matrix of Coriolis and centrifugal terms ${{{\boldsymbol{G}}}_n}$, and generalized forces ${\boldsymbol{Q}}_n^e$. For elastic multibody systems, however, $\mathop{{{\dot{\boldsymbol{y}}}}}\nolimits_n$ in the Projection Equation (6.4) contain spatial derivatives of the holonomic minimal velocities ${\dot{\boldsymbol{q}}}$, due to the dependencies on space and time of the bending functions and torsion. A Ritz approximation for the distributed parameters is applied to obtain ordinary differential equations; see Ref. [2] for details.

6.3.1.2 Canonical Hamiltonian Equations

Using the canonical Hamilton Equations requires the Legendre transformation

$$ {\boldsymbol{p}} = \mathop{{\left( {\frac{{\partial T}}{{\partial {\dot{\boldsymbol{q}}}}}} \right)}}\nolimits^T = {\boldsymbol{M}}({\boldsymbol{q}}){\dot{\boldsymbol{q}}}, $$

(6.5)

where T denotes the kinetic energy and p the generalized momenta. Therefore, ${\dot{\boldsymbol{q}}}$ must be replaced with ${\dot{\boldsymbol{q}}} = {\boldsymbol{M}}{{({\boldsymbol{q}})}^{{ - 1}}}{\boldsymbol{p}}$ in the Hamiltonian function, which leads to

$$ H({\boldsymbol{q}},{\boldsymbol{p}}) = T({\boldsymbol{q}},{\boldsymbol{p}}) + V({\boldsymbol{q}}), $$

(6.6)

with the potential energy V. We thus yield for the Hamilton Equations

$$ {\dot{\boldsymbol{x}}} = \left[ {\begin{array}{llll} 0 &{\boldsymbol{E}} \\{ - {\boldsymbol{E}}} & { - {{{\boldsymbol{R}}}_m}}\\\end{array}} \right]\left( {\begin{array}{llll} {\mathop{{\left({\frac{{\partial H({\boldsymbol{q}},{\boldsymbol{p}})}}{{\partial{\boldsymbol{q}}}}} \right)}}\nolimits^T } \\{\mathop{{\left({\frac{{\partial H({\boldsymbol{q}},{\boldsymbol{p}})}}{{\partial{\boldsymbol{p}}}}} \right)}}\nolimits^T } \\\end{array} } \right) +\left( {\begin{array}{llll} 0 \\{\boldsymbol{Q}} \\\end{array} } \right),$$

(6.7)

with ${{{\boldsymbol{x}}}^T} = ({{{\boldsymbol{q}}}^T}{{{\boldsymbol{p}}}^T})$. If the system representation with an affine system input u can be given in

$$ \begin{array}{llll}{\dot{\boldsymbol{x}}} & = \left({{\boldsymbol{J}}({\boldsymbol{x}}) - {\boldsymbol{R}}({\boldsymbol{x}})}\right)\mathop{{\left( {\frac{{\partial H}}{{\partial{\boldsymbol{x}}}}} \right)}}\nolimits^T +\;{\boldsymbol{G}}({\boldsymbol{x}}){\boldsymbol{u}} \\{\boldsymbol{y}} & ={\boldsymbol{G}}{{({\boldsymbol{x}})}^T}\mathop{{\left( {\frac{{\partial H}}{{\partial {\boldsymbol{x}}}}}\right)}}\nolimits^T,\end{array}$$

(6.8)

the system is called a port-controlled Hamiltonian system with dissipation (PCHD). In this context, ${\boldsymbol{Q}} = {\boldsymbol{B}}({\boldsymbol{x}}){\boldsymbol{u}}$ with ${\boldsymbol{G}}{{({\boldsymbol{x}})}^T} = (0{\boldsymbol{B}}{{({\boldsymbol{x}})}^T})$ is applied. The output vector y is defined as a collocated output to the input vector u. The product of these variables ${{{\boldsymbol{y}}}^T}{\boldsymbol{u}}$ characterizes an energy port and represents the power flow into the system from the environment, while ${\boldsymbol{J}}({\boldsymbol{x}}) = - {\boldsymbol{J}}{{({\boldsymbol{x}})}^T}$ represents the internal power flow and ${\boldsymbol{R}}({\boldsymbol{x}}) = {\boldsymbol{R}}{{({\boldsymbol{x}})}^T}$ the dissipative effects in the system. More details can be found in Ref. [16].

6.3.2 Ritz Model

Here we present how – building on the foundations outlined in Sect. 6.3.1.1 – the equations of motion can be computed by decomposing the system into subsystems. The ${{n}^{\rm{th}}}$-subsystem of the Ritz model includes a motor-gear device and an elastic beam as is the case for the upper arm and the forearm pictured in Fig. 6.2. The vector of describing velocities of such a subsystem,

Fig. 6.2

An elastic subsystem which includes a motor-gear device and an elastic beam such as the upper arm and the forearm

$$ \mathop{{{\dot{\boldsymbol{y}}}}}\nolimits_n^T =\mathop{{\left( {{\boldsymbol{v}}_o^T\;\omega_F^T\;{{\Omega}_M}\;{{\Omega }_A}\;\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_R^T }\right)}}\nolimits_n, $$

(6.9)

contains the guiding velocities ${{{\boldsymbol{v}}}_o},{{\omega }_F}$ and the relative angular velocities ${{\omega }_A}$, ${{\omega }_M}$. The latter represent the arm and motor rotations relative to the moving frame. In this context for an infinitesimal element dm, the vector to the center of gravity is written as ${\boldsymbol{r}}_c^T = (x\;\vee (x,t)\;w(x,t))$ and the vector of rotation angles as ${{\varphi }^T} = (\vartheta (x,t) - w{\prime }(x,t)\;\vee {\prime }(x,t))$. For an approximation of the elastic deflection $\vee (x,t) =\ {\boldsymbol{v}}{{(x)}^T}{{{\boldsymbol{q}}}_\upsilon }$ in the $_Ry$-direction and $w(x,t) = {\boldsymbol{w}}{{(x)}^T}{{{\boldsymbol{q}}}_w}$ in the $_Rz$-direction as well as $\vartheta (x,t) = \vartheta {{(x)}^T}{{{\boldsymbol{q}}}_{\vartheta }}$ for torsion in the $_Rx$-direction, Ritz Ansatz functions are introduced. Therefore, $\mathop{{{\dot{\boldsymbol{y}}}}}\nolimits_n$ also contains the time derivatives of the Ritz coordinates ${\boldsymbol{q}}_R^T = ({\boldsymbol{q}}_\vee ^T\;{\boldsymbol{q}}_w^T\;{\boldsymbol{q}}_{\vartheta }^T)$. All shape functions as well as the infinitesimal mass element dm and the infinitesimal moments of inertia d J are defined piecewise in order to include the rigid parts ($x < {{L}_A}$ and $x > {{L}_B}$) of the link. The gear torque depends on the difference between arm and motor angles ${{q}_A} - {{q}_M}$ with the gear elasticity ${{k}_G}$. Finally, the motor unit is considered with ${{m}_M}$ and ${{{\boldsymbol{J}}}_M}$. The angular velocities are defined as ${{\omega }_A} = \mathop{{\dot{q}}}\nolimits_A$ and ${{\omega }_M} = {{i}_G}\mathop{{\dot{q}}}\nolimits_M$ with the gear ratio ${{i}_G}$. The resulting ordinary differential equations of motion are given in form of (6.1) with ${{{\boldsymbol{q}}}^T} = ({\boldsymbol{q}}_M^T\;{\boldsymbol{q}}_A^T\;{\boldsymbol{q}}_{{el}}^T)$, ${\boldsymbol{q}}_M^T \in {{R}^3}$, ${\boldsymbol{q}}_A^T \in {{R}^3}$ and ${\boldsymbol{q}}_{{el}}^T \in {{R}^{{10}}}$, where five Ritz coordinates are included for each arm. More information on the modeling of elastic robots can be found in Refs. [17, 18].

6.3.3 Lumped Element Model

Alternatively, we propose a relatively simple model – based on lumped elements – for control design. This technique is justified since the considered elastic robot fulfills the assumption of small elastic deformations. Various investigations have shown that only three springs are essential for good concordance with the real model: ${{k}_1}$, ${{k}_2}$, and ${{k}_3}$ for elasticity in the base, the shoulder, and in the elbow respectively (see Fig. 6.3). Obviously, all torsional effects and the elasticity in the z-direction of the forearm are neglected in this model. In order to reintroduce these effects, a position-dependent spring stiffness for the base ${{k}_1}({{q}_{{A2}}},{{q}_{{A3}}})$ (determined experimentally) is suggested. The equations of motion can be decomposed into a system of differential equations for motor movements (subscript M) and a system of arm equations (subscript A)

Fig. 6.3

Lumped Element Model limited to the first deformation mode

$$ {{{\boldsymbol{Q}}}_M} ={{{\boldsymbol{M}}}_M}\mathop{{{\ddot{\boldsymbol{q}}}}}\nolimits_M +\;{{{\boldsymbol{Q}}}_A} + {{{\boldsymbol{Q}}}_F} $$

(6.10)

$$ {{{\boldsymbol{Q}}}_A} ={{{\boldsymbol{M}}}_A}\mathop{{{\ddot{\boldsymbol{q}}}}}\nolimits_A +\;{{{\boldsymbol{G}}}_A}\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A +\;{{{\boldsymbol{Q}}}_{{G,A}}} $$

(6.11)

$$ {{{\boldsymbol{Q}}}_A} = {\boldsymbol{K}}({{{\boldsymbol{q}}}_M} - {{{\boldsymbol{q}}}_A}), $$

(6.12)

with a motor torque vector ${{{\boldsymbol{Q}}}_M}$, a friction vector ${{{\boldsymbol{Q}}}_F}$, and a vector with gravitational dependences ${{{\boldsymbol{Q}}}_{{G,A}}}$. The algebraic (6.12) links (6.10) to (6.11), where K denotes the stiffness matrix. The equations listed above constitute the fundamental basis for the following control design.

6.3.4 Model Verification

Clearly, mathematical description and experiment must coincide for model-based control design. In order to obtain a meaningful comparison, the desired reference trajectory is selected in the following manner: In the first part of verification, each axis is moved separately. In the second part, a straight line in space with a length of 2.1 m and a duration of 1.2 s is tracked. For a comparative study in terms of dynamic model matching, the same cascaded PD motor joint parameters are used in simulation and experiment. The plots in Fig. 6.4 show that simulation and experimental results for both the Ritz model and the LEM are strongly concordant. Since the Ritz model typically includes numerous parameters, the model is very close to the experiment, but also sensitive to parameter uncertainties. The LEM, in contrast, is relatively straightforward, as it depends only on a few parameters; hence, it is suitable for control design.

Fig. 6.4

Simulated and measured TCP accelerations

6.4 Computation of the Arm Positions and Velocities

If the LEM is used for control design, the feedback loop requires knowledge of the state vector

$$ {{{\boldsymbol{x}}}^T} = ({\boldsymbol{q}}_M^T\;{\boldsymbol{q}}_A^T\;\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_M^T \;\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A^T ). $$

(6.13)

Motor position ${{{\boldsymbol{q}}}_M}$ and motor velocity $\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_M$ are available on each servo drive. Next we show a straightforward calculation of the unknown arm position ${{{\boldsymbol{q}}}_A}$ and arm velocity $\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A$ with the installed IMUs. The main idea is a rebuild of the equations of motion with the acceleration sensors

$$ _i{{{\boldsymbol{a}}}_{{IMU,i - 1}}} ={{({{a}_{{x,i}}}{{a}_{{y,i}}}{{a}_{{z,i}}})}^T}{{ =\ }_i}{{\dot{\boldsymbol{v}}}}_i {{ + }_i}\mathop{{\tilde{\omega}}}\nolimits_{{Ii}} {{}_i}{{{\boldsymbol{v}}}_i}{{ -}_i}{{{\boldsymbol{g}}}_i} $$

(6.14)

and the angular rate sensors

$$ _i{{\omega }_{{IMU,i - 1}}} = {{({{\omega }_{{x,i}}}{{\omega }_{{y,i}}}{{\omega }_{{z,i}}})}^T}{{=\ }_i}{{\omega }_{{Ii}}} $$

(6.15)

with i = 2 for the upper arm frame and i = 3 for the forearm frame (Fig. 6.3). In the first step, the mathematical model with ${{N}_B}$ bodies is divided into ${{N}_1}$ driving units and ${{N}_2}$ arm units. In the considered configuration, the interconnections between these units equate to spring forces with

$$ {{{\boldsymbol{Q}}}_A} = {\boldsymbol{K}}({{{\boldsymbol{q}}}_M} - {{{\boldsymbol{q}}}_A}). $$

(6.16)

These findings can easily be taken into account in the Projection Equation (6.3), by replacing the projection in the direction of unconstrained motion with the projection in arm-space according to

$$ \sum\limits_{{i = 1}}^{{{{N}_2}}} \mathop{{\left[{\begin{array}{llll} {\mathop{{\displaystyle\left({\frac{{{{\partial}_i}{{{\boldsymbol{v}}}_c}}}{{\partial\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}} \right)}}\nolimits^T } {\mathop{{\displaystyle\left( {\frac{{{{\partial }_i}{{\omega}_c}}}{{\partial \mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}}\right)}}\nolimits^T } \\\end{array} } \right]}}\nolimits_i\mathop{{\left[ {\begin{array}{llll} {_i{\dot{\boldsymbol{p}}} + {{\:}_i}\mathop{{\tilde{\omega }}}\nolimits_{{Ii}}{{\:}_i}{\boldsymbol{p}} - {{\:}_i}{{{\boldsymbol{f}}}^e}} \\{_i{\dot{\boldsymbol{L}}} + {{}_i}\mathop{{\tilde{\omega }}}\nolimits_{{Ii}}{{\:}_i}{\boldsymbol{L}} - {{\:}_i}{{{\boldsymbol{M}}}^e}} \\\end{array} }\right]}}\nolimits_i = {{{\boldsymbol{Q}}}_A}, $$

(6.17)

where ${{{\boldsymbol{Q}}}_A}$ appears on the right-hand side. If we treat the beam and the tip body as one unit, there are two arm units (upper arm unit and forearm unit), and therefore ${{N}_2} = 2$. If the IMUs are mounted at ${{l}_i}$, we yield with (6.14) and (6.15) and $_i{{{\boldsymbol{p}}}_i} = {{m}_i}{{}_i}{{{\boldsymbol{v}}}_i}$, $_i{\boldsymbol{f}}_i^e = {{m}_i}{{}_i}{{{\boldsymbol{g}}}_i}$, $_i{{{\boldsymbol{L}}}_i}{{\ =\ }_i}{{{\boldsymbol{J}}}_i}{{}_i}{{\omega }_{{Ii}}}$, ${{{\boldsymbol{M}}}^e} = 0$:

$$ \begin{array}{llll} {{{\boldsymbol{Q}}}_A} = \sum\limits_{{i= 2}}^3& \mathop{{\left[ {\begin{array}{llll}{\mathop{{\displaystyle\left({\frac{{{{\partial}_i}{{{\boldsymbol{v}}}_c}}}{{\partial\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}} \right)}}\nolimits^T }{\mathop{{\displaystyle\left( {\frac{{{{\partial }_i}{{\omega}_c}}}{{\partial \mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}}\right)}}\nolimits^T } \\\end{array} } \right]}}\nolimits_i \cdot\\& \left[ \begin{array}{llll} {{m}_i}{{}_i}{{{\boldsymbol{a}}}_{{IMU, i -1}}} \hfill\\i{{{\boldsymbol{J}}}_i}{{}_i}\mathop{{\dot{\omega}}}\nolimits_{{IMU,i - 1}} + {{\:}_i}\mathop{{\tilde{\omega}}}\nolimits_{{IMU,i - 1}} {{}_i}{{{\boldsymbol{J}}}_i}{{}_i}{{\omega}_{{IMU,i - 1}}}\end{array}\right].\end{array} $$

(6.18)

Taking (6.16) into account, the arm position can be computed solving (6.18) recursively, which is shown in the next steps. However, since the considered robot features two heavy tip bodies on each link, all arm inertia can be neglected, because the Steiner term dominates. Further, on the elbow and on the TCP lumped elbow mass ${{m}_2}$ and lumped tip body mass ${{m}_3}$ are assumed respectively. For this reason, the IMUs are mounted on the endpoint of each link, and we obtain

$$ \begin{array}{llll}{{{\boldsymbol{Q}}}_A} & \approx\sum\limits_{{i = 2}}^3 \mathop{{\left({\frac{{{{\partial}_i}{\boldsymbol{v}}}}{{\partial\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}} \right)}}\nolimits_i^T \mathop{{\left[ {m{{(}_i}{\dot{\boldsymbol{v}}}\;{{ +}_i}\mathop{{\tilde{\omega }}}\nolimits_{{Ii}} {{}_i}{\boldsymbol{v}}{{-}_i}{\boldsymbol{g}})} \right]}}\nolimits_i \\&\approx{{m}_2}\mathop{{\left({\frac{{{{\partial}_2}{{{\boldsymbol{v}}}_2}}}{{\partial\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}} \right)}}\nolimits^T{{}_2}{{{\boldsymbol{a}}}_{{IMU1}}} + {{m}_3}\mathop{{\left({\frac{{{{\partial }_3}{{{\boldsymbol{v}}}_3}}}{{\partial\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}} \right)}}\nolimits^T {{}_3}{{{\boldsymbol{a}}}_{{IMU2}}},\end{array} $$

(6.19)

and

$$ \begin{array}{llll}{{Q}_{{A1}}} & = - {{m}_2}{\rm cos}({{q}_{{A2}}}){{l}_2}{{a}_{{z2}}} - {{m}_3}{{a}_{{z3}}}\left( {{\rm cos}({{q}_{{A2}}}){{l}_2} + {\rm cos}({{q}_{{A2}}} + {{q}_{{A3}}}){{l}_3}} \right) \\{{Q}_{{A2}}} & = {{m}_2}{{l}_2}{{a}_{{y2}}} + {{m}_3}({\rm sin}({{q}_{{A3}}}){{l}_2}{{a}_{{x3}}} + ({\rm cos}({{q}_{{A3}}}){{l}_2} + {{l}_3}){{a}_{{y3}}}) \\{{Q}_{{A3}}} & = {{m}_3}{{l}_3}{{a}_{{y3}}}.\end{array} $$

(6.20)

Based on the above findings and on (6.16), this problem can be solved by starting with the last line. In summary, the arm positions ${{{\boldsymbol{q}}}_A}$ can be computed as a function of the motor positions and the acceleration measurements at the elbow and the TCP. In order to obtain the arm velocities, only the analog rate sensors are considered. The arm velocities $\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A$ are included linearly in $_i{{\omega }_{{IMU,i - 1}}}$. Therefore we yield

$$ {\boldsymbol{\omega }_{{IMU}}} = \left( {\begin{array}{llll} _2\boldsymbol{\omega}_{{IMU1}} \\{_3{\boldsymbol{\omega}}_{{IMU2}}} \\\end{array}} \right) = \left( {\frac{{\partial {\boldsymbol{\omega }}_{{IMU}}}}{{\partial \mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A }}} \right)\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A = {\boldsymbol{J}}({{{\boldsymbol{q}}}_A})\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_A. $$

(6.21)

With the pseudoinverse of J, we obtain the arm velocities in a very straightforward manner.

6.5 Tracking Control Framework

Several solutions to the tracking problem can be found in the literature. We use a 2DoF control scheme based on the LEM: a flatness based feedforward control ${{{\boldsymbol{u}}}_{{FF}}}$ using ${{{\boldsymbol{q}}}_A}$ as the flat output y and a passivity-based tracking error feedback control ${{{\boldsymbol{u}}}_{{FB.}}}$ For simplicity, we employ a quasi-static tracking error system obtained by linearization along trajectory ${{{\boldsymbol{q}}}_d}$ (abbreviated LTV: linear time variant system), with $\mathop{{{\dot{\boldsymbol{q}}}}}\nolimits_d =\mathop{{{\ddot{\boldsymbol{q}}}}}\nolimits_d = 0$ and $\mathop{{{\dot{\boldsymbol{p}}}}}\nolimits_d = 0$. Thus, the tracking error dynamics in Hamilton representation can be written as

$$ \mathop{{{\dot{\boldsymbol{x}}}}}\nolimits_e = \left({\begin{array}{llll} {{\dot{\boldsymbol{e}}}} \\{\mathop{{{\dot{\boldsymbol{p}}}}}\nolimits_e } \\\end{array} } \right) = \left[{\begin{array}{llll} 0 & {{\boldsymbol{M}}_e^{{ - 1}}} \\{ -{{{\boldsymbol{K}}}_P}} & { - {{{\boldsymbol{R}}}_m}{\boldsymbol{M}}_e^{{ - 1}}}\\\end{array} } \right]\left( {\begin{array}{llll} {\boldsymbol{e}}\\{{{{\boldsymbol{p}}}_e}} \\\end{array} } \right) + \left({\begin{array}{llll} 0 \\{{{{\boldsymbol{B}}}_e}} \\\end{array} }\right){{{\boldsymbol{u}}}_e}, $$

(6.22)

with ${\boldsymbol{e}} = {\boldsymbol{q}} - {{{\boldsymbol{q}}}_d}$, ${{{\boldsymbol{p}}}_e} = {\boldsymbol{p}} - {{{\boldsymbol{p}}}_d}$ and

$$ {{{\boldsymbol{M}}}_e} = \left[ {\begin{array}{llll} {{{{\boldsymbol{M}}}_M}} & 0 \\0 & {{{{\boldsymbol{M}}}_A}} \\\end{array} } \right],\quad {{{\boldsymbol{R}}}_m} = \left[ {\begin{array}{llll} {{{{\boldsymbol{D}}}_M}} & 0 \\0 & {{{{\boldsymbol{D}}}_A}} \\\end{array} } \right],\quad {{{\boldsymbol{K}}}_P} = \left[ {\begin{array}{llll} {{{{\boldsymbol{K}}}_A}} & { - {{{\boldsymbol{K}}}_A}} \\{ - {{{\boldsymbol{K}}}_A}} & {{{{\boldsymbol{K}}}_A} + {{{\boldsymbol{K}}}_G}} \\\end{array} } \right], $$

(6.23)

where ${{{\boldsymbol{K}}}_P}$ contains the spring stiffness matrix ${{{\boldsymbol{K}}}_A}$ and the gravity force matrix ${{{\boldsymbol{K}}}_G.}$The following abbreviations are used: ${{{\boldsymbol{D}}}_M} = {\text{diag}}([{{d}_{{M1}}},{{d}_{{M2}}},{{d}_{{M3}}}])$, ${{{\boldsymbol{D}}}_A}=\text{diag}([{{d}_{{A1}}},{{d}_{{A2}}},{{d}_{{A3}}}])$, ${{{\boldsymbol{K}}}_A} = {\text{diag}}([{{k}_1},{{k}_2},{{k}_3}])$, ${\boldsymbol{G}}_e^T = (0{\boldsymbol{B}}_e^T)$.

6.6 IDA-PB Control

In this section, we focus on the control design of port-controlled Hamiltonian systems of the tracking error dynamics

$$ \mathop{{{\dot{\boldsymbol{x}}}}}\nolimits_e = \left( {{\boldsymbol{J}} - {\boldsymbol{R}}} \right)\mathop{{\left( {\frac{{\partial {{H}_e}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T +\; {{{\boldsymbol{G}}}_e}{{{\boldsymbol{u}}}_e},\quad \quad {{{\boldsymbol{y}}}_e} = {\boldsymbol{G}}_e^T\mathop{{\left( {\frac{{\partial {{H}_e}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T, $$

(6.24)

see also (6.8). The fundamental idea is based on the formulation of a target for the closed loop systems, such as

$$ \mathop{{{\dot{\boldsymbol{x}}}}}\nolimits_e = \left( {{{{\boldsymbol{J}}}_d} - {{{\boldsymbol{R}}}_d}} \right)\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T. $$

(6.25)

Obviously, this results in

$$ \begin{array}{lll}\mathop{{{\dot{\boldsymbol{x}}}}}\nolimits_e & = \left( {{\boldsymbol{J}}-{\boldsymbol{R}}}\right)\mathop{{\left( {\frac{{\partial{{H}_e}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T + {{{\boldsymbol{G}}}_e}{{{\boldsymbol{u}}}_e} \\& \mathop{ = }\limits^{!}\left( {{{{\boldsymbol{J}}}_d} - {{{\boldsymbol{R}}}_d}}\right)\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial{{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T,\end{array} $$

(6.26)

setting the original plant of the tracking error system in (6.24) equal to the desired system in (6.25). Computing the control law directly with

$$ {{{\boldsymbol{G}}}_e}{{{\boldsymbol{u}}}_e} = \left( {{{{\boldsymbol{J}}}_d} - {{{\boldsymbol{R}}}_d}} \right)\mathop{{\left({\frac{{\partial {{H}_e}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T - \;\left( {{\boldsymbol{J}} - {\boldsymbol{R}}} \right)\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T $$

(6.27)

and the pseudoinverse of ${{{\boldsymbol{G}}}_e}$, by some rearranging is apparently simple. Clearly, in the case of a non-full rank matrix ${{{\boldsymbol{G}}}_e}$ (i.e. if the system is underactuated), there are general limitations, which are shown next. In accordance with the literature [14, 19], damping injection

$$ {{{\boldsymbol{G}}}_e}{{{\boldsymbol{u}}}_e} = {{{\boldsymbol{G}}}_e}{{{\boldsymbol{u}}}_I} + \left( {{{{\boldsymbol{J}}}_d} - {{{\boldsymbol{R}}}_d}} \right)\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T - \left( {{\boldsymbol{J}} - {\boldsymbol{R}}} \right)\mathop{{\left( {\frac{{\partial {{H}_e}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T, $$

(6.28)

with an additional input ${{{\boldsymbol{u}}}_I}$ is introduced. Roughly speaking, a proportional feedback of the collocated output y with

$$ {{{\boldsymbol{u}}}_I} = - {{{\boldsymbol{D}}}_I}{\boldsymbol{y}} = - {{{\boldsymbol{D}}}_I}{\boldsymbol{G}}_e^T\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T $$

(6.29)

is added. Thus, we obtain

$$ {{{\boldsymbol{G}}}_e}{{{\boldsymbol{u}}}_e} = \left( {{{{\boldsymbol{J}}}_d} - \mathop{{\overline {\boldsymbol{R}} }}\nolimits_d } \right)\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T - \left( {{\boldsymbol{J}} - {\boldsymbol{R}}} \right)\mathop{{\left( {\frac{{\partial {{H}_e}}}{{\partial {{{\boldsymbol{x}}}_e}}}} \right)}}\nolimits^T $$

(6.30)

with the new damping matrix $\mathop{{\overline {\boldsymbol{R}} }}\nolimits_d = {{{\boldsymbol{R}}}_d} + {{{\boldsymbol{G}}}_e}{{{\boldsymbol{D}}}_I}{\boldsymbol{G}}_e^T$. Up to this point, the approach involves numerous design parameters, such as ${{H}_d} > 0,{{{\boldsymbol{J}}}_d} = - {\boldsymbol{J}}_d^T,{{{\boldsymbol{R}}}_d} = {\boldsymbol{R}}_d^T,{{{\boldsymbol{D}}}_I}$ and is therefore unsuitable from the design perspective. Typically, simplification is achieved by means of the IDA-PBC, which assumes a physical representation of the desired closed loop system, with

$$ {{H}_d} = \frac{1}{2}{{{\boldsymbol{e}}}^T}{{{\boldsymbol{K}}}_d}{\boldsymbol{e}} + \frac{1}{2}{\boldsymbol{p}}_e^T{\boldsymbol{M}}_d^{{ - 1}}{{{\boldsymbol{p}}}_e} > 0 $$

(6.31)

and

$$ \begin{array}{lll}{{{\boldsymbol{J}}}_d} = -{\boldsymbol{J}}_d^T = \left[ {\begin{array}{llll} 0 &{{\boldsymbol{M}}_e^{{ - 1}}{{{\boldsymbol{M}}}_d}} \\{ -{{{\boldsymbol{M}}}_d}{\boldsymbol{M}}_e^{{ - 1}}} &{{{{\boldsymbol{S}}}_{{22d}}}} \\\end{array} } \right],{{{\boldsymbol{R}}}_d} = {\boldsymbol{R}}_d^T = \left[ {\begin{array}{llll}0 & 0 \\0 & {{{{\boldsymbol{R}}}_{{22d}}} +{{{\boldsymbol{B}}}_e}{{{\boldsymbol{D}}}_I}{\boldsymbol{B}}_e^T} \\\end{array}} \right].\end{array} $$

(6.32)

Adding these equations and referring to (6.26) and (6.30), we can calculate

$$ \begin{array}{llll} \mathop{{{\dot{\boldsymbol{x}}}}}\nolimits_e & = \left[ {\begin{array}{llll} 0 & {\boldsymbol{E}} \\{ - {\boldsymbol{E}}} & { - {{{\boldsymbol{R}}}_m}} \\\end{array} } \right]\left( {\begin{array}{llll} {\mathop{{\left({\displaystyle\frac{{\partial {{H}_e}}}{{\partial {\boldsymbol{e}}}}} \right)}}\nolimits^T } \\{\mathop{{\left( {\displaystyle\frac{{\partial {{H}_e}}}{{\partial {{{\boldsymbol{p}}}_e}}}} \right)}}\nolimits^T } \\\end{array} } \right) + \left( {\begin{array}{llll} 0 \\{{{{\boldsymbol{B}}}_e}} \\\end{array} } \right){{{\boldsymbol{u}}}_e} \\& \mathop{ = }\limits^{!} \left[ {\begin{array}{llll} 0 & {{\boldsymbol{M}}_e^{{ - 1}}{{{\boldsymbol{M}}}_d}} \\{ - {\boldsymbol{M}}_e^{{ - 1}}{{{\boldsymbol{M}}}_d}} & {{{{\boldsymbol{S}}}_{{22d}}} - ({{{\boldsymbol{R}}}_{{22d}}} + {{{\boldsymbol{B}}}_e}{{{\boldsymbol{D}}}_I}{\boldsymbol{B}}_e^T)} \\\end{array} } \right]\left( {\begin{array}{llll} {\mathop{{\left( \displaystyle{\frac{{\partial {{H}_d}}}{{\partial {\boldsymbol{e}}}}} \right)}}\nolimits^T } \\{\mathop{{\left( {\displaystyle\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{p}}}_e}}}}\right)}}\nolimits^T } \\\end{array} } \right).\end{array} $$

(6.33)

Satisfying the first constraint – the so-called matching condition of the generalized momenta – becomes

$$ {\dot{\boldsymbol{e}}} = \mathop{{\left( {\frac{{\partial{{H}_e}}}{{\partial {{{\boldsymbol{p}}}_e}}}} \right)}}\nolimits^T ={\boldsymbol{M}}_e^{{ - 1}}{{{\boldsymbol{p}}}_e}\mathop{ = }\limits^{!}{\boldsymbol{M}}_e^{{ - 1}}{{{\boldsymbol{M}}}_d}\mathop{{\left({\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{p}}}_e}}}}\right)}}\nolimits^T = {\boldsymbol{M}}_e^{{ -1}}{{{\boldsymbol{M}}}_d}({\boldsymbol{M}}_d^{{ - 1}}{{{\boldsymbol{p}}}_e}) = {\dot{\boldsymbol{e}}}. $$

(6.34)

As already mentioned, we obtain with the left-hand annihilator ${\boldsymbol{B}}_e^{ \bot }$ implying ${\boldsymbol{B}}_e^{ \bot }{{{\boldsymbol{B}}}_e} = 0$ the non-actuated projected space

$$ \begin{array}{lll} {\boldsymbol{B}}_e^{ \bot }\left({\mathop{{\left( {\frac{{\partial {{H}_e}}}{{\partial{\boldsymbol{e}}}}} \right)}}\nolimits^T +{{{\boldsymbol{R}}}_m}\mathop{{\left( {\frac{{\partial{{H}_e}}}{{\partial{{{\boldsymbol{p}}}_e}}}}\right)}}\nolimits^T -\;{{{\boldsymbol{M}}}_d}{\boldsymbol{M}}_e^{{ -1}}\mathop{{\left({\frac{{\partial {{H}_d}}}{{\partial {\boldsymbol{e}}}}}\right)}}\nolimits^T } \right. + \left.{\underbrace{{({{{\boldsymbol{J}}}_{{22d}}}-{{{\boldsymbol{R}}}_{{22d}}})}}_{{{\boldsymbol{JR}}}}\mathop{{\left({\frac{{\partial {{H}_d}}}{{\partial {{{\boldsymbol{p}}}_e}}}}\right)}}\nolimits^T } \right) = 0 &,\end{array} $$

(6.35)

for an underactuated system. Commonly, these are partial differential equations and can be split for the LTV system into e-dependent and ${{{\boldsymbol{p}}}_e}$-dependent constraints

$$ {\boldsymbol{B}}_e^{ \bot }({{{\boldsymbol{K}}}_P} - {{{\boldsymbol{M}}}_d}{\boldsymbol{M}}_e^{{ - 1}}{{{\boldsymbol{K}}}_d}) = 0 $$

(6.36)

$$ { \boldsymbol{B}}_e^{ \bot }({{{\boldsymbol{R}}}_m} + {\boldsymbol{JRM}}_d^{{ - 1}}{{{\boldsymbol{M}}}_e}) = 0, $$

(6.37)

and results in a coefficient comparison. Nevertheless, proof of the stability remain outstanding and implies for ${{H}_d} > 0$ (viz. ${{{\boldsymbol{M}}}_d},{{{\boldsymbol{K}}}_d} > 0$) in

$$ \begin{array}{lll}\mathop{{\dot{H}}}\nolimits_d & = -\left( {\frac{{\partial {{H}_d}}}{{\partial{{{\boldsymbol{p}}}_e}}}}\right)\left( {{{{\boldsymbol{R}}}_{{22d}}} +{{{\boldsymbol{B}}}_e}{{{\boldsymbol{D}}}_I}{\boldsymbol{B}}_e^T}\right)\mathop{{\left( {\frac{{\partial {{H}_d}}}{{\partial{{{\boldsymbol{p}}}_e}}}} \right)}}\nolimits^T \\& = - \mathop{{{\dot{\boldsymbol{e}}}}}\nolimits^T {{{\boldsymbol{M}}}_e}{\boldsymbol{M}}_d^{{ -1}}\left( {{{{\boldsymbol{R}}}_{{22d}}} +{{{\boldsymbol{B}}}_e}{{{\boldsymbol{D}}}_I}{\boldsymbol{B}}_e^T}\right){\boldsymbol{M}}_d^{{ - 1}}{{{\boldsymbol{M}}}_e}{\dot{\boldsymbol{e}}}\le 0,\end{array} $$

(6.38)

if ${{{\boldsymbol{R}}}_{{22d}}} + {{{\boldsymbol{B}}}_e}{{{\boldsymbol{D}}}_I}{\boldsymbol{B}}_e^T \ge 0$. Choosing the design parameters is complex and not intuitive. For the elastic robot we use

$$ \begin{array}{lll}{{{\boldsymbol{M}}}_d} & = \left[ {\begin{array}{llll} {{{{\boldsymbol{K}}}_1}{{{\boldsymbol{M}}}_M}} & 0 \\0 & {{\boldsymbol{K}}_2^{{ - 1}}{{{\boldsymbol{M}}}_A}({{{\boldsymbol{q}}}_{{A,d}}})} \\\end{array} } \right], \\{{{\boldsymbol{K}}}_d} & = \left[{\begin{array}{llll} {{{{\boldsymbol{K}}}_3}} & { - {{{\boldsymbol{K}}}_A}{{{\boldsymbol{K}}}_2}} \\{ - {{{\boldsymbol{K}}}_2}{{{\boldsymbol{K}}}_A}} & {{{{\boldsymbol{K}}}_2}({{{\boldsymbol{K}}}_A} + {{{\boldsymbol{K}}}_G})} \\\end{array} } \right],\end{array} $$

(6.39)

and

$$ {{{\boldsymbol{J}}}_{{22d}}} = \left[ {\begin{array}{llll} 0 & { - \frac{1}{2}{{{\boldsymbol{D}}}_2}} \\{\frac{1}{2}{{{\boldsymbol{D}}}_2}} & 0 \\\end{array} } \right],\quad {{{\boldsymbol{R}}}_{{22d}}} = \left[ {\begin{array}{llll} 0 & {\frac{1}{2}{{{\boldsymbol{D}}}_2}} \\{\frac{1}{2}{{{\boldsymbol{D}}}_2}} & {{\boldsymbol{K}}_2^{{ - 1}}{{{\boldsymbol{D}}}_A}} \\\end{array} } \right], $$

(6.40)

with ${{{\boldsymbol{D}}}_I} = {{{\boldsymbol{D}}}_1}$, where all constraints are fulfilled, with ${{{\boldsymbol{K}}}_1},{{{\boldsymbol{K}}}_2},{{{\boldsymbol{K}}}_3},{{{\boldsymbol{D}}}_1},{{{\boldsymbol{D}}}_2} > 0$. The structure of

$$ {{{\boldsymbol{M}}}_A} = \left[ {\begin{array}{llll} {{{m}_{{a11}}}} & 0 & 0 \\0 & {{{m}_{{a22}}}} & {{{m}_{{a23}}}} \\0 & {{{m}_{{a23}}}} & {{{m}_{{a33}}}} \\\end{array} } \right] $$

(6.41)

makes satisfying the constraints in (6.23) easy by choosing two identical entries in ${{{\boldsymbol{K}}}_2} = {\text{diag}}([{{k}_{{21}}},{{k}_{{22}}},{{k}_{{22}}}])$. Permutation then yields

$$ {\boldsymbol{M}}_d^{{ - 1}}{{{\boldsymbol{M}}}_e} = \left[ {\begin{array}{llll} {{\boldsymbol{K}}_1^{{ - 1}}} & 0 \\0 & {{{{\boldsymbol{K}}}_2}} \\\end{array} } \right],\quad {{{\boldsymbol{M}}}_d}{\boldsymbol{M}}_e^{{ - 1}} = \left[ {\begin{array}{llll} {{{{\boldsymbol{K}}}_1}} & 0 \\0 & {{\boldsymbol{K}}_2^{{ - 1}}} \\\end{array} } \right], $$

(6.42)

which always results in a diagonal form and provides decoupled constraints. Finally, we obtain the IDA-PBC control law for the tracking error system

$$ \begin{array}{lll}{{{\boldsymbol{u}}}_e} & = {\boldsymbol{B}}_e^{ + }\left( {{{{\boldsymbol{K}}}_A}({{{\boldsymbol{e}}}_M} -{{{\boldsymbol{e}}}_A}) - {{{\boldsymbol{K}}}_1}{{{\boldsymbol{K}}}_3}{{{\boldsymbol{e}}}_M} +{{{\boldsymbol{K}}}_1}{{{\boldsymbol{K}}}_2}{{{\boldsymbol{K}}}_A}{{{\boldsymbol{e}}}_A}}\right. \\& \quad + \left. {({{{\boldsymbol{D}}}_M} -{\boldsymbol{B}}_e^T{{{\boldsymbol{D}}}_1}{{{\boldsymbol{B}}}_e}{\boldsymbol{K}}_1^{{- 1}})\mathop{{{\dot{\boldsymbol{e}}}}}\nolimits_M -{{{\boldsymbol{D}}}_2}{{{\boldsymbol{K}}}_2}\mathop{{{\dot{\boldsymbol{e}}}}}\nolimits_A } \right).\end{array} $$

(6.43)

with (6.30) and the pseudo inverse ${\boldsymbol{B}}_e^{ + }.$ Analysis of the above equation shows that the cascaded motor joint control with very short cycle time is preserved with some rearranging. However, since the control concept requires knowledge of arm positions and velocities, which result from relatively complex calculations, the control scheme cannot be implemented entirely on the servo drives.

6.7 Experiments

As mentioned in the introduction, we verified the control strategy experimentally in a fast straight line in space and a ball catching scenario.

6.7.1 Straight Line in Space

First, we tested the suggested concept for a straight line in space with the following TCP trajectory performance specifications: length approx. 2.1 m, duration 1.05 s, maximum velocity 5 m/s, and maximum acceleration 18 m/s². Figure 6.5 shows the desired TCP accelerations and those achieved with a common motor joint control (PD) and with the suggested flatness-based feedforward control in combination with the IDA-PBC feedback control design (IDA). Clearly, the PD motor joint control cannot be used for fast applications because of the high oscillations at the TCP. The proposed control concept, in contrast, suppresses vibration considerably.

Fig. 6.5

TCP accelerations tracking a straight line

6.7.2 Ball Catching Scenario

In this section, we demonstrate the versatility of a lightweight structure – such as the considered elastic robot. Since we have only three joints, orientation cannot be controlled independently. Thus, we selected the setup shown in Fig. 6.6. The task of the robot was to overtake and then catch the object with the mounted hat. This process took 0.6 s from the start command to the catch and required approximately 1.4 m in gravitational direction from start to end position. The resulting TCP acceleration in the y-direction (along which gravity is acting) is shown in Fig. 6.7. At approximately t = 0.6 s – when the ball is being caught – the measured acceleration deviates from the ideal. Figure 6.8 shows an image sequence of this scenario.

Fig. 6.6

Ball catching setup: The ball leaves the mount as the robot moves sideways, and accelerates towards the ground

Fig. 6.7

TCP accelerations in gravitational direction during ball catching

6.8 Interpretation and Conclusion

In summary, a wide range of techniques are required to handle elastic vibrations for tracking fast trajectories with satisfying accuracy. We started with a detailed model of the complex elastic multibody systems introducing Ritz Ansatz functions for the distributed parameters. Due to the complexity of the system an efficient method for modeling repeating assemblies, namely the Projection Equation in subsystem representation is used. In contrast, a relatively simple model – the LEM – is adequate for control design. Our control concept requires knowledge of arm positions and velocities. To this end, we propose a technique in which acceleration and angular rate sensors, mounted on the elbow and on the TCP can be used, to derive these system states. The Projection Equation forms the basis of this approach.

In a 2DoF control scheme, we use a flatness-based feedforward control and a passivity-based feedback control. For the latter, the IDA-PBC feedback controller was designed and resulted in a feasible feedback loop. Experimental results showed a great improvement in terms of vibration damping.

In conclusion this approach to vibration suppression in highly nonlinear systems is focused on techniques that meet the industrial demand for a lower-level implementation on common industrial hardware. We have shown that model-based control using acceleration and angular rate sensors meets this demand.

Fig. 6.8

Image sequence of the ball catching scenario