Académique Documents
Professionnel Documents
Culture Documents
http://www.coep.ufrj.br/gscar 1/36
Cinematica Inversa
Fernando Lizarralde
DEL-POLI/UFRJ e PEE-COPPE/UFRJ
JJ
II
J
I
Rio de Janeiro, 15 de outubro de 2009
Voltar
Fechar
Cinematica Inversa
2/36
A cinematica direta pode ser descrita de duas formas:
Tbe() ou x = k()
Ela define uma relacao entre os angulos das juntas e a configuracao
do efetuador no sistema de coordenadas da base, {pbe, Rbe}.
A cinematica inversa determina os angulos das juntas a partir da con-
figuracao do efetuador {pbe, Rbe}, e tem as seguintes caractersticas:
1. Ela e geralmente nao linear, e difcil de representar em forma fechada
2. podem existir multiplas solucoes, ou
JJ
3. podem existir infinitas solucoes = manipuladores redundantes II
(n > 6), ou J
I
4. pode nao existir solucao Voltar
Fechar
Para ter uma ideia da dificuldade consideremos um manipulador de n
juntas:
Rbe() pbe()
Tbe() = IRn
0 1
3/36
tendo 9 equacoes em R + 3 equacoes em p, portanto 12 EQUACOES
com n INCOGNITAS.
JJ
II
J
I
Voltar
Fechar
Desacoplamento Cinematico
4/36
Calcular a cinematica inversa em forma fechada requer intuicao algebrica
e geometrica.
Em muitos robos industriais e possvel desacoplar a posicao da ori-
entacao do efetuador, por exemplo, no caso do manipulador ter um
punho esferico.
Consideremos por exemplo um manipulador de 6 DOF sendo que os
eixos das 3 ultimas juntas interceptam num ponto
JJ
II
J
Pode ser observado que qualquer movimento destas 3 juntas nao alteram I
a posicao do ponto O. Voltar
Fechar
Entao considerando a posicao e orientacao do efetuador como
Rbe() = Re pbe() = pe
o problema e determinar o vetor de juntas .
5/36
Considerando a junta esferica temos o seguinte
JJ
p pw = d6 z5 II
J
onde z5 pode ser obtido de Re = [x5 y5 z5] e portanto I
pe = pw + d6 z5 pw = pe d6 z5 Voltar
Fechar
Entao dado Re e pe podemos calcular pw que e funcao de 1, 2, 3, i.e.,
pw (1, 2, 3) tendo 3 equacoes com 3 incognitas.
Uma vez calculada os angulos das 3 primeiras juntas, podemos calcular
Rb3(1, 2, 3) e como temos que 6/36
T
Rbe = Rb3 R3e R3e = Rb3 Rbe
Inversa da posicao
Agora considere o manipulador Elbow da figura
JJ
II
J
I
Voltar
Fechar
Dado pw o objetivo e calcular 1, 2, 3, onde
px
pw = py
pz 7/36
Da projecao no plano x y
8/36
JJ
II
J
Uma segunda solucao valida e
I
1 = atan2(py , px) + Voltar
Fechar
Uma vez calculado 1 podemos calcular 2, 3.
Entao observando o plano definido pelo elo 2 e 3,
9/36
11/36
JJ
II
J
I
Voltar
Fechar
Inversa da orientacao
T
Uma vez calculadas 1, 2, 3 pode-se calcular R3e = Rb3 Rbe.
Devido as 3 ultimas juntas estarem na configuracao dos angulos de Euler
(ZYZ), temos o trabalho simplificado dado que a estrutura de R3e e: 12/36
c 4 s5 r11 r12 r13
R3e = s4s5 = r21 r22 r23
s5c6 s5s6 c5 r31 r32 r33
Entao temos:
q
2 + r2 , r )
5 = atan2( r13 23 33
4 = atan2(r23/s5, r13/s5)
6 = atan2(r32/s5, r31/s5)
JJ
II
Existindo um singularidade para s5 = 0 que implica 5 = 0, , . J
I
Exerccio: Cinematica inversa de um manipulador RRP. Voltar
Fechar
Enfoque Iterativo
13/36
Considere a posicao e orientacao do efetuador representada no espaco
operacional x:
p
x=
onde pe e a posicao e e e uma representacao da orientacao do efetuador.
Considerando que a cinematica direta do manipulador e dada por:
x = k()
onde IRn, tem-se que a cinematica inversa e definida pela deter-
minacao da funcao inversa de k(): JJ
= k 1(pe, e) II
J
O calculo da solucao da cinematica inversa pode ser realizado utilizando I
um algoritmo recursivo. Voltar
Fechar
Para isto, considere a funcao de erro:
dV d
= (x k())T k()
d d
Equivalentemente
dV d
= [x k()]T J()
d d
onde
J() = k()
Entao com o objetivo de garantir a convergencia do algoritmo podemos JJ
escolher: II
J
d dV I
= J()1 [x k()] = = ||x k()||2 < 0
d d Voltar
Fechar
dV
Entao dado que d < 0 e V > 0 tem-se que V 0 com .
Isto implica que o erro tambem tende para zero, i.e.
||x k()|| 0 para 15/36
k(.)
JJ
II
J
I
Voltar
Fechar
Decomposicao em subproblemas
17/36
Conhecido como metodo de Paden-Kahan.
Para certos manipuladores, a solucao da cinematica inversa pode ser
calculada atraves da solucao de uma serie de subproblemas.
Subproblema 0
Dado ~u e ~v , satisfazendo ||~u|| = ||~v ||, e o vetor unitario w
~ perpendicular
a ~u e ~v .
Calcule tal que
exp(w)
~ ~u = ~v
JJ
Sempre existe uma solucao unica. v II
J
I
Voltar
u
Fechar
Subproblema 1
Dado ~u e ~v , satisfazendo ||~u|| = ||~v ||, e o vetor unitario w
~ tal que
~ ~u = w
w ~ ~v 18/36
19/36
20/36
JJ
Cinematica Inversa: dado p04 calcular (1, 2, 3). II
J
Escolhendo ~h1, ~h2, ~h3 como os eixos de rotacao tem-se que
I
R01 = e1h1 , R12 = e2h2 , R23 = e3h3 Voltar
Fechar
Notando que
(~p24)0 = (~p04)0 (~p02)0
Por outro lado tem-se
22/36
(~p24)0 = (~p23)0 + (~p34)0 = R01R12 [(~p23)2 + R23 (~p34)3]
Manipulador SCARA
h1 h2 h3, h4
p12 p23
O1
O2 O3
p34
p01 JJ
O4 II
O0 p04 J
I
Voltar
Fechar
Dados p04 e R04 o problema e determinar 1, 2, 3, 4.
Tem-se que
~h1 = ~h2 = ~h3 = ~h4 = ~z
Agora tem-se
v 26/36
Pode-se calcular
~u ~v
= acos
||~u|| ||~v ||
Alternativamente tem-se
||~u ~v || /2 ||~u ~v || JJ
= 2 atan = 2 atan II
||~u + ~v || /2 ||~u + ~v ||
J
~ tem a mesma direcao que ~u ~v entao e positivo senao troca de
Se I
~ (~u ~v ) < 0 = = .
sinal, i.e., Voltar
Fechar
Subproblema 1: Prova
Girando ~u com respeito a w
~ forma um cone
27/36
Tem-se:
~u0 = ~u (~ ~u) ~
~v 0 = ~v (~ ~v )
~
Aplicando o subproblema 0 e considerando que ~ ~u = ~ ~v tem-se:
0
||~u ~v 0|| JJ
||~u ~v ||
= 2 atan = 2 atan II
||~u0 + ~v 0|| ||~u + ~v 2(~ ~u) ~ ||
J
~ tem a mesma direcao que ~u0 ~v 0 entao e positivo, senao troca
Se I
~ (~u ~v ) < 0 = = .
de sinal, i.e., Voltar
Fechar
Subproblema 2: Prova
Se
~ 1 for colinear com ~ 2 recai-se no subproblema 1.
Suponha que ~1 e ~ 2 nao sao colineares.
28/36
~z = ~ 2 + (~1
~1 + ~ 2)
Calculando a projecao de ~z em
~ i tem-se: 29/36
~ 1 ~z = + (~1
~ 1 ~v
~ 2) =
~ 2 ~z = (~1
~ 2) + = ~ 2 ~u
Em forma matricial:
1 (~1
~ 2) ~ 1 ~v
=
(~1
~ 2) 1 ~ 2 ~u
Dado que
~1 e
~ 2 nao sao colineares este sistema tem solucao dada por:
1 (~1 ~ 2)
JJ
II
(~1 ~ 2) 1
~ 1 ~v
J
=
~ 2 )2
1 (~1 ~ 2 ~u
I
Voltar
Fechar
Para ter a expressao de ~z falta calcular a componete .
Ela pode ser calculada da norma de ~z:
~z = ~1 + ~2 + (~1
~ 2)
~ 1) ~v = exp(2
~z = exp(1 ~ 2) ~u
obtendo
||~u ~z||
2 = 2 atan
||~u + ~z 2(~2 ~u)
~ 2||
~ 2 tem a mesma direcao que ~u ~z entao 2 e positivo senao troca
Se
de sinal, i.e., ~ 2 (~u ~z) < 0 = 2 = 2.
||~v ~z||
1 = 2 atan JJ
||~v + ~z 2(~1 ~v )
~ 1||
II
~ 1 tem a mesma direcao que ~v ~z entao 1 e positivo senao troca
Se J
de sinal, i.e., ~ 1 (~v ~z) < 0 = 1 = 1. I
Voltar
Fechar
Resumindo, pelo subproblema 1 podem ser resolvidos 1 e 2:
||~u ~z||
2 = 2 atan
||~u + ~z 2(~2 ~u) ~ 2||
||~v ~z|| 32/36
1 = 2 atan
||~v + ~z 2(~1 ~v ) ~ 1||
onde
~z = ~1 + ~2 + (~1 ~ 2)
com
~ 1 ~v (~1
~ 2)(~2 ~u)
=
1 (~1 ~ 2 )2
~ 2 ~u (~1
~ 2)(~1 ~v )
=
1 (~1 ~ 2 )2
s
||~u||2 2 2 2 (~1
~ 2) JJ
= II
||~1 ~ 2||2
J
~ 2 (~u ~z) < 0 = 2 = 2.
Se I
~ 1 (~v ~z) < 0 = 1 = 1.
Se Voltar
Fechar
Subproblema 3: Prova
Tem-se que = ||~v exp(w)
~ ~u|| .
33/36
~u0 = ~u (~ ~u) ~
~v 0 = ~v (~ ~v )
~
JJ
Por outro lado II
02 = 2 ((~u ~v )
~ )2 J
I
Voltar
Fechar
Entao pela lei do cosseno, pode-se calcular o angulo entre exp()~u0
e ~v 0:
2 2
02 = ||~u0|| + ||~v 0|| 2 ||~u0|| ||~v 0|| cos()
que implica ! 34/36
Desta forma
= 0
onde 0 (angulo entre os vetores ~u0 e ~v 0) pode ser calculado de ~u0 e ~v 0
como (Livro Murray, Li e Sastry, pag 103)
~ (~u0 ~v 0)
JJ
0 = atan II
~u0 ~v 0
J
I
Voltar
Fechar
Alternativamente, 0 pode ser calculado utilizando o subproblema 0 para
os vetores unitarios ~u0/ ||~u0|| e ~v 0/ ||~v 0||, i.e.,
0
~u / ||~u0|| ~v 0/ ||~v 0||
0 = 2 atan 35/36
~u0/ ||~u0|| + ~v 0/ ||~v 0||
~ tem a mesma direcao que ~u0 ~v 0 entao 0 e positivo senao troca
Se
de sinal, i.e., ~ (~u0 ~v 0) < 0 = 0 = 0.
Desta forma
!
0 2 0 2 02
||~u || + ||~v ||
= 0 ; = acos
2 ||~u0|| ||~v 0||
onde existem as seguintes situacoes:
0 2 2
1. se ||~u || + ||~v 0|| 02 > 2 ||~u0|| ||~v 0|| 6 solucao ;
JJ
0 2 2
2. se ||~u || + ||~v 0|| 02 = 2 ||~u0|| ||~v 0|| 1 solucao e; II
J
0 2 0 2 I
3. se ||~u || + ||~v || < 2 ||~u0|| ||~v 0|| 2 solucoes.
02
Voltar
Fechar
Exerccio: Subproblema 4 (Exercicio 4, Cap. 3, Livro Murray, Li e Sastry,
pag. 149).
36/36
JJ
II
J
I
Voltar
Fechar