Designspecifikation Redaktör: Version 1.0 Granskad Godkänd Status Sida 1
PROJEKTIDENTITET 2009/VT, Linköpings Tekniska Högskola, ISY Gruppdeltagare Namn Ansvar Telefon E-post Martin Larsson Projektledare (ML) 073-324 99 96 marla083@student.liu.se Dokumentansvarig (PM) 070-367 44 55 patmo414@student.liu.se Jimmy Engman Testansvarig (JE) 073-031 94 56 jimen245@student.liu.se Magnus Andersson Hårdvaruansvarig (MA) 073-995 46 50 magan452@student.liu.se Tomas Andersson Mjukvaruansvarig (TA) 073-631 17 14 toman012@student.liu.se Nils Ericsson Mobilitetsansvarig (NE) 070-679 18 47 niler034@student.liu.se Jenny Arkad Sensoransvarig (JA) 073-328 16 49 jenar374@student.liu.se Moa Gustafsson Nätverksansvarig (MG) 070-677 48 85 moagu291@student.liu.se Joel Askling Hermansson SLAM-ansvarig (JH) 073-808 03 36 joehe054@student.liu.se E-postlista för hela gruppen: Hemsida: Kommer senare Kund: Saab Bofors Dynamics Kontaktperson hos kund: Pelle Carlbom, 013-18 62 13, Pelle.Carlbom@dynamics.saab.se Kursansvarig: David Törnqvist (ISY), 013-28 18 82, tornqvist@isy.liu.se Handledare: Karl Granström (ISY), 013-28 28 03, karl@isy.liu.se Handledare: Torbjörn Crona (Saab Bofors Dynamics), 013-18 62 13 Handledare: Johan Bejeryd (Saab Bofors Dynamics), johan.bejeryd@saabgroup.com Sida 2
Innehåll Dokumenthistorik 5 1 Inledning 6 2 Översikt av systemet 7 3 Master 10 3.1 Användargränssnitt................................ 10 3.1.1 Anslutningsfönster............................ 10 3.1.2 Huvudfönster............................... 11 3.1.3 Banväljarfönster............................. 18 3.1.4 Hjälpfönster................................ 19 3.1.5 Statusfönster............................... 19 3.1.6 Beskrivning av klasser.......................... 21 3.2 Bildbehandling.................................. 22 3.2.1 Prediktering av landmärkenas positioner................. 23 3.2.2 Associering av landmärken........................ 23 3.2.3 Uppdatering med kalmanfilter...................... 23 3.2.4 Upptäcka och initiera nya landmärken.................. 24 3.2.5 Gränssnitt................................. 24 4 Scoutenheten 25 4.1 Hårdvara...................................... 26 4.1.1 Nödstopp på scouten........................... 26 4.1.2 Strömförsörjning............................. 26 4.1.3 Hur laptop och sensorer ska fästas på robot............... 26 4.2 Sensormodul................................... 27 4.2.1 Inledning................................. 27 4.2.2 GPS.................................... 27 4.2.3 IMU.................................... 28 4.2.4 Kamera.................................. 30 4.2.5 Odometer................................. 30 Sida 3
4.3 Mobilitetsmodulen................................ 31 4.3.1 Inledning................................. 31 4.3.2 Koordinatsystem............................. 31 4.3.3 Notationer................................ 32 4.3.4 Estimeringen............................... 33 4.3.5 Navigering................................ 35 4.3.6 Regulator................................. 36 4.3.7 Fordonsmodell.............................. 37 5 Nätverksmodulen 41 5.1 Masterenhet.................................... 41 5.1.1 Funktioner................................ 41 5.2 Scoutenheten................................... 42 5.2.1 Funktioner................................ 42 5.3 Schema över nätverkskommunikationen..................... 44 Referenser 45 Sida 4
Dokumenthistorik Version Datum Utförda förändringar Utförda av Granskad 0.1 2009-02-25 Första utkast Alla ML,NE 0.2 2009-03-04 Andra utkastet Alla ML 1.0 Godkänd Sida 5
1 Inledning Målet med projektet är att utveckla en terränggående robot vid namn Mobile Scout. Roboten kommer att styras trådlöst av en operatör via en laptop, en så kallad masterenhet. Systemet kommer att bestå av två delar, en scoutenhet och en masterenhet. Scoutenheten består av en robotplattform, en dator, hjulodometer, kamera, GPS och IMU. Roboten kommer att samla in information om dess omgivning och skicka till masterenheten. Operatören kommer ha möjlighet att styra roboten manuellt eller autonomt. Dokumentet beskriver mer i detalj hur produkten ska utvecklas. Först visas en översikt av systemet och därefter beskrivs varje delsystem mer ingående. Sida 6
2 Översikt av systemet Produkten kommer att byggas upp av två stora delsystem. En masterenhet och en scoutenhet. Masterenheten består av en laptop och hårdvara för trådlös kommunikation för att kunna styra scoutenenheten. Scoutenheten består av en robotplatform med en laptop och sensorer så som odometer, GPS, IMU och kamera. En skiss över systemet visas i figur 1 Systemet består av flera moduler för att göra det överskådligt och för att moduler lätt ska vara utbytbara. Nedan följer en lista av de olika delarna. Masterenhet Mjukvara: Användargränssnitt Nätverkmodul Bildbehandlingsmodul Hårdvara Scoutenhet Laptop Nätverk Mjukvara: Nätverksmodul Mobilitetsmodul Sensormodul Hårdvara Laptop Nätverk Robotplattform GPS IMU Kamera Mjukvaran på masterdatorn består av ett användargränssnitt, en nätverksmodul och en bildbehandlingsmodul. Användargränssnittet är ett grafiskt gränssnitt som gör det enkelt för användaren att styra roboten. Nätverksmodulen gör det möjligt att skicka information mellan masterenheten och scoutenheten. Modulen för bildbehandling tar emot sensordata från kameran på scoutenheten via nätverksmodulen, behandlar denna och sänder tillbaka till mobilitetsmodulen för beslut. Sida 7
Mobile Scout Hela Systemet Master Mjukvara Hårdvara Användarinterface Nätverksmodul Bildbehandling Scout Mjukvara Hårdvara Nätverksmodul Mobilitetsmodul Sensormodul Sensorer - Hjulodometer - GPS - IMU - Kamera Figur 1: Bilden visar en översikt över systemet L IPs Sida 8
Mjukvaran på scoutdatorn består av sensormodulen, mobilitetsmodulen och nätverksmodulen. Sensormodulen tar hand om indata från sensorerna och översätter dessa till värden som är lätta att tolka. Mobilitetsmodulen sköter reglering och navigering av roboten och nätverksmodulen tar hand om kommunikationen mellan scoutenheten och masterenheten. Mjukvaran kommer att utvecklas på ett sådant sätt att varje modul, det vill säga sensormodulen, nätverksmodulen, mobilitetsmodulen, grafiska gränssnittet och bildbehandlingen kommer ha sin egen exekveringstråd (s.k threads) och där varje modul motsvara en egen klass. Detta gör att till exempel sensormodulen kan samla in sensordata utan att någon annan modul har begärt datat. Detta är en fördel om man till exempel vill medelvärdesbilda över flera värden från sensorerna. Överföring av data mellan modulerna kommer att ske genom en begäran från den modul som vill ha data, t.ex genom en funktion GetIMUData() på sensormodulen. Resterande delen av dokumentet utgör en mer detaljerad beskrivning av varje del. Först kommer masterenhetens delar och sedan scoutenheten. Observera att näverksmodulen tillhör både masterenheten och scoutenheten och därför beskrivs den i en egen sektion. Sida 9
3 Master Masterdatorn består av följande delsystem Masterenhet Mjukvara: Användargränssnitt Nätverksmodul Bildbehandling Hårdvara Laptop Nätverk Mjukvaran på masterdatorn består av ett användargränssnitt, nätverksmodul och en bildbehandlingsmodul. Användargränssnittet är ett grafiskt gränssnitt som ska göra det enkelt för användaren att styra roboten. Nätverksmodulen gör det möjligt att skicka information mellan masterenheten och scoutenheten. Bildbehandlingsmodulen tar emot sensordata från kameran på scoutenheten via nätverksmodulen, behandlar denna och sänder tillbaka till mobilitetsmodulen för beslut. 3.1 Användargränssnitt Ett grafiskt användargränssnitt (GUI) kommer att implementeras i masterenheten. Användargränssnittet fungerar som en länk mellan roboten och operatören där användaren ansluter till scoutenheten via nätverksmodulen. Programmet är uppdelat i ett interface för anslutning, manuell körning och autonom körning och kommer att utvecklas med Qt som är ett open-source bibliotek utvecklat till Windows-, Mac OS X- och Unixsystem. Vid start av programmet på masterenheten möts användaren först av ett fönster för anslutning till scoutenheten. Vid anslutning har användaren möjlighet att styra scoutenheten via ett manuellteller autonomt gränssnitt. Nedan följer en mer detaljerad beskrivning av de olika gränssnitten och under punkt 3.1.6 följer en lista som beskriver alla Qt-klasser som behövs för att realisera programmet. 3.1.1 Anslutningsfönster I fönstret för nätverksanslutning visas ett fält där användaren anger en ip-adress som denne vill ansluta till, en statusrad där vi kan se om masterdatorn är ansluten eller ej, en knapp för connect, disconnect och close. Figur 2 visar en skiss över hur det grafiska gränssnittet för nätverksanslutning kommer att se ut. Sida 10
Connect to IP address: 91:423:123:01 Status: Disconnected Connect Disconnect Close Figur 2: Visar gränssnitt för nätverksanslutningen QT-klasser som behövs för att realisera: QDialog QLabel QLineEdit QPushButton QObject 3.1.2 Huvudfönster Huvudfönstret visas för användaren så fort kontakt upprättats med en scoutenhet. Vilken information och vilka kommandon som användaren har tillgång till i huvudfönstret bestäms av vilket mod som huvudfönstret ställts in i, autonomt eller manuellt läge. Denna uppdelning är till för att göra användargränssnittet enklare att använda. Generellt för huvudfönster Gemensamt för de båda moderna för huvudfönstret är möjligheten att byta till annat mod, informationsvisningen av sensordata från scouten, status för nätverket samt en menyrad överst i huvudfönstret, och dessa delar beskrivs nedan. Modbyte: Modbyten kommer att ske genom att användaren klickar i en väljare för den andra moden. När detta görs markeras det nya modet för väljaren, och vidare kommer huvudfönstrets innehåll bytas ut för att tillåta användaren att ge kommandon för den nya moden. QT-klasser som behövs för att realisera: QLabel Sida 11
QRadioButton QObject Sensordata: Användaren kommer att ha möjlighet att analysera sensordata från en odometer, GPS, IMU. Ett fält för vardera sensor kommer att visa dess data. Data från varje sensor kommer även att loggas i en fil för analys efter genomförd körning. Uppdatering av sensorvärden kommer att ske genom en begäran från användargränssnittet till nätverksmodulen om ny sensordata. Data från sensorerna kommer tas emot som objekt enligt tabell 1. En knapp för att tömma fälten med sensordata kommer att finns tillgänglig för användaren. Objektnamn Data OdometerData double hw_dist; sträcka körd av höger hjul [m] double lw_dist; sträcka körd av vänster hjul [m] double t; tidstämpel [ms] GPSData double x; position i x-led i referenssystemet [m] double y; position i y-led i referenssystemet [m] double z; position i z-led i referenssystemet [m] double t; tidstämpel [ms] IMUData double ax; acc. i x i1 -led [m/s 2 ] doubel ay; acc. i x i2 -led [m/s 2 ] doubel az; acc. i x i3 -led [m/s 2 ] double vx; vinkelhast. i x i1 -led [rad/s] doubel vy; vinkelhast. i x i2 -led [rad/s] doubel vz; vinkelhast. i x i3 -led [rad/s] double mx; magnetfält i x i1 -led [T ] doubel my; magnetfält i x i2 -led [T ] doubel mz; magnetfält i x i3 -led [T ] double t; tidstämpel [ms] Tabell 1: Visar objekt för sensordata QT-klasser som behövs för att realisera: QLabel QTextBrowser QPushButton QObject QTimer Sida 12
Menyrad: Överst i huvudfönstret kommer det att finnas menyer som ger möjlighet att koppla ifrån scoutenheten och att koppla upp på nytt. Vid alternativet att koppla ifrån bryter masterenheten anslutningen till scoutenheten och vid alternativet koppla upp på nytt, anslutningsfönstret att visas för användaren. Det kommer även att finnas ett alternativ i menyraden att starta hjälpfönstret som kommer att beskrivas nedan. QT-klass som behövs för att realisera: QMeny QObject Network status: Visningsfält som visar information om nätverkskopplingen mellan scout och masterenheten. QT-klasser som behövs för att realisera: QTextEdit QLabel QPushButton Manuell styrning I manuellt läge kommer användaren ha möjlighet styra scoutenheten med piltangenterna samt analysera sensordata. Nedan följer en detaljerad beskrivning för hur det grafiska användargränssnittet i manuell styrning kommer se ut (figur 3) och implementeras. Sida 13
File Help Mode: Manual Automatic Network status: Disconnected Sensor data Odometer - LW GPS - X IMU - X Time Odometer - RW GPS - Y IMU - Y Clear Speed Figur 3: Visar gränssnittet för manuell styrning Väljare för variabel hastighet: Vid manuell styrning kommer användaren kunna variera scoutens hastighet. Hastigheten väljs via en slider där hastigheten definieras i m/s. Denna hastighet kommer att finnas tillgänglig för det manuella kommandon som skickas till scoutenheten. QT-klasser som behövs för att realisera: QSlider QObject QLabel Styrknappar: För att styra roboten i manuellt läge kommer operatören använda piltangenterna på masterdatorn. Användargränsnittet kommer att ta emot kommandon för framåt, bakåt, sväng höger och sväng vänster. Gränssnittet kommer att registrera när operatören trycker ner en piltangent på tangentbordet. Vid knapptryckning skickas ett Manual - objekt till scoutenheten, innehållande en hastighet (v) och en rotationshastighet (fi_prick), se tabell 2. När operatören släpper piltangenten kommer ett Manual - objekt att skickas med hastighet och rotationshastighet noll och roboten stannar. QT-klasser som behövs för att realisera: QObject QKeyEvent Sida 14
Objektnamn Data Manual double v [m/s] double fi_prick [rad/s] Tabell 2: Visar objektnamnet och dess information som skickas vid manuell körning QKeySequence Autonom styrning I autonomt läge kommer scoutenheten att manövrera efter en lista med brytpunkter, och huvudfönstrets autonoma mod kommer att innehålla kommandon som möjliggör för användaren att hantera denna brytpunktslista, samt en karta för att visa hur scoutenheten kör efter banan. File Help Mode: Manual Automatic Show track Show scout Show both Start Stop Reset Network status: Odometer - LW GPS - X Connected Sensor data Odometer - RW GPS- Y Show status Breakpoints IMU - X IMU - Y Load track Upload track 5,4 Add point Time Remove point Clear Clear Figur 4: Visar gränssnittet för automatisk styrning Start/Stopp/Reset knappar: När användaren trycker på en av dessa knappar kommer ett Automaticobjekt (se tabell 3) med respektive kommando att skickas till scouten via nätverksmodulen. Stop innebär att scoutenheten ska stanna sin körning, men att den ska kunna återupptas med start kommandot. Reset innebär att scoutenhetens brytpunktslista ska raderas. Reset kommer även medföra att den visade kartan nollställs. QT-klasser som behövs för att realisera: QPushButton Sida 15
QObjekt Show status knapp: När användaren trycker på denna knapp öppnas statusfönstret, vilket visar information om scoutens körning. QT-klasser som behövs för att realisera: QPushButton QObjekt Load track knapp: När användaren trycker på denna knapp öppnas Banväljarfönstret, vilket tillåter användaren att ladda in förgenererade brytpunkter. QT-klasser som behövs för att realisera: QPushButton QObjekt Upload track knapp: När användaren trycker på denna knapp kommer listan med brytpunkter som finns i visningsfönstret att skickas till scoutmodulen som ett Automatic-objekt (se tabell: 3) från nätverksmodulen till scoutenheten. Om scouten redan har en inmatad brytpunktslista kommer denna att ersättas av den senast skickade listan. Objektnamn Data Automatic VectList path; bool pause; pause = true, då roboten ska stå stilla bool reset; reset = true, då alla brytpunkter ska raderas VectList List<Vect> vects; Vect double x1; [m] double x2; [m] double x3; [m] Tabell 3: Visar objektnamnet och dess information som skickas vid autonom körning QT-klasser som behövs för att realisera: QPushButton QObjekt Clear knapp: När användaren trycker på denna knapp rensas den i användargränssnittet sparade brytpunktslistan. Detta rensar inte scoutenhetens interna lista, men möjliggör för nyinmatning av brytpunkter från användaren. QT-klasser som behövs för att realisera: Sida 16
QPushButton QObjekt Visningsfönster för brytpunkter: Här visas de i användargränssnittet lagrade brytpunkterna i form av en lista med x- och y-koordinater. Fönstret kommer att tillåta att användaren markerar en brytpunkt för att denna ska kunna tas bort med hjälp av Remove point knappen. Fönstret kommer att uppdateras så fort som någon ändring sker på användargränssnittets lagrade brytpunktslista. QT-klasser som behövs för att realisera: QTextBrowser Inmatningsfönster för brytpunkter: I detta inmatningsfönster kan användaren skriva in koordinater för nya brytpunkter. Dessa ska vara på formen: x koord, y koord och använda punkt för att ange decimaler. QT-klasser som behövs för att realisera: QLineEdit QObjekt Add point knapp: När användaren trycker på denna knapp matas den i inmatningsfönstret skrivna brytpunkten in till användargränssnittets brytpunktslista, och läggs till sist i denna lista. Felaktiga brytpunkter matas inte in. Denna knapp lägger inte till brytpunkter i scoutenhetens interna lista utan bara i användargränssnittets lista. QT-klasser som behövs för att realisera: QPushButton QObjekt Remove point knapp: När användaren trycker på denna knapp och har en brytpunkt markerad i visningsfönstret för brytpunkter kommer den markerade brytpunkten att tas bort från listan. Denna knapp raderar inte brytpunkter i scoutenhetens interna lista utan bara i användargränssnittets lista. QT-klasser som behövs för att realisera: QPushButton QObjekt Sida 17
Kartmodsväljare: Tillåter användaren att välja vilken information som ska visas i startfältet. När användaren klickar i ett val av ett annat visningsmod uppdateras kartan med informationen angående detta mod. Vid Show track mod visas bara den av brytpunkter anvisade körbanan för scoutenheten. Vid Show scout mod visas bara hur scoutenheten har kört fram tills tidpunkten och i Show both mode visas båda dessa kurvorna i olika färger. QT-klasser som behövs för att realisera: QObject QRadioButton QLabel Karta: Kartan kommer att kunna visa en kurva över hur scoutenheten har rört sig under en körning, samt hur den planerade körningen ser ut. Vilken information som visas bestäms av användaren via kartmodsväljaren. Kartan kommer att uppdateras var gång användargränssnittet får ny sensordata. Vid kartans kanter kommer det anges längdskalor och fönstret kommer automatiskt att skalas om då enhetens körning hamnar utanför banan. QT-klasser som behövs för att realisera: QPainter 3.1.3 Banväljarfönster Banväljarfönstret (figur 5) kommer låta användaren välja mellan flertalet förgenererade banor, och fönstret startas ifrån huvudfönstret med hjälp av knappen Load track. Fönstret kommer att visa bilder över de banalternativ som finns förgenererat för användaren. När användaren valt bana och klickar på add kommer banväljarfönstret att stängas och den valda banan att matas in till användargränssnittets lista med brytpunkter. Choose Track Add Close QT-klasser som behövs för att realisera: Figur 5: Banväljarfönstret Sida 18
QDialog QTextBrowser QPushButton QObjekt QRadioButton QLabel QTimer 3.1.4 Hjälpfönster Kommer bestå av en textlista med beskrivning av hur man använder sig av användargränssnittet. QT-klasser som behövs för att realisera: QDialog QTextBrowser QPushButton QObjekt QLabel 3.1.5 Statusfönster Statusfönstret (se figur 6) kommer att innehålla ytterligare information om scouten, främst tänkt att användas vid felsökning. Fönstret kommer att visa statusinformation för användaren och uppdateras var gång en begäran om statusdata skickas till nätverksmodulen vilket kommer att hanteras med en timer. Statusdatan kommer att skickas som ett Status-objekt enligt tabell 4. Alla statusdata kommer även att loggas i en fil på datorn för senare analys. Close knapp När användaren trycker på denna knapp kommer statusfönstret att stängas. Sida 19
Scout - status Position Angle Current breackpoint Speed Angle acceleration Close Figur 6: Scoutstatusfönster Objektnamn Data Status Vect position; Position relativt start [m] Vect velocity; Hastighet [m/s] Vect angle; Vinkel relativt start [grader] Vect angularvelocity Vinkelhastighet relativt start [rad/s] Vect breakpoint; Aktuell brytpunkt double t; Tidstämpel [ms] Vect double x1; double x2; double x3; Tabell 4: Visar objektnamnet för statusinformationen QT-klasser som behövs för att realisera: QDialog QTextBrowser QPushButton QObjekt QLabel QTimer Sida 20
3.1.6 Beskrivning av klasser Nedan följer en lista och beskrivning över alla Qt-klasser som kommer att användas i utvecklingen av användagränssnittet. Qt-klass QApplication QWidget QGridLayout QDialog QMeny QLineEdit QTextBrowser QLabel QPushButton QRadioButten QSlider QPainter QKeyEvent QKeySequence QTimer Beskrivning Skapar main-loopen för det grafiska gränssnittet Skapar huvudfönstret i programmet Möjliggör placering av fält enligt ett rutnät Skapar ett fönster Skapar en menyrad Skapar en texteditor på rad Skapar ett fält för att visa text Skapar text Skaper klickbara knappar Skapar en radiobutton för val av manuellt eller autonomt läge Skapar en slider Ritar ut enkla symboler. T.ex för att rita ut en karta över hur roboten åker. Möjliggör registrering av signal när knapp trycks ner och släpps Möjliggör att ta emot kommandon när två eller fler tangenter trycks ner Timer för att kunna göra vissa uppgifter med jämna mellanrum, t.ex för att kunna hämta sensordata. Tabell 5: Beskiver alla Qt-klasser som kommer att användas i utvecklingen. Sida 21
3.2 Bildbehandling Bildbehandlingen ska utifrån de bilder som kommer från kameran estimera scoutens position. Detta görs genom att hitta punkter, även kallat landmärken, i en bild som kan återupptäckas i kommande bilder. Genom att utnyttja hur dessa landmärken förflyttar sig i bilden kan bildbehandlingen vara en del i estimeringen av scouts position. Figur 7: Flödesschema för bildbehandlingen I figur 7 visas ett flödesschema över bildbehandlingen. Här följer en kort förklaring av varje moment. Scouts position samt landmärkenas positioner används för att prediktera vart i bilden som gamla landmärken befinner sig. Landmärken hittas i den nya bilden. Avvikelsen mellan uppmätt och predikterad position av landmärkena beräknas. Avvikelsen samt en modell för mätningen används för att uppdatera tillståndet av scout Nya landmärken upptäcks och initieras Sida 22
3.2.1 Prediktering av landmärkenas positioner Utifrån scouts position samt landmärkenas positioner beräknas var i den nya bilden som landmärkena borde befinna sig. Först måste landmärkenas positioner, som är givna i det fixa koordinatsystmet, beräknas om till en position i kamerans koordinatsystem. Eftersom kamerasystemet är fixt i scoutsystemet beräknas först landmärkenas positioner i scoutsystemet och sedan i kamerasystemet. För beskrivning av koordinatsystem se 4.3.2. där l s = l f = l x1 l x2 l x3 l sx1 l sx2 l sx3 l s = cos(θ) sin(θ) 0 sin(θ) cos(θ) 0 0 0 1 (l f T s ) är landmärkets position i scoutens interna koordinat system är landmärkets position i det fixa koordinatsystemet. På samma sätt fås landmärkets position i kamerans koordinat system l c som l c = R cs (l s T cs ) Där R cs och T cs beror på vart kameran monteras på scouten. Nu kan l c räknas om till pixelkoordinater beroende på vilken kamera som används 3.2.2 Associering av landmärken För initiering och associering av landmärken används SIFT. 3.2.3 Uppdatering med kalmanfilter Uppdateringen av tillståndsvektorn och kovariansmatrisen görs med ett kalmanfilter. Låt oss kalla den funktion som predikterar landmärkenas positioner i bilden för h(x). h(x) använder alltså scouts position och landmärkenas positioner i tillstånds verktorn x för att beräkna landmärkenas positioner i bilden som beskrevs i avsnitt 3.2.1. Först beräknas jacobianen h x = h x och uppdateringen blir sedan: S = h x P h T x + R K = h x P S 1 x new = x + Ke Sida 23
P new = P P h T x K Där R är kovariansen för mätfelet, e är skillnaden mellan uppmätt och predikterat mätvärde, x tillståndsvektorn och P tillståndsvektorns kovariansmatris. 3.2.4 Upptäcka och initiera nya landmärken För att initiera nya landmärken måste först bra landmärken hittas och sedan läggas till i tillståndsvektorn x och kovariansmatrisen P. Hur detta görs måste studeras mer noggrant innan det designas. 3.2.5 Gränssnitt Indata Indata till bildbehandlingen är ett ImageProcessingData-objekt. Objektet innehåller en bild, tillståndsvektor och kovariansmatris samt en tidsstämpel. Tillståndsvektorn innehåller scouts position och samtliga landmärkens positioner. Tidsstämpeln är tidpunkten då bilden togs och scout hade tillståndet i tillståndsvektorn. Utdata Utdata från bildbehandlingen är ett ImageMeasurment-objekt som består av en uppdaterad tillståndsvektor och kovariansmatris samt samma tidsstämpel som indata hade. Sida 24
4 Scoutenheten Scoutenheten består av följande delsystem Mjukvara: Nätverksmodul Mobilitetsmodul Sensormodul Hårdvara Laptop Nätverk Robotplattform GPS IMU Kamera Figur 8: Systemskiss över scoutenheten Mjukvaran på scoutdatorn består av sensormodulen, mobilitetsmodulen och nätverksmodulen. Sensormodulen tar hand om indata från sensorerna och översätter dessa till värden som är lätta att tolka. Mobilitetsmodulen sköter reglering och navigering av roboten och nätverksmodulen tar hand om kommunikationen mellan scoutenheten och masterenheten. Sida 25
4.1 Hårdvara 4.1.1 Nödstopp på scouten Det finns en inbyggd switch som stryper strömtillförseln till motorerna på roboten. Den ska tills vidare vara vår nödstoppsknapp. 4.1.2 Strömförsörjning Batterierna till robotens motor kan utan problem få plats inuti chassit. Batterierna seriekopplas och räcker för att köra roboten i cirka 2 timmar. Laptopen och alla sensorer strömförsörjs från laptopens batteri. Laptopens batterikapacitet är okänd men bör räcka i minst två timmar om skärmen är avstängd. Batteri Kapacitet Spänning Datorbatteri 3.4Ah 10.4V Motorbatteri1 3.6Ah 12V Motorbatteri2 3.6Ah 12V Tabell 6: Tillgängliga batterier. Sensor Strömförbrukning Spänning Laptop - 10.4VDC IMU 65mA 5.2-12VDC GPS - 5VDC USB Kamera <25mA 5VDC USB Odometer 40mA 5VDC USB Tabell 7: Sensorernas strömförbrukning. 4.1.3 Hur laptop och sensorer ska fästas på robot Laptopen ska sättas fast på roboten via kardborrband eftersom vi ofta kommer att lyfta av laptopen från roboten när vi ska programmera. Kardborrbandet fästs fast på roboten och laptopens undersida via dubbelhäftande tejp. Odometer, IMU och GPS kommer att fästas med enbart dubbelhäftande tejp. Sida 26
4.2 Sensormodul 4.2.1 Inledning Fyra sensorer finns kopplade till scouten, en GPS, en IMU, en kamera och en odometer. Sensormodulen är ett gränssnitt mellan sensorer och de moduler som använder sig av sensorvärden. Modulen hämtar sensorvärden från sensorerna, sparar dessa och tillhandahåller mätvärden för nätverksmodulen och mobilitetsmodulen. I sensormodulen kommer det att finnas modeller av sensorerna för att alla moduler ska kunna testas utan att hårdvaran finns tillgänglig. Indata till sensormodulen från sensorerna: Acceleration och vinkelhastighet från IMU:n GPS-koordinater från GPS:en Bilder från kameran Hjulhastigheter från hjulodometern Utdata till estimeringen och nätverksmodulen är positionen från GPS:en, acceleration och vinkelhastighet från IMU samt sträcka från odometern. Utdata från sensormdulen till nätverksmodulen är även kamerabilder. För att implementera detta kommer objekt för varje sensor att skapas, ett kameraobjekt, ett GPS-objekt, ett IMU-objekt och ett odometerobjekt. Dessutom kommer varje objekt att tidstaggas vid det tillfälle då datat har blivit insamlat. Sensor GPS IMU Kamera Odometer Märke Vector från Hemisphere 3DM-GX1 från Microstrain Firefly från Point Grey Enkoder från robotmarketplace och mikrokontroller från Atmel Tabell 8: Sensorerna 4.2.2 GPS I detta projekt kommer en GPS-mottagare av märket Hemisphere Vector att användas. Den kommer att kommunicera med datorn via serieport direkt via RS-232 och blir strömförsörjd via en USB-port. Kommunikationen sker med standardiserade NMEA-meddelande i ascii-format. Det kommer attt räcka med att använda NMEA:S GGA-meddelande. Denna typ av meddelande tillhandahåller bland annat information som position i longitud och latitud. Informationen Sida 27
kommer att sparas som ett speciellt GPS-objekt som andra moduler kommer ha tillgång till. Den information som sparas i objektet kommer vara en sträcka i nord-sydlig riktning och en sträcka i öst-västlig riktning som är beräknad utifrån startpositionen och den postition där roboten befinner sig nu. Även höjden som roboten befinner sig på, beräknad utifrån startpositionen, kommer att sparas i objektet. GGA-meddelanden ser ut på följande sätt: $GPGGA,hhmmss.ss,ddmm.mmmm,s,dddmm.mmmm,s,n,qq,pp.p,saaa.aa,M,±xxxx.xx,M,sss,aaaa*cc<CR>< Fält hhmmss.ss ddmm.mmmm s dddmm.mmmm s n qq pp.p saaa.aa M ±xxxx.xx M sss aaa *cc <CR><LF> Beskrivning UTC-tid givet i timmar, minuter och sekunder Latitud i grader, minuter och decimalminuter s=n eller s=s för nord eller syd i latitudled Longitud i grader, minuter och decimalminuter s=e eller s=w för öst eller väst i longitudellt led Kvalitetsindikator, 0 = ingen position beräknad, 1 = position beräknad i normalläge, 2 = position beräknad i differentialläge, 9= position beräknad mha uppslagsvärden Antal använda satelliter vid positionsberäkning HDOP =0.0 to 9.9, horisontell precisionsutspädning Antenn-altitud Altitudenhet, M = meter Geoidal separation Enhet för geoidal separation, M = meter Ålder hos differentialdata i sekunder Differential referensstationsidentitet Checksum, för att kontrollera data Carriage return och line feed Tabell 9: GGA-meddelande 4.2.3 IMU Denna har tre olika sensorer, accelerometer, gyroskop och magnetometer. Accelerometern mäter acceleration, gyroskopet mäter vinkelhastighet och magnetometern mäter magnetfält. Sen- Sida 28
GPSData double x; double y; double z; double t; position i x-led i referenssystemet position i y-led i referenssystemet position i z-led i referenssystemet tidstämpel Tabell 10: GPS-objektet med dess datamedlemmar sorerna är triaxiella vilket betyder att de mäter i tre ortogonala riktningar. För varje läsning får vi alltså nio mätvärden. Sensorerna kan läsas upp till 350 Hz och är redan kalibrerade. Sensormodulen ska med en konstant periodicitet (storleksordning 0.05 s) läsa av IMU:ns tre sensorer och spara dessa. När sensormodulen sedan ska leverera data så medelvärdesbildar den de senaste tio mätvärdena. Medelvärdesbildningen är nödvändig eftersom de tre sensorerna är mycket brusiga. Figur 9: 3DMGX1 IMUData double ax; doubel ay; doubel az; double vx; doubel vy; doubel vz; double mx; doubel my; doubel mz; double t; acc. i x i1 -led acc. i x i2 -led acc. i x i3 -led vinkelhast. i x i1 -led vinkelhast. i x i2 -led vinkelhast. i x i3 -led magnetfält i x i1 -led magnetfält i x i2 -led magnetfält i x i3 -led tidstämpel Tabell 11: IMU-objektet med dess datamedlemmar. För beskrivning av IMU-koordinatsystemet x i, se avsnitt 4.3.2. Sida 29
4.2.4 Kamera Bilderna från kameran har en upplösning på 640x480 pixlar. Kameran kopplas in via Firewire och sensormodulen får kamerabilderna via denna. Kameran är inte enbart en kamera, den innehåller också en IMU. Vill man utnyttja IMU-data från kameran måste man använda sig av USB, till skillnad från kamerabilderna som fås via firewire. IMU-datat består av accelerationer och vinkelhastigheter i de tre dimensionerna. 4.2.5 Odometer Odometern är en sensor som mäter hur mycket vänster och höger hjul har roterat. På varje hjul finns det 500 punkter som sensorn känner av. När hjulet har roterat en punkt så skickas en 3- kanals TTL signal. Robotmarketplace har kontaktats för att få större grepp hur enkoderna ser ut. Eftersom varje hjul har tre kanaler så behövs det sex kopplingar från enkodern. Figur 10: Mikrokontroller AT91SAM7S64 till vänster och mitten samt dess programmerare till höger. Vänstra bilden av mikrokontrollern visar kontakt för USB och JTAG. Den andra bilden visar kontakter för inkoppling av enkoder-signaler, den övre kontakten är EXT1. Programmeraren heter ARM-USB-TINY och är ifrån Olimex. Vi ansluter en mikrokontroller som tar hand om dessa sex signaler och hanterar de som avbrott samt räknar lite på hur långt hjulet har roterat. Det kan bli upp till 2 3 500 antalvarv st avbrott i sekunden. En real-time timer (RTT) håller koll på tiden mellan avbrotten. Mikrokontrollern kopplas via USB till datorn och resultatet av mikrokontrollerns beräkningar skickas seriellt RS2-232. OdometerData double hw_dist; double lw_dist; double t; sträcka körd av höger hjul [m] sträcka körd av vänster hjul [m] tidstämpel [ms] Tabell 12: Odometer-objektet med dess datamedlemmar Sida 30
Figur 11: Beskriver var signalerna från enkodrarna ska kopplas in till mikrokontrollern. Alla IO pinnar på mikrokontrollern är upp till 5.5 volts kompatibla och kan programmeras så att det tolkas som ett avbrott om insignalen går från hög till låg eller tvärtom. 4.3 Mobilitetsmodulen 4.3.1 Inledning Mobilitetsmodulen består av tre delar. Estimeringen som utifrån sensordata skattar tillstånden för scouten, Navigatorn som navigerar utifrån de kommandon som mastern skickar samt Regulatorn som som styr roboten utifrån önskemålen från Navigatorn. 4.3.2 Koordinatsystem Det finns 5 olika koordinatsystem att hålla reda på. Samtliga är högerorienterade med den tredje koordinataxeln rakt upp. Koordinatsystemen definieras enligt nedan: Fixt koordinatsystem (x 1, x 2, x 3 ) utgår ifrån robotens initalläge. x 1 sätts i den riktning som är rakt fram för roboten då den startar. x 2 pekar 90 åt vänster och x 3 rakt upp ur roboten. Origo i detta system anses vara robotens centrum i initialäget. Detta kommer att refereras till som det fixa koordinatsystemet. GPS-koordinatsystem (x g1, x g2, x g3 ) vars origo sammanfaller med det fixa koordinatsystemet. x g1 -axel är i östlig riktning och x g2 -axel i nordlig riktning. Observera att dess orientering inte är känd vid start utan måste skattas efter hand som scout rör sig. Hädanefter refereras detta system till som det globala koordinatsystemet. Internt koordinatsystem (x s1, x s2, x s3 ) som är fäst vid scouten. Den första vektorn pekar alltid i robotens körriktning. Vid initialläget är detta koordinatsystem helt överlappande med det fixa. Detta koordinatsystem kommer att refereras till som det interna koordinatsystemet. IMU-koordinatsystem (x i1, x i2, x i3 ) som är fixt i det interna koordinatsystemet med origo i IMU:n. Hädanefter refererat till som IMU-koordinatsystemet. Sida 31
Kamerans koordinatsystemet (x c1, x c2, x c3 ) som är fixt i det interna koordinatsystemet med origo i kamerans optiska centrum och x 3 -axeln längs kamerans optiska axel. Hädanefter refereras detta system till som kamerakoordinatsystemet. x c2 Kamerasystemet x c1 x c3 x i2 IMU-systemet x i1 x s2 T gs x s1 x 2 x g2 T s Interna systemet x s3 T gs x i3 x 3xg3 ψ x Fixa systemet 1 x g1 Globala systemet Figur 12: Illustration av de olika koordinatsystemen 4.3.3 Notationer e r Målposition Här följer en beskrivning av x x s1 2 de notationer som användas: (x g1, x g2, x g3 ) - koordinater i det globala koordinatsystemet (x 1, x 2, x 3 ) - koordinater i det Tfixa koordinatsystemet s θ (x s1, x s2, x s3 ) - koordinater i det interna koordinatsystemet (x i1, x i2, x i3 ) - koordinater i IMU-koordinatsystemet x (x 3 c1, x c2, x c3 ) - koordinater i kameransxkoordinatsystemet 1 e φ x = (x 1, x 2, x 3 ) - anger en vektor av koordinater i ett koordinatsystem x = ( dx 1, dx 2, ) dx 3 dt dt dt - anger en hastighetsvektor för ett koordinatssytem ˆ x[k + l k] - skattning av tillstånd vid tiden k + l då tillstånden vid tiden k är kända ψ - minsta vinkeln (högerorienterad) från x g1 till x 1, på ( π, π], se figur 12 θ - minsta vinkeln (högerorienterad) från x 1 till x s1, på ( π, π]. se figur 14 α - minsta vinkeln (högerorienterad) från x 2 till x s2, på ( π, π] β - minsta vinkeln (högerorienterad) från x 3 till x s3, på ( π, π] Sida 32
4.3.4 Estimeringen Estimeringen får in en mängd olika mätdata från sensormodulen (se tabell 13). Dessa vägs samman för att så bra som möjligt kunna avgöra position, hastighet, vinkel (gentemot det fixa koordinatsystemet) och vinkelhastighet. Dessa fyra värden kommer nedan refereras till som robotens tillstånd. En viss mängd av den data som erhålls av sensormodulen måste förbehandlas, detta rör sig framför allt om odometerdata som ska omvandlas från sträckan på vardera bandpar till position, och rotation. Estimeringens två huvuddelar ses i figur 13. Figur 13: De två huvuddelarna i estimeringen. Det är främst odometerdata som förbehandlas. Sensordata De olika typer av data som skickas till estimeringen ges av tabell 13. Notera specialfallet med kamerabilder. Dessa skickas inte direkt från sensormodulen till estimeringen utan behandles först på masterenheten för att ta beräkningskraft från scouteneheten. En viss del av denna data behöver förbehandlas. Körd sträcka Position Acceleration Tidstämpel (r track, l track ) Odometer (x g1, x g2, x g3 ) GP S x IMU (hh.mm.ss.ms) (x c1, x c2, x c3 ) Kamera θimu Tabell 13: Datatyper från olika sensorer som Estimeraren erhåller. * Positionsdata från kameran är beräknad på Masterenheten (se vidare avsnitt 3.2) Förbehandling av data Den data som estimeringen får in av odometern består av körd sträcka på respektive bandpar. Först tas det föregående värdet bort för att få ändringen. (r wheel, l wheel ) ( r wheel, l wheel ) Detta ska nu användas för att beräkna en postionsändring (i det fixa koordinatsystemet) samt en svängd vinkel θ. Vinkeln räknas ut enligt ekvation (1). θ = r wheel l wheel 2d (1) Sida 33
Parametern d svara mot avståndet från robotens centrum ut till ett av bandens mittpunkt. Tecknet på θ är beroende på vilket av odometerparametrarna som har högst värde. Detta helt i analogi med hur θ definierades i avsnitt 4.3.3. Den sträcka som roboten färdats sedan förra mätning kan representeras av en cirkelbåge som är medelvärdet av körd sträcka på höger och vänster bandpar. Den tänkta cirkelrand som roboten då färdas längs har radien R, som fås enligt ekvation (2). R = r wheel + l wheel 2 θ (2) Denna radie kommer skifta tecken beroende på i vilken riktning roboten färdas. För att räkna om detta till kartesiska koordinater beräknas först positionsändringen utifrån robotens interna koordinatsystem och räknas sedan om till det fixa koordinatsystemet med parametern θ. { xs1 = R sin(sgn( r wheel l wheel ) θ) x s2 = R sgn( r wheel l wheel ) (1 cos( θ)) Här kan det först tyckas att x S1 0 då θ 0, men eftersom R är en funktion av θ fås gränsvärdet R( θ) sin( θ) = r wheel + l wheel 2 sin θ θ r wheel + l wheel, θ 0 2 vilket blir just den längd robotens centrum färdats. Koordinaterna räknas sedan om från det interna koordinatsystmet till det fixa. Även skillnaden i x 3 -led beräknas. x 1 = x s1 cos(θ) x s2 sin(θ) x 2 = x s1 sin(θ) + x s2 cos(θ) x 3 = x g3 [k] x g3 [k 1] Kalmanfiltret Som synes finns det redundant information då data kommer från olika källor. Det är upp till estimeringen att avgöra hur sensorvärderna ska viktas vid en viss tidpunkt k. Utgående från en tillståndsmodell för robotens rörelse enligt. x[k + 1] = A k x[k] + B k ū[k] + w[k] Där A k är övergångsmodellen från tillstånden vid k 1 till k. B k är modell för styrsignalernas verkan på systemet (roboten). ū k är de styrsignalerna som ges av regulatorn och w k är processbrus i modellen enligt figur 15. Kovariansmatrisen för skattningsfelet definieras enligt följande. P [k l] = E { (ˆ x[k l] x[k])(ˆ x[k l] x[k]) T } Kalmanfiltret arbetar i två faser: mätuppdatering och tidsuppdatering. I mätuppdateringen skattas tillstånden och koovariansmatrisen i tidpunkt k utifrån data upp till tidpunkt k 1 enligt Sida 34
ekvation (3) - (4). Tidsuppdateringen sker enligt (5) - (7) ˆ x[k + 1 k] = A kˆ x[k k] + B k ū[k] (3) P[k + 1 k] = A k P[k k]a T k + Q k 1 (4) ˆ x[k k] = ˆ x[k k 1] + K[k](y[k] C kˆ x[k k 1]) (5) P[k k] = P[k k 1] K[k]C k P[k k 1] (6) K[k] = P[k k 1]C T k (C k P[k k 1]C T k + R k ) 1 (7) Där Q är kovariansmatrisen för felet w[k] och R är kovariansen för mätfelen v[k]. 4.3.5 Navigering Navigeringen ska skicka indata i form av reglerfel (e r, e ϕ ) till regulatorn för att styra scouten längs den bana som specificerats av operatören. Banan består av en lista av koordinater, givna i det fixa koordinatsystemet, som scout ska åka till en efter en. Låt oss kalla den koordinat som scout för tillfället ska till för målpositionen. Eftersom scout rör sig i tre dimensioner men målpositionen är given i två dimensioner (endast x 1 och x 2 ) är målpositionen egentligen en linje och ingen punkt som visas i figur 14. Den punkt som scout ska navigera mot är denna linjes skärning med det plan som scout befinner sig i. Om lutningen av planet antas liten kan istället en approximerad målposition användas som är på samma höjd (x 3 koordinat) som scout. Indata till regulatorn består av den approximerade målpositionen givet i scoutens interna koordinatsystem omräknat till polära koordinater. Det vill säga T msapprox i figur 14, givet i polära koordinater. Även tidsderivatan av målpositionen är indata till regulatorn. Figur 14: Navigeringen beräknar T msapprox i polära koordinater som är insignal till regulatorn. Sida 35
Först måste T msapprox beräknas. Om R sf är rotationsmatrisen från fixa koordinatsystemet till scouts koordinatsystem fås. m s1 T msapprox = m s2 = R sf (T m T s ) m s3 Sedan beräknas målpositionen i polära koordinater e r = m 2 s1 + m 2 s2 e ϕ = arctan ( ms2 m s1 Även tidsderivatan av målpositionen ska beräknas ṁ s1 Ṫ msapprox = ṁ s2 = R sf Ṫ s ṘsfT s ṁ s3 ) ė r = ė ϕ = m s1 m ṁ s1 + s2 ṁ s2 m 2 s1 + m 2 s2 m 2 s1 + m 2 s2 ( 1 1 ( ) 2 ṁ s2 m ) s2 ṁ m 1 + s2 m s1 m 2 s1 s1 m s1 4.3.6 Regulator Strukturen för styrsystemet för scouten ser ut som i figur 15. Navigator kommer att kontrollera vilket av de två lägena som ska köras. De två alternativen som kan väljas mellan illustreras med en omkopplare. Regulatorn kommer att återkopplas med de skattade tillstånden som fås av en observatör, i vårt fall kommer vi kalla observatören för estimeringen. Denna kommer att få sina mätdata från sensormodulen, se vidare avsnitt 4.3.4. I modellen i figur 15 har vi en rad mätfel: mätbrus (n), processbrus (w) samt att vi även har en störkälla w u. Den sistnämda kommer att kunna betraktas som försumbar, ty detta är bara räknefel som sker inom programkod. I modellen har vi tagit med tre regulatorer F r, F y och F, dessa skulle egentligen kunna sammanfogas till endast en. Men detta görs inte p.g.a att strukturen vill hållas generell och att den för tillfället passar en LQG struktur. Det behöver dock inte nödvändigtvis bli att vi använder en sådan struktur men med upplägget som bilden visar har vi möjlighet till många val av reglerdesigner. Sida 36
Figur 15: Schematisk bild på hur reglersystemet kommer att se ut Efter regulatorn F tillkommer det ett block Aktuator, det är den som kommer att verkställa den utsignal som regulatorn bestämt till ett faktiskt moment och kan ses som ett gränssnitt mellan datorn och motorerna. När vi har verkställt styrsignalen kommer den att verka på det sanna systemet G 0. Här kommer vi i simuleringar använda oss av en modell för scouten som vi kallar för G, mer om denna modell i kapitel 4.3.7 där modellen diskuteras i detalj. Signalen z kommer vara det vi vill reglera efter, men eftersom det kommer att komma in mätstörningar (n) kommer detta inte vara det vi mäter. Det vi kommer mäta är y som vi återkopplar till vår observatör (som i vårt fall kallas för estimeraren). De referenssignaler som vi kommer att reglera efter kommer att vara avståndet till målet (e r ) och en vinkel (e ϕ ) angiven från x s1 till vektorn längs den snabbaste vägen mellan scoutens mittpunkt till målet, se figur 14. Aktuator double speed_right; double speed_left; double speed_right; double speed_left; Controller bool on_off; data reference_data; data estimator_data; data controll_data; Tabell 14: Övre delen är indata och undre är utdata 4.3.7 Fordonsmodell Fordonsmodellen kommer att bestå av en del förenklingar. Det kommer även finnas parametrar som inte är bestämda, dessa kommer att vara nödvändiga att exprimentiellt bestämma. Nedan förklaras de beteckningar som används för krafter, moment, och vinkelaccelerationer. Dessa är uppdelade i två kategorier, mätsignaler och okända saker som måste bestämmas antingen genom modellering eller att man på något sätt bestämmer dem. Sida 37
Mätsignaler: F r - Drivkrafter, höger F l - Drivkrafter, vänster p - Vinkelhastigheten kring x-axeln q - Vinkelhastigheten kring y-axeln r - Vinkelhastigheten kring z-axeln ω d,l - Vinkelhastigheten för drivande hjulet, vänster ω d,r - Vinkelhastigheten för drivande hjulet, höger β - Lutningen på marken i roll-led α - Lutningen på marken i tipp-led ẍ - acceleration i x-led ÿ - acceleration i y-led z - acceleration i z-led Okända: R r,r - Rullmotståndet, höger R r,l - Rullmotståndet, vänster W - Totalla tyngdkraften G l - Friktionskraften i lateral led, vänster G r - Friktionskraften i lateral led, höger W f,r - Normalkraften, fram, höger W f,l - Normalkraften, fram vänster W r,r - Normalkraften, bak, höger W r,l - Normalkraften, bak, vänster Samt att tröghetsprodukterna är okända, dvs. I xx, I yy, I zz, I xz Modellförenklingar Rotationsenergin från banden, hjulen kommer inte att beaktas. Scouten beaktas som en stel kropp som får sin drivkraft från två fiktiva krafter F l och F r som egentligen genereras av moment från motorerna, dessa kommer att modelleras med ett beroende av spänningen u. Luftmotstånd och luftdragmotstånd kommer att antas ha försumbar effekt, detta eftersom Scouten inte kommer köras i områden där denna effekt har betydelse. Effekter som nersjunkning i underlaget kommer inte att beaktas, bedömningen är att Scouten inte väger tillräckligt mycket för att kunna tillämpa den teori som finns. Förenklingar kommer att göras när tröghetsprodukterna ska bestämmas, dessa beskrivs under Parameterbestämning. Parameterbestämning Sida 38
Rullmotstånd: Rullmotståndet kommer att förenklas till en fix kraft som är oberoende av några parametrar. Det kommer krävas ett experiment för att bestämma denna. Utförande: Roboten kommer att vägas, sedan placerar man roboten på en skiva med ett underlag som är representativt. Därefter ökas vinkeln tills scouten precis börjar antyda till att börja rulla, när detta gjorts mäts vinkeln. Med hjälp av detta kommer det vara möjligt att rullmotståndet. Figur 16: Vinkeln varieras till att Scouten precis börjar röra sig Experimentet illustreras i figure 16. Man kan inse att detta kommer att vara en approximation, eftersom det kommer att även innefatta rullmotståndet från själva bandet och hjulen som egentligen ska modelleras som egna krafter. Detta gör bara att alla rull- och friktionskrafter sammanförs till en kraft som betraktas som konstant. Figur 17: En väldigt förenklad modell kommer att betraktas när tröghetsprodukterna ska beaktas Tröghetsprodukter: Vid beräkning av tröghetsprodukter kommer dessa också att förenklas. Modellen utgår från att origo är i masscentrum, detta gör att innan man applicerat annan utrustning kan förenkla dessa produkter eftersom ett symmetriplan uppstår som gör att två tröghetsprodukter blir noll. När man applicerar föremål kommer dessa att förenklas till enkla former med en homogen densitet. Sida 39
Modell Rörelse ekvationerna kommer att få utseendet: F x = m(ẍ + qż rẋ) F y = m(ÿ + rẋ pż) F z = m( z + qẏ qẋ) Figur 18: Koordinatsystemen för Scouten M x = I x ṗ I xy q I zx ṙ I xz qp I zy q 2 I z rq + I xy pr I y qr + I zy r 2 M x = I xy ṗ + I y q I zy ṙ I x pr I xy qr I zy r 2 + I xz p 2 I zy qp I z pr M x = I xz ṗ I zy q + I z ṙ I xy p 2 + I y qp I zy rp I x pq + I xy q 2 + I z pq Friläggning av roboten kommer att utföras med en förenklad modell där eulervinklar används för att gå mellan det fixa koordinatsystemet som är fixt i marken och det som följer med Scouten. De eulervinklar som kommer att användas är standard inom flygmekanik. Vi har även att: F i R r,i µ(w f,i + W r,i ), i = l, r Vi vill också kunna bestämma slippet genom följande ekvation: i slip = 1, i = l, r V r wheel ω d,i Sida 40
5 Nätverksmodulen Nätverksmodulens uppgift är att möjliggöra kommunikation mellan masterdatorn och scoutdatorn via WLAN. Modulen kommer programmeras med hjälp av socket-library anpassat för datorer med operativsystemet Linux. När ett egendefinierat objekt med data skall skickas, kommer objektets variabler att läsas och skickas en i taget. Objektet kommer sedan att rekonstrueras på mottagarsidan. 5.1 Masterenhet Från masterenheten ska det i manuellt läge skickas kommandon för hur operatören vill att roboten ska åka. I autonomt läge ska det skickas en specifik bana som scouten ska följa samt information om beräkningarna i ImageProcessing. Den ska även kunna ta emot sensordata, kamerabilder och uppskattat tillstånd från scouten som skall skickas vidare till användargränssnittet och ImageProcessing. Eftersom operatören endast är intresserad av nuvarande status och sensordata kommer nätverksenheten inte buffra något utan skriva över de gamla objekten när nya objekt skickas. 5.1.1 Funktioner Send En sendfunktion för att skicka data från användargränssnittet till scoutdatorn måste finnas. Funktionen kommer antingen att skicka styrkommandon eller en serie koordinater beroende på om scouten ska köras i manuellt eller autonomt läge. I autonomt läge skall även data från ImageProcessing skickas till scouten varför det även skall finnas en funktion för att skicka sådan information. Funktionerna skall indikera om sändningen lyckades eller inte. Recieve Nätverksmodulen tar emot data från scoutenheten och vidarebefordrar det till användargränssnittet och ImageProcessing. Data till användargränssnittet består av sensordata från GPS, IMU, Odometrar, status och estimerat tillstånd. Till ImageProcessing skall ImageProcessingData skickas. Connection Funktioner för att hantera kopplingen mellan mastern och scouten måste finnas. Scouten agerar host och en connect-funktion ansluter mastern till scouten. Det skall även finnas funktioner för att koppla ner och för att kontrollera att vi har kontakt med scouten. Sida 41