C++ “奇怪的”；偏航“；使用atan2（）函数模拟中的行为

我正在开发一个应用程序，一个模拟器，其中一个四旋翼从一个航路点飞到另一个航路点。 在我的代码中，我实现了一个函数来使用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;
}

如您所见，交叉点为“continuos”，但它从174°转向186°，而不是向右（最小）方向

我所期望的是，四转子通过微小调整移动，并围绕360°旋转，而不是几度

我怎样才能解决这个问题？我需要一个平稳的偏航运动在我的应用程序。

---

关于

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

如果你想得到精确的弧度角度，你可能需要写这样的东西（我以前做过，效果很好）：

//首先找到坐标所在的部分，然后将所需的（x*pi）值添加到结果中：
双重结果=atan2（…）；
如果（（x2-x1>0）和&（y2-y1>0））{
//截面=1；
结果+=0；
}
如果（（x2-x1<0）和&（y2-y1>0））{
//截面=2；
结果+=pi；
}
否则如果（（x2-x1<0）和&（y2-y1<0））{
//截面=3
结果+=pi；
}
如果（（x2-x1>0）和&（y2-y1>0））{
//截面=4
结果+=2*pi；
}
else如果（x2==x1）{
如果（y2>y1）{result=pi/2）；
如果（y1>y2）{result=-pi/2）；
}
否则如果（y2==y1）{
如果（x2>x1）{result=0；}
如果（x1>x1）{result=pi；}
}
如果（（x1==x2）和&（y1==y2））{
标准：：coutOk。
我得到了它。
经过数小时的调查，我意识到这个问题与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;
}

#包括
#包括
#包括
#包括
int main（int argc，字符**argv）{
ros:：init（argc，argv，“state_publisher”）；
ros：：nodehandlen；
发行商联合出版=n.广告（“联合州”，1）；
tf：：电视广播公司；
ros：速率环_速率（30）；
常数双度=M_PI/180；
//机器人状态
双倾斜=0，tinc=度，旋转=0，角度=0，高度=0，hinc=0.005；
//消息声明
几何学：奥多姆变速器；
传感器：接头状态接头状态；
odom_trans.header.frame_id=“odom”；
odom_trans.child_frame_id=“轴”；
while（ros:：ok（））{
//更新联合状态
joint_state.header.stamp=ros:：Time:：now（）；
关节状态。名称。调整大小（3）；
关节状态、位置、大小（3）；
关节状态名称[0]=“旋转”；
关节状态位置[0]=旋转；
关节状态名称[1]=“倾斜”；
关节状态位置[1]=倾斜；
联合州名称[2]=“潜望镜”；
关节状态位置[2]=高度；
//更新变换
//（在半径为2的圆中移动）
odom_trans.header.stamp=ros:：Time:：now（）；
odom_transform.translation.x=cos（角度）*2；
odom_trans.transform.translation.y=sin（角度）*2；
odom_transform.translation.z=.7；
odom_transform.rotation=tf:：createQuaternionMsgFromYaw（角度+M_PI/2）；
//发送关节状态并转换
联合出版（联合州）；
广播公司sendTransform（奥多姆运输公司）；
//创建新的机器人状态
倾斜+=tinc；
如果（0）tinc*=-1；
高度+=hinc；
如果（高度>0.2 | |高度）不清楚你在问什么。一定有一个交叉点。你期望的是什么行为？所以问题是你从360向下插值到0，所以直升机会做360，还是我误解了？在哪一点上，这是一个插值问题，而不是atan问题。（P.s:不错的项目）代码的这一位
yaw+2.0f*M_PI；
没有任何意义。该值没有存储或使用。尝试如下：像这样使用：
yaw=clapradians（atan2（y2-y1，x2-x1））；
@VáclavZeman可能是OP的本意+=对不起，kobay，但我认为atan2不是问题：（角度是对的，我认为问题在于其他方面。嗯，希望你能找到答案。如果我没有错，你是在尝试将路线分成相等的部分。如果第一对x-y坐标计算错误怎么办？：）好的……事实并非如此，因为调用我在这里发布的函数会关注给定点的正确顺序。我现在非常确定问题不在我的代码中
// 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
}

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