Teknisk Dokumentation

Transkript

1 Teknisk Dokumentation Joachim Lundh TSRT10 - SEGWAY 10 december 2010 Version 1.0 Status: Granskad Alla 10 december 2010 Godkänd Markus (DOK) 10 december 2010

2 PROJEKTIDENTITET Segway, HT 2010 Tekniska högskolan vid Linköpings universitet, ISY Namn Ansvar Tele f on E post Joachim Lundh Projektledare (PL) joalu331 Markus Sundbrandt Dokumentansvarig (DOK) marsu752 David Elofsson Designansvarig hårdvara (DES) davel369 Pontus Jernberg Bluetoothansvarig (BLT) ponje141 Karl Cederman Regulatoransvarig (REG) karjo492 Henrik Hagelin Testansvarig (TST) henha374 Dan Gunning Följningsansv (FLJ) dangu444 Håkan Richter Observatöransvarig (OBS) hakri323 E-postlista för hela gruppen: tsrt10segway@googlegroups.com Kund: David Törnqvist, ISY kundtelefon: , e-postadress: tornqvist@isy.liu.se Beställare: Patrik Axelsson, , axelsson@isy.liu.se Handledare: Ylva Jung, , ylvju@isy.liu.se Kursansvarig: David Törnqvist, , tornqvist@isy.liu.se 2

3 Innehåll 1 Introduktion Bakgrund Mål Översikt av systemet Översikt av mjukvaran på roboten Beskrivning av systemet som ska regleras Systemmodell Tillstånd Beskrivning av reglersystemen Delsystem Kommunikation och användargränssnitt Kommunikation Ingående komponenter Användargränssnitt Datapaket Datatypskonvertering Undvikande av hinder och följning Ingående komponenter Ultraljudssensor Ljusintensitetssensor Handföljning Flödesschema Funktionalitet Implementation Linjeföljning Flödesschema Funktionalitet Kalibrering Implementation Undvika hinder Flödesschema Funktionalitet Implementation Referenssignaler till regulator

4 5 Observatör Syfte Modell Linjärisering Observerbarhet Observatörsdesign Observatörsförstärkning Polplacering Kalmanförstärkning Utvärdering Resultat Regulatorer LQG Implementation Resultat PID Implementation Resultat H Implementation Resultat Stopp av motorerna Utvärdering av regulatorerna Vidareutveckling Kommunikation och användargränssnitt Undvikande av hinder och följning Observatör Regulatorer

5 Dokumenthistorik Version Datum Utförda förändringar Utförda av Granskad Första utkast Alla Alla Andra utkast Alla Alla Tredje utkast Alla Alla Fjärde utkast Alla Alla Första versionen Alla Alla 5

6 Kapitel 1 Introduktion Under hösten 2010 genomfördes en reglerteknisk projektkurs vid Tekniska Högskolan vid Linköpings universitet. Projektet gick ut på att reglera en segway byggd i Lego Mindstorms. Detta dokument är en beskrivning av de tekniska aspekterna av projektet. En segway är en inverterad pendel monterad på ett koncentriskt hjulpar. Genom att utnyttja signaler från gyro, ultraljud- och ljussensorer ska roboten regleras så att den håller sig stående upprätt. Användaren ska kunna växla mellan olika regulatorer, körlägen samt kunna styra roboten manuellt, via ett grafiskt gränssnitt på en PC. Som hjälpmedel i projektet har en given simulinkmodell [7] tillhandahållits, i denna modell har sedan funktionaliteten implementerats. Modellen innehåller bland annat en simuleringsmiljö och funktioner för att kompilera och överföra programmen till roboten. 1.1 Bakgrund Projektet är en del av kursen TSRT10, en projektkurs som ges till femteårsstudenter vid LiTH. Kursen syftar till att ge förståelse för hur man jobbar i projekt och ska ge studenterna tillfälle att tillämpa sina kunskaper inom reglerteknik. 1.2 Mål Målet med projektet är att reglera en segway byggd i Lego Mindstorms. Den ska bland annat kunna hållas stående upprätt och styras manuellt. Roboten ska ha två olika regulatorstrukturer för att hålla den stående upprätt. Den ska även kunna följa en linje på golvet, följa en hand, undvika hinder samt undvika att köra ner för stup. 6

7 Kapitel 2 Översikt av systemet Detta kapitel ger en översikt av systemet och de delar som ingår. I figur 2 ses systemet i sin helhet. De delsystem som ska konstrueras är regulator, observatör samt GUI. Merparten av mjukvaran är implementerad i Simulink. Segway Observatör Regulator Motorer Sensorer Bluetooth PC GUI Figur 2.1: Översikt av systemet och de delsystem som ingår. 2.1 Översikt av mjukvaran på roboten Mjukvaran som körs på roboten är implementerad i Simulink och består av en schemaläggare, se figur 2.2, som kör ett antal processer. Varje process har en samplingstid som anger hur ofta den ska exekveras. Samplingstiderna finns definierade i filen param_controller.m. Roboten framförs med en konstant maxhastighet beroende på körläge, denna kan ändras i funktionen figure1_keypressfcn i filen nxtway_gui.m. I figur 2.3 visas de processer som körs. De fyra första processerna fanns implementerade i den grundmodell som tillhandahölls gruppen. Grundmodellen har sedan utvidgats med resterande processer, de tillhandahållna processerna har modifierats och utökats med ytterligare funktionalitet. Sampeltiden för de fyra första pro- 7

8 cesserna var givna i den ursprungliga modellen, övriga sampeltider har valts för att vara i ungefär samma storleksordning som dessa. För process tsk_ts8 har en längre sampeltid valts då det är onödigt att uppdatera denna alltför ofta. Processerna beskrivs kortfattat nedan och mer detaljerat i de senare kapitlen. task_init: Initialization, sätter initialvärden och körmodsflaggor. task_ts1: Balance & Drive Control, sköter regleringen av roboten i autonomt och manuellt läge. task_ts2: Obstacle Check, mäter avståndet framför roboten med hjälp av ultraljudssensorn och sätter en flagga om hinder upptäcks. task_ts3: Time Check & Battery Average, mäter systemtid och batterinivå. task_ts4: Edge check (front), mäter ljusintensiteten för den främre ljussensorn och sätter en flagga om intensiteten är lägre än ett visst tröskelvärde. task_ts5: Edge check (rear), mäter ljusintensiteten för den bakre ljussensorn och sätter en flagga om intensiteten är lägre än ett visst tröskelvärde.) task_ts6: Follow hand, mäter avståndet framför roboten med hjälp av ultraljudssensorn och sätter en flagga beroende på vilket avstånd som uppmätts. y-gs Controller Model based on Rate Monotonic Scheduling odel consists of four parts : Device task_ts7: Inputs, Follow Device line, mäter Outputs, ljusintensiteten Task Scheduler, för den främre and Application ljussensorn och Task sätter Subsystem. er: en flagga beroende på vilken ljusintensitet som uppmätts. ) is a trademark of the LEGO Group of companies which does not sponsor, authorize or endorse this demo. ) and Mindstorms(R) are registered trademarks task_ts8: of Send The data, LEGO läser Group. sensorvärden och observerade tillstånd och skickar dessa via bluetooth till användargränssnittet. In1 C Revolution Sensor Interface Port = C Priority = -1 In1 B Revolution Sensor Interface1 Port = B Priority = -1 S2 ### OSEK Tasks ### task_init: Init task_ts1: [sec] task_ts2: 0.03 [sec] task_ts3: 0.1 [sec] task_ts4: 0.02 [sec] task_ts5: 0.02 [sec] task_ts6: 0.05 [sec] task_ts7: 0.06 [sec] task_ts8: 0.25 [sec] ExpFcnCalls Scheduler Priority = 0 task_init task_ts1 task_ts2 task_ts3 task_ts4 task_ts5 task_ts6 task_ts7 task_ts8 task_init_fc task_ts1_fc task_ts2_fc task_ts3_fc task_ts4_fc task_ts5_fc task_ts6_fc task_ts7_fc task_ts8_fc rambot C Servo Motor Interface1 Port = C Priority = 1 B Servo Motor Interface Port = B Priority = 1 BTTx Ultrasonic Sensor Interface Port = S2 Priority = -1 S4 Gyro Sensor Interface Port = S4 Priority = -1 S3 Light Sensor Interface Figur 2.2: Schemaläggare för de olika processerna. ## Requires only MATLAB products ## Generate code from task subsystem using RTW-EC ## Requires additional 3rd party tools 8 ## Generate code and build the generated code Bluetooth Tx Interface Priority = 1 Freq Dur Sound Tone Interface Priority = 1 S1 Download (NXT enhanced firmware) Download (SRAM) Light Sensor Interface1 Click the annotations to generate/build code and download it into NXT

9 Application Task Subsystems Each function-call subsystems are drived by function-call signals of the task scheduler and generated as OSEK task functions in the generated code using Export functions feature of RTW-EC. 1 task_init_fc 2 task_ts1_fc 3 task_ts2_fc function() function() function() task_init Initialization task_ts1 Balance & Drive Control task_ts2 Obstacle Check 4 task_ts3_fc 5 task_ts4_fc 6 task_ts5_fc function() function() function() task_ts3 Time Check & Battery Average task_ts4 Edge check (front) task_ts5 Edge check (rear) 7 task_ts6_fc 8 task_ts7_fc 9 task_ts8_fc function() function() function() task_ts6 Follow hand task_ts7 Follow line task_ts8 Send data Shared Data Figur 2.3: Segway-programmets olika processer. Data Store Memory is used as a shared data between tasks. obstacle detection flag_detected_obstacle 1 : NXTway-GS is detecting an obstacle line follow flag flag_line 1 : NXTway-GS is following a line 2.2 Beskrivning DataType = boolean 0 : av NXTway-GS systemet is not detecting som an obstacleska regleras DataType = boolean 0 : NXTway-GS is not following a line En segway kan modelleras som en inverterad pendel på två hjul. Roboten är byggd flag_start start time flag hand follow flag 1 : gyro calibration is finished flag_hand DataType = boolean 1 : NXTway-GS is following a hand 0 : gyro calibration is not finished enligt ritningar från [4] men har modifierats så att den nu DataType har = boolean två ljusintensitetssen- 0 : NXTway-GS is not following a hand sorer, en ultraljudssensor, ett gyro samt två servon. Ultraljudssensorn används för att flag_auto autonomous time flag obstacle avoid flag 1 : autonomous drive is ready flag_obstacle DataType = boolean 1 : NXTway-GS is avoiding obstacles mäta avståndet till ett eventuellt 0 : autonomous framförvarande drive is not ready hinder och undvika att köra DataType = boolean 0 : NXTway-GS in i dessa. is not avoiding obstacles Ljusintensitetssensorerna mäter hur mycket ljus som reflekteras från underlaget, dessa flag_run_forward run forward flag flag_turn_right turn right flag 1 : NXTway-GS is driving forward används fördatatype stupdetektering = boolean och vid linjeföljning. Från robotens DataType = boolean gyro och 1 : NXTway-GS servonis turning kan right 0 : NXTway-GS is not driving forward 0 : NXTway-GS is not turning right man mäta lutningsvinkelhastighet och motorvinklar. Hur sensorerna är monterade på run backwards flag flag_run_backwards 1 : NXTway-GS is driving backwards roboten ses i figur 2.4. DataType = boolean 0 : NXTway-GS is not driving backwards DataType = uint16 lift detection 1: NXTway-GS is lifted 0: NXTway-GS is not lifted flag_turn_left DataType = boolean DataType = boolean turn left flag 1 : NXTway-GS is turning left 0 : NXTway-GS is not turning left De mätbara storheterna som används för att reglera roboten ges av följande uppsättning. flag_lifted flag_tilt Höger motorvinkel: θ mr, vinkeln på det högra servot. Vänster motorvinkel: θ ml, vinkeln på det vänstra servot. tilt detection 1: NXTway-GS is tilted 0: NXTway-GS is not tilted battery DataType = single gyro_offset DataType = single start_time DataType = uint32 line_thr_upper DataType = uint16 line_thr_lower DataType = uint16 counter DataType = double average ba gyro offset program sta upper line t lower line th fall detectio Lutningsvinkelhastighet: ψ, förändringshastighet för lutningen på roboten. Dessa storheter är dock inte tillräckliga för att utföra en reglering av roboten. En ny uppsättning storheter skapas med enkla beräkningar definierade av geometrin i figur 2.5. 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 + ψ 9

10 Ultraljudssensor Kontrollenhet Gyro Vänster servo Bakre ljusintensitetssensor Främre ljusintensitetssensor Figur 2.4: Översikt av roboten med markeringar för särskilda komponenter. 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 + ψ Snitthjulvinkel: θ, den genomsnittliga vinkeln på hjulen. θ = θ r + θ l 2 = θ m r + θ ml 2 + ψ = θ m + ψ (2.1) Girvinkel: ϕ, vinkeln med vilken roboten roterar. ϕ = R W (θ r θ l ) = R W ( θmr θ ml ) Då inte alla vinklar och vinkelhastigheter kan mätas krävs en observatör som skattar dessa Systemmodell Systemmodellen ges utifrån de linjäriserade ekvationerna hämtade från [7], där beskrivs också hur modellerna är framtagna. Den linjäriserade modellen beskrivs av ẋ = Ax + Bu y = Cx + Du (2.2) 10

11 Ψ φ θ θ m W/2 R Figur 2.5: Definition av vinklar. där A = , 2 141, 3 141, ,8 62,1 62,1 B = C = D = ,3 173,3 60, [ ] [ ] (2.3) Första raden i C matrisen ges av (2.1) och att det är θ m som vi mäter. De numeriska värdena i (2.3) är beräknade med uppmätta värden på de ingående storheterna istället för de värden som används i [7] eftersom roboten har blivit utsatt för viss modifikation Tillstånd De tillstånd som används i systemmodellerna är snitthjulvinkeln, snitthjulvinkelhastigheten, lutningsvinkeln och lutningsvinkelhastigheten. Dessa är således de tillstånd som observatören behöver skatta för att regulatorn ska fungera tillfredställande. Snitthjulvinkel: θ, den genomsnittliga vinkeln mellan höger och vänster hjul. Lutningsvinkel: ψ, den vinkel med vilken roboten lutar Snitthjulvinkelhastighet: θ, den genomsnittliga vinkelhastigheten mellan höger och vänster hjul. 11

12 Lutningsvinkelhastighet: ψ, den vinkelhastighet med vilken robotens lutningsvinkel förändras. Den skattade tillståndsvektorn, systemets in- och utsignal har följande utseende: ˆθ ˆx = ˆψ ˆθ, ˆψ [ ] u = vl, y = v r [ ] θṁ ψ där v l är spänningen i volt till vänster servo och v r är spänningen i volt till höger servo. 2.3 Beskrivning av reglersystemen De reglersystem som har implementerats är två regulatorer i form av en LQG- och en PID-regulator. Dessutom har en H -regulator tagits fram, dock har den inte fåtts att fungera. Utöver dessa har även olika körlägen tagits fram. De olika regulatorerna beskrivs mer detaljerat i kapitel 4 och Delsystem De delsystem som har tagits fram i projektet är följande: Kommunikation och gränssnitt: Sköter kommunikationen mellan robot och PC samt ansvarar för det grafiska användargränssnittet. Delsystemets funktionalitet och implementation beskrivs i kapitel 3. Följning och undvikande av hinder: Ser till så att eventuell följning utförs och att hinder undviks. Delsystemets funktionalitet och implementation beskrivs i kapitel 4. Observatör: Skattar tillstånd för roboten med hjälp av mätdata och styrsignaler. Delsystemets funktionalitet och implementation beskrivs i kapitel 5. Regulator: Ser till att roboten håller sig upprätt. Delsystemets funktionalitet och implementation beskrivs i kapitel 6. 12

13 Kapitel 3 Kommunikation och användargränssnitt Kapitlet beskriver det delsystem som hanterar kommunikationen mellan dator och robot. Här återfinns även en beskrivning av det användargränssnitt som används för att kontrollera roboten. 3.1 Kommunikation Detta system tar hand om kommunikationen mellan datorn och roboten. Datorn ska via bluetooth hela tiden logga vissa angivna värden och visa detta i ett program. Från detta program kan användaren även välja vilket körläge som ska vara aktivt samt vilken av regulatorstrukturerna som ska användas. I det modellbibliotek [1] som använts finns fördefinierade block som använts för att skicka och ta emot data, datapaketen finns beskrivna mer detaljerat i kapitel En skiss över kommunikationssystemet ses i figur 3.1. Referenser Modbyte Regulatorbyte Kalibreringsdata Sensordata Observerade värden Figur 3.1: Systemskiss av delsystemet kommunikation. 13

14 3.1.1 Ingående komponenter Delsystemet består av roboten samt en dator med Matlab och bluetoothanslutning. Kommunikationen mellan dator och robot sker mellan robotens inbyggda bluetoothadapter och datorn Användargränssnitt För att en användare enkelt ska kunna kommunicera med roboten så har ett gränssnitt tagits fram. Gränssnittet tar emot sensordata som roboten skickar till datorn via bluetooth. Från datorn kan användaren ansluta till roboten, växla aktiv regulator, aktivt körläge samt välja vilka data som ska plottas. Då roboten är i körläge för manuell styrning kan man från gränssnittet även styra roboten med hjälp av datorns piltangenter. Vid linjeföljning kan även ljussensorerna kalibreras via gränssnittet. De sensordata som presenteras i gränssnittet är uppmätt avstånd framåt från ultraljudssensorn, uppmätta ljusintensiteter från ljussensorerna samt hjulens vinklar. Dessutom presenteras tillståndet ψ. Dessa värden presenteras i grafer. De momentana värdena presenteras även i en ruta. I figur 3.2 ses hur användargränssnittet ser ut. Användargränssnittet är implementerat i Matlab, och dess funktion beskrivs mer detaljerat i [6]. Figur 3.2: Det grafiska användargränssnittet. Figur 3.2 visar användargränsnittet som är uppdelat i två fönster, ett skriv- och ett läsfönster. Anledningen till uppdelningen av gränssnittet är för att underlätta realtidsavläsningen av data som skickas från roboten till PC. Roboten skickar data till PC:n med intervallet 0, 25 sekunder och roboten tar emot nya styrkommandon etc med samma frekvens som man uppdaterar regulatorn i roboten (0, 004 sekunder). Om man vill ändra uppdateringsfrekvensen för avläsning av värden så är det fyra filer 14

15 man måste ändra detta i. Dessa filer är param_controller.m, input_gui.m, watchsensors.m och updateplots.m. I den första filen heter parametern som ska ändras ts8 och i de övriga heter den T. På samma sätt om man vill ändra hastigheten hos de manuella utslagen så är det i filen nxtway_gui.m och funktionen figure1_keypressfcn som man ska ändra Datapaket De datapaket som skickas från roboten till datorn är sensordata, observerade data och styrsignal. De data som skickas från datorn till roboten är aktuell regulator och körläge samt eventuella styr- eller kalibreringskommandon. Då man endast kan skicka data av typen uint8 via bluetoothlänken krävs datatypskonvertering för att kunna skicka all den tänkta datan, hur dessa konverteringar går till beskrivs i kapitel Figur 3.3 visar den simulinkmodell som används för att skicka data från roboten till PC och tabell 3.1 innehåller en sammanfattning av datapaketet som skickas. Tabell 3.2 beskriver på samma sätt det datapaket som skickas från PC till robot. Send data Task Reads and sends data to a PC via bluetooth. Out1 Out2 f() In1 System Clock Read Out3 Out4 Subsystem Out1 Out2 Out1C In1 Revolution Sensor Left Out3 Out4 Out1 Subsystem1 Out2 Out1B In1 Revolution Sensor Right Out3 Out4 BTTx S4 Subsystem2 In1 Out1 Bluetooth Tx Write Out2 S2 Gyro Sensor Read Convert Subsystem3 Ultrasonic Sensor Read Data Type Conversion2 Out1 S3 In1 Out2 Light Sensor Read Out1 Subsystem4 S1 In1 Light Sensor Read1 Out1 Out2 Subsystem5 obs_out_psi Message generator In1 Out1 Out2 zeros(1, 10) Subsystem6 Constant Figur 3.3: Simulink-modell för datatransmission i roboten. 15

16 Tabell 3.1: Datapaket från robot till PC Systemtid Vänster motor Höger motor Gyro Ultraljud... 4 Byte 4 Byte 4 Byte 2 Byte 1 Byte Ljussensor fram Ljussensor bak Meddelanden Observerat ψ Ledig plats 2 Byte 2 Byte 1 Byte 2 Byte 10 Byte Tabell 3.2: Datapaket från PC till robot Man. ref. θ Man. ref. ϕ Körläge Regulator... 1 Byte 1 Byte 1 Byte 1 Byte Linjekal. undre tröskel Linjekal. övre tröskel Ledig plats 2 Byte 2 Byte 24 Byte Datatypen för variablerna i datapaketen beskrivs i tabell 3.3. Tabell 3.3: Datatyper Robot till PC Systemtid Vänster motor Höger motor Gyro Ultraljud Ljussensor fram Ljussensor bak Meddelanden Observerat ψ PC till robot Man. ref. snitthjulvinkelhastighet θ Man. ref. girvinkelhastighet ϕ Körläge Regulator Linjekalibrering undre tröskel Linjekalibrering övre tröskel uint32 int32 int32 int16 uint8 uint16 uint16 uint8 single int8 int8 uint8 uint8 uint16 uint16 Anledningen till att gyrodata skickas från roboten till PC trots att denna till synes inte används är att den till en början användes istället för vinkeln ψ för att granska robotens lutning. Inte heller systemtiden används längre. Till en början var tanken att graferna skulle plottas med systemtiden men då det komplicerade realtidsavläsningen av datan så övergavs detta. Dessa extra storheter kan användas till felsökning eller liknande. Då övriga observatörstillstånd och styrsignaler inte används i PC:n skickas inte dessa från roboten. 16

17 3.1.4 Datatypskonvertering För att kunna skicka och ta emot data med annan datatyp än uint8 krävs en datatypskonvertering både före och efter sändning. Ett bra exempel på hur dessa konverteringar fungerar är att granska sändningen av observerat ψ från roboten till PC. Det observerade värdet har från början datatypen single vilket innefattar både decimaler och tecken. Då uint8 endast innefattar positiva heltal mellan 0 och 255 så måste decimalerna hanteras. Decimalerna kan haneteras genom att multiplicera single-värdet med 10 n där n är antal intressanta decimaler. Nästa steg är att byta datatyp från single till int. Då trunkeras de ointressanta decimalerna men tecknet bevaras. Antalet bitar som int-värdet behöver beror på hur många decimaler man vill bevara. En int8 kan endast representera tal mellan 128 och 128 medan en int16 kan representera tal mellan och Detta int-värde kan nu delas upp i uint8-värden som sparas i datapaketet och sänds över bluetoothlänken. Uppdelningen går till som så att åtta bitar maskas ut ur int-värdets binärform och dessa åtta bitar lagras sedan i en uint8. För att maska ut delarna används bitvis multiplikation med ett givet mönster för de bitar som ska maskas ut. Efter att bitarna maskats ut måste de skiftas så att de i binärform ligger placerade som de första åtta bitarna. När datapaketet tagits emot måste värdet återskapas. För att återskapa värdet läggs uint8 delarna ihop till ett tal och sedan används tvåkomplementsmetoden för att få rätt tecken. Hela talet multipliceras sedan med 10 n. Nedan följer ett räkneexempel på detta. I sändare: ψ = 0, rad ψ int16 = int16( 0, ) = int16 ψ 1 uint8 = uint8(bitwisemult(ψ 16,[ ])) ψ 2 uint8 = uint8(rightshift(bitwisemult(ψ 16,[ ]),8)) I mottagare: ψ = TwosComplement(256ψ 2 uint8 + ψ 1 uint8)10 5 = 0, rad 17

18 Kapitel 4 Undvikande av hinder och följning Kapitlet beskriver de tre delsystemen handföljning, linjeföljning och undvikande av hinder. I delsystemet handföljning använder roboten ultraljudssensorn för att hålla ett givet avstånd till en hand. I delsystemet linjeföljning följer roboten en tejpremsa markerad på underlaget. Vid manuell styrning ser delsystemet undvikande av hinder till att roboten stannar på tillräckligt avstånd från ett eventuellt hinder. De trösklar och hastigheter som beskrivs i detta kapitel sätts, och kan ändras, i matlabfilen param_controller.m. 4.1 Ingående komponenter Roboten använder sig av två olika sorters sensorer, vilka beskrivs nedan Ultraljudssensor Sensorn gör att roboten ser avstånd till och kan detektera objekt. Den ger avståndsvärden från 0 till 255 cm med en precision på +/- 3 cm. Sensorn mäter avståndet genom att beräkna tiden det tar för en ljudvåg att träffa ett objekt och återvända till sensorn (som ett eko). Stora, plana och hårda objekt ger bättre avläsningar än små, kurviga och mjuka objekt, enligt [5] Ljusintensitetssensor Sensorerna läser av reflekterad ljusintensitet hos underlaget. Den uppmätta intensiteten används dels för att detektera om roboten befinner sig på tejp, i sådana fall vilken intensitet tejpen har, och dels för att detektera stup. Sensornerna ger intensitetsvärden mellan 0 och 1023, där ett stort värde svarar mot en låg reflekterad ljusintensitet (t.ex. svart tejp eller stup), enligt [5]. 18

19 4.2 Handföljning Handföljning innebär att roboten följer en hand på ett givet avstånd. Om man flyttar handen framåt eller bakåt håller roboten hela tiden samma avstånd till handen och på så sätt kan man styra hur och vart roboten ska åka. Avståndsavläsningen görs med hjälp av en ultraljudssensor, se avsnitt Avståndet mellan handen och roboten ska vara mellan 0 till 50 cm för att roboten ska följa den. En skiss över systemet för handföljning ses i figur 4.1. Avstånd Handföljning Referens Regulator Figur 4.1: Systemskiss av delsystemet handföljning Flödesschema I figur 4.2 visas flödesschemat för delsystemet handföljning. I detta delsystem är undvikande av hinder ej aktivt. Ultraljudssensorn gör avståndsavläsningar med samplingstiden 0,05 s. Vid starten står roboten still och väntar på att ultraljudssensorn ska detektera en hand. När en hand har detekterats kontrolleras om avståndet är för långt, för kort eller korrekt och därefter agerar roboten enligt flödesschemat i figur 4.2. Stå still Stå still Kör bakåt Kör framåt Nej Ja Ja Ja Start Finns hand inom 0-50 cm? Ja Avstånd till hand cm? Nej Avstånd till hand 0-20 cm? Nej Avstånd till hand cm? Nej Figur 4.2: Flödesschema över delsystemet handföljning. 19

20 4.2.2 Funktionalitet Roboten använder ultraljudssensorn för att mäta avståndet till handen. Om handen befinner sig på ett avstånd som är mindre än 20 cm backar roboten till dess att avståndet är cm. Motsvarande princip gäller då avståndet är större än 30 cm. Om handen förs längre bort än 50 cm från sensorn stannar roboten och håller sig upprätt på stället, se figur 4.3. Roboten läser in värden från sensorn, dessa behandlas och sedan skickas en referenssignal på snitthjulvinkelhastigheten θ till delsystemet regulator, se avsnitt 4.5. Figur 4.3: Robotens beteende beroende på avståndet mellan handen och ultraljudssensorn Implementation Delsystemet handföljning är implementerat i Simulink, se figur 4.4. Denna del sätter flaggor vilka sedan används för att sätta referenssignalen på snitthjulvinkelhastigheten hos roboten vilket får den att köra framåt respektive bakåt, se avsnitt 4.5. Flaggan flag_run_backwards sätts då sensorvärdet från ultraljudssensorn är mindre än eller lika med tröskeln dst_hand_short som har värdet 20 cm. Flaggan flag_run_forward sätts då det avlästa värdet ligger mellan trösklarna dst_hand_long och dst_hand_max, vilket har värdena 30 cm respektive 50 cm. 20

21 S2 Ultrasonic Sensor Read Port = S2 dst_hand_short DataType = int32 < flag_run_backwards dst_hand_long < DataType = int32 AND flag_run_forward dst_hand_max < Logical Operator DataType = int32 Figur 4.4: Delsystemet handföljning implementerat i Simulink. 4.3 Linjeföljning Roboten klarar av att följa en bestämd bana på marken som markeras med en linje i form av en tejpremsa, radien hos banans kurvor måste vara större än 15 cm. Vid linjeföljning används den främre ljusintensitetssensorn för att läsa av banan, mer information om sensorn finns i avsnitt En skiss över linjeföljningssystemet ses i figur 4.5. Intensitet Linjeföljning Referens Regulator Figur 4.5: Systemskiss av delsystemet linjeföljning Flödesschema I figur 4.6 visas flödesschemat för delsystemet linjeföljning, i detta delsystem är undvikande av hinder ej aktivt. Ljusintensitetssensorn läser av den reflekterade intensiteten hos underlaget med samplingstiden 0,06 s. Referenssignalen på robotens snitthjulvinkelhastighet θ och girvinkelhastighet ψ som skickas till regulatorn beror på sensorvärdet. Ett stort värde ger en referenssignal om att vrida roboten åt vänster, ett litet värde ger vridning åt höger och ett mellanliggande värde ger att roboten ska fortsätta köra i samma riktning. 21

22 Kör rakt fram Korrigera åt höger Korrigera åt vänster Ja Ja Ja Start Mittenlinje? Nej Vänster linje Nej Höger linje Nej Figur 4.6: Flödesschema över delsystemet linjeföljning Funktionalitet Linjen som ljusintensitetssensorn läser av är en tejpremsa i tre färger, se figur 4.7 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. Ljusintensitetssensorn ger ett intensitetsvärde beroende på underlaget. Ett ljust underlag har hög reflektivitet och får ett litet värde, ett mörkt underlag har låg reflektivtet och får därmed ett stort värde. Mittenfärgen ska ha en reflektivitet som ger ett värde vilket ligger mellan de intilliggande färgernas värden. Beroende på om sensorvärdet är stort eller litet så korrigerar roboten åt höger eller vänster, ligger värdet mellan dessa håller den samma kurs. Skulle roboten köra utanför banan kommer den att köra rakt fram eftersom underlaget utanför och i mitten av banan är detsamma. Roboten läser in värden från sensorn, dessa behandlas, sedan skickas en referenssignal till delsystemet regulator. Figur 4.7: Utseendet på linjen, pilen markerar robotens färdriktning Kalibrering Innan man kan börja följa linjen måste en kalibrering av varje färg utföras, detta görs via gränssnittet på PC:n. För att minimera inverkan av felaktiga sensorvärden sätts trösklar mellan de olika kalibreringsvärdena, se figur 4.8 för ett exempel. I exemplet är kalibreringsvärdena 590, 660 och 710. Genom att beräkna medelvärdet av de närliggande färgerna fås trösklarna 625 och 685. Sensorvärden under 625 tolkas sedan som vitt, värden över 685 tolkas som svart och mellanliggande värden tolkas som grågrönt. 22

23 Figur 4.8: Svarta siffror är kalibreringsvärden för respektive färg, röda siffror är tröskelvärden Implementation Delsystemet linjeföljning är implementerat i Simulink, se figur 4.9. Denna del sätter flaggor vilka sedan används för att sätta referenssignaler på snitthjulvinkelhastigheten och girvinkelhastigheten hos roboten. Dessa flaggor får roboten att köra framåt alternativt svänga vänster eller höger, se avsnitt 4.5. Flaggan flag_turn_left sätts då värdet från ljusintensitetssensorn är större än tröskeln line_thr_upper, vilken sätts vid kalibreringen. Flaggan flag_turn_right sätts då värdet från ljusintensitetssensorn är mindre än tröskeln line_thr_lower, vilken sätts vid kalibreringen. Flaggan flag_run_forward sätts då det avlästa värdet ligger mellan trösklarna line_thr_upper och line_thr_lower. line_thr_upper > Relational Operator2 flag_turn_left S3 NOT AND flag_run_forward Light Sensor Read NOT Logical Operator line_thr_lower > Relational Operator1 flag_turn_right Figur 4.9: Delsystemet linjeföljning implementerat i Simulink. I simulinkschemat i figur 4.10 beräknas differensen mellan den senast avlästa hjulvinkeln och theta_ml, vilken är hjulvinkeln roboten hade innan flaggan flag_turn_right sattes. När denna differens är större än tröskeln turn_angle, vilken vi valt till 210 för att få önskad svängstorlek, nollställs flaggan och roboten slutar svänga. Flaggan är alltså satt till dess att hjulvinkeln ökat med 210. Motsvarande princip gäller vid vänstersväng. 23

24 Out1C Revolution Sensor Read Port = C 1 theta_m_l turn_angle DataType = int32 <= flag_turn_right Figur 4.10: Simulinkschema för högersväng vid linjeföljning. 4.4 Undvika hinder Delsystemets uppgift är att undvika hinder som uppkommer under körning. Roboten klarar av två sorters hinder, väggar och stup. För att undvika att köra in i en eventuell vägg framifrån ställer sig roboten en bit från den och håller sig sedan upprätt. Roboten upptäcker när den närmar sig ett stup framifrån och bakifrån och ställer sig en bit från kanten, därefter håller den sig upprätt. Undvika hinder är inte aktivt vid varken linjeföljning eller handföljning. En skiss över systemet för att undvika hinder ses i figur Delsystemet använder en ultraljudssensor och två ljusintensitetssensorer, se avsnitt 4.1. Avstånd Intensitet Undvika hinder Referens Regulator Figur 4.11: Systemskiss av delsystemet undvika hinder Flödesschema I figur 4.12 visas flödesschemat för delsystemet undvika hinder vilket endast är aktivt vid manuell styrning. Både ljusintensitetssensorerna och ultraljudssensorn är aktiverade och data skickas 50 gånger per sekund till gränssnittet på datorn, detta ger en samplingstid på 0,02 s. Om en ljusintensitetssensor detekterar ett stup eller om ultraljudssensorn detekterar en vägg kör roboten framåt (stup bak) respektive bakåt (stup fram och vägg) till dess att hindret inte längre detekteras. När ett hinder detekteras tillåts inte användaren att köra mot det. 24

25 Stanna och tillåt inte att köra framåt Stanna och tillåt inte att körabakåt Stanna och tillåt inte att köra framåt Ja Ja Ja Start Detekteras stup fram? Nej Detekteras stup bak? Nej Detekteras vägg? Nej Figur 4.12: Flödesschema över delsystemet undvika hinder Funktionalitet För att upptäcka och undvika eventuella väggar använder roboten sig av en ultraljudssensor. Då avståndet till en framförliggande vägg är mindre än 20 cm backar roboten några centimeter och står sedan upprätt. När roboten kör på ett enfärgat underlag används ljusintensitetssensorerna för att upptäcka och undvika stup. Roboten detekterar ett stup då sensorvärdet överstiger tröskelvärdet 700, vilket är strax under det värde som registreras vid stup. Roboten läser in värden från sensorn, dessa behandlas, sedan skickas en referenssignal på snitthjulvinkelhastigheten θ till delsystemet regulator Implementation Delsystemet undvika hinder är implementerat i Simulink, se figur De uppmätta sensorvärdena jämförs med trösklar och därefter sätts flaggor. Dessa används för att sätta referenssignalen på snitthjulvinkelhastigheten hos roboten vilket får den att köra framåt respektive bakåt, se avsnitt 4.5. Flaggan flag_run_backwards_wall sätts då ultraljudssensorn registrerar ett värde som är mindre än eller lika med tröskeln dst_thr som har värdet 20 cm. Flaggan flag_run_backwards sätts då värdet från den främre ljusintensitetssensorn är större än eller lika med tröskeln edge_thr som har värdet 700. Flaggan flag_run_forward sätts då det avlästa värdet från den bakre ljusintensitetssensorn är större än eller lika med tröskeln edge_thr. 25

26 S2 flag_run_backwards_wall Ultrasonic Sensor Read Port = S2 dst_thr DataType = int32 <= flag_detected_obstacle S3 flag_run_backwards Light Sensor Read (front) edge_thr DataType = uint16 <= flag_detected_obstacle S1 flag_run_forward Light Sensor Read (rear) edge_thr DataType = uint16 <= flag_detected_obstacle Figur 4.13: Delsystemet undvika hinder implementerat i Simulink. 4.5 Referenssignaler till regulator Tidigare i detta kapitel beskrevs flaggorna som hanterar styrningen samt de villkor som används för att avgöra deras värden. I denna del förklaras vilka värden som skickas till regulatorn beroende på vilka flaggor som är satta. Styrningen är implementerad i Simulink enligt figur Eftersom hastigheten max_speed_fwd är definierad som negativ och hastigheten max_speed_bwd är definierad som positiv och regulatorn har en motsatt definition görs teckenbytet. Värdet på flaggan flag_run_forward avgör tillsammans med flaggorna flag_run_backwards och flag_run_backwards_wall samt eventuella bluetoothsignaler från PC:n vilken referens på snitthjulvinkelhastigheten thetadot_cmd som skickas till regulatorn. Flaggorna kan ej vara aktiva samtidigt utan endast en i taget. Är flaggan flag_run_forward satt skickas den maximala hastigheten framåt max_speed_fwd, annars skickas värdet 0 och roboten kör inte framåt. Är flaggan flag_run_backwards eller flag_run_backwards_wall satt skickas den maximala hastigheten bakåt max_speed_bwd, annars skickas värdet 0 och roboten kör inte bakåt. Om ingen av de tre ovanstående flaggorna är satta läses data för att köra framåt eller bakåt in via bluetooth från PC:n. 26

27 max_speed_fwd 1 flag_run_forward DataType = int8 0 DataType = int8 2 flag_run_backwards 3 flag_run_backwards_wall Add3 max_speed_bwd DataType = int8 0 DataType = int8 Add1-1 1 thetadot_cmd 0 DataType = int8 Add2 bt Out1 6 bluetooth_rx U Y int8 Round = Simplest Indices = [1 2] BT check bt Out1 BT check1 turn_left_speed 4 flag_turn_left DataType = int8 0 Add 2 phidot_cmd DataType = int8 turn_right_speed 5 flag_turn_right DataType = int8 0 DataType = int8 Figur 4.14: Simulinkschema för styrningen av roboten. Flaggan flag_turn_left avgör tillsammans med flaggan flag_turn_right samt eventuella bluetoothsignaler från PC:n vilken referens på girvinkelhastigheten phidot_cmd som skickas till regulatorn. Är flaggan flag_turn_left satt skickas hastigheten för vänstersväng turn_left_speed, annars skickas värdet 0 och roboten svänger inte till vänster. Är flaggan flag_turn_right satt skickas hastigheten för högersväng turn_right_speed, annars skickas värdet 0 och roboten svänger inte till höger. Data för att svänga kan även läsas in via bluetooth från PC:n. 27

28 Kapitel 5 Observatör Kapitlet beskriver hur observatören implementerades, vilka designval som gjordes och vilka problem som stöttes på under arbetet. 5.1 Syfte Observatören ska designas så att regulatorn får så bra signaler som möjligt att reglera på genom att reducera brus i mätsignaler och skatta tillstånd som inte kan mätas. I robotens fall där det enbart fanns möjlighet att mäta lutningsvinkelhastigheten ψ och snittmotorvinkeln θ m användes dessa för att skatta övriga tillstånd, snitthjulvinkeln θ, snitthjulvinkelhastigheten θ och lutningsvinkeln ψ. Observatören bygger på en fysikalisk modell som beskriver en sådan robot som projektet nyttjar. Modellen är hämtad från [7] och bygger på antagandet att systemet kan modelleras som en inverterad pendel på två hjul. Observatören använder systemets insignaler, snittmotorvinkeln och lutningsvinkelhastigheten för att kontinuerligt minimera skattningsfelet. Observatörsförstärkningen är en avvägning mellan brusundertryckning och snabbhet. De tillstånd som observeras är kopplade till robotens balanseringsfunktion. 5.2 Modell Det krävs en modell av systemet för att observera de tillstånd som inte mäts direkt via sensorer. Observatören baseras på en olinjär modell som sedan linjäriserats för att underlätta implementeringen Linjärisering Roboten är ett olinjärt system och kräver en linjärisering för att man enkelt ska kunna utföra någon reglering. Att ta fram systemmodeller och linjärisera dessa har dock inte ingått i detta projektet utan var resultat som kunde hämtas från tidigare studier. Mer om systemmodellerna kan läsas i avsnitt

29 5.2.2 Observerbarhet Det är enkelt att kontrollera om det är möjligt att implementera en observatör som skattar de tillstånd som är intressanta. Det vill säga om systemet är observerbart. Detta görs utifrån de systemmodeller som är framtagna i avsnitt O = C CA CA 2 CA 3, (n = dim(x 1) = 4). Om rang(o) = n så är systemet observerbart. För roboten är detta fallet Observatörsdesign Alla matriser och storheter i detta stycke är hämtade från definitionerna i avsnitt Observatören beskrivs av uttrycket ˆx = A ˆx + Bu + K(y C ˆx) (5.1) För att uttrycket ovan ska kunna användas i ett tillståndsblock i Simulink görs följande omskrivning. ˆx = A obs ˆx + B obs u obs y obs = C obs ˆx + D obs u obs där A obs = [A KC] B obs = [B,K] C obs = I 4x4 D obs = 0 4x4 u obs = 5.3 Observatörsförstärkning [ ] u y (5.2) För att beräkna observatörsförstärkningen användes två olika metoder, polplacering och kalmanförstärkning Polplacering Man bestämmer observatörens egenskaper genom att placera observatörens poler på godtyckliga ställen. Grundidén är att placera observatörens poler lite snabbare än det slutna systemets poler för att observatören ska klara av att skatta tillstånden. Polerna hos det slutna systemet (A BL) ligger i [ ,5i 2 1,5i ] För att observatören ska bli snabbare än systemet placeras polerna längre in i vänstra halvplanet än systemets poler. En första ansats till lämplig polplacering blir då ungefär [ ] Genom att placera polerna parvis lika reduceras antalet designparametrar från fyra till två. Tanken var att först trimma dessa två val av polställen tills en fungerande observatör erhölls. Därefter skulle polerna justeras sinsemellan och eventuellt införa komplexa poler för att trimma in observatören ytterliggare. Problemet var som beskrivs i 29

30 avsnitt att storleksordningen på polplaceringen var helt fel. Detta resulterade i att steget med justering av polernas inbördes placering och eventuellt införande av komplexa poler aldrig blev nådd. Denna designmetod var den som fokus var tänkt att ligga på då observatören designades. Med polplaceringen p(s) beräknas K ur uttrycket det(si A obs ) = det(si (A KC)) = p(s). Detta görs lämpligen i Matlab med kommandot place. P = [-300, -300, -150, -150]; K = place(a,c, P) ; Då erhålls värden för elementen i förstärkningsvektorn och observatören testas och evalueras. Om de skattade tillstånden inte uppfyller kraven på följning och brusundertryckning väljs en ny polplacering och en ny förstärkningsvektor beräknas. Detta moment upprepas tills en tillräckligt bra följning uppnås. Avvägningen som görs är mellan observatörens snabbhet och brusundertryckning. Svårigheten ligger i att veta hur designparametrarna (polerna) påverkar observatören. Även om polerna är placerade tillräckligt långt från origo krävs det känsla för att trimma in observatören. De polplaceringar som var rimliga och testades visade sig ge fina resultat i den simulerade miljön. Men när de testades på roboten fungerade observatören inte alls. Detta beror på att simuleringsmodellen är en linjär modell till skillnad mot det verkliga systemet som är olinjärt. Värdena som observatören levererade vid verklig körning stämde inte alls överens med värdena vid simuleringen. Beteendet hos observatören var mycket märkligt, den levererade ett första värde när den startade för att sedan ställa tillståndsvektorn konstant lika med nollvektorn. Vad beteendet med nollvektorn berodde på är ännu oklart. Om det skulle vara så att någon instabilitet är orsaken borde observatören ändå leverera några värden istället för bara nollor. Problematiken med detta ledde till ett försök att skapa observatörsförstärkningen genom att beräkna kalmanförstärkningen Kalmanförstärkning Att beräkna observatörsförstärkningen på detta vis görs genom att använda processoch mätbrusets varianser som designparametrar för att skapa ett kalmanfilter. Lösningen P till riccatiekvationen [2] ger K enligt nedanstående uttryck. { K = PC T R 1 AP + PA T + NQN T PC T R 1 PC = 0 Svårigheten med metoden ligger i de okända brusintensiteterna Q och R. 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. För att bestämma Q och R görs en uppskattning av störningens storlek på insignalen (u) och mätbruset. Robotens insignal varierar ungefär mellan -5 och 5 volt. Störningsmatrisen för insignalen kan då sägas vara en diagonalmatris med det ungefärliga värdet 25 i diagonalelementen. Det brus som påverkar mätningen visar sig vara mycket små, varpå R som också valts till en diagonalmatris fått relativt små värden i sin diagonal. Mätningarna av hjulvinklarna visade sig ha cirka tio gånger större brusnivå än mätningen av vinkelhastigheten. Q och R fick då följande utseende där c är en ensam designparameter som användes för att trimma in systmet. 30

31 Q = [ ] [ ] , R = c Genom att sätta ett värde på c, skapa K och simulera systemet trimmades observatören in för att få bra prestanda. Ett bra värde på c visade sig vara 0,005. Beräkningen för K utfördes med Matlabs inbyggda funktion lqe enligt K = lqe(a, B, C, R, Q); Nu kan observatörens systemmatriser skapas enligt ekvation 5.2 för att sedan diskretiseras med matlabkommandot c2d. Reglersystemet uppdateras 250 gånger per sekund, samplingstiden i c2d blir således 0,004s. Det är sedan OBSd som används i tillståndsblocket för att forma observatören i Simulink. OBSc = ss(a-k*c, [B, K], eye(4), zeros(4)); OBSd = c2d(obsc, 0.004); Då observatörsförstärkningen beräknats kunde observatörens poler kontrolleras och det visade sig då att dessa låg en storleksordning längre från origo än vad som använts vid polplaceringen. Polerna för observatören hamnade i [ ,2 + 3,1i 2,2 3,1i 3,3 ] Detta ger indikationer om att det tidiga försöket var helt fel ute Utvärdering Polplacering och kalmanförstärkning är i detta fallet ekvivalenta metoder, det som skiljer är de olika designparametrarna. Samma observatör kan alltså skapas genom att med polplaceringsmetoden placera polerna på platserna som beräknats från kalmanförstärkningen. Skillnaden blir att det i olika fall är olika intuitivt att designa med vissa parametrar, i detta fall visade det sig vara enklare att designa med kalmanförstärkningen. Vid jämförelse mellan de sanna (simulerade) värdena och skattningen från observatören kan det konstateras att de skattade värdena har bra dynamik men har för låg amplitud under insvängningsförloppet. Detta kan förklaras med att observatörens initialvärde är noll medan det simulerade systemets initialvärde är nollskilt. När observatören svängt in följer de skattade värdena de verkliga mycket bra. Figur 5.1 visar θ och ψ och figur 5.2 visar θ och ψ vid insvängningsförloppet. För att se vilka förändringar den nya observatören inneburit gjordes en jämförelse med den observatör som beskrivs i [7] och som roboten från början var utrustad med, denna observatör nämns i denna text som den gamla observatören. Den gamla observatören fungerar genom att ψ integreras och θ deriveras för att skatta ψ och θ. Denna observatör har ingen återkoppling på de skattade värdena, till skillnad mot den nya. Jämförelsen gjordes med en simulering i Simulink i en körsekvens där roboten utsätts för en störning i ψ och ska stå stilla på stället. Regleringen sker med LQG- regulatorn med tillstånd från den nya observatören, den gamla observatören skattar också tillstånden som jämförelse. Figurerna 5.3 och 5.4 visar resultatet där de gröna linjerna är de skattade tillstånden genererade med den gamla observatören och de blåa visar tillstånden från den nya. 31

32 4 3 Theta [rad] Tid [s] Psi [rad] Tid [s] Figur 5.1: Jämförelse av de verkliga tillstånden (grön) och observatörens skattade tillstånd (blå). Värdena på ψ har ungefär samma dynamik och amplitud från bägge observatörerna, det som skiljer är att ψ från den gamla observatören svänger runt ett nollskilt värde trots att den borde svänga runt noll vilket den nya observatören gör. Anledningen till detta är att värdena på ψ från den gamla observatören baseras på en integration av sensorvärdena på ψ utan någon återkoppling. Detta gör att ψ från den gamla observatören blir känsligare för felaktiga värden i ψ. Utseendet för θ skiljer sig mycket åt, den nya observatören ger en betydligt jämnare kurva till skillnad mot den gamla. Även amplituden skiljer sig en del åt i detta fall. Tillstånden θ och ψ följer samma dynamik och har i princip samma amplitud för bägge observatörerna. Den gamla observatören gör ingen behandling alls av dessa tillstånd utan gör enbart enkla beräkningar av sensorvärden för att skatta tillstånd. Den nya observatören har bland annat θ m och ψ som insignaler. Det finns då mycket information om ψ:s och θ:s (på grund av den nära relationen med θ m ) verkliga värde och skattningen av dessa blir bra. Det ska dock sägas att det är svårt att säga om resultaten är bra eller dåliga. Det är kombinationen regulator/observatör som har betydelse för om regleringen ska bli bättre eller sämre. Ovanstående jämförelse visar att den nya observatören skapar mindre svängiga signaler vilket borde vara fördelaktigt. Det är dock svårt att säga vilka amplituder och utseenden som är bättre än några andra. 32

33 6 4 Thetadot [rad/s] Tid [s] Psidot [rad/s] Tid [s] Figur 5.2: Jämförelse av de verkliga tillstånden (grön) och observatörens skattade tillstånd (blå). 5.4 Resultat Den färdiga observatören implementerades i Simulink enligt schema i figur 5.5. De block som ligger till vänster om själva observatören är till för förbehandlingen så att observatören får rätt indata. Där kalibreras gyrot och medelvärdet tas av hjulvinklarna. Blocket Cal gyro_offset används till att kalibrera signalen från gyrot. Modellerna och observatören är uppbyggda utifrån definitionen att ψ och ψ är noll när roboten står stilla och vertikalt. Signalen som kommer från gyrot är däremot av en annan storleksordning, ungefär ψ När roboten startas och hålls upprätt läses kalibreringsvärdet av och signalen justeras sedan i gyrokalibreringsblocket utifrån detta. Själva observatören består av ett tidsdiskret tillståndsblock med systemmatriser som skapats med ett kalmanfilter och diskretiserats enligt avsnitt

34 4 3 2 theta [rad] tid [s] psi [rad] tid [s] Figur 5.3: Jämförelse av skattade tillstånd från den gamla (grön) och den nya (blå) observatören. 34

35 10 thetadot [rad] tid [s] psidot [rad] tid [s] Figur 5.4: Jämförelse av skattade tillstånd från den gamla (grön) och den nya (blå) observatören. 4 u 1 theta_mr 2 theta_ml 3 gyro In1 Out1 In2 Mean pi/180 theta_m deg2rad psidot pi/180 deg2rad1 y(n)=cx(n)+du(n) x(n+1)=ax(n)+bu(n) Observer 1 x_hat 2 theta 3 psi gyro gyro_offset In Cal gyro_offset send to bluetooth Figur 5.5: Observatören tillsammans med de block som krävs för att förbehandla insignalerna. 35

36 Kapitel 6 Regulatorer Kapitlet beskriver de två regulatorerna för att hålla roboten upprätt. Regulatorerna som används är en LQG- och en PID-regulator. Från en dator kan användaren växla mellan dessa två regulatorer. Även en H -regulator försöktes implementeras men tyvärr uteblev ett fungerande resultat. I figur 6.1 visas en översiktsbild över hur regulatorerna NXTway-GS Controller implementeras i Simulink med dess omgivning. Calculate PWM duty to minimize the difference between reference and measured value. thetadot_cmd 1 thetadot_cmd pwm_turn pwm_turn 2 phidot_cmd [phidot_cmd] phidot_cmd x_ref Cal Reference x_ref x_hat vol LQG U PID U U vol pwm_l 1 LQG Regulator Subsystem theta_m_l pwm_l theta_m_r Scope theta_ref [phidot_cmd] phidot_cmd pwm_r 2 pwm_r 4 theta_m_r theta_mr x_hat psi_ref battery battery Cal PWM 3 theta_ml vol theta_m_l 5 gyro theta theta gyro u psi psi Observer 3 psi PID Figur 6.1: Översiktsbild över de två regulatorerna och dess omgivning i Simulink. 6.1 LQG Den första regulatorn som implementerades var av typen LQG (Linear Quadratic Gaussian). Teori till avsnittet är hämtat från [2]. Målet 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 (6.1) där e är reglerfelet och u är insignalerna. Observatören i kapitel 5 skattar tillstånden och antalet skattade tillstånd är fyra. Insignaler till regulatorn är de skattade tillstånden och referenssignalen. I regulatorn utförs en LQ-reglering och en översiktsbild över denna regulator finns i figur

37 Den optimala linjära regulatorn ges av följande ekvationer där y är robotens utsignaler. u(t) = L ˆx(t) (6.2a) ˆx(t) = A ˆx(t) + Bu(t) + K(y(t) C ˆx(t)) (6.2b) 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 4 4. Q 2 bestämmer viktningen mellan de olika insignalerna till systemet. I det här fallet är det två insignaler och följaktligen är dimensionen 2 2. Förhållandet mellan Q 1 och Q 2 bestämmer hur stora insignalerna ska vara i förhållande till reglerfelen. L är förstärkningen på återkopplingsmatrisen enligt ekvation (6.2). Detta L beräknas med ekvationen L = Q 1 2 BT S. (6.3) Med hjälp av riccatiekvationen 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 (6.4) där M är hur man vill att utsignalerna ska uppföra sig. 1 x_ref x_hat L Regulator u u Robot y y _obs u_obs Observatör Figur 6.2: Skiss över regulatorstrukturen för LQ-reglering Implementation Nedan beskrivs i vilken ordning arbetet utfördes. 1. Q 1 och Q 2 bestämdes. Båda matriserna sattes till 1 i deras diagonal element. 2. Riccatiekvationen (6.4) löstes. 3. L beräknades med ekvation (6.3). 4. Regulatorn implementerades i Simulink. 5. Regulatorn testades i simuleringsmiljö. 6. Resultaten analyserades och om beteendet inte var önskvärt började processen om på steg 1 och Q 1 och Q 2 ändrades. 7. Regulatorn testades på roboten. I figur 6.3 visas hur regulatorn implementerades i Simulink. 37

38 1 x_ref L_lqg* u L_LQG 1 vol 2 x_hat Figur 6.3: LQG-regulatorns implementation i Simulink Resultat I Q 1 straffades lutningsvinkeln ψ mest, snitthjulvinkel θ straffades lite och de övriga sattes till 1 eftersom de inte har så stor inverkan. Snitthjulvinkeln straffas för att roboten skall stå still och lutningsvinkeln för att den inte skall luta så mycket fram och tillbaka. Insignalen straffas i Q 2 för att hålla värdena inom det tillåtna intervallet för motorerna. Detta gav slutligen dessa matriser Q 1 = (6.5) [ ] Q 2 = (6.6) Dessa straffmatriser och tillståndsmodellen används för att beräkna regulatorn i Matlab med kommandot LQR [LL_lqg,S,E] = LQR(A,B,Q1,Q2,N) där N antogs vara 0 då insignalerna och utsignalerna kunde straffas var för sig. Detta ger en regulator med följande värde på L LQG L LQG = [ 2, ,0616 2,0445 4,9597 ]. (6.7) I avsnitt 6.5 görs en jämförelse med denna regulator mot PID-regulatorn. 6.2 PID Den andra regulatorn som implementerades var en PID-regulator (Proportional Integrative Derivative). Teori till avsnittet är hämtat från [3]. Regulatorn bestäms enligt u(t) = K p e(t) + K i t 0 d e(τ)dτ + K d e(t) (6.8) dt K p styr den proportionella regleringen mot reglerfelet. K i styr den integrerande delen och K d relaterar till den deriverande delen. K p styr i första hand snabbheten på systemet. K i reducerar det stationära felet och K d dämpar överslängen. För att undvika oändlig förstärkning implementeras ett lågpassfilter på den deriverande delen i Matlab. De två tillstånden som PID-regulatorn reglerar på är snitthjulvinkeln och lutningsvinkeln. 38

39 6.2.1 Implementation Nedan följer ordningen i vilket arbetet utfördes. 1. K p ansattes så att systemet blev tillräckligt snabbt. 2. K i och K d ansattes så att det stationära felet och dämpningen reducerades. 3. Simulering gjordes och analyserades, om resultatet inte var önskvärt började processen om på steg 1 med andra parametrar. 4. Regulatorn implementerades på robot och resultatet analyserades. I figurerna 6.4 och 6.5 visas hur PID-regulatorn implementerades i Simulink. I figur 6.4 visas hur hela PID-regulatorn implementerades medan figur 6.5 visar den specifika regulatorn för lutningsvinkeln. Snitthjulvinkeln implementerades på ett liknande sätt. 2 psi_ref 4 psi psi In1 Out1 1 theta_ref Discrete PID Psi 1 vol 3 theta psi In1 Out1 Discrete PID Theta Figur 6.4: Översiktsbild för PID-regulatorerna. 50 P 1 In1 20 I in out Discrete Integrator (forward euler) Add 1 Out1 2 in out D Discrete Derivative (backward difference) Figur 6.5: PID-regulatorn för ψ. 39

40 6.2.2 Resultat Lutningsvinkeln regleras hårdast eftersom det är viktigt att roboten inte lutar för mycket. Anledningen till att snitthjulvinkeln också regleras är för att roboten inte skall driva för mycket. Reglervariablerna fick följande värden K p = 50 K p = 2.5 ψ : K i = 20 θ : K i = 0 (6.9) K d = 2 K d = 0.8 I kapitel 6.5 görs en jämförelse mellan denna regulator och LQG-regulatorn. 6.3 H Teori för avsnittet hämtat från [2]. I figur 6.6 syns strukturen för systemet med en H - reglering. Referenssignalen skickas in i F y tillsammans med utsignalen y. W u, W t och W s är viktfunktioner som implementerar kraven för regulatorn. Signalerna z 1, z 2 och z 3 mäter hur väl kraven uppfylls. Med hjälp av den här regulatorstrukturen väljs vid vilka frekvenser till exempel mät- och systembrus skall straffas. Målet med H -regulatorn är att minimera H -normen för G ec, enligt följande G ec = max ω σ(g ec(iω)) (6.10) där σ(g ec (iω)) är det största singulära värdet av G ec (iω). 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 (6.11) W S S Den sökta H -regulatorn ska uppfylla En konsekvens av (6.11) och (6.12) är att G ec < γ (6.12) W S (iω)s(iω) < γ ω (6.13a) W T (iω)t (iω) < γ ω (6.13b) W u (iω)g wu (iω) < γ ω (6.13c) γ skall väljas så litet som möjligt. Om riccatiekvationen (6.15) saknar lösning så ökas γ tills en lösning hittas. Bara för att ett γ hittas som uppfyller kraven i (6.13) betyder inte det att kraven i (6.12) är uppfyllda eftersom (6.12) är ett hårdare krav än (6.13). För de tre viktningsmatriserna sker viktningen primärt på diagonalelementen eftersom ingen eller endast en liten korskoppling mellan hjulen antogs föreligga. W S är viktningsmatrisen för känslighetsfunktionen (från systembrus till utsignal). För att bestämma W s gjordes en signalanalys på de två mätsignalerna. Signalanalysen gjordes för att bestämma vid vilken frekvens mest störningar fanns. 40

41 För att ta fram lämpligt straff på komplementära känslighetsfunktionen togs systemets poler fram och systemets bandbredd analyserades. Förhållandet mellan känslighetsfunktionen S och den komplementära känslighetsfunktionen T är T = I S. (6.14) W u är viktningsmatrisen för G wu. Den bestämmer hur de olika styrsignalerna ska straffas vid olika frekvenser. Till att börja med sattes dessa till 1 där tanken var att trimma dessa värden genom att analysera styrsignalerna utifrån givna W t och W s. Följande riccatiekvation löses A T S + SA + M T M + S(γ 2 NN T BB T )S = 0 (6.15) där A och B fås ifrån modellen för roboten och N är hur mätbruset påverkar utsignalen. M är matrisen för hur utsignalerna är kopplade till tillstånden. Den har en positiv semidefinit lösning S = S γ som uppfyller att är stabil. Då kan L beräknas enligt följande A BB T S γ (6.16) H -regulatorn blir då L = B T S γ. (6.17) ˆx(t) = A ˆx(t) + Bu(t) + N(y(t) C ˆx(t)) u(t) = L ˆx(t). (6.18a) (6.18b) 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 6.6: Skiss över regulatorstrukturen för H -reglering Implementation Nedan följer ordningen i vilket arbetet utfördes. 1. Överföringsfunktionen för systemet, G, beräknades med hjälp av tillståndsbeskrivningen. 2. W u, W S och W T bestämdes 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 togs fram genom analyseringen av systemet och dess mätsignler som angivits i kapitel

42 3. Ett γ ansattes. 4. Ekvationerna (6.15)-(6.17) löstes. Om ingen lösning fanns provades att öka γ. Steg 3 och 4 gjordes via matlabkommandot hinfsyn. 5. Regulatorn implementerades i Simulink. 6. Regulatorn testades i simuleringsmiljö. 7. Resultatet analyserades och om beteendet inte var önskvärt börjades processen om på steg Regulatorn testades på roboten Resultat På mätsignalerna gjordes frekvensanalys. Detta gjordes genom att medelvärdet och/eller trenden på mätsignalerna eliminerades. Signalen fönstrades sedan genom matlab-kommandona bartlett och fft. Resultatet av detta visas i figur 6.7. Genom att använda kommandona iddata och etfe togs de exakta frekvenserna ut till 30,72Hz för gyrot och 4,5Hz för motorvinkelsensorerna. I figur 6.7 ses resultatet av frekvensanalysen där toppar träder fram för givna frekvenser. Resultatet används till att straffa känslighetsfunktionen vid rätt frekvenser. Figur 6.7: Signalbehandling av bruset. I övre bilden syns gyrobruset, med en topp på 30,72Hz och i undre bilden syns motorbruset, med en topp på 4,5Hz. 42

43 För att analysera systemet togs dess poler fram. Polerna för systemet blev följande (6.19) Det visade sig att systemet hade svårhanterade poler på två ställen. Det första var de två positiva polerna som gör att systemet är instabilt. För att ta hand om detta sätts den komplementära känslighetsfunktionen så att den får en bandbredd på minst det dubbla enligt en tumregel i [2]. Den andra svårhanterade polen är den i 0. Detta betyder att systemet innehåller en ren integrator. Då kommandot hinfsyn i Matlab inte fungerar med ett system med en pol i origo så försöktes denna pol att flyttas in lite i vänstra halvplanet. Detta försök lyckades inte då hinfsyn hela tiden gav tomma värden till regulatorn. Tyvärr lyckades inte en fungerande H implementeras. Om det hade lyckats är det troligt att den hade varit något bättre än LQG i och med att systemet då kan regleras så mycket det tillåts fysikaliskt. 6.4 Stopp av motorerna I roboten implementerades två olika säkerhetssystem för att stoppa motorerna. Det första stoppar motorerna om roboten lyfts upp. När roboten lyfts upp så detekterar både främre och bakre ljusintensitetssensorer samma tröskelvärde som för ett stup, då sätts en flagga om att roboten har lyfts upp. När denna flagga sätts så stoppas motorerna och flaggan skickas till GUI:t som ger ett meddelande om att roboten har lyfts upp. Det andra säkerhetssystemet stoppar motorerna om roboten välter. När roboten överstiger 10 grader i lutningsvinkel så mäts tiden den varit i över denna lutning. Om tiden överstiger tröskelvärdet för tiden så sätts en flagga om att roboten har vält och motorerna stoppas. Flaggan skickas till GUI:t som ger ett meddelande om att roboten har vält. 6.5 Utvärdering av regulatorerna De singulära värdena för systemet ses i figur 6.8. Där ses att för alla frekvenser är det en stor skillnad i förstärkning, vilket innebär att systemet är relativt frekvensoberoende. I figuren ses att singuläravärdena aldrig överstiger 0 db vilket betyder att förstärkningen är 1. I figur 6.9 visas resultatet av LQG-regulatorn. Resultat blir att roboten variera mellan -2 och 2 grader med några små toppar utanför intervallet. I figur 6.10 visas resultatet av PID-regulatorn. Resultat blev nästan detsamma som för LQG-regulatorn. Med den skillnaden att LQG-regulatorn inte är riktigt lika svängig som PID-regulatorn. På roboten uppvisar LQG-regulatorn ett mer stabilt beteende än PID-regulatorn. Det beror på att den är konstruerad enligt en modell av systemet. LQG-regulatorn klarar av större störningar både när det gäller knuffar och när det gäller ändring av hastighet. När det gäller att röra sig längsgående vid stillastående står LQG-regulatorn mer stilla 43

44 Figur 6.8: De singulära värdena för det linjäriserade systemet, G, för roboten. Figur 6.9: LQG-regulatorn testad i roboten. än vad PID-regulatorn gör. De två regulatorerna som har tagits fram i projektet uppvisar ett bättre resultat än den regulator som fanns från början av projektet. Den observatör som har tagits fram i projektet är också bättre än den som fanns i roboten från början vilket är en del i förklaringen till att de framtagna regulatorerna är bättre. 44

45 Figur 6.10: PID-regulatorn testad i roboten. 45