Robotika I laboratorijske vaje

Transcript

1 Robotika I laboratorijske vaje

2

3 ROBOTIKA I LABORATORIJSKE VAJE MATJAŽ MIHELJ Fakulteta za elektrotehniko, Univerza v Ljubljani Univerza v Ljubljani, Založba FE in FRI Ljubljana, 2007

4 iv ROBOTIKA I

5 Kazalo 1. HITROSTNI PROFIL GIBANJA ROBOTA 1 1 Uvod 1 2 Naloga 2 3 Obdelava podatkov 3 2. GEOMETRIJSKA JACOBIJEVA MATRIKA 5 1 Direktni kinematični model 5 2 Vpliv sklepnih hitrosti na gibanje vrha robota 5 3 Translacijska Jacobijeva podmatrika 6 4 Rotacijska Jacobijeva podmatrika 8 5 Naloga 8 3. ANALITIČNA JACOBIJEVA MATRIKA 13 1 ZYZEulerjevikoti 14 2 Analitična Jacobijeva matrika 15 3 Naloga INVERZNA KINEMATIKA Z UPORABO JACOBIJEVE MATRIKE 19 1 Izvedba izračuna inverzne kinematike 19 2 Naloga TRANSFORMACIJE SIL IN NAVOROV 23 1 Transformacije hitrosti 23 2 Transformacije sil in navorov 25 3 Naloga 26

6 vi ROBOTIKA I 6. NAČRTOVANJE TRAJEKTORIJE 29 1 Interpolacija 29 2 Naloga NEWTON-EULERJEVA DINAMIKA 39 1 Enačbe za izračun dinamičnih enačb robota 39 2 Izračun inverznega dinamičnega modela 40 3 Izračun direktnega dinamičnegamodela 41 4 Naloga 42

7 HITROSTNI PROFIL GIBANJA ROBOTA 1. Uvod Pri vodenju gibanja robotov uporabljamo različne hitrostne profile, ki določajo časovni potek spremenljivk sklepov in posledično lege vrha manipulatorja. Slika 1.1 prikazuje časovni potek položaja, hitrosti in pospeška pri gibanju robota s trapeznim hitrostnim profilom. 6 q(rad) q(rad/s) q(rad/s 2 ) t(s) Slika 1.1. Pozicija, hitrost in pospešek pri vodenju s trapeznim hitrostnim profilom.

8 2 ROBOTIKA I 2. Naloga Napišite in izvedite program za gibanje robota po trikotni trajektoriji. Med izvajanjem programa merite gibanje robota z uporabo optičnega merilnega sistema OPTOTRAK. Podatke prenesite v MATLAB in določite hitrosti in pospeške robota kot prvo in drugo diferenco položaja. Gibanje po trikotni trajektoriji izvajajte pri največji hitrosti in pri različnih: načinih gibanja (MOVE oziroma MOVES) pospeških in pojemkih (ACCEL 30, 30; ACCEL 60, 60; ACCEL 90, 90). Določite oglišča trikotnika ABC v vertikalni ravnini kot prikazuje slika 1.2. Pri tem upoštevajte sledeča navodila: 1 gibanje naj se izvaja samo v drugem in tretjem sklepu robota, 2 trajektorijo med točkama A in B definirajte tako, da bo zahtevala samo zasuk v tretjem sklepu, torej spremembo kota ϕ, 3 trajektorijo med točkama B in C definirajte tako, da bo zahtevala samo zasuk v drugem sklepu, torej spremembo kota ϑ, 4 posledično bo gib med točkama C in A zahteval sočasno gibanje v obeh sklepih, 5 spremembi kotov v drugem in tretjem sklepu naj bosta približno enaki in dovolj veliki, da bo robot med gibom dosegel največjo hitrost. Za vodenje robota po trikotni trajektoriji napišite program v programskem jeziku VAL II. Program naj vodi gibanje vrha robota po izbrani trajektoriji s predpisanima hitrostjo in pospeškom. Na začetku programa definirajte hitrosti (SPEED x) ter pospeške in pojemke (ACCEL x,x). Uporabite ukaz ALWAYS, če naj določena hitrost velja za vse naslednje gibe. Za premikanje robota uporabite ukaza MOVE oziroma MOVES, kot je določeno v besedilu naloge. Ko je gib robota dokončan se stanje STATE(2) postavi na vrednost 2 (WAIT STATE(2) == 2). Gibanje robota merite z optičnim merilnim sistemom OPTOTRAK. Merilni markerji (aktivne infrardeče diode) so pritrjeni na robota kot prikazuje slika 1.2vtočkah M 1, M 2 in M 3. Merilni sistem omogoča merjenje položaja merilnih markerjev v prostoru z natančnostjo reda velikosti 0.1 mm. Frekvenca vzorčenja merilnega sistema naj bo 100 Hz. Meritve se shranijo v datoteko, ki je primerna za obdelavo s programskim paketom MATLAB. V datoteki so zapisane prostorske koordinate vseh merilnih markerjev.

9 Hitrostni profil gibanja robota 3 z6 z5 d6 x6 x5 d4 z0 z2 x0 a2 x2 Slika 1.2. Določitev oglišč trikotnika in postavitev merilnih markerjev na robota. B B A A C C Slika 1.3. Trikotna trajektorija z uporabo ukazov MOVES oziroma MOVE. 3. Obdelava podatkov V MATLABu napišite program za izračun kotov v sklepih robota glede na sliko 1.2. Hitrosti in pospeške izračunajte kot prvo oziroma drugo diferenco položaja. Ker odvajanje ojačuje šum pri visokih frekvencah, je potrebno rezultate meritev predhodno filtrirati z nizkopasovnim filtrom. Za filtriranje uporabite filter, ki ga izračuna program MATLAB. Uporabite lahko Butterworthov filter (BUTTER) oziroma Chebysev filter (CHEBY1, CHEBY2).

10 4 ROBOTIKA I Butterworthov filter [B,A] = BUTTER(N,Wn), kjer sta A in B vektorja parametrov filtra dolžine N+1, N je red filtra ter Wn mejna frekvenca filtra določena kot količnik glede na polovico vzorčne frekvence in lahko zavzame vrednosti med 0.0 < Wn< 1.0. Chebyshev filter [B,A] = CHEBY1(N,R,Wn), kjer so oznake enake kot pri BUTTER filtru, R pa določa valovitost v db v propustnem področju filtra. Začetna vrednost R naj bo približno 0.5. [B,A] = CHEBY2(N,R,Wn), kjer R določa valovitost v zapornem področju filtra. Začetna vrednost R naj bo približno 20. Za filtriranje uporabite funkcijo FILTFILT, ki ne spreminja faze filtriranega signala. Y = FILTFILT(B, A, X) filtrira signal, ki je podan v vektorju X s filtrom, ki je opisan z vektorjema A in B, s čimer dobimo filtriran signal zapisan v vektorju Y. Filtriranje lahko opišemo z naslednjo diferenčno enačbo: y(k)=b 0 x(k)+b 1 x(k 1)+...+ b n x(k n) a 1 y(k 1)... a n y(k n). Izračun odvoda Za numerični izračun odvoda uporabite funkcijo Y=DIFF(X),kivrnevektor diferenc med sosednjimi elementi (dolžina(y) = dolžina(x) - 1).

11 GEOMETRIJSKA JACOBIJEVA MATRIKA 1. Direktni kinematični model Slika 2.1 prikazuje robot Staubli v referenčni legi z označenimi koordinatnimi sistemi posameznih segmentov. Na podlagi skice je mogoče določiti direktni geometrijski model robota s homogenimi transformacijskimi matrikami A 1...A 6, ki opisujejo medsebojno lego segmentov robota. Homogene transformacijske matrike A i skupaj z matriko T 6, ki opisuje lego vrha robota glede na bazni koordinatni sistem, bomo uporabili za izračun geometrijske Jacobijeve matrike robota. 2. Vpliv sklepnih hitrosti na gibanje vrha robota Vpliv sklepne hitrosti q j na hitrost vrha robota bomo analizirali glede na sliko 2.2. Rotacijsko hitrost sklepa j izraženo glede na bazni koordinatni sistem lahko zapišemo kot ω j = z j 1 q j, (2.1) kjer vektor z j 1 določa os sklepa glede na bazni koordinanti sistem in q j predstavlja skalarno hitrost vrtenja sklepa. Hitrost vrha robota kot posledico sklepne hitrosti q j določimo z vektorskim produktom ṗ 6 ( q j )= ω j r j 1,6 = z j 1 q j (p 6 p j 1 )=z j 1 (p 6 p j 1 ) q j. (2.2) Če je sklep translacijski, je prispevek sklepa k celotni hitrosti gibanja vrha robota kar enak translacijski hitrosti sklepa ṗ 6 ( q j )=z j 1 q j. (2.3) Rotacijska hitrost vrha robota, kot posledica sklepne hitrosti q j, pa je kar enaka kotni hitrosti v sklepu, izraženi glede na bazni koordniatni sistem. Torej ω 6 ( q j )= ω j = z j 1 q j. (2.4)

12 6 ROBOTIKA I z 6 z 5 x 6 d 6 x 4,x 5 y 4 d 4 z 0 z 3 x 0,x 1 a 2 x 2,x 3 y 1 y 2 Slika 2.1. Robot Staubli v referenčni legi in postavitev koordinatnih sistemov posameznih segmentov glede na Denavit-Hartenbergovo notacijo Translacijski sklep ne spreminja orientacije vrha robota in je zato rotacijska hitrost vrha kot posledica delovanja translacijskega sklepa enaka nič ω 6 ( q j )=0. (2.5) 3. Translacijska Jacobijeva podmatrika Izraza (2.2) in (2.5) lahko s pridom uporabimo za izračun geometrijske Jacobijeve matrike robota, ki jo bomo razdelili na translacijski in rotacijski del J 6 6 = [ ] J 3 6 P J 3 6. (2.6) O Posamezen stolpec v Jacobijevi matriki določa kako posamezna sklepna hitrost vpliva na hitrost premikanja vrha robota. Najprej določimo Jacobijevo podmatriko za izračun translacijskih hitrosti vrha robota J P = [ j P1... j Pj... j P6 ] 3 6. (2.7)

13 Geometrijska Jacobijeva matrika 7 ω 6 ṗ 6 y 6 z 6 z j 1 ω j r j 1,6 x 6 z 0 q j p 6 p j 1 y 0 x 0 Slika 2.2. Vpliv hitrosti v sklepu j na hitrost gibanja vrha robota. Za konkretne razmere prikazane na sliki velja j = 2. Stolpec Jacobijeve matrike j Pj opisuje kako hitrost v sklepu j vpliva na translacijsko hitrost vrha robota. Torej lahko zapišemo z j 1 za translacijski sklep j Pj =, (2.8) z j 1 (p 6 p j 1 ) za rotacijski sklep kar izhaja iz enačb (2.2) in (2.3). Vektor z j 1 je določen kot tretji stolpec rotacijske matrike R j 1,torej z j 1 = R 1 (q 1 )...R j 1 (q j 1 )z 0, (2.9) kjer je z 0 = [ ] T.Vektorp6 določa položaj vrha robota glede na bazni koordinatni sistem in je torej določen s prvimi tremi elementi četrtega stolpca matrike T 6 = A 1 (q 1 )...A 6 (q 6 ). (2.10) Vektor p j 1 je določen s prvimi tremi elementi četrtega stolpca transformacijske matrike T j 1 = A 1 (q 1 )...A j 1 (q j 1 ); p 0 =

14 8 ROBOTIKA I 4. Rotacijska Jacobijeva podmatrika J O je Jacobijeva podmatrika za izračun rotacijskih hitrosti vrha robota izraženih glede na bazni koordinatni sistem J O = [ j O1... j O j... j O6 ] 3 6, (2.11) kjer je 0 za translacijski sklep j O j = za rotacijski sklep. z j 1 (2.12) Pri tem smo upoštevali, da translacijski sklep ne spreminja orientacije vrha robota in je zato rotacijska hitrost vrha kot posledica delovanja translacijskega sklepa enaka nič. Rotacijski sklep pa povzroča rotacijo vrha robota, kot je določeno v enačbi (2.5). Vektor z j 1 je določen v enačbi (2.9). 5. Naloga V Matlabu napišite funkcije za izvedbo sledečih nalog: - function A = hdh(ϑ, d, a, α), ki na podlagi Denavit-Hartenbergovih parametrov ϑ, d, a in α izračuna homogeno transformacijsko matriko A; - function A = dirkina(q), ki za vrednosti kotov v sklepih robota zapisanih v vektorju q, izračuna listo matrik A, pri tem uporabite za izračun ene matrike A funkcijo hdh(ϑ, d, a, α); - function T6 = dirkint6(a), ki iz liste matrik A izračuna lego vrha robota zapisano z matriko T6 - function J = jacobi0(q); z uporabo prej pripravljenih zgoraj omenjenih funkcij napišite funkcijo za izračun geometrijske Jacobijeve matrike; vhodni parameter je vektor spremenljivk kotov sklepov robota q, izhod pa geometrijska Jacobijeva matrika robota J. V nadaljevanju so podane predloge vseh štirih zgoraj omenjenih funkcij. Vaša naloga je, da pravilno dopolnite funkcije na mestih, kjer je označeno s komentarjem %%% STUDENT %%%.

15 Geometrijska Jacobijeva matrika 9 Predloga funkcije function A = hdh(ϑ, d, a, α) function A = hdh(th, d, a, al) % A = hdh(th, d, a, al) % th, d, a, al - Denavit-Hartenbergovi parametri robota (vhod) - po vrsti: % Theta(rotz), d(transz), a(transx), Alpha(rotx) % A - homogena transformacijska matrika (izhod) ct = cos(th); % kosinus kota Theta st = sin(th); % sinus kota Theta % matrika rotacije okrog osi z rz = []; %%% STUDENT %%% % matrika translacije vzdolz osi z in x trans = []; %%% STUDENT %%% cal = cos(al); % kosinus kota Alpha sal = sin(al); % sinus kota Alpha % matrika rotacije okrog osi x rx = []; %%% STUDENT %%% % izracun homogene transformacijske matrike A = ; %%% STUDENT %%% Testni rezultat: hdh(1,2,3,4) = Predloga funkcije function A = dirkina(q) function A = dirkina(q) % A = dirkina(q) % q - vektor kotnih spremenljivk za 6 sklepov (vhod) % A(:,:,1)... A(:,:,6) - lista matrik A (izhod) % paramteri robota a2=0.45; d4=0.45; d6=0.085; A = zeros(4,4,6); A(:,:,1) = hdh(); %%% STUDENT %%% A(:,:,2) = hdh(); %%% STUDENT %%% A(:,:,3) = hdh(); %%% STUDENT %%% A(:,:,4) = hdh(); %%% STUDENT %%% A(:,:,5) = hdh(); %%% STUDENT %%% A(:,:,6) = hdh(); %%% STUDENT %%%

16 10 ROBOTIKA I Predloga funkcije function T6 = dirkint6(a) function T6 = dirkint6(a) % T6 = dirkint6(a) % A(:,:,1)... A(:,:,6) - lista matrik A (vhod) % T6 - matrika lege vrha robota (izhod) T6 = ; %%% STUDENT %%% Testni rezultat: dirkint6(dirkina([1,2,3,4,5,6])) =

17 Geometrijska Jacobijeva matrika 11 Predloga funkcije function J = jacobi0(q) function J = jacobi0(q) % J = jacobi0(q) % q - vrednost kotov v sklepih robota (vhod) % J - geometrijska Jacobijeva matrika (izhod) % lista transformacijskih matrik A A = ; %%% STUDENT %%% % transformacijska matrika lege vrha T6 = ; %%% STUDENT %%% % inicializacija prazne Jacobijeve matrike Jp = zeros(3,6); % pozicijska podmatrika Jo = zeros(3,6); % orientacijska podmatrika % inicializacija spremenljivk z_0 = [0,0,1] ; % vektor v smeri osi z_0 p_0 = [0,0,0] ; % vektor polozaja prvega sklepa % glede na referencni koordinanti sistem T_jmin1 = eye(4); % zacetna vrednost matrike T_jmin1 for i = 1:6, if (i==1) % za prvi sklep izracunamo posebej % vektor, ki doloca smer osi sklepa glede na ref. k.s. z_jmin1 = ; %%% STUDENT %%% % vektor, ki doloca polozaj sklepa glede na ref. k.s. p_jmin1 = ; %%% STUDENT %%% else % za vse ostale sklepe % matrika, ki doloca lego segmenta glede na ref. k.s. T_jmin1 = ; %%% STUDENT %%% % vektor, ki doloca smer osi sklepa glede na ref. k.s. z_jmin1 = ; %%% STUDENT %%% % vektor, ki doloca polozaj sklepa glede na ref. k.s. p_jmin1 = ; %%% STUDENT %%% end % izracun i-tega stolpca pozicijske Jacobijeve podmatrike Jp(:,i) = ; %%% STUDENT %%% % izracun i-tega stolpca rotacijske Jacobijeve podmatrike Jo(:,i) = ; %%% STUDENT %%% end % celotna Jacobijeva matrika je sestavljena iz obeh podmatrik J = [Jp; Jo]; Testni rezultat: jacobi0([1,2,3,4,5, 6]) =

18 12 ROBOTIKA I V Simulinku izvedite preprosto simulacijo, kjer kot vhod definirate želeno hitrost vrha v zunanjih koordinatah ẋ = [ ] T,prekoinverzne Jacobijeve matrike izračunate hitrost v notranjih koordinatah, to hitrost integrirate in izračunane položaje v sklepih robota uporabite za vodenje robota Staubli v navideznem in realnem okolju. Slika 2.3. Simulacijska shema v Simulinku.

19 ANALITIČNA JACOBIJEVA MATRIKA Rotacijske matrike podajajo redundanten opis orientacije. Orientacija objekta v prostoru je določena s tremi parametri, medtem ko jih rotacijske matrike vsebujejo devet. Minimalno predstavitev orientacije s tremi parametri lahko izvedemo ob uporabi treh kotov φ = [ ϕ ϑ ψ ]. Predpostavimo rotacijsko matriko, ki opisuje rotacijo okrog ene osi kot funkcijo enega izmed kotov. Splošno rotacijo okrog treh osi torej dobimo s kombinacijo treh zaporednih rotacij, pri čemer moramo biti pozorni na to, da dveh zaporednih rotacij ne izvedemo okrog dveh paralelnih osi. Za to, da opišemo orientacijo objekta v prostoru lahko uporabimo 12 različnih kombinacij treh osnovnih rotacij okrog posameznih osi (na primer kombinacija ZYZ pomeni, da najprej izvedemo rotacijo okrog osi z, nato okrog osi y že zasukanega koordinatnega sistema in na koncu še okrog osi z predhodno dvakrat zasukanega koordinatnega sistema; razmere so prikazane na sliki 3.1). Vsako takšno zaporedje rotacij predstavlja trojček Eulerjevih kotov. z z z z ϑ z z y y y y y y x ϕ x x x ψ x x Slika 3.1. Predstavitev orientacije z ZYZ Eulerjevimi koti

20 14 ROBOTIKA I 1. ZYZEulerjevikoti Rotacija ZYZ je definirana z zaporedjem naslednjih osnovnih rotacij (slika 3.1): - Rotacija referenčnega koordinatnega sistema za kot ϕ okrog osi z. Rotacijo opišemo z rotacijsko matriko R z (ϕ). - Rotacija trenutnega koordinatnega sistema za kot ϑ okrog osi y. Rotacijo opišemo z rotacijsko matriko R y (ϑ). - Rotacija trenutnega koordinatnega sistema za kot ψ okrog osi z. Rotacijo opišemo z rotacijsko matriko R z (ψ). Rezultirajočo celotno rotacijo dobimo z zaporedjem rotacij okrog trenutnega koordinatnega sistema, torej pri množenju matrik uporabimo postmultiplikacijo R(φ)=R z (ϕ)r y (ϑ)r z (ψ) = 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 ϑ (3.1) = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 S primerjavo elementov matrik v enačbi (3.1) lahko izračunamo tri Eulerjeve kote. Ob predpostavki, da r 13 0inr 23 0 velja ϕ = atan2(r 23,r 13 ), (3.2) kjer je atan2 štiri-kvadrantna arkus tangens funkcija. S kvadriranjem in seštevanjem elementov r 13 in r 23 ter upoštevaje r 33 dobimo ϑ = atan2( r r2 23,r 33). (3.3) Izbira pozitivnega predznaka za izraz r r2 23 omejuje vrednosti kota ϑ na (0, π). Kotψ je mogoče določiti na osnovi enačbe ψ = atan2(r 32, r 31 ). (3.4) Če povzamemo, trojček ZYZ Eulerjevih kotov lahko iz splošne rotacijske matrike določimo na podlagi sledečih enačb ϕ = atan2(r 23,r 13 ) ϑ = atan2( r r2 23,r 33) (3.5) ψ = atan2(r 32, r 31 ).

21 Analitična Jacobijeva matrika 15 Lego objekta v prostoru lahko tako v minimalni obliki zapišemo s šestimi parametri, kjer prvi trije določajo pozicijo in tvorijo vektor p in drugi trije določajo orientacijo in tvorijo vektor φ [ p x =. (3.6) φ] 2. Analitična Jacobijeva matrika V poglavju 2 smo izračunali geometrijsko Jacobijevo matriko pri čemer smo upoštevali geometrijske relacije med hitrostmi v sklepih in hitrostjo vrha robota glede na referenčni koordinatni sistem. Pri opisu lege vrha robota z minimalnim številom parametrov x pa obstaja možnost izračuna Jacobijeve matrike z odvajanjem elementov vektorja x po sklepnih spremenljivkah ẋ = x q q = J A(q) q, (3.7) torej lahko izračunamo translacijsko hitrost vrha robota kot ṗ = p q q = J P(q) q (3.8) in rotacijsko hitrost vrha robota kot φ = φ q q = J φ(q) q, (3.9) pri čemer vektor φ določa trojček Eulerjevih kotov. Upoštevaje translacijo in rotacijo lahko torej zapišemo ẋ = [ṗ φ] = [ ] JP (q) q = J J φ (q) A (q) q. (3.10) z ϕ z z y ϑ y ψ y x x z Slika 3.2. Rotacijske hitrosti ZYZ Eulerjevih kotov v trenutnem koordinatnem sistemu.

22 16 ROBOTIKA I z ϑ z ω z ω ϕ ψ y ϑ ω y ϕ y ω x x Slika 3.3. Razstavitev hitrosti ZYZ Eulerjevih kotov v rotacijske hitrosti v referenčnem koordinatnem sistemu. V nadaljevanju bomo poiskali transformacijsko matriko, ki povezuje geometrijsko Jacobijevo matriko z analitično Jacobijevo matriko določeno na osnovi ZYZ Eulerjevih kotov. V ta namen je potrebno poiskati relacije med rotacijskimi hitrostmi ω = [ ω x ω y ω z ] T in hitrostmi ZYZ Eulerjevih kotov φ. Na sliki 3.2 so hitrosti ϕ, ϑ in ψ predstavljene glede na osi trenutnega koordinatnega sistema. Slika 3.3 prikazuje postopek izračuna doprinosa posamezne hitrosti Eulerjevega kota k hitrostim izraženim glede na osi referenčnega koordinatnega sistema: - kot rezultat hitrosti ϕ: [ ω x ω y ω z ] T = ϕ [ ] T ; pri tem smo upoštevali, da je na začetku os z trenutnega koordinatnega sistema poravnana z osjo z referenčnega koordinatnega sistema, - kot rezultat hitrosti ϑ: [ ] T ω x ω y ω z = ϑ [ s ϕ c ϕ 0 ] T ;pritemsmo upoštevali, da je med referenčnim in trenutnim koordinatnim sistemom zasuk ϕ okrog osi z referenčnega koordinatnega sistema, [ ] T [ ] T - kot rezultat hitrosti ψ: ωx ω y ω z = ψ cϕ s ϑ s ϕ s ϑ c ϑ ;pritem smo upoštevali, da je med referenčnim in trenutnim koordinatnim sistemom

23 Analitična Jacobijeva matrika 17 zasuk ϕ okrog osi z referenčnega koordinatnega sistema in ϑ zasuk okrog osi y trenutnega koordinatnega sistema. Vplive vseh treh hitrosti Eulerjevih kotov na kotne hitrosti okrog referenčnega koordinatnega sistema dobimo, če zgornje izraze združimo v matrično obliko ω = 0 s ϕ c ϕ s ϑ 0 c ϕ s ϕ s ϑ φ = T(φ) φ. (3.11) 1 0 c ϑ Preostane nam še določitev transformacijske matrike za transformacijo med geometrijsko in analitično Jacobijevo matriko. Pri tem bomo upoštevali, da se translacijske hitrosti vrha robota ne spremenijo, če uporabimo različne zapise za orientacijo vrha. To pomeni, da je podmatrika Jacobijeve matrike, ki se nanaša na translacijske hitrosti enaka, neglede na izbiro analitične ali geometrijske Jacobijeve matrike. Transformacija rotacijskih hitrosti pa je določena z matriko T(φ). Celotno transformacijo lahko torej zapišemo sledeče J(q)= [ ] [ ][ ] JP (q) I 0 JP (q) = J O (q) 0 T(φ) J φ (q) = T A (φ)j A (q) (3.12) kjer je I enotska matrike dimenzij 3 3 in 0 matrika ničel dimenzij 3 3. Velja tudi obratno J A (q)=t 1 A (φ)j(q) (3.13) 3. Naloga V Matlabu napišite funkcije za izvedbo sledečih nalog: - x = q2zyzeul(q), ki za vrednosti kotov v sklepih robota zapisanih v vektorju q, izračuna lego vrha robota v smislu položaja XYZ in orientacije predstavljene z ZYZ Eulerjevimi koti; položaj in Eulerjevi koti tvorijo vektor lege vrha robota; pri tem uporabite funkcijo za irzačun direktne kinematike robota; - function invj = ijaczyzeul(q), ki za vrednosti kotov v sklepih robota zapisanih v vektorju q, izračuna inverzno analitično Jacobijevo matriko; pri tem uporabite funkcije za izračun geometrijske Jacobijeve matrike in lege robota predstavljene s položajem in Eulerjevimi koti. V nadaljevanju sta podani predlogi obeh zgoraj omenjenih funkcij. Vaša naloga je, da pravilno dopolnite funkcije na mestih, kjer je označeno s komentarjem %%% STUDENT %%%.

24 18 ROBOTIKA I Predloga funkcije x = q2zyzeul(q) function x = q2zyzeul(q) % x = q2eulvec(q) % q - vektor kotnih spremenljivk robota (vhod) % x - lega vrha robota v smislu polozaja XYZ in orientacije predstavljene z % ZYZ Eulerjevimi koti % matrika lege vrha robota T6 = ; %%% STUDENT %%% % orientacija vrha predstavljena z vektorjem treh ZYZ Eulerjevih kotov ea = []; %%% STUDENT %%% % lega vrha robota v smislu polozaja XYZ in orientacije predstavljene z % ZYZ Eulerjevimi koti x = []; %%% STUDENT %%% Testni rezultat: q2zyzeul([1,2,3,4,5,6]) = [ ] T Predloga funkcije function invj = ijaczyzeul(q) function invj = ijaczyzeul(q) % invj = ijaczyzeul(q) % q - vrednost kotov v sklepih robota (vhod) % invj - inverzna analiticna Jacobijeva matrika, kjer je orientacija % predstavljena z ZYZ Eulerjevimi koti % izracun geometrijske Jacobijeve matrike J0 = ; %%% STUDENT %%% % izracun lege vrha robota z ZYZ Eulerjevimi koti za opis orientacije X = ; %%% STUDENT %%% % izracun transformacijske matrike za pretvorbo iz geometrijske v % analiticno Jacobijevo matriko Tr = []; %%% STUDENT %%% % izracun inverzne analiticne Jacobijeve matrike invj = ; %%% STUDENT %%% Testni rezultat: i jaczyzeul([1,2,3,4, 5, 6]) =

25 INVERZNA KINEMATIKA Z UPORABO JACOBIJEVE MATRIKE 1. Izvedba izračuna inverzne kinematike Jacobijevo matriko robota moremo uporabiti za izračun hitrosti vrha robota ẋ (koordinate vrha robota so predstavljene z minimalnim številom parametrov položaj in orientacija z Eulerjevimi koti), če poznamo vektor sklepnih hitrosti q ẋ = J q. (4.1) Seveda lahko v uporabimo tudi obratno relacijo, če Jacobijeva matrika ni singularna q = J 1 ẋ. (4.2) Za izračun inverzne kinematike robota na osnovi inverzne Jacobijeve matrike bomo kot izhodišče uporabili preprosto Eulerjevo integracijsko metodo. Vrednosti kotov sklepov q(t k+1 ) v trenutku t k+1 lahko določimo na osnovi vrednosti kotov sklepov q(t k ) v trenutku t k in hitrosti gibanja robota q(t k ) q(t k+1 )=q(t k )+ q(t k )Δt, (4.3) pri čemer je Δt računski korak regulatorja robota. Upoštevaje enačbo (4.2) lahko zgornjo enačbo zapišemo kot q(t k+1 )=q(t k )+J 1 (q(t k ))ẋ(t k )Δt. (4.4) Na osnovi enačbe (4.4) bi bilo mogoče izračunavati inverzno kinematiko robota ob poznavanju začetne lege in hitrosti gibanja vrha robota. Vendar, ker numerična integracija vnaša lezenje, je takšen rezultat neuporaben. Rešitev problema lezenja je mogoče najti v vpeljavi povratne zanke, ki odpravlja lezenje s primerjavo prave lege vrha robota x d z lego vrha robota x izračunano na

26 20 ROBOTIKA I osnovi kotov določenih v enačbi (4.4). Najprej definirajmo napako izračunane lege vrha robota e = x d x = x d k(q), (4.5) pri čemer k(q) označuje direktni geometrijski model robota. Enačbo (4.5) odvajamo po času in dobimo ė = ẋ d ẋ = ẋ d J A (q) q. (4.6) Ker x predstavlja zapis lege vrha robota z minimalnim številom parametrov (položaj in Eulerjevi koti) smo v enačbi uporabili analitično in ne geometrijske Jacobijeve matrike. Potrebno je še poiskati takšno relacijo med q in e, ki zagotavlja konvergenco napake proti nič. Ena od možnosti je izbira sledečega regulacijskega algoritma Z vstavitvijo enačbe (4.7) v enačbo (4.6) dobimo q = J 1 A (q)(ẋ d + Ke). (4.7) ė + Ke = 0. (4.8) Če je matrika ojačenj regulatorja K pozitivno-definitna (običajno diagonalna), je sistem (4.8) asimptotično stabilen. Izvedba izračuna inverzne kinematike robota na osnovi inverzne Jacobijeve matrike je prikazana na sliki 4.1. ẋ d x d + + x e K J 1 A q q k( ) Slika 4.1. Inverzna kinematika z uporabo inverzne Jacobijeve matrike. Na shemi predstavlja K diagonalno matriko ojačenj, J A je analitična Jacobijeva matrika, integrator, k( ) izračun direktne kinematike, x d lega vrha robota, ki jo želimo pretvoriti v notranje koordinate, ẋ d želene hitrost vrha robota in x ocenjena lega vrha na podlagi izračunanih vrednosti kotov. Vse lege vrha robota so izražene z minimalnim številom parametrov (položaj in orientacija z Eulerjevimi koti). 2. Naloga Na osnovi slike 4.1 zgradite v Simulinku simulacijsko shemo za izračun inverzne kinematike na osnovi inverzne Jacobijeve matrike, kot je prikazano

27 Inverzna kinematika z uporabo Jacobijeve matrike 21 na sliki 6.4. Delovanje sistema preizkusite s testnimi podatki, ki omogočajo gibanje robota po krivulji v ravnini XZ, kot je prikazano na sliki 4.3. Testni podatki so shranjeni v datoteki xtrajec.mat. Slika 4.2. Simulacijska shema v Simulinku. 0.5 Trajektorija v XZ ravnini z (m) x (m) Slika 4.3. Preizkusna trajektorija za testiranje izračuna inverzne kinematike robota.

28

29 TRANSFORMACIJE SIL IN NAVOROV Za analizo transformacij sil in navorov bomo uporabili koncept kineto-statične dualnosti. 1. Transformacije hitrosti ω 1 z 1 ṗ 1 y 1 O 1 x 1 r 12 ω 2 y 2 z 2 p 1 z 0 O 2 x 2 ṗ 2 p 2 O 0 x 0 y 0 Slika 5.1. Predstavitev linearnih in kotnih hitrosti v različnih koordinatnih sistemih na istem togem telesu. Predpostavimo referenčni koordinatni sistem O 0 x 0 y 0 z 0 in togo telo, ki se premika glede na ta koordinatni sistem. Naj bosta O 1 x 1 y 1 z 1 in O 2 x 2 y 2 z 2 koordinatna sistema pritrjena na telo (slika 5.1). Povezava med translacijskima

30 24 ROBOTIKA I in rotacijskima hitrostma obeh koordinatnih sistemov je določena z ṗ 2 = ṗ 1 + ω 1 r 12 (5.1) ω 2 = ω 1, pri čemer so vsi vektorji izraženi glede na referenčni koordinatni sistem O 0 x 0 y 0 z 0. Vektorski produkt, ki nastopa v enačbi (5.1) bomo zapisali kot produkt poševno-simetrične matrike S(r 12 ) in vektorja ω 1,torej ω 1 r 12 = r 12 ω 1 = S(r 12 )ω 1, (5.2) kjer je poševno-simetrična matrika S(r 12 )= 0 r z r y r z 0 r x. (5.3) r y r x 0 Za poševno-simetrično matriko lahko zapišemo sledeči lastnosti S(R 1 r 1 12 )=R 1S(r 1 12 )RT 1 (5.4) in S + S T = 0. (5.5) Z uporabo poševno-simetrične matrike sedaj enačbi (5.1) zapišemo kot ṗ 2 = ṗ 1 S(r 12 )ω 1 ω 2 = ω 1, (5.6) oziroma v matrični obliki ] [ṗ2 = ω 2 [ ] I S(r12 ) ][ṗ1, (5.7) 0 I ω 1 kjer je I enotska matrika ustreznih dimenzij. Vektorji v enačbi (5.7) so izraženi glede na referenčni koordinatni sistem O 0 x 0 y 0 z 0. Če pa so vektorji izraženi v lastnih koordinatnih sistemih, je potrebno upoštevati ustrezne transformacije. Vektor r 1 12 izražen v koordinatnem sistemu O 1 x 1 y 1 z 1 lahko zapišemo v osnovnem koordinatnem sistemu O 0 x 0 y 0 z 0 kot r 12 = R 1 r 1 12, (5.8) kjer matrika R 1 določa orientacijo koordinatnega sistema O 1 x 1 y 1 z 1 glede na O 0 x 0 y 0 z 0. Podobno lahko transformiramo tudi vektorja ṗ 1 1 in ω1 1,kista izražena v koordinatnem sistemu O 1 x 1 y 1 z 1 ṗ 1 = R 1 ṗ 1 1 ω 1 = R 1 ω 1 1. (5.9)

31 Transformacije sil in navorov 25 Vektorja ṗ 2 2 in ω2 2 sta izražena v koordinatnem sistemu O 2 x 2 y 2 z 2, zato je transformacija v osnovni koordinatni sistem sledeča ṗ 2 = R 2 ṗ 2 2 = R 1 R 1 2ṗ2 2 ω 2 = R 2 ω 2 2 = R 1R 1 2 ω2 2, (5.10) kjer matrika R 2 določa orientacijo koordinatnega sistema O 2 x 2 y 2 z 2 glede na O 0 x 0 y 0 z 0 in matrika R 1 2 orientacijo koordinatnega sistema O 2 x 2 y 2 z 2 glede na O 1 x 1 y 1 z 1. Upoštevaje zapise (5.8), (5.9) in (5.10) ter lastnost poševno-simetrične matrike (5.4) v enačbah (5.6) dobimo R 1 R 1 2ṗ2 2 = R 1 ṗ 1 1 R 1 S(r 1 12)R T 1 R 1 ω 1 1 R 1 R 1 2 ω2 2 = R 1ω 1 1. (5.11) Enačbi pomnožimo na obeh straneh z R 1 1 in R = R 2 1 ter rezultat zapišemo vmatrični obliki [ṗ2 ] [ 2 R 2 ω 2 = 1 R 2 1 S(r1 12 ) ][ṗ1 ] R 2 1 ω 1. (5.12) 1 Enačba (5.12) predstavlja splošno transformacijo hitrosti iz začetnega v nov koordinatni sistem. Transformacijska matrika v enačbi (5.12) ima vlogo Jacobijeve matrike, saj predstavlja transformacijo hitrosti, zato lahko enačbo zapišemo tudi v obliki v 2 2 = J 2 1v 1 1, (5.13) kjer je v 1 1 = [ ṗ 1 1 ω 1 T 1], v 2 2 = [ ṗ 2 2 ω 2 T 2] in [ J 2 R 1 = 2 1 R 2 1 S(r1 12 ) ] 0 R 2. (5.14) 1 2. Transformacije sil in navorov Spomnimo se kineto-statične dualnosti ẋ = J q τ = J T F. (5.15) Enačbi (5.15) določata razmerje med hitrostmi izraženimi v sklepnem koordinatnem sistemu ter hitrostmi vrha manipulatorja oziroma silami na vrhu manipulatorja in navori v sklepih. Torej gre v obeh enačbah za transformacije iz sklepnega v koordinatni sistem vrha oziroma obratno. Relacija, ki določa transformacijo, je Jacobijeva oziroma transponirana Jacobijeva matrika. Dualnost predstavljeno z enačbama (5.15) bomo uporabili za določitev transformacij sil in navorov. V ta namen je potrebno poiskati transformacijo dualno transformaciji (5.13). Glede na dualnost (5.15) lahko zapišemo γ 1 1 = J2 1T γ 2 2, (5.16)

32 26 ROBOTIKA I kjer vektor γ 1 1 = [ f 1 1 μ 1 T 1] predstavlja sile in navore izražene v koordinatnem sistemu O 1 x 1 y 1 z 1 in vektor γ 2 2 = [ f 2 2 μ 2 T 2] sile in navore izražene v koordinatnem sistemu O 2 x 2 y 2 z 2.MatrikaJ 2 1T je enaka [ J 2 T 1 = R 2 T 1 0 ( R 2 1 S(r1 12 ))T R 2 1 T ]. (5.17) Upoštevaje lastnost poševno-simetrične matrike (5.5) lahko transformacijo ( R 2 1 S(r 1 12 )) T zapišemo kot ( R 2 1 S(r 1 12 )) T = S T (r 1 12 )R1 2 = S(r1 12 )R1 2. (5.18) S tem dobimo končno enačbo, ki omogoča transformacijo sil in navorov med dvema koordinatnima sistemoma [ f 1 1 μ 1 1 ] [ R = S(r 1 12 )R1 2 R 1 2 ][ f 2 2 μ 2 2 ]. (5.19) 3. Naloga Na konec orodja pritrjenega na robot Staeubli je obešena masa kot prikazuje slika 5.2. Silo, ki jo povzroča obešena masa merimo s senzorjem sile pritrjenim na vrh robota. Ker senzor sile podaja izmerjeno silo v lastnem koordinatnem sistemu, nas zanima kolikšna sta dejanska sila in navor izražena v koordinatnem sistemu pritrjenem na ročko oziroma v osnovnem koordinatnem sistemu robota. Nalogo izvedite v Matlabu. Robota premikajte z ukazom q = move staubli(qd),kjer qd predstavlja vektor želenih vrednosti kotov manipulatorja in q vektor dejanskih doseženih kotov. Vektor izmerjenih vrednosti kotov lahko uporabite v funkciji za izračun direktne kinematike robota. Sile, ki jih izmeri senzor odčitajte z ukazom [w, ft, q] = read staubli, kjer w določa položaj vrha robota, ft izmerjene sile in navore in q trenutne kote v sklepih robota. Transformacija med koordinatnim sistemom vrha robota in koordinatnim sistemom senzorja sile je določena z T 6 f = (5.20)

33 Transformacije sil in navorov 27 ročka senzor vrh m z 0 z e z f z 6 x 0 x f x 6 x e Postavitev koordinatnih sistemov baze, vrha, senzorja sile ter orodja na robot Stae- Slika 5.2. ubli. in transformacija med koordinatnim sistemom senzorja sile ter koordinatnim sistemom orodja (ročke) z T fe = (5.21) V nadaljevanju sta podani predlogi funkcij za preračun sil in navorov v koordinatni sistem ročke oziroma koordinatni sistem baze robota. Vaša naloga je, da pravilno dopolnite funkcije na mestih, kjer je označeno s komentarjem %%% STUDENT %%%.

34 28 ROBOTIKA I Predloga funkcije fte = ft2endeffector(q, ft) function fte = ft2endeffector(q, ft) % fte = ft2endeffector(q, ft) % Funkcija preracuna sile in navore, ki jih izmeri senzor v koordinatni % sistem, ki je pritrjen na vrhu robota v rocki % Vhodna vektorja sta polozaji v sklepih q in sile ter navori, ki jih % izmeri senzor ft % Izhod funkcije je vektor sil in navorov izrazen v koordinatnem sistemu % rocke fte %transformacija od k.s. vrha robota do k.s. senzorja t6f = [eye(3),[0; 0; 0.035]; [ ]]; %transformacija od k.s. senzorja do k.s. rocke tfe = [eye(3),[0; 0; 0.14]; [ ]]; % Izracunajte transformacijsko matriko T za transformacijo sil in navorov med % koordinatnim sistemom senzorja in koordinatnim sistemom rocke T = zeros(6,6); %%% STUDENT %%% fte = ; %%% STUDENT %%% Testni rezultat: ft2ende f f ector([1,2,3,4,5,6],[1,2,3,4,5,6] )= [ ] T Predloga funkcije ftb = ft2base(q, ft) function ftb = ft2base(q, ft) % fte = ft2endeffector(q, ft) % Funkcija preracuna sile in navore, ki jih izmeri senzor v koordinatni % sistem, ki je pritrjen v bazi robota % Vhodna vektorja sta polozaji v sklepih q in sile ter navori, ki jih % izmeri senzor ft % Izhod funkcije je vektor sil in navorov izrazen v koordinatnem sistemu % baze robota ftb %transformacija od k.s. vrha robota do k.s. senzorja t6f = [eye(3),[0; 0; 0.035]; [ ]]; %transformacija od k.s. senzorja do k.s. rocke tfe = [eye(3),[0; 0; 0.14]; [ ]]; % Izracunajte transformacijsko matriko T za transformacijo sil in navorov med % koordinatnim sistemom senzorja in koordinatnim sistemom baze robota T = zeros(6,6); %%% STUDENT %%% ftb = ; %%% STUDENT %%% Testni rezultat: ft2base([1,2,3,4,5,6],[1,2,3,4,5,6] )= [ ] T

35 NAČRTOVANJE TRAJEKTORIJE 1. Interpolacija V nekaterih nalogah potrebujemo kompleksnejše robotske gibe kot le gibanje od točke do točke. Pri varjenju je, na primer, potrebno slediti ukrivljenim krivuljam površine objektov. Takšne trajektorije gibanja lahko dosežemo, če poleg začetne in končne točke predpišemo še vmesne točke, skozi katere se mora robot gibati. V nadaljevanju bomo analizirali problem, kjer želimo interpolirati trajektorijo skozi n vmesnih točk {q 1,...,q n }, ki jih mora robot doseči v časovnih trenutkih {t 1,...,t n }. Interpolacijo bomo izvedli z uporabo trapeznega hitrostnega profila. Celotna trajektorija bo sestavljena iz zaporedja linearnih segmentov za opis gibanja med dvema vmesnima točkama ter parabol za opis prehodov skozi vmesne točke. V izogib nezveznosti prvega odvoda v trenutku t k,moranamreč funkcija q(t) imeti parabolični profil v okolici q k.pritemje drugi odvod v točki q k (pospešek) še vedno nezvezna funkcija. Interpolirana trajektorija, podana kot zaporedje linearnih polinomov s paraboličnimi prehodi skozi vmesne točke (čas prehoda Δt k ), je analitično opisana s sledečimi pogoji q(t)= { a 1,k (t t k )+a 0,k b 2,k (t t k ) 2 + b 1,k (t t k )+b 0,k t k + Δt k 2 t < t k+1 Δt k+1 2 t k Δt k 2 t < t k + Δt k 2 (6.1) Koeficienta a 0,k in a 1,k določata potek trajektorije v linearnem delu, pri čemer indeks k predstavlja zaporedno številko linearnega segmenta. Koeficienti b 0,k, b 1,k in b 2,k določajo potek trajektorije v paraboličnih prehodih. Indeks k vtem primeru določa zaporedno številko parabole. Najprej izračunajmo hitrosti gibanja v linearnih segmentih iz podanih točk in časov gibanja med točkami, kot so določeni na sliki 6.1. Za začetno in.

36 30 ROBOTIKA I končno hitrost predpostavimo, da sta enaki nič, torej velja 0 k = 1 q q k 1,k = k q k 1 t k t k 1 k = 2,...,n 0 k = n + 1. (6.2) V nadaljevanju določimo koeficienta linearnih funkcij a 0,k in a 1,k.Koeficient a 0,k določimo iz linearne funkcije v pogojih (6.1) z upoštevanjem znane pozicije ob času t k, ko robot doseže vmesno točko q k q(t k )=q k = a 1,k (t k t k )+a 0,k = a 0,k, (6.3) torej t = t k a 0,k = q k k = 1,...,n 1. (6.4) Koeficient a 1,k določimo iz odvoda linearne funkcije (6.1) po času, torej q(t)=a 1,k. (6.5) Z upoštevanjem znane hitrosti v časovnem intervalu t k,k+1 dobimo a 1,k = q k,k+1 k = 1,...,n 1. (6.6) e k q 2 q n = q f q q 3 q 1 = q i Δt 2 Δt 1 Δt 2,3 t 1 = 0 t 2 t 3 t n = t f t Slika 6.1. Interpolacija trajektorije skozi n točk uporabljeni so linearni segmenti s paraboličnimi prehodi.

37 Načrtovanje trajektorije 31 S tem so linearni segmenti trajektorije določeni in lahko preidemo na določanje koeficientov kvadratičnih funkcij. Predpostavimo, da je čas prehoda vmesne točke predpisan z Δt k. Če ne predpišemo časa, določimo absolutno vrednost pospeška q k vvmesnitočki in iz pospeška ter hitrosti pred in po vmesni točki izračunamo čas prehoda točke. V tem primeru je potrebno določiti le predznak pospeška. Pri tem izhajamo iz predznaka spremembe hitrosti ob prehodu vmesne točke q k = sign( q k,k+1 q k 1,k ) q k, (6.7) kjer funkcija sign( ) določa predznak izraza v oklepaju. Z znanim pospeškom vvmesnitočki ter hitrostmi gibanja pred in po vmesni točki določimo čas gibanja skozi vmesno točko Δt k (zaviranje in pospeševanje) Δt k = q k,k+1 q k 1,k. (6.8) q k V nadaljevanju izračunajmo koeficiente kvadratičnih funkcij. Zahtevana zveznost hitrosti pri prehodu iz linearnega v parabolični del trajektorije v časovnih trenutkih (t k Δt k 2 ) in zahtevana zveznost hitrosti pri prehodu iz paraboličnega v linearni del trajektorije v časovnih trenutkih (t k + Δt k 2 ) predstavlja izhodišče za izračun koeficientov b 1,k in b 2,k. Najprej izračunamo časovni odvod kvadratične funkcije v pogojih (6.1) q(t)=2b 2,k (t t k )+b 1,k. (6.9) Upoštevamo, da je hitrost v trenutku (t k Δt k 2 ) enaka q k 1,k in hitrost v trenutku (t k + Δt k 2 ) enaka q k,k+1, torej velja ( q k 1,k = 2b 2,k t k Δt ) k 2 t k + b 1,k = b 2,k Δt k + b 1,k t = t k Δt k 2 ( q k,k+1 = 2b 2,k t k + Δt ) k 2 t k + b 1,k = b 2,k Δt k + b 1,k t = t k + Δt k 2. (6.10) Če seštejemo enačbi (6.10), moremo določiti koeficient b 1,k kot b 1,k = q k,k+1 + q k 1,k 2 k = 1,...,n, (6.11) če pa enačbi (6.10) odštejemo med sabo, moremo določiti koeficient b 2,k kot b 2,k = q k,k+1 q k 1,k 2Δt k = q k 2 k = 1,...,n. (6.12) Z upoštevanjem zveznosti pozicije v trenutku (t k + Δt k 2 ) določimo še koeficient b 0,k kvadratičnega polinoma. V trenutku (t k + Δt k 2 ) velja, da je pozicija q(t)

38 32 ROBOTIKA I izračunana iz linearne funkcije ( q t k + Δt ) k = a 1,k 2 ( t k + Δt k 2 t k enaka izračunu q(t) iz kvadratične funkcije ( q t k + Δt ) ( k = b 2,k t k + Δt k 2 2 t k = q k,k+1 q k 1,k 2Δt k ( Δtk 2 ) + a 0,k = q k,k+1 Δt k 2 + q k (6.13) ) 2 + b 1,k ( t k + Δt k 2 t k ) 2 + q k,k+1 + q k 1,k 2 ) + b 0,k Δt k 2 + b 0,k. (6.14) Z izenačitvijo enačb (6.13) in (6.14) je mogoče določiti koeficient b 0,k kot b 0,k = q k +( q k,k+1 q k 1,k ) Δt k 8. (6.15) Preveriti je mogoče, da tako izračunani koeficient b 0,k zagotavlja tudi zveznost pozicije v trenutku (t k Δt k 2 ). Takšna izbira koeficienta b 0,k onemogoča, da bi trajektorija gibanja sklepa dejansko potekala skozi točko q k. Robot se točki le bolj ali manj približa. Oddaljenost trajektorije od referenčne točke je odvisna predvsem od časa zaviranja in pospeševanja Δt k, ki pa je določen s prednastavljenim pospeškom q k. Napako trajektorije e k lahko ocenimo, če primerjamo želeno lego q k z dejansko doseženo lego q(t k ) v trenutku t k,kijo dobimo z vstavitvijo časa t k vkvadratično funkcijo v pogojih (6.1), torej e k = q k q(t k )=q k b 0,k = ( q k,k+1 q k 1,k ) Δt k 8. (6.16) Vidimo,dajenapakae k enaka nič le v primeru, ko sta hitrosti v linearnih odsekih pred in za vmesno točko enaki, ali pa je čas Δt k enak nič, oziroma pospešek neskončen, kar pa seveda fizikalno ni mogoče. Takšen pristop k interpolaciji trajektorije ima manjšo pomanjkljivost. Iz enačbe (6.16) vidimo, da robot vmesne točke ne doseže, ampak jo obide. Ker smo kot vmesni točki obravnavali tudi začetno in končno pozicijo, smo s tem vnesli napako v načrtovanje trajektorije. Na začetku trajektorije se namreč trenutna lega robota in želena lega razlikujeta za napako e 1 (slika 6.2, svetlejša krivulja prikazuje trajektorijo brez upoštevanja korekcije), ki sledi iz enačbe (6.16). Napaka predstavlja stopničast skok pozicije, ki pa v robotiki ni zaželen. Da bi se temu izognili moramo prvo in zadnjo točko trajektorije obravnavati ločeno od ostalih vmesnih točk. Zahtevani hitrosti v začetni in končni točki naj bosta enaki nič. Hitrost ob koncu časovnega intervala Δt 1 mora biti enaka hitrosti v prvem linearnem se-

39 Načrtovanje trajektorije 33 gmentu trajektorije. Najprej izračunajmo hitrost v linearnem delu q 1,2 = q 2 q 1 t 2 t Δt. (6.17) 1 Enačba (6.17) je podobna enačbi (6.2), le da smo tokrat v imenovalcu enačbe odšteli 1 2 Δt 1, ker se v tem času pozicija robota le malo spremeni (začetek parabole na sliki 6.2). S tem smo dosegli večjo hitrost v linearnem segmentu trajektorije. Ob koncu časa pospeševanja Δt 1 torej velja q 2 q 1 t 2 t Δt 1 = q 1 Δt 1 (6.18) Določiti moramo še pospešek q 1 na začetku trajektorije. Ker predpostavljamo, da smo predefinirali njegovo absolutno vrednost q 1, moramo ustrezno izbrati le še predznak, kar bomo naredili na osnovi razlike pozicij (načeloma je za določitev predznaka pospeška potrebno upoštevati razliko hitrosti, vendar vemo, e 2 q 2 q e 1 q Δt 1 Δt 2 Δt 1 Δt 1,2 t 1 = 0 t 2 t Slika 6.2. Interpolacija natančnejši prikaz prvega segmenta trajektorije, kijepredstavljena na sliki 6.1. S svetlejšo barvo je prikazana trajektorija brez korekcije, s črno barvo pa popravljena trajektorija.

40 34 ROBOTIKA I da je začetna hitrost nič, zato lahko izračun prevedemo na razliko pozicij) q 1 = sign(q 2 q 1 ) q 1, (6.19) kjer funkcija sign( ) določa predznak izraza v oklepaju. Iz enačbe (6.18) bomo izračunali Δt 1 (q 2 q 1 )= q 1 Δt 1 (t 2 t Δt 1). (6.20) Po preureditvi dobimo 1 2 q 1Δt q 1 (t 2 t 1 )Δt 1 (q 2 q 1 )=0 (6.21) in izrazimo Δt 1 = q 1(t 2 t 1 ) ± q 2 1 (t 2 t 1 ) 2 2 q 1 (q 2 q 1 ), (6.22) q 1 kar lahko poenostavimo v Δt 1 =(t 2 t 1 ) (t 2 t 1 ) 2 2(q 2 q 1 ) q 1. (6.23) Venačbi (6.23) smo izmed dveh možnosti izbrali minus predznak pred korenom, saj mora biti Δt 1 manjši od časa (t 2 t 1 ). Iz enačbe (6.17) lahko sedaj izračunamo hitrost v linearnem delu trajektorije. Kot je razvidno iz slike 6.2 (temnejša krivulja predstavlja popravljeno trajektorijo), smo z vpeljavo korekcije odpravili začetno napako pozicije. Podobno kot za prvi segment, moramo tudi za zadnji segment med točkama q n 1 in q n izračunati ustrezne korekcije. Hitrost v zadnjem linearnem segmentu znaša q n 1,n = q n q n 1 t n t n Δt. (6.24) n V imenovalcu enačbe (6.24) smo odšteli 1 2 Δt n, ker se, tik preden se robot ustavi, pozicija le malo spreminja. Ob prehodu iz zadnjega linearnega segmenta v zadnjo parabolo velja enakost hitrosti q n q n 1 t n t n Δt n = q n Δt n. (6.25) Za zadnji parabolični segment določimo pospešek (pojemek) na podlagi razlike pozicij q n = sign(q n 1 q n ) q n. (6.26)

41 Načrtovanje trajektorije 35 Zgornjo enačbo vstavimo v (6.25) in podobno, kot smo to naredili za prvi parabolični segment, tudi za zadnjega izračunamo čas trajanja Δt n =(t n t n 1 ) (t n t n 1 ) 2 2(q n q n 1 ). (6.27) q n Iz enačbe (6.24) nato določimo še hitrost v zadnjem linearnem segmentu. Upoštevaje korekcije na začetku in na koncu trajektorije, izračunamo še potek skozi vsevmesnetočke. S tem smo interpolirali celotno trajektorijo skozi n točk. 2. Naloga V Matlabu napišite funkcijo function [time, q, dq, ddq] = interpolate(t v, q v, dt) za interpolacijo trajektorije. Vhodni parametri so t v, vektor, ki določa trenutke, ko robot doseže via točko, q v vektor via točk (samo ena koordinata, saj bomo vsako koordinato interpolirali posebej) in dt, ki predstavlja vzorčni čas interpolatorja. V funkciji izračunajte koeficiente linearnih in paraboličnih segmentov trajektorije in jih sestavite v celotno pot, ki jo vrh robota opiše vzdolž ene koordinate. Izhodi funkcije so tako time, ki določa vektor časa, q, ki določa vektor interpoliranega položaja, dq, ki določa vektor interpolirane hitrosti in ddq, ki določa vektor interpoliranega pospeška. V nadaljevanju je podana predloga funkcije. Vaša naloga je, da pravilno dopolnite funkcije na mestih, kjer je označeno s komentarjem %%% STUDENT %%%. Predloga funkcije function [time, q, dq, ddq] = interpolate(t v, q v, dt) function [time, q, dq, ddq] = interpolate(t_v, q_v, dt) % t_v - casi, ko robot doseze via tocko % q_v - vektor via tock % t - vektor casovnih trenutkov via tock % D_t - cas preleta via tock % dq_i in dq_f - zacetna in koncna hitrost % Ts - vzrocni cas interpolatorja % T, q, dq, ddq - vektorji casa, polozaja, hitrosti in pospeska za % celotno trajektorijo % zacetna in koncna hitrost dq_i = 0; dq_f = 0; % stevilo via tock N = length(t_v); % casovni intervali S_t = diff(t_v); % linearne hitrosti dq_l = ; %%% STUDENT %%% dq_l = [dq_i dq_l dq_f]; % dodamo zacetno in koncno hitrost

42 36 ROBOTIKA I % cas trajanja parabolicnih segmentov D_t = 0.5*ones(1,17); % cas prehodov via tock je konstanten in znasa 0.5s % za vse via tocke % koeficienti linearnih segmentov a_0 = ; %%% STUDENT %%% a_1 = ; %%% STUDENT %%% % koeficienti parabolicnih spojev b_2 = ; %%% STUDENT %%% b_1 = ; %%% STUDENT %%% b_0 = ; %%% STUDENT %%% % izracun trajektorije; inicializacija vektorjev casa, polozaja, hitrosti % in pospeska time =[]; q = []; dq = []; ddq = []; % izracun trajektorije po posameznih intervalih med via tockami for k=1:n-1, % parabolicni segment if (k == 1), % prvi parabolicni segment; dolocitev vektorja casa t_p = ; %%% STUDENT %%% else %vmesni parabolicni segmenti; dolocitev vektorja casa t_p = ; %%% STUDENT %%% end % izracun polozaja, hitrosti in pospeska v parabolicnih segmentih q_p = ; %%% STUDENT %%% dq_p = ; %%% STUDENT %%% ddq_p = ; %%% STUDENT %%% % linearni segment % dolocitev vektorja casa za poljubni linearni segment t_l = ; %%% STUDENT %%% % izracun polozaja, hitrosti in pospeska v linearnih segmentih q_l = ; %%% STUDENT %%% dq_l = ; %%% STUDENT %%% ddq_l = ; %%% STUDENT %%% % vektorje casa, polozaja, hitrosti in pospeska zdruzimo v en vektor za % celotno trajektorijo s tem, da na konec prejsnjega vektorja dodamo nov % izracun time = [time; t_p ; t_l ]; q =[q;q_p ;q_l ]; dq =[dq;dq_p ;dq_l ]; ddq =[ddq;ddq_p ;ddq_l ]; end; % za zadnji parabolicni segment izracunamo posebej % zadnji parabolicni segment; dolocitev vektorja casa t_p = ; %%% STUDENT %%% % izracun polozaja, hitrosti in pospeska v zadnjem parabolicnem segmentu q_p = ; %%% STUDENT %%% dq_p = ; %%% STUDENT %%% ddq_p = ; %%% STUDENT %%% % dodamo se zadnji segment k celotni trajektoriji time = [time; t_p ]; q =[q;q_p ]; dq =[dq;dq_p ]; ddq =[ddq;ddq_p ];

43 Načrtovanje trajektorije 37 Funkcija trajectory.m z uporabo funkcije interpolate izračuna trajektorijo gibanja vrha robota. V prvem delu je izračun via točk, potem pa še izvedba interpolacije. Funkcija je izpisana v nadaljevanju. Rezultat funkcije je matrika xtrajec s celotno trajektorijo gibanja robota za vseh šest zunanjih koordinat. % izracun trajektorije gibanja vrha robota v XZ ravnini x0 = q2zyzeul(q0); % izracun lege vrha robota pri polozaju q0 x_v = zeros(1,17); % inicializacija x koordinat via tock z_v = zeros(1,17); % inicializacija z koordinat via tock t_v = zeros(17,1); % inicializacija vektorja casa via tock r = 0.15; % radij kroznice po kateri se giba vrh robota ii = 0; % stevec % izracun via tock za gibanje vrha robota po kroznici v XZ ravnini for phi = 0:pi/8:2*pi, % kot od 0 do 2 PI ii = ii + 1; x_v(ii) = x0(1) + r*(cos(phi)-1); % izracun X koordinate z_v(ii) = x0(3) + r*sin(phi); % izracun Z koordinate % izracun vektorja casa za via tocke deltat = (1-0.5)/16; if (ii>1) t_v(ii,1) = t_v(ii-1) (ii-1)*deltat; else t_v(ii,1) = 0; end end % uporaba funkcije "interpolate" za interpolacijo trajektorij v oseh X in Z [time, x, dx, ddx] = interpolate(t_v, x_v, dt); [time, z, dz, ddz] = interpolate(t_v, z_v, dt); % dolocitev vseh sestih koordinat trajektorije gibanja vrha robota % X in Z koordinati se spreminjata, Y koordinata in Eulerjevi koti so % konstantni xtrajec = [time, x, x0(2)* ones(length(time),1), z, x0(4)* ones(length(time),1), x0(5)* ones(length(time),1), x0(6)* ones(length(time),1)]; Rezultat interpolacije je krožna krivulja, kot je prikazano na sliki 6.3. Interpolirano trajektorijo uporabite v simulacijski shemi v Simulinku, ki ste jo načrtali za izračun inverzne kinematike na osnovi inverzne Jacobijeve matrike(slika 6.4).

44 38 ROBOTIKA I 0.45 Trajektorija v XZ ravnini z (m) x (m) Slika 6.3. Interpolirana krožna trajektorija. Slika 6.4. Simulacijska shema v Simulinku.

45 NEWTON-EULERJEVA DINAMIKA 1. Enačbe za izračun dinamičnih enačb robota Izhajajoč iz baze robota, najprej določimo kotne hitrosti in kotne pospeške v posameznih sklepih ter translacijske pospeške izhodišč koordinatnih sistemov pritrjenih na posamezen segment ter translacijske pospeške masnih središč posameznih segmentov. Izhajamo iz predpostavke, da sta kotna hitrost ter kotni pospešek segmenta nič (baze robota) enaka nič (ω 0 0 = 0, ω0 0 = 0). Spremenljivke sklepov so označene s črko q in velja q ϑ za rotacijski sklep ter q d za translacijski sklep. Skica 7.1 prikazuje sile in momente, ki delujejo na prosti segment. Ker velja, da je vsota vseh sil, ki delujejo na segment enaka spremembi gibalne količine težišča segmenta lahko zapišemo sledečo relacijo f i f i+1 + m i g 0 = m i p Ci, (7.1) prav tako velja, da je vsota vseh momentov, ki delujejo na segment enaka spremembi vrtilne količine segmenta okrog težišča, torej μ i + f i r i 1,Ci μ i+1 f i+1 r i,ci = d dt (Īi ω i ). (7.2) V nadaljevanju bomo določili vse kinematične veličine, ki jih potrebujemo za izračun sil in momentov. Vse izračunane veličine se nanašajo na lokalni koordinatni sistem i, zaradi česar je izračunavanje enostavnejše in preglednejše.

46 40 ROBOTIKA I os sklepa i ω mi+1 os sklepa i + 1 f i ω i μ i ṗ Ci f i+1 O i 1 C i r i 1,Ci m i g 0 r i,ci μ i+1 r i 1,i O i segment i Slika 7.1. Prosti segment. 2. Izračun inverznega dinamičnega modela Kotno hitrost segmenta i določimo kot R i 1 T ω i i ω i 1 i 1 i = R i 1 T ( i ω i 1 i 1 + q ) iz 0 Kotni pospešek segmenta i določimo kot R i 1 T ω i i ω i 1 i 1 i = R i 1 T ( i ω i 1 i 1 + q iz 0 + q i ω i 1 i 1 z ) 0 za translacijski sklep za rotacijski sklep za translacijski sklep za rotacijski sklep (7.3) (7.4) Pokažimo, da je odvod R i 1 T i ω i 1 i 1 T enak Ri 1i ω i 1 i 1 : (R i 1 T i ω i 1 R i 1 T i ω i 1 T ω i 1 = R i 1 i i 1 ) = R i 1 i i 1 + M(ωi i )Ri 1 i T ω i 1 i 1 + Ṙi 1 T i ω i 1 i 1 = T ω i 1 i 1 i 1 + ωi i ω i i = R i 1 i T ω i 1 i 1, (7.5)

47 Newton-Eulerjeva dinamika 41 kjer je M(ω i i ) poševno simetrična matrika. Linearna hitrost izhodišča koordinatnega sistema i je R i 1 T (ṗi 1 ṗ i i i 1 + q ) iz 0 + ω i i r i i 1,i za translacijski sklep i = R i 1 i T ṗi 1 i 1 + ωi i ri i 1,i za rotacijski sklep (7.6) Linearni pospešek izhodišča koordinatnega sistema i je R i 1 T i 1 i ( p i 1 + q ) iz qi ω i i Ri 1 T ) i z0 p i + ω i i i = ri i 1,i + ωi i (ω i i ri i 1,i za translacijski sklep ) R i 1 T i p i 1 i 1 + ωi i ri i 1,i + ωi i (ω i i ri i 1,i za rotacijski sklep (7.7) Linearni pospešek masnega središča segmenta je p C i i = p i i + ω i i r i i,c i + ω i i ( ω i i r i ) i,c i (7.8) Izračun sil in momentov v sklepih začnemo na vrhu manipulatorja, ob predpostavki, da poznamo silo in moment, ki deluje na vrh manipulatorja. Če robot ne deluje v dotiku z okolico, potem lahko predpostavimo, da velja f n+1 n+1 = 0ter μ n+1 n+1 = 0, kjer je n število sklepov manipulatorja. Sila, ki jo izvaja segment i 1nasegmenti je tako enaka f i i = Ri i+1 fi+1 i+1 + m i p C i i (7.9) Moment, ki ga izvaja segment i 1nasegmenti je μ i i = f i i ( r i i 1,i + r i ) i,c i + R i i+1 μ i+1 i+1 + Ri i+1f i+1 i+1 ri i,c i + Ī i i ωi i + ω i i ( Ī i iω i i (7.10) Posplošene sile, ki delujejo v posameznem sklepu so določene s sledečo enačbo τ i = f i i μ i i T R i 1T i z0 + F vi q i + F si sgn( q i ) T R i 1T i z0 + F vi q i + F si sgn( q i ) za translacijski sklep za rotacijski sklep ) (7.11) 3. Izračun direktnega dinamičnega modela Na podlagi enačb (7.11) je mogoče ob poznavanju gibanja robota izračunati navore, ki delujejo v sklepih. τ = B(q) q + C(q, q)+f v q + F s + g(q). (7.12)

48 42 ROBOTIKA I Za simulacijo dinamike robota pa običajno potrebujemo direktni dinamični model, ki iz navorov, ki jih generirajo motorji robota, izračunava gibanje robota. q = B 1 (q)(τ (C(q, q)+f v q + F s + g(q))). (7.13) Z uporabo Newton-Eulerjeve formulacije je do direktnega dinamičnega modela relativno enostavno priti. Izraz v oklepaju (C(q, q)+f v q + F s + g(q)) je mogoče preprosto izračunati upoštevaje q = 0, kar sledi neposredno iz enačbe (7.12). Še več, stolpec b i matrike vztrajnosti robota B lahko izračunamo, če definiramo g 0 = 0, q = 0, q i = 1in q j = 0, če j i. Zopet lahko to enostavno preverimo, če zgornje parametre vnesemo v enačbo (7.12). Izvedbo direktnega dinamičnega modela v okolju Simulink prikazuje slika 7.2. Slika 7.2. Izvedba direktnega dinamičnega modela v okolju Simulink. 4. Naloga V Matlabu napišite funkcije za izvedbo sledečih nalog: - function tau = torque(ddq, dq, q, ddp0), ki za vrednosti kotov v sklepih robota zapisanih v vektorju q, vrednosti sklepnih hitrosti zapisanih v vektorju dq, vrednosti sklepnih pospeškov zapisanih v vektorju ddqq in vektorja pospeška, ki deluje na bazo robota ddp0, izračuna inverzni dinamični model robota, torej vektor sklepnih navorov tau; izračun naj bo izveden samo za prve tri segmente robota; - function B = inertia3(q), ki za vrednosti kotov v sklepih robota zapisanih v vektorju q,izračuna vztrajnostno matriko robota; pri tem uporabite kot osnovo funkcijo za izračun inverznega dinamičnega modela robota; - function tau = tau3 q dq(dq, q), ki za vrednosti kotov v sklepih robota zapisanih v vektorju q in vrednosti sklepnih hitrosti zapisanih v vektorju dq, izračuna navore v sklepih robota, ki so posledica delovanja Coriolisovih in gravitacijskih komponent ter posledica trenja; pri tem

49 Newton-Eulerjeva dinamika 43 uporabite kot osnovo funkcijo za izračun inverznega dinamičnega modela robota. V nadaljevanju so podane predloge vseh treh zgoraj omenjenih funkcij. Vaša naloga je, da pravilno dopolnite funkcije na mestih, kjer je označeno s komentarjem %%% STUDENT %%%. Predloga funkcije tau = torque(ddq, dq, q, ddp0) function tau = torque(ddq, dq, q, ddp0) % tau = torque(ddq, dq, q, ddp0) % Funkcija izracuna inverzni dinamični model za manipulator s tremi % rotacijskimi prostostnimi stopnjami. % Vhodni vektorji so polozaji v sklepih q, sklepne hitrosti dq, % sklepni pospeski ddq in vektor ddp0, ki doloca pospesek baze robota % Izhod funkcije je vektor sklepnih navorov tau % mase segmentov robota m = [ ]; % vektorji od sklepa i-1 do sklepa i r = [[0 0 0] [ ] [0 0 0] ]; % vektorji tezisc segmentov glede na lokalni koordinatni sistem rc = [[0 0 0] [ ] [ ] ]; % vztrajnostne matrike segmentov I = zeros(3,3,3); I(:,:,1) = [0 0 0; ; 0 0 0]; I(:,:,2) = [ ; ; ]; I(:,:,3) = [ ; ; ]; % parametri viskoznega trenja za posamezne sklepe fv = [10 5 5]; % struktura z vsemi robotskimi parametri (npr. masa prvega segmenta je rob.m(1)) rob = struct( m, m, rc, rc, r, r, I, I, fv, fv); % izracun transformacijskih matrik za prve tri segmente a2=0.45; % dolzina drugega segmenta A = zeros(4,4,3); A(:,:,1) = ; %%% STUDENT %%% A(:,:,2) = ; %%% STUDENT %%% A(:,:,3) = ; %%% STUDENT %%% % vektor v smeri osi z0 z0 = [0 0 1] ; % inicializacija vektorja navora tau = zeros(3,1); % dolocitev rotacijskih matrik kot podmatrik matrik A R = zeros(3,3,3); R(:,:,1) = ; %%% STUDENT %%% R(:,:,2) = ; %%% STUDENT %%% R(:,:,3) = ; %%% STUDENT %%% % VSE INICIALIZACIJE V NADALJEVANJU SO IZVEDENE ZA VSE TRI SKLEPE IN % SEGMENTE ROBOTA, ZATO SO UPORABLJENE MATRIKE DIMENZIJ 3x3, KJER VSAK % STOLPEC PREDSTAVLJA VEKTOR, KI OPISUJE EN SEGMENT ALI SKLEP ROBOTA % inicializacija vektorjev kotnih hitrosti segmentov omega = zeros(3,3); % inicializacija vektorjev kotnih pospeskov segmentov domega = zeros(3,3); % inicializacija vektorjev transl. pospeskov koordinatnih sistemov % segmentov ddp = zeros(3,3);

50 44 ROBOTIKA I % inicializacija vektorjev transl. pospeskov tezisc segmentov ddpc = zeros(3,3); % inicializacija vektorjev sile v sklepih f = zeros(3,3); % inicializacija vektorjev navora v sklepih mi = zeros(3,3); % izracun kinematicnih velicin for ii = 1:3, if (ii == 1) % robni pogoj za prvi sklep % kotna hitrost segmenta omega(:,ii) = ; %%% STUDENT %%% % kotni pospesek segmenta domega(:,ii) = ; %%% STUDENT %%% % translacijski pospesek segmenta ddp(:,ii) = ; %%% STUDENT %%% else % kotna hitrost segmenta omega(:,ii) = ; %%% STUDENT %%% % kotni pospesek segmenta domega(:,ii) = ; %%% STUDENT %%% % translacijski pospesek segmenta ddp(:,ii) = ; %%% STUDENT %%% end % translacijski pospesek tezisca segmenta ddpc(:,ii) = ; %%% STUDENT %%% end % izracun dinamicnih velicin for ii = 3:-1:1, if (ii==3), % robni pogoj za silo na vrhu % sila, ki deluje na sklep f(:,ii) = ; %%% STUDENT %%% % navor, ki deluje na sklep mi(:,ii) = ; %%% STUDENT %%% else % sila, ki deluje na sklep f(:,ii) = ; %%% STUDENT %%% % navor, ki deluje na sklep mi(:,ii) = ; %%% STUDENT %%% end % navor, ki deluje v osi sklepa (obremenjuje motor) tau(ii) = ; %%% STUDENT %%% end Testni rezultat: torque([123],[456],[789],[009.81] )= [ ] T

51 Newton-Eulerjeva dinamika 45 Predloga funkcije B = inertia3(q) function B = inertia3(q) % B = inertia3(q) % Funkcija izracuna vztrajnostno matriko za manipulator s tremi segmenti % Vhod je vektor kotov v sklepih q % inicializacijs vztrajnostne matrike B = zeros(3,3); % upostevamo pogoj za pospesek, ki deluje na bazo robota ddp0 = ; %%% STUDENT %%% % upostevamo pogoj za kotne pospeske v sklepih dq = ; %%% STUDENT %%% for ii = 1:3, % upostevamo pogoj za kotne pospeske v sklepih ddq = ; %%% STUDENT %%% ddq(ii) = ; %%% STUDENT %%% % uporabimo funkcijo za izracun inverzne dinamike robota za izracun % navorov, ki predstavljajo stolpec matrike vztrajnosti B(:,ii) = ; %%% STUDENT %%% end Testni rezultat: inertia3([1,2,3] )= Predloga funkcije tau = tau3 q dq(dq, q) function tau = tau3_q_dq(dq, q) % tau = tau3_q_dq(q, dq) % Funkcija izracuna navore v sklepih robota, ki so posledica delovanja % Coriolisovih in gravitacijskih komponent ter posledica trenja % Vhodni vektorji so polozaji v sklepih q in sklepne hitrosti dq % Izhod funkcije je vektor sklepnih navorov tau % upostevamo pogoj za pospesek, ki deluje na bazo robota ddp0 = ; %%% STUDENT %%% % upostevamo pogoj za kotne pospeske v sklepih ddq = ; %%% STUDENT %%% % uporabimo funkcijo za izracun inverzne dinamike robota za izracun navorov tau = ; %%% STUDENT %%% Testni rezultat: tau3 q dq([1,2,3],[4,5,6] )= [ ] T V Simulinku zgradite simulacijsko shemo, ki vključuje sledeče komponente: interpolirano krožno trajektorijo, inverzno kinematiko robota na osnovi inver-

52 46 ROBOTIKA I zne Jacobijeve matrike, direktni dinamični model robota ter enostavni PD regulator (PD regulator je prikazan na sliki 7.3). Celotna simulacijska shema je prikazana na sliki 7.4. Slika 7.3. Enostavni PD regulator. qr predstavlja referenčni položaj, q dejanski položaj in dq dejansko hitrost robota. KpinKd sta položajni in hitrostni ojačenji regulatorja Slika 7.4. Simulacijskashema, ki vključuje vse komponente: interpolirano krožno trajektorijo, inverzno kinematiko robota na osnovi inverzne Jacobijeve matrike, direktni dinamični model robota ter enostavni PD regulator.