@HBhannover
Ich bin kein Spezialist für 3D-Transformationen und für Matrizenrechnung schon gar nicht, deshalb alles Folgende ohne Gewähr:
Ich gehe von folgenden Annahmen aus:
- Du verwendest ein rechtsdrehendes Koordinatensystem
(x-Achse in Fahrtrichtung des Roboters, y-Achse nach links, z-Achse nach oben)
- Dein Roboter bewegt sich auf einer Ebene (= die z-Werte ändern sich von einer Position zur nächsten nicht.
- Deinem Roboter sind folgende Bewegungungen möglich
a) Translation T = der Roboter verändert seine Position in der xy-Ebene;
b) Rotation R = Drehung um die z-Achse;
c) Skalierung s = es findet keine Skalierung = Änderung des Größenmaßstabs statt,
also: s =1
Damit musst Du meiner Meinung nach nur bis Seite 12 etwas verstehen, denn die Bursa-Wolf -Transformation behandelt spezielle Fälle der Umrechnung von Geodäsie-Daten, also von einer Landkarten-Projektion in eine andere. Es sei denn, Dein Roboter verwendet GPS.
Du willst die Daten von Position1 (x1,y1,z1) auf die Position2 (x2,y2,z3) transformieren.
Wenn Du die Winkelabweichung der x-Richtung von einer Position1 zur Position2 aus den Odometriedaten kennst, drehst Du mit einer Rotationsmatrix für die Drehung um die z-Achse (siehe Seite 8 ) die x-Achse der Pos1 bis sie mit der x-Achse von Pos2 übereinstimmt.
(Winkel kappa = Drehwinkel)
Zu den nun gedrehten Werten addierst Du nun die Translationswerte Tx,Ty,Tz
Tx = x2-x1, Ty = y2-y1, Tz =0, da der Roboter in einer Ebene fährt.
In C# sieht das dann beispielsweise so aus:
x2 = x1 * Math.Cos(kappa) + y1 * Math.Sin(kappa) + tx2);
y2 = x1 * (-Math.Sin(kappa)) + y1 * Math.Cos(kappa) + ty2);
z2 = z1;
Viel Erfolg,
Günter
Zitieren
Lesezeichen