robotica 05 a cinematica inversa numerica

5/19/2018 ROBOTICA 05 a Cinematica Inversa Numerica

1/107

Robtica

Prof. Reinaldo Bianchi

Centro Universitrio da FEI2013


2/107

5aAula

Parte A - Cinemtica Inversa

Numrica


3/107

Objetivos desta aula

Modelo cinemtico inverso: Mtodos analticos (ou solues

fechadas): Geomtrico (por Trigonometria).

Algbrico.

Mtodos numricos: Modelo recursivo utilizando a matriz Jacobiana.

Matlab.

Exerccios.


4/107

Objetivos desta aula

Modelo cinemtico inverso: Mtodos analticos (ou solues

fechadas): Geomtrico (por Trigonometria).

Algbrico.

Mtodos numricos: Modelo recursivo utilizando a matriz Jacobiana.

Matlab.

J visto na

aula passada.


5/107

Relembrando a aula passada


6/107

Cinemtica Inversa

(1 n) (x, y, z, x, y, z)

K-1


7/107

Cinemtica Inversa

Como o prprio nome diz: Como encontrar as posies das juntas

dadas a posio e a orientao daferramenta.

Problema complexo:

Planejamento de trajetria Dinmica.


8/107

Cinemtica Inversa

Enquanto a funo f() relativamente fcil decomputar, f-1() geralmente no o .

Pode ser solucionado de diversas maneiras: Geometricamente.

Algebricamente.

Numericamente.

Maior problema que podem existir: Nenhuma soluo.

Uma soluo.

Mltiplas solues.


9/107

Solues analticas x numricas

Solues do problema da cinemticainversa podem ser classificadas em:

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 ecomputacionalmente mais custosos.


10/107

Solues de forma fechada

Forma fechada significa: um mtodo de soluo baseado em

expresses analticas ou na soluode um polinmio de grau 4 ou menor.

Apenas clculos no iterativos so

suficientes para chegar a umasoluo.


11/107

Exemplo 3: Manipulador 3R

Como trabalhamos com ummanipulador planar, a especificao

desses pontos alvos pode ser obtidacom mais facilidade especificando-setrs nmeros:x,ye , sendo a

orientao do elo 3 no plano.


12/107

Soluo Geomtrica 3R


13/107

Soluo analtica 3R

Igualando as duas matrizes, chegamosa um conjunto de quatro equaes no

lineares que devem ser resolvidas para1, 2e 3:

c= c123, (4.8)

s=s123, (4.9)x = l1c1 + l2c12, (4.10)

y = l1s1 + l2s12. (4.11)


14/107

Cinemtica Inversa 3R

Os ngulos so encontrados utilizandoas seguintes equaes:

3 = 1 + 2( )

1 =atan2(y,x) arccos x2 +y2 + l1

2 l2

2

2l1 x2+y2

2=arccos x

2

+y2

l12

l22

2l1l2


15/107

Mtodos Numricos

Por sua natureza iterativa, as soluesnumricas em geral so muito mais

lentas do que suas correspondentes deforma fechada: Para a maioria das aplicaes no

estamos interessados na abordagemnumrica para as solues cinemticas.

Mtodos de soluo numrica iterativossero vistos na prxima aula.


16/107

Reviso de clculo

Antes de entrar nos mtodos

analticos, precisamos noslembrar da matemtica


17/107

Derivada de uma funo escalar

Se tivermos uma funo escalarfcomuma nica varivelx, podemos escrev-

la comof(x).A derivada da funo em respeito ax

df/dx.

A derivada definida como:


18/107

Derivada de uma funo escalar

f-axis

x-axis x

f(x)Slope=df/dx


19/107

Derivada def(x)=x2


20/107

Gradientes

Gradiente uma derivada de primeiraordem de uma funo em relao suas

variveis:

D informaes sobre a taxa devariao de uma funo em relao avariveis independentes.


21/107

Gradiente


22/107

Gradiente a normal superfcie


23/107

Gradiente a normal superfcie


24/107

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?


25/107

Derivadas vetoriais

Derivadas de valores escalares porvalores 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.


26/107

Jacobianos

Um Jacobiano a derivada de um vetor poroutro.

Se tivermos uma funof(x), o Jacobiano a

matriz de derivadas parciais para cadacomponente dos vetores

O Jacobiano contm toda a informao

necessria para relacionar uma mudana emum componente dexa uma mudana em umcomponente def

O Jacobiano geralmente escrito como J(f,x):

Mas na prtica, equivale conceitualmente a df/dx


27/107

Jacobiano

J f,x( ) =df

dx=

f1 x1

f1 x2

... f1 xN

f2

x1

f2

x2... ...

... ... ... ... fM

x1... ...

fM

xN


28/107

Exemplo: Rob 2R

2

1

(x , y)

l2

l1

x

y

=f1( 1, 2 )

f2 ( 1, 2 )

=

l1 cos 1 + l2cos( 1 + 2 )

l1 sin 1 + l2sin( 1 + 2 )

J f,( ) =

f1 1

f1 2

f2

1

f2

2

= l1 sin 1 l2sin( 1 + 2 ) l2sin( 1 + 2 )

l1 cos 1 +l2cos( 1 + 2) l2cos( 1 + 2 )


29/107

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.


30/107

(Re)viso de Clculo Numrico


31/107

Exato x Aproximado

Muitos algoritmos necessitam dacomputao da derivada.

Em alguns casos possvel computaranaliticamente a derivada. Por exemplo:

f x( ) =x 2 dfdx

= 2x


32/107

Exato x Aproximado

Em outros casos a funo a serderivada muito complexa,

impossibilitando o clculo exato. Mas, desde que possamos computar a

funo, podemos aproximar a derivada:

df

dx

f x +

x( )

f x( )

x

para valores pequenos de x


33/107

Derivada aproximada

f-axis

x-axis x

f(x)f(x+x)

Slope=f/x


34/107

Valores prximos

Se sabemos o valor da funo em algumponto x, podemos estimar o valor da funo

em pontos prximos a ele.


35/107

Mtodo de Descida de Gradiente

Existem diversas maneiras de computaraproximadamente as razes de uma

funo: valores dexque tornaf(x) = 0.

Uma maneira o Mtodo de descida

de Gradiente. um mtodo de otimizao bem

conhecido.


36/107

Idia central

Se pudermos computarf(x)e df/dx paraqualquer valor dex, podemos sempre

seguir o gradiente na direo do valorzero.


37/107

Idia central

Se pudermos computarf(x)e df/dx paraqualquer valor dex, podemos sempre

seguir o gradiente na direo do valorzero.


38/107

Mtodo de Descida de Gradiente

Iniciaremos em um valorx0e tomaremospequenos passos:

xi+1= xi+ xat encontrarmos um valorxNondef(xN)=0

Para cada passo, tentamos encontrar um valordexque nos colocar mais prximos ao valordesejado.

Podemos utilizar a derivada como umaaproximao da inclinao da funo.


39/107

Descida de Gradiente

f-axis

x-axisxi

f(xi)

df/dx


40/107

Escolhendox

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 quepassam por zero.


41/107

Escolhendox

Se desejarmos aproximar linearmentexpara nos levar ao valor dexondef(x) = 0

podemos usar:


42/107

Descida de gradiente

f-axis

x-axisxi

f(xi)

df/dx

xi+1


43/107

Utilizando passos menores

Se a funo no for bem comportada, no

podemos aproximar linearmentex.

Uma modificao possvel adiciona oparmetropara diminuir o passo, onde

0 1:

a taxa de aprendizado.


44/107

Descida de gradiente

f-axis

x-axisxi

f(xi)

df/dx

xi+1


45/107

Exemplo de descida de gradiente


46/107

Minimizao

Se of(x) desejado no for 0, o valordesejado pode ser considerado um erro.

O objetivo do mtodo de descida degradiente minimizar este erro.

Cada passo nos leva mais prximos da

soluo, e paramos quando estivermosperto o suficiente da resposta desejada.

Este processo iterativo comum na

maioria dos algoritmos numricos.


47/107

Minimizando f(x)=g

Se desejamos encontrar o valor dexpara quando a funof(x) seja igual a

um valor qualquer g diferente de zero,basta minimizar paraf(x)-g e tentarchegar emg:


48/107

Descida de gradiente paraf(x)=g

f-axis

x-axis

xi

f(xi)

df/dx

gx

i+1


49/107

Algoritmo Descida de Gradiente


50/107

Parando a descida

necessrio parar a descida em algumponto.

Idealmente, paramos quandochagamos no objetivo, levando emconta alguma tolerncia.

Porm, existem casos onde podemosficar presos em uma determinadaregio:

Problemas de mnimo local.


51/107

Cinemtica e o Jacobiano


52/107

Objetivo final do atuador

representa o vetor de estado atual

das posies das juntas:

e representa os valores atuais de

posio e orientao do efetuador:

g representa o valor desejado para o

atuador (goal).

Q =

1

2 ...

M

e = e1 e2 ... eN


53/107

Exemplo 4: manipulador 2R

Imagine um rob 2D com 2 juntasrotacionais:

1

2

e=[exey]


54/107

Exemplo 4: Jacobiano 2R

A matriz Jacobiana J(e,) mostra comocada componente do vetor e varia, com

respeito a cada junta:

J e, Q( ) =

ex

1

ex

2

ey

1

ey

2

2

1


55/107

Exemplo 4: variao em 1

O que acontece ao vetor ese variarmos1um pouco?

1

e

1

= ex

1

ey

1


56/107

Exemplo 4: variao em 2

O que acontece ao vetor ese variarmos 2um pouco?

2

e

2

= ex

2

ey

2


57/107

Jacobiano x domnio

Da mesma maneira que uma derivadaescalar df/dx de uma funof(x) pode

variar sobre o domnio de valores dex,o JacobianoJ(e,) varia sobre odomnio de poses de .

Para cada valor de pose de , pode-secalcular os componentes individuais doJacobiano.


58/107

Mudanas incrementais de pose

Se tivermos uma mudana querepresenta uma pequena mudana nos

valores das juntas, a mudana em epode ser aproximada por:

e J e,Q

( ) Q

= J Q


59/107

Utilizao do jacobiano

Se desejarmos mudar a posio final doatuador em e, que mudana em

devemos realizar?A matriz JacobianaJ(e,)mostra como

cada componente do vetor evaria, comrespeito a cada junta:

Para se obter a posio a partir da pose,basta usar:

e J Q

J

1

e


60/107

Mudanas na posio do atuador

2

1

Q J 1 e

e


61/107

Mudanas na posio do atuador

Podemos utilizar o Jacobiano paracalcular valores prximos da posio

atual. Lembre-se que a cinemtica direta

envolve funes no lineares.

Assim, temos que repetir o clculo doJacobiano a cada passo, atchegarmos na posio desejada...


62/107

Escolhendo e

Queremos um valor de eque vaideixar o atuador mais prximo de g. Um

chute inicial pode ser:e = g - e

Infelizmente, devido a no linearidade,

devemos tomar passos menores nadireo desejada:

e = (g - e), onde 0 1

l i d i i


63/107

Algoritmo de Cinemtica Inversa

usando o Jacobiano

while (eestiver longe demais de g) {Compute J(e,) para a pose atual Compute J-1 // inverta a matriz Jacobiana

e= (g - e) // escolha um passoapropriado

= J-1 e // compute as mudanas nasjuntas

= + // aplique as mudanas nasjuntasCompute o novo e // utilize cinemtica direta

// para ver onde voc// foi parar..


64/107

Algumas questes

Como computar o JacobianoJ?

Como inverterJpara computarJ -1?

Como escolher(step size) Como determinar quando parar de

realizar iteraes (ou seja, qual o erro

permitido)?


65/107

Calculando o Jacobiano


66/107

Calculando o Jacobiano

Para calcular a matriz jacobiana completa, necessrio calcular f/x para cada juntaexistente.

possvel para qualquer tipo de junta: Prismtica Rotacional, Esfricas ...

Caso seja impossvel calcular o Jacobianoanaliticamente, este valor pode serencontrado geometricamente ou aproximadopor um mtodo numrico...


67/107

Jacobiano

J f,x( ) =df

dx=

f1 x1

f1 x2

... f1 xN

f2

x1

f2

x2... ...

... ... ... ... fM

x1... ...

fM

xN


68/107

Exemplo: Rob 2R

2

1

(x , y)

l2

l1

x

y

=f1(

1,

2 )

f2 ( 1, 2 )

=

l1 cos

1 + l2cos(

1 +

2 )

l1 sin 1 + l2sin( 1 + 2 )

J f,( ) =

f1

1

f1

2

f2

1

f2

2

= l1 sin 1 l2sin( 1 + 2 ) l2sin( 1 + 2 )

l1 cos 1 +l2cos( 1 + 2) l2cos( 1 + 2 )

E t d J bi d


69/107

Encontrando o Jacobiano usando

geometria

Pode-se calcular o Jacobianogeometricamente.

e um vetor 3D representando aposio do atuador no espao real. O Jacobiano ser uma matriz 3 x N, onde N

o nmero de juntas. Para cada junta i, devemos analisar

como emodifica o valor de i.


70/107

Juntas rotacionais (1R)

necessrio fixar um eixo e um ponto derotao no espao do mundo.

A partir dos dados da configurao domanipulador, temos: o offset rida junta relativa ao seu elo. o eixo de rotao airelativo a seu elo.

Podemos encontrar o eixo de rotao e oponto de rotao (pivo) em coordenadas domundo atravs das transformaeshomogneas...


71/107


Os valores em coordenadas do mundo sodados por:

a i = a iWi parent

ri = riWi parent


72/107


Agora que temos o eixo e o ponto de apoioem coordenadas do mundo, podemoscalcular como emodifica se rotacionarmosa junta:

Este o valor de uma coluna do jacobiano

e

i

= ai e ri( )


73/107


ai: unit length rotation axis in world spaceri: position of joint pivot in world spacee: end effector position in world space

e

i= ai e ri( )

e

i

ia

e

ire

ir


74/107

Juntas rotacionais 3R em 3D

Complica um pouco ser em 3D...

Devemos considerar translaes 3D e

diversas rotaes, para encontrar osvalores no espao do mundo.


75/107


Onde Rx(x), Ry(y), e Rz(z) so rotaes em x, y, e

z e T(r) uma translao de um offset. A coordenada do mundo fica:

a i = 1 0 0 0[ ]Ry y( )Rz z( )Wparent

a i = 0 1 0 0[ ]Rz z( )Wparent

a i = 0 0 1 0[ ]Wparent:

:

:

dofz

dofy

dofx

W =Rx x( )Ry y( )Rz z( )T r( )Wparent


76/107


Para cada eixo no espao do mundocriamos uma coluna no jacobiano:

Essencialmente funciona como 3 juntas 1Rno espao 3D.

Podemos utilizar a mesma frmula:

Repetimos este clculo para os 3 eixos

x y e z

e i

= ai e ri( )


77/107

Juntas prismticas em 3D

Funciona da mesma maneira, pormno existe rotao.

Basta realizar uma translao ao longode um eixo arbitrrio aidefinido noespao do elo anterior.

Podemos usar:

a i = a iWi parent


78/107


Como na 1R em 3D, necessriocomputar cada coluna do Jacobiano:

Uma mudana translacional resulta

simplesmente em uma translao noespao do mundo. Computao trivial.

e

i

= ai


79/107


a i

e

i


80/107

Invertendo o Jacobiano


81/107

Sistemas inversveis

Se o nmero de graus de liberdade noatuador for igual ao nmero de graus de

liberdade das juntas, o sistema chamado de inversvel, ou bem-comportado.

As juntas devem estar arrumadas demaneira que seus eixos no sejamredundantes.

Jacobiano uma matriz quadrada

Sistemas redundantes


82/107

Sistemas redundantes

(underconstrained)

Se o sistema tiver mais graus deliberdades nas juntas do que no

atuador, existir um contnuo desolues redundantes para umadeterminada posio desejada.

Este sistemas so chamadosredundantes ou underconstrained.

Jacobiano tem mais colunas do que

linhas


83/107

Sistemas overconstrained

Se existirem mais graus de liberdade noatuador do que nas juntas o sistema

chamado overconstrained: pode no existir solues para um

determionado problema.

Sistemas pouco comuns.

Jacobiano possui mais linhas do quecolunas.


84/107

Invertendo o Jacobiano

Se o Jacobiano quadrado, como nocaso de um sistema invertvel, basta

inverter a matriz:

Para obter a inversa, calcula-se amatriz dos cofatores

J 1J=JJ 1 =


85/107

Invertendo uma matriz...

Mtodo geral:

Exemplo para uma matriz 2x2:


86/107

Pseudo-Inversa

Se tivermos uma matriz jacobiana querepresenta um sistema overconstrained

ou underconstrained, podemos utilizar apseudo-inversa do Jacobiano:

A vantagem, que a pseudo-inversa uma matriz quadrada.

J+ = (JTJ) 1JT

http://en.wikipedia.org/wiki/MoorePenrose_pseudoinverse


87/107

Quando parar?

Trs condies de parada: Encontrou-se uma soluo desejada (dentro de

certa tolerncia).

O sistema ficou parado em um mnimo local. Ele levou tempo demais (nmero mximo de

iteraes).

Basta monitorar o progresso nos valores de Estas regras devem ser codificadas em um

comando while() e serem verificadas sempre.


88/107

Outros mtodos possveis

Existem dezenas de tcnicas de clculonumrico que podem ser usadas:

Foi mostrada apenas uma. Outros mtodos usados:

Mtodo de Newton-Raphson

Algoritmo de LevenbergMarquardt Mtodo dos Mnimos quadrados


89/107

Mtodo de Newton

A comparison ofgradient descent(green) andNewton's method(red) for minimizinga function.

Newton's methoduses curvatureinformation to take amore direct route.


90/107

E no Matlab?


91/107

A funo ikine no Matlab

A soluo encontrada iterativamenteutilizando a pseudo-inversa do

Jacobiano:

A soluo geral (vlida para qualquer

rob). Utiliza o mtodo de descida de gradiente.

Mais lenta que solues especficas.


92/107

ikine.ms o que importawhile true

% update the count and test against iteration limitcount = 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);

q = q + dq'; % update the estimated solution

if norm(e(m))


93/107

Usando ikine

r.ikine(T, Q, M), onde: R = rob;

T = transformada com a posio desejada Q = situao atual do rob

M = matriz mscara: 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 bythe manipulator joint coordinates. In such a case it is necessary to providea mask matrix, M, which specifies the Cartesian DOF (in the wristcoordinate frame) that will be ignored in reaching a solution. The maskmatrix has six elements that correspond to translation in X, Y and Z, androtation 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 ofmanipulator DOF.


94/107

Dados gerados para a soluo

Modificando ikine.m, podemos pedirque se imprima diversos grficos:

Posio Derivada da Posio

Erro

Modificar falsepara truena linha abaixoem ikine.m:opt.plot = false;


95/107

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])


96/107

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


97/107

Posico [ 2/3 2/3 2/3]

Resultado


98/107

Resultado


99/107

Evoluo dos ngulos

0 20 40 60 80 100 120 140 1600.5

1

1.5

2

2.5

3

3.5

4

iteration

q


100/107

Evoluo das derivadas d

l d ( )


101/107

Evoluo do erro (eg)

l d | | l


102/107

Evoluo do |erro| em monolog

U d l


103/107

Usando plot

Eu modifiquei o ikine para gravar ospontos por onde passou.

Assim, podemos ver o rob semovendo:[a,b,pontos] =r.ikine (T, Q, M)

r.plot(pontos,'delay',0.2)

C l


104/107

Concluso

Tcnicas analticas so restritas. Mas resolvem diversos problemas prticos

(PUMA, Stanford-Arm, 1R, 2R, 3R) Existem dezenas de tcnicas de clculo

numrico que podem ser usadas:

Foi mostrada apenas uma. Quando usar cada um:

depende do problema!

Bibli fi


105/107

Bibliografia

Captulos 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.

Li k


106/107

Links

Introduction to Inverse Kinematics withJacobian Transpose, Pseudoinverse

and Damped Least Squares methods http://math.ucsd.edu/~sbuss/ResearchWeb/ikmethods/iksurvey.pdf


107/107

Intervalo

Continuao no Lab com Matlab

Exerccios.

robotica 05 a cinematica inversa numerica

Documents