四元数欧拉角相互转换与奇异值问题

须臾所学

已于 2024-03-28 17:55:27 修改

阅读量2k

点赞数 2

文章标签：线性代数 c++

于 2022-07-28 10:15:45 首次发布

本文链接：https://blog.csdn.net/qq_45878098/article/details/126028904

版权

四元数转欧拉角

四元数转欧拉角
- 四元数转欧拉角的奇异值问题
- Eigen库中的四元数与欧拉角相互转换

四元数转欧拉角

参考文章：四元数和欧拉角的相互转换

四元数转欧拉角的奇异值问题

在四元数转欧拉角时遇到奇异值问题，可参考四元数和欧拉角的相互转换,在文章里面说明了。

那么将Z-Y-X欧拉角（或RPY角：绕固定坐标系的X-Y-Z依次旋转α,β,γ角）转换为四元数：

$\left[ \begin{matrix} cos\frac{\gamma}{2} \\ 0 \\ 0 \\ sin\frac{\gamma}{2} \end{matrix} \right] \left[ \begin{matrix} cos\frac{\beta}{2} \\ 0 \\ sin\frac{\beta}{2} \\ 0 \end{matrix} \right] \left[ \begin{matrix} cos\frac{\alpha}{2} \\ sin\frac{\alpha}{2} \\ 0 \\ 0 \end{matrix} \right] = \left[ \begin{matrix} cos\frac{\alpha}{2} cos\frac{\beta}{2} cos\frac{\gamma}{2} + sin\frac{\alpha}{2} sin\frac{\beta}{2} sin\frac{\gamma}{2} \\ sin\frac{\alpha}{2} cos\frac{\beta}{2} cos\frac{\gamma}{2} - cos\frac{\alpha}{2} sin\frac{\beta}{2} sin\frac{\gamma}{2} \\ cos\frac{\alpha}{2} sin\frac{\beta}{2} cos\frac{\gamma}{2} + sin\frac{\alpha}{2} cos\frac{\beta}{2} sin\frac{\gamma}{2} \\ cos\frac{\alpha}{2} cos\frac{\beta}{2} sin\frac{\gamma}{2} - sin\frac{\alpha}{2} sin\frac{\beta}{2} cos\frac{\gamma}{2} \end{matrix} \right]$

根据上面的公式可以求出逆解，即由四元数 $q=(q_0,q_1,q_2,q_3)$ 或 $q = (w, x, y, z)$ 。到欧拉角的转换为：
$\left[ \begin{matrix} \alpha \\ \beta \\ \gamma \end{matrix} \right] = \left[ \begin{matrix} arctan \frac{2(q_0 q_1 + q_2 q_3)}{1-2(q_1^2 + q_2^2)} \\ arcsin (2(q_0 q_2 - q_1 q_3)) \\ arctan \frac{2(q_0 q_3 + q_1 q_2)}{1-2(q_2^2 + q_3^2)} \end{matrix} \right]$

由于arctan和arcsin的取值范围在 $\frac{-\pi}{2}$ 和 $\frac{\pi}{2}$ 之间，只有180°，而绕某个轴旋转时范围是360°，因此要使用atan2函数代替arctan函数：
$\left[ \begin{matrix} \alpha \\ \beta \\ \gamma \end{matrix} \right] = \left[ \begin{matrix} atan2(2(q_0 q_1 + q_2 q_3),1-2(q_1^2 + q_2^2)) \\ arcsin (2(q_0 q_2 - q_1 q_3)) \\ atan2 (2(q_0 q_3 + q_1 q_2), 1-2(q_2^2 + q_3^2)) \end{matrix} \right]$

对于tan(θ) = y / x ：
θ = ATan(y / x)求出的θ取值范围是[-PI/2, PI/2]；
θ = ATan2(y, x)求出的θ取值范围是[-PI, PI]。

当 (x, y) 在第一象限, 0 < θ < PI/2
当 (x, y) 在第二象限 PI/2 < θ≤PI
当 (x, y) 在第三象限, -PI < θ < -PI/2
当 (x, y) 在第四象限, -PI/2 < θ < 0

Eigen库中的四元数与欧拉角相互转换

在 Eigen 库中，将四元数转换为欧拉角，首先将四元数转换为旋转矩阵，然后将选择矩阵转换为欧拉角。欧拉角的旋转顺序为 Z-Y-X，对应数字 (2,1,0)。

Eigen::Vector3d eul = q.toRotationMatrix().eulerAngles(2, 1, 0);

欧拉角转四元数：

  Eigen::AngleAxis   roll(eul(2), Eigen::Vector3d::UnitX());
  Eigen::AngleAxis   pitch(eul(1), Eigen::Vector3d::UnitY());
  Eigen::AngleAxis   yaw(eul(0), Eigen::Vector3d::UnitZ());
  Eigen::Quaterniond q = yaw * pitch * roll;
  q.normalize();

但是 Eigen 库自带的转换方法，将四元数转换为欧拉角时，存在角度突变的问题，即两个四元数表示的姿态相差较小，但是欧拉角相差很大，主要发生在某一个轴 90° 或 180° 的情况下。

原因是欧拉角的角度范围为：roll[-pi,pi]，pitch[-pi/2,-pi/2]，yaw[-pi,pi]，但是Eigen库中转换欧拉角（使用函数.eulerAngles()）时做了特殊处理，使转换后的欧拉角范围为：0[0,pi]，1[-pi,pi]，2[-pi,pi]，即第一个角度永远为正(+)。即使转换后第一个角度为负，Eigen库也会对第二个和第三个角度加或减pi，使第一个角度为正。

例如四元数xyzw(0.00392036,−0.00511095,−0.613622,0.789573)，对应的欧拉角ypr(−1.32133,−0.00325971,0.0124636)，但是在Eigen中转换为欧拉角ypr(1.82026,−3.13833,−3.12913);

使用Eigen库转换欧拉角时，会使人误以为物体的第一个角度无法为负，例如：如果使用第一个角度表示agv的偏航角，会使人误以为agv的偏航角永远为正（永远右转），但实际上，虽然第一个角度为正，但是欧拉角ypr整体表示的姿态中，agv既可以向左转又可以向右转，左转或右转取决于后面两个角度的值。

所以，使用欧拉角表示姿态变换时，需要将ypr当做一个整体来处理；

可以通过上文中的换算公式，自行编写转换代码，以 Z-Y-X 为例：

四元数转欧拉角：

Eigen::Vector3d TagLocation::quaternion2Euler(Eigen::Quaterniond q)
{
  Eigen::Vector3d euler(0, 0, 0);
  euler(0) = atan2(2 * (q.y() * q.z() + q.w() * q.x()),
                   q.w() * q.w() - q.x() * q.x() - q.y() * q.y() + q.z() * q.z());
  euler(1) = asin(-2 * (q.x() * q.z() - q.w() * q.y()));
  euler(2) = atan2(2 * (q.x() * q.y() + q.w() * q.z()),
                   q.w() * q.w() + q.x() * q.x() - q.y() * q.y() - q.z() * q.z());
  return euler;
}

欧拉角转四元数：

Eigen::Quaterniond TagLocation::euler2Quaternion(Eigen::Vector3d v){
  Eigen::Quaterniond q;
  double alpha_2 = v(0)/2, beta_2 = v(1)/2, gamma_2 = v(2)/2;
  q.w() = cos(alpha_2) * cos(beta_2) * cos(gamma_2) + sin(alpha_2) * sin(beta_2) * sin(gamma_2);
  q.x() = sin(alpha_2) * cos(beta_2) * cos(gamma_2) - cos(alpha_2) * sin(beta_2) * sin(gamma_2);
  q.y() = cos(alpha_2) * sin(beta_2) * cos(gamma_2) + sin(alpha_2) * cos(beta_2) * sin(gamma_2);
  q.z() = cos(alpha_2) * cos(beta_2) * sin(gamma_2) - sin(alpha_2) * sin(beta_2) * cos(gamma_2);
  return q;
}

或者参考文章和维基百科中的方法，将四元数转换为欧拉角：

//四元数 --> 欧拉角(Z-Y-X，即RPY)（确保pitch的范围[-pi/2, pi/2]）
Eigen::Vector3d quaterniond2Euler(Eigen::Quaterniond q) {
    Eigen::Vector3d angles;

    // 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(2) = std::atan2(sinr_cosp, cosr_cosp);

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

    // 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(0) = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

旋转矩阵转换欧拉角：

//旋转矩阵 --> 欧拉角(Z-Y-X，即RPY)（确保pitch的范围[-pi/2, pi/2]）
Eigen::Vector3d eulerAngle_mine;
Eigen::Matrix3d rot = rotationMatrix_ea;
eulerAngle_mine(2) = std::atan2(rot(2, 1), rot(2, 2));
eulerAngle_mine(1) = std::atan2(-rot(2, 0), std::sqrt(rot(2, 1) * rot(2, 1) + rot(2, 2) * rot(2, 2)));
eulerAngle_mine(0) = std::atan2(rot(1, 0), rot(0, 0));