Controllo ibrido di forza/velocità

Corso di Robotica 2
Controllo ibrido di forza/velocità
Prof. Alessandro De Luca
Robotica 2
A. De Luca
A.A. 2008-2009
Vincoli naturali e artificiali


il contatto fra robot e ambiente (rigido e privo di attrito = puramente
geometrico) vincola in modo “naturale” il moto dell’E-E
in condizioni ideali (robot e ambiente perfettamente rigidi, assenza di
attrito), nello spazio del compito (task space) si possono individuare due
set di direzioni generalizzate selezionate in modo che





è possibile il moto dell’E-E nel primo set di k direzioni
(lungo cui l’ambiente non può opporre alcuna reazione)
esistono reazioni vincolari nel secondo set di 6-k direzioni
(lungo cui l’ambiente impedisce il moto dell’E-E)
vincoli “naturali”
su forza e moto
imposti dal compito
queste direzioni sono “ortogonali” tra loro (in realtà complementari, ossia
complessivamente coprono tutto lo spazio cartesiano) e definiscono un
opportuno task frame SRt (tipicamente associato poi all’E-E del robot)
per compiti di interazione generali, posizione e orientamento del task
frame dipendono dal tempo
le specifiche di esecuzione del compito impongono poi vincoli “artificiali” in
termini di valori desiderati (e imposti dal controllo) per le velocità, nelle k
direzioni ammissibili di moto, e per le forze di contatto, nelle 6-k direzioni
ammissibili di forza
Robotica 2
A. De Luca
A.A. 2008-2009
2
Task frame e vincoli
z
compito: far slittare il cubo
lungo il binario
SRt
y
x
v = velocità lineare
ω = velocità angolare
F = forza
M = momento
Robotica 2
A. De Luca
- esempio 1
6-k=4
k=2
vincoli naturali (geometrici)
vy = vz = 0
6-k=4
ωx = ωz = 0
k=2
Fx = My = 0
vincoli artificiali
(imposti dal controllore)
Fy = Fy,des = 0
Mx = Mx,des = 0, Mz = Mz,des = 0
Fz = Fz,des
ωy = ωy,des = 0
vx = vx,des
A.A. 2008-2009
3
Selezione delle direzioni
z
#1
%0
%
# v & %0
%" ( = % 0
$ ' %
%0
y
%0
qui, costanti $
SRt
x
Robotica 2
A. De Luca
"0
$1
$
" F % $0
$M ' = $ 0
# & $
$0
$0
#
- esempio 1
0
0
1
0
0
0&
0(
(
0( # vx &
# vx &
)% ( = T )% (
(
0 $" y '
$" y '
(
1(
0 ('
0
0
0
1
0
0%
0 ' "!Fy %
" Fy %
T⋅Y = 0
T
' $ '
$F '
0 ' Fz
z
$
'
$
'
(
=
Y
(
reazioni vincolari non
'
0 $Mx '
Mx ' possono compiere lavoro
$
' $ '
$M '
0 ' # Mz &
# z&
0 0 1 '&
A.A. 2008-2009
4
Task frame e vincoli
z SRt(α) z0
SR0
compito: girare la manovella
(manicotto folle)
y0
x
α
y
S0
Robotica 2
A. De Luca
Rz(α)
St(α)
- esempio 2
x0
vincoli naturali
vx = vz = 0
ωx = ωy = 0
Fy = M z = 0
vincoli artificiali
Fx = Fx,des = 0, Fz = Fz,des = 0
Mx = Mx,des = 0, My = My,des = 0
vy = vy,des
ωz = ωz,des = 0
A.A. 2008-2009
5
Selezione delle direzioni
z SRt(α) z0
x
y
SR0
α
"1
$0
$!
0
T
" F % " R (( )
0 %$ 0
$0 '=$
'$ 0
T
M
0
R
(
(
)
# & #
&$
$0
$0
#
Robotica 2
A. De Luca
- esempio 2
#0
%1
y0
%
0
T
# v & #R () )
0 &% 0
=
%0 ( %
(
R T () ) '% 0
x0 $ " ' $ 0
%
%0
%0
$
0
0
1
0
0
0
0
0
1
0
0%
0 ' " Fx %
" Fx %
' $ '
$F '
0 ' Fz
$ ' = Y(( ) ) $ z '
)
0 ' $Mx '
$ Mx '
' $ '
$M '
1 ' #My &
# y&
0 0 0 '&
0&
0(
(
0( # vy &
#vy &
*
=
T(
)
)
*
%" (
0 ( %$ " z ('
$ z'
(
0(
1 ('
TT(α)⋅Y(α) = 0
A.A. 2008-2009
6
Task frame e vincoli
- esempio 3
compito: avvitare una vite
in un foro filettato
x
y
p = passo della vite
SRt
z
la vite si muove sia lungo che
intorno l’asse z, ma non in
modo indipendente! (1 dof)
analogamente, Fz ed Mz non
possono essere indipendenti
Robotica 2
A. De Luca
vincoli naturali (parziali…)
vx = vy = 0
ωx = ωy = 0
vincoli artificiali (abbondanti…)
Fx = Fx,des = 0, Fy = Fy,des = 0
Mx = Mx,des = 0, My = My,des = 0
vz = vz,des , ωz = ωz,des = (2π/p)⋅vz,des
Fz = Fz,des , Mz = Mz,des (Fz,des )
le direzioni di forza sono ortogonali a quelle di moto!
A.A. 2008-2009
7
Selezione delle direzioni
- esempio 3
#v&
%" ( = 0 0 1 0 0
$ '
(
y
!
p
SRt
2)
p
)
T
* vz = T * vz
k=1
Y: tale che TT⋅Y = 0
x
Fz = "
2#
Mz
p
6-k=5
z
le colonne di T ed Y
non coincidono necessariamente
con direzioni cartesiane
(colonne della matrice identità)
➮ direzioni generalizzate
Robotica 2
A. De Luca
"F %
$M '
# &
"1
$0
$0
=$
$0
$0
$
#0
0 0 0 0 % " Fx %
" Fx %
$ '
1 0! 0 0 ' $ F '
Fy '
2) ' $ y '
$
0 0 0 ( p
' * $M ' = Y * $M '
x
x
0 1 0 0 ' $ '
$
'
M
M
$ y'
0 0 1 0 ' $ y'
$M '
' $ '
0 0 0 1 & # Mz &
# z&
A.A. 2008-2009
8
!
Terne di interesse
- esempio 4
moto planare di un robot 2R in contatto con superficie (M=2)
yt
y0
x0
ye
xt
superficie
ambientale
xe
 terna SRt di task, per la definizione indipendente delle specifiche ibride
(qui: tvx,des [k=1] e tFy,des [M-k=1]) e il calcolo degli errori per il controllo
 terna SRe del sensore (qui = SR2) dove si misura la forza eF = (eFx,eFy)
 terna SR0 di base nella quale è espressa la velocità dell’organo terminale
(qui, 0v = (0vx,0vy) di O2), ottenuta dal calcolo dello Jacobiano e dalle
velocità di giunto
servirà poi esprimere (ruotare) tutte le grandezze
nella stessa terna di riferimento: quella di task!
Robotica 2
A. De Luca
A.A. 2008-2009
9
Formalismo per il controllo ibrido
descrizione
del contatto
robot-ambiente:
definisce il
“task frame”
!
#v&
%" ( = T(s) ) s˙
$ '
le forze di contatto non
compiono lavoro sugli
spostamenti dell’E-E
s " IRk
parametrizza
il moto libero dell’E-E
"F % !
M$k
"
#
IR
=
Y(s)
(
)
$M '
# &
parametrizza
le forze di contatto
nei primi tre esempi precedenti
!
e in !
generale M=6
dinamica
robot
Robotica 2
A. De Luca
!
+
TT(s)⋅Y(s) = 0
le direzioni generalizzate
del task frame dipendono
in generale da s
(dalla posa dell’E-E
rispetto all’ambiente)
"F %
B(q)˙q˙ + S(q, q˙ )q˙ + g(q) = u + J (q)$ '
#M &
T
cinematica # v &
%" ( = J(q)q˙
robot
$ '
A.A. 2008-2009
10
Controllo ibrido di forza/velocità

obiettivo del controllo: imporre andamenti desiderati ai
parametri di moto s e a quelli di forza λ
s(t) → sd(t), λ(t) → λd(t)

legge di controllo progettata in due passi
1. disaccoppiamento e linearizzazione via feedback nel task frame
modello ad
anello chiuso
# s˙˙ & # as &
%" ( = %a (
$ ' $ "'
2. sintesi (lineare) di as e aλ per imporre la dinamica desiderata agli
errori es = sd − s, ed eλ = λd − λ

assunzioni: N = M (= 6,! di solito), J(q) non singolare
(+ TT⋅Y = 0)
Robotica 2
A. De Luca
A.A. 2008-2009
11
Feedback linearizzazione nel task space
˙˙ = J"1 T # ˙s˙ + T˙ # s˙ " J˙ # q˙
q
(
)
"F %
T
˙
˙
˙
˙
B(q)q + S(q, q)q + g(q) = u + J (q)$ ' = u + J T (q)Y(s) ( )
M&
#
!
!
!
!
˙˙ + J˙ " q˙ = T " ˙s˙ + T˙ " s˙
J" q
J(q) " q˙ = T(s) " s˙
$ ˙s˙ '
B(q)J (q)T(s) "J (q)Y(s) & ) + B(q)J"1 (q) T˙ (s)s˙ " ˙J(q)q˙ + S(q, q˙ )q˙ + g(q) = u
%# (
(
"1
T
(
)
)
!
matrice N x N
non singolare
sotto le ipotesi
fatte
!
Robotica 2
A. De Luca
controllo
$ as '
"1 ˙
˙
u = BJ T "J Y & ) + BJ Ts˙ " Jq˙ + S * q˙ + g disaccoppiante
% a# (
e linearizzante
(
"1
T
(
)
# s˙˙ & # as &
%" ( = %a (
$ ' $ "'
k
)
s ha “grado relativo” 2
M − k λ ha “grado relativo” 0
A.A. 2008-2009
12
Stabilizzazione con as e aλ
al solito, sufficienti tecniche di controllo lineare (sui singoli canali ingressouscita scalari)
as = ˙s˙d + K D (s˙ d " s˙ ) + K P (sd " s)
˙˙s + K De˙ s + K Pes = 0
e
es = sd " s # 0
!
a" = " d + K I % ( " d # ")d$
!
"˙# + K I "# = 0
"# =
!
!
N.B. qui basterebbe a" = "d
ma non avremmo un “errore”
di forza (scarsa robustezza)
& (#d $ !#)d% ' 0
.
servono “misure” di s, s e λ!
!
Robotica 2
A. De Luca
!
A.A. 2008-2009
13
Filtraggio misure di posizione e forza
.
.
s, s ricavate dalle misure di q e q, uguagliando le descrizioni di posa e
velocità dell’E-E dal “punto di vista del robot” (cinematica diretta e
differenziale) e dal “punto di vista dell’ambiente” (funzioni di s)
esempio
z0
r
L
"L cos s %
$
'
0
r = 0f(q) = $ L sin s '
$
'
y0
0
#
&
s=α
!
x0
s = ATAN2( 0 fy (q),0 fx (q))
J(q) " q˙ = T(s)
! " s˙
s˙ = T # (s)J(q) " q˙
di
! delle forze/momenti !
λ ricavato dalle misure
all’end-effector pseudoinverse
matrici “strette”
Robotica 2
A. De Luca
"F %
$M ' = Y(s) ( )
# &
#F &
#
" = Y (s)% (
$M '
a rango pieno,
ad es. (TTT)-1TT
(o anche pesate)
A.A. 2008-2009
14
!
Schema a blocchi complessivo
q, q˙
˙s˙ d + K Ds˙ d + K Ps d
+
as
+
_
_
+
λd
_
feedback
u
linearizzazione
!+
KI
nel task space
s + aλ
s, s˙
!
M-k anelli di forza
k anelli di moto
!
KD
KP
!
λ
s
s˙
q
robot
in contatto
ambiente
q˙
"F %
$M '
# &
!
filtraggio
misure
casi limite k=M (moto nello spazio libero): nessun controllo di forza, solo di moto
k=0 (E-E del robot “bloccato”):!nessun controllo di moto, solo di forza
Robotica 2
A. De Luca
A.A. 2008-2009
15
Fonti di incongruenze fra misure
1. presenza di attrito al contatto
 c’è una componente di forza nelle direzioni “libere” di velocità che
si oppone al moto (se attrito di Coulomb, dipendente anche dalla
forza normale applicata…)
2. cedevolezza nella struttura robotica e/o al contatto
 ci può essere uno spostamento nelle direzioni “vincolate” dalla
reazione ambientale
N.B. se
però la geometria dell’ambiente è nota con precisione, gli errori
causati da 1. e 2. sulle “misure” di s e λ sono filtrati
automaticamente dalla pseudo-inversione di T ed Y
3. incertezze sulla geometria dell’ambiente nel contatto
(da stimare real-time con sensori esterni: visione, ma anche forza!)
Robotica 2
A. De Luca
A.A. 2008-2009
16
Identificazione di un profilo incognito
si può identificare un profilo incognito della superficie
di contatto, a partire dalle misure di velocità e forza?
1.
normale = direzione della forza misurata
... ma la presenza di attrito provoca una
rotazione della forza rispetto alla normale
effettiva di un angolo γ (incognito)
2.
tangente = direzione della velocità misurata
... ma l’elasticità nella struttura robotica e/o al !
contatto possono indurre una componente di
velocità lungo la normale
3.
Ft = µ " Fn = tg # " Fn
vt
v
vn
metodo misto (fusione sensoriale) con RLS
a. tangente identificata in modo ricorsivo da
misure di posizione
b. angolo di attrito identificato in modo ricorsivo
dalla tangente trovata in a. e dalle misure di
forza
Robotica 2
A. De Luca
Fn
F
il robot è
mantenuto in moto
di contatto “in
qualche modo”…
A.A. 2008-2009
17
Identificazione e controllo ibrido
effettuati in modo simultaneo
Robotica 2
A. De Luca
robot MIMO-CRF (DIS, Laboratorio di Robotica, 1992)
A.A. 2008-2009
18
Tangente stimata da misure di posizione
[gradi]
(superficie circolare percorsa a velocità costante)
Robotica 2
A. De Luca
tempo [intervalli di campionamento]
A.A. 2008-2009
19
Tangente stimata da misure di forza
[gradi]
(stessa superficie circolare percorsa a velocità costante)
Robotica 2
A. De Luca
tempo [intervalli di campionamento]
A.A. 2008-2009
20
Differenza fra le tangenti stimate
[gradi]
differenze
dell’ordine
dei 7-8°...
ma qual è
quella “giusta”?
… meglio una
fusione
sensoriale!!
Robotica 2
A. De Luca
tempo [intervalli di campionamento]
A.A. 2008-2009
21
Profilo superficie riconosciuta
identificazione con metodo RLS (Recursive Least Squares),
fondendo sia misure di forza che di velocità
…era la
“pizza” di
un film
(raggio
= 17 cm)
[m]
Robotica 2
A. De Luca
[m]
A.A. 2008-2009
22
Forza normale
regolata a 20 N
durante il moto
[N]
i picchi
corrispondono
a scanalature
sulla superficie
Robotica 2
A. De Luca
tempo [intervalli di campionamento]
A.A. 2008-2009
23
Sbavatura robotizzata di parabrezza



parabrezza con spigoli vivi e
tolleranze dimensionali dovute alla
lavorazione e all’eccesso di
materiale collante (PVB) tra strati
vetrosi del parabrezza
il robot segue il percorso
prefissato, nonostante gli errori
sulla forma nominale, grazie alla
cedevolezza passiva nella testa di
sbavatura
la forza di contatto fra lama e
vetro può essere controllata, in
modo indipendente dal moto,
grazie a un cilindro pneumatico
nella testa di sbavatura
la testa di sbavatura è dotata di


Robotica 2
A. De Luca

due lame per tagliare il PVB
cella di carico per misurare la forza applicata
sistema di controllo
A.A. 2008-2009
24
Modello testa di sbavatura
fisico
matematico
per l’analisi della stabilità (con luogo delle radici) del controllo di forza in una singola
direzione in presenza di masse/elasticità multiple come in questo caso,
vedi Eppinger & Seering, IEEE CSM, 1987 (materiale didattico)
Robotica 2
A. De Luca
A.A. 2008-2009
25
Video di ricapitolazione
compliance control
impedance control
(controllo attivo di cedevolezza (controllo di impedenza)
senza sensore di forza)
force control
(controllo di forza come
anello esterno ad un
controllore di posizione)
robot COMAU
c/o Università di Napoli
Robotica 2
A. De Luca
hybrid force/position control
(controllo ibrido)
A.A. 2008-2009
26