České vysoké učení technické v Praze Fakulta strojní. Vojtěch Rada. Lanový HexaSphere

Transkript

1 České vysoké učení technické v Praze Fakulta strojní Lanový HexeSphere 2014

2 Obsah prezentace 1 Popis mechanismu 2 Konstrukční úpravy 3 Matematický model Kinematický model Dynamický model Simulace 4 Protivůlové řízení Simulace 5 Návrh regulátoru 6 Návrh regulátoru Simulace 7 Implementace řízení 8 Experimenty 9 Závěr

3 Popis mechanisu rám s centrálním kloubem platforma 6x vozíků platforma s vozíky spojena tyčemi pohyb vozíků pomocí pohybový šroubů

4 Konstrukční úpravy Uchycení lan

5 Konstrukční úpravy Uchycení lan Kladky osa naklapeni kladky kladka K lano V vozik vodici tyc pohybovy sroub

6 Kinematický model Závislost polohy vozíků na poloze platformy Poloha platformy popsána Eulerovými úhly ψ, ϑ a ϕ Podmínka odvalování platformy ϕ = ψ Schéma mechanismu z1 BE φd z2 BF f y2 AD AE BE S2 BF α α βp AE AD y2 O2 CD CF CF CD x2 φd EB sz x2 EA y1 DA DC x1 FB βv α y1 EA FC βv α EB FB FC βv DA DC Schéma kladky P x1 µ l1 3 z N u z3p rk l2 λ η K O3 OK 3 x x3p l3 V

7 Dynamický model Předpoklady: Všechny části jsou dokonale tuhé Lana přenášejí tahové i tlakové síly Zanedbání kladek P K l 1 l 2 V Moment setrvačnosti motoru a šroubu redukovaný moment vozíků Uvolnění vozíku: T av g F v T a V = F V m red z V + m voz g z

8 Dynamický model Zrychlení vozíku: z V = f ( ω 12 ) z V = b V ω 12 + c V Dynamika platformy Eulerova dynamická rovnice: 2 I O2 ω 12 + ω 12 (2 I O2 ω 12 ) = i 2 M i z1 ht AE AD BE BF y2 TaAE T TaBE O2 G CD CF TaCF TaBF y1 DA DC x1 EA EB FC FB Výsledný vektor úhlového zrychlení ω 12 = (2 I O2 + m red A S B C ) 1 AS (F c + m voz g c m red C c ) R 2T 2G ω 12 (2 I O2 ω 12 ) kde F c síly ze všech pohonů A S, B c, C c pomocné matice

9 Simulace modelu s tuhými lany cardan om cardant Model mechanismu: phi om Euler_dyn_Sfunkce S Function omt 1 s Integrator om euler_kin_rce Embedded MATLAB Function1 phit 1 phii s Integrator1 Ta PID* Saturation PID regulator delta_s 1 Psi inv_kin_euler_sfunkce inv_kin_cardan_sfunkce Clock rychlost trajektorie S Function3 s_zadana s_aktualni S Function2 Theta Výsledek simulace: skutecna poloha voziku s skutecna [m] cas [s] Sily v lanech T a [N] cas [s]

10 Protivůlového řízení Pro vektor kladných sil: M x = J +T T as, T as > 0 Řešení musí být nezávislé na vektoru M x : 0 = J +T T as, T as > 0 Tato podmínka slouží k testování pracovního prostoru Vede na soustavu m rovnic pro n neznámých nekonečně mnoho řešení Hledáme minimální řešení Převedení soustavy na kvadratické programování: min ( T T as T as ) 0 = J +T T as T as > T a min

11 Vektor sil v lanech Nalezení vektoru sil v lanech pro zatížení platformy M x : min ( T T as T as ) Změna předepnutí lan: M x = J +T T as T a min < T as < T a max Vektor T as zapíšeme jako součet vektorů T as0 výpočet pro T a min = 0 T as = T as0 + λ T as, T as výpočet pro T a min > 0, M x = 0

12 Simulace s protivůlovým řízením dtas time cardan om cardant Model mechanismu: phi om Tas Euler_dyn_Sfunkce S Function omt 1 s Integrator euler_kin_rce Embedded MATLAB Function1 phit phii 1 s Integrator1 Ta PID* dtas Saturation PID regulator delta_s Predpocitane_sily_dTas Clock 1 rychlost trajektorie Psi inv_kin_euler_sfunkce S Function3 s_zadana s_skutecna inv_kin_cardan_sfunkce S Function2 Theta Výsledky simulace: Sila z regulatoru F reg [N] Sily v lanech T a [N] cas [s] cas [s]

13 Návrh regulátorů Nastavení regulátorů v kaskádní regulaci V kaskádě 2 regulátory polohový, rychlostní K řízení použijeme stavovou zpětnou vazbu Stavový popis ve formě: ẋ ss = A ss x ss + B ss u ss Stavový popis nalezneme linearizací mechanismu Rovnice popisující dynamiku rozseparujeme do tvaru M l ( ϕ) + L l ( ϕ) + K l (ϕ) = J +T T as Budeme hledat matice M l, L l, K l

14 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly K

15 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly Řídící vektor u ss dán jako u ss = K x ss + w u ss = K 1 x 1 K 2 x 2 + K 2 x 2z K

16 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly Řídící vektor u ss dán jako u ss = K x ss + w u ss = K 1 x 1 K 2 x 2 + K 2 x 2z Dosazením za stavy dostaneme: u ss = K 1 ( ϕ) + K 2 (ϕ z ϕ) K

17 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly Řídící vektor u ss dán jako u ss = K x ss + w u ss = K 1 x 1 K 2 x 2 + K 2 x 2z Dosazením za stavy dostaneme: u ss = K 1 ( ϕ) + K 2 (ϕ z ϕ) Převedení na polohy vozíků: u ss = K 1 Q s ṡ + K 2 Q s z s K

18 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly Řídící vektor u ss dán jako u ss = K x ss + w u ss = K 1 x 1 K 2 x 2 + K 2 x 2z Dosazením za stavy dostaneme: u ss = K 1 ( ϕ) + K 2 (ϕ z ϕ) Převedení na polohy vozíků: u ss = K 1 Q s ṡ + K 2 Q s z s Nové matice zesílení K 1s = K 1 Q s a K 2s = K 2 Q s u ss = K 1s ṡ + K 2s s K

19 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly Řídící vektor u ss dán jako u ss = K x ss + w u ss = K 1 x 1 K 2 x 2 + K 2 x 2z Dosazením za stavy dostaneme: u ss = K 1 ( ϕ) + K 2 (ϕ z ϕ) Převedení na polohy vozíků: u ss = K 1 Q s ṡ + K 2 Q s z s Nové matice zesílení K 1s = K 1 Q s a K 2s = K 2 Q s u ss = K 1s ṡ + K 2s s Řídící vektor u ss v kaskádní regulaci dán jako: u ss = K K 1s ṡ + KK 1s KK 2s s, porovnáním získáme matice: K K 1s = K 1s, K K 2s = K 1 1s K 2s K

20 Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w u x ẋ = A x + B u zvolené póly Řídící vektor u ss dán jako u ss = K x ss + w u ss = K 1 x 1 K 2 x 2 + K 2 x 2z Dosazením za stavy dostaneme: u ss = K 1 ( ϕ) + K 2 (ϕ z ϕ) Převedení na polohy vozíků: u ss = K 1 Q s ṡ + K 2 Q s z s Nové matice zesílení K 1s = K 1 Q s a K 2s = K 2 Q s u ss = K 1s ṡ + K 2s s Řídící vektor u ss v kaskádní regulaci dán jako: u ss = K K 1s ṡ + KK 1s KK 2s s, porovnáním získáme matice: K K 1s = K 1s, K K 2s = K 1 1s K 2s Z matic K K 1s a KK 2s vybereme pouze diagonální prvky představují proporcionální zesílení polohových a rychlostních regulátorů K

21 Simulace modelu s navrženými regulátory Model mechanismu: phi om Tas Euler_dyn_Sfunkce S Function 1 s Integrator cardan phit 1 euler_kin_rce cardant om s Integrator1 Embedded MATLAB Function1 phii st_aktualni Ta delta_st rychlosti_st_sfunkce dtas dtas time rychlostni reg. K1s K2s st_zadana polohovy reg. delta_s S Function1 Predpocitane_sily_dTas 1 Psi inv_kin_euler_sfunkce inv_kin_cardan_sfunkce Clock rychlost trajektorie S Function3 s_zadana s_aktualni S Function2 Theta Výsledky simulace: Sila z regulatoru F reg [N] Sily v lanech T a [N] cas [s] cas [s]

22 Sestava Řízení mechanismu stejnosměrné motory MAXON řídící jednotky EPOS 24/5 real-time procesor dspace DC-1103 nad azený po íta ízení stroje p íkaz (tj. nap. relativní pohyb o ) ízení polohy zpracování p íkaz skute ná poloha vnitní p íkaz generátor profilu takt (1 khz) požadovaná poloha poloha regulátor polohy takt (1 khz) požadovaný proud poloha motor sníma PWM (50 khz) výkonové zesilova e skute nýproud regulátor proudu režim PWM takt (10 khz) y,q Inverzní kinematika dspace f1_ž f6_ž CAN w1_ž f1_a w6_ž f 6_a EPOS EPOS i1 w1_a i6 w6_a Maxon.... Maxon t t

23 Řídící program spušt ní sb rnice CAN status sb rnice CAN

24 Komunikace s jednotkami EPOS

25 Regulace

26 Trajektorie 2 Start_trajektorie 3 Auto/man 4 Mod volno volno start_cas start auto_man rs mod spinac Switch1 1 Constant1 >= Relational Operator 1 2 Multiport Switch2 ts(end) Constant tsn(end) Constant3 psi_r rad2deg psi_d rad2deg theta_r rad2deg theta_d rad2deg1 psi_d theta_d 6 Stop mod_prev mod_prev stop hod In1 Out1 7 z Ulozeni_aktualni_polohy Aktual_poz vystup_prev vystup Rozhodnuti 2 Druh_traj 1 cas vystup_prev In1 Out1 Cas startu Add 1 Rychlost_Traj Psi_spirala psi prepocet_uhlu2 psi0 Prepocet_uhlu_spirala 1 2 Multiport Switch euler pohony rozmery inv_kin Switch 2 z_traj 5 ov ení p ed spušt ním dv p edpo ítané trajektorie Theta_spirala Psi_naklon psi prepocet_uhlu2 psi0 Prepocet_uhlu_naklon rozmery zac_prev zac Inverse Kinematics zac_predchozi inverzní kinematika Theta_naklon cas ffwd 1 2 Kf 1 p edpo ítané protiv lové ízení ffwd_spirala casn ffwdn ffwd_naklon Multiport Switch1 predpeti1 0 sily ( ) Constant2 Switch2 ffwd

27 GUI p přírůstky jednotlivých pohonů p pro ruční řízení jejich polohy absolutně a přírůstkově p epínání mezi ru ním ízením a trajektorií pohonů inicializace komunikace s pohony pohonů nastavení výchozí polohy as od spušt ní pohonů išt ní chyb uvoln ní uv motorů m motorů motorů motorů

28 GUI konstanty polohového PID regulátoru omezení maximálního proudu zapnutí trajektorie s protiv lovým ízením p epínání mezi p edpo ítanými trajektoriemi aktuální poloha pohon spušt ní trajektorie inicializace komunikace s pohony nastavení výchozí polohy zadání výchozí polohy uvoln ní motor zapnutí motor rychlé zastavení motor zastavení trajektorie pr b hy zaznamenaných hodnot aktuální proud v motorech regula ní odchylka išt ní chyb rychlost projetí trajektorie velikost p edepnutí lan požadovaná poloha platformy aktuální a požadovaná rychlost pohon nastavení a spušt ní zaznamenávání

29 Určení proudové konstanty Experiment m/s m/s 0.009m/s i [ma] 350 v G sila [N]

30 Určení proudové konstanty Experiment m/s m/s 0.009m/s i [ma] 350 v G sila [N] Realizace protivůlového řízení In3 prirustek polohy = Tas/(Kr*Ki) zadana_rychlost poloha_zadana Kp Kr i Ki sila v lane i s predepnutim Polohovy regulator Rychlostni regulator Proudova konstanta poloha_aktualni rychlost_aktualni

31 Videa z experimentu bez předepnutí

32 Videa z experimentu s předepnutím

33 Závěr Splněné úkoly: Vytvoření simulačního modelu Doplnění simulačního modelu o protivůlové řízení Návrh nastavení regulátorů v kaskádní regulaci Vytvoření řídícího programu a uživatelského prostředí Dokončení montáže Provedení experimentů