Cinemtica Inversa

Prof. Dr. Nielsen C. Damasceno

Slide 4
Introduo
Introduo
K-1

( n) (x, y, z,
1 x, y, z)
Introduo
O problema da cinemtica inversa
pode ser estabelecido da seguinte
forma:
especificada a localizao (posio e
orientao) que se deseja que a garra
alcance;
determinar o valor das variveis de junta
(ngulos ou deslocamentos de junta)
necessrios para levar a garra a tal
localizao.
Introduo
Tipos de solues:
Numrica;
Frmula fechada;
Numrica
As equaes no lineares
simultneas podem ser resolvidas por
mtodos iterativos.
Por utilizar mtodos iterativos,
esquemas baseados neste tipo de
abordagem podem ter problemas de
convergncia. Por esta razo, no so
muito apropriados para implementaes
em tempo real.
Frmula fechada
As equaes so resolvidas por
mtodos algbricos, frequentemente
fazendo uso de consideraes
geomtricas, resultando numa
expresso analtica computvel.
Este tipo de abordagem resulta em
solues fceis de implementar e que
envolvem pouco esforo computacional,
encorajando aplicaes em tempo real.
Frmula fechada
Robs manipuladores de seis graus
de liberdade e com trs eixos se
intersectando num ponto possuem
soluo em frmula fechada (Soluo
de Pieper).
A grande maioria dos manipuladores
industriais projetada e construda
de modo a possuir soluo em
frmula fechada.
Solues analticas x numricas

Analticas (ou solues fechadas):

Encontram uma soluo exata atravs da inverso
das equaes de cinemtica direta.
possvel apenas para problemas simples.
Numricas:
Utilizam aproximao e diversas iteraes para
tentar convergir para a soluo.
Tendem a ser mais genricos e
computacionalmente mais custosos.
Solues de forma fechada

Forma fechada significa:

um mtodo de soluo baseado em
expresses analticas ou na soluo
de um polinmio de grau 4 ou menor.
Apenas clculos no iterativos so
suficientes para chegar a uma
soluo.
Manipulador RR
Manipulador RR
Manipulador RR
Manipulador RR
Manipulador RR
Manipulador RR
Manipulador RR
Manipulador RR
Esta expresso mostra que o ngulo
1 depende de 2 , que j foi
determinado previamente no clculo
da cinemtica inversa.
Pode-se, substituir os valores do seno
e do cosseno de 2 nesta expresso.
Porm isto s aumentaria a
complexidade da equao e tornaria o
clculo mais trabalhoso.
Manipulador RR
mais prtico deixar nesta forma,
desde que todas as variveis que
apaream na equao estejam
previamente calculadas.
Manipulador RR
A ttulo de exemplo, adotando-se a
soluo positiva de 2 e substituindo o
seno e o cosseno deste ngulo na
expresso acima se chega, aps uma
simplificao, a
 Manipulador RRR

A cinemtica inversa no pode ser resolvida, pois h apenas

duas equaes com 3 incgnitas (os trs ngulos das juntas).
Manipulador RRR
Manipulador RRR

necessrio assumir uma condio a

mais e esta condio j foi estabelecida
no enunciado do problema ao se fixar a
orientao da junta J3 com o ngulo
(com relao horizontal).
Manipulador RRR

Isto significa que nem todas as

solues satisfazem as equaes, mas
somente aquela (ou aquelas) nas quais
o ngulo do elo a3 com relao
horizontal for igual a (fornecido).
Manipulador RRR
Manipulador RRR
Manipulador RRR

Finalmente para o clculo do terceiro

ngulo percebe-se que o ngulo
igual soma dos ngulos das juntas, ou
seja, 1 + 2 + 3 . Logo, como
conhecido e 1 e 2 j foram
determinados, ento
Manipulador RLR
Manipulador RLR
Manipulador RLR
Manipulador RLR
Mltiplas solues

Para N = 6 juntas 6 equaes e 6

incgnitas. Para uma localizao
especificada dentro do espao de
trabalho existe um nmero finito de
solues. At 16 solues diferentes
podem existir.
Mltiplas solues

Para N < 6 juntas 6 equaes e

N< 6 incgnitas. No possvel
alcanar objetivos gerais no espao
3D. As localizaes para as quais
existe soluo da cinemtica inversa
constituem um subespao do espao
3D.
Mltiplas solues
Para N > 6 juntas 6 equaes e N > 6
incgnitas. Para uma dada especificao de
localizao podem existir infinitas solues
para o problema da cinemtica inversa.
necessrio escolher uma soluo dentro
do conjunto de possveis solues. Deve-
se definir algum critrio de escolha, como
por exemplo: menor movimento em
relao localizao atual, movimento das
juntas mais leves, contorno de obstculos,
etc.
Cinemtica inversa
Cinemtica inversa

De esta forma possvel descrever as

rotaes e translaes de um elo do
sistema com respeito ao elo anterior. A
extenso a 3D imediata
(acrescentado uma linha e uma coluna).
Por exemplo:
Cinemtica inversa
Cinemtica inversa
Cinemtica inversa
Utilizando matrizes de transformao
homognea:
Cada elo do sistema tem associado um
sistema de referncia.
possvel representar as translaes e
rotaes relativas entre vrios elos (ou
entre todos).
A matriz representa a posio e
orientao relativa entre os sistemas
associados a dois elos sucessivos.
Cinemtica inversa
A representao total do sistema que
serve de exemplo:

Assim como no caso da cinemtica

direta, pode-se aplicar mtodos
geomtricos:
Cinemtica inversa
Da cinemtica direta temos:

onde q o vetor das coordenadas

articulares:

O problema da cinemtica inversa

consiste em encontrar a inversa de
tal que
Cinemtica inversa
Devido aos senos e cossenos das
rotaes, no linear em q. Por isso,
sua inversa pode no existir ou no ser
uma funo elementar (arctan, cos,).
A ideia utilizar uma aproximao
linear da funo e achar sua inversa:
Cinemtica inversa
Onde o jacobiano da funo no
ponto e a aproximao anterior
vlida perto desse ponto.
Relembrando

Antes de entrar nos mtodos

analticos, precisamos nos
lembrar da matemtica
Derivada de uma funo escalar

Se tivermos uma funo escalar f com

uma nica varivel x, podemos escrev-
la como f(x).
A derivada da funo em respeito a x
df/dx.
A derivada definida como:
 Derivada de uma funo escalar

f(x)
Slope=df/dx
f-axis

x-axis x
Derivada de f(x)=x2
Gradientes

Gradiente uma derivada de primeira

ordem de uma funo em relao suas
variveis:

D informaes sobre a taxa de

variao de uma funo em relao a
variveis independentes.
Gradiente a normal superfcie
Gradiente a normal superfcie
Derivadas vetoriais

Sabemos como:
Derivar um escalar por outro escalar.
Derivar um vetor por um escalar.
Mas como podemos:
Derivar um escalar por um vetor?
Derivar um vetor por outro?
Derivadas vetoriais

Derivadas de valores escalares por

valores vetoriais so comuns nos
campos de:
Dinmica dos fluidos,
Equaes de teoria de campos potenciais.
etc
Mas o importante hoje como calcular
a derivada de um vetor por outro...o
Jacobiano.
Jacobianos
Um Jacobiano a derivada de um vetor por
outro.
Se tivermos uma funo f(x), o Jacobiano a
matriz de derivadas parciais para cada
componente dos vetores
O Jacobiano contm toda a informao
necessria para relacionar uma mudana em
um componente de x a uma mudana em um
componente de f
O Jacobiano geralmente escrito como J(f,x):
Mas na prtica, equivale conceitualmente a df/dx
Jacobiano

f1 f1 f1
...
x1 x2 xN
f2 f2
df ... ...
J f,x x1 x2
dx
... ... ... ...
fM fM
... ...
x1 xN
 Exemplo: Rob 2R
(x , y)
x f1 ( 1, 2 ) l1 cos 1 l 2 cos( 1 2 )
y f2 ( 1, 2 ) l1 sin 1 l 2 sin( 1 2 ) 2

l2

l1
1

f1 f1
l1 sin 1 l 2 sin( 1 2) l 2 sin( 1 2)
J f, 1 2
f2 f2 l1 cos 1 l 2 cos( 1 2) l 2 cos( 1 2)
1 2
Derivadas parciais

O uso do smbolo em vez de d

para derivadas parciais indica que um
componente em um vetor de derivadas.
Para propsitos prticos, as derivadas
parciais se comportam como uma
derivada de um escalar por outro.
Relembrando de Clculo
Numrico
Exato x Aproximado

Muitos algoritmos necessitam da

computao da derivada.
Em alguns casos possvel computar
analiticamente a derivada.
Por exemplo:
2 df
f x x 2x
dx
Exato x Aproximado

Em outros casos a funo a ser

derivada muito complexa,
impossibilitando o clculo exato.
Mas, desde que possamos computar a
funo, podemos aproximar a derivada:
df f x x f x
dx x
para valores pequenos de x
 Derivada aproximada

f(x+ x)
f(x)
Slope= f/ x
f-axis

x-axis x
Valores prximos

Se sabemos o valor da funo em algum

ponto x, podemos estimar o valor da funo
em pontos prximos a ele.
Mtodo de Descida de Gradiente

Existem diversas maneiras de computar

aproximadamente as razes de uma
funo:
valores de x que torna f(x) = 0.
Uma maneira o Mtodo de descida
de Gradiente.
um mtodo de otimizao bem
conhecido.
Idia central

Se pudermos computar f(x) e df/dx para

qualquer valor de x, podemos sempre
seguir o gradiente na direo do valor
zero.
Idia central

Se pudermos computar f(x) e df/dx para

qualquer valor de x, podemos sempre
seguir o gradiente na direo do valor
zero.
Mtodo de Descida de Gradiente

Iniciaremos em um valor x0 e tomaremos

pequenos passos:
xi+1 = xi + x
at encontrarmos um valor xN onde f(xN)=0
Para cada passo, tentamos encontrar um valor
de x que nos colocar mais prximos ao valor
desejado.
Podemos utilizar a derivada como uma
aproximao da inclinao da funo.
 Descida de Gradiente
df/dx
f(xi)
f-axis

xi x-axis
Escolhendo x

Se a funo utilizada variar muito:

mais prudente andar em passos
pequenos.
Se a funo que se deseja minimizar
bem comportada:
Pode-se tentar aproximaes lineares que
passam por zero.
Escolhendo x

Se desejarmos aproximar linearmente

x para nos levar ao valor de x onde f(x) = 0
podemos usar:
 Descida de gradiente
df/dx
f(xi)
f-axis

xi+1 xi x-axis
Utilizando passos menores

Se a funo no for bem comportada, no

podemos aproximar linearmente x.
Uma modificao possvel adiciona o
parmetro para diminuir o passo, onde
1:

a taxa de aprendizado.
 Descida de gradiente
df/dx
f(xi)
f-axis

xi+1 xi x-axis
Exemplo de descida de gradiente
Minimizao

Se o f(x) desejado no for 0, o valor

desejado pode ser considerado um erro.
O objetivo do mtodo de descida de
gradiente minimizar este erro.
Cada passo nos leva mais prximos da
soluo, e paramos quando estivermos
perto o suficiente da resposta desejada.
Este processo iterativo comum na
maioria dos algoritmos numricos.
Minimizando f(x)=g

Se desejamos encontrar o valor de x

para quando a funo f(x) seja igual a
um valor qualquer g diferente de zero,
basta minimizar para f(x)-g e tentar
chegar em g:
 Descida de gradiente para f(x)=g
df/dx
f(xi)
f-axis
g xi+1 xi

x-axis
Algoritmo Descida de Gradiente
Parando a descida

necessrio parar a descida em algum

ponto.
Idealmente, paramos quando
chagamos no objetivo, levando em
conta alguma tolerncia.
Porm, existem casos onde podemos
ficar presos em uma determinada
regio:
Problemas de mnimo local.
Calculo do Jacobiano
Precisamos calcular o jacobiano e logo invert-lo. As
colunas do jacobiano so as derivadas parciais de f no
ponto q0. Como da representao de D-H temos

onde s Ri depende de i e s Ti depende de ri ,

pela regra do produto as derivadas parciais so
calculadas como sendo:

i
f (q ) R1T1 Ri Ti RnTn x,

ri
f (q ) R1T1 RiTi RnTn x.

Por isso necessrio derivar matrizes de rotao e

translao.
 Calculo do Jacobiano
Derivada de uma matriz de rotao R:
RR T 1 R RT R (R )T 0 R RT (R R T )T
Ento a velocidade angular R R T uma matriz
antissimtrica, que est relacionada com o eixo de rotao
( x, y, z )
via
0 z y
0
z
0 x
0
y x
0 0
0 0 0 0

Finalmente, R R RT R R.
Calculo do Jacobiano
Derivada de uma matriz de translao T:
Por exemplo:

1 0 0 0 0 0 0 0
0 1 0 r 0 0 0 1
T T
0 0 1 0 0 0 0 0
0 0 0 1 0 0 0 0
 Calculo do Jacobiano
Retomando nosso exemplo, se
x c 1 s 1 0 1 0 r1 c 2 s 2 0 1 0 r2 0
y s 1 c 1 0 0 1 0 s 2 c 2 0 0 1 0 0
1 0 0 1 0 0 1 0 0 1 0 0 1 1

ento o jacobiano resulta ser:

c 1
r2 s( 1 2
) r1 s 1
c( 1 2
) r2 s( 1 2
)
J s 1 r2 c( 1 2
) r1 c 1
s( 1 2
) r2 c( 1 2
)
0 0 0 0

O vetor das coordenadas articulares

q (r1, 1, r2 , 2 )
Calculo do Jacobiano
Inversa generalizada:
A inversa do jacobiano tal que, dada uma pequena
variao da posio do extremo, possvel calcular a
variao nas coordenadas articulares:
q J1 x
Em geral, no existe a inversa do jacobiano, mas sim uma
inversa generalizada B, que cumpre alguma das
condies de Moore-Penrose:
1. JBJ J 2. BJB B
3. (JB )T JB 4. (BJ )T BJ
Se B cumpre todas as quatro condies, dita pseudo-
inversa, e nica: B J
Achar a inversa generalizada um processo lento e que
no lida adequadamente com singularidades.
Invertendo o Jacobiano

Se o Jacobiano quadrado, como no

caso de um sistema invertvel, basta
inverter a matriz:
1 1
J J JJ I
Para obter a inversa, calcula-se a
matriz dos cofatores
Invertendo uma matriz...

Mtodo geral:

Exemplo para uma matriz 2x2:

Pseudo-Inversa

Se tivermos uma matriz jacobiana que

representa um sistema overconstrained
ou underconstrained, podemos utilizar a
pseudo-inversa do Jacobiano:
T 1 T
J (J J) J
A vantagem, que a pseudo-inversa
uma matriz quadrada.
http://en.wikipedia.org/wiki/MoorePenrose_pseudoinverse
 Calculo do Jacobiano
Transposta do jacobiano:
Em lugar de utilizar a pseudo-inversa do jacobiano, pode-se
utilizar a transposta:
q JT x
muito mais barato e ainda por cima evita problemas com
singularidades.
Esta aproximao motivada por consideraes fsicas
(trabalho virtual).

Para resolver certos problemas de escala, pode-se

introduzir um fator de escala h, e iterar at atingir a
convergncia:
q ( i 1) h J T x ( i )
Algoritmo de Cinemtica Inversa
usando o Jacobiano
representa o vetor de estado atual
das posies das juntas:
1 2 ... M
e representa os valores atuais de
posio e orientao do efetuador:
e e1 e2 ... eN
g representa o valor desejado para o
atuador (goal).
Algoritmo de Cinemtica Inversa
usando o Jacobiano
Imagine um rob 2D com 2 juntas
rotacionais:

e=[ex ey]

1
 Algoritmo de Cinemtica Inversa
usando o Jacobiano
A matriz Jacobiana J(e, ) mostra como
cada componente do vetor e varia, com
respeito a cada junta:

ex ex
1 2
J e, 2
ey ey
1 2
1
Algoritmo de Cinemtica Inversa
usando o Jacobiano
while (e estiver longe demais de g) {
Compute J(e, ) para a pose atual
Compute J-1 // inverta a matriz Jacobiana
e = (g - e) // escolha um passo
apropriado
= J-1 e // compute as mudanas nas
juntas
= + // aplique as mudanas nas
juntas
Compute o novo e // utilize cinemtica direta
// para ver onde voc
// foi parar..
}
Escolhendo e

Queremos um valor de e que vai

deixar o atuador mais prximo de g. Um
chute inicial pode ser:
e=g-e
Infelizmente, devido a no linearidade,
devemos tomar passos menores na
direo desejada:
e = (g - e), onde 1