Kinematika - Prímá úloha, Inverzní úloharobotika/prednasky/2017_MRBT...Prˇ´ıma´ u´loha kinematiky Inverznı´ u´loha kinematiky DH: postup 1. Ocˇ´ıslovat jednotlive´

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



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



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



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



DH: postup


• 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



DH: postup


• 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



DH: postup


• 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



DH: postup


• 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



DH: postup


• 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



DH: postup


• 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



DH: postup


• 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



DH: postup


• 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 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 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



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




Resenı numericky


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)




Resenı numericky



dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)





RRH dydz

000 1

=

pR3x(q)


000 1

0R3H(q) 0

0000 1






Resenı numericky



dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)





RRH dydz

000 1

=

pR3x(q)


000 1

0R3H(q) 0

0000 1






Resenı numericky



dx = pR3x(q)

dy = pR3y(q)

dz = pR3z(q)





RRH dydz

000 1

=

pR3x(q)


000 1

0R3H(q) 0

0000 1






Resenı numericky


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β




Resenı numericky




r21 r22 r23r31 r32 r33

=


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




Resenı numericky




r21 r22 r23r31 r32 r33

=


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.




Resenı numericky




r21 r22 r23r31 r32 r33

=


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ı!




Resenı numericky

r11 r12 r13r21 r22 r23r31 r32 r33

=


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 −π · · ·π)




Resenı numericky

r11 r12 r13r21 r22 r23r31 r32 r33

=


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 −π · · ·π)




Resenı numericky

Singularity

r11 r12 r13r21 r22 r23r31 r32 r33

=


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




Resenı numericky

Singularity

r11 r12 r13r21 r22 r23r31 r32 r33

=


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




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.

Documents

Kinematika - Prímá úloha, Inverzní úloharobotika/prednasky/2017_MRBT...Prˇ´ıma´ u´loha kinematiky Inverznı´ u´loha kinematiky DH: postup 1. Ocˇ´ıslovat jednotlive´