Nochmals: Distanzberechnung mit kartesichen Winkelberechnung bei Drehung des Bezugspunktes
-
Hallo Werner,
danke für diese ausführliche Antwort. Ich hatte mir sowas bereits
gedacht. Hatte aber keinen wirklichen Einstieg im Netz gefunden.Hier habe ich erstmal eine Menge zum Nachlesen und nachvollziehen.
Dein Beispiel, sofern ich das hier sehe, ist ja schon die Lösung.
Gruss
-
Hallo Zusammen (Werner),
irgendwie habe ich immer noch Ärger mit dieser Klasse.
Wenn ich den Roboter rotieren lasse, bekomme ich falsche Werte für X / Y.
Mittlerweile hatte ich noch die Ausrichtung der einzelnen Sensoren bei Ausrichtung 0 eingefügt.
Der Roby ist eine Rechteck mit Sensoren an allen Seiten Die Sensoren sind hier jeweils um 90 Grad versetzt montiert, eben in jede Himmelsrichtung.Bei Winkeln, welche nicht im 90 Grad Raster sind, kommen falsche Werte für X und Y zu Tage ?.
Die fltSensorAusrichtung[sensor] Variablen, sind ein Array für die Sensorausrichtung.
Auf welchen Bezug liegt der Wert "0.0" (Zeile
in der Klasse Lage ?#include <iostream> #include <complex> #include <cmath> class Lage { public: explicit Lage( const std::complex< double >& rot = std::complex< double >( 1.0, 0.0 ), const std::complex< double >& trans = std::complex< double >() ) : m_rot( rot ), m_pos( trans ) {} std::complex< double > operator*( const std::complex< double >& v ) const { return m_rot * v + m_pos; } Lage& operator*=( const Lage& b ) { m_pos += m_rot * b.m_pos; m_rot *= b.m_rot ; return *this; } private: std::complex< double > m_rot, m_pos; }; Lage operator*( Lage a, const Lage& b ) { return a *= b; } const double GRAD2RAD = std::acos(-1.0) / 180.0; int main() { using namespace std; // der Roboter double Xa = 60.0; double Ya = 50.0; double alpha = 30.0; // Grad // der Sensor auf dem Roboter double Xs1 = 10.0; double Ys1 = 5.0; double S = 100.0; // Objektanstand vor Sensor // world_T_rob ist die Lage des Roboters im Welt-System // Rotation Position Lage welt_T_rob( polar( 1.0, alpha * GRAD2RAD ), complex< double >( Xa, Ya ) ); // rob_T_sensor ist die Lage des Sensors im Roboter-Systen // Bem.: Winkel also Ausrichtung des Sensors bezogen auf den Roboter == 0.0 Lage rob_T_sensor( polar( 1.0, fltSensorAusrichtung[sensor]), complex< double >( Xs1, Ys1 ) ); Lage welt_T_sensor = welt_T_rob * rob_T_sensor; // Lage Sensor im Welt-System complex< double > welt_p_obj = welt_T_sensor * complex< double >( S, fltSensorAusrichtung[sensor] ); cout << "Die Position des Objekts ist " << welt_p_obj << endl; return 0; }Für eine kleine Hilfe wäre ich sehr dankbar.
Gruss
-
Hallo Ritchie,
Dein Code kann kein korrektes Ergebnis liefern, da Du die Variable 'fltSensorAusrichtung[sensor]' einmal als Winkel mit der Einheit [rad] (in Zeile 53) und einmal als Position mit der Einheit 'Länge' (in Zeile 57) benutzt.
Ritchie schrieb:
Die fltSensorAusrichtung[sensor] Variablen, sind ein Array für die Sensorausrichtung.
Ich unterstelle mal, dass hier der Winkel des Sensors gegenüber dem Roboter in der Einheit Grad enthalten ist. Dann muss die Zeile 53 richtig heißen:
Lage rob_T_sensor( polar( 1.0, fltSensorAusrichtung[sensor] * GRAD2RAD ), complex< double >( Xs1, Ys1 ) );Natürlich müssen Xs1 und Ys1 auch für jeden Sensor angepasst werden.
Die von Dir angegebene Graphik ist leider nicht mehr verfügbar. Kannst Du mir noch mitteilen, wo die Sensoren liegen? (Positionen X,Y und Richtung in Grad alles bezogen auf den Roboter)
Ritchie schrieb:
Auf welchen Bezug liegt der Wert "0.0" (Zeile
in der Klasse Lage ?Der Wert 0.0 ist der Sinus des Winkels von Lage gegenüber dem Bezugssystem. Ein Lage-Objekt gibt u.a. den relativem Winkel zwischen dem Bezug- zum Zielsystem an. Der Wert 1.0 davor ist der Cosinus des gleichen Winkels. Also bei sin(phi)=0 und cos(phi)=1 ist phi=0Grad
Gruß
WernerEdit: Antwort auf Ritchies letzte Frage korrigiert
-
Hallo Werner,
danke nochmals für die Hilfe:
Hier das gesamte Test-Programm:
Hier die Klasse für die Sensorumrechnung für das eigentliche Hauptprogramm:
#include <stdio.h> #include <math.h> #include <iostream> #include "sensordata.h" #include "roboterpose.h" #define WEGPRONAVIGATIONSSTRECKE M_PI // Achtung, A und B Werte k�nnen springen // A offset der Sensoren zum Mittelpunkt double SensorData::fltAbstandX[16]={ +20.0, +20.0, +20.0, +20.0, +17.5, +6.0, -6.0, -17.5, -20.0, -20.0, -20.0, -20.0, -17.5, -6.0, +6.0, +17.5}; // Ausrichtung Y des Sensors double SensorData::fltAbstandY[16]={ -10.5, -4.0, +4.0, +10.5, +14.5, +14.5, +14.5, +14.5, +10.5, +4.0, -4.0, -10.5, -14.5, -14.5, -14.5, -14.5}; // Ausrichtung des Sensors in rad bei Robby == 0 rad double SensorData::fltSensorAusrichtung[16]={0.0, 0.0, 0.0, 0.0, (M_PI/2.0), (M_PI/2.0), (M_PI/2.0), (M_PI/2.0), M_PI, M_PI, M_PI, M_PI, -(M_PI/2.0), -(M_PI/2.0), -(M_PI/2.0), -(M_PI/2.0)}; // ************************************************************************** // Konstruktor // ************************************************************************** SensorData::SensorData(void) { Akt_X=0.0; Akt_Y=0.0; fltDistanz=0.0; fltAusrichtung=0.0; intGefunden=false; intObjekt_X=0; intObjekt_Y=0; } // ************************************************************************** // Destruktor // ************************************************************************** SensorData::~SensorData(void) { } // ************************************************************************** // Gib Zurueck, ob der US Sensor den kleinern Wert hat // ************************************************************************** int SensorData::IsUSSensorValid(int intUSWert, int intIRWert) { if(intUSWert == 0) { return false; } else { if( intIRWert == 0 ) return 1; else { if( intIRWert < intUSWert) return 0; else return 1; } } } // ************************************************************************** // Berechne die Pose des Objekts // ************************************************************************** void SensorData::BerechnePose(void) { double xwert,ywert; pose welt_T_rob( std::polar( 1.0, fltAusrichtung), std::complex< double >( Akt_X, Akt_Y ) ); // rob_T_sensor ist die Lage des Sensors im Roboter-Systen // Bem.: Winkel also Ausrichtung des Sensors bezogen auf den Roboter == 0.0 pose rob_T_sensor( std::polar( 1.0, fltSensorAusrichtung[intSensor] ), std::complex< double >( fltAbstandX[intSensor], fltAbstandY[intSensor] ) ); pose welt_T_sensor = welt_T_rob * rob_T_sensor; // Lage Sensor im Welt-System std::complex< double > welt_p_obj = welt_T_sensor * std::complex< double >( fltDistanz, fltSensorAusrichtung[intSensor] ); xwert=welt_p_obj.real(); ywert=welt_p_obj.imag(); intObjekt_X= (int) ( xwert / WEGPRONAVIGATIONSSTRECKE); intObjekt_Y= (int) ( ywert / WEGPRONAVIGATIONSSTRECKE ); } // ************************************************************************** // Nummer des Sensors, von dem die Daten stammen // ************************************************************************** void SensorData::SetAbs_Ausrichtung(double fltWert) { fltAusrichtung=fltWert; } double SensorData::GetAbs_Ausrichtung(void) { return fltAusrichtung; } // ************************************************************************** // Nummer des Sensors, von dem die Daten stammen // ************************************************************************** void SensorData::SetDistanz(double fltWert) { intGefunden=false; fltDistanz= fltWert; } double SensorData::GetDistanz(void) { return fltDistanz; } // ************************************************************************** // Nummer des Sensors, von dem die Daten stammen // ************************************************************************** void SensorData::SetSensor(int intWert) { intGefunden=false; // Zwischenwert loeschen if ( intWert > 0 ) intWert --; intSensor=intWert; } int SensorData::GetSensor(void) { return intSensor+1; } // ************************************************************************** // Absolute kartesiche X-Koordinate des Objekt // ************************************************************************** void SensorData::SetAbs_X(int intWert) { intGefunden=false; // Zwischenwert loeschen Akt_X= (double) intWert * WEGPRONAVIGATIONSSTRECKE; } int SensorData::GetAbs_X(void) { if( intGefunden == false ) { BerechnePose(); intGefunden=true; } return intObjekt_X; } // ************************************************************************** // Absolute kartesiche Y-Koordinate des Objekt // ************************************************************************** void SensorData::SetAbs_Y(int intWert) { intGefunden=false; // Zwischenwert loeschen Akt_Y=(double) intWert * WEGPRONAVIGATIONSSTRECKE; } int SensorData::GetAbs_Y(void) { if( intGefunden == false ) { BerechnePose(); intGefunden=true; } return intObjekt_Y; }Der Define .h File
----------------------------class SensorData { private: static double fltAbstandX[16]; // Strecke Y vom Mittelpunkt zum Sensor static double fltAbstandY[16]; // Strecke X vom Mittelpunkt zum Sensor static double fltSensorAusrichtung[16]; // Ausrichtung des Sensors zum 0 Punkt // Zwischenspeicher der Memberfunctions double fltAusrichtung; // Momentane Ausrichtung des Roboter in rad int intSensor; // Nummer des Sensors, von dem die Daten sind double fltDistanz; // Distanzwert gemessen vom Sensor double Akt_X; // Absolute X Position des Mittelpunkt double Akt_Y; // Absolute Y Position des Mittelpunkt int intObjekt_X; // Position X ermittelt in der Weltkarte int intObjekt_Y; // Position Y ermittelt in der Weltkarte int intGefunden; // Flag, Berechnung bereits durchgeführt void BerechnePose(); public: SensorData(); ~SensorData(); void SetAbs_Ausrichtung( double ); double GetAbs_Ausrichtung( void ); void SetDistanz( double ); double GetDistanz( void ); void SetSensor( int ); int GetSensor( void ); void SetAbs_X(int); int GetAbs_X(void); void SetAbs_Y(int); int GetAbs_Y(void); int IsUSSensorValid(int,int); };Die Klasse "Pose"
#include <complex> #include <cmath> #ifndef _ROBOTER_POSE #define _ROBOTER_POSE using namespace std; class pose { public: explicit pose( const complex< double >& rot = complex<double>( 1.0, 0.0 ), const complex< double >& trans = complex< double >() ) : m_rot( rot ), m_pos( trans ) { } complex< double > operator*( const complex< double >& v ) const { return m_rot * v + m_pos; } pose& operator*=( const pose& b ) { m_pos += m_rot * b.m_pos; m_rot *= b.m_rot ; return *this; } private: complex< double > m_rot, m_pos; }; pose operator*( pose a, const pose& b ) { return a *= b; } #endifDas Hauptprogramm zum Testen der Wert.
/****************************************************************************/ //includes /****************************************************************************/ #include <stdio.h> #include <math.h> #include <iostream> #include <string> #include "sensordata.h" #define ZWEI_PI 6.2831853 SensorData SensorKoordinaten; /****************************************************************************/ // main /****************************************************************************/ int main(int argc, char *argv[]) { int intAktX,intAktY; // Aktuelle Position des Robby int x,y; // Hindernis Pos. float fltTest; // Eingabehilfe double fltAktWinkel; // Errechnter Akt. Winkel float fltDistanz; // Distanz zum Objekt int i; // Schleifenzaehler printf("------ Test Program Hinderniskarte ------\n"); printf("Akt. Winkel (PI/2*x) (0->4): "); scanf("%f", &fltTest); if( fltTest > 4.0 ) fltTest=4.0; fltAktWinkel=(2.0*M_PI/4.0)*fltTest; printf(" Aktueller Winkel : %f\n",fltAktWinkel); printf(" Aktuelle X Pos : "); scanf("%i", &intAktX); if(intAktX < 10 ) intAktX = 10; printf(" Aktuelle Y Pos : "); scanf("%i", &intAktY); if(intAktY < 10 ) intAktY = 10; printf("Distanz Objekt (cm) : "); scanf("%f", &fltDistanz); fflush(stdin); SensorKoordinaten.SetAbs_X( intAktX ); SensorKoordinaten.SetAbs_Y(intAktY ); SensorKoordinaten.SetAbs_Ausrichtung( fltAktWinkel ); SensorKoordinaten.SetDistanz( fltDistanz ); printf("\n------------ Sensor Daten ----------\n"); printf("Nr.\t\tX-Pos\t\tY-Pos\n"); printf("---------------------------------------\n"); printf("Vorne \n"); for(i=1;i<17;i++) { SensorKoordinaten.SetSensor( i ); x=SensorKoordinaten.GetAbs_X(); y=SensorKoordinaten.GetAbs_Y(); printf("%i",i); printf("\t\t%i\t\t%i\n",x,y); if( i == 4 ) printf("rechts \n"); if( i == 8 ) printf("hinten \n"); if( i == 12 ) printf("links \n"); } return 0; }Alle Wert innerhalb meiner Klasse sind in rad.
Innerhalb des Programmes werden die Daten von 0 -> 2PI verarbeitet.Die Grafik ist jetzt wieder verfügbar.
Weitere Grafik nur als Zusatzinformationen:
Hier der Quellcode als ZIP-Datei:
Gruss
-
.. muss Dich leider vertrösten, kann wahrscheinlich erst morgen Abend antworten.
Gruß
Werner
-
Kein Problem.
Trotzdem vielen Dank schon im voraus.
Hier noch einige Infos.
Mein Koordinatensystem hat oben links 0.0 und unten rechts 300,300.
Folgende Daten sind bei den Testläufen herausgekommen:
------ Test Program Hinderniskarte ------ Akt. Winkel (PI/2*x) (0->4): 0 Aktueller Winkel : 0.000000 Aktuelle X Pos : 31 Aktuelle Y Pos : 31 Distanz Objekt (cm) : 31.4 ------------ Sensor Daten ---------- Nr. X-Pos Y-Pos --------------------------------------- Vorne 1 47 27 2 47 29 3 47 32 4 47 34 rechts 5 36 45 6 32 45 7 28 45 8 24 45 hinten 9 14 33 10 14 31 11 14 28 12 14 26 links 13 24 16 14 28 16 15 32 16 16 36 16Diese Daten sind korrekt.
Wenn ich jetzt den Roboter um 180 Grad drehe:
------ Test Program Hinderniskarte ------ Akt. Winkel (PI/2*x) (0->4): 2 Aktueller Winkel : 3.141593 Aktuelle X Pos : 31 Aktuelle Y Pos : 31 Distanz Objekt (cm) : 31.4 ------------ Sensor Daten ---------- Nr. X-Pos Y-Pos --------------------------------------- Vorne 1 14 34 2 14 32 3 14 29 4 14 27 rechts 5 25 16 6 29 16 7 33 16 8 37 16 hinten 9 47 28 10 47 30 11 47 33 12 47 35 links 13 37 45 14 33 45 15 29 45 16 25 45Sind auch diese Daten korrekt (eben nur gespiegelt)
Ein Test bei nur 90 Grad zeigt:
------ Test Program Hinderniskarte ------ Akt. Winkel (PI/2*x) (0->4): 1 Aktueller Winkel : 1.570796 Aktuelle X Pos : 31 Aktuelle Y Pos : 31 Distanz Objekt (cm) : 31.4 ------------ Sensor Daten ---------- Nr. X-Pos Y-Pos --------------------------------------- Vorne 1 34 47 2 32 47 3 29 47 4 27 47 rechts 5 16 36 6 16 32 7 16 28 8 16 24 hinten 9 28 14 10 30 14 11 33 14 12 35 14 links 13 45 24 14 45 28 15 45 32 16 45 36Hier stimmen schon die Y Wert von Vorne nicht mehr. Sie sollten kleiner 31 sein.
Wie es scheint ist die Drehrichtung vertauscht. Aber wieso ? Eigentlich sollte der
Roboter mit der Nase nach oben zeigen. Laut Koordinaten zeigt er aber nach unten !Gruss
-
Hallo Ritchie,
Da sind mehrere Probleme.
Den ersten Fehler hatte ich Dir schon in meiner ersten Antwort mitgeteilt. In der Methode SensorData::BerechnePose in Zeile 131 muss es richtig heißenstd::complex< double > welt_p_obj = welt_T_sensor * std::complex< double >( fltDistanz, 0 );Der Ausdruck hinter welt_T_sensor ist eine Position. Die Position beschreibt die Position des Objekts vor dem Sensor und vorne heißt hier in X-Richtung. In Y-Richtung hat das Objekt keine Auslenkung - immer bezogen auf den Sensor.
#define WEGPRONAVIGATIONSSTRECKE M_PI.. das ist Quatsch; insbesondere weil Du anscheinend mit dem Wert WEGPRONAVIGATIONSSTRECKE eine Lägenskalierung vornehmen willst und das auch noch falsch. Lass das Makro weg und lösche überall WEGPRONAVIGATIONSSTRECKE.
Falls Du mit unterschiedlichen Einheiten rechnen möchtest, solltest Du Deine Variablen auch so benennen - also z.B. 'aktX_cm'. Aus Deinem Code ist nicht ersichtlich was Du wie umrechnen möchtest.Bei Der Ausgabe der Daten (Zeile 68 und 72 im main) hast Du "links" und "rechts" vertauscht. Die Sensoren mit den positiven Y-Werten liegen links.
Wenn Du Fließkommazahlen (double oder float) nach int umrechnest, so solltes Du korrekt runden. Insesondere, wenn die ints z.B. später Pixel-Koordinaten in einer Graphik sein sollen. Das geht mit
intObjekt_X= int( std::floor( xwert + 0.5 ) );(erfordert #include <cmath>; bitte nicht mit <math.h> mischen). Am besten immer mit double rechnen, und erst ganz zum Schluß nach int umrechnen falls notwendig. Sonst gibt's hässliche Rundungsfehler, die sich aufaddieren.
Richie schrieb:
Wie es scheint ist die Drehrichtung vertauscht. Aber wieso ? Eigentlich sollte der
Roboter mit der Nase nach oben zeigen. Laut Koordinaten zeigt er aber nach unten !.. der Roboter zeigt doch nach oben. Das erkennt man daran, dass die Y-Koordinaten bei 'vorn' größer sind als die Y-Koordinaten von 'hinten'. Und Y zeigt in Deiner Skizze nach oben.
Richie schrieb:
Mein Koordinatensystem hat oben links 0.0 und unten rechts 300,300.
das passt aber nicht zu Deiner Skizze, was ist denn nun richtig?
Zu dem Code lässt sich einiges sagen. Im Prinzip zu viel Code für zu wenig Funktion, C-lastig und stark prozedural.
Nur soviel: wenn Du sehr viel mit Positionen und auch Lagen 'pose' umgehst, solltest Du diese auch als Variablen und Parameter im Code wiederfinden - also stattvoid SetAbs_X(int); int GetAbs_X(void); void SetAbs_Y(int); int GetAbs_Y(void); void SensorData::SetAbs_Ausrichtung(double fltWert); // usw.besser
MoveTo( const pose& welt_T_roboter );Gruß
Werner
-
by the hammer
-
by the hammer
Hä ?
Hallo Werner,
meine DSL-Leitung ist leicht gestört. Daher eine kurze Antwort von einem anderen Anschluss.
#define WEGPRONAVIGATIONSSTRECKE M_PI
Ich verwende eine Karte, welche mir den Bereich meiner Wohnung intern in meinem Roboter darstellt. Diese Karte hat ein Raster von M_PI.
Dieses Raster hat sich ergeben, durch die Impulse meiners RAD-Encoder (Rückmeldung für Drehung der Räder).Meine Karte, welche ich abgelegt habe, ist in diesem Bereich nicht ganz aktuell, sorry. Der Nullpunkt der Karte ist links oben, vielleicht liegt hier ja auch noch ein Problem. Muss ich mir näher ansehen.
Zu dem Code lässt sich einiges sagen. Im Prinzip zu viel Code für zu wenig Funktion, C-lastig und stark prozedural.
Nur soviel: wenn Du sehr viel mit Positionen und auch Lagen 'pose' umgehst, solltest Du diese auch als Variablen und Parameter im Code wiederfinden - also stattStimmt. Ich tue mich immer noch schwer damit. In diesem Fall ist es auch noch ein Testprogramm, wo ich die Sache noch mehr vernachlässigt habe.
Eigentlich schreibe ich mehr ASM Programme, C und VB.Trotzdem Danke für die Hinweise.
Ich muss erstmal meine DSL Leitung von Provider prüfen lassen. Kann keine 6M, vielleicht 4M.. oder doch wieder 2M
(wurde gestern umgeschaltet)Gruss
-
Hallo Werner,
danke nochmals für die ausführliche Antwort.
Nachdem ich Deine Anmerkung in Sachen "#include <cmath>" geändert habe sind jetzt auch die Positionberechnungen symmetrisch.
------ Test Program Hinderniskarte ------ Akt. Winkel (PI/2*x) (0->4): 1 Aktueller Winkel : 1.570796 Aktuelle X Pos : 31 Aktuelle Y Pos : 31 Distanz Objekt (cm) : 31.4 ------------ Sensor Daten ---------- Nr. X-Pos Y-Pos --------------------------------------- Vorne 1 34 47 2 32 47 3 29 47 4 27 47 rechts 5 16 36 6 16 32 7 16 29 8 16 25 hinten 9 27 14 10 29 14 11 32 14 12 34 14 links 13 45 25 14 45 29 15 45 32 16 45 36So zeigen jetzt die Wert von 27-30 die gleichen Zahlenwerte wie auch 12-15 mit 22-25 .
Ein Umstand, den ich mir nicht wirklich erklären konnte.Jetzt habe ich nur noch die Frage/Problem mit der Ausrichtung.
Meine letzte Karte (Netzversion leider nicht aktuell), hatden X=0 und Y=0 oben links und X=300 und Y=300 unten rechts.Jedoch scheint der Code hier
pose welt_T_rob( std::polar( 1.0, fltAusrichtung), std::complex< double >( Akt_X, Akt_Y ) ); // rob_T_sensor ist die Lage des Sensors im Roboter-Systen // Bem.: Winkel also Ausrichtung des Sensors bezogen auf den Roboter == 0.0 pose rob_T_sensor( std::polar( 1.0, fltSensorAusrichtung[intSensor] ), std::complex< double >( fltAbstandX[intSensor], fltAbstandY[intSensor] ) ); pose welt_T_sensor = welt_T_rob * rob_T_sensor; // Lage Sensor im Welt-System std::complex< double > welt_p_obj = welt_T_sensor * std::complex< double >( fltDistanz, 0.0 );X=0 = Y=0 links unten und Y=300 und X=300 rechts oben zu erwarten. Sieht man deutlich an den Zahlen in Zeile 17-20 diese sollten eigentlich von aufsteigend sein (nicht absteigend).
Wie kann ich diese Funktion ändern oder rechnen diese Standardfunktionen generell mit diesem Koordiantensystem
Bei Der Ausgabe der Daten (Zeile 68 und 72 im main) hast Du "links" und "rechts" vertauscht. Die Sensoren mit den positiven Y-Werten liegen links.
Dies ist die Nummerung der Sensoren und deren Position.
In der Grafik ist S1 der erste Sensor und die Sensoren zählen dann im Uhrzeigersinn rund im den Roboter. Daher ist links von S1 der Sensor S16 und rechts S2.Gruss