Upload
scr
View
9
Download
3
Embed Size (px)
DESCRIPTION
Denavit-Hartenberg
Citation preview
KIDUM KRIM, AGH w Krakowie 1
Katedra Robotyki i Mechatroniki
Akademia Górniczo-Hutnicza w Krakowie
Wojciech Lisowski
2Opis położenia i orientacji efektora
Zastosowanie modelu geometrycznego – zadanie proste
Kinematyka i dynamika układów mechatronicznych
KIDUM KRIM, AGH w Krakowie 2
Zagadnienia:Macierz przekształcenia jednorodnego: interpretacja elementówInterpretacja zadanej orientacji w oparciu o kosinusy kierunkowe lub kąty RPYNotacja Denavita-HartenbergaPrzykłady modeli geometrycznych
KIDUM KRIM, AGH w Krakowie 3
x
y
z
u
v
w
O
P
Położenie:x, y, z
Orientacja:ϕ, θ, ψ
Opis z wykorzystaniem macierzy przekształcenia jednorodnego
KIDUM KRIM, AGH w Krakowie 4
Macierz przekształcenia jednorodnego pozwala określić położenie i orientację lokalnego układu współrzędnych Puvw w układzie odniesienia Oxyz
x
y
z
u
v
w
O
P
=
1000Pzzz
Pyyy
Pxxx
zwvuywvuxwvu
A
=
zzz
yyy
xxx
wvuwvuwvu
R
wzvzuz
wyvyuy
wxvxux
kkjkikkjjjijkijiii
)o
))o
))o
)
)o
))o
))o
)
)o
))o
))o
)
= R RT− =1
KIDUM KRIM, AGH w Krakowie 5
R x C SS C
( , )α α αα α
= −
1 0 000
Podstawowe macierze rotacji
KIDUM KRIM, AGH w Krakowie 6
),(),(),(),,( θϕα zRotyRotxRotcbaTraA =
Rot xC SS C
( , )αα αα α
=−
1 0 0 00 00 00 0 0 1
Rot y
C S
S C( , )ϕ
ϕ ϕ
ϕ ϕ=
−
0 00 1 0 0
0 00 0 0 1
Rot z
C SS C
( , )θ
θ θθ θ
=
−
0 00 0
0 0 1 00 0 0 1
=⋅⋅=
1000100010001
),(),(),(),,(cba
czTrabyTraaxTracbaTra
4 podstawowe macierze przekształcenia jednorodnego
Uwaga! Składanie przekształceń jednorodnych nie jest przemienne:),(),(),(),( αϕϕα xRotyRotyRotxRot ≠
KIDUM KRIM, AGH w Krakowie 7
Przykład 1z0
x0
y0
O0
3
5
2
O1
x1
z1
y1
P
A1
352
0 0 0 1
=
? ? ?? ? ?? ? ?
T
T
T
z
y
x
]0,1,0[
]0,0,1[
]1,0,0[
1
1
1
=
−=
−=
A1
0 1 0 30 0 1 51 0 0 2
0 0 0 1
=
−
−
TPPO ]0,1,2[11 ==
TP ]1,0,1,2[1 =(
01
1( (P A P=
2501
0 1 0 30 0 1 51 0 0 2
0 0 0 1
2101
=
−
−
TPPO ]0,5,2[00 ==
KIDUM KRIM, AGH w Krakowie 8
Orientacja: ϕ, θ, ψ
),(),(),(),,( ΨΘΦ=ΨΘΦ wRotvRotzRotEU ),(),(),(),,( ψθφψθφ xRotyRotzRotRPY =
Kąty Eulera KKąąty: RPYty: RPY
(Fu)PRECESJI - ΦNUTACJI - ΘOBROTU WŁASNEGO - Ψ
OBROTU (Roll) - φPOCHYLENIA (Pitch) - θSKRĘTU (Yaw) - ψ
(Fu)
KIDUM KRIM, AGH w Krakowie 9
Wyznaczanie kątów RPY
−+−+++−
=
1000000
),,(ψθψθθ
ψθφψφψθφψφθφψθφψφψθφψφθφ
ψθφCCSCS
CSSSCSSSCCCSCSCSSSSCCSCC
RPY
=
1000zzzz
yyyy
xxxx
PAONPAONPAON
T
φ
θ
ψ
=
=−
−
=
arctgNN
arctg N
N
arctg OA
y
x
z
z
z
z
1 2
Roll (z)
Pitch (y)
Yaw(x)
Wyjątki:
o90=θ
( )( ) y
x
OO
=−=−
φψφψ
cossin
y
x
OOarctg=−φψ
o90−=θ
( )( ) y
x
OO
=+=+−
φψφψ
cossin
y
x
OOarctg −
=+φψ
KIDUM KRIM, AGH w Krakowie 10
Przykład 2
−
−−
−
=
10000010
0230
21
0210
23
1A
u v w
x ±150° ±90° ±60°
y ±120° ±90° ±150°
z ±90° ±180° ±90°
v
w
u
-60°
-150°
-150° 120°
x
y
z
KIDUM KRIM, AGH w Krakowie 11
( ) 001
0=
−=θtg 0=θ
( ) −∞=−
=01ψtg
φ
θ
ψ
=
=−
−
=
arctgNN
arctg N
N
arctg OA
y
x
z
z
z
z
1 2
o90−=ψ
x
y
z
-90°
x′
z′
y′
-150°
v
w
u
−
−−
−
=
10000010
0230
21
0210
23
1A
( )3
1
23
21
−−
=−
−=φtg °−= 150φ
KIDUM KRIM, AGH w Krakowie 12
Założenia budowy modelu geometrucznego
Rozważana klasa manipulatorów - struktura nienadmiarowa- otwarty łańcuch kinematyczny- człony modelowane jako bryły sztywne- człony połączone ze sobą przegubowo lub pryzmatycznie
Łańcuch kinematyczny manipulatora o jednym końcu zamocowanym do podstawy a drugim swobodnym.
W wolnym końcu łańcucha kinematycznego manipulatora zamocowany jest efektor, którym może być: - chwytak- narzędzie montażowe- narzędzie technologiczne - narzędzie inspekcyjne
KIDUM KRIM, AGH w Krakowie 13
Wspólne cechy seryjnie produkowanych manipulatorów:wszystkie złącza stanowią pary kinematyczne klasy 5, obrotowe
lub pryzmatyczneczłony manipulatora są prostolinioweoś obrotu złączy obrotowych jest albo równoległa albo
prostopadła do bliższego podstawy członu manipulatora
Ramię
Kiść
KIDUM KRIM, AGH w Krakowie 14
Wzajemne położenie par sąsiednich członów jest opisywane przez tzw. współrzędne złączowe (przegubowe):
q1, q2, ..., qn
Położenie i orientacja efektora (lub dowolnego członu) w przestrzeni jest określona przez współrzędne kartezjańskie:
x, y, z, ϕ, θ, ψ
Model geometryczny manipulatora pozwala wyznaczyćzależności między wartościami współrzędnych złączowych a wartościami współrzędnych kartezjańskich, określających położenie i orientację efektora w jego przestrzeni roboczej, co jest konieczne do zaplanowanie operacji manipulacyjnej lub technologicznej.
KIDUM KRIM, AGH w Krakowie 15
macierz Ai opisuje położenie i orientację układu i-tego w i-1
iiii AAAATT 1210 ... −==
Z podstawą manipulatora związany jest układ współrzędnych odniesieniaOx0y0z0z każdym i-tym członem związany jest układ lokalny Oxiyizimacierz 0Tn przekształcenia jednorodnego pozwala opisaćpołożenie i orientację dowolnego członu manipulatora (efektora) względem przyjętego układu odniesienia Ox0y0z0.
KIDUM KRIM, AGH w Krakowie 16
Notację Denavita-Hartenberga określa:sposób definiowania lokalnych układów współrzędnych definicję przekształcenia jednorodnego Ai
Środki lokalnych układów współrzędnych należy przyjmować:w punkcie przecięcia osi ruchu, jeśli osie te się przecinajątak by przesunięcia wzdłuż osi ruchu były minimalne (najlepiej 0) jeśli osie ruchu są równoległew punkcie końcowym odcinka wspólnej normalnej, jeśli osieruchu są skośne w przypadku wolnego końca łańcucha kinematycznego manipulatora: w osi kiści, na kołnierzu kiści lub w wybranym punkcie efektora.
KIDUM KRIM, AGH w Krakowie 17
Zasady definiowania lokalnych układów współrzędnych:skierować osie xi wszystkich lokalnych układów współrzędnych
jak oś x0 układu odniesieniaosią obrotu członu i-tego w złączach obrotowych jest oś zi-1osią przemieszczenia członu i-tego w złączach postępowych jest
oś zi-1osie xi-1 mogą być również osiami ruchu przy spełnieniu
dodatkowych warunków
Składowe przekształcenia jednorodne opisane macierzą Aito 4 przekształcenia podstawowe:
obrotu wokół osi zi-1 o kąt θiprzemieszczenia wzdłuż osi zi-1 o diprzemieszczenia wzdłuż osi xi-1 o aiobrotu wokół osi xi o kąt αi
KIDUM KRIM, AGH w Krakowie 18
),(),(),(),( 111 iiiiiiiii xRotaxTradzTrazRotA αθ ⋅⋅⋅= −−−
yi-1
zi-1
xi-1
xi
zi'
zi
yi'
yi
di
ai
αi
θi
KIDUM KRIM, AGH w Krakowie 19
Manipulator SCARA RRPR
Człon Nr θ d a α Zakres ruchu
1 θ1 v 0 a1 0 -120o÷120o
2 θ2 v 0 a2 π 0o÷150o
3 0 d3 v 0 0 0.1 m ÷0.3 m
4 θ4 v 0 0 0 -180o÷180o
x0
y0
z0
x1
y1z1
x2y2
z2
x3y3
z3
x4y4
z4
KIDUM KRIM, AGH w Krakowie 20
( )( )
o
o
1801
0
01
0
cossin
421421
421
412412
412412
=
−
=
=
=
−+=
−+−+
=
+−
=
arctg
arctg
arctgSSCCSCCSarctg
ψ
θ
θθθθθθθθθ
φφ
θ
ψ
=
=−
−
=
arctgNN
arctg N
N
arctg OA
y
x
z
z
z
z
1 2
x a C a Cy a S a Sz d
= += += −
1 1 2 12
1 1 2 12
3
04
12 4 12 4 12 4 12 4 1 1 2 12
12 4 12 4 12 4 12 4 1 1 2 12
3
00
0 0 10 0 0 1
T
C C S S C S S C a C a CS C C S C C S S a S a S
d=
+ − + +− − − +
− −
oe
x x x x
y y y y
z z z z
N O A PN O A PN O A P
τ =
0 0 0 1
Wyznaczenie współrzędnych kartezjańskich SCARA
KIDUM KRIM, AGH w Krakowie 21
x4
y4
z4
x2y2
z2
x0
y0
z0
x1y1
z1
x3
y3
z3
x5
y5
z5
Manipulator RRR RR
Link No.
θ d a α
1 θ1 v d1 0 -90o
2 θ2 v 0 a2 0
3 θ3 v 0 a3 0
4 θ4 v 0 0 90o
5 θ5 v d5 0 0
KIDUM KRIM, AGH w Krakowie 22
Link No. θ d a α
1 θ1 v d1 0 -90o
2 θ2 v 0 a2 0
3 θ3+90o v 0 0 90o
4 θ4 v d4 0 -90o
5 θ5 v 0 0 90o
6 0 0 0 α6 v
6 0 d7 0 0
manipulator RRRRRR(RRR+RPY)
x0
y0
z0
x1y1
z1
x2y2
z2
x3
y3
z3
x4
y4z4
x5
y5
z5
x6
y6
z6
x7
y7
z7
KIDUM KRIM, AGH w Krakowie 23
Nr członu θ d a α
1 0 d1 v 0 0
2 0 0 a2 v 90o
3 0 d3 v 0 0
T
ad
d3
2
3
1
1 0 00 0 10 1 00 0 0 1
=− −
Ramię kartezjańskie PPP
x0
x1
x2
x3
y0
y1
y2
y3
z0
z1
z2
z3
KIDUM KRIM, AGH w Krakowie 24
Ramię cylindryczne PRP
x0
y0
z0
x1
y1
z1
x2
y2
z2
x3
y3
z3
Nr członu θ d a α
1 0 d1 v 0 0
2 θ2 v 0 a2 -90o
3 0 d3 v 0 0
T
C S a C d SS C a S d C
d3
2 2 2 2 3 2
2 2 2 2 3 2
1
00
0 1 00 0 0 1
=
− −+
−
KIDUM KRIM, AGH w Krakowie 25
Nr członu θ d a α
1 θ1 v d1 0 -90o
2 θ2 v 0 0 90o
3 0 d3 v 0 0
Ramię sferyczne RRP
T
C C S C S d C SS C C S S d S S
S C d d C3
1 2 1 1 2 3 1 2
1 2 1 1 2 3 1 2
2 2 1 3 200 0 0 1
=
−
− +
x0
y0
z0
x1y1
z1
y2
z2
x3
y3
z3
KIDUM KRIM, AGH w Krakowie 26
Nr członu θ d a α
1 θ1 v d1 0 -90o
2 θ2 v 0 a2 0
3 θ3 v 0 0 90o
T
C C S C S a C CS C C S S a S C
S C d a S3
1 23 1 1 23 2 1 2
1 23 1 1 23 2 1 2
23 23 1 2 200 0 0 1
=
−
− −
Ramię antropomorficzne RRR
x0
y0
z0
x1y1
z1
x2y2
z2
x3
y3
z3
KIDUM KRIM, AGH w Krakowie 27
Konwencja orientowania osi chwytaka
Wektory: n – normalny (xe)o – orientacji (ye)a – zbliżenia (ze)
aon =×
(Fu)
KIDUM KRIM, AGH w Krakowie 28
Kiść Eulera (typ RBR)
KIDUM KRIM, AGH w Krakowie 29
x3
y3
z3
x4
y4z4
x5
y5
z5
x6
y6
z6
Nr członu θ d a α
4 θ4 v d4 0 -90o
5 θ5 v 0 0 90o
6 θ6 v d6 0 0
36
4 5 6 4 6 4 5 6 4 6 4 5 6 4 5
4 5 6 4 6 4 5 6 4 6 4 5 6 4 5
5 6 5 6 5 4 6 5
0 0 0 1
T
C C C S S C C S S C C S d C SS C C C S S C S C C S S d S S
S C S S C d d C=
− − −+ − +
− +
Kiść Eulera (typ RBR)
KIDUM KRIM, AGH w Krakowie 30
Kiść RPY (typ RBB)
KIDUM KRIM, AGH w Krakowie 31
x3
y3
z3
x7
y7
z7
x4
y4z4
x5
y5
z5
x6
y6
z6
Kiść RPY (typ RBB)
Nr członu θ d a α
4 θ4 v d4 0 -90o
5 θ5 v 0 0 90o
6 0 0 0 α6 v
6 0 d7 0 0
( )( )3
6
4 5 4 5 6 4 6 4 5 6 4 6 7 4 5 6 4 6
4 5 4 5 6 4 6 4 5 6 4 6 7 4 5 6 4 6
5 5 6 5 6 4 7 5 6
0 0 0 1
T
C C C S S S C C S C S S d C S C S SS C S S S C C S S C C S d S S C C S
S C S C C d d C C=
− + ++ − −
− +
KIDUM KRIM, AGH w Krakowie 32
y3
z3
x3
y4
z4
x4
y5
z5
x5
y6
z6
x6
Kiść RPY (typ BBR)
Nr członu θ d a α
4 θ4 v 0 0 0
5 0 0 0 α5 v
6 θ6 v d6 0 0
36
4 5 6 4 6 4 5 6 4 6 4 5 6 4 5
4 5 6 4 6 4 5 6 4 6 4 5 6 4 5
5 6 5 6 5 6 5
0 0 0 1
T
S C S C C S C C C S S S d S SC C S S C C C C S S C S d C S
S S S C C d C=
− + − −+ − − −
KIDUM KRIM, AGH w Krakowie 33
xd
xr
y0
z0
ydzd
yr
zr
Pd
Pr
∆P
Ocena różnicy pozycji
=
1000
0
dzdzdzdz
dydydydy
dxdxdxdx
d PAONPAONPAON
T
Zadana pozycja:
Osiągnięta pozycja:
=
1000
0
rzrzrzrz
ryryryry
rxrxrxrx
r PAONPAONPAON
TRóżnica położenia:
dr PP∆P −=
Różnica orientacji:
rT
drd
rd RRRRR 000
0 == ϕdr, θdr, ψdr
2222 rdrdrd AAOONN ×+×+×=∆Φ
drdrdr φθψ ++=∆Φ1