Modeling and Control Robots with Flexible Links · 2004. 10. 26. · ScuoladiDottoratoCIRA...
Embed Size (px)
Transcript of Modeling and Control Robots with Flexible Links · 2004. 10. 26. · ScuoladiDottoratoCIRA...

Scuola di Dottorato CIRA
Controllo di Sistemi Robotici per la Manipolazione e la Cooperazione
Bertinoro (FC), 14–16 Luglio 2003
Robots with Flexible Links:
Modeling and Control
Alessandro De Luca
Dipartimento di Informatica e Sistemistica (DIS)
Università di Roma “La Sapienza”

Outline
• Motivation for considering distributed link flexibility• Dynamic modeling of FL robots: single link and multiple link cases• Formulation of control problems• Controllers for regulation tasks• Controllers for joint and endeffector trajectory tracking tasks• Controllers for resttorest motion tasks• Conclusions• References
2

Motivation
• distributed link deformation in robot manipulators arises when– the design of very long and slender arms is needed for the application– lightweight materials are used (without special care)
• ‘link rigidity’ is always an ideal assumption and may fail when increasing– payloadtoweight ratio– motion speed– control bandwidth
• flexible structures in motion are present in different domains: space manipulators,robots for underwater and underground waste sites, automated cranes, . . .
• as for joint elasticity, neglected link flexibility limits static (steadystate error) ordynamic (vibrations, poor tracking) task performance
• from the control point of view, there is an additional problem of noncolocationbetween input commands and typical outputs to be controlled
3

Robots with link flexibility — SSRMS
• Space Shuttle Remote Manipulation System (Canadarm)→ telemanipulated byastronauts• link bending due to fast motion (not gravity!)
4

Robots with link flexibility — Sam II
• developed at Georgia Tech (W. Book)• macromicro concept for remote exploration
and manipulation of nuclear waste sites
5

Robots with link flexibility — prototypes in Roma
• DMA harmonic steel beam (0.5 kg): DDDC motor, encoder, 7 strain gauges• DIS/DIA FLEXARM: planar twolink with flexible forearm (1.8 kg), DDDC
motors, encoders, onboard optical sensor measuring deformation at three points
6

Robots with link flexibility — prototype in Waterloo
• WATFLEX planar 2R with both link flexible (each with 2 strain gauges), movingwith air bearings on a glass table to support the weight of the second motor;encoders, tachometers, overviewing CCD camera
7

Robots with link flexibility — prototypes in Japan
• spatial 3R flexible arm at Kyoto • cooperating 6R flexible arms at Tohoku
8

Frequency identification of a single flexible link
0 1 2 3 4 5 6 7 8 9 10 11−6000
−4000
−2000
0
2000
4000
6000
s
deg/
s2
10 20 30 40 50 60 70 80
30
40
50
60
70
80
90
100
110
[ left ] frequency sweep joint acceleration signal: plant vs. model
[ right ] joint acceleration frequency response: plant vs. model (matching within 1% of
resonances at f1 = 14.4, f2 = 34.2, and f3 = 69.3 Hz)
9

Dynamic modeling of a single flexible link
• onelink flexible arm modeled as a EulerBernoulli beam in rotation
θJ
J
X
Y
x
y CoM
θ θct
pmp
• length �, uniform density ρ, Young modulus · crosssection inertia EI• actuator inertia J0, payload mass mp and inertia Jp• reference frames: (X,Y ) absolute; (x, y) moving with instantaneous CoM
10

Assumptions and definitions
• EulerBernoulli theory applies to slender arm design (length vs. section)• beam undergoes small deformations of pure bending type in the plane of motion
(no torsion or compression)
• bending deformation w(x, t), with x ∈ [0, �], is directed along the y direction(no shear)
• rotational inertia of beam sections is neglected (→ Timoshenko theory) as wellas the isoperimetric constraint (‘extension’ of beam neutral axis is negligible)
• definition of other relevant angular variables:– position θ(t) of the CoM (not measurable, but convenient)
– position θc(t) of the tangent to the link base (measured by motor encoder)
– position θt(t) of a line pointing to the beam tip (measurable in several ways)
11

• build the Lagrangian from kinetic and elastic potential energy of the beam• using Hamilton principle and calculus of variations, the bending deformation
w(x, t) and angle θ(t) satisfy the linear differential equations
EIw′′′′(x, t) + ρ(ẅ(x, t) + xθ̈(t)) = 0 τ(t)− Jθ̈(t) = 0
i.e., a PDE and an ODE (rigid motion), where J = J0+(ρ�3)/3+Jp+mp�2
and τ = torque input
• geometric/dynamic boundary conditions associated to the PDE
w(0, t) = 0
EIw′′(0, t) = J0(θ̈(t) + ẅ′(0, t))− τ(t) (balance of moments at base)EIw′′(�, t) = −Jp(θ̈(t) + ẅ′(�, t)) (balance of moments at tip)EIw′′′(�, t) = mp(�θ̈(t) + ẅ(�, t)) (balance of shear forces at tip)
12

• in free evolution (τ(t) ≡ 0⇒ θ̈(t) = 0), PDE solved by separation of variables
w(x, t) = φ(x)δ(t) ⇒ EIρ
φ′′′′(x)φ(x)
= − δ̈(t)δ(t)
∆= ω2
for a positive constant ω2 (selfadjoint problem) to be determined
• time solutionδ̈(t) = −ω2δ(t) ⇒ δ(t) = c1 sinωt + c2 cosωt
with c1, c2 depending on the initial conditions δ(0) and δ̇(0)
• space solution (try it!)
φ′′′′(x) = β4φ(x) β4 =ρω2
EI
⇒ φ(x) = A sinβx + B cosβx + C sinhβx + D coshβxwith A,B,C,D obtained from the geometric/dynamic b.c.’s on w(x, t)
13

• using w(x, t) = φ(x)δ(t) and δ̈ = −ω2δ, and holding the b.c.’s for any δ(t),these are rewritten in terms of φ(x) only
φ(0) = 0
EIφ′′(0) + J0 ω2φ′(0) = 0EIφ′′(�)− Jp ω2φ′(�) = 0EIφ′′′(�) + mpω2φ(�) = 0
• using the general solution φ(x), a system of linear homogeneus equations follows A(EI, ρ, �, J0,mp, Jp, β)
ABCD
= 0 (�)
to exclude the trivial solution, the determinant of matrix A should be zero(eigenvalue problem)
14

• det A(β) = 0 at infinite (but countable!) positive increasing roots βi of thetrascendental characteristic equation
(c sh− s ch)− 2mpρ βi s sh−mpρ2
β4i (J0 + Jp)(c sh− s ch)−2Jpρ β
3i c ch
−J0ρ β3i (1 + c ch) +J0Jpρ2
β6i (c sh + s ch)−J0Jpmp
ρ3β7i (1− c ch) = 0
where s = sinβi�, c = cosβi�, sh = sinhβi�, ch = coshβi�
• this is an exact result, that includes common physical approximations
– clampedfree model: mp = Jp = 0, J0 →∞ ⇒ 1 + c ch = 0
– pinnedfree model: mp = Jp = J0 = 0 ⇒ c sh− s ch = 0
15

• associated to each root βi we have:– an eigenfrequency ωi =
√EIβ4i /ρ, characterizing a system vibration
– an eigenvector φi(x) —a spatial deformation mode, defined up to a constant
– a deformation variable δi(t), oscillatory in time
• a finitedimensional approximation of the distributed bending deformation isobtained by truncation
w(x, t) =∞∑i=1
φi(x)δi(t) ≈ne∑i=1
φi(x)δi(t)
where ne is the (arbitrary) number of orthogonal modes that we wish to include
• normalization of the modes can be chosen in different ways (some integral ofthe φi(x) equal to 1, to m, . . . )
16

• resulting dynamic model is particularly simple (after fairly complex analysis . . . )
Jθ̈ = τ
δ̈i + ω2i δi = φ
′i(0)τ i = 1, . . . , ne
• remarkable properties:– rigid motion θ(t) and each vibratory motion δi(t) are decoupled in free
evolution (τ(t) ≡ 0)– all modes are excited by an input command τ(t) �= 0, with a weighting that
depends on φ′i(0) —the tangent at the link base of each deformation mode
– arm ‘stiffness’ is summarized by the (squared) eigenfrequencies ωi
– each vibratory motion is persistent during free evolution, if initially excited
by δi(0) �= 0 (absence of damping)
17

• modal damping can be easily included in the dynamic model
Jθ̈ = τ
δ̈i+2ζiωiδ̇i + ω2i δi = φ
′i(0)τ i = 1, . . . , ne
with coefficients ζi ∈ [0,1)
• its matrix version, with generalized coordinates q = ( θ δ1 . . . δne )T ∈ IRne+1,shows the classical massspringdamper form
Mq̈ + Dq̇ + Kq = B τ
with
M =
[J 0
0 I
]D =
[0 0
0 2ZΩ
]K =
[0 0
0 Ω2
]B =
[1
Φ′
]
Ω = diag {ω1, . . . , ωne}, Z = diag {ζ1, . . . , ζne}, Φ′ = (φ′1(0) . . . φ′ne(0) )T
18

• a different (but equivalent) choice of generalized coordinates may let the inputτ appear in just one equation
(θ, δ) = (θ, δ1, . . . , δne)⇓
(θc, δ) = (θ + δTΦ′, δ) = (θ +∑
φ′i(0)δi , δ1, . . . , δne)
leads to[J −JΦ′T
−JΦ′ I + J2Φ′Φ′T] [
θ̈c
δ̈
]+
[0 0
0 2ZΩ
] [θ̇c
δ̇
]+
[0 0
0 Ω2
] [θc
δ
]=
[1
0
]τ
with same (diagonal) damping D and stiffness K matrices, but full inertia matrix
19

Choice of the controlled output
• joint level angular output (clamped angle)
θc = θ +ne∑i=1
φ′i(0) δi
!! is always minimum phase (zeros in lefthand side of complex plane)
• tip level angular output
θt = θ +ne∑i=1
φi(�)
�δi
!! is typically nonminimum phase (at least for no tip payload)
• output at a point x ∈ [0, �] along the link
θx = θ +ne∑i=1
φi(x)
xδi
20

Transfer functions
• torque τ �→ clamped joint angle θc
Pc(s) =1
Js2+
ne∑i=1
φ′i(0)2
s2 + 2ζiωis + ω2i
=1J
∏nei=1(s
2 + 2ζiωis + ω2i ) + s
2∑nei=1 φ
′i(0)
2∏nej �=i(s
2 + 2ζjωjs + ω2j )
s2∏ne
i=1(s2 + 2ζiωis + ω
2i )
• torque τ �→ tip angle θt
Pt(s) =1
Js2+
ne∑i=1
φ′i(0)φi(�)�
s2 + 2ζiωis + ω2i
=1J
∏nei=1(s
2 + 2ζiωis + ω2i ) + s
2∑nei=1 φ
′i(0)
φi(�)�
∏nej �=i(s
2 + 2ζjωjs + ω2j )
s2∏ne
i=1(s2 + 2ζiωis + ω
2i )
21

A numerical example
• physical data
J0 = 0.002, � = 1, ρ = 0.5, EI = 1 (mp = Jp = 0)
• by considering up to ne = 5 modes (and no damping), we obtain
Ω2 = diag {421.585, 3122.603, 10273.194, 31562.286, 82049.350}
Φ′T =[7.8259 14.6803 12.1284 6.4761 3.7648
]
ΦT� =[−2.6954 2.3268 −2.4970 2.7380 −2.7982
]
!! note the alternating signs of φi(�)’s . . .
22

First four mode shapes
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 13
2.5
2
1.5
1
0.5
0
0.5
1
1.5
2
x [m]
phi_
1
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 12
1.5
1
0.5
0
0.5
1
1.5
2
2.5
x [m]
phi_
2
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 12.5
2
1.5
1
0.5
0
0.5
1
1.5
2
x [m]
phi_
3
0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9 13
2
1
0
1
2
3
x [m]
phi_
4
at f1 = 3.2678, f2 = 8.8936, f3 = 16.1314, and f4 = 28.2751 [Hz]
23

Polezero patterns
1 0.5 0 0.5 160
40
20
0
20
40
60
Real Axis
Imag
Axi
s
polizeri FdT di giunto (2 modi)
40 20 0 20 4060
40
20
0
20
40
60
Real Axis
Imag
Axi
s
polizeri FdT di tip (2 modi)
1 0.5 0 0.5 1150
100
50
0
50
100
150
Real Axis
Imag
Axi
s
polizeri FdT di giunto (3 modi)
100 50 0 50 100150
100
50
0
50
100
150
Real Axis
Imag
Axi
s
polizeri FdT di tip (3 modi)
two modes clamped joint and tip output three modes
24

Frequency responses
101
100
101
102
103
100
50
0
50
100
Frequency (rad/sec)
Mag
nitu
de (
dB)
101
100
101
102
103
90
180
0
Frequency (rad/sec)
Pha
se (
deg)
101
100
101
102
103
100
50
0
50
100
Frequency (rad/sec)
Mag
nitu
de (
dB)
101
100
101
102
103
360
720
0
Frequency (rad/sec)
Pha
se (
deg)
clamped joint three modes tip
25

Useful controloriented remarks
• in polezero patterns of Pc(s), zeros precede and alternate with poles on theimaginary axis ⇒ passivity property• zero patterns of Pt(s) are symmetric w.r.t. the imaginary axis⇒ nonminimum
phase property ⇒ no direct system inversion is feasible• while moving the output along the link (Px(s)), zeros migrate along imaginary
axis and several phenomema occurr:
– total polezero cancellation when pointing at CoM (vibrations unobservable
from rigid motion variable θ)
– for a particular x∗ ∈ (0, �), all zeros vanishes together at infinity (Px∗(s)has maximum relative degree equal to 2(ne + 1))
– beyond x∗ (e.g., for x = �), all pairs of zeros reappear in Re+/Re−
26

• modal damping destroys perfect symmetry in zeros and poles (system is anywayasymptotically stable), but not the nonminimum phase property of Pt(s)
• from the Bode plots, it follows that classical controller synthesis in the frequencydomain is harder for the tip output
– multiple crossing of 0dB axis of Pt(jω) —especially for high control gain– increasing phase lag when adding modes
27

Dynamic modeling of robots with multiple flexible links
• a convenient kinematic description should be used, both for rigid body motionand flexible deformation
• recursive procedures can be set up for open chains with flexible links, similarlyto the rigid case
• differential kinematic relationships are needed for computing kinetic and potential energy, within a Lagrangian approach
• modeling results from the onelink case can be embedded (with caution) in thedescription of each flexible link of the robot
• to limit complexity, we sketch here only the planar case (with gravity) of robotswith N flexible links undergoing small bending deformations
28

Kinematics
θ1
θ2
X0
X1
Y1
Y0w1(x1)
X2 = X3
Y2 = Y3
Y1
X1
X2
Y2
w2(x2 )
• link i: rigid motion by clamped angle θi(t); lateral bending wi(xi, t), xi ∈[0, �i]• position vectors and (rigid/flexible) rotation matrices (w′ie =
∂wi∂xi
∣∣∣xi=�i
)
ipi(xi) =
[xi
wi(xi)
]iri+1 =
ipi(�i) Ai =
[cos θi − sin θisin θi cos θi
]Ei =
[1 −w′iew′ie 1
]
29

• recursive equations for absolute quantities in (X̂0, Ŷ0)
pi = ri + Wiipi ri+1 = ri + Wi
iri+1 Wi = Wi−1Ei−1Ai
• differential kinematics– absolute angular velocity of frame (Xi, Yi)
α̇i =i∑
j=1
θ̇j +i−1∑k=1
ẇ′ke
– absolute linear velocity of a point on link i
ṗi = ṙi + Ẇiipi + Wi
iṗi
where iṗi = [0 ẇi(xi) ]T (link extension is neglected)
30

Kinetic energy
T =N∑
i=1
Thi +N∑
i=1
T�i + Tp
• hub i
Thi =1
2mhiṙ
Ti ṙi +
1
2Jhiα̇
2i
• link i
T�i =1
2
∫ �i0
ρi(xi)ṗi(xi)T ṗi(xi)dxi
• payload
Tp =1
2mpṙ
TN+1ṙN+1 +
1
2Jp(α̇N + ẇ
′Ne)
2
31

Potential energy
U =N∑
i=1
Uei +N∑
i=1
Ughi +N∑
i=1
Ug�i + Ugp
• elastic energy of link i
Uei =1
2
∫ �i0
(EI)i(xi)
(d2wi(xi)
dx2i
)2dxi
• gravitational energy of hub i and of link i
Ughi = −mhigT0 ri Ug�i = −gT0∫ �i0
ρi(xi)pi(xi)dxi
• gravitational energy of payloadUgp = −mpgT0 rN+1
where g0 is the gravity acceleration vector
32

EulerLagrange equations
• introduce any finitedimensional discretization for wi(xi, t)
wi(xi, t) =nei∑j=1
ϕij(xi)δij(t) i = 1, . . . , N
• Lagrangian is in terms of N + M generalized coordinates, M = ∑Ni=1nei,L = T − U = L
({θi(t), δij(t), θ̇i(t), δ̇ij(t)}
)and satifies to
d
dt
(∂L
∂θ̇i
)− ∂L
∂θi= τi i = 1, . . . , N
d
dt
(∂L
∂δ̇ij
)− ∂L
∂δij= 0 j = 1, . . . , nei i = 1, . . . , N
being τi the torque delivered by the actuator at joint i
33

• the general dynamic model of flexible robots (including modal damping) is then[Mθθ(θ, δ) Mθδ(θ, δ)
MTθδ(θ, δ) Mδδ(θ, δ)
] [θ̈
δ̈
]+
[cθ(θ, δ, θ̇, δ̇)
cδ(θ, δ, θ̇, δ̇)
]+
[gθ(θ, δ)
gδ(θ, δ)
]+
[0
Dδ̇ + Kδ
]=
[τ
0
]
with blocks of suitable dimensions (e.g., Mθδ in the inertia matrix is (N×M)),or in the more compact form
M(q)q̈ + c(q, q̇) + g(q) +
[0
Dδ̇ + Kδ
]=
[τ
0
]
with q := (θ, δ) ∈ IRN+M
• vector of centrifugal/Coriolis terms can be factorized using Christoffel symbols
c(q, q̇) = C(q, q̇)q̇ =
[Cθθ(q, q̇) Cθδ(q, q̇)
Cδθ(q, q̇) Cδδ(q, q̇)
] [θ̇
δ̇
]
34

Model properties
• as usual, matrix Ṁ−2C is skewsymmetric — also blockwise, e.g. Ṁδδ−2Cδδ• spatial dependence in the kinetic and potential energy of the links can be resolved
introducing a set of dynamic coefficients so that (De Luca, Siciliano, 1991)
Y (θ, δ, θ̇, δ̇, θ̈, δ̈)a =
[τ
0
]
where constant vector a summarizes the mechanical (rigid + flexible) properties
of the links and can be computed offline (or identified experimentally)
• choice of assumed modes —basis functions ϕij(xi) for bending deformation− admissible functions satisfy only geometric b.c.’s− comparison functions (FE method, RitzKantorovich) satisfy also natural b.c.’s− orthonormal eigenfunctions (links modeled as EulerBernoulli beams) leads to
simplifications in inertia submatrix Mδδ (block diagonal, constant)
35

• a common approximation evaluates total kinetic energy in the undeformed armconfiguration δ = 0
⇒ M = M(θ) and thus c = c(θ, θ̇, δ̇)⇒ cδ loses quadratic dependence on δ̇
• moreover, if Mδδ is constant⇒ cθ loses quadratic dependence on δ̇⇒ cδ is a quadratic function of θ̇ only
• if also Mθδ is constant⇒ cδ ≡ 0⇒ cθ is a quadratic function of θ̇ only
• finally, small deformation of each link implies gδ = gδ(θ)
36

Control problems
• regulation to a constant equilibrium configuration (θ, δ, θ̇, δ̇) = (θd, δd,0,0)– only the desired joint position θd is assigned, while δd has to be determined
– θd may come from the kinetostatic inversion of a desired cartesian pose xd,
but no closedform solution exists (see De Luca, Panzieri, 1994)
– direct kinematics of FL robots is in fact a complete function of rigid and
flexible variables: x = kin(θ, δ)
• tracking of a joint trajectory θd(t) —the easy case• tracking of a endeffector trajectory xd(t) —the difficult case• resttorest motion in given time T (a trajectory planning problem in first place)
∗ in tracking problems, controllers try to stiffen the flexible arm at a point in away or the other
37

Sensing requirements
• full state feedback requires sensing of joint/motor variables (θ, θ̇), deflectionsδ, and deflection rates δ̇ (no direct sensor available)
• at least encoder + tachometer on the motor axis (sometimes is enough . . . )• a range of sensors for measuring δ (or deformation related quantities), each
with pros and cons: strain gauges, accelerometers, optical sensors, video camera
(onboard or fixed in workspace), piezoelectric actuating/sensing devices, . . .
38

• problems with camera: frame rate, field of view/accuracy
39

Regulation with joint PD + feedforward
• for regulation tasks, consider the control law
τ = KP (θd − θ)−KDθ̇ + gθ(θd, δd)
with symmetric (diagonal) KP > 0, KD > 0, and the associated link deflection
δd := −K−1gδ(θd)
Theorem (De Luca, Siciliano, 1993) If
λmin
([KP 0
0 K
])> α
then the closedloop equilibrium state (θd, δd,0,0) is asymptotically stable
40

Remarks
• Lyapunovbased proof similar to the joint elastic case (not repeated here)• determination of α
– in view of small deformation
Ue =1
2δTKδ ≤ Ue,max ⇒ ‖δ‖ ≤
√2Ue,maxλmax(K)
– bound on the gradient of the gravitational term∥∥∥∥∥∂g∂q∥∥∥∥∥ ≤ α0 + α1‖δ‖ ≤ α0 + α1
√2Ue,maxλmax(K)
=: α
• in absence of modal damping D = 0, special care in LaSalle analysis• for tip regulation, compute θd by solving via iterative techniques
kin(θ,−K−1gδ(θ)
)= xd
41

Numerical results
• a twolink flexible arm with two bending modes for each link with f11 = 1.4,f12 = 5.1, f21 = 5.2, f22 = 32.4 [Hz]
• pointtopoint motion: θ(0) = (−90◦,0)→ θd = (−45◦,0)
100
50
0
50
0 1 2 3 4
joint angles
sec
deg
10
0
10
20
0 1 2 3 4
joint torques
sec
Nm
0.2
0.1
0
0.1
0 1 2 3 4
1st link deflections
sec
m
0.01
0.005
0
0.005
0 1 2 3 4
2nd link deflections
sec
m
42

Joint trajectory tracking
• given a desired θd(t) ∈ C2, assuming that the state is measurable and thedynamic model of FL robot is available, we proceed by system inversion from
the joint position output θ
• a nonlinear static state feedback is obtained that decouples and linearizes theinputoutput behavior, leaving an unobservable internal (nonlinear) dynamics
• exponential stabilization of the output tracking error is performed on the linearside of the problem
• some stability/boundedness of the internal system dynamics should be enforced
• original results in (De Luca, Siciliano, 1993b)
43

System inversion
• from second set of M equations in the dynamic model, solve (globally) for δ̈
δ̈ = −M−1δδ (cδ + gδ + Kδ + Dδ̇ + MTθδθ̈)
and plug in first set of N equations ⇒ effects of flexible dynamics onto rigiddynamics(Mθθ −MθδM−1δδ M
Tθδ
)θ̈+cθ+gθ−MθδM−1δδ
(cδ + gδ + Kδ + Dδ̇
)= τ
• Matrix Mθθ −MθδM−1δδ MTθδ has always full rank, since[Mθθ Mθδ
MTθδ Mδδ
] [I 0
−M−1δδ MTθδ I
]=
[Mθθ −MθδM−1δδ MTθδ Mθδ
0 Mδδ
]
44

• system output θ has uniform vector relative degree {2,2, . . . ,2} (θ̈ depends onτ in a nonsingular way)
• define the nonlinear control lawτ =
(Mθθ −MθδM−1δδ M
Tθδ
)a+ cθ + gθ−MθδM−1δδ (cδ + gδ +Kδ +Dδ̇)
in which only the inversion inertia block M−1δδ is required• the closedloop system is
θ̈ = a
δ̈ = −M−1δδ(MTθδa + cδ + gδ + Dδ̇ + Kδ
)
• for stabilizing the output tracking error e = θd − θ, choosea = θ̈d + KD(θ̇d − θ̇) + KP (θd − θ)
with (diagonal) KP > 0, KD > 0
45

Analysis of internal dynamics
• zero dynamics, when output θ(t) ≡ 0 (or constant):
δ̈ = −M−1δδ(cδ + gδ + Dδ̇ + Kδ
)asymptotically stable (via Lyapunov argument)⇒ whole closedloop system too• clamped dynamics, when output θ(t) ≡ θd(t):
δ̈ = −A2(t)δ̇ −A1(t)δ + fδ(t)where (in the case M independent of δ)
fδ(t) = −M−1δδ (θd)(MTθδ(θd)θ̈d + cδ(θd, θ̇d) + gδ(θd))A1(t) = M
−1δδ (θd)K
A2(t) = M−1δδ (θd)D
all timevarying functions are bounded ⇒ closedloop stability is ensured46

Remarks on joint trajectory tracking
• the inputoutput linearization result is the nonlinear/MIMO counterpart of thetransfer function τ → θc with minimum phase zeros (stable zero dynamics)
• the more ‘rigid’ is the tracking of a desired joint trajectory, the less vibrationenergy is taken out from (or the more is injected into) the rest of flexible arm!!
• a nominal feedforward is computed by forward integration of flexible dynamics
δ̈ = −M−1δδ (θd, δ)(cδ(θd, δ, θ̇d, δ̇) + gδ(θd) + Dδ̇ + Kδ + MTθδ(θd, δ)θ̈d)
from δ(0) = δ0, δ̇(0) = δ̇0 ⇒ nominal (and bounded) evolution δd(t), δ̇d(t)
• substitution of (θd(t), δd(t), θ̇d(t), δ̇d(t)) in the expression of the control law(w/out feedback) yields τd(t) and the simple local tracking controller
τ = τd(t) + KP (θd − θ) + KD(θ̇d − θ̇)
47

Endeffector trajectory tracking
• accurate endeffector trajectory tracking is the toughest control problem forflexible robots
• direct extension of inversion strategies to endeffector output ⇒ closedloopinstabilities
– linear (singlelink) case: nonminimum phase tip transfer function
– nonlinear (multilink) case: unstable zero dynamics for endeffector motion
• main ideas proposed in the literature:– resort to suitable feedforward strategy (noncausal solutions)
– use feedback, but avoid cancellation (causal solutions)
∗ selection of suitable endeffector trajectories that induce smaller arm deflectionsis of interest in any case
48

Inversion in frequency domain
• idea: desired motion trajectory as being part of a periodic profile⇒ use Fouriertransforms (Bayo, 1987)
• singlelink flexible arm (with generic variables)[mθθ m
Tδθ
mδθ Mδδ
] [θ̈
δ̈
]+
[0 0
0 D
] [θ̇
δ̇
]+
[0 0
0 K
] [θ
δ
]=
[τ
0
]
• tip position output
y(t) = [ 1 cTe ]
[θ
δ
]
• rewrite in terms of (y, δ)[mθθ m
Tδθ −mθθcTe
mδθ Mδδ −mδθcTe
] [ÿ
δ̈
]+
[0 0
0 D
] [ẏ
δ̇
]+
[0 0
0 K
] [y
δ
]=
[τ
0
]
49

• take bilateral Fourier transforms
Ÿ (ω) =∫ ∞−∞
exp(jωt)ÿ(t)dt ∆̈(ω) =∫ ∞−∞
exp(jωt)δ̈(t)dt
T (ω) =∫ ∞−∞
exp(jωt)τ(t)dt
and obtainmθθ mTδθ −mθθcTemδθ Mδδ −mδθcTe +
1
jωD − 1
ω2K
[Ÿ (ω)
∆̈(ω)
]=
[T (ω)
0
]
• solve for accelerations[Ÿ (ω)
∆̈(ω)
]=
[g11(ω) g
T12(ω)
g21(ω) G22(ω)
] [T (ω)
0
]
50

• torque is obtained through inversion (in the frequency domain)
T (ω) =1
g11(ω)Ÿ (ω) = r(ω)Ÿ (ω)
• for a given zeromean ÿd(t), with ÿd(t) = 0 for t ≤ −T/2 and t ≥ T/2, thiscan be embedded into a periodic signal from (−∞,+∞)
• ÿd(t)→ Ÿd(ω)→ Td(ω)→ τd(t) from finite inverse Fourier transform
τd(t) =∫ ∞−∞
r(t− τ)ÿd(τ)dτ =∫ T/2−T/2
r(t− τ)ÿd(τ)dτ
expanding beyond [−T/2, T/2] (noncausal inverse system)
51

Remarks
• outside the given interval T of output motion, the computed input torque has a– precharging action, bringing internal flexible state from rest to a suitable
initial state at t = −T/2– discharging action, bringing internal flexible state from the final state at
t = T/2 to rest
• obtained initial condition is the unique state from which inversion control doeslead to bounded internal evolution for the desired endeffector output trajectory!
• truncations (in time or frequency domain) inherent to actual computations (FFT)
• can be extended to the nonlinear (multilink flexible) setting, by repeated linearapproximations along nominal trajectory (starting from rigid body motion)
52

Extension to nonlinear case: Endeffector bangbang trajectory
0 0.5 1 1.5 2 2.5 3 3.5 4100
80
60
40
20
0
20
40
60
80
100
sec
grad
i / s
ec ^
2
0 0.5 1 1.5 2 2.5 3 3.5 44
3
2
1
0
1
2
3
sec
Nm
1 0.8 0.6 0.4 0.2 0 0.2 0.4 0.6 0.8 1
0.2
0
0.2
0.4
0.6
0.8
1
1.2
x (m)
y (m
)
acceleration profiles command torques FLEXARM motion
53

Extension to nonlinear case: Endeffector sinusoidal trajectory
0 0.5 1 1.5 2 2.5 3 3.5 4150
100
50
0
50
100
150
sec
grad
i / s
ec ^
2
0 0.5 1 1.5 2 2.5 3 3.5 41.5
1
0.5
0
0.5
1
1.5
2
sec
Nm
0.1 0 0.1 0.2 0.3 0.4 0.5 0.6 0.7 0.8 0.9
0.2
0.1
0
0.1
0.2
0.3
0.4
0.5
x (m)
y (m
)
acceleration profiles command torques FLEXARM motion
54

Regulation theory
• endeffector trajectory tracking in robots with flexible links is an instance ofasymptotic output tracking with internal state stability (regulation problem)
• wellestablished technique in linear case and, by now, also in nonlinear case
• idea: compute the (bounded!) state trajectory associated to the desired outputtrajectory (generated by an autonomous antistable system, the exosystem)
• in linear case, write statespace equations (with x = (q, q̇)) for the flexible arm
ẋ = Ax + Bτ e = y − ydand for the generator of desired output
ẇ = Sw yd = −Qw
55

• when (A,B) is stabilizable, the problem has a solution if and only if the followingregulator equations are solvable in Π and Γ
ΠS = AΠ + BΓ CΠ + Q = 0
• a state feedback + feedforward controller is then
τ = F (x−Πw) + Γw
with feedback matrix F such that (A + BF ) is Hurwitz
• the computed Πw is the desired state trajectory; xd(0) = Πw(0) is the uniqueinitial state from which inversion control does lead to bounded internal evolution!
• control solutions with dynamic output feedback are also available
56

Numerical results for sinusoidal endeffector trajectory
0 1 2 3 4 5 6 7 8 9 10100
80
60
40
20
0
20
40
60
80
100autovalori in 10 (quattro) e 10 (quattro)
sec
usci
ta ti
p (d
eg)
0 1 2 3 4 5 6 7 8 9 1010
0
10
20
30
40
50
60
70autovalori in 10 (quattro) e 10 (quattro)
sec
erro
re a
l tip
(de
g)tip output tip error
57

0 1 2 3 4 5 6 7 8 9 100.03
0.02
0.01
0
0.01
0.02
0.03autovalori in 10 (quattro) e 10 (quattro)
sec
varia
bili
defo
rmaz
ione
del
ta (
m)
0 1 2 3 4 5 6 7 8 9 101.5
1
0.5
0
0.5
1
1.5autovalori in 10 (quattro) e 10 (quattro)
sec
cont
rollo
(N
m)
deformation variables torque input
58

0 1 2 3 4 5 6 7 8 9 10100
80
60
40
20
0
20
40
60
80
100autovalori in 10 (quattro) e 10 (quattro)
sec
usci
ta a
l giu
nto
(deg
)
clamped joint angle (and desired tip output)
59

Resttorest motion
• task: execute a slew motion with a FL robot arm between two undeformedconfigurations in given time
• problem: fast transfers induce residual oscillations extending the actual taskcompletion time
• strategy: design a suitable output and plan output trajectories (and associatedtorque profiles) inducing complete absence of vibrations at the desired final time
• solution: output with maximum relative degree (no zeros); closedform algorithmin the linear case; direct extension to MIMO nonlinear case (DFL or flat output,
no zero dynamics)
60

Timebased algorithm for a single flexible link
• choose a parametric output y (unknown ci’s) (De Luca, Di Giovanni, 2001)
y = θ +ne∑i=1
ciδi = θ + cT δ
• impose τ independence of (even) output derivatives
ÿ = (1
J+
ne∑i=1
ciφ′i(0))τ −
ne∑i=1
ciω2i δi ⇒
∑ciφ′i(0) = −1/J
y[4] =:d4y
dt4= −
ne∑i=1
ciω2i φ′i(0) τ +
ne∑i=1
ciω4i δi ⇒
∑ciω
2i φ′i(0) = 0
and so on, until a set of ne equations are generated (torque τ appears in the
2(ne + 1)th output derivative)
61

• solve for the coefficients c = (c1, . . . , cne)
V · diag{φ′1(0), . . . , φ′ne(0)} c = [−1/J 0 . . . 0 ]T
with Vandermonde matrix V generated by (ω21, . . . , ω2ne)
• nominal torque τd(t) computed by inversion on highest derivative imposing
y[2(ne+1)] = y[2(ne+1)]d
for a suitably planned output trajectory yd(t), t ∈ [0, T ] (given trasfer time)• for the output trajectory yd(t), solve a simple interpolation problem
yd(0) = θi yd(T ) = θfdiyddti
(0) =diyddti
(T ) = 0 i = 1, . . . ,2ne + 1
e.g., a polynomial of degree 4ne + 3 will be sufficient
62

Laplacebased algorithm (only in the linear case)
• impose that the transfer function has no zerosy(s)
τ(s)=
1
Js2+
ne∑i=1
ciφ′i(0)
s2 + ω2i
∆=
K
s2∏ne
i=1(s2 + ω2i )
• partial fractions expansion yields closedform expressions
K =1
J
ne∏i=1
ω2i ci = −1
Jφ′i(0)
ne∏j=1j �=i
ω2j
ω2j − ω2i(i = 1, . . . , ne)
• set y = yd and invert in the transformed domain (then back to time → τd(t))
τd(s) =J∏ne
i=1 ω2i
s2 ne∏
i=1
(s2 + ω2i )
yd(s)
63

Remarks
• method applies to any linear (controllable) model of a singlelink flexible arm• output structure for modal damping (De Luca, Caiano, Del Vescovo, 2003)
y = θ +ne∑i=1
ciδi + γθ̇ +ne∑i=1
diδ̇i
• design output is (in the limit) a specific point x∗ on the physical beam: for agiven ne, ci = φi(x
∗ne)/x
∗ne while limne→∞ x
∗ne = x
∗
• for improved torque/time performance, modified method generates smoothedbangbang/bangcoastbang torque profiles, with polynomial interpolating phases
• trajectory planning (feedforward) combined with feedback control
τ = τd(t) + KP (θc,d(t)− θc) + KD(θ̇c,d(t)− θ̇c)with clamped joint reference θc,d(t) computed from the algorithm
64

Numerical results
• ne = 3 modes with f1 = 4.05, f2 = 12.34, f3 = 22.87 [Hz]• θf − θi = 90◦ in T = 2 s• 19th degree polynomial (continuous torque derivative)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
10
20
30
40
50
60
70
80
90
100
s
deg
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 23
2
1
0
1
2
3
s
Nm
output trajectory resttorest torque
65

0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20
10
20
30
40
50
60
70
80
90
100
s
deg
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20.025
0.02
0.015
0.01
0.005
0
0.005
0.01
0.015
0.02
0.025
s0.6 0.4 0.2 0 0.2 0.4 0.6
0.6
0.4
0.2
0
0.2
0.4
0.6
m
m
clamped (—), tip angle ( ) deformation variables arm motion
66

Experiment on DMA single flexible link
67

Extension to nonlinear case: Resttorest motion
• timebased algorithm for twolink with flexible forearm (De Luca, Di Giovanni,2001b)
0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 20.02
0.015
0.01
0.005
0
0.005
0.01
0.015
0.02
0.025delta
s0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2
8
6
4
2
0
2
4
6
8u1, u2
s
Nm
1 0.5 0 0.5 11
0.8
0.6
0.4
0.2
0
0.2
0.4
0.6
0.8
1
m
m
first deformation mode command torques FLEXARM motion
68

Conclusions
• extra effort in dynamic modeling pays off– modelbased controllers for accurate trajectory tracking
– proof of stability for modelinpedendent regulation controllers
• conventional control strategies tend to suppress vibrations wherever they arise– outcome of the analysis: controlled system should be brought to a vibratory
behavior compatible with the given output task
• EJ robots are similar to FL robots in mechanical modeling, but intrinsicallydifferent from the control point of view
69

What has been left out . . .
• singular perturbation modeling and control (for joint or link stiffness K →∞),including corrective and invariant manifold controllers
• iterative learning control that yields same accuracy (for all types of tasks) withoutusing a dynamic model but assuming repetitive tasks
• model uncertainties, disturbances, . . .
70

References
(Bayo, 1987) “A finiteelement approach to control the endpoint motion of a singlelink flexible
robot,” JRS, 4, 63–75
(De Luca, Caiano, Del Vescovo, 2003) “Experiments on resttorest motion of a flexible arm,” in
Experimental Robotics VIII (Siciliano, Dario Eds), STAR 5, Springer, 338–349
(De Luca, Di Giovanni, 2001) “Resttorest motion of a onelink flexible forearm,” IEEE/ASME
AIM 01, 923–928
(De Luca, Di Giovanni, 2001b) “Resttorest motion of a twolink robot with a flexible forearm,”
IEEE/ASME AIM 01, 929–935
(De Luca, Lanari, Ulivi, 1991) “Endeffector trajectory tracking in flexible arms: Comparison of
approaches based on regulation theory,” in Advanced Robot Control (Canudas de Wit Ed), LNCIS
162, Springer, 190–206
(De Luca, Panzieri, 1994) “An iterative scheme for learning gravity compensation in flexible robot
arms,” Automatica, 30(6), 993–1002
71

(De Luca, Panzieri, Ulivi, 1998) “Stable inversion control for flexible link manipulators,” IEEE
ICRA, 799–805
(De Luca, Siciliano, 1991) “Closedform dynamic model of planar multilink lightweight robots,”
IEEE SMC, 21(4), 826–839
(De Luca, Siciliano, 1993) “Regulation of flexible arms under gravity,” IEEE TRA, 9(4), 463–467
(De Luca, Siciliano, 1993b) “Inversionbased nonlinear control of robot arms with flexible links,”
AIAA JGCD, 16(6), 1169–1176
(De Luca, Siciliano, 1996) “Flexible links,” in Theory of Robot Control (Canudas de Wit, Siciliano,
Bastin Eds), Springer, 219–261
72