Algorithmus zur Lokalisation auf einem Fussballfeld gesucht

Hallo zusammen,

ich suche einen (mikrocontrollertauglichen) Algorithmus, um mich auf einem Tisch mit hohem Rand mit Hilfe von Abstandsensoren lokalisieren zu können. Der Tisch hat zum Beispiel Abmaße von 2mx1m ich verwende 3 Laser Abstandssensoren im Winkel von 120 Grad. Der eine spuckt mir einen Wert von 50, der zweite von 70 und der dritte von 120 Zentimetern zum Rand aus. Frage: Wo bin steht der Robby auf dem Tisch?

Bislang habe ich es so gemacht, dass ich über meinen Tisch ein Netz mit einer Auflösung von 2 cm gelegt habe und dann für alle Punkte markiert habe die 50cm und weniger von einer Wand entfernt sind. Dann eine zweite Matrix mit allen Punkten die 70 cm und weniger von der Wand entfernt sind und eine dritte ... Dann habe ich die 3 Matrizen übereinander gelegt und geschaut ob es Punkte gibt, die in allen 3 Matrizen enthalten sind. Meist waren das wegen der Ungenauigkeit der Sensoren mehrere.

Das Problem ist nur ... das Verfahren ist zu aufwendig für meinen 8bit Controller :-( Kann man das Ganze nicht irgendwie analytisch lösen. So was muss doch schon mal irgendwer effektiver gelöst haben.

Freue mich auf Eure Anmerkungen, Links, Hinweise

Sebastian

Denks dir mal durch.

Den Gedankengang erschwerst du dir schon, dass du die Sensoren je 120 Grad angeordnet hast. Angenommen du hast 4 Abstandssensoren N, S, W, O. Dann kannst du ziemlich genau sagen, wo sich der Roboter befindet - WENN er parallel zu einem Seitenrand ist!

Ist er schräg dann wirds schwerer das Ganze auszuwerten.

Die meisten Roboter haben für solche Operationen auch noch einen Kompass, dieser ist jedoch leicht durch Elektronik zu stören. Ultraschallsensoren würden meiner Meinung auch noch Sinn machen um weitere Gegenstände zu identifizieren.

Angenommen du nimmst den Kompass:
Dann könntest du deine Richtung recht genau bestimmen, die differenziellen Grade zur parallelen Bande bestimmen und die Abstandssensoren dann darauf umrechnen.

=> Arbeit. Abstand wählen => Werte austesten etc.
=> recht statisch auf ein bestimmtes Feld bezogen.

Interessant für dich ist sicherlich das hier:
http://www.scipub.org/fulltext/jcs/jcs62173-179.pdf

Hallo!

@ Sebastian.Zug

Ich stimme dem RoboNull völlig zu, dass du dir die Entwicklung selber erheblich erschwerst. Theoretisch um 2D Position zu bestimmen reichen zwei um 90° drehbare Laser in zwei Ecken aus.

MfG

Hallo zusammen,

Ihr habt natürlich recht, die gängigen Verfahren beruhen auf einer deutlich größeren Menge von Punkten, die zum Beispiel mit Laserscannern erzeugt und aus einem Kamerabild gefiltert wurden.

Ich habe am Wochenende beim RoboCub aber auch die Liga für Kinder angeschaut, deren größtes Problem darin bestand festzustellen wo man sich befindet. Nun habe ich die oben beschriebene, sehr aufwendige Lösung gehackt ... für Mikrocontroller aber völlig unbrauchbar.

Nun versuche ich das ganze in ein Gleichungssystem zu drücken und dabei den Rechenaufwand möglichst gering zu halten.

... in dem Paper von @RoboNull nutzt man einen anderen Ansatz, da werden die Roboter von einem stationären Kamerasystem getrackt, ich möchte das ganze aber gern auf dem Roboter laufen lassen.

Na vielleicht fällt noch jemandem was ein ...

Hallo,
Du hast also 3 Abstandssensoren im Winkel von 120° auf dem Robi? Die Position kannst mit ein paar Winkelfunktionen ausrechnen, einfach mal bei Dreiecken nachschauen.
Der Haken an der Sache ist, wenn der Robi beliebig fahren darf, gibt es 4 korrekte Ergebnisse! Du weisst ja nicht wo der kürzeste Abstand ist, Norden, Süden, Osten, Westen?
In Kombination mit einem Kompass müsste es eindeutig sein. Dazu braucht man ein globales=Feld und ein örtliches=Robi Koordinatensystem, und kann so das örtliches System immer am globalen ausrichten.
Habe mir sowas ähnliches schon selber überlegt, aber muss das erst mal eine Idee haben wie man das auf 25x43m umsetzt ohne dass man von einem Laser erblindet.

LG!

Sebastian,

da hast Du ein sehr interessantes Problem gestellt :-) !

Lass' uns mal schnell die Bekannten und Unbekannten des Gleichungssystems durchgehen:
Bekannt sind die Richtungen der Peilrichtungen im fahrzeugfesten Koordinatensystem. Ausserdem sind die drei Entfernungen entlang der Peilrichtungen zur Spielfeldbegrenzung (Bande) bekannt.
Gesucht sind die x- und y-Koordinate des Standortes im spielfeldfesten Koordinatensystem und der Drehwinkel des Fahrzeugs gegenüber einer dieser beiden Koordinaten (3 Unbekannte). Unbekannt sind auch die Lage der Schnittpunkte der Bande mit den Peilrichtungen (3 Unbekannte). Es geht also in Summe um 6 Unbekannte.

Mit den bekannten Grössen können wir drei Gleichungen aufstellen; für jeden Schnittpunkt zwischen Peilrichtung und Bande gibt's eine Gleichung.

Im Moment überblicke ich noch nicht, ob diese Gleichungen Vektor- oder Skalargleichungen sind. Wenn's Vektorgleichungen sind, ist das Problem lösbar (6 Gleichungen mit 6 Unbekannten). Sind die Gleichungen skalar, dann fehlen 3 Gleichungen.

Wie denkst Du darüber?

Ciao,

mare_crisium

EDIT: Habe 3 Unbekannte übersehen, nämlich die Nummern der Banden, mit denen die Peilrichtungen sich schneiden. Das macht in Summe 9 Unbekannte. Damit wäre das Problem mit drei Peilrichtungen nicht lösbar. - Oder?

Eine eindeutige Lösung kann es nicht geben, es müsste aber möglich sein das ganze iterativ zu lösen.
Meine Idee ist folgende: Wenn man von jedem Randpunkt einen Kreis aufspannt mit dem Radius a (z.b die gemessenen 50) und von jedem Punkt an diesem Kreis einen weiteren Kreis zeichnet mit Radius b (z.b 70) und dann nachsieht wo der 2te Kreis die Seitenwand schneidet. Wenn die 3 Punkte einen Winkel von 120° einschließen müsste man eine potenzielle Lösung haben. Um die letzten Falschen noch zu eliminieren zeichnet man einen 3ten Kreis mit Radius c vom Mittelpunkt von Kreis 2 aus und schneidet wiederum mit der Wand. So müsste es doch funktionieren (schnell ist es zwar nicht wirklich, aber vielleicht kann man es noch vereinfachen)

LG XBert

Hallo zusammen,

vielen Dank für Eure Beiträge! Ich würde mich freuen, wenn wir die Diskussion in dieser Intensität fortsetzen !!!

Ich habe mich nun auch an die analytische Lösung gemacht. Wie schon @mare_crisium deutlich machte, kann man mithilfe einiger trigonometrischer Gleichungen die Position bestimmen. Schwierig ist das Ganze nur wegen der verschiedenen Fälle, die es zu beachten gilt: Der Rechenweg unterscheidet sich zum Beispiel für die Fälle dass:

• * jeder Sensor eine andere Wand erfassen
  * 2 Sensoren zu einer gemeinsamen Wand messen

Wenn man nun überlegt, dass es 4 Wände (A,B,C,D) gibt, dann kann man für 3 Sensoren (m1,m2,m3) eine Tabelle aufstellen, die die Fälle die @mare_crisium im Edit zu seinem Post andeutet, zu erschlagen

Code:

---

Fall          m1      m2      m3
1              A        B        C
2              A        A        B
3              A        B        B
usw

---

Wie viele gültige Kombinationen es gibt muss ich mir noch überlegen. Im Prinzip muss ein Programm nun in allen Fällen nach einer Lösung oder einem Lösungsraum suchen.

Für den Fall (1) aus obiger Tabelle möchte ich einen Berechnungsweg zur Diskussion stellen, der auf die anderen (3 Sensoren sehen 3 Wände) Fälle übertragbar ist. Für Fall (1) kann es anders als bei (2) nur eine eindeutige Lösung geben kann. Es geht also darum, die bekannten Faktoren so zu kombinieren, dass entweder eine wahre Aussage entsteht oder ein Widerspruch produziert wird.

Ich habe mich auf die Winkelsumme von alpha, beta, gamma und phi konzentriert, die ja 180 Grad ergeben müsste. Dafür habe ich voneinander unabhängige Gleichungen dafür aufgestellt. Ich werde es dann gleich mal in einem Skript testen ... aber vielleicht findet ja jemand von Euch einen Weg der 2 oder 3 Rechenschritte weniger braucht :-)

Grüße und ein schönes Wochenende

Sebastian

PS: Die Idee von @XBert habe ich leider nicht verstanden, vielleicht kannst Du sie mit einer Skizze verdeutlichen. Danke.

PSS: Natürlich hat @damfino mit seiner Überlegung recht, dass sich Mehrdeutigkeiten und schlimmstenfalls Lösungsräume ergeben. Die möchte ich damit abfangen, dass ich den Roboter in seiner Bewegung ausgehend von einer bekannten Inertialposition anhand einer Odometrie tracke und die Positionsschätzung der Sensoren zur Korrektur gegenüber dem Absolutsystem verwende.

Sebastian,

ich gehe solche Sachen lieber mit Vektorrechnung an, das ist für mich anschaulicher als die Trigonometrie und liefert dieselben Ergebnisse. Deshalb habe ich ein pdf angehängt, in dem ich versucht habe zu erklären, wie ich an die Sache herangehen würde.

Aus den Gründen, die ich in meinem letzten Post erklärt habe, bin ich der Meinung, dass das Problem unlösbar ist. Dann habe ich aber folgendes Gedankenexperiment gemacht (wenn ich's in Wirklichkeit probiert hätte, hätte ich Ärger mit meiner Herzdame bekommen :-) ):

Man stecke drei Schaschlik-Spiesse um 120° versetzt in eine Kartoffelscheibe. Die herausstehenden Enden sollen die Entfernungen zu den Banden darstellen. Jetzt lege man die Scheibe auf die Glasscheibe eines gerahmten Bildes :oops: oder auf ein rechteckig eingefasstes Tablett. Wenn es gelingt, die Scheibe in eine Position zu manövrieren, in der jede der drei Spiess-Enden den Rahmen berührt, dann gibt's eine Lösung. Das schien mir dann doch nicht so umöglich zu sein ;-) .

Dabei ist mir klargeworden, dass sich jede Lösung wiederholt. Bei einem rechteckigen Spielfeld zweimal (um 180° gedreht) und bei einem quadratischen viermal (um je 90° gedreht). Insofern hat XBert mit der vermuteten Mehrdeutigkeit recht.

Ciao,

mare_crisium

EDIT: Anhang gelöscht, wg. Upload-limit

Vielleicht kann man synthetisch vorgehen (statt analytisch). Stichwort MonteCarlo Localisation.

Wenn Du glaubst, dass der Roboter sich an Position (x, y, a) befindet kann man errechnen was Laser 1 (der nach vorne in Winkelrichtung a schaut) theoretisch für einen Abstand zum Rand messen müsste (Solldistanz). Das kann man für alle drei Laser machen:
sd1 ist Solldistanz für Laser 1 mit Winkel a.
sd2 ist Solldistanz für Laser 2 mit Winkel a+120.
sd3 ist Solldistanz für Laser 3 mit Winkel a-120.

Das kann man mit den Istdistanzen (also den konkreten Messwerten) der Laser vergleichen.
id1 ist Istdistanz von Laser 1 mit Winkel a.
id2 ist Istdistanz von Laser 2 mit Winkel a+120.
id3 ist Istdistanz von Laser 3 mit Winkel a-120.

Jetzt kann man x, y und a ein wenig und vorsichtig variieren, produziert/synthetisiert sich also eine kleine Menge von
Positionsvarianten.

Schließlich nimmt man genau die Positionsvariante als neue/bestätigte Position des Roboters an, welche mit
ihren sd-Werten am besten mit den tatsächlichen id-Werten übereinstimmt.

Mit dem folgenden Code könnte man die sd-Werte (also sd1, sd2 und sd3) berechnen. (Stimmt der?)

Code:

---

double dsim(double x, double y, double a) {
        double xd=max(xmax, ymax), yd=max(xmax, ymax);
        double c = cos(a);
        double s = sin(a);
        if (c < 0)        xd = (0    - x) / c;
        if (c > 0)        xd = (xmax - x) / c;
        if (s < 0)        yd = (0    - y) / s;
        if (s > 0)        yd = (ymax - y) / s;
        double dsim = (xd < yd ? xd : yd);
        return dsim;
}

---