Robótica
Prof. Reinaldo Bianchi
Centro Universitário da FEI
2013
5a Aula
Parte A - Cinemática Inversa
Numérica
Objetivos desta aula

Modelo cinemático inverso:
– Métodos analíticos (ou soluções
fechadas):
• Geométrico (por Trigonometria).
• Algébrico.
– Métodos numéricos:
• Modelo recursivo utilizando a matriz Jacobiana.


Matlab.
Exercícios.
Objetivos desta aula

Já visto na
aula passada.
Modelo cinemático inverso:
– Métodos analíticos (ou soluções
fechadas):
• Geométrico (por Trigonometria).
• Algébrico.
– Métodos numéricos:
• Modelo recursivo utilizando a matriz Jacobiana.

Matlab.
Relembrando a aula passada
Cinemática Inversa
K-1
( 1 …
 n)
(x, y, z, x, y, z)
Cinemática Inversa

Como o próprio nome diz:
– Como encontrar as posições das juntas
dadas a posição e a orientação da
ferramenta.

Problema complexo:
– Planejamento de trajetória
– Dinâmica.
Cinemática Inversa


Enquanto a função f() é relativamente fácil de
computar, f-1() geralmente não o é.
Pode ser solucionado de diversas maneiras:
– Geometricamente.
– Algebricamente.
– Numericamente.

Maior problema é que podem existir:
– Nenhuma solução.
– Uma solução.
– Múltiplas soluções.
Soluções analíticas x numéricas

Soluções do problema da cinemática
inversa podem ser classificadas em:
– Analíticas (ou soluções fechadas):
• Encontram uma solução exata através da inversão
das equações de cinemática direta.
• É possível apenas para problemas simples.
– Numéricas:
• Utilizam aproximação e diversas iterações para
tentar convergir para a solução.
• Tendem a ser mais genéricos e
computacionalmente mais custosos.
Soluções de forma fechada

“Forma fechada” significa:
– um método de solução baseado em
expressões analíticas ou na solução
de um polinômio de grau 4 ou menor.
– Apenas cálculos não iterativos são
suficientes para chegar a uma
solução.
Exemplo 3: Manipulador 3R

Como trabalhamos com um
manipulador planar, a especificação
desses pontos alvos pode ser obtida
com mais facilidade especificando-se
três números: x, y e ϕ, sendo ϕ a
orientação do elo 3 no plano.
Solução Geométrica 3R
ϕ
Solução analítica 3R

Igualando as duas matrizes, chegamos
a um conjunto de quatro equações não
lineares que devem ser resolvidas para
θ1, θ2 e θ3:
cϕ = c123,
(4.8)
sϕ = s123,
(4.9)
x = l1c1 + l2c12,
(4.10)
y = l1s1 + l2s12.
(4.11)
Cinemática Inversa 3R

Os ângulos são encontrados utilizando
as seguintes equações:
æ x 2 + y2 + l 2 - l 2 ö
1
2 ÷
q1 = atan 2(y, x) ± arccos çç
÷
2
2
è 2l1 x + y ø
æ x 2 + y 2 - l12 - l22 ö
q 2 = arccos ç
÷
2l1l2
è
ø
q3 = f - (q1 + q2 )
Métodos Numéricos

Por sua natureza iterativa, as soluções
numéricas em geral são muito mais
lentas do que suas correspondentes de
forma fechada:
– Para a maioria das aplicações não
estamos interessados na abordagem
numérica para as soluções cinemáticas.

Métodos de solução numérica iterativos
serão vistos na próxima aula.
Revisão de cálculo
Antes de entrar nos métodos
analíticos, precisamos nos
lembrar da matemática…
Derivada de uma função escalar



Se tivermos uma função escalar f com
uma única variável x, podemos escrevêla como f(x).
A derivada da função em respeito a x é
df/dx.
A derivada é definida como:
Derivada de uma função escalar
f-axis
Slope=df/dx
x-axis
f(x)
x
Derivada de
2
f(x)=x
Gradientes

Gradiente é uma derivada de primeira
ordem de uma função em relação suas
variáveis:

Dá informações sobre a taxa de
variação de uma função em relação a
variáveis independentes.
Gradiente
Gradiente é a normal à superfície
Gradiente é a normal à superfície
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 são comuns nos
campos de:
– Dinâmica dos fluidos,
– Equações 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 função f(x), o Jacobiano é a
matriz de derivadas parciais para cada
componente dos vetores
O Jacobiano contém toda a informação
necessária para relacionar uma mudança em
um componente de x a uma mudança em um
componente de f
O Jacobiano é geralmente escrito como J(f,x):
– Mas na prática, equivale conceitualmente a df/dx
Jacobiano
é ¶f1
ê ¶x
ê 1
¶f
df ê 2
J (f,x ) =
= ê ¶x1
dx ê
...
ê ¶f M
ê ¶x
ë 1
¶f1
¶x 2
¶f 2
¶x 2
...
...
...
...
...
...
¶f1 ù
ú
¶x N
ú
... ú
ú
... ú
¶f M ú
ú
¶x N û
Exemplo: Robô 2R
é x ù é f1 (q1, q 2 )
ê
ú=ê
êë y úû êë f2 (q1, q 2 )
ù é l cosq + l cos(q + q )
1
2
1
2
ú=ê 1
úû êë l1 sinq1 + l2 sin(q1 + q 2 )
ù
ú
úû
(x , y)
2
l2
1 l1
é ¶f1
ê ¶q
J ( f ,q ) = ê 1
ê ¶f 2
êë ¶q1
¶f1 ù
¶q 2 ú é-l1 sin q1 - l2 sin(q1 + q 2 ) -l2 sin(q1 + q 2 ) ù
ú=
¶f 2 ú êë l1 cos q1 + l2 cos(q1 + q 2 ) l2 cos(q1 + q 2 ) úû
¶q 2 úû
Derivadas parciais


O uso do símbolo “∂” em vez de “d”
para derivadas parciais indica que é um
componente em um vetor de derivadas.
Para propósitos práticos, as derivadas
parciais se comportam como uma
derivada de um escalar por outro.
(Re)visão de Cálculo Numérico
Exato x Aproximado


Muitos algoritmos necessitam da
computação da derivada.
Em alguns casos é possível computar
analiticamente a derivada.
– Por exemplo:
f ( x) = x
2
df
= 2x
dx
Exato x Aproximado


Em outros casos a função a ser
derivada é muito complexa,
impossibilitando o cálculo exato.
Mas, desde que possamos computar a
função, podemos aproximar a derivada:
df f ( x + Dx ) - f ( x )
»
dx
Dx
para valores pequenos de Dx
Derivada aproximada
f-axis
Slope=Δf/Δx
x-axis
f(x)
f(x+Δx)
Δx
Valores próximos

Se sabemos o valor da função em algum
ponto x, podemos estimar o valor da função
em pontos próximos a ele.
Método de Descida de Gradiente

Existem diversas maneiras de computar
aproximadamente as raízes de uma
função:
– valores de x que torna f(x) = 0.


Uma maneira é o “Método de descida
de Gradiente”.
É um método de otimização bem
conhecido.
Idéia central

Se pudermos computar f(x) e df/dx para
qualquer valor de x, podemos sempre
seguir o gradiente na direção do valor
zero.
Idéia central

Se pudermos computar f(x) e df/dx para
qualquer valor de x, podemos sempre
seguir o gradiente na direção do valor
zero.
Método 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 próximos ao valor
desejado.
Podemos utilizar a derivada como uma
aproximação da inclinação da função.
Descida de Gradiente
df/dx
f(xi)
f-axis
xi
x-axis
Escolhendo Δx

Se a função utilizada variar muito:
– É mais prudente andar em passos
pequenos.

Se a função que se deseja minimizar é
bem comportada:
– Pode-se tentar aproximações 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 função não for bem comportada, não
podemos aproximar linearmente Δx.

Uma modificação possível adiciona o
parâmetro β para diminuir o passo, onde
0≤ β ≤1:

β é a “taxa de aprendizado”.
Descida de gradiente
df/dx
f(xi)
f-axis
xi+1 xi
x-axis
Exemplo de descida de gradiente
Minimização




Se o f(x) desejado não for 0, o valor
desejado pode ser considerado um erro.
O objetivo do método de descida de
gradiente é minimizar este erro.
Cada passo nos leva mais próximos da
solução, e paramos quando estivermos
perto o suficiente da resposta desejada.
Este processo iterativo é comum na
maioria dos algoritmos numéricos.
Minimizando f(x)=g

Se desejamos encontrar o valor de x
para quando a função 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



É necessário parar a descida em algum
ponto.
Idealmente, paramos quando
chagamos no objetivo, levando em
conta alguma tolerância.
Porém, existem casos onde podemos
ficar presos em uma determinada
região:
– Problemas de mínimo local.
Cinemática e o Jacobiano
Objetivo final do atuador



“Θ” representa o vetor de estado atual
das posições das juntas:
Q = éë q1 q 2 ... q M ùû
“e” representa os valores atuais de
posição e orientação do efetuador:
e = éë e1 e2 ... eN ùû
“g” representa o valor desejado para o
atuador (goal).
Exemplo 4: manipulador 2R

Imagine um robô 2D com 2 juntas
rotacionais:
• e=[ex ey]
θ2
θ1
Exemplo 4: Jacobiano 2R

A matriz Jacobiana J(e, Θ) mostra como
cada componente do vetor e varia, com
respeito a cada junta:
é
ê
ê
J ( e, Q) = ê
ê
êë
¶ex
¶q1
¶ey
¶q1
¶ex ù
ú
¶q 2 ú
¶ey ú
ú
¶q 2 úû
•
θ2
θ1
Exemplo 4: variação em θ 1

O que acontece ao vetor e se variarmos θ1
um pouco?
é
¶e ê ¶ex
=
¶q1 êë ¶q1
ù
¶ey
ú
¶q1 úû
θ1
•
Exemplo 4: variação em θ 2

O que acontece ao vetor e se variarmos θ2
um pouco?
é
¶e ê ¶ex
=
¶q 2 êë ¶q 2
¶ey
¶q 2
ù
ú
úû
•
θ2
Jacobiano x domínio


Da mesma maneira que uma derivada
escalar df/dx de uma função f(x) pode
variar sobre o domínio de valores de x,
o Jacobiano J(e,Θ) varia sobre o
domínio de poses de Θ.
Para cada valor de pose de Θ, pode-se
calcular os componentes individuais do
Jacobiano.
Mudanças incrementais de pose

Se tivermos uma mudança ΔΘ que
representa uma pequena mudança nos
valores das juntas, a mudança em e
pode ser aproximada por:
De » J ( e,Q) × DQ = J× DQ
Utilização do jacobiano


Se desejarmos mudar a posição final do
atuador em Δe, que mudança em ΔΘ
devemos realizar?
A matriz Jacobiana J(e,Θ) mostra como
cada componente do vetor e varia, com
respeito a cada junta:
De » J× DQ

Para se obter a posição a partir da pose,
basta usar:
-1
DQ » J × De
Mudanças na posição do atuador
Δe
•
θ2
-1
DQ » J × De
θ1
Mudanças na posição do atuador



Podemos utilizar o Jacobiano para
calcular valores próximos da posição
atual.
Lembre-se que a cinemática direta
envolve funções não lineares.
Assim, temos que repetir o cálculo do
Jacobiano a cada passo, até
chegarmos na posição desejada...
Escolhendo Δe


Queremos um valor de Δe que vai
deixar o atuador mais próximo de g. Um
chute inicial pode ser:
Δe = g - e
Infelizmente, devido a não linearidade,
devemos tomar passos menores na
direção desejada:
Δe = β(g - e), onde 0≤ β ≤1
Algoritmo de Cinemática 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 mudanças nas
juntas
Θ = Θ + ΔΘ
// aplique as mudanças nas
juntas
Compute o novo e // utilize cinemática direta
// para ver onde você
// foi parar..
}
Algumas questões…




Como computar o Jacobiano J ?
Como inverter J para computar J -1 ?
Como escolher β (step size)
Como determinar quando parar de
realizar iterações (ou seja, qual o erro
permitido)?
Calculando o Jacobiano
Calculando o Jacobiano


Para calcular a matriz jacobiana completa, é
necessário calcular f/x para cada junta
existente.
É possível para qualquer tipo de junta:
– Prismática
– Rotacional,
– Esféricas ...

Caso seja impossível calcular o Jacobiano
analiticamente, este valor pode ser
encontrado geometricamente ou aproximado
por um método numérico...
Jacobiano
é ¶f1
ê ¶x
ê 1
¶f
df ê 2
J (f,x ) =
= ê ¶x1
dx ê
...
ê ¶f M
ê ¶x
ë 1
¶f1
¶x 2
¶f 2
¶x 2
...
...
...
...
...
...
¶f1 ù
ú
¶x N
ú
... ú
ú
... ú
¶f M ú
ú
¶x N û
Exemplo: Robô 2R
é x ù é f1 (q1, q 2 )
ê
ú=ê
êë y úû êë f2 (q1, q 2 )
ù é l cosq + l cos(q + q )
1
2
1
2
ú=ê 1
úû êë l1 sinq1 + l2 sin(q1 + q 2 )
ù
ú
úû
(x , y)
2
l2
1 l1
é ¶f1
ê ¶q
J ( f ,q ) = ê 1
ê ¶f 2
êë ¶q1
¶f1 ù
¶q 2 ú é-l1 sin q1 - l2 sin(q1 + q 2 ) -l2 sin(q1 + q 2 ) ù
ú=
¶f 2 ú êë l1 cos q1 + l2 cos(q1 + q 2 ) l2 cos(q1 + q 2 ) úû
¶q 2 úû
Encontrando o Jacobiano usando
geometria


Pode-se calcular o Jacobiano
geometricamente.
e é um vetor 3D representando a
posição do atuador no espaço real.
– O Jacobiano será uma matriz 3 x N, onde N
é o número de juntas.

Para cada junta i, devemos analisar
como e modifica o valor de θi.
Juntas rotacionais (1R)


É necessário fixar um eixo e um ponto de
rotação no espaço do mundo.
A partir dos dados da configuração do
manipulador, temos:
– o offset ri da junta relativa ao seu elo.
– o eixo de rotação ai relativo a seu elo.

Podemos encontrar o eixo de rotação e o
ponto de rotação (pivo) em coordenadas do
mundo através das transformações
homogêneas...
Juntas rotacionais (1R)

Os valores em coordenadas do mundo são
dados por:
a¢i = a i × Wi- parent
ri¢ = ri × Wi- parent
Juntas rotacionais (1R)

Agora que temos o eixo e o ponto de apoio
em coordenadas do mundo, podemos
calcular como e modifica θ se rotacionarmos
a junta:
¶e
= a¢i ´ ( e - ri¢)
¶qi

Este é o valor de uma coluna do jacobiano
Juntas rotacionais (1R)
¶e
¶q i
¶e
= a¢i ´ ( e - ri¢)
¶qi
a’i: unit length rotation axis in world space
r’i: position of joint pivot in world space
e: end effector position in world space
•
e
e - ri¢
ri¢ •
a¢i
Juntas rotacionais 3R em 3D


Complica um pouco ser em 3D...
Devemos considerar translações 3D e
diversas rotações, para encontrar os
valores no espaço do mundo.
Juntas rotacionais 3R em 3D
x - dof :
y - dof :
z - dof :


( )
a¢i = [1 0 0 0]× Ry q y × Rz (q z )× Wparent
a¢i = [0 1 0 0]× Rz (q z )× Wparent
a¢i = [0 0 1 0]× Wparent
Onde Rx(θx), Ry(θy), e Rz(θz) são rotações em x, y, e
z e T(r) é uma translação de um offset.
A coordenada do mundo fica:
( )
W = Rx (q x )× Ry q y × Rz (q z )× T(r )× Wparent
Juntas rotacionais 3R em 3D

Para cada eixo no espaço do mundo
criamos uma coluna no jacobiano:
– Essencialmente funciona como 3 juntas 1R
no espaço 3D.

Podemos utilizar a mesma fórmula:
¶e
= a¢i ´ ( e - ri¢)
¶qi

Repetimos este cálculo para os 3 eixos
x, y e z.
Juntas prismáticas em 3D



Funciona da mesma maneira, porém
não existe rotação.
Basta realizar uma translação ao longo
de um eixo arbitrário ai definido no
espaço do elo anterior.
Podemos usar:
a¢i = a i × Wi- parent
Juntas prismáticas em 3D

Como na 1R em 3D, é necessário
computar cada coluna do Jacobiano:
¶e
= a¢i
¶qi

Uma mudança translacional resulta
simplesmente em uma translação no
espaço do mundo.
– Computação trivial.
Juntas prismáticas em 3D
•
•
a¢i
¶e
¶q i
Invertendo o Jacobiano
Sistemas inversíveis



Se o número de graus de liberdade no
atuador for igual ao número de graus de
liberdade das juntas, o sistema é
chamado de inversível, ou bemcomportado.
As juntas devem estar arrumadas de
maneira que seus eixos não sejam
redundantes.
Jacobiano é uma matriz quadrada.
Sistemas redundantes
(underconstrained)



Se o sistema tiver mais graus de
liberdades nas juntas do que no
atuador, existirá um contínuo de
soluções redundantes para uma
determinada posição desejada.
Este sistemas são chamados
redundantes ou underconstrained.
Jacobiano tem mais colunas do que
linhas.
Sistemas overconstrained

Se existirem mais graus de liberdade no
atuador do que nas juntas o sistema é
chamado overconstrained:
– pode não existir soluções para um
determionado problema.
– Sistemas pouco comuns.

Jacobiano possui mais linhas do que
colunas.
Invertendo o Jacobiano

Se o Jacobiano é quadrado, como no
caso de um sistema invertível, “basta”
inverter a matriz:
-1
-1
J J = JJ = I

Para obter a inversa, calcula-se a
matriz dos cofatores…
Invertendo uma matriz...

Método 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:
+
-1
J = (J J) J

T
T
A vantagem, é que a pseudo-inversa é
uma matriz quadrada.
http://en.wikipedia.org/wiki/Moore–Penrose_pseudoinverse
Quando parar?

Três condições de parada:
– Encontrou-se uma solução desejada (dentro de
certa tolerância).
– O sistema ficou parado em um mínimo local.
– Ele levou tempo demais (número máximo de
iterações).


Basta monitorar o progresso nos valores de Θ
Estas regras devem ser codificadas em um
comando while() e serem verificadas sempre.
Outros métodos possíveis

Existem dezenas de técnicas de cálculo
numérico que podem ser usadas:
– Foi mostrada apenas uma.

Outros métodos usados:
– Método de Newton-Raphson
– Algoritmo de Levenberg–Marquardt
– Método dos Mínimos quadrados
–…
Método de Newton


A comparison of
gradient descent
(green) and
Newton's method
(red) for minimizing
a function.
Newton's method
uses curvature
information to take a
more direct route.
E no Matlab?
A função ikine no Matlab

A solução é encontrada iterativamente
utilizando a pseudo-inversa do
Jacobiano:
–
– A solução é geral (válida para qualquer
robô).
– Utiliza o método de descida de gradiente.
– Mais lenta que soluções específicas.
ikine.m – só o que importa
while true
% update the count and test against iteration limit
count = count + 1;
if count > opt.ilimit
error('ikine: iteration limit %d exceeded (row %d), i, nm);
e = tr2delta( robot.fkine(q'), T); % compute the error
J = jacob0(robot, q);
% compute the Jacobian
% compute change in joint angles to reduce the error,
dq = opt.alpha * pinv( J(m,:) ) * e(m);
end
q = q + dq';
% update the estimated solution
if norm(e(m))<= opt.tol
break
% stopping condition
Usando ikine

r.ikine(T, Q, M), onde:
– R = robô;
– T = transformada com a posição desejada
– Q = situação atual do robô
– M = matriz máscara:
– If the manipulator has fewer than 6 DOF then this method of solution will
fail, since the solution space has more dimensions than can be spanned by
the manipulator joint coordinates. In such a case it is necessary to provide
a mask matrix, M, which specifies the Cartesian DOF (in the wrist
coordinate frame) that will be ignored in reaching a solution. The mask
matrix has six elements that correspond to translation in X, Y and Z, and
rotation about X, Y and Z respectively. The value should be 0 (for ignore)
or 1. The number of non-zero elements should equal the number of
manipulator DOF.
Dados gerados para a solução

Modificando ikine.m, podemos pedir
que se imprima diversos gráficos:
– Posição
– Derivada da Posição
– Erro

Modificar false para true na linha
abaixo em ikine.m:
opt.plot = false;
Comandos para criar um 3R

Criando os links:
L1 = Link([0 0 1 0])
L2 = Link([0 0 1 0])
L3 = Link([0 0 1 0])

Criando o robô:
r = SerialLink([L1 L2 L3])
r.ikine(T, Q, M)

Definindo T:
T = [1 0 0 1; 0 1 0 1; 0 0 1 0; 0 0 0 1]

Definindo Q:
Q = [ 2* pi/3 2*pi/3 2*pi/3]

Definindo M:
M = [ 1 1 0 0 0 1]

Usando ikine():
r.ikine (T, Q, M)
ans = 1.8235 2.6362
1.8235
Posicão [ 2π/3 2π/3 2π/3]
Resultado
Evolução dos ângulos Θ
4
3.5
3
q
2.5
2
1.5
1
0.5
0
20
40
60
80
iteration
100
120
140
160
Evolução das derivadas dΘ
Evolução do erro (e – g)
Evolução do |erro| em monolog
Usando plot


Eu modifiquei o ikine para gravar os
pontos por onde passou.
Assim, podemos ver o robô se
movendo:
[a,b,pontos] =r.ikine (T, Q, M)
r.plot(pontos,'delay',0.2)
Conclusão

Técnicas analíticas são restritas.
– Mas resolvem diversos problemas práticos
(PUMA, Stanford-Arm, 1R, 2R, 3R)

Existem dezenas de técnicas de cálculo
numérico que podem ser usadas:
– Foi mostrada apenas uma.

Quando usar cada um:
– depende do problema!
Bibliografia


Capítulos 4 e 5 do Craig.
Robot Manipulators: Mathematics,
Programming, and Control
– Paul, R. P. - 1982 - MIT Press.

Robot Analysis: The Mechanics of
Serial and Parallel Manipulators
– Lung-Wen TSAI - 1999 - John Wiley.
Links

Introduction to Inverse Kinematics with
Jacobian Transpose, Pseudoinverse
and Damped Least Squares methods
– http://math.ucsd.edu/~sbuss/ResearchWeb
/ikmethods/iksurvey.pdf
Intervalo
Continuação no Lab com Matlab
Exercícios.