math: 四元数与欧拉角（RPY角）的相互转换

1 四元数

1.1 理论基础

在我们能够完全理解四元数之前，我们必须先知道四元数是怎么来的。四元数的根源其实是复数。

四元数的概念是由爱尔兰数学家Sir William Rowan Hamilton发明的, 公式是：

$i^2 = j^2 = k^2 = ijk = -1$

一般表达式：

$q = w + xi + yj + zk$

性质：

$|q|^2 = w^2 + x^2 + y^2 + z^2 = 1$

其中：i，j，k都是复数。并且通过公式：

$i^2 = j^2 = k^2 = ijk$

可以推出：

$ij = k$

$jk = i$

$ki = j$

通过公式：

$ijk = -1$

$i^{-1}= -i$

$j^{-1}= -j$

$k^{-1}= -k$

可以推出：

$ji = -k$

$kj = -i$

$ik = -j$

你可能已经注意到了，i、j、k之间的关系非常像笛卡尔坐标系下 单位向量的叉积规则：

${ \vec x \times \vec y= \vec z} { \space \longrightarrow \space} { \vec y \times \vec x=− \vec z}$

${ \vec y \times \vec z= \vec x} { \space \longrightarrow \space} { \vec z \times \vec y=− \vec x}$

${ \vec z \times \vec x= \vec y} { \space \longrightarrow \space} { \vec x \times \vec z=− \vec y}$

Hamilton自己也发现i、j、k虚数，可以被用来表达3个笛卡尔坐标系的单位向量i、j、k，并且仍然保持有虚数的性质，也即： $i^2=j^2=k^2=−1$。

1.1.1 三维空间下：

向量差乘即 向量积可以被定义为：
$| \vec a \times \vec b |= | \vec a | \cdot | \vec b | \cdot \sin \theta$

叉乘(向量的外积)是物理里面常常用到的概念, 它是由两个向量得到一个新的向量的运算。一般我们都是从几何意义下手: 向量 $\vec a$ 和向量 $\vec b$ 叉乘，得到的是一个垂直于 $\vec a$ 和 $\vec b$ 的向量，$\vec c = \vec a \times \vec b$ 它的方向由右手螺旋法则确定, 它的长度是 $\vec a$ 和 $\vec b$ 组成的一个平行四边形的面积。

如下图所示：

叉乘满足的基本的性质如下:

• $\vec a \times \vec a = \vec 0$： 因为夹角是0, 所以平行四边形面积也是0, 即叉积长度为0；
• $\vec a \times \vec b = -(\vec b \times \vec a)$：等式两边的叉积等大反向, 模长因为平行四边形不变而相同, 方向因为右手法则旋转方向相反而相反；

• $(\lambda \vec a)\times \vec b = \lambda (\vec a \times \vec b)$：这点比较好想, 因为:

  1. 正数 $\lambda$ 数量乘不会影响 $\vec a$ 的方向, 所以左右的叉积方向一样; 负数 $\lambda$ 使得 $\vec a$ 反向了, 但也使得左右叉积方向相反.
  2. 对 $\vec a$ 进行缩放, 平行四边形面积也同等缩放.

• $(\vec a + \vec b)\times \vec c = \vec a \times \vec c + \vec b \times \vec c$：这最难想象的了放弃

1.1.2 四维空间下：

同理将i、j、k虚数用来表达3个笛卡尔坐标系单位向量X、Y、Z如下图所示：

可以得到：

${ \vec i \times \vec j= \vec k} { \space \longrightarrow \space} { \vec j \times \vec i=− \vec k}$

${ \vec j \times \vec k= \vec i} { \space \longrightarrow \space} { \vec k \times \vec j=− \vec i}$

${ \vec k \times \vec i= \vec j} { \space \longrightarrow \space} { \vec i \times \vec k=− \vec j}$

至此，可以将四元素看作空间中任意一个向量了，为了简便，使用有序对的形式表示一个四元素：

${q = w + xi + yj + zk} { \space \longrightarrow \space} {[w, \vec v]}$

其中向量 $\vec v$ 有 $xi + yj + zk$ ，x ，y，z轴3个方向的分量。

1.1.3 四元数加减：

和复数类似，四元数也可以被加减，假设有两个向量：

$q_a = [w_a, \vec a]$
$q_b = [w_b, \vec b]$

于是有

$q_a + q_b = [w_a + w_b, \vec a + \vec b]$
$q_a - q_b = [w_a - w_b, \vec a - \vec b]$

1.1.4 四元数的积：

$q_aq_b = [w_a, \vec a][w_b, \vec b]$
$= (w_a + x_ai + y_aj + z_ak)(w_b + x_bi + y_bj + z_bk)$
$= (w_aw_b - x_ax_b - y_ay_b - z_az_b)$
$+ (w_ax_b + w_bx_a + y_az_b + y_bz_a)i$
$+ (w_ay_b + w_by_a + z_ax_b + z_bx_a)j$
$+ (w_az_b+ w_bz_a + x_ay_b + x_by_a)k$

2. 欧拉角

首先介绍roll，pitch，yaw的概念：

2.1 Roll:横滚

2.2 Pitch: 俯仰

2.3 Yaw: 偏航（航向）

在无人车或者机器上主要考虑的是Yaw偏航/航向角，因此这里不全部公式偏向于对yaw角的转换计算。

3. 四元数转欧拉角

假设有一个四元数的向量：$Q(x, y, z, w)$，绕轴$A（a_x，a_y，a_z）$旋转一个固定角度 $\alpha$ , 将该动作分解为绕x，y，z（即$a_x，a_y，a_z$）轴旋转角度roll，yaw，pitch，有以下公式：

$|q|^2 = w^2 + x^2 + y^2 + z^2 = 1$

$\left[\begin{matrix} roll\\pith\\yaw \end{matrix} \right] = \left[\begin{matrix} \phi \\ \theta \\ \psi \end{matrix} \right] = \left[\begin{matrix} atan2(\frac{2(wx + yz)}{w^2 - x^2 - y^2 + z^2}) \\ \arcsin(a(wy-zx)) \\ atan2(\frac{2(wz + zx)}{w^2 + x^2 - y^2 - z^2}) \end{matrix} \right] = \left[\begin{matrix} atan2(\frac{2(wx + yz)}{1 - 2(x^2 + y^2）}) \\ \arcsin(a(wy-zx)) \\ atan2(\frac{2(wz + zx)}{1 - 2(y^2 + z^2）}) \end{matrix} \right]$

由于：

$1 - 2(x^2 + y^2）= w^2 - x^2 - y^2 + z^2$
$1 - 2(y^2 + z^2）= w^2 + x^2 - y^2 - z^2$

PS：这里不用$\arctan$是因为：$\arctan$ 和 $\arcsin$ 的结果是$\left[\begin{matrix} -\frac{\pi}{2}\\\frac{\pi}{2} \end{matrix} \right]$， 这并不能覆盖所有朝向(仅仅对于 pith角 $\theta$ ，$\left[\begin{matrix} -\frac{\pi}{2}\\\frac{\pi}{2} \end{matrix} \right]$的取值范围满足，但是我们主要考虑的是yaw角)，因此需要用atan2 来代替 $\arctan$。

4. 欧拉角转四元数

4.1 转化公式

用$Q(x, y, z, w)$表示一个四元数的向量，绕轴$A（a_x，a_y，a_z）$旋转角度$\alpha$ 有以下公式：
完整公式：

$q = \left[\begin{matrix} w\\x\\y\\z \end{matrix} \right] = \left[\begin{matrix} \cos(\phi /2)\cos(\theta /2)\cos(\psi /2) + \sin(\phi/2)\sin(\theta/2)\sin(\psi/2) \\ \sin(\phi /2)\cos(\theta /2)\cos(\psi /2) - \cos(\phi/2)\sin(\theta/2)\sin(\psi/2) \\ \cos(\phi /2)\sin(\theta /2)\cos(\psi /2) + \sin(\phi/2)\cos(\theta/2)\sin(\psi/2) \\ \cos(\phi /2)\cos(\theta /2)\sin(\psi /2) - \sin(\phi/2)\sin(\theta/2)\cos(\psi/2) \end{matrix} \right]$

如若只绕z轴旋转简化公式：

$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)$

其中$\alpha$是绕旋转轴旋转的角度，$\cos(\beta_x)$, $\cos(\beta_y)$, $\cos(\beta_z)$分别为旋转轴在x,y,z方向的分量（由此确定了旋转轴），这里绕z轴旋转，因此$\cos(\beta_x) = 0$ 并且$\cos(\beta_y) = 0$， $\cos(\beta_z) = 1$。

5. 代码实例

 void eulerAnglesToQuaternion(void) 
{ 
    cosRoll = cosf(roll * 0.5f); 
    sinRoll = sinf(roll * 0.5f);

    cosPitch = cosf(pitch * 0.5f);
    sinPitch = sinf(pitch * 0.5f);

    cosHeading = cosf(hdg * 0.5f);
    sinHeading = sinf(hdg * 0.5f);

    q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
    q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
    q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
    q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; 
}

void quaternionToRotationMatrix(void) 
{ 
    float q1q1 = sq(q1); 
    float q2q2 = sq(q2); 
    float q3q3 = sq(q3);

    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q3 = q2 * q3;

    rMat[0][0] = 1.0f - 2.0f * q2q2 - 2.0f * q3q3;
    rMat[0][1] = 2.0f * (q1q2 + -q0q3);
    rMat[0][2] = 2.0f * (q1q3 - -q0q2);

    rMat[1][0] = 2.0f * (q1q2 - -q0q3);
    rMat[1][1] = 1.0f - 2.0f * q1q1 - 2.0f * q3q3;
    rMat[1][2] = 2.0f * (q2q3 + -q0q1);

    rMat[2][0] = 2.0f * (q1q3 + -q0q2);
    rMat[2][1] = 2.0f * (q2q3 - -q0q1);
    rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2;
}

void quaternionToEulerAngles(void) 
{ 
    roll = atan2f(2.f * (q2q3 + q0q1), q0q0 - q1q1 - q2q2 + q3q3); 
    pitch = asinf(2.f * (q0q2 - q1q3)); 
    yaw = atan2f(2.f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3); 
}

参考：

https://en.wikipedia.org/wiki/Cross_product
https://www.3dgep.com/understanding-quaternions/
https://www.cnblogs.com/zzdyyy/p/7643267.html
https://blog.csdn.net/hziee_/article/details/1630116