Designspecifikation. Joachim Lundh TSRT10 - SEGWAY 6 december 2010 Version 1.0. Status:

Designspecifikation Joachim Lundh TSRT10 - SEGWAY 6 december 2010 Version 1.0 Status: Granskad Alla 6 december 2010 Godkänd Markus (DOK) 6 december 2010

PROJEKTIDENTITET Segway, HT 2010 Tekniska högskolan vid Linköpings universitet, ISY Namn Ansvar Tele f on E post Joachim Lundh Projektledare (PL) 070-293 29 66 joalu331 Markus Sundbrandt Dokumentansvarig (DOK) 073-955 12 23 marsu752 David Elofsson Designansvarig hårdvara (DES) 070-292 89 80 davel369 Pontus Jernberg Bluetoothansvarig (BLT) 070-939 16 15 ponje141 Karl Cederman Regulatoransvarig (REG) 070-565 56 67 karjo492 Henrik Hagelin Testansvarig (TST) 070-898 95 06 henha374 Dan Gunning Följningsansv (FLJ) 073-697 47 51 dangu444 Håkan Richter Observatöransvarig (OBS) 073-532 47 95 hakri323 E-postlista för hela gruppen: tsrt10segway@googlegroups.com Kund: David Törnqvist, ISY kundtelefon: +46 13 281882, e-postadress: tornqvist@isy.liu.se Beställare: Patrik Axelsson, +46 13 284474, axelsson@isy.liu.se Handledare: Ylva Jung, +46 13 284474, ylvju@isy.liu.se Kursansvarig: David Törnqvist, +46 13 281882, tornqvist@isy.liu.se 2

Innehåll 1 Inledning och bakgrund 6 1.1 Syfte och mål......................................... 6 1.2 Användning.......................................... 6 1.3 Översikt av systemet..................................... 6 2 Kommunikation och användargränssnitt 7 2.1 Delsystem 1 Kommunikation................................. 7 2.1.1 Ingående komponenter................................ 7 2.1.2 Gränssnitt....................................... 7 2.1.3 Datapaket....................................... 11 3 Följning och undvikning av hinder 12 3.1 Delsystem 2 Handföljning.................................. 12 3.1.1 Ingående komponenter................................ 12 3.1.2 Gränssnitt....................................... 12 3.1.3 Flödesschema..................................... 13 3.2 Delsystem 3 Linjeföljning.................................. 14 3.2.1 Ingående komponenter................................ 14 3.2.2 Gränssnitt....................................... 14 3.2.3 Kalibrering och tröskling............................... 15 3.2.4 Flödesschema..................................... 15 3.2.5 Reglering....................................... 16 3.3 Delsystem 4 Undvika hinder................................. 16 3.3.1 Ingående komponenter................................ 17 3.3.2 Gränssnitt....................................... 17 3.3.3 Flödesschema..................................... 17 4 Observatör 19 4.1 Mätbara tillstånd....................................... 19 4.2 Gyrosignaler......................................... 20 4.3 Observatör för balansering.................................. 21 4.3.1 Tillstånd....................................... 21 4.3.2 Systemmodeller................................... 21 4.3.3 Observerbarhet.................................... 21 4.4 Observatör för girvinkel.................................... 21 4.4.1 Tillstånd....................................... 22 4.4.2 Systemmodeller................................... 22 4.4.3 Observerbarhet.................................... 22 4.5 Implementation........................................ 22 4.5.1 Observatör för balansering.............................. 22 4.5.2 Observatör för girvinkel............................... 23 4.5.3 Observatörsförstärkning............................... 23 3

5 Regulator 24 5.1 LQG.............................................. 24 5.2 Implementation för LQG................................... 25 5.3 H............................................... 25 5.4 Implementation för H.................................... 26 4

Dokumenthistorik Version Datum Utförda förändringar Utförda av Granskad 0.1 2010-10-07 Första utkast Alla Alla 0.2 2010-10-12 Andra utkast Alla Alla 0.3 2010-10-14 Tredje utkast PL, DOK Alla 1.0 2010-10-15 Div. små ändringar PL, DOK Alla 5

Kapitel 1 Inledning och bakgrund Detta dokument är en specifikation över hur ett system för en segway, byggd i Lego Mindstorms, ska fungera. Dokumentet beskriver systemet först på en övergripande nivå och sedan på detaljnivå där varje delsystem beskrivs. 1.1 Syfte och mål Syftet med detta projekt är att reglera en segway. Den ska bland annat kunna hållas stående upprätt, följa en referensbana samt styras manuellt. Projektets mål är att roboten ska ha minst två olika regulatorstrukturer för att hålla den stående upprätt. Den skall även kunna följa en linje på golvet, kunna följa en hand, undvika hinder, undvika att köra ner från ett stup samt kunna styras manuellt med hjälp av bluetooth. Regulatorer m.m. implementeras i Simulink. 1.2 Användning Roboten ska användas till att utvärdera olika regulatorstrukturer samt möjligheter till bluetoothkommunikation och olika följningar. Följningarna som ska implementeras är handföljning och linjeföljning. Roboten ska dessutom kunna styras manuellt via bluetooth. 1.3 Översikt av systemet Systemet kommer delas in i fem olika delar: bluetooth, handföljning, linjeföljning, undvikning av hinder och regulator för att hålla roboten upprätt. Med hjälp av bluetooth sker kommunikationen med roboten såsom dataloggning och manuella styrsignaler. Med bluetooth görs även valet av vilket delsystem som ska vara aktivt. En översikt av systemet ses i figur 1.1. Figur 1.1: Översikt av systemet i dess omgivning. 6

Kapitel 2 Kommunikation och användargränssnitt Detta kapitel beskriver det delsystem som har hand om kommunikationen mellan dator och robot. Här återfinns även en beskrivning av det gränssnitt som kommer användas för att kontrollera roboten. 2.1 Delsystem 1 Kommunikation Detta system tar hand om kommunikationen mellan datorn och roboten. Datorn ska via bluetooth hela tiden logga vad roboten gör och visa detta i ett program. Från detta program kan användaren välja vilken mod som ska vara aktiv samt vilken av regulatorstrukturerna som ska användas. En skiss över kommunikationssystemet ses i figur 2.1. Figur 2.1: Systemskiss av delsystemet kommunikation. 2.1.1 Ingående komponenter Delsystemet kräver, förutom roboten och dator, tillgång till en bluetoothadapter. Adaptern som kommer att användas är en MINDSTORMS Bluetooth Dongle [3]. Kommunikationen mellan dator och robot kommer ske mellan robotens inbyggda bluetoothadapter och donglen som är ansluten till datorn. 2.1.2 Gränssnitt För att en användare enkelt ska kunna kommunicera med roboten så krävs dock ett väldefinierat gränssnitt. Gränssnittet ska ta emot sensordata som roboten skickar via bluetooth till datorn. Från datorn ska användaren kunna växla aktiv regulator och aktiv mod. Då roboten är i mod för manuell styrning ska man från gränssnittet även kunna styra roboten. Vid linjeföljning ska även ljussensorerna kunna kalibreras via gränssnittet. De sensordata som kommer presenteras i gränssnittet är robotens vridnings- och lutningsvinkel (definierade enligt figur 4.1), uppmätt avstånd framåt från ultraljudssensorn, uppmätt ljusintensitet från ljussensorn samt hjulens hastigheter. Dessa värden kommer presenteras genom att plottas i grafer samt genom att visa deras momentana värde, en prototyp för hur användargränssnittet är tänkt att se ut ses i figur 2.2. Implementationen av gränssnittet kommer att ske i MATLAB. 7

Figur 2.2: Prototyp av det grafiska användargränssnittet. 8

Utöver att presentera sensordata så kommer programmet att vidarebefordra sensorvärdena till en styralgoritm enligt vald mod. Styralgoritmerna kommer sedan att ge referensvärden för robotens vridnings- och lutningsvinklar. Dessa referensvärden skickas sedan till roboten. De olika styralgoritmerna beskrivs utförligare i avsnitt 3.1, 3.2 samt 3.3. I figur 2.3 ses även ett flödesschema över hur gränssnittet är tänkt att fungera. Ett flödesschema över hur kommunikationen är tänkt att fungera i roboten syns i figur 2.4. Figur 2.3: Flödesschema över delsystemet kommunikation på PC. 9

Figur 2.4: Flödesschema över delsystemet kommunikation i robot. 10

2.1.3 Datapaket De datapaket som kommer skickas från roboten till datorn är sensordata och styrsignal. De data som kommer skickas från datorn till roboten är aktuell regulator och mod samt eventuell styr- eller kalibreringskommando för manuell körning. Tabell 2.1 beskriver formatet på de datapaket som skickas från roboten till PC och tabell 2.2 beskriver på samma sätt de datapaket som skickas från PC till robot. Tabell 2.1: Datapaket från robot till PC Systemtid Left motor Right motor Gyro Ultraljudssensor Ljussensor Ledig plats 4 B 4 B 4 B 2 B 1 B 2 B 15 B Tabell 2.2: Datapaket från PC till robot Regulator Mod Kalibrering Manuell referens Ledig plats 1 B 1 B 1 B 2 B 27 B Datatypen för variablerna i datapaketen beskrivs i tabell 2.3. Tabell 2.3: Datatyper Robot till PC Systemtid uint32 Left motor int32 Right motor int32 Gyro int16 Ultraljud uint8 Ljussensor uint8 PC till robot Regulator uint8 Mod uint8 Kalibrering uint8 Manuell referens uint16 11

Kapitel 3 Följning och undvikning av hinder I detta kapitel beskrivs de tre delsystemen handföljning, linjeföljning och undvikning av hinder. Om en hand befinner sig framför ultraljudssensorn ser delsystemet handföljning till att roboten håller ett angivet avstånd till handen. Delsystemet linjeföljning ser till att roboten följer en tejpremsa markerad på underlaget. Skulle roboten närma sig ett stup eller en vägg ser delsystemet undvikning av hinder till att den stannar på tillräckligt avstånd från hindret. 3.1 Delsystem 2 Handföljning Handföljning innebär att roboten ska klara av att följa en hand på ett givet avstånd. Om man flyttar handen framåt eller bakåt ska roboten hela tiden hålla samma avstånd och på så sätt kan man styra hur och vart roboten ska åka. Avståndet avläses med hjälp av en avståndssensor och avståndet mellan handen och roboten ska vara mellan 0 till 25 cm för att roboten ska följa den. En skiss över systemet för handföljning ses i figur 3.1. För att veta hur man ska styra kommer en regulator behövas. Regulatorn kommer designas enligt samma princip som i??. Figur 3.1: Systemskiss av delsystemet handföljning. 3.1.1 Ingående komponenter Detta delsystem kräver tillgång till en ultraljudssensor. Den gör att roboten kan se och detektera objekt. I detta delsystem kommer ultraljudssensorn att användas för att detektera samt mäta avståndet till handen. Sensorn ger avståndsvärden från 0 till 255 cm med en precision på +/- 3 cm. Den mäter avståndet genom att beräkna tiden det tar för en ljudvåg att träffa ett objekt och returnera till sensorn (som ett eko). Ultraljudssensorn kan maximalt göra 50 avståndsavläsningar per sekund. Stora, plana och hårda objekt ger bättre avläsningar än små, kurviga och mjuka objekt, [3]. 3.1.2 Gränssnitt Roboten kommer att använda ultraljudssensorn för att mäta avståndet till handen. Om handen befinner sig på ett avstånd som är mindre än önskat så kommer roboten backa till dess att avståndet är det önskade. Motsvarande princip gäller då avståndet är större än önskat. Om handen skulle föras längre bort än 25 cm från sensorn ska roboten stanna och hålla sig upprätt på stället. Roboten läser in värden från sensorn, dessa behandlas, sedan skickas en referenssignal till delsystemet regulator. Skulle minnesproblem uppstå skickas sensorvärdena istället till datorn där de behandlas, sedan skickas en referenssignal till delsystemet regulator. 12

3.1.3 Flödesschema I figur 3.2 visas flödesschemat för delsystemet handföljning. I detta delsystem är undvikning av hinder ej aktivt. Vid starten står roboten still och väntar på att ultraljudssensorn ska detektera en hand. När en hand har detekteras så kontrolleras om avståndet är för långt, för kort eller korrekt. Den referenssignal på robotens lutning som skickas till regulatorn beror på detta avstånd. Figur 3.2: Flödesschema över delsystemet handföljning. 13

3.2 Delsystem 3 Linjeföljning Roboten ska klara av att följa en bestämd bana på marken som markeras med en linje i form av en tejpremsa. För att linjeföljning ska vara möjlig är roboten utrustad med en sensor som läser av hur tejpremsan är dragen. Hur bra roboten ska klara av att följa linjen samt vilka färdvägar som är möjliga specificeras i kravspecifikationen. En skiss över linjeföljningssystemet ses i figur 3.3. Figur 3.3: Systemskiss av delsystemet linjeföljning. 3.2.1 Ingående komponenter Detta delsystem kräver tillgång till en ljussensor. Den gör att roboten kan mäta reflekterad ljusintensitet hos färgade ytor. Den kommer att användas för att detektera om roboten befinner sig på tejp och i sådana fall vilken färg den har. Sensorn ger intensitetsvärden mellan 0 och 100%, enligt [3]. 3.2.2 Gränssnitt Roboten kommer att använda sig av en ljussensor för att registrera linjen som är en tejpremsa i tre färger, se figur 3.4 för ett exempel på hur en sådan kan se ut. För att linjeföljningen ska fungera korrekt måste det ljusa underlaget vara till vänster och det mörka vara till höger. Ljussensorn ger ett intensitetsvärde beroende på underlaget. Ett ljust underlag har hög reflektivitet och får ett högt intensitetsvärde, ett mörkt underlag har låg reflektivtet och får därmed ett lågt värde. Beroende på om sensorvärdet är högt eller lågt kan vi avgöra om roboten ska korrigera åt höger eller vänster, ligger värdet mellan dessa så ska roboten hålla samma kurs. Roboten läser in värden från sensorn, dessa behandlas, sedan skickas en referenssignal till delsystemet regulator. Skulle minnesproblem uppstå skickas sensorvärdena istället till datorn där de behandlas, sedan skickas en referenssignal till delsystemet regulator. Figur 3.4: Utseendet på linjen, pilen markerar robotens färdriktning. 14

3.2.3 Kalibrering och tröskling Innan man kan börja följa linjen måste en kalibrering av varje färg utföras, detta görs med hjälp av gränssnittet på PC:n. För att minimera inverkan av felaktiga sensorvärden sätts trösklar mellan de olika kalibreringsvärdena, se ett exempel i figur 3.5. I exemplet är kalibreringsvärdena 50%, 40% och 30%. Genom att beräkna medelvärdet av de närliggande färgerna fås trösklarna 45% och 35%. Sensorvärden över 45% tolkas sedan som vitt, värden under 35% tolkas som svart och mellanliggande värden tolkas som grågrönt. Figur 3.5: Svarta siffror är kalibreringsvärden för respektive färg, röda siffror är tröskelvärden. Värdena är angivna i procent. 3.2.4 Flödesschema I figur 3.6 visas flödesschemat för delsystemet linjeföljning. I detta delsystem är undvikning av hinder ej aktivt. Vid starten är roboten kalibrerad, står centrerad över linjen och inväntar startsignal från användaren. Ljussensorn läser sedan, med jämna intervall, av den reflekterade ljusintensiteten hos underlaget. Referenssignalen på robotens lutning och vridning som skickas till regulatorn beror på intensitetsvärdet. Ett högt värde ger en referenssignal om att vrida roboten åt höger, ett lågt värde ger vridning åt vänster och ett mellanliggande värde ger att roboten ska fortsätta köra i samma riktning. Figur 3.6: Flödesschema över delsystemet linjeföljning. 15

3.2.5 Reglering För att roboten ska följa linjen krävs en regulator. Vi har valt att använda oss av en PID-regulator eftersom den är relativt enkel att implementera och fungerar bra för denna tillämpning. En PID-regulator reglerar på tre sätt: P - Proportionell förstärkning, K p I - Integrerande förstärkning, K i D - Deriverande förstärkning, K d Formeln för en PID-regulator är: u(t) = K p e(t)+k t i 0 e(τ)dτ +K d de(t) dt, se figur 3.7 nedan. Derivatan beräknas med hjälp av en approximation. Figur 3.7: Beskrivning av PID-regulator med referenssignal r, reglerfel e, styrsignal u, och utsignal y. P-delen ger snabbare reglering men ofta även ett stationärt fel. Detta fel samt stegstörningar kan elimineras med hjälp av I-delen vilken i sin tur kan göra systemet mer oscillativt. D-delen minskar överslängar men gör systemet mer känsligt för mätbrus, se [2]. PID-regulatorn beräknar och skickar en referenssignal u till delsystemet regulator. Beräkningen görs genom att bilda reglerfelet e = r - y, där r är en referens på önskad hjulvinkel och y är nuvarande hjulvinkel. Reglerfelet används i formeln för PID-regulatorn ovan. Det största värde referenssignalen u kan anta beräknas genom att subtrahera kaliberingsvärdet för det mörka underlaget från värdet för det ljusa underlaget, styrsignalens minsta värde är 0. Beroende på referenssignalens värde svänger roboten olika mycket åt höger (höga värden) respekvive vänster (låga värden). 3.3 Delsystem 4 Undvika hinder Detta systems uppgift är att undvika hinder som uppkommer under körning. Det finns två sorters hinder, väggar och stup. Vilken eller vilka algoritmer för att undvika hinder som används beror på vilken mod roboten befinner sig i. Roboten ska upptäcka när den närmar sig ett stup framifrån och stanna en bit innan kanten så att den kan röra sig lite för att hålla sig upprätt utan att ramla ner. För att undvika att köra in i en eventuell vägg framifrån ska roboten stanna en bit innan väggen. På så sätt kan den hålla sig upprätt utan att köra in i väggen. Undvika hinder är varken aktivt vid linjeföljning eller handföljning. En skiss över systemet för att undvika hinder ses i figur 3.8. 16

Figur 3.8: Systemskiss av delsystemet undvika hinder. 3.3.1 Ingående komponenter Detta delsystem kräver tillgång till en ultraljudssensor och en ljussensor, se 3.1.1 och 3.2.1. 3.3.2 Gränssnitt För att upptäcka och undvika eventuella väggar kommer roboten att använda sig av en ultraljudssensor. Då avståndet till en framförliggande vägg är mindre än 10-20 cm kommer roboten att stanna och stå upprätt. När roboten kör på ett enfärgat underlag används ljussensorn för att upptäcka och undvika stup. Närmar roboten sig ett stup kommer sensorvärdet att avvika från sitt normala intervall. När en vägg eller ett stup upptäcks ska man, oavsett signaler från användaren om att köra framåt eller svänga, endast kunna backa. Roboten läser in värden från sensorn, dessa behandlas, sedan skickas en referenssignal till delsystemet regulator. Skulle minnesproblem uppstå skickas sensorvärdena istället till datorn där de behandlas, sedan skickas en referenssignal till delsystemet regulator. 3.3.3 Flödesschema I figur 3.9 nedan visas flödesschemat för delsystemet undvika hinder. Detta delsystem är endast aktivt i manuell mod. Vid starten kör roboten runt i manuellt läge. Både ljussensorn och ultraljudssensorn är aktiverade och data skickas med jämna mellanrum till gränssnittet på datorn. Om ljussensorn detekterar ett stup eller om ultraljudssensorn detekterar en vägg ska roboten stanna och användaren tillåts endast att backa. Vid ankomst tills stup backar roboten dessutom automatiskt ca en decimeter för att få lite extra spelrum för upprätthållningen. 17

Figur 3.9: Flödesschema över delsystemet undvika hinder. 18

Kapitel 4 Observatör Observatören ska designas så att regulatorn får så bra signaler som möjligt att reglera på, det handlar alltså om att reducera brus i mätsignaler och skatta tillstånd som inte kan mätas. Observatören bygger på en fysikalisk modell som beskriver en sådan segway-robot som projektet nyttjar. Modellen är hämtad från [4] och bygger på antagandet att systemet kan modelleras som en inverterad pendel på två hjul. Observatören återkopplas med de tillstånd som kan mätas för att observatörens skattningsfel kontinuerligt ska minimeras. Förstärkningen på återkopplingen är en avvägning mellan brusundertryckning och snabbhet. Designen delas upp i två delar där den ena behandlar skattningen av värden som är till för LQ-regleringen till robotens balanssystem och den andra behandlar skattningen av värden som används för att roboten ska åka rakt. 4.1 Mätbara tillstånd Från robotens gyro och servon kan man mäta lutningsvinkelhastighet och motorvinklar. Höger motorvinkel: θ mr, vinkeln på det högra servot. Vänster motorvinkel: θ ml, vinkeln på det vänstra servot. Lutningsvinkelhastighet: ψ, lutningen på roboten. Utifrån dessa mätbara storheter skapas med enkla beräkningar andra storheter definierade av geometrin i figur 4.1 som kommer att användas som indata i observatörernas tilltåndsmodeller. Höger hjulvinkel: θ r, vinkeln på det högra hjulet. Då utväxlingsförhållandet mellan hjul och motor är n = 1 så blir θ r = θ mr n + ψ = θ mr + ψ (4.1) Vänster hjulvinkel: θ l, vinkeln på det vänstra hjulet. Då utväxlingsförhållandet mellan hjul och motor är n = 1 så blir θ l = θ ml n + ψ = θ ml + ψ (4.2) Snitthjulvinkel: θ, den genomsnittliga vinkeln på hjulen. θ = θ r + θ l 2 = θ m r + θ ml 2 + ψ = θ m + ψ (4.3) Girvinkel: ϕ, vinkeln med vilken roboten roterat med. ϕ = R W (θ r θ l ) = R W ( θmr θ ml ) (4.4) Man kan konstatera att man inte kommer kunna göra en förbehandling för att använda θ, ψ och ϕ för återkoppling i observatören. Detta på grund av att man inte kan mäta ψ utan enbart har tillgång till ψ. ψ är dock ett tillstånd i systemet och kan skattas med observatören. De signaler, utöver styrsignalerna, som kommer vara insignal till observatörerna är θ m, ψ och ϕ. 19

Ψ φ θ θ m W/2 R Figur 4.1: Definition av vinklar 4.2 Gyrosignaler Gyron har en tendens att driva lite under gång, detta betyder att man om det blir en påtagligt drift vill kompensera för detta. För att kompensera för driften kan man utöka observatören med ett ytterligare tillstånd. Gyrodriften är proportionell mot det stationära fel som erhålls vid kalibreringen. Gyrots signal är förskjuten så att 0rad/s motsvarar ca 600 i mätsignal och inte noll. 20

4.3 Observatör för balansering Detta avsnitt beskriver observatören som används för balansering. 4.3.1 Tillstånd De tillstånd som observatören för balansering behöver skatta är snitthjulvinkeln, snitthjulvinkelhastigheten, lutningsvinkeln och lutningsvinkelhastigheten. Snitthjulvinkel: θ, den genomsnittliga vinkeln mellan höger och vänster hjul. Snitthjulvinkelhastighet: θ, den genomsnittliga vinkelhastigheten mellan höger och vänster hjul. Lutningsvinkel: ψ, den vinkel med vilken roboten lutar Lutningsvinkelhastighet: ψ, den vinkelhastighet med vilken robotens lutningsvinkel förändras. Den skattade tillståndsvektorn, systemets in- och utsignal har följande utseende: 4.3.2 Systemmodeller ˆθ ˆx 1 = ˆψ ˆθ u = ˆψ Tillståndsmodell över systemet ser ut som följande [ vl v r ] y 1 = [ ] θṁ ψ ẋ 1 = A 1 x 1 + B 1 u y 1 = C 1 x 1 + D 1 u (4.5) där 0 0 1 0 A 1 = 0 0 0 1 0 409, 7184 162, 1273 162, 1273 B 1 = 0 269, 6273 78, 1496 78, 1496 C 1 = [ 1 1 0 ] 0 0 0 0 1 0 0 0 0 157, 5798 157, 5798 75, 9576 75, 9576 D 1 = [ ] 0 0 0 0 De numeriska värdena i matriserna kommer ifrån fysikaliska samband beskrivna i [4] med instoppade värden på samtliga parametrar. 4.3.3 Observerbarhet För att överhuvudtaget kunna göra en observatör krävs att systemet är observerbart, dvs. att man kan observera alla tillstånd. C 1 O 1 = C 1 A 1 C 1 A 2, (n = dim(x 1) = 4) 1 C 1 A 3 1 Om rang(o) = n så är systemet observerbart. För segway-roboten är detta fallet. 4.4 Observatör för girvinkel Denna del beskriver observatören för girvinkel. (4.6) 21

4.4.1 Tillstånd De tillstånd som observatören för girvinkel behöver skatta är girvinkeln och girvinkelhastigheten. Girvinkel: ϕ, rotationen kring robotens vertikalaxel. Denna vinkel ska styras mot noll när man kör rakt fram. Girvinkelhastighet: ϕ, rotationsvinkelhastigheten kring robotens vertikalaxel. Den skattade tillståndsvektorn, systemets in- och utsignal har följande utseende: [ [ ] ˆϕ ˆϕ] vl ˆx 2 = u = y 2 = ϕ 4.4.2 Systemmodeller Tillståndsmodell över systemet ser ut som följande v r ẋ 2 = A 2 x 2 + B 2 u y 2 = C 2 x 2 + D 2 u (4.7) där [ ] 0 1 A 2 = 0 97, 0090 [ ] 0 0 B 2 = 53, 8789 0 C 2 = [ 1 0 ] D 2 = [ 0 0 ] (4.8) De numeriska värdena i matriserna kommer ifrån fysikaliska samband beskrivna i [4] med instoppade värden på samtliga parametrar. 4.4.3 Observerbarhet På samma sätt som i 4.3.3 kan man visa att systemet är observerbart. 4.5 Implementation Implementeringen delas upp i två delar. Den ena delen behandlar skattningen av värden som är till för LQregleringen för robotens balanssystem och den andra skattningen av värden som används för att roboten inte ska driva från tänkt körriktning. Teori för observatör hämtad från [2]. 4.5.1 Observatör för balansering Alla matriser och storheter i detta stycke är hämtade från definitionerna i kapitel 4.3. Observatören beskrivs av följande uttryck: ˆx 1 = A 1 ˆx 1 + B 1 u + K 1 (y 1 C 1 ˆx 1 ) (4.9) För att uttrycket ovan ska kunna användas i ett tillståndsblock i Simulink görs följande omskrivning. ˆx 1 = A 1obs ˆx 1 + B 1obs u 1obs y 1obs = C 1obs ˆx 1 + D 1obs u 1obs (4.10) där A 1obs = [A 1 K 1 C 1 ] B 1obs = [B 1,K 1 ] C 1obs = I 4x4 D 1obs = 0 4x4 u 1obs = [ ] u y 1 (4.11) 22

4.5.2 Observatör för girvinkel Alla matriser och storheter i detta stycke är hämtade från definitionerna i kapitel 4.4. Observatören beskrivs av följande uttryck: ˆx 2 = A 2 ˆx 2 + B 2 u + K 2 (y 2 C 2 ˆx 2 ) (4.12) För att uttrycket ovan ska kunna användas i ett tillståndsblock i Simulink görs följande omskrivning. ˆx 2 = A 2obs ˆx 2 + B 2obs u 2obs y 2obs = C 2obs ˆx 2 + D 2obs u 2obs (4.13) där A 2obs = [A 2 K 2 C 2 ] B 2obs = [B 2,K 2 ] C 2obs = I 2x2 D 2obs = 0 2x3 u 2obs = [ ] u y 2 (4.14) 4.5.3 Observatörsförstärkning Inga sensorer levererar brusfria signaler. Två metoder för att bestämma observatörsförstärkningen kommer att testas och utvärderas. Den ena, Kalman-filtret, kräver att brusintensiteterna är kända. Dessa kan i bästa fall uppskattas och det kan vara tillräckligt för att observatören ska få adekvata egenskaper. Den andra, polplaceringsmetoden, bygger på att man ansätter en polplacering med poler på godtyckliga ställen och kräver inte någon mer förkunskap än förståelse för polernas inverkan på ett system. Kalman-filter Svårigheten med metoden ligger i de okända brusintensiteterna. Det är dock rimligt att anta att det brus som mätsignalerna utsätts för har ett konstant spektra och kan ses som vitt brus. Det är smidigt då man enkelt kan använda sig av ett kalmanfilter för att skapa observatören. Kalmanfiltret ges av förstärkningen K. { K = PC T R 1 2 AP + PA T + NR 1 N T PC T R 1 2 PC = 0 (4.15) Man uppskattar alltså R 1 och R 2 och löser Riccatti-ekvationen för att beräkna förstärkningen K. Polplaceringsmetoden Man bestämmer observatörens egenskaper genom att placera dess poler på godtyckliga ställen. Utgående från ekvation 4.9 väljer man en lämplig polplacering p(s) och beräknar sedan K med hjälp av Matlabkommandot place. Det är följande uttryck som place beräknar K ur. det(si (A KC)) = det(si A obs ) = p(s) Då erhåller man värden för elementen i förstärkningsvektorn. Man evaluerar sedan om de skattade tillstånden uppfyller kraven på följning och brusundertryckning. Är så inte fallet väljs en ny polplacering och en ny förstärkningsvektor beräknas. Detta moment upprepas tills en tillräckligt bra skattning uppnås. Avvägningen man får göra är mellan modellens snabbhet och brusundertryckning. 23

Kapitel 5 Regulator I detta kapitel kommer de två regulatorerna för att hålla roboten upprätt att beskrivas närmare. Roboten kommer bestå av en LQG- och en H -regulator. Från en dator kommer man kunna växla mellan dessa för att kunna jämföra dem. Robotens motorer styrs med pulsviddsmodulering (PWM). Detta kommer implementeras enligt [4]. I alla lägen som roboten ska färdas rakt fram krävs någon form av hjulsynkronisering. Detta löses genom att ställa referensvärdet r φ = 0 så att roboten inte driver. Så länge någon form av följning är aktivt kommer detta inte vara nödvändigt då roboten kommer förväntas svänga då omgivningen kräver det. 5.1 LQG Den första regulatorn som skall implementeras är en LQG (Linear Quadratic Gaussian) regulator. Teori till avsnittet är hämtat från [1]. Syftet med regulatorn är att minimera kriteriet: min( e 2 Q 1 + u 2 Q 2 ) = min e T (t)q 1 e(t) + u T (t)q 2 u(t)dt (5.1) Där e är reglerfelet och u är insignalerna. Observatören skattar tillstånden, vilka beskrivs i avsnittet om observatören. Antalet skattade tillstånd är fyra. Insignaler till regulatorn är de skattade tillstånden och referenssignalen. I regulatorn utförs en LQ-reglering. En översiktsbild över denna regulator finns i figur 5.1. Den optimala linjära regulatorn ges av följande ekvationer där y är robotens utsignaler. u(t) = L ˆx(t) (5.2a) ˆx(t) = A ˆx(t) + Bu(t) + K(y(t) C ˆx(t)) (5.2b) Utsignalen (u) från regulatorn är en spänning. Det omvandlas av ett senare system till en procentsats av maximal tillgänglig spänning i batteriet. I regulatorn måste Q 1 och Q 2 väljas. Q 1 bestämmer viktningen mellan de olika tillstånden, det vill säga reglerfelet mellan observerat tillstånd och referenssignal. Eftersom det är fyra tillstånd kommer dimensionen vara 4x4. Q 2 bestämmer viktningen mellan de olika insignalerna till systemet. I det här fallet är det bara en insignal och följaktligen är dimensionen 1. Förhållandet mellan Q 1 och Q 2 spelar också in då det bestämmer hur stora insignalerna ska vara i förhållande till reglerfelen. L är förstärkningen på återkopplingsmatrisen, det vill säga enligt ekvation (5.2). L beräknas enligt: L = Q 1 2 BT S (5.3) Med hjälp av Riccati-ekvationen beräknas S, där S är den entydiga, positivt semidefinita, symmetriska lösningen till ekvationen: A T S + SA + M T Q 1 M SBQ 1 2 BT S = 0 (5.4) 24

1 x_ref x_hat L Regulator u u Robot y y _obs u_obs Observatör Figur 5.1: Skiss över regulatorstrukturen för LQ-reglering. 5.2 Implementation för LQG Nedan beskrivs i vilken ordning arbetet kommer att utföras. 1. Bestäm Q 1 och Q 2. Q 2 sätts till 1. I Q 1 varieras det inbördes förhållandet mellan diagonalelementen. 2. Lös Riccatiekvationen (5.4). 3. Beräkna L (5.3). 4. Implementera i Simulink. 5. Simulera i simuleringsmiljö. 6. Analysera resultatet och om beteendet inte är önskvärt börja om på 1. 7. Testa på roboten. 5.3 H Teori för avsnittet hämtat från [1]. I figur 5.2 syns strukturen för systemet med H -reglering. Referenssignalen skickas in i F y tillsammans med utsignalen y. W u,w t och W s är viktfunktionerna som kommer utge kraven för regulatorn. Genom z 1, z 2 och z 3 kan man mäta hur väl kraven uppfylls. Med hjälp av den här regulatorstrukturen kan man till exempel välja vid vilka frekvenser mät- och systembrus skall straffas. Syftet med H -regulatorn är att minimera H -normen för G ec, enligt följande: där σ(g ec (iω)) är det största singulära värdet av G ec (iω). G ec = max ω σ(g ec(iω)) (5.5) G ec är överföringsfunktionen från w (systemstörning) till z (utsignal) från systemet, enligt: z = W ug wu W T T w = G ec w (5.6) W S S Det finns ett krav på H -normen för G ec som säger: G ec < γ (5.7) 25

Konsekvensen av (5.6) och (5.7) är: W S (iω)s(iω) < γ ω (5.8a) W T (iω)t (iω) < γ ω (5.8b) W u (iω)g wu (iω) < γ ω (5.8c) γ skall väljas så litet som möjligt. Om Riccatiekvationen saknar lösning så ökas γ tills en lösning hittas. Bara för att ett γ hittas som uppfyller kraven i (5.8) betyder inte det att kraven i (5.7) är uppfyllda. För de tre viktningsmatriserna sker viktningen primärt på diagonalelementen eftersom ingen eller endast en liten korskoppling antas föreligga. W S är viktningsmatrisen för känslighetsfunktionen (från systembrus till utsignal). Där känslighetsfunktionen är liten fås mycket mätbrus, därför ska känslighetsfunktionen vara liten där mätstörningarna är små. W S skall väljas så att frekvenser som inte roboten arbetar i straffas. Systembrus ligger ofta på lägre frekvenser, så om det är mycket systembrus vill man straffa låga frekvenser. W T är viktningsmatrisen för den komplementära känslighetsfunktionen. Förhållandet mellan känslighetsfunktionen S och den komplementära känslighetsfunktionen T är: T = I S (5.9) W u är viktningsmatrisen för G wu. Den bestämmer hur de olika styrsignalerna skall straffas vid olika frekvenser. Den bestämmer även viktningen mellan de olika styrsignalerna. Följande riccatiekvation löses. A T S + SA + M T M + S(γ 2 NN T BB T )S = 0 (5.10) Den har en positiv semidefinit lösning S = S γ som uppfyller att är stabil. Då kan L beräknas enligt följande: Regulatorn för H blir följande: A BB T S γ (5.11) L = B T S γ (5.12) ˆx(t) = A ˆx(t) + Bu(t) + N(y(t) C ˆx(t)) u(t) = L ˆx(t) (5.13a) (5.13b) 5.4 Implementation för H Nedan följer ordningen i vilket arbetet kommer att utföras. 1. Överföringsfunktionen för systemet, G, beräknas med hjälp av tillståndsbeskrivningen. 2. W u, W S och W T bestäms utifrån de krav som finns på känslighetsfunktionen, den komplementära känslighetsfunktionen samt överföringsfunktionen mellan w (systemstörning) och u (insignal). Dessa krav kommer tas fram genom att testa sig fram. 3. Ansätt ett γ. 4. Lös ekvation (5.10)-(5.12). Om ingen lösning finns prova att öka γ. 5. Implementera i Simulink. 6. Simulera i simuleringsmiljö. 7. Analysera resultatet och om beteendet inte är önskvärt börja om på 2. 8. Testa på roboten. 26

u x' = Ax+Bu y = Cx+Du W_u 1 z_1 1 ref x' = Ax+Bu y = Cx+Du -F_y u x' = Ax+Bu y = Cx+Du Robot x' = Ax+Bu y = Cx+Du W_T 2 z_2 w y x' = Ax+Bu y = Cx+Du W_S 3 z_3 Figur 5.2: Skiss över regulatorstrukturen för H -reglering. 27

Litteraturförteckning [1] Torkel Glad och Lennart Ljung. Reglerteori - Flervariabla och olilnjära metoder. Studentlitteratur, 2003. [2] Torkel Glad och Lennart Ljung. Reglerteknik - Grundlaggande teori. Studentlitteratur, 2006. [3] LEGO.com. What is nxt? http://mindstorms.lego.com/en-us/whatisnxt/default. aspx, 2010. [Online; accessed 29-September-2010]. [4] Mathworks.com. Nxtway-gs (self-balancing two-wheeled robot) controller design. http://www. mathworks.com/matlabcentral/fileexchange/19147, 2010. [Online; accessed 01-Maj- 2010]. 28