Robotika Kinematika 13. dubna 2017 Ing. František Burian Ph.D.,
Řízení stacionárních robotů P P z q = f 1 (P) q z Pøímá úloha q U ROBOT q P R q = h(u) P = f (q)
DH: Denavit-Hartenberg (4DOF/kloub) A i = T rans(z, d i )Rot(z, θ i )T rans(x, a i )Rot(x, α i ) HARTENBERG, R. DANAVIT, J.: Kinematic Synthesis of Linkages, 1964 SPONG, 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, uzavřené a paralelní) A i = T rans(x, a i )Rot(x, α i )T rans(z, d i )Rot(z, θ i ) KHALIL W. KLEINFINGER J.: A new geometric notation for open and closed-loop robots, 1986 SVEJDA M.: Kinematika robotických architektur, doktorát ZCU, 2011 SU: Sheth-Uicker (6DOF/kloub, paralelní) SHETH P. UICKER J.:A generalized Symbolic notation for mechanisms, 1971 BONGARDT 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 A i = Rot(z, θ i )T rans(z, d i )T rans(x, a i )Rot(x, α i ) = f(θ i, d i, a i, α i ) Kloub i Kloub i + 1 Kloub i 1 Podmínky kladení os DH: Osa X i musí být kolmá k Z i 1 Osa X i musí protínat Z i 1 z i x i Parametry DH: θ i Kloubový úhel, Joint angle h i 1 z i 1 h i d i o i 1 a i θ i x i 1 x i o i α i d i Délka ramene, Link offset a i Předsunutí, Link Length α i Otočení, Link twist z i 1
Postup DH konvence 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n): Nalézt osy z i 3. Definovat s.s rámu (x 0, y 0, z 0 ) i efektoru, pravotočivě. *4. i (1,, n): Nalézt společnou příčku n i os z i 1 a z i *5. i (1,, n): Nalézt o i a h i jako průsečíky n i a os z i a z i 1 *6. i (1,, n 1): Položit x i a y i tak aby systém byl pravotočivý. 7. Vytvořit tabulku DH parametrů (θ i, d i, a i, α i ) α i = xi (z i 1, z i ) a i = h i o i d i = o i 1 h i θ i = zi 1 (x i 1, x i ) 8. Spočítej A i za pomocí DH parametrů 9. Spočítej T 0n = A 1 A 2 A n za pomocí DH parametrů
DH: postup 1. Očíslovat jednotlivé klouby od 1 3 2 1 0
DH: postup 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i Osa rotace rotačního kloubu i + 1 Osa posuvu translačního kloubu i + 1 Směr dle uvážení (obě varianty možné) z n zatím vynecháme z 2 z 1 z 0
DH: postup 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. Výchozí bod s.s. o 0 musí být na ose z 0 Poloha o 0 je dána vnějšími omezeními (dno manipulačního prostoru) Osu x 0 volíme volně, (ve směru robotu dopředu ) o n obvykle uprostřed efektoru Osa x n musí být kolmá k z n 1 Osa z n libovolně y n = z n x n a y 0 = z 0 x 0 Systémy os jsou pravotočivé z 2 z 1 z 0 y 0 z 3 o 3 x 3 y 3 o 0 x 0
DH: postup z 0 y 0 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. 4. i (1,, n): Nalézt n i z i 1 a z i kolmé: n i = z i 1 z i z i 1 a z i rovnoběžné: libovolný kolmý vektor z i 1 a z i mimoběžné: nejkratší příčka z 2 z 1 n 2 n 3 n 1 z 3 o 3 x 3 y 3 o 0 x 0
DH: postup z 0 y 0 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. o 2, h 3 z 2 n 2 n 3 z 3 o 3 x 3 y 3 4. i (1,, n): Nalézt n i o 1, h 1, h 2 5. i (1,, n): Nalézt o i a h i o i je průsečík n i a z i h i je průsečík n i a z i 1 z 1 n 1 o 0 x 0
DH: postup 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. 4. i (1,, n): Nalézt n i 5. i (1,, n): Nalézt o i a h i 6. i (1,, n 1): Nalézt x i a y i x i klademe podél n i x i vychází z o i směr od z i 1 y i = z i x i Systém os (x i, y i, z i ) je pravotočivý z 0 y 0 o 1, h 1, h 2 z 1 o 2, h 3 n 3 z 2 y 2 n x 2 2 y 1 x 1 n 1 z 3 o 3 x 3 y 3 o 0 x 0
DH: postup 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. 4. i (1,, n): Nalézt n i 5. i (1,, n): Nalézt o i a h i 6. i (1,, n 1): Nalézt x i a y i 7. Vytvoř tabulku DH parametrů q z 2 3 o 1, h 1, h 2 q 2 d 1 d 2 z 1 o 2, h 3 d 3 y 2 x 2 y 1 x 1 q 1 z 3 o 3 x 3 y 3 α i = xi (z i 1, z i ) a i = h i o i d i = o i 1 h i θ i = zi 1 (x i 1, x i ) z 0 y 0 o 0 x 0 i α i a i d i θ i 1 90 0 d 1 q 1 2 0 d 2 0 q 2 90 3 90 d 3 0 q 3 + 90
DH: postup 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. 4. i (1,, n): Nalézt n i 5. i (1,, n): Nalézt o i a h i q z 2 3 o 1, h 1, h 2 q 2 d 1 d 2 z 1 o 2, h 3 d 3 y 2 x 2 y 1 x 1 q 1 z 3 o 3 x 3 y 3 6. i (1,, n 1): Nalézt x i a y i 7. Vytvoř tabulku DH parametrů 8. Sestav matice transformace z parametrů A i = Rot(z, θ i )T rans(z, d i )T rans(x, a i )Rot(x, α i ) z 0 y 0 o 0 x 0 i α i a i d i θ i 1 90 0 d 1 q 1 2 0 d 2 0 q 2 90 3 90 d 3 0 q 3 + 90
DH: postup 1. Očíslovat jednotlivé klouby od 1 2. i (0,, n 1): Nalézt osy z i 3. Definovat s.s. báze a konce. 4. i (1,, n): Nalézt n i 5. i (1,, n): Nalézt o i a h i 6. i (1,, n 1): Nalézt x i a y i 7. Vytvoř tabulku DH parametrů 8. Sestav matice transformace z parametrů 9. Spočítej H 0n za pomocí DH parametrů H 0n = A 1 A 2 A n q z 2 3 d 1 d 2 o 1, h 1, h 2 q 2 z 1 o 2, h 3 y 2 x 2 y 1 x 1 q 1 z 0 y 0 d 3 z 3 y 3 o 3 x 3 o 0 x 0 i α i a i d i θ i 1 90 0 d 1 q 1 2 0 d 2 0 q 2 90 3 90 d 3 0 q 3 + 90
p 0 = H 0n (q)p n Matici H 0n (q) lze po roznásobení zpátky rozložit na jednotlivé komponenty následovně: [ ] R0n (q) d H 0n (q) = 0n (q) 0 1 d 0n (q) je poloha koncového bodu vůči rámu robotu R 0n (q) je orientace koncového bodu vůči rámu robotu možno převést na úhly za pomocí Eulerových úhlů
- příklad yb L1 q1 y1 x1 L2 q2 yk P xk Matici H BK (q) lze po roznásobení zpátky rozložit na jednotlivé komponenty následovně: xb i α i a i d i θ i 1 0 L 1 0 q 1 2 0 L 2 0 q 2 x = L 1 cos(q 1 ) + L 2 cos(q 1 q 2 ) y = L 1 sin(q 1 ) + L 2 sin(q 1 q 2 ) z = 0 ϕ = q 1 q 2 H BK = H B1 (q)h 1K (q) = Rot(z, q 1 )T rans(l 1, 0, 0)Rot(z, q 2 )T rans(l 2, 0, 0) = c 1 s 1 0 l 1 c 1 c 2 s 2 0 l 2 c 2 c 12 s 12 0 l 1 s 1 + l 2 c 12 = s 1 c 1 0 l 1 s 1 s 2 c 2 0 l 2 s 2 0 0 1 0 0 0 1 0 = s 12 c 12 0 l 1 s 1 + l 2 s 12 0 0 1 0 0 0 0 1 0 0 0 1 0 0 0 1
Řešení numericky H RH (q) Rameno a zápěstí H HK Efektor s nástrojem H RS Póza objektu H SO Póza otvoru Aby se nástroj dotkl otvoru pod správným úhlem, musí platit: K = O H RH (q)h HK = H RS H SO Musíme vyřešit pózu manipulátoru, konstanty přesuneme doprava: H RH (q) = H RS H SO H 1 HK
Řešení numericky 1. Rozdělíme si rameno a zápěstí: H RH (q) = H RS H SO H 1 HK = H R3(q)H 3H (q) H R3 (q) Rameno udává polohu a orientaci H 3K (q) Zápěstí udává orientaci
Řešení numericky 1. Rozdělíme si rameno a zápěstí: H RH (q) = H RS H SO H 1 HK = H R3(q)H 3H (q) H R3 (q) Rameno udává polohu a orientaci H 3K (q) Zápěstí udává orientaci 2. To přepíšeme maticově: d x p R3x (q) 0 R RH d y d z = R R3 (q) p R3y (q) R 3H (q) 0 p R3z (q) 0 000 1 000 1 000 1
Řešení numericky 3. Řešíme soustavu: d x = p R3x (q) d y = p R3y (q) d z = p R3z (q) 1. Rozdělíme si rameno a zápěstí: H RH (q) = H RS H SO H 1 HK = H R3(q)H 3H (q) H R3 (q) Rameno udává polohu a orientaci H 3K (q) Zápěstí udává orientaci 2. To přepíšeme maticově: d x p R3x (q) 0 R RH d y d z = R R3 (q) p R3y (q) R 3H (q) 0 p R3z (q) 0 000 1 000 1 000 1
Řešení numericky 3. Řešíme soustavu: d x = p R3x (q) d y = p R3y (q) d z = p R3z (q) 1. Rozdělíme si rameno a zápěstí: H RH (q) = H RS H SO H 1 HK = H R3(q)H 3H (q) H R3 (q) Rameno udává polohu a orientaci H 3K (q) Zápěstí udává orientaci 2. To přepíšeme maticově: d x p R3x (q) 0 R RH d y d z = R R3 (q) p R3y (q) R 3H (q) 0 p R3z (q) 0 000 1 000 1 000 1 4. nalezené kloubové souřadnice ramene vytkneme ven a řešíme: R 3H (q) = R 1 R3 R RH(q)
Řešení numericky 4. nalezené kloubové souřadnice ramene vytkneme a řešíme (RPY zápěstí): R 3H (q) = R 1 R3 R RH(q) = R ZY Z (α, β, γ) r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β
Řešení numericky 4. nalezené kloubové souřadnice ramene vytkneme a řešíme (RPY zápěstí): R 3H (q) = R 1 R3 R RH(q) = R ZY Z (α, β, γ) r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β Porovnáním prvků nalezneme r 33 = cos β jež má triviální řešení β = arccos(r 33 ) To ale nejde použít, funkce arccos má dvě řešení na intervalu ( π, π)!
Řešení numericky 4. nalezené kloubové souřadnice ramene vytkneme a řešíme (RPY zápěstí): R 3H (q) = R 1 R3 R RH(q) = R ZY Z (α, β, γ) r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β Porovnáním prvků nalezneme r 33 = cos β jež má triviální řešení β = arccos(r 33 ) To ale nejde použít, funkce arccos má dvě řešení na intervalu ( π, π)! Jediná použitelná goniometrická funkce která nalezne úhel přes všechny kvadranty je tan ϕ = sin ϕ. Musíme tedy tento poměr v matici cos ϕ nalézt.
Řešení numericky 4. nalezené kloubové souřadnice ramene vytkneme a řešíme (RPY zápěstí): R 3H (q) = R 1 R3 R RH(q) = R ZY Z (α, β, γ) r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β Sice z goniomerie víme, že sin 2 ϕ + cos 2 ϕ = 1 2, tedy platí: r 231 + r232 = sin 2 β cos 2 γ + sin 2 β sin 2 γ = 1 sin β ± r 2 31 + r2 32 r 33 = sin β cos β ± r 2 = tan β β = arctan 31 + r32 2 r 33 Více řešení!
Řešení numericky r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β sin(β > 0) r 23 + r 2 31 + r2 32 r 33 sin α cos β = r 13 cos α cos β = sin α cos α = tan α α = arctan r 23 r 13 r 32 sin β sin γ = r 31 sin β cos γ = sin γ cos γ = tan γ γ = arctan r 32 = sin β cos β r 31 + r 2 = tan β β = arctan 31 + r32 2 r 33...s výhodou používáme funkce programovacích jazyků atan2(y,x) která počítá ve všech kvadrantech (vrací úhel v rozsahu π π)
Řešení numericky r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β sin(β < 0) r 23 r 2 31 + r2 32 r 33 sin α cos β = r 13 cos α cos β = sin α cos α = tan α α = arctan r 23 r 13 r 32 sin β sin γ = r 31 sin β cos γ = sin γ cos γ = tan γ γ = arctan r 32 r 31 = sin β cos β = tan β β = arctan r 2 31 + r32 2 r 33...s výhodou používáme funkce programovacích jazyků atan2(y,x) která počítá ve všech kvadrantech (vrací úhel v rozsahu π π)
Singularity Řešení numericky r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β sin β = 0, tedy že β = 0, a cos β = +1 r 11 r 12 r 13 cos(α + γ) sin(α + γ) 0 r 21 r 22 r 23 = sin(α + γ) cos(α + γ) 0 r 31 r 32 r 33 0 0 1 a řešení je nekonečně mnoho: r 21 r 11 = sin(α + γ) cos(α + γ) = tan(α + γ) (α + γ) = arctan r 21 r 11
Singularity Řešení numericky r 11 r 12 r 13 c α c β c γ s α s γ c α c β s γ s α c γ c α s β r 21 r 22 r 23 = s α c β c γ c α s γ s α c β s γ + c α c γ s α s β r 31 r 32 r 33 s β c γ s β s γ c β sin β = 0, tedy že β = 180, a cos β = 1 r 11 r 12 r 13 cos(α γ) sin(α γ) 0 r 21 r 22 r 23 = sin(α γ) cos(α γ) 0 r 31 r 32 r 33 0 0 1 a řešení je nekonečně mnoho: r 21 r 11 = sin(α γ) cos(α γ) = tan(α γ) (α γ) = arctan r 21 r 11
Numerické (iterativní) řešení Řešení numericky Hledáme neznámé q v rovnici: p R = H RH (q)p H
Děkuji za pozornost 13. dubna 2017 Ing. František Burian Ph.D.