Robot Eksperimentarium: Tips Exercise 4 & 5

From ImageWiki

Jump to: navigation, search

Contents

Hvordan måler man tid i programmet?

Hvis man benytter player / stage til at løse opgave 4 kan man ikke afl�se odometri informationen fra robotten, da dette ikke p� nuv�rende tidspunkt er implementeret i driveren. Man skal i stedet benytte den hastighed man har givet robotten til at estimere hvor langt man har kørt. Dette kræver at man kan måle tid i sit program, således at man kan oversætte meter/sekund til meter. Til dette form�l kan man anvende en af f�lgende funktioner:

  #include <sys/timeb.h>
  int ftime(struct timeb *tp);

eller

  #include <sys/time.h>
  int gettimeofday(struct timeval *tv, struct timezone *tz);


Fejl i kodeeksempel til metode 2 (og dermed ogs� metode 3) i Øvelse 5?

Under antagelse af at F er en normalfordeling...

Skal denne kode:

weight += p1.weight * F(dist)

ikke �ndres til:

weight += p1.weight * F(dist-measuredDist)

...? (Alternativt kan middelv�rdien for F s�ttes forskelligt fra 0)

Problemer med detection af landmarks i opgave 5

Dette afsnit drejer sig om hhv. brug af exercise4.cc i opgave 5 og brug af den nye landmark_detection.cc (den som ligger i /Exercise5 i svn).

  • I landmark_detection.cc b�r:
    const CvArr *black_vects[blue_count];
    

    �ndres til:

    const CvArr *black_vects[black_count];
    
  • I landmark_detection.cc b�r:
    // Clean up after training
    for (int i = 0; i < blue_count; i++) {
        CvMat *tmp = (CvMat*)black_vects[i];
        cvReleaseMat(&tmp);
    

    �ndres til:

    // Clean up after training
    for (int i = 0; i < black_count; i++) {
        CvMat *tmp = (CvMat*)black_vects[i];
        cvReleaseMat(&tmp);
    
  • Hauberg: Eftersom blue_count og black_count antager præcis samme værdi er der ingen grund til disse rettelser.

  • I landmark_detection.cc er det vores erfaring at der opn�s en langt bedre detection ved at �ge denne variabels v�rdi:
    const float max_dist = 3.0; //2.5;
    
  • Hauberg: dette stemmer ikke overens med min erfaring. Men det er uden tvivl en betydende parameter, som det muligvis er n�dvendigt at pille ved...

  • Hvis exercise4.cc bruges som udgangspunkt i opgave 5, bør der tilføjes kode som gemmer/henter information om det sorte landmark (samme sted som den tilsvarende kode for det gule og blå landmark findes):
    save_matrix(yellow_mean, "yellow_mean.mat");
    save_matrix(yellow_invC, "yellow_invC.mat");
    

    og

    load_matrix("yellow_mean.mat", yellow_mean);
    load_matrix("yellow_invC.mat", yellow_invC);
    

Forslag til protokol

I vores system bruges S�rens send_float- og get_float-funktioner til at sende og modtage over en TCPSocket (fra PracticalSocket.cc).

Arkitektur

  • Et program (A) lytter p� en angivet port (angives som parameter til programmet).
  • Et andet program (B) forbindelser til A (b�de A's IP og port angives som parameter til programmet).

Beskederne

De to processer udveksler beskeder best�ende af 3 float's. Der sendes altid 3 floats n�r der sendes og der modtages altid 3 floats n�r der modtages.

De tre floats angives relativt til den forrige besked s�dan at:

  • Den f�rste float angiver hvor meget robotten har flyttet sig p� aksen som st�r parallelt med robottens retning som den var ved den forrige besked. Positivt tal betyder at robotten har flyttet sig fremad.
  • Den anden float angiver hvor meget robotten har flyttet sig p� aksen som st�r vinkelret p� robottens retning som den var ved den forrige besked. Positivt tal er til h�jre for robottens tidligere retning.
  • Den tredje float angiver hvor meget robotten har drejet sig i forhold til dens retning som den var i den forrige besked. Positivt tal betyder at robotten er drejet med uret (set fra oven) - det er s�dan robottens odometry returnerer vinklen.

Eksempler

Eksempler (repr�senteret i tekst):

  • 10 0 0 - Robotton er k�rt 10 cm fremad siden sidste besked.
  • -10 0 0 - Robotten er k�rt 10 cm tilbage siden sidste besked.
  • 10 0 Pi/4 - Robotten befinder sig 10 cm l�ngere fremme end ved den forrige besked og den er drejet 45 grader med uret.
  • 10 10 0 - Robotten befinder sig 10 cm l�ngere fremme og 10 cm l�ngere mod h�jre end ved den forrige besked.

Hvordan benyttes disse tal s�?

I praksis bruges en rotationsmatrix, R, til at rotere de f�rste to koordinater hen til en ny position. Herefter translateres og roteres der.

Hvis en robot st�r i det absolutte punkt (3,4,Pi/4) og beskeden (10,0,Pi/2) modtages...

...s� st�r den nu i punktet:

[3 4]' + R(Pi/4) * [10 0]'

=

[3] + [ cos(Pi/4) -sin(Pi/4) ] * [10] 
[4]   [ sin(Pi/4) cos(Pi/4)  ]   [ 0]

=

[3] + [ 7.07 ]
[4]   [ 7.07 ]

=

[10.07]
[11.07]

...og den har vinklen:

Pi/4-Pi/2 = -Pi/4

Fortegnene i vinkeludregningen er m�ske lidt kryptiske, men det skyldes at de absolutte koordinater (Pi/4) er angivet "mod uret" og robottens drejning (Pi/2) er angivet "med uret". S� l�nge vi er enige om hvordan robottens drejning angives skal det nok g�, og odometrien fort�ller os alts� vinklen "med uret".

Dette er rimeligt implementationsuafh�ngigt, og det g�r det nemt at flytte partiklerne p� samme m�de som robotten har flyttet sig (og i forhold til deres egen retning) hvis man vil det. Det mener vi er den mest korrekte fremgangsm�de.

Hvis nogle hellere vil udveksle absolutte koordinater kan man evt. sende en identifikator med som siger noget om hvordan de tre floats er angivet.

Fejl i SEND-makroen

Der er ikke fejl i SEND-makroen. Jeg tog fejl. Desuden virker S�rens send_float- og get_float-funktioner til UG s� lad os bruge dem.

Floktendenser

Her er et par indsigter, observationer og id�er. Ingen garantier gives.

Indsigt

N�r man k�rer mod et landmark - og det er det f�rste landmark man ser, b�r partiklerne danne en ring om det observerede landmark. Ogs� n�r man k�rer rundt om det. Ringen b�r bevares.

Observation

�v. S�dan opf�rer partiklerne sig ikke.

Symptom 1: Der er en ring til at starte med, men partiklerne finder hurtigt sammen i en klump

Ved translation: Flyt partikler i deres egen retning istedetfor at flytte dem alle i samme retning.

Symptom 2: Ok, det hjalp, men partiklerne finder stadig sammen i en klump. Der g�r bare l�ngere tid

Det er min teori at samplingen giver partiklerne en tendens til at flokkes.

id� 1: Lav en "bl�dere" sampling, der begr�nser hvor mange gange en partikel kan udv�lges. Dette kan naivt g�res i O(n^2 * log n). (Hvis du har en algoritme med bedre k�retid, kom med den!)

Kim siger: Lad v�re med at g�re dette! Ved at lave denne strategi �ndre man den fordeling man �nsker at approksimere og sample fra, dvs. at det er en forkert strategi. Husk at partiklerne er en approksimativ repr�sentation af fordelingen, dvs. �ndre vi p� hvordan vi placere og sampler partiklerne s� �ndre vi p� den fordeling vi �nsker at f�lge over tid.


id� 2: Bel�n outsiderne. Alts�: En partikel v�gtes h�jere hvis den ligger langt fra andre partikler der har en h�j v�gt. En meget simpel version kan laves ved blot at se p� hvor langt partiklen ligger fra estimated_pose. Dette virker - men partiklerne ligger sig i 2-3 grupper omkring landmarket - og ikke i en smuk ring som vi gerne ville have. Der er et problem med denne id�: Partiklerne vil brede sig ud i en ring om landmarket. Dette er godt n�r det er f�rste landmark vi ser, men ikke s� godt n�r vi ser det andet landmark . N�r vi ser det andet landmark udv�lges de partikler der ligger i sk�ringen mellem de to cirkler omkring de to landmarks. disse 2 gode punktm�ngder f�r vi ikke noget godt ud af at sprede omkring landmark #2. Id�en er alts� kun god hvis vi er meget usikre p� estimated_pose. Den feks med fordel benyttes s� l�nge man kun har set �t landmark.

Kim siger: Dette er ogs� en d�rlig ide af samme grunde som ovenfor. Man skal passe p� med at �ndre p� m�den man beregner v�gtene og sampling strategien.

Bj�rn

Tilpasning til odometri

Det er mit indtryk at m�lingerne fra odometrien konsekvent er for sm�, med st�rrelsesordenen 50%. Om det er gulvets glathed eller d�rlig software skal jeg ikke kunne sige. Det er dog �benlyst at det bliver v�rre ved h�j acceleration.

ide: En simpel model af odometrifejlene er at m�lingerne konsekvent afviger med en given faktor. Giv partiklerne en attribut "correction_factor". N�r partiklen skal flyttes i egen retning (ved translatering), korrigeres med partiklens "correction_factor". Partikler med en korrektion der passer nogenlunde med robotten-odemtrien har dermed st�rre sandsynlighed for at overleve. V�gtningen skal nok v�re rimelig mild (h�j varians).

Bj�rn

Problem med l�sning af sensor data

Hvis der er problemer med player/stage at l�se sensor data, via Read(), s� som at programmet venter p� data fordi man l�ser for hurtigt, eller man ikke l�ser hurtigt nok s� man f�r gammel data fra bufferen, kan man inds�tte f�lgende linier kode i starten af sit program:

robot.SetDataMode( PLAYERC_DATAMODE_PULL );
robot.SetReplaceRule( true, -1, -1, -1 );

Hvor robot er ens instans af PlayerClient

Personal tools