Rob´ Ro b´otic oticaa e Au Auto toma ma¸¸c˜ao http://www.coep.ufrj.br/gscar
1/27
Cont Co ntro rolle Cin inem em´ ´ atico ati Fernando Lizarralde DEL-POLI/UFRJ e PEE-COPPE/UFRJ
Rio de Janeiro, 23 de novembro de 2009
Voltar
Cont Co ntro rolle Cin inem em´ ´ atico ati Considera-se Considera-se o problema problema de controle cinem´ cinem´atico atico de um manipulado manipuladorr rob´otico. otico.
2/27
Considera-se que a d a diinˆamica do manipulador pode ser desprezada ser desprezada.. Esta hip´otese otese ´e aceit´avel avel para manipuladores manipul adores que apresentam elevados eleva dos fatores de redu¸c˜ c˜ao ao nas engrenagen engre nagenss ou quando quand o baixas velocidad velo cidades es s˜ao ao utilizadas durante a realiza¸c˜ c˜ao ao de taref tarefas as..
Voltar
Considere o sistema dado pela cinem´atica diferencial:
v = J (θ) θ˙ ω 3/27
Ent˜ao, considerando que a maioria dos manipuladores geralmente possuem uma malha de controle de velocidade em n´ıvel de juntas:
u
+
e
τ
v K
PWM
Θ Robo
X Ja
X 1/S
−
para uma entrada u = θ˙d e um controle de alto ganho, i.e., K → ∞, tem-se que e → 0 e conseq¨uentemente u ≈ θ˙
Voltar
Portanto, o movimento do manipulador pode ser descrito por θ˙i = ui ,
i = 1, · · · , n ,
onde ui ´e um sinal de controle de velocidade aplicado ao motor da i -´esima junta.
4/27
Ent˜ao, o sistema de controle pode ser considerado como:
v ≈ J (θ) u ω
Voltar
Problema de Regula¸c˜ ao O objetivo de controle ´e achar u(t) que leve a configura¸c˜ao do manipulador { p(t), R(t)} para uma configura¸c˜ao desejada { pd, Rd}.
5/27
Para isto tem que ser definida uma m´etrica do erro. Para a posi¸c˜ao podemos utilizar a norma Euclidiana: 1 2 e p = || p − pd || , 2
e˙ p = ( p − pd )T p˙ = e pT p˙
E para a orienta¸c˜ao R ∈ SO(3) ?
Voltar
Erro de Orienta¸c˜ ao Para abordar o problema de controle de atitude de um corpo r´ıgido temos que reconsiderar a defini¸c˜ao do erro de orienta¸c˜ao. Considere z
Eb
6/27
Ed
y z x
y x
DESEJADO
Ea
CORPO
z y
x
INERCIAL
¯ ∗ E ¯b Rab = E a
e
¯ ∗ E ¯d Rad = E a
¯B −→ E ¯D . Objetivo de Controle: E
Voltar
N˜ao ´e aconselh´avel utilizar e = Rab −Rad dado que e ∈ SO(3) perdendo todas as propriedades deste grupo (SO(3)). O erro de orienta¸ca˜o pode ser representado por uma matriz de erro de orienta¸c˜ao Re ∈ SO (3), que pode ser definida de acordo com os seguintes enfoques:
7/27
1. Enfoque Inercial: T ¯ ∗E ¯b E ¯ ∗E ¯ = E e1 = Rab Rad a d a
¯b = E ¯d. Neste caso e1 = I ⇒ E 2. Enfoque do Corpo: T ¯ ∗ E ¯a E ¯ ∗ E ¯b = Rdb = RT e2 = Rad Rab = E d a bd
I
¯b = E ¯d. Neste caso e2 = I ⇒ Rdb = I ⇒ E
Voltar
Do ponto de vista de controle, considerando Rad = cte (problema de regula¸c˜ao) a equa¸c˜ao do dinˆamica do erro ´e dada por: 1. Enfoque Inercial: 8/27
T T = ωa × RabRad = ωa × e1 e˙ 1 = R˙ ab Rad
e1
2. Enfoque do Corpo:
T ˙ T T T e˙ 2 = Rad Rab = Rad ωa × Rab = Rad Rab Rab ωa × Rab = e2 ωb ×
e2
(Rab ωa )×
Para outras defini¸c˜oes de erro de orienta¸c˜ao no contexto de controle vide “The Attitude Control Problem”, J.T. Wen and K. Kreutz-Delgado, IEEE Trans. on Automatic Control, Vol. 36, No 10, pp. 1148-1162, October 1991.
Voltar
Controle Cinem´ atico de Manipuladores O sistema de controle ´e dado por
9/27
x˙ = J a(θ) θ˙ = J a(θ) u
O objetivo de controle ´e dado por: x → xd(t)
e = xd − x → 0 para t → ∞
Considerando J a ∈ IRn×n n˜ao singular, uma lei de controle ´e dada por: ¯ u = J a (θ)−1 u
lei que lineariza o sistema (i.e., x˙ = u¯), sendo que u¯ pode ser escolhida como: ¯ = x˙ d + K (xd − x) u
Voltar
O sistema em malha fechada ´e dado por: e˙ + Ke = 0
Para K > 0 o sistema ser´a exponencialmente est´avel,
10/27
e(t) = e−Kt e(0)
Obsserva¸c˜ao: A velocidade desejada x˙ d ´e necess´aria no caso de seguimento de trajet´orias, para evitar a existˆencia de um erro de estado estacion´ario. No caso de x˙ d n˜ao ser dispon´ıvel, ao inv´es de um controle proporcional, poderiam ser utilizadas leis de controle do tipo PI ou PID.
Voltar
Controle desacoplado posi¸c˜ ao/orienta¸c˜ ao No caso de um manipulador antropom´ orfico de 6 DOF , o jacobiano geom´etrico do manipulador adquire a forma de uma matriz bloco triangular, caso seja considerada a configura¸c˜ao do punho. Isto caracteriza o desacoplamento entre a posi¸c˜ao e a orienta¸c˜ao do efetuador para esta classe de manipuladores. Assim, ´e poss´ıvel separar as vari´aveis das juntas em parcelas respons´aveis pela posi¸c˜ao e pela orienta¸c˜ao θ=
θ p θo
Ent˜ao, a equa¸c˜ao de cinem´atica diferencial assume a forma
v ω
=
J 11 0 J 21 J 22
11/27
u p uo
Voltar
Considerando o caso quadrado e J ii n˜ao singular, a inversa do jacobiano ´e particularmente simples. A lei de controle que lineariza o sistema ent˜ao ´e dada por:
u p uo
=
−1 J 11 −1 −1 −J 22 J 21 J 11
0 −1 J 22
¯ p u ¯o u
12/27
Como pode ser observado, o controle de posi¸c˜ao ´e completamente de−1 sacoplado: u p = J 11 ¯ p . u Por outro lado o controle de orienta¸c˜ao uo depende de u p , sendo calculado da seguinte forma: −1 [¯ uo = J 22 uo − J 21 u p ]
Ent˜ao a lei de controle que garante a estabilidade assint´otica do sistema ´e dada por ¯ p = K p e p + vd , u ¯o = K o eo + ωd . u onde vd e ωd s˜ao as velocidades linear e angular desejadas (feedforward).
Voltar
Sistema Base M´ ovel / Manipulador Rob´ otico ¯I , da base m´ovel E ¯0 e Considere os sistemas de coordenadas Inerciais E ¯N . do efetuador E 13/27
BASE z
E0
En
y z x
y x
EFETUADOR
Ei
z y x
INERCIAL
RIN = RI 0 R0N p IN = p I 0 + p0N ωIN = ω I 0 + ω0N vIN = vI 0 + ωI 0 × p 0N + v0N
Voltar
Sendo que:
v0N = J (θ) θ˙ ω 0N
Em forma matricial tem-se
vIN = ωIN
14/27
I − p 0N × 0 I
vI 0 ωI 0
+ J (θ) θ˙
O sistema pode ser representado em forma compacta: V N = Φ0N V 0 + J (θ) u
Considerado que o objetivo de controle ´e levar xN → xd apesar do movimento da base m´ovel, ent˜ao considerando J ∈ IRn×n n˜ao singular u = J −1 (θ) [¯ u − Φ0N V 0 ]
=⇒
¯ V N = u
lineariza o sistema e rejeita o movimento da base. Para o caso de regula¸c˜ao, u¯ que estabiliza o sistema no ponto de equil´ıbrio ´e dada por u¯ = − K (xd − xN ) com K > 0.
Voltar
Controle de manipuladores redundantes Neste caso o n´umero de juntas n ´e maior que o n´umero de graus de liberdade da tarefa m, portanto x˙ = J (θ) θ˙ ; J (θ) ∈ IRm×n
15/27
Pode-se utilizar a pseudo-inversa de J , J † = J T (JJ T )−1
JJ † = I
onde o posto de J ´e igual ao posto de JJ T . Uma lei de controle que leva x → xd ´e dada por u = J † (θ) [x˙ d + K (xd − x)]
Em malha fechada o erro e = xd − x ´e governado por e˙ + K e = 0
ent˜ao para K > 0 o sistema ´e est´avel.
Voltar
Observando que (I − J † J ) expande o espa¸co nulo de J , i.e., J (I − J †J ) = (J − J J † J ) = 0
I
16/27
pode-se propor a seguinte lei de controle:
u = J † (θ) [x˙ d + K (xd − x)] + (I − J † J ) µ
A equa¸c˜ao do erro continu´a sendo e˙ + Ke = 0
A vari´avel µ ´e um grau de liberdade adicional do controlador que pode ser utilizado para maximizar uma fun¸c˜ao objetivo w(θ) > 0: µ = K 0
∂w(θ ) ∂θ
T
Voltar
A fun¸c˜ao w(θ) pode ser escolhida de diferentes formas: 1. Manipulabilidade: w(θ ) =
det(J J T )
2. Limites das Juntas: θim ≤ θi ≤ θiM , 1 w (θ ) = − 2n
n
i=1
θi − θ¯i θiM − θim
2
17/27
onde θ¯i ´e a m´edia entre θiM e θim. 3. Distˆancia a obst´aculos: w(θ) = min || p(θ) − O|| OBJETO
p(q)
Voltar
Servo-Vis˜ ao Rob´ otica Considera-se o problema de controle cinem´atico. Setup experimental:
18/27
Voltar
A configura¸c˜ao do efetuador x ∈ IRm ´e definida pela cinem´atica direta x = k (θ)
onde θ ∈ IRn ´e o vetor de ˆangulo das juntas. Considerando manipuladores planares n˜ao redundantes, i.e., m = n = 2; a velocidade do efetuador em fun¸c˜ao da velocidade das juntas ´e dada por x˙ = J (θ) θ˙ onde J (θ) =
∂k (θ ) ∂θ
19/27
∈ IR2×2 ´e o Jacobiano do manipulador.
Considerando θ˙i como o sinal de controle ui (i = 1, 2), tem-se o seguinte sistema de controle x˙ = J (θ) u Uma lei de controle cartesiana v pode ser transformada para um sinal de controle de juntas utilizando:
u = J −1 (θ) v
Voltar
Sistemas de Coordenadas em Servovis˜ ao Ee Et
Ec
20/27 Ei
Eb
onde as seguintes letras indicam referˆencia
•b
`a base do manipulador.
•e
ao efetuador do robˆ o.
•c
`a cˆamera.
•i
`a imagem.
•t
ao alvo.
Voltar
Vis˜ ao Computacional Para utilizar servo-vis˜ao ´e necess´ario entender os aspectos geom´etricos do processo de forma¸c˜ao da imagem. Uma cˆamera cont´em lentes que formam uma proje¸c˜ao 2D da cena no plano da imagem onde o sensor est´a localizado.
21/27
Utilizando o modelo de proje¸c˜ao perspectiva (outras op¸co˜es s˜ao proje¸c˜ao ortogr´afica escalada e proje¸c˜ao ajustada), e considerando o sistema de ¯c = [xc yc zc ], assume-se que coordenadas da cˆamera E
• O eixo z c do sistema de coordenadas da cˆamera ´e perpendicular ao plano da imagem (ao longo do eixo o´tico), e a origem est´a localizada a uma distˆancia f atr´as do plano da imagem, onde f > 0 ´e a distˆancia focal da lente da cˆamera.
Voltar
Plano de Trabalho
P (x0,y0,z0)
Plano da
22/27
Imagem
zc
xc
f
I (u,v,f)
Eixo Otico
yc
Dado que a origem da cˆamera, os pontos I e P s˜ao colineares ´e v´alida a seguinte rela¸c˜ao:
k
x0 y0 z 0
=
u v f
=⇒
f u = v z 0
x0 y0
Voltar
No entanto (u, v ) s˜ao coordenadas f´ısicas (e.g., em metros) e tem que ser transformadas em coordenadas da tela (em pixels). Em geral a origem da tela se encontra num vˆertice e n˜ao no centro. Al´em disto existem fatores de escala da cˆamera α1 e α2 em (pixels/m).
23/27
Com isto as coordenadas (xc1 , xc2 ) (pixels) na tela s˜ao definidas como:
xc1 xc2
=
0 0 −α2
α1
f = z 0
0 0 −α2
α1
u v
+
x0 y0
o1 o2
+
o1 o2
Por outro lado, as coordenadas (x0 , y0 , z 0 ) do ponto P no plano de trabalho s˜ao expressas no sistema de coordenadas da cˆamera, e dever˜ao ser transformadas para o sistema de coordenadas inercial.
Voltar
No caso planar tem-se: x =
cos(φ) − sin(φ) x1 = sin(φ) cos(φ) x2
x0 y0
+ pbc
onde φ ´e o ˆangulo de rota¸c˜ao da cˆamera em torno do eixo ´otico e pbc ´e a posi¸c˜ao da cˆamera com respeito `a base do manipulador. Consequentemente, tem-se:
x0 = y0
cos(φ) sin(φ) − sin(φ) cos(φ)
24/27
(x − pbc)
Portanto, a rela¸c˜ao entre as coordenadas do ponto P na cˆamera e no sistema inercial ´e dada por: f = xc z 0
0 0 −α2
α1
cos(φ) sin(φ) (x − pbc) + − sin(φ) cos(φ)
o1 o2
Voltar
Desta forma, a transforma¸c˜ao cˆamera/espa¸co de trabalho ´e dada por: xc = K p x + xc0
onde
f K p = z 0
α1 0 0 α2
cos(φ) sin(φ) sin(φ) − cos(φ)
25/27
e f xc0 = z 0
α1 0 0 α2
− cos(φ) − sin(φ) − sin(φ) cos(φ)
pbc +
o1 o2
xc : posi¸c˜ao do efetuador no sistema de coordenadas da imagem. o. xc0 : offset da posi¸c˜ao do frame da camera com respeito `a do robˆ o. K p : orienta¸c˜ao e escalamento da cˆamera com respeito ao robˆ o. φ: ˆangulo de orienta¸c˜ao da cˆamera com respeito ao frame do robˆ f : distˆancia focal da cˆamera. o. z 0 : profundidade do plano da imagem ao plnao de trabalho do robˆ αi > 0: fatores de escala (pixels/m).
Voltar
Controle por servo-vis˜ ao Neste caso, ser´a considerado que a posi¸c˜ao do efetuador ´e medida utilizando uma cˆamera CCD fixa monocular com o eixo o´tico perpendicular ao espa¸co de trabalho do robˆo.
26/27
A transforma¸c˜ao cˆamera/espa¸co de trabalho ´e dada por: xc = K p x + xc0 f K p = z 0
α1 0 0 α2
cos(φ) sin(φ) sin(φ) − cos(φ)
xc : posi¸c˜ao do efetuador no sistema de coordenadas da imagem. o. xc0 : offset da posi¸c˜ao do frame da camera com respeito `a do robˆ o. K p : orienta¸c˜ao e escalamento da cˆamera com respeito ao robˆ o. φ: ˆangulo de orienta¸c˜ao da cˆamera com respeito ao frame do robˆ f : distˆancia focal da cˆamera. o. z 0 : profundidade do plano da imagem ao plnao de trabalho do robˆ αi > 0: fatores de escala (pixels/m).
Voltar
No sistema de coordenada da cˆamera o problema de controle cartesiano ´e dado por: x˙ c = K p v onde v T = [v1 v2]. Considere que xcd ∈ IR2 ´e a trajet´oria desejada do efetuador no plano da cˆamera. Uma lei de controle cartesiano para xc → xcd ´e dada por
27/27
v = K p−1 [x˙ cd + K (xcd − xc )]
Tendo ent˜ao u = (K pJ (θ))−1 [x˙ cd + K (xcd − xc )]
onde J v = K p J (θ) ´e o Jacobiano da Imagem. O sistema em malha fechado ´e dado por: e˙ cd + K ecd = 0
para K > 0 tem-se que os sistema ´e exponencialmente est´avel, ecd(t) = e−Kt ecd(0)
Voltar