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.

[RP6conrad]

Ich siehe ueberhaupt kein Goniometrische Function in ihre Berechnungen. In ein XY Cartesische System soll isch es so machen (center Robby = 0,0):
PosXobjext : Abstand Sensor*cos(Rel.Winkel Sensor auf Robby+Abs Winkel Robby)+ (Abstand Sensor-Center Robby)*cos(Abs.Winkel Robby)
PosYobject : Abstand Sensor*sin(Rel.Winkel Sensor auf Robby+Abs Winkel Robby)+ (Abstand Sensor-Center Robby)*sin(Abs.Winkel Robby)
Das ist gans einfach die 2 Vectoren in X/Y gesplittet und aufaddiert.

[Ritchie]

Hi,

nun ja, hier wird die ganze Sache anderes angegangen.

pose world_T_roboter( std : polar( 1.0, fltActualAngleRoboter * GRAD2RAD), std::complex< double >( ActRoboterPos.High_x(), ActRoboterPos.High_y()));

Hier wird der Winkel über

std: polar( 1.0, fltActualAngleRoboter * GRAD2RAD)

angegeben.

Die Position ist wird hier über complexe Zahlen dargestellt.

Das ganze dauert mir aber auch in der Berechnung zu lange. Ich wollte das ganze jetzt über eine Transformationsmatrix
versuchen. Ist auch neu für mich, wird aber üblicherweise so gemacht.

Das ganze versteckt sich dann in der Class "RoboterPose", welche ich nur im Code beigefügt habe.

Die Routine da oben hat mir mal ein "Mathegenie" geschrieben. Nur kann ich diese nur teilweise nachvollziehen.

Ich würde auch sagen, das es zwei Vektoren sind, welche ich berücksichtigen muss.

1) Mittelpunkt zur Mess-Stelle
2) Messtelle zum Objekt.


Gruss
R.