Ein seltsames "Gier" Verhalten in der Simulation mit atan2() Funktion

Question

Ich arbeite an einer Anwendung, einem Simulator, wo ein Quadrotor vom Wegpunkt zum Wegpunkt fliegt. In meinem Code habe ich eine Funktion implementiert, um die mit atan2 Funktion zu berechnen. Aber wenn sich der Quadrotor um 360 ° dreht, bewegt er sich nicht auf dem kürzesten Weg, sondern bewegt sich um den 360 ° -Bereich herum, um die neue Richtung zu erreichen.

Hier habe ich eine video gepostet. Werfen Sie einen Blick auf sein Verhalten, wenn es über 360 °.

Ok Jungs hier die komplette Funktion jetzt:

geometry_msgs::Pose getYaw(double x1, double x2, double y1, double y2) { 

geometry_msgs::Pose output_trajectory; 

/* Extrapolate the yaw information between two contigous points */ 
double yaw = atan2((y2 - y1), (x2 - x1)); 

    if(yaw < 0.0f) // * read later on 
    yaw += 2.0f * M_PI; 

output_trajectory.orientation = tf::createQuaternionMsgFromYaw(yaw); 

    return output_trajectory; 
} 

wo tf :: createQuaternionMsgFromYaw eine Bibliothek aus dem ROS-Framework ist. Hier die Definition: link. geometry_msgs :: Pose ist einfach ein Container: link.

*: hier einen Auszug aus dem Gierwert: hier habe ich verwandte Themen und Fragen hier in Stackoverflow und diese Funktion ordnet die zurückgegebenen Ausgabe von atan2 in 0 ° -360 °

UPDATE lesen

... 
Yaw: 131.3678 
Yaw: 133.3495 
Yaw: 135.6426 
Yaw: 138.3442 
Yaw: 141.5859 
Yaw: 145.5487 
Yaw: 150.4813 
Yaw: 156.7167 
Yaw: 164.6657 
Yaw: 174.7288 
Goal reached 
Moving to the 3 waypoint 
Yaw: 174.7288 
Yaw: 186.4225 
Yaw: 196.3789 
Yaw: 204.1349 
Yaw: 210.1296 
Yaw: 214.7946 
Yaw: 218.4716 
Yaw: 221.4110 
Yaw: 223.7921 
Yaw: 225.7431 
Yaw: 227.3565 
... 

Wie Sie sehen können, ist der Kreuzungspunkt "continuos", aber er dreht sich von 174 ° zu 186 ° nicht in der rechten (kleinsten) Richtung.

Was ich erwarte, ist, dass der Quadrotor sich durch kleine Anpassungen bewegt und rund um 360 ° gedreht ein paar Grad dreht.

Wie kann ich dieses Problem loswerden? Ich brauche eine sanfte Gierbewegung in meiner Anwendung. Grüße

Answer 1

Ok. Ich habe es. Nach stundenlanger Untersuchung wurde mir klar, dass das Problem nicht mit der atan2() Funktion zusammenhängt oder eine Zeichenänderung des Winkels, wenn es um 180 ° oder 360 ° springt.

Lesen sorgfältig den folgenden Code wie zB

#include <string> 
#include <ros/ros.h> 
#include <sensor_msgs/JointState.h> 
#include <tf/transform_broadcaster.h> 

int main(int argc, char** argv) { 
    ros::init(argc, argv, "state_publisher"); 
    ros::NodeHandle n; 
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1); 
    tf::TransformBroadcaster broadcaster; 
    ros::Rate loop_rate(30); 

    const double degree = M_PI/180; 

    // robot state 
    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005; 

    // message declarations 
    geometry_msgs::TransformStamped odom_trans; 
    sensor_msgs::JointState joint_state; 
    odom_trans.header.frame_id = "odom"; 
    odom_trans.child_frame_id = "axis"; 

    while (ros::ok()) { 
     //update joint_state 
     joint_state.header.stamp = ros::Time::now(); 
     joint_state.name.resize(3); 
     joint_state.position.resize(3); 
     joint_state.name[0] ="swivel"; 
     joint_state.position[0] = swivel; 
     joint_state.name[1] ="tilt"; 
     joint_state.position[1] = tilt; 
     joint_state.name[2] ="periscope"; 
     joint_state.position[2] = height; 


     // update transform 
     // (moving in a circle with radius=2) 
     odom_trans.header.stamp = ros::Time::now(); 
     odom_trans.transform.translation.x = cos(angle)*2; 
     odom_trans.transform.translation.y = sin(angle)*2; 
     odom_trans.transform.translation.z = .7; 
     odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2); 

     //send the joint state and transform 
     joint_pub.publish(joint_state); 
     broadcaster.sendTransform(odom_trans); 

     // Create new robot state 
     tilt += tinc; 
     if (tilt<-.5 || tilt>0) tinc *= -1; 
     height += hinc; 
     if (height>.2 || height<0) hinc *= -1; 
     swivel += degree; 
     angle += degree/4; 

     // This will adjust as needed per iteration 
     loop_rate.sleep(); 
    } 


    return 0; 
} 

, die ich here fand ich erkannte, dass die Variable Winkel jedes Mal einer kleinen Menge erhöht wird, und dann auf die quaternion Bibliothek tf::createQuaternionMsgFromYaw() Diese Mittel bestanden 2 Dinge:

1. Sie kümmern sich nie um den Sprung über 180 ° oder 360 °;
2. wichtigere Sache, Sie passieren nie einen absoluten Winkel.Vorläufig rufen Sie nie Ihren Code tf :: createQuaternionMsgFromYaw (degtorad (179)) und dann tf :: createQuaternionMsgFromYaw (degtorad (182)) auf, sondern tf :: createQuaternionMsgFromYaw (angle + delta_angle);

Grüße

Answer 2

Ich glaube nicht, atan gibt Ihnen den richtigen Winkel. Atan gibt die Ergebnisse von -pi/2 ~ + pi/2.

Wenn Sie genaue Winkel in Radiant erhalten möchten, können Sie so etwas schreiben müssen (was ich vorher arbeitete schön):

// First find the section in which your coordinate is, then add the needed (x*pi) value to result: 
double result = atan2(..); 
if((x2 - x1 > 0) && (y2 - y1 > 0)){ 
    //section = 1; 
    result += 0; 
} 
else if((x2 - x1 < 0) && (y2 - y1 > 0)){ 
    //section = 2; 
    result += pi; 
} 
else if((x2 - x1 < 0) && (y2 - y1 < 0)){ 
    //section = 3 
    result += pi; 
} 
else if((x2 - x1 > 0) && (y2 - y1 > 0)){ 
    //section = 4 
    result += 2*pi; 
} 
else if(x2 == x1){ 
    if(y2 > y1){result = pi/2); 
    if(y1 > y2){result = -pi/2); 
} 
else if(y2 == y1){ 
    if(x2 > x1){result = 0;} 
    if(x1 > x1){result = pi;} 
} 
else if((x1 == x2) && (y1 == y2)){ 
    std::cout << "This is not line, just a point\n"; // :P 
}