1

PONTIFÍCIA UNIVERSIDADE CATÓLICA DO RIO GRANDE DO SUL FACULDADE DE ENGENHARIA ENGENHARIA DE CONTROLE E AUTOMAÇÃO

44646-04 SISTEMAS ROBOTIZADOS Prof. Felipe Kühne

Lista de Exercícios 3 - Cinemática Inversa

1. Determine o centro do punho de um manipulador de seis graus de liberdade, dada a matriz de cinemática direta 6

0H e o parâmetro 6d abaixo:

−−−

−

−

=

1000

6,30187,005,0

505,0087,0

770010

60H , mmd 1006 =

2. Considerando que o manipulador da questão anterior possui um punho esférico nas suas três últimas

juntas e sabendo que as três primeiras juntas possuem ângulos correspondentes à seguinte matriz de rotação:

−

−=

100

010

00130R ,

calcule a cinemática inversa de orientação. 3. Mostre que a matriz de rotação de um punho esférico resume-se a

−

=

100

0

0

4646

464663 cs

sc

R

quando 05 =θ .

4. Dada a matriz de transformação homogênea de um manipulador de seis graus de liberdade e a matriz

de rotação para a cinemática inversa de posição abaixo, determine a solução para a cinemática inversa de orientação:

−−

−

−−

=

1000

6,301100

50071,071,0

770071,071,0

60H

−

−=

100

010

00130R

2

5. O desenho abaixo representa um manipulador esférico com as suas três primeiras juntas. Apresente as equações que definem a cinemática inversa de posição para este robô.

6. Considerando que o manipulador do exercício anterior possui os seguintes parâmetros de Denavit-

Hartenberg e as seguintes coordenadas para o centro do punho, determine as variáveis das juntas para a cinemática inversa de posição.

7. Para o mesmo manipulador do exercício 5, considere agora um punho esférico anexado ao seu corpo,

formando um manipulador de 6GDL. Para as variáveis das juntas encontradas no exercício anterior, calcule a cinemática inversa de orientação, dada a seguinte matriz de transformação homogênea total:

−=

1000

525097,026,0

850100

0026,097,0

60H

i ai ααααi di θθθθi

1 0 π/2 525 θ1 *

2 0 π/2 0 θ2 *

3 0 0 d3 * 0

4 0 -π/2 0 θ4 *

5 0 π/2 0 θ5 *

6 0 0 100 θ6 *

=

525

750

0

cp

3

8. O desenho abaixo representa um manipulador articulado cujo último sistema de coordenadas localiza-se no centro de um punho esférico ( 4O ). Determine as expressões para a cinemática inversa de posição deste robô.

9. Para o manipulador do exercício anterior, determine o centro do punho, dada a seguinte matriz de

transformação homogênea e o parâmetro 6d :

−

−=

1000

76,99871,0071,0

59,75235,087,036,0

5,130361,05,061,0

60H , mmd 1006 =

10. Para o manipulador do exercício 8, calcule a cinemática inversa de posição, dados os seguinte

parâmetros construtivos do robô (grandezas em milímetros):

740525100770150 41321 ===== ddaaa

11. Apresente as equações para a cinemática inversa de posição para o manipulador SCARA da figura

abaixo, sabendo que as variáveis das juntas são 1θ , 2θ e 3d , e o centro do punho é

[ ]T

ccc zyx=cp .

4

12. Para o seguinte manipulador esférico, apresente pelo menos uma solução possível para a cinemática inversa de posição e orientação.

i ai ααααi di θθθθi

1 0 π/2 525 θ1 *

2 0 π/2 0 θ2 *

3 0 0 d3 * 0

4 0 -π/2 0 θ4 *

5 0 π/2 0 θ5 *

6 0 0 100 θ6 *

−=

1000

55,878001

78,2265,0230

79,392235,00

60H

5

UMA BREVE EXPLICAÇÃO SOBRE AS FUNÇÕES ATAN E ATAN2:

A função trigonométrica ( )xyarctan é a função inversa de ( )xytan , ou ( )xy1tan − , onde y é o cateto

oposto e x é o cateto adjacente de um triângulo retângulo formado por θ .

Uma outra forma de escrever esta função é através do seno e cosseno:

( )( )( )α

αα

cos

sintan = →

( )( )

=

α

αα

cos

sinarctan

Se o numerador e o denominador são positivos, o ângulo α encontra-se no primeiro quadrante; se o numerador é negativo e o denominador é positivo, α encontra-se no segundo quadrante; se ambos numerador e denominador são positivos, α encontra-se no terceiro quadrante; por fim, se o numerador é positivo e o denominador é negativo, α encontra-se no quarto quadrante. No caso de ambos numerador e denominador serem negativos, teremos que:

( )( )

( )( )

=

−

−=

α

α

α

αα

cos

sinarctan

cos

sinarctan

Ou seja, o ângulo na realidade encontra-se no quarto quadrante, mas esta informação é perdida quando do cálculo do arco-tangente. O ângulo calculado desta forma está incorreto. A função atan2 leva em consideração então os sinais dos dois argumentos, colocando o resultado no quadrante correto. Por exemplo, os dois cálculos abaixo utilizam argumentos -1 para o numerador e -1 para o denominador. Fica óbvio que o resultado correto apenas é obtido ao usar a função atan2.

( )o45

1arctan

1

1arctan

=

=

−

−=α

( )o135

1,12arctan

−=

−−=α

Podemos organizar os valores da função atan2 da seguinte forma, conforme o quadrante do ângulo:

( )

( )

( )

( )

==

−<=

>=

−<<

+≥<

>

=

indefinidoyx

yx

yx

xyyx

xyyx

xyx

xy

0,0

20,0

20,0

arctan0,0

arctan0,0

arctan0

,2arctan

π

π

π

π

6

RESPOSTAS

1.

−

=

−

−

−

=

−−

−

−

−

−

=

−=

215

0

770

87,0

5,0

0

100

6,301

50

770

1

0

0

87,005,0

5,0087,0

010

100

6,301

50

770

60

60

c

c

p

kRdp

2. Como 63

30

60 RRR = , pode-se isolar a matriz de rotação do punho esférico: ( ) 6

0

130

63 RRR

−= .

Definiremos também a matriz U , que corresponde genericamente a 63R . Assim:

( )

( )

UR

RR

RRR

=

−

=

−−

−

−

−=

=

=−

:

87,005,0

5,0087,0

010

87,005,0

5,0087,0

010

100

010

001

63

60

30

60

130

63

T

com

=

333231

232221

131211

uuu

uuu

uuu

U

A matriz de rotação para um punho esférico é definida pela cinemática direta deste componente:

−

+−+

−−−

=

56565

546465464654

54646546465463

csscs

ssccscsscccs

sccssccssccc

R .

Uma matriz de rotação é dita ortonormal, já que as linhas e colunas desta matriz possuem módulo unitário. Assim, se 133 ±≠u , os elementos 13u , 23u , 31u e 32u serão diferentes de zero.

7

No caso deste exercício, temos que ( ) 87,0cos 533 == θu . Assim, como 1sincos 52

52 =+ θθ , pode-se

dizer que ( ) 2335 1sin u−±=θ . Ainda, ( ) ( ) ( )555 cossintan θθθ = , ou seja, ( ) ( )( )555 cossinarctan θθθ = .

Então:

±=

−±=

−±=

87,0

5,0arctan

87,0

87,01arctan

1arctan

2

33

233

5u

uθ

65πθ ±=

Assim, temos duas possíveis soluções para 5θ . Analisando os outros elementos de 6

3R , vemos que o valor

de 5θ vai influenciar também nos valores de 4θ e 6θ . Ou seja, dependendo de qual solução escolhermos

para 5θ , teremos valores diferentes para 4θ e 6θ . Resolvendo 4θ e 6θ para ( )5sin θ positivo, tem-se que:

( )

( ) ( )( ) ( )

( )( )4

4

54

54

13

234

cos

sin

sincos

sinsin

tan

θ

θ

θθ

θθ

θ

==

=u

u

−=

=

0

5,0arctanarctan

13

234

u

uθ

24πθ −=

( )

( ) ( )( ) ( )( )

( ) ( )( ) ( )

( )( )6

6

65

65

65

65

31

326

cos

sin

cossin

sinsin

cossin

sinsin

tan

θ

θ

θθ

θθ

θθ

θθ

θ

==

−−=

−=

u

u

−=

−=

5,0

0arctanarctan

31

326

u

uθ

πθ =6

Agora, se considerarmos a solução para ( )5sin θ negativo, temos que:

( )

( ) ( )( )( )( ) ( )( )( )

( ) ( )( ) ( )

( )( )4

4

54

54

54

54

13

234

cos

sin

sincos

sinsin

sincos

sinsin

tan

θ

θ

θθ

θθ

θθ

θθ

θ

==

−−

−−=

−

−=

u

u

=

−

−=

0

5,0arctanarctan

13

234

u

uθ

24πθ =

( )

( ) ( )( )( )( ) ( )

( ) ( )( ) ( )

( )( )6

6

65

65

65

65

31

326

cos

sin

cossin

sinsin

cossin

sinsin

tan

θ

θ

θθ

θθ

θθ

θθ

θ

==

−−

−−=

−=

u

u

=

−=

5,0

0arctanarctan

31

326

u

uθ

06 =θ

8

Logo, temos dois conjuntos de ângulos que satisfazem a solução da cinemática inversa de orientação:

( )5sin θ positivo:

=

=

−=

06

2

6

5

4

θ

πθ

πθ

( )5sin θ negativo:

=

−=

=

06

2

6

5

4

θ

πθ

πθ

3. Como 05 =θ , ( ) 1cos 5 =θ e ( ) 0sin 5 =θ e a matriz de rotação é ortonormal (o módulo das linhas e

colunas é unitário), temos que:

+−+

−−−

=

100

0

0

64646464

64646464

63 ccsssccs

csscsscc

R .

Utilizando as seguintes identidades trigonométricas:

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )646464

646464

sincossinsincos

cossinsincoscos

θθθθθθ

θθθθθθ

+=+

+=−

vemos que a matriz 6

3R se torna:

−

=

100

0

0

4646

4646

63 cs

sc

R ,

onde ( )6446 cos θθ +=c e ( )6446 sin θθ +=s .

4.

( )

( )

UR

RR

RRR

=

−

−−

=

−

−

−−

−=

=

=−

:

100

071,071,0

071,071,0

100

071,071,0

071,071,0

100

010

001

63

60

30

60

130

63

T

A matriz de rotação para um punho esférico é definida pela cinemática direta deste componente:

−

+−+

−−−

=

56565

546465464654

546465464654

63

csscs

ssccscsscccs

sccssccssccc

R .

9

Nota-se que, neste caso, ( ) 1cos 533 == θu . Como a matriz é ortogonal e possui linhas e colunas com

módulo unitário, os elementos 13u , 23u , 31u e 32u serão nulos (se ( ) 1cos 5 =θ , ( ) 0sin 5 =θ ). Substituindo-se estes valores na matriz acima, tem-se que:

+−+

−−−

=

100

0

0

64646464

64646464

63 ccsssccs

csscsscc

R .

Utilizando as seguintes identidades trigonométricas:

( ) ( ) ( ) ( ) ( )

( ) ( ) ( ) ( ) ( )646464

646464

sincossinsincos

cossinsincoscos

θθθθθθ

θθθθθθ

+=+

+=−

vemos que a matriz 6

3R se torna:

−

=

100

0

0

4646

4646

63 cs

sc

R ,

onde ( )6446 cos θθ +=c e ( )6446 sin θθ +=s .

Por este motivo, não é possível o cálculo dos ângulos 4θ e 6θ individualmente, mas

apenas a soma deles. O significado físico disto é de simples entendimento, visto que um ângulo na junta 5 de 05 =θ graus implica em os eixos 3z e 5z

estarem alinhados, como na figura ao lado. Assim, o efeito em termos de rotação em torno de 3z e 5z é o mesmo. Analisando 11u e 21u

de 63R , temos que:

( )

( )( )64

64

11

2164

cos

sin

tan

θθ

θθ

θθ

+

+=

=+u

u

=+

11

2164 arctan

u

uθθ

No caso deste exercício,

−=+

71,0

71,0arctan64 θθ

43

64πθθ =+

Assim, qualquer combinação de valores de forma que 43

64πθθ =+ é uma solução válida.

10

A solução completa para a cinemática inversa de orientação é, então:

−=+

=

4

0

64

5

πθθ

θ

5. Observe as definições dos sistemas de coordenadas e dos parâmetros de Denavit-Hartenberg. Isto é

essencial na determinação das equações. Sendo [ ]Tzyx ppp=cp as coordenadas do centro do

punho, tem-se:

=

x

y

p

parctan1θ ,

2arctan2

πθ +

=

r

s e 22

3 srd += ,

onde:

1dps z −= e 22yx ppr += .

6. o901 =θ o902 =θ mmd 7503 = .

7. A matriz de rotação do punho esférico é calculada com ( ) 60

130

63 RRR

−= , que para este exercício é

−

=

100

026,097,0

097,026,063R

Note que neste caso a matriz de rotação possui o termo 133 =u , então, ( ) 1cos 5 =θ , ou o05 =θ . De acordo

com o exercício 3, o formato de 63R reduz-se a:

−

=

100

0

0

4646

4646

63 cs

sc

R

Assim, apenas a soma 64 θθ + pode ser determinada.

o75

arctan11

2164

=

=+

u

uθθ

8. Respostas omitidas.

11

9.

=

−

−

=

−=

76,1069

59,717

5,1242

71,0

35,0

61,0

100

76,998

59,752

5,1303

606

60

c

c

p

kRdp d

10. o301 =θ , o452 =θ , o453 =θ , o04 =θ , o455 −=θ , o06 =θ .

11.

22yx ppr +=

Daa

aar:

2cos

22

21

22

21

2

2 =−−

=θ

22 1sin D−±=θ

−±=

D

D2

2

1arctanθ

αφθ −=1

=

x

y

p

parctgφ

+=

221

22

cos

sin

θ

θα

aa

aarctg

zpdd −= 13

12

4

3

255,353

55,353arctan

2arctan

2

2

πθ

π

πθ

=

+

=

+

=

r

s

6

19,306

78,176arctan

arctan

1

1

πθ

θ

=

=

=

y

x

p

p

=

−

=

−−

=

−=

55,878

78,176

19,306

0

5,02

3

100

55,878

78,226

79,392

1

0

0

001

5,0230

235,00

100

55,878

78,226

79,392

60

60

c

c

p

kRdp

500

250000

55,35355,353

3

22

223

=

=

+=

+=

d

rsd

12.

Com 61 πθ = , 432 πθ = e 5003 =d , temos que a matriz de rotação correspondente às três primeiras

variáveis das juntas é:

−−

−

=

−

−=

=

2202

24

22

34

261,05,061,0

100

010

001

0102

2022

2202

2

0102

305,0

5,0023

30

32

21

10

30

R

RRRR

( )

−

=

−

−

−−

=

=−

2202

2010

2202

2

001

5,0230

235,00

22

4261,0

0235,0

22

4261,0

60

130

63 RRR

13

Tendo 63R , podemos resolver a cinemática inversa de orientação.

−

=

=

2202

2010

2202

2

333231

232221

131211

uuu

uuu

uuu

U

( )( )

4

1arctan22

22arctan

22

221arctan

1arctan

5

2

33

233

5

πθ

θ

±=

±=

±=

−±

=

−±=

u

u

Resolvendo 4θ e 6θ para ( )5sin θ positivo:

πθ

θ

=

−=

=

4

13

234

22

0arctanarctan

u

u

πθ

θ

=

−=

−=

6

31

326

22

0arctanarctan

u

u

Agora, se considerarmos a solução para ( )5sin θ negativo:

0

22

0arctanarctan

4

13

234

=

=

−

−=

θ

θu

u

0

22

0arctanarctan

6

31

326

=

=

−=

θ

θu

u

Temos então duas soluções possíveis, dependendo do valor de 5θ :

=

=

=

=

=

=

πθ

πθ

πθ

πθ

πθ

6

5

4

3

2

1

4

5004

36

d

=

−=

=

=

=

=

04

0

5004

36

6

5

4

3

2

1

θ

πθ

θ

πθ

πθ

d