České vysoké učení technické v Praze Fakulta strojní
Lanový HexeSphere 2014
Vojtěch Rada
Vojtěch Rada
Lanový HexaSphere
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 Vojtěch Rada
Lanový HexaSphere
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ů
Vojtěch Rada
Lanový HexaSphere
Konstrukční úpravy Uchycení lan
Vojtěch Rada
Lanový HexaSphere
Konstrukční úpravy Uchycení lan
Kladky osa naklapeni kladky kladka K lano V vozik vodici tyc
pohybovy sroub
Vojtěch Rada
Lanový HexaSphere
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
φd
BE BF
z2 f
BE
AE y2
AD
α
BF
AE
α
O2
CD
CD
x2 φD
EB EA
x2 y1
DA
x1
DC
EA
FB
βv
α
FC
βv
α
FC
EB
βv
FB
DC x1
Schéma kladky P µ l1 3
z
z3P
u
N l2
K≡ O3
rk λ
η OK
3
x x3P
l3
V
Vojtěch Rada
y2
CF
CF
sz
AD
βp
S2
Lanový HexaSphere
y1
DA
Dynamický model Předpoklady: Všechny části jsou dokonale tuhé Lana → přenášejí tahové i tlakové síly Zanedbání kladek P l1 K l2 V
Moment setrvačnosti motoru a šroubu → redukovaný moment vozíků Uvolnění vozíku: Tav g
Fv
Ta V = FV − mred · z¨V + mvoz · gz Vojtěch Rada
Lanový HexaSphere
Dynamický model Zrychlení vozíku: z¨V = f (ω˙ 12 ) → z¨V = bV · ω˙ 12 + cV Dynamika platformy → Eulerova rovnice: dynamická P IO2 · ω˙ 12 + ω 12 × 2 IO2 · ω 12 = i 2 Mi
2
z1
hT y2 TaAE
AE AD
BE
BF
T TaBE
O2
CF
G
CD TaBF
y1
DA
TaCF
x1
DC
EA FC
EB FB
Výsledný vektor úhlového zrychlení −1 ω˙ 12 = 2 IO2 + mred · AS · BC · AS · (Fc + mvoz · gc − mred · Cc ) + +2 R2T · 2 G − ω 12 × 2 IO2 · ω 12 kde Fc → síly ze všech pohonů AS , Bc , Cc → pomocné matice Vojtěch Rada
Lanový HexaSphere
Simulace modelu s tuhými lany Model mechanismu: phi om phit
cardan
euler_kin_rce
omt
1 s
Euler_dyn_Sfunkce S−Function
phii
1 s
cardant
om
om
Integrator1
Embedded MATLAB Function1
Integrator
Ta PID* Saturation
Psi
1 Clock
PID regulator
delta_s
inv_kin_cardan_Sfunkce
inv_kin_euler_Sfunkce
rychlost trajektorie
S−Function3
s_aktualni
s_zadana
S−Function2
Theta
Výsledek simulace: 30 25
−0.2
20 −0.25 Sily v lanech Ta [N]
skutecna poloha voziku sskutecna [m]
−0.15
−0.3 −0.35
15 10 5 0
−0.4 −5 −0.45 −0.5 0
−10 10
20
30 cas [s]
40
50
Vojtěch Rada
−15 0
10
Lanový HexaSphere
20
30 cas [s]
40
50
Protivůlového řízení Pro vektor kladných sil: Mx = −J+T · Tas ,
Tas > 0
Řešení musí být nezávislé na vektoru Mx : 0 = −J+T · Tas ,
Tas > 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 TT as · Tas 0 = −J+T · Tas Tas > Ta min
Vojtěch Rada
Lanový HexaSphere
Vektor sil v lanech
Nalezení vektoru sil v lanech pro zatížení platformy Mx : min TT as · Tas Mx = −J+T · Tas
Ta min < Tas < Ta max Změna předepnutí lan: Vektor Tas zapíšeme jako součet vektorů Tas = Tas0 + λ · ∆Tas , Tas0 → výpočet pro Ta min = 0
∆Tas → výpočet pro Ta min > 0, Mx = 0
Vojtěch Rada
Lanový HexaSphere
Simulace s protivůlovým řízením Model mechanismu: phi om phit cardan
omt
euler_kin_rce
1 s
Euler_dyn_Sfunkce Tas
1 s
cardant
om
phii
Integrator1
Embedded MATLAB Function1
Integrator
S−Function Ta PID* Saturation
dTas
delta_s
PID regulator
time
dTas
Predpocitane_sily_dTas
Psi
1 Clock
inv_kin_euler_Sfunkce
rychlost trajektorie
S−Function3
inv_kin_cardan_Sfunkce s_skutecna
s_zadana
S−Function2
Theta
25
100
20
90 80
15 Sily v lanech Ta [N]
Sila z regulatoru Freg [N]
Výsledky simulace:
10 5 0 −5
60 50 40 30
−10 −15 0
70
20 10
20
30 cas [s]
40
50
Vojtěch Rada
10 0
10
20
Lanový HexaSphere
30 cas [s]
40
50
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ě: x˙ ss = Ass · xss + Bss · uss Stavový popis nalezneme linearizací mechanismu
Rovnice popisující dynamiku rozseparujeme do tvaru Ml · (ϕ) ¨ + Ll · (ϕ) ˙ + Kl · (ϕ) = J+T · Tas Budeme hledat matice Ml , Ll , Kl
Vojtěch Rada
Lanový HexaSphere
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Vojtěch Rada
Lanový HexaSphere
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Řídící vektor uss dán jako uss = −K · xss + w → uss = −K1 · x1 − K2 · x2 + K2 · x2z
Vojtěch Rada
Lanový HexaSphere
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Řídící vektor uss dán jako uss = −K · xss + w → uss = −K1 · x1 − K2 · x2 + K2 · x2z
Dosazením za stavy dostaneme: uss = K1 · (−ϕ) ˙ + K2 · (ϕz − ϕ)
Vojtěch Rada
Lanový HexaSphere
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Řídící vektor uss dán jako uss = −K · xss + w → uss = −K1 · x1 − K2 · x2 + K2 · x2z
Dosazením za stavy dostaneme: uss = K1 · (−ϕ) ˙ + K2 · (ϕz − ϕ) ∂Q ∂Q Převedení na polohy vozíků: uss = −K1 · ∂s · s˙ + K2 · ∂s · ∆s z
Vojtěch Rada
Lanový HexaSphere
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Řídící vektor uss dán jako uss = −K · xss + w → uss = −K1 · x1 − K2 · x2 + K2 · x2z
Dosazením za stavy dostaneme: uss = K1 · (−ϕ) ˙ + K2 · (ϕz − ϕ) ∂Q ∂Q Převedení na polohy vozíků: uss = −K1 · ∂s · s˙ + K2 · ∂s · ∆s z Nové matice zesílení K1s = K1 · uss = −K1s · s˙ + K2s · ∆s
Vojtěch Rada
∂Q ∂s
a K2s = K2 ·
Lanový HexaSphere
∂Q ∂s
→
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Řídící vektor uss dán jako uss = −K · xss + w → uss = −K1 · x1 − K2 · x2 + K2 · x2z
Dosazením za stavy dostaneme: uss = K1 · (−ϕ) ˙ + K2 · (ϕz − ϕ) ∂Q ∂Q Převedení na polohy vozíků: uss = −K1 · ∂s · s˙ + K2 · ∂s · ∆s z Nové matice zesílení K1s = K1 · uss = −K1s · s˙ + K2s · ∆s
∂Q ∂s
a K2s = K2 ·
∂Q ∂s
→
Řídící vektor uss v kaskádní regulaci dán jako: K K uss = −KK 1s · s˙ + K1s · K2s · ∆s, −1 K porovnáním získáme matice: KK 1s = K1s , K2s = −K1s · K2s
Vojtěch Rada
Lanový HexaSphere
Stavová zpětná vazba Pomocí stavové zpětné vazby posuneme póly soustavy na libovolně w
u
x˙ = A · x + B · u
x
K
zvolené póly
Řídící vektor uss dán jako uss = −K · xss + w → uss = −K1 · x1 − K2 · x2 + K2 · x2z
Dosazením za stavy dostaneme: uss = K1 · (−ϕ) ˙ + K2 · (ϕz − ϕ) ∂Q ∂Q Převedení na polohy vozíků: uss = −K1 · ∂s · s˙ + K2 · ∂s · ∆s z Nové matice zesílení K1s = K1 · uss = −K1s · s˙ + K2s · ∆s
∂Q ∂s
a K2s = K2 ·
∂Q ∂s
→
Řídící vektor uss v kaskádní regulaci dán jako: K K uss = −KK 1s · s˙ + K1s · K2s · ∆s, −1 K porovnáním získáme matice: KK 1s = K1s , K2s = −K1s · K2s
K Z matic KK 1s a K2s vybereme pouze diagonální prvky → představují proporcionální zesílení polohových a rychlostních regulátorů Vojtěch Rada
Lanový HexaSphere
Simulace modelu s navrženými regulátory Model mechanismu: phi om cardan
phit
phii
1 s
om
Integrator1
Embedded MATLAB Function1
Integrator
S−Function
cardant
euler_kin_rce
1 s
Euler_dyn_Sfunkce Tas
st_aktualni rychlosti_st_Sfunkce
delta_st
Ta K1s
S−Function1
K2s dTas time
dTas
st_zadana
rychlostni reg.
polohovy reg.
delta_s
Predpocitane_sily_dTas
Psi
1 Clock
inv_kin_euler_Sfunkce
inv_kin_cardan_Sfunkce s_zadana
rychlost trajektorie
s_aktualni
S−Function3
S−Function2
Theta
25
100
20
90 80
15 Sily v lanech Ta [N]
Sila z regulatoru Freg [N]
Výsledky simulace:
10 5 0 −5
60 50 40 30
−10 −15 0
70
20 10
20
30 cas [s]
40
50
Vojtěch Rada
10 0
10
20
Lanový HexaSphere
30 cas [s]
40
50
Sestava
Řízení mechanismu stejnosměrné motory MAXON řídící jednotky EPOS 24/5 real-time procesor dSpace DC-1103 dSpace
CAN EPOS1
f1_ž
+
w1_ž -
+ -
nad azený po íta
regulátor polohy takt (1 kHz) t
požadovaný proud poloha
takt (1 kHz)
požadovaná poloha
regulátor proudu
PWM (50 kHz)
sníma skute ný proud
vnit ní p íkaz
poloha
11001..
p íkaz (tj. nap . „relativní pohyb o 10000“) ízení polohy zpracování p íkaz skute ná poloha
f1_a
výkonové zesilova e
ízení stroje
generátor profilu
motor
y,Q
. . . .
Inverzní kinematika
f6_ž
+ -
režim PWM takt (10 kHz)
Vojtěch Rada
Lanový HexaSphere
Maxon
. . . .
EPOS6 w6_ž
f6_a
t
i1
w1_a
+
i6 -
w6_a
Maxon
Řídící program spušt ní sb rnice CAN
status sb rnice CAN
Vojtěch Rada
Lanový HexaSphere
Komunikace s jednotkami EPOS
Vojtěch Rada
Lanový HexaSphere
Regulace
Vojtěch Rada
Lanový HexaSphere
Trajektorie >=
volno 2 Start_trajektorie
Switch1
tS(end)
2 Multiport Switch2
Constant tSn(end)
1
volno start_cas
3 Auto/man
1
Relational Operator
Constant3
Constant1
start
rad2deg
psi_r
psi_d
psi_d
rad2deg
auto_man
4 Mod
rs
rad2deg
theta_r
theta_d
mod
spinac
theta_d
rad2deg1
mod_prev
6 Stop 7 Aktual_poz
mod_prev
hod
Out1
In1
stop
Ulozeni_aktualni_polohy
z vystup vystup_prev
2
Rozhodnuti
Druh_traj prepocet_uhlu2
psi
vystup_prev
psi0
1
euler
2 1 cas
In1
Out1
Psi_spirala
1 Cas startu
Add
Prepocet_uhlu_spirala
pohony
Multiport Switch
Rychlost_Traj
rozmery
Switch
inv_kin
2 z_traj
5 rozmery zac_prev
Theta_spirala
ov ení p ed spušt ním
zac
psi
dv p edpo ítané trajektorie
Psi_naklon
prepocet_uhlu2
inverzní kinematika
Inverse Kinematics
psi0
Prepocet_uhlu_naklon zac_predchozi
Theta_naklon ffwd
cas
1 2
p edpo ítané protiv lové ízení
ffwd_spirala
Multiport Switch1
Kf predpeti1
0 sily
ffwdn
casn
(0 0 0 0 0 0) ffwd_naklon Constant2
Vojtěch Rada
Lanový HexaSphere
Switch2
1 ffwd
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ů
Vojtěch Rada
motorů
Lanový HexaSphere
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
inicializace komunikace s pohony
aktuální poloha pohon
spušt ní trajektorie
aktuální proud v motorech
nastavení výchozí polohy
regula ní odchylka išt ní chyb
zadání výchozí polohy
rychlost projetí trajektorie
uvoln ní motor
velikost p edepnutí lan
zapnutí motor
požadovaná poloha platformy
rychlé zastavení motor
aktuální a požadovaná rychlost pohon
zastavení trajektorie pr b hy zaznamenaných hodnot
nastavení a spušt ní zaznamenávání
Vojtěch Rada
Lanový HexaSphere
Určení proudové konstanty Experiment 600 550
0.0045m/s 0.006m/s 0.009m/s
500 450
i [mA]
400 350 300 250 200
G v
150 100 0
5
10
Vojtěch Rada
15
20 sila [N]
25
30
Lanový HexaSphere
35
40
Určení proudové konstanty Experiment 600 0.0045m/s 0.006m/s 0.009m/s
550 500 450
i [mA]
400 350 300 250 200
G
150
v
100 0
5
10
15
20 sila [N]
25
30
35
40
Realizace protivůlového řízení In3
prirustek polohy = Tas/(Kr*Ki) zadana_rychlost i
poloha_zadana Kp
Kr
Polohovy
Rychlostni
regulator
regulator
sila v lane i s predepnutim Ki
Proudova konstanta
rychlost_aktualni poloha_aktualni
Vojtěch Rada
Lanový HexaSphere
Videa z experimentu
bez předepnutí
Vojtěch Rada
Lanový HexaSphere
Videa z experimentu
s předepnutím
Vojtěch Rada
Lanový HexaSphere
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ů
Vojtěch Rada
Lanový HexaSphere
