Robotica e Automacao

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, 21 de setembro 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.

Outros possveis metodos para resolver a cinematica inversa:

1. Desacoplamento cinematico
2. Algoritmo Iterativo
3. Decomposicao em sub-problemas (Paden-Kahan)

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 = d6z5 II
J
onde z5 pode ser obtido de Re = [x5 y5 z5] e portanto I
pe = pw + d6z5 pw = pe d6z5 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 = Rb3R3e R3e = Rb3 Rbe

Com R3e(4, 5, 6) definindo mais equacoes que incognitas.

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

claramente pode ser observado que JJ

1 = atan2(py , px) II
J
desde que px 6= 0 e py 6= 0. A posicao px = py = 0 caracteriza uma I
singularidade. Voltar
Fechar
Singularidade (top of the head):

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

temos pela lei do cosseno que JJ

II
c2 = r2 + s2 = a22 + a23 2a2a3 cos( 3) J
I
onde s2 = p2z e r2 = p2x + p2y . Voltar
Fechar
Como cos( 3) = cos(3) tem-se
r2 + s2 a22 a23
cos(3) = =D
2a2a3
10/36
com D [1, 1] para existir solucao. Senao a posicao pe esta fora do
espaco de trabalho.
Entao considerando sin(3) = 1 D2:
p
3 = atan2( 1 D2, D)

tendo 2 solucoes: cotovelo para acima, cotovelo para abaixo.

Similarmente

2 = atan2(s, r) atan2(a3s3, a2 + a3c3)

| {z } |

{z

} JJ
II
onde q  J
= atan2 p2x + p2y , pz I
Voltar
Fechar
As 4 solucoes dependem da configuracao do cotovelo.

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
Temos que pe = f1() e Rbe = f2() entao

= f 1(pe, Rbe)

Mas se utilizarmos uma representacao da orientacao Rbe temos que

= f2() e f = [f1 f2]T .
Entao definindo a funcao de erro
2
2


2V = ||pe f1()|| + f2() > 0


derivando V com respeito a variavel de iteracao

JJ
dV d d II
= (pe f1())T f1() + ( f2())T f2() J
d d d
I
Voltar
Fechar
  T
dV pe f1() d
= J()
d f2() d
onde   14/36
f1()
J() =
f2()
Entao com o objetivo de garantir a convergencia do algoritmo podemos
escolher:
   2
d pe f1() dV  pe f1()  < 0
= J()1
 
= =
d f2() d  f2() 

Entao dado que dVd < 0 e V > 0 tem-se que V 0 com .

Isto  o erro tambem tende para zero, i.e. ||pe f1()|| 0
 implica que JJ
e  f2() 0 com .
II
J
I
Voltar
Fechar
Diagrama Bloco do Algoritmo
Pe

J 1 1/s
15/36

f(.)

Ou ainda podemos escolher

 
d p f ()
= J T () e 1 gradiente conjugado
d f2()
com (0) = 0.

Versao Discreta: subtituir s 7 (1 z 1). JJ

II
J
I
Voltar
Fechar
Resumo de cinematica inversa
1. Enfoque Analtico: forca bruta e tedioso (6 DOF: solucao de po-
linomio de ordem 16 - MLS secao 3.4; casos especiais, e.g. punho
esferico: solucao analtica). 16/36

2. Enfoque Geometrico: elegante e utiliza a geometria do manipulador,

mas nao sempre funciona (aplicavel a manipuladores com punho
esferico).
3. Enfoque Iterativo: simples de implementar, mas o tempo de com-
putacao depende da configuracao. A tecnica nao funciona bem perto
de singularidades.

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

Calcule tal que

exp(w)
~ ~u = ~v JJ
Sempre existe uma solucao unica. II
J
I
Voltar
Fechar
Subproblema 2
Dado ~u e ~v , satisfazendo ||~u|| = ||~v ||, e os vetores unitarios w
~1 e w
~ 2.

19/36

Calcule 1 e 2 tal que JJ

II
~ 1) exp(2w
exp(1w ~ 2) ~u = ~v J
I
Pode ter 0, 1 ou 2 solucoes. Voltar
Fechar
Subproblema 3
Dado ~u e ~v , o vetor unitario w,
~ e a constante positiva .

20/36

Calcule tal que JJ

||~v exp(w)
~ ~u|| = II
J
Pode ter 0, 1 ou 2 solucoes. I
Voltar
Fechar
Exemplos de calculos da cinematica inversa
Manipulador Elbow
Considere somente a posicao do efetuador de um manipulador Elbow
21/36
(3DOF).

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]

Comparando as duas equacoes anteriores tem-se

(~p24)0 = (~p04)0 (~p02)0 = R01R12 [(~p23)2 + R23 (~p34)3]

Calculando a norma de ambos lados,

||(~p04)0 (~p02)0|| = ||R01R12[(~p23)2 + R23 (~p34)3]|| = ||(~p23)2 + R23 (~p34)3||

Deixando em evidencia 3 tem-se entao

JJ
||(~p04)0 (~p02)0|| = ||(~p23)2 + R23(3) (~p34)3|| II
J
Sendo que ||(~p04)0 (~p02)0|| , (~p23)2, (~p34)3 sao conhecidos, pode-se uti- I
lizar o Subproblema 3 para calcular 3 (0,1 ou 2 solucoes). Voltar
Fechar
Uma vez calculado 3 tem-se

(~p23)0 + (~p34)0 = R01R12 [(~p23)2 + R23 (~p34)3]

| {z } | {z }
conhecido conhecido
23/36
Dado que R01(1), R12(2), para calcular 1, 2 poder ser utilizado o
Subproblema 2 (0,1 ou 2 solucoes).

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

Por outro lado, 24/36

p~04 = p~01 + p~12 + p~23 + p~34

Primeiramente, considerando que p~34 = p~34(0) 3~z, tem-se que:

(~p04 ~z) ~z = p~01 p~34(0) + 3~z = 3 = (~p04 ~z) ||~p01|| + ||~p34(0)||

Por outro lado, p~04 no sistema de coordenadas 0 e dado por:

(~p04)0 = (~p01)0 + R01(~p12)1 + R01R12(~p23)2 + R01R12R23(~p34)3

Desta ultima equacao e dado que R23 = I podemos escrever:

JJ
II
(~p04)0 (~p01)0 = R01 (~p12)1 +R12 ((~p23)2 + (~p34)3) J
| {z } | {z } | {z } I
conhecido conhecido conhecido Voltar
Fechar
Entao considerando a norma desta equacao e ||R01|| = 1:

||(~p04)0 (~p01)0|| = ||(~p12)1 + R12(2) ((~p23)2 + (~p34)3)||

O subproblema 3 fornece 2 solucoes para 2. 25/36

Agora tem-se

(~p04)0 (~p01)0 = R01(1) (~p12)1 + R12 ((~p23)2 + (~p34)3)

| {z } | {z }
conhecido conhecido
O subproblema 1 fornece 1 solucao para 1.

O angulo 4 pode ser determinado de:

T
R04 = R01(1)R12(2)R34(4) = R34(4) = R12 T
(2)R01 (1)R04 JJ
II
dado que R34 e uma rotacao elementar ao redor de z. J
I
Voltar
Fechar
Subproblema 0: Prova
Sabe-se que ~u ~v = ||~u|| ||~v || cos().

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

Os 2 cones gerados podem ter 0, 1 ou 2 intersecoes.

JJ
Se ~z e o vetor dado pela intersecao tem-se II
~ 1) ~v = exp(2
~z = exp(1 ~ 2) ~u J
I
Conhecendo ~z recai-se no subproblema 1. Voltar
Fechar
O vetor ~z pode ser representado na base {~1, ~1
~ 2, ~ 2}, entao:

~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||2 = ~z ~z = 2 + 2 + 2 (~1 ~ 2||2 = ||~u||2

~ 2) + 2 ||~1
30/36
Tendo-se entao
s
||~u||2 2 2 2 (~1
~ 2)
=
~ 2||2
||~1
Desta forma existem as seguintes situacoes:
1. complexo 6 solucao ;
2. = 0 1 solucao e;
3. IR 6= 0 2 solucoes. JJ
II
J
I
Voltar
Fechar
Uma vez calculadas (, , ) podemos calcular ~z:

~z = ~1 + ~2 + (~1
~ 2)

e pelo subproblema 1 resolver 31/36

~ 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

Do cone gerado pela rotacao de ~u, tem-se

~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

||~u0||2 + ||~v 0||2 02

= acos
2 ||~u0|| ||~v 0||
 
 0 2 2
se ||~u || + ||~v 0|| 02 > 2 ||~u0|| ||~v 0|| nao existe solucao.


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