Liste der Anhänge anzeigen (Anzahl: 2)
Positionsberechnung eines Objektes im Raum
Hallo Zusammen,
ich verzweifle derzeit an meiner Objekterkennung innerhalb meiner Weltkarte. Bei der Berechnung eines gefundenen Objektes von einem Sensor, wird dieser in eine Karte eingetragen und zur Navigation verwendet.
Die
Ich habe eine Klasse, welche mir die X,Y Position eines Objektes im Raum zurückgeben soll.
hier die Sensor Data Class
Code:
void SensorData::BerechnePose(void)
{
double fltAngleOftheSensor; // Angle of the sensor in the world car
double fltAngle; // an
if( intSensor == 0 )
{
if( intAngle < 90.0 ) // Angle between 0 ... 90 Grad
{ // Convert angle to Quadrant I
fltAngleOftheSensor =90.0 - intAngle; // is it the front sensor
}
else
{ // Convert angle to Quadrant 4
fltAngleOftheSensor =450.0 - intAngle; // is it the front sensor
} // Angle is between 90.0 - 180.0
}
else
{ // Angle is from 0.. 180 Grad
fltAngleOftheSensor = 270.0 - intAngle; // it is the rear sensor
}
// world_T_rob is the pose of the roboter in the world card
// Rotation Position
pose world_T_roboter( std::polar( 1.0, fltActualAngleRoboter * GRAD2RAD), std::complex< double >( ActRoboterPos.High_x(), ActRoboterPos.High_y()));
// rob_T_sensor is the Pose of the Sensor in relativ to the roboter system
// Bem.: Winkel also Ausrichtung des Sensors bezogen auf den Roboter == 0.0
pose roboter_T_sensor( std::polar( 1.0,fltAngleOftheSensor * GRAD2RAD), std::complex< double >( fltSensorDistanceX[intSensor],0.0 ) );
pose world_T_sensor = world_T_roboter * roboter_T_sensor; // Pose of the sensor in the world card
std::complex< double > world_p_obj = world_T_sensor * std::complex< double >( fltDistance, 0.0 );
ActObjectPos.SetXHighPos ((int) world_p_obj.real()); // Position of the object in the world card
ActObjectPos.SetYHighPos((int) world_p_obj.imag());
}
Die Sensoren (zwei US / IR Sensoren) auf einem Servo montiert (siehe Zeichnung), können auf eine Winkel gesetzt werden und der Roboter misst dann den Abstand des Objekts.
Nur Erscheinen mir die Werte der Position nicht korrekt. Wenn ich die Position eines Objekt nach vorne berechne, stimmt die Position. Nach hinten der gleiche Abstand stimmt wieder nicht.
Diese Daten werden dann in eine Karte eingetragen und zur Navigation verwendet. Da ich diese Werte auf dem PC sichtbar mache, kann ich den Blödsinn sehen, den er macht. Nur finde ich den Grund nicht.
Kann hier jemand einen helfenden Blick drüber werfen. Anbei der komplette Quellcode eines kleinen Testprogram unter kDevelop (gcc).
Gruss R.
