Positionsberechnung eines Objektes im Raum

[Ritchie]

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

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.