我正在开发一个模拟器应用程序,其中四旋翼飞行器从航点到航点飞行。
在我的代码中,我实现了一个使用atan2函数计算yaw的函数。
但是当四旋翼飞行器转过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框架中的一个库。这里是定义:link。 geometry_msgs :: Pose只是一个容器:link。
*:我在stackoverflow上阅读了相关主题和问题,并发现此函数将atan2返回的输出映射到0°-360°
更新: 这里是yaw值的提取:
...
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度旋转而不是几度移动。
我该如何解决这个问题?我需要在我的应用程序中平稳的偏航运动。 谢谢。
yaw + 2.0f * M_PI;
部分没有任何意义,该值既未存储也未被使用。 - wilxyaw = clampRadians(atan2(y2 - y1, x2 - x1));
- Brandondouble
? - wilx