# Model Predictive Control for Flexible Joint Robots

Maged Iskandar, Christiaan van Ommeren, Xuwei Wu, Alin Albu-Schäffer, and
Alexander Dietrich The authors are with the Institute of Robotics and
Mechatronics, German Aerospace Center (DLR), Wessling, Germany,
<EMAIL_ADDRESS>
###### Abstract

Modern Lightweight robots are constructed to be collaborative, which often
results in a low structural stiffness compared to conventional rigid robots.
Therefore, the controller must be able to handle the dynamic oscillatory
effect mainly due to the intrinsic joint elasticity. Singular perturbation
theory makes it possible to decompose the flexible joint dynamics into fast
and slow subsystems. This model separation provides additional features to
incorporate future knowledge of the joint-level dynamical behavior within the
controller design using the Model Predictive Control (MPC) technique. In this
study, different architectures are considered that combine the method of
Singular Perturbation and MPC. For Singular Perturbation, the parameters that
influence the validity of using this technique to control a flexible-joint
robot are investigated. Furthermore, limits on the input constraints for the
future trajectory are considered with MPC. The position control performance
and robustness against external forces of each architecture are validated
experimentally for a flexible joint robot.

##  I Introduction

The lightweight robot design is a beneficial concept in various aspects such
as energy efficiency, payload-to-weight ratio, and safety features in human-
robot interaction [1, 2]. One of the main challenges related to that is to
design a controller that can handle both the oscillatory behavior (local joint
vibrations), caused by the mechanical flexibility, and the motor constraints
(e. g., torque or velocity limits). If both are not considered properly,
instabilities can occur [3]. Therefore, for model-based control, it is crucial
to provide a sufficiently accurate model within the range of operation.

Various approaches are known from the literature to control a robot with
flexible elements [4, 5, 6, 7]. A common technique that aims at splitting the
system dynamics into a slow and fast subsystem and enabling the design of the
control law for both the slow and the fast models independently is Singular
Perturbation (SP) [8]. The control law of the link-dynamics is usually
obtained based on the approximate slow model of the zeroth order [4], and the
joint-level torque control loop is designed and regulated separately within
the SP approach. In addition to that, parameter uncertainties are considered
through a regression vector. An extension of the control law by corrective
terms is suggested in [5], while in [6] the slow model of the flexible-joint
robot is improved by adding an additional perturbation term which reduces the
overall tracking error. Furthermore, in [7] shaping parts of the system
dynamics using a desired dynamical behavior is achieved to control the
flexible-joint in the link-side directly. As known from the literature, the
preservation of the natural inertia is highly beneficial in terms of
robustness [9].

The main restrictions of SP are the inability to incorporate actuator
saturation and the fact that only current reference points are considered. A
technique that addresses these drawbacks is Model Predictive Control (MPC). It
has been applied across different areas, such as autonomous driving [10],
drones [11], or interconnected tanks [12]. Extensions of the standard linear
or non-linear MPC involve passivity-based state constraints [13], the addition
of safety filters [14], the consideration of process uncertainty [15], or
various learning components [16]. In the area of flexible joint robots, the
authors in [17] have implemented an explicit MPC with the advantage that
hardware-requirements for real-time execution are lower. However, any offline
solutions need to be recalculated if the model is modified. In [18] gravity is
considered in the model linearization which results in an adaptive MPC, as it
adapts its linear model according to the link position. In general, the MPC
approach can be combined with SP. For example, in [19] the SP is applied to
separate into a slow and a fast model, then MPC is applied to control both of
them. In [20] a vision-based MPC approach is developed to handle the joint
flexibility and dampen out the corresponding end-effector vibrations.

Figure 1: Conceptual example of flexible joint robot and its simplified single
joint dynamical model. The elastic joint torque is transmitted between the
inertia of the motor and the link via the joint stiffness.

One of the main challenges of applying SP to flexible-joint dynamics is to
identify under which circumstances this theory can be applied. The reason is
that an upper bound of the perturbation parameter can still exist, even though
it is approximated to be zero in the slow model [21, 22]. This upper bound can
be found by the contraction mapping principle [23] or based on the frequency
domain approach [24]. When a simplified model [25] is considered, this
parameter is typically proportional to the inverse of the square of the joint
stiffness. Since the overall dynamics is altered by the control law, the
relative distance towards the upper bound can be manipulated. However, even if
this upper bound would be higher than the actual perturbation parameter,
instabilities could still occur due to actuator saturation. Therefore, it is
crucial that the parameters of the control law are aligned with the
capabilities of the system to achieve higher control performance.

In this work, three different (multiple) MPC structures are proposed for the
general flexible-joint robot dynamics. The control structures follow the
concept of the two-time-scales of SP theory. The approach allows to impose
constraints into the control law e. g. the maximum allowable torque of the
joints actuators. Experimental comparisons on a flexible joint robot
demonstrate the performance of the MPC framework, also w. r. t. the classical
SP approach and a standard motor-PD controller. In addition, the validity of
the SP method is investigated w. r. t. joint stiffness and actively shaping
the motor inertia at different control frequencies. Furthermore, the approach
allows to realize link-side impedance/position actions, enabling direct
controllable interaction with the environment.

The paper is organized as follows. In Section II, the system model to be used
throughout this work is introduced, and an overview of the standard SP
approach for the flexible joint robot is provided. The proposed MPC design
idea and structures are presented in Section Section III. Experimental results
and validations of the control design approach are shown and discussed in
Section IV. Finally, the conclusion in Section V closes the paper.

##  II Fundamentals

The dynamical equations of flexible-joint robots require twice the number of
generalized coordinates compared to rigid-body systems. For revolute-joint
robots, they can be written based on the assumptions that the joint elasticity
can be modeled through a linear torsional spring and the kinetic energy of
each motor is only due to its own rotation [25]:

$\displaystyle\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}}+\boldsymbol{g}(\boldsymbol{q})$
$\displaystyle=\boldsymbol{K}(\boldsymbol{\theta}-\boldsymbol{q})+\boldsymbol{\tau}_{\text{ext}}\
,$ (1)
$\displaystyle\boldsymbol{B}\ddot{\boldsymbol{\theta}}+\boldsymbol{K}(\boldsymbol{\theta}-\boldsymbol{q})$
$\displaystyle=\boldsymbol{\tau}_{\text{m}}\ .$ (2)

Herein, ${\boldsymbol{q}\in\mathbb{R}^{n}}$ represents the vector of the $n$
link-side coordinates and ${\boldsymbol{\theta}\in\mathbb{R}^{n}}$ describes
the corresponding motor position coordinates. Let
${\boldsymbol{\tau}=\boldsymbol{K}(\boldsymbol{\theta}-\boldsymbol{q})}$ be
the elastic joint torque which is transmitted between motor and link, with the
positive definite joint stiffness ${\boldsymbol{K}\succ 0}$. Gravitational
torques are represented by
${\boldsymbol{g}(\boldsymbol{q})\in\mathbb{R}^{n}}$, and the symmetric and
positive definite link-side inertia matrix is defined by
${\boldsymbol{M}(\boldsymbol{q})\in\mathbb{R}^{n\times n}}$. Also,
${\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\in\mathbb{R}^{n\times
n}}$ is the Coriolis/centrifugal matrix which is formulated such that the
skew-symmetry
$\dot{\boldsymbol{M}}(\boldsymbol{q})=\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})+\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})^{T}$
holds. External torques are described by $\boldsymbol{\tau}_{\mathrm{ext}}$.
The motor inertia is given by ${\boldsymbol{B}\in\mathbb{R}^{n\times n}}$ and
accelerated by the motor torque $\boldsymbol{\tau}_{\text{m}}$.111The motor is
modeled as an ideal torque source neglecting underlying (electrical) dynamics.
Here, a wider class of systems can be considered without loss of generality e.
g. to include prismatic joints.

Commonly, the torque dynamics can be regulated by using the singular-
perturbation (SP) approach [8, 26] as

$\boldsymbol{\tau}_{\text{m}}=\boldsymbol{\tau}_{\mathrm{d}}+\boldsymbol{K}_{\mathrm{T}}(\boldsymbol{\tau}_{\mathrm{d}}-\boldsymbol{\tau})-\epsilon\boldsymbol{K}_{\mathrm{S}}\dot{\boldsymbol{\tau}}\
.$ (3)

That can be interpreted as classical PD control for torque regulation [27, 3].
The desired joint torque $\boldsymbol{\tau}_{\mathrm{d}}$ is fed forward and
the (measured) joint torque $\boldsymbol{\tau}$ and its time derivative
$\dot{\boldsymbol{\tau}}$ are used in the feedback loop incorporating the
corresponding positive definite gain matrices $\boldsymbol{K}_{\mathrm{T}}$,
$\boldsymbol{K}_{\mathrm{S}}$, and $\epsilon$ is a small positive parameter.
The term $\dot{\boldsymbol{\tau}}$ is usually derived numerically, based on
$\boldsymbol{\tau}$, and $\epsilon$ is motivated by the singular-perturbation
method and is responsible for the two time-scale separation. The dynamics
(1)–(2) can be rewritten as

$\displaystyle\boldsymbol{M}(\boldsymbol{q})\ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}}+\boldsymbol{g}(\boldsymbol{q})$
$\displaystyle=\boldsymbol{\tau}+\boldsymbol{\tau}_{\text{ext}}\ ,$ (4)
$\displaystyle\boldsymbol{B}\boldsymbol{K}^{-1}\ddot{\boldsymbol{\tau}}+\boldsymbol{\tau}$
$\displaystyle=\boldsymbol{\tau}_{\text{m}}-\boldsymbol{B}\ddot{\boldsymbol{q}}\
.$ (5)

According to SP theory the system can be expressed in a two-time-scale manner,
with

$\begin{split}(\boldsymbol{M}(\boldsymbol{q}_{\text{slow}})+\boldsymbol{B})\ddot{\boldsymbol{q}}_{\text{slow}}+\boldsymbol{C}(\boldsymbol{q}_{\text{slow}},\dot{\boldsymbol{q}}_{\text{slow}})\dot{\boldsymbol{q}}_{\text{slow}}\\\
+\boldsymbol{g}(\boldsymbol{q}_{\text{slow}})=\boldsymbol{\tau}_{\text{m,}\text{slow}}+\boldsymbol{\tau}_{\text{ext,}\text{slow}}\\\
\end{split}$ (6)

for the slow dynamics. The so-called boundary layer system can be obtained
through the deviation of the actual joint torque from its quasi-steady-state
value. That also defines the fast component of the torque as

$\boldsymbol{\tau}_{\text{fast}}=\boldsymbol{\tau}-\boldsymbol{\tau}_{\text{slow}}\,,$
(7)

where $\boldsymbol{\tau}_{\text{slow}}$ is the slow component of the joint
torque. By applying the SP approach to the flexible-joint dynamics, one
obtains two subsystems that can be separately controlled. Intuitively
speaking, the slow part (LABEL:eq:slow_dynamics) forms an equivalent rigid-
body model, which one also obtains when neglecting the motor or torque
dynamics. From that perspective the classical SP control law (3) can be
straightforwardly reformulated as

$\boldsymbol{\tau}_{\text{m}}=\underbrace{(\boldsymbol{I}+\boldsymbol{K}_{\mathrm{T}})\boldsymbol{\tau}_{\mathrm{d}}-\boldsymbol{K}_{\mathrm{T}}\boldsymbol{\tau}_{\text{slow}}}_{\mathrm{Slow\,component}}+\underbrace{\boldsymbol{K}_{\mathrm{T}}(\boldsymbol{\tau}_{\text{slow}}-\boldsymbol{\tau})-\epsilon\boldsymbol{K}_{\mathrm{S}}\dot{\boldsymbol{\tau}}}_{\mathrm{Fast\,component}}$
(8)

which results in the slow controlled link-side dynamics

$(\boldsymbol{M}(\boldsymbol{q})+(\boldsymbol{I}+\boldsymbol{K}_{\mathrm{T}})^{-1}\boldsymbol{B})\ddot{\boldsymbol{q}}+\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})\dot{\boldsymbol{q}}+\boldsymbol{g}(\boldsymbol{q})=\boldsymbol{\tau}_{\mathrm{d}}\
.$ (9)

Analogous to (LABEL:eq:slow_dynamics), it has the form of the rigid-body model
with inertia matrix
${\boldsymbol{M}(\boldsymbol{q})+(\boldsymbol{I}+\boldsymbol{K}_{\mathrm{T}})^{-1}\boldsymbol{B}}$.
From a physical point of view, this can be intuitively interpreted as an
active reduction of the motor inertia from $\boldsymbol{B}$ to the desired
value $\boldsymbol{B}_{\mathrm{d}}$ when choosing
$\boldsymbol{K}_{\mathrm{T}}=\boldsymbol{B}\boldsymbol{B}_{\mathrm{d}}^{-1}-\boldsymbol{I}$
in (3). In this case the torque control loop can be specified through the
damping parameter $\boldsymbol{K}_{\mathrm{S}}$ and the ratio of the reduction
of the apparent motor inertia
$\boldsymbol{B}\boldsymbol{B}_{\mathrm{d}}^{-1}$. The commanded motor torque
(3) can be combined with a link-side PD tracking controller as

$\boldsymbol{\tau}_{\mathrm{d}}=\boldsymbol{g}(\boldsymbol{q})+(\boldsymbol{M}(\boldsymbol{q})+\boldsymbol{B}_{\mathrm{d}})\ddot{\boldsymbol{q}}_{\mathrm{d}}-\boldsymbol{K}_{q}(\boldsymbol{q}-\boldsymbol{q}_{\mathrm{d}})-\boldsymbol{D}_{q}(\dot{\boldsymbol{q}}-\dot{\boldsymbol{q}}_{\mathrm{d}}),$
(10)

with ${\boldsymbol{K}_{q},\boldsymbol{D}_{q}\succ 0}$ being the desired link-
side stiffness and damping, respectively, and $\boldsymbol{q}_{\mathrm{d}}$ is
the desired link position. Noticeably, (10) is obtained using link-side
coordinates but conventionally the motor-side position and velocity are used
to generate the desired torque [27]. Interestingly, using the link-side
coordinates, one can consider the classical control methods applied on rigid-
body dynamics [28].

In (1)–(2), dissipative friction effects can be included in different forms.
The most dominant component in flexible-joint robots is motor-side friction
due to gear friction [27], which is actively reduced by the factor
$\boldsymbol{B}\boldsymbol{B}_{\mathrm{d}}^{-1}$ as a result of (3). Further
friction effects can be considered by using model-based compensation
techniques [29, 30] or the use of motor-side friction observers [31].
Nevertheless, this work focuses on the effects of joint elasticity, and
therefore, friction effects are not explicitly considered in the following
analysis.

##  III Methods

Inspired by SP theory, the two-time-scale property can be used to apply the
MPC technique in order to handle different ranges of dynamical effects. This
also allows to impose practically motivated constraints within the low-level
joint control to consider the physical limitations of the system actuation.

###  III-A MPC-Fast (motor dynamics)

Since SP theory allows the separation between fast dynamics (torque dynamics)
and slow dynamics (rigid-body link dynamics), it creates the prerequisites to
apply MPC for the low-level torque control loop. The joint stiffness
$\boldsymbol{K}$ is related to the perturbation parameter $\epsilon$ through

$\boldsymbol{K}=\frac{\boldsymbol{K}_{\epsilon}}{\epsilon^{2}}\ .$ (11)

Let ${\boldsymbol{K}_{\epsilon}\succ 0}$ be a diagonal matrix. Using (4)–(5)
the dynamics can be reformulated222Dependencies on the states have been
omitted for the sake of readability. as

$\epsilon^{2}\ddot{\boldsymbol{\tau}}=\boldsymbol{K}_{\epsilon}(\boldsymbol{B}^{-1}\boldsymbol{\tau}_{\text{m}}-(\boldsymbol{M}^{-1}+\boldsymbol{B}^{-1})\boldsymbol{\tau}+\boldsymbol{M}^{-1}\boldsymbol{n})\
,$ (12)

with
${\boldsymbol{n}=\boldsymbol{C}(\boldsymbol{q},\dot{\boldsymbol{q}})+\boldsymbol{g}(\boldsymbol{q})}$.
Analogous to (7), the motor torque is composed of slow and fast terms as

$\boldsymbol{\tau}_{\text{m}}=\boldsymbol{\tau}_{\text{m,}\text{slow}}+\boldsymbol{\tau}_{\text{m,}\text{fast}}$
(13)

When ${\epsilon\rightarrow 0}$ in (12) the slow component of the torque
dynamics $\boldsymbol{\tau}_{\text{slow}}|_{\epsilon\rightarrow 0}$ can be
obtained.

$\boldsymbol{\tau}_{\text{slow}}=(\boldsymbol{M}^{-1}+\boldsymbol{B}^{-1})^{-1}(\boldsymbol{B}^{-1}\boldsymbol{\tau}_{\text{m,}\text{slow}}+\boldsymbol{M}^{-1}\boldsymbol{n})\
.$ (14)

Using (7) the fast component of the torque dynamics is

$\boldsymbol{\tau}_{\text{fast}}=\boldsymbol{\tau}-(\boldsymbol{M}^{-1}+\boldsymbol{B}^{-1})^{-1}(\boldsymbol{B}^{-1}\boldsymbol{\tau}_{\text{m,}\text{slow}}+\boldsymbol{M}^{-1}\boldsymbol{n})\
.$ (15)

By means of SP [32] and through the substitution in (12) the fast time scale
can be introduced as $\nu=t/\epsilon$ with time $t$. Thus, the second
derivative of (15) w. r. t. the time scale $\nu$ is333The first and second
time derivatives in the normal time scale $t$ are
${\dot{\boldsymbol{z}}:=\frac{d\boldsymbol{z}}{dt},\ddot{\boldsymbol{z}}:=\frac{d^{2}\boldsymbol{z}}{dt^{2}}}$,
while in the fast time scale $\nu$, they are given by
${\boldsymbol{z}^{\prime}:=\frac{d\boldsymbol{z}}{d\nu},\boldsymbol{z}^{\prime\prime}:=\frac{d^{2}\boldsymbol{z}}{d\nu^{2}}}$,
with the relations
${\boldsymbol{z}^{\prime}=\epsilon\dot{\boldsymbol{z}},\boldsymbol{z}^{\prime\prime}=\epsilon^{2}\ddot{\boldsymbol{z}}}$.

${\boldsymbol{\tau}^{\prime\prime}_{\text{fast}}}=\boldsymbol{\tau}^{\prime\prime}={\epsilon^{2}}\ddot{\boldsymbol{\tau}}$
(16)

substituting $\ddot{\boldsymbol{\tau}}$ from (12). Keeping in mind that the
dynamics can be expressed in the fast time scale, one obtains

${\boldsymbol{\tau}^{\prime\prime}_{\text{fast}}}=-\boldsymbol{K}_{\epsilon}(\boldsymbol{M}^{-1}+\boldsymbol{B}^{-1})\boldsymbol{\tau}_{\text{fast}}+\boldsymbol{K}_{\epsilon}\boldsymbol{B}^{-1}\boldsymbol{\tau}_{\text{m,}\text{fast}}\,.$
(17)

At this point (17) can be converted back to the normal time scale by
$\boldsymbol{\tau}^{\prime\prime}_{\text{fast}}=\epsilon^{2}\ddot{\boldsymbol{\tau}}_{\text{fast}}$,
that is,

$\ddot{\boldsymbol{\tau}}_{\text{fast}}=-\boldsymbol{K}(\boldsymbol{M}^{-1}+\boldsymbol{B}^{-1})\boldsymbol{\tau}_{\text{fast}}+\boldsymbol{K}\boldsymbol{B}^{-1}\boldsymbol{\tau}_{\text{m,}\text{fast}}\
.$ (18)

That yields the full representation of the fast torque dynamics featuring

$\begin{bmatrix}\dot{\boldsymbol{\tau}}_{\text{fast}}\\\
\ddot{\boldsymbol{\tau}}_{\text{fast}}\end{bmatrix}\\!\\!=\\!\\!\underbrace{\begin{bmatrix}\boldsymbol{0}&\boldsymbol{1}\\\
-\boldsymbol{K}(\boldsymbol{M}^{-1}\\!+\\!\boldsymbol{B}^{-1})&\boldsymbol{0}\end{bmatrix}}_{\displaystyle\boldsymbol{A}_{\text{fast}}}\begin{bmatrix}\boldsymbol{\tau}_{\text{fast}}\\\
\dot{\boldsymbol{\tau}}_{\text{fast}}\end{bmatrix}\\!+\\!\underbrace{\begin{bmatrix}\boldsymbol{0}\\\
\boldsymbol{K}\boldsymbol{B}^{-1}\end{bmatrix}}_{\displaystyle\boldsymbol{E}_{\text{fast}}}\boldsymbol{\tau}_{\text{m,}\text{fast}}\
.$ (19)

The model (19) can be used as a reference to predict the behavior of the fast
torque dynamics, which is directly related to local vibrations in the joints.
Based on this prediction it is possible to determine the motor torque
respecting physical constraints, including the maximum feasible motor torque.

###  III-B MPC-Slow (link dynamics)

Similarly, the link-side dynamics (LABEL:eq:slow_dynamics) can be targeted in
terms of MPC. Then the outer loop is controlled that way while the inner
torque loop is controlled through the classical SP scheme (3). Thus, the MPC
is responsible for the slow component in the commanded motor torque.
Therefore, the slow dynamics is used to compute the link velocity and
acceleration as following

$\begin{bmatrix}\dot{\boldsymbol{q}}\\\
\ddot{\boldsymbol{q}}\end{bmatrix}\\!\\!=\\!\\!\underbrace{\begin{bmatrix}\boldsymbol{0}&\boldsymbol{1}\\\
\boldsymbol{0}&-(\boldsymbol{M}+\boldsymbol{B})^{-1}\boldsymbol{C}\end{bmatrix}}_{\displaystyle\boldsymbol{A}_{\text{slow}}}\begin{bmatrix}\boldsymbol{q}\\\
\dot{\boldsymbol{q}}\end{bmatrix}+\underbrace{\begin{bmatrix}\boldsymbol{0}\\\
(\boldsymbol{M}+\boldsymbol{B})^{-1}\end{bmatrix}}_{\displaystyle\boldsymbol{E}_{\text{slow}}}\boldsymbol{\tau}_{\text{m,}\text{slow}}\
.$ (20)

Figure 2: Feasibility check of prediction and control horizon parameters for
the different MPC structures. The black line indicates the border of the
feasible region to run the controllers in real-time.

That represents the reference model deployed to predict the link-side
dynamical behavior when the gravity effects is compensated.

###  III-C MPC-Full (motor dynamics & link dynamics)

The full model of the flexible-joint robot can be used to obtain the total
commanded MPC motor torque directly. In this case the state vector contains
both the motor- and the link-side coordinates, rendering (1)–(2) to

$\begin{bmatrix}\dot{\boldsymbol{q}}\\\ \ddot{\boldsymbol{q}}\\\
\dot{\boldsymbol{\theta}}\\\
\ddot{\boldsymbol{\theta}}\end{bmatrix}\\!\\!=\\!\\!\underbrace{\begin{bmatrix}\\!\boldsymbol{0}\\!&\\!\boldsymbol{1}\\!&\\!\boldsymbol{0}\\!&\\!\boldsymbol{0}\\!\\\
\\!-\boldsymbol{M}^{-1}\boldsymbol{K}\\!&\\!-\boldsymbol{M}^{-1}\boldsymbol{C}\\!&\\!\boldsymbol{M}^{-1}\boldsymbol{K}\\!&\\!\boldsymbol{0}\\!\\\
\\!\boldsymbol{0}\\!&\\!\boldsymbol{0}\\!&\\!\boldsymbol{0}\\!&\\!\boldsymbol{1}\\!\\\
\\!\boldsymbol{B}^{-1}\boldsymbol{K}\\!&\\!\boldsymbol{0}\\!&\\!-\boldsymbol{B}^{-1}\boldsymbol{K}\\!&\\!\boldsymbol{0}\\!\end{bmatrix}}_{\displaystyle\boldsymbol{A}_{\text{full}}}\\!\\!\begin{bmatrix}\boldsymbol{q}\\\
\dot{\boldsymbol{q}}\\\ \boldsymbol{\theta}\\\
\dot{\boldsymbol{\theta}}\end{bmatrix}\\!\\!+\\!\\!\underbrace{\begin{bmatrix}\\!\boldsymbol{0}\\!\\\
\\!\boldsymbol{0}\\!\\\ \\!\boldsymbol{0}\\!\\\
\\!{\boldsymbol{B}^{-1}}\\!\end{bmatrix}}_{\displaystyle\boldsymbol{E}_{\text{full}}}\\!\boldsymbol{\tau}_{\text{m}}$
(21)

The resultant model (21) uses twice the number of states compared to the
previous ones, namely (19) and (20), which affects the execution time as will
be discussed in Section IV.

###  III-D MPC formulation, objective function

The MPC formulation presented in the following can be applied to all three
model structures, namely (19)–(21). Therefore, the general formulation of the
system dynamics as a function of its states $\boldsymbol{z}$ is given:

$\displaystyle\dot{\boldsymbol{z}}$
$\displaystyle=\boldsymbol{A}_{x}\boldsymbol{z}+\boldsymbol{E}_{x}\boldsymbol{u}$
(22) $\displaystyle\boldsymbol{y}$
$\displaystyle=\boldsymbol{C}_{x}\boldsymbol{z}+\boldsymbol{D}_{x}\boldsymbol{u}$
(23)

Depending on the considered reference model the matrix $\boldsymbol{A}_{x}$
may equal $\boldsymbol{A}_{\text{fast}}$, $\boldsymbol{A}_{\text{slow}}$, or
$\boldsymbol{A}_{\text{full}}$. Analogously, the same applies to the matrix
$\boldsymbol{E}_{x}$ with respect to $\boldsymbol{E}_{\text{fast}}$,
$\boldsymbol{E}_{\text{slow}}$, or $\boldsymbol{E}_{\text{full}}$. Further,
$\boldsymbol{C}_{x}$ is the output matrix, $\boldsymbol{D}_{x}$ is the feed-
forward matrix, and $\boldsymbol{u}$ is the vector of the control input. That
leads to different state vectors based on the selected dynamic expression. The
actuation torque can be determined via the optimization problem

$\begin{split}&\min_{\hat{\boldsymbol{u}}}(\hat{\boldsymbol{y}}-\hat{\boldsymbol{y}}_{\mathrm{ref}})^{T}\boldsymbol{Q}_{y}(\hat{\boldsymbol{y}}-\hat{\boldsymbol{y}}_{\mathrm{ref}})+\hat{\boldsymbol{u}}^{T}\boldsymbol{Q}_{u}\hat{\boldsymbol{u}}\\\
\end{split}$ (24)

with

$\displaystyle\boldsymbol{Q}_{y}$
$\displaystyle=\mathrm{diag}(\boldsymbol{Q}_{y,1},\ldots,\boldsymbol{Q}_{y,{N_{P}}})$
(25) $\displaystyle\boldsymbol{Q}_{u}$
$\displaystyle=\mathrm{diag}(\boldsymbol{Q}_{u,1},\ldots,\boldsymbol{Q}_{u,{N_{C}}})$
(26)

which define the weighting terms for the observed states and the system input,
respectively, with ${N_{P}}$ prediction horizon and ${N_{C}}$ control horizon,
for more details see [33]. Additionally, (24) is subject to (22)–(23) and the
input constraints $\boldsymbol{u}\in\boldsymbol{\mathcal{U}}$, where
$\boldsymbol{\mathcal{U}}$ describes the maximum feasible actuator torques.
The predicted output $\hat{\boldsymbol{y}}\in\mathbb{R}^{N_{P}}$ as a function
of the current states $\boldsymbol{z}_{k}$ and the input vector
$\hat{\boldsymbol{u}}$ is given by

$\hat{\boldsymbol{y}}=\hat{\boldsymbol{C}}(\boldsymbol{A}_{x},\boldsymbol{C}_{x})\boldsymbol{z}_{k}+\hat{\boldsymbol{D}}(\boldsymbol{A}_{x},\boldsymbol{E}_{x},\boldsymbol{C}_{x},\boldsymbol{D}_{x})\hat{\boldsymbol{u}}\
.$ (27)

Further, $\hat{\boldsymbol{C}}$ and $\hat{\boldsymbol{D}}$ are the augmented
discrete output and input matrices. From an implementation point of view, the
objective function is formulated as a quadratic programming problem to find
$\hat{\boldsymbol{u}}$. Depending on the selected reference model of the MPC
structures the optimization searches the optimal commanded motor torque that
complies with (24) and the imposed constraints.

##  IV Simulations and Experiments

The proposed MPC variations are implemented in a cascaded fashion. However,
both the slow and the fast control loops are running at the same sampling rate
in the actual robot implementation. The experimental validation is conducted
using a flexible robot joint constructed by an elastic element from the DLR
C-Runner [34] with a DLR LWR III drive unit [1] with a feasible torque range
of $\pm$100 Nm, see Fig. 3. The controller gains are reported in Table I,
which are represented as the outer-loop control frequency $\omega_{n}$ and
damping ratio $\zeta$. Furthermore, the shaping ratio of the apparent motor
inertia is
$\boldsymbol{\gamma}_{\mathrm{rf}}=\boldsymbol{B}\boldsymbol{B}_{\mathrm{d}}^{-1}$
and the damping ratio of the fast torque dynamics is $\zeta_{\mathrm{f}}$. The
parameters of the considered joint are ${B=0.5980\,\mathrm{kgm}^{2}}$(motor
inertia), ${K~{}=~{}362}$ Nm/rad (intrinsic joint stiffness), and
${M~{}=~{}1\,\mathrm{kgm}^{2}}$ for the link inertia.

Figure 3: Experimental setup of the flexible joint robot. The system is
enhanced with a pendulum-like load to be able to exert external forces. Figure
4: The validity of SP as a function of the joint stiffness and the shaping
factor of the motor inertia $\boldsymbol{B}\boldsymbol{B}_{\mathrm{d}}^{-1}$.
Multiple regions are characterized based on the outer-loop control frequency
$\omega_{n}$ . Figure 5: Experimental evaluation of different control
techniques on a flexible joint. The performance of the classical motor-PD and
singular-perturbation controller are compared with three MPC structures. Link-
side and motor-side position and velocity are visualized for each case as well
as the associated commanded motor torque.

The values for the control and prediction horizons, respectively, are
determined experimentally; each of the magenta crosses in Fig. 2 shows a
trial/test. The light green area shows the feasible region for $N_{P}$ and
$N_{C}$, which are larger for MPC-slow and MPC-fast than for MPC-full. This is
expected since MPC-full uses twice the number of states and thus involves a
numerically more complex model within the prediction. The red borderline shows
the maximum allowable region for real-time execution of the controllers.

###  IV-A Validity of the SP method

The SP theory assumes that there is a sufficiently small parameter responsible
for the separation between the fast and the slow dynamics. In the general case
of a flexible-joint robot, the parameter $\epsilon$ directly related to the
joint stiffness $\boldsymbol{K}$ is in control of this separation, see (11).
In Fig. 4, it can be observed that the validity of the SP assumption depends
not only on the joint stiffness but also on the reduction factor of the
apparent motor inertia and the outer loop frequency. Practically, this could
mean that if the system is not controllable with the SP approach for a
specific stiffness, one could increase the reduction factor for the motor
inertia or decrease the outer loop control frequency. The solid lines in Fig.
4 represent the region for which the mean average error of the link position
over five seconds is larger than 0.26 rad.

Figure 6: The experimental result of a position trajectory smoothly from 0 to +0.26 rad is shown in the top. In the bottom the total motor torque is shown as two components, one is slow and responsible to achieve the desired position, the other is fast due to the dynamic deflection during motion phase. Figure 7: The experimental result when using sinusoidal desired position signal with increasing frequency (chirp signal). The desired and actual link-side velocities are visualized on the left, and on the right, the corresponding motor torque is shown. TABLE I: Parameters of the controllers in the experiments |  $\omega_{n}$, $\zeta$ | $\gamma_{\mathrm{rf}}$ | $\zeta_{\mathrm{f}}$ | $\boldsymbol{Q}_{y}$ | $\boldsymbol{Q}_{u}$
---|---|---|---|---|---
Motor-PD | 14 rad/s, 0.7 |  |  |  |
SP | 15 rad/s, 1.0 | 2.0 | 1.0 |  |
MPC-full |  |  |  | [60, 2e-2, 5e-4] | [2e-6]
MPC-slow |  | 2.0 | 1.0 | [5, 1e-2] | [1e-5]
MPC-fast | 15 rad/s, 1.0 |  |  | [1, 5e-3] | [1.3]

###  IV-B Link-side position and impedance control

The performance of the position and impedance control is validated for all MPC
variations together with the standard motor-PD controller and the standard SP
implementation [26]. The parameters have been tuned to achieve comparable
rising times for all controllers to provide conditions for fair comparisons.
All controllers (except the motor-PD) are applied to the link dynamics and
actively use $(\boldsymbol{q},\dot{\boldsymbol{q}})$ as feedback signals.
Figure 5 shows the experimental results for the step responses, the associated
velocities, and generated motor torques. Notably, overshoot can be observed in
the case of the classical motor-PD controller as it ignores the flexibility in
the joint. The SP controller (3) shows continuous oscillations as well as
commanded torque saturation. Figure 5 shows that the MPC versions minimize the
generated motor torque as a result of the respective objective functions.
Interestingly, the proposed MPC-fast structure shows smooth and faster
convergence in the link-side position. With MPC-slow and SP in the inner
control loop, slower convergence can be observed due to the limited
performance of the classical SP. The MPC-full approach is expected to achieve
superior performance as it uses the complete model in the prediction phase,
but due to the limitation of increasing the value of the prediction horizon
$N_{P}$, the performance is limited in the real implementation.

Additionally, a smooth trajectory with a step from 0 to 0.26 rad is applied to
evaluate the tracking performance generated through a septic polynomial to
ensure that it is at least two times differentiable. In Fig. 6 the different
MPC structures are compared together with the standard SP approach and the
motor-PD controller. The MPC-fast variant shows the lowest link position
error. To gain a closer look of the control action, the individual components
of the commanded motor torque are also visualized in Fig. 6 (bottom) for the
MPC-fast case. It can be seen that the total motor torque consists of two
components, one is slow and responsible for position regulation/tracking, the
other is relatively fast and actively damps oscillations introduced by the
joint elasticity. This result reflects the physical interpretation of the
control action, which also incorporates the future prediction of the fast
dynamics. As the trajectory here is less demanding, the standard SP controller
behaves better w. r. t. the step command case. However, undesirable
oscillations can be noticed while following a more dynamical trajectory.

Further, a chirp signal (from 0 to approx. 4Hz) is used as a desired position
to validate the system for more dynamical effects. The Root Mean Square Error
(RMSE) for the position and velocity are depicted in Table II, and in Fig. 7
the velocities and the corresponding motor torques are shown. The motor-PD
controller features an early velocity deviation compared to the link-side
controllers. The SP approach shows a noticeable high velocity amplitude (up to
4.7 rad/s) when the desired signal frequency increases as the commanded motor
torque has already reached the saturation limits. In this case, the experiment
is terminated through the activation of the safety stop and before the end of
the trajectory execution at 20 sec, see Fig. 7. Here, MPC-full and MPC-slow
controllers have a comparatively high RMSE over the whole range of motion due
to phase shift w. r. t. the desired signals. The MPC-fast controller has a
comparable position error to the SP approach but a smaller velocity error. It
also can be seen from Fig. 7 that all MPC variations do not exceed the
actuation torque limits when the frequency of the desired trajectory is
increased.

TABLE II: Root Mean Square Error (RMSE) | Pos. RMSE [rad] | Vel. RMSE [rad/s] | Additional info.
---|---|---|---
Motor-PD | 0.1945 | 2.1071 | –
SP | 0.0893 | 1.4248 | Safety stop
MPC-full | 0.1364 | 1.5461 | –
MPC-slow | 0.1509 | 1.7658 | –
MPC-fast | 0.0803 | 0.7692 | –
Figure 8: The experimental evaluation of the physical interaction behavior.
The system is excited using the same impact force through a pendulum-like
external load.

###  IV-C Physical interaction behavior

The interaction behavior is illustrated by means of an external force applied
through a pendulum-like load, see Fig. 3. In the experiments, the system is
excited with the same amount of energy by shifting the pendulum to a fixed
distance from the link. The injected energy can be seen by the comparable
initial peak velocity for the considered controllers, see Fig. 8 (middle).
Interestingly, the SP approach saturates two times unnecessarily by the effect
of the initial impact. The MPC-fast controller features superior
vibration/oscillation damping as it aims to control the torque dynamics. From
a human-robot interaction perspective, the MPC-fast controller is more
transparent to parameterize in order to achieve the desired interaction
behavior. This is because it can be interfaced with a link-side impedance
controller as an outer control loop such that the desired stiffness can be
specified accurately. Simultaneously, the torque tracking performance is
maintained in the inner control loop through the MPC while respecting the
torque limits.

##  V Conclusions

A method to derive a joint-level controller for flexible joint robots based on
MPC techniques was presented. Motivated by the two-time-scales separation from
the singular perturbation theory, three different MPC design structures were
investigated. The first one uses the fast torque dynamics as a reference model
and aims at regulating the torque control loop within the joint torque limits.
The second one considers the slow link-side dynamics and is used to design an
outer position/impedance control loop. The third one exploits the full model
the flexible-joint robots to generate the commanded torque respecting the
actuator constraints. The proposed methods have been tested experimentally on
a flexible-joint setup. Compared to the state of the art, significant
improvements in the dynamic link-side position errors were achieved. This work
also provides more insights into the validity of the singular-perturbation
assumption for flexible-joint robot dynamics.

##  VI Acknowledgment

The authors would like to thank Martin Görner for the drawing in Fig. 1 and
Florian Loeffl for his support with hardware.

## References

  * [1] A. Albu-Schäffer, S. Haddadin, C. Ott, A. Stemmer, T. Wimböck, and G. Hirzinger, “The dlr lightweight robot: design and control concepts for robots in human environments,” _Industrial Robot: an international journal_ , 2007.
  * [2] M. Iskandar, O. Eiberger, A. Albu-Schäffer, A. De Luca, and A. Dietrich, “Collision detection, identification, and localization on the dlr sara robot with sensing redundancy,” in _2021 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, 2021, pp. 3111–3117.
  * [3] A. De Luca and W. J. Book, “Robots with flexible elements,” in _Springer Handbook of Robotics_. Springer, 2016, pp. 243–282.
  * [4] C. Ott, A. Albu-Schäffer, and G. Hirzinger, “Comparison of adaptive and nonadaptive tracking control laws for a flexible joint manipulator,” in _IEEE/RSJ international conference on intelligent robots and systems_ , vol. 2. IEEE, 2002, pp. 2018–2024.
  * [5] A. de Luca and P. Tomei, “Elastic joints,” in _Theory of Robot Control_ , 1st ed., C. de Wit and B. G. Siciliano, Bruno, Eds. Springer-Verlag London, 1996, ch. 5, pp. 181–219.
  * [6] J. Kim, “Two-time scale control of flexible joint robots with an improved slow model,” _IEEE Transactions on Industrial Electronics_ , vol. 65, no. 4, pp. 3317–3325, 2018.
  * [7] M. Iskandar, C. Ott, O. Eiberger, M. Keppler, A. Albu-Schäffer, and A. Dietrich, “Joint-level control of the dlr lightweight robot sara,” in _Proc. of the 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2020, pp. 8903–8910.
  * [8] H. K. Khalil, _Nonlinear Systems (Third Edition)_. Prentice Hall, 2002.
  * [9] A. Dietrich, X. Wu, K. Bussmann, M. Harder, M. Iskandar, J. Englsberger, C. Ott, and A. Albu-Schäffer, “Practical consequences of inertia shaping for interaction and tracking in robot control,” _Control Engineering Practice_ , vol. 114, p. 104875, 2021.
  * [10] J. Kong, M. Pfeiffer, G. Schildbach, and F. Borrelli, “Kinematic and dynamic vehicle models for autonomous driving control design,” in _2015 IEEE intelligent vehicles symposium (IV)_. IEEE, 2015, pp. 1094–1099.
  * [11] R. Dyro, J. Harrison, A. Sharma, and M. Pavone, “Particle mpc for uncertain and learning-based control,” _2021 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)_ , pp. 7127–7134, 2021.
  * [12] E. Gonzalez, J. Sanchis, S. Garcia-Nieto, and J. Salcedo, “A comparative study of stochastic model predictive controllers,” _Electronics_ , vol. 9, no. 12, 2020.
  * [13] T. Raff, C. Ebenbauer, and P. Allgöwer, _Nonlinear Model Predictive Control: A Passivity-Based Approach_. Berlin, Heidelberg: Springer Berlin Heidelberg, 2007, pp. 151–162.
  * [14] K. P. Wabersich and M. N. Zeilinger, “A predictive safety filter for learning-based control of constrained nonlinear dynamical systems,” _arXiv:1812.05506v3 [cs.SY]_ , Feb. 2021.
  * [15] S. V. Raković, _Robust Model Predictive Control_. London: Springer London, 2019, pp. 1–11.
  * [16] L. Hewing, K. P. Wabersich, M. Menner, and M. N. Zeilinger, “Learning-based model predictive control: Toward safe learning in control,” _Annual Review of Control, Robotics, and Autonomous Systems_ , vol. 3, no. 1, pp. 269–296, May 2020.
  * [17] N. Z. Wei, H. X. Sun, Q. X. Jia, and P. Ye, “Position tracking control of flexible robot joints using model predictive control,” in _Machine Design and Manufacturing Engineering III_ , ser. Applied Mechanics and Materials, vol. 607. Trans Tech Publications Ltd, 10 2014, pp. 799–802.
  * [18] S. P. M. Hasan, M. Haris and S. Qin, “Adaptive model predictive control for a class of nonlinear single-link flexible joint manipulator,” _2019 Second International Conference on Latest trends in Electrical Engineering and Computing Technologies (INTELLECT)_ , pp. 1–6, 2019.
  * [19] P. Sanila and J. Jeevamma, “Composite fast-slow MPC design for flexible joint manipulator,” _Proceedings of the International Conference on Advanced trends in Engineering and Technology (FORSCHUNG)_ , pp. 24–29, Mar. 2014.
  * [20] J. Fehr, P. Schmid, G. Schneider, and P. Eberhard, “Modeling, simulation, and vision-/mpc-based control of a powercube serial robot,” _Applied Sciences_ , vol. 10, no. 20, 2020.
  * [21] B. Subudhi and A. S. Morris, “Singular perturbation based neuro-hâ^ž control scheme for a manipulator with flexible links and joints,” _Robotica_ , vol. 24, no. 2, pp. 151–161, 2006.
  * [22] H. R. Karimi, M. J. Yazdanpanah, R. V. Patel, and K. Khorasani, “Modeling and control of linear two-time scale systems: Applied to single-link flexible manipulator,” _Journal of Intelligent and Robotic Systems_ , vol. 45, no. 3, pp. 235–265, Mar. 2006.
  * [23] P. V. KokotoviÄ‡, H. K. Khalil, and J. O’Reilly, _Singular Perturbation Methods in Control: Analysis and Design_ , 1st ed. Society for Industrial and Applied Mathematics, 1999.
  * [24] B.-S. Chen and C.-L. Lin, “On the stability bounds of singularly perturbed systems,” _IEEE Transactions on Automatic Control_ , vol. 35, no. 11, pp. 1265–1270, 1990.
  * [25] M. W. Spong, “Modeling and Control of Elastic Joint Robots,” _Journal of Dynamic Systems, Measurement, and Control_ , vol. 109, no. 4, pp. 310–319, December 1987.
  * [26] C. Ott, “Cartesian impedance control of redundant and flexible-joint robots,” in _Springer Tracts in Advanced Robotics (STAR)_ , vol. 49. Springer, Berlin, Heidelberg, 2008.
  * [27] A. Albu-Schäffer, C. Ott, and G. Hirzinger, “A Unified Passivity-based Control Framework for Position, Torque and Impedance Control of Flexible Joint Robots,” _International Journal of Robotics Research_ , vol. 27, no. 1, pp. 23–39, January 2007.
  * [28] B. Paden and R. Panja, “Globally asymptotically stable pd+ controller for robot manipulators,” _International Journal of Control_ , vol. 47, no. 6, pp. 1697–1712, 1988.
  * [29] S. Wolf and M. Iskandar, “Extending a dynamic friction model with nonlinear viscous and thermal dependency for a motor and harmonic drive gear,” in _2018 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, May 2018, pp. 783–790.
  * [30] M. Iskandar and S. Wolf, “Dynamic friction model with thermal and load dependency: modeling, compensation, and external force estimation,” in _2019 IEEE International Conference on Robotics and Automation (ICRA)_. IEEE, May 2019, pp. 7367–7373.
  * [31] L. Le Tien, A. Albu-Schäffer, A. De Luca, and G. Hirzinger, “Friction observer and compensation for control of robots with joint torque measurement,” in _2008 IEEE/RSJ International Conference on Intelligent Robots and Systems_. IEEE, 2008, pp. 3789–3795.
  * [32] C. Ott, A. Albu-Schaffer, and G. Hirzinger, “Comparison of adaptive and nonadaptive tracking control laws for a flexible joint manipulator,” in _IEEE/RSJ international conference on intelligent robots and systems_ , vol. 2. IEEE, 2002, pp. 2018–2024.
  * [33] P. Van Overschee and B. De Moor, _Subspace identification for linear systems: Theory Implementation Applications_. Springer Science & Business Media, 2012.
  * [34] F. Loeffl, A. Werner, D. Lakatos, J. Reinecke, S. Wolf, R. Burger, T. Gumpert, F. Schmidt, C. Ott, M. Grebenstein _et al._ , “The dlr c-runner: Concept, design and experiments,” in _2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)_. IEEE, 2016, pp. 758–765.