Robótica_Cinemática_Inversa

CINEMÁTICA INVERSA DE MANIPULADORES Robótica I ROBÓTICA I

Prof. Rogério Oliveira www.ifce.edu.br

Robótica I [email protected]

CINEMÁTICA INVERSA A cinemática inversa é o conjunto de processos que determinam as funções inversas do sistema das expressões da cinemática direta. A cinemática inversa determina o conjunto de valores das juntas que se adéquam a uma dada configuração do espaço operacional ou cartesiano.

Cinemática direta

rr r r rr ==FF(q()q )

Espaço cartesiano

Espaço das juntas Cinemática inversa

r −1 r q = F (r )

−1

F :ℜ a ℜ n

6


1 O PROBLEMA DA CINEMÁTICA INVERSA A cinemática inversa nem sempre é um problema com solução analítica, ou por vezes não tem mesmo solução! Mais complexo ainda é o fato de não haver uma metodologia única de aplicação direta. Para as soluções mais usadas já se tem estudos prontos. Só em casos especiais será necessário conceber novas soluções.

−1

F :ℜ a ℜ n

6


Cinemática inversa para um manipulador 1 DOF Planar Cinemática direta x1 = L1 cos( M 1 ) z1 = L1 sen( M 1 )

z

Invertendo para obter a cinemática inversa:  x1  M 1 = arccos   L1 

{1} z1

ou

L1

 z1  M 1 = arcsen   L1 

M1 {0}

x1

x


1.1 Cinemática inversa para um manipulador 2 DOF Planar Cinemática direta x 2 = L1 cos( M 1 ) + L2 cos( M 1 + M 2 )

z

z 2 = L1 sen( M 1 ) + L2 sen( M 1 + M 2 )

{2}

z2 L2

M2

{1}

L1 M1 {0}

x2

x


Cinemática inversa para um manipulador 2 DOF Planar x 2 = L1 cos( M 1 ) + L2 cos( M 1 + M 2 )

Cinemática direta

z 2 = L1 sen( M 1 ) + L2 sen( M 1 + M 2 )

z

{2}

z2 L2

M2

{1}

L1 M1 {0}

x2

x


2 DOF Planar – Calculando M2

2 2 2 2 2 2 2 2

x x x x

x 2 = L1 cos( M 1 ) + L2 cos( M 1 + M 2 )

Cinemática direta

z 2 = L1 sen( M 1 ) + L2 sen( M 1 + M 2 ) Elevando ambos os membros ao quadrado

z

{2}

2

= (L1 cos( M 1 ) + L2 cos( M 1 + M 2 ) ) = L12 cosz22( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L2 cos( M 1 ) cos( M 1 + M 2 ) = L12 cos 2 ( M 1 ) + L22 cos 2 ( ML 1 + M 2 ) + 2 L1 L2 cos(M 1 )(cos(M 1 ) cos(M 2 ) − sen( M 1 ) sen( M 2 ) ) 2 2 2 2 2 2 = L1 cos (M 1 ) + L2 cos (M 1 + M 2 ) + 2LM 1 L2 cos ( M 1 ) cos(M 2 ) − 2 L1 L2 cos(M 1 ) sen( M 1 ) sen( M 2 ) 2

{1}

z 22 = (L1 sen( M 1 ) + LL 1 (M 1 + M 2 )) 2 sen z 22 = L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L 2 sen ( M 1 ) sen ( M 1 + M 2 ) 2

2 z 22 = L12 sen 2 ( M 1 ) + L22M sen 1 ( M 1 + M 2 ) + 2 L1 L2 sen( M 1 )(sen( M 1 ) cos( M 2 ) + sen( M 2 ) cos( M 1 ) )

z 22 = L12 sen2 (M 1 ) + L22 sen2 (M 1 + M 2 ) + 2L1 L2 sen2 (M 1 ) cos(M 2 ) + 2L1 L2 sen(M 1 )sen(M 2 ) cos(M 1 ) {0}

x2

x


2 DOF Planar – Calculando M2 Somando os dois termos x22 = L12 cos2 (M1 ) + L22 cos2 (M1 + M 2 ) + 2L1 L2 cos2 (M1 ) cos(M 2 ) − 2L1 L2 cos(M1 )sen(M1 )sen(M 2 )

z22 = L12 sen2 (M1 ) + L22 sen2 (M1 + M 2 ) + 2L1 L2 sen2 (M1 ) cos(M 2 ) + 2L1 L2 sen(M1 )sen(M 2 ) cos(M1 ) x 22 + z 22 = L12 cos 2 ( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L 2 cos 2 ( M 1 ) cos( M 2 ) − 2 L1 L 2 cos( M 1 ) sen ( M 1 ) sen ( M 2 ) + + L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L 2 sen 2 ( M 1 ) cos( M 2 ) + + 2 L1 L 2 sen ( M 1 ) sen ( M 2 ) cos( M 1 )

x 22 + z 22 = L12 cos 2 ( M 1 ) + L12 sen 2 ( M 1 ) + + L22 cos 2 ( M 1 + M 2 ) + L22 sen 2 ( M 1 + M 2 ) + + 2 L1 L2 cos 2 ( M 1 ) cos( M 2 ) + 2 L1 L2 sen 2 ( M 1 ) cos( M 2 ) +

0

− 2 L1 L2 cos( M 1 ) sen( M 1 ) sen( M 2 ) + 2 L1 L2 sen( M 1 ) sen( M 2 ) cos( M 1 )

1

[

1 1

]

x 22 + z 22 = L12 cos 2 ( M 1 ) + sen 2 ( M 1 ) +

[

] x ( M )] + 0

+ L22 cos 2 ( M 1 + M 2 ) + sen 2 ( M 1 + M 2 ) +

[

+ 2 L1 L2 cos( M 2 ) cos 2 ( M 1 ) + sen 2

1

2 2

+ z 22 = L12 + L22 + 2 L1 L2 cos( M 2 )


2 DOF Planar – Calculando M2

2 2

2 2

2 1

2 2

x + z = L + L + 2 L1 L 2 cos( M 2 )

2 2 2 2  −1 x 2 + z 2 − L1 − L2 M 2 = ± cos  2 L1 L2 

  

•É uma solução analítica em função das dimensões do robô e da posição •Pelo sinal ± mostra que tem duas soluções matemáticas para M2.


2 DOF Planar – Calculando θ1

O cálculo de θ1 é mais trabalhoso Usaremos o recurso trigonométrico de “soma de tangentes”. Usaremos os ângulos auxiliares α e β

M1 = α − β

z

{2}

z2 tan α = x2

z2 L2

M2

β L1

{1}

tan A − tan B tan ( A − B ) = 1 + tan A tan B

α

M1 {0}

L2 senM 2 tan β = L1 + L2 cos M 2

x2

x



M1 = α − β

z

{2}

z2 tan α = x2

z2 L2

M2

β L1

{1}


α

M1 {0}

L2 senM 2 tan β = L1 + L2 cos M 2

x2

x

z


z2 β L1 {0}


{2}

L2

M2

{1}

α

M1

x2

x

tan A − tan B tan ( A − B ) = 1 + tan A tan B L2 senM 2 tan β = L1 + L2 cos M 2 z2 tan α = x2

z 2 (L1 + L2 cos M 2 ) − x 2 L2 senM 2 x 2 (L1 + L2 cos M 2 ) tan (M 1 ) = x 2 ⋅ (L1 + L2 cos M 2 ) + z 2 (L2 senM 2 ) x 2 ⋅ (L1 + L2 cos M 2 )

z 2 (L1 + L2 cos M 2 ) − x 2 L2 senM 2 tan (M 1 ) = x 2 ⋅ (L1 + L2 cos M 2 ) + z 2 (L2 senM 2 )

tan (M 1 ) = tan (α − β ) z2 L2 senM 2 − x2 L1 + L2 cos M 2 tan (M 1 ) = z2 L2 senM 2 1+ ⋅ x2 L1 + L2 cos M 2

 z 2 (L1 + L2 cos M 2 ) − x 2 L2 senM 2   M 1 = tan   x 2 ⋅ (L1 + L2 cos M 2 ) + z 2 (L2 senM 2 )  −1


2 DOF Planar: Cinemática inversa 2 2 2 2  + − − x z L L 2 1 2 M 2 = ± cos −1  2 2 L1 L 2 

  

 z 2 (L1 + L 2 cos M 2 ) − x 2 L 2 senM 2   M 1 = tan   x 2 ⋅ (L1 + L 2 cos M 2 ) + z 2 (L 2 senM 2 )  −1

z y2 Temos duas possíveis soluções:

M 2 '> 0  M 2 " < 0

L2

M’2

L1

θ’1 x2

x



  



M 2 '> 0  M 2 " < 0

M” 2

L2

M”1 L 1

x2

x



  



M 2 '> 0  M 2 " < 0

M” 2

L2

M’2

M”1 L 1

θ’1 x2

x

1.2) 2 DOF não planar (Tridimensional)

ROBÓTICA I




•Equação cinemática já estudada. •Usaremos somente a posição

C1C2 S C R TH =  1 2  S2   0

− C1S 2 − S1S 2

S1 − C1

C2 0

0 0

L2C1C2   nx L2 S1C2  n y = L2 S 2 + L1   nz   1  0

sx sy

ax ay

sz 0

az 0

px  p y  pz   1



pz px

C1C2 S C R TH =  1 2  S2   0

py

O ângulo M1 é obtido pela relação entre px e py. py

L2 S1C2 S1 = = = tan M 1 p x L2C1C2 C1

 py   M 1 = tan   px  −1

− C1S 2

S1

− S1S 2 C2 0

− C1 0 0

L2C1C2   nx L2 S1C2  n y = L2 S 2 + L1   nx   1  0

sx

ax

sy sz 0

ay az 0

O ângulo M2 é obtido pela posição de pz. p z = L1 + L2 senM 2

 p z − L1   M 2 = sen   L2  −1

px  p y  pz   1


1.3 Métodos e condições de existência de soluções

A resolução analítica apresentada para o caso RR planar ou o RR no espaço nem sempre pode ser facilmente estendida a mais graus de liberdade ou dimensões.

Existem vários métodos alternativos na literatura dos quais se destacam os seguintes: – – – – –

Transformações inversas (Paul et al., 1981) Matrizes duais (Denavit, 1956) Quaternions duais (Young & Freudenstein, 1964) Métodos iterativos (Vicker et al., 1964) Abordagens geométricas (Lee & Ziegler, 1984)

Em qualquer dos métodos há porém várias condições e situações para a existência de soluções, e das quais se destacam as seguintes: – O ponto no espaço operacional deve estar no espaço de trabalho – Não se deve exceder os limites físicos das juntas – Para haver solução analítica (num caso de 6 eixos) é suficiente que 3 eixos de juntas sucessivas se interceptem ou sejam paralelos (Solução de Pieper)


1.4 A redundância cinemática 1.

Redundância -> Quando um manipulador

pode atingir uma dada posição no espaço com mais de que uma configuração das juntas. 2. Redundância é uma posição alternativa devido a uma junta, e se for N o número de redundâncias então o número de soluções é dado por 2N. 3. Se o número de soluções for maior que o número de juntas então a resolução da cinemática inversa do manipulador pode trazer dificuldades adicionais, como a impossibilidade de obter uma soluç solução. ão


A redundância cinemática Matematicamente a existência de redundância detecta-se normalmente pelo aparecimento da função cos-1(x) ou de radiciação (√x). 4. A situação de redundância pode ocorrer quando há mais graus de mobilidade do que os necessários. 5. Manipuladores com mais de seis graus de liberdade são ditos infinitamente redundantes. 3.


As quatro soluções de um manipulador antropomórfico usando só as juntas primárias

•Cotovelo em baixo •Cotovelo em baixo •Ombro à direita •Ombro à esquerda •Cotovelo em cima •Ombro à direita

•Cotovelo em cima •Ombro à esquerda


Manipulador com um número infinito de configurações

•Quando o manipulador tem um número infinito de configurações para uma dada posição da garra diz-se que se atingiu uma situação de degeneração.

Há um número infinito de combinações de (θ4,θ6) para o end-effector nas configurações indicadas.

Área de trabalho – RWS e DWS

ROBÓTICA I



Espaço de trabalho RWS

– Reachable workspace

– O espaço que pode ser encontrado pelo endeffector, pelo menos uma vez.

DWS

– Desterous workspace

– O espaço formado pelos pontos que o endeffector pode mais de uma orientação. – É uma subconjunto do RWS.


RWS - 2 DOF planar L1 = 10 L2 = 5 0° < M1 < 180° -90° < M2 < 120°

20 15 10

Z

5 0 -5 -10 -15 -20 -20

RWS -10

0 X

10

20


RWS e DWS - 2 DOF planar L1 = 10 L2 = 5 0° < M1 < 180° -90° < M2 < 120°

20 15 10

Z

5 0 -5 -10 -15 -20 -20

RWS DWS -10

0 X

10

20

RWS e DWS – RR não planar z

RH y

M1

y H M2

10

RH x

x

Z

{R}

L1

20

0

-10

-20 20 10

L1 = 10 L2 = 5 0° < M1 < 180° 0° < M2 < 150°

 L1C1C 2  R TH = − L1 S1C 2   L1 S 2 

20

0

10 0

-10 -20

Y

-10 -20

X

DWS 20

10

Z


RWS

RH z

0

-10

-20 20 10 20

0

10 0

-10 Y

-20

-10 -20

X

RWS - RRR não planar RWS

z 20

H L y 2 M 3

RH

10

RH x

x Z

L1 {R} M1 M 2

0

-10

y

-20 20

L1 = 10 L2 = 5 -45° < M1 < 45° 0° < M2 < 90° -180° < M3 < 180°

10 20

0

10 0

-10 -20

Y

 C1 (L1C2 + L2C23 )  R H = − S1 (L1C2 + L2C23 )  L1S 2 + L2 S 23 

-10 -20

X

DWS 20

10

Z


RH z

0

-10

-20 20 10 20

0

10 0

-10 Y

-20

-10 -20

X


Fanuc ARC Mate 0iA – Área de trabalho


Kuka KR 16 L6-2 – Área de trabalho


Área de trabalho e diagrama de carga IRB 660-180/3.15

Algoritmo

ROBÓTICA I


2 ALGORITMO PARA UMA HEURÍSTICA DE CINEMÁTICA INVERSA Robótica I [email protected]

Dadas as dificuldades para a obtenção da cinemática inversa para um caso geral, o que se propõe usualmente é o de seguir um conjunto de regras de forma a obter uma dada heurística (descoberta) em que os pontos principais são os seguintes:

1- Equacionar a matriz de transformação geral e a matriz de transformação do manipulador. 2- Procurar em ambas as matrizes: – Elementos que envolvam um só variável de junta – Pares de elementos que, quando divididos, produzem uma só expressão numa variável de junta (e que se possa usar a função atan de preferência) – Elementos ou suas combinações simplificáveis trigonometricamente.

3- Equacionar elementos nas duas expressões. 4- Repetir o processo para todos os termos identificados


Algoritmo para uma heurística de cinemática inversa Início Compara a expressão de transformação geométrica do manipulador (A1A2...An) com a expressão de transformação geral

Procurar elementos a) que envolvam uma só variável b)pares de elementos divisíveis que resultem numa expressão a uma variável (que possa usar a função atan() de preferência) c) elementos ou combinações de elementos simplificáveis trigonometricamente. Resolver todas as equações identificadas no passo anterior

Alguma solução sofre de risco de imprecisão numérica ou mesmo indefinições?

I S

N Tentar usar as componentes do vetor de translação (p) mesmo que não tenha variáveis isoladas

Falta determinar variáveis ?

S

B

F N


B Pré-multiplicar a equação matricial pela inversa de A1 ou Pós-multiplicar a equação matricial pela inversa de An e repetir esse processo para as restantes Ai até se esgotarem

I Falta solução para Alguma das variáveis de Junta?

S

Recorrer às soluções Imprecisas rejeitadas anteriormente E definir as zonas de funcionamento sem risco.

N Se ainda houver variáveis de junta por resolver, é provável que seja fisicamente impossível chegar à solução para o manipulador em causa

F FIM

3 RECURSO A TRANSFORMAÇÕES INVERSAS E COMPARAÇÃO DOS ELEMENTOS MATRICIAIS Robótica I [email protected]

Além do algoritmo heurístico da cinemática inversa, pode-se recorrer às transformações inversas. A transformação inversas pode obter expressões mais solúveis.

A partir da expressão matricial cinemática do manipulador: R H 1 2 n Reorganizando:

T = A A KA

R R

−1 n

TH .A = A1A2 KAn−1 −1 n

−1 n−1

TH .A .A

= A1A2 KAn−2

E assim sucessivamente enquanto possível ou favorável, ou respectivamente por pré-multiplicação.


Exemplo: Processo de pré-multiplicação pelas inversas com manipulador RR 3D Usando o robô de 2 elos já estudado: C1 S 0 T2 = A 1 ⋅ A 2 =  1 0  0

0 S1 0 − C1 1 0

0 0

0  C 2 0   S 2 ⋅ L1   0   1  0

− S2 C2 0 0

Recorrendo a pré multiplicação C1 S A 1−1 ⋅0 T2 = A 2 ⇔  1 0  0

0

S1

0 − C1 1

0

0

0

−1

0 L2 C 2  C1C 2 0 L2 S 2   S1C 2 = 1 0   S2   0 1   0

− C1 S 2 − S1 S 2

S1 − C1

C2 0

0 0

L2 C1C 2  L2 S1C 2  L2 S 2 + L1   1 

A1−1 ⋅R TH = A 2

0 r r r  0  n s a ⋅ L1   0 0 0  1

C 2 r p   S 2 =  1  0  0

− S2 C2 0 0

0 L2 C 2  0 L2 S 2  1 0   0 1 

⇒...


...continuando nxC1 + n y S1  nz   nx S1 − n y C1  0 

s xC1 + s y S1

a x C1 + a y S1

sz s x S1 − s y S1 0

az a x S1 − a y C1 0

p x C1 + p y S1  C2 p z − L1   S 2 = p x S1 − p y C1   0   1  0

− S2 C2 0 0

0 L2C2  0 L2 S 2  1 0   0 1 

Usando apenas os termos referentes a posição, pois é mais vantajoso;

 p x C1 + p y S1 = L2C2  p z − L1 = L2 S 2   p x cos M 1 + p y senM 1 = L2 cos M 2  p z − L1 = L2 senM 2  ⇒...


...continuando  p x cos M 1 + p y senM 1 = L2 cos M 2  p z − L1 = L2 senM 2  De onde obtemos M2:

  − p L 1 z  M 2 = tan   p cos M + p senM  1 y 1   x −1

Este expressão que usa a função arco tangente é uma alternativa a equação obtida anteriormente, que usa uma função arco seno. −1  p z − L1   M 2 = sen   L2  Esta técnica de matrizes inversas é uma alternativa e muitas vezes a única solução.


4 SOLUÇÃO DE UMA EQUAÇÃO USUAL NO PROBLEMA DA CINEMÁTICA INVERSA

Existe um tipo de equação trigonométrica que surge com relativa freqüência na determinação de cinemática inversa. Veja uma equação já desenvolvida: nxC1 + n y S1  nz   nx S1 − n y C1  0 

s x C1 + s y S1 sz

a xC1 + a y S1 az

s x S1 − s y S1 0

a x S1 − a y C1 0

p xC1 + p y S1  C2 p z − L1   S 2 = p x S1 − p y C1   0   1  0

− S2 C2 0 0

0 L2C2  0 L2 S 2  1 0   0 1 

Tem uma forma genérica:

k1 cos θ + k 2 sen θ = k3 Continua ⇒...


Calculando co-seno

k1 cos θ + k 2 sen θ = k3 Seja

m = tan

θ 2

2 sen e sabendo que:

θ 2

=

1 − cos θ 2

e

cos 2

θ 2

=

1 + cos θ 2

então: 1 − cosθ tan = m2 = 2 1 + cosθ 2

θ

então:

1 − m2 cosθ = 1 + m2

Prova:

1 − cosθ 1− 2 1− m 1 + cosθ − 1 + cosθ 2 cosθ θ 1 + cos = = = = cosθ 2 1 − cosθ 1 + cosθ + 1 − cosθ 1+ m 2 1+ 1 + cosθ Calculando o seno, Continua ⇒...


Calculando o seno...

2m sen θ = 1 + m2

Similarmente: prova:

1 − cos θ 1 − cos θ 2 2(1 + cos θ ) 2 tan 2m 1 + cos θ = 2 = 1 + cos θ = = 1 + m 2 1 + 1 − cos θ 1 + 1 − cos θ 1 + cos θ + 1 − cos θ 1 + cos θ 1 + cos θ

θ

=

(1 + cos θ )2 (1 − cosθ ) = (1 + cos θ )(1 − cos θ ) =

Logo:

1 + cos θ

1 − cos 2 θ = senθ

2m tan θ = 1 − m2

Substituindo estes termos na equação original:

⇒...


substituindo Substituindo estes termos na equação original: 1 − m2 cosθ = 1 + m2

k1 cos θ + k 2 sen θ = k 3

1− m   2m   + k2  k1  = k3 2  2  1+ m  1+ m  k1 1 − m 2 + k2 (2m ) = k3 1 + m 2

)

(

2m 1 + m2

m = tan

θ 2

k2 ± k22 − (k1 + k3 ) ⋅ (k3 − k1 ) m= k1 + k3

2

(

sen θ =

)

k1 − k1m 2 + 2k2 m − k3 − k3m 2 = 0

k2 ± k12 + k22 − k32 m= k1 + k3

(− k1 − k3 )m2 + (2k2 )m + k1 − k3 = 0 (k1 + k3 )m2 − (2k2 )m + (k3 − k1 ) = 0 2k2 ± 4k 22 − 4(k1 + k3 ) ⋅ (k3 − k1 ) m= 2(k1 + k3 )

 k ± k2 + k2 − k2 1 2 3 θ = 2 ⋅ tan −1  2  k1 + k3 

   

⇒...


Exemplo: Usando o método da eq. Trigonométrica comum

nx C1 + n y S1  nz  nx S1 − n y C1  0 

s x C1 + s y S1

a x C1 + a y S1

sz

az

s x S1 − s y S1

a x S1 − a y C1

0

0

k1 cos θ + k 2 sen θ = k3  k ± k2 + k2 − k2 1 2 3 θ = 2 ⋅ tan −1  2  k1 + k3 

p x C1 + p y S1  C2 p z − L1   S 2 = p x S1 − p y C1   0   1  0

− S2 C2 0 0

0 L2C2  0 L2 S 2  1 0   0 1 

Substituindo:    

Extraindo os parâmetros de kn:

 a ± a2 + a2 y x y θ1 = 2 ⋅ tan −1  ax 

   

Exemplo 2

a x C1 + a y S1 = 0

n x C1 + n y S1 = C 2

k2 = n y

k1 = a x

k1 = n x

k3 = cos θ 2

k2 = a y

 n ± n 2 + n 2 − cos 2 θ x y 2 −1  y θ1 = 2 ⋅ tan  1 + cosθ 2 

k3 = 0

   


5 ALGUMAS SOLUÇÕES ANALÍTICAS PADRÃO A

maioria dos manipuladores industriais tem sua cinemática conhecida, e a este grupo tem suas soluções descritas na literatura.


5.1 Robô planar de 3 elos - RRR A cinemática inversa de um robô planar de três eixos determina as expressões de [θ1, θ2, θ3] dado [x, y, φ]. Se a orientação φ não fosse dada então haveria redundância, e o braço teria um número infinito de soluções. Note-se para já que a orientação φ é dada por: Solução: O ponto Pw tem sua cinemática conhecida  PWx = L1C1 + L2C12 P = L S + L S  W y 1 1 2 12  PWx = x − L3Cφ  P = y − L Sφ  W y 3 Continua ⇒...


Robô planar de 3 elos - RRR

PWx = x − L3Cφ P = y − L Sφ  Wy 3 De resultados anteriores

 PWx P  W y  PWx P  W y  PWx P  W y

= L1C1 + L2C12 = L1S1 + L2 S12 = L1C1 + L2 C1C 2 − L2 S1 S 2 = L1 S1 + L2 C1 S 2 + L2 S1C 2 = L1 cos θ1 + L2 cos θ1 cos θ 2 − L2 senθ1senθ 2 = L1S1 + L2 cos θ1senθ 2 + L2 senθ1 cos θ 2 Continua ⇒...


Robô planar de 3 elos – RRR obtenção de θ2  PWx = L1 cos θ1 + L2 cos θ1 cos θ 2 − L2 sen θ1 sen θ 2  P = L S + L cos θ sen θ + L sen θ cos θ  Wy 1 1 2 1 2 2 1 2 Esta solução é conhecida: 22 W 0x

22 0Wy

2 2 1 1

2 2 2 2

P x ++yP = =L L+ +L L+ +2 L21LL12Lcos θ 2θ 2 2 cos 2 2 2 2  P + P − L − L 1 2 Wy θ 2 = ± cos −1  W x  2 L1 L2 

   

O sinal de ± mostra a redundância

Podemos obter θ1 através da tangente como já foi mostrado, ou por outro método alternativo, apesar de ser mais extenso. ⇒...


Robô planar de 3 elos – RRR obtenção de θ1 Usando a expressão  PWx = L1 cos θ1 + L2 cos θ1 cos θ 2 − L2 sen θ1 sen θ 2  P = L S + L cos θ sen θ + L sen θ cos θ  W y 1 1 2 1 2 2 1 2 Extraindo C1 e S1

cos θ1 =

PWx + L2 sen θ1 sen θ 2 L1 + L2 cos θ 2

sen θ1 =

PW y − L2 cos θ1 sen θ 2

Substituindo senθ1 na expressão de cosθ1  PW y − L2 cos θ1 sen θ 2   PWx + L2 sen θ 2   L1 + L2 cos θ 2   cos θ1 = L1 + L2 cos θ 2

=

PW x (L1 + L2 cos θ 2 ) + PW y L2 sen θ 2 − L22 cos θ1 sen 2 θ 2

(L1 + L2 cos θ 2 )2

L1 + L2 cos θ 2


Robô planar de 3 elos – RRR obtenção de θ1 PW x (L1 + L2 cos θ 2 ) + PW y L2 sen θ 2 − L22 cos θ1 sen 2 θ 2

cos θ1 =

(L1 + L2 cos θ 2 )2

(

)

cos θ1 L12 + L22 cos 2 θ 2 + 2 L1 L2 cos θ 2 + L22 cos θ1 sen 2 θ 2 = PWx (L1 + L2 cos θ 2 ) + PW y L2 sen θ 2 PWx (L1 + L2 cos θ 2 ) + PW y L2 sen θ 2

cos θ1 =

L12 + L22 + 2 L1 L2 cos θ 2

2 2 2 2 Substituindo com a expressão conhecida: PW x + PWy = L1 + L2 + 2 L1 L2 cos θ 2

cos θ1 = De forma similar:

PWx (L1 + L2 cos θ 2 ) + PW y L2 sen θ 2

senθ1 =

PW2x + PWy2 PWy (L1 + L2 cos θ 2 ) − PW y L2 senθ 2 PW2x + PWy2


Robô planar de 3 elos – RRR obtenção de θ1 PWx (L1 + L2 cos θ 2 ) + PWy L2 sen θ 2 PW2x + PWy2 sen θ1 tan θ1 = = cos θ1 PWx (L1 + L2 cos θ 2 ) + PWy L2 sen θ 2 PW2x + PWy2

 PWx (L1 + L2 cos θ 2 ) − PW y L2 senθ 2   θ1 = tan   PW (L1 + L2 cos θ 2 ) + PW L2 senθ 2  y  x  −1

θ3 pode ser obtido por:

θ 3 = φ − θ1 − θ 2

Robô planar de 3 elos – RRR Robótica I [email protected]

Resumo

 PWx = x − L3Cφ  P = y − L Sφ  W y 3

2 2 2 2   P + P − L − L 1 2 Wy −1  Wx  θ 2 = ± cos   2 L L 1 2  

 PWx (L1 + L2 cos θ 2 ) − PW y L2 senθ 2   θ1 = tan   PW (L1 + L2 cos θ 2 ) + PW L2 senθ 2  y  x  −1

θ 3 = φ − θ1 − θ 2


5.2 Solução do braço antropomórfico 3 DOF O braço antropomórfico com 3 graus de liberdade consta de três juntas rotacionais numa configuração série. A expressão da cinemática direta do manipulador RRR antropomórfico pode ser obtida da solução do manipulador de 5 DOF, bastando considerar as 3 primeiras juntas. C1C2C3 − C1S 2 S 3 S C C − S S S R TH =  1 2 3 1 2 3  S 2 C3 − C 2 S 3  0 

− C1C2 S 3 − C1S 2C3

S1

− S1C2 S 3 − S1S 2C3

− C1

− S 2 S 3 − C 2 C3

0

0

0

LC (C1C2C3 − C1S 2 S3 ) + LB C1C2  LC (S1C2C3 − S1S 2 S3 ) + LB S1C2  LC (S 2C3 − C2 S3 ) + LB S 2 + LA   1 


Solução do braço antropomórfico 3 DOF C1C2C3 − C1S 2 S 3 S C C − S S S R TH =  1 2 3 1 2 3  S 2 C3 − C 2 S 3  0 

− C1C2 S 3 − C1S 2C3

S1

− S1C2 S 3 − S1S 2C3

− C1

− S 2 S 3 − C 2 C3

0

0

0

LC (C1C2C3 − C1S 2 S3 ) + LB C1C2  LC (S1C2C3 − S1S 2 S3 ) + LB S1C2  LC (S 2C3 − C2 S3 ) + LB S 2 + LA   1 

Multiplicando as matrizes,

C1C23 S C R TH =  1 23  S 23   0

− C1S 23

S1

− S1S 23

− C1

C23

0

0

0

C1 (LC C23 + LB C2 )  S1 (LC C23 + LB C2 )  LC S 23 + LB S 2 + LA   1 

As coordenadas de Pw é a mesma de OT3 , e usando somente a posição:

 PWx   C1 (LC C23 + LB C2 )      PW =  PWy  =  S1 (LC C23 + LB C2 )   PWz   LC S 23 + LB S 2 + LA 


Solução do braço antropomórfico 3 DOF Então obtemos θ1:

S1 (LC C23 + LB C2 ) tan θ1 = = PWx C1 (LC C23 + LB C2 ) PWy

 PWy   θ1 = tan   PWx  −1

Nesta expressão, LCC23 +LBC2 pode ser positivo ou negativo, o que resulta em: − PWy  P −1  Wy  −1   = tan   − π θ1 = tan   PWx   − PWx  Implica isto que nesse caso θ2 passa a ter o valor (π−θ2) e que θ3 passa a ter o valor (−θ3). Esta redundância não implica alterações nas componentes pWx e pWy,


Braço antropomórfico 3 DOF – Cálculo de θ3 Também a posição Pwz não é alterada. Os valores das juntas 2 e 3 não interferem em S23 e S2 , e torna C23 e C2 simétricos

PWz = LC S 23 + LB S 2 + LA sen(π − θ 2 ) = S 2 sen(π − θ 2 − θ 3 ) = sen(π − (θ 2 + θ 3 )) = sen(θ 2 + θ 3 ) = S 23 cos(π − −θ 2 ) = −C2 cos(π − θ 2 − θ 3 ) = cos(π − (θ 2 + θ 3 )) = −C23

Para determinar a expressão para as variáveis θ2 e θ3 . Para isso, tomam-se os elementos da matriz de posição (não sem antes ter rearranjado PWz e migrar o termo LA para o outro membro da equação), e elevam-se ao quadrado e adicionam-se conforme a seqüência seguinte:


Braço antropomórfico 3 DOF – Cálculo de θ3 2 Wx

P

2 Wx

2 Wy

+P

2 Wy

P +P

( + (P

2 Wy

+ P

2 Wy

− LA

)

)

2

2

(

)

= (L B C 2 + L 2 C 23 ) C12 + S 12 + (L B S 2 + LC S 23 )

2

= L2B C 22 + L2C C 232 + 2 LB LC C 2 C 23 + L2B S 22 + L2C S 232 + 2 LB LC S 2 S 23 2

= L2B + L2C + 2 L B LC (C 2 C 23 + S 2 S 23 )

2

= L2B + L2C + 2 L B LC (C 2 (C 2 C 3 − S 2 S 3 ) + S 2 (S 2 C 3 + C 2 S 3 ))

− LA

) ) )

2

= L2B + L2C + 2 L B LC C 22 C 3 + S 22 C 3

PW2x + PW2y + PW2y − L A

)

= L2B + L2C + 2 LB LC C3

2 Wx

P

2 Wy

+P

PW2x + PW2y 2 Wx

P

2 Wy

+P

+ (P + (P + (P

− LA

2

2 Wy

− LA

2 Wy

− LA

2 Wy

(

2

(

(

)

)

 P 2 + P 2 + P 2 − L 2 − L2 − L2 W Wy Wy A B C θ 3 = ±COS −1  x 2 LB LC  

    ⇒...


Braço antropomórfico 3 DOF – Cálculo de θ2 Partindo da equação:

 PWx   C1 (LC C23 + LB C2 )  PW =  PWy  =  S1 (LC C23 + LB C2 )   PWz   LC S 23 + LB S 2 + LA 

Obtemos,

 P 2 + P 2 = L C + L C = L C + L C C − L S S Wy B 2 C 23 B 2 C 2 3 C 2 3  Wx  PWz − LA = LC S 2C3 + LC C2 S3 + LB S 2

PW2x + PW2y + LC S 2 S3 = C2 (LB + LC C3 )

C2 =

PW2x + PW2y + LC S 2 S3 LB + LC C3

(1)


Braço antropomórfico 3 DOF – Cálculo de θ2

A segunda expressão:

P 'Wz = PWz − L A = LC S 2 C 3 + LC C 2 S 3 + LB S 2 Substituindo C2

 P2 + P2 + L S S  Wy C 2 3   Wx P 'Wz = LB S 2 + LC S 2C3 + LC S3   LB + LC C3    

Colocando em evidência

 P2 + P2 + L S S  Wy C 2 3   Wx P'Wz = S 2 (LB + LC C3 ) + LC S3   LB + LC C3    

(LB + LC C3 )P'Wz = S 2 (LB + LC C3 )2 + LC S3 ( Fazendo

LB + LC C3 × LB + LC C3

PW2x + PW2y + LC S 2 S3

)

PW2x + PW2y = r

(LB + LC C3 )P'Wz = S 2 (LB + LC C3 )2 + LC S3 (r + LC S 2 S3 ) (LB + LC C3 )P'Wz = S 2 (LB + LC C3 )2 + rLC S3 + LC S3 (LC S 2 S3 )


Braço antropomórfico 3 DOF – Cálculo de θ2

(LB + LC C3 )P'Wz = S 2 (LB + LC C3 )2 + rLC S3 + LC S3 (LC S 2 S3 ) (LB + LC C3 )P'Wz −rLC S3 = S 2 (LB + LC C3 )2 + L2C S 2 S32

(LB + LC C3 )P'Wz −rLC S3 = S 2 [(LB + LC C3 )2 + L2C S32 ] ( LB + LC C3 )P'Wz − rLC S3 S2 = (LB + LC C3 )2 + L2C S32 S2 =

=

(LB + LC C3 )P'Wz −

PW2x + PW2y LC S3

L2B + L2C C32 + 2 LB LC C3 + L2C S32

(LB + LC C3 )P'Wz −

PW2x + PW2y LC S3

(2)

L2B + L2C + 2 LB LC C3

2 2 2 2 P + P = L + L Wy 1 2 + 2 L1 L2 cos θ 2 Após manipulações: W x P'Wz = PWz − LA

(

PW2x + PW2y + PW2y − L A

)

2

= L2B + L2C + 2 LB LC C 3

S2 =

(LB + LC C3 )(PWz − LA ) − LC S3 PW2x + PWy2

PW2x + PW2y


Braço antropomórfico 3 DOF – Cálculo de θ2 Partindo da expressão 1 e 2 e substituindo S2: C 2 =

PW2x + PW2y

LC S 3 C2 = + ⋅ LB + LC C 3 LB + LC C 3

PW2x + PW2y

LC S 3 C2 = + ⋅ LB + LC C 3 LB + LC C 3

C2 =

C2

( =

PW2x + PW2y LB + LC C 3

)(

LB + LC C 3

(LB + LC C 3 )P 'Wz −

PW2x + PW2y LC S 3

L2B + L2C + 2 LB LC C 3

(LB + LC C 3 )P 'Wz −

PW2x + PW2y LC S 3

L2B + L2C + 2 LB LC C 3

PW2x + PW2y LC S 3

LC S 3 P 'Wz − +

PW2x + PW2y + LC S 2 S 3

LB + LC C 3 L2B + L2C + 2 LB LC C 3

) (L + L C )(L

PW2x + PW2y L2B + L2C + 2 LB LC C3 + LC S 3 P 'Wz (LB + LC C3 ) − PW2x + PW2y LC S 3 B

C

3

2 B

+ L2C + 2 LB LC C3

)


Braço antropomórfico 3 DOF – Cálculo de θ2 Conseqüentemente:

S2 θ 2 = tan C2 −1

 (L + L C )(P − L ) − L S P 2 + P 2  B C 3 Wz A C 3 Wx Wy  −1  θ 2 = tan   (LB + LC C3 ) PW2x + PW2y + LC S3 (PWz − LA )   


Braço antropomórfico 3 DOF – Resumo

− PWy   PWy  −1   = tan   − π θ1 = tan   PWx   − PWx  −1

 (L + L C )(P − L ) − L S P 2 + P 2  B C 3 Wz A C 3 Wx Wy  −1  θ 2 = tan   (LB + LC C3 ) PW2x + PW2y + LC S3 (PWz − LA )   

(

)

 P 2 + P 2 + P 2 − L 2 − L2 − L2 Wy Wy A B C −1  W x θ 3 = ±COS  2 LB LC  

   

Devido a redundância em θ1 e θ3 temos 22 = 4 soluções.


5.3 Solução do punho esférico


5.4 Manipuladores com um punho esférico

Cinemática inversa Pêndulo duplo invertido

Robô bípede – Uma perna ROBÓTICA I



Exemplo: 2 DOF -Pêndulo duplo Robô bípede – Uma perna Z

X 2 = ∆x

K0

K 0 = L1 + L2

Z 2 = K 0 − ∆z

M1

L1

x 2 = L1 sen( M 1 ) + L2 sen( M 1 + M 2 ) z 2 = L1 + L2 − L1 cos( M 1 ) − L2 cos( M 1 + M 2 )

∆Z

L2 z2

M2

∆X

x2

x


2 DOF -Pêndulo duplo – Calculando M2 ∆x = L1 sen( M 1 ) + L2 sen( M 2 ) ∆z = L1 cos( M 1 ) + L2 cos( M 1 + M 2 ) Elevando ambos os membros ao quadrado ∆x 2 = (L1 sen( M 1 ) + L2 sen( M 1 + M 2 ) )

2

∆x 2 = L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L2 sen( M 1 ) sen( M 1 + M 2 ) ∆x 2 = L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L2 sen( M 1 )(sen( M 1 ) cos( M 2 ) + sen( M 2 ) cos( M 1 ) ) ∆x 2 = L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L2 sen 2 ( M 1 ) cos( M 2 ) + 2 L1 L2 sen( M 1 ) sen( M 2 ) cos( M 1 )

∆z 2 = (L1 cos( M 1 ) + L2 cos( M 1 + M 2 ) )

2

∆z 2 = L12 cos 2 ( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L2 cos( M 1 ) cos( M 1 + M 2 ) ∆z 2 = L12 cos 2 ( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L2 cos( M 1 )(cos( M 1 ) cos( M 2 ) − sen( M 1 ) sen( M 2 ) ) ∆ z 2 = L12 cos 2 ( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L 2 cos 2 ( M 1 ) cos( M 2 ) − 2 L1 L 2 cos( M 1 ) sen ( M 1 ) sen ( M 2 )

2 DOF -Pêndulo duplo – Calculando M2 Robótica I [email protected]

Somando os dois termos ∆x 2 = L12 cos 2 ( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L2 cos 2 ( M 1 ) cos( M 2 ) − 2 L1 L2 cos( M 1 ) sen( M 1 ) sen( M 2 ) ∆z 2 = L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L2 sen 2 ( M 1 ) cos( M 2 ) + 2 L1 L2 sen( M 1 ) sen( M 2 ) cos( M 1 ) ∆x 2 + ∆z 2 = L12 cos 2 ( M 1 ) + L22 cos 2 ( M 1 + M 2 ) + 2 L1 L2 cos 2 ( M 1 ) cos( M 2 ) − 2 L1 L2 cos( M 1 ) sen( M 1 ) sen( M 2 ) + + L12 sen 2 ( M 1 ) + L22 sen 2 ( M 1 + M 2 ) + 2 L1 L2 sen 2 ( M 1 ) cos( M 2 ) + + 2 L1 L2 sen( M 1 ) sen( M 2 ) cos( M 1 ) ∆x 2 + ∆z 2 = L12 cos 2 ( M 1 ) + L12 sen 2 ( M 1 ) + + L22 cos 2 ( M 1 + M 2 ) + L22 sen 2 ( M 1 + M 2 ) + 2

2

+ 2 L1 L2 cos ( M 1 ) cos( M 2 ) + 2 L1 L2 sen ( M 1 ) cos( M 2 ) +

0

− 2 L1 L2 cos( M 1 ) sen( M 1 ) sen( M 2 ) + 2 L1 L2 sen( M 1 ) sen( M 2 ) cos( M 1 )

1

[

]

∆x 2 + ∆z 2 = L12 cos 2 ( M 1 ) + sen 2 ( M 1 ) +

[

] ( M )] + 0

+ L22 cos 2 ( M 1 + M 2 ) + sen 2 ( M 1 + M 2 ) +

[

+ 2 L1 L2 cos( M 2 ) cos 2 ( M 1 ) + sen 2

1

1 1 ∆x 2 + ∆z 2 = L12 + L22 + 2L1 L2 cos(M 2 )


2 DOF -Pêndulo duplo – Calculando M2 ∆x 2 + ∆z 2 = L12 + L22 + 2 L1 L 2 cos( M 2 ) 2 2 2 2  ∆ + ∆ − − x z L L 1 2 M 2 = ± cos −1  2 L1 L2 

  

2 2 2 2  x + ( z − z ) − L − L 0 2 1 2 M 2 = ± cos −1  2 2 L1 L2 

   

•É uma solução analítica em função das dimensões do robô e da posição •Pelo sinal ± mostra que tem duas soluções matemáticas para M2.

2 DOF -Pêndulo duplo – Calculando M1 Robótica I [email protected]

O cálculo de M1 é mais trabalhoso Usaremos o recurso trigonométrico de “soma de tangentes”. Usaremos os ângulos auxiliares α e β

Z

M1 = α − β

M1

∆X tan α = ∆Z L2 senM 2 tan β = L1 + L2 cos M 2

z0 ∆Z

L1 β L2

α z2


M2 ∆X

x2

x


2 DOF -Pêndulo duplo – Calculando M1 tan A − tan B tan ( A − B ) = 1 + tan A tan B L2 senM 2 tan β = L1 + L2 cos M 2 ∆x tan α = ∆z

∆x(L1 + L2 cos M 2 ) − ∆zL2 senM 2 ∆z (L1 + L2 cos M 2 ) tan (M 1 ) = ∆z ⋅ (L1 + L2 cos M 2 ) + ∆x(L2 senM 2 ) ∆z ⋅ (L1 + L2 cos M 2 )

∆x(L1 + L2 cos M 2 ) − ∆zL2 senM 2 tan (M 1 ) = ∆z ⋅ (L1 + L2 cos M 2 ) + ∆x(L2 senM 2 )

tan (M 1 ) = tan (α − β ) L2 senM 2 ∆x − ∆z L1 + L2 cos M 2 tan (M 1 ) = L2 senM 2 ∆x 1+ ⋅ ∆z L1 + L2 cos M 2

 ∆x(L1 + L2 cos M 2 ) − ∆zL2 senM 2   M 1 = tan   ∆z (L1 + L2 cos M 2 ) + ∆x(L2 senM 2 )  −1


2 DOF -Pêndulo duplo Cinemática inversa 2 2 2 2  ∆ x + ∆ z − L − L 1 2 M 2 = ± cos −1  2 L1 L 2 

  

∆ = x X 2 onde ∆z = L1 + L2 − Z 2

 ∆ x (L1 + L 2 cos M 2 ) − ∆ zL 2 senM 2   M 1 = tan   ∆ z (L1 + L 2 cos M 2 ) + ∆ x (L 2 senM 2 )  −1

Z Temos duas possíveis soluções: M 2 '> 0  M 2 " < 0

K0 ∆Z z2

M1 L1 L2 M2

∆X

x2

x


Simulação % % Cinemática inversa de um pendulo duplo invertido % Simulando uma perna % (c) Rogério Oliveira 2008 clear all; close all; L1=10; L2=10; x0 = 0; z0 = L1+L2-2;

%Largura da passada %Altura da perna

p=-5:1:5; %Pontos k=1; while k<=length(p), x2(k) =p(k); z2(k) =5*sind(180*(k-1)/(length(p)-1)); dx = x2(k); dz = z0 - z2(k); Teta2(k)= -acosd((dx^2 + dz^2 - L1^2 - L2^2)/(2*L1*L2)); Aux1 = L1 + L2*cosd(real(Teta2(k))); Aux2 = L2*sind(real(Teta2(k))); Teta1(k)= atand( (dx*Aux1 - dz*Aux2)/(dz*Aux1+dx*Aux2)); x1(k) = x0 + L1*sind(Teta1(k)); z1(k) = z0 - L1*cosd(Teta1(k)); k = k+1; end;


Simulação figure(1); subplot(221); plot(x2,z2,'k:',x2,z2,'ko'); title('Pontos'); axis([-10 10 0 L1+L2]);grid on; subplot(223); plot(p,Teta1,'b-',p,Teta1,'ko'); title('\Theta 1'); grid on; subplot(224); plot(p,Teta2,'r-',p,Teta2,'ko'); title('\Theta 2'); grid on; for k = 1:length(p), subplot(222); plot(x2(k),z2(k),'ko'); line([x0 x1(k)]', [z0 z1(k)]', 'LineWidth',4,'Color','b'); line([x1(k) x2(k)]', [z1(k) z2(k)]', 'LineWidth',4,'Color','r'); axis([-10 10 0 L1+L2]); grid on; Quadro(k) = getframe; Quadro(2*length(p)-k) = Quadro(k); end; title('Robô bípede'); hold off; movie(Quadro,10,6);


Simulação - resultado


Simulação de um bípede

Desafio! Faça você mesmo um programa em matlab para reproduzir este bípede baseado no programa de uma perna.

Fim

ROBÓTICA I


Robótica_Cinemática_Inversa

Recommend Documents