我正在开发一个应用程序，一个模拟器，其中四旋翼飞行器从一个航点飞到另一个航点。在我的代码中，我实现了一个使用 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：:P ose 只是一个容器：链接。
*：在这里我已经阅读了堆栈溢出中的相关主题和问题，此函数将 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° 而不是几度。
我怎样才能摆脱这个问题？我需要在我的应用程序中平稳地偏航运动。问候
我不认为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 pointn"; // :P
}
好的。知道了。经过数小时的调查，我意识到这个问题与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;
}
我在这里发现，我意识到可变角度每次都会增加少量，然后传递到四元数库tf::createQuaternionMsgFromYaw()这意味着两件事：
- 你永远不会照顾跨越 180° 或 360° 的跳跃;
- 更重要的是，你永远不会通过一个绝对的角度。对于 istance，你永远不会在你的代码中调用 tf：：createQuaternionMsgFromYaw(degtorad(179(( 然后调用 tf：：createQuaternionMsgFromYaw(degtorad(182((，而是 tf：：createQuaternionMsgFromYaw(angle + delta_angle(;
问候