4. dinamika manipulatora - people.etf.unsa.bapeople.etf.unsa.ba/~jvelagic/laras/dok/dinamika.pdf ·...
TRANSCRIPT
Dinamika 76
4. DINAMIKA MANIPULATORA 4.1 UVOD Određivanje dinamičkog modela (eng. dinamic model) manipulatora je bitno sa stajališta simulacije kretanja, analize strukture manipulatora i sinteze algoritma upravljanja. Dinamičko ponašanje manipulatora dano je u obliku vremenske promjene konfiguracije robotske ruke u odnosu na momente zglobova, izazvane djelovanjem aktuatora pridruženih korespodentnim zglobovima manipulatora. Ova povezanost se može izraziti skupom diferencijalnih jednadžbi, tzv. jednadžbi kretanja, koje određuju odziv manipulatora uz prisustvo ulaznih momenata zglobova. Odziv manipulatora predstavlja kretanje segmenata manipulatora. Postoje dvije osnovne metode određivanja jednadžbi kretanja, odnosno dinamičkog modela robota, a to su: Lagrangeova i Newton-Eulerova metoda. Obje metode opisuju dinamike manipulatora u zglobovskom prostoru. Prva metoda opisuje dinamičko ponašanje manipulatora na osnovu rada i energije upotrebom generaliziranih koordinata. Sve radne i prinudne sile se automatski eliminiraju primjenom ove metode, a rezultantne jednadžbe su općenito izražene pomoću pomaka i momenata zglobova. Newton-Eulerova metoda se temelji na direktnoj primjeni drugog Newtonovog zakona kretanja, koji opisuje dinamiku sistema pomoću sila i momenata. Jednadžbe kretanja, dobivene ovom metodom, uključuju sve sile i momente koji djeluju na pojedinačne zglobove, kao i sile i momente koji djeluju među zglobovima. Newton-Eulerov postupak dobivanja dinamičkog modela robota je rekurzivan, te je pogodan za proračun dinamičkog modela na računaru, posebno kod robota sa većim brojem osi. Lagrangeov model je konceptualno jednostavniji i sistematičniji. U nastavku se tretira problematika dinamičkog modela manipulatora upotrebom Lagrangeovog postupka. 4.2 LAGRANGEOVA JEDNADŽBA Dinamički model manipulatora opisuje veze između momenata koji pogone zglobove i kretanja strukture manipulatora. Lagrangeovim postupkom se formiraju jednadžbe kretanja na sistematičan način neovisno o referentnom koordinatnom sistemu. Ovaj model se temelji na pojmovima: generalizirane koordinate i generalizirane sile. Generalizirane koordinate ,...,n,,iλi 21= opisuju položaj segmenata manipulatora sa n-stupnjeva pokretljivosti. Za manipulator sa strukturom otvorenog kinematičkog lanca, generalizirane koordinate čine vektor varijabli zglobova:
. (4.1) q=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
nλ
λM
1
Generalizirane sile su sile koje djeluju na zglobove manipulatora, odnosno, sile koje preostaju nakon uklanjanja inercijalnih sila i sile teže, pri čemu su inercijalne sile povezane sa kinetičkom energijom robotske ruke, a gravitacijske sile sa potencijalnom nergijom. Lagrangeova funkcija mehaničkog sistema se može definirati kao funkcija generaliziranih koordinata:
)()()( qqqqq U,T,L −= && , (4.2) gdje T i U predstavljaju kinetičku i potencijalnu energiju sistema.
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 77
Kinetička energija ovisi o položaju i brzini manipulatora ( ), a potencijalna energija ovisi samo o položaju (q), tj. zakretu robotske ruke.
qq &,
Korištenjem Lagrangeove funkcije, jednadžba kretanja manipulatora (Lagrangeova jednadžba) poprima slijedeći oblik:
,...,n , iξ,Lq
,Lqdt
di
ii
1)()( ==∂∂
−∂∂ qqqq &&&
(4.3)
gdje je generalizirana sila povezana sa generaliziranom koordinatom . iξ iλJednadžba (4.3) uspostavlja relacije između generalizirani sila narinutih na manipulator i položaja, brzine i ubrzanja zglobova. Slijedi da je pri izvodu dinamičkog modela manipulatora potrebno početi sa određivanjem kinetičke i potencijalne enrgije sistema. 4.2.1 Računanje kinetičke energije Ukupna kinetička energija manipulatora sa n krutih segmenata je definirana kao suma doprinosa kretanja svakog segmenta i doprinosa kretanja svakog aktuatora (motora):
, (4.4) (∑=
+=n
iml ii
TTT1
) gdje je kinetička energija segmenta i, a kinetička energija motora koji osnažuje segment i.
ilT
imTKinetička energija segmenta i dana je slijedećim izrazom:
∫=il
i V iTil dVT ρpp &&
21 , (4.5)
gdje su vektor linijske brzine, ip& ρ gustoća elementarnog dijela volumena dV i volumen segmenta i (Sl. 4.1.).
ilV
ilp
0x
0z
0y
ip
iril
p&
iω
Slika 4.1. Kinematički opis segmenta i za potrebe Lagrangeove formulacije.
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 78
Vektori pozicije elementarnog dijela i centra mase segmenta izraženi su u odnosu na koordinatni sistem baze. Razlika ovih vektora izražena je vektorom :
ipil
p
ir
iliT
iziyixi rrr ppr −== ] [ . (4.6) Vektor centra mase segmenta i dan je slijedećim izrazom:
∫=il
i
i V il
l dVm
ρpp 1 (4.7)
gdje je masa segmenta. Kao rezultat nevedenog, brzina zamišljene tačke zgloba može se napisati kao:
ilm
,)( iil
iili
i
i
rωSp
rωpp
+=
×+=
&
&& (4.8)
gdje i predstavljaju respektivno linijsku brzinu centra mase i ugaonu brzinu segmenta.
ilp& iω
Uvrštavanjem izraza (4-8) u (4-5) dobivaju se slijedeći doprinosi kinetičkih energija uslijed translacijskog i rotacijskog kretanja: Translacijsko. Doprinos je :
iii
ilii l
TllV l
Tl mdV pppp &&&&
21
21
=∫ ρ . (4.9)
Međusobno. Doprinos je :
0)()(212)(
212 =⎟
⎠⎞
⎜⎝⎛ −=⎟
⎠⎞
⎜⎝⎛ ∫∫
ilii
ili V lii
TlV ii
Tl dVdV ρρ ppωrω SpSp &&
budući da na osnovu (4.7) vrijedi . ∫∫ =
ili
il VlV i dVdV ρρ pp
Rotacijsko. Doprinos je :
iV iiiTT
iV iiiiTT
iilil
dVdV ωrSrωrωSω ⎟⎠⎞
⎜⎝⎛= ∫∫ ρρ )()(
21)()(
21 SSr
gdje je iskorišteno svojstvo . S obzirom na izraz matričnog operatora iiii ωrSrωS )()( −= )(⋅S
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−
−
−
=
0
0
0
)(
ixiy
iyiz
iyiz
i
rr
rr
rr
rS ,
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 79
dobiva se
ilTiV iiii
TTi i
il
dV ωIωrωSω21)()(
21
=∫ ρSr . (4.10)
Matrica
)( - -
- )( -
- - )(
22
22
22
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
+
+
+
=
∫∫∫∫∫∫∫∫∫
dVrrdVrrdVrr
dVrrdVrrdVrr
dVrrdVrrdVrr
iyixiziyizix
iziyizixiyix
izixiyixiziy
li
ρρρ
ρρρ
ρρρ
I
(4.11) ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
−−
−−
−−
=
zzlyzlxzl
yzlyylxyl
xzlxylxxl
iii
iii
iii
III
III
III
je simetrična i predstavlja tenzor inercije relativno u odnosu na centar mase segmenta i izraženog u koordinatama sistema baze. Tenzor inercije određuje raspodjelu mase krutog tijela. Matrica (4.11) se sastoji od šest različitih volumnih integrala pri čemu se izrazi na dijagonali te matrice nazivaju momenti inercije, a preostali vandijagonalni izrazi su produkti inercije. Budući da je pozicija segmenta i ovisna o konfiguraciji manipulatora to je i tenzor inercije, mjeren u odnosu na ishodište koordinatnog sistema baze, također ovisan o konfiguraciji manipulatora. Ugaona brzina izražena je u koordinatama baze. Međutim, za računanje ugaone brzine segmenta i izražene u koordinatama sistema centra mase segmenta (kao kod Denevit-Hartenbergove konvencije), potrebno je koristiti matricu rotacije na slijedeći način:
iω
i
Ti
ii ωRω =
gdje predstavlja matricu rotacije iz koordinatnog sistema segmenta i u koordinatnatni sistem baze. U odnosu na koordinatni sistem segmenta, tenzor inercije je konstantan, jer segment i njemu pridruženi koordinatni sistem rotiraju zajedno. Ako se ovaj konstantni tenzor označi sa i upotrijebi relacija (4.10) dobiva se:
iR
ili
I
iTi
ili
Tiil
Ti ii
ωωωIω RIR21
21
= . (4.12)
Iz izraza (4.12) slijedi izraz za računanje tenzora inercije :
ilI
. (4.13) T
iilil ii
RIRI = Ako se osi koordinatnog sistema centra segmenta i podudaraju s koordinatama tačke određene vektorom , tada su produkti inercije jednaki nuli i tenzor inercije relativno u odnosu na centar mase je dijagonalna matrica.
ip
Prema tome, ukupna kinetička energija segmenta i, koja obuhvaća i translacijska i rotacijska kretanja, jednaka je
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 80
iili
Til
Tll iiii
mT ωRIRωpp Tili 2
121
+= && . (4.14)
Kinetička energija manipulatora sastavljenog od n segmenata jednaka je zbroju kinetičkih energija svih njegovih segmenata:
∑=
+=
n
i
iili
Til
Tl
liii
mT
1 2ωRIRωpp T
ili&&
(4.15)
Sada je potrebno kinetičku energiju napisati kao funkciju generaliziranih koordinata manipulatora, odnosno varijabli zglobova. U tu će se svrhu primijeniti geometrijski postupak računanja Jacobiana na međusegmente. Rezultat ovoga su slijedeći izrazi:
(4.16) qJJJp &&&& )()(1
)(1 ... iii
i
lPi
lPi
lPl qq =++=
, (4.17) qJJJω &&& )()(1
)(1 ... iii l
Oil
Oil
Oi qq =++= gdje je doprinos stupaca matrice Jacobiana brzini zglobova uzet u obzir za trenutni segment i. Jacobiani i jednaki su: )( il
PJ )( ilOJ
(4.18) ] ... ... [ )()(
1)( 00iii l
Pil
Pl
P JJJ =
(4.19) ] ... ... [ )()(1
)( 00iii loi
lo
lo JJJ =
i stupci matrica (4.18) i (4.19) mogu se izračunati u skladu sa (3-5), dajući
(4.20) ⎪⎩
⎪⎨⎧
−×=
−−
−
zglob obrtni za )(
zglob jski translaciza
11
1)(
jlj
jlP
i
i
j ppz
zJ
, (4.21) ⎩⎨⎧
=− zglob obrtni za
zglob jski translaciza
1
)(
j
lO
i
j zJ
0
gdje su vektor pozicije ishodišta koordinatnog sistema j-1 i jedinični vektor duž osi z istog koordinatnog sistema, respektivno.
1−jp 1−jz
Uzimajući u obzir izraze (4.14) – (4.19), izraz za ukupnu kinetičku energiju segmenta i (4.14) postaje:
qJRIRJqqJJq &&&& )()()()(
21
21
i
i
iii
ii
lO
Ti
ili
TlO
TlP
TlP
Tll mT += . (4.22)
Kinetička energija motora koji pokreće zglob i može se izračunati na formalno analogan način onome provedenom za segment manipulatora. Razmatat će se tipičan slučaj rotirajućeg električnog motora (koji može pokretati i rotirajuće i translacijske zglobove uz upotrebu odgovarajućih prijenosnijh mehanizama). Pretpostavlja se da je stator motora smješten na segment manipulatora (masi segmenta pridodaje se masa statora), tako da se računa samo kinetička energija rotora (Sl. 4.2).
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 81
imp
0x
0z
0y
imp& ir
imω
Slika 4.2. Kinematički opis motora. Na temelju Sl. 4.2 motor zgloba i smješten je na segment i-1. U praksi, kod sinteze mehaničke strukture otvorenog kinematičkog lanca manipulatora nastoji se smjestiti pogonske motore što je moguće bliže bazi manipulatora tako da se olakša dinamički teret prvom zglobu lanca. Pogonski momenti razvijeni na osovinama motora prenose se na zglobove preko mehaničkih prijenosnika (reduktora, zupčanika). Doprinos kinetičke energije prijenosnog mehanizma može se na prikladan način uključiti u ukupnu kinetičku energiju motora. Također se pretpostavlja da nema pojave induciranih kretanja, tj. kretanje zgloba i ne utječe na kretanje drugih zglobova. Kinetička energija rotora i može se napisati kao
iiiiii mm
Tmm
Tmm mT ωIωpp
im 21
21
+= && , (4.23)
gdje je masa rotora, označava linijsku brzinu centra mase rotora, je tenzor inercije rotora u odnosu na centar mase i označava ugaonu brzinu rotora.
immimp&
imI
imωNeka
imϑ označava ugaonu poziciju rotora. U slučaju upotrebe krutih prijenosnika dobiva se slijedeća relacija: , (4.24)
imiir qk ϑ&& = gdje je omjer redukcije prijenosnika snage. Važno je napomenuti da je u slučaju pogona translacijskog zgloba omjer redukcije kvantitativno dimenzioniran.
irk
U skladu s pravilom kompozicije ugaonih brzina i relacije (4.24), ukupna ugaona brzina rotora je:
iii mri-m qk zωω i&+= 1 (4.25)
gdje je ugaona brzina segmenta i-1 na koji je motor smješten, i označava jedinični vektor duž osi rotora.
1−iω imz
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 82
Da bi napisali kinetičku energiju rotora kao funkciju zglobovskih varijabli, neophodno je izraziti linijsku brzinu centra mase rotora – slično u (4.16) kao
. (4.26) qJp && )( i
i
mPm =
Jacobian se računa na slijedeći način:
(4.27) ] ... ... [ )(1,
)(1
)( 00iii miP
mP
mP −= JJJ
čiji su stupci jednaki
(4.28) ⎪⎩
⎪⎨⎧
−×=
−−
−
zglob obrtni za )(
zglob jski translaciza
11
1)(
jmj
jmP
i
i
j ppz
zJ
gdje je vektor pozicije ishodišta koordinatnog sistema relativno u odnosu na zglob 1−jp 1−j . Komponenta Jacobiana u izrazu (4.27), jer je centar mase rotora uzet da leži duž osi rotacije. 0=)( i
i
mPJ
Ugaona brzina u (4.25) može se napisati kao funkcija zglobovskih varijabli, tj., . (4.29) qJω &)( i
i
mOm =
Komponenta Jacobiana koja se odnosi na orijentaciju ima oblik:
(4.30) ] ... ... [ )()(1,
)(1
)( 00iiii mOi
miO
mO
mO JJJJ −=
čiji stupci se računaju prema izrazu
(4.31) ⎪⎩
⎪⎨⎧
=
==
− .
11
1
)()(
ijk
,...,i-j
jr
lOm
O
i
i
ji
j z
JJ
Za računanje druge relacije u (4.31) dovoljno je poznavati komponente jediničnog vektora osi rotacije rotora s obzirom na koordinatni sistem baze. Slijedi, kinetička energija rotora i može se napisati kao
imz
qJRIRJqqJJq &&&& )()()()(
21
21
i
i
i
ii
iii
ii
mO
Tm
mmm
TmO
TmP
TmP
Tmm mT += . (4.32)
Konačno, sumiranjem različitih doprinosa u odnosu na pojedinačne segmente (4.22) i pojedinačne rotore (4.32) kao i u (4.4), ukupna kinetička energija manipulatora zajedno sa pogonima je dana u kvadratičnom obliku:
qqBqq &&&& )(21)(
21
1 1
Tn
i
n
jjiij qqbT ∑∑
= =
== (4.33)
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 83
gdje je
(4.34) ( )∑=
+++=n
i
mO
Tm
mm
TmO
mP
TmPm
lO
Ti
ii
TlO
lP
TlPl
i
i
i
i
iii
i
iiii
imm
1
)()()()()()()()()( JRIRJJJJRIRJJJqBii ml
(nxn) matrica inercije koja je:
• simetrična • pozitivno definitna • ovisna o konfiguraciji (općenito).
4.2.2 Računanje potencijalne energije Ukupna potencijalna energija akumulirana u manipulatoru se dobije kao suma doprinosa svakog pojedinačnog segmenta i svakog pojedinačnog motora
. (4.35) ( )∑=
+=n
iml ii
UUU1
Uz pretpostavku krutih segmenata, potencijalna energija segmenta i određena je samo djelovanjem gravitacijskih sila1:
(4.36) ii
ili l
TlV i
Tl mdVU pgpg 00 −== ∫ ρ
gdje je vektor gravitacijskog ubrzanja u koordinatnom sistemu baze (npr. ako je os z vertikalna), pri čemu je g koeficijent ubrzanja sile teže, čija standardna vrijednost iznosi:
. su vektori položaja centara mase segmenta i i rotora motora i u odnosu na koordinatni sistem baze.
0g Tg] 0 0[0 =g
20 / 80665.9 smg =
ii ml pp ,
Slično relaciji (4.36) dobiva se izraz za potencijalnu energiju rotora motora:
, (4.37) iii m
Tmm pmU 0g−=
Uvrštavanjem (4.36) i (4.37) u (4.35) dobiva se izraz za ukupnu potencijalnu energiju
. (4.38) ( )∑=
+−=n
im
Tml
Tl iiii
mmU1
00 pgpg
Na temelju jednadbže (4.38) dolazi se do zaključka da je potencijalna energija, preko vektora i
, funkcija samo varijabli zglobova q i da ne ovisi o brzinama zglobova q . il
p
imp &
1 U slučaju fleksibilnog segmenta postoji komponenta potencijalne energije uslijed djelovanja elastičnih sila. Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 84
4.2.3 Dinamičke jednadžbe kretanja Na temelju izračunatih potencijalnih i kinetičkih energija (4.33) i (4.38), Lagrangeova funkcija (4.1) za manipulator može se napisati kao
( )∑ ∑∑= ==
++=
−=n
i
n
im
Tml
Tl
n
jjiij iiii
mmqqb
U,T,L
1 100
1)()()(
21
)()()(
qpgqpgq
qqqqq
&&
&&
. (4.39)
Korištenjem dobivene Lagrangeove funkcije, jednadžba kretanja manipulatora (uzimajući u obzir činjenicu da potencijalna energija ne ovisi o ) poprima slijedeći oblik: q&
∑∑∑
∑∑
= ==
==
∂∂
+=
+=∂
∂=
∂∂
n
j
n
kjk
k
ijn
jjij
n
jj
ijn
jjij
ii
qqq
bqb
qdt
dbqb
q,T
dtd
q,L
dtd
1 11
11
,)(
)(
)()()()(
&&&&
&&&&
&
&
&
qqqqqq
(4.40)
∑∑= = ∂
∂=
∂∂ n
j
n
kjk
i
jk
i
qqq
bq
,T1 1
)(21)(
&&&
& qqq (4.41)
i
( )∑
∑
=
=
=+−=
⎟⎟⎠
⎞⎜⎜⎝
⎛
∂
∂+
∂
∂−=
∂∂
n
ji
mPi
Tm
lPi
Tl
n
j i
mTm
i
lTl
i
gmm
qm
qm
qU
j
j
j
j
j
j
j
j
1
)(0
)(0
100
)()()(
)()()(
qqJgqJg
qpg
qpgq
(4-42)
Uvrštavanjem izraza (4.40)–(4.42) u jednadžbu kretanja manipulatora (4.2) dobiva se:
(4.43) ∑∑∑= ==
==++n
j
n
kiijkijk
n
jjij nigqqhqb
1 11
.,...,1 )()()( ξqqq &&&&
gdje je
i
jk
k
ijijk q
bqb
h∂∂
−∂∂
=21 . (4.44)
Fizička interpretacija jednadžbe (4.43) otkriva slijedeće:
• Koeficijent predstavlja moment inercije u osi zgloba i, u trenutnoj konfiguraciji manipulatora, kada su ostali zglobovi blokirani.
iib
Koeficijent objašnjava djelovanje ubrzanja zgloba i na zglob j. ijb
• Izraz predstavlja centrifugalni efekat zgloba i izazvan brzinom zgloba j. 2jijjqh &
Izraz jeste Coriolisov efekat induciran na zglobu i pomoću brzina zglobova j i k. kjijk qqh &&
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 85
• Izraz označava moment generiran u osi i-tog zgloba manipulatora, u trenutnoj konfiguraciji, u prisustvu gravitacijskog djelovanja.
ig
Neki dinamička spregnuća zglobova, kao što su koeficijenti i , mogu se reducirati ili eliminirati u postupku sinteze strukture, tako da se pojednostavi problem upravljanja.
ijb ijkh
U pogledu nekonzervativnim sila koje djeluju na zglobove manipulatora, one su dane sa pogonskim momentom minus moment viskoznog i statičkog trenja. Momenti viskoznog trenja dani su sa , gdje označava (nxn) matricu koeficijenata visoznog trenja. Kao pojednostavljeni model momenata statičkog trenja, može se pretpostaviti , gdje je (nxn) dijagonalna matrica i označava vektor dimenzija (nx1) sa komponentama izraženim preko sign funkcija brzina pojedinačnih zglobova.
τ qF &v
vF)sgn(qF &S sF )sgn(q&
Ako je vrh manipulatora u dodiru sa okolinom, dio pogonskih momenata se koristi za uravnoteženje momenata izazvanih zglobovima sa dodirnim silama. Ovi momenti se mogu izraziti sa , pri čemu h označava vektor sila i momenata izazvanih djelovanjem vrha manipulatora na okolinu.
hqJ )(T
Uzimajući u obzir sve navedeno, jednadžbe kretanja (4.43) mogu se napisati u kompaktnom matričnom obliku, predstavljajući dinamički model zglobovskog prostora, na slijedeći način: , (4.45) hqJτqqFqFqqqCqqB g )()()(sgn)()( T
sv, −=++++ &&&&&&
gdje C predstavlja prikladnu (nxn) matricu čiji elementi zadovoljavaju slijedeću jednadžbu: ijc
. (4.46) ∑ ∑∑= = =
=n
j
n
j
n
kjkijkjij qqhqc
1 1 1
&&&
4.2.4 Bitna svojstva dinamičkog modela Izbor matrice C nije jednoznačan, budući da postoji više matrica C čiji elementi zadovoljavaju jednadžbu (4.46). Da bi se dobili koeficijenti potrebno je prvo izračunati elemrnte , na osnovu izraza (4.44), a nakon toga razmatrati cjelokupan izraz na desnoj strani jednadžbe (4.46). Na taj način se dobiva:
ijc ijkh
∑∑
∑ ∑∑
= =
= = =
∂∂
−∂∂
=
=
n
j
n
kjk
i
jk
k
ij
n
j
n
j
n
kjkijkjij
qqqb
qb
qqhqc
1 1
1 1 1
)21( &&
&&&
.
Gornja jednadžba se može dalje napisati kao:
∑∑∑∑∑= == == ∂
∂−
∂∂
+∂∂
=n
j
n
kjk
i
jk
j
ikn
j
n
kjk
k
ijn
jjij qq
qb
qbqq
qb
qc1 11 11
)(21
21
&&&&& ,
iz koje slijedi izraz za računanje koeficijenata : ijc
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 86
, (4.47) ∑=
=n
kkijkij qcc
1
&
gdje su jednaki: ijkc
⎟⎟⎠
⎞⎜⎜⎝
⎛
∂∂
−∂∂
+∂∂
=i
jk
j
ik
k
ijijk q
bqb
qb
c21 . (4.48)
Elementi nazivaju se Christoffelovi simboli prve vrste. S obzirom na svojstvo simetrije matrice B, vrijedi da je:
ijkc
ikjijk cc = . (4.49)
Izbor matrice C vodi ka određivanju prvog važnog svojstva jednadžbe kretanja (4.45), a to je da je matrica (4.50) ),(2)(),( qqCqBqqN &&& −= kososimetrična. To znači da za bilo koji vektor w dimenzije (nx1) vrijedi slijedeća relacija: . 0),( =wqqNw &T (4.51) Supstitucijom iz (4.48) u (4.47) dobiva se: ijkc
,)(
21
21
)(21
21
1
11
∑
∑∑
=
==
∂∂
−∂∂
+=
∂∂
−∂∂
+∂∂
=
n
kk
i
jk
j
ikij
n
kk
i
jk
j
ikn
kk
k
ijij
qqb
qbb
qqb
qbq
qb
c
&&
&&
a zatim uvrštavanje dobivenog u jednadžbu (4.50) daje: ijc
∑= ∂
∂−
∂∂
=−=n
kk
j
ik
i
jkijijij q
qb
qb
cbn1
)(2 && . (4.52)
Promatranjem izraza (4.52) slijedi slijedeća jednakost: jiij nn −= . Interesantno svojstvo, koje slijedi iz direktne primjene kososimetričnog svojstva matrice , postavljanjem jeste:
),( qqN &
qw &= . (4.53) 0),( =qqqNq &&&T
Može se lahko pokazati da izraz (4.53) vrijedi za bilo koji izbor matrice C, budući da je on rezultat principa očuvanja energije (hamilton). Na osnovu ovog principa, ukupan iznos vremenske derivacije kinetičke energije je uravnotežen sa snagom koju generiraju sve sile koje djeluju na zglobove
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 87
manipulatora. Nasuprot tome, izraz (4.51) vrijedi samo za pojedinačan izbor elemenata matrice C, kako je definirano u (4.47) i (4.48). Osim kososimetričnog svojstva matrice , drugo važno svojstvo dinamičke jednadžbe kretanja jeste linearnost u dinamičkim parametrima. Ova linearnost se odnosi na parametre karakterizirane manipulatorskim segmentima i rotorima motora. O ovom svojstvu se ovdje neće podrobno diskutirati, budući da zahtijeva malo složeniji izvod.
),( qqN &
4.2.5 Primjeri dinamičkih modela osnovnih struktura manipulatora Primjer 4.1 Dvosegmentna planarna ruka Promatrajmo robotsku ruku na Sl. 4.3, za koju je vektor generaliziranih koordinata . [ ]T21,ϑϑ=q
0x
0y
1a
2a
1l
1ϑ
2ϑ
11, ll Im
2l
22, ll Im
Slika 4.3. Dvosegmentna planarna ruka. Neka su udaljenosti centara mase segmenata od osi zglobova, respektivno. Mase pojedinačnih segmenata označene su sa , a mase rotora motora pridruženih zglobovima sa . Neka se sa označe momenti inercije s obzirom na osi rotora, a sa momenti inercije u odnosu na centre mase segmenata. U proračunima se pretpostavlja da je
21,ll
21, ll mm
21, mm mm
21, mm II
21, ll II
1−= im ppi
i , za i =1,2,..., tj. motori su locirani na osi zglobova s centrima mase u ishodištima kordinatnih sistema.
1−= im zzi
Jednadžba dinamike u kompaktnoj matričnoj formi je:
(4.54) hqJτqqFqFqqqCqqB g )()()(sgn)()( T, −=++++ &&&&&& sv
gdje su: - matrica inercija )(qB - matrica koja je nastala na temelju fizikalnih svojstava sistema )( qqC &, - moment viskoznog trenja qF &v
- moment statičkog trenja )(sgn qF &s
)(qg - moment usljed gravitacijskog ubrzanja - aktivni moment τ
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 88
- moment kojim vrh manipulatora djeluje na okolinu
hqJ )(T
a) Računanje Jacobiana za segmente Geometrijski Jacobiani koji daju vezu između linearnih brzina centara masa prvog i drugog segmenta, i brzina zglobova, su:
]0 [ )()( 1
1
1 llPP JJ =
. ] [ )()()( 2
2
2
1
2 lllPPP JJJ =
Budući da je prvi zglob obrtni, imamo :
)( 00)(
1
1
1ppzJ −×= l
lP
Matrica homogene transformacije centra mase prvog segmenta u odnosu na ishodište koordinatnog sistema baze je:
.
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
1 0 0 00 1 0 0
0 0
1111
1111
0 scscsc
l
l
l1 A
Vektor pozicije centra mase prvog segmenta u odnosu na bazni koordinatni sistem i ort su:
1lp 0z
, ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
000
; 100
; 0
0011
11
1pzp s
cl
l
l
a na temelju njih Jacobiani , odnosno su: )( 1
1
lPJ )( 1l
PJ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡×⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=−×=
0
0 100
)( 11
11
11
11
00)(
1
1
1cs
sc
P l
l
l
l
ll ppzJ
. (4.55)
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
0 0 0 0
11
11)( 1 c
s
P l
llJ
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 89
Analogan postupak se provodi za računanje , čije su komponente : )( 2l
PJ
)( 00)(
2
2
1ppzJ −×= l
lP
)( 11)(
2
2
2ppzJ −×= l
lP .
Matrice homogenih transformacija prvog segmenta, centra mase prvog i drugog segmenta u odnosu na bazni koordinatni sistem su:
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
1 0 0 00 1 0 0
0 0
1111
1111
01
sacscasc
A ; ;
.
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡ −
=
1 0 0 00 1 0 0
0 0
2222
2222
1 scscsc
l
l
l2 A
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡++−
=
1 0 0 00 1 0 0
0 0
122111212
122111212
0 ssacsccasc
l
l
l2 A
Na temelju njih se dobivaju slijedeći pripadajući vektori pozicija i ortovi, potrebni za izračunavanje Jacobiana :
100
; 100
; 0
; 0
; 000
1012211
12211
11
11
10 2
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡++
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡= zzppp ssa
ccasaca
l
l
l
Uvrštavanjem vrijednosti gornjih vektora u izraze za komponente matrice Jacobiana:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡+−−
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡++
×⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=−×=
0
0 100
)( 12211
12211
12211
12211
00)(
2
2
1ccassa
ssacca
P l
l
l
l
ll ppzJ
, ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡×⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=−×=
0
0 100
)( 122
122
122
122
11)(
2
2
2cs
sc
P l
l
l
l
ll ppzJ
dobiva se matrica Jacobiana koja daje ovisnost između linearne brzine centra mase drugog segmenta i brzine zglobova:
. (4.56) ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡+
−−−=
0 0
12212211
12212211)( 2 ccsa
sssa
P ll
lllJ
Veze između ugaonih brzina centara mase segmenata i brzina zglobova su izražene slijedećim Jacobianima:
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 90
]0 [ )()( 1
1
1 llOO JJ =
] [ )()()( 2
2
2
1
2 lllOOO JJJ =
Postupak izračunavanja ovih Jacobiana je slijedeći:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡==
100
0)( 1
1zJ l
O
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
0 10 00 0
)( 1lOJ (4.57)
; ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡==
100
0)( 2
1zJ l
O
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡==
100
1)( 2
2zJ l
O
. (4.58) ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
1 10 00 0
)( 2lOJ
Sljedeći korak se sastoji u izračunavanju Jacobiana koji se odnose na rotore motora smještenih na pojedine segmente. Važno je napomenuti da se mase statora motora pridodaju masama korespodentnih segmenata, a mase rotora motora se uzimaju odvojeno ( ).
21, mm mm
Povezanost linearnih brzina centara masa rotora motora i brzina zglobova je izražena slijedećim izrazima:
]0 [ )()( 1
1
1 mP
mP JJ =
] [ )()()( 2
2
2
1
2 mP
mP
mP JJJ =
gdje su:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=−×=−×=
000
)()( 00000)(
1
1
1ppzppzJ m
mP
, (4.59) ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
0 00 00 0
)( 1mPJ
i Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 91
0
0 1
00
)()( 11
11
11
11
01000)(
2
2
1
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡×⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=−×=−×= ca
sasaca
mm
P ppzppzJ
000
)()( 11011)(
2
2
2
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=−×=−×= ppzppzJ m
mP
. (4.60)
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡−=
0 0 0 0
11
11)( 2 ca
sam
PJ
Veze između ugaonih brzina centara masa rotora motora i brzina zglobova je dana sa:
]0 [ )()( 1
1
1 mO
mO JJ =
. ] [ )()()( 2
2
2
1
2 mO
mO
mO JJJ =
Postupak izračunavanja Jacobiana je slijedeći:
[ ]⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡===
1
011)( 0
0 0 ]0 [
1
1
1
r
rmrm
O
kzkzkJ
(4.61)
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
0 0 0 0 0
1
)( 1
r
mO
kJ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡==
100
)()( 2
1
2
1
lO
mO JJ
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡===
2
122)( 0
0
2
2
2
r
rmrm
O
kzkzkJ
. (4.62) ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
2
)(
10 00 0
2
r
mO
kJ
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 92
Matrica inercije za zadanu strukturu manipulatora je oblika:
)()()()()()(
22
2)()()(
)()()()()(1
11
)()()(
2
2
2
22
222
2
2
2
222
2
1
1
1
11
111
1
1
1
111
1
mO
Tm
mmm
TmO
mP
TmPmO
TTOP
TP
mO
Tm
mmm
TmO
mP
TmPmO
TTOP
TP
mm
mm
JRIRJJJJRIRJJJ
JRIRJJJJRIRJJJqB
++++
+++=l
llll
l
ll
llll
Smjenom i u gornju jednadžbu dobiva se: Ti
iiii llll RIRI = Tm
mmmm i
i
iiiRIRI =
.
)()()()()()()()()(
)()()()()()()()(
2
2
222
2
2
2
222
2
1
1
111
1
1
1
111
1
mOm
TmO
mP
TmPmO
TOP
TP
mOm
TmO
mP
TmPmO
TOP
TP
mm
mm
JIJJJJIJJJ
JIJJJJIJJJqB
++++
+++=l
llll
l
ll
llll
Uvrštavanjem dobivenih vrijednosti matrica Jacobiana u izraz za matricu inercije imamo:
, (4.63) ⎥⎦
⎤⎢⎣
⎡=
)()( )(
)(22221
212211
bbbb
ϑϑϑ
qB
gdje su:
222
222
2222111
22
2222
2221222112
21221
22
21
21
2111
)(
)2(
mr
mr
mmmr
kmb
kcambb
amcaamkmb
ΙΙ
ΙΙ
ΙΙΙΙ
++=
+++==
++++++++=
l
ll
lll
ll
ll
llll
Važna svojstva matrice inercije su : simetričnost, pozitivna definitnost i ovisnost o konfiguraciji. Cristoffelovi simboli prvog tipa su:
021
21
1
11
1
11
1
11
1
11111 =
∂∂
=⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂
−∂∂
+∂∂
=qb
qb
qb
qbc
hsamqb
qb
qb
qbc =−=
∂∂
=⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂
−∂∂
+∂∂
= 2212
11
1
12
1
12
2
11112 22
121
ll
hsamqb
qb
qb
qbc =−=
∂∂
=⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂
−∂∂
+∂∂
= 2212
12
1
22
2
12
2
12122 22
1ll (4.64)
hsamqb
qb
qb
qb
c −==∂∂
−=⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂
−∂∂
+∂∂
= 2212
11
2
11
1
21
1
21211 22
121
ll
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 93
021
21
1
22
2
12
1
22
2
21221212 =
∂∂
−=⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂
−∂∂
+∂∂
==qb
qb
qb
qbcc
021
21
2
22
2
22
2
22
2
22222 =
∂∂
=⎟⎟⎠
⎞⎜⎜⎝
⎛∂∂
−∂∂
+∂∂
=qb
qb
qb
qbc
Na temelju Cristoffelovih simbola dobiva se matrica , čiji su elementi: ),( qqC &
0
)(
2222
2
112212222
12212
2
112112121
21212122
2
111211212
221122112
2
111111111
=+==
−=+==
+=+=+==
==+==
∑
∑
∑
∑
=
=
=
=
qcqcqcc
hqcqcqcc
hhhqcqcqcc
hcqcqcqcc
kkk
kkk
kkk
kkk
&&&
&&&&
&&&&&&&
&&&&&
ϑ
ϑϑϑϑ
ϑϑ
,
odnosno
. (4.65) ⎥⎥⎦
⎤
⎢⎢⎣
⎡
−
+=
0
)( ),(
1
212
ϑ
ϑϑϑ&
&&&&
h
hhqqC
Računanje matrice N daje:
, (4.66) ⎥⎥⎦
⎤
⎢⎢⎣
⎡
+
−=
⎥⎥⎦
⎤
⎢⎢⎣
⎡
−
+−
⎥⎥⎦
⎤
⎢⎢⎣
⎡
−=−=
0 2
2- 0
0
)( 2
0
2),(2)(),(
21
21
1
212
2
22
ϑϑ
ϑϑ
ϑ
ϑϑϑ
ϑ
ϑϑ&&
&&
&
&&&
&
&&&&&
hh
hh
h
hh
h
hhqqCqBqqN
što omogućuje provjeru negativno-simetričnog svojstva matrice N. Elementi vektora )(qg se računaju po formuli:
( )∑=
+−=n
j
mP
TmP
Ti
j
ij
j
ijmmg
1
)(0
)(0 )()()( qJgqJgq l
l , (4.67)
gdje je vektor gravitacijskog ubrzanja. [ 0 00 g−=g ] Na osnovu (4.67) dobiva se:
( ) ( )
( ) 1221111
)(0
)(0
)(0
)(01
2221
2
12
2
12
1
11
1
11
)()()()()(
gcmgcamamm
mmmmg
m
mP
TmP
TmP
TmP
T
ll lll
ll
ll
+++=
=+−+−= qJgqJgqJgqJgq
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 94
( ) ( )
122
)(0
)(0
)(0
)(02
2
2
22
2
22
1
21
1
21
)()()()()(
gcm
mmmmg mP
TmP
TmP
TmP
T
ll
ll
ll
=
=+−+−= qJgqJgqJgqJgq
Uz zanemarenje djelovanja sila trenja i svih tipova kontaktnih sila, rezultirajuća dinamička jednadžba glasi:
(4.68)
( )( )
( )
( ) (2122
21221
222
2212221
22
11221111
2222121221
2222122
121221
22
21
21
21
22
222222
2221
22
222
21222111
)(
2
)(
)2(
τϑ
ϑΙΙϑΙΙ
τ
ϑϑϑ
ϑΙΙ
ϑΙΙΙΙ
=++
++++++
=++++
−−
++++
++++++++
gcmsam
kmkcam
gcmgcamamm
samsam
kcam
amcaamkm
mrmr
m
mr
mmmr
l&l
&&l&&ll
ll
&l&&l
&&ll
&&lll
ll
llll
lll
ll
ll
llll
)
4.3 NEWTON-EULEROVA JEDNADŽBA Lagrangeova formulaciji dinamičke jednadžbe manipulatora opisuje ponašanje sistema u obliku rada i energije pohranjenih u sistemu (ukupni Lagrangian sistema). Prisilne sile uključene u sistem se automatski eliminiraju primjenom ovog postupka izvođenja dinamičkih jednadžbi manipulatora. U Euler-Newtonovoj formulaciji, jednadžbe kretanja se izvode na temelju drugog Newtonovog zakona, koji povezuje sile i momente. Jednadžbe u sebi uključuju sve sile i momente koji djeluju na pojedini segment manipulatora, kao i sile i momente koji nastaju kao rezultat međudjelovanja između segmenata. Newton-Eulerova formulaciji se temelji na ravnoteži svih sila koje djeluju segment manipulatora. Ovo vodi ka skupu jednadžbi čija struktura omogućuje dobivanje rekurzivnog oblika rješenja; rekurzija prema naprijed se obavlja u svrhu dobivanja brzina i ubrzanja segmenta (jednadžbe s računanjem prema naprijed) i na temelju njih se dalje u povratnom prostiranju izračunavaju sile i momenti koji djeluju na svaki segment robotske ruke (jednadžbe s računanjem unatrag). U tim se jednadžbama kretanje svakog segmenta opisuje pomoću njegovog susjednog segmenta. Prema tome, Newton-Eulerovim postupkom se dva puta prolazi po segmentima, kako je prikazano na Sl. 4.4.
vrh manipulatora
baza
brzina, ubrzanje
sile, momenti
Slika 4.4. Rekurzivna Newton-Eulerova metoda.
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 95
U ovom poglavlju ćemo izvesti jednadžbe kretanja pojedinačnog segmenta manipulatora. Kako je već naglašeno ranije, kretanje krutog tijela može se dekomponirati u translacijsko kretanje odgovarajuće tačke fiksirane na krutom tijelu i rotacijsko kretanje krutog tijela oko te tačke. Dinamičke jednadžbe krutog tijela mogu se također prikazati dvjema jednadžbama: jedna opisuje translacijsko kretanje centroida (centra mase tijela), dok druga opisuje rotacijsko kretanje oko centroida. Promatrajmo osnaženi segment i (segment i kojemu je pridodat motor zgloba i+1) u kinematičkom lancu manipulatora na Sl. 4.5.
0gim
if
iCi,r
1+imω
Koordinatna os zgloba i
Koordinatna os zgloba i+1
Segment i
iµ
iCi ,1−r
1−iOiO
1+− iµ1+− if
iω
iCp&
iC
Slika 4.5. Označavanje segmenta za Newton-Eulerova formulaciju. Centar mase osnaženog segmenta karakteriziraju slijedeći parametri: iC masa osnaženog segmenta, im iI tenzor inercije osnaženog segmenta, moment inercije rotora,
imI vektor položaja centra mase segmenta u odnosu na ishodište kord. sistema i – 1,
iCi ,1−r vektor položaja centra mase segmenta u odnosu na ishodište kord. sistema i,
iCi ,r vektor položaja ishodišta sistema i u odnosu na ishodište sistema i – 1. ii ,1−r Oznake brzina i ubrzanja na Sl. 4.5 imaju slijedeća značenja: linijska brzina centra mase ,
iCp& iC linijska brzina ishodišta koordinatnog sistema i, ip& ugaona brzina segmenta, iω ugaona brzina rotora,
imω linijsko ubrzanje centra mase ,
iCp&& iC
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 96
linijsko ubrzanje ishodišta koordinatnog sistema i, ip&& ugaono ubrzanje segmenta, iω& ugaono ubrzanje rotora,
imω&
gravitacijsko ubrzanje. 0g Sile i momenti imaju slijedeće oznake: sila kojom segment i – 1 djeluje na segment i, if sila kojom segment i + 1 djeluje na segment i, 1+i-f moment kojim segment i – 1 djeluje na segment i s obzirom na ishodište sistem i -- 1, iµ moment kojim segment i + 1 djeluje na segment i s obzirom na ishodište sistem i 1+i-µ Također se pretpostavlja da su vektori i matrice izraženi u odnosu nakoordinatni sistem baze. Newtonova jednadbža koja opisuje translacijsko kretanje centra mase može se napisati kao 001 =−+− + iCpgff &&iiii mm . (4.69) Eulerova jednadžba za rotacijsko kretanje segmenta (momenti centra mase) može se napisati na slijedeći način:
)(1111,,11,1 ++++++− +=×−−×+
iiii mmiiriiCiiiCiii Iqkdtd zωIrfµrfµ & (4.70)
Napomenimo da gravitacijska sila ne generira bilo kakav momenat, jer je koncentrirana u centar mase.
0gim
Kako i kod Lagrangeove formulacije, prirodno je izraziti tenzor inercije u tekućem koordinatnom sistemu (konstantan tenzor). Slijedi, u skladu sa (4.13), da imamo T
iiiii RIRI = , gdje je matrica
rotacije iz koordinatnog sistema i u bazni koordinatni sistem. Uvrštavanjem ove relacije u prvi izraz na desnoj strani jednadbže (4.70) dobiva se:
iR
)(
)()(
)(
iiiii
iTi
iiiiii
Ti
iiii
Ti
iiii
iTi
iiii
Ti
iiii
Ti
iiiiidt
d
ωIωωIωRIRωωSωRIRωRIRωS
ωRIRωRIRωRIRωI
T
×+=
++=
++=
&
&&
&&&
(4.71)
gdje drugi izraz na desnoj strani ( )( iii ωIω × ) predstavlja žiroskopski moment izazvan ovisnošću iI o orijentaciji segmenta. Osim toga, uzimajući u obzir da jedinični vektor rotira prema segmentu i, potrebno je derivirati samo drugi izraz na desnoj strani u jednadžbi (4.70):
1+imz
111111 111 )(
++++++×+= +++ iiiiii mimimmimmi IqIqIq
dtd zωzz &&&& . (4.72)
Smjenom (4.71) i (4.72) u (4.70), rezultirajuća Eulerova jednadžba poprima oblik:
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 97
1111 11,11,
,11,1
)(
++++×++
×+=×−−×+
++++
++−
iiii
ii
mimiirmmiir
iiiiiCiiiCiii
IqkIqk zωz
ωIωωIrfµrfµ&&&
&. (4.73)
Jednadžbe (4.69) i (4.73) vladaju dinamičkim ponašanjem pojedinačnog segmenta robotske ruke. Kompletan skup jednadžbi za cijeli manipulator dobiva se razvojem navedenih jednadžbi zva sve segmente manipulatora, i=1,...,n. Generalizrana sila u zglobu i može se izračunati iz projekcije sile za translacijski zglob, odnosno momenta za rotirajući zglob, duž osi zgloba. Pored toga, postoji doprinos momenta inercije rotora
. Slijedi da je generalizirana sila u zglobu i jednaka:
if
iµ
ii mTmmri Ik zω
i&
(4.74) ⎪⎩
⎪⎨⎧
+
+=
−
−
zglob rotacijski za
zglob jski translaciza
1
1
ii
ii
mTmmrii
Ti
mTmmrii
Ti
i Ik
Ik
zωzµ
zωzfτ
i
i
&
&
Newton-Eulerove jednadžbe (4.69) i (4.73) i jednadžba (4.74) zahtijevaju računanje linijskog i ugaonog ubrzanja segmenta i i rotora i. Računanje se može obaviti na temelju relacija izvedenih za linijske i ugaone brzine:
(4.75) ⎩⎨⎧
+=
−−
−
zglob rotacijski za
zglob jski translaciza
11
1
iii
ii zω
ωω
ϑ&
i
(4.76) ⎪⎩
⎪⎨⎧
×+
×++=
−−
−−−
zglob rotacijski za zglob jski translaciza
,11
,111
iiii
iiiiiii
drωp
rωzpp
&
&&&
Deriviranjem po vremenu (4.75) dobivaju se izrazi za ugaono ubrzanje segmenta sa translacijskim zglobom 1−= ii ωω && , (4.77) i ugaono ubrzanje segmenta sa rotacijskim zglobom . (4.78) 11111 −−−−− ×+++= iiiiiiiii zωωzωω ϑϑϑ &&&&&&
Linijsko ubrzanje segmenta sa translacijskim zglobom dobiva se deriviranjem prvog elementa (4.76) po vremenu na slijedeći način: , (4.79) )( ,11,11111 iiiiiiiiiiiiiiiii ddd −−−−−−− ××+×+×+×++= rωωzωrωzωzpp &&&&&&&&&
gdje je iskorištena relacija . Uzimajući u obzir (4.77), jednadžba (4.79) može se napisati kao:
iiiiiii d ,111,1 −−−− ×+= rωzr &&
. (4.80) )(2 ,1,1111 iiiiiiiiiiiiii dd −−−−− ××+×+×++= rωωrωzωzpp &&&&&&&&
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 98
A n dobiva deriviranjem drugog nalog o se izraz za linijsko ubrzanja u slučaju rotacijskog zgloba,
. (4.81)
) i (4.81) mogu se sažeti na slijedeći način:
×+++ −−−−−
−
zglob rotacijski za 11111
1
iiiiiiii
i
zωωzω ϑϑϑ &&&&& (4.82)
(4.83)
brzanje centra mase segmenta i, koje se pojavljuje na lijevoj strani Newton-Eulerove jednadžbe
elementa (4.76) po vremenu:
)( ,1,111 iiiiiiiiiii d −−−− ××+×++= rωωrωzpp &&&&&&&
ednadžbe (4.77), (4.78), (4.80J
zglob jski translaciza &
⎨⎧
=i
ωω&
⎩ i
⎪⎩
⎪⎨⎧
××+×+
××+×+×++=
−−−
−−−−−
zglob rotacijski za )(zglob jski translaciza )(2
,1,11
,1,1111
iiiiiiii
iiiiiiiiiiiiii
ddrωωrωp
rωωrωzωzpp
&&&
&&&&&&&&
U(4.69), dobiva se deriviranjem izraza
ii CiiiC ,rωpp ×+= && na slijedeći način: )( ,, iii CiiiCiiiC rωωrωpp +×+×+= &&&&& . (4.84)
anje rotora dobiva se deriviranjem (4-25):
na kraju, izraz za ugaono ubrzI qkqk zωzωω
iiii mi-irmirii-m ×++= &&&&& . 11 (4.85)
izvedene u prethodnoj sekciji nisu u prikladnom obliku za upotrebe
toru zglobova, rekurzivno se računaju
•
.3.1 Rekurzivni algoritam 4
ewton-Eulerove jednadžbeN
analize dinamike i sinteze regulatora. One ne opisuju ulazno-izlazne relacije, odnosno relacije dobivene iz kinematike i statike. U ovom potpoglavlju, obavit će se modifikacija Newton-Eulerovih jednadžbi kako bi se dobile eksplicitne ulazno-izlazne relacije. Jednadbže kretanja nisu u zatvorenom obliku, jer je kretanje jednog segmenta spregnuto sa kretanjem drugih segmenata kroz kinematičke veze za brzine i ubrzanja. Kada su poznate pozicije, brzine i ubrzanja zglobova, mogu se na temelju njih izračunati brzine i ubrzanja segmenata, koja se dalje mogu iskoristiti u Newton-Eulerovim jednadbžama za pronalaženje sila i momenata koje djeluju na svaki segment u rekurzivnom obliku, polazeći od sila i momenata primijenjenih na vrh manipulatora. S druge strane, brzine segmenta i rotora mogu biti izračunate rekurzivno polazeći od brzine i ubrzanja baznog segmenta. Sumarno, rekurzivni algoritam se konstruira na slijedeći način:
• Polazeći od baze, uz poznavanje kretanja robota u prosbrzine i ubrzanja svakog segmenta manipulatora pa se dobivaju jednadžbe s računanjem prema naprijed (eng. forward recursion). Pomoću izračunatih brzina i ubrzanja segmenata mogu se, počinjući s vrhom manipulatora i nastavljajući prema bazi, izračunati sile i momenti koji djeluju na svaki segment robotske ruke. Tako dobivene jednadžbe nazivaju se jednadžbe s računanjem unatrag (eng. backward recursion).
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 99
Za rekurziju prema naprijed, specificiraju se , te brzina i ubrzanje baznog segmenta . Nakon toga se računaju upotrebom izraza (4.75), (4.82), (4.83),
(4.84) i (4.85), respektivno. Linearno ubrzanje može se uzeti kao
qqq &&&,,0000 ,, ωgpω &&& −
ii mCiii ωppωω &&&&&& i ,,,
00 gp &&&& − tako da se ukorporira izraz
0g− u računanju ubrzanja centra mase pomoću jednadžbi (4.84) i (4.85). iCp&&
Kada je završeno računanje brzina i ubrzanja u rekurziji prema naprijed od baznog segmenta do vrha manipulatora, rekurzijom prema unatrag mogu se odrediti sile. Ako je zadano (ili eventualno h=0), Newtonova jednadžba (4.69) može se napisati kao
TTn
Tn ] [ 11 ++= mfh
iCpff &&iii m+= +1 , (4.86) jer je doprinos gravitacijskog ubrzanja sadržan u . Nadalje, Eulerova jednadžba daje
iCp&&
1111 11,11,
,11,,1
)()(
++++×++
×++×+++×−=
++++
++−
iiii
ii
mimiirmmiir
iiiiiCiiiCiiiii
IqkIqk zωz
ωIωωIrfµrrfµ&&&
&, (4.87)
što je izvedeno iz izraza (4.73), gdje je izražen kao suma dva vektora koji su se već pojavili u računanje rekurzije prema naprijed. Rezultirajuće generalizirane sile u zglobovima se mogu izračunati pomoću (4.74) kao
iCi ,1−r
, (4.88) ⎪⎩
⎪⎨⎧
+++
+++=
−
−
zglob rotacijski za )sgn(
zglob jski translaciza )sgn(
1
1
FFIk
dFdFIk
isiivimT
mriiTi
isiivimT
mriiT
ii
ii
ii
ϑϑτ
&&&
&&&
zωzµ
zωzf
i
i
m
m
gdje uključen utjecaj momenta uslijed viskoznog trenja. U gornjim izvodima pretpostavljeno je da se svi vektori odnose na bazni koordinatni sistem. Rekurzivno računanje se znatno pojednostavljuje i postaje efikasnije ukoliko se svi vektore odnose na trenutni koordinatni sistem pridružen segmentu i. Ovo ima za posljedicu da se svi koji trebaju biti transformirani iz koordinatnog sistema i+1 u sistem i množe matricom rotacije , odnosno da se svi
vektori koji trebaju biti transformirani iz koordinatnog sistema i-1 u sistem i množe matricom .
ii 1+R
Tii
1−RPrema tome, izrazi (4.74), (4.82)-(4.88) mogu se ponovo napisati kao:
(4.89) ⎪⎩
⎪⎨⎧
+=
−−
−
−−
−
zglob rotacijski za )(
zglob jski translaciza
011
1
11
1
zωR
ωRω
T
T
iii
ii
ii
iii
iϑ&
(4.90) ⎪⎩
⎪⎨⎧
×++=
−−
−−
−
−−
−
zglob rotacijski za ) (
zglob jski translaciza
0110
11
1
11
1
zωzωR
ωRω
T
T
iiii
ii
ii
ii
iii
iϑϑ &&&&
&&
(4.91) ⎪⎩
⎪⎨⎧
××+×+
××+×+×++=
−−−−
−
−−−−
−−
zglob rotacijski za )(
zglob jski translaciza )(2)(
,1,111
1
,1,101
011
1
iii
ii
ii
iii
ii
ii
ii
iii
ii
ii
iii
ii
ii
iiii
ii
iii
i
dd
rωωrωpR
rωωrωzRωzpRp
T
TT
&&&
&&&&&&&&
(4.92) )( ,,iCi
ii
ii
iCi
ii
ii
iC iii
rωωrωpp +×+×+= &&&&&
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 100
(4.93) 111
111
1 −−− ×++= im
i-i-r
imr
i-i-
im iiiii
qkqk zωzωω ii &&&&&
i
ii
iii
ii m
iCpfRf &&+= +++
111 (4.94)
im
iimiir
immiir
ii
ii
ii
ii
ii
iCi
ii
iii
ii
iCi
iii
ii
ii
iiii
ii
IqkIqk1111 11,11,
,1
1111,,1
)()(
++++×++
×++×+++×−=
++++
+++++−
zωz
ωIωωIrfRµRrrfµ
&&&
& (4.95)
⎪⎩
⎪⎨⎧
+++
+++=
−−−
−−−
zglob rotacijski za )sgn(
zglob jski translaciza )sgn(11
01
110
1
FFIk
dFdFIk
isiiviim
Timri
ii
Tii
isiiviim
Timri
ii
Tiii
i
ii
ii
ϑϑτ
&&&
&&&
zωzRµ
zωzRf
i
i
m
m. (4.96)
Gornje jednadžbe imaju prednost da su i
Ciii i, , rI i konstantni izrazi, što uzrokuje da je
.
1−imi
zT]1 0 0[0 =z
00
00
00
00 ,, ωω &&& gp −
(4-89) (4-90) (4-91) (4-92) (4-93)
0111
11
11 11
,,,, mCpp ωωω &&&&&&
M
2111
11
11 11
,,,, −−−−
−−
−− −−
nm
nC
nn
nn
nn nn
pp ωωω &&&&&&
1,,,, −nm
nC
nn
nn
nn nn
pp ωωω &&&&&&
(4-89) (4-90) (4-91) (4-92) (4-93)
nnn qqq &&& ,,
111 ,, qqq &&&
(4-94) (4-95)
(4-94) (4-95)
11
11 , +
+++
nn
nnf µ
nn
nnf µ,
22
22 ,µf
11
11 ,µf (4-96)
(4-96)
1τ
nτ
Slika 4.6. Računarska struktura Newton-Eulerovog rekurzivnog algoritma.
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 101
Sumarno, rekurzivni algoritam, za zadane pozicije, brzine i ubrzanja zglobova, izvodi se u slijedeće dvije faze:
• Sa poznatim početnim uvjetima i , korištenjem izraza (4.89), (4.90), (4.91), (4.92) i (4.93) za i=1,...,n, računaju se i .
00
00
00 , gpω −&& 0
0ω&iC
ii
ii
ii i
ppωω &&&&& ,,, 1−imi
ω&
• Sa poznatim krajnjim (rubnim) i , korištenjem izraza (4.94) i (4.95) za i=1,...,n, računaju se i . Nakon toga se pomoću izraza (4.96) računa
11++n
nf 11++
nnm
iif i
im iτ . Računarska struktura algoritma je shematski prikazana na Sl. 4.6. Primjer 4.2 Dvosegmentna planarna ruka U ovom primjeru će se ilustrirati primjena Newton-Eulerovog algoritma na dvosegmentnu planarnu ruku. Zadani su početni uvjeti za brzine i ubrzanja: , 0===− 0
000
00
00 ]0 0[ ωωgp &&& Tg
i rubni uvjeti za sile: . 00 == 3
33
3 µf Budući da se svi vektori posmatraju u odnosu na trenutni koordinatni sistem segmenta, dobivaju se slijedeći vektori konstantnih iznosa:
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡=
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=0 0
0 0
0 0
0 0
222,1
2,2
11
1,01,1
2
2
1
1
alal C
C
C
C rrrr
gdje i idu sa negativnim predznakom. Matrice rotacije potrebne za transformaciju iz jednog u drugi koordinatni sistem su:
1Cl 2Cl
. 2,1 , 1 0 00 0
23
1 IRR ==⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡ −=− ics
sc
ii
iiii
Nadalje se pretpostavlja da se osi rotacije dva rotora podudaraju sa respektivnim osima zglobova, tj.
za i=1,2. Timi
]1 0 0[01 ==− zz
U skladu sa jednadžbama (4.89)-(4.96), Newton-Eulerov algoritam zahtijeva izvođenje slijedećih korka:
• Rekurzija prema naprijed: segment 1
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 102
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=
1
11 0
0
ϑ&ω
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=
1
11 0
0
ϑ&&&ω
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
+−
=0
111
12
11
11 gca
gsa
ϑ
ϑ&
&
&&p
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
++
++−
=
0
)(
)(
111
12
11
11
1
1gcal
gsal
C
C
C ϑ
ϑ&&
&
&&p
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
=
11
0 0 0
1
ϑ&&&
r
m
k
ω
• Rekurzija prema naprijed: segment 2
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
=
21
22 0
0
ϑϑ &&
ω
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
=
21
22 0
0
ϑϑ &&&&
&ω
⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
++++
++−−
=0
)(
)(
122
121212121
122
2122
121121
22 gcsaaca
gsacasa
ϑϑϑϑ
ϑϑϑϑ&&&&&&&
&&&&&
&&p
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
+++++
+++−−
=
0
))((
))((
122
121212121
122
2122
121121
22
2
2gcsaalca
gsalcasa
C
C
C ϑϑϑϑ
ϑϑϑϑ&&&&&&&
&&&&&
&&p
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 103
. ⎥⎥⎥
⎦
⎤
⎢⎢⎢
⎣
⎡
+
=
221
1 0 0
2
ϑϑ &&&&
&
r
m
k
ω
• Rekurzija prema unatrag : segment 2
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
+++++
+++−−
=
0
)))(((
)))(((
122
1212121212
122
2122
1211212
22 2
2
gcsaalcam
gsalcasam
C
C
ϑϑϑϑ
ϑϑϑϑ&&&&&&&
&&&&&
f
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
++++
++++++
∗∗
=
12222
12212
12212212
22212
22
)()(
)()()()(
22
22
gcalmsalam
calamalmI
CC
CCzz
ϑ
ϑϑϑϑϑ&
&&&&&&&&&&µ
.)()(
))((
)))()(((
12222
12212
222
2222
122212
2222
22
22
222
gcalmsalam
IkalmI
IkcalaalmI
CC
mrCzz
mrCCzz
++++
++++
+++++=
ϑ
ϑ
ϑτ
&
&&
&&
(4.97)
• Rekurzija prema unatrag : segment 1
⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
++++−
+++++
++++−
−+−++−
=
0
)()()(
)()()(
)()()(
)()()(
1212
21222
21222112111
1212
21222
2112
211121222
11
2
21
2
12
gcmmsalm
calmamalm
gsmmcalm
amalmsalm
C
CC
C
CC
ϑϑ
ϑϑϑϑ
ϑϑ
ϑϑϑϑ
&&
&&&&&&&&
&&
&&&&&&
f
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave
Dinamika 104
⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢
⎣
⎡
+++++
++−++
++++
+++++
+++++
∗∗
=
1222112111
2212212
212212
22212
22
212212212
1221212
11121211
11
)()(
)()()(
)()(
)()()(
)()(
21
22
22
2
21
gcalmgcamgcalm
salamsalam
Ikalm
calamI
calamalmamI
CC
CC
mrC
Czz
CCzz
ϑϑϑ
ϑϑϑ
ϑϑϑϑ
ϑϑϑϑ
&&&
&&&&&&
&&&&&&&&
&&&&&&&&
µ
.)())((
)()(2
)))()(((
)))(2)(()((
122211211
222212212212
222212
222
12212
22122
21
21111
21
22
222
2211
gcalmgcamalm
salamsalam
IkcalaalmI
calaalamIIkalmI
CC
CC
mrCCzz
CCzzmrCzz
+++++
+−+−
++++++
+++++++++=
ϑϑϑ
ϑ
ϑτ
&&&
&&
&&
(4.98)
Momentne komponente označene sa ‘*’ nisu se računale jer nisu povezane sa zglobovskim momentima 2τ i 1τ . Rezultirajući Newton-Eulerov model je identičan Lagrangeovom (4-68), jer vrijedi:
.)(
)(
)(
)(
222
222
222
2
211
211
111
1
222
22
2
2111
11
21
almIlmI
almlm
mm
IalmIlmI
almlm
mmm
llCzz
lC
l
mllCzz
lC
ml
−+=+
−=
=
+−+=+
−=
+=
(4.99)
Mr.sc. Jasmin Velagić, dipl.inž.el. Laboratorija za Robotiku i autonomne sustave