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;
}
Lesezeichen