You are on page 1of 45

Robotics 2

Dynamic model of robots:


Analysis, properties, extensions,
parametrization, identification, uses
Prof. Alessandro De Luca

Analysis of inertial couplings


!

Cartesian robot

"b
0%
B = $ 11
'
# 0 b22 &

Cartesian skew robot

"b11 b12 %
B =$
'
#b21 b22 &

PR robot

2R robot

" !b11
b12 (q2 )%
B(q) = $
'
b22 &
#b21 (q2 )
"b!
b12 (q2 )%
11 (q2 )
B(q) = $
'
b22 &
#b21 (q2 )
!

3R articulated robot

!
!

dynamic model turns out to be linear if


!
!

Robotics 2

"b (q ,q )
0
0 %
11
2 3
$
'
B(q) = $
0
b22 (q3 ) b23 (q3 )'
$
0
b23 (q3 )
b33 '&
#

g0
B constant c 0

! d = 0 in PR
d2 = 0 in 2R

center of mass
of link 2
on joint 2 axis

Analysis of gravity term


!

static balancing
!

mechanical compensation
!
!

distribution of masses (including motors)

g(q) " 0

articulated system of springs


closed kinematic chains

absence of gravity
!
!

applications in space
constant U (motion on horizontal plane)

Robotics 2

Bounds on dynamic terms


!

for an open-chain (serial) manipulator, there always exist


positive. real constants k0 to k7 such that, for any value of
q and q

k 0 " B(q) " k 1 + k 2 q + k 3 q

S(q, q ) " q

k 4 + k5 q

g(q) " k 6 + k 7 q

inertia matrix
factorization matrix of
Coriolis/centrifugal terms
gravity vector

if the robot has only revolute joints, these simplify to

k 0 " B(q) " k 1

S(q, q ) " k 4 q

g(q) " k 6

NOTE: norms are either for vectors or for matrices (induced norms)

Robotics 2

Robots with closed kinematic chains

Comau Smart NJ130

Robotics 2

-1

MIT Direct Drive Mark II and Mark III

Robots with closed kinematic chains

MIT Direct Drive Mark IV


(planar five-bar linkage)
Robotics 2

-2

UMinnesota Direct Drive Arm


(spatial five-bar linkage)
6

Robot with parallelogram structure


(planar) kinematics and dynamics

! c3

! c4

! c1

!5

! c2

! q1

center of mass:
arbitrary ! ci

q2 - !
5

E-E

parallelogram:

!1 = ! 3
! !2 = ! 4

q2

!
!

direct kinematics
c1 % " ! 5 c2 %
" ! 1c1 % " ! 5 cos(q2 ( ) ) % " ! 1!
p EE = $
+
=
' $ ! sin(q ( ) ) ' $ !!
' ( $! s '
!
s
s
# 1 1& # 5
2
& # 1 1& # 5 2&
position of center of masses

!" ! c %
p c1 = $ c1 1 '
# ! c1s1 &

"! c %
p c2 = $ c2 2 '
# ! c2s 2 &

"! c % "! c %
p c3 = $ 2 2 ' + $ c3 1 '
# ! 2s 2 & # ! c3s1 &

Robotics 2

"! c % "! c %
p c4 = $ 1 1 ' ( $ c4 2 '
# ! 1s1 & # ! c4s 2 &
7

Kinetic energy
linear/angular velocities
# "! s &
v c1 = % c1 1 ( q 1
$ ! c1c1 '

# "! s &
v c2 = % c2 2 ( q 2
$ ! c2c2 '

# "! s &
# "! s &
v c4 = % 1 1 ( q 1 " % c4 2 ( q 2
$ ! 1c1 '
$ ! c4c2 '
!

Ti

# "! s &
# "! s &
v c3 = % c3 1 ( q 1 + % 2 2 ( q 2
$ ! c3c1 '
$ ! 2s 2 '

"1 = " 3 = q 1
!

" 2 = " 4 = q 2

Note: a 2D notation is used here!

1
1
1
1
!
2
2
2
2 !
2

T2 = m2! c2q2 + Ic2,zz q 22


T1 = m1! c1q1 + Ic1,zzq1
2
2
2
2
1
1
2

T3 = Ic3,zz q1 + m3 (!22q 22 + !2c3q 12 + 2! 2! c3c2"1q 1q 2 )


2
2
1
1! 2 2 2 2
2
T4 = Ic4,zz q 2 + m4 (! 1q 1 + ! c4q 2 " 2! 1! c4c2"1q 1q 2 )
2
2

!
Robotics 2

Robot inertia matrix


4

1
T = " Ti = q TB(q)q
2
i=1

# Ic1,zz + m1!2c1 + Ic3,zz + m3!2c3 + m4!21


B(q) = %
(m3! 2! c3! " m4! 1! c4 )c2"1
$
structural condition
in mechanical design

&
symm
(
Ic2,zz + m2!2c2 + Ic4,zz + m4!2c4 + m3!22 '

m 3 ! 2 ! c3 = m 4 ! 1! c4 (*)

!
B(q) diagonal and constant centrifugal and Coriolis terms 0

mechanically DECOUPLED and LINEAR


dynamic model (up to the gravity term g(q))

big advantage for the design of a motion control law!


Robotics 2

Potential energy and gravity terms


Ui

from the y-components of vectors pci

U1 = m1g0! c1s1

U2 = m2g0! c2s 2

U3 = m3g0 (! 2s 2 + ! c3s1 )
!
!

!
U=

U4 = m4g0 (! 1s1 " ! c4s 2 )


4

" Ui

i=1
!
# " U & T # g0 (m1! c1 + m3! c3 + m4! 1 )c1 & # g1 (q1 ) &
g(q) = % ( = %
(=%
(
$ " q ' $ g0 (m2! c2 + m3! 2 ) m4! c4 )c2 ' $g2 (q2 )'

in addition,
when (*) holds

1 + g1 (q1 ) = u1
b11q
b22q2 + g2 (q2 ) = u2

gravity
components
are always
decoupled

ui
(non-conservative) torque
producing work on qi

further structural conditions in the mechanical design lead to g(q) 0!!


Robotics 2

10

Adding dynamic terms ...


!

dissipative phenomena due to friction at the joints/transmissions


! viscous, dry, Coulomb, ...
! local effects at the joints
! difficult to model in general, except for:

uV,i = "FV,i q i
!

uS,i = "FS,i sgn(q i)

inclusion of electrical motors (as additional rigid bodies)


! motor i mounted on link i-1 (or before), typically with its motion
with joint i axis
!(spinning) axis aligned
!
! (balanced) mass of motor included in total mass of carrying link
! (rotor) inertia has to be added to robot kinetic energy
! transmissions with reduction gears (often, large reduction ratios)
! in some cases, multiple motors cooperate in moving multiple links:
use a transmission coupling matrix T (with off-diagonal elements)

Robotics 2

11

Placement of motors along the chain


.

!m1

!mN
motor 1

motor N

joint 2

RFw
(world frame)

link 2

link 1

RFN-1

RF1

RF0

link N

RFN

link N - 1
link 0
(base)

motor 2

joint N

joint1

!mi = nri !i
"i = nri "mi

!m2
Robotics 2

12

Resulting dynamic model


!

simplifying assumption: in the rotational part of kinetic


energy, only the spinning rotor velocity is considered
1
1
1
2
Tmi = Imi "mi
= Imi n2ri q 2i = Bmi q 2i
2
2
2

1 T
Tm = " Tmi = q Bm q
2
i=1
N

diagonal, >0

including all added terms, the robot dynamics becomes


!

moved to the left ...

(B(q) + Bm )q + c(q, q ) + g(q) + FV q + FS sgn(q ) = "


constant

Robotics 2

does NOT
contribute to c

Fv > 0, FS > 0
diagonal

motor torques
(after the
reduction gears)
13

Including joint elasticity


!

in industrial robots, use of motion transmissions based on


!
!
!

belts
harmonic drives
long shafts

introduces flexibility between actuating motors (input) and


driven links (output)
in research robots for human cooperation, compliance in
the transmissions is introduced on purpose for safety
actuator relocation by means of (compliant) cables and pulleys
! harmonic drives and lightweight (but rigid) link design
! redundant (macro-mini or parallel) actuation, with elastic couplings
in both cases, flexibility is modeled as concentrated at the joints
most of the times, assuming small joint deformation (elastic domain)
!

!
!

Robotics 2

14

Robots with joint elasticity

DLR LWR-III

Dexter

with harmonic drives

with cable transmissions

motor

elastic
load/link
spring

(stiffness K)

Quanser Flexible Joint

(1-dof linear, educational)

Robotics 2

video

Stanford DECMMA

with micro-macro actuation

15

Dynamic model
of robots with elastic joints
!

introduce 2N generalized coordinates


!
!

q = N link positions
! = N motor positions (after reduction, !i = !mi/nri)

1
1
1
2
Tmi = Imi "mi
= Imi n2ri "i2 = Bmi "i2
2
2
2

1
Tm = " Tmi = # T Bm #
2
i=1

diagonal, >0

add elastic potential energy Ue to that due


! to gravity Ug(q)
!

1
2

add motor kinetic energy Tm to that of the links Tq = q T B(q) q

K = matrix of joint stiffness (diagonal, >0)


N
2
1
1
1 !
T
2
Uei = K i (qi " (#mi /nri)) = K i (qi " # i) Ue = "Uei = (q - # ) K (q - # )
2
2
2
i=1

apply Euler-Lagrange equations w.r.t. (q, !)


2N 2nd-order
differential
equations

Robotics 2

B(q) q + c(q, q ) + g(q) +K(q " # ) = 0


!
B # + K(# " q) = $

no external torques
performing work on q

16

Dynamic parameters of robots


!

consider a generic link


of a fully rigid robot

Center of Mass
(CoM) frame i
link i

ICi

joint i
CoM Ci
mig0
base
frame 0
!

-1

zi
rCi

yi
Oi

ri

kinematic frame i
(DH or modified DH)

xi

" Ici,xx Ici,xy


"r %
xi
Ici,yy
mi rCi = $ryi ' ICi = $$
$ '
#rzi &
#symm

each link is characterized by 10


dynamic parameters

Ici,xz %
'
Ici,yz '
Ici,zz &

however, the robot dynamics depends in a nonlinear way on some of these


parameters (e.g., through the combination Ici,zz+mirxi2)

Robotics 2

17

Dynamic parameters of robots


!

-2

kinetic energy and gravity potential energy can both be rewritten so


that a new set of dynamic parameters appears only in a linear way
" need to re-express link inertia and CoM position in (any) known kinematic
frame attached to the link (same orientation as the barycentric frame)

fundamental kinematic relation

v ci = v i + " i # rci = v i + S(" i)rci = v i $ S(rci)" i


!

kinetic energy of link i

T
ci

1
2

T
i ci

Ti = mi v v ci + " I " i = mi ( v i # S(rci)" i ) ( v i # S(rci)" i) + 12 " iT Ici " i


!
= 12 mi v iT v i + 12 " iT (Ici + mi S T (rci)S(rci)) " i # v iT S(mi rci )" i
1
2

Steiner theorem
Robotics 2

1
2

" Ii,xx Ii,xy Ii,xz %


2
$
'
= Ici + mi rci E - rciT rci = Ii = $
Ii,yy Ii,yz '
Ii,zz &
# symm

3!3 identity matrix

18

Dynamic parameters of robots


!

-3

gravitational potential energy of link i

Ui = "mi g0Tr0,ci = "mi g0T (ri + rci) = "mi g0Tri - g0T (mi rci)
!

by expressing vectors and matrices in frame i, both Ti and Ui are


linear in the set of 10 (constant) standard parameters "i

! Ti = 12 mi iv iT i v i + mi irciT S(i v i ) i " i + 12 i" iT iIi i" i


mass
of link i

(0-th order
moment)

mass ! CoM
position
of link i

# mi &
" i = %mi irci( = mi mi irci,x
% i (
$ Ii '

Ui = "mi g0Tri " ig0T (mi irci)


inertia
of link i

(1-st order
moment)
i

mi rci,y

(2-nd order
moment)

!
i

mi rci,z

Ii,xx

Ii,xy

Ii,xz

Ii,yy

Ii,yz

Ii,zz

since the E-L equations involve only linear operations on T and U, also
the robot dynamic model is linear in the standard parameters " # R10N$

Robotics 2

19

Dynamic parameters of robots


!

-4

using a N!10N regression matrix Y" that depends only on kinematic


quantities, the robot dynamic equations can always be rewritten linearly
in the standard dynamic parameters as

B(q)q + c(q, q ) + g(q) = Y" (q, q , q) " = u


" T = ( " 1T " 2T ! " NT )
!

the open kinematic chain structure of the manipulator implies that the i-th
dynamic equation may depend only on the base parameters of links i to N
!
Y" has a block upper triangular structure

# Y1,1
% 0
Y" (q, q , q) = %
% !
$ 0

Y1,2 Y1,N &


Y2,2 Y2,N (
(
"
! (
0 YN,N '

with Yi,j row vectors of size 1!10

A nice property: element bij of B(q) is a function of (qk+1, ... ,qN), with k=min{i,j},
and of the inertial parameters of links r to N, with r=max{i,j}

Robotics 2

20

Dynamic parameters of robots


!

-5

many standard parameters do not appear (play no role) in the dynamic


model of the given robot the associated columns of Y" are 0!
some standard parameters may appear only in fixed combinations with
others the associated columns of Y" are linearly dependent!
one can isolate p << 10N independent groups of parameters "
(associated to p functionally independent columns Yindep of Y") and partition
matrix Y" in two blocks, the second containing dependent (or zero) columns
as Ydep = YindepT, for a suitable constant p!(10N-p) matrix T

# " indep &


) " = Yindep Ydep %
Y" (q, q , q
( = Yindep Yindep T
"
$ dep '
) a
= Yindep " indep + T" dep = Y(q, q , q

# " indep &


%" (
$ dep '

these grouped parameters are called dynamic coefficients a # Rp,


the only that matter in robot dynamics (= base parameters by W. Khalil)
!! the minimal number p of dynamic coefficients that is needed can also be
checked numerically (see later Identification)
!

Robotics 2

21

Linear parameterization
of robot dynamics
it is always possible to rewrite the dynamic model in the form
regression
a = vector of
matrix
dynamic coefficients

B(q)q + c(q, q ) + g(q) = Y(q, q , q) a = u


N!p

p!1

e.g., the heuristic grouping (found by inspection) on a 2R planar robot

#q

% 1
$0

1 + q
2 ) " s 2 (q 22 + 2q 1q 2 )
2
c2 (2q
q
1 + s 2q 12
q1+ q
2
c2q

c1
0

# a1 &
%a (
&
c12 % 2 ( #u1 &
( a3 = % (
c12 ' %a ( $u2 '
% 4!
(
$ a5 '
!

a1 = Ic1,zz + m1d12 + Ic2,zz + m2d22 + m2!21


a2 = m2! 1d2

a3 = Ic2,zz + m2d22

a4 = g0 (m1d1 + m2! 1 )
a5 = g0 m2d2
!

NOTE: 4 more coefficients will be present, if including the coefficients Fv,i and Fs,i of viscous and dry
!
friction at the joints (decoupled terms, each appearing only in the respective equation i, i=1,2)
Robotics 2

22

Linear parametrization
of a 2R planar robot (N=2)

being the kinematics known (i.e., ! 1 and g0), the number of dynamic
coefficients can be reduced since we can merge the two coefficients
a2 = m2! 1d2 a5 = g0 m2d2
a2 = m2d2 (factoring out ! 1 and g0)
therefore, after regrouping, p = 4 dynamic coefficients are sufficient

#q
1
%
$0

1 + q
2 ) " ! 1s 2 (q 22 + 2q
1q 2 ) + g0c12
2
! 1!
c2 (2q
q
!
1 + ! 1s 2q 12 + g0c12
q1+ q
2
! 1c2q

a1 = Ic1,zz + m1d12 + Ic2,zz + m2d22 + m2!21


!

a3 = Ic2,zz + m2d22

# a1 &
#u1 &
g0c1 & %!
a2 (
(% ( = Ya =u =% (
$u2 '
0 ' % a3 (
$ a4 '

a2 = m2d2
a4 = m1d1 + m2! 1

! of robot dynamics is not unique,


! this (minimal) linear parametrization

both in terms of the chosen set of dynamic coefficients a and for the
!
associated
regression matrix Y !
!

Robotics 2

a systematic procedure for its derivation would be preferable


23

Linear parametrization
of a 2R planar robot (N=2)

in alternative, from the general procedure


!
!

10N = 20 standard parameters are defined for the two links


from the assumption made on CoM locations, only 5 such parameters actually
appear, namely (with di = rci,x)
m1d1

!
!

I1,zz = Ic1,zz + m1d12

(link 1)

m2 m2d2

I2,zz = Ic2,zz + m2d22

in the 2!5 matrix Y" , the 3rd column (associated to m2) is Y"3 = Y"1 ! 1 + Y"2 !21
after regrouping/reordering, again p = 4 dynamic coefficients are sufficient
# a1 &
2
#g c q


!
& % a2 (
#u1 &
!
c
2
q
+
q
"
!
s
q
q
(
)
(
0 1
1
1 2
1
2
1 2
2 + 2q1q2 ) + g0c12
1+ q2
=
Y
a
=
u
=
%
(
% (
%a (
2
$u2 '

0
! 1c2q1 + ! 1s 2q1 + g0c12
q1+ q2 ' % 3 (
$ 0
$ a4 '

a1 = m1d1 + m2! 1 a2 = I1,zz + m2!21 = (Ic1,zz + m1d12 ) + m2!21


!

(link 2)

a3 = m2d2 a4 = I2,zz = Ic2,zz + m2d22

determining a minimal parameterization (i.e., minimizing p) is important for


!
!

experimental identification of dynamic coefficients


!
!
!
adaptive/robust
control design in the presence of uncertain
parameters

Robotics 2

24

Identification of dynamic coefficients


!

!
!

in order to use the model, one needs to know the numeric values of
the robot dynamic coefficients
! robot manufacturers provide at most only a few principal dynamic
parameters (e.g., link masses)
estimates can be found with CAD tools (e.g., assuming uniform mass)
friction coefficients are (slowly) varying over time
! lubrication of joints/transmissions
for an added payload (attached to the E-E)
! a change in the 10 dynamic parameters of last link
! this implies a variation of (almost) all robot dynamic coefficients
preliminary identification experiments are needed
! robot in motion (dynamic issues, not just static or geometric ones!)
! only the robot dynamic coefficients can be identified (not all link
standard parameters!)

Robotics 2

25

Identification experiments
1. choose a motion trajectory that is sufficiently exciting, i.e.,
" explores the robot workspace and involves all components in the
robot dynamic model
" is periodic, with multiple frequency components
2. execute this motion (approximately) by means of a control law
" taking advantage of any available information on the robot model
. .
" usually, u = KP(qd-q)+KD(qd-q) (PD, no model information used)
.
3. measure q (encoder) and, if available, q in nc time instants
" joint velocity and acceleration can be later estimated off line by
numerical differentiation (use of non-causal filters is feasible)
4. with such measures/estimates, evaluate the regression matrix Y (on
the left) and use the applied commands (on the right) in the
expression

Y(q(t k ), q (t k ), q(t k )) a = u(t k )


Robotics 2

k = 1,,nc
26

Least Squares (LS) identification


!

set up the system of linear equations

" Y(q(t1 ), q (t1 ), q(t1 )) %


" u(t1 ) %
$
'
$
'
!
a
=
Ya = u
nc ! N $
'
$
'
$ Y(q(t ), q (t ), q(t )) '
$ u(t ) '
n
n
n
#
c
c
c
&
# nc &
! sufficiently exciting trajectories, large enough number of
samples (nc ! N_ p), and their suitable selection/position,
guarantee rank(Y) = p (full column!
rank)
_
!
! solution by pseudoinversion of matrix Y
#

a=Y u= Y Y
!

-1

YT u

one can also use a weighted pseudoinverse, to take into


account different levels of noise in the collected measures

Robotics 2

27

Additional remarks on LS identification


!

it is convenient to preserve the block (upper) triangular structure of


the regression matrix, by stacking all time evaluations sequenced by
the rows of the original Y matrix

" Y1 (q(t1 ), q (t1 ), q(t1 )) %


" u1(t1 ) %
nc $
'
$ ! '
!
$ Y (q(t ), q (t ), q(t )) '
$ u (t ) '
1
n
n
n
c
c
c
$
'
$ 1 nc '
(t1 )) '
u (t )
$ Y2 (q(t1 ), q (t1 ), q
N!
a =$ 2 1 '
!
$
'
$ ! '
$ YN (q(t1 ), q (t1 ), q(t1 )) '
$ uN(t1 ) '
'
$ ! '
!
nc $
$
$
'
(t n ))'&
u
(t
)
N
n
# YN (q(t n c ), q (t n c ), q
#
c
c &

nc

Ya = u

Y=
nc

!
" numerical check of full column rank is
more robust rank = p (# of cols)

further practical hints


_
! outlier data can be eliminated in advance (when building Y)
! if complex friction models are not included in Ya, discard the data
collected at joint velocities that are close to zero

Robotics 2

28

Summary on dynamic identification


KUKA IR 361
robot and
optimal
excitation
trajectory

J. Swevers, W. Verdonck, and J. De Schutter:


Dynamic model identification for industrial robots
IEEE Control Systems Mag., Oct 2007

Robotics 2

results after identification (first three joints only)

29

Dynamic identification of KUKA LWR4


video

data acquisition for identification


dynamic coefficients: 30 inertial, 12 for gravity
C. Gaz, F. Flacco, A. De Luca:
Identifying the dynamic model used by the KUKA LWR:
A reverse engineering approach
IEEE ICRA 2014, Hong Kong, June 2014
Robotics 2

validation after identification (for all 7 joints):


on new desired trajectories, compare
torques computed with the identified model
and torques measured by joint torque sensors

30

Use of the dynamic model


Inverse dynamics

given a desired trajectory qd(t) of motion


..
! twice differentiable ( qd(t))
! possibly obtained from a task/Cartesian trajectory rd(t), by
(differential) kinematic inversion
the input torque needed to execute it is

" d = (B(qd ) + Bm )q d + c(qd , q d ) + g(qd ) + FV q d + FS sgn(q d )


!
!

useful also for control (e.g., nominal feedforward)


however, this algebraic computation (t) is not efficient when
performed using the above Lagrangian approach
! symbolic terms grow much longer, quite rapidly for larger n
! in real time, better a numerical computation based on
Newton-Euler method

Robotics 2

31

Dynamic scaling of trajectories


uniform time scaling of motion

given a smooth original trajectory qd(t) of motion for t [0,T]


! suppose to rescale time as t r(t) (a strictly increasing function of t)
! in the new time scale, the scaled trajectory qs(r) satisfies

dqd dqs dr
q d (t) =
=
= q's (r) r
dt
dr dt

qd (t) = qs (r(t))

same path executed


(at different instants of time)

dq d " dq's dr %
q d (t) =
=$
' r + q's (r)r = q''s (r) r2 + q's (r)r
dt # dr dt &
!

!
!

uniform scaling of the trajectory occurs when r(t) = k t

q d (t) = k q's (kt)

q d (t) = k 2 q''s (kt)

Q: what is the new input torque needed to execute the scaled trajectory?
(suppose dissipative terms can be neglected)
Robotics 2

32

Dynamic scaling of trajectories


inverse dynamics under uniform time scaling

the new torque could be recomputed through the inverse dynamics,


for every r = k t [0,T]=[0,kT] along the scaled trajectory, as

" s (kt) = B(qs )q''s + c(qs,q's ) + g(qs )


!

however, being the dynamic model linear in the acceleration and


quadratic in the velocity, it is

" d (t) = B(qd )q d + c(qd , q d ) + g(qd ) = B(qs )k 2q''s + c(qs,kq's ) + g(qs )


! 2
= k (B(qs )q''s + c(qs ,q's )) + g(qs ) = k 2 (" s (kt) # g(qs )) + g(qs )
!

thus, saving separately the total torque %d(t) and gravity torque gd(t)
in the inverse dynamics computation along the original trajectory, the
new input torque is obtained directly as

1
" s (kt) = 2 (" d (t) # g(qd (t))) + g(qd (t))
k
Robotics 2

k>1: slow down


reduce torque

k<1:
speed up
increase torque

gravity term (only position-dependent): does NOT scale!

33

Dynamic scaling of trajectories


numerical example

rest-to-rest motion with cubic polynomials for planar 2R robot under gravity
(from downward equilibrium to horizontal arm + vertical up forearm)
original trajectory lasts T=0.5 s
only joint 1 torque is shown
torque only due
to non-zero initial
acceleration
total torque

gravity torque
component

at equilibrium
= zero gravity
torque

for both joints

Robotics 2

34

Dynamic scaling of trajectories


numerical example

original
total
torque

gravity torque
to sustain the link
at steady state

scaling with k=2 (slower) T = 1 s

" d (0.1) # g(qd (0.1)) = 20 Nm


*

" s (2# 0.1) $ g(qs (2# 0.1)) =

T=0.5 s

scaled
total
torque

20
= 5 Nm
22

gravity torque
component
does not scale

0 Nm

1
4

!
T=0.5 s

Robotics 2

T=1 s

k =2

T=1 s

35
!

State equations
Direct dynamics

Lagrangian
dynamic model

B(q)q + c(q, q ) + g(q) = u

N differential
2nd-order
equations

" x1 % " q %
2N
x
=
=
(
IR
defining the vector of state variables as
$ x ' $ q '
# 2& # &
state equations

x2
% " 0 %
" x 1 % "
x = $ ' = $ (1
u
! ' + $ (1
'

# x 2 & # (B (x1 )[c(x1 ,x 2 ) + g(x1 )] & # B (x1 ) &

2N differential
1st-order
equations

= f(x) + G(x)u
2N!1 2N!N

" q %

another choice... x = $
'

B(q)
q
#
&
Robotics 2

generalized
momentum

.
x = ... (do it as exercise)
~

36

Dynamic simulation
Simulink
block
scheme
input torque
command
!
(open-loop u
or in
feedback)

c(q, q )

here, a generic 2-dof robot

q, q

q 1 (0)

q1

! _

B-1(q)

"!
! q 2 (0)

q!

q 1

q2

!
g(q)

"

q1 (0)

q 2

"

q1

"

q2

q2 (0)

including inv(B)

" initialization (dynamic coefficients and initial


state) !
!
! Matlab
!
" calls to (user-defined)
functions for the evaluation of model terms
" choice of a numerical integration method (and of its parameters)
Robotics 2

37

an incorrect realization
c1+ g1

_
u1

b!12
u2

1
b11
!

_
b12

q1 (0)

q 1 (0)

q1

"

q 1

"

q1
causality
principle
is violated!!

!
algebraic
loop!

q2 (0)

q 2 (0)

!1

b22
!

q2

"

q 2

"

q2

!
c2 + g2 !
!
inversion of the
! robot inertia
! matrix in toto is needed
(and not only of its elements on the diagonal...)
Robotics 2

38

Approximate linearization
!

we can derive a linear dynamic model of the robot, which is valid


locally around a given operative condition
! useful for analysis, design, and gain tuning of linear (or, the linear
part of) control laws
! approximation by Taylor series expansion, up to the first order
! linearization around a (constant) equilibrium state or along a
(nominal, time-varying) equilibrium trajectory
! usually, we work with (nonlinear) state equations; for mechanical
systems, it is more convenient to directly use the 2nd-order model
!

same result, but easier derivation


.

..

equilibrium state (q,q) = (qe,0) [ qe=0 ]


.

g(qe ) = ue

equilibrium trajectory (q,q) = (qd (t),qd(t))

Robotics 2

B(qd )q d + c(qd , q d ) + g(qd ) = ud


!

39

Linearization at an equilibrium state


!

variations around an equilibrium state


q = qe + "q

q = q e + "q = "q

q = q e + "q = "q

keeping into account the quadratic dependence of c terms


on velocity (thus, neglected around the zero velocity)
#g

B(qe ) "q + g(qe ) +


"q + o( "q , "q ) = ue + "u
# q q=q
e

in state-space format
# "q &
"x = % (
$ "q '

Robotics 2

u = ue + "u

G(qe )

infinitesimal terms
of second or higher order

0
I&
#
# 0 &
"x = % -1
"x + % -1
"u = A "x + B "u
(
! $ -B (qe )G(qe ) 0 ('
$ B (qe ) '
40

Linearization along a trajectory


!

variations around an equilibrium trajectory


q = qd + "q

q = q d + "q

= q
d + "q

u = ud + "u

developing terms in the dynamic model ...

B(qd + "q)(q d + "q ) + c(qd + "q, q d + "q ) + g(qd + "q) = ud + "u

$b
B(qd + "q) # B(qd ) + % i
e iT "q
i=1 $ q q=q
d
C1 (qd , q d )
N

!
!

Robotics 2

i-th row of the


identity matrix

$c
$c
c(qd + "q, q d + "q ) # c(qd , q d ) +
"q +
"q
$ q q=q d
$ q q=q d
q = q d
q = q d
!
g(qd + "q) # g(qd ) + G(qd )"q
C2 (qd , q d )
41

Linearization along a trajectory


!

(cont)

after simplifications

B(qd ) "q + C2 (qd , q d )"q + D(qd , q d , q d )"q = "u


with

" bi
d e iT
q
i=1 " q q=q
N

d ) = G(qd ) + C1 (qd , q d ) + #
D(qd , q d , q

!
!

in state-space format

0
I
#
&
# 0 &
"x = % -1
"x + % -1
"u
-1
(
(

$ -B (qd )D(qd , qd , qd ) -B (qd )C2 (qd , qd ) '


$ B (qd ) '
= A(t ) "x + B (t ) "u

a linear, but time-varying system!!


!

Robotics 2

42

Coordinate transformation
+ c(q, q ) + g(q) = B(q)q
+ n(q, q ) = u q
q " IR N B(q)q

if we wish/need to use a new set of generalized coordinates p

p " IR N p = f(q)
!
"f(q)
p =
q = J(q)q
"q
+ J(q)
!p = J(q)q
! q

q = f (p)
"1

T
q = J"1 (q)p , u q = J (q)u p

= J"1 (q) p " J (q)J"1 (q)p


q

! "1
"1
T

B(q)J (q)p " B(q)J (q) J(q)J (q)p + n(q, q) = J (q)u p


"1

J"T!
(q) #

!
Robotics 2

pre-multiplying the whole equation...


43

Robot dynamic model

after coordinate transformation

+ J" T (q) n(q, q ) " B(q)J"1 (q) J(q)J"1 (q)p = up


J" T (q)B(q)J"1 (q)p
for actual computation,
these inner substitutions
are not necessary

q"p

q, q " p,p

+ c p (p,p ) + gp (p) = up
B p (p)p
B p = J BJ
"T

cp = J

"T

"1

symmetric,
positive definite
(out of singularities)

gp = J"T g

c " BJ JJ p = J"T c " B p J J"1p

"1 "1

c p (p,p ) = S p (p,p )p

non-conservative
generalized forces
performing work on p

quadratic
.
dependence on p

B p " 2Sp skew - symmetric

when p = E-E pose, this is the robot dynamic model in Cartesian coordinates
Q: What if the robot is redundant with respect to the Cartesian task?
Robotics 2

44

Optimal point-to-point robot motion


considering the dynamic model

" given the initial and final robot configurations (at rest) and
actuator torque bounds, find
! the minimum-time Tmin motion
! the (global/integral) minimum-energy Emin motion
and the associated command torques needed to execute them
" a complex nonlinear optimization problem solved numerically
video

Robotics 2

video

Tmin= 1.32 s, E = 306

T = 1.60 s, Emin = 6.14

45

You might also like