【问题标题】:A weird "yaw" behavior in simulation using atan2() function使用 atan2() 函数进行模拟时出现奇怪的“偏航”行为
【发布时间】:2015-02-19 11:08:59
【问题描述】:

我正在开发一个应用程序,一个模拟器,一个四旋翼飞行器从一个航点飞到另一个航点。 在我的代码中,我实现了一个函数来计算 yaw 使用 atan2 函数。 但是,当四旋翼翻转 360° 时,它并没有通过最短路径,而是在 360° 范围内移动,以到达新的方向。

我在这里发布了video。看看它在 360° 时的行为。

好的,现在完整的功能在这里:

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;
}

其中 tf::createQuaternionMsgFromYaw 是来自 ROS 框架的库。这里是定义:link。 geometry_msgs::Pose 只是一个容器:link。

*:这里我在stackoverflow中阅读了相关的主题和问题,这个函数将atan2的返回输出映射到0°-360°

更新: 这里是偏航值的摘录:

...
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
...

如您所见,交叉点是“连续的”,但它从 174° 转向 186°,而不是在正确(最小)方向。

我期望的是四旋翼通过小调整移动并在 360° 周围旋转几度。

我怎样才能摆脱这个问题?我需要在我的应用程序中进行平滑的偏航运动。 问候

【问题讨论】:

  • 不清楚你在这里问什么。某处必须有一个交叉点。您期待什么行为?
  • 所以问题是你从 360 内插回 0 所以直升机做了 360,还是我误解了?在这一点上,这是一个插值问题,而不是一个 atan 问题。 (PS:不错的项目)
  • 这段代码yaw + 2.0f * M_PI; 没有任何意义。该值未存储或使用。
  • 试试这个:pastebin.com/7hf0U5Vu 像这样使用它:yaw = clampRadians(atan2(y2 - y1, x2 - x1));
  • @VáclavZeman 可能 OP 打算这样做 +=

标签: c++ function graphics return-value simulation


【解决方案1】:

我不认为 atan 给了你正确的角度。 Atan 给出 -pi/2 ~ +pi/2 范围内的结果。

如果你想得到精确的弧度角度,你可能需要写这样的东西(我以前做过,效果很好):

// 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
}

【讨论】:

  • 抱歉 kobay 但我不认为 atan2 是问题 :( 角度是对的,我认为问题出在其他地方
  • 嗯,希望你能找到答案。
  • 如果我没记错的话,您是在尝试将路线分成相等的部分。如果第一对 x-y 坐标计算错误怎么办?
  • :) 好的...不是这种情况,因为调用我在此处发布的内容的函数会关注给定点的正确顺序。我现在很确定问题不在我的代码中
【解决方案2】:

好的。 我得到了它。 经过数小时的调查,我意识到问题与 atan2() 函数或跳过 180° 或 360° 时角度的某些符号变化无关。

仔细阅读以下代码作为示例

#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;
}

我发现here我意识到变量角度每次都会增加少量然后传递给四元数库tf::createQuaternionMsgFromYaw() 这意味着两件事:

  1. 您从不关心 180° 或 360° 的跳跃;
  2. 更重要的是,你永远不会通过绝对角度。例如,您永远不会在代码中调用 tf::createQuaternionMsgFromYaw(degtorad(179)) 和 tf::createQuaternionMsgFromYaw(degtorad(182)),但是 tf::createQuaternionMsgFromYaw(angle + delta_angle);

问候

【讨论】:

    猜你喜欢
    • 1970-01-01
    • 2021-10-04
    • 2019-07-05
    • 1970-01-01
    • 2013-12-27
    • 1970-01-01
    • 2013-02-15
    • 1970-01-01
    • 1970-01-01
    相关资源
    最近更新 更多