Professional Documents
Culture Documents
Cartesian robot
"b
0%
B = $ 11
'
# 0 b22 &
"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
!
!
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
static balancing
!
mechanical compensation
!
!
g(q) " 0
absence of gravity
!
!
applications in space
constant U (motion on horizontal plane)
Robotics 2
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
S(q, q ) " k 4 q
g(q) " k 6
NOTE: norms are either for vectors or for matrices (induced norms)
Robotics 2
Robotics 2
-1
-2
! 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
1
1
1
1
!
2
2
2
2 !
2
!
Robotics 2
1
T = " Ti = q TB(q)q
2
i=1
&
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
U1 = m1g0! c1s1
U2 = m2g0! c2s 2
U3 = m3g0 (! 2s 2 + ! c3s1 )
!
!
!
U=
" 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
10
uV,i = "FV,i q i
!
Robotics 2
11
!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
1 T
Tm = " Tmi = q Bm q
2
i=1
N
diagonal, >0
Robotics 2
does NOT
contribute to c
Fv > 0, FS > 0
diagonal
motor torques
(after the
reduction gears)
13
belts
harmonic drives
long shafts
!
!
Robotics 2
14
DLR LWR-III
Dexter
motor
elastic
load/link
spring
(stiffness K)
Robotics 2
video
Stanford DECMMA
15
Dynamic model
of robots with elastic joints
!
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
1
2
Robotics 2
no external torques
performing work on q
16
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,xz %
'
Ici,yz '
Ici,zz &
Robotics 2
17
-2
T
ci
1
2
T
i ci
Steiner theorem
Robotics 2
1
2
18
-3
Ui = "mi g0Tr0,ci = "mi g0T (ri + rci) = "mi g0Tri - g0T (mi rci)
!
(0-th order
moment)
mass ! CoM
position
of link i
# mi &
" i = %mi irci( = mi mi irci,x
% i (
$ Ii '
(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
-4
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
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
-5
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
p!1
#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 '
!
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
a3 = Ic2,zz + m2d22
# a1 &
#u1 &
g0c1 & %!
a2 (
(% ( = Ya =u =% (
$u2 '
0 ' % a3 (
$ a4 '
a2 = m2d2
a4 = m1d1 + m2! 1
both in terms of the chosen set of dynamic coefficients a and for the
!
associated
regression matrix Y !
!
Robotics 2
Linear parametrization
of a 2R planar robot (N=2)
!
!
(link 1)
m2 m2d2
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 '
(link 2)
Robotics 2
24
!
!
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
k = 1,,nc
26
a=Y u= Y Y
!
-1
YT u
Robotics 2
27
nc
Ya = u
Y=
nc
!
" numerical check of full column rank is
more robust rank = p (# of cols)
Robotics 2
28
Robotics 2
29
30
Robotics 2
31
dqd dqs dr
q d (t) =
=
= q's (r) r
dt
dr dt
qd (t) = qs (r(t))
dq d " dq's dr %
q d (t) =
=$
' r + q's (r)r = q''s (r) r2 + q's (r)r
dt # dr dt &
!
!
!
Q: what is the new input torque needed to execute the scaled trajectory?
(suppose dissipative terms can be neglected)
Robotics 2
32
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:
speed up
increase torque
33
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
Robotics 2
34
original
total
torque
gravity torque
to sustain the link
at steady state
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
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
'
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 )
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)
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
!
..
g(qe ) = ue
Robotics 2
39
q = q e + "q = "q
q = q e + "q = "q
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
q = q d + "q
= q
d + "q
u = 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
$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
(cont)
after simplifications
" 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
(
(
Robotics 2
42
Coordinate transformation
+ c(q, q ) + g(q) = B(q)q
+ n(q, q ) = u q
q " IR N B(q)q
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
! "1
"1
T
J"T!
(q) #
!
Robotics 2
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
"1 "1
c p (p,p ) = S p (p,p )p
non-conservative
generalized forces
performing work on p
quadratic
.
dependence on p
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
" 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
45