Robot met GPS ?

Uit RobotMC.be
Ga naar: navigatie, zoeken

Google Earth

Google Earth is onmisbaar voor het evalueren van GPS-Tracks ! Via "Extra" en "GPS" kan je NMEA tracks inlezen en zichtbaar maken in Google. Na het importeren kan je ook het hoogteprofiel en de snelheid zichtbaar maken : rechtsklikken op "gpx GPS-apparaat" en een extra venster met het grafisch verloop verschijnt. In dit menu kan je ook nog andere zaken instellen via "Eigenschappen". Je kan ook de track laten afspelen met de schuifbalk bovenaan. Van deze animatie kan je dan ook de afspeelsnelheid aanpassen. Ook hier was ik weer verbaasd over de nauwkeurigheid : je kan duidelijk zien waar ik even gestopt ben tijdens de wandeling. Google Earth heeft voldoende aan de $GRMC-string om de track te tonen. Hier vindt je ook nog een "How to" video om NMEA Tracks in Google Earth te importeren : NMEA tracks in Google Earth importeren.

https://www.youtube.com/watch?v=85jZPnwTEyM


GPS-Onvangers

GPS-ontvangers zijn erg goedkoop geworden : op Ebay vindt je voor enkele Euro's hele performante GPS-modules. Er bestaan zogenaamde "GPS-Muizen" die je kan aansluiten op een gewone seriele poort van je computer, maar voor µ-controllers zijn er ook modules die rechtstreeks op 3.3V of 5 Volt werken. Je kan deze modules direct aan de seriele poort van de µ-controller hangen. Deze modules hebben dikwijls een kleine backup batterij, zodat de laatste fix, maar ook de instellingen bewaard blijven na het uitschakelen. Dikwijls is er ook software van de fabrikant die je kan gebruiken om de instellingen te veranderen, of GPS data te visualiseren. Voor de Sirfstar kan je "SirfDemo" downloaden. Opgelet, de backup batterij van de Sirfstar kan maar enkele dagen overbruggen.

Sirfstar.jpg

Een meer performante GPS is de Ublox M8n. Dit is een GPS die ook Chinese en Russische GPS Sattellieten kan ontvangen. Hiervoor zijn er zelfs 2 ontvangers ingebouwd. Standaard is de GPS update ook aan 1Hz, maar deze kan ingesteld worden tot zelfs 20 Hz ! Het instellen van deze GPS gaat met de software Ucenter. Dit is een vrij uitgebreid pakket waar je alle mogelijke visualisaties en instellingen van de Ublox GPS kan aanpassen. Met mijn GPS was het blijkbaar niet mogelijk om de instellingen op te slaan, maar met de juiste seriele commandos kan je de GPS bij elke opstart ook instellen.

Enkele tracking testen met een SirfstarIII ontvanger tonen duidelijk het belang aan van een "open" hemel : Bij een wandeling doorheen het natuurgebied Andersstad waren de afwijkingen kleiner dan 1 meter, maar de rit ernaartoe leverde plots een sprong op van wel 10 meter !

De NMEA data van de GPS-module kunnen we loggen via een terminal programma (ik gebruik HTerm). Standaard geeft deze SirfstarIII ontvanger de NMEA data serieel uit met 4800 baud. HTerm kan deze data dan loggen en opslaan. Deze tekstdata kan je best opslaan met als suffix .nmea. In Google Earth heb je een menu "GPS". Daar kan je ook rechstreeks een seriele poort aangeven, dan wordt de GPS positie realtime zichtbaar. Helaas schakelt Google dan een vast zoom-factor in, en deze is niet ideaal (wel te veranderen via rechtsklikken op "gpx GPS-apparaat"). Als je een track inleest, kan je wel de zoom-factor kiezen.

Mijn GPS-Robot :

GPS robot.jpg Als basis heb ik een oude RC-auto van Tamiya gebruikt. De GPS is een SirfstarIII met een update-rate van 1 Hz, en a ls kompas gebruik ik een MPU9250. µ-controller is een nucleo bordje met een STM32 F401 die loopt aan 84 MHz. Het algoritme is vrij eenvoudig :

NMEA-data wordt over de usart1 van de F401 ontvangen aan 4800 baud. De string met de Lengte en Breedte positie wordt gedecodeerd en als float opgeslagen. De robot krijgt dus elke sekonde nieuwe informatie over zijn actuele GPS-positie. De MPU9250 is een IMU (inertial measurement unit) met 9 DOF (grade of freedom). Er zit dus ook een 3-assige versnelling sensor, 3-assen gyroscoop en een 3-assen kompas in. Via I2C wordt al deze sensoren uitgelezen en via een welbekend algoritme wordt dan de actuele kompaskoers gemeten. Aan de hand van de werkelijke GPS-positie en de gewenste GPS-positie wordt dan de richting berekend naar de "gewenste positie". Het kompas geeft de werkelijke richting, we kennen de gewenste positie, met het verschil tussen beide wordt dan het signaal van de stuurservo bepaald. Indien de robot rijdt, zal de werkelijke richting (kompaskoers) zich vrij snel aan passen omdat er in die richting gestuurd wordt ! Bij de eerste testen is alleen de richting-servo aangestuurd door de robot. De snelheid (regelaar motor) heb ik met een klassieke modelbouw zender (35 MHz) zelf aangestuurd. Problemen : Een GPS positie kan een vrij grote fout hebben, maar met open hemel kan men toch een nauwkeurigheid van ca 3 meter verwachten De update rate van 1 sek is vrij laag, vooral als de robot snel rijdt is dit merkbaar. Nieuwere GPS modules gaan naar 10 Hz en meer (ublox). De RC-auto die ik gebruik haalt makkelijk snelheden van 15m/s. Bij het naderen van een track-point, moet je dus de snelheid verminderen om te vermijden dat je er ver overheen gaat. De lengte en breedte positie in een NMEA string wordt niet als float gegeven, maar als "graden, minuten, 0,0001 minuten. Dit moet dus eerst omgerekend worden in een float. Op het net vindt je talloze librarys die dat klusje klaren. Er wordt gebruik gemaakt van de functie "Atan2(Breedte, Lengte)" om de hoek te berekenen. Deze functie geeft dan een hoek tussen de -180° en +180°. De hoek kan dus verspringen van -179 naar +179 als de werkelijke hoek maar 2° verspringt ! Het magnetisch kompas is zeer gevoelig aan verstoring van het veld door alle ijzer / magneten / stromen in de buurt van het kompas. Tracht het kompas zo ver mogelijk van de motor in te bouwen. Vermijd magnetisch metaal in de buurt van het kompas (niet vastzetten met een stalen boutje !) Na inbouw van het kompas is een kalibratie noodzakelijk. Deze kalibratie is eigenlijk de meting van alle vaste velden die veroorzaakt worden door de robot zelf. Als deze velden gekend zijn, kunnen deze eenvoudig gecompenseerd worden door een offset waarde in te voeren. Een kleine offset fout van gemeten kompas koers heeft als gevolg dat de robot met een boog naar het nieuwe trackpoint toe rijdt : de berekende koers is juist, maar de rij richting van de robot wijkt hier van af met de offset. Als de robot dichter bij het trackpoint komt, wordt elke seconde de koers opnieuw berekend, en hier wordt dan gecompenseerd voor deze offset fout. De robot rijdt in een spiraal vorm naar het tracking point. You Tube videos van de GPS-rover : Bij deze poging wordt de richting door de robot geregeld, de snelheid regel ik zelf met een RC-zender. De robot rijdt telkens voorbij het track point, en keert dan terug.

https://www.youtube.com/watch?v=yRVqLKYsAZ8

Bij de volgende pogingen heb ik een betere GPS gebruikt (Ublox M8N) met een update rate van 10 Hz. De robot rijdt verschillende trackpointen aan (rechthoek van 12 * 8 meter.

https://youtu.be/01hAJ5zExT4

De nauwkeurigheid van de Ublox is erg goed, vooral bij open zicht :

https://www.youtube.com/watch?v=5vCAOM6f5UE

Na nog wat optimeringen kunnen we nu aan behoorlijke snelheden onze rondjes draaien :

https://www.youtube.com/watch?v=yMbmp7c2mrw

Keuze van de µ-controller

Eigenlijk kan je al met een eenvoudige Arduino Uno een GPS-rover besturen, maar met een wat krachtiger µ-controller wordt alles toch wat performanter. Ik maak gebruik van de volgende periferie van de F401 :

Eén UART om de NMEA-strings van de GPS-module te ontvangen. Deze moet ook kunnen zenden, om bij de opstart de GPS te kunnen initialiseren. Zo stuur ik bij elke opstart enkele vast strings naar de GPS om deze in te stellen. Sommige modules hebben een EEPROM / Flash / Batterij backup zodat deze instellingen bewaard blijven, maar zeker de goedkopere modules hebben niet altijd deze functionaliteit. Opgelet, elke byte die ontvangen wordt via een UART veroorzaakt een interrupt. Als je dus de GPS module instelt, is het belangrijk hier rekening mee te houden. NMEA-strings die je niet gebruikt, kan je beter uitschakelen. Bij mijn rover gebruik ik enkel de "$GMRC"string. Alle andere NMEA strings worden via de initialisatie uitgeschakeld. Daarnaast is ook de baudrate + datarate belangrijk. Vb bij 10 Hz worden er standaard makkelijk 4 verschillende NMEA-strings verstuurd elke 100 ms. Als de baudrate te laag is, past dit niet in het tijdsframe van 100 ms. Oplossing : baudrate verhogen of NMEA-strings uitschakelen. Een zeer handige "feature" van de Nucleo is de virtuele com poort van de ST-link. Via deze link kan je dus zowel flashen en een seriele verbinding opbouwen ! Een tweede UART (seriele verbinding) om data via Bluetooth draadloos te verzenden. Dit is zeer nuttig om GPS-tracks te loggen, en andere variabelen te kunnen volgen tijdens een rit. Je kan ook strings naar de robot verzenden via deze verbinding. Dit kan nuttig zijn om variabelen te veranderen, of opdrachten te geven. Een BT HC05 heeft in open veld toch een reikwijdt van zeker 20 meter. Zeker zo makkelijk is het om data te loggen via een ESP8266. Zie ook het artikel http://www.robotmc.be/joomla/index.php/artikels/9-loggen-met-de-wemos-d1. Standaard is een string verzenden met de UART "Blocking". Dat wil zeggen dat de programma-afloop moet wachten tot de string volledig verzonden is. Vooral bij lage baudrates en lange strings is dit nadelig. Er zijn verschillende oplossingen voor dit probleem : Een hoge baudrate kiezen (115 k of hoger), telkens maar korte strings verzenden, of UART over DMA : Dit laat toe om een string gereed te zetten in het geheugen, en dan via "DMA" te verzenden. Dit verloopt volledig automatisch zonder tussenkomst van het hoofdprogramma. Om een idee te geven : standaard had ik 60 ms nodig om een lange string te verzenden aan 38400 baud, via DMA was dit nog 60 µs... Om het som-signaal van de ontvanger te decoderen, is een "Input Capture" functie ideaal : Het exacte tijdstip van puls wordt hardwarematig opgeslagen en kan dan later via een interrupt verwerkt worden. Hiervoor gebruik ik een 32-bit timer die loopt aan 1 MHz. Bij elke positieve flank wordt de actuele tellerwaarde in een register opgeslagen. Tevens wordt er een interrupt getriggerd. In deze interrupt wordt dan deze tellerstand vergeleken met de vorige, en zo heb je dan een exacte meting van de pulslengte. De timer die aan 1 MHz loopt, wordt dan ook nog eens gebruikt om een exact tijdsinterval te meten als ik de gyro-waarde ga integreren. Immers, de gyro geeft een "draaisnelheid" aan, maar we zijn eigenlijk geinteresseerd in de "hoek". Dit wordt bereikt door de draaisnelheid te vermenigvuldigen met het tijdsinterval, en dit dan te sommeren. Omdat het hoofdprogramma door talloze interrupts wordt onderbroken, is dit tijdsinterval niet altijd hetzelfde. Via een exacte meting met de 1 MHz timer kan dit gecompenseerd worden. Een andere timer wordt gebruikt om de PWM signalen voor de servos en de snelheidsregelaar te genereren. Dit is een 16-bit timer die loopt aan 1 Mhz, de resolutie is dus ook 1µs. De PWM periode is 20 ms, via 2 "Compare" uitgangen kan men dan de servo / snelheidsregelaar aansturen. Externe interrupt wordt gebruikt om de encoder van de motorsnelheid uit te lezen. I2C om de MPU9250 uit te lezen. Dit is een 9 DOF IMU met een 3-assen gyro, 3-assen kompas en 3 assen accelerometer. De gyro en de ACC kunnen aan 200 Hz uitgelezen worden, het kompas aan 100 Hz. De ledstrip met 8*WS2812B wordt aangestuurd via DMA / SPI. Nuttige software librarys : Om NMEA strings te decoderen bestaan er al voldoende librarys, ik heb deze gebruikt :

https://github.com/cloudyourcar/minmea 

Deze lib kan vrijwel alle NMEA boodschappen decoderen, maar ik gebruik alleen de &GRMC string. Voor de I2C communicatie vond ik deze library (specifiek voor de STM32 F4 µ controllers) :

https://stm32f4-discovery.net/2014/05/library-09-i2c-for-stm32f4xx/

Om de WS2812B ledstrip aan te sturen vond ik op deze site een library voor de F4 :

http://mikrocontroller.bplaced.net/wordpress/?page_id=3665 

En tenslotte voor de MPU9250 is er deze library :

https://github.com/kriswiner/MPU-9250 

Uiteindelijk heb ik hiervan alleen de initialisatie + uitlees functie van deze lib gebruikt. Het "IMU" gedeelte kreeg ik uiteindelijk stabiel werkend, belangrijk is de aanpassing in de lib van de werkelijke X/Y/Z orientatie tov de robot.

De programma afloop :

Richting en afstand:

De GPS geeft in een NMEA string de breedte en de lengte graad. Standaard is dit formaat in graden, minuten, °Minuten. De breedtegraad is dus de hoek van de positie ten opzichte van de evenaar. Alle posities op de evenaar hebben dus breedtegraad 0°, op de noordpool is de breedtegraad 90°, op de zuidpool -90°. Belgie ligt ongeveer op 52°. De lengtegraad wordt dan uitgedrukt tov "Greenwich". Volgens afspraken ligt dus het nul graden punt bij Greenwich (Engeland, hiervan dus het gezegde : Hij denkt dat de meridiaan van Greenwich door zijn ... loopt!)). Belgie ligt op ca 4°westerlengte. Als je nu graden wil vertalen naar afstanden is dit vrij eenvoudig : De omtrek van de aarde bedraagt ca 40000 km, dus 360 breedtegraden zijn gelijk aan 40000 km. 1 Breedtegraad is dan 40000/360 = 111,11 km. Een lengtegraad echter heeft ter hoogte van Belgie een kleinere afstand : ongeveer 68,4 km. Voor onze GPS-rover kunnen we gerust deze waarde fix aannemen, maar als je op een andere breedtegraad wil rijden, moet je hier rekening mee houden.

Om naar een waypoint te rijden heeft je rover dus een richting nodig, en de afstand tot het waypoint. De formule is vrij eenvoudig als we de GPS-coördinaten van beide kennen :

Richting = atan2 ((Waypoint.breedte-Actuele.breedte)*111.11,(Waypoint.lengte-Actuele.lengte)*68.4))*180/3.1415
Afstand = Vierkantswortel((Waypoint.breedte-Actuele.breedte)²*111.11²+(Waypoint.lengte-Actuele.lengte)²*68.4²)

De functie atan2(y,x) vindt je in de math.h lib van GCC. Het is niets anders dan de boogtangens y/x. Het resultaat is een float en wordt uitgedrukt in Radialen (360°=2PI radialen). De afstand wordt berekend met de bekende Pythagoras stelling a²+b²=c² (geldig voor een rechte hoek). In dit geval wordt de afstand berekend in km.

Werkelijke richting van de rover:

Dankzij onze GPS en was eenvoudig rekenwerk weten we dus waar we naar toe moeten, en welke afstand we nog moeten afleggen naar het waypoint. Maar, we weten nog niet in welke richting onze rover wijst ! Immers, bij stilstand geeft de GPS geen richting (heading) ! Hiervoor hebben we dus een andere sensor nodig, meestal wordt een magnetisch kompas gebruikt. We moeten minimaal in 2 dimensies de sterkte van het aard magneetveld kunnen meten, hieruit kan dan weer een richting bepaald worden. Moderne kompassensoren hebben meestal 3 assen, het magneetveld kan dus in alle richtingen bepaald worden. Het aardmagneetveld wijst eigenlijk niet naar het magnetische noorden, maar gaat onder een scherpe hoek de grond in ! Daarom moet ook een gewoon kompas steeds goed horizontaal gezet worden voor een betrouwbare meting. Ook voor ons elektronisch kompas moeten we hier rekening mee houden ! Als we het magnetisch veld meten in de 2 horizontale assen, kunnen we hieruit het noorden bepalen ;

Hoek tov Noord = atan2(Veldsterkte Y-as-Offset Y-as, Veldsterkte X-as-Offset X-as)*180/PI

Enorm belangrijk is de kalibratie van het kompas : de metingen dat we doen omvatten immers ook de offset + de velden die veroorzaakt worden door onze rover zelf ! We moeten deze fouten dus vooraf meten, en corrigeren. Hier nog wat info over kalibratie :

Goedkope_10DOF_met_I2C_aansluiting

Kompas, tilt en roll kompensatie :

Zoals reeds vermeld wordt de gemeten kompasrichting sterk beinvloed door afwijkingen van de horizontaal stand. De bovenstaande formule is dus alleen nauwkeurig als de kompas sensor loodrecht staat. In een rijdend voertuig is dit natuurlijk niet altijd het geval ! Hellingen, overhellen van de gps-car in bochten maken dat onze kompas sensor niet altijd loodrecht staat ! Beter als alleen een kompas is dan ook een IMU (Inertial measurement unit). Een IMU wordt samengesteld uit een cluster van 3 verschillende sensoren :

Magnetische sensoren, in 3 assen.
Acceleratie sensoren, wederom in 3 assen.
Gyro sensoren, ook in 3 assen.

Hiervan komt de term 9DOF : het is dus een cluster van in het totaal "9 Degrees Of Freedom", ofwel een meetsysteem dat 9 onafhankelijke grootheden kan meten. Met deze metingen kan men theoretisch steeds de stand van deze sensor-cluster meten ten opzicht van de aarde :

Met de 3-assen magneetsensor kan men de richting van het aardmagnetisch veld meten (in Belgie Noord en ca 70° naar de grond in)

De 3-assen ACC sensor geeft de richting aan van de zwaartekracht (naar het middelpunt van de aarde), indien de sensor in rust is en ondersteund (geen vrije val).

Met de gyro-sensor kan men fouten van de beide andere sensoren compenseren. De gyro op zich kan nooit een absolute richting meten : hij meet alleen een "draaisnelheid" tov een bepaalde as. Een gyro heeft wel last van drift : zelfs als de sensor stil staat, geeft hij toch een (kleine) draaisnelheid aan. Om de hoek te kennen echter, moet deze draaisnelheid "opgeteld worden"(integratie). De MPU9250 bijvoorbeeld heeft een drift van ca 1°/minuut. Ook hier is kalibratie van groot belang : men doet een meting bij stilstand van de sensor, deze "nul-meting" wordt dan later gebruikt als compensatie.

Sensor fusion :

Zo noemt men het algoritme dat al deze metingen combineert zodanig dat men steeds de stand van de IMU kent tov het "inertiaal stelsel", in ons geval de aarde. Men bekomt dus de "yaw", "pitch" en "roll" ten opzichte van de loodrechte en ten opzichte van het magnetische noorden. Deze termen zijn gekend in de luchtvaart : In een vliegtuig is yaw de richting of koers, pitch is de neus stand omhoog/omlaag, en roll is de helling links/rechts. Voor onze GPS-rover zijn we vooral geinteresseerd in de "yaw", dit is dus de hoek tov het magnetische noorden.

Ik gebruik het welgekende "madgwick" algoritme (zoek op MPU9250 / kris winer). De 9 gemeten grootheden worden in dit algoritme gebruikt om tenslotte yaw, roll en pitch te bekomen. Voor een stabiele meting is het wel absoluut noodzakelijk dat de magneetsensor goed gekalibreerd is. Daarnaast moeten ook de verschillende assen in de juiste configuratie en met het geschikte teken gebruikt worden. Bij mijn Mini bijvoorbeeld ziet de aanroep er als volgt uit :

MadgwickQuaternionUpdate(-ay,-ax, az, gy*PI/180.0f, gx*PI/180.0f, -gz*PI/180.0f,mx,my,mz)

Hier worden nu de "Quaternionen" berekend, om naar yaw, roll en pitch te gaan moet je deze nog omrekenen :

yaw   = -atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);

Met de MPU9250 en dit algoritme kan je makkelijk een bandbreedte van 200 Hz halen. Wel belangrijk, de maximale draaisnelheid wordt bepaald door de "range" instelling van de gyro, standaard gebruik ik de gevoeligste range met een maximum van 200°/sekonde. Als het toch sneller draait, krijg je grote afwijkingen !