Door: Ad van der Weiden



Dovnload 36.37 Kb.
Datum27.09.2016
Grootte36.37 Kb.
Sensorpakket voor een mobiele robot
Door: Ad van der Weiden
Momenteel ben ik bezig met het bouwen van een sensorpakket voor gebruik met de RoboInterface van FischerTechnik. Dit pakket is ook geschikt voor andere mobiele robots en daarom misschien interessant voor de lezers van dit blad. Het pakket is nog niet helemaal af maar ik wilde mijn ideeen toch al met jullie delen omdat ik op dit moment door een gebroken been weinig anders kan dan lezen en schrijven.
Bij het ontwerp ben ik geinspireerd door een aantal artikelen in Elektor [1] en door ideeen van andere FischerTechnik fans. Voor wat betreft de FischerTechnik kant is dat vooral de interfacing naar de RoboInterface vio de I/O Extension bus en eventueel naar andere interfaces via de RS232 interface. Via de RS232 interface heb ik eerder al een gemodificeerde versie van de CMUCAM3 op de RoboInterface aangesloten. De CMUCAM3 paste precies in een FT cassette van 60x60x30 mm. Daarom wilde ik ook dit sensorpakket in zo’n cassette proberen te proppen.
De oorspronkelijke wensen waren als volgt:

  1. Aansturing via I/O Extension bus

  2. LCD display (Starburst display van VOTI)

  3. Kompas sensor (CMPS03)

  4. Versnellingssensor (MMA7260QT) voor standbepaling en berekening van snelheid en positie

  5. Ingangen voor optische sensoren (8 CNY70 of 4 kwadratuur sensoren uit oude muizen)

  6. Digitale ingangen voor schakelaars (4 stuks)

  7. RS232 interface voor verbinding met andere FT interfaces of een computer

  8. EEPROM voor log-data of programma’s die naar een FT interface ge-upload kunnen worden.

Door ruimtegebrek kan ik niet alles in een standaard cassette realiseren, de ingangen voor de optische sensoren en de schakelaars zullen dus waarschijnlijk sneuvelen, misschien maak ik daar later nog een uitbreiding voor. De FT I/O Extension was gebaseerd op een ATMEGA met nog wat extra logica. Ik vond dit te ingewikkeld en ben uitgegaan van de versnellingssensor uit Elektor. Dit projekt is gebaseerd op een Freescale microcontroller HCS08 (MC9S08QG8CPBE, gratis sample) en een 3-assige versnellingssensor (ook van Freescale). Als je de printjes besteld bij Elektor krijg je er 2 sensors bij die bovendien al op een printje gesoldeerd zijn.


De microcontroller gebruik ik eigenlijk als een grote protokol converter. Ik heb namelijk te maken met de volgende interfaces:

  • I2C, voor de LCD, CMPS03, EEPROM en de I/O uitbreiding voor de digitale ingangen;

  • SPI, voor de FT I/O Extension

  • RS232, voor de RS232 interface

  • Analoog, voor de versnellingsopnemer

De Freescale microcontroller kan ondanks zijn slechts 16 pennen al deze funkties tegelijk vervullen (in hardware)! Voor niet FT toepassingen zou men dus de SPI interface kunnen gebruiken voor communicatie met de centrale processor maar de software moet op dit punt dan wel behoorlijk gewijzigd worden. Men zou ook op de I2C bus kunnen aanhaken maar de Freescale is al busmaster, het wordt dan dus waarschijnlijk een multi-master systeem en ik heb geen idee wat dat voor complicaties geeft.


In het vervolg van dit stukje wil ik me concentreren op de signaalverwerking van het kompas en de versnellingsopnemer. De software is gebaseerd op Application Notes van Freescale voor de versnellingsopnemer voor zowel standbepaling [2] als snelheids- en positieberekening [3].
Tilt
De gebruikte sensor is een 3-assige sensor, de x en y assen geven de voorwaartse en de zijwaartse versnelling aan, de z-as geeft de neerwaartse versnelling aan, oftewel de valversnelling. Als men de sensor kantelt in pitch of roll richting zullen componenten van de zwaartekracht vector in de x en y as terecht komen. Met behulp van wat goniometrie kunnen dan de pitch en de roll berekend worden. De yaw (draaiing om de verticale as) kan zo niet bepaald worden maar daar kan het kompas uitkomst brengen. Om deze meting betrouwbaar uit te voeren moet de robot stilstaan of eenparig bewegen omdat anders de versnellingen van de robot de metingen verstoren.
In de Application Note [2] vond ik de volgende formule voor de pitch:

en in een ander document [7] deze formules zijn equivalent. Voor de roll staat in de Application Note [2]:

en in een ander document deze formules zijn niet equivalent, slechts als er alleen sprake is van roll of van pitch komen ze overeen. Als er sprake is van zowel pitch als roll geeft alleen de tweede formule het juiste resultaat. Het is van belang om de rotaties in de juiste volgorde uit te voeren. Eerst wordt de pitch uitgevoerd, draai het voertuig bv. eerst 45º om z’n wielas (rijdt vrij steil omhoog) en rol het vervolgens 90º om z’n lengte as. In deze volgorde zijn de g componenten [0,7 0,7 0,0], zou men eerst de rol uitvoeren en daarna de pitch dan werden de componenten [0,0 1,0 0,0], de formules geven dan een pitch van 0 wat niet overeenkomt met wat we gedaan hebben.

De volgende opgave is om deze formules te berekenen. Als dit willen berekenen met de gebruikelijke C bibliotheken is het geheugen van de microcontroller gauw vol, er blijkt echter een eenvoudigere methode die werkt met integers en qua snelheid vergelijkbaar is met vermenigvuldigen. De methode heet CORDIC en op Internet is alles erover te vinden [4]. Met CORDIC in vectoring mode kunnen berekeningen als arctan en pythagoras heel eenvoudig uitgevoerd worden.


#define ITERATIONS 15

int term[ITERATIONS];


void vectoring(int &x, int &y, int &z) //C++ style call by reference

{ if (x<0) //CORDIC werkt alleen in het rechter halfvlak

{ x *= -1;

y *= -1;


z += 18000;

}

for (int i = 0; i < ITERATIONS; i++) //het eigenlijke CORDIC proces



if (y > 0)

{ int t = x;

x += y>>i;

y -= t>>i;

z += term[i];

}

else



{ int t = x;

x -= y>>i;

y += t>>i;

z -= term[i];

}

}
Het array ‘term[]’ moet wel eerst gevuld worden maar dit kan met een lijst constanten. Hieronder staat C code waarmee de constanten berekend kunnen worden. Merk overigens op dat de CORDIC loop alleen optel/aftrek en schuif operaties bevat.


for (int i = 0; i < ITERATIONS; i++)

term[i] = 18000/M_PI*atan(1.0/(1<
De table is hier zo gekozen dat hoeken worden gerepresenteerd in honderdste graden, de integer 18000 komt dus overeen met een hoek van 180,00 graden. Men is vrij een andere representatie te kiezen zolang het maar in 16 bits past. Wil men meer of minder bits dan moet het aantal iteraties worden aangepast.
De CORDIC processor heeft 3 argumenten: x, y en een hoek. Hij roteert de x,y vector over de hoek. In vectoring mode wordt de vector geroteerd tot hij langs de (positieve) x-as ligt. De hoek fungeert dan als uitgang en geeft de hoek aan waarover de vector gedraaid is. Omdat de vector na rotatie op de x-as ligt (y=0), is de x waarde noodzakelijkerwijs de lengte van de x,y vector. Althans dat zouden we graag willen maar de eenvoud van het proces heeft z’n prijs en die prijs is dat bij iedere iteratie de vector iets langer wordt. Omdat het aantal iteraties constant is, is ook de totale lengteverandering constant.en bedraagt bij 16 bit een factor 1, 647. Dit is wel iets waar men rekening mee moet houden. Om nu bv. de pitch te berekenen uit de versnellingsvector doen we de volgende berekeningen:
int dummy = 0;

vectoring(Ay, Az, dummy);

//Az=0, Ay=1,647 * SQRT(Ay*Ay+Az*Az), dummy = atan(Az/Ay)

vectoring(Ax, 0, dummy);

//Ax = 1,647*Ax, operatie om Ax te vermenigvuldigen met de CORDIC gain

int pitch = 0;

vectoring(Ay, Ax, pitch);

// pitch = atan(y/x)


De CORDIC processor kan ook ingezet worden voor het berekenen van sinus, cosines, arcsinus, arccosinus en nog veel meer functies. Op die manier zou men dus ook de roll kunnen berekenen. We kunnen de sinus-formule voor de roll echter omschrijven naar een tangens-formule, deze blijkt ook nog eens veel eenvoudiger te zijn: roll = arctan(Ay/Az). Deze is met een enkele vectoring operatie te realiseren.

Dead Reckoning


Een heel andere toepassing van de versnellingsopnemer is dead reckoning, ik heb me laten vertellen door een maritieme vriend dat de nederlandse term ‘gegist bestek’ is. Het komt erop neer dat je uitgaande van je vertrekpunt op basis van koers en vaart uitrekent waar je zou moeten zijn. In ons geval hebben we alleen versnellingsinformatie in x en y richting. In principe zouden we dit kunnen aanvullen met informatie van het kompas maar ik wilde me in eerste instantie houden aan het principe zoals beschreven in de Application Note [3]. De code in de AN is bedoeld voor een muis maar is eenvoudig geschikt te maken voor een robot. De calibratie stap is vrijwel gelijk, het enige verschil is dat wij, vanwege het gebruik als tilt-sensor, ook de z-as gebruik. De z-as geeft echter niet het nulpunt aan maar het 1g punt. Voor het nulpunt kan men beter het gemiddelde van de x- en y-as gebruiken (gelijke drift en spreiding). Dead reckoning kan men zowiezo alleen in het platte vlak gebruiken omdat bij stijgen/dalen of kantelen de zwaartekracht een enorme fout introduceert. Het is voor zover ik weet niet eenvoudig mogelijk om onder die omstandigheden nog goede resultaten te bereiken.

Men zou eventueel het volgende kunnen proberen:



  • Met extra sensoren de pitch en roll bepalen en hiermee de zwaartekracht vector als het ware weer terugdraaien naar de z-as. Als sensoren zijn gyro’s heel geschikt maar men zou ook kunnen experimenteren met twee extra lineaire versnellingsopnemers die iets voor en iets naast de hoofdsensor worden geplaatst. Uit het verschil is dan de draaiing te bepalen.

  • Aangenomen dat de versnellingen voor het verplaatsen een hogere frequentie hebben dan de versnellingen gerelateerd aan de stand zou men op basis van frequentie de versnellingen kunnen scheiden. Ik heb echter geen idee of deze aanname reeel is.

  • Met een geschikt filter (bv. een Kalman filter) de stand van de robot schatten en deze schatting van tijd tot tijd calibreren met de werkelijke stand door de robot tot stilstand te brengen.

Het leuke van het gebruik van versnellingsopnemers t.o.v. bv wielsensoren is dat men ook nog een redelijke schatting van de positie krijgt als men de robot oppakt, op een ander voertuig of in de lift zet.


De bovenstaande problemen overwegende heeft het niet veel zin om te proberen de vertikale positie te bepalen. Alle robots die ik tot dusver gezien heb in RoboBits waren gemaakt voor een vlakke vloer.

De Freescale AN maakt voor de berekening van de snelheid en positie gebruik van de trapezium integratie. Er wordt gesuggereerd dat deze veel nauwkeuriger is dan de simpele rechthoek integratie. In de praktijk valt dat mee, in beide gevallen worden gewoon alle samples opgeteld alleen worden bij de trapezium integratie het allereerste en het allerlaatste (dus het huidige) sample maar voor de helft meegeteld. De code in de AN berekent de snelheid als volgt: v1 = v0 + a0 + (a1 – a0)/2 dit is uiteraard eenvoudiger te schrijven als: v1 = v0 + (a0 + a1)/2 dat scheelt vast weer een optelling. Het is ook eenvoudig in te zien dat het n-de sample vn = v0 + a0/2 + a1 + a2 + ... + a(n-1) + an/2. Daarom kunnen we eens kijken of simpele rechthoek integratie niet net zo goed is als trapezium integratie. Ik ga geen uitspraak doen welke methode ‘beter’ is maar bekijk alleen de onderlinge verschillen. Ik zal u de wiskunde besparen maar als we ervan uit gaan dat aan het begin de versnelling, snelheid en positie nul zijn, kom ik tot de volgende resultaten:

Vtrap(nT) = Vrect(nT) - ½Ta(nT) = Vrect((n-½)T)

Strap(nT) = Srect(nT) – T Vrect(nT) + ¼T²a(nT) = Srect((n-1)T) + ¼T²a(nT)

Het komt erop neer dat bij de berekening van de snelheid er een verschuiving van een halve sample periode optreedt en bij bepaling van de afstand een hele sample periode. Het lijkt me dat deze verschillen voor alle praktische toepassingen te verwaarlozen zijn maar puristen kunnen altijd de rechthoek integratie gebruiken vanwege het rekenwerk en dan een kleine correctie aanbrengen om het resultaat van de intuitief betere trapezium integratie te bereiken.

De vermenigvuldiging met de sampletijd T om van versnelling naar snelheid en van snelheid naar afstand te komen wordt natuurlijk niet iedere periode uitgevoerd maar pas bij het weergeven op een display o.i.d. men kan dan gelijk de gevoeligheid van de sensor (800mV/g) en de A/D conversie (3,3V/1023) verdisconteren. De berekeningen die uitgevoerd moeten worden zijn dus simpelweg:

v += a;

s += v;


en dat is heel wat eenvoudiger dan de code van Freescale.

Freescale doet nog twee extra dingen om de nauwkeurigheid te verbeteren, ten eerste gebruiken ze 64-voudige oversampling die ze simpelweg middelen. Dit vermindert uiteraard de ruis maar middeling is niet zo geschikt voor het onderdrukken van hogere frequenties. Een echt decimatiefilter zou beter zijn maar vergt veel rekenwerk. Ten tweede hebben ze een discriminatievenster van ±3 gebruikt zodat hele kleine versnelling die waarschijnlijk ruis of drift zijn niet meetellen in het resultaat. Of dit veel zin heeft weet ik niet, ruis en drift worden al onderdrukt dus ik zal hier wat mee moeten experimenteren. Als laatste gebruikt Freescale een ‘movement_end_check’, dit houdt in dat wordt gekeken of de versnelling 25 keer achter elkaar nul is (na toepassing van het discriminatievenster). Als dat het geval is wordt aangenomen dat het object stil staat en wordt de snelheid op nul geforceerd. Dit is bij een muis wel handig maar bij een robot weten we meestal wel of hij rijdt of stilstaat (tenzij we hem oppakken natuurlijk).


Kompas
Het gebruik van het kompas is eigenlijk vrij recht-toe-recht-aan. Het enige hardware probleem is dat het kompas op een 5V I2C bus aangesloten moet worden en de rest van de sensoren op een 3,3V bus, gelukkig bracht een Application Note van Philips [8] uitkomst. Een andere Application Note van Philips [5] geeft inzicht in de processing die nodig is om tot zinvolle waarden te komen. Gelukkig voor ons is het meeste hiervan al in de processor op de CMPS03 module gestopt. Wel is het een voorwaarde dat het kompas absoluut horizontaal wordt gehouden omdat anders de compensatie voor de inclinatie (of dip) niet meer klopt. In de AN staat een methode beschreven om als men de tilt van het kompas kent, de aanwijzing te compenseren. Dankzij de versnellingssensor kennen we de roll en de pitch dus het begin is er. Volgens de AN moeten we echter over alle drie de componenten van het magnetisch veld beschikken en dat is wel een probleem. Ten eerste heeft de CMPS03 maar twee sensoren en de meetwaarden daarvan zijn beschikbaar in registers 4-7 maar het is niet duidelijk wat die waarden precies betekenen. Het tweede probleem is het gebrek aan de z-as magneetsensor.

Intuitief zou je zeggen dat er toch ruim voldoende informatie is. Van de magnetisch veld vector is voor een bepalde plaats op aarde alles bekend, de stand van het kompas t.o.v. de vertikaal is bekend, alleen de orientatie (azimuth) ontbreekt dus nog, dat zou met 1 of 2 sensoren toch moeten kunnen. Ik ben gelukkig niet de eerste die dit bedenkt en na enig zoekwerk vond ik een aantal oplossingen:



  • Het kompas calibreren voor een aantal tilt hoeken en met behulp van de calibratietabel de azimuth compenseren.

  • Filteren van de azimuth gebaseerd op het gegeven dat de tilt een ruis karakter heeft

  • Het schatten van de ontbekende veldcomponent gebaseerd op het feit dat totale veldsterkte constant is en de andere twee componenten worden gemeten.

Ik heb gekozen voor de laatse oplossing omdat die vrij nauwkeurig is omschreven in patent US6836971 en in het artikel van Cho en Park [7]. Of de gebruikte formules helemaal equivalent zijn heb ik niet gecontroleerd. Ik heb [7] gebruikt omdat daar duidelijker instaat hoe ze tot de formules zijn gekomen en ook de locale inclinatie expliciet wordt gebruikt.

We beginnen met het schatten van de magnetische z-component uit de overige beschikbare gegevens (inclinatie λ, roll φ, pitch θ en de magnetisch veldcomponenten X en Y).

Met behulp van deze geschatte waarde kunnen nu gecompenseerde waarden voor de azimuth componenten worden berekend


Azimuth = arctan(b/a) met:

Net als bij de berekening van de tilt hoeken doemen weer schrikbeelden op van en IEEE floating point berekeningen. Gelukkig brengt ook hier CORDIC uitkomst. Na wat manipulaties met de formules blijkt dat we de hele berekening in slechts 7 CORDIC operaties kunnen uitvoeren, 5 voor sinus en cosinus berekeningen, 1 voor compensatie van de CORDIC gain en 1 voor de arctangens. Voor het berekenen van de sinus en cosinus gebruiken we de CORDIC in rotation mode (zie [4]). In rotation mode wordt de ingangsvector gedraaid over de opgegeven hoek, deze hoek wordt in het proces naar 0 geregeld.
void rotation(int &x, int &y, int &z)

{ for (int i = 0; i < ITERATIONS; i++)

if (z < 0)

{ int t = x;

x += y>>i;

y -= t>>i;

z += term[i];

}

else



{ int t = x;

x -= y>>i;

y += t>>i;

z -= term[i];

}

}
De hoek z moet wel tussen plus en min 90 graden liggen. Als je de rotation met de vectoring vergelijkt zie je dat het verschil eigenlijk alleen de conditie van het if statement is. Om de code leesbaar te houden heb ik functies gemaakt voor elk specifiek gebruik van CORDIC.


int cordic_cos(int x, int arg) // x*sin(arg)

{ rotation(x, 0, arg);

return x;

}
int cordic_sin(int x, int arg) // x*cos(arg)

{ int t = 0;

rotation(x, t, arg);

return t;

}
void cordic_gain(int *x, int *y) // x*=1,647 y*=1,647

{ rotation(*x, *y, 0);

}
int cordic_arctan(int x, int y) // arctan(y/x)

{ int phase = 0;

vectoring(x, y, phase);

return phase;

}
De uiteindelijke berekening voor de tilt compensatie wordt dan:


int tiltcomp(int x, int y, int p, int r, int d)

{ //unity scaled to 10000

int sl = 10000*sin(d*M_PI/180.0); //sin lambda (tilt or dip angle)

y = cordic_cos(y, p);

int xt = x;

cordic_gain(&xt, &y);

int a = cordic_cos(xt + cordic_sin(sl, p), r);

int b = cordic_sin(cordic_sin(x, p) + sl*fact, r) - y;

return cordic_arctan(a, b);

}
In de bovenstaande functie komen nog twee berekeningen voor die door constanten moeten worden vervangen, dat zijn de berekening van sin λ en de vermenigvuldiging met ‘fact’, fact is de CORDIC gain (1,647). Na die wijziging bevat de gehele procedure alleen nog maar optellen en aftrekken en schuifoperaties.


Conclusies
Hoewel nog lang niet af, zijn de meeste componenten voor het sensorpakket beschikbaar, zowel in hardware als in software. Op een experimenteerprint heb ik de aansturing van de I2C bus naar LCD en EEPROM, de SPI bus naar de RoboInterface en de RS232 aansluiting voor elkaar. Ook de versnellingssensor is aangesloten via een verloopprintje. De software voor de aansturing van de bussen en de LCD is ook klaar en getest. Momenteel ben ik bezig met de commando afhandeling (commando’s kunnen komen van FischerTechnik (via SPI of RS232) of van de computer (via RS232). Naar aanleiding van de commando’s worden dan gegevens verstuurd naar de SPI of RS232 bus of naar de LCD. De data acquisitie is op het kompas na bijna klaar (sommige parameters als sample frequentie, middeling en discriminatievenster moeten nog bepaald worden). Zoals hierboven te zien is heb ik ook de code voor de data verwerking al gereed. Daarnaast ben ik nog bezig met een printontwerp in Eagle. Zodra alles gereed is zal ik alles op mijn website publiceren of eventueel een aanvullend artikel insturen.
Referenties
[1] Accelerometer, Elektor april 2007

[2] Freescale AN3461, Tilt Sensing Using Linear Accelerometers

[3] Freescale AN3397, Implementing Positioning Algorithms Using Accelerometers

[4] Ray Andraka, A survey of CORDIC algorithms for FPGA based computers

[5] Philips AN00022, Electronic Compass Design using KMZ51 and KMZ52

[6] Devantech, CMPS03 – Robot Compass Module, datasheet



[7] Cho and Park, Electronic Letters 30-10-2003, Vol.39 No.22, Tilt compensation algorithm for 2-axis magnetic compass.

[8] Philips AN97055, Bi-directional level shifter for I²C-bus and other systems



De database wordt beschermd door het auteursrecht ©opleid.info 2017
stuur bericht

    Hoofdpagina