Kinematika - Prímá úloha, Inverzní úloharobotika/prednasky/2017_MRBT...Prˇ´ıma´ u´loha...

Robotika

Kinematika13. dubna 2017

Ing. Frantisek Burian Ph.D.

Prıma uloha, Inverznı uloha

Prıma ulohakinematiky

Inverznı ulohakinematiky

Rızenı stacionarnıch robotu

ROBOT

q = h(U)R

P

Pøímá úloha

kinematiky

P = f (q)

Inverzní úloha

kinematiky

q = f −1(P)

R

Inverzní úloha

kinematiky

∆q = g(∆P)

q∆q U PPz qz

Pz ∆P qz∆qz

Prıma ulohakinematiky

Inverznı ulohakinematiky

Prıma uloha kinematiky

• DH: Denavit-Hartenberg (4DOF/kloub)Ai = Trans(z, di)Rot(z, θi)Trans(x, ai)Rot(x, αi)HARTENBERG, R. DANAVIT, J.: Kinematic Synthesis of Linkages, 1964SPONG, M. VIDYASAGAR, M.: Robot dynamics and control, 1989, 1st ed.SPONG, M. HUTCHINSON S. ...: Robot dynamics and control 2004, 2nd ed.LEWIS F. ...: Robot Manipulator Control: Theory and Practice 2004, 2nd ed.

• KK: Khalil-Kleinfinger (4DOF/kloub, uzavrene a paralelnı)Ai = Trans(x, ai)Rot(x, αi)Trans(z, di)Rot(z, θi)KHALIL W. KLEINFINGER J.: A new geometric notation for open and closed-loop robots, 1986SVEJDA M.: Kinematika robotickych architektur, doktorat ZCU, 2011

• SU: Sheth-Uicker (6DOF/kloub, paralelnı)SHETH P. UICKER J.:A generalized Symbolic notation for mechanisms, 1971BONGARDT B.: Sheth-Uicker convention revisited, 2013

• HR: Hayati-Roberts (6DOF/kloub)BRUYNINCKX, H.:Robot Kinematics and Dynamics, 2011, kap 2.5.4

Prıma ulohakinematiky

Inverznı ulohakinematiky

Denavit-Hartenberg: Parametry

Ai = Rot(z, θi)Trans(z, di)Trans(x, ai)Rot(x, αi) = f(θi, di, ai, αi)Kloub i+ 1Kloub i

Kloub i− 1

hi−1

oi−1

zi−1

xi−1

hi

oi

xi

zi

xi

θiαi

ai

di

zi−1

Podmınky kladenı os DH:• Osa Xi musı byt kolma k Zi−1

• Osa Xi musı protınat Zi−1

Parametry DH:• θi Kloubovy uhel, Joint angle• di Delka ramene, Link offset• ai Predsunutı, Link Length• αi Otocenı, Link twist

Prıma ulohakinematiky

Inverznı ulohakinematiky

Postup DH konvence

• 1. Ocıslovat jednotlive klouby od 1• 2. i ∈ (0, · · · , n): Nalezt osy zi• 3. Definovat s.s ramu (x0, y0, z0) i efektoru, pravotocive.• *4. i ∈ (1, · · · , n): Nalezt spolecnou prıcku ni os zi−1 a zi• *5. i ∈ (1, · · · , n): Nalezt oi a hi jako prusecıky ni a os zi a zi−1

• *6. i ∈ (1, · · · , n− 1): Polozit xi a yi tak aby system byl pravotocivy.• 7. Vytvorit tabulku DH parametru (θi, di, ai, αi)

αi = ^xi(zi−1, zi)

ai = |hioi|di = |oi−1hi|θi = ^zi−1(xi−1, xi)

• 8. Spocıtej Ai za pomocı DH parametru• 9. Spocıtej T0n = A1 ·A2 · · ·An za pomocı DH parametru

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

0

1

2

3

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi◦ Osa rotace rotacnıho kloubu i+ 1◦ Osa posuvu translacnıho kloubu i+ 1◦ Smer dle uvazenı (obe varianty mozne)◦ zn zatım vynechame

• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

z0

z1

z2

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.◦ Vychozı bod s.s. o0 musı byt na ose z0◦ Poloha o0 je dana vnejsımi omezenımi

(dno manipulacnıho prostoru)◦ Osu x0 volıme volne,

(ve smeru robotu ”dopredu”)◦ on obvykle uprostred efektoru◦ Osa xn musı byt kolma k zn−1

◦ Osa zn libovolne◦ yn = zn × xn a y0 = z0 × x0◦ Systemy os jsou pravotocive

• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

z1

z2

z0y0

x0

x3

y3

z3

o0

o3

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni◦ zi−1 a zi kolme: ni = zi−1 × zi◦ zi−1 a zi rovnobezne: libovolny kolmy vektor◦ zi−1 a zi mimobezne: nejkratsı prıcka

• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

z1

z2

z0y0

x0

x3

y3

z3

o0

o3

n1

n2

n3

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi◦ oi je prusecık ni a zi◦ hi je prusecık ni a zi−1

• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

z0y0

x0

x3

y3

z3

o0

o3

n1

n2

n3

z1

z2

o1, h1, h2

o2, h3

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi◦ xi klademe podel ni◦ xi vychazı z oi smer od zi−1

◦ yi = zi × xi◦ System os (xi, yi, zi) je pravotocivy

• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

z0y0

x0

x3

y3

z3

o0

o3

n1

n2

n3

z1

z2

o1, h1, h2

o2, h3

x1

x2

y1

y2

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru

αi = ^xi(zi−1, zi)

ai = |hioi|di = |oi−1hi|θi = ^zi−1

(xi−1, xi)

• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametru

z0y0

x0

x3

y3

z3

o0

o3

z1

z2

o1, h1, h2

o2, h3

x1

x2

y1

y2

d 1d 2

d3

q2

q3

q1

i αi ai di θi1 90 0 d1 q12 0 −d2 0 q2 − 90

3 −90 d3 0 q3 + 90

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametruAi = Rot(z, θi)Trans(z, di)Trans(x, ai)Rot(x, αi)

• 9. Spocıtej H0n za pomocı DH parametru

z0y0

x0

x3

y3

z3

o0

o3

z1

z2

o1, h1, h2

o2, h3

x1

x2

y1

y2

d 1d 2

d3

q2

q3

q1

i αi ai di θi1 90 0 d1 q12 0 −d2 0 q2 − 90

3 −90 d3 0 q3 + 90

Prıma ulohakinematiky

Inverznı ulohakinematiky

DH: postup

• 1. Ocıslovat jednotlive klouby od 1

• 2. i ∈ (0, · · · , n− 1): Nalezt osy zi• 3. Definovat s.s. baze a konce.• 4. i ∈ (1, · · · , n): Nalezt ni• 5. i ∈ (1, · · · , n): Nalezt oi a hi• 6. i ∈ (1, · · · , n− 1): Nalezt xi a yi• 7. Vytvor tabulku DH parametru• 8. Sestav matice transformace z parametru• 9. Spocıtej H0n za pomocı DH parametruH0n = A1A2 · · ·An

z0y0

x0

x3

y3

z3

o0

o3

z1

z2

o1, h1, h2

o2, h3

x1

x2

y1

y2

d 1d 2

d3

q2

q3

q1

i αi ai di θi1 90 0 d1 q12 0 −d2 0 q2 − 90

3 −90 d3 0 q3 + 90

Prıma ulohakinematiky

Inverznı ulohakinematiky

Prıma uloha kinematiky

p0 = H0n(q)pn

Matici H0n(q) lze po roznasobenı zpatky rozlozit na jednotlivekomponenty nasledovne:

H0n(q) =

[R0n(q) d0n(q)

0 1

]

• d0n(q) je poloha koncoveho bodu vuci ramu robotu• R0n(q) je orientace koncoveho bodu vuci ramu robotu◦ mozno prevest na uhly za pomocı Eulerovych uhlu

Prıma ulohakinematiky

Inverznı ulohakinematiky

Prıma uloha kinematiky - prıklad

q1

L1

q2

L2

P

xK

yKx1

y1

xB

yB

i αi ai di θi1 0 L1 0 q12 0 L2 0 q2

Matici HBK(q) lze po roznasobenızpatky rozlozit na jednotlivekomponenty nasledovne:

x = L1 cos(q1) + L2 cos(q1 − q2)y = L1 sin(q1) + L2 sin(q1 − q2)z = 0

ϕ = q1 − q2

HBK = HB1(q)H1K(q) = Rot(z, q1)Trans(L1, 0, 0)Rot(z, q2)Trans(L2, 0, 0) =

=

c1 −s1 0 l1c1s1 c1 0 l1s10 0 1 00 0 0 1

c2 s2 0 l2c2−s2 c2 0 l2s20 0 1 00 0 0 1

=

c12 −s12 0 l1s1 + l2c12s12 c12 0 l1s1 + l2s120 0 1 00 0 0 1

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Inverznı uloha kinematiky

• HRH(q) Rameno a zapestı• HHK Efektor s nastrojem• HRS Poza objektu• HSO Poza otvoru

Aby se nastroj dotkl otvoru podspravnym uhlem, musı platit:

K = O

HRH(q)HHK = HRSHSO

Musıme vyresit pozu manipulatoru,konstanty presuneme doprava:

HRH(q) = HRSHSOH−1HK

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

3. Resıme soustavu:

dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)

1. Rozdelıme si rameno a zapestı:HRH(q) = HRSHSOH

−1HK = HR3(q)H3H(q)

• HR3(q) Rameno udava polohu a orientaci• H3K(q) Zapestı udava orientaci

2. To prepıseme maticove:dx

RRH dydz

000 1

=

pR3x(q)

RR3(q) pR3y(q)pR3z(q)

000 1

0R3H(q) 0

0000 1

4. nalezene kloubove souradnice ramene vytkneme ven a resıme:

R3H(q) = R−1R3RRH(q)

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

3. Resıme soustavu:

dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)

1. Rozdelıme si rameno a zapestı:HRH(q) = HRSHSOH

−1HK = HR3(q)H3H(q)

• HR3(q) Rameno udava polohu a orientaci• H3K(q) Zapestı udava orientaci

2. To prepıseme maticove:dx

RRH dydz

000 1

=

pR3x(q)

RR3(q) pR3y(q)pR3z(q)

000 1

0R3H(q) 0

0000 1

4. nalezene kloubove souradnice ramene vytkneme ven a resıme:

R3H(q) = R−1R3RRH(q)

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

3. Resıme soustavu:

dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)

1. Rozdelıme si rameno a zapestı:HRH(q) = HRSHSOH

−1HK = HR3(q)H3H(q)

• HR3(q) Rameno udava polohu a orientaci• H3K(q) Zapestı udava orientaci

2. To prepıseme maticove:dx

RRH dydz

000 1

=

pR3x(q)

RR3(q) pR3y(q)pR3z(q)

000 1

0R3H(q) 0

0000 1

4. nalezene kloubove souradnice ramene vytkneme ven a resıme:

R3H(q) = R−1R3RRH(q)

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

3. Resıme soustavu:

dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)

1. Rozdelıme si rameno a zapestı:HRH(q) = HRSHSOH

−1HK = HR3(q)H3H(q)

• HR3(q) Rameno udava polohu a orientaci• H3K(q) Zapestı udava orientaci

2. To prepıseme maticove:dx

RRH dydz

000 1

=

pR3x(q)

RR3(q) pR3y(q)pR3z(q)

000 1

0R3H(q) 0

0000 1

4. nalezene kloubove souradnice ramene vytkneme ven a resıme:

R3H(q) = R−1R3RRH(q)

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

4. nalezene kloubove souradnice ramene vytkneme a resıme (RPYzapestı):

R3H(q) = R−1R3RRH(q) = RZY Z(α, β, γ)r11 r12 r13

r21 r22 r23r31 r32 r33

=

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β

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

4. nalezene kloubove souradnice ramene vytkneme a resıme (RPYzapestı):

R3H(q) = R−1R3RRH(q) = RZY Z(α, β, γ)r11 r12 r13

r21 r22 r23r31 r32 r33

=

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β

Porovnanım prvku nalezneme r33 = cosβ jez ma trivialnı resenıβ = arccos(r33) To ale nejde pouzıt, funkce arccos ma dve resenı naintervalu (−π, π) !

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

4. nalezene kloubove souradnice ramene vytkneme a resıme (RPYzapestı):

R3H(q) = R−1R3RRH(q) = RZY Z(α, β, γ)r11 r12 r13

r21 r22 r23r31 r32 r33

=

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β

Porovnanım prvku nalezneme r33 = cosβ jez ma trivialnı resenıβ = arccos(r33) To ale nejde pouzıt, funkce arccos ma dve resenı naintervalu (−π, π) !Jedina pouzitelna goniometricka funkce ktera nalezne uhel pres

vsechny kvadranty je tanϕ =sinϕ

cosϕ. Musıme tedy tento pomer v matici

nalezt.

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Resenı dekompozicı

4. nalezene kloubove souradnice ramene vytkneme a resıme (RPYzapestı):

R3H(q) = R−1R3RRH(q) = RZY Z(α, β, γ)r11 r12 r13

r21 r22 r23r31 r32 r33

=

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β

Sice z goniomerie vıme, ze sin2 ϕ+ cos2 ϕ = 12, tedy platı:√

r231 + r232 =

√sin2 β cos2 γ + sin2 β sin2 γ =

√1 sinβ

±√r231 + r232r33

=sinβ

cosβ= tanβ β = arctan

±√r231 + r232r33

Vıce resenı!

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

r11 r12 r13r21 r22 r23r31 r32 r33

=

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β

sin(β > 0)

r23r13

=sinα cosβ

cosα cosβ=

sinα

cosα= tanα α = arctan

r23r13

r32−r31

=sinβ sin γ

sinβ cos γ=

sin γ

cos γ= tan γ γ = arctan

r32−r31

+√r231 + r232r33

=sinβ

cosβ= tanβ β = arctan

+√r231 + r232r33

...s vyhodou pouzıvame funkce programovacıch jazyku atan2(y,x)ktera pocıta ve vsech kvadrantech (vracı uhel v rozsahu −π · · ·π)

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

r11 r12 r13r21 r22 r23r31 r32 r33

=

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β

sin(β < 0)

−r23−r13

=− sinα cosβ

− cosα cosβ=− sinα

− cosα= tanα α = arctan

−r23−r13

−r32r31

=− sinβ sin γ

− sinβ cos γ=− sin γ

− cos γ= tan γ γ = arctan

−r32r31

−√r231 + r232−r33

=− sinβ

− cosβ= tanβ β = arctan

−√r231 + r232−r33

...s vyhodou pouzıvame funkce programovacıch jazyku atan2(y,x)ktera pocıta ve vsech kvadrantech (vracı uhel v rozsahu −π · · ·π)

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Singularity

r11 r12 r13r21 r22 r23r31 r32 r33

=

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β

sinβ = 0, tedy ze β = 0◦, a cosβ = +1

r11 r12 r13r21 r22 r23r31 r32 r33

=

cos(α+ γ) − sin(α+ γ) 0sin(α+ γ) cos(α+ γ) 0

0 0 1

a resenı je nekonecne mnoho:

r21r11

=sin(α+ γ)

cos(α+ γ)= tan(α+ γ) (α+ γ) = arctan

r21r11

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Singularity

r11 r12 r13r21 r22 r23r31 r32 r33

=

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β

sinβ = 0, tedy ze β = 180◦, a cosβ = −1r11 r12 r13

r21 r22 r23r31 r32 r33

=

cos(α− γ) sin(α− γ) 0sin(α− γ) − cos(α− γ) 0

0 0 1

a resenı je nekonecne mnoho:

r21r11

=sin(α− γ)cos(α− γ) = tan(α− γ) (α− γ) = arctan

r21r11

Prıma ulohakinematiky

Inverznı ulohakinematiky

Resenı dekompozicı

Resenı numericky

Numericke (iterativnı) resenı

Hledame nezname q v rovnici:

pR = HRH(q)pH

Dekuji za pozornost

13. dubna 2017

Ing. Frantisek Burian Ph.D.