Vejfinder 8

Blog8:

Varighed: 14.6 timer, Deltagere: Rolf og Janus

Dagens mål er at bygge GridBuilder færdig, så den kan opbygge et fuldstændigt kort over vejnettet. Vi vil desuden lave en metode til at få robotten til at flytte sig til et bestemt sted på kortet.

Vi vil opnå vores mål idag ved at:

• Lave en algoritme i MapFollower, som kan udregne den korteste vej imellem to punkter i koordinatsystemet - vel at mærke ved kun at følge kendte veje.
• Tilføje mulighed for at suppresse LineFollower
• Tilføje en ny behaviour (MapFollower), som får robotten til at køre imellem to blokke i kortet uden at køre "på skrå" over grønne arealer.
• Få en bedre præcision på rotationerne ved at kigge på TachoNavigator klassen.
• Få mere fart på kort-mappingen ved at ændre LineFollower til at kunne klare lige strækninger hurtigere.
• Få mere præcision på sensor-aflæsningerne ved at lave bedre kalibrering.
• Lave den "aktive" GridBuilder funktionalitet.

En shortest-path algoritme under lejOS

Det lidt specielle miljø (lejOS) stiller nogle lidt specielle krav til shortest-path algoritmen. Vi kan ikke blot benytte en helt normal implementation af vejfinder-algoritmer, eftersom lejOS ikke har understøttelse for garbage collection endnu. Det interessante er derfor at designe en algoritme, som udfører det samme, men som ikke skaber "tab" af objekter i hukkommelsen.

For at løse dette problem har vi lavet en graffarvnings-algoritme, som med et konstant memory-footprint (dvs. uafhængigt af antal gange, der skal findes en vej) kan finde den korteste rute imellem to blokke i vejnettet.
Algoritmen er opbygget som følger:

1. Klargør farverne på kortet og afstandsvariablene således at alle farver er 0 og alle afstande er "uendelig".
2. Sæt farven på destinationen til 1 og på oprindelsesstedet til 2.
3. Så længe der ikke findes en blok, som har en nabo farvet med 1 og en anden nabo farvet med 2: Farv alle blokke ved siden af allerede farvede blokke og sæt deres distance til én højere end den blok, som de fik deres farve fra.
4. Nu findes der (mindst) én blok der har naboer i to forskellige farver. Vi ved at eftersom dette skridt var det første, hvor dette var tilfældet, vil en af de korteste ruter inkludere denne brik i midten af ruten. Vi kan derfor kalde rekursivt og dele ruten op i to dele:

   rute(start, den fundne mellem-blok);
   rute(den fundne mellem-blok, stop);
   

Når vi har kaldt rekursivt så mange gange, at ruten kun er 2 blokke lang (start-blokken og stop-blokken), kan vi se hvordan disse blokke er forbundet til hinanden, og derefter flytte os direkte fra den ene blok til den anden.

I forhold til en normal depth-first eller width-first søgning kræver denne søgnings-algoritme ingen ekstra datastrukturer ud over dem, som vi allerede har i forvejen. Algoritmen bruger en farve og en distance-værdi, som sættes som fields på selve blokken - og den udnytter at BlockGrid i forvejen er lavet til ikke at lave memory-leaks på lejOS. I dette divide-and-conquer-pattern skal man dog være opmærksom på at nogle af de gemte objekter er klasse-variable, hvilket er grunden til at vi kalder rekursivt på følgende måde:

lookatBlock = existsPathBlock(1,2);
intermediateX = lookatBlock.getXPosition();
intermediateY = lookatBlock.getYPosition();
navigateFromBlockToBlock(fromBlock, core.getBlockGrid().getBlock(intermediateX, intermediateY));
navigateFromBlockToBlock(core.getBlockGrid().getBlock(intermediateX, intermediateY), toBlock);

istedet for at kalde som man normalt ville:

lookatBlock = existsPathBlock(1,2);
navigateFromBlockToBlock(fromBlock, lookatBlock); <-- Denne linie ændrer lookatBlock(!)
navigateFromBlockToBlock(lookatBlock, toBlock); <-- Vi kan ikke bruge den igen her fordi nu er den en anden

Vores shortest-path algoritme bliver kaldt som en del af GridBuilderen når den skal bevæge sig fra et kendt område på kortet til det næste ukendte sted, som skal udforskes. Den kan også kaldes via BlueTooth, så robotten via kortet i hukkommelsen kan bevæge sig langs vejene på kommando.

Under dette afsnit findes en visuel gennemgang af algoritmens udførsel. Figuren læses fra venstre mod højre i hver linie, og viser et lille vejsystem med nogle forbundne blokke:

1. Oprindelsesstedet (rød) og destinationen (blå) markeres.
2. Efter som der endnu ikke findes en blok, som har både røde og blå naboer, "gror" vi det besøgte område, så naboer i alle udgående veje bliver farvet.
3. Nu findes der mindst én blok (grøn), som har både røde og blå naboer. Vi ved at denne blok nødvendigvis må være med i ruten fra oprindelsessted til destination.
4. Der kaldes nu rekursivt; først på turen fra det oprindelige oprindelsessted til den blok, som vi fandt i forrige skridt. Oprindelsesstedet markeres med rød og destinationen (den fundne blok) markeres med blå.
5. Der findes allerede i denne opsætning en blok med både blå og røde naboer, og den må nødvendigvis være med i ruten.
6. Vi kalder rekursivt på oprindelsesstedet til den blok, som vi lige fandt i sidste skridt. Dette er et kald, hvor de to blokke ligger direkte ved siden af hinanden, og der er derfor en triviel rute (gul). Nu udføres det andet rekursive kald, som startede i (3). Start og destination markeres (henholdsvist rød og blå).
7. Her findes igen direkte en blok, som både har røde og blå naboer, og vi behøver ikke at "gro" farverne.
8. Vi kalder rekursivt i to skridt, og finder den resterende rute trivielt. Vi har dermed en fuld rute fra det oprindelige oprindelsessted til den oprindelige destination.

Billedet viser idéen i vores shortest-path algoritme.

MapFollowerBehaviour - en klasse med undertrykkelses-attitude

MapFollowerBehaviours største ønske er at udføre ruten fundet i shortest-path-algoritmen, derfor undertrykker den de andre behaviours så længe den har en rute at følge. Selve udførslen af ruten er relativt simpel: Hvis der er en positions-ændrende forespørgel i kø, bevæger den robotten en plades længde i den retning, som svarer til retningen i forespørgslen.

Kalibrering er vigtigt

Som nævnt i [fodnote] vil der mange gange være stærkt forskellige situationer alt efter lysforhold, strømstyrke på batteriet, underlag osv. Af denne grund har vi lavet et system til at kalibrere hvor meget robotten skal dreje for at have udført et x-graders sving. Den parameter vi ændrer på er afstanden imellem hjulene i TachoNavigatoren, da TachoNavigatoren skal "dreje mere" hvis hjulene var længere væk fra hinanden. På denne måde har vi fundet at hjulene skal stå til at være 12cm væk fra hinanden, selv om de faktisk fysisk er 11cm fra hinanden.

Kalibreringen foregår ved at foretage en række 90-graders sving og køre lidt frem og tilbage (for at skalere fejlen, så den bliver mere synlig). Hvis hjulafstanden er indstillet forkert bliver det tydeligt efter ganske få drejninger om tallet skal sættes op eller ned.

Aktiv GridBuilder

Vores første passive del af GridBuilder, som ikke på noget tidspunkt tager styrringen af motorerne, virker ikke helt optimalt til at detektere kryds og t-kryds. Derfor har vi lavet en "aktiv" del af GridBuilder, som bliver kørt, hvis der er mulighed for at det nuværende vejstykke kunne være et kryds/t-kryds.

Når et vejstykke er kørt igennem, og den passive GridBuilder-del står med kryds/t-kryds som mulige valg, startes den aktive del, som gør som følger:

1. Supress LineFollowBehavior.
2. Gem positionen/rotation.
3. Kør til centrum af den nuværende plade.
4. Rotér, så de to lyssensorer til LineFollowing er over det sted, hvor den ene mulige sidevej burde være.
5. Se på input fra de to lyssensorer om de er over vej / græs.
6. Rotér, så de to lyssensorer til LineFollowing er over det sted hvor den anden mulige sidevej burde være.
7. Se på input fra de to lyssensorer om de er over vej / græs.
8. Sæt typen på vejstykket alt efter om der var set vej i de to tilfælde.
9. Kør tilbage til den gemte position, og roter til den gemte rotation.
10. Unsupress LineFollowBehavior.

På grund af at vi kører til "centrum" af vejstykket, er det kritisk at robottens position i TachoNavigator svarrer temmeligt nøjagtigt til dens rent fysiske position, da lys sensorerne ellers ikke vil være positioneret korrekt over sidevejene, og de derfor muligvis ikke bliver detekteret.

Optimering af LineFollowBehavior

Vores oprindelige kode til LineFollowBehavior brugte rotate(5) og travel(5) til at bevæge sig i små trin, da den oprindelige TachoNavigator ikke kunne opdatere position/rotation imens der køres. Samtidigt checkede vi ikke i driveForwardABit() om vi stadig holdt "ok" til bare at køre videre fremad, hvilket gjorde at vi, specielt på lige strækninger, heletiden stoppede op og spurgte findAndGotoEdge() om robotten holdt lige - selvom dette allerede var tilfældet.

Med den nye TachoNavigator, som vi som nævnt tidligere har hentet fra SVN, var det ikke længere et krav at robotten skulle holde stille når der kaldes updatePosition(). Med andre ord behøvede vi ikke længere at bevæge os i små hak, men kunne istedet for at kalde travel(5), bare kalde forward(), og opdatere position/rotation hele tiden. (Naturligvis skal man så huske at kalde stop(), når man er færdig med at bevæge robotten.)

For at optimere den hastighed, som robotten kunne følge lige kanter med, ændrede vi driveForwardABit() yderligere, så den nu blev ved med at køre frem, så længe venstre sensor er på vej og højre sensor på græs.

Videomateriale

Her er en video, hvor det ses at robotten bruger det allerede opbyggede kort til at finde rundt på, således at det resterende kort kan opbygges (Det ses at robotten her stadig laver små hak, men nu kører hurtigt frem på lige stykker, denne video er fra før de nye "bløde" bevægelser blev lavet)


På videoen ses en del af kalibreringsmanøvren. Robotten forsøger at foretage 90-graders sving.

Konklusion

MapFollower virker efter hensigten, og sammen med GridBuilder og LineFollower betyder det at robotten nu kan finde hele kortet. Når GridBuilder er færdig med at finde så meget af kortet som muligt ved bare at følge højre kant (ie. når den rammer noget, som den allerede har været ved før), tager MapFollower over og kører robotten til det første ukendte sted på kortet. Derfra fortsætter GridBuilder og LineFollower med at opbygge kortet. Som det ses på videoen, resulterer det i at hele kortet rent faktisk bliver fundet! Efter tilføjelsen af den aktive GridBuilder-del, som tager over i kryds / t-kryds, fik vi også en stabil detektering af disse typer af vejblokke. (Dette er ikke inkluderet i videoen ovenfor).
De nye optimeringer af LineFollower har også virket efter hensigten. Robotten kører nu både mere flydende og samtidigt også hurtigere. Vi havde dog et større problem senere på dagen, hvor robotten pludselig kørte mere og mere "skævt" - TachoNavigatorens position passede overhovedet ikke med hvor robotten fysisk befandt sig. Efter mange timers debugging, kalibrering og testing, kom vi frem til at det var strømniveauet, der var kommet for langt ned. Næste gang håber vi at det hele virker igen med et nyt, opladet batteri.

Noter

• [Fodnote] Roland Siegwart og Illah Reza Nourbakhsh beskriver i 5.2.3, s. 188 i "Introduction to Autonomous Mobile Robots" (2004, MIT press) hvordan "effector noise" kan give problemer i forbindels med positionering af robotter. En af kilderne til den slags "støj" er netop usikkerheden omkring motorernes og tachonavigatorens ydeevne under forskellige spændingsstyrker på batteriet. Hele afsnit 5.2 omhandler udfordringer i forbindelse med positionering, og har desuden interessante bemærkninger omkring sensor aflæsninger og støj for disse.