Positionsbestimmung von Asuro

[julien]

Hallo! Ich stehe vor einem Problem. Und zwar:
Ich möchte die Koordinaten ermitteln, in denen sich der Asuro vom Startpunkt aus (X=0; Y=0) befindet. Ich habe folgendes Programm zur Bestimmung des zurückgelegten Weges des linken- und rechten Rades per Odometrie geschrieben:

//Dieses Programm dient zur Ermittlung des zurückgelegten Weges des linken und rechten Rades.
//zusammengestellt und umgeschrieben von julien

#include "asuro.h"

int main(void)
{
    unsigned int Rmin = 1024, Rmax = 0, Lmin = 1024, Lmax = 0, Rmitte = 512, Lmitte = 512, data[2];
    unsigned int wegr=0, wegl=0;
	unsigned char flagl=FALSE, flagr=FALSE; 
    Init(); 
    MotorDir(FWD,FWD);
    MotorSpeed(100,125);
    while(1) {
        OdometrieData(data); // 0. links, 1. rechts		
        // max links
        if (data[0] > Lmax)
            Lmax += (data[0] - Lmax) / 2;
        // min links
        if (data[0] < Lmin)
            Lmin -= (Lmin - data[0]) / 2;
        // max rechts
        if (data[1] > Rmax)
            Rmax += (data[1] - Rmax) / 2;
        // min rechts
        if (data[1] < Rmin)
            Rmin -= (Rmin - data[1]) / 2;
	 Rmitte=(Rmax+Rmin)/2;
	 Lmitte=(Lmin+Lmax)/2;
		if ((data[0] < Lmitte) && (flagl == TRUE)) {
		flagl = FALSE;
		wegl++;
		}
		if ((data[0] > Lmitte) && (flagl == FALSE)) {
		flagl = TRUE;
		wegl++;
		} 
		if ((data[1] < Rmitte) && (flagr == TRUE)) {
		flagr = FALSE;
		wegr++;
		} 
		if ((data[1] > Rmitte) && (flagr == FALSE)) {
		flagr = TRUE;
		wegr++;
		}
     SerWrite("WL: ",4);
     PrintInt(wegl); //Ausgabe Weg linkes Rad
     SerWrite("WR: ",4);
     PrintInt(wegr); //Ausgabe Weg rechtes Rad);
     SerWrite("/r/n",2);
   }
    return 0;
}

Dieses Programm kann man bestimmt noch vielseitig dafür einsetzen (natürlich nur mit ein paar Änderungen). Zum Bestimmen der Koordinaten bräuchte man z.B. noch den Winkel, in dem sich Asuro relativ zum Ausgangspunkt befindet und Formeln, um die Koordinaten zu bestimmen. Naja, ich glaube, dass das nicht so einfach sein wird aber es währe auf jeden Fall mal einen Versuch wert (und nützlich währe es für viele Zwecke auch noch [z.B. virtuelle Landkarte]). Ich würde mich auf Lösungsvorschläge freuen.

MfG julien

[womb@t]

schon beim drehen treten ungenauigkeiten auf, das sieht man zb bei dem wettbewerb ein haus vom nikolaus zu malen!
Das mit koordinaten zu machen wir schwer werden

[pebisoft]

ein compassmocul cmps03 brauchst du unter anderem, ist ein sehr guter sensor. kann ich für die positionsbestimmung zusätzlich empfehlen. die anderen sachen die hier teilweise im forum genannt werden sind ungenau (steppmotor-wegstreckenmessung, irbake , die ir-bake ist immer ein guter ausgangspunkt oder anfahrpunkt um neu zu beginnen, ist aber nicht für die richtungsbestimmung geeignet).
die virtuelle karte ist auch nicht so einfach. ist zur zeit auch mein thema.
dazu setze ich ein : ir-bake im raum plus 2x je ein tsop1736 in einer filmdose mit einem 1mm senkrechten schlitz auf dem robby vorn rechts und links montiert, 2x ultra srf04 je auf servo 180grad schwenkbar, compassmodul cmps03 und cmucam2. klappt ganz gut. du siehst aber auch, was man alles für Sensoren braucht um diesen traum einigermassen zu verwirklichen. und diese Sensoren kann der kleine nicht alle verdauen, sei es gewichtsmässig nicht und pinanzahlmässig nicht.
mfg pebisoft

[Winne]

hi,

ich bin ebenfalls dabei eine Virtuellekarte zu "zeichen". Die Auswertung bei der Odometrie ist im Augenblick der Schwerpunkt. Mit einem Testprogramm versuch ich einen Weg zu finden, wo die Odometrie halbwegs funktioniert.
Falls interesse besteht kann ich das Programm posten. Die Beschreibung müsste ich aber erstmal überarbeiten.

Gruss Winne

[julien]

Ja, das mit der Odometrie hab ich ja schon in den Griff bekommen (geht bei jedem Umgebungslicht [siehe obriges Programm]). Bloß das mit der virtuellen Landkarte fehlt mir, wie man das realisieren kann. Währe schön, wenn du mir da weiterhelfen könntest.

[Winne]

Hier ist ein Link, der einen interessanten Ansatz liefert.

http://www.mindrobots.de/public/algorithms/sosmar.pdf

Gruss winne

[pebisoft]

das wird der asuro nicht verkraften. da baut euch man eine gössere plattform und verwendet dafür die erworbenen kenntnisse vom asuro.
mfg pebisoft

[julien]

Ich meine ja nur mit Hilfe der Odometrie. Mein Programm oben macht das doch ziemlich genau. Man müsste nur mit den Wegstrecken der Räder die Position bestimmen können, bloß mir fällt da kein Algorithmus für eine einfache zweidimensionale virtuelle Karte ein. Weiß jemand, wie das geht?
MfG julien

[julien]

Ich hab jetzt ein Programm geschrieben. Das war ein Haufen Arbeit. Ich hoffe, dass es vom Prinzip her funktionieren würde. Ich weiß nicht, woran der Fehler liegt. Die Status LED flackert nur, daher behaupte ich, dass da ein Fehler im Programm sein muss.

#include "asuro.h"
#include <math.h>
int main(void){
	float w=0,r=0,puls=1;
	unsigned int obj=0, objx[500], objy[500], Rmin = 1024, Rmax = 0, Lmin = 1024, Lmax = 0, Rmitte = 512, Lmitte = 512, data[2], wegr=0, wegl=0, richt=1, x=0, y=0, i=0; 
    unsigned char flagl=FALSE, flagr=FALSE, eingetr=FALSE;
	Init();
   DDRD |= (1 << DDD1);      // Port D1 als Ausgang 
   PORTD &= ~(1 << PD1);      // PD1 auf LOW 
   puls = 1;
	while(1){
	      if (PIND & (1 << PD0)){
         StatusLED(GREEN);   // kein Hindernis
		 } else {
		if(eingetr==FALSE){
		objx[obj]=x;
		objy[obj]=y;
		obj++;
		eingetr=TRUE;
		}
		richt=2;
		StatusLED(RED);      // Hindernis erkannt
		}
   puls = 1.02 * puls;      // Pulsbreite wird um 2% erhöht 
   if (puls > 10){
   if (PIND & (1 << PD0)){ richt=1; eingetr=FALSE; }
   puls = 1; }
   OCR2 = 255 - (int)(puls);
	if(richt==0) MotorDir(BREAK,BREAK);
    if(richt==1) MotorDir(FWD,FWD);
    if(richt==2) MotorDir(BREAK,FWD);
    MotorSpeed(150,150);
	
	for(i=0;i<=obj;i++){
	if((x<=objx[i]+10) && (y<=objy[i]+10)) richt=2;
	}
//-----------------------Schritte zählen-----------------------------------
	  OdometrieData(data); // 0. links, 1. rechts
        // max links
        if (data[0] > Lmax)
            Lmax += (data[0] - Lmax) / 2; 
        // min links 
        if (data[0] < Lmin)
            Lmin -= (Lmin - data[0]) / 2;
        // max rechts
        if (data[1] > Rmax)
            Rmax += (data[1] - Rmax) / 2;
        // min rechts
        if (data[1] < Rmin)
            Rmin -= (Rmin - data[1]) / 2;
         Rmitte=(Rmax+Rmin)/2;
         Lmitte=(Lmin+Lmax)/2;
      if ((data[0] < Lmitte) && (flagl == TRUE)) {
      flagl = FALSE;
      }
      if ((data[0] > Lmitte) && (flagl == FALSE)) {
      flagl = TRUE;
      wegl++;
      }
      if ((data[1] < Rmitte) && (flagr == TRUE)) {
      flagr = FALSE;
      }
      if ((data[1] > Rmitte) && (flagr == FALSE)) {
      flagr = TRUE;
      wegr++;
      }
//------------------------------------------------------------------------
//------------------------POSITIONSBESTIMMUNG-----------------------------
w=1.607143*(wegl-wegr); //winkel
r=((wegl+wegr)/2)*w;
x=(int)(r*cos(w));
y=(int)(r*sin(w));

	}
	return(0);
}

[Rakke]

Hei Julien,

Posistionsbestimmung über Odo interessiert mich auch. Eine Odo habe ich noch nie geschrieben, ich stelle es mir ungefähr so vor: Position vom Asuro ist darstellbar als x,y,phi (Verdrehung)

• Odo_li und Odo_re vergleichen
  wenn gleich: Stückchen gradaus gefahren, also letzte Position verschieben, Verdrehung gleich halten
  wenn unterschiedlich: Bogen gefahren; Radius und Mittelpunkt des Bogens ermitteln, letzten Punkt verschieben, Verdrehung aktualisieren

Zugegeben etwas theoretisch - habs halt noch nicht gemacht. Ich schätze, die Tücke liegt im Detail: die Rechnerei stinkt förmlich nach trigonometrischen Funktionen ((arc-)sin, cos, tan), das wird den kleinen Asuro-Prozessor ganz schön fordern.

Es sei denn... jemand hat eine bessere Idee? Habe noch nicht in den Weiten des Internets geforscht.

Was soll denn eigentlich 'rein in deine Karte? Der Link von Winne zeigt jedenfalls mal, was man mit geschickten Karten alles erledigen kann...Respekt!

Gruß Rakke