Varighed: 9-16
Deltagere: Daniel, Leni, Martin A og Martin N
1. Formål
Formålet med denne øvelse var at bygge en LEGO robot, der kunne følge bane "The Alishan train track" op til toppen og ned igen.
Følgende regler blev opstillet for gennemførslen af banen:
1) Robotten skal starte i startområdet
2) Ved tryk på Enter skal robotten følge banen op til toppen og tilbage til startområdet, hvor gennemløbstiden udskrives til displayet.
3) På toppen skal robotten være helt inden i "retrace" området, inden den vender om.
2. Øvelsesgennemgang
Foranalyse
Starten af denne øvelse gik med at overveje, hvilke erfaringer fra de tidligere øvelser med hhv. lyssensoren, LEGO bil og "Line Follower" vi kunne bruge til at bygge en robot der kunne gennemløbe "The Alishan train track" hurtigst muligt.
Konklusionen af denne foranalyse var følgende:
Den hurtigste robot vil være baseret på en open-loop controller, da der derved ikke skal reageres på input fra omverdenen, men blot følges et i forvejen bestemt sæt regler.
Det vil dog resultere i en løsning, der er meget følsom overfor ændringer i miljøet omkring den, da den ikke vil kunne kompensere for disse ændringer.
Det kunne bl.a. være; startsposition og -vinkel, batteriniveau samt motorbelastning grundet samlingen mellem LEGO klodserne.
Gruppen besluttede at lave en closed-loop løsning baseret på en PID regulator, vel vidende at vi muligvis vil blive slået af en godt justeret open-loop løsning. Dette gav os mulighed for at benytte den tidligere arbejde med en PID regulering og lave en mere generel løsning.
Størst topfart opnås enten ved at benytte store hjul eller gearing som medfører større belastning af motorerne.
Grundet LEGO motorernes lave drejningsmoment, vil hastighedsforøgelsen ved at benytte gearing være på bekostning af acceleration, og da gennemkørslen af "The Alishan train track" kræver 5 opbremsninger ved sving og relativt korte distancer, besluttede gruppen at benytte store hjul på robotten.
Mht. sensorerne, indså vi bl.a. på baggrund af artiklen A PID Controller For Lego Mindstorms Robots, at et stor detekteringsområde vil give robotten mulighed for at køre hurtigere, uden at miste banen af syne. Derfor besluttede vi at koble 4 lyssensorer til robotten, og kombinere deres sensorværdier til en enkelt værdi, der repræsenterer den sorte stregs position i forhold til robotten.
Design
Den første store opgave der skulle løse, var hvordan de 4 sensorer bedst kunne benyttes til at bestemme, hvor langt robotten var fra den sorte linie som den skulle følge.
Løsningen skulle give en enkelt værdi mellem -100 og 100, som repræsenterer hvor langt robottens midtpunkt er fra den sorte linie, som kan bruges til input til PID regulatoren.
Derudover ønskes et monotont stigende output fra sensorblokken for monotont stigende afstand fra den sorte linie, hvilket sikrer en jævn regulering.
Sensor arrayet blev opbygget af de gamle LEGO sensorer, da de kunne placeres tættest på hinanden, hvilket er nødvendigt for at sikre at den tynde sorte linie på "The Alishan train track", ikke blev "tabt" mellem detektionsområdet for to sidestillede sensorer.
Selv med disse mindre sensorer var det vanskeligt at detektere linien når den lå mellem to sensorer, og vi undersøgte derfor to mulige konfigurationer af sensorarrayet:
1. Sensorarray
2. SensorarrayVi besluttede at benytte den første mulighed, da det gav et større detektionsområde, og problemet med at opnå en pålidelig detektering af linien når den lå mellem to sensorer kunne løses i software.
Følsomheden af lyssensorerne blev undersøgt, ved at placere dem i en lukket beholder over et hvidt stykke papir, og derefter se hvilken værdi de returnerede fra getNormalizedLightValue() når deres lys var tændt. Dette gav vidt forskellige værdier for de 4 sensorer, så i stedet for at forsøge at finde 4 sensorer, der havde samme følsomhed, besluttede vi at implementere en individuel sensor normalisering baseret på floating point værdier for hver af de 4 sensorer.
Dette blev gjort ved at gemme en værdi for hvidt og sort underlagt for hver sensor, når robotten kalibreres inden start og derefter beregne en normaliseret sensorværdi for hver af de 4 sensorer ud fra følgende formel (i funktionen RobotRace::Normalize()):
normalized = 100*( 1 - (value - black)/(white - black) )
Herved opnås en værdi mellem 0 og 100 for hver af de 4 sensorer, hvor 0 repræsenterer hvidt og 100 repræsenterer sort. Årsagen til denne invertering af værdien bliver klargjort i efterfølgende.
Med det valgte sensorarray, blev der fortaget målinger for forskellige placeringer af den sorte linie i forhold til sensorarrayet som vist herunder:
Sensor 1: 1
Sensor 2: 4
Sensor 3: 3
Sensor 4: 97
Sensor 1: 3
Sensor 2: 2
Sensor 3: 15
Sensor 4: 70
Sensor 1: 3
Sensor 2: 4
Sensor 3: 34
Sensor 4: 32
Sensor 1: 3
Sensor 2: 5
Sensor 3: 67
Sensor 4: 13
Ud fra disse målinger indså vi at summen af to sidestående sensorer altid vil være tæt på 100 når den sorte streg er et sted mellem dem. Derfor gemmes værdien af Sensor1 + Sensor2, Sensor2 + Sensor3 og Sensor3 + Sensor4, hvorved der altid vil være mindst en variable med en værdi tæt på 100.
Ved at lade den sorte værdi udbrede sig mod venstre, ved at overskrive venstrestående sensorværdier med en højrestående, hvis den højrestående er større, opnås en monotont stigende værdi, når den sorte streg bevæges fra venstre mod højre på sensorarrayet.
Disse beregninger er implementeret i funktionen RobotRace::sensorValue(), der tager de 4 normaliserede sensor værdier som input, og beregner den sorte stregs placering.
Et udsnit af funktionen der viser den centrale funktionalitet er vist herunder:
sensor1 = sensor1 + sensor2;
sensor2 = sensor2 + sensor3;
sensor3 = sensor3 + sensor4;
sensor4 = sensor4;
if (sensor4 > sensor3) {
sensor3 = sensor4;
}
if (sensor4 > sensor2) {
sensor2 = sensor4;
}
if (sensor4 > sensor1) {
sensor1 = sensor4;
}
if (sensor3 > sensor2) {
sensor2 = sensor3;
}
if (sensor3 > sensor1) {
sensor1 = sensor3;
}
if (sensor2 > sensor1) {
sensor1 = sensor2;
}
return (sensor1 + sensor2 + sensor3 + sensor4)/4;
Returværdien fra denne funktion bruges som input til en PID regulator der er implementeret i classen PIDController.
For at bestemme værdien af de forskellige konstanter i PID regulatoren, benyttede vi Ziegler–Nichols metode, hvor der startes med en P-regulator, hvor Kp sættes til en størrelse der resulterer i oscillation. Denne Kp benævnes 'Ku', og oscillations perioden benævnes 'Pu'. Vi bestemte denne Pu ved at måle den samlede tid for 10 oscillationer når robotten følger en lige streg på banen.
Derudover blev PID loopets gennemløbstid 'dT' målt, ved at beregne den tid det tager for NXT bricken at gennemløbe beregningerne af sensorarray og opdatering af PID-controlleren 10000 gange i funktionen RobotRace::TestLoopRuntime(), vis funktionalitet er gengivet herunder:
int loopTime = (int)System.currentTimeMillis();
for (int i = 0; i != 10000; i++)
{
readout1 = Normalize(sensor1.getNormalizedLightValue(), 1);
readout2 = Normalize(sensor2.getNormalizedLightValue(), 2);
readout3 = Normalize(sensor3.getNormalizedLightValue(), 3);
readout4 = Normalize(sensor4.getNormalizedLightValue(), 4);
weightedRead = sensorValue(readout1, readout2, readout3, readout4);
error = weightedRead - 40.0F;
PIDValue = controller.controlLoop(error);
controlCar(PIDValue);
}
loopTime = (int)System.currentTimeMillis() - loopTime;
LCD.clear();
LCD.drawString("10K loop runtime (ms):", 0, 0);
LCD.drawInt(loopTime, 0, 1);
LCD.refresh();
Der blev målt følgende værdier: Ku = 900, Pu = 0,4s, dT = 0,5ms.
Ved at følge Ziegler–Nichols metode gav det os følgende PID konstanter:
Kp = 540
Ki = 0,81
Kd = 27
Vi endte dog med at øge vores Kp til 1000, for at opnå et hurtigere indsving, hvilket gjorde at robotten kunne håndtere svingene hurtigere, men stadig stabilt.
Den sidste del af designet bestod i at håndtere de forskellige zoner på "The Alishan train track", hhv. startzonen, retrace zonen og mål zonen.
Da robottens opførsel skulle være anderledes i disse zoner, besluttede vi at implementere en tilstandsmaskine, der træffer beslutninger om robottens tilstand ud fra sensorarray's værdier, inden PID regulatoren kaldes.
Derved er det muligt at tilsidesætte PID controllerens regulering af motorne, fx når robotten befinder sig i retrace zonen og derfor skal lave en 180 graders drejning inden den kører ned af banen igen.
Da vi benytter 4 sensorer, kan overgangen ind til retrace og mål zonen detekteres, ved at hode øje med om 3 eller flere sensorer ser en sort linie på samme tid, og for at undgå at svingene kunne trigge dette tilstandsskift, benyttede en timer, der sikrer at robotten har passeret begge sving inden den holder øje med om den er i retrace eller mål zonen.
3. Resultat
Videon af vores gennemløb kan ses herunder:
Tiden for gennemløb blev 40,6 sekunder (40652 ms).
Videon af vores gennemløb kan ses herunder:
Tiden for gennemløb blev 40,6 sekunder (40652 ms).
3. Konklusion
Det lykkedes at bygge en robot baseret på closed-loop regulering vha. en PID controller, der kunne gennemføre "The Alishan train track" på 40,6 sekunder.
Denne tid blev opnået ved at benytte et sensorarray med stort detekteringsområdet, samt en tilstandsbaseret styring foran PID regulatoren til at holde styr på, hvilket område af banen som robotten befinder sig i, og herudfra justere dens opførsel.
4. Kildekode
Kildekoden til RobotRace robotten kan findes her.
Det lykkedes at bygge en robot baseret på closed-loop regulering vha. en PID controller, der kunne gennemføre "The Alishan train track" på 40,6 sekunder.
Denne tid blev opnået ved at benytte et sensorarray med stort detekteringsområdet, samt en tilstandsbaseret styring foran PID regulatoren til at holde styr på, hvilket område af banen som robotten befinder sig i, og herudfra justere dens opførsel.
4. Kildekode
Kildekoden til RobotRace robotten kan findes her.
