Dynamics Calibration of a Redundant Flexible Joint Robot based on

Gyroscopes and Encoders

Dennis Sch

uthe

, Felix Wenk

and Udo Frese

Multi-Sensor Interactive Systems Group, University of Bremen, Enrique-Schmidt-Str. 5, Bremen, Germany

CPS, German Research Center for Artiﬁcial Intelligence (DFKI), Enrique-Schmidt-Str. 5, Bremen, Germany

Keywords:

Parameter Identiﬁcation, Dynamics Identiﬁcation, Model Fitting, Flexible Joints, Least-square, Entertainment

Robot.

Abstract:

In this paper we show the identiﬁcation of the dynamic parameters of a redundant ﬂexible joint robot with a

ﬂexible bearing in the ﬁrst joint. The bearing leads to distortions of the link velocities measured by gyroscopes.

The bearing ﬂexibility 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 veriﬁcation we compare the measured data with the predicted data

computed using the identiﬁed 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 ﬁrst bearing connecting the

robot’s body to the base.

1.1 System

The system under calibration and parameter ﬁnding

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 ﬁrst

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 ﬁrst

joint to turn with the robot. Additionally, our circuit

board with a microcontroller, motor drivers and an-

other IMU sensor is placed on the ﬁrst 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

335

base

joint 1

yaw

body 1

joint 2

pitch

body 2

joint 3

roll

body 3

IMU1

IMU2

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

and the voltage to torque constant

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 τ

, the spring

constant k

, and spring damping constant d

A major challenge in the calibration is the ﬁrst

bearing, between base and ﬁrst body. It is a ball

bearing with some kind of ﬂexibility, 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 ﬂexible 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 identiﬁcation. 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 identiﬁcation, 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 identiﬁcation of friction

or the motor parameters, are not possible. (3.) The

third possibility is the identiﬁcation 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 identiﬁcation we

make use of (2.) and (3.) in combination.

In (Grotjahn et al., 2001) and (Vuong and Ang Jr,

2009) the identiﬁcation of dynamic parameters of in-

dustrial robots is presented. In (Grotjahn et al., 2001)

the identiﬁcation 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 identiﬁcation 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 ﬂexibility.

The identiﬁcation with ﬂexible joints is discussed

and demonstrated in (Moberg, 2010), where also the

friction is estimated. Additionally, the model is en-

hanced by ﬂexible 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 ﬂexible 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 identiﬁcation process the least-squares

solver takes a big role. Also for our identiﬁcation 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

f rom

R are needed. A rotation of the z-axes moves the

correlated body together with its coordinate system

(Figure 1). The rotations –

IMU1

R and

IMU2

EOF

R – should

also be identiﬁed 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 = M

M(q

q + c

c(q

q) + τ

q)+

q − θ

θ) + D



q −



+ τ

ﬂ

(1)

= B

θ + K

(θ

θ − q

q) + D



θ −



+ τ

, (2)

where motor angle, velocity and acceleration are θ

θ,

θ, and

θ respectively. M

M(q

q) is the link inertia,

the Coriolis and centrifugal terms are represented by

c(q

q), τ

q) holds the gravitational terms. The cou-

pling between motor and link is approximated by

a spring with stiffness K

= diag(k

) and damping

= diag(d

). The friction on link and motor side

is denoted as τ

ﬂ

and τ

. The motor torque τ

and

the motor inertia B

B = bI

I (I

I is the unit matrix) have

to be transformed to the link side by the gear ratio

= 42.092.

B = n

motor

(3)

= n

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 τ

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

pwm

, k

, and b are identical, because the motors are

of the same type. For the spring parameters each ele-

ment i of the vectors k

and d

represents the speciﬁc

stiffness and damping of the i-th joint. The simpliﬁed

dynamics

0 = M

M(q

q + τ

(5)

= diag(k

)(q

q − θ

θ) + diag(d

)



q −



(6)

0 = b

θ − τ

+ τ

− τ

θ) (7)

can be rearranged to formulate a dynamical state

space representation, where the highest derivative is

placed on the left hand:

x = f

dyn

x, u

u), with the state

x =





. (8)

The input vector deﬁnes a pwm (pulse width modu-

lation) signal of the voltage passed to the DC motor.

is the torque to the system as result of the voltage.

The transfer function f

dyn

x, u

u) is

dyn

x, u

u) =







−M

M(q

−1

(bI

−1



+ τ

θ) − τ









. (9)

Link Inertia. The link moment of inertia M

M(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(q

q) =





1,1

1,2

1,3

1,2

2,2

2,3

1,3

2,3

3,3





(10)

with the matrix entries

1,1

= 1.824c

− 2.975c

− 503.760c

0.106c

+ 0.183c

− 0.138c

1012.973

1,2

= 0.8192s

− 8.5760c

+ 0.0691c

0.0916s

− 0.1062c

− 503.7604c

1,3

= 514.2253s

+ 0.0916c

− 0.0691c

2,2

= 503.7604c

− 0.0531sin(2 ∗ q

) + 40.0367

2,3

= 0.0691c

+ 0.0916s

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.2

0.4

0.6

0.8

/Nm

˙q/

rad

Figure 2: Coulomb friction as Signum function (dashed

line) and approximated Sigmoid function (solid line).

The inertia is given in gm

. The inertia depends on the

i-th link position of the time step k with c

= cos(q

k,i

)

and s

= sin(q

k,i

Motor Torque τ

. The torque is given by the pwm

Signal u

u, the maximum voltage of the power supply

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

) and depends on the motor

velocity

θ, which is the second part of the equation.

θ) = k

pwm

max

u − k

θ (11)

Motor Friction τ

. 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 τ

= diag(µ

)sgn(

q), where τ

is the friction

torque and µ

the kinetic friction coefﬁcient. The

Signum function (sgn) translates the velocity

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

= 2diag(µ

)







1+exp(−400 ˙q

)

− 0.5

1+exp(−400 ˙q

)

− 0.5

1+exp(−400 ˙q

)

− 0.5







. (12)

This differs from the sgn function only close to zero

velocity (Figure 2). Our calibration goal is to get µ

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 ﬁrst axis around its x- and

y-axes, which is normally the case in the joint’s turn-

ing deﬁnition. But in our case the bearing of Axis 1

allows little movements around those axes due to its

ﬂexibility. Leading to a deviation between model and

real world. We want to see, if it is possible to ﬁnd

parameters that still ﬁt the model to the real behavior.

Therefore, we expect that movements around x

- and

-axes will be put into the springs and dampings of

axes 2 and 3.

The motivation for this simpliﬁcation 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

kF(Z

Z, ϑ

ϑ)k

, (13)

where 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

. For

N samples we have

ϑ =



calib

state



with ϑ

state







state1

stateN







. (14)

state

is the state vector as deﬁned in (8) at time t

and the concrete parameters to be calibrated are

calib

= (

pwm

b µ

1,0

2,0

IMU1

IMU2

EOF

)

(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), the rotation of

the coordinate frame of the ﬁrst axis relative to the

IMU mounted after that axis (

IMU1

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

IMU2

EOF

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

and to obtain δ

δ from two rotation

matrices R

, R

, namely

 : SO(3) × R

→ SO(3)

with R

= R

 δ

δ = R

RRot(δ

δ) (16)

 : SO(3) × SO(3) → R

with δ

δ = R

 R

= aRot(R

−1

). (17)

Rot(v

v) = expv

with v

u = v

v × u

u for any u

u ∈ R

the rotation matrix whose scaled axis representation

is 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 ﬁlled 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

, 1 ≤

k ≤ N over the duration of the calibration motion.

Each sensor is sampled at each time t

(i.e. when a

measurement is available the time of the microcon-

troller is used as time stamp), so for time t

, there is

a stack of measurements Z

, which contains a sample

of each sensor,



meas,k

IMU1

1,k

IMU2

2,k



. (18)

Dynamics Calibration of a Redundant Flexible Joint Robot based on Gyroscopes and Encoders

339

In (18), θ

meas,k

∈ R

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 =





k=1

. (19)

Error Function. The error function F of (13) con-

sists of following components. For each time t

there

is a component pertaining to the motor

motor,k

= θ

meas,k

− θ

, (20)

where θ

is the known zero-position of the motor and

the respective component of the parameters ϑ

ϑ as

deﬁned in (14) and (8), respectively.

Similarly, there are error components considering

the gyroscopes (F

gyro1,k

, F

gyro2,k

), which complete



motor,k

gyro1,k

gyro2,k



. (21)

The error considering only the gyroscope at the ﬁrst

axis is

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

world

−1 IMU1

−1

(

IMU1

1,k

− ω

1,0

(23)

with the rotation from world to ﬁrst frame

world

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

gyro2,k

IMU2

−

IMU2

2,k

(25)

with

IMU2

EOF



EOF

IMU1

−1 IMU1

1,k

EOF

˙q

EOF

˙q



+ ω

2,0

, (26)

where

EOF

˙q





0 sin(q

k,3

) 0

0 −cos(q

k,3

) 0

0 0 0





, (27)

EOF

˙q





0 0 0

0 0 −1





, (28)

EOF

R =





0 −s

−s

−c





. (29)

This looks more complicated than it is. F

gyro2,k

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 ﬁrst 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

= t

, 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

initial



− θ

meas,1

)



. (30)

We also know that the system behaves approx-

imately according to the dynamics function f

dyn

(9), which relates the states of two successive time

points t

and t

k+1

. This contributes to the error func-

tion N − 1 times, with T

= t

k+1

−t

and the pwm u

dyn,k

= ϑ

state

+ T

dyn

(ϑ

state

, u

)

− ϑ

state

,k+1

. (31)

With these error functions, the bias concerning the

horizontal axes of the gyroscope at the ﬁrst axis is

unobservable. To ﬁx this, we keep the commands of

the ﬁrst M ≤ N time steps zero, i.e. u

= 0

0 for 1 ≤ k ≤

M. To let the solver exploit this knowledge, we deﬁne

the error function components only valid until t

bias,k



(ω

1,0

− ω

1,k

)

(ω

2,0

− ω

2,k

)



(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, ϑ

ϑ) =







initial

]

k=1



dyn,k



N−1

k=1

bias,k

]

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

denoting P repetitions of

A, the covariance is

Σ = blkdiag(Σ

initial

, Σ

meas

, Σ

N−1

dyn

, Σ

bias

). (34)

5 EVALUATION

Our identiﬁcation 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 identiﬁcation

ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics

340

20 30 40 50 60 70 80 90 100 110

−50

t / s

θ/deg

20 30 40 50 60 70 80 90 100 110

−0.5

0.5

t / s

p/%

Figure 3: On top we see the pwm signal 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 ﬁrst is slow

with large displacement (u

u = 0.15). This should help

the identiﬁcation of the friction coefﬁcient µ

. In the

second stage we move very fast (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

and

). The parameters for the motor, i.e. k

pwm

, k

, and

b, should be detectable at any time the robot moves.

The rotations

IMU1

R and

IMU2

EOF

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 ﬂexible bear-

ing is best seen on the ﬁrst 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 ﬁrst gyro-

scope’s x- and z-axis, due to the ﬂexibility 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(σ

encoder

3×3

, σ

gyro

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 inﬂuence 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

deg

t / s

Figure 4: Gyroscope 1 measurement inﬂuenced by the bear-

ings ﬂexibility. 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

3×3

, 1.21 × 10

−4

3×3

−12

3×3

, 6.889 × 10

−5

3×3

)

(36)

initial

= blkdiag(0.0076I

3×3

, σ

encoder

6×6

) (37)

For the identiﬁcation we need to set an initial

guess of the parameters. For the motor we can com-

pute the k

pwm

and k

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 ﬁrst

slow movements. We took the parameters which ﬁt

quite well as initial guess. The initial guess and the

identiﬁed parameters are given in Table 1. We can

see only a slight difference for k

pwm

, b, µ

, bias ω

1,0

and ω

2,0

, and for the rotation matrices. This result

is, except for µ

, expectable, as we can measure or

compute the parameters with a small tolerance. For

we just had a good hand tuning with the simu-

lation. The divergence of parameter k

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 ﬂexibility 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 identiﬁed parameters.

Initial guess Identiﬁed

pwm

/ NmV

−1

3.487 4.696

/ Nmsrad

−1

15.563 31.328

b/ kgm

2.681 2.428

/ Nm (2.0 2.0 2.0)

(1.599 2.565 4.855)

/ Nmrad

−1

(400 400 600)

(187.555 120.070 103.056)

/ Nmsrad

−1

(1.0 1.0 1.0)

(5.335 4.143 8.043)

1,0

/ deg (2.615 0.014 − 1.273)

(2.615 0.014 −1.273)

2,0

/ deg (2.723 − 0.102 − 0.652)

(2.723 −0.103 − 0.652)

IMU1

axis1



−1 0 0

0 0 −1

0 −1 0





−0.999 −0.0439 −0.0199

0.0189 0.0219 −0.999

0.044 −0.999 −0.020



IMU2

EOF



0 0 1

0 −1 0

1 0 0





−0.009 0.027 0.999

0.030 −0.999 0.027

0.999 0.030 0.008



5.2 Veriﬁcation

To verify our results we compare the measured data

with predicted data of a model. (1.) We take the

ﬁrst state SLOM optimized as our initial state x

. (2.)

We call our dynamics recursively to compute the new

state from the old x

= x

k−1

+ T

dyn

k−1

, u

k−1

) like

in (31), with T

= 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 ω

based on the state can be extracted

from (23). This means a rotation of the ﬁrst axis leads

to a change in ω

. (4.) With (26) we get the predicted

using the predicted ω

, q

q, and

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 ﬁts 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 ﬁtted than the link

velocity transferred to the gyroscope motion. As ex-

pected, there is no inﬂuence of the ﬂexible bearing.

Movements of the ﬂexible bearing are provoked

when the motor turning the pitch axis changes di-

rection (Figure 6(a)). Here, the ﬂexible 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 ﬁts again to the measured data, but

our predicted second gyroscope values ﬁt not as well

as for the ﬁrst axis. As we neglect the ﬂexible bear-

ing in the model, the predicted measurement of the

ﬁrst gyroscope is close to zero. Adjusting the spring

stiffness is the solver’s only chance to account for the

movements of the ﬂexible bearing. Thus, it ﬁnds a

compromise between spring stiffness and ﬂexibility

of the bearing to ﬁt 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

→ ∞),

the link would follow the motor immediately. The op-

posite would be a very ﬂexible tooth belt, i.e. k

→ 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(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 ﬂexibility.

Cross Veriﬁcation To cross check our identiﬁed 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

θ/deg

25 30 35 40 45 50

−50

θ/deg s

−1

25 30 35 40 45 50

−50

/deg s

−1

25 30 35 40 45 50

−50

/deg s

−1

t / s

(a) Slow motion.

55 55.5 56 56.5 57 57.5 58 58.5

−50

θ/deg

55 55.5 56 56.5 57 57.5 58 58.5

−100

100

θ/deg s

− 1

55 55.5 56 56.5 57 57.5 58 58.5

−100

100

/deg s

− 1

55 55.5 56 56.5 57 57.5 58 58.5

−100

100

/deg s

− 1

t / s

(b) Joint 1 moving (yaw).

Figure 5: Comparison of measured (solid) and predicted data (dashed). For each ﬁgure we see from top to bottom the motor

position θ

θ, the motor velocity

θ, the second gyroscope

IMU2

, and the ﬁrst

IMU1

. 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 ﬁts

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

θ/deg

63 63.5 64 64.5 65 65.5 67 67.5 68 68.5

−100

100

θ/deg s

− 1

63 63.5 64 64.5 65 65.5 67 67.5 68 68.5

−100

100

/deg s

− 1

63 63.5 64 64.5 65 65.5 67 67.5 68 68.5

−40

−20

/deg s

− 1

t / s

(a) Joint 2 moving (pitch).

79 79.5 80 80.5 81 81.5 82 82.5 83

−20

θ/deg

79 79.5 80 80.5 81 81.5 82 82.5 83

−100

100

θ/deg s

− 1

79 79.5 80 80.5 81 81.5 82 82.5 83

−100

100

/deg s

− 1

79 79.5 80 80.5 81 81.5 82 82.5 83

−40

−20

/deg s

− 1

t / s

(b) Joint 3 moving (roll).

Figure 6: Comparison of measured (solid) and predicted data (dashed). For each ﬁgure we see from top to bottom the motor

position θ

θ, the motor velocity

θ, the second gyroscope

IMU2

, and the ﬁrst

IMU1

. 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 ﬁt 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

θ/deg

94 96 98 100 102 104 106 108 110

−100

100

θ/deg s

−1

94 96 98 100 102 104 106 108 110

−100

100

/deg s

−1

94 96 98 100 102 104 106 108 110

−100

100

/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.

4.5 5 5.5 6 6.5 7 7.5 8

x 10

−50

θ/deg

4.5 5 5.5 6 6.5 7 7.5 8

x 10

−200

200

θ/deg s

− 1

4.5 5 5.5 6 6.5 7 7.5 8

x 10

−200

200

/deg s

− 1

4.5 5 5.5 6 6.5 7 7.5 8

x 10

−200

200

/deg s

− 1

t / s

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 ﬁnd parame-

ters for the dynamics even if the model takes no ﬂex-

ibility of the bearing into account, which moves the

whole body. To get more accurate results the solver

packs the bearing’s ﬂexibility into the ﬂexibility be-

tween joints and links. The predicted behavior of the

robot based on the identiﬁed 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 ﬂexi-

bility gets stimulated. The overall approximation is

ﬁtting 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 ﬂexibility of the

bearing is part of the dynamic model. We believe that

this can be achieved by putting two new joints before

the ﬁrst 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 ﬂexibility

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

uniﬁed passivity-based control framework for posi-

tion, torque and impedance control of ﬂexible 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 identiﬁcation 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 ﬁtting 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 identiﬁcation 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). Identiﬁca-

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

ﬂexible robots using accelerometers and angular rate

sensors. Mechatronics, 22(8):1043 – 1049.

Vuong, N. D. and Ang Jr, M. H. (2009). Dynamic model

identiﬁcation for industrial robots. Acta Polytechnica

Hungarica, 6(5):51–68.

Wu, J., Wang, J., and You, Z. (2010). An overview of

dynamic parameter identiﬁcation of robots. Robotics

and Computer-Integrated Manufacturing, 26(5):414 –

419.

ICINCO 2016 - 13th International Conference on Informatics in Control, Automation and Robotics

346