四元数与欧拉角(Yaw、Pitch、Roll)的转换

Table of Contents

0、简介

一、四元数的定义

二、欧拉角到四元数的转换

2.1 公式:

2.2 code:

三、四元数到欧拉角的转换

3.1 公式

3.2 code:

3.3 四元素到旋转矩阵转换

四. 奇点

五. 矢量旋转

证明:

六 . 其他参考

七 python 四元素欧拉角互相转换

8 Eigen transform

欧拉角到四元素

四元素得到yaw

四元素到旋转向量

旋转轴向量到四元素


0、简介

四元数与欧拉角之间的转换

百度百科四元素

在3D图形学中,最常用的旋转表示方法便是四元数和欧拉角,比起矩阵来具有节省存储空间和方便插值的优点。

本文主要归纳了两种表达方式的转换,计算公式采用3D笛卡尔坐标系:

四元数与欧拉角(Yaw、Pitch、Roll)的转换_第1张图片

定义\psi\theta\phi分别为绕Z轴、Y轴、X轴的旋转角度,如果用Tait-Bryan angle表示,分别为Yaw、Pitch、Roll。

四元数与欧拉角(Yaw、Pitch、Roll)的转换_第2张图片

一、四元数的定义

q=[w,x,y,z]^T

\left | q \right |^2 = w^2+x^2+y^2+z^2 =1

  • 通过旋转轴和绕该轴旋转的角度可以构造一个四元数:

w=cos(\alpha/2)

x=sin(\alpha/2)cos(\beta_x)

y=sin(\alpha/2)cos(\beta_y)

z=sin(\alpha/2)cos(\beta_z)

  • 其中α是一个简单的旋转角(旋转角的弧度值),而cos(\beta _x),cos(\beta _y),cos(\beta _z)是定位旋转轴的“方向余弦”(欧拉旋转定理)。

利用欧拉角也可以实现一个物体在空间的旋转,它按照既定的顺序,如依次绕z,y,x分别旋转一个固定角度,使用roll,yaw ,pitch分别表示物体绕,x,y,z的旋转角度,记为\psi\theta\phi,可以利用三个四元数依次表示这三次旋转,即:

Q_1=cos(\psi /2 ) +sin(\psi /2) k

Q_2=cos(\theta /2 ) +sin(\theta /2) j

Q_3=cos(\phi /2 ) +sin(\phi /2) i

二、欧拉角到四元数的转换

2.1 公式:

四元数与欧拉角(Yaw、Pitch、Roll)的转换_第3张图片

2.2 code:

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

Quaternion ToQuaternion(double yaw, double pitch, double roll) // yaw (Z), pitch (Y), roll (X)
{
    // Abbreviations for the various angular functions
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);

    Quaternion q;
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;

    return q;
}

三、四元数到欧拉角的转换

3.1 公式

可以从四元数通过以下关系式获得欧拉角:

四元数与欧拉角(Yaw、Pitch、Roll)的转换_第4张图片

  • arctan和arcsin的结果是[-\frac{\pi}{2},\frac{\pi}{2}],这并不能覆盖所有朝向(对于\theta[-\frac{\pi}{2},\frac{\pi}{2}]的取值范围已经满足),因此需要用atan2来代替arctan。

四元数与欧拉角(Yaw、Pitch、Roll)的转换_第5张图片

3.2 code:

#define _USE_MATH_DEFINES
#include 

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

struct EulerAngles {
    double roll, pitch, yaw;
};

EulerAngles ToEulerAngles(Quaternion q) {
    EulerAngles 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.roll = 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.pitch = std::copysign(M_PI / 2, sinp); // use 90 degrees if out of range
    else
        angles.pitch = 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.yaw = std::atan2(siny_cosp, cosy_cosp);

    return angles;
}

 

3.3 四元素到旋转矩阵转换

或等效地,通过齐次表达式:

四. 奇点

当螺距接近±90°(南北极)时,必须意识到欧拉角参数化的奇异性。这些情况必须特别处理。这种情况的通用名称是万向节锁。

处理奇异点的代码可从以下网站获取:www.euclideanspace.com

五. 矢量旋转

定义四元素的尺度q_0 和向量 \overrightarrow{q},有{\displaystyle \mathbf {q} =(q_{0},{\vec {q}})=q_{0}+iq_{1}+jq_{2}+kq_{3}}.

请注意,通过定义欧拉旋转的四元数{\displaystyle q}来旋转三维矢量{\vec{v}}的规范方法是通过公式:

{\displaystyle \mathbf {p} ^{\,\prime }=\mathbf {qpq} ^{\ast }}

这儿:{\displaystyle \mathbf {p} =(0,{\vec {v}})=0+iv_{1}+jv_{2}+kv_{3}}是包含嵌入向量{\vec{v}}的四元数,{\displaystyle \mathbf {q} ^{\ast }=(q_{0},-{\vec {q}})} {\displaystyle \mathbf {q} ^{\ast }=(q_{0},-{\vec {q}})}是共轭四元数,

在计算实现中,这需要两个四元数乘法。一种替代方法是应用一对关系:

{\displaystyle {\vec {t}}=2{\vec {q}}\times {\vec {v}}}

{\displaystyle {\vec {v}}^{\,\prime }={\vec {v}}+q_{0}{\vec {t}}+{\vec {q}}\times {\vec {t}}}

\times:表示三维矢量叉积。这涉及较少的乘法,因此计算速度更快。数值测试表明,对于矢量旋转,后一种方法可能比原始方法快30%[4]。

证明:

标量和矢量部分的四元数乘法的一般规则由下式给出:

{\displaystyle {\begin{aligned}\mathbf {q_{1}q_{2}} &=(r_{1},{\vec {v}}_{1})(r_{2},{\vec {v}}_{2})\\&=(r_{1}r_{2}-{\vec {v}}_{1}\cdot {\vec {v}}_{2},r_{1}{\vec {v}}_{2}+r_{2}{\vec {v}}_{1}+{\vec {v}}_{1}\times {\vec {v}}_{2})\\\end{aligned}}}

利用这种关系{\displaystyle \mathbf {p} =(0,{\vec {v}})}可以找到:

{\displaystyle {\begin{aligned}\mathbf {pq^{\ast }} &=(0,{\vec {v}})(q_{0},-{\vec {q}})\\&=({\vec {v}}\cdot {\vec {q}},q_{0}{\vec {v}}-{\vec {v}}\times {\vec {q}})\\\end{aligned}}}

并替换为三乘积:

{\displaystyle {\begin{aligned}\mathbf {qpq^{\ast }} &=(q_{0},{\vec {q}})({\vec {v}}\cdot {\vec {q}},q_{0}{\vec {v}}-{\vec {v}}\times {\vec {q}})\\&=(0,q_{0}^{2}{\vec {v}}+q_{0}{\vec {q}}\times {\vec {v}}+({\vec {v}}\cdot {\vec {q}}){\vec {q}}+q_{0}{\vec {q}}\times {\vec {v}}+{\vec {q}}\times ({\vec {q}}\times {\vec {v}}))\\\end{aligned}}}

{\displaystyle q_{0}^{2}=1-{\vec {q}}\cdot {\vec {q}}} {\displaystyle q_{0}^{2}=1-{\vec {q}}\cdot {\vec {q}}}

{\displaystyle {\vec {q}}\times ({\vec {q}}\times {\vec {v}})=({\vec {q}}\cdot {\vec {v}}){\vec {q}}-({\vec {q}}\cdot {\vec {q}}){\vec {v}}}

可得到:

{\displaystyle {\begin{aligned}\mathbf {p} ^{\prime }&=\mathbf {qpq^{\ast }} =(0,{\vec {v}}+2q_{0}{\vec {q}}\times {\vec {v}}+2{\vec {q}}\times ({\vec {q}}\times {\vec {v}}))\\\end{aligned}}}

在定义{\displaystyle {\vec {t}} = 2 {\vec {q}} \times {\vec {v}}}时,可以按标量和矢量部分来表示:

{\displaystyle (0,{\vec {v}}^{\,\prime })=(0,{\vec {v}}+q_{0}{\vec {t}}+{\vec {q}}\times {\vec {t}}).}

六 . 其他参考

  • Rotation operator (vector space)
  • Quaternions and spatial rotation
  • Euler Angles
  • Rotation matrix
  • Rotation formalisms in three dimensions

七 python 四元素欧拉角互相转换

def EulerAndQuaternionTransform( intput_data):
	data_len = len(intput_data)
	angle_is_not_rad = False

	if data_len == 3:
		r = 0
		p = 0
		y = 0
		if angle_is_not_rad: # 180 ->pi
			r = math.radians(intput_data[0]) 
			p = math.radians(intput_data[1])
			y = math.radians(intput_data[2])
		else:
			r = intput_data[0] 
			p = intput_data[1]
			y = intput_data[2]

		sinp = math.sin(p/2)
		siny = math.sin(y/2)
		sinr = math.sin(r/2)

		cosp = math.cos(p/2)
		cosy = math.cos(y/2)
		cosr = math.cos(r/2)

		w = cosr*cosp*cosy + sinr*sinp*siny
		x = sinr*cosp*cosy - cosr*sinp*siny
		y = cosr*sinp*cosy + sinr*cosp*siny
		z = cosr*cosp*siny - sinr*sinp*cosy

		return {w,x,y,z}

	elif data_len == 4:

		w = intput_data[0] 
		x = intput_data[1]
		y = intput_data[2]
		z = intput_data[2]

		r = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
		p = math.asin(2 * (w * y - z * x))
		y = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))

		if angle_is_not_rad ==False: # 180 ->pi

			r = r / math.pi * 180
			p = p / math.pi * 180
			y = y / math.pi * 180

		return {r,p,y}

8 Eigen transform

欧拉角到四元素

      Eigen::Quaterniond RollPitchYaw(const double roll, const double pitch,
                                      const double yaw) {
        const Eigen::AngleAxisd roll_angle(roll, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd pitch_angle(pitch, Eigen::Vector3d::UnitY());
        const Eigen::AngleAxisd yaw_angle(yaw, Eigen::Vector3d::UnitZ());
        return yaw_angle * pitch_angle * roll_angle;
      }

四元素得到yaw

    template 
      T GetYaw(const Eigen::Quaternion& rotation) {
        const Eigen::Matrix direction =
          rotation * Eigen::Matrix::UnitX();
        return atan2(direction.y(), direction.x());
      }

四元素到旋转向量

    template 
      Eigen::Matrix RotationQuaternionToAngleAxisVector(
        const Eigen::Quaternion& quaternion) {
        Eigen::Quaternion normalized_quaternion = quaternion.normalized();
        // We choose the quaternion with positive 'w', i.e., the one with a smaller
        // angle that represents this orientation.
        if (normalized_quaternion.w() < 0.) {
          // Multiply by -1. http://eigen.tuxfamily.org/bz/show_bug.cgi?id=560
          normalized_quaternion.w() = -1. * normalized_quaternion.w();
          normalized_quaternion.x() = -1. * normalized_quaternion.x();
          normalized_quaternion.y() = -1. * normalized_quaternion.y();
          normalized_quaternion.z() = -1. * normalized_quaternion.z();
        }
        // We convert the normalized_quaternion into a vector along the rotation axis
        // with length of the rotation angle.
        const T angle =
          2. * atan2(normalized_quaternion.vec().norm(), normalized_quaternion.w());
        constexpr double kCutoffAngle = 1e-7;  // We linearize below this angle.
        const T scale = angle < kCutoffAngle ? T(2.) : angle / sin(angle / 2.);
        return Eigen::Matrix(scale * normalized_quaternion.x(),
                                      scale * normalized_quaternion.y(),
                                      scale * normalized_quaternion.z());
      }

旋转轴向量到四元素

    template 
      Eigen::Quaternion AngleAxisVectorToRotationQuaternion(
        const Eigen::Matrix& angle_axis) {
        T scale = T(0.5);
        T w = T(1.);
        constexpr double kCutoffAngle = 1e-8;  // We linearize below this angle.
        if (angle_axis.squaredNorm() > kCutoffAngle) {
          const T norm = angle_axis.norm();
          scale = sin(norm / 2.) / norm;
          w = cos(norm / 2.);
        }
        const Eigen::Matrix quaternion_xyz = scale * angle_axis;
        return Eigen::Quaternion(w, quaternion_xyz.x(), quaternion_xyz.y(),
                                    quaternion_xyz.z());
      }

 

你可能感兴趣的:(四元数与欧拉角(Yaw、Pitch、Roll)的转换)