kinematika - prímá úloha, inverzní úloharobotika/prednasky/2017_mrbt...prˇ´ıma´ u´loha...
TRANSCRIPT
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.