Autonóm működésű négyrotoros helikopter irányítási algoritmusainak összehasonlítása
Regula Gergely VI. Vill.,
[email protected] Konzulensek: Dr. Lantos Béla, BME IIT,
[email protected] Dr. Gáspár Péter, MTA SZTAKI,
[email protected]
2
Kivonat Az MTA SZTAKI Rendszer- és Irányításelméleti Kutatólaboratóriuma, a BME Irányítástechnika és Informatika Tanszéke és a Közlekedésautomatika Tanszéke együttműködésében folyó kutatás célja egy beltéri miniatűr autonóm működésű négyrotoros helikoptermodell megépítése és irányítási rendszerének kifejlesztése. A dolgozat az utóbbihoz kapcsolódik. A négyrotoros helikopter rotorjai egy négyzet négy csúcsában helyezkednek el. Minden rotor a szomszédjával ellentétes irányban forog, de mindegyik azonos irányba ható erőt fejt ki. A rotorok fordulatszámának változtatásával a helikopter különféle mozgásokra képes. A négyrotoros helikopter egy 6 szabadságfokkal rendelkező nemlineáris rendszer, amely ráadásul alulaktuált is (a beavatkozó jelek száma kisebb a szabadságfoknál). A 4 beavatkozó jel a négy rotor szögsebessége. Ez megnehezíti a pályatervezést és az irányítást. Alapvető célja az irányításnak, hogy a helikopter képes legyen egy adott pálya mentén minél kisebb hibával végighaladni. A dolgozatban három irányítási algoritmust vizsgálok, egy lineáris (PID) elvűt, továbbá két nemlineáris szabályozási algoritmust, egy visszalépéses technikán (backstepping) és egy csúszó szabályozáson alapulót [3]. Rámutatok továbbá az egyes megoldások erősségeire és a gyakorlatban való alkalmazhatóságára. A dolgozat második részében a rendszer továbbfejlesztésével foglalkozom, mégpedig komplex, navigációs célú feladatok megoldásával és a realizálhatósági kérdésekkel. Előbb egy olyan algoritmust ismertetek, amellyel kiegészítve az irányítási rendszert a helikopter képes a sarokpontjaival előre definiált pályát meghatározott pontossággal, folyamatos mozgással követni. Végül pedig egy egykamerás markerbázisú képfeldolgozó rendszer és inerciális szenzorok által szolgáltatott mérések fúzióján alapuló állapotbecslési
módszert
mutatok
be
[7].
Az
állapotbecslést
két,
egymással
hierarchikusan összekapcsolt kiterjesztett Kalman-szűrő végzi, amelyek képesek kezelni a két mérőrendszer közötti működési sebességbeli különbséget.
3
Tartalomjegyzék 1. Bevezetés
5
2. A négyrotoros helikopter működése
6
3. A négyrotoros helikopter kinematikai modellje
7
4. A helikopter szabályozása
9
4.1. A helikopter szabályozása lineáris (PID) szabályozókkal 4.2. A helikopter pozíciójának és függőleges tengely körüli elfordulásának (Ψ) szabályozása visszalépéses technikával (backstepping) 4.3. A helikopter orientációjának szabályozása csúszó szabályozással (sliding control) 4.4. A három bemutatott szabályozás összehasonlítása
10 17 22 25
5. A szabályozás továbbfejlesztése – pályakövetés és állapotbecslés beépítése a rendszerbe
25
5.1. A pályakövetés elve 25 5.2. Állapotbecslés 27 5.3. A helikopter orientációjának, szögsebességének és szöggyorsulásának becslése kiterjesztett Kalmanszűrővel 28 5.4. A helikopter pozíciójának és sebességének becslése kiterjesztett Kalman-szűrővel 30 5.5. Szimulációs eredmények a pályakövetés és az állapotbecslés beiktatásával 32
6. Összefoglalás
38
7. Ábrajegyzék
39
8. Irodalomjegyzék
40
4
1. Bevezetés A négyrotoros helikopter egy már régóta létező, de a gyakorlatban nem elterjedt típusa a helikoptereknek. Elterjedésének gátja a négy motor egymástól független, ugyanakkor összehangolt működtetésének bonyolultsága és megvalósításának költségessége volt. Az első ilyen helikoptereket robbanómotorral hajtották, de a módszer nem bizonyult működőképesnek. A technika fejlődésével azonban lehetővé vált a rotorok összehangolt működtetésének megoldása [1, 2, 8].
1. ábra. Gyroplane No. 1, Louis and Jacques Breguet, 1907
2. ábra. Convertawings Model "A" Quadrotor, Amityville, 1956
Az MTA SZTAKI Rendszer- és Irányításelméleti Kutatólaboratórium, a BME Irányítástechnika és Informatika Tanszék és a Közlekedésautomatika Tanszék közösen fejleszti egy beltéri, autonóm működésű négyrotoros helikopter irányító- és navigációs rendszerét, valamint intelligens aktuátorait és szenzorait. A szenzorrendszer gyorsulás- és szögsebesség-érzékelőket tartalmaz. Ezt egészíti ki egy markerbázisú egykamerás képfeldolgozó rendszer. A helikopter irányítási rendszerének alapvető célja, hogy e mérési információk felhasználásával a helikopter képes legyen önállóan követni egy előre megtervezett pályát.
5
2. A négyrotoros helikopter működése A négyrotoros helikopter rotorjai egy négyzet csúcsaiban helyezkednek el. Minden rotor tengelye függőleges irányú, a propellerek pedig fix dőlésszögűek. Ez a kialakítás mechanikailag egyszerűbb a hagyományos helikopterénél, hiszen például nincs szükség a bonyolult és drága rotoragyra. A szemközti rotorok azonos irányban forognak, kettő az óramutató járásával azonos, kettő pedig azzal ellentétes irányban, és kizárólag felfelé ható erő kifejtésére képesek. A helikopter mozgását a rotorok szögsebességeinek változtatásával lehet befolyásolni, több beavatkozó jel nincsen. Mivel kevesebb a beavatkozó jelek száma, mint a jármű szabadságfokainak száma – a helikopter egy hat szabadságfokú rendszer –, alulaktuált rendszerrel állunk szemben, hasonlóan a hagyományos helikopterhez. Ez várhatóan azt jelenti, hogy a helikopter bizonyos fajta mozgásállapotot a felépítéséből következően képtelen megtartani. Egyszerű példa erre az egy helyben való lebegés, miközben a rotorok tengelyei nem függőlegesek.
3. ábra. A négyrotoros helikopter szerkezete
A rotorok hajtásáért kefe nélküli egyenáramú (BLDC – Brushless DC) motorok felelősek, amelyek mindegyikéhez külön rotorvezérlő elektronika kapcsolódik [9].
6
3. A négyrotoros helikopter kinematikai modellje A helikopter bázis-koordinátarendszerhez képesti pozícióját a q = [x y z ]T vektor, az orientációját az η = [Φ Θ Ψ ]T vektor adja. A modellt Newton—Euler módszerrel számítjuk [4]: mq&& + Fg = RFext Kω& + ω × ( Kω ) = τ ext
,
ahol: −
Fg = [0 0 − mg ]
T
a gravitációs erő,
− R a rotációs mátrix, −
Fext = [0 0 u ]
T
a külső erők összege,
− K az inerciamátrix, −
ω a szögsebességvektor,
− τ ext a külső nyomatékok összege. A rotációs mátrix tartalmaz egy z, y és x tengely körüli elforgatást, és alakja a következő: ⎡CΨ CΘ R = ⎢⎢ S Ψ CΘ ⎢⎣ − S Θ
C Ψ S Θ S Φ − S Ψ CΦ S Ψ S Θ S Φ + C Ψ CΦ CΘ S Φ
C Ψ S Θ CΦ + S Ψ S Φ ⎤ S Ψ S ΘCΦ − CΨ S Φ ⎥⎥ ⎥⎦ CΘ C Φ
A szögsebességvektor felírható ω = Γη& alakban, ahol ⎡1 0 Γ = ⎢⎢0 CΦ ⎢⎣0 S Φ
− SΘ ⎤ CΘ S Φ ⎥⎥ CΘCΦ ⎥⎦
a Jacobi-mátrix. A szögsebességvektor deriváltja ω& = Γη&& + Γ& η& . Ennek számításához szükséges a Γ mátrix idő szerinti deriváltja: ⎡1 0 d Γ& = ⎢⎢0 CΦ dt ⎢⎣0 S Φ
& ⎤ − S Θ ⎤ ⎡0 − CΘ Θ 0 ⎢ ⎥ & & −S Θ &⎥ CΘ S Φ ⎥ = ⎢0 − S Φ Φ Θ S Φ + CΘ CΦ Φ ⎥ & C −C S Φ & − SΘΘ &⎥ CΘCΦ ⎥⎦ ⎢⎣0 CΦ Φ Φ Θ Φ ⎦
7
A külső nyomatékok értékeit a következőképpen kapjuk: minden egyes rotor a szögsebességének négyzetével arányos felhajtóerőt fejt ki ( f i = bΩ i2 ). Az összes felhajtóerőt (u) és a nyomatékokat ( τ ) az alábbi összefüggések adják: 4
u = f1 + f 2 + f 3 + f 4 = b∑ Ω i2 i =1
⎤ l( f4 − f2 ) lb(Ω 24 − Ω 22 ) ⎡ ⎤ ⎡ ⎢ ⎥ ⎢ ⎥ 2 2 l ( f 3 − f1 ) lb(Ω 3 − Ω1 ) τB = ⎢ ⎥ ⎥=⎢ ⎢⎣ M 2 + M 4 − M 1 − M 3 ⎥⎦ ⎢⎣d (Ω 22 + Ω 24 − Ω12 − Ω 32 )⎥⎦ ahol l és b a helikopterre jellemző geometriai paraméter: l a rotorok távolsága a helikopter tömegközéppontjától, b pedig az egy rotor által kifejtett felhajtóerő és a rotor szögsebesség négyzetének aránya (“thrust factor”). Ezen kívül figyelembe kell még venni a giroszkopikus hatást is, amely a következő 4
alakot ölti: τ G = −∑ K r (ω × k )Ω i . Itt Kr a rotor inerciája (azonos az összes rotornál), k i =1
pedig a z irányú. A továbbiakban a következőket feltételezzük az egyszerűsítés érdekében: &⎤ ⎡Φ ⎡1 0 0⎤ Φ≈0 ⎢ & ⎥ és ⇒ Γ ≈ ⎢⎢0 1 0⎥⎥ ⇒ ω ≈ ⎢ Θ ⎥ Θ≈0 &⎥ ⎢Ψ ⎢⎣0 0 1⎥⎦ ⎣ ⎦ ⎡K x K ≈ ⎢⎢ 0 ⎢⎣ 0
0 Ky 0
0⎤ 0 0 ⎤ ⎡1 / K x ⎢ ⎥ −1 0 ⎥⇒K ≈⎢ 0 1/ K y 0 ⎥⎥ ⎢⎣ 0 K z ⎥⎦ 0 1 / K z ⎥⎦
A fentieket felhasználva megkaphatjuk az állapotegyenleteket: ⎡− ω y ⎤ ω& = K {−ω × ( Kω ) − ∑ K r ⎢⎢ ω x ⎥⎥ Ω i + τ B } , i =1 ⎢⎣ 0 ⎥⎦ −1
4
ahol &Ψ &⎤ & & ⎤⎡K Φ & ⎤ ⎡( K y − K z )Θ ⎡ 0 Ψ −Θ x ⎢ ⎢ & ⎥ ⎥ ⎢ & &⎥ & & − ω × ( Kω ) = ⎢ − Ψ 0 Φ ⎥ ⎢ K y Θ ⎥ = ⎢( K z − K x ) Ψ Φ ⎥ &⎥ & & ⎥ ⎢ ( K − K )Φ &Θ & ⎢ Θ −Φ 0 ⎥⎦ ⎢⎣ K z Ψ y ⎦ ⎣ x ⎦ ⎣ 8
A mozgásegyenletek: &x& = (CΦ S ΘCΨ + S Φ S Ψ )u / m &y& = (CΦ S Θ S Ψ − S Φ CΨ )u / m &z& = − g + CΦ CΘu / m &Ψ & − Kr Θ & (Ω + Ω − Ω − Ω ) + 1 τ && = K y − K z Θ Φ B 2 4 1 3 Kx Kx Kx x && = K z − K x Φ & + Kr Φ &Ψ & (Ω 2 + Ω 4 − Ω1 − Ω3 ) + 1 τ By Θ Ky Ky Ky && = K x − K y Φ &+ 1 τ &Θ Ψ B Kz Kz z
A differenciálegyenletekből
& ,Ψ & )T & ,Θ ( x, y, z , x& , y& , z&, Φ, Θ, Ψ , Φ
állapotválasztással az
állapotegyenlet könnyen felírható.
4. A helikopter szabályozása
A négyrotoros helikopter, ahogy azt az előbbiekben láttuk, amellett, hogy nemlineáris rendszer, alulaktuált is. Lényeges kérdés ezért, hogy a szabályozás során ezt a tulajdonságot mi módon vesszük figyelembe, milyen korlátozásokat vezetünk be. Látni fogjuk, hogy a lineáris és nemlineáris szabályozótípusok egymástól eltérő módon közelítik meg a problémát. Bizonyos speciális feltételek mellett olyan klasszikus szabályozási algoritmus is alkalmazható, mint amilyen a PID. Ennek a szabályozófajtának azonban megvannak a korlátai, amelyek miatt nehezen alkalmazható a gyakorlatban. Mivel a helikopter maga egy nemlineáris rendszer, robusztusabb nemlineáris szabályozások alkalmazása inkább indokolt. Ezek ráadásul a rendszer stabilitását garantálják általánosabb esetekben is, mint a PID szabályozás. A helikoptert irányító rendszerrel szemben fontos elvárás még, hogy a helikopter repülése során közel vízszintes helyzetben maradjon (Φ és Θ közel 0 legyen), hiszen ekkor érvényesek az előző fejezetben levezetett mozgásegyenletek. Arra is ügyelni kell,
9
hogy a beavatkozó jelek nagysága ne ütközzön fizikai korlátokba, vagyis a rotorok képesek legyenek az előírt szögsebességgel forogni. A helikopter térbeli pozíciójának és orientációjának mérése elengedhetetlen az irányításhoz, így ezeket az információkat mindhárom szabályozótípus felhasználja. A nemlineáris szabályozótípusok ezen kívül igénylik a pillanatnyi sebesség- és szögsebesség értékeket is. A szabályozási módszereket bemutató alfejezetek során az irányításhoz szükséges jelek mindegyikét mérhetőnek feltételezzük. Az 5. fejezet tárgyalja, milyen eszközökkel lehetséges e mennyiségek becslése más mérhető mennyiségek alapján.
4.1. A helikopter szabályozása lineáris (PID) szabályozókkal Létezik négy olyan mozgástípus, amelyek egymásutánjából bonyolultabb mozgásokat lehet megvalósítani. Ezek a következők: függőleges irányú mozgás és a helikopter koordinátarendszerének három tengelye körüli elfordulás úgy, hogy a helikopter tartja a magasságát. Ezeket úgy kapjuk, hogy a rendszert leíró differenciálegyenletekhez megfelelő korlátozásokat állítunk fel: − függőleges irányú mozgás: Φ =Θ=Ψ =0⇒
& =Ψ & = 0 τB =τB =τB = 0 & =Θ Φ y z ⇒ x && = Ψ && = 0 && = Θ &x& = &y& = 0 Φ
− z tengely körüli elfordulás: Φ=Θ=0⇒
& = 0 τB =τB = 0 & =Θ Φ y ⇒ x && = 0 && = Θ &x& = &y& = 0 Φ
&z& = 0 ⇒ u = állandó = 2b(Ω 2a + Ωb2 ) = mg Ω2 = Ω4 = Ωa Ω1 = Ω 3 = Ωb
− x tengely körüli elfordulás: x, z állandó ⇒ &x& = &z& = 0 & =Ψ & =0 Θ Θ=Ψ =0⇒ && = Ψ && = 0 Θ
10
− y tengely körüli elfordulás: y, z állandó ⇒ &y& = &z& = 0 & =0 & =Ψ Φ Φ=Ψ =0⇒ && = 0 && = Ψ Φ
Feltételezve, hogy a környezet tökéletesen zajmentes és minden lehetséges fizikai mennyiség közvetlenül mérhető, ezekhez a mozgásfajtákhoz külön-külön tervezhetünk egyszerűbb (PID jellegű) folytonos idejű szabályozókat. Legyen a szabályozók referenciajele a célhelyzetben az adott állapotváltozó kívánt értéke. Tekintettel kell azonban arra is lenni a referenciajel megtervezésekor, hogy a jel második deriváltjának is folytonosnak kell lennie, hogy a rendszer stabil maradjon. Ennek érdekében simítani kell a rendszerben a jeleket. A rendszer bemeneteit, azaz a rotorszögsebességeket úgy a legegyszerűbb meghatározni, ha a pályához tartozó felhajtóerőből (u) és nyomatékokból (τ) azokat visszaszámoljuk, ugyanis ez utóbbiak a rotorszögsebességek négyzeteinek lineáris kombinációi: ⎡ ⎢ 0 ⎛ Ω12 ⎞ ⎢ 1 ⎜ 2 ⎟ ⎢− ⎜ Ω 2 ⎟ ⎢ 2lb ⎜ 2⎟ = ⎢ ⎜ Ω3 ⎟ ⎢ 0 ⎜ Ω2 ⎟ ⎝ 4⎠ ⎢ 1 ⎢ ⎣ 2lb
−
1 2lb 0
1 2lb 0
1 4d 1 4d 1 − 4d 1 4d −
1⎤ 4b ⎥ τ 1 ⎥⎛⎜ Bx ⎞⎟ ⎥ 4b ⎥⎜τ By ⎟ 1 ⎥⎜τ B ⎟ ⎜ z⎟ 4b ⎥⎜ u ⎟ 1 ⎥⎝ ⎠ ⎥ 4b ⎦
A szabályozók egymáshoz hasonló struktúrájúak, a három pólus közül egy tisztán valós, a többi kettő pedig egy komplex póluspár. A tervezéskor ezek értékeit határozzuk meg.
4.1.1. Magasságszabályozás
A helikopter modellje erre a mozgásra: &z& = − g +
u ~ =u m 11
A szabályozó alakja a következő: u~ := &z&a + PID = &z&a + k P ( z a − z ) + k I
t
∫ (z
a
τ =0
− z )dτ + k D ( z&a − z& )
A helikopter és a szabályozó együtt (a zárt kör): t
&z& = &z&a + k P ( z a − z ) + k I
∫ (z
a
τ =0
− z )dτ + k D ( z&a − z& )
Ezt átrendezve és bevezetve az e = z a − z jelölést a hibára a következőt kapjuk: &e&& + k D e&& + k P e& + k I e = 0
Ennek a rendszernek a karakterisztikus egyenlete: s3 + kD s 2 + kP s + kI = 0 A referenciamodell legyen a következő alakú: ( s + ω1 (cos ϕ + j sin ϕ ))( s + ω1 (cos ϕ − j sin ϕ ))( s + ω0 ) = = s 3 + (ω0 + 2ω1 cos ϕ ) s 2 + (ω12 + 2ω0ω1 cos ϕ ) s + ω0ω12
Az együtthatókat egyeztetve a szabályozó paraméterei a következőképpen alakulnak: k D = ω0 + 2ω1 cos ϕ k P = ω12 + 2ω0ω1 cos ϕ k I = ω0ω12
A szögsebesség az u-ból számítható: Ωi =
u , i = 1,...,4 4b
4.1.2. z tengely körüli forgás szabályozása Formailag nagyon hasonlít ez az eset az előzőhöz, pár apróbb eltérés akad csak közöttük. A helikoptermodell természetesen módosul: && = τ Bz Ψ Kz A szabályozó (hasonló az előzőhöz):
τB
z
Kz
12
t
&& + PID = Ψ && + k (Ψ − Ψ ) + k (Ψ − Ψ )dτ + k (Ψ & −Ψ &) := Ψ a a P a I ∫ a D a τ =0
A paraméterek meghatározása ugyanúgy történik, mint az előbbi esetben. A nyomatékokhoz és a felhajtóerőhöz tartozó rotorszögsebességek: Ωa =
mg τ Bz + 4b 4d
Ωb =
mg τ Bz − 4b 4d
4.1.3. x tengely körüli forgás szabályozása Ez bonyolultabb az előzőeknél, mivel a modell összetettebb: & (Ω 2 + Ω 4 − Ω1 − Ω 3 ) + τ B = 0 KrΦ y && = τ B = lb(Ω 24 − Ω 22 ) K xΦ x A szabályozót két lépésben tervezhetjük meg: az első lépésben tervezünk egy szabályozót && = τ az előzőekhez hasonlóan a K x Φ rendszerhez. Ezt követően az egyszerűsítés Bx
érdekében a következő közelítéssel élünk: a τ By -ra vonatkozó összefüggésben felhasználjuk az éppen kiszámított 2-es és 4-es rotor szögsebességét. Ezt követően az alábbi összefüggés jól közelítési a többi rotor fordulatszámát: Ω32 − Ω12 = Ω32 + Ω12 =
τB
y
lb mg 2b cos Φ
Innen Ω1 =
τB mg − y 4b cos Φ 2lb
Ω3 =
τB mg + y 4b cos Φ 2lb
Az y tengely körüli forgás hasonló ehhez.
13
4.1.4. Mozgás egyenes mentén y irányban Felhasználhatjuk az előző eredményeket. A mozgást úgy lehet megvalósítani, hogy a helikopter magasságát tartjuk, miközben az x tengely körül hagyjuk elfordulni. A betartandó feltételek tehát a következők: &x& = 0 &y& = − SΦ
u m
&z& = − g + CΦ
Innen u-ra fennáll, hogy u =
u =0 m
⎛ &y& ⎞ mg m&y& =− , ahonnan &y& = − gTΘ , tehát Φ a = arctg ⎜⎜ − ⎟⎟ . CΦ SΦ ⎝ g⎠
Ha tehát ez az alapjele annak a szabályozónak, amely a Φ-t szabályozta, akkor egyenes vonalú pályát kapunk.
4.1.5. Bonyolultabb pályák tervezése Összetett mozgások megvalósíthatók úgy, hogy a szabályozókat a megfelelő időpillanatokban átkapcsoljuk, azaz a mozgást szakaszokra bontjuk. Ekkor azonban számolni kell azzal, hogy ezek a szabályozók nem tudják kezelni azokat a pontatlanságokat, amelyek abból fakadnak, hogy a rendszerben a hibák nem tűnnek el teljesen. A rendszer blokkvázlata az alábbi ábrákon látható: Szabályozás (PID) Referenciajelek
Hibajelek
Pozíció, orientáció W(rotor)
W(rotor)
Helikoptermodell Mért jelek
4 ábra. PID szabályozás - blokkvázlat
14
Kapcsolólogika
z szabályozása zhiba
W(rotor)
x szabályozása xhiba
W(rotor) 1 W(rotor)
y szabályozása yhiba
W(rotor)
Multiport Switch
1 hibajelek
Fi szabályozása Fihiba
W(rotor)
Theta szabályozása Thetahiba
W(rotor)
Pszi szabályozása Pszihiba
W(rotor)
5. ábra. PID szabályozás - A szabályozóegység belső felépítése
Az ilyesfajta szabályozások elég durvák olyan tekintetben, hogy nem korlátozzák a jelek meredekségét, így például előfordulhat olyan eset, hogy ereszkedésnél leáll az összes rotor. Ez pedig a valóságban nyilván nem engedhető meg.
4.1.6. Szimulációs eredmények Az alábbiakban egy olyan mozgás eredményei láthatóak, amely során mindegyik szabályozóegység szerephez jut. A mozgás egy emelkedési szakasszal (3m) kezdődik, ezt követően a helikopter a függőleges tengelye körül elfordul (π/6 radián), majd egyenesen mozog előre (2m), megállás után a függőleges tengely körüli fordulás nélkül ismét egyenes mentén mozog az előbbi irányra merőlegesen (1m), végül pedig leszáll. A mozgás az origóból indul.
15
Pozíció
4 3.5 3
z [m]
2.5 2 1.5 1 0.5 0 1
0.5
0
y [m]
-0.5
0
-0.5
0.5
2.5
2
1.5
1
x [m]
6. ábra. PID szabályozás - térbeli pálya
Pozíció [m]
Orientáció [rad]
3.5
0.6 x y z
3
0.5
2.5
0.4
Phi Theta Psi
2 0.3 1.5 0.2 1 0.1
0.5
0
0 -0.5
0
20
40
60
80
100 t [s]
120
140
160
180
7. ábra. PID szabályozás - pálya-idő függvény
200
-0.1
0
20
40
60
80
100 t [s]
120
140
160
180
200
8. ábra. PID szabályozás - orientáció-idő függvény
Az eredményekből kitűnik, hogy bár megfelelően pontosan követi a pályát a helikopter, a stabilitással problémák vannak: viszonylag hosszú ideig nem kapcsolhatóak egymás között a szabályozók, emiatt szükséges a hosszú idő a pályán való végighaladáshoz. A következő alfejezetekben tárgyalt szabályozási módszereknél ilyen problémába nem ütközünk.
16
4.2. A helikopter pozíciójának és függőleges tengely körüli elfordulásának (Ψ) szabályozása visszalépéses technikával (backstepping)
Ez a szabályozás képes arra, hogy a helikoptert egy kezdőpozícióból egy célpozícióba juttassa úgy, hogy közben egy előírt szögbe (Ψ) forduljon a függőleges tengelye körül. A célhelyzetben a helikopter vízszintes síkban marad.
Ref.-jelek
Referenciajelek
Szabályozás (Backstepping)
Pozíció, orientáció W(rotor)
W(rotor)
Helikoptermodell Mért jelek
Mért jelek
9. ábra. Backstepping - blokkvázlat
A rendszert leíró hat differenciálegyenletet két részre bonthatjuk, és egymással kapcsolatban
álló
részrendszerekként
kezelhetjük
őket.
Az
első
három
differenciálegyenlet egy transzlációs, a második három pedig egy rotációs alrendszert alkot. A pozíciószabályozó –, amely a transzlációs alrendszert szabályozza – a helikopter és a célhelyzet közötti távolság alapján két elérni kívánt szögelfordulást (Φd és Θd) szolgáltat az orientáció beállításáért felelő szabályozónak, ezen kívül a felhajtóerőt (u) számítja. A rotációs alrendszerhez tartozó szabályozó pedig a kívánt orientáció eléréséhez szükséges nyomatékokat számítja. A felhajtóerőből és a nyomatékokból a fentebb ismertetett összefüggés alapján számíthatóak a rotorszögsebességek. A szabályozók tervezése [3] alapján történik.
17
2 mért jelek
Fi (ref.)
Pozíció, orientáció, v (mért) Pozíciószabályozás Pozíció (ref.)
Orientáció, w, W (mért) Orientációszabályozás
Theta (ref.)
tau
Orientáció (ref.)
u
tau u
W(rotor)
1 W(rotor)
1 referenciajelek
10. ábra. Backstepping - a szabályozó felépítése
Az egyszerűség kedvéért bizonyos együtthatókat érdemes összevonni: a1 = ( K y − K z ) / K x a2 = − K r / K x a3 = ( K z − K x ) / K y a4 = K r / K y a5 = ( K x − K y ) / K z b1 = l / K x b2 = l / K y b3 = l / K z u x = CΦ S Θ C Ψ + S Φ S Ψ u y = CΦ S Θ S Ψ − S Φ C Ψ Ω = Ω 2 + Ω 4 − Ω1 − Ω 3
4.2.1. A rotációs rendszer szabályozása Legyen az x tengely körüli elfordulás hibája q1 = Φ d − Φ . Definiáljunk egy a célhelyzet környezetében pozitív definit Ljapunov-függvényt a következőképpen:
V (q1 ) =
1 2 q 2 1
& d −Φ & ) , amely negatív szemidefinit függvény Ennek idő szerinti deriváltja V& (q1 ) = q1 (Φ
& =Φ & d + α1q1 , akkor & -ot olyannak választjuk, hogy Φ kell, hogy legyen. Ha Φ V& (q1 ) = −α1q12 . Pozitívnak kell tehát α1 -et választani, hogy a feltétel teljesüljön. Egy
18
& −Φ & d − α1q1 , újabb változót bevezetve módosítjuk a Ljapunov-függvényt: legyen q2 = Φ így
V (q1 , q2 ) =
1 2 (q1 + q22 ) 2
Ennek a függvénynek az idő szerinti deriváltja &Ψ & +a Θ & V& (q1 , q 2 ) = q2 (a1Θ 2 Ω + b1τ Bx ) − && − α (q + α q )) − − q 2 (Φ d 1 2 1 1 − q1q 2 − α 1q12
Innen τ B x megválasztható úgy, hogy a függvény negatív definit maradjon:
τB = x
1 &Ψ & −a Θ & (q1 − α1 (q2 + α1q1 ) − α 2 q2 − a1Θ 2 Ω) , b1
a Ljapunov-függvény idő szerinti deriváltja pedig V& (q1 , q2 ) = −α1q12 − α 2 q22 . Legyen α 2 is && = 0 . Az α q tag q stabilizálását szolgálja. pozitív és Φ d 2 2 1
A többi tengelyre ható nyomatékot ehhez hasonlóan kaphatjuk:
τB = y
τB
z
1 &Φ & −a Φ & (q3 − α 3 (q4 + α 3q3 ) − α 4 q4 − a3Ψ 4 Ω) b2
1 &) &Θ = (q5 − α 5 (q6 + α 5 q5 ) − α 6 q6 − a5Φ b3
,
ahol q3 = Θ d − Θ & −Θ & −α q q4 = Θ d 3 3 q5 = Ψd − Ψ & −Ψ & −α q q6 = Ψ d 3 5
4.2.2. A transzlációs rendszer szabályozása A magasságszabályozás a rotációs rendszerben leírtakhoz hasonló módon történik: u=
m (q7 − α 7 (q8 + α 7 q7 ) − α 8 q8 + g ) , CΦ CΘ
ahol
19
q7 = zd − z q8 = z& − z&d − α 7 q7 A modellt leíró egyenletekből látszik, hogy az x és y irányú elmozdulás a felhajtóerőtől függ. Ha ux-et és uy-t úgy értelmezzük, hogy meghatározza u-nak x és y irányú komponenseit, akkor az előzőekhez hasonlóan számíthatunk ezeknek egy-egy értéket: m (q9 − α 9 (q10 + α 9 q9 ) − α10 q10 ) u , m u y = (q11 − α11 (q12 + α11q11 ) − α12 q12 ) u ux =
ahol q9 = xd − x q10 = x& − x&d − α 3q3 q11 = yd − y q12 = y& − y& d − α 3q3
.
Innen pedig a következő összefüggések alapján visszaszámolhatóak a kívánt szögek:
S Φ = S Ψ u x − CΨ u y SΘ =
CΨ u x + S Ψ u y CΦ
4.2.3. Szimulációs eredmények A szimuláció során a szükséges mennyiségeket mérhetőnek feltételeztem, és a mérési hibákat 0 középértékű fehér zajjal modelleztem. Az alábbi szimuláció esetén a helikopter az origóból indult, és a kívánt célhelyzet az alábbi volt:
xd = −1 m, yd = 1 m, z d = 3 m Ψd = 1 rad
20
A legfontosabb jelalakok: Pozíció
4 3
z [m]
2 1 0 -1 1.5 1
0.5 0
0.5
-0.5
0
-1 -0.5
y [m]
-1.5
x [m]
11. ábra. Backstepping - térbeli pálya Pozíció [m]
Orientáció [rad]
3.5
1.2
3
1
2.5
0.8
x y z
2 1.5
Phi Theta Psi
0.6
1
0.4
0.5 0
0.2
-0.5
0
-1 -1.5
0
5
10
15
20
25 t [s]
30
35
40
45
-0.2
50
12. ábra. Backstepping - pálya-idő függvény
0
5
10
15
20
25 t [s]
30
35
40
45
50
13. ábra. Backstepping - orientáció-idő függvény
Rotorszögsebességek [1/s] 800
700
600
500
400
300
200
100
0
0
5
10
15
20
25 t [s]
30
35
40
45
50
14. ábra. Backstepping – a rotorok szögsebességei a szimuláció során
21
4.2.4. A szabályozás értékelése A szabályozott rendszer jól reagál a viszonylag nagy zavarhatásokra is. Ennél a szabályozónál a 12 paraméter behangolása nem egyszerű feladat, mivel az egyes paraméterek hatása a szabályozott jelekre összetett. Emiatt a hangolást részben a Matlabba épített válaszfüggvény-optimalizálóra (Response Optimization) bíztam. Itt egy bemenetre adandó válasz tulajdonságaira határozhatunk meg bizonyos korlátozásokat (pl. a jelnek milyen határok között kell maradnia). Ez a fajta szabályozó már alkalmas arra, hogy a helikopter egy magasabb szinten megtervezett pályát képes legyen követni.
4.3. A helikopter orientációjának szabályozása csúszó szabályozással (sliding control) A tervezés menete hasonlít a visszalépéses technikán alapuló szabályozó tervezéséhez egészen q2 bevezetéséig [3]. Itt azonban s2-nek nevezzük el az új változót: & −Φ & d − α1q1 , így a Ljapunov-függvény alakja a következőre módosul: s2 = Φ
V (q1 , s2 ) =
1 2 2 (q1 + s2 ) 2
Az irányítási törvényt másként választjuk meg, mint előbb, természetesen az s2 s&2 < 0 feltételt betartva:
s&2 = −k1 sgn( s2 ) − k 2 s2 = && − Φ && − α q& = =Φ 2
d
1 1
&Ψ & +a Θ & && = a1Θ 2 Ω + b1τ B x − Φ d + α1 ( q2 + α 1q1 ) Innen τ B x meghatározható:
τB = x
1 2 &Ψ & −a Θ & (−a1Θ 2 Ω − α1 q1 − k1 sgn( s2 ) − k 2 s2 ) b1
A többi két nyomatékot is hasonló módon kapjuk:
22
τB = y
τB
z
1 &Φ & − a4 Φ & Ω − α 22 q3 − k3 sgn( s4 ) − k 4 s4 ) (−a3Ψ b2
1 & − α 2 q − k sgn( s ) − k s ) &Θ = (−a5Φ 3 5 5 6 6 6 b3
,
ahol
q3 = Θ d − Θ & −Θ & −α q s4 = Θ 3 3 d q5 = Ψd − Ψ & −Ψ & −α q s6 = Ψ d 3 5
4.3.1. Szimulációs eredmények A szimuláció paraméterei az összehasonlíthatóság kedvéért megegyeznek a visszalépéses technikát bemutató szimuláció paramétereivel. A legfontosabb jelalakok a következők:
Pozíció
4 3
z [m]
2 1 0 -1 1.5 1
0.5 0
0.5
-0.5
0 y [m]
-1 -0.5
-1.5
x [m]
15. ábra. Csúszó szabályozás - térbeli pálya
23
Pozíció [m]
Orientáció [rad]
3.5
1.2
3
1
2.5 x y z
2 1.5
0.8
Phi Theta Psi
0.6
1 0.4
0.5 0
0.2
-0.5 0
-1 -1.5
0
5
10
15
20
25 t [s]
30
35
40
45
-0.2
50
16. ábra. Csúszó szabályozás - pálya-idő függvény
0
5
10
15
20
25 t [s]
30
35
40
45
50
17. ábra. Csúszó szabályozás - orientáció-idő függvény
Rotorszögsebességek [1/s] 700
600
500
400
300
200
100
0
0
5
10
15
20
25 t [s]
30
35
40
45
50
18. ábra. Csúszó szabályozás - a rotorok szögsebességei a szimuláció során
4.3.2. A szabályozás értékelése A szabályozó ez esetben is képes a helikopter stabilizálására, azonban a csúszó szabályozás kapcsoló jellege miatt a célhelyzetben kis amplitúdójú oszcilláció figyelhető meg. A tisztán visszalépéses technikán alapuló szabályozással szemben itt 15 a szabad paraméterek száma. Ezek hangolása hasonlóan történik, mint az előbbi esetben.
24
4.4. A három bemutatott szabályozás összehasonlítása A PID szabályozás alapjaiban különbözik a nemlineáris szabályozótípusoktól. Ahhoz, hogy ez a fajta szabályozás valós körülmények között is működőképes legyen, elengedhetetlen egy bonyolult magasabb szintű logika, amely elegendően gyors és intelligens ahhoz, hogy a helikopter aktuális állapotát ismerve képes legyen a szabályozóegységek működését összehangolni. Ez azt jelenti, hogy megfelelő időpillanatokban ki kell választania azt az alegységet, amelynek át kell vennie a helikopter szabályozását. Így azonban a szabályozás összességében bonyolultabb lenne, mint egy nemlineáris szabályozás alkalmazása esetén. A két nemlineáris szabályozó hasonló tulajdonságokkal bír. Bizonyos zajkorlátok mellett mindkettő garantálja a helikopter stabilitását, és a rendszert könnyű kiegészíteni úgy, hogy a helikopter egy előre definiált pályát követni tudjon. Ez pedig nagyon előnyös tulajdonság a későbbi fejlesztés szempontjából. A szimulációs eredmények azt mutatják, hogy a visszalépéses technikán alapuló szabályozásnak az az előnye a csúszó szabályozóval szemben, hogy a beavatkozó jelei simábbak, egyenletesebben oszlik el a terhelés a propellereken, és pontosabban követi a referenciajeleket. Éppen ezekért a tulajdonságokért a helikopter irányításának továbbfejlesztésekor ezt a szabályozótípust alkalmazzuk.
5. A szabályozás továbbfejlesztése – pályakövetés és állapotbecslés beépítése a rendszerbe
5.1. A pályakövetés elve A pályatervezés és -követés első közelítésben viszonylag egyszerű elvű. A bejárandó pályát
részekre
bontjuk,
azaz
megadunk
megfelelően
sok
sarokpontot
az
inerciarendszerben megadott pozíciójával és orientációjával (Ψ), amelynek a közelében
25
kis hibával el kell a helikopternek haladnia. Ha az aktuális sarokpontot “elegendően” megközelítette a helikopter –, vagyis mind a célpozíciót, mind pedig a kívánt orientációt –, akkor átugrunk a következőre (feltéve, ha van még ilyen). Az “elegendően” közel megfogalmazása a következő lehet: a pozíció esetén az aktuális és célpozíció koordinátái közti négyzetes hibák összege legyen kisebb 1. egy előre definiált fix értéknél, vagy 2. az éppen elhagyott és következő, még el nem ért sarokpontok közötti eltérés négyzetes összegének 1/16-ánál (tehát ha túl van a két sarokpont közti út 3/4-én, akkor a következőre ugrunk), vagy pedig 3. az előző kettő közül a nagyobbiknál! A második feltételnek az a célja, hogy csökkentse az alacsony sebességű szakaszokat a mozgás során, vagyis hogy folyamatosabb legyen a repülés. A harmadik pedig az első kettő előnyeit ötvözi, vagyis jól működik akkor is, ha a sarokpontok egymáshoz a kelleténél közelebb vannak, vagy jelentősek a zavarhatások. Az orientáció követése eltér a fenti módszertől, mivel fontos lehet, hogy a helikopternek adott pozícióban milyen az orientációja. Így ez esetben a következő sarokpontra ugrás feltétele az, hogy a szögeltérés egy fix értéknél legyen kisebb. Ezt a feltételt a következőképp fogalmazhatjuk meg: mivel gyakran nem érdektelen, milyen irányba fordul a helikopter a mozgása során, a Ψ szög értéke bármekkora lehet, nem feltétlenül egy 2π szélességű intervallum eleme, mint ahogyan a képfeldolgozásból származó érték. Az orientációs hiba meghatározásához ezért azt az α h ∈ [0,2π ) értéket vesszük alapul, amelyre teljesül, hogy
αh ≡ α −αd
(mod 2π )
Erre az értékre teljesülnie kell a következő feltételek valamelyikének:
α h < α 0 , vagy 2π − α 0 < α h Itt α 0 ∈ [0, π ) választása szükséges, hogy legyen olyan αh, amelyre a fenti feltételek valamelyike nem teljesül, de a gyakorlatban ennél lényegesen kisebb küszöbértékre van szükség.
26
5.2. Állapotbecslés A szimulációk során az eddigiekben minden jelet hozzáférhetőnek feltételeztük. Ez azonban a valóságban nem minden esetben áll fenn: bizonyos jelek mérhetőek, mások nem, vagy nem megfelelő pontossággal, ill. gyakorisággal. Ahhoz, hogy a szabályozás valóban működőképes legyen, ezeket az akadályokat állapotbecslővel kell elhárítani. A fejlesztés alatt álló helikopter navigációjához egy Crossbow MNAV100CA típusú inerciális érzékelő (IMS – Inertial Measurement Sensor) háromdimenziós gyorsulás- és szögsebesség-érzékelőinek jeleit használhatjuk fel, amelyek relatív mérési adatokat szolgáltatnak. Ezt egészíti ki egy egykamerás markerbázisú képfeldolgozó rendszer [10], amely abszolút pozíciót és orientációt mér. A két egység egymástól eltérő frekvencián működik: az IMS 100Hz-en, míg a képfeldolgozás hozzávetőlegesen 10Hz-en. Ezen kívül rendelkezésre áll a négy rotor szögsebessége, az IMS-hez hasonlóan 100Hz gyakorisággal. A szabályozáshoz szükséges jeleket ezek alapján kell becsülni, a mérési zajokat figyelembe véve. A mérési zajokat az IMS esetén a valódi értékhez hozzáadódó bias és fehér zaj kombinációjával, a képfeldolgozásnál a valódi értékhez hozzáadódó fehér zajjal modellezünk. Az állapotbecslő tervezése [6] alapján történik. A becslést két kiterjesztett Kalmanszűrő végzi, amelyek egymással össze vannak kapcsolva. A Kalman-szűrők egyike becsüli a helikopter orientációját, szögsebességét és szöggyorsulását, míg a másik a helikopter pozícióját és sebességvektorát becsüli. A továbbiakban feltételezzük a következőket. Legyen a világ-koordinátarendszer K, a helikopter saját koordinátarendszere Kh, a szenzor-koordinátarendszer pedig Ks. Kh legyen azonos azzal a koordinátarendszerrel, amelyet a dinamikus modell levezetésekor használtunk. Az egyszerűség kedvéért a helikopter kezdeti pozíciója és orientációja olyan legyen, hogy K és Kh kezdetben azonos. Feltételezhetjük továbbá, hogy a szenzor a helikopter mozgása közben nem mozdul el a helikopterhez képest, és hogy a képfeldolgozás eredményei világ-koordinátarendszerben értendőek (mennyezetre szerelt kamera, host gépen futó képfeldolgozás, rádiókapcsolat a helikopteren telepített irányító rendszerrel).
27
A koordinátarendszerek pozíciói és orientációi közötti kapcsolat tehát az alábbi: ARPY , p As , ps K ⎯⎯ ⎯→ K h ⎯⎯ ⎯→ K s
Itt ARPY megegyezik a kinematikai modellben használt R mátrixszal. A továbbiakban az alsó indexek utalnak arra, hogy egy adott vektor melyik koordinátarendszerben van felírva. A szenzor-koordinátarendszert azért szükséges bevezetni, mert az IMS szenzorai által mért értékeket át kell transzformálni a helikopter koordinátarendszerébe, mielőtt felhasználnánk azokat. Fizikailag nem lehetséges ugyanis az IMS összes szenzorát a helikopter tömegközéppontjába helyezni, és a helikopter koordinátarendszerével azonos orientációba állítani.
5.3. A helikopter orientációjának, szögsebességének és szöggyorsulásának becslése kiterjesztett Kalman-szűrővel Gyűjtsük össze az IMS által mért szögsebességkomponenseket egy ρs vektorba! Ez az érték tartalmazza az összes mérési hibát, azaz felírható
ρ s = ρ s ,0 + ρ s ,b + ρ s ,n alakban. A b alsó index a biast, míg az n a fehér zajt, ρs,0 pedig a valódi értéket jelöli. Ezt kell a helikopter koordinátarendszerébe transzformálni, vagyis As-sel meg kell szorozni balról:
As ρ s = As ρ s ,0 + As ρ s ,b + As ρ s ,n Ezt az összefüggést átrendezve a következőt kapjuk (a zaj előjele itt nem számít):
ρ h = As ρ s ,0 = As ρ s − As ρ s ,b + As ρ s ,n A ρ vektorból meghatározhatóak a szögsebességvektor valódi komponensei. A szögsebességvektor világ-koordinátarendszerbeli értékeit a ρh vektor és a Jacobi-mátrix inverzének szorzataként kapjuk:
& ⎞ ⎡1 T ΘS Φ ⎛Φ ⎜ ⎟ ⎢ & ⎟= 0 CΦ ⎜Θ ⎢ ⎜Ψ ⎟ & ⎝ ⎠ ⎢⎣0 S Φ / C Θ
28
⎤⎛ PB ⎞ ⎥ ⎜ Q ⎟ = F ( Φ , Θ) ρ ⎥⎜ B ⎟ CΦ / C Θ ⎥⎦⎜⎝ RB ⎟⎠ T ΘCΦ − SΦ
A Jacobi-mátrix azonos a kinematikai modellben használt Γ mátrixszal. A biast időben közelítően konstansnak tekinthetjük, így az állapotbecslést a következő modell alapján végezhetjük: &⎞ ⎛Φ ⎜ ⎟ & ⎟ = − FA ρ + FA ρ + FA ρ ⎜Θ s s ,b s s s s ,n ⎜Ψ &⎟ ⎝ ⎠ ρ& s ,b = ρ s ,b ,n ⎛Φ⎞ ⎛Φ⎞ ⎛Φ⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ ⎜Θ⎟ = ⎜Θ⎟+⎜Θ⎟ ⎜Ψ⎟ ⎜ ⎟ ⎜ ⎟ ⎝ ⎠m ⎝ Ψ ⎠ ⎝ Ψ ⎠n
A fenti folytonos idejű modellnek megfeleltethető az alábbi diszkrét idejű modell (Eulermódszer, T a mintavételi idő):
x1,k +1 = x1,k − TFk As x2,k + TFk As uk + TFk As w1,k x2,k +1 = x2,k + Tw2,k yk = x1,k + z k A jelölések a következők:
(
ρ sT,b ) = (x1T T
x= Φ Θ Ψ u = ρs
(
w = ρ sT,n
x2T
)
T
ρ sT,b,n ) = (w w)T T
F = F ( Φ , Θ) Ez a rendszer felírható a következő alakban:
xk +1 = f ( xk , uk , wk ) y k = g ( xk , z k ) Ehhez a diszkrét idejű nemlineáris rendszerhez kiterjesztett Kalman-szűrő tervezhető [5] alapján. A továbbiakban tételezzük fel, hogy w és z egymással korrelálatlanok, és vezessük be az alábbi jelöléseket:
Rw,k = E[ wk wkT ] Rz ,k = E[ z k z kT ]
29
∂f ( xˆk , uk ,0) ∂x ∂f ( xˆk , uk ,0) Bw,k = ∂w ˆ ∂g ( xk ,0) Ck = ∂x ∂g ( xˆk ,0) C z ,k = ∂z Ak =
A Kalman-szűrés lépései pedig: 1. predikció: xk = f ( xˆk −1 , uk −1 ,0) M k = Ak −1Σ k −1 AkT−1 + Bv ,k −1 Rv ,k −1 BvT,k −1
2. frissítés: S k = Ck M k CkT + C z ,k Rz ,k C zT,k Gk = M k CkT S k−1 xˆ k = xk + Gk ( yk − g ( xk ,0)) Σ k = M k − Gk S k GkT Az állapotbecslő mintavételi ideje 10ms, ilyen időközönként érkezik új mérési adat az IMS-től. Az IMS és a képfeldolgozás működése közti sebességkülönbséget úgy vesszük figyelembe, hogy amíg meg nem érkezik az új abszolút mérés, addig a frissítési szakaszban xˆk = xk + 0 -val számolunk. A mért szögsebesség értékét korrigáljuk a bias értékével és a szöggyorsulás értékét a korrigált érték deriváltjaként kapjuk. Ez utóbbi azonban túlságosan pontos becslés.
5.4. A helikopter pozíciójának és sebességének becslése kiterjesztett Kalman-szűrővel A kiterjesztett Kalman-szűrő tervezése az előző pontban bemutatotthoz hasonlóan történik, azonban az állapotbecsléshez használt modell természetesen eltér az előbbitől.
30
Lényeges különbség a gyorsulásérzékelők és a giroszkópok között, hogy a gravitáció hatására az előbbieknek nyugalmi állapotban sem 0 közeli a kimenetük! A mért érték tehát a következőkből tevődik össze: as = as ,0 + as ,b + as ,n − As−1 A−1 g
Meg kell határozni as,0 kapcsolatát a helikopter koordinátarendszerében felírt gyorsulással, a-val. Ehhez nem elég a Kh és Ks koordinátarendszerek közötti transzformáció
ismerete,
szükséges
még
a
helikopter
szögsebesség-
és
szöggyorsulásvektora is. A mozgó koordinátarendszerben történő deriválás szabálya alapján as ,0 = As−1 (a + ε × ps + ω × (ω × ps ))
Az eredményt visszahelyettesítve az előbbi egyenlőségbe, majd az egyenlet mindkét oldalát As-sel szorozva a következőt kapjuk: As as = ah + ε × ps + ω × (ω × ps ) + As as ,b + As as ,n − A−1 g
Az egyenletet átrendezzük ah-ra: ah = As as − ε × ps − ω × (ω × ps ) + A−1 g − As as ,b − As as ,n
Bevezethetjük a későbbi egyszerűsítés érdekében a következő jelölést: am = As as − ε × ps − ω × (ω × ps ) + A−1 g
Ismét alkalmazva a mozgó koordinátarendszerben érvényes deriválási szabályt ( a = v&h + ω × vh ), és az előzőekhez hasonlóan feltételezve, hogy a bias időben közelítően konstans, eljutunk ahhoz a modellhez, amely alapján az állapotbecslést végezzük. A modell tehát v&h = −ω × vh − As as ,b + am + As as ,n a& s ,b = as ,b , n p& = ARPY v + v p ,n p m = p + pn
Hasonlóan az előbbiekhez ennek a modellnek is megfeleltethető egy diszkrét idejű modell, amely a következő:
31
vk +1 = vk − T [ω×]k vk − TAs as ,b ,k + Tam ,k + TAs as ,n ,k as ,b ,k +1 = as ,b ,k + Ta s ,b ,n ,k pk +1 = pk + TARPY ,k vk + Tv p ,n ,k yk = pm ,k = pk + Tpn ,k A rendszer xk +1 = f ( xk , uk , wk ) y k = g ( xk , z k ) alakra hozható az alábbi jelölések bevezetésével: x = (v as ,b
p )T
u = am w = ( a s ,n
as ,b ,n
v p ,n )T
y = pm z = pn A kiterjesztett Kalman-szűrő tervezése innentől azonos az előbbivel. Látható am és a diszkrét idejű modell alakjából, miért van szükség arra, hogy a két Kalman-szűrő hierarchikusan helyezkedjen el egymáshoz képest. Szükséges ugyanis az orientáció, a szögsebesség és a szöggyorsulás ismerete (becslése) ahhoz, hogy pozíció- és sebességbecslés álljon elő.
5.5. Szimulációs eredmények a pályakövetés és az állapotbecslés beiktatásával A rendszer szimulálásához kétféle, egymástól különböző jellegű pályát definiáltam. Az egyik egy karácsonyfa alakú pálya, amely tartalmaz egy emelkedési szakaszt, valamint egy lineárisan csökkenő sugarú térbeli spirál alakú pályát. A másik egy R sugarú körbe írható szabályos n-szög alakú pálya adott magasságban. Míg az előbbi túlnyomóan görbe vonalú részekből áll, az utóbbit kizárólag egyenes szakaszok alkotják. Fontos, hogy a pálya sima legyen, tehát még az állapotváltozók második deriváltjai – a gyorsulás és a szöggyorsulás – sem tartalmazhatnak ugrásokat, emiatt a megtervezett pályát simítani kell. A simításhoz a következő átviteli függvényű modellt alkalmaztam:
32
W ( s) =
psn ( s + ps ) n
n = 3, ps = −1.5
A pályakövetés paramétereit a következőkre állítottam: a pozíció hibanégyzetének a kezdő- és végpont közötti út ¼-énél, vagy 5cm-nél, az orientációs hibának π/45-nél kell kisebbnek lennie, hogy a helikopter továbbhaladhasson. A mérés modellezésére a következőket feltételeztem: a Kh és Ks koordinátarendszerek közötti transzformációt az alábbi paraméterek jellemzik: As = RPY (π / 100, π / 50, π / 40) ps = (0.01m 0.02m 0.03m)T
A szimuláció során a mérési zajok nagysága a következő: Mért mennyiség
Bias
Zaj szórása
Pozíció
0
0,02 [m]
Sebesség
0
0,01 [m/s]*
Gyorsulás
0,01 [m/s2]
0,01 [m/s2]
Orientáció
0
2*π/180 [rad]
Szögsebesség
0,01 [1/s]
0,01 [1/s]
Szöggyorsulás
0
0
Rotorszögsebesség
0
5*2π [rad/s]
* Belső paraméter a Kalman-szűréshez
Ezek alapján határoztam meg a kiterjesztett Kalman-szűrők paramétereit. A szimulációk 10ms-os fix lépésközzel történtek, ugyanis ez a szabályozás elvárt működési sebessége. Megjegyzendő még, hogy a célprocesszorra való fejlesztéskor Simulink → Real Time Workshop → Target Compiler sorrendben lehet áttenni a kifejlesztett programot a Simulink alól, azonban ehhez előírás a fix lépésköz és az algebrai hurkoktól mentes modell. A szimulációs eredmények emiatt mindig fix lépésköz mellett készültek.
33
A rendszer struktúrája az alábbi: Követendö pálya
pálya Pályakövetés Ref.-jelek Becsült jelek
Ref.-jelek
Szabályozás (Backstepping)
Pozíció, orientáció W(rotor)
W(rotor)
Helikoptermodell Mért jelek
Becsült jelek
Állapotbecslö Becsült jelek
Mért jelek
19. ábra. Pályakövetés és állapotbecslés - blokkábra
Pozíció
1
Orientáció Kalman-szürö Gyorsulás -> Pozíció, Sebesség w
mért jelek
Pozíció
a
v
a Orientáció A szenzorjelek transzformálása a helikopter koordinátarendszerébe w a_trans beta
a
1 becsült jelek
Orientáció
Orientáció
Kalman-szürö w Szögsebesség -> Orientáció, Szögsebesség w
beta
20. ábra. Pályakövetés és állapotbecslés - az állapotbecslő felépítése
34
Szabályos ötszög pálya esetén a következő eredmények születtek:
21. ábra. Ötszög alakú pálya - térbeli pálya
22. ábra. Ötszög alakú pálya - pálya-idő függvény
23. ábra. Ötszög alakú pálya - orientáció-idő függvény
24. ábra. Ötszög alakú pálya - a nyomatékok és a felhajtóerő a szimuláció során
35
Az eredmények spirálpálya esetén:
25. ábra. Spirálmozgás - térbeli pálya
26. ábra. Spirálmozgás - pálya-idő függvény
27. ábra. Spirálmozgás - orientáció-idő függvény
28. ábra. Spirálmozgás - a nyomatékok és a felhajtóerő a szimuláció során
36
5.5.3. A Kalman-szűrők által kiadott jelek és a valódi értékek összehasonlítása
29. ábra. Állapotbecslés - pozíció
30. ábra. Állapotbecslés - orientáció
31. ábra. Állapotbecslés - sebesség
5.5.4. Az eredmények értékelése A térbeli ábrákon jól látható, hogy a helikopter képes követni az előírt pályát, akár egyenes, akár görbe vonalú (a második pálya esetén a ritkábban elhelyezkedő sarokpontok miatt tér el a helikopter jobban az ideálistól, amely a térbeli ábrákon más színnel van jelölve). A pozíció—idő grafikonokról le lehet olvasni, hogy a szabályozó jobban teljesít, amikor a helikopternek nem kell a magasságát változtatnia, emiatt az ilyen szakaszokat érdemes kevesebb sarokponttal megadni. Látható, hogy a rotorok szögsebességei és a felhajtóerő több igen keskeny tüskét tartalmaz. Ez azonban sem a pozícióban, sem pedig az orientációban nem mutatkozik
37
meg, hiszen a helikopter dinamikája (és a valóságban a propellereké is) a meglévő tehetetlenségek miatt ezeket kiszűri. Az eredményekből látszik az is, hogy a Kalman-szűrők megfelelő pontosságú becslést adnak a fent látható mennyiségekre.
6. Összefoglalás A dolgozatban három különböző szabályozást elemeztem, amellyel a helikopter elméletileg szabályozható. Ezek hatékonyságát összehasonlítottam, és kiválasztottam közülük azt, amelyik a legjobban teljesített, és ezt fejlesztettem tovább. Ez a szabályozás a visszalépéses technikán (backstepping) alapul. A későbbiekben bemutattam, hogyan lehet a rendszert alkalmassá tenni arra, hogy egy előre definiált pályát kis hibával képes legyen a helikopter követni. A dolgozat utolsó részében pedig állapotbecslőt építettem a rendszerbe, amely kizárólag azon mennyiségek alapján következtet a helikopter állapotára, amelyek a valóságban is mérhetők. A dolgozat felépítésekor igyekeztem a tervezési folyamatnak megfelelő sorrendben tárgyalni a felmerülő feladatokat. Az irányítási rendszer megvalósítása és tesztelése MPC555 célprocesszoron további kutató-fejlesztő munkám részét képezi a BME IIT tanszéken. A kutatás illeszkedik az MTA SZTAKI és BME IIT közös kutatómunkájához, amely keretében korábbi diplomatervekben már megvalósult a motorvezérlő mintapéldányának és a markerbázisú mozgásanalízis képfeldolgozó rendszerének kifejlesztése, amelyhez illeszkedik a dolgozatban bemutatott irányítási rendszer beágyazott megvalósítása.
38
7. Ábrajegyzék 1. ábra. Gyroplane No. 1, Louis and Jacques Breguet, 1907
5
2. ábra. Convertawings Model "A" Quadrotor, Amityville, 1956
5
3. ábra. A négyrotoros helikopter szerkezete
6
4 ábra. PID szabályozás - blokkvázlat
14
5. ábra. PID szabályozás - A szabályozóegység belső felépítése
15
6. ábra. PID szabályozás - térbeli pálya
16
7. ábra. PID szabályozás - pálya-idő függvény
16
8. ábra. PID szabályozás - orientáció-idő függvény
16
9. ábra. Backstepping - blokkvázlat
17
10. ábra. Backstepping - a szabályozó felépítése
18
11. ábra. Backstepping - térbeli pálya
21
12. ábra. Backstepping - pálya-idő függvény
21
13. ábra. Backstepping - orientáció-idő függvény
21
14. ábra. Backstepping – a rotorok szögsebességei a szimuláció során
21
15. ábra. Csúszó szabályozás - térbeli pálya
23
16. ábra. Csúszó szabályozás - pálya-idő függvény
24
17. ábra. Csúszó szabályozás - orientáció-idő függvény
24
18. ábra. Csúszó szabályozás - a rotorok szögsebességei a szimuláció során
24
19. ábra. Pályakövetés és állapotbecslés - blokkábra
34
20. ábra. Pályakövetés és állapotbecslés - az állapotbecslő felépítése
34
21. ábra. Ötszög alakú pálya - térbeli pálya
35
22. ábra. Ötszög alakú pálya - pálya-idő függvény
35
23. ábra. Ötszög alakú pálya - orientáció-idő függvény
35
24. ábra. Ötszög alakú pálya - a nyomatékok és a felhajtóerő a szimuláció során
35
25. ábra. Spirálmozgás - térbeli pálya
36
26. ábra. Spirálmozgás - pálya-idő függvény
36
27. ábra. Spirálmozgás - orientáció-idő függvény
36
28. ábra. Spirálmozgás - a nyomatékok és a felhajtóerő a szimuláció során
36
29. ábra. Állapotbecslés - pozíció
37
30. ábra. Állapotbecslés - orientáció
37
31. ábra. Állapotbecslés - sebesség
37
39
8. Irodalomjegyzék [1]
Castillo P., Lozano R., Dzul A.E.: Modelling and Control of Mini-Flying Machines
[2]
Bouabdallah S., Murrieri P., Siegwart R.: Design and Control of an Indoor Micro Quadrotor. Proceedings of the 2004 IEEE International Conference on Robotics and Automation, New Orleans, April 2004.
[3]
Bouabdallah S., Siegwart R.: Backstepping and Sliding-mode Techniques Applied to an Indoor Micro Quadrotor. Proceedings of the 2005 IEEE International Conference on Robotics and Automation, Barcelona, Spain, 2005.
[4]
Lantos B.: Irányítási rendszerek elmélete és tervezése II. Korszerű szabályozási rendszerek. Akadémiai Kiadó, Budapest, 2003, pp. 9-44.
[5]
Lantos B.: Irányítási rendszerek elmélete és tervezése II. Korszerű szabályozási rendszerek. Akadémiai Kiadó, Budapest, 2003, pp. 201-203.
[6]
Lantos B.: Irányítási rendszerek elmélete és tervezése II. Korszerű szabályozási rendszerek. Akadémiai Kiadó, Budapest, 2003, p. 381.
[7]
Lantos B.: Beltéri autonóm rendszerek képfeldolgozáson és inerciális érzékelőkön alapuló
állapotbecslése.
Kutatási
beszámoló,
BME
Irányítástechnikai
és
Informatikai Tanszék, 1997, pp. 1-25. [8]
Soumelidis A., Gáspár P., Lantos B., Bauer P., Prohászka Z.: Design of an Embedded Microcomputer Based Mini Quadrotor UAV. In: Tzafestas SG , Antsakis PJ (ed.) Proceedings of the European Control Conference ECC ’07, Kos, Greece, 2-5 July 2007, pp. 2236-2241.
[9]
Molnár Cs.: BLDC motor szenzor nélküli vezérlése és fordulatszám-szabályozása négyrotoros helikopter meghajtásához. Diplomaterv, BME Irányítástechnikai és Informatikai Tanszék, 2007.
[10] Matusik Sz.: Valósidejű pozíció- és orientáció-meghatározó modul vezető nélküli négyrotoros
modellhelikopter
beltéri
üzeméhez.
Irányítástechnikai és Informatikai Tanszék, 2007.
40
Diplomaterv,
BME
