Introdução à Robótica Industrial - mecatronica.eesc.usp.br · • Representação eixo/ângulo...

Introdução à Robótica Industrial

Adriano A. G. Siqueira

Aula 3 - Cinematica Direta

Introducao a Robotica Industrial – p. 1/38

Exemplo

• Considere um manipulador planar de dois elos.

• Encontrar posição da origem do Sistema de Coordenas (X2,Y2)em funçãodeθ1 e θ2.


Rotação

x0

y1

y0

x1

z1

z0

PS


Rotação

• {i0, j0,k0} a base ortonormal deOx0y0z0 e{i1, j1,k1} a base ortonormal deOx1y1z1

• o vetor ligando a origem O ao ponto P pode ser representado:• no sistemaOx0y0z0 por:

p0 = p0xi0 + p0y

j0 + p0zk0

• no sistemaOx1y1z1 por:

p1 = p1xi1 + p1y

j1 + p1zk1.

• relação entre as coordenadas das duas representações

p0x= p0.i0 = p1.i0 = p1x

i1.i0 + p1yj1.i0 + p1z

k1.i0

p0y= p1x

i1.j0 + p1yj1.j0 + p1z

k1.j0

p0z= p1x

i1.k0 + p1yj1.k0 + p1z

k1.k0.Introducao a Robotica Industrial – p. 4/38

Rotação

• Podemos reescrever essas três equações como

p0 = R1

0p1

sendo

R1

0=

i1.i0 j1.i0 k1.i0

i1.j0 j1.j0 k1.j0

i1.k0 j1.k0 k1.k0

.

• Similarmente podemos escrever

p1 = R0

1p0

sendo

R0

1=

i0.i1 j0.i1 k0.i1

i0.j1 j0.j1 k0.j1

i0.k1 j0.k1 k0.k1


Propriedade e Exemplo

• Como o produto interno é comutativo, temos que

R0

1= (R1

0)−1 = (R1

0)T .

• Exemplo: Suponha que o sistemaOx1y1z1 seja rotacionado de um ânguloθem torno do eixoz0. Encontre a matriz de transformação resultanteR1

0.

• Temos:

i1.i0 = cos θ k1.i0 = 0

j1.j0 = cos θ k1.j0 = 0

j1.i0 = − sin θ i1.k0 = 0

i1.j0 = sin θ j1.k0 = 0

k1.k0 = 1


Exemplo

x0

x1

y0

y1

i0i1 j0

j1

k0 k1

z0 z1

qq


Matrizes de Rotação Básicas

• Então

R1

0= Rz,θ =

cos θ − sin θ 0

sin θ cos θ 0

0 0 1

• Rotações básicas em torno dos eixosx ey são dadas por

Rx,θ =

1 0 0

0 cos θ − sin θ

0 sin θ cos θ

Ry,θ =

cos θ 0 sin θ

0 1 0

− sin θ 0 cos θ


Composição de Rotações

• Três sistemas:Ox0y0z0, Ox1y1z1 eOx2y2z2.

• O vetorp pode então ser representado de três maneiras:p0, p1 ep2.

• Temos

p0 = R1

0p1(1)

p0 = R2

0p2(2)

p1 = R2

1p2(3)

• Substituindo (3) em (1) fornece

p0 = R1

0R2

1p2(4)

• Comparando (2) e (4) nós temos a relação

R2

0= R1

0R2

1


Composição de Rotações

• A ordem das rotações é importante?

RARB = RBRA?

• Exemplo: Rotações emz ex.


Rotação em torno de um Eixo Arbitrário

x0

y0

z0

k

kx

ky

kz

q

a

b



• Sejak = (kx, ky, kz)T um vetor unitário que define um eixo arbitrário.

• Rotação deθ graus em torno do eixo arbitrário.

• Matriz de rotação:

Rk,θ = Rz0,αRy0,βRz0,θRy0,−βRz0,−α.

• Comok é um vetor unitário, vemos que

sinα =ky

√

k2x + k2

y

cosα =qx

√

k2x + k2

y

sinβ =√

k2x + k2

y

cosβ = kz.Introducao a Robotica Industrial – p. 12/38


• Temos

Rk,θ =

k2

xvθ + cθ kxkyvθ − kzsθ kxkzvθ + kysθ

kxkyvθ + kzsθ k2

yvθ + cθ kykzvθ − kxsθ

kxkzvθ − kysθ kykzvθ + kxsθ k2

zvθ + cθ

sendovθ = 1 − cθ, cθ = cos θ, sθ = sin θ.

• Representação eixo/ângulo de uma matriz de rotação arbitrária

Rk,θ =

r11 r12 r13

r21 r22 r23

r31 r32 r33


Representação Eixo/Ângulo

• O ângulo equivalenteθ e o eixo equivalentek são dados pelas expressões

θ = cos−1

(

Tr(R) − 1

2

)

= cos−1

(

r11 + r22 + r33 − 1

2

)

sendoTr(R) o traço deR, e

k =1

2 sin θ

r32 − r23

r13 − r31

r21 − r12

• Quatro variáveis (θ e coordenadas dek) e uma restrição:k2

x + k2

y + k2

z = 1.


Ângulos de Euler

• (φ, θ, ψ) - três variáveis idependentes.

• Rotação em torno dez0 pelo ânguloφ

• Rotação em torno dey′

0pelo ânguloθ

• Rotação em torno dez′′

0pelo ânguloψ


Ângulos de Euler

• Matriz de rotação

Rz0,φRy′0,θRz′′

0,ψ =

cφ −sφ 0

sφ cφ 0

0 0 1

cθ 0 sθ

0 1 0

−sθ 0 cθ

cψ −sψ 0

sψ cψ 0

0 0 1

=

cφcθcψ − sφsψ −cφcθsψ − sφcψ cφsθ

sφcθcψ + cφsψ −sφcθsψ + cφcψ sφsθ

−sθcψ sθsψ cθ


Ângulos de Roll, Pitch e Yaw

• Rotações em torno dos eixos coordenadosx0, y0, z0

• Rotação em torno dex0 pelo ânguloψ, movimento de yaw

• Rotação em torno dey0 pelo ânguloθ, movimento de pitch

• Rotação em torno dez0 pelo ânguloφ, movimento de roll

x0

y0

z0

Guinar

Inclinar

Rolar


Ângulos de Roll, Pitch e Yaw


Rz0,φRy0,θRx0,ψ =

cφ −sφ 0

sφ cφ 0

0 0 1

cθ 0 sθ

0 1 0

−sθ 0 cθ

1 0 0

0 cψ −sψ

0 sψ cψ

=

cφcθ −sφcψ + cφsθsψ sφsψ + cφsθcψ

sφcθ cφcψ + sφsθsψ −cφsψ + sφsθcψ

−sθ cθsψ cθcψ


Transformações Homogêneas

• Sistemas de coordenadasO0x0y0z0 eO1x1y1z1

• Movimento de corpo rígido: combinação de uma rotação e uma translação

• Representação dep no sistemaOx0y0z0

p0 = R1

0p1 + d1

0

• O vetord1

0liga a origemO0 à origemO1 e é expresso no sistema de

coordenadasO0x0y0z0

• Representação matricial

[

p0

1

]

=

[

R1

0d1

0

0 1

][

p1

1

]



• Transformação homogênea

H1

0=

[

R1

0d1

0

0 1

]

• A transformação homogênea mais geral

H =

[

R3×3 d3×1

f1×3 s1×1

]

=

[

Rotacao Translacao

perspectiva fator de escala

]

utilizada para a interface de um sistema de visão ou para simulação gráfica.



• Exemplo: Rotação deα graus em torno dex0

• Translação de a unidades ao longo do eixox0

• Translação de d unidades ao longo do eixoz0

• Rotação deθ graus em torno dez0

H = Rotz0,θTransz0,dTransx0,aRotx0,α =

cθ −sθ 0 0

sθ cθ 0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1 d

0 0 0 1

1 0 0 a

0 1 0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 cα −sα 0

0 sα cα 0

0 0 0 1

=

cθ −sθ 0 a

cαsθ cαcθ −sα −sαd

sαsθ sαcθ cα cαd

0 0 0 1


Cinemática Direta

• Dados: variáveis das juntas (ângulos ou deslocamentos)

• Procurado: posição e orientação do efetuador

• Notação

• Numeração das juntas: 1 a n• Numeração dos elos : 0 a n (0 = base)• Variável da junta:qi• Sistema de coordenadas inercial (base):O0x0y0z0• Sistemas de coordenadas: 1 a n• Transformação homogênea entre os sistemas de coordenadasi e i− 1

Hii−1

(qi)


Cinemática Direta

• Transformação homogênea entre os sistemas de coordenadas da base (0) edo efetuador (n)

Tn0

=

[

Rn0

dn0

0 1

]

=

[

n0 s0 a0 dn0

0 0 0 1

]

=

nx sx ax dx

ny sy ay dy

nz sz az dz

0 0 0 1

• Sistema de coordenadas n no caso de uma garra• eixozn = a, na direção deapproach• eixoyn = s, na direção desliding• eixoxn = n, na direçãonormal aos outros eixos


Representação Denavit-Hartenberg

• Transformação homogênea entre os sistemas de coordenadasi e i− 1

Hii−1

= Rotzi−1,θiTranszi−1,di

Transxi,aiRotxi,αi

=(5)

cθi−sθi

0 0

sθicθi

0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 1 0 0

0 0 1 di

0 0 0 1

1 0 0 ai

0 1 0 0

0 0 1 0

0 0 0 1

1 0 0 0

0 cαi−sαi

0

0 sαicαi

0

0 0 0 1

=

cθi−sθi

cαisθisαi

aicθi

sθicθicαi

−cθisαi

aisθi

0 sαicαi

di

0 0 0 1

sendoθi, ai, di, αi parâmetros do eloi e juntai.



• Defina o eixozi ao longo do eixo da juntai+ 1. O eixozn é definido nadireção do eixozn−1 se a juntan é rotacional.

• DefinaO0 no eixoz0 ex0 convenientemente.

• Parai = 1, · · · , n− 1:

• Posicionar a origemOi onde a normal comum azi e zi−1 intersectazi.Sezi intersectazi−1 posicionarOi nesta intersecção. Sezi e zi−1 sãoparalelos, posicionarOi na juntai+ 1.

• Definaxi ao longo da normal comum azi e zi−1 com direção da juntaiparai+ 1, ou na direção normal ao planozi−1 − zi sezi−1 e zi seintersectam.

• DefinaOn convenientemente ao longo do eixozn (no centro da garra) exnnormal ao eixozn − 1.

• Escolhayi para completar a “regra da mão direita”Introducao a Robotica Industrial – p. 25/38


• Criar uma tabela dos parâmetros dos elos,ai, di, αi e θi.

• ai = distância ao longo dexi da intersecção dos eixosxi e zi−1 atéOi.

• di = distância ao longozi−1 deOi−1 até a intersecção dos eixosxi ezi−1. di é variável se a juntai é prismática.

• αi = o ângulo entrezi−1 e zi medido em torno dexi.

• θi = o ângulo entrexi−1 exi medido em torno dezi−1. θi é variável se ajuntai é rotativa.

• Calcular as matrizes de transformação homogêneasHii−1

substituindo osparâmetros acima em (5).

• CalcularTn0

= H1

0· · ·Hn

n−1.


Exemplo: Stanford


Exemplo: Stanford

• Parâmetros de D-H:

Elo ai di αi θi Offset

1 0 d1 -90◦ θ1 -90◦

2 0 d2 90◦ θ2 -90◦

3 0 d3 0 -90◦ 0

4 0 0 -90◦ θ4 0

5 0 0 90◦ θ5 0

6 0 d6 0 θ6 0


Ângulos de Orientação do KUKA

• Rotações em torno dos eixos fixos ao corpo

• Rotação em torno dez0 pelo ânguloφ

• Rotação em torno dey′

0pelo ânguloθ

• Rotação em torno dex′′

0pelo ânguloψ




Rz0,φRy′0,θRx′′

0,ψ =

cφ −sφ 0

sφ cφ 0

0 0 1

cθ 0 sθ

0 1 0

−sθ 0 cθ

1 0 0

0 cψ −sψ

0 sψ cψ

=

cφcθ cφsθsψ − sφcψ cφsθcψ + sφsψ

sφcθ sφsθsψ + cφcψ sφsθcψ − cφsψ





Rz0,φRy′0,θRx′′

0,ψ =

cφcθ cφsθsψ − sφcψ cφsθcψ + sφsψ

sφcθ sφsθsψ + cφcψ sφsθcψ − cφsψ


• Matriz de rotação genérica

R =

r11 r12 r13

r21 r22 r23

r31 r32 r33



• Cálculo dos ângulos: ser11 6= 0 e r21 6= 0

φ = atan2(r21, r11)

θ = atan2(−r31,√

r211

+ r221

)

ψ = atan2(r32, r33)



• Cálculo alternativo:

θ = asin(−r31)

ψsin = asin(r32/cos(θ))

ψcos = acos(r32/cos(θ))

ψ =

π − ψsin, se ψcos > π/2 e ψsin > 0

−π − ψsin, se ψcos > π/2 e ψsin < 0

ψsin, se ψcos < π/2

φsin = asin(r21/cos(θ))

φcos = acos(r21/cos(θ))

φ =

π − φsin, se φcos > π/2 e φsin > 0

−π − φsin, se φcos > π/2 e φsin < 0

φsin, se φcos < π/2



• Rotação em torno de um eixo, considerando o sistema WORLD:

• T1: matriz de transformação atual

• T2: matriz de transformação resultante

• Ri: matriz de transformação da rotação em torno dei (A = Z; B = Y; C = X)

T2 = RiT1



• Rotação em torno de um eixo, considerando o sistema da ferramenta:

• T1: matriz de transformação atual

• T2: matriz de transformação resultante

• Ri: matriz de transformação da rotação em torno dei (A = Z; B = Y; C = X)

T2 = T1Ri


Calibração da Ferramenta

• Método XYZ - 4 Pontos

• Mover o TCP (Tool Center Point) para um ponto de referência com 4orientações diferentes.

• Translação da origem do sistema de coordenadas na flange até oTCP:

TCPx

TCPy

TCPz

• Transformação homogênea do sistema de coordenadas na flangeaté o TCP:

Q =

1 0 0 TCPx

0 1 0 TCPy

0 0 1 TCPz

0 0 0 1



• A seguinte equação é satisfeita

TiQ = TjQ, i, j ∈ 1, 2, ..., N e i 6= j

• Denote

Ti =

[

Ra ta

0 1

]

e Tj =

[

Rb tb

0 1

]

• Então

[Ra −Rb]

TCPx

TCPy

TCPz

= −[ta − tb]

• Método dos Mínimos Quadrados



• Método XYZ - Ponto de Referência

• Mover um ponto conhecido da flange até um ponto de referência

• Mover o TCP até o mesmo ponto de referência

• Ponto conhecido da flange:

Fx

Fy

Fz

• Então

Ti

[

I F

0 1

]

= TjQ

[

Ra ta

0 1

][

I F

0 1

]

=

[

Rb tb

0 1

]

1 0 0 TCPx

0 1 0 TCPy

0 0 1 TCPz

0 0 0 1


Introdução à Robótica Industrial - mecatronica.eesc.usp.br · • Representação eixo/ângulo...

Documents

Transcript of Introdução à Robótica Industrial - mecatronica.eesc.usp.br · • Representação eixo/ângulo...