Teknisk rapport Remotely Operated Underwater Vehicle

Transkript

1 Teknisk rapport Remotely Operated Underwater Vehicle Version 1.0 Författare: Patricia Sundin Datum: 5 december 2012 Status Granskad Alla 05/12/2012 Godkänd Isak Nielsen 05/12/2012

2 Projektidentitet Grupp Hemsida: Beställare: Kund: Kursansvarig: Projektledare: Handledare: tsrt10 Isak Nielsen, Avdelningen för Reglerteknik vid ISY, LiTH Telefon: +46(0) , Micael Derelöv, Saab Underwater Systems Telefon: +46(0) , Daniel Axehill, Avdelningen för Reglerteknik vid ISY, LiTH Telefon: +46(0) , Emelie Nilsson Jonas Linder, Avdelningen för Reglerteknik vid ISY, LiTH Telefon: +46(0) , Gruppmedlemmar Namn Ansvar Telefon Emelie Nilsson (EN) Projektledare emeni712 Alva Olsson (AO) Kvalitetsansvarig alvol428 Christian Andersson Designansvarig chrna575 Naesseth (CAN) Erik Bergman (EB) Testansvarig eribe518 Joakim Zachrisson (JZ) Utvecklingsansvarig mjukvara joaza772 Johan Andersson (JA) Informationsansvarig johan607 Linus Envall (LE) Utvecklingsansvarig hårdvara linen837 Patricia Sundin (PS) Dokumentansvarig patsu498

3 Dokumenthistorik Version Datum Utförda förändringar Utförda av Granskad /11/2012 Första utkastet Alla Alla /11/2012 Andra utkastet Alla Alla /12/2012 Tredje utkastet Alla Alla /12/2012 Fjärde utkastet Alla Alla /12/2012 Första versionen Alla Alla

4 Innehåll 1 Inledning Definitioner Systemöversikt Intern PC ROS Sensor- och motorenhet Motorer Sensorer Extern PC Koordinatsystem Interface Intern PC Topics Hårdvara Intern PC Extern PC Sensor- och motorenhet SONAR Resultat och diskussion Modellering Modell Linjärisering och diskretisering Resultat och diskussion Reglersystem Översikt Reglersystemets kommunikation mellan delsystem Linjärkvadratisk reglering Modellbaserad prediktionsreglering Resultat och diskussion Planering Kommunikation mellan delsystem Matematiska beräkningar för banplanering Kortaste väg Bruten kontakt Delsystemet i ROS Resultat och diskussion Sensorfusion Kommunikation mellan delsystem Extended Kalman Filter Modell

5 8.2.2 Initiering Iterationer Sensorerna IMU Trycksensor SONAR Sensorfusionen i ROS Resultat och diskussion Kommunikation Kommunikation med externa enheter Till moduler Från moduler Kommunikation med interna delsystem Delsystemet i ROS Grafiskt användargränssnitt Utseende Bruten kontakt Delsystemet i ROS Resultat och diskussion A Topics och Messages 32 B Komponentlista 35 C Använd modell 36 C.1 Modell - Rotation C.2 Modell - Translation C.3 Modell - Fullständig C.4 Modellparameterskattning D Kodreferenser 41 D.1 Intern PC D.2 Simulerad funktionalitet Referenser 42

6 Underwater ROV 1 1 Inledning 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. Exempel på uppgifter för en sådan farkost kan vara övervakning, räddningsuppdrag, kartering eller reparationsarbeten. Syftet med detta projekt var att vidareutveckla en Remotely Operated (underwater) Vehicle (ROV) med ett robust reglersystem, samt införa nya sensorer för navigering i syfte att komma närmare en helt autonom farkost. Projektet, som är en påbyggnad från tidigare års projekt där den fysiska konstruktionen av farkosten, modell, simuleringsverktyg samt viss reglering tagits fram, genomfördes på Instutitionen för Systemteknik (ISY) på Linköpings Universitet (LiU) i samarbete med Saab Underwater Systems och Institutionen för Ekonomisk och Industriell utveckling (IEI). I detta dokument ges en detaljerad beskrivning av ROV:n och dess delsystem. Det beskriver hur implementationen har gått till för att uppfylla samtliga krav med prioritet 1 som finns listade i projektets kravspecifikation [1]. 1.1 Definitioner API Arduino-kortet AUV EKF GUI IEI IMU ISY LiU LQ MPC PC PWM RAM ROS ROV SONAR SSPC TCP/IP UDP USB Application Programming Interface Arduino Mega 2560, DEV Autonomous Underwater Vehicle Extended Kalman Filter Graphical User Interface Instutitionen för Ekonomisk och Industriell utveckling Inertial Measurement Unit Instutitionen för Systemteknik Linköpings Universitet Linear Quadratic Model Predictive Control Personal Computer Pulse-Width Modulation Random Access Memory Robot Operating System Remotely Operated (underwater) Vehicle Sound Navigation And Ranging Sea Scan PC Transmission Control Protocol / Internet Protocol User Datagram Protocol Universal Serial Bus

7 Underwater ROV 2 2 Systemöversikt Systemet är uppbyggt så att framföring av ROV:n kan ske enligt följande: Manuell drift Stabiliserad drift Autonom drift I manuell drift styrs ROV:ns motorer direkt via en Xbox-handkontroll kopplad till en extern PC. Reglersystemen är här inaktiva. I stabiliserad drift styrs ROV:n via referensvärden som skickas till ROV:n från den externa PC:n och regulatorerna stabiliserar ROV:ns orientering utifrån dessa referensvärden. I autonom drift är stabiliseringen aktiv och ROV:n styrs via kommandon som skickas till ROV:n från den externa PC:n, exempelvis att färdas till en viss position och orientering. Detta läge kan i framtiden komma att uppgraderas till att utföra mer komplexa uppdrag. Autonom drift har endast testats i simuleringar och är ännu ej implementerat på ROV:n. ROV Sensor- och motorenhet Motorer Ethernet Extern PC Intern PC Arduino Sensorer Figur 1: Blockschema för hårdvaran i systemet där enheterna Extern PC, Intern PC samt Sensor- och motorenhet kan ses. Den röda streckade linjen illustrerar den hårdvara som finns på ROV:n. Pilarna visar informationsflödet. Hårdvarudelen av systemet består av en kontrollstation (extern PC) kopplad via en Ethernet-kabel till ROV:n, samt en intern PC och en sensor- och motorenhet på ROV:n. Figur 1 visar en bild på hur systemet är uppbyggt i hårdvara. All hårdvara beskrivs mer detaljerat i Kapitel 4. Mjukvarudelen av systemet består av de fyra delsystemen Reglersystem, Planering, Sensorfusion och Kommunikation. Figur 2 visar en bild på hur mjukvaran är uppdelad i dess olika delsystem. Mjukvaran finns beskriven mer i detalj i Kapitel 3.

8 Underwater ROV 3 ROV Intern PC Kommunikation Sensorer Extern PC Ethernet Arduino Mjukvara PC Reglersystem Mjukvara Arduino Planering Sensorfusion Motorer Figur 2: Blockschema för mjukvaran i systemet. Intern PC innefattar de fyra delsystemen Kommunikation, Planering, Reglersystem och Sensorfusion. Mjukvara i den externa PC:n och Arduino-kortet visas också. Den röda streckade linjen illustrerar den hård- och mjukvara som finns på ROV:n. Mjukvarumoduler illusteras av helstreckade block och hårdvara av de svarta streck-prickade linjerna. Pilarna visar informationsflödet.

9 Underwater ROV 4 I Figur 1 och 2 visas även hur det huvudsakliga informationsflödet sker. Den röda streckade linjen i båda figurer illustrerar den hårdvara som finns på ROV:n. I Figur 2 illustreras mjukvarumoduler av helstreckade block samt hårdvara, inkluderat för att förtydliga informationsflödet, vilket representeras av de svarta streck-prickade linjerna. Nedan följer en mer detaljerad beskrivning av hårdvaruenheterna. Figur 3: Schematisk bild av ROV:n och motorer sedd; (a) ovanifrån och (b) från sidan. Röda fyrkanter markerar motorernas position. G är tyngdpunkten, B markerar var flytkraften påverkar ROV:n. 2.1 Intern PC Denna enhet består av en dator som kör Robot Operating System (ROS). I denna enhet implementerades mjukvaran för de fyra delsystemen Reglersystem, Planering, Sensorfusion och Kommunikation, se Figur 2. Dessa delsystem beskrivs i Kapitel 6, 7, 8 och 9. Förutom dessa fyra delsystem används ytterligare två moduler, rosserial och xsens IMU, för att kommunicera med Arduino-kortet och IMU:n, se Avsnitt 2.2. Deras kopplingar till övriga moduler visas i Figur 2. Hårdvaran beskrivs mer detaljerat i Kapitel 4. Samtlig kod som körs på den interna PC:n har skrivits i C ROS I ROS delas systemet upp i nodes där varje delsystem representeras av en node. Dessa nodes kommunicerar med varandra via topics. En node kan vara antingen publisher eller subscriber till en topic, som har en eller flera publishers respektive subscribers. En publisher är en node som skickar information genom att skriva ett message till en topic. Ett message är uppbyggt som en struct som kan innehålla flera olika datatyper, till exempel integer, boolean eller floating point. En node som behöver innehållet i en viss topic kallas för dess subscriber och kan hämta information genom att läsa från den.

10 Underwater ROV Sensor- och motorenhet Denna enhet kontrolleras av en Arduino Mega 2560, DEV (Arduino-kortet) som tar emot styrsignaler från regulatorn och omvandlar dessa till pulsbreddsmodulerade (eng: Pulse-Width Modulation) (PWM) signaler till motorerna. Signaler från sensorer skickas obehandlade vidare till sensorfusionssystemet. En Inertial Measurement Unit (IMU) är kopplad via USB direkt till den interna PC:n. Detta beskrivs mer detaljerat i Kapitel 4. Denna enhet skickar även vidare information om batterinivåer och läckagesensordata till den externa PC:n, via den interna PC:n Motorer ROV:n har fem motorer som driver propellrar; en huvudmotor bak samt två mindre motorer vardera i y- och z-led, vilket kan ses i Figur 3 där motorerna är utmärkta i rött. Detta medför att man kan styra fem av ROV:ns sex frihetsgrader separat. Dessa frihetsgrader innefattar x-, y- och z-position samt yaw- och pitchvinkel, men inte rollvinkel. Mer detaljerad information om motorernas mekanik finns att läsa i [4]. Hur samtliga koordinataxlar och vinklar är definierade illustreras i Figur Sensorer För navigation och tillståndsskattning utnyttjas information från en IMU, en trycksensor och SONAR-sensorer. IMU:n består av tre gyron, tre accelerometrar samt en magnetometer, som tillsammans används för att skatta ROV:ns orientering. Eftersom trycksensorn och SONAR-sensorerna ej är integrerade har dessa istället simulerats för att skatta ROV:ns position. En mer detaljerad beskrivning av hur sensorerna används för skattning av tillstånd presenteras i Kapitel Extern PC Den externa PC:n är en kontrollstation för ROV:n som kopplas med en Ethernet-kabel till ROV:ns interna PC. Kommunikationen mellan den externa och den interna PC:n sker via Transmission Control Protocol / Internet Protocol (TCP/IP). I den externa PC:n finns ett grafiskt användargränssnitt där en användare kan övervaka och kontrollera ROV:n. I stabiliserad drift kan användaren skicka referenssignaler för pitch- och yawvinkel, medan man i manuell drift kan styra motorerna direkt med en Xbox-handkontroll. En mer detaljerad beskrivning av GUI:t finns i Kapitel Koordinatsystem Koordinatsystemen som används för att representera ROV:ns pose är ett ROV-fixt S = (x, y, z) T med origo i ROV:ns tyngdpunkt samt ett jordfixt G = (X, Y, Z) T som antas vara ett inertialsystem. ROV:ns position är definierad som den punkt i G där ROV:ns tyngdpunkt befinner sig, det vill säga i origo för det ROV-fixa S (se Figur 4). G är ett North East Down (NED)- system, vilket innebär att X är riktad mot den magnetiska nordpolen. För att underlätta kartrepresentationen kan ett till koordinatsystem G T = ( X, Ỹ, Z) introduceras, som är roterat vinkeln ψ runt Z, där ψ är vinkeln mellan bassängen och nordlig riktning. ROV:ns orientering i det jordfixa koordinatsystemet (X, Y, Z) definieras genom en rotation av (X, Y, Z) i tre steg för att erhålla ett koordinatsystem som är parallellt med det

11 Underwater ROV 6 kroppsfixa (x, y, z) (se Figur 4). Den totala rotationen fa s da (X, Y, Z) fo rst roteras vinkeln ψ positivt runt Z fo r att erha lla ett tillfa lligt koordinatsystem (X 0, Y 0, Z 0 ), vilket i sin tur roteras vinkeln θ positivt runt Y 0 var pa (X 00, Y 00, Z 00 ) erha lls, som till sist roteras vinkeln φ positivt runt X 00. Om rotationen go rs sa dan att det slutliga koordinatsystemet (i Figur 4, det ro da (x, y, z)) a r parallellt med (x, y, z) sa a r ROV:ns orientering da va ldefinierad av vinklarna φ, θ och ψ. Figur 4: Illustration av koordinatsystemen samt definitioner av vinklarna φ, θ och ψ. De ro da koordinatsystemen (X 0, Y 0, Z 0 ) och (X 00, Y 00, Z 00 ) illustrerar stegen i rotationen av det jordfixa (X, Y, Z) och det ro da (x, y, z) a r det slutgiltiga som erha lls efter rotationen och a r parallellt med det kroppsfixa (x, y, z). p, q och r a r vinkelhastigheter i det kroppsfixa koordinatsystemet. Kursnamn: Projektgrupp: Kurskod: Projekt: Reglerteknisk projektkurs, CDIO ROV TSRT10 Underwater ROV Dokumentansvarig: Fo rfattarens Dokumentnamn: tsrt10 rov@googlegroups.com Patricia Sundin patsu498@student.liu.se Teknisk rapport

12 Underwater ROV 7 3 Interface I detta kapitel beskrivs interface mellan de olika delarna av ROV:n. I Avsnitt 3.1 beskrivs interface mellan de fyra delsystemen Reglersystem, Planering, Sensorfusion och Kommunikation. 3.1 Intern PC Delsystemen i den interna PC:n använder sig av topics och messages för att kommunicera mellan varandra. På den interna PC:n är varje delsystem en node. Tabell 1 beskriver de nodes som finns i den interna PC:n tillsammans med tillhörande namn och kapitel. Tabell 1: Alla nodes. Namn Motsvarande delsystem Kapitel control Reglersystem Kapitel 6 plan Planering Kapitel 7 sensor Sensorfusion Kapitel 8 com Kommunikation Kapitel 9 GUI Grafiskt användargränssnitt Kapitel 10 arduino Arduino Kapitel 2.2 xsens IMU IMU Kapitel Topics En topic är en typ av kommunikationsbuss i ROS som gör det möjligt för nodes att utbyta data. En topic har en eller flera publishers och en eller flera subscribers. Publishers och subscribers behöver ej vara medvetna om varandras existens. En topic har fördefinierade message-typer som anger strukturen på de messages som en publisher kan skicka. Tabell 4 i Appendix A visas de topics som används, innehållet i relaterade messages samt publishers och subscribers. Alla message består av varsin struct, som är uppbyggd enligt Tabell 6 i Appendix A. Då vissa av dessa topics och messages är återanvända från tidigare projekt så följs ej namnkonventionen helt. En tabell som förklarar variabelnamn för kommunikation med externa enheter kan ses i Appendix A, Tabell 5. Alla topics har ett main message bestående av msg plus namnet på respektive topic minus top, till exempel är msg Mode main message för topic top Mode. Undantag är: top GuiRef vars main är message msg Command; top GuiCalcAutoRef vars main message är msg Ref; top GuiControlSig vars main message är msg ControlSignals; samt top GuiStates vars main message är msg States.

13 Underwater ROV 8 4 Hårdvara Här beskrivs de hårdvarukomponenter som används i ROV:n. En lista över komponenterna som används finns i Appendix B, och ett kopplingschema över hårdvaran finns att tillgå i [4]. 4.1 Intern PC Den interna PC:n består av ett moderkort med inbyggd processor (Intel D525MW), Random Access Memory (RAM) (Kingston 4096MB (2x2GB) DDR3 PC MHz) och en hårddisk (OCZ 60 GB Vertex II E-series). På denna PC körs ROS och det är där delsystemen Sensorfusion, Planering, Kommunikation och Reglering är implementerade. 4.2 Extern PC Det finns en extern PC i form av en laptop med operativsystemet Linux. Denna kan kopplas till ROV:ns interna PC via en Ethernet-kabel. I tidigare projekt har User Datagram Protocol (UDP) använts för dataöverföring, vilket inte fungerat önskvärt (problem med package loss uppstod). Därför används i detta projekt istället TCP/IP. Detta protokoll är inbyggt i den ROS-kommunikation som används mellan den interna och externa PC:n. Från denna externa PC kan en användare styra ROV:n, dels via ett grafiskt användargränssnitt, beskrivet i Kapitel 10 men även via en Xbox-handkontroll. 4.3 Sensor- och motorenhet Denna enhet består av Arduino-kortet, samtliga sensorer samt motorer. Arduino-kortet är anslutet till den interna PC:n via USB och har två uppgifter; att styra motorerna och att läsa in sensordata från en tryckgivare som är kopplad till en analog ingång på Arduino-kortet. Kortet styr motorerna genom att ta emot styrsignaler från delsystemet Reglersystem på den interna PC:n och omvandla dessa till PWM-signaler vilka skickas till motorernas styrkort. Övriga sensorer innefattar en IMU och var tänkt att innefatta SONAR-sensorer, beskrivna i Kapitel 4.4. IMU:n är av modell MTi från Xsens och är kopplad via en USB-kabel till den interna PC:n. Dock har inga SONAR-sensorer integrerats i detta projekt, men det kommer vara aktuellt i framtida projekt. 4.4 SONAR SONAR-sensorer skulle tillhandahållas av Saab Underwater Systems och användas för navigering, och tre alternativ skulle utvärderas. De alternativ som skulle finnas att tillgå var följande: Delta T Imaging 837 från Imagenex Sea Scan PC (SSPC) Side Scan Sonar System från Marine Sonic Technology, Ltd Använda båda ovanstående Det alternativ som framstod som bäst var SSPC-sensorerna, då Delta T-sensorn troligtvis skulle göra ROV:n framtung. Detta skulle eventuellt påverka dynamiken mer än vad

14 Underwater ROV 9 som är att betrakta som försumbart. Delta T-sensorn kan mäta absolut avstånd utan någon ytterligare information. För att bestämma avstånd med SSPC-sensorerna krävs att avståndet till botten är känt, vilket kan bestämmas med trycksensorn och en karta över omgivningen. SSPC-sensorerna skickar ut pulser som reflekteras, och tidsberoende intensitet för de reflekterade pulserna kan erhållas. Då svarar maxima i intensitet mot de pulser som reflekterats rakt från sidan och rakt nedifrån. Med denna information kan avståndet till väggarna bestämmas, då maximumet som svarar mot reflexionen från botten kan sorteras bort eftersom detta avstånd är känt. Till SSPC-sensorerna hör ett antal kretskort, till vilka de är inkopplade. Eftersom sensorerna är av dubbelfrekvenstyp kopplas fyra signalkanaler in. Detta kort kommunicerar med den interna PC:n via RS232. Den interna PC:n kan styra kortet genom att skicka kommandon med Host/Remote-protokollet från Marine Sonic. På kortet sitter en masslagringsenhet på vilken mätdata lagras. Denna information skickas också kontinuerligt till den interna PC:n. Delta T-sensorn kommunicerar med den interna PC:n via TCP/IP med en Ethernetkabel. Denna kopplas längst fram på ROV:n vilket kan användas för att skatta position och hastighet i x-led. 4.5 Resultat och diskussion I detta projekt var det tänkt att trycksensorn skulle integreras för djupmätningar. Av tekniska skäl var detta dock inte möjligt. Ett problem har varit Arduino-kortet då dess databuffert snabbt blir full, och kortet hinner inte med att skicka all data. Eftersom trycksensorn ska kopplas in till Arduino-kortet kommer ännu mer data att behöva skickas. Detta är uppenbarligen ett problem då kortet redan är överbelastat. Arduino-kortet kommer därför troligtvis att behöva bytas ut mot ett kort med högre kapacitet. SSPC-sensorerna har ett antal tillhörande kort, vilka ej fick plats i den nuvarande ROV:n. Därför kunde de ej integreras, utan detta skulle istället vara ett framtida projekt för en ny ROV. Istället skulle de användas för mätningar på en testrigg och ge ut mätdata som skulle användas för positionering. Det visade sig dock vara omöjligt att genomföra då Delta T-sensorn tillhandahölls långt senare än vad som hade behövts och SSPC-sensorerna inte tillhandahölls alls. Delta T-sensorn var också en möjlighet för positionering. Det visade sig dock att inte heller denna gick att använda på den nuvarande ROV:n av olika skäl. Även denna kräver elektronik som inte får plats. Dessutom väger den relativt mycket, och är tänkt att sitta längst fram på ROV:n, vilket troligtvis skulle göra ROV:n framtung, och därmed försvåra stabilisering. Enligt simuleringar klarade regulatorerna av att stabilisera ROV:n då en vikt på 0,5 kg hängs på längst fram. Detta krävde dock maximala styrsignaler, vilket indikerar att det inte vore möjligt att stabilisera för större vikter. Denna sensor skulle sitta längst fram, och väger 2,49 kg, och alltså vore det omöjligt att stabilisera ROV:n med denna sensor monterad. Detta är ett problem som måste lösas i framtiden om denna sensor ska kunna användas, exempelvis genom att använda någon typ av pontoner för att erhålla en större flytkraft. Eftersom ROV:ns nos redan är fylld med frigolit skulle dessa behöva monteras externt vilket skulle påverka ROV:ns dynamik, något som troligtvis behöver modelleras. Denna sensor tillhandahölls, dock så sent att det i praktiken var omöjligt att implementera någon navigering med den. Det visade sig att IMU:n störs av batterierna och/eller elektroniken. Det var känt sedan innan att vinkelskattningen störs när motorerna körs med full spänning. Det visade sig dock att skattningen av yawvinkeln är kraftigt störd oavsett motorspänning; dels erhålls vinkeln 0 då ROV:n ej är riktad norrut, och dels är vinkelförändringen skalad. Vid en

15 Underwater ROV 10 vridning på 90 grader i yawled förändrades den skattade vinkeln bara omkring 30 grader. Detta är ett problem som måste tas hänsyn till i kommande projekt; IMU:n måste placeras någon annanstans i ROV:n.

16 Underwater ROV 11 5 Modellering Modellen som använts i detta projekt är den felaktiga som togs fram i [3], se Appendix C. Härledningen i [3] tar ej hänsyn till att ROV:n är en stel kropp och både roterar samt translaterar. En mer korrekt modell diskuteras i Avsnitt 5.1 och linjärisering och diskretisering av modellen diskuteras i Avsnitt 5.2. Definition av parametrar kan ses i Tabell 2 och variabler i Tabell 3. Tabell 2: Definitioner av modellparametrar använda i tillståndsmodellen, se (1) och (2). Parameter C zr, C zf, C yr, C yf, C T I x, I y, I z I x, I y, I z C r, C p, C y C rd, C pd, C yd z B F B x zr, x zf, x yr, x yf C Dx, C Dy, C Dz A x, A y, A z ρ m m x, m y, m z m g V Förklaring Motorkoefficienter Rotationströghetsmoment i roll-, pitch- och yaw-led Tillagt rotationströghetsmoment i roll-, pitch- och yawled p.g.a. rörelse i vätska Linjära dämpningskoefficienter för roll-, pitch- och yawhastighet Kvadratiska dämpningskoefficienter för roll-, pitch- och yaw-hastighet Avstånd i z-led från masscentrum till flytkraftsangreppspunkten Flytkraften Avstånd från motorer till masscentrum Motståndskoefficienter för translationsvattenmotstånden i x-, y- och z-led Tvärsnittsarea för ROV:n i x-, y- och z-led Vattendensitet, antas konstant ROV:ns massa Tillagd massa i roll-, pitch- och yaw-led p.g.a. rörelse i vätska Tillagd massa p.g.a. acceleration i vätska Tyngdacceleration ROV:ns vattentäta volym Tabell 3: Definitioner av modellvariabler använda i tillståndsmodellen, se (1) och (2). Variabler u T, u zr, u zf, u yr, u yf Förklaring Styrsignaler till motorerna (bakre thruster, vertikal thruster bak/fram, horisontell thruster bak/fram), [V ] φ, θ, ψ Roll, pitch och yaw-vinkel, [rad], eulervinklar n, e, d Norr-, öst- och ned position, [m], världsfix u, v, w Surge-, sway- och heave hastighet, [m/s], kroppsfix p, q, r Roll-, pitch- och yaw hastighet, [rad/s], kroppsfix 5.1 Modell Här ges en mer korrekt modell än den som tidigare tagits fram då den även tar hänsyn till att ROV:n är en stelkropp. Den har dock inte använts på grund av att upptäckten att den gamla modellen var felaktig gjordes då projektet var i ett väldigt sent skede. Härledningen här bygger på framställningen i [8], Kapitel 3 och 4, men även delvis på

17 Underwater ROV 12 krafter och moment framtagna i [3]. Med notation enligt Tabell 2 och 3 fås ekvationerna för kroppsfix dynamik enligt u = C T u T u T C DxA x ρ (m ρv )g u u sin(θ) wq + vr, (1a) m + m x m + m x m + m x v = Cyr m+ m y u yr u yr + C yf m+ m y u yf u yf C DyA yρ m+ m y v v + (m ρv )g + m+ m y cos(θ) sin(φ) + wp ur, (1b) ẇ = Czr m+ m z u zr u zr + C zf m+ m z u zf u zf C DzA zρ m+ m z w w + (m ρv )g + m+ m z cos(θ) cos(φ) + uq vp, (1c) ṗ = C T (I u C x+ I x) T u T r (I p C rd x+ I x) (I p p x+ I x) z BF B (I x+ I x) sin(φ) ((Iz+ Iz) (Iy+ Iy)) (I x+ I x) qr, (1d) q = Czrxzr I y+ I y u zr u zr C zf x zf I y+ I y u zf u zf Cp I y+ I y q C pd I y+ I y q q z BF B I y+ I y sin(θ) ((Ix+ Ix) (Iz+ Iz)) I y+ I y rp, (1e) ṙ = Cyrxyr (I z+ I z) u yr u yr + C yf x yf (I z+ I z) u yf u yf Cy (I z+ I z) r C yd (I z+ I z) r r ((Iy+ Iy) (Ix+ Ix)) (I z+ I z) pq. (1f) Högerleden i dessa ekvationer beskrivs med kompakt notation f b ( ). Här har matriserna för tillagd massa och tröghetsmoment antagits vara diagonala, några moment har satts till noll enligt diskussion i [3] samt vattenmotstånd har modellerats enligt diskussion i Avsnitt C.2. Vidare om R(φ, θ, ψ) och T (φ, θ, ψ) definieras som R(φ, θ, ψ) = cos φ cos θ sin ψcosφ + cos ψ sin θ sin φ sin ψ sin φ + cos ψ cos φ sin θ sin ψ cos θ cos ψcosφ + sin φ sin θ sin ψ cos ψ sin φ + sin ψ cos φ sin θ sin θ cos θ sin φ cos θ cos φ 1 sin φ tan θ cos φ tan θ T (φ, θ, ψ) = 0 cos φ sin φ, 0 sin φ cos θ cos φ cos θ och med η = (n, d, e, φ, θ, ψ) T, ν = (u, v, w, p, q, r) T fås den fullständiga tillståndsmodellen som ( ) R(φ, θ, ψ) 03x3 η = ν, (2a) 0 3x3 T (φ, θ, ψ) }{{} J, ν = f b (η, ν, u). (2b)

18 Underwater ROV 13 Med en mer kompakt notation x = (η T, ν T ) T och u = (u T, u zr, u zf, u yr, u yf ) T kan tillståndsmodellen skrivas som ( ẋ = f(x, u) = Jν f b (x, u) ) (3) 5.2 Linjärisering och diskretisering För att använda tillståndsmodellen och implementera en LQ-regulator krävs en linjär och diskret modell. Modellen kan först linjäriseras runt en punkt (x 0, u 0 ) med en första ordningens Taylor-utveckling och därefter, givet sampeltiden T s, diskretiseras med Euler framåt ẋ x k+1 x k T s = f(x k, u k ). (4) Med x = x x 0 och u = u u 0 fås den diskreta och linjäriserade tillståndsekvationen där elementen i matriserna A och B ges av A ij = f i(x, u) x j x k+1 = (I + T s A) x k + T s B u, (5) (x,u)=(x0,u 0) och B ij = f i(x, u) u j (6) (x,u)=(x0,u 0). De valda linjäriseringspunkterna och resulterande matriserna beskrivs närmare i Avsnitt 6.3 för den felaktiga modellen i Appendix C. 5.3 Resultat och diskussion Då modellen som har använts för ROV:n är felaktig och enbart gäller då ROV:n står still alternativt roterar runt en axel så har den gett en dålig grund att stå på för de tekniska kraven för reglering, planering och sensorfusion, vilket bland annat har försvårat uppfyllandet av reglerkraven på ROV:n. Följdaktligen leder detta till att en ny modell måste härledas och parametrar skattas för den nya ROV:n då man måste ta hänsyn till stelkroppsdynamiken och att det kroppsfixa koordinatsystemet rör sig relativt det världsfixa. För detta bör man basera den nya framtagna modellen på (3) vilken bygger på modellering för båtar och skepp enligt [8]. Det kan även vara intressant att jämföra denna med framställningen av en ROV:s dynamiska ekvationer som finns i [9]. Ytterliggare en utökning av den nya modellen måste ske då ROV:n i framtiden kommer att ha två roder för att styra enklare vid höga hastigheter.

19 Underwater ROV 14 6 Reglersystem Detta kapitel beskriver vilka regleralgoritmer som ROV:n utnyttjar och hur de fungerar samt reglersystemets interna kommunikation. 6.1 Översikt ROV:n utnyttjar två sorters regleralgoritmer för olika syften. Den har en linjärkvadratisk (eng: Linear Quadratic) (LQ-)regulator för att bevisa att det är möjligt att kunna reglera ROV:n överhuvudtaget. Denna används som standardregulator i den färdiga produkten. LQ-regulatorn har omarbetats från föregående projekt och testats på ROV:n. Den andra algoritmen är en olinjär MPC och det är en alternativ regulator som kan aktiveras via GUI:t. LQ-regulatorn använder sig av följande fysikaliska grundmodell på linjär och tidsdiskret tillståndsform, x k+1 = Ax k + Bu k, (7a) z k = Cx k, (7b) där x k är tillståndsvektorn, u k styrsignalsvektorn och z k reglerstorhetsvektorn. A, B, och C är tillhörande matriser. 6.2 Reglersystemets kommunikation mellan delsystem Reglersystemet kommunicerar med delsystemen Planering, Kommunikation och Sensorfusion. Från kommunikationssystemet fås information om huruvida den ska använda LQreglering eller MPC. Reglersystemet får referensvärden från delsystem Planering för att sedan skicka styrsignalerna till delsystem Kommunikation, som i sin tur skickar styrsignalerna till ROV:ns motorer. Från delsystemet Sensorfusion erhåller reglersystemet skattade tillstånd. För illustration av detta informationsflöde se Kapitel 2, Figur Linjärkvadratisk reglering LQ-reglering bygger på att man vill reglera systemet med en avvägning mellan snabb reglering och små styrsignaler. Detta kan översättas till följande ekvation J (x 0 ) = min x,u ( x k 2 Q + u k 2 R), (8) k=0 där Q och R är viktmatriser. Exempelvis gör ett stort Q relativt R att regulatorn prioriterar att driva tillstånden till origo snabbt men med följden att styrsignalerna kan bli stora. LQ-regleringen har även tillgång till integralverkan för position, pitch- och yaw-vinklar för att motverka de stationära felen. För att implementera integralverkan har nya tillstånd införts enligt x I,k+1 = x I,k + T s (z k r k ), (9)

20 Underwater ROV 15 där z k är vektorn med de tillstånd som integreras och r k är en vektor med referensvärden. Detta ger tillsammans med (7) det utökade systemet [ xk+1 x I,k+1 ] = F [ xk z k = [ C x I,k ] [ ] 0 + Gu k r T s I k, 5 (10a) ] [ ] x 0 k 5 5, x I,k (10b) där F, G, och C är tillhörande matriser med modellen i Appendix C.3 linjäriserad i origo enligt Avsnitt 5.2. Dessa matriser fås enligt z BF B I xx Cr I xx z BF B I yy Cp I yy A = Cy I zz , (11a) ρv g mg 0 0 M ρv g mg M C T I xx C zrx zr I yy C zf x zf I yy Cyrxyr C yf x yf B = I zz I zz 0, (11b) C T M C 0 0 yr C yf M M C zr C zf M M [ ] I12 + A T F = s , (11c) C T s I 5 [ ] B Ts G =, (11d) C = (11e) Där I N är en enhetsmatris med storleken N N och 0 N M är en nollmatris med storleken N M.

21 Underwater ROV 16 Systemet optimeras[ sedan ] enligt (8) med skillnaden att tillståndsvektorn är utökad enligt (10a) till x k = xk. De straffmatriser som används är x I,k Q = diag([ ]), R = diag([ ]). (12) Straffmatriserna är framtagna genom empiriska test för att reglersystemet ska uppfylla simuleringskraven. Endast de integrerade tillstånden straffas eftersom det ger bäst resultat. Att sista elementet i Q-matrisen är avsevärt större än de andra beror på att de krav som berör referensföljning i Z-led är svårare att uppfylla och medför att dessa krav uppfylls. De integrerade tillstånden uppdateras i varje iteration enligt (9). Dessa tillstånd nollställs då byte sker från manuellt till stabiliserat läge eller då byte sker från MPC till LQ. 6.4 Modellbaserad prediktionsreglering Ett problem med LQ-regulatorn är att ROV:n har begränsade styrsignaler vilket LQregleringen inte kan ta hänsyn till. Att MPC kan hantera bivillkor på tillstånd och styrsignaler är en av MPC:ns största styrkor. I detta projekt realiserades en olinjär MPC för stabilisering i rotationsled. Problemet som löses vid varje iteration beskrivs av minimera x( ),u( ) då t0+t t 0 x(t) x ref (t) 2 Q + u(t) u ref(t) 2 R dt + + x(t 0 + T ) x ref (t 0 + T ) 2 P, x(t 0 ) = x 0, ẋ(t) = f(x(t), u(t)), x x(t) x, u u(t) u, (13) där x och u är systemets tillstånd respektive styrsignaler med respektive referensvärde x ref och u ref, och T är styrsignalshorisonten. Vidare är x(t) x ref (t) 2 Q = (x(t) x ref (t)) T Q(x(t) x ref (t)) för en positivt semi-definit matris Q, analogt för styrsignalerna och matrisen R samt sluttillståndsstraffet x(t 0 + T ) x ref (t 0 + T ) 2 P och matrisen P. Bivillkor består av initialtillståndet x 0, dynamiska ekvationen ẋ = f(x, u) och linjära bivillkor på tillstånd och styrsignaler, x, x, u, u. Denna framställning är en förenkling av den mer generella givet i [13, Kap. 7], och är olinjär då x måste uppfylla den dynamiska ekvationen. Med hjälp av ACADO-kodgenerering, se [13, Kap. 9], har en olinjär MPC realiserats i C-kod för realtidsimplementation av stabilisering av ROV:ns orientering. För denna implementation samt simuleringarna valdes T = 3 s och straffmatriserna som följer

22 Underwater ROV 17 Q = diag([ ]), R = diag([0, 1 0, 1 0, 1 0, 1 1, 0]), P = diag([ ]). (14) Då ACADO kodgenerering ej stödjer användande av funktioner av typen x x har dessa approximerats med ett polynom av grad 9, dvs b 1 x + b 2 x 2 + b 3 x b 9 x 9. Detta gav, med minimering av kvadratfelsumman, ett Root Mean Square Error (RMSE) på cirka 0, 01 på intervallet [ 2, 5, 2, 5]. Bivillkoren för styrsignalerna sattes alla till u = 2, 5 V och u = 2, 5 V, för tillstånden sattes bivillkoret att roll-vinkeln ska hålla sig inom ±15 grader, dvs cirka φ = 0, 26 rad och φ = 0, 26 rad. All simulering av MPC:n som beskrivs i [2] har utförts med ACADO på genererad C-kod. Design och testning av denna kod har även gjorts i bassäng. För simuleringarna för den fullständiga modellen, se (40), har följande straffmatriser använts Q = diag([ ]), R = diag([0, 1 0, 1 0, 1 0, 1 1, 0]), P = diag([ ]). (15) Kodgenerering och simulering har annars utförts på samma sätt som för rotationsmodellen. 6.5 Resultat och diskussion Resultat för simulering av MPC:n var som väntat, den följer referensvärdena väl då systemet ej utsätts för någon störning. Då ROV:n utsätts för konstanta störningar har MPC:n problem att få ROV:n till sin utgångsorientering eller önskad orientering då den saknar integratorverkan. Integratorverkan vore önskvärt men ingen lösning för att implementera detta med ACADO kodgenerering hittades. Vid test av MPC:n i bassäng verkade den interna PC:n ej ha några problem att lösa optimeringsproblemet i realtid. MPC:n såg även till att bivillkor för roll och styrsignaler hölls. Regulatorn kunde återgå till ursprungsorientering efter störning i form av att en yttre kraft applicerades momentant. MPC:n uppfyllde kraven, dock testades även flera av kraven som endast gällde simuleringar och dessa uppfylls ej. Precis som MPC:n uppfyller LQ-regulatorn kraven i simuleringar. På grund av integratorverkan klarar LQ-regulatorn även av att driva tillbaka ROV:n till sin utgångsorientering då den utsätts för konstanta störningar. Eftersom modellen som ligger till grund för LQregulatorn är linjäriserad kan man inte få samma prestanda som den olinjära MPC:n men genom att använda fler linjäriseringspunkter och interpolera mellan de tillhörande L-matriserna (så kallad gain scheduling) skulle bättre prestanda kunna erhållas. Detta är något som har undersökts men inte testats på ROV:n eftersom de olika L-matriserna var så pass lika varandra. Dessutom gick de uppsatta kraven gick att uppnå utan gain scheduling i detta projekt. Det kan dock vara intressant att vidare undersöka detta i framtida projekt efter att en ny modell tagits fram.

23 Underwater ROV 18 Efter test med flera kombinationer av straffmatriser, erhölls aldrig någon tillfredsställande referensföljning vid undervattenstest för varken MPC:n eller LQ-regulatorn. Antingen reagerade motorerna för häftigt eller inte alls. Detta ansågs till stor del bero på ROV:ns oförmåga att hålla sig under ytan under hela manövern men även på brister i den använda modellen.

24 Underwater ROV 19 7 Planering Detta kapitel beskriver hur delsystemet Planering kommunicerar med andra delsystem samt hur en banplanering för att hitta kortaste vägen till en slutposition har tagits fram vid simulering. Syftet med delsystemet Planering är att ta fram en banplanering genom att generera en referenstrajektoria som skickas till delsystemet Reglersystem. 7.1 Kommunikation mellan delsystem I första hand tar planeringen hand om styrkommandon som har skickats från den externa PC:n via delsystemet Kommunikation. Vid manuell drift går styrsignalerna direkt från delsystemet Kommunikation till Arduino-kortet, utan att ta omvägen via planeringen. Vid stabiliserad drift skickas referenssignalerna från den externa PC:n via planeringen, utan att förändras, till delsystemet Reglersystem. Från delsystemet Sensorfusion fås information om skattade tillstånd. Planeringen får även information om kontakten med den externa PC:n har brutits. För illustration av detta informationsflöde se Kapitel 2, Figur Matematiska beräkningar för banplanering Detta avsnitt förklarar hur banplaneringen beräknas i simuleringar samt hur bruten kontakt hanteras Kortaste väg För att hitta kortaste vägen till en position och för att nå en slutorientering som är givet av den externa PC:n ska ROV:n först roteras så att den är riktad mot slutpositionen, sedan ska den köra rakt mot slutpositionen. Väl vid slutpositionen ska ROV:n rotera så att den får rätt orientering. Planeringen behöver få en slutposition och slutorientering från den externa PC:n för att klara av denna uppgift samt en karta över omgivningen. Hur referenssignalerna ska tas fram för att nå slutpositionen och slutorienteringen beskrivs i detta delavsnitt. ROV:n utgångsposition är (x 0, y 0, z 0 ) och utgångsorienteringen är (ψ 0, θ 0 ), då roll-vinkeln ej går att styras separat finns inget utgångsvärde för denna vinkel. För att ROV:n ska komma till slutposition (x 1, y 1, z 1 ), ska den först roteras så att ROV:n kan köra rakt mot slutpositionen. ROV:n börjar med att ställa sig i rätt vinkel i yaw-led. Referensvinkeln i yaw-led ges av Därefter beräknas referensvinkeln i pitch-led enligt ψ ref = 90 arctan y 1 y 0 x 1 x 0. (16) θ ref = arctan z 1 z 0 x 1 x 0. (17) Då θ ref och ψ ref har beräknats, skickas dessa referenssignaler till delsystemet Reglersystem. Detta tillämpas sedan i varje sampel tills dess att ROV:n har erhållit rätt orientering vilket ges av kraven ψ ref ψ 0 < 2, (18a) θ ref θ 0 < 2. (18b)

25 Underwater ROV 20 När ROV:n har erhållit önskad orientering, enligt de båda referenssignalerna ψ ref och θ ref, skapas en referensvektor genom att ta skillnaden mellan ROV:ns position i rummet och referenspositionen enligt v ref = x ref y ref z ref x 1 x 0 = y 1 y 0. (19) z 1 z 0 Denna vektor diskretiseras sedan i N delar med k som index enligt v ref,k = k N v ref, k {1, 2,..., N} (20) där sedan ROV:n reglerar mot en referenspunkt i taget med en noggrannhet på där tills den når den slutgiltiga referenspositionen x 1, y 1, z 1. v ref,k v 2 < 0.1 m (21) x v = y (22) z När slutpositionen väl har erhållits, roterar ROV:n för att nå den givna referensorienteringen (ψ 1, θ 1 ). Dessa vinklar skickas som referenssignaler till reglersystemet, utan bearbetning av planeringen Bruten kontakt Om kontakten med den externa PC:n bryts, skickar planeringen referenssignaler till delsystemet Reglersystem som resulterar i att ROV:n kör upp till ytan. Denna planering innebär att referenspositionen är z 1 = 0 med bibehållna x- och y-positioner samt θ ref = 0. Referenssignaler skickas till reglersystemet så att ROV:n långsamt förflyttar sig till denna position. 7.3 Delsystemet i ROS Detta delsystem är implementerat i node plan i ROS. Mer om interface mellan olika nodes i Kapitel Resultat och diskussion Vid autonom drift var det tänkt att planeringen skulle beräkna egna referenssignaler och skicka vidare dessa till delsystemet Reglersystem. Referensvärden fås från den externa PC:n i dagsläget, dock används de ej. Det är även tänkt att planeringen ska få en karta över ROV:ns omgivning från delsystem Kommunikation. Denna karta behövs för att kunna ta fram en banplanering i det autonoma läget. Inte heller detta har hunnit implementeras då inga SONAR-sensorer finns monterade på ROV:n och därmed finns inget sätt att bestämma ROV:ns position. Denna funktionalitet är dock implenterat och testad i simuleringar. I början av projektet var det tänkt att en banplanering för att hitta kortaste väg till en position och orientering skulle implementeras i planeringen. Tyvärr fanns ingen tid

26 Underwater ROV 21 över för detta av samma anledning som med kartan, så banplanering för kortaste väg simulerades endast. Funktionaliteten med bruten kontakt må vara fullt implementerad i planeringen, dock ignorerar delsystemet Reglersystem dessa referensvärden. Även delsystemet Reglersystem blir tilldelad informationen om att kontakten är bruten och kommer då att sätta ut konstanta värden på motorernas referensvärden. Anledningen till detta är att trycksensorn ej är integrerad och därmed kan position i z-led inte bestämmas.

27 Underwater ROV 22 8 Sensorfusion Detta kapitel beskriver teoretiskt lösningar och algoritmer som rör skattningar av ROV:ns tillstånd. 8.1 Kommunikation mellan delsystem Oberoende av ifall ROV:n befinner sig i autonom drift, stabiliserad drift, manuell drift eller nödläge har detta delsystem samma funktionalitet. Sensorfusionen hämtar alltid data från IMU:n via kommunikationsenheten och publicerar systemets tillstånd tillbaka till kommunikationsenheten för att alla andra delsystem ska komma åt tillstånden. 8.2 Extended Kalman Filter Eftersom en linjär modell av ROV:n troligtvis är otillräcklig har ett Extended Kalman Filter (EKF) utvärderats för skattning av dess tillstånd. Beroende på om första eller andra ordningens taylorutvecklingar av sensor- och rörelsemodellen används erhålls antingen EKF1 eller EKF2. Enligt [3] användes EKF1 med tillfredställande resultat, så därför användes EKF1 även i detta fall. För att eventuellt kunna uppnå högre prestanda hos beräkningarna kan en Square Root Implementation av EKF1 undersökas. I detta projekt har dock enbart implementation för simuleringar gjorts, och då har prestandan inte varit av alltför stor vikt. För vidare information om Kalmanfilter, EKF och Square Root Implementation, se Gustafsson 2010 [7] Modell Process- och mätbrus antas vara additiva. Detta är ett vanligt antagande inom sensorfusion och implementationen har gett tillfredsställande resultat. Med additiva brus erhålls modellerna x k+1 = f(x k, u k ) + v k, Cov(v k ) = Q k, (23a) y k = h(x k, u k ) + e k, Cov(e k ) = R k. (23b) Initiering Innan första iterationen initieras tillståndsskattningen och kovariansen för felet i skattningen enligt ˆx 1 0 = E(x 0 ), (24a) P 1 0 = Cov(x 0 ). (24b) Iterationer Uppdatering av tillståndsskattning och kovarians för felet i skattningen givet en ny mätning ges enligt

28 Underwater ROV 23 while true do Mätuppdatering: S k = h x(ˆx k k 1 )P k k 1 (h x(ˆx k k 1 )) T + R k K k = P k k 1 (h x(ˆx k k 1 )) T S 1 k ɛ k = y k h(ˆx k k 1 ) Uppdatera skattning av tillståndsvektorn: ˆx k k = ˆx k k 1 + K k ɛ k Uppdatera skattning av kovariansen för felet: P k k = P k k 1 P k k 1 (h x(ˆx k k 1 )) T S 1 k h x(ˆx k k 1 )P k k 1 Tidsuppdatera tillståndsvektorn: ˆx k+1 k = f(ˆx k k ) Tidsuppdatera kovariansmatrisen: P k+1 k = f x(ˆx k k )P k k (f x(ˆx k k )) T + Q k end 8.3 Sensorerna Här modelleras hur mätningar från de olika sensorerna ser ut IMU Med hjälp av IMU:n som är integrerad kan ROV:ns orientering skattas. IMU:n är en enhet som är utrustad med accelerometrar, magnetometer och gyroskop som mäter resulterande acceleration och resulterande magnetfältstyrka i tre ortogonala riktningar samt vinkelhastigheter runt var och en av axlarna i dessa riktningar. Detta gör det möjligt att mäta ROV:ns orientering i bassängen med stor noggrannhet. Dock är det med endast en IMU svårt att skatta hastighet och position då dessa skattningar driver och felet ökar snabbt. IMU:n som används i ROV:n har ett inbyggt kalmanfilter som implementerats av tillverkaren, Xsens [3], vilket gör det möjligt att ta ut orienteringen direkt. Utdata från IMU:n kan även fås som råa mätningar eller kalibrerade mätningar där det temperaturberoende bias som finns hos gyroskopen har tagits hänsyn till. Rå mätdata är tillgänglig både direkt från A/D-omvandlaren eller i fysikaliska storheter. Den skattade orienteringen finns tillgänglig som eulervinklar, kvaternioner eller som en rotationsmatris. Det kan vara av intresse att ha tillgång till både filtrerad och ofiltrerad data för att eventuellt kunna utvärdera skillnad i kvalitet hos skattningen redan filtrerad orientering används kontra rådata i fusion av sensordata från samtliga sensorer. I detta projekt har det interna Kalmanfiltret använts, då inga andra sensorer hann integreras på ROV:n, och datan som används är eulervinklar. En mätning från IMU:n är alltså en vektor (φ, θ, ψ) där φ, θ och ψ är definierade enligt Figur 4 och är mätta i grader: y IMU k = (φ k θ k ψ k ) T + e IMU k, e IMU k N(µ k, σk). 2 (25) IMU:n sitter placerad så att dess position är förskjuten något i x-led i det kroppsfixa koordinatsystemet och roterad ett halvt varv runt x- respektive z-axeln. Till följd av roteringen så negeras mätningarna av accelerationer längs x- och z-axlarna samt rolloch yaw-vinklen samt roll- och yaw-vinkelhastigheterna för att mätningarna ska vara i enlighet med koordinatsystemen. Till följd av förskjutningen påverkas accelerationerna av vinkelhastigheterna och vinkelaccelerationerna på ett sådant sätt att accelerationen som mäts är a IMU = a ROV + α r IMU + ω (ω r IMU ), (26)

29 Underwater ROV 24 där a IMU är den uppmätta accelerationen, a ROV är accelerationen för tygndpunkten hos ROV:n, α är vinkelaccelerationsvektorn, ω är vinkelhastighetsvektorn och r IMU är IMU:ns placering i det kroppsfixa koordinatsystemet. Eftersom det dock är svårt att mäta exakt vart tyngdpunkten är och eftersom att accelerationerna varken tagits med som, eller använts för beräkning av tillstånd, har detta samband inte visats hänsyn till i mätningarna Trycksensor Trycksensorn som sitter monterad i ROV:n mäter, likt många andra trycksensorer, skillnaden mellan det aktuella trycket och ett referenstryck. I detta fall är dock referenstrycket 0 P a (vakuum) [6], vilket innebär att trycket mäts absolut. Utsignalen för trycksensorn är en skalning av dess matningsspänning och varierar mellan 10 % och 80 % av denna. 10 % svarar mot det minimala trycket (0 psi) som sensorn kan mäta och 80 % svarar mot det maximala trycket (100 psi 6, 8 atm) som sensorn kan mäta. Enligt tillverkaren mäts trycket med ett fel som maximalt är ±2 % av det faktiska trycket. A/D-omvandling av utsignalen görs av Arduino-kortet som har 10 bitars upplösning där standardinställningar är att GND svarar mot 0 och +5V svarar mot 1023, vilket innebär en upplösning på 4, 88mV. Om vattnet som omger ROV:n antas vara en ideal vätska kan det hydrostatiska trycket som funktion av vattendjup beräknas som P h = mg A = ρv g = ρgh, (27) A där A är den area som man tänker sig att kraften från vattenpelaren med höjd h appliceras på, V = Ah, g är tyngdaccelerationen på jordytan och ρ är densiteten för vattnet som kommer att bero på vattnets temperatur. För att erhålla det totala trycket i vattnet adderas lufttrycket P 0 till det hydrostatiska trycket P h P total = P 0 + P h. (28) Utifrån detta borde sensorn i teorin klara av att mäta trycket upp till ungefär 59 meters djup beroende på lufttrycket och vattnets temperatur. Enligt databladet [6] är utspänningen proportionellt mot trycket vilket ger en tryckmätning som funktion av djupet h k y p k = KP tot + c + e k = K(ρgh k + P 0 ) + c + e p k, ep k N(µ k, σ 2 k). (29) Trycksensorn är placerad på ROV:ns x-axel en sträcka l från origo, det vill säga i punkten X l X l Y Z = Y Z R ROV NED (30) Där NED ROV NED R ROV NED = R Z (ψ)r Y (θ)r X (φ), R X (φ) = 0 cos(φ) sin(φ), 0 sin(φ) cos(φ cos(θ) 0 sin(θ) R Y (θ) = 0 1 0, sin(θ) 0 cos(θ) (31a) (31b) (31c)

30 Underwater ROV 25 cos(φ) sin(φ) 0 R Z (ψ) = sin(φ) cos(φ) (31d) Roteringen ger alltså postionen för mätningen X l Y Z = NED ROV X + l cos(ψ) cos(θ) Y + l cos(θ) sin(ψ) Z l sin(θ). (32) NED Detta insatt i ekvationen för trycksensorn ger en mätning som funktion av ROV:ns absoluta Z-position y p k = K(ρg(Z k l sin(θ)) + P 0 ) + c + e p k, ep k N(µ k, σ 2 k), (33) där y p k är spänning och proportionalitetskonstanten K beskriver förhållandet mellan spänning och tryck. c är den offsetkonstant som krävs för att en linjär sensormodell ska kunna användas samtidigt som vakuum ska svara mot en nollskild spänning. Detta innebär alltså att c = 0, 1V cc, där V cc är matningsspänningen till sensorn. Empiriska tester måste göras för att se att denna modell stämmer åtminstone i det arbetsområde som ROV:n ska verka i. Med avseende på A/D-omvandlingens upplösning på 4, 88 mv kommer upplösningen för 4,88 mv trycket att bli K. Om K verkligen är konstant kan K beräknas genom att lösas ut ur förhållandet som råder vid en mätning av maximaltrycket: K = 0,8Vcc 0,1Vcc P a = 5, ,8 mv V/P a. Detta ger att upplösningen i tryck som är K = 961 P a, vilket ger en upplösning i djup på 961 P a gρ = 0, 098 m = 9, 8 cm. För att få bättre upplösning av mätningarna kan hänsyn tas till att ROV:n bara kommer att operera på ett begränsat djup, exempelvis mellan 0 m och 20 m. Med detta antagande kommer spänningen att variera mellan 1, 06 V och 2, 01 V. Om A/D-omvandlaren konfigureras för spänningar i detta intervall, det vill säga om referensspänningarna för den analoga ingången på Arduino-kortet, Vmin ref ref och Vmax ändras till 1, 06 V respektive 2, 01 V, medför detta en upplösning på 2,01 1, V = 0, 93 mv hos den A/D-omvandlade spänningen. Detta ger en upplösning av trycket på 183 P a och för djupet 0, 019 m = 1, 9 cm SONAR För framtida projekt är det nödvändigt att någon form av SONAR ska integreras på ROV:n för att möjliggöra navigering. De alternativ som diskuterats är att det placeras två stycken sidoskannande SONAR-sensorer på vardera sida om ROV:n, för att givet ett djup kunna mäta avståndet åt sidorna, samt en framåttittande SONAR för att mäta avstånd framåt. 8.4 Sensorfusionen i ROS Detta delsystem är implementerat i node sensor i ROS. Mer om interface mellan olika nodes i Kapitel Resultat och diskussion Eftersom SONAR-sensorerna aldrig tillhandahölls var det inte möjligt att implementera sensorfusionen för att skatta position och hastighet på ROV:n. Istället har simuleringar gjorts. De övriga sensorerna, IMU:n och trycksensorn, finns tillgängliga sedan innan

31 Underwater ROV 26 och det är känt vilka mätvärden de ger, och med hur stor noggrannhet. Därför kunde någorlunda relevanta simuleringar utföras. För positionering med SONAR fick dock vissa antaganden göras. Då det inte var känt vilka SONAR-sensorer som skulle användas var det inte heller känt vilka mätvärden som skulle finnas att tillgå för positionering. För att ändå kunna utföra någon typ av sensorfusion antogs mätningarna vara Time Of Arrival (TOA). ROV:ns position beror av mätdata från SONAR-sensorerna, men det ansågs för trivialt att anta positionsmätning då simuleringar skulle göras. Då skulle hela sensorfusionen för position enbart bestå av att filtrera bort brus från mätningar. Att anta råa ljudmätningar däremot antogs vara för invecklat, då det är möjligt att programvaran till SONAR-elektroniken utför någon typ av signalbehandling. Aktiva SONAR-sensorer kan fungera genom att sända ut ljudpulser som reflekteras. Ur detta är det möjligt att bestämma vilken tid pulsen återkommer, och därför antogs att mätnignarna är TOA. Simuleringarna gav tillfredställande resultat, se Figur 5. För att simulera mätningar gjordes först simuleringar med modellen då modellen påverkades av processbrus. Tillstånden från dessa simuleringar sågs då som de sanna. Från dessa simulerades mätningarna och mätbrus adderades. Bruset gavs väntevärde noll; sensorerna antogs alltså inte ha något konstant fel. Brusets varianser besämdes så att mätningarnas noggrannhet är de som står i databladen. För SONAR-sensorerna fanns ingen kännedom om deras noggrannhet, men bruset fick ha liknande egenskaper som för IMU:n och trycksensorn; väntevärde noll och varians som ansågs vara av rimlig storleksordning för att positionering ska vara möjlig. För att säkerställa att resultaten är användbara användes process- och mätbrus med egenskaper som varierades mellan olika körningar. Brusen var normalfördelade och okorrelerade, men hade slumpade offsets på standardavvikelserna. Trots detta fungerade sensorfusionen tillfredställande. Kraven på skattningarnas noggrannhet uppfylldes, vilket tyder på att sensorfusionen är väl genomförd. Det är dock osäkert hur väl det kommer att fungera i praktiken. Då det inte är känt vilka SONAR-sensorer som kommer att användas finns ingen kännedom om huruvida de antaganden som gjorts för deras noggrannhet är rimliga. Om dessa antaganden är rimliga, eller om sensorerna faktiskt är noggrannare än vad som antagits, bör det dock fungera väl.

32 Underwater ROV 27 (a) Position Y-led (b) Hastighet Y-led Figur 5: Simulering för skattning av position och hastighet i Y-led genom fusion av accelerations- och TOA-mätningar. Eftersom eulervinklar använts för att representera ROV:ns orientering, erhålls singulariteter vid vissa vinklar. Vid dessa vinklar erhålls ett fashopp, vilket gör det omöjligt att bestämma vinkeln. Detta är troligtvis inget problem i roll- och pitch-led, eftersom ROV:n under normal drift inte ska ha dessa vinklar. I yaw-led däremot är det ett påtagligt problem, då det inte är något otänkbart scenario att ROV:n roterar ett helt varv. Detta kan lösas genom att använda en bättre orienteringsrepresentation, exempelvis kvaternioner, vilka dessutom är mer beräkningseffektiva. Eulervinklar har dock använts i detta projekt för att få en enklare uttryckt rörelsemodell.

33 Underwater ROV 28 9 Kommunikation Detta delsystem sköter kommunikationen mellan de externa komponenterna i systemet samt de interna delsystemen. 9.1 Kommunikation med externa enheter Detta kapitel förklarar kommunikationen med externa enheter. För mer information om vad som skickas mellan dem, se Figur 2 och Appendix A Till moduler Delsystem Kommunikation skickar framtagna styrsignaler samt ROV:ns skattade tillstånd, position och orientering till den externa PC:n Från moduler Den externa PC:n skickar information till delsystem Kommunikation om ROV:ns läge, det vill säga autonom-, stabiliserad-, manuell- eller nöddrift. Delsystem Kommunikation har sedan som uppgift att distribuera denna information till de andra delsystemen. Delsystem Kommunikation ansvarar även för att samla in rå sensordata från IMU:n samt Arduino-kortet och sedan distribuera detta vidare till övriga nodes. 9.2 Kommunikation med interna delsystem Delsystem Kommunikation skickar och tar emot data från de olika delsystemen och den vidarebefordrar även viss data till den externa PC:n. Denna består av data från sensorsystemet, reglersystemet, batterinivåer, läckagesensor, motorstyrkor och status på reläerna. För mer information om vad som skickas mellan dem, se Figur 2 och Appendix A. 9.3 Delsystemet i ROS Kommunikationen är implementerad i node com i ROS. Mer om interface mellan olika nodes i Kapitel 3.

34 Underwater ROV Grafiskt användargränssnitt Här beskrivs det grafiska användargränssnitt (GUI) som har vidareutvecklats för den externa PC:n. GUI:t har en enkel utformning och visar ett antal värden, så som batterispänning, position och orientering. Det finns funktionalitet för att ändra referensvärden i stabiliserad- och autonom drift samt skicka kommandon till ROV:n. Via detta användargränssnitt kan användaren utföra följande: Växla mellan nödläge, manuell, stabiliserad eller autonom drift. Växla mellan LQ-regulator och MPC. Skicka kommandon till ROV:n om att hålla en viss orientering i stabiliserad drift. Nödstopp Utseende De numeriska värden i Figur 6 står för följande: 1. Go/Stop knappen startar och stoppar systemet. 2. Emergency knappen aktiverar nödläge vilket resulterar i att ROV:n avbryter den drift den är i och förflyttar sig horisontellt till ytan. 3. ROV:ns hastighet, relävärden, motorernas värden samt om lampan på ROV:n är tänd eller ej. Relävärderna kan vara 0 eller 255, där 0 representerar att reläerna är av och 255 representerar att reläerna är på. Motorernas värden kan gå mellan -100 och 100 där -100 representerar att motorerna kör full effekt bak, värdet 0 representerar att motorerna står stilla, och värdet 100 representerar full effekt fram. 4. ROV:ns systemspänning och batterispänningar i enheten volt, samt läckagedetektorns värde. 5. Send mode knappen skickar vidare den valda informationen som har valts i punkt 6-8 nedan. 6. Flervalsalternativ där användaren kan välja mellan manuell-, autonom-, stabiliseradeller nödläge. 7. Flervalsalternativ där användaren kan välja LQ regulator eller NMPC. 8. Checkbox där användaren kan välja att spara data till loggfiler. 9. Send ref-sig knappen skickar vidare de valda referensvärdena, i stabiliserad- och autonom drift, som valts i punkt 10 nedan. 10. Textfält där användaren kan skriva in referensvärden för stabiliserad- och autonom drift. Observera att dessa fält måste ha minst ett värde vardera och vara av typen heltal eller flyttal (där decimaldelen i flyttalet markeras med en. ). 11. ROV:ns position och orientering i enheten grader. 12. ROV:ns styrsignaler till motorerna som är enhetslösa värden. 13. ROV:ns beräknade referenssignaler i autonom drift i enheten grader för referensvinklarna och enheten meter för referenspositionerna.

35 Underwater ROV 30 Figur 6: En skärmdump över det grafiska användargränssnittet. Förklaring över vad varje siffra representerar finns förklarat i den numrerade listan i Kapitel Larm i form av en popup-textruta för följande fel eller problem kan visas, se Figur 7: Läckage. Låg batterinivå. Figur 7: En skärmdump över hur en varning kan se ut. Varningen i figuren kommer upp då läckage har upptäckts. Det finns även en varning för låg batterinivå.