Sistemas Mecatrônicos 1

Cinemática inversa

Prof. Rafael V. Aroca

Departamento de Engenharia Mecânica
Universidade Federal de São Carlos
 Próximas aulas

IMPORTANTE - Verificação
21 de Maio
Controle em malha fechada de todos os eixos
 Cinemática
• Cinemática direta
• Cinemática inversa
 Cinemática
• Cinemática direta
• Cinemática inversa
– Solução fechada
• Geométrica
• Analítica
– Solução numérica
• Iterativa
– Outras
 Soluções para a cinemática inversa
(mais comuns)

• Forma fechada • Solução numérica

• Preferida, se encontrada
• Inspeção
• Conjunto de equações
• Necessária para
robôs redundantes
• Fácil de usar
• Lenta
• Método de Newton
 Solução fechada
• Passos
– Criar modelo do robô
– Determinar a cinemática direta
– Tentar definir várias equações e isolar os ângulos
das juntas
• Insight
Solução geométrica: exemplo

Y (x,y)

q2

q1
X
 Solução geométrica: exemplo
q1 = ?
q2 = ?
r
y r2 = x2 + y2
α
r2 = a12 + a22 – 2.a1.a2.cosα

Fogira: http://www.dinamatica.com.br/2011/03/uma-deducao-grafica-da-lei-dos-cossenos.html
 Solução geométrica: exemplo
q1 = ?
q2 = ?
r
α y r2 = x2 + y2

r2 = a12 + a22 – 2.a1.a2.cosα

cos(α) = (a12 + a22 - r2 ) / 2.a1.a2

x
Cos(α) = (a12 + a22 - x2 - y2 ) /
2.a1.a2
cos(q2) = (x2 + y2 - a12 - a22 ) / 2.a1.a2
q2 = π-α

cos(q2) = -cos(α)
Solução geométrica: exemplo

a2.sen(q2)

a2  (a2 cos(q 2)) 2  (a2 sen (q 2)) 2

2

a2.cos(q2) 1  cos(q 2) 2  sen (q 2) 2

sen (q 2)  1  cos(q 2) 2
 Solução geométrica: exemplo

a2.sen(q2)

a2 sen (q 2)
tan(  ) 
a1  a2 cos(q 2)
 a2 sen (q 2) 
  tan 
1

ϐ  a1  a2 cos(q 2) 
Solução geométrica: exemplo
y
tan( ) 
x
1  y 
  tan  
x
 a2 sen (q 2) 
  tan 
1

r  a1  a2 cos(q 2) 
y q1    
ϐ
 y 1  a2 sen (q 2) 
q1  tan    tan 
1

γ x  a1  a2 cos(q 2) 

x
Solução geométrica: exemplo

x 2  y 2  a12  a22
cos(q 2) 
2a1a 2
 y 1  a2 sen (q 2) 
q1  tan    tan 
1

x  a1  a2 cos(q 2) 
 Solução geométrica: exemplo

x 2  y 2  a12  a22
cos(q 2) 
2a1a 2
 y 1  a2 sen (q 2) 
q1  tan    tan 
1

x  a1  a2 cos(q 2) 
Figura adaptada de: http://www.mathwarehouse.com/trigonometry/graph-equation-cosine/graph-of-cosine-properties-pictures-examples.php
 Solução geométrica: exemplo

Simetria do cosseno
-Duas soluções
-Ângulo positivo e negativo

 x 2
 y 2
 a 2
 a 2

q 2  cos 
1 1 2

 2a1a 2 
 y 1  a2 sen (q 2) 
q1  tan    tan 
1

x  a1  a2 cos(q 2) 

Figura adaptada de: http://www.mathwarehouse.com/trigonometry/graph-equation-cosine/graph-of-cosine-properties-pictures-examples.php

 Duas soluções
(x,y)
Y
q2

q1
X

 x 2
 y 2
 a 2
 a 2
  x 2
 y 2
 a 2
 a 2

q 2  cos1  1 2
 q 2   cos1  1 2

 2a1a 2   2a1a 2 
 y 1  a2 sen (q 2)   y 1  a2 sen (q 2) 
1
q1  tan    tan    1
q1  tan    tan   
x  a1  a2 cos(q 2)  x  a1  a2 cos(q 2) 
 Solução analítica
• Calcular cinemática direta
• Isolar os ângulos nas equações da cinemática
direta
• Resolver para x,y,z, P, R,Y desejados
Solução analítica: exemplo

Y (x,y)

q2

q1
X
Solução analítica: exemplo

trchain('Rz(q1) Tx(a1) Rz(q2) Tx(a2)', [q1 q2])

... ... ... a2 * (cos(q1)* cos(q2)  sin(q1) * sin(q2)) + a1* cos(q1)

... ... ... a2 * (cos(q1)* sin(q2) + cos(q2)* sin(q1)) + a1* sin(q1) 

0 0 1 0 
 
0 0 0 1 

x = a2*(cos(q1)*cos(q2) +sin(q1)*sin(q2)) + a1*cos(q1)

y = a2*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + a1*sin(q1)

x = a2*cos(q1+q2) + a1*cos(q1)
y = a2*sin(q1+q2) + a1*sin(q1)
Solução analítica: exemplo
x = a2*cos(q1+q2) + a1*cos(q1)
y = a2*sin(q1+q2) + a1*sin(q1)

q1 = ?
q2 = ?
α

x2 + y2 = a12 + a22 – 2.a1.a2.cosq2

cos q2 = (x2 + y2 - a12 - a22 ) / 2.a1.a2

Solução analítica: exemplo
x = a2*cos(q1-q2) + a1*cos(q1)
y = a2*sin(q1+q2) + a1*sin(q1)

cos(q2) = C2
sen(q2) = S2
α
x = (a1+a2.C2)cos(q1)-a2.S2sin(q1)
y = (a1 + a2C2)sen(q1) + a2S2cos(q1)

a.cos(t) + bsen(t) = c

Theta = tan-1((c/(+-sqrt(a2 + b2 -c2)) )- tan-

1(a/b)

q1 = tan -1(y/x) – tan -1(a2S2/(1+a2C2)

 Soluções na forma fechada
• Vantagens
– Solução compacta e rápida de executar
– Determinística
– Mostra várias configurações explicitamente

• Desvantagem
– Difícil de obter dependendo do número de juntas
 Toolbox
• SerialLink.ikine3
– http://www.petercorke.com/RTB/robot.pdf
– Página 233
– q = R.ikine3(T) is the joint coordinates
corresponding to the robot end-effector pose T
represented by the homogenenous transform. This
is a analytic solution for a 3-axis robot (such as the
first three joints of a robot like the Puma 560)
 Toolbox
• mdl_puma560
• p560.plot(qz)
• T = transl(0.6, 0.1, 0) * rpy2tr(0, 180, 0, ‘deg’)
• trplot(T)
• qu = p560.ikine3(T) # 3 graus de liberdade sem pulso
• qu = p560.ikine6s(T) #3 graus de liberdade + 3 no pulso
• qu = p560.ikine6s(T, ‘r’) #s = pulso esférico
• qu = p560.ikine6s(T, ‘d’)
• p560.fkine(qu)
• p560.plot(qu) / p560.plot3d(qu)
 Solução numérica
• Solução na forma fechada
– Pode não existir
– Pode ser muito complicada

• Alternativa: solução numérica

x,y,z, R, P, Y θ1 θ2 θ3 θ4 θ5 θ6
 Solução numérica
x,y,z, R, P, Y θ1 θ2 θ3 θ4 θ5 θ6

• Sempre pode ser encontrada

• Fazer iterações nos ângulos até achar a pose
desejada
• Robôs redundantes
• Otimização
 Solução numérica
• Pontos importantes
– Convergência
• Escolha das juntas iniciais das coordenadas
– Não é possível garantir uma certa configuração
– Pode ser muito lento
• Questão de tempo real
 Toolbox
• Numérica
– ikine(pose, ângulos iniciais, máscara)
• mdl_planar
• Cinemática direta para conhecer os pontos
– p2.fkine([0 0])
– p2.fkine([pi 0])
• Inversa
– qini = p2.ikine(transl(2,0,0), [0 pi/4], [1 1 0 0 0 0])
– qfin = p2.ikine(transl(-2,0,0), [0 pi/4], [1 1 0 0 0 0])
 Toolbox
• Valores iniciais / evitar singularidade
– qini = p2.ikine(transl(2,0,0), [0 0], [1 1 0 0 0 0])
– qfin = p2.ikine(transl(-2,0,0), [0 0], [1 1 0 0 0 0])
– Warning: Initial joint configuration results in a
(near-)singular configuration, this may slow
convergence
 Toolbox
• Alpha
– Warning: solution diverging at step 2476, try reducing
alpha
• Iterações
– qfin = p2.ikine(transl(-2,0,0), [0 0], [1 1 0 0 0 0])
• Warning: ikine: iteration limit 1000 exceeded (row 1), final err
4.000000
– qfin = p2.ikine(transl(-2,0,0), [0 0], [1 1 0 0 0 0], 'alpha',
0.5, 'ilimit', 50000)
– qfin = p2.ikine(transl(-2,0,0), [0 0], [1 1 0 0 0 0], 'alpha',
0.5, 'ilimit', 50000, ‘verbose=2’)
• ilimit => Iterações numéricas (padrão = 1000)
• alpha => Passo de cada iteração
 Trajetórias
• Movimento de um ponto A para um ponto B
• 1. Cinemática inversa de A e B
– Ângulos iniciais e finais
• 2. Interpolar valores das juntas entre A e B
– Θintermediario = (1-s).Θinicial+s.Θfinal
• Precisamos fazer isso?
• Toolbox
– jtraj(qA, qB, 50)
– jtraj(ângulos inicias, ângulos finais, passos)
• Inspecionar resultado
 Toolbox
• mdl_planar2
• Cinemática direta para conhecer os pontos
– p2.fkine([0 0])
– p2.fkine([pi 0])
• Inversa
– qini = p2.ikine(transl(2,0,0), [0 pi/4], [1 1 0 0 0 0])
– qfin = p2.ikine(transl(-2,0,0), [0 pi/4], [1 1 0 0 0 0])
– jtraj(qini,qfin,10)
– jtraj(qini,qfin,100)
Figura: Corke, P. Introduction to Robotics: vision and control. Página 154.
 Trajetórias
• Interpolação nas juntas não gera trajetória
linear
– A cada instante de tempo
– 1. Interpolar a pose de A para B!
– 2. Calcular a cinemática inversa para cada pose
interpolada!
 Trajetórias
• Movimento cartesiano
– Toolbox
• ctraj(Ta, Tb, 50)
• ctraj(pose inicial, pose final, passos)
• Cada elemento é uma pose intermediária
– Inspecionar cada elemento
– Calcular a cinemática inversa para cada elemento
 Toolbox
• mdl_planar2
• p2.fkine(qz)
• p2.fkine([0 pi/2])
• Ta = transl(2,0,0) ou Ta = p2.fkine(qz)
• Tb = tranls(1,1,0) ou Tb = p2.fkine([0 pi/2])
• Trajetoria = ctraj(Ta, Tb, 100)
• Trajetoria = ctraj(Ta, Tb, 10)
• Trajetoria = ctraj(Ta, Tb, 2)
• Trajetoria = ctraj(Ta, Tb, 3)
• Trajetoria(:,:,1) / Trajetoria(:,:,2) / Trajetoria(:,:,3)
• Tranimate(Trajetoria)
Figura: Corke, P. Introduction to Robotics: vision and control. Página 156.
 Velocidades
• Relação entre velocidade das juntas e do TCP
x = a2*cos(q1+q2) + a1*cos(q1)
y = a2*sin(q1+q2) + a1*sin(q1)

• Derivada da posição x,y

– Ângulos das juntas como função do tempo

x' = -a1.q1’.sin(q1)-a2.(q1’+q2’).sin(q1+q2)
y' = a1.q1’.cos(q1)+a2.(q1’+q2’).cos(q1+q2)

Crédito das equações: Peter Corke (Introduction to Robotics)

 Matriz Jacobiana

Jacobiana:
- Equivalente matricial da derivada
- Função dos ângulos das juntas
- Relação linear das velocidades das juntas e a velocidade da ponta do robô

Crédito das equações: Peter Corke (Introduction to Robotics)

 Matriz Jacobiana

Inversa da Jacobiana
- Determina a velocidade das juntas com base na velocidade desejada do TCP

q2 = 0 -> Jacobiana infinita

Crédito das equações: Peter Corke (Introduction to Robotics)

 Matriz Jacobiana

(x,y)

q2=0

q1
X
 Matriz Jacobiana

(x,y)

q2=0

Singularidade

q1
X
 Matriz Jacobiana
Movimento
Y impossível
(x,y)

Equivalente a um robô
com um grau de liberdade

q1
X
 Matriz Jacobiana
Dificuldade de movimento

Y
(x,y)

q2 próximo de 0

Determinante pequeno
Condition number da matriz grande
cond()
q1
X
 Cinemática inversa nos projetos
• Tentar encontrar solução analítica ou
geométrica
• Definir parâmetros de Denavit-Hartenberg
• Criar modelo do robô no Matlab
– Ver aulas anteriores
• Executar as sub-rotinas de cinemática inversa
• Verificar se é consistente com a cinemática
direta
 Créditos e leitura
• Aula baseada em vídeo do curso “Introduction
to Robotics” do Prof. Peter Korke
• Robotics Toolbox manual (2015)
– http://www.petercorke.com/RTB/robot.pdf
• Using the Robotics Toolbox with a real robot
– http://petercorke.com/doc/robotarm.pdf
• Livro Peter Corke
– Capítulo 7