使用atan2()函数在模拟中出现奇怪的"偏航"行为

Question

我正在开发一个应用程序,一个模拟器,四旋翼飞行器从航点飞到航点.在我的代码中,我实现了一个使用atan2函数计算偏航的函数.但是当四旋翼飞行器翻转360°时,它不会以最短的速度移动,但它会在360°范围内移动以达到新的方向.

我在这里发布了一个视频.看看它在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框架的库.这里的定义:链接.geometry_msgs :: Pose只是一个容器:链接.

*:这里我已经在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度旋转几度.

我该怎样摆脱这个问题？我的应用程序需要一个平滑的偏航运动.问候

Answer 1

好的。\n我明白了。\n经过几个小时的调查，我意识到问题与 atan2() 函数或跳过 180\xc2\xb0 或 360\xc2\xb0 时角度的某些符号变化无关。

\n\n

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

\n\n

#include <string>\n#include <ros/ros.h>\n#include <sensor_msgs/JointState.h>\n#include <tf/transform_broadcaster.h>\n\nint main(int argc, char** argv) {\n    ros::init(argc, argv, "state_publisher");\n    ros::NodeHandle n;\n    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);\n    tf::TransformBroadcaster broadcaster;\n    ros::Rate loop_rate(30);\n\n    const double degree = M_PI/180;\n\n    // robot state\n    double tilt = 0, tinc = degree, swivel=0, angle=0, height=0, hinc=0.005;\n\n    // message declarations\n    geometry_msgs::TransformStamped odom_trans;\n    sensor_msgs::JointState joint_state;\n    odom_trans.header.frame_id = "odom";\n    odom_trans.child_frame_id = "axis";\n\n    while (ros::ok()) {\n        //update joint_state\n        joint_state.header.stamp = ros::Time::now();\n        joint_state.name.resize(3);\n        joint_state.position.resize(3);\n        joint_state.name[0] ="swivel";\n        joint_state.position[0] = swivel;\n        joint_state.name[1] ="tilt";\n        joint_state.position[1] = tilt;\n        joint_state.name[2] ="periscope";\n        joint_state.position[2] = height;\n\n\n        // update transform\n        // (moving in a circle with radius=2)\n        odom_trans.header.stamp = ros::Time::now();\n        odom_trans.transform.translation.x = cos(angle)*2;\n        odom_trans.transform.translation.y = sin(angle)*2;\n        odom_trans.transform.translation.z = .7;\n        odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(angle+M_PI/2);\n\n        //send the joint state and transform\n        joint_pub.publish(joint_state);\n        broadcaster.sendTransform(odom_trans);\n\n        // Create new robot state\n        tilt += tinc;\n        if (tilt<-.5 || tilt>0) tinc *= -1;\n        height += hinc;\n        if (height>.2 || height<0) hinc *= -1;\n        swivel += degree;\n        angle += degree/4;\n\n        // This will adjust as needed per iteration\n        loop_rate.sleep();\n    }\n\n\n    return 0;\n}\n

\n\n

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

\n\n

\n
1. 你永远不会关心跨越 180\xc2\xb0 或 360\xc2\xb0 的跳跃；
\n
2. 更重要的是，你永远不会通过绝对角度。例如，您永远不会在代码中调用 tf::createQuaternionMsgFromYaw(degtorad(179)) 然后调用 tf::createQuaternionMsgFromYaw(degtorad(182))，而是调用 tf::createQuaternionMsgFromYaw(angle + delta_angle);
\n

\n\n

问候

\n