关于ABB机器人四元数转欧拉角计算方式 C++ C# Python

将四元数转换为欧拉角可以使用以下步骤：

1. 将四元数表示为q = (q0, q1, q2, q3)，其中q0为实部，q1、q2、q3为虚部。
2. 计算旋转矩阵R，其中R的第一行为[1-2*(q2^2+q3^2), 2*(q1q2-q0q3), 2*(q1q3+q0q2)]，第二行为[2*(q1q2+q0q3), 1-2*(q1^2+q3^2), 2*(q2q3-q0q1)]，第三行为[2*(q1q3-q0q2), 2*(q2q3+q0q1), 1-2*(q1^2+q2^2)]。
3. 根据旋转矩阵R计算欧拉角。
   • 如果R(3,1)不等于1或-1，则可计算欧拉角yaw、pitch和roll为：
     • yaw = atan2(R(2,1), R(1,1))
     • pitch = asin(-R(3,1))
     • roll = atan2(R(3,2), R(3,3))
   • 如果R(3,1)等于1，则存在奇异性，可令yaw等于0，然后计算pitch和roll：
     • pitch = pi/2
     • roll = atan2(R(1,2), R(1,3))
   • 如果R(3,1)等于-1，则存在奇异性，可令yaw等于0，然后计算pitch和roll：
     • pitch = -pi/2
     • roll = atan2(-R(1,2), -R(1,3))

注意，上述公式中的atan2函数表示从y轴到x轴的反正切值，asin函数表示反正弦值。

本文采用C++设计，实现四元数转欧拉角

1.C++创建输入输出结构体

#include<iostream>

struct Quaternion {
    double w, x, y, z;
};

struct EulerAngles {
    double roll, pitch, yaw;
};
 int main()
{

return 0;
}

2.四元数转欧拉角函数

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles angles;
    double M_PI = 3.14159265354;
    // roll (x-axis rotation)
    double sinr_cosp = 2 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = 1 - 2 * (q.x * q.x + q.y * q.y);
    angles.roll = (std::atan2(sinr_cosp, cosr_cosp))*180/ M_PI;

    // pitch (y-axis rotation)
    double sinp = 2 * (q.w * q.y - q.z * q.x);
    if (std::abs(sinp) >= 1)
        angles.pitch = (std::copysign(M_PI / 2, sinp)) * 180 / M_PI; // use 90 degrees if out of range
    else
        angles.pitch = (std::asin(sinp)) * 180 / M_PI;

    // yaw (z-axis rotation)
    double siny_cosp = 2 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = 1 - 2 * (q.y * q.y + q.z * q.z);
    angles.yaw = (std::atan2(siny_cosp, cosy_cosp)) * 180 / M_PI;

    return angles;
}