Teknisk rapport. Remotely Operated Underwater Vehicle. Version 1.0 Martin Lindfors ROV. Status

Transkript

1 Remotely Operated Underwater Vehicle Martin Lindfors ROV Status Granskad PK, MM, JK Godkänd Isak Nielsen

2 ii Projektidentitet E-postlista till gruppen: Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: Isak Nielsen, ISY, Telefon: +46(0) E-post: Micael Derelöv, Saab Dynamics, Underwater Systems Telefon: +46(0) E-post: Daniel Axehill, ISY, Telefon: +46(0) E-post: Malte Moritz Telefon: E-post: Jonas Linder, ISY, Telefon: +46(0) E-post: Gruppmedlemmar Namn Ansvar Telefon E-post Malte Moritz (MM) Projektledare malmo541 Kristoffer Bergman (KB) Testansvarig kribe606 Johan Karlén (JK) Modellansvarig johka641 Per-Erik Karlsson (PK) Mjukvaruansvarig perka625 Martin Lindfors (ML) Designansvarig marli984 Tobias Magnusson (TM) Dokumentansvarig tobma696 Katarina Mollén (KM) Informationsansvarig katmo425 Jacob Svensson (JS) Hårdvaruansvarig jacsv832 Fredrik Söderstedt (FS) Kommunikationsansvarig freso273

3 iii Dokumenthistorik Version Datum Ändringar Utförda av Granskad 1.0 Första versionen Alla PK, MM, JK Tredje utkastet Alla JS, KM, FS Andra utkastet Alla KM, PK, JS Första utkastet Alla Alla

4 iv Innehåll 1 Inledning Syfte och mål Användning Notation Översikt av systemet Modell Hårdvara Intern PC Sensorer Extern PC Hårdvara Arduinokortet Trycksensor Upplösning i Arduinon Omvandling till tryck IMU och magnetometer Xsens MTi APM 2.6 med extern kompass Magnetometerns position SONAR Delta-T Läckagesensor Motorer Intern PC Övrig hårdvara Resultat och diskussion Semifysikalisk modell Koordinatsystem Förenklingar Parameterskattning Framtagning av modell Resultat och diskussion Skattning av modellparametrar Tester Roll-dynamik Pitch-dynamik Yaw-dynamik Parameterskattning Translationsdynamik Rotationsdynamik Validering Resultat och diskussion

5 v 6 Delsystem Sensorfusion Observatör Mätuppdateringar Mätuppdatering SONAR Mätuppdatering trycksensor Mätuppdatering accelerometer Mätuppdatering magnetometer Mätuppdatering gyro Signalbehandling SONAR-data Resultat och diskussion Delsystem Reglering Decentraliserad regulator LQ-regulator Linjärisering av ROV:n Val av regulatorparametrar Resultat och diskussion Mjukvara ROS Implementerade noder Reglering Sensorfusion Kommunikation GUI Arduino IMU DeltaT Menyval P Datagram Externa kontrollkommandon Resultat och diskussion A Kopplingsschema Arduino-kort 42 B ROS 43 B.1 Noder och topics B.2 Skriva koden B.3 Bygga koden B.4 Köra koden C Topics och messages 44

6 1 1 Inledning Detta dokument innehåller en detaljerad beskrivning av hela ROV-systemet samt dess delsystem. Plattformen som vidareutvecklats är designad och konstruerad på universitetet och består av en torpedliknande ubåt som är utrustad med styrsystem, motorer och ett antal olika sensorer. Inom såväl civila som militära tillämpningar ökar intresset och behovet av autonoma farkoster som kan utföra uppdrag till sjöss, i luften och på land utan kontakt med en operatör. För en undervattensfarkost kan ett sådant uppdrag till exempel vara att detektera och desarmera minor. 1.1 Syfte och mål Det övergripande syftet med detta projekt var att få kunskap om och erfarenhet av utveckling av dessa undervattensfarkoster. Projektet är en del av ett större projekt vars långsiktiga mål är att utveckla en helt autonom farkost som kan vara med i den europeiska tävlingen för autonoma undervattensfarkoster SAUC-E. Tävlingen går ut på att farkosten ska kunna utföra vissa givna uppdrag på så kort tid som möjligt. För att lyckas med detta så behöver den befintliga ROV:n utvecklas vidare till en helt autonom farkost som klarar av att orientera sig i sin omgivning. Dessutom måste den vara utrustad med hård- och mjukvara som klarar av de uppgifter som tilldelas. Det kortsiktiga målet med projektet var att utveckla ett robust styrsystem för en väl fungerande reglering och navigering för ROV:n. Regulatorprestandan skulle utvärderas efter implementation av både decentraliserad reglulator för referensföljning av vinkelhastigheter samt en regulator av mer avancerad karaktär för att reglera djup och orientering. En observatör för skattning av vinklar och djup skulle implementeras, och funktionaliteten och precisionen skulle uppfylla de krav som var givna i kravspecifikationen, Malte Moritz m.fl. [6]. Den korrekt framtagna modellen av ROV:n, som togs fram i slutet av förra projektet, skulle utvärderas och parametriseras. SONAR-sensorer skulle integreras och användas vid positionsskattnig. Annan hårdvara som skulle integreras var ett nytt styr- och mätkort samt en trycksensor för djupmätning. En detaljerad genomgång av kraven återfinns i kravspecifikationen för projektet, Malte Moritz m.fl. [6]. 1.2 Användning Systemet är tänkt att användas i undervattensmiljöer med hårda krav på precis manöverförmåga, exempelvis i tävlingen SAUC-E nämnd ovan. 1.3 Notation ROV AUV SAUC-E LQ ROS SONAR IMU EKF GUI PID Remotely Operated Vehicle Autonomous Underwater Vehicle Student AUV Challenge Europe Linear Quadratic Robot Operating System SOund Navigation And Ranging Inertial Measurement Unit Extended Kalman Filter Graphical User Interface Proportionell-Integral-Derivata

7 2 2 Översikt av systemet I figur 1 nedan åskådliggörs systemet i form av ett blockdiagram. En Xbox-kontroll kommunicerar med den externa PC:n, som i sin tur är kopplad till ROV:n via en Ethernetkabel. Den interna PC:n är uppdelad i två delsystem, Reglering och Sensorfusion, samt ytterligare mjukvara nödvändig för kommunikation med bland annat sensorer, som alla kan kommunicera internt med varandra. Dessutom finns det ett antal olika sensorer, ett styr- och mätkort samt motorer. Extern PC Xbox-kontroll ROV Intern PC Ethernet Reglering Sensorfusion IMU SONAR Motorer Styr- & mätkort Trycksensor Figur 1: En översikt av systemet. Streckade block motsvarar avgränsningar i mjukvara, heldragna rundade block motsvarar avgränsningar i ROV:ns inbyggda hårdvara medan heldragna kantiga block motsvarar avgränsningen mellan ROV och extern PC. ROV:n kan köras i två olika lägen: manuellt eller stabilt. I manuellt läge väljer operatören pådraget till varje motor individuellt. I stabilt läge kopplas en regulator in. Två regulatorer kan väljas: en enkel regulator, benämnd decentraliserad regulator, samt en avancerad, benämnd linjärkvadratisk. I det första fallet styr operatören vinkelhastigheter och translation. I det andra fallet styr operatören vinklar. 2.1 Modell Den modell som används för att modellera systemet är framtagen med hjälp av Fossen [3] och sedan vidareutvecklad av projektgruppen. Den modell som användes i föregående projekt frångicks, då den visade sig innehålla en del felaktigheter. Parametrar för modellen har skattats under projektets gång. Mer information om modellen finns under kapitel Hårdvara Hårdvaran på ROV:n utgörs av en intern PC, en mikrokontroller med tillhörande Arduinokort, fem styrkort, en trycksensor, en IMU samt en läckagesensor. ROV:n är också utrustad med fem thrusterpropellrar. En thruster är riktad bakåt, två nedåt och två åt sidan. Utöver det finns ett antal batterier, två lampor samt en kamera. Mer information om hårdvaran på ROV:n finns under kapitel 3.

8 3 2.3 Intern PC På den interna PC:n finns mjukvara som hanterar reglering, navigering och kommunikation. Den interna PC:n på ROV:n körs i grund och botten med ett Linuxbaserat operativsystem. Den interna PC:n är uppdelad i två delsystem: Reglering Sensorfusion samt infrastruktur för att kommunicera med sensorer och hårdvara. Kommunikationen mellan delsystemen och infrastrukturen går via operativsystemet ROS (Robot Operating System) som ligger ovanpå det Linuxbaserade operativsystemet. Mer information om den interna PC:ns mjukvara hittas i kapitel Sensorer För att kunna skatta ROV:ns orientering och position används tre olika sensorer: en IMU med inbyggd magnetometer, en trycksensor och en SONAR. Skattningarna sker med hjälp av ett extended Kalmanfilter. SONAR-datan förbehandlas dessutom i flera steg. Mer om hur skattningarna görs är beskrivet i kapitel Extern PC En extern PC, bestående av en laptop med operativsystemet Linux, finns ansluten till ROV:n via en Ethernetkabel. Den externa PC:n övervakar läckagesensorn och meddelar ROV:n om läckage indikeras. I PC:ns GUI kan användaren avläsa ROV:ns position och batterinivåer. Operatören kan också välja driftläge och även stoppa ROV:n för att föra upp den till ytan i en nödsituation. Dessutom kan referenssignaler och parametrar för de implementerade regulatorerna ändras av användaren via GUI:t. Operatören av systemet kan styra ROV:n genom antingen den externa PC:ns GUI, eller med hjälp av en Xbox-kontroll som går att ansluta till den externa PC:n.

9 4 3 Hårdvara Hårdvaran på ROV:n utgörs av en PC (benämnd den interna PC:n), ett Arduinokort, fem styrkort, en trycksensor, en IMU med magnetometer, en SONAR samt en läckagesensor. Utöver det finns ett antal batterier, thrusterpropellrar, lampor, reläer och en kamera. I detta kapitel kommer Arduinokortet, sensorerna, propellrarna och PC:n att beskrivas. En schematisk skiss av de ingående komponenterna och hur de kommunicerar med varandra ses i figur 2. Extern PC Magnetometer ROV Ethernet IMU USB Trycksensor Läckagesensor Intern PC USB Arduino Ethernet Styrkort Batterispänning SONAR Motorer Figur 2: En översikt av hur de olika hårdvarukomponenterna är sammankopplade. Den externa PC:n och SONAR:en är kopplad till interna PC:n via varsin ethernetkabel. Den interna PC:n kommunicerar via USB med IMU:n och Arduinon. Till Arduinon är tryck- och läckagesensorn, styrkorten och batterispänningarna kopplade. Riktningen på pilarna motsvarar åt vilket håll informationen flödar. 3.1 Arduinokortet Styr- och mätkortet som sitter i ROV:en är av typen Arduino Mega Kortet bygger på mikrokontrollern ATmega2560 som har utrustats med bland annat USB-anslutning, strömanslutning, 16MHz oscillator och en återställningsknapp. Arduinokortet har analoga ingångar och utgångar som används för att mäta sensordata och för att skicka styrsignaler till motorernas styrkort. Arduinon kommunicerar seriellt med den interna PC:n med hjälp av ROS (som beskrivs mer under kapitel 8). Arduinon programmeras med en modifierad variant av C/C++ som bland annat har tillagda funktionsprototyper. En översikt över inoch utgångarna på kortet kan ses i bilaga A.

10 5 3.2 Trycksensor Trycksensorn som sitter på ROV:en är från serien PX2 och är tillverkad av Honeywell. Den är placerad på den delen av ROV:ns tryckkärl (höljet som innesluter elektroniken) som är riktad mot aktern. Utsignalen från trycksensorn är en spänning som är proportionell mot trycket och kan anta värden mellan 10 % och 90 % av matningsspänningen. Lägsta utsignalen svarar mot ett tryck på 0 Pa medan maximala utsignalen svarar mot 689 kpa (100 psi). Med en matningsspänning på 5 V och antagandet att sambandet är linjärt erhålls U = k p + c (3.1) där U är uppmätt spänning och p är aktuellt tryck. Trycket är summan av lufttrycket och vattentrycket. k och c är konstanter som enkelt kan beräknas med vetskapen om att 0 Pa svarar mot 0,5 V och 689 kpa mot 4.5 V. Det slutgiltiga sambandet mellan utspänning och tryck ges av U = p + 0.5, 0 p (3.2) Upplösning i Arduinon Trycksensorn är kopplad till en A/D-omvandlare på Arduinokortet. A/D-omvandlaren har en upplösning på 10 bitar, det vill säga 1024 steg, vilket svarar mot en spänning mellan 0 V och en valbar referensspänning V ref. Med detta givet blir upplösningen i A/D-omvandlingen V ref 1024 (3.3) Med referensspänningen satt till 5V innebär det att upplösningen blir 5 mv. Instoppat i (3.2) motsvarar det 984 Pa. För att få en uppfattning om det är tillräckligt bra kan motsvarande djupupplösning beräknas via sambandet p = ρhg (3.4) där ρ är vattnets densitet (998 kg/m 3 ), h är vattendjupet och g är tyngdaccelerationen (9,82 m/s 2 ). Med värden instoppade erhålls en upplösning på 10,0 cm. Det maximala trycket som kan mätas med den inställningen blir med (3.2) 689 kpa. Med antagandet att lufttrycket är 101 kpa (SMHI [7]) ges det maximala djupet som kan mätas av (3.4), det vill säga 59 m Omvandling till tryck Med vetskapen om upplösningen hos A/D-omvandlaren, dess största och minsta möjliga spänningar samt (3.2), kan resultatet mappas från A/D-omvandlaren till tryck. I tabellen nedan visas ytterligheterna i mappningen. Den övre gränsen på 5 V sätts av referensspänningen medan den undre gränsen på 0,5 V är den minimala spänningen trycksensorn kan ge ifrån sig. Tabell 1: Omvandlingstabell från binärt tal till spänning, tryck och vattendjup. Binärt tal Omvandlad spänning Uppmätt tryck Motsvarande vattendjup 100 0,50 V 0 kpa - (vakum) 204 1,02 V 101kPa 0 m 900 4,5 V 689 kpa 59 m ,0 V 689 kpa 59 m

11 6 Det linjära, kvantiserade sambandet mellan binärt tal och tryck blir således 0 b 2 < 100 p = 861 (b 2 100) 100 b b 2 > 900 (3.5) där b 2 är det binära talet. 3.3 IMU och magnetometer IMU:n (Internal Measurement Unit) med tillhörande magnetometer används för att mäta vinkelhastigheter, vinkelacceleration och magnetfält. Sedan tidigare var det känt att dess magnetometer blir utsatt för störningar i ROV:n. Den största störningskällan har enligt Bernhard & Johansson [1] varit ROV:ns motorer. Dock har ingen utredning gjort om även spolar, hårddiskar, kablar och reläer bidragit till störningar. Därför gjordes mätningar för att hitta en lämplig placering som både tog hänsyn till att minimera störningar samtidigt som det är fördelaktigt ur mätsynvinkel att ha en placering nära ROV:ns masscentrum, samt att den nuvarande IMU:n inte är vattentålig. För att underlätta placeringen av IMU:n och magnetometern köptes en IMU med extern magnetometer in, dock hann denna inte integreras i ROV:n Xsens MTi Denna IMU var vid projektstart redan integrerad i systemet. Xsens:ens styrka är att den har noggranna sensorer med lågt mätbrus samt ett väldefinierat gränssnitt genom USB. Dock har den ingen extern kompass, vilket medför att magnetometern störs ut i dess nuvarande position. IMU:n har egna algoritmer för att med hjälp av sensorfusion beräkna orientering, men i detta projekt används enbart rådatan. Den kalibrerade rådatan skickas till interna PC:n enligt tabell 2. Tabell 2: Tabell över datan som skickas från Xsens MTi. accx accy accz gyrx gyry gyrz magx magy magz TS där varje värde är ett 32-bitars flyttal. Accelerationen mäts i m/s 2, vinkelhastigheten mäts i rad/s och magnetfältet är en arbiträr enhet normaliserad mot jordens magnetfält. TS representerar tidpunkten APM 2.6 med extern kompass Den andra lösningen bygger på ett separat kort, ArduPilot APM 2.6, som har en inbyggd krets med både accelerometer och gyrometer (MPU-6000). Till kortet kopplas en extern magnetometer (HMC5883L) som kommunicerar med sladd via I2C och som därför kan placeras i valfritt ställe i ROV:en. Placeringen kan därför väljas så att de magnetiska störningarna är små. Varje sensor mäter värden i x-, y- och z-led vilket gör att kortet totalt har nio frihetsgrader. APM:en kommunicerar med den interna PC:n via USB och kommer likt Xsens MTi strömma rå sensordata till PC:n. Kortet har även 8 st ingångar och 8 st utgångar som går att använda på liknande sätt som det nuvarande styr-och mätkortet. Det möjliggör för att i framtiden flytta funktionaliteten från Arduinon och IMU:n till ett enda kort Magnetometerns position IMU:ns magnetometer kan vara väldigt känslig för störningar av magnetfältet. Därför gjordes en mätning i början av projektet för att ge en bild av hur stora störningar olika platser har i och utanför ROV:en.

12 7 Figur 3 visar resultatet av mätningarna och av dessa valdes position 5, då det var den placering som gav minst störningar på det uppmätta magnetfältet av de placeringar som var möjliga med nuvarande IMU och magnetometer Absolutbeloppet av magnetfã ltet Pos1: 4cm ovan ROV:en, 3/4 fram Pos2: 4cm ovan ROV:en, fram Pos3: 4cm ovan ROV:en, mitten Pos4: I taket innuti ROV:en, fram Pos5: I taket innuti ROV:en, 3/4 fram Pos6: Tidigare placering Pos7: Placering framme vid SONAR Tid [s] Figur 3: Mätningar av absolutbeloppet av det magnetiska fältet på olika platser i och utanför ROV:en. På samtliga positioner kördes motorerna på hög effekt under en kort tid en bit in i mätningen. 3.4 SONAR Delta-T 837 SONAR:en som används i detta projekt tillhandahölls av Saab Dynamics, Underwater Systems, och modellen är Delta-T 837 tillverkad av Imagenex. Placeringen är längst fram i ROV:en och den läser av omgivningen i horizontelt led längst ROV:ens primära rörelseriktning. SONAR:en skickar ut 20 ping per sekund och mäter därefter hur lång tid det tar för att få svar för varje av de 120 diskreta vinklarna. Det avsökta området täcker 120 spridning i yaw-led och 0.75 i pitch-led. Den har en hårdkodad ip-adress som är och ansluts till samma lokala nätverk som den externa och interna PC:n.

13 8 Figur 4: Översikt över hur SONAR:en kopplas in enligt databladet. Trigger-funktionen används ej i detta projekt. För att ansluta SONAR:en används en specialgjord kabel (figur 4) som har åtta ledare och en vattentät kontakt som bör smörjas in med vaselin för att undvika att den fastnar. I andra änden ansluts fyra av ledarna till en Ethernetkabel och två av ledarna till en spänningskälla. Spänningskällan ska vara rippelfri och leverera spänningar mellan VDC och klara 12 W. Under detta projekt har SONAR:en strömsatts med en spänningskub som stått på land, men den borde med fördel kunna kopplas in direkt till den interna PC:ns nätaggregat. Eftersom SONAR:en är känslig för spänningsförändringar skulle det kunna vara aktuellt att koppla in ett filter vid spänningsmatningen för att undvika mätstörningar. När SONAR:en är strömsatt börjar mätningarna omedelbart och den måste därför befinna sig i vatten för att inte gå sönder. Av mätdatan skapas därefter ett 83P UDP datagram som skickas vidare till den interna PC:n. För mer information om datagrammet och mjukvaran som behövs för att tolka data, se kapitel Läckagesensor Läckagesensorn är utvecklad i tidigare projekt kring ROV:n men förbättras av en av projektgrupperna på IEI under projektet. Den är placerad under batterierna och ska detektera om vatten har trängt in i farkosten. Sensorn består av ett motstånd vilket spänningen mäts över. Då inget läckage förekommer är denna spänning 4.8 V. När vatten kommer i kontakt med sensorn jordas motståndet via vattnet och spänningen över motståndet sjunker. Spänningen över motståndet är utsignalen från sensorn och avläses av Arduinon. Spänningsmätningen skickas vidare till interna PC:n och sedan till GUI:t. En mer detaljerad beskrivning av läckagesensorn finns att tillgå i Bernhard & Johansson [1, sid 18]. 3.6 Motorer På ROV:n finns fem thrusterpropellrar. En schematisk skiss över dess placering kan ses i figur 5. Propellern längst bak styr surge-rörelsen (u) medan två är placerade i vertikalled och styr sway-rörelsen (v) tillika yaw-rotationen (r) och två är placerade i horisontalled och styr heave-rörelsen (w) tillika pitch-rotationen (q). Detta är möjligt tack vare att propellrarna genererar samma kraft oavsett vilket håll de körs åt enligt Bernhard & Johansson [1, sid 15]. ROV:n har alltså fem styrbara frihetsgrader.

14 9 B&G x y z B G x Figur 5: Motorernas placering på ROV:n, markerade med en röd rektangel. B anger var flytkraften påverkar ROV:n och G visar var ROV:ns tyngdpunkt är. Observera att figuren inte är skalenlig. Varje motor är kopplat till ett styrkort. Styrkortets uppgift är att omvandla den pulsbreddsmodulerade signalen från Arduino-kortet till en elektrisk signal med spänning och ström anpassad till motorn. De olika motorerna kräver olika mycket ström och styrkorten är individuellt anpassade till varje motor. Styrkorten kan dessutom stängas av och sättas på via reläer. Dessa reläer går att styra från Arduinon via de digitala utgångarna. 3.7 Intern PC Den interna PC:n innehåller Processor Intel D525MW Minne Kingston 2x2 GB DDR3 PC MHz Hårddisk OCZ 60 GB Vertex II E-series Mjukvaran på interna PC:n beskrivs närmare i kapitel Övrig hårdvara På ROV:n finns även följande hårdvara: Webbkamera längst fram på ROV:n som ansluts via USB till den interna PC:n. Lampor längst fram som styrs via Arduinon och lyser upp för kameran. Totalt 6 st batteripaket som ger ca 24 volt och spänningssätter hela ROV:n. 3.9 Resultat och diskussion Årets projekt upplevde inte lika stora problem som tidigare grupper med magnetometern och störningar, men det påverkar prestandan och till exempel har lookup-tabeller använts för att få bättre resultat. För att minska magnetfältet i ROV:n och på så sätt minska störningarna ytterligare skulle man kunna flytta den överdimensionerade säkerhetsmagneten eller hitta en annan lösning till säkerhetsbrytare. När det är bestämt vilken magnetometer som ska användas går det även att placera magnetometern utanför ROV:en för att på så sätt komma längre ifrån störningarna.

15 10 SONAR:en är redo att kopplas in direkt till ROV:en, men för att göra det behövs en kortare sladd med en ethernetkontakt och strömförsörjningskontakt införskaffas. Det behövs även en mindre hub i ROV:en eftersom tre nätverkskablar möts. Vi blev varnade om att SONAR:en är väldigt känslig för störningar vid strömförsörjningen och därför så kan ett filter behövas kopplas in. Detta var inte något vi märkte av men kan vara bra att ha i åtanke om resultaten inte blir som förväntat. Trycksensorn använder inte sin fulla noggrannhet eftersom vårt intresseintervall är mellan 1 och 2 V medan vi mäter spänningar mellan 0 och 5 V. Ett sätt att halvera onoggrannheten är att ändra referensspänningen på Arduinon till 2.56 V när den ska göra trycksensormätningar. Dock finns en intern kapacitans inkopplad som gör att de första mätningarna troligtvis kommer att bli fel varje gång referensspänningen ändras. Detta kan lösas med lite programmering. Ett annat sätt att öka noggrannheten kan vara att använda externa komponenter, vilket vi inte gjort för att vi prioriterade grundfunktionaliteten och försökte minimera antalet felkällor. Det går troligtvis även att integrera trycksensorn med APM:en som då kan ha en egen referensspänning. IMU:n från Xsens tillhör inte projektet och måste därför bytas ut. En APM köptes under projektet. Denna har både extern magnetometer, möjlighet att koppla in GPS samt in- och utportar. Alltså kan den ersätta både den nuvarande IMU:n och Arduinon. För att komma dit måste det dock undersökas hur ROS integreras på ett smidigt sätt med APM:en, vilket det inte fanns tid för i detta projekt.

16 11 4 Semifysikalisk modell Den modell som har använts i projektet är framtagen från grunden enligt Fossen [3]. Detta har inneburit att den korrekta modellen framtagen i Patricia Sundin m.fl. [5] till viss del har frångåtts, detta för att den tidigare modellens rimlighet ej undersökts. Modellen från det föregående projektet tog inte hänsyn till den tillagda massan för Corioliseffektens inverkan på ROV:ns dynamik. Den innehöll även en enligt Fossen [3] felaktig teckenkonvention där vissa krafter var definierade positiva åt fel håll. Den nya modellen innehåller en i sammanhanget vedertagen notation för modellparametrarna för att förenkla såväl förståelsen för modellen som möjligheten att sätta sig in i modellen för framtida projekt. Rörelseekvationerna har härlets från Newtons kraftekvation i det ROV-fasta koordinatsystemet, sedan har de bidragande krafterna lagts till, se Fossen [3]. Dessa ekvationer beskriver hastigheterna i det kroppsfasta koordinatsystemet för ROV:n. Vinkelaccelerationerna har härlets från Newtons momentekvation tecknad i det ROV-fasta koordinatsystemet. Dessa ekvationer beskriver då vinkelaccelerationerna för ROV:en i det kroppsfasta koordinatsystemet. För att beskriva ROV:ns hastigheter i det jordfasta koordinatsystemet ges en ny orientering genom en rotationstransform. Om Eulervinklar används kommer singularitet kunna inträffa då pitch-vinkeln θ = ±90. Därför används en alternativ modell med kvaternioner q = (q 0, q 1, q 2, q 3 ) T i delsystem Sensorfusion. I delsystem Reglering används dock Eulervinklar, eftersom det känns mer intuitivt att reglera vinklar istället för kvaternioner. Vid normal använding av ROV:n antas detta dock inte vara något problem. 4.1 Koordinatsystem För modellering av ROV:n har två koordinatsystem införts, ett ROV-fixt (x, y, z) T samt ett jordfixt (X, Y, Z) T. Det sistnämda betraktas som ett inertialsystem där Newtons rörelseekvationer gäller. Koordinatsystemet har sitt origo vid vattenytan och är ett så kallat North- East- Down-system med X-axeln pekandes norr, Y -axeln pekandes österut och Z-axeln pekandes nedåt. Det ROV-fixa koordinatsystemet har x-axeln pekandes rakt fram längs ROV:ns kropp, y-axeln pekandes åt styrbord och z-axeln nedåt. Origo har placerats i ROV:ns masscentrum, se figur 6. Transformationen från jord-fixa accelerationer till ROV-fixa har skett med rotationsmatrisen R som definieras enligt (4.14). Matrisen har härletts från tre rotationer. Först har ett koordinatsystem (X, Y, Z ) T tecknats genom en rotation ψ kring den jord-fixa Z-axeln. Därefter har ännu ett nytt koordinatsystem (X, Y, Z ) T införts genom en rotation θ kring Y -axeln. Slutligen erhölls det ROV-fixa koordinatsystemet utifrån en rotation φ runt X -axeln. För att uttrycka de kroppsfixa vinkelhastigheterna i Eulervinklarnas derivator användes matrisen T som definiers enligt enligt (4.16).

17 12 Figur 6: Illustrering av det jord-fixa samt ROV-fixa koordinatsystemet samt rotationen mellan dessa. Bilden är hämtad från Patricia Sundin m.fl. [5]. De variabler som använts för att beskriva ROV:ns position, orientering samt motorspänning beskrivs i tabell 3. Tabell 3: Beskrivning av variabler Variabler Beskrivning n, e, d ROV:ns globala koordinater i X-, Y - och Z-led. φ, θ, ψ Eulervinklar för roll, pitch och yaw. q 0, q 1, q 2, q 3 Kvaternioner som beskriver transformationen mellan det lokala och det globala koordinatsystemet. u, v, w Hastigheter i det lokala koordinatsystemet längs x-, y-, och z-axlarna. p, q, r Rotationshastigheter i det lokala koordinatsystemet runt x-, y- och z-axlarna. u T, u yf, u yr, u zf, u zr Den pålagda spänningen på respektive motor. 4.2 Förenklingar För att få en hanterbar modell förenklades den fysikaliska modellen. Symmetri antogs för xy-, xz- och yz-planet. Självklart är inte ROV:n helt symmetrisk kring dessa plan men med detta antagande kom tröghetsmatrisen att bli diagonal. Eftersom tröghetsmomenten påverkar ekvationerna för vinkelaccelerationerna blev inte modellen helt korrekt med detta antagande, men då vinkelhastigheterna mäts erhålls en korrigering av felet. För att förenkla beräkningen av dämpningen från de hydrodynamiska effekterna antogs rörelserna för ROV:n vara frikopplade på grund av ROV:ns relativt låga hastigheter.

18 Parameterskattning För att beskriva systemmodellen behövs ett antal parametrar, de definieras i tabell 4. Parametrar K p, M q, N r K p p, M q q, N r r m X u, Y v, Zẇ I x, I y, I z Kṗ, M q, Nṙ z B x yf, x yr, x zf, x zr X u, Y v, Z w X u u, Y v v, Z w w C T, C MT, C yr, C yf, C zr, C zf V ρ g Tabell 4: Beskrivning av parametrar Beskrivning Linjära motståndskoefficienter för rotation i vatten. Kvadratiska motståndskoefficienter för rotation i vatten. ROV:ns massa. Tillagd massa i x-, y- och z-led på grund av translation i vatten. Tröghetsmoment runt x-, y- och z-axlarna. Ökat tröghetsmoment runt x-, y- och z-axlarna på grund av rotation i vatten. Avstånd längs z-axeln från masscentrum till flytkraftens angreppspunkt. Avstånd längs x-axeln från masscentrum till respektive motor. Linjära motståndskoefficienter för translation i vatten. Kvadratiska motståndskoefficienter för translation i vatten. Motorkoefficienter. ROV:ns undanträngningsvolym. Vattnets densitet. Tyngdacceleration. Dessa parametrar skattades med hjälp av systemidentifiering och information från en viktriktig CAD-modell av ROV:n. Ett fåtal parametrar beräknades analytiskt. Se appendix 5 för en utförlig beskrivning av parameterskattningen. 4.4 Framtagning av modell Med notationen η = (n, e, d, φ, θ, ψ) T alternativt η = (n, e, d, q 0, q 1, q 2, q 3 ) T för position och orientering samt ν = (u, v, w, p, q, r) T för de kroppsfixa hastigheterna kan ROV:ns dynamik beskrivas i det kroppsfixa koordinatsystemet enligt där M ν + C(ν)ν + D(ν)ν + g(η) = τ (4.1) M = M RB + M A = tröghetsmatris för en stel kropp samt tillagd massa C(ν) = C RB (ν) + C A (ν) = matris beskrivande Corioliseffekt för en stelkropp samt tillagd massa D(ν) = hydrodynamisk dämpningsmatris g(η) = vektor med tyngdkraftens inverkan på ROV:en τ = kraft- och momentvektor som beskriver motorernas inverkan på ROV:n Om symmetri antas kring de tre planen som de kroppsfixa axlarna spänner upp fås enligt Fossen [3, sid 122,173] M = M RB + M A = diag[m, m, m, I x, I y, I z ] + diag[x u, Y v, Zẇ, Kṗ, M q, Nṙ] = diag[m + X u, m + Y v, m + Zẇ, I x + Kṗ, I y + M q, I z + Nṙ] (4.2) Med omskrivningarna v = (u, v, w) T och ω = (p, q, r) T samt med definitionen S(aA)B = aa B kan

19 14 Corioliseffekten C RB (ν)ν skrivas (enligt Fossen [3, sid 49]) [ [ ] ms(ω) 03 3 v C RB (ν)ν = = S(Iω)] ω [ ] mω v (Iω) ω Den tillagda massans inverkan på Corioliseffekten kan förenklas enligt Fossen [3, sid 122] Zẇw Y v v Zẇw 0 X u u C A = Y v v X u u 0 0 Zẇw Y v v 0 Nṙr M q q Zẇw 0 X u u Nṙr 0 Kṗp Y v v X u u 0 M q q Kṗp 0 (4.3) (4.4) Med antagandet att ROV:n har tre symmetriplan och att den hydrodynamiska dämpningen i de sex frihetsgraderna är frikopplad fås en diagonal D(ν)-matris. Om termer av högre ordning än två försummas kan enligt Fossen [2, sid 43] dämpningsmatrisen skrivas som D(ν) = diag[x u, Y v, Z w, K p, M q, N r ] diag[x u u u, Y v v v, Z w w w, K p p p, M q q q, N r r r ] (4.5) ROV:n är byggd för att vara stabil, och ska återgå till jämviktsläge. Flytpunkten är alltså placerad rakt ovanför tyngdpunkten, i det kroppsfixa koordinatsystemet befinner sig flytpunkten ovanför tyngdpunkten längs z-axeln enligt Bernhard & Johansson [1]. Därmed kan tyngdkraftens inverkan uttryckas (W B)sθ g(m ρv )sθ (W B)cθsφ g(m ρv )cθsφ (W B)cθcφ g(m ρv )cθcφ g(η) = z b Bcθsφ z b Bsθ 0 = z b ρgcθsφ z b ρgsθ 0 Krafterna och momenten från motorerna modelleras som fluidkrafter där kraften är proportionell mot kvadraten av thrustrarnas rotationshastighet. Rotationshastigheten antas vara proportionell mot motorns styrsignal. Thrustrarna är placerade så att deras kraftbidrag verkar ortogonalt mot x-axeln vilket resulterar i moment kring y- och z-axeln. Sidothrustrarna är placerade på en axel som går genom ROV:ns geometriska centrum och är parallell med x-axeln. Eftersom masscentrumet befinner sig under ROV:ens geometriska centrum bidrar sidothrustrarna även med ett vridande moment i roll-led. Detta moment antas vara försumbart. Krafterna och momenten modelleras enligt τ = C T u T u T C yr u yr u yr + C yf u yf u yf C zr u zr u zr + C zf u zf u zf C MT u T u T C zr x zr u zr u zr C zf x zf u zf u zf C yr x yr u yr u yr C yf x yf u yf u yf (4.6) (4.7)

20 15 Ekvation (4.1) kan nu skrivas på formen u = C T u T u T + X u + X u u u u m + X u m + X u C yr v = u yr u yr + m + Y v g(m ρv ) m + X u C yf u yf u yf + Y v + Y v v v v + m + Y v m + Y v sin θ m Z ẇ m + X u wq + m Y v m + X u vr (4.8) g(m ρv ) m + Y v cos θ sin φ + + m Z ẇ wp m X u ur (4.9) m + Y v m + Y v ẇ = C zr u zr u zr + C zf u zf u zf + Z w + Z w w w g(m ρv ) w + cos θ cos φ + m + Zẇ m + Zẇ m + Zẇ m + Zẇ + m X u uq m Y v vp (4.10) m + Zẇ m + Zẇ ṗ = C M T u T u T + K p + K p p p p + z BρV g cos θ sin φ I x + Kṗ I x + Kṗ I x + Kṗ (I z Nṙ) (I y M q ) I x + Kṗ q = C zrx zr I y + M q u zr u zr C zf x zf qr Y v Zẇ vw (4.11) I x + Kṗ u zf u zf + M q + M q q q I y + M q I y + M q q + z BρV g I y + M q sin θ (I x Kṗ) (I z Nṙ) pr Z ẇ X u uw (4.12) I y + M q I y + M q ṙ = C yrx yr u yr u yr C yf x yf u yf u yf + N r + N r r r r I z + Nṙ I z + Nṙ I z + Nṙ (I y M q ) (I x Kṗ) I z + Nṙ pq X u Y v uv (4.13) I z + Nṙ Högerledet i ekvationerna betecknas f b så att ν = f b (ν, η). För att uttrycka de kroppsfixa accelerationerna i globala koordinater används rotationsmatrisen R som funktion av Eulervinklar enligt cθcψ cθsψ sφ R = R(φ, θ, ψ) = sφsθcψ cφsψ sφsθsψ + cφcψ sφcθ (4.14) cφsθcψ + sφsψ cφsθsψ sφcψ cφcθ alternativt som en funktion av kvaternioner enligt q0 2 + q1 2 q2 2 q3 2 2q 1 q 2 2q 0 q 3 2q 0 q 2 + 2q 1 q 3 R = R(q 0, q 1, q 2, q 3 ) = 2q 0 q 3 + 2q 1 q 2 q0 2 q1 2 + q2 2 q3 2 2q 0 q 1 + 2q 2 q 3 (4.15) 2q 0 q 2 + 2q 1 q 3 2q 2 q 3 + 2q 0 q 1 q0 2 q1 2 q2 2 + q3 2 De kroppsfixa vinkelhastigheterna transformeras till derivator av Eulervinklar genom matrisen T som skrivs T = T (φ, θ, ψ) = 1 sφtθ cφtθ 0 cφ sφ 0 sφ cθ cφ cθ (4.16) alternativt med kvaternioner enligt T = T (q 0, q 1, q 2, q 3 ) = 1 2 q 1 q 2 q 3 q 0 q 3 q 2 q 3 q 0 q 1 q 2 q 1 q 0 (4.17)

21 16 där cθ = cos θ, sθ = sin θ, etc. Sambandet mellan lokala och globala hastigheter blir således η = J ν där J är transformationsmatrisen [ ] R(φ, θ, ψ) 0 J(φ, θ, ψ) = (4.18) 0 T (φ, θ, ψ) alternativt J(q 0, q 1, q 2, q 3 ) = [ R(q0, q 1, q 2, q 3 ) 0 0 T (q 0, q 1, q 2, q 3 ) ], (4.19) där 0 är nollmatriser av lämplig dimension. Med x = (η T, ν T ) T samt u = (u T, u yf, u yr, u zf, u zr ) T skrivs den fullständiga modellen [ ] Rv Jν ẋ = f(x, u) = = T ω (4.20) f b f b (ν, η) 4.5 Resultat och diskussion Denna modell, framtagen enligt Fossen [3], innehåller många förenklingar. En mer detaljerad modell antas dock inte förbättra modellens förmåga att beskriva det verkliga systemet. Detta eftersom skattningensprecisionen av de tillagda parametrarna kräver en stor noggrannhet från mätningarna för att bli korrekta, en noggrannhet som med nuvarande sensorer förmodligen är omöjlig att uppnå.

22 17 5 Skattning av modellparametrar Detta kapitel beskriver hur modellparametrar har skattats genom att implementera de olinjära rörelseekvationerna som en grey-boxmodell med Matlab-kommandot indlgrey. Denna identifikationsmetod anpassar de okända modellparametrarna för att beskriva estimeringsdatasekvenserna, alltså mätdatasekvenserna så bra som möjligt. Anpassingen har genomförts med hjälp av den prediktionsfelsminimerande funktionen pem i Matlab. Se figur 7 för illustration av grey-boxskattningen. Insignaler Grey-boxstruktur Mätdata Grey-Box Ska ade parametrar Figur 7: Schematisk bild för strukturen hos en grey-box-modell. Eftersom SONAR:en inte kunde skatta translationshastigheterna kunde inte translationsdynamiken bestämmas med hjälp av pem. Detta innebär att parametrarna som ingår i translationsdynamiken istället skattas med analytiska approximationer. En del modellparametrar har erhållits från den CAD-modell som skapats av den projektgrupp som ansvarar för ROV:ns mekaniska hårdvara. 5.1 Tester ROV:ns rotationsdynamik undersöks kring en axel i taget. Då rotationen runt en axel betraktas, hålls övriga tillstånd så nära noll som möjligt, detta för att kunna skatta så få parametrar som möjligt åt gången. Innan testet startar säkerställs att ROV:n inte har någon begynnelsehastighet och att vattnet runt omkring ROV:n har försumbar flödeshastighet Roll-dynamik Vid följande test hålls alla variabler förutom roll-vinkelhastigheten så nära noll som möjligt för att skatta den rena roll-dynamiken. ROV:n roteras i roll-led för hand cirka 60 och släpps därefter från vila. Mätdata samlas in under tiden då ROV:n svänger in till sitt jämviktsläge Pitch-dynamik Insamling av data till skattningen av den hydrodynamiska dämpningen görs genom att motställa sidomotorena och accelerera vinkelhastigheten för att sedan tvärt stänga av motorerna. Insamlingen av data sker då under deaccelerationen. Insamling av data för motorernas inverkan sker då styrsignalen utför en ramp från 0 V till 2.5 V och sedan går ner igen till 0 V med samma hastighet, se figur Yaw-dynamik Yaw-dynamiken testas på samma sätt som pitch-dynamiken.

23 18 Matningspänning [v] u yf u zf u yr u zr Tid [s] Figur 8: Den typ av ramp på styrsignalerna som används vid insamling av data till parameterskattningen. 5.2 Parameterskattning Nedan tas modellparametrar fram som beskriver translationsdynamiken samt rotationsdynamiken. Resultatet av parameterskattningen ses i tabell Translationsdynamik Translationsdynamiken skattades med hjälp av analytiska approximationer. Om ROV:n approximeras som en ellipsoid kan masströgheterna enligt Fossen [2] beräknas som där X u = α 2 α m (5.1) Y v = β 2 β m (5.2) Zẇ = Y v (5.3) ( ) 1 e 2 ( ( ) ) e α = 2 e 3 2 log e (5.4) 1 e β = 1 ( ) 1 e 2 ( ) 1 + e e 2 (2 e 3 ) log (5.5) 1 e ( ) 2 b e = 1 (5.6) a Där a är hälften av ROV:ns längd och b är ROV:ns radie. De analytiskt beräknade värdena för masströgheterna ses i tabell 5. Dämpningskoefficienterna för translationsdynamiken kan approximeras med hjälp av Rayleighs motståndsekvation. Eftersom motståndet är proportionellt mot tvärsnittsarean kan de olika dämpnings-koefficienterna skrivas som X u u = 1 2 ρ C D (2 b)(2 b) (5.7) Y v v = 1 2 ρ C D a(2 b) (5.8) Z w w = 1 2 ρ C D a(2 b) (5.9)

24 Rotationsdynamik De insamlade datasekvenserna från kapitel 5.1 användes för att med hjälp av grey-box skatta rotationsdynamiken. Eftersom flera modellparametrar är osäkra har kvoter skattats för att erhålla en bra skattning av modellens dynamik. Resultatet kan ses i tabell 5. Efter att dessa kvoter skattats kan de enskilda parametrarna beräknas approximativt. Detta eftersom de enskilda parametrarna med nuvarande implementation behövs till den exakta linjäriseringen som tagits fram till LQ-regulatorn. De fyra sidotrustrarnas styrka har här antagits vara lika. Tabell 5: Värdena för de skattade kvoterna och parametrarna Parametrar Approximerat värde X u Y v Zẇ X u u -20 Y v v -200 Z w w -200 M q q I y+m q z B ρv g I y+m q C zr I y+m q = C zf N r r I z+nṙ C yr I z+nṙ = C yf I z+nṙ K p p I x+kṗ z B ρv g I x+kṗ I y+m q Validering Rotationsmodellen valideras mot datasekvenser som ej används till parameterskattningen. Pitch- och yawdynamik valideras på sekvenser där insignalen är en dubbelsidig ramp, se figur 10 och 11. Rolldynamiken valideras mot en datasekvens som är insamlad under ett test utfört enligt kapitel och kan ses i figur 9. Då ett tillförlitligt sätt att mäta ROV:ns hastighet saknas har modellen endast validerats för rotationer kring enskilda axlar. De delar av modellen som har kunnat valideras har stämt väl överens med det sanna systemet, dock kan modellen ej påstås vara komplett då validering för translation och fleraxliga rotationer ej genomförts. Flera olika projektgrupper har ansvarat för konstruktionen av ROV:n. Den har byggts om och uppgraderats flera gånger vilket har resulterat i att dess viktneutralitet har förlorats. Då tester har utförts har vikter hängts på ROV:ns undersida för att minska den onödigt starka flytkraften. ROV:ns dynamik kan därför skilja sig från test till test, beroende på var vikterna fästs. En modell av ett kontinuerligt förändrande system kommer aldrig vara exakt.

25 20 Figur 9: Valideringsplot för rotation kring x-axeln, alltså vid roll av ROV:n. Den gråa kurvan är uppmätt rotationshastighet i rad/s och den blåa är modellens utsignal. Mätsekvensen är insamlad enligt kapitel Figur 10: Valideringsplot för rotation kring y-axeln, alltså vid pitch av ROV:n. Den gråa kurvan är uppmätt rotationshastighet i rad/s och den blåa är modellens utsignal. Mätsekvensen är insamlad enligt kapitel

26 21 Figur 11: Valideringsplot för rotation kring z-axeln, alltså vid yaw av ROV:n. Den gråa kurvan är uppmätt rotationshastighet i rad/s och den blåa är modellens utsignal. Mätsekvensen är insamlad enligt kapitel Resultat och diskussion Resultatet för de skattade parametrarna vid rotation av ROV:n tycks beskriva det verkliga systemet bra. De analytiskt skattade translationsparametrarna är däremot approximativa och svåra att validera. De ger däremot en rimlig skattning av modellparametrarna eftersom de till stor del beror på ROV:ns form och vikt, som är kända. Parametrarna tagna från CAD-modellen är osäkra. Dessa är avståndet från masscentrum till flytkraften samt trögheterna i tröghetstensorn. Detta är anledningen till att kvoter av modellparametrarna har skattats. Detta angreppssätt ger en mer korrekt beskrivning av modellens beteende. De enskilda parametrarna har sedan beräknats utifrån de skattade kvoterna och de osäkra CAD-parametrarna. Dessa parametrar har använts i den exakta linjäriseringen, se kapitel

27 22 6 Delsystem Sensorfusion Delsystemet Sensorfusion består av ett filter som utgående från ett antal olika sensorer skattar ROV:ns tillstånd i form av en tredimensionell orientering (yaw, pitch och rollvinkel), tredimensionell position (n, e, d) relativt initialpositionen, samt motsvarande vinkelhastigheter och hastigheter. Internt i EKF:n representeras orienteringen med hjälp av kvaternioner. Dessa skattningar kommer sedan skickas vidare till berörda delsystem. 6.1 Observatör Det finns två vanliga sätt att implementera ett Extended Kalman Filter (EKF); EKF1 och EKF2. Man gör antingen en första eller en andra ordningens taylorutveckling av den olinjära tillståndsekvationen. Då EKF2 kräver extra beräkningar brukar man vanligtvis använda sig av EKF1 om det fungerar tillräckligt bra. Med en olinjär modell från Gustafsson [4] enligt: erhålls algoritmen för EKF1 enligt följande: x k+1 = f(x k, u k ) + v k, där v k N(0, Q k ) (6.1a) y k = h(x k, u k ) + e k, där e k N(0, R k ) (6.1b) S k = R k + h (ˆx k k 1 )P k k 1 (h (ˆx k k 1 )) T K k = P k k 1 (h (ˆx k k 1 )) T S 1 k ɛ k = y k h(ˆx k k 1 ) ˆx k k = ˆx k k 1 + K k ɛ k (6.2a) (6.2b) (6.2c) (6.2d) P k k = P k k 1 P k k 1 (h (ˆx k k 1 )) T S 1 k h (ˆx k k 1 )P k k 1 (6.2e) ˆx k+1 k = f(ˆx k k ) (6.2f) P k+1 k = Q k + f (ˆx k k )P k k (f (ˆx k k )) T (6.2g) Tillståndsmodellen f(x k, u k ) i (6.1a) finns beskriven under kapitel 4 och kommer att representeras av (4.20). I derivatan av tillstånden försummas alla trigonometriska funktioner. För körning på land finns en konstantfartsmodell implementerad, där f b (ν, η) i (4.20) sätts till 0 istället. Det finns även en accelerometerdödräkningsmodell enligt Singer-approximationen, f b (ν, η) = [ ya 0 ] βν (6.3) där β är en designparameter. Denna modell fungerar dock inte tillfredsställande i nuvarande implementation. Mätmodellen h(x k, u k ) i (6.1b) fås från sensormodellerna som för SONAR:en ges av (6.4), trycksensorn ges av (6.6), accelerometern ges av (6.7), magnetometern ges av (6.9a) och för gyroskopet ges av (6.11). 6.2 Mätuppdateringar Under denna del kommer alla sensorers mätuppdateringar att beskrivas. Dessa mätuppdateringar kommer sedan användas i EKF:n för att skatta positioneringen och orienteringen. Samtliga mätuppdateringar har

28 23 additivt brus där varje sensors brus antas vara okorrelerat med de andra. Eftersom gränsvärden för outlierfiltrering och variansen på sensorbruset v är trimningsparametrar hänvisas läsaren till implementerigen för numeriska värden Mätuppdatering SONAR Mätuppdateringen för SONAR:n i nuvarande implementation är mycket enkel, enligt signalbehandling SONAR 6.3 skickas en skattad position i north och east enligt [ n s = e ] + v s (6.4) Mätuppdatering trycksensor Trycksensorn skickar absolut tryck P tot = P luft + P vatten = P luft + ρgd (6.5) till delsystemet. Innan mätningen skickas in i EKF:en, så omvandlas trycket till ett djup för att undvika att olika element i innovationskovariansmatrisen (S) är för olika i storlek. Detta kan göra att matrisinverteringen får dålig precision. Det visade sig att man var tvungen att införa en ytterligare konversionsfaktor α för att korrigera en felaktig skalning som uppstod i A-D-omvandlingen för trycksensorn. Det kompenseras även för att pitchvinkeln i ROV:n gör att trycksensorn placeras på ett annat djup än ROV:ns geometriska centrum. Detta gör att den slutgiltiga mätekvationen blir d = α P tot P luft ρg l sin θ + v d. (6.6) Eftersom lufttrycket P luft kan variera 50 hpa från dag till dag enligt SMHI [7] måste lufttrycket kalibreras vid körning. Det görs genom att sätta P luft till trycket från trycksensorn vid vattenytan. Densiteten för vatten ändras även med temperaturen, men i testbassängen kommer den att vara relativt konstant Mätuppdatering accelerometer För att räkna ut pitch och roll-vinklar används accelerometern enligt a = R T (q)(g + a kropp ) + v a, g = g ˆd. (6.7) Man vill använda denna mätuppdatering då a kropp är liten, eftersom man då mäter tyngdkraften. Därför kastar man bort mätvärden då där ɛ a är en designparameter. Då kan a kropp approximeras till 0. a g > ɛ a (6.8)

29 Mätuppdatering magnetometer Magnetometern används för att bestämma yaw-vinkeln. Här används en implicit mätekvation för att magnetometerstörningar inte ska medföra en felaktig pitch- och roll-vinkel. Detta görs genom en implicit mätuppdatering där magnetfältets e-komponent sätts till 0 enligt m = R(q)y m, (6.9a) 0 = m 2 + v m. (6.9b) Denna mätuppdatering driver andra komponenten i magnetfältet mot 0, men man kan få ett skattningsfel 180 i yaw-vinkeln eftersom magnetfältet kan vara noll både om ROV:en är riktad öst eller väst. Därför vänder man 180 om m 1 < 0.1 genom att ändra på q. Elektronik i ROV:n kan skapa störningar b el i magnetfältet. Dessa är oönskade, varför man kastar bort mätvärden då m b jord > ɛ m (6.10) där ɛ m är en designparameter. För att kompensera för störningar, så finns en lookup-tabell implementerad i koden för att kunna modellera skattningsfelet i yaw-vinkel givet antagandet att skattningsfelet främst beror på statiska magnetfält. Test för att experimentellt bestämma lookup-tabellen gjordes dock aldrig på grund av tidsbrist, vilket skulle kunna göras för att korrigera vinkeln. Den nuvarande koden implementerar bara en identitetsavbildning i lookup-tabellen Mätuppdatering gyro Gyroskopet används för att mäta de tre vinkelhastigheterna på ROV:n. Vinkelhastigheterna p, q och r är definerade i kapitel 4 som vinkelhastigheterna runt de kroppsfixa axlarna x, y och z. Mätuppdateringen för gyrot blir således ω = p q r + v g. (6.11) 6.3 Signalbehandling SONAR-data Positioneringen av ROV:en bygger på en fördefinierad karta bestående av ett godtyckligt antal linjer som representerar väggarna i en bassäng. Algoritmen beskrivs mera detaljerat av Romagós [10]. Ett fel räknas ut för varje SONAR-lob mellan den faktiska mätningen och avståndet till alla linjer från ett skattat tillstånd i bassägen. Varje lob associeras sedan till den linje med det minsta felet. Nästa steg blir att räkna ut en ny skattning av positionen genom att göra en linjärisering av mätekvationen i det skattade tillståndet. Denna nya skattade position skickas sedan som en implicit mätekvation till EKF:en.

30 25 ɛ(b n, L n ) ρ l n φ s n r s n θ l n N ψ φ s m ɛ(b m, L m ) r s m N E Figur 12: Beskrivning av SONAR:ens lober samt bassängen I figur 12 finns enheterna som skriver väggar samt lober definierade, alla enheter är i rovens referensram om inget annat sägs. Varje lob b definieras i polära koordinater som [ r s b i,p = i De omvandlas sedan till kartesiska koordinater enligt φ s i ], i {1, 2,.., N} (6.12) [ ] x s b i = i = y s i [ ] ri cos φ i r i cos φ i (6.13) Linjerna som representerar bassägen definieras i det globala koordinatsystemet enligt [ ρ L G G j = j θ G j ], j {1, 2,.., M} (6.14) där M är antalet linjer. De omvandlas sedan till ROV:ens lokala koordinatsystem genom de skattade tillstånden north ( n), east (ê) och yaw ( ψ) l ] [ ] [ˆρ ˆL j = j ρn ncosθ n êcosθ n = θ n ψ ˆθ l j (6.15) De skattade felen blir de implicita mätekvationerna ɛ = ɛ 1. (6.16) ɛ N

31 26 där varje element i kolonnvektorn utgörs av ɛ i (b i, ˆL j ) = x s i cos ˆθ l j + y s i sin ˆθ l j ˆρ s i (6.17) Den linje som ger det minsta absoluta felet mellan loben och den skattade positionen associeras med loben l(i) = argmin ɛ 2 i (b i, L j ) (6.18) j där l(i) är det index som associerar lob i (b i ) med den linje som minimerar felet. Mätekvationen deriveras med avseende på positionen och ger uttrycket x = [ ] cos ˆθ l(1) n, C =. e. cos ˆθ l(n) sin ˆθ l(1).. (6.19) sin ˆθ l(n) Det är även möjligt att införa ett biastillstånd Φ mellan kartan och skattningen av yaw-vinkeln enligt n cos ˆθ l(1) sin ˆθ l(1) x s 1 cos ˆθ l(1) y1 s sin θ l(1) ψ b = ψ + Φ, x = e, C =... (6.20) Φ cos ˆθ l(n) sin ˆθ l(n) x s N cos ˆθ l(n) yn s sin ˆθ l(n) som hanterar definieringsfel av kartan samt bias i skattningen av yaw-vinkeln. I den nuvarande implementeringen skattas en ny position med minstakvadratskattning ˆx(k + 1) = ˆx(k) + (C T C) 1 C T ɛ (6.21) utan bias-vinkel som i sin tur skickas som mätekvation till EKF:n. En annan möjlig lösning är att använda (6.19) och (6.20) med lämpligt vald kovarians direkt i EKF:n. 6.4 Resultat och diskussion Roll- och pitch-vinklarna skattas på ett tillfredsställande sätt och uppfyller kraven i Malte Moritz m.fl. [6]. Däremot finns det fortfarande problem att lösa för att yaw-vinkeln ska kunna skattas tillräckligt bra. Trots att magnetometern placerats på en väl vald plats i ROV:n, så fanns fortfarande ett visst skattningsfel i yaw-vinkeln. Detta berodde på att vissa magnetfält fortfarande påverkade mätningarna. Detta handlar om magnetfält från t.ex. relämagneten och thrustrarna, som består av borstlösa motorer med permanentmagneter som roterar vid drift. Det finns flera alternativ för att förbättra yaw-skattningen. Exempelvis kan den implementerade lookuptabellen användas. Alternativt, så skulle man kunna implementera ett yaw-bias-tillstånd i EKF:en och använda SONAR:en för att skatta biasen. SONAR:en skulle möjligen klara av att skatta en yaw-bias inom ca. 20 enligt våra uppskattningar, givet att man har en rektangulär bassäng. Slutligen skulle en magnetisk avskärmning av t.ex. relämagneten kunna hjälpa. Positionsmätningen i n- och e-led har tyvärr inte kunnat bli testad online. Därför är skattningarna i de leden endast baserade på modellen. Den felaktiga yaw-vinkeln inverkar också här.

32 27 7 Delsystem Reglering Under projektets gång har två regulatorer implementerats på ROV:n. Detta kapitel beskriver vilka två typer som har används, hur de tagits fram samt resultat och slutsatser om varje regulator. De implementerade regulatorerna är en decentraliserad regulator och en LQ-regulator. Med hjälp av den decentraliserade regulatorn styrs vinkelhastigheter i pitch- och i yaw-led oberoende av varandra. Det är också möjligt att styra ROV:n upp och ned, höger och vänster samt framåt och bakåt när den decentraliserade regulatorn används. Eftersom nuvarande motoruppsättning inte möjliggör direkt styrning av rollvinkel, var regelering i denna led inte möjlig att implementera. LQ-regulatorn används då ROV:n ska regleras till en önskad position med en önskad orientering (yaw och pitch) relativt startpositionen, och sedan bibehålla dess position. Vilket reglersystem som används kan väljas med den externa PC:n. 7.1 Decentraliserad regulator Decentraliserad reglering innebär att man reglerar ett system med flera in- och utsignaler genom att låta en insignal, med hjälp av en regulator, styra en utsignal. Resultatet blir flera oberoende loopar med varsin tillhörande regulator. I ROV:ns system frikopplar den decentraliserade regulatorn vinkelhastigheterna i yaw- och pitch-led från varandra i det kroppsfixa koordinatsystemet. De två oberoende looparna regleras med hjälp av varsin PID-regulator. Det finns även möjlighet att styra ROV:n höger och vänster samt framåt och bakåt i det kroppfixa koordinatsystemet. Vinkelhastigheterna och de linjära hastigheterna kan styras som om de är oberoende av varandra. I figur 13 beskrivs hur regulatorn fungerar. Regulatorns insignaler är referenssignaler från externa PC:n samt skattade tillstånd från delsystem Sensorfusion. Dessa skickas till varsin PID-regulator, vilka bestämmer styrsignaler för de fem thrustrarna tillsammans med den öppna styrningen för de linjära rörelserna. För att styra vinkelhastigheterna kommer de lodrätt riktade motorerna respektive de vågrätt riktade motorerna fram och bak köras i motsatt riktning för att motverka drift i positionen. För att köra bakåt/framåt kommer den bakre motorn att användas. För att köra upp/ned respektive höger/vänster kommer de lodrätt och vågrätt riktade motorerna att användas. Är referenssignalen för att köra upp/ned satt till noll, finns en P-regulator som ser till att ROV:n inte flyter upp till ytan. En skalfaktor för styrsignalerna lades till för att förhindra drift, eftersom motorerna har olika avstånd till rotationscentrum. Därmed behövdes ingen modell av systemet att användas vid framtagningen av den decentraliserade regulatorn.

33 28 r ref ˆr PID -1 Σ 0.95 y f Σ y r höger/vänster q ref ˆq PID -1 Σ z f Σ z r upp/ner Σ z ref ẑ PID fram/bak T Figur 13: En översikt av den decentraliserade regulatorn, med insignaler och utsignaler. 7.2 LQ-regulator Linjärkvadratisk reglering (LQ-reglering) innebär att systemet regleras med en avvägning mellan snabbhet och små styrsignaler. LQ-regulatorn innehåller en tillståndsåterkoppling från den tillståndsskattning som beskrivs under delsystem Sensorfusion. Referenssignalerna till regulatorn är orientering (yaw och pitch) samt djup. Regulatorn har också referenssignaler för horisontell position för att kunna förhindra drift i position hos ROV:n. Utsignalerna från regulatorn är önskad acceleration i det jordfixa koordinatsystemet. Hur den översätts till styrsignaler till de fem thrustrarna beskrivs i kapitel Linjärisering av ROV:n För att använda en LQ-regulator behövdes en linjär tillståndsbeskrivning av systemet tas fram. I detta projekt användes exakt linjärisering. Det fungerar på så sätt att en styrsignal till systemet väljs så att de olinjäriteter som finns kompenseras, vilket resulterar i ett linjärt system. Detta är vanligt inom styrning av robotar och brukar ibland kallas computed torque/force control. Hur man väljer τ är beskrivet nedan. Betrakta ekvationerna som beskriver modellen, hämtade från avsnitt 4. Då vi inte har möjlighet att styra ROV:n i roll-led har dess inverkan försummats i samtliga matriser och vektorer beskrivna i avsnitt 4. Ekvationerna är η = Jν, (7.1) M ν + C(ν)ν + D(ν)ν + g(η) = τ. (7.2)

34 29 Där både ν och η estimeras i delsystem Sensorfusion, och finns tillgängliga för delsystem Reglering. Genom att derivera den kinematiska ekvationen av systemet (7.1) med avseende på tiden fås ν = J 1 [ η Jν] (7.3) Eftersom J endast är singulär när pitchvinkeln θ är ±90, har det hittills inte varit några problem med att använda inversen av J. Om man väljer följande olinjära styrlag τ = Ma b + C(ν)ν + D(ν)ν + g(η) (7.4) där a b beskriver accelerationen i det kroppsfixa koordinatsystemet och man sätter in (7.4) i (7.2) samtidigt som resultatet i (7.3) används, fås följande ekvation Om man betraktar M( ν a b ) = MJ 1 [ η Jν Ja b ] = 0 (7.5) η = a n (7.6) där a n ses som den önskade accelerationen i det jordfixa NED-koordinatsystemet. Genom att välja och sätta in (7.7) i ekvation (7.5) fås följande linjära system a n = Jν + Ja b (7.7) M ( η a n ) = 0 (7.8) Där M = J T MJ > 0. Ekvation (7.8) är därför ekvivalent med att Från (7.7) kan man få ut att η a n = 0. (7.9) a b = J 1 [a n Jν] = f(a n ) (7.10) a n ses som styrsignal till det linjära systemet (7.9), och väljs av LQ-regulatorn. Styrsignalerna till ROV:n är u = (u T, u yf, u yr, u zf, u zr ) T. För att översätta det beräknade värdet på τ från (7.4) till styrsignaler användes följande samband, hämtat från kapitel 4 Där ũ = (u T u T, u yf u yf, u yr u yr, u zf u zf, u zr u zr ) T och τ = C u ũ (7.11)

35 30 C T C yf C yr 0 0 C u = C zf C zr C zf x zf C zr x zr 0 C yf x yf C yr x yr 0 0 (7.12) Genom att använda detta samband går det att beräkna vilka styrsignaler som ska skickas till ROV:n utifrån önskat värde på τ. I figur 14 återfinns en schematisk bild på hur den exakta linjäriseringen av ROV:n är utformad, där τ väljs så att systemet blir linjärt. a b τ ν η M Σ 1 η ROV J s f(a n ) C(ν) D(ν) a n Σ Figur 14: En översikt av den exakta linjäriseringen av ROV:n, med a n som insignal. Denna översätts till a b, och sedan väljs τ så att ett linjärt system erhålls Val av regulatorparametrar Efter att linjäriseringen av systemet gjordes, erhölls ett linjärt och frikopplat system enligt (7.9), där varje insignal styr en specifik acceleration i det jordfixa koordinatsystemet. Genom att minimera de kvadratiska kostnadsfunktionerna J i = 1 2 T 0 (η ri η i ) 2 + λ 1i η 2 i + λ 2i a 2 n i dτ i = 1,..., 5 (7.13) Där man viktar skillnad mellan önskad och skattad position eller orientering mot dess tidsderivata samt önskad acceleration. Den önskade accelerationen är insignal till det linjäriserade systemet och η i samt η i är tillstånden i tillståndsbeskrivningen hos systemet. Enligt Fossen, Paulssen [9] leder minimeringarna av kostnadsfunktionerna ovan till följande val av önskad acceleration

36 31 a n i = K pi (η ri η i ) K d η i i = 1,.., 5 (7.14) Där ( ) ( 1/2 ( ) ) 1/2 1/2 1 1 K pi = ; K = di 2 + λ 1 i i = 1,..., 5. (7.15) λ 2i λ 2i λ 2i För att få med integralverkan modifierades den önskade accelerationen till följande T a n i = K pi (η ri η i ) K di η i + K Ii (η ri η i ), dτ i = 1,..., 5 (7.16) 0 där K Ii valdes enligt K Ii = K p i 5K di i = 1,..., 5 (7.17) Vilket motsvarar att använda sig av tumregeln: T Ii = 5T di i en PID-regulator. Därmed består regulatorn av fem oberoende PID-regulatorer. PD-konstanterna bestämdes med hjälp av kostnadsfunktionerna (7.13) och I-konstanten enligt tumregeln ovan. Värdena på straffkonstanterna λ bestämdes genom att variera storleken på både λ 1 och λ 2 tills dess att önskat resultat uppnåddes. 7.3 Resultat och diskussion Simulering av den decentraliserade regulatorn utfördes i Matlab och reslutaten från dem var som väntat. Efter simulering skedde en implementation på ROV:n, varpå regulatorparametrar bestämdes genom att styra ROV:n i vatten med den decentraliserade regulatorn igång. Därefter testades prestandakraven av regulatorn som finns beskrivna i kravspecifikationen. Då framdrivningen inte fungerade vid tillfället dröjde testet av den öppna styrningen framåt och bakåt. Vid testen noterades att ROV:n hade en ganska stor flytkraft och flöt upp till ytan relativt snabbt då man åkt ner på ett visst djup. För att motverka att ROV:n flöt upp till ytan implementerades en P-regulator för att bibehålla ett visst djup. Därför är denna regulator är endast igång när referenssignalen i detta led är noll. Sammanfattningsvis fungerade den decentraliserade regulatorn som planerat. Resultatet av simulering av LQ-regulatorn med exakt linjärisering blev som förväntat, orientering och positionering kunde styras oberoende av varandra. Eftersom den exakta linjäriseringen baseras helt och hållet på den framtagna modellen, testades att förändra några modellparametrar i simulering för linjäriseringen, då dessa med största sannolikhet inte är helt exakta. Trots en avvikelse upp till 10% (högre avvikelser testades aldrig) på de kvadratiska dämpningskoefficienterna, lyckades fortfarande regulatorn att reglera ROV:n till referensvärdena. Den exakta linjäriseringen och regulatorn är i nuläget beskriven med hjälp av Eulervinklar, då det ansågs mer intuitivt att reglera vinklar istället för kvaternioner. För att bli av med singulariteten i pitch-led skulle man kunna låta regulatorn hantera Eulervinklar, sedan omvandla dem till kvaternioner och använda kvaternionerna i linjäriseringen. Detta leder också till att matrisen J alltid kommer vara inverterbar. Vid test i bassäng klarade regulatorn av att reglera djup, pitch- och yawvinkel. Dock drev ROV:n mycket på grund av att inga skattningar för position fanns att reglera. Den betedde sig också suspekt vid

37 32 leveransen. Anledningen till detta hann aldrig undersökas. Intrimning av λ 1 och λ 2 i bassäng har heller inte riktigt hunnits med, då testerna av LQ-regulatorn i bassäng först kunde genomföras samma vecka som leverans av produkten skulle ske. Därför bör den felsökas och regulatorparametrarna trimmas mer för att uppnå önskad funktionalitet och prestanda hos regulatorn. 8 Mjukvara På den interna PC:n finns den mjukvara som hanterar reglering, navigering och kommunikation. I grund och botten körs Ubuntu på PC:n. Ovanpå det körs delsystemen och övrig nödvändig funktionalitet i operativsystemet ROS (Robot Operating System). Mjukvaran för SONAR:en, DeltaT, körs separat från ROS genom gränssnittet Wine vilket beskrivs mer i kapitel 8.3. ROS körs även på den externa PC:n. En överblick av de olika processerna och på vilken hårdvara de körs finns i figur ROS ROS är inte ett operativsystem i vardaglig mening eftersom det kräver en underliggande Linuxdistribution för att köras. ROS tillhandahåller dock många av de funktioner som kan förväntas av ett operativsystem, såsom Abstraktion av hårdvara Möjligheten att köra flera processer samtidigt Kommunikation mellan processer i form av meddelandepassning ROS gör det alltså möjligt att bygga upp ett modulärt system samt att kunna abstrahera bort skillnaden mellan en ren mjukvarukomponent och extern hårdvara. Processerna, som kallas för noder, kommunicerar sinsemellan med meddelanden, så kallade topics. 8.2 Implementerade noder Här beskrivs hur de noder som körs i systemet är implementerade samt kommunikationen sinsemellan. ROS Core är kärnprocessen som måste köras för att köra någon annan nod. En sammanställning av alla topics som används i kommunikationerna mellan noderna finns i appendix C Reglering Regleringsnodens uppgift är att beräkna och förmedla styrsignaler till kommunikation- och sensornoden, där sedan kommunikationsnoden förmedlar styrsignalerna till motorerna. Referenssignaler och tillstånd hämtas kontinuerligt via callbackfunktioner, som anropas en gång varje loop. Noden är enbart aktiv då ROV:ns mode inte är i manuellt läge. Är nödläge aktiverat kommer styrsignalerna som publiceras att föra ROV:n horisontellt till ytan. Är ROV:n istället i stabiliserat läge anropas en funktion som, utifrån vilken regulator som är vald, väljer vilken funktion som ska beräkna och publicera nya styrsignaler. Är den decentraliserade regulatorn vald, anropas en funktion som läser av de internt lagrade decentraliserade referenssignalerna. Referenser i vinkelhastighet skickas till varsin PID-regulator som är konstruerade enligt Enqvist m.fl [11], med förhindring av integratoruppvridning. Är den öppna styrningen för rörelse i z-led satt till noll, sätts en flagga som indikerar att P-regulatorn för hålla konstant höjd ska användas. Dessa beräknade styrsignalerna mättas vid behov. Till sist adderas de öppna styrsignalerna för styrning av translation, och skalas vid behov för att förhindra att styrsignalbegränsningarna överskrids. De beräknade styrsignalerna sparas internt för publicering.

38 33 Xbox-kontroll USB Extern PC ROS Arduino GUI Intern PC Arduino Ethernet ROS Core USB Sensorfusion Reglering Kommunikation IMU Localhost Wine USB IMU DeltaT Ethernet SONAR Figur 15: En schematisk bild av mjukvaran. De rundade boxarna är olika processer (i ROS kallade noder) som körs. De streckade boxarna motsvarar fysiskt skild hårdvara. De heldragna, fyrkantiga boxarna är olika miljöer, i vilka processer kan kommunicera. Alla processer i ROS kan alltså kommunicera med varandra. För att kommunicera med processer eller hårdvara utanför sin miljö krävs att kommunikationen implementeras på lågnivå. Om LQ-regulatorn är vald, anropas istället en funktion som läser av de internt lagrade LQ-referenssignalerna. Dessa referenssignaler skickas till varsin regulator, där regulatorparametrarna är valda enligt kapitel Ut fås då önskad acceleration i det jordfixa koordinatsystemet. Denna acceleration skickas till en funktion som beräknar styrsignaler genom att utföra exakt linjärisering av ROV:n. För att förhindra att styrsignalbegränsningarna nås när exakt linjärisering utförs finns det en gräns implementerad för hur stor den önskade accelerationen får vara. På så sätt förhindras olinjäriteter i den exakta linjäriseringen att uppkomma. De beräknade styrsignalerna sparas internt för publicering.

39 Sensorfusion Huvuduppgiften för Sensorfusion är att skatta tillstånd till Reglering, i figur 16 visas hur klasserna anropas i huvudloopen. För linjär algebra används biblioteket Eigen [12]. Noden Sensorfusion består av tre huvudklasser. Sensor - Sköter kommunikation med andra noder, sänder tillstånd och tar emot sensormätningar. Ekf - Sköter linjär algebra beräkningar i enlighet med kapitel 6.1. DeltaT - Tar in SONAR-data och gör grundläggande signalbehandling i enlighet med kapitel 6.3. Figur 16: Flödesschema Sensor. Figuren visar nodens komponenter och ordningen de anropas i när noden körs. Ekf-klassen som lätt kan blandas ihop med EKF-systemet är designad runt följande huvudfunktioner, vilka tillsammans utgör EKF:et enligt figur 16. TimeUpdate(u, y) - Tidsuppdatering, här finns dynamiken för systemet. MeasurementUpdate(y, ŷ, C, R) - Mätuppdatering, tar in olika mätningar och mätekvationer. I tidsuppdateringen finns det en konstanthastighetsmodell för tester på land samt modellen 4, det finns även funktionalitet för att dödräkna position med hjälp av accelerometrarna. Modellen för ROV:n behöver utvecklas i translationsled, och dödräkning av accelerometerdata fungerar inte i nuvarande implementation. Notera att även om Ekf internt representerar orientering med hjälp av kvaternioner, så skickar den endast ut Eulervinklar till övriga noder. DeltaT-klassen upprättar en UDP-anslutning till DeltaT.exe och hämtar SONAR-mätningar. Funktionen filterdata(..) returnerar ett measurement-objekt som Ekf-klassen kan använda. I measurement-objektet kan en godtycklig mätekvation placeras definerad som listan nedan. y - Tänkt att vara mätningar från en sensor, men kan vara en implicit mätning.

40 35 ŷ - Skattade mätningar. C - Linjära sambandet med tillstånden. R - Kovariansen för mätningen. Implementeringen är lite rörig nu eftersom flera olika mätekvationer har testats parallellt Kommunikation Kommunikationsnodens primära uppgift är att skicka vidare styrsignaler till Arduinon samt sammanställa sensorvärden och status från Arduinon och skicka detta vidare till GUI-noden. Kommunikationen mot Arduinon är speciell eftersom den är seriell. Antingen måste egendefinierade message-typer genereras till seriell form eller så användas message-typer som är seriella till naturen, det vill säga arrayer. Nodens arbete sker i två steg. Först läses alla callbacks av och de mottagna värdena lagras lokalt. Därefter läser den av de lagrade värdena och publicerar dessa. Noden tar emot värden från läckagesensorn och trycksensorn samt batterispänningar från Arduinon. Detta görs i två olika callbacks vilket möjliggör att de kan skickas med olika frekvens. Kommunikationsnoden skickar de mottagna sensor- och batterivärdena till GUI-noden i topicen com/arduinodata. Alla varningar om läckage och låga batterinivåer görs sedan i GUI-noden. Under manuell drift begär kommunikationsnoden styrsignaler från motorerna från GUI-noden genom att publicera topicen updaterequest. Styrsignalerna tas sedan emot i en callback. Om ROV:n körs med en regulator (stabilt läge) publicerar regulatornoden istället styrsignalerna. Kommunikationsnoden märker dock ingen skillnad varifrån styrsignalerna kommer. Styrsignalerna till motorerna, relärna och lampan skickas som en array från kommunikationsnoden till Arduinon. Värdena som skickas är de senast mottagna, oavsett om de är direkt från användaren eller beräknade av regulatorn. Detta förhindrar att Arduinon får både manuella och uträknade styrsignaler samtidigt om systemet mot förmodan skulle bli osynkroniserat. Styrsignalerna publiceras i topicen control. Nodens andra uppgift är att detektera om kontakten med GUI-noden bryts. Detta görs genom att en pingräknare räknas upp en gång varje loop. GUI-noden ska, så länge den fungerar som den borde, skicka ping till kommunikationen. När detta ping tas emot nollställs räknaren. Om detta ping inte tas emot på ett antal looper, det vill säga då GUI-noden inte fungerar som den borde, skickar kommunikationsnoden ut ett meddelande till de andra noderna om att ställa om till nödläge. Detta görs genom att noden publicerar topicen mode GUI GUI-nodens uppgift är dels att visa ROV:ens status i ett grafiskt interface och dels att låta användaren interagera med ROV:en. Noden fungerar som en vanlig nod, med skillnaden att den kan skriva till och läsa från fält i det grafiska interfacet. Det grafiska användargränssnittet är skrivet i Qt och är definierat i en xml-fil. Med hjälp av en timerfunktion körs vissa funktioner med jämna mellanrum. En timerfunktion används eftersom GUI:t är en klass som körs från en annan main-funktion. I denna läses Xbox-kontrollen av, och den är implementerad som ett eget objekt som läser av knappar och spakar på kontrollen och konverterar dessa till signaler som passar ROV:n. Xbox-objektet läser i sin tur av callbacks som publiceras av en standardnod i ROS som tar hand om lågnivåkommunikationen med Xbox-kontrollen. Informationen från ROV:n tas emot i ett antal callbacks som läses av i samma timerfunktion. Noden läser av topicsen com/arduinodata - Batterispänningar, reläer, trycksensorn och läckagesensorn.

41 36 control/controlsignals - Styrsignalerna som skickas till motorerna. sensor/states - Tillstånden skattade av observatören. updaterequest - Kommunikationsmodulen som begär styrsignaler från GUI-noden. Funktionen som tar emot data från Arduinon beräknar ett medelvärde av tio mottagna set med data innan det presenteras i GUI:t. Tryckmätningen omvandlas dessutom från spänning till tryck enligt (3.2). Signalerna till motorerna som tagits emot i kommunikationsnoden skickas även i det här callbacket och presenteras i GUI:t. I callbacken för styrsignaler tas styrsignalen utställd av regulatorn emot. Denna tar alltså inte emot någon data då ROV:n körs i manuell drift. I funktionen som tar emot callbacken för de skattade tillstånden konverteras vinklar från radianer till grader. Därefter presenteras de och den skattade position i GUI:t. I callbacken för updaterequest besvarar GUI:t ROV:ns begäran av styrsignaler. Beroende på vilket läge samt om Xbox-kontrollen är aktiverad eller ej kan GUI:t skicka manuella styrsignaler, referenssignaler till LQ-regulatorn, referenssignaler till den decentraliserade regulatorn, testsignaler (som används vid systemidentifiering) samt ljus- och relävärden. Då styr- eller referenssignaler från Xbox-kontrollen skickas används en dödszon för att nollställe signalerna vid småvärden. Detta beror på att Xbox-kontrollen ej nollställer sig själv. Om Xbox-kontrollen ej är aktiverad, samt om ROV:n körs i stabiliserat läge (med regulator på), skickas styrsignaler genom att trycka på en knapp i GUI:t. Beroende på vilken regulator som är vald körs funktioner som läser av referenssignalerna inskrivna i GUI:t varpå de skickas till ROV:n. En annan timerfunktion tar hand om skickandet av det ping som tas emot av ROV:n för att hålla koll på att det finns kommunikation mellan ROV:n och externa PC:n. Dessutom finns det funktioner som aktiveras av användaren: en för att läsa av en parameterfil och skicka nya parametrar till ROV:n och en för att ändra regulator och driftläge på ROV:n Arduino Arduinon har två uppgifter: att skicka styrsignaler till motorer, lampor och reläer samt att läsa av batterispänningarna, läckagesensorn, och trycksensorn, för att sedan skicka vidare dessa mätningar, se figur 17. Först kontrollerar Arduinon hur länge sedan det var den fick ett meddelande från interna PC:n. Detta görs genom att öka värdet på en räknare och jämför det med ett tröskelvärde. Om räknaren överstiger tröskelvärdet stängs motorerna av. Därefter läser Arduinon av om den mottagit några callbacks från interna PC:n. Den enda topic den prenumerar på är control som innehåller styrsignaler till reläer, lampa och motorer. Dessa styrs med pulsbreddsmodulering (lampa och motorer) och som en digital signal (reläer). Till sist nollställs tidigare nämnd räknare. Sedan avläses ingångarna med A/D-omvandling. Värderna för batterispänning, tryck- och läckagesensor sparas lokalt. Därefter publiceras batterispänningar i topicen ard batteries samt tryck- och läckagesensor i topicen ard sensors.

42 37 Figur 17: Flödesschema över den implementerade koden i Arduinon IMU IMU-noden är tillverkad av Xsens Technologies och gör det möjligt att ansluta till IMU:n. Den använder sig av ROS-biblioteket för att publicera IMU-data inklusive magnetometerdata. Detta krävde lite modifieringar av koden. Mätningarna skickas i ett egendefinierat meddelande kallat msg ImuAndMagData och publiceras i topicen imu/imuandmagdata. 8.3 DeltaT DeltaT är ett program som körs utanför ROS på den interna PC:n. Programmet ansluter till SONAR:en via TCP och signalbehandlar data från SONAR:en till formatet.83p, som består av en avståndsmätning för varje lob. Den behandlade SONAR-datan skickas sedan vidare till ROS via UDP. Mjukvaran är utvecklad för Windows men den interna PC:n använder Ubuntu som operativsystem så programmet körs genom Wine som är en miljö som emulerar Windows. Den implementerade lösningen öppnar därefter DeltaT.exe-fönstret på den externa PC:n så att SONAR-bilden kan ses och så att inställningar kan ändras direkt i GUI:t.

43 38 Figur 18: Skärmbild från mjukvaran DeltaT.exe. I mitten ses SONAR:ens data motsvarande 83B-formatet. Fönstret till höger ställer in uppspelningshastigheter. Fönstret till vänster ger en översikt över SONARdatans rubrik (se kapitel 8.3.2). I menyn högst upp och cirklarna längst ner görs inställningar enligt kapitel Menyval DeltaT har många inställningarval i GUI:t, se figur 18, men menyval enligt tabell 6 kan vara bra att känna till P Datagram Med SONAR:en finns möjlighet att få ut råa mätdata (83B) som är vinklar med intensitetsarrayer eller ett skattad avstånd av föremål för varje lob (83P). Eftersom ROV:n navigerar i en bassäng med tydliga kanter används den senare varianten. Datan som fås ut är en vektor som beskriver avståndet för varje lob till närmsta objekt. Efter varje ping så kommer DeltaT.exe returnera ett paket på totalt 495 bytes enligt tabell 7. Avståndsprofilen kan även kompletteras med ytterligare två bytes per ping som beskriver varje pings intensitet. Den totala paketstorleken blir då 735 bytes.