Dynamics Calibration of a Redundant Flexible Joint Robot based on
Gyroscopes and Encoders
Dennis Sch
¨
uthe
1
, Felix Wenk
2
and Udo Frese
1
1
Multi-Sensor Interactive Systems Group, University of Bremen, Enrique-Schmidt-Str. 5, Bremen, Germany
2
CPS, German Research Center for Artificial Intelligence (DFKI), Enrique-Schmidt-Str. 5, Bremen, Germany
Keywords:
Parameter Identification, Dynamics Identification, Model Fitting, Flexible Joints, Least-square, Entertainment
Robot.
Abstract:
In this paper we show the identification of the dynamic parameters of a redundant flexible joint robot with a
flexible bearing in the first joint. The bearing leads to distortions of the link velocities measured by gyroscopes.
The bearing flexibility is not modeled explicitly, resulting in a very simple model. For the joint positions
encoders are used. We show how to pack the calibration problem into the sparse least-squares on manifolds
toolkit (SLOM) and present the results. For verification we compare the measured data with the predicted data
computed using the identified dynamic parameters.
1 INTRODUCTION
For model predictive control it is essential to have a
good model. One question is the qualitative structure
of the model, i.e. the decision which of the countless
effects happening in reality are modeled and which
of them are ignored. This is usually answered by ex-
pert knowledge, i.e. experience and experiment. The
other question is to determine the quantitative param-
eters governing these modeled effects, which need to
be determined by a calibration procedure.
In this paper we are concerned with a specialized
ball playing 3-DOF entertainment robot, which has
on purpose a very low-cost design (Figure 1) being
built in large parts from laser cut acrylic. This design
comes with some mechanical problems, most notably
a high friction and high elasticity almost everywhere
in the structure and actuator system.
We want to illustrate in this paper the calibration
of an elastic joint robot model to this machine using
motor encoders and gyroscopes. The encoders are
placed on every motor, i.e. every joint. But, not ev-
ery joint has a gyroscope. Thus, the data has to be
extracted from the sensors for each joint.
We also show that the obtained model is reason-
ably precise even though the robot’s elasticity is not
only located in the joints, as assumed by the model,
but also in the whole structure. Most notably the ef-
fect is recognized in the first bearing connecting the
robot’s body to the base.
1.1 System
The system under calibration and parameter finding
is a 2.1 m tall ball playing entertainment robot named
“Doggy” (Figure 1). Its task is to hit balls with its
head. The head is made of a 40 cm Styrofoam sphere
and holds one of our two IMU sensors. Moreover,
the heads orientation does not matter when playing a
ball – as it is a sphere, so only the position and veloc-
ity matter. The head is connected to the robot’s three
revolute jointed system via a carbon rod. The first
axis acts like a hip and gives the robot a redundant de-
gree of freedom (DOF). The redundancy is a result of
the common intersection point of all axes (Figure 1).
Therefore, the end-effector (EOF) moves on a par-
tial sphere (due to joint limits) and the two DOF are
controlled by three redundant joints. A stereo cam-
era system for ball tracking is placed after the first
joint to turn with the robot. Additionally, our circuit
board with a microcontroller, motor drivers and an-
other IMU sensor is placed on the first body.
Each joint is driven by a DC motor using a tooth
belt as coupling. As the tooth belt is not totally stiff,
we get elasticity between the motor and the link. Each
motor has an encoder for the positioning, read by the
microcontroller.
The material used to build our robot is mostly
acrylic glass (roughly 80%) and the base plate is made
of steel. The rest is made of aluminum (Figure 1).
Schüthe, D., Wenk, F. and Frese, U.
Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders.
DOI: 10.5220/0005976603350346
In Proceedings of the 13th International Conference on Informatics in Control, Automation and Robotics (ICINCO 2016) - Volume 1, pages 335-346
ISBN: 978-989-758-198-4
Copyright
c
2016 by SCITEPRESS Science and Technology Publications, Lda. All rights reserved
335
base
joint 1
yaw
body 1
joint 2
pitch
body 2
joint 3
roll
body 3
IMU1
IMU2
1
2
3
EOF
Figure 1: “Doggy” the ball playing entertainment robot. From left to right: The costume of the robot,
its interior, the coordinate systems with color encoding (the world coordinate frame is on the ground in the
same orientation as 1 in its initial position), and the kinematic chain. The video (http://www.informatik.uni-
bremen.de/agebv2/downloads/videos/schuetheIcinco16
calibrationMotion.mp4) shows the movement of the calibration.
1.2 Problem Statement
Our goal is to calibrate the parameters of the robot’s
dynamics to use them in a following work for model
predictive control (MPC) which we already developed
in simulation (Sch
¨
uthe and Frese, 2015). For such an
MPC we need the state x
x
x of our robotic system and the
dynamic function with known parameters. The states
are not measurable directly. Thus, we will have to
build an observer, e.g. a Kalman-Filter, to estimate the
states using the dynamic model. Moreover, the aim is
to control the robot using an optimal controller which
needs the model of the controlled system as well. For
both we need the dynamics including its parameters.
Some of the dynamic parameters are known a pri-
ori, such as the inertia of the robot, which is pretty
well known from the CAD model. Other parame-
ters, like the motor inertia b, the motors mutual induc-
tion constant k
mi
and the voltage to torque constant
k
pwm
are known only approximately, as there is only
few information on the used low-cost motor. How-
ever, some parameters are not known and can only be
guessed. These are the motor friction τ
τ
τ
fm
, the spring
constant k
k
k
s
, and spring damping constant d
d
d
s
.
A major challenge in the calibration is the first
bearing, between base and first body. It is a ball
bearing with some kind of flexibility, especially when
moving joint 2 and 3 (see link of Figure 1). So
the IMUs will measure the movements of the motors
added with we could say “noise”, which is the
result of the flexible bearing.
To solve the calibration problem we use the mo-
tor encoders’ and the gyroscopes’ data, with a 1 kHz
sampling frequency. We explicitly relinquish of the
accelerometer data to show that the robots parame-
ters can be calibrated without accelerometers. More-
over, our upcoming estimator should work on gyro-
scope data only and we want to see if it works also for
the calibration.
1.3 Contribution
We show that the robots’ parameters can be obtained
by using encoders and only gyroscopes for measure-
ments. We prove that it is also possible to do this if
there is a large error on the gyroscopes’ data. To our
knowledge the estimation of the link position and ve-
locity has never been done by using gyroscope data
only with additional information of encoders. More-
over, we explain how to do such a calibration by using
the sparse least-squares on manifold (SLOM) toolkit
and that it is very straightforward to implement.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
336
2 RELATED WORK
The options for identifying dynamic parameters are
summarized in the overview paper by (Wu et al.,
2010), where the authors distinguish on-line and off-
line identification. The former deals with adaptive
control algorithms and neural networks. In adaptive
control the parameters are adjusted and estimated on-
line for using them in the controller. Neural net-
works are effectively identifying nonlinear systems.
The parameters are used as weights for the network
and are approached by training the weights in real-
time (Narendra and Parthasarathy, 1990). The lat-
ter, off-line identification, has three methods to name:
(1.) Physical experiments, such as isolating each link
and measure its mass, the inertia and center of grav-
ity (COG). (2.) Computer aided design (CAD) tech-
niques can be used for inertia and COG measuring.
But other parts, such as the identification of friction
or the motor parameters, are not possible. (3.) The
third possibility is the identification by analyzing the
input/output behavior on a well known motion and
minimizing the difference of the real data and the es-
timated data of the model. In our identification we
make use of (2.) and (3.) in combination.
In (Grotjahn et al., 2001) and (Vuong and Ang Jr,
2009) the identification of dynamic parameters of in-
dustrial robots is presented. In (Grotjahn et al., 2001)
the identification does not require the friction model
a priori. Vuong et al. have to deal with highly nonlin-
ear friction and build a model which overcomes this
problem. They also demonstrate the identification us-
ing an off-line method with a least-squares algorithm,
which is enhanced by constraints to adjust the least-
squares result (Mata et al., 2005). Both have a model
for a rigid body robot, so there is no flexibility.
The identification with flexible joints is discussed
and demonstrated in (Moberg, 2010), where also the
friction is estimated. Additionally, the model is en-
hanced by flexible links. To identify the parameters a
least-squares off-line method is used.
How accelerometers and gyroscopes can be uti-
lized to obtain the state of flexible robots and use them
for control was shown in (Staufer and Gattringer,
2012; Cheng and Oelmann, 2010). However, there
is a combination of both IMU sensors to estimate the
state. In our case we only want to use the gyroscope.
In the identification process the least-squares
solver takes a big role. Also for our identification pro-
cess we make use of a least-squares solver packed into
a framework called SLOM (Hertzberg, 2008), which
can be used for mapping problems, but also for cali-
bration and sensor fusion (Hertzberg et al., 2012).
3 MODEL
To identify the parameters for the dynamics we need
an appropriate dynamic model. It is also necessary
to know the kinematics for transformations from one
coordinate system to the other. This is needed for the
measurement functions, which will be explained later.
3.1 Kinematics
With the Kinematics we describe the relations of co-
ordinate systems. Each joint and each IMU has one
coordinate system. Additionally, there are the world
coordinate system and the end-effector (EOF) coordi-
nate system. We can transform from one system to
another using translations and rotations. In our case
we only need to transform velocities to create a re-
lation between angular motor velocities
˙
θ
θ
θ and gyro-
scope measurements ω
ω
ω. Only rotations of the form
to
f rom
R
R
R are needed. A rotation of the z-axes moves the
correlated body together with its coordinate system
(Figure 1). The rotations
IMU1
1
R
R
R and
IMU2
EOF
R
R
R – should
also be identified by our calibration, as they could be
misaligned by a few degrees.
3.2 Dynamics - Elastic Joint Model
As mentioned before, we need to take into account
the elasticity between motor and link due to the tooth
belt coupling. This dynamic model is well known
and described in (Siciliano and Khatib, 2008; Albu-
Schaeffer et al., 2007) as
0
0
0 = M
M
M(q
q
q)
¨
q
q
q + c
c
c(q
q
q,
˙
q
q
q) + τ
τ
τ
g
(q
q
q)+
K
K
K
s
(q
q
q θ
θ
θ) + D
D
D
s
˙
q
q
q
˙
θ
θ
θ
+ τ
τ
τ
(1)
τ
τ
τ
m
= B
B
B
¨
θ
θ
θ + K
K
K
s
(θ
θ
θ q
q
q) + D
D
D
s
˙
θ
θ
θ
˙
q
q
q
+ τ
τ
τ
fm
, (2)
where motor angle, velocity and acceleration are θ
θ
θ,
˙
θ
θ
θ, and
¨
θ
θ
θ respectively. M
M
M(q
q
q) is the link inertia,
the Coriolis and centrifugal terms are represented by
c
c
c(q
q
q,
˙
q
q
q), τ
τ
τ
g
(q
q
q) holds the gravitational terms. The cou-
pling between motor and link is approximated by
a spring with stiffness K
K
K
s
= diag(k
k
k
s
) and damping
D
D
D
s
= diag(d
d
d
s
). The friction on link and motor side
is denoted as τ
τ
τ
and τ
τ
τ
fm
. The motor torque τ
τ
τ
m
and
the motor inertia B
B
B = bI
I
I (I
I
I is the unit matrix) have
to be transformed to the link side by the gear ratio
n
G
= 42.092.
B
B
B = n
2
G
B
motor
(3)
τ
τ
τ
m
= n
G
τ
motor
(4)
A common way to simplify (2) is to neglect the
Coriolis and centrifugal terms, which are hardly no-
ticeable in our system. We can also neglect the gravi-
tational terms τ
τ
τ
g
(q
q
q) as we inserted springs that coun-
teract the gravitational force on the pitch and yaw
Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders
337
axes, i.e. axes 2 and 3. Thus, we can position the
robot anywhere in the working space and it holds its
position without any torque input. Additionally, we
neglect the link friction τ
τ
τ
like in (Moberg, 2010). In
some test cases we have seen that the link friction is
close to zero. We assume that the motor parameters
k
pwm
, k
mi
, and b are identical, because the motors are
of the same type. For the spring parameters each ele-
ment i of the vectors k
k
k
s
and d
d
d
s
represents the specific
stiffness and damping of the i-th joint. The simplified
dynamics
0
0
0 = M
M
M(q
q
q)
¨
q
q
q + τ
τ
τ
c
(5)
τ
τ
τ
c
= diag(k
k
k
s
)(q
q
q θ
θ
θ) + diag(d
d
d
s
)
˙
q
q
q
˙
θ
θ
θ
(6)
0
0
0 = b
¨
θ
θ
θ τ
τ
τ
c
+ τ
τ
τ
fm
τ
τ
τ
m
(u
u
u,
˙
θ
θ
θ) (7)
can be rearranged to formulate a dynamical state
space representation, where the highest derivative is
placed on the left hand:
˙
x
x
x = f
dyn
(x
x
x, u
u
u), with the state
x
x
x =
q
q
q
T
˙
q
q
q
T
θ
θ
θ
T
˙
θ
θ
θ
T
T
. (8)
The input vector defines a pwm (pulse width modu-
lation) signal of the voltage passed to the DC motor.
τ
τ
τ
m
is the torque to the system as result of the voltage.
The transfer function f
dyn
(x
x
x, u
u
u) is
f
dyn
(x
x
x, u
u
u) =
˙
q
q
q
M
M
M(q
q
q)
1
τ
τ
τ
c
˙
θ
θ
θ
(bI
I
I)
1
τ
τ
τ
c
+ τ
τ
τ
m
(u
u
u,
˙
θ
θ
θ) τ
τ
τ
fm
. (9)
Link Inertia. The link moment of inertia M
M
M(q
q
q) is
the computation result of Matlab’s Spatial V2 library
(Featherstone, 2012). We fed it with CAD model in-
formation of each joint, i.e. its COG inertia, its mass,
and its displacement between axis coordinate system
and COG.
M
M
M(q
q
q) =
m
1,1
m
1,2
m
1,3
m
1,2
m
2,2
m
2,3
m
1,3
m
2,3
m
3,3
(10)
with the matrix entries
m
1,1
= 1.824c
2
s
2
2.975c
2
2
503.760c
2
2
c
2
3
+
0.106c
2
2
c
3
s
3
+ 0.183c
2
c
3
s
2
0.138c
2
s
2
s
3
+
1012.973
m
1,2
= 0.8192s
2
8.5760c
2
+ 0.0691c
3
s
2
+
0.0916s
2
s
3
0.1062c
2
c
2
3
503.7604c
2
c
3
s
3
m
1,3
= 514.2253s
2
+ 0.0916c
2
c
3
0.0691c
2
s
3
m
2,2
= 503.7604c
2
3
0.0531sin(2 q
3
) + 40.0367
m
2,3
= 0.0691c
3
+ 0.0916s
3
m
3,3
= 514.2253 .
−0.02 −0.015 −0.01 −0.005 0 0.005 0.01 0.015 0.02
−1
−0.8
−0.6
−0.4
−0.2
0
0.2
0.4
0.6
0.8
1
τ
fm
/Nm
˙q/
rad
s
Figure 2: Coulomb friction as Signum function (dashed
line) and approximated Sigmoid function (solid line).
The inertia is given in gm
2
. The inertia depends on the
i-th link position of the time step k with c
i
= cos(q
k,i
)
and s
i
= sin(q
k,i
).
Motor Torque τ
τ
τ
m
. The torque is given by the pwm
Signal u
u
u, the maximum voltage of the power supply
U
max
= 32 V, and a gain k
pwm
which translates the
voltage to a torque. This torque is countered by a mu-
tual induction (value of k
mi
) and depends on the motor
velocity
˙
θ
θ
θ, which is the second part of the equation.
τ
τ
τ
m
(u
u
u,
˙
θ
θ
θ) = k
pwm
U
max
I
I
Iu
u
u k
mi
I
I
I
˙
θ
θ
θ (11)
Motor Friction τ
τ
τ
fm
. The motor friction consists of
different parts (Olsson et al., 1998). First, the static
friction which holds the joint at a position until a force
higher than this friction is produced. As our goal is to
control the robot mostly in motion, we neglect this
friction. Secondly, there is viscous friction, which in-
creases proportionally to the velocity. We can also
neglect that as we use a tooth belt. Finally, there is
the kinetic or Coulomb friction acting at a constant
rate over all velocities. This is typically described
as τ
τ
τ
fm
= diag(µ
µ
µ
fm
)sgn(
˙
q
q
q), where τ
τ
τ
fm
is the friction
torque and µ
µ
µ
fm
the kinetic friction coefficient. The
Signum function (sgn) translates the velocity
˙
q
q
q to a
constant value over all velocities depending on its
sign. The disadvantage of this model is the discon-
tinuity of the sgn-function – this is unwanted in simu-
lations, e.g. when using an ODE-Solver. We approxi-
mated the Signum function by a Sigmoid function
τ
τ
τ
fm
= 2diag(µ
µ
µ
fm
)
1
1+exp(400 ˙q
1
)
0.5
1
1+exp(400 ˙q
2
)
0.5
1
1+exp(400 ˙q
3
)
0.5
. (12)
This differs from the sgn function only close to zero
velocity (Figure 2). Our calibration goal is to get µ
µ
µ
fm
.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
338
Not Modeled. In our model we leave out the vibra-
tion and movement of the first axis around its x- and
y-axes, which is normally the case in the joint’s turn-
ing definition. But in our case the bearing of Axis 1
allows little movements around those axes due to its
flexibility. Leading to a deviation between model and
real world. We want to see, if it is possible to find
parameters that still fit the model to the real behavior.
Therefore, we expect that movements around x
1
- and
y
1
-axes will be put into the springs and dampings of
axes 2 and 3.
The motivation for this simplification is: First, that
in general the simplest model that works is the best
model. Second, that incorporating the elasticity in
the Axis 1 bearing directly would require four addi-
tional states, namely position and velocity for 2-DOF
of elastic movement.
4 MEASUREMENT FUNCTIONS
To determine the parameters governing the behavior
of the robot, we search for the parameters which best
explain sensor measurements taken while exercising
the degrees of freedom of the robot during a cali-
bration motion. I.e. we search for the parameters ϑ
ϑ
ϑ
which result in the least squared difference of the ac-
tual measurements and the measurements predicted
from the parameters. Formally, we search the least-
squares estimate
ˆ
ϑ
ϑ
ϑ
ˆ
ϑ
ϑ
ϑ = argmin
ϑ
ϑ
ϑ
1
2
kF(Z
Z
Z, ϑ
ϑ
ϑ)k
2
Σ
, (13)
where Z
Z
Z is the vector of all stacked measurements and
Σ the covariance of the measurement error calculated
by F. F computes the difference between actual and
predicted (using the parameters) measurements.
4.1 Calibration Parameters
ϑ
ϑ
ϑ consists of the time-invariant parameters ϑ
ϑ
ϑ
calib
, in
which we are interested primarily, and the parameters
ϑ
ϑ
ϑ
state
describing the state of the robot during the cal-
ibration motion at each sensor sampling time t
k
. For
N samples we have
ϑ
ϑ
ϑ =
ϑ
ϑ
ϑ
calib
ϑ
ϑ
ϑ
state
with ϑ
ϑ
ϑ
state
=
ϑ
ϑ
ϑ
state1
.
.
.
ϑ
ϑ
ϑ
stateN
. (14)
ϑ
ϑ
ϑ
state
k
is the state vector as defined in (8) at time t
k
and the concrete parameters to be calibrated are
ϑ
ϑ
ϑ
calib
= (
k
pwm
k
mi
b µ
µ
µ
T
fm
k
k
k
T
s
d
d
d
T
s
ω
ω
ω
T
1,0
ω
ω
ω
T
2,0
IMU1
1
R
R
R
T
IMU2
EOF
R
R
R
T
)
T
(15)
In addition to model quantities, ϑ
ϑ
ϑ
calib
contains param-
eters pertaining to the sensors used. These are the ro-
tation of the end-effector frame relative to the IMU
mounted on the end-effector (
IMU2
EOF
R
R
R), the rotation of
the coordinate frame of the first axis relative to the
IMU mounted after that axis (
IMU1
1
R
R
R, see Figure 1),
and biases of the gyroscopes (ω
ω
ω
1,0
and ω
ω
ω
2,0
). While
not really time-invariant, the gyroscope biases vary so
slowly that we consider them to be constant over the
duration of the calibration motion.
The two rotation matrices of ϑ
ϑ
ϑ
calib
have only three
DOF each (
IMU1
1
R
R
R,
IMU2
EOF
R
R
R SO(3)), but do not have a
3-dimensional, singularity-free parametrization. The
solver we use, SLOM, can perform least squares op-
timization on elements of manifolds meeting certain
criteria, which are called -manifolds and include
SO(3), by parameterizing changes to the manifold el-
ements vectorially. The -theory is treated very thor-
oughly by (Hertzberg et al., 2013). For the purpose of
this particular calibration problem, we have two oper-
ators to apply a vectorially represented change δ
δ
δ to a
rotation matrix R
R
R
1
and to obtain δ
δ
δ from two rotation
matrices R
R
R
1
, R
R
R
2
, namely
: SO(3) × R
3
SO(3)
with R
R
R
2
= R
R
R
1
δ
δ
δ = R
R
RRot(δ
δ
δ) (16)
: SO(3) × SO(3) R
3
with δ
δ
δ = R
R
R
2
R
R
R
1
= aRot(R
R
R
1
1
R
R
R
2
). (17)
Rot(v
v
v) = expv
v
v
×
with v
v
v
×
u
u
u = v
v
v × u
u
u for any u
u
u R
3
is
the rotation matrix whose scaled axis representation
is v
v
v and aRot is its inverse.
These operators are best thought of as +’ and
operations for rotations, which encapsulate the struc-
ture of the rotation representation. They are easily ex-
tended to the parameter vector by using ordinary +
and ’ for the vectorial components and ‘ and ‘
for the rotation components.
4.2 Measurements
Sensor Measurements. The components related to
measurements of (13) are filled in here. There are
two types of measurements. Sensor measurements
and pseudo-measurements derived from properties
we know about the robot.
The sensors are sampled at N time points t
k
, 1
k N over the duration of the calibration motion.
Each sensor is sampled at each time t
k
(i.e. when a
measurement is available the time of the microcon-
troller is used as time stamp), so for time t
k
, there is
a stack of measurements Z
Z
Z
k
, which contains a sample
of each sensor,
Z
Z
Z
k
=
θ
θ
θ
T
meas,k
IMU1
ω
ω
ω
T
1,k
IMU2
ω
ω
ω
T
2,k
T
. (18)
Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders
339
In (18), θ
θ
θ
meas,k
R
3
are rotary encoder measurements
of each axis and
IMU1
ω
ω
ω
1,k
,
IMU2
ω
ω
ω
2,k
the gyroscope
measurements in their sensor coordinate frame.
All these measurements build the measurement
vector
Z
Z
Z =
Z
Z
Z
k
N
k=1
. (19)
Error Function. The error function F of (13) con-
sists of following components. For each time t
k
there
is a component pertaining to the motor
F
motor,k
= θ
θ
θ
meas,k
θ
θ
θ
k
θ
θ
θ
0
, (20)
where θ
θ
θ
0
is the known zero-position of the motor and
θ
θ
θ
k
the respective component of the parameters ϑ
ϑ
ϑ as
defined in (14) and (8), respectively.
Similarly, there are error components considering
the gyroscopes (F
gyro1,k
, F
gyro2,k
), which complete
F
k
=
F
T
motor,k
F
gyro1,k
F
T
gyro2,k
T
. (21)
The error considering only the gyroscope at the first
axis is
F
gyro1,k
= ˙q
k,1
world
ω
1,k,2
, (22)
with ˙q
k,i
the i-th link angle velocity,
world
ω
1,k,2
the y-
axis of gyroscope one, and
world
ω
ω
ω
1,k
=
1
world
R
R
R
1 IMU1
1
R
R
R
1
(
IMU1
ω
ω
ω
1,k
ω
ω
ω
1,0
),
(23)
with the rotation from world to first frame
1
world
R
R
R =
cos(q
k,1
) sin(q
k,1
) 0
sin(q
k,1
) cos(q
k,1
) 0
0 0 1
. (24)
I.e. it is the difference in the angular velocity of the
axis as stored in ϑ
ϑ
ϑ and the vertical component of
measured angular velocity corrected by the gyroscope
bias and rotated into the world reference frame.
The error function component for the end-effector
gyroscope is
F
gyro2,k
=
IMU2
ˆ
ω
ω
ω
2
IMU2
ω
ω
ω
2,k
(25)
with
IMU2
ˆ
ω
ω
ω
2
=
IMU2
EOF
R
R
R
EOF
1
R
R
R
IMU1
1
R
R
R
1 IMU1
ω
ω
ω
1,k
+
EOF
ω
ω
ω
˙q
2
+
EOF
ω
ω
ω
˙q
3
+ ω
ω
ω
2,0
, (26)
where
EOF
ω
ω
ω
˙q
2
=
0 sin(q
k,3
) 0
0 cos(q
k,3
) 0
0 0 0
˙
q
q
q
k
, (27)
EOF
ω
ω
ω
˙q
3
=
0 0 0
0 0 0
0 0 1
˙
q
q
q
k
, (28)
EOF
1
R
R
R =
c
2
0 s
2
s
2
s
3
c
3
c
2
s
3
s
2
c
3
s
3
c
2
c
3
. (29)
This looks more complicated than it is. F
gyro2,k
is
the difference between the sensor measurement of the
gyroscope at the end-effector and the expected angu-
lar velocity at the location of the end-effector gyro-
scope, which is the measurement of the gyroscope at
the first axis plus the angular velocity caused by the
rotations of the second and third axes.
In addition to the sensor measurement, we know
that initially, at t
k
= t
1
, all angular velocities of the
links and the motors are zero and that the links have
approximately the same angles as the motors. The
corresponding component of the error function is
F
initial
=
(q
q
q
1
θ
θ
θ
meas,1
)
T
˙
q
q
q
T
1
˙
θ
θ
θ
T
1
. (30)
We also know that the system behaves approx-
imately according to the dynamics function f
dyn
in
(9), which relates the states of two successive time
points t
k
and t
k+1
. This contributes to the error func-
tion N 1 times, with T
s
= t
k+1
t
k
and the pwm u
u
u
k
,
F
dyn,k
= ϑ
ϑ
ϑ
state
,k
+ T
s
f
dyn
(ϑ
ϑ
ϑ
state
,k
, u
u
u
k
)
ϑ
ϑ
ϑ
state
,k+1
. (31)
With these error functions, the bias concerning the
horizontal axes of the gyroscope at the first axis is
unobservable. To fix this, we keep the commands of
the first M N time steps zero, i.e. u
u
u
k
= 0
0
0 for 1 k
M. To let the solver exploit this knowledge, we define
the error function components only valid until t
M
:
F
bias,k
=
(ω
ω
ω
1,0
ω
ω
ω
1,k
)
T
(ω
ω
ω
2,0
ω
ω
ω
2,k
)
T
T
(32)
That is, as long as there is no command to move, the
gyroscopes should measure their bias.
In summary, the error function leads to
F(Z
Z
Z, ϑ
ϑ
ϑ) =
F
initial
[F
k
]
N
k=1
F
dyn,k
N1
k=1
[F
bias,k
]
M
k=1
. (33)
The corresponding covariance is a block-diagonal
matrix. We assumed the same covariance for all errors
of the same kind, so with A
P
denoting P repetitions of
A, the covariance is
Σ = blkdiag(Σ
initial
, Σ
N
meas
, Σ
N1
dyn
, Σ
M
bias
). (34)
5 EVALUATION
Our identification uses an off-line method based on
a trajectory the robot is following. Here we imple-
mented a simple P-controller to move to different po-
sitions. The maximum torque set to the motors by the
controller can also be adjusted. For our identification
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
340
20 30 40 50 60 70 80 90 100 110
−50
0
50
t / s
θ/deg
20 30 40 50 60 70 80 90 100 110
−0.5
0
0.5
t / s
p/%
Figure 3: On top we see the pwm signal u
u
u and on bottom the
measured motor angle θ
θ
θ transformed to the link side. The
color encoding is red, green, and blue for yaw (1), pitch (2),
and roll (3) axis, respectively.
we move the robot in two pwm stages, the first is slow
with large displacement (u
u
u = 0.15). This should help
the identification of the friction coefficient µ
µ
µ
fm
. In the
second stage we move very fast (u
u
u = 0.90). In the fast
stage we implemented a kind of a chirp signal, e.g. be-
tween 53 s and 56 s in Figure 3. The chirp should be
useful to classify the parameters of the spring (d
d
d
s
and
k
k
k
s
). The parameters for the motor, i.e. k
pwm
, k
mi
, and
b, should be detectable at any time the robot moves.
The rotations
IMU1
1
R
R
R and
IMU2
EOF
R
R
R should be close to the
known values from the CAD.
Figure 3 shows the pwm signal together with the
measured angles of the encoders. The movement with
the distortion of the bearing can be seen in the video
mentioned in Figure 1. The effect of the flexible bear-
ing is best seen on the first gyroscope’s x- and z-axis,
due to an indirect effect by movements of joints 2 or
3 (Figure 4).
Joints 2 and 3 indirectly affect the first gyro-
scope’s x- and z-axis, due to the flexibility of the bear-
ing.
5.1 Initial Guess
The data-sheet of encoders and IMU sensors give us
the standard deviations σ
encoder
= 6.9259 × 10
5
rad
and σ
gyro
= 1.745 × 10
3
rad/s, which remains the
same for all measurements. The measurement covari-
ance is given by
Σ
meas
= blkdiag(σ
2
encoder
I
I
I
3×3
, σ
2
gyro
I
I
I
6×6
) . (35)
For the initial covariance Σ
initial
only the compo-
nent regarding the displacement of link and motor
angle changes. This is due to the influence of the
spring. Σ
dyn
is set to a change in the motor veloc-
ity in 1000 steps of 15
/s. The link velocity is set to
65 70 75 80 85
−40
−30
−20
−10
0
10
20
30
40
ω
1
/
deg
s
t / s
Figure 4: Gyroscope 1 measurement influenced by the bear-
ings flexibility. The effect on the x- and z-axis is shown,
which should be 0 in theory by a movement of joints 2 or 3.
Red, green, and blue denotes x, y, and z axis, respectively.
change 20
/s. For motor and link position covariance
is chosen small, as the position error is mostly given
by the velocity error. The dynamic covariance is con-
stant for all time steps. It emphasizes only the main
effects and neglects cross-correlations. I.e. elements
on the diagonal are chose conservatively and are zero
off diagonal.
Σ
dyn
= blkdiag(10
12
I
I
I
3×3
, 1.21 × 10
4
I
I
I
3×3
,
10
12
I
I
I
3×3
, 6.889 × 10
5
I
I
I
3×3
)
(36)
Σ
initial
= blkdiag(0.0076I
I
I
3×3
, σ
2
encoder
I
I
I
6×6
) (37)
For the identification we need to set an initial
guess of the parameters. For the motor we can com-
pute the k
pwm
and k
mi
from basic motor data in our
case this information is limited to the rated current
and speed, the voltage supplied, and the output torque.
We approximate the motor inertia by its weight and
radius. The gyroscopes offset ω
ω
ω
1,0
and ω
ω
ω
2,0
can be
measured beforehand and used as initial guess. All
other parameters were hand tuned within a simulation
of the dynamics to get close to the measurement for
each axis separately. This was only done for the first
slow movements. We took the parameters which fit
quite well as initial guess. The initial guess and the
identified parameters are given in Table 1. We can
see only a slight difference for k
pwm
, b, µ
µ
µ
fm
, bias ω
ω
ω
1,0
and ω
ω
ω
2,0
, and for the rotation matrices. This result
is, except for µ
µ
µ
fm
, expectable, as we can measure or
compute the parameters with a small tolerance. For
µ
µ
µ
fm
we just had a good hand tuning with the simu-
lation. The divergence of parameter k
mi
is not a sur-
prise, as we could only roughly compute this a pri-
ori. For the spring parameters we also expected a low
value for the stiffness, but for Axis 3 we expected a
higher value than for Axis 2. As one can feel that
Axis 3 is more “stiff than Axis 2 in its spring. On the
other hand, we had the idea that the calibration algo-
rithm will pack the bearings flexibility into the spring
stiffness, which we can see here.
Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders
341
Table 1: Initial guess and identified parameters.
Initial guess Identified
k
pwm
/ NmV
1
3.487 4.696
k
mi
/ Nmsrad
1
15.563 31.328
b/ kgm
2
2.681 2.428
µ
µ
µ
fm
/ Nm (2.0 2.0 2.0)
T
(1.599 2.565 4.855)
T
k
k
k
s
/ Nmrad
1
(400 400 600)
T
(187.555 120.070 103.056)
T
d
d
d
s
/ Nmsrad
1
(1.0 1.0 1.0)
T
(5.335 4.143 8.043)
T
ω
ω
ω
1,0
/ deg (2.615 0.014 1.273)
T
(2.615 0.014 1.273)
T
ω
ω
ω
2,0
/ deg (2.723 0.102 0.652)
T
(2.723 0.103 0.652)
T
IMU1
axis1
R
R
R
1 0 0
0 0 1
0 1 0
T
0.999 0.0439 0.0199
0.0189 0.0219 0.999
0.044 0.999 0.020
T
IMU2
EOF
R
R
R
0 0 1
0 1 0
1 0 0
T
0.009 0.027 0.999
0.030 0.999 0.027
0.999 0.030 0.008
T
5.2 Verification
To verify our results we compare the measured data
with predicted data of a model. (1.) We take the
first state SLOM optimized as our initial state x
x
x
0
. (2.)
We call our dynamics recursively to compute the new
state from the old x
x
x
k
= x
x
x
k1
+ T
s
f
dyn
(x
x
x
k1
, u
u
u
k1
) like
in (31), with T
s
= 1 ms. Thus we get all states and
can compare the measured position and velocity with
the predicted position θ
θ
θ and velocity
˙
θ
θ
θ. (3.) The pre-
dicted value for ω
ω
ω
1
based on the state can be extracted
from (23). This means a rotation of the first axis leads
to a change in ω
ω
ω
1
. (4.) With (26) we get the predicted
ω
ω
ω
2
using the predicted ω
ω
ω
1
, q
q
q, and
˙
q
q
q. We compare the
predicted and measured gyroscope values.
The comparison is splitted into ve different re-
gions of the measurement for better visualization. We
start with the slow behavior for all three axes in Fig-
ure 5(a). We see that the predicted data fits well
to the measured data, except for the motor positions.
The reason is an accumulated error on the velocity
will affect the joint’s position. Also small velocity er-
rors are leading to drift. This is a well known behav-
ior. For the velocities there should be no drift visible.
The measurements are approximated by our predic-
tion with little divergence for velocities.
The second region shows the yaw axis movement
(Figure 5(b)). There is a difference between the part
where the joint’s velocity changes fast and the part
where it moves more constantly in one direction. The
motor velocity is slightly better fitted than the link
velocity transferred to the gyroscope motion. As ex-
pected, there is no influence of the flexible bearing.
Movements of the flexible bearing are provoked
when the motor turning the pitch axis changes di-
rection (Figure 6(a)). Here, the flexible bearing gets
stimulated. After the shock, the joint performs very
little force on the bearing, so it can recover from the
impact and return to its initial state, i.e. zero position
and velocity. While recovering it oscillates. Although
the effect is clearly visible in the gyroscopes, the mo-
tion of the turning axis can still be detected. For the
motor velocity it fits again to the measured data, but
our predicted second gyroscope values fit not as well
as for the first axis. As we neglect the flexible bear-
ing in the model, the predicted measurement of the
first gyroscope is close to zero. Adjusting the spring
stiffness is the solver’s only chance to account for the
movements of the flexible bearing. Thus, it finds a
compromise between spring stiffness and flexibility
of the bearing to fit the data as well as possible.
This effect is much better seen on the roll axis
(Figure 6(b)). Note the delay between measurement
and predicted data. This is caused by the spring stiff-
ness. If we put a rod instead of a tooth belt (k
k
k
s
),
the link would follow the motor immediately. The op-
posite would be a very flexible tooth belt, i.e. k
k
k
s
0.
Then the link would follow eventually. The compro-
mise SLOM does is to take motions from the bear-
ing and the joints into account. From the view of
SLOM, there is no difference between these two mo-
tions. Thus, it lowers the stiffness.
In Figure 7 all axes move together. It is hard to
make a statement here, because the drift of the angles
affects the other joints velocity as they are coupled
through the link inertia M
M
M(q
q
q). However, in principle
the motor velocities are predicted well, whereas the
gyroscope values have higher variations to the mea-
sured values, due to the under-determined spring, as
result of the bearings flexibility.
Cross Verification To cross check our identified pa-
rameters we took another motion sample and com-
pare measured and predicted data for a new pwm
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
342
25 30 35 40 45 50
−50
0
50
θ/deg
25 30 35 40 45 50
−50
0
50
˙
θ/deg s
1
25 30 35 40 45 50
−50
0
50
ω
2
/deg s
1
25 30 35 40 45 50
−50
0
50
ω
1
/deg s
1
t / s
(a) Slow motion.
55 55.5 56 56.5 57 57.5 58 58.5
−50
0
50
θ/deg
55 55.5 56 56.5 57 57.5 58 58.5
−100
0
100
˙
θ/deg s
1
55 55.5 56 56.5 57 57.5 58 58.5
−100
0
100
ω
2
/deg s
1
55 55.5 56 56.5 57 57.5 58 58.5
−100
0
100
ω
1
/deg s
1
t / s
(b) Joint 1 moving (yaw).
Figure 5: Comparison of measured (solid) and predicted data (dashed). For each figure we see from top to bottom the motor
position θ
θ
θ, the motor velocity
˙
θ
θ
θ, the second gyroscope
IMU2
ω
ω
ω
2
, and the first
IMU1
ω
ω
ω
1
. For motor angle and velocity the red,
green, and blue denote yaw, pitch, roll axis respectively. For the gyroscopes it denotes x, y, z axis based on its coordinate
system.
signal. Figure 8 shows this new recorded motion.
The predicted motion (dashed line) approximately fits
the measurement as described in the previous sec-
tion. Additionally, there is no chirp in this move-
ment, which simulates the normal behavior we want
to achieve. It shows that the parameters found are a
Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders
343
63 63.5 64 64.5 65 65.5 67 67.5 68 68.5
−50
0
50
θ/deg
63 63.5 64 64.5 65 65.5 67 67.5 68 68.5
−100
0
100
˙
θ/deg s
1
63 63.5 64 64.5 65 65.5 67 67.5 68 68.5
−100
0
100
ω
2
/deg s
1
63 63.5 64 64.5 65 65.5 67 67.5 68 68.5
−40
−20
0
20
40
ω
1
/deg s
1
t / s
(a) Joint 2 moving (pitch).
79 79.5 80 80.5 81 81.5 82 82.5 83
−20
0
20
θ/deg
79 79.5 80 80.5 81 81.5 82 82.5 83
−100
0
100
˙
θ/deg s
1
79 79.5 80 80.5 81 81.5 82 82.5 83
−100
0
100
ω
2
/deg s
1
79 79.5 80 80.5 81 81.5 82 82.5 83
−40
−20
0
20
40
ω
1
/deg s
1
t / s
(b) Joint 3 moving (roll).
Figure 6: Comparison of measured (solid) and predicted data (dashed). For each figure we see from top to bottom the motor
position θ
θ
θ, the motor velocity
˙
θ
θ
θ, the second gyroscope
IMU2
ω
ω
ω
2
, and the first
IMU1
ω
ω
ω
1
. For motor angle and velocity the red,
green, and blue denote yaw, pitch, roll axis respectively. For the gyroscopes it denotes x, y, z axis based on its coordinate
system.
good fit for other motions. Thus, we are able to pre-
dict the velocities quite well, but need to have in mind
the drift of the positions.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
344
94 96 98 100 102 104 106 108 110
−50
0
50
θ/deg
94 96 98 100 102 104 106 108 110
−100
0
100
˙
θ/deg s
1
94 96 98 100 102 104 106 108 110
−100
0
100
ω
2
/deg s
1
94 96 98 100 102 104 106 108 110
−100
0
100
ω
1
/deg s
1
t / s
Figure 7: Comparison of measured (solid) and predicted data (dashed). For motor angle θ
θ
θ and velocity
˙
θ
θ
θ the red, green, and
blue denote yaw, pitch, roll axis respectively. For the gyroscopes ω
ω
ω it denotes x, y, z axis based on its coordinate system.
Figure 8: Comparison of measured (solid) and predicted data (dashed). For motor angle θ
θ
θ and velocity
˙
θ
θ
θ the red, green, and
blue denote yaw, pitch, roll axis respectively. For the gyroscopes ω
ω
ω it denotes x, y, z axis based on its coordinate system.
Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders
345
6 CONCLUSION
We have been shown that it is possible to find parame-
ters for the dynamics even if the model takes no flex-
ibility of the bearing into account, which moves the
whole body. To get more accurate results the solver
packs the bearing’s flexibility into the flexibility be-
tween joints and links. The predicted behavior of the
robot based on the identified model parameters has
deviations to the measured data. These deviations are
larger whenever the velocities of the second and third
bodies change direction, because the bearing’s flexi-
bility gets stimulated. The overall approximation is
fitting to the measurements well. However, while the
model is reasonably good and, at the same time, very
simple, we suspect it will not be good enough for our
controller.
By now, we are working on an extension of the
model presented herein, where the flexibility of the
bearing is part of the dynamic model. We believe that
this can be achieved by putting two new joints before
the first one, acting directly on the same coordinate
system as joint one. The bodies of the two inserted
joints will have no mass and no inertia. Their rotation
is about the x and y axes of joint one. The flexibility
is given by a spring acting between the origin and the
joints position. We hope that this will give us a better
model and separate the behavior of joint two and three
from the behavior of the bearing.
Additionally, we have to calibrate the cameras for
the ball tracking, too. We want to add this calibration
into the calibration we were stated herein. Moreover,
we can use the cameras to examine in some positions
the position of the links 2 and 3.
ACKNOWLEDGEMENT
This work has been supported by the Graduate School
SyDe, funded by the German Excellence Initiative
within the University of Bremen’s institutional strat-
egy.
REFERENCES
Albu-Schaeffer, A., Ott, C., and Hirzinger, G. (2007). A
unified passivity-based control framework for posi-
tion, torque and impedance control of flexible joint
robots. The International Journal of Robotics Re-
search, 26(1):23–39.
Cheng, P. and Oelmann, B. (2010). Joint-angle measure-
ment using accelerometers and gyroscopes a sur-
vey. IEEE Transactions on Instrumentation and Mea-
surement, 59(2):404–414.
Featherstone, R. (2012). Spatial v2 (version 2).
http://royfeatherstone.org/spatial/v2/notice.html.
Grotjahn, M., Daemi, M., and Heimann, B. (2001). Fric-
tion and rigid body identification of robot dynam-
ics. International Journal of Solids and Structures,
38(1013):1889 – 1902.
Hertzberg, C. (2008). A framework for sparse, non-linear
least squares problems on manifolds. Diploma thesis,
Universit
¨
at Bremen.
Hertzberg, C., Wagner, R., and Frese, U. (2012). Tutorial
on quick and easy model fitting using the slom frame-
work. In Stachniss, C., Schill, K., and Uttal, D., ed-
itors, Spatial Cognition VIII, volume 7463 of Lecture
Notes in Computer Science, pages 128–142. Springer-
Verlag Berlin Heidelberg.
Hertzberg, C., Wagner, R., Frese, U., and Schr
¨
oder, L.
(2013). Integrating generic sensor fusion algorithms
with sound state representations through encapsula-
tion of manifolds. Information Fusion, 14(1):57–77.
Mata, V., Benimeli, F., Farhat, N., and Valera, A. (2005).
Dynamic parameter identification in industrial robots
considering physical feasibility. Advanced Robotics,
19(1):101–119.
Moberg, S. (2010). Modeling and Control of Flexible Ma-
nipulators. PhD thesis, Link
¨
oping University, Auto-
matic Control, The Institute of Technology.
Narendra, K. S. and Parthasarathy, K. (1990). Identifica-
tion and control of dynamical systems using neural
networks. IEEE Transactions on Neural Networks,
1(1):4–27.
Olsson, H., strm, K., de Wit, C. C., Gfvert, M., and Lischin-
sky, P. (1998). Friction models and friction compen-
sation. European Journal of Control, 4(3):176 – 195.
Sch
¨
uthe, D. and Frese, U. (2015). Optimal control with state
and command limits for a simulated ball batting task.
In IEEE/RSJ International Conference on Intelligent
Robots and Systems (IROS), 2015, pages 3988–3994.
Siciliano, B. and Khatib, O. (2008). Springer handbook of
robotics. Springer.
Staufer, P. and Gattringer, H. (2012). State estimation on
flexible robots using accelerometers and angular rate
sensors. Mechatronics, 22(8):1043 – 1049.
Vuong, N. D. and Ang Jr, M. H. (2009). Dynamic model
identification for industrial robots. Acta Polytechnica
Hungarica, 6(5):51–68.
Wu, J., Wang, J., and You, Z. (2010). An overview of
dynamic parameter identification of robots. Robotics
and Computer-Integrated Manufacturing, 26(5):414
419.
ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics
346