Lijnvolgen

Lijnvolgen met een differential drive robot, een theoretische benadering

In deze schets zie je de standaard opbouw voor een snelle differential drive lijnvolger. Enkele typische kenmerken :

De sensoren staan steeds ruim voor de wielbasis opgesteld.
Meestal gebruikt men meerdere sensoren over een zeker breedte.
Het zwaartepunt ligt meestal vrij dicht bij de wielbasis.

Wat is de maximale snelheid bij een gegeven bochtstraal ?

De maximale snelheid wordt begrensd omdat de grip van de wielen op de ondergrond begrensd is. Als de zijdelingse kracht hoger wordt dan de grip, gaat de robot onherroepelijk slippen. Deze snelheid kan eenvoudig berekend worden indien we de wrijvingscoefficient kennen en de ligging van het zwaartepunt (CG, center of gravity). Als het zwaartepunt exact op de wielbasis ligt is de formule :

Vmax² = µ.g.R

Hierin is Vmax de maximale bochtsnelheid, µ is de wrijvingscoefficient (typisch 0.8), g de zwaartekracht versnelling (9.81 m/s²) en R de bochtstraal in meter. Onze huidige Roborama baan heeft een kleinste bochtstraal van 0.25 m, Vmax wordt dan : 1.4 m/s ! Helaas ligt het zwaartepunt vrijwel nooit op de wielbasis. In de schets zie je een realistische positie van het zwaartepunt. Het gevolg hiervan is dat de wielen ook nog eens een extra kracht krijgen te verduren om de robot in het spoor te houden ! In de schets zie je dit aangeduid met blauwe pijlen. Ook kan je hier zien dat de verhouding wielbasis tov de afstand van het zwaartepunt-wielbasis eigenlijk de belangrijke parameter is ! Immers, er moet steeds een tegenwerkend koppel zijn (blauwe pijlen), en dit koppel is evenredig met de wielbasis ! De aangepaste formule voor onze Vmax wordt dan (gevoelsmatig, kan iemand dit bewijzen ?):

Vmax² = µ.g.R*(wielbasis-Afstand)/wielbasis

in onze schets is deze verhouding ca 0.6, dan wordt de max. snelheid 1.17 m/s !

Wat is het verband tussen wielbasis, bochtstraal en wielsnelheid ?

De gemiddelde bochtsnelheid is eenvoudig te berekenen : (Snelheid L-wiel + Snelheid R-Wiel)/2. Dit geeft ons dan de snelheid van het middelpunt van de wielbasis. De bochtstraal wordt gegeven door de volgende vergelijking :

               (Snelheid_Links + Snelheid_Rechts)*Wielbasis
Bochtstraal =  --------------------------------------------
               2*(Snelheid_Links-Snelheid_Rechts)

We kunnen grofweg de maximale bochtsnelheid berekenen door de formule in de voorgaande paragraaf (functie van de wrijvingscoefficient µ). Stel, we willen een lijnvolger ontwerpen die een maximale snelheid van 1.4 m/s in een bocht van 250 mm straal moet bereiken. We kiezen voor een wielbasis van 250 mm. Bij deze wielbasis wordt de bochtstraal van 250 mm bereikt als het binnenwiel half zo snel draait als het buitenwiel, dus binnenwiel 0.93 m/s en het buitenwiel 1.86 m/s. Als we kiezen voor een wielbasis van 125 mm, dan bereiken we deze 1.4 m/s als het binnenwiel 1.17 m/s haalt en het buitenwiel 1.63 m/s. Het is direct duidelijk dat de wielbasis een grote invloed heeft op de keuze van motor/overbrenging en wieldiameter.

Hoe en waar moet onze lijnsensor geplaatst worden

Eerst en vooral kunnen we kiezen uit verschillende types lijnsensoren : Lijnvolgen kan al met één analoge sensor, maar met meerdere sensoren gaat het veel beter. In de schets hieronder zie je ook waarom. Met één enkele sensor heb je een zeer klein gebied waar je enigszins een betrouwbaar signaal van deze sensor kan verwachten : je moet nauwkeurig op de rand van de lijn blijven. Op het moment dat je de lijn verliest, is het onmogelijk te bepalen of de lijn links of rechts van de sensor ligt. Daarom heb je dus een kleine "blauwe" zone, en al de rest is rood. Als je 2 sensoren naast mekaar zet, heb je al een breder nuttig signaal bereik, maar daarnaast kan je ook vrij goed schatten aan welke zijde de lijn zich bevindt als je deze verliest (de "gele" zone)! Met meer sensoren krijg je nog een beter signaal : je kan over een vrij grote breedte perfect de positie van de lijn bepalen. Belangrijk ook is de afstand tussen de sensoren onderling : deze is best kleiner dan de breedte van de lijn. De hoogte instelling van de sensoren is afhankelijk van het type en de gevoeligheid / sterkte van de IR-leds. Als je sensoren echter zeer dicht tegen de lijn moeten staan voor een goed signaal, krijg je waarschijnlijk een probleem als de baan niet perfect vlak is : elk hobbeltje zal deze afstand dan sterk beinvloeden, en daarmee het signaal !

Een tweede zeer belangrijke parameter is de afstand tussen sensor-array en wielbasis. Hoe groter deze afstand, hoe meer tijd er is om de gemeten positie van de lijn om te zetten in een richtingsverandering. Hoe kleiner deze afstand, hoe groter de hoekfout van de robot kan zijn tov de lijn voor een betrouwbare meting. Als vuistregel zou ik hier stellen dat bij de kleinste straal van de lijn je lijnsensoren nog steeds een betrouwbare meting kunnen doen (zoals in de schets bovenaan). Het grote voordeel van zo een opstelling is dat de proportionele regel factor lager kan ingesteld worden. Ideaal is de P-regeling zodanig dat je zonder oscillatie (= snel heen en weer draaien) de complete lijn kan volgen.

Een snelle lijnvolger

De hardware, welke sensoren ?

In principe kan je met één enkele IR-sensor een zwarte lijn volgen. In dat geval moet je de rand van de zwarte lijn zien te volgen, maar met een smalle lijn is dit erg moeilijk. Beter gaat het met 2 sensoren, en nog beter gaat het met 4 sensoren ! Immers, hoe meer sensoren je hebt, hoe groter de afwijking van je robot kan zijn waarbij je nog steeds informatie hebt waar de lijn zich bevindt ! Ik gebruik 4 IR fototransistoren over een breedte van ca 60 mm. Elke fototransistor heeft links en rechts een IR-LED om de lijn te belichten. De weerkaatsing van deze IR-LEDS wordt dan analoog gemeten met de fototransistor. Een geschikte IR-LED is de TSUS5402 (Voti, 0,3€). De continue forward current bedraagt 150 mA, de afstralingshoek ca 25°, spanningsval bij 100 mA max. 1,7 Volt. Je kan natuurlijk elke LED een serieweerstand geven, en dan deze LED voeden uit de 5 Volt. Dit resulteert wel in een hoog stroomverbruik (5*100 mA=500 mA).Ik heb de 5 LEDS in serie geschakeld, maximale spanningsval wordt dan 8,5 Volt. Om een stabiele stroom van 100 mA in te stellen, kan je een serieweerstand of een transistor gebruiken :De basistroom van de NPN transistor komt vanuit de discovery, dus max 10 mA en max 3 Volt. Als ik nu een transistor heb met gekende hfe (stroomversterking), dan kan ik perfect de basisweerstand uitrekenen. Helaas is deze hfe steeds een parameter met een enorme spreiding. Vb NPN transistor BC247 heeft een hfe tussen 100 en 300 ! Maar meten is weten, dus heb ik mijn schakeling eerst opgebouwd met een potmeter van 10 k als basisweerstand. Daarna de potmeter instellen tot de stroom door de LEDs 100 mA bedroeg, en dan de potmeter vervangen door een vaste weerstand. Voordeel van deze schakeling : de batterijspanning kan varieren tussen 12 en 16 Volt, maar de stroom zal weinig veranderen omdat de transistor een konstante stroom instelt !

Als fototransistor gebruikt ik een BPW40 (Voti, 0,6€). Hieronder zie de gaatjesprint met opgebouwde sensoren + IR-leds.

Het schema, hoe de sensoren aan te sluiten

Bovenaan links zie het voedingsgedeelte van mijn Discovery-robot. Een 4s lipo batterij zorgt voor een voedingsspanning van max 16,8 Volt. Na de hoofdschakelaar komt de beveiligingsdiode, om schade te voorkomen bij polariteitsfouten. Vanaf daar heb ik een spanning beschikbaar van tussen de 12 en 16 Volt (afhankelijk van de laadtoestand). Ook heb ik daar een spanningsdeler (R1/R2) voorzien om de batterijspanning analoog te kunnen meten. Hierna komt er een geschakelde regelaar die de spanning naar 8 Volt regelt. Daarna een klassieke lineaire regelaar 7805 om 5 Volt te bekomen. Op het discovery bord zit er dan een lineaire regelaar naar 3.3 Volt.

Daaronder de 5 IR-leds, direct gevoed vanuit de beveiligde batterijspanning. Om deze LEDs te kunnen schakelen, en om de stroom in te stellen op ca 100 mA gebruik ik dan een NPN transistor met een bepaalde basisweerstand. Deze basisweerstand moet voor elke transistor opnieuw bepaald worden, omdat de hfe geen vaste waarde heeft !!! Je kan gelijk welke NPN-transistor hiervoor gebruiken, maar let wel op het maximaal vermogen : er kan immers een spanningsval van (16 Volt-5*1,3 Volt)= 9,5 Volt over deze transistor vallen. Als je dan een stroom instelt van 100 mA moet deze transistor wel bijna 1 Watt in warmte dissiperen !

Daaronder dan de analoge schakeling van de fototransistoren. Voeding vanuit de 3 Volt, en dan de BPW40 in serie met een weerstand van 320 ohm naar massa. De analoge ingang van de discovey is verbonden tussen de weerstand en de BPW40. Indien de BPW40 onbelicht is, heeft deze een zeer grote weerstand. De spanning op het knooppunt zal dus vrijwel 0 bedragen. Als de BPW40 sterk belicht wordt met IR, zal deze een zeer lage weerstand hebben. De spanning van het knoppunt zal dan eerder 3 Volt bedragen. Op deze wijze bekomen we een analoge spanning evenredig met de hoeveelheid invallend IR licht.

Kalibratie van de IR-sensoren

Om een nauwkeurige analoge weergave van de lijn tov de achtergrond te bekomen, en om een gelijke respons van de 4 sensoren te garanderen moeten deze gekalibreerd worden. Het komt er op neer dat we de minimale en maximale analoge waarde van elke sensor gaan bepalen. Dan kunnen we later elke gemeten waarde van elke sensor apart vergelijken met deze "referentie". Mijn algoritme is als volgt :

IR-leds aanschakelen
Continu alle 4 analoge ingangen meten
De robot draait 180° over de zwarte lijn
Tijdens het draaien worden de maximale en minimale waarde van elke analoge ingang opgeslagen in een variabele
Na het draaien worden deze max/min waarden opgeslagen in de EEPROM / Flash van de µ
IR-leds uitschakelen

Nu we deze waarden gemeten hebben, kunnen we als volgt de meetwaarde interpreteren :

Grijswaarde = Analoge meting IR -(Max+Min)/2
We krijgen een positief getal bij wit, en ongeveer een even groot negatief getal bij zwart.
Tussenliggende waarde geven dan een overgang weer van wit naar zwart.

Waar bevindt de lijn zich tov de 4 IR-sensoren ?

Idealerwijze zouden we een analoge waarde willen die de positie van de lijn weergeeft tov de 4 IR-sensoren. Welnu, dit kan vrij eenvoudig door aan elke sensor een weegfactor toe te kennen en daarna deze waarden op te tellen :

Lijnpositie = 2*Sensor_1+Sensor_2-Sensor_3-2*Sensor_4

Op de grafiek zie deze sommatie als de robot ongeveer 90° over de lijn draait :

Bij 0° (X-as) zien alle sensoren de witte ondergrond, de sommatie is dan ca 25 (rechtse Y-schaal).
Bij 33° staat Sensor_1 volledig boven de zwarte lijn, de waarde wordt + 200.
Bij 45° hebben we dus de waarde 0 (de lijn bevindt zich mooi in het midden, tussen sensor_2 en sensor_3).
Bij 63° wordt de waarde -170, de lijn bevindt zich dan onder sensor_4.
Bij 90° wordt de waarde terug 25, alle sensoren zien terug de witte ondergrond.

Het interessante bereik, waar we dus vrijwel een lineair verband hebben tussen meetwaarde en de hoek van de robot tov de zwarte lijn is dus van 33° tot 63°. Deze meetwaarde varieert dan van 200 tot -170. Om de robot deze lijn te laten volgen, volstaat het om met deze meetwaarde de PWM te beinvloeden van de linker en rechter motor. Opgelet, om een snelle respons van de motoren te verkrijgen is een "harde" aansturing van de PWM noodzakelijk ! Zie ook MotorStapResponse. Een vereenvoudigd algoritme kan er zo uit zien (stel dat onze motor aangestuurd wordt met een PWM van -100 tot +100):

PWM_L = Basis_PWM + meetwaarde*versterkingsfactor ;
PWM_R = Basis_PWM - meetwaarde*versterkingsfactor ;

De Basis_PWM beinvloed de gemiddelde snelheid van de robot, met de versterkingsfactor regel je dan eigenlijk hoe krachtig de reactie wordt als de robot afwijkt van de lijn. Dit stelt dan een eenvoudige "P-regeling" voor. Omdat de meting met de IR-fototransistoren zeer snel kan verlopen (de IR-sensor reageert binnen 1 ms na het sturen van de IR-LED, de analoge meting bij mijn Discovery STM32 duurt ca 250 µs voor 10 kanalen), kan je ook dus de PWM waarde zeer snel updaten ! Het resultaat kan je in volgende filmpje bekijken (en nee, dit is echt niet versneld weergegeven !):