【发布时间】: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