Odometrie

Uit RobotMC.be
Ga naar: navigatie, zoeken

Odometrie

Bij het programmeren van robots is het vaak wenselijk om het antwoord te weten op de vraag "waar ben ik?". Als de startpositie van de robot bekend is (of als we een andere referentie hebben), dan kan odometrie ons helpen bij het antwoord. In dit artikel gaan we in op veel van de aspecten van odometrie. We nemen hierbij een robot met differential drive als basis. Dit is een robot met twee wielen die ieder onafhankelijk met een eigen motor worden aangedreven en een zwenkwiel of caster ('rollertje') als derde wiel.

Het principe

Columbus en de meeste andere zeevaarders uit zijn tijd, maakte gebruik van "dead reckoning". In dead reckoning, vindt de navigator zijn positie aan de hand van de route en afgelegde afstand vanaf het begin punt. Wanneer hij vertrekt vanuit een bepaald gekend punt, zoals een haven, meet de navigator zijn route en zijn afstand op een kaart uit vanaf dat punt, en prikt met een pin om de nieuwe positie te markeren. De eindpositie van elke dag, zal het startpunt zijn voor de route- en afstandsmeting van de volgende dag.

Bij odometrie wordt de methode van Columbus geimiteerd door de afgelegd weg te berekenen over een bepaalde tijd. Zogenaamde "timestamps" zijn bedoeld om vaste punten in de tijd te creeren waarop een nieuwe berekening gemaakt wordt. Per tijdsinterval wordt de afgelegde weg berekend. Dat is dus te vergelijken met de dagelijkse pin op de kaart.

De basis

Odometrie bij robots begint met het bepalen van de afgelegde weg per wiel. Hiervoor kunnen quadratuur-encoders worden gebruikt. Dit zijn encoders met twee outputs, waardoor ze 4 'toestanden' kunnen hebben. De volgorde van de 'toestanden' geeft de draairichting aan. Zo is 0->1->3->2->0 met de klok mee en is 0->2->3->1->0 tegen de klok in. Grafisch ziet dit er als volgt uit:

(plaatje encoder pulsen)

De eerste stap is om het signaal van de encoder om te zetten in een teller. Dit kan bijvoorbeeld door 'polling' - het regelmatig uitlezen van de encoder en eventuele wijzigingen verwerken in de teller. Het is hierbij van belang om de encoder zo snel uit te lezen dat geen toestand wordt gemist en daarom ligt het voor de hand dit in de interrupt te doen. De minimale frequentie van de interrupt kunnen we uitrekenen:

Stel we hebben een motor met vertraging die maximaal 92 omwentelingen/minuut maakt en een encoder heeft die 1410 pulsen per omwenteling (per kanaal) geeft. Dit is dus maximaal 1410 * 4 toestanden * 92 omwentelingen/min / 60 seconden/minuut = 8640 toestanden per seconden. De interrupt zal dus minimaal 8640 Hz moeten zijn. Een routine om een teller te onderhouden door het pollen van een encoder:

(map 2 <-> 3, bereken verschillen).

De teller die we nu hebben geeft de afgelegde weg van een wiel aan. Als we de fictieve motor (overigens gebaseerd op een Faulhaber 1624E012S495) voorzien van een wiel met een diameter van 67mm. De omtrek van dit wiel is pi * d = 3.14 * 67 = 210mm en op deze omtrek hebben we 1410 pulsen met ieder 4 toestanden. Dit betekent dat iedere stap van de teller overeenkomt met 210 / 1410 / 4 = 0.037 mm is.

Een andere manier om de pulsen van de encoder om te zetten naar een teller maakt gebruik van een interrupt op de flanken van 1 van de twee signalen van de encoder. Deze aanpak geeft alleen een interrupt als er daadwerkelijk een flank is en belast daarmee de processor aanzienlijk minder. En doordat op slechts 1 van de twee signalen een interrupt wordt gegenereerd, halveert ook het aantal interrupts, maar ook het aantal toestenden en dus de resolutie. In ons voorbeeld wordt de stapgrootte daarmee 2 * 0.037 = 0.074 mm en dat is nog steeds uitstekend. Maar als de encoder een lage resolutie heeft (relatief weinig pulsen per cm) heeft de eerste methode de voorkeur: de maximale resolutie wordt gerealiseerd en met een lage resolutie zal ook de benodigde interrupt-frequentie laag zijn.

De resolutie van de encoder heeft een grote invloed op de nauwkeurigheid : stel dat je wielbasis 100 mm bedraagt. De resolutie van de encoder is 1 mm. Dan bedraagt de resolutie van je e_compass maximaal de Boogtangens (1/100) = 0.57 graden. Op zich geen slechte waarde, maar stel dat je 3 meter rechtdoor wil rijden. De afstand kan theoretisch nauwkeurig zijn op 1 mm, maar dwars daarop kan de fout wel 3000 mm*sin(0.57°)= 30 mm bedragen!! Dit in de veronderstelling dat alle andere invloeden geen fout veroorzaken natuurlijk.

De veelgebruikte AVR 8-bit processor (Mega32) heeft slechts 3 externe interrupts. Hier is het dus niet mogelijk om van 2 kwadratuur encoders zowel het A als het B kanaal via een interrupt te laten tellen. We kiezen dan om maar één kanaal aan een interrupt te verbinden, het andere kanaal wordt dan in de ISR zelf afgevraagd. Mijn robot heeft encoders met een stap van 0.4 mm. De maximale snelheid bedraagt ongeveer 70 cm/sek. Daarmee wordt de maximale interruptfrequentie 1500 Hz per encoder. Indien de interrupt enkel actief is bij een stijgende of dalende flank, kan de interrupt routine er zo uitzien :


Isr encoder.png

Deze routines gaan enkel nog maar de tellers aanpassen bij elke flank van kanaal A. Als we nu de snelheid willen weten, is het voldoende om op regelmatige tijdstippen de variabele voor de snelheid uit te lezen en terug op 0 te zetten. Hiervoor is een tijdgestuurde interrupt de makkelijkste weg. De periode neem je zo klein mogelijk, maar natuurlijk wel zo dat je voldoende pulsen meet per periode. Vb minimale snelheid = 3 pulsen/periode, maximale snelheid = 100 pulsen/periode. Een betrouwbare snelheidsmeting is de basis voor de volgende stap : snelheidsregeling !

Het motor model

Een gelijkstroom motor zet elektrische energie om in mechanische energie. We sluiten een spanning aan, deze veroorzaakt een stroom doorheen een geleider. Op zijn beurt wordt hierdoor een magnetisch veld geinduceerd, en daar de geleider zich reeds in een bestaand statisch veld bevindt, wordt er een kracht op deze geleider uitgeoefend : de motor begint te draaien. De wiskundige verbanden zijn vrij eenvoudig :

De kracht (koppel) van de motor is evenredig met de stroom en met de grootte van het statisch veld. De stroom is omgekeerd evenredig met het toerental en met de ohmse weerstand. De inductieve weerstand heeft vooral gevolgen indien we de stroom plots onderbreken : dit veroorzaakt steeds spanningspieken die moeten "geblust" worden. Daarvoor zijn er vrijloopdiodes noodzakelijk in de H-brug.

Motor-model.png

Uit dit model volgt direct dat de maximale stroom loopt bij een geblokkeerde motor. Als de motor draait, gaat de stroom dalen omdat de generatorwerking een tegengestelde spanning opwekt.Het maximale toerental is afhankelijk van de aangelegde spanning en van de belasting. Met onze PWM-regeling gaan we eigenlijk de "gemiddelde" spanning regelen, en bijgevolg het toerental. Het toerental gaat zich instellen naargelang de belasting. Immers, bij een hogere belasting zakt het toerental en de stroom gaat stijgen. Hierdoor stijgt het motorkoppel weer. Uiteindelijk resulteert dit in een motorkoppel dat gelijk is aan de belasting, maar het toerental zal dus ook wijzigen ! Dit is eigenlijk de hoofdreden dat een snelheidsregeling noodzakelijk is om rechtdoor te rijden !

PWM met de H-brug

De manier om de snelheid van onze gelijkstroommotor te regelen is natuurlijk de welgekende H-brug. In de afbeelding hieronder zie een typisch schema van een dergelijk H-brug. We herkennen 3 logische ingangen, die we verbinden met onze µ-processor. Input 1, input 2 en PWM. Input 1 en input 2 verbinden aansluiting 1 en 2 van de motor met de + van onze voeding, of met de massa. PWM gaat alle aansluitingen tegelijkertijd verbinden (laagohmig) of verbreken (hoogohmig). Deze aansturing is wordt het meeste gebruikt. Het voordeel is dat je tijdens de OFF-periode van de PWM geen stroomverbruik hebt. De motor is dan immers volledig losgekoppeld van de voeding. Hij wordt dan niet afgeremd en loopt vrij uit. Helaas is dit dan ook het nadeel : bij 0% PWM loopt de motor vrij uit, en wordt er dus totaal niet afgeremd. Remmen kan je alleen doen door op dat moment Input 1 en 2 om te keren, en terug PWM aan te sturen. Het is dus redelijk komplex om op deze wijze af te remmen.

H-brug PWM.png

Het alternatief is om het PWM signaal aan te sluiten aan één van de Inputs, de PWM aansluiting dient dan alleen nog als "Noodstop". De H-brug reageert nu helemaal anders : één aansluting van de motor wordt nu verbonden met de +, of met de massa. Dit betekent dat de motor ofwel wordt aangedreven, ofwel wordt kortgesloten. Het nadeel is natuurlijk dat het stroomverbruik aanzienlijk zal toenemen. In de aan-fase van de PWM gaat de motor versnellen, in de uit-fase gaat de motor afremmen. Het voordeel is natuurlijk dat onze motor nu veel beter te regelen valt met het PWM-signaal : 0% PWM is vol in de remmen !! Om de draairichting te veranderen, moeten we nu ook het PWM-signaal inverteren + de andere input omschakelen. Ik heb beide methoden eens getest met de H-brug L293D en een kleine motor. Het PWM-signaal werd verhoogd van 0 tot 100% en daarna plots terug op 0 % gezet. In onderstaande grafiek zie het resultaat : de gele kromme is het snelheidsverloop van de klassieke PWM aansturing, de blauwe kromme is de alternatieve wijze. De paarse kromme is het PWM-signaal. Hieruit blijkt dat de alternatieve aansluiting absoluut beter is indien we een "closed loop" snelheidsregeling willen hebben.

PWM input.png

De eigenlijke snelheidsregeling

Het gestuurde gedeelte

Als we de bovenstaande grafiek bekijken, dan is het duidelijk dat er een zeker verband is tussen %PWM en de snelheid die daaruit volgt. Voor de paarse curve is dit zelfs een zeer eenvoudig lineair verband ! We herkennen eerst een stukje van 0 tot 5% PWM waar de snelheid 0 blijft. Dit is meestal zo en wordt veroorzaakt door de wrijving. De motor zal pas beginnen te draaien indien het motorkoppel deze wrijving kan overwinnen. Daarna zien we een praktisch volledig lineair verband. In ons programma kunnen we dus reeds een "gestuurd" aandeel voorzien :

PWM_value = MIN_VALUE + 0.4*soll_speed

Hierin stelt de MIN_VALUE de waarde voor die nodig is om de wrijving te overwinnen. Meestal zal deze waarde tussen 5% en 30% liggen. De tweede term is dan het lineair gebied tussen MIN_VALUE (vb 5%) en 100% PWM. Je kan deze waarde ook schatten door 100% PWM te delen door de hoogst mogelijke snelheid die je robot kan bereiken (vb 100% PWM = 200 Tics/sekonde, de factor bedraagt dan 0.5).Opgelet, dit is nog geenzins een regellus, omdat we op geen enkele manier rekening houden met de werkelijke snelheid. Ik zou zeggen dat dit gedeelte reeds een goede gok is, maar de fine-tuning moet nog komen !! Overigens is het grote voordeel van het "gestuurde gedeelte" dat de werking ogenblikkelijk is. Er is geen enkele vertraging, op het moment dat de "soll-speed" verandert zal de PWM waarde ogenblikkelijk aangepast worden. Het geregelde gedeelte heeft altijd een vertraging, omdat er eerst gemeten moet worden alvorens er kan geregeld worden...

Het geregelde gedeelte : P-aandeel

Als eerste verbetering gaan we nu het "proportioneel" aandeel van onze regellus bekijken. Hiertoe moeten we eerst nog enkele begrippen verklaren, die veelvuldig gebruikt worden in de regeltechniek. Als eerste de "gewenste snelheid". Het is duidelijk dat als we de snelheid willen regelen, dat we dan ook een variabele krijgen die deze wens uitdrukt. "Soll-wert", "Setpoint" of "Gewenste waarde" dus. Deze gewenste waarde moet steeds van buitenaf het proces komen, de regeling zelf gaat hier nooit ingrijpen ! In de bovenstaande grafiek kan deze "Gewenste snelheid" gaan van 0 tot maximaal 180 tics/sekonde. Stel dat we aan gematigde snelheid willen rijden, dan kunnen we voor deze waarde bij voorbeeld 80 tics/sekonde kiezen. Zonder te regelen, maar met de bovenstaande "gestuurde" formule krijgen we dan een PWM-waarde van : 5%+0.4*80 = 41% Onze robot vertrekt, en dank zij onze gesofistikeerde kwadratuur encoders zien we dadelijk het resultaat : 74 tics/sekonde !! Niet echt dus wat we eigenlijk gewenst hadden...

Een nieuw begrip moeten we hier definieren : de "Fout" of "Error". Zoals je kan raden is dit natuurlijk het verschil tussen de "Gewenste" en de "Werkelijke" waarde. In dit geval hebben we een fout van : 80-74 = 6 tics/sekonde ! Als we deze fout nu eens gebruiken om onze PWM-waarde bij te sturen...

PWM_Value= MIN_VALUE+0.4*Soll_speed+P_factor*Fout

De P_factor bepaalt in hoeverre de fout onze PWM waarde gaat beinvloeden. Indien de P_factor klein is, zal er weinig gebeuren, maar indien de P-factor te groot is, gaan we steeds van te veel naar te weinig PWM, de snelheid zal dus steeds op en neer gaan ! Als de P-factor de juiste waarde heeft, gaat onze werkelijke snelheid de gewenste snelheid zeer dicht benaderen !! Het P-deel kan dus de fout sterk verkleinen, maar volledig weg regelen is niet altijd mogelijk met alleen een P-aandeel. De reden is dat er steeds een fout moet aanwezig zijn alvorens er een P-aandeel kan bijregelen !

Onze regeling verbeteren met een I-aandeel

Om de restfout van de vorige regeling beter te kunnen wegwerken, kan men overgaan tot een I-aandeel. We bekijken de restfout over een langere periode, en we gaan deze fout dan steeds optellen bij elke "regelperiode". Deze som (=Integraal) gaan we dan gebruiken om onze PWM waarde bij te regelen. Praktisch gezien gaan we vaste tijdstippen (vb elke 100 ms) de gemeten fout (de fout kan zowel positief als negatief zijn !)optellen. We krijgen dan een variabele I-som. Deze variabele vermenigvuldigen we weer met een I_factor en wordt bijgeteld bij de vorige regelformule. Het I-aandeel gaat de restfout volledig kunnen wegregelen, omdat zolang er een restfout bestaat, de I-som steeds zal verhogen totdat de fout effectief 0 bedraagt. Dat kan er dan zo uitzien (I_som wordt hier elke 100 ms opnieuw berekend):

If (PID_Timer>99){I_som=I_som+Fout;PID_Timer=0;}

PWM_Value= MIN_VALUE+0.4*Soll_speed+P_factor*Fout+I_factor*I_som

In de praktijk wordt dikwijls de waarde van I-som nog begrensd, omdat onder bepaalde omstandigheden deze Integraal veel te hoge waarden kan aannemen. Indien dan de reden van verstoring wegvalt, duurt het veel te lang om deze I-som weer terug af te tellen (Anti Windup). Ook wordt hij soms van buitenaf terug op 0 gezet, vb bij grote veranderingen van onze "Setpoint". Als we dit I-aandeel nader bekijken voor onze robot, dan blijkt I-som eigenlijk de gecumuleerde fout te zijn van alle tics sinds het begin van de regeling ! Het is dus de totale fout op de afstand, omdat we de snelheid niet exact hebben kunnen aanhouden. Stel dat we aan 80 tics/sekonde willen rijden, dan zouden we na 10 sekonden exact 800 tics moeten bereiken. Welnu, de I-som zal de totale fout over deze 10 sekonden bevatten ! Daarom is voor odometrie dit I-aandeel zo belangrijk. Immers, dit gedeelte zal ervoor zorgen dat snelheidsfouten zich niet kunnen cumuleren. Overigens is de integraal van de snelheid de afgelegde afstand !

Overshoot vermijden met een D-aandeel !

Voor sommige toepassingen is nog derde soort regeling noodzakelijk : Het D-aandeel. Dit deel gaat kijken hoe "snel" de fout verandert, en dan deze "verandering" gedeeltelijk tegenwerken. Een balanceer-robot kan bij voorbeeld niet zonder D-aandeel.Immers, een balans-bot zal zeer gemakkelijk "overshoot" vertonen, dat wil zeggen als hij na een verstoring terug rechtop wordt geregeld, kan hij zeer makkelijk naar de andere kant doorschieten. Dit kan men niet opvangen met alleen een P en een I-aandeel. Het is noodzakelijk dat men een regel-deel heeft, dat kijkt hoe snel de "fout" verandert. Hoe doen we dat nu in de praktijk ? Vooreerst moeten we weer op vaste tijdstippen naar de "fout" gaan kijken. Als we nu de vorige fout aftrekken van de huidige fout, weten we exact hoe de fout verandert is tov de vorige keer. We meten dus de verandering van de fout per tijdseenheid. Om het eenvoudig te maken, kunnen we dezelfde periode als het I-aandeel gebruiken :

If (PID_Timer>99){D_aandeel=Fout-Vorige_fout;Vorige_fout=fout;PID_Timer=0;}

PWM_Value= MIN_VALUE+0.4*Soll_speed+P_factor*Fout+I_factor*I_som+D_factor*D_aandeel

Voor een normale snelheidsregeling is een D-aandeel meestal niet noodzakelijk, maar voor een lijnvolger kan het wel interessant zijn. Het D-aandeel gaat dempend werken. Opgelet, zorg er wel voor dat de "verandering van de fout" effectief veroorzaakt wordt door het proces, en niet door ruis op het signaal ! In dat geval is een D-aandeel nutteloos en verslechtert alleen maar de zaak.

De positie regeling

Nu dat we perfect onze snelheid onder controle hebben, is het ook mogelijk om op een snelle en nauwkeurige manier een bepaalde afstand af te leggen. Als we vanuit stilstand vertrekken, moeten we eerst versnellen tot aan de gewenste snelheid. Daarna moeten we tijdig afremmen, zodat we op de juiste positie tot stilstand komen. Als we deze gewenste snelheid in een grafiek uittekenen, zien we een typisch "trapezium" profiel.

Trapzium.png

We starten dus vanuit stilstand op tijdstip 0.De rode kromme heeft een versnelling van 20 tics/sek, na 1 sek wordt de maximum snelheid bereikt (punt A). Dan wordt er tot in punt B met een vaste snelheid gereden. Vanaf punt B wordt er afgeremd totdat we in punt C stilstaan. Deze snelheden zijn onze Gewenste snelheid, Soll_speed !. We moeten in ons programma dus steeds de gewenste snelheid (Soll-Speed) aanpassen, en wel zo dat we aan volgende regels voldoen :

A. De versnelling en afremming zal volgens een instelbare waarde verlopen (a= aantal tics/sek²)

B. De maximale gewenste snelheid zal ook ingesteld worden.

C. De afremming (punt B) zal zodanig gebeuren dat we eindigen op de gewenste positie !

Regeltje A en B kunnen vrij eenvoudig bereikt worden : daar we toch al elke 100 ms onze PID-loop doorlopen, kunnen we in deze loop ook vrij eenvoudig nog onze Soll-speed aanpassen. Hierin is Max_speed dus de gewenste maximum snelheid en Acceleration is de versnelling  :

If (PID_Timer>99){ If (Soll_speed<Max_speed) Soll_speed=Soll_speed+Acceleration;PID_Timer=0;}

Regel C is wat lastiger, immers vanaf welke afgelegde afstand moeten we gaan afremmen zodat we uiteindelijk stilstaan op de gewenste positie ? Hier moeten we de formule toepassen van de eenparig versnelde beweging. Deze geeft het verband tussen tijd, afstand en versnelling indien we een eenparig versnelde/vertraagde beweging hebben. Deze afstand is gelijk aan de 0.5*(Max_speed)²/Acceleration. Stel dat de Max_speed 20 Tics/sek, de afremming = 20 Tics/sek², dan zal de afgelegde afstand 10 Tics bedragen. Indien de afremming = 10 Tics/sek², dan bedraagt deze afstand 20 Tics bedragen ! We hoeven dus alleen maar deze remafstand te berekenen, om te weten vanaf welk punt we onze Soll_Speed terug moeten afbouwen !! In de praktijk neem je nog een zekere veiligheidsmarge extra, en leg je de laatste Tics af tegen kruipsnelheid.

Rechtuit rijden

Nu we van beide wielen de afgelegde weg meten, kunnen we een stuk rechtuit rijden. Als we beide motoren met gelijk vermogen aansturen, zal de robot ongeveer rechtuit rijden. Als we op de tellers van de beide wielen letten, zullen we echter zien dat dit niet precies gelijk op gaat. Dit wordt veroozaakt doordat de ene motor net iets lichter loopt dan de andere, of doordat deze net iets minder wordt belast en het gevolg is dat de rijrichting langzaam verandert en daarmee het verschil tussen de twee tellers oploopt. Als we rechtdoor willen rijden (= de rijrichting constant willen houden), moeten we de motoren dus bijregelen om een verschil tussen de twee teller te compenseren. Als je een PID-regeling hebt om de snelheid van elke motor te regelen, zal je robot redelijk goed rechtdoor rijden indien beide snelheden (soll_speed_links / soll_speed_rechts) gelijk zijn. Het zal echter nog niet perfect zijn, omdat er nog steeds een klein verschil in tics tussen linker en rechter wiel kan zijn. Om dit te vermijden, kan je dit verschil echter ook weer in de regeling opnemen. Ik heb ervoor gekozen om het verschil L/R Tics gewoon mee te tellen in de fout van de PID-regeling :

int16_t PD_compass;  //corrigeren richting met E-compass, term wordt opgeteld bij "error" van PID !!
int16_t P_compass;	//Absolute fout in tics afwijking Links/rechts
int16_t e_compass ; //Verschil in tics tussen links en rechts, update in ISR encoders 
int16_t e_compass_soll ; //gewenste richting van robot			
P_compass = e_compass_soll-e_compass;				
if (compass==TRUE)	//alleen corrigeren indien gewenst
	PD_compass = P_compass*0.3; //Versterkingsfactor van richtingsfout = 0.3
else PD_compass=0;
	#define LIMIT_PD 40		//maximale fout beperken, anders teveel overshoot
	if (PD_compass>LIMIT_PD) PD_compass = LIMIT_PD;			
	if (PD_compass<-LIMIT_PD)PD_compass=-LIMIT_PD;		
// Errorterm left motor speed control:
	int16_t error_left = mleft_desired_speed - mleft_speed+PD_compass;//P-fout
// Errorterm right motor speed control:
	int16_t  error_right = mright_desired_speed - mright_speed-PD_compass;//P-fout	

Indien er nu een afwijking optreedt tussen werkelijk richting (e_compass) en de gewenste richting (e_compass_soll) zal de ene motor sterker en de andere motor zwakker worden aangestuurd. In de praktijk blijkt dit bij mijn robot uitstekend te werken. Het verschil in tics tussen L/R blijft bij alle snelheden zeer klein (<3). Bij deze code moet wel de diameters van je wielen gelijk zijn aan elkaar. Immers, de regeling baseert zich op het feit dat één tic links en rechts ook gelijke afstanden links en rechts zijn !

We kunnen op deze manier een programmatje maken dat de robot bijvoorbeeld 4 meter rechtuit laat rijden en dan laat stoppen. En als dit werkt, pak dan een meetlint en meet de gereden afstand nauwkeurig op. Kijk vervolgens naar de waarde van de tellers in de robot (die geven waarschijnlijk aan dat er iets meer dan 4 meter is gereden). Met deze gegevens kun je de eerder berekende waarde van 0.037 mm /stap controleren en zo nodig aanpassen (calibreren).

Rijden en draaien

We hebben gezien dat het verschil tussen de twee tellers de rijrichting bepaalt en dit kunnen we niet alleen gebruiken om rechtuit te rijden, maar ook om te draaien. Om te bepalen welk verschil overeenkomt met een graad draaiing, moeten we - buiten de eerder bepaalde stapgrootte - de wielbasis van de robot weten. Als we aannemen dat onze robot een wielbasis heeft van 150mm en we willen de robot een volledige circel laten draaien door te rijden met het rechter wiel terwijl het linker wiel stil staat, dan legt het rechter wiel een afstand af van 2 * pi * 150 = 942mm. De teller van het rechter wiel wordt daarmee 942 / 0.037 = 25459 verhoogd, ofwel 25459 / 360 = 70.7 stappen per graad.

Met al deze gegevens kunnen we de robot een vierkantje laten rijden: 4 meter vooruit, 90 graden draaien, 3 meter vooruit, 90 graden draaien, en zo voort totdat we weer op het startpunt staan. En datzelfde kunnen we ook doen door na 3 meter -90 graden te draaien, ofwel de andere kant op rijden. Ook dan zijn we na 4 keer weer op het startpunt. Althans... in de buurt van het startpunt.

Nauwkeurigheid

We hebben hiervoor gerekend met stappen van 1/30 mm en je zou verwachten dat je dus op deze manier heel precies weet waar je bent. Helaas zijn er wat zaken die roet in het eten gooien. Zaken die allerlei kleine afwijkingen veroorzaken en doordat bij odometrie de nieuwe positie steeds bepaald wordt op basis van de voorgaande, wordt de fout steeds groter. De afwijkingen (fouten) kunnen we verdelen in twee groepen in systematische fouten - fouten die (gemiddeld) steeds dezelfde afwijking geven en dus voorspelbaar zijn - en niet-systematische fouten - fouten die niet voorspelbaar zijn. In UMBmark worden de volgende fouten onderkend:

1. Niet-systematische fouten:

  • Rijdeon over niet-vlakke vloer
  • Over 'objecten' op de vloer rijden
  • Wiel-slip door:
    • gladde vloeren
    • te grote versnelling
    • snel draaien (slippen)
    • Externe krachten ('interactie met externe lichamen' ofwel botsen)
    • Interne krachten (bijvoordeeld van het zwenkwiel)
    • doordat het wiel niet op 1 enkel punt contact maakt met de vloer

2. Systematische fouten:

  • Beperkte encoder resolutie
  • Beperkte encoder sampling rate
  • Ongelijke wiel-diameters
  • Gemiddelde van beide wiel-diameters wijkt van de nominale diameter
  • Foutieve uitlijning van de wielen
  • Onzekerheid over de effectieve wielbasis (doordat het wiel niet op 1 enkel punt contact maakt met de vloer)


Een hele lijst met mogelijke fout-bronnen, die allen de nauwkeurigheid van de plaatbepaling nadelig beinvloeden. De eerste twee bij niet-systematische fouten zijn erg vervelend: ze kunnen grote invloed hebben op de naukeurigheid van odometrie en je kunt er niets aan doen. Gelukkig rijden onze robotjes - zeker tijdens de wedstrijden - op een mooie vlakke baan en hebben we van deze fouten weinig last. De derde fout - wiel-slip - is ook en vervelende, maar daar dat heb je zelf voor een belangrijk deel in de hand. Rustig opstrekken en stoppen, nog rustiger draaien en zorgen dat je zwenkwiel soepel loopt - en liever nog een rollertje gebruiken dan een zwenkwiel - voorkomt veel ellende.

De eerste twee systematische fouten zijn encoder resolutie en sampling rate. De eerste is gevoelsmatig wel duidelijk - als je 1 puls per 4 cm krijgt, kun je niet nauwkeurig je rijrichting bepalen. Helaas wordt in het paper niet ingegaan wat de vereiste resolutie is. De vereiste sampling rate is eerder in dit artikel besproken en mag duidelijk zijn dat een te lage sampling rate fouten geeft. De volgende twee fouten hebben betrekking op de afmetingen van de wielen en zullen een stabiele invloed hebben op de fout. Dat geldt niet voor de laatste twee - foutieve uitlijning en effectieve wielbasis. Zeker als minder dan 1 wielomwenteling wordt gereden is het effect van deze fouten niet in te schatten en bovendien zal de fout niet bij iedere wiel-omwenteling gelijk zijn. Maar we kunnen wel bepalen wat over lange afstand de gemiddelde fout is en daarmee de nauwkeurigheid van de odometrie verbeteren. In het paper wordt aangegeven dat 'ongelijke wieldiameters' en 'onzekerheid over de effectieve wielbasis' de belangrijkste systematische foutbronnen zijn en beschrijven een methode om voor de systematische fouten te corrigeren.

Vierkantjes rijden

Voor de calibratie van Odometrie is door Borenstein et. al. een methode ontwikkeld die uitgebreid wordt omschreven in 'UMBMark - A Method for Measuring, Comparing, and Correcting Dead-reckoning Errors in Mobile Robots' (UMBMark). In dit artikel wordt onderbouwd dat er twee bronnen zijn die fouten veroorzaken in de orientatie:

  • Fouten in de vermeende wielbasis, bijvoorbeeld doordat de wielen (gemiddeld) niet precies in het midden de grond raken of omdat de wielen niet helemaal recht draaien.
  • Verschillen in wiel-diameter, waarbij een fout van 0.1% al een behoorlijke afwijking geeft bij het rijden van een ‘rechte’ lijn.

Daarbij komt dat de impact van een fout in de rijrichting aanzienlijk is – als je in een andere richting rijdt, kom je nu eenmaal ergens anders uit dan de bedoeling is. En omdat er twee bronnen zijn die beide invloed hebben op deze fout, is het niet eenvoudig deze te calibreren. Tenzij… je de procedure volgt die het genoemde document is omschreven. Paul Bouchier heeft de methode beknopt beschreven in http://www.dprg.org/articles/2009-02a/. Het is dus niet nodig om dit nog een keer te doen en we beperken ons hier dan ook tot de toepassing van deze methode.

We kunnen deze methode toepassen als we de robot - op basis van odometrie - rechtuit kunnen laten rijden en dat we deze een bocht van +/- 90 graden kunnen laten draaien. Dit alles in een rustig tempo om wielslip en dergelijke te voorkomen. Vanaf hier doen we de volgende stappen:

Stap 1: calibreer de afstand.

Dit is optionele stap, die niet nodig voor de calibratie van de rijrichting. Maar meestal wil je de afstand wel calibreren en in dat geval is het handig om dit als eerste te doen. De procedure is al beschreven in de inleiding:

  • Zet de robot op een startpunt en bepaal de waarden van de beide encoders.
  • Laat de robot een stuk rijden in een rechte lijn over een vlakke vloer.
  • Meet de gereden afstand en bepaal opnieuw de waarden van de beide encoders.
  • Bereken de stapgrootte van de encoders door afstand te delen door het aantal encoder-stappen.

Opmerking: als het goed is, is het aantal stappen van de encoder (ongeveer) gelijk, want ‘in een rechte lijn rijden’ op basis van odometrie is gedefinieerd als ‘de encoders lopen gelijk op.

De naukeurigheid van deze calibratie is groter als de afstand die de robot rijdt groter wordt en je slip op de wielen voorkomt doordat de robot rustig start en stopt. Ook kun je de meting meerdere keren herhalen en calibreren met het gemiddelde van de metingen.

Stap2: Rijd een vierkant van lengte L met de klok mee (CW)

Na de proef meet je het verschil tussen start zoals aangegeven in onderstaande figuur. Ook hier geldt dat rustig rijden van een flink vierkant over een vlakke vloer (waarvoor je dus behoorlijk wat ruimte vraagt) de naukeurigheid ten goede komt. En ook hier kun je meerdere metingen doen en het gemiddelde gebruiken om een nauwkeuriger getal te krijgen.

Stap 3: Rijd een vierkant van lengte L tegen de klok in (CCW)

Ook hier meet je weer het verschil tussen start en eind.

In onderstaande figuur is het traject van een robot weergegeven die een behoorlijke afwijking van de wielgrootte, waardoor hij geen rechte lijn rijdt. Daarnaast draait hij niet precies 90 graden. Het effect is dat hij bij een blokje met de klok mee (CW) op een ander punt eindigt dan bij een blokje tegen de klok in (CCW).

Vierkant.jpg

Na iedere proef meten we de afwijking van het eindpunt ten opzichte van de startlijn. In ons voorbeeld is CW een positieve waarde, terwijl CCW een negatieve waarde is. De lengte L hebben we zelf ingesteld en de nauwkeurigheid hiervan is minder van belang. Het rekenwerk laten we over aan een [Excel sheet]. De geel gemarkeerde velden vul je zelf in, de groene velden zijn de uitvoer die je kunt gebruiken. Naast L, CW en CCW vullen we de wielbasis van de robot in. De eenheid (mm, cm) maakt niet uit, zolang deze maar voor alle waarden gelijk is. Het resultaat zijn twee correctiefactoren: C1 voor de hoek van 90 graden en C2 voor het rechtuit rijden. Onder iedere correctiefactor kun je eventueel de huidige constante uit je programma invullen en in de kolom daarachter wordt twee gecorrigeerde waarden weergegeven. Welke van de twee je moet gebruiken (correctiefactor omhoog of omlaag) hangt af van de opzet van je programma.

Na het uitvoeren van de correcties kun je de meting herhalen om te bepalen wat de overgebleven afwijking is.

Assenstelsel

We hebben op basis van odometrie vierkantjes gereden door 4 keer een vaste afstand te rijden en vervolgens een vaste hoek (van 90 graden) te draaien. Door de symetrie van het vierkant is dit overzichtelijk en kun je je eenvoudig een voorstelling maken van het patroon. Maar stel dat de opgave is:

  • rij 90 cm rechtuit
  • draai 15 graden en rij 40 cm vooruit,
  • draai -80 graden en rij 60 cm vooruit

Nu de afstanden verschillend zijn en de hoeken geen 90 graden, is het ingewikkeld om te in te schatten waar de robot is na het uitvoeren van deze drie stappen. Dit komt omdat we de verplaatsing beschrijven als een afstand + richting en die kun je niet eenvoudig bij elkaar optellen. Maar gelukkig kunnen we dit aanpakken door de positie van de robot na iedere verplaatsting om te rekenen naar een punt in het assenstelsel.

Laten we de roborama-baan als uitgangspunt nemen voor ons assenstelsel. De hoek links-onder nemen we als oorsprong (nulpunt), de lange zijde nemen we als x-richting en haaks daarop is de y-richting. Als eenheid nemen we cm. In onderstaande figuur staat de robot precies in het midden van vak A, dus 30 cm van de achterwand en 60 cm van de lange zijde. Dit punt kunnen we aanduiden als (30, 60) - de x-waarde van dit punt is 30 en de y-waarde 60. We stellen de robot eigenlijk voor als een punt op de kaart en dat punt ligt in het midden tussen de twee wielen. De afmetingen van de robot laten we in dit verhaal buiten beschouwing, maar het mag duidelijk zijn dat je 400 punten misloopt als je naar punt (1,1) rijdt... Buiten de plaats van de robot, willen we ook weten in welke richting de neus van de robot wijst. In ons voorbeeld gaan we ervan uit dat deze richting vak B wijst en dat definieren we als 0 graden.

(roborama baan met robot op 30, 60 en heading 0; aantal hoekpunten + robotpositie voorzien van coordinaten)


We kunnen nu bovenstaande opgave uitvoeren:

(plaatje met verplaatsingsvectoren en coordinaten) De pijlen geven de drie trajecten aan en van de eindpunten van de trajecten zijn de coordinaten weergegeven. Zoals we al eerder gezien hebben, is de startpositie (30, 60). Als we vervolgens 90 cm rijden in de x-richting wordt de x-positie dus verhoogd, terwijl de y-positie niet verandert en komen we op punt (120, 60). De volgende stap is 15 graden draaien en weer 40 cm rijden. Let op dat 15 graden draaien - een positief getal - betekent dat we tegen de klok in draaien. We gaan dus 40 cm rijden onder een hoek van 15 graden. De bijdrage van die 40 cm aan x en y is niet zo te zien, maar kunnen we bepalen met de volgende twee formules:


dx = afstand * cos(koers)

dy = afstand * sin(koers)


dx = 40 * cos(15) = 40 * 0.966 = 38.64

dy = 40 * sin(15) = 40 * 0.259 = 10.35


De verplaatsing (delta, vandaar de 'd') is dus afgerond (39, 10). Dit tellen we op bij de startpositie van deze beweging (120, 60) en zo komen we uit op (159, 70).

De laatste stap was -80 graden draaien (met de klok mee, dus) en 60 cm rijden. Het eerste wat we moeten doen (en wat waar we hierboven even buiten beschouwing hebben gelaten) is de nieuwe koers bepalen. De rijrichting (koers) van de robot is bij aanvang 15 graden en we gaan nu -80 graden draaien. De nieuwe rijrichting wordt nu 15 + (-80) = -65 graden. De delta-x en delta-y kunnen we met dezelfde formules bepalen:


dx = 60 * cos(-65) = 60 * 0.423 = 25.38

dy = 60 * sin(-65) = 60 * -0.906 = -54.36


De nieuwe positie wordt (159, 70) + (25, -54) = (184, 16)


(implementatie - radialen, resolutie)

Odometrie formules voor willekeurige verplaatsingen

Indien de robot louter rechtdoor rijdt, of ter plaatse draait, zijn de formules voor plaatsbepaling in een XY (Cartesiaans) assenstelsel voor de hand liggend. Maar wat als onze robot eerder willekeurige verplaatsingen maakt ? Welnu, dit kan ook op een vrij eenvoudige manier opgelost worden. Het basisidee ist dat elke verplaatsing opgedeeld wordt in kleine stukjes. Omdat deze stukjes klein zijn, kan en mag men veronderstellen dat de beweging eigenlijk terug een rechte lijn is. Op de volgende figuur ziet men zo een vereenvoudiging :

Diff drive.png

De eigenlijk afgelegde weg van de robot is de blauwe boog. De lengte van deze boog is gelijk aan de gemiddelde afstand die de 2 wielen afgelegd hebben. Dit geldt voor alle denkbare bewegingen (één wiel staat stil, het andere beweegt, de twee wielen draaien elk in een andere richting of de twee wielen draaien in dezelfde richting). In ons rekenmodel gaan we nu deze boog vervangen door een rechte lijn (de paarse lijn), die dezelfde lengte heeft, en waarvan de richting gelijk is aan de "gemiddelde" hoek. Dit kunnen we eenvoudig berekenen door (Eindhoek-beginhoek)/2. De beweging wordt dus voorgesteld door een vector met een gekende lengte en een gekende hoek. Het is duidelijk dat we hier een fout maken tov de werkelijk afgelegde boog. Maar, hoe kleiner de boog, hoe kleiner ook deze fout. Het blijkt nu dat bij een voldoende kleine resolutie van de encoders, en een voldoende kleine verplaatsing voor een herberekening, deze "wiskundige" fout volledig te verwaarlozen is tov andere invloeden. Overigens vond ik hier een mooie simulatie van de invloed van deze vereenvoudiging : MotionApplet

Hier een voorbeeld van mijn code :

void odometrie (void){ 
		
static int16_t e_compass_old;	//waarde e_compass bij vorige doorloop (encodertics)	
delta_compass=e_compass - e_compass_old;	//hoekverdraaing tov vorige doorloop (encodertics)
//Allen bij voldoende grote afstand of hoekverdraaing berekenen we opnieuw de positie	
if((abs(delta_distance) > 200)||(abs(delta_compass) > 100))){	
	int32_t delta_x = 0;								 	int32_t delta_y = 0;
	double hoek= ((e_compass_old+e_compass)/2/E_COMPASS_RESOLUTION)*M_PI/180;//hoek omzetten naar radialen	
	delta_x = delta_distance*cos(hoek);	
	delta_y = delta_distance*sin(hoek);	 	total_distance=total_distance+delta_distance;	//totaal afgelegde baan-afstand
	delta_distance=0;		//resetten van delta_distance ; 
	x_pos=x_pos+delta_x;	//aktuele x_pos updaten *ENCODER_RESOLUTION/2
	y_pos=y_pos+delta_y;	//aktuele y-pos updaten *ENCODER_RESOLUTION/2
	e_compass_old=e_compass;
	}
}

De formules zijn dus relatief eenvoudig. Bij mijn robot wordt de variabele "delta_distance" en "e_compass" voortdurend in de ISR van de encoders actueel gehouden. In de Main loop wordt dan telkenmale deze "Odometrie" functie doorlopen. Indien er een voldoende grote afstand of hoekverandering tov de vorige berekening is, wordt er opnieuw een actuele positie berekend. Ik heb er ook voor gekozen om zoveel mogelijk met integers te rekenen, om de processor weinig te belasten.

Odometrie toepassing in mijn discovery robot

Hieronder de link naar een video waar odometrie wordt toegepast : de robot rijdt een slalom rond blikken (Roborama opdracht), de positie van de robot wordt elke 100 ms doorgeseind via Bluetooth naar een pc, waar de afgelegde weg zichtbaar wordt gemaakt.