四元数(二)

2019-10-16

四元数（Quaternion）(二)

四元数(一)

3. 四元数

3.1 性质

3.1.1 模长(范数)

仿照复数的定义, 可以暂时将一个四元数$q=a+bi+cj+dk$的模长定义为:

$\Vert q \Vert = \sqrt{a^2+b^2+c^2+d^2}$

若用标量向量有序对的形式进行表示的话, $q = [s, \mathbf v]$的模长为:

$\Vert q \Vert = \sqrt{s^2 + \Vert \mathbf v \Vert ^2}\\ = \sqrt{s^2 + \mathbf v · \mathbf v}$

四元数很难用几何的方法进行理解, 因为它代表四维长度. 但是, 和高维向量的模长一样, 这只是类比复数模长进行衍生定义的结果.
>>

3.1.2 四元数的加减法

跟复数一样, 直接相加(减), 假设我们有$q_1=a+bi+cj+dk$和$q_2=e+fi+gj+hk$, 求和(减法也一样):

$q_1 \pm q_2 = a \pm bi \pm cj \pm dk \pm e \pm fi \pm gj \pm hk\\ = (a \pm e) \pm (b \pm f)i \pm (c \pm g)j \pm (d \pm h)k$

写成标量向量有序对, $q_1=[s, \mathbf v], q_2=[t, \mathbf u]$, 那么有:

$q_1 \pm q_2 = [s \pm t, \mathbf v \pm \mathbf u]$

3.1.3 四元数的乘法

首先说四元数与标量之间的乘法, 假设有四元数$q=a+bi+cj+dk$和标量$s$, 它们的乘积表示为: $sq=s(a+bi+cj+dk)$, 并且遵守交换律.
而四元数与四元数的乘法不同, 不遵守交换律, 即$q_1q_2 \neq q_2q_2$, 而结合律和分配律都是成立的.
同样, 假设有两个四元数$q_1=a+bi+cj+dk$和$q_2=e+fi+gj+hk$, 它们的乘积表示为:

$q1q2 = (a + bi + cj + dk)(e + f i + gj + hk)\\ = ae + a f i + agj \\+ ahk+ bei + b f i^2 \\+ bgij + bhik+cej \\+ c f ji + cgj^2 + chjk\\+dek + d f ki + dgkj \\+ dhk^2 \tag{1}$

尽管结果很混乱, 但是别忘了复数的性质, 由于$i^2 = j^2 = k^2 = ijk = -1$, 可以得出

$ijk = -1\\ iijk = -i\\ -jk = -i\\ jk = i$

这里只写4种情况, 用同样的思路可以得到更多等式, 最终得到这样一张表格:

×	1	i	j	k
1	1	i	j	k
i	i	-1	k	-j
j	j	-k	-1	i
k	k	j	-i	-1

利用这张表格将公式(1)化简, 得到:

$q_1q_2 = \\ (ae − b f − cg − dh)+\\ (be + a f − dg + ch)i+\\ (ce + d f + ag − bh)j+\\ (de − c f + bg + ah)k \tag{2}$

从前面可以发现, 四元数的相乘其实也是一个线性组合, 我们同样可以将它写成矩阵形式:

$q_1q_2 = \left[ \begin{matrix} a&-b&-c&-d\\b&a&-d&c\\c&d&a&-b\\d&-c&b&a \end{matrix} \right] \left[ \begin{matrix} e\\f\\g\\h \end{matrix} \right]$

注意这里是$q_1q_2$, 由于不符合交换律, $q_2q_1$的结果可以自己推, 这里就不写了.
现在将公式(2)写成这样的形式:

$q_1q_2 = (ae − (b f + cg + dh))+\\ (be + a f + ch − dg)i+\\ (ce + ag + d f − bh)j+\\ (de + ah + bg − c f)k$

现在令$\mathbf v = [b,c,d]^T, \mathbf = [f,g,h]^T$, 可以得到:

$\mathbf u \mathbf v = bf + cg + dh\\ \mathbf u × \mathbf v = \left| \begin{matrix} \mathbf i & \mathbf j & \mathbf k\\ b & c & d\\ f & g & h \end{matrix} \right|= (ch − dg)\mathbf i − (bh − d f)\mathbf j + (bg − c f)\mathbf k$

$\mathbf u, \mathbf v$是向量, $\mathbf i, \mathbf j, \mathbf k$是向量的基, 如果用标量向量有序对来表示, 就写成如下形式:

$q_1q_2 = [ae - \mathbf v · \mathbf u, a\mathbf u + e\mathbf v + \mathbf v × \mathbf u] \tag{3}$

公式(3)给出的也就是Graßmann积(Graßmann Product).

3.1.3 纯四元数

仅有虚部的四元数被称为纯四元数, 纯四元数被表示为如下形式:

$v = [0, \mathbf v]$

纯四元数的一条比较重要的特性, 假设有两个纯四元数$v=[0, \mathbf v], u=[0, \mathbf u]$, 那么:

$vu = [0 - \mathbf v · \mathbf u, 0 + \mathbf v × \mathbf u] = [-\mathbf v · \mathbf u, \mathbf v × \mathbf u]$

3.1.4 逆与共轭

假设一个四元数$q$, 那么它的逆就是$q^{-1}$, 可以得到$qq^{-1}=q^{-1}q=1(q \neq 0)$, 也就是说:

$(pq)q^−1 = p(qq^−1) = p · 1 = p\\ q^−1(qp) = (q^−1q)p = 1 · p = p$

所以, 右乘$q$的逆运算为右乘$q^{-1}$, 左乘$q$的逆运算为左乘$q^{-1}$, 这与矩阵的性质十分相似.
事实上, 四元数要满足$qq^{-1}=q^{-1}q=1$是非常困难的, 我们可以使用四元数共轭的一些性质来获得$q^{-1}$.
假设一个四元数$q=a+bi+ck+dj$的共轭为

$q^*=a-bi- cj-dk$

用标量向量有序对来定义

$q=[s, \mathbf v]$

共轭为

$q^*=[s, -\mathbf v]$

共轭四元数的一个重要性质是:

$qq^*=[s, \mathbf v]·[s, -\mathbf v]\\ =[s^2-\mathbf v ·(-\mathbf v), s(-\mathbf v)+s\mathbf v + \mathbf v ×(- \mathbf v)]\\ =[s^2+\mathbf v · \mathbf v, \mathbf 0] (平行向量叉乘为0)$

可以发现最后得到的结果是一个实数, 并且是四元数模长的平方:

$qq^*=[s^2+\mathbf v · \mathbf v, \mathbf 0]\\ =s^2 + x^2 + y^2 + z^2\\ =\Vert q \Vert ^2\\ 又因为(q^*)^*=[s, -(-\mathbf v)] = [s, \mathbf v] = q,\\ q^*q=(q^*)(q^*)^*\\ =\Vert q^* \Vert ^2\\ =s^2+x^2+y^2+z^2\\ =\Vert q \Vert ^2\\ =qq^*$

至此, 得出$q^ q = q q^$, 想想四元数逆的定义:

$qq^{-1} = 1\\ q^*qq^{-1}=q^*\\ (q^*q)q^{-1}=q^*\\ \Vert q \Vert ^2 · q^{-1} = q^*\\ q^{-1} = \frac {q^*}{\Vert q \Vert ^2}$

利用这种方式可以高效得到一个四元数的逆, 总的来说, 如果$\Vert q \Vert = 1$, 则$q$是一个单位四元数, 可以得到:

$q^{-1} = \frac {q^*}{1^2} = q^*$

3.2 四元数与3D旋转

终于, 千辛万苦来到正题, 四元数如何表示一个3D旋转.
如果我们需要将一个向量$\mathbf v$沿着一个用单位向量所定义的旋转轴$\mathbf u$旋转$\theta$度, 那么需要对向量做分量变换, 对分量进行旋转(第一部分过的), 所以我们拥有几个向量, $\mathbf u, \mathbf v, \mathbf v_\bot, \mathbf v_\Vert, \mathbf v^′, \mathbf v^′_\bot, \mathbf v_\Vert^′$ 将它们表示为纯四元数:

$v = [0, \mathbf v]\\ ...(其他的也是一样的, 不写了)$

没有加粗的都表示是纯四元数, 所以:

$v=v_\Vert + v_\bot\\ v^′=v_\Vert^′+v_\bot^′$

OK, 和之前一样, 对$v_\bot, v_\Vert$分别讨论.

3.2.1 $v_\bot$的旋转

同样, $v_\bot$正交与旋转轴$\mathbf u$, 因此(不记得的可以看下第一部分):

$\mathbf v^′ = \cos({\theta})\mathbf v_\bot + \sin({\theta})(\mathbf u × \mathbf v_\bot)$

直接将对应的虚四元数替换后, 还剩一个$\mathbf u × \mathbf v_\bot$. 不过利用四元数的性质, 可以将它写成四元数积的形式.

$由vu=[-\mathbf v · \mathbf u, \mathbf v ×\mathbf u],\\ uv_\bot=[-\mathbf u · \mathbf v_\bot, \mathbf u × \mathbf v_\bot]\\ 因为\mathbf v_\bot 正交于\mathbf u, 所以\mathbf u · \mathbf v_\bot=0,\\ uv_\bot=[0, \mathbf u × \mathbf v_\bot]\\ =\mathbf u × \mathbf v_\bot$

这里$uv_\bot$仍然是纯四元数, 将这个等式与之前的定义的纯四元数联系起来, 得到:

$v_\bot^′=\cos({\theta})v_\bot+\sin({\theta})(uv_\bot)\\ =(\cos({\theta})+\sin({\theta})u)v_\bot$

至此, 如果将$((\cos({\theta})+\sin({\theta})u))$看做一个四元数, 就已经将旋转与四元数的积联系起来了. 令$q=\cos({\theta})+\sin({\theta})u$, 有

$v_\bot^′=qv_\bot$

构造一个$q$, 就能对$q$进行变形:

$q = \cos({\theta}) + \sin({\theta})u\\ =[\cos({\theta}), \mathbf 0]+[0, \sin({\theta})\mathbf u]\\ =[\cos({\theta}), \sin({\theta})\mathbf u]$

也就是说, 如果旋转轴$\mathbf u$的坐标为$[u_x, u_y, y_z]^T$,旋转角为$\theta$, 那么完成这一旋转所需要的四元数$q$可以构造为:

$q = \cos({\theta}) + \sin({\theta})u_xi+\sin({\theta})u_yj+\sin({\theta})u_zk$

总结起来, 当$\mathbf v_\bot$正交与旋转$\mathbf u$时, 旋转$\theta$角度之后的$\mathbf v_\bot^′$可以用四元数惩罚来获得, 令$v_\bot=[0, \mathbf v_\bot], q=[\cos({\theta}), \sin({\theta})\mathbf u]$, 那么:

$v_\bot^′=qv_\bot$

3.2.2 $v_\Vert$的旋转

同样, 由于$\mathbf v_\Vert$与旋转轴$\mathbf u$平行, 旋转不会对它做任何变换, 也就是说:

$v^′_\Vert = v_\Vert$

3.2.3 $v$的旋转

所以, 现在我们知道一般情况下, 对于$v^′$:

$v^′ = v^′_\Vert + v^′_\bot\\ =v_\Vert + qv_\bot$

注意, $q=[\cos({\theta}), \sin({\theta})\mathbf u]$.
按照以前的做法, 我们会把$v_\Vert$和$v_\bot$拆开继续化简, 但是这里不会这么做, 因为有更好的办法.
首先介绍几个引理:

Lemma 1. 如果$q=[\cos({\theta}), \sin({\theta})\mathbf u]$, 而且$\mathbf u$为单位向量, 那么$q^2=qq=[\cos({2\theta}), \sin({2\theta})\mathbf u]$.

证明不难:

$q^2 = [\cos({\theta}), sin({\theta})\mathbf u]·[cos({\theta}), sin({\theta})\mathbf u]\\ =[\cos^2({\theta}) - (\sin({\theta})\mathbf u · \sin({\theta})\mathbf u), \\(\cos({\theta})\sin({\theta})+\sin({\theta})\cos({\theta}))\mathbf u + (\sin({\theta})\mathbf u × \sin({\theta})\mathbf u)]\\ =[\cos^2({\theta}) - \sin^2({\theta})\Vert \mathbf u \Vert^2, 2\sin({\theta})\cos({\theta})\mathbf u + \mathbf 0]\\ =[\cos^2({\theta}) - \sin^2({\theta}), 2\sin({\theta})\cos({\theta})\mathbf u]\\ =[\cos({2\theta}), \sin({2\theta})\mathbf u]$

简单解释一下, 如果绕着同一个轴$\mathbf u$连续旋转$\theta$度两次, 那么所做出的变换等同于直接绕着$\mathbf u$旋转$2\theta$度.
基于这个引理, 同时根据四元数逆的定义$qq^{-1}=1$, 对旋转公式变形:

$v^′ = v_\Vert + qv_\bot\\ = 1·v_\Vert + qv_\bot\\ (令q=p^2, 则p=[\cos({\frac {\theta}{2}}), \sin({\frac {\theta}{2}})\mathbf u])\\ =pp^{-1}v_\Vert + ppv_\bot$

这里又引入了新的四元数$p=[\cos({\frac {\theta}{2}}), \sin({\frac {\theta}{2}})\mathbf u])$, 根据前面的引理, 可以验证:

$pp=p^2\\ =[\cos({2·\frac {\theta}{2}}), \sin({2·\frac {\theta}{2}})\mathbf u])\\ =q$

这里注意到, 和$q$一样, $\Vert p \Vert=1$, $p$也是一个单位四元数, 所以:

$p^{-1} = p^*$

将这个结果代入之前的等式中:

$v^′ = pp^{-1}v_\Vert + ppv_\bot\\ =pp^*v_\Vert + ppv_\bot$

这个等式还能继续化简, 至此再引入两个定理(证明的过程就不写了, 都是利用Graßmann 积来证)

Lemma 2. 假设$v_\Vert$=[0, $\mathbf v_\Vert$]是一个纯四元数, 而$q=[\alpha, \beta \mathbf u]$, 其中$\mathbf u$是一个单位向量, $\alpha, \beta \in \mathbb R$. 在这样的条件下, 如果$\mathbf v_\Vert$平行于$\mathbf u$, 那么$qv_\Vert=v_\Vert q$
Lemma 3. 假设$v_\bot = [0, \mathbf v_\bot]$是一个纯四元数, 而$q=[\alpha, \beta \mathbf u]$, 其中$\mathbf u$是一个单位向量, $\alpha, \beta \in \mathbb R$. 该情况下, 如果$\mathbf v_\bot$正交于$\mathbf u$, 那么$qv_\bot=v_\bot q^*$

有了以上两条引理, 对等式进一步化简:

$v^′ = pp^*v_\Vert + ppv_\bot\\ =pv_\Vert p^* + pv_\bot p^*\\ =p(v_\Vert + v_\bot)p^*$

显然, $(v_\Vert + v_\bot)$其实就是$v$, 所以:

$v^′=pvp^*$

至此, 3D空间中任意一个旋转都能够用三个四元数相乘的形式表达出来, 尽管推理过程十分漫长, 但是结论却非常简洁.换句话说, 如果我们有$q=[\cos({\theta}), \sin({\theta})\mathbf u]$, 那么$v^′ = qvq^*$可以将$\mathbf v$沿着$\mathbf u$旋转$2\theta$度.

3.3 3D旋转的矩阵形式

在实际应用中, 通常将旋转和平移缩放进行复合, 所以需要用到四元数旋转的矩阵形式, 推导过程并不困难:
左乘一个四元数$q=a+bi+cj+dk等同于$:

$L(q)=\left[ \begin{matrix} a&-b&-c&-d\\b&a&-d&c\\c&d&a&-b\\d&-c&b&a \end{matrix}\right]$

右乘$q$等同于:

$R(q)=\left[ \begin{matrix} a&-b&-c&-d\\b&a&d&-c\\c&-d&a&b\\d&c&-b&a \end{matrix}\right]$

所以, 利用这两个公式将$v^′ = qvq^$写成矩阵形式. 假设$a=\cos({\frac {\theta}{2}}), b=\sin({\frac {\theta}{2}})u_x, c=\sin({\frac {\theta}{2}})u_y, d=\sin({\frac {\theta}{2}})u_z, q = a+bi+cj+dk$, 并且$R(q^)=R(q)^T$, 那么剩下的就是套公式就行了(请记住, $a^2+b^2+c^2+d^2=1$), 直接写出结论:

$qvq^*=\\ \left[ \begin{matrix} 1 & 0 & 0 & 0\\ 0 & 1-2c^2-2d^2 & 2bc-2ad & 2ac+2bd\\ 0 & 2bc+2ad & 1-2b^2-2d^2 & 2cd-2ab\\ 0 & 2bd-2ac & 2ab+2cd & 1-2b^2-2c^2 \end{matrix} \right]v$

这就是3D旋转的矩阵形式, 因为最外圈不会对$v$有任何变换, 所以可以压缩成$3×3$矩阵.
总结一下,任意向量$\mathbf v$沿着以单位向量定义的旋转轴$\mathbf u$旋转$\theta$角度之后的$\mathbf v^′$可以用矩阵乘法来获得. 令$a=\cos({\frac {\theta}{2}}), b=\sin({\frac {\theta}{2}})u_x, c=\sin({\frac {\theta}{2}})u_y, d=\sin({\frac {\theta}{2}})u_z$:

$\mathbf v^′ = \\ \left[ \begin{matrix} 1-2c^2-2d^2 & 2bc-2ad & 2ac+2bd\\ 2bc+2ad & 1-2b^2-2d^2 & 2cd-2ab\\ 2bd-2ac & 2ab+2cd & 1-2b^2-2c^2 \end{matrix} \right] \mathbf v$

3.4 Coding 欧拉角到四元数的转换.

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

四元数到欧拉角:

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

EulerAngles ToEulerAngles(Quaternion q)
{
    EulerAngles angles;

    // roll (x-axis rotation)
    double sinr_cosp = +2.0 * (q.w * q.x + q.y * q.z);
    double cosr_cosp = +1.0 - 2.0 * (q.x * q.x + q.y * q.y);
    angles.roll = atan2(sinr_cosp, cosr_cosp);

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

    // yaw (z-axis rotation)
    double siny_cosp = +2.0 * (q.w * q.z + q.x * q.y);
    double cosy_cosp = +1.0 - 2.0 * (q.y * q.y + q.z * q.z);  
    angles.yaw = atan2(siny_cosp, cosy_cosp);

    return angles;
}

python中有一个库pyquaternion, 可以帮助我们进行类似这样的变换, 其中给出一个例子, 在点云标注中往往会标注出物体的偏航角(yaw):

from pyquaternion import Quaternion
quat = Quaternion(scalar=np.cos(yaw / 2), vector=[0, 0, np.sin(yaw / 2)]) # 从偏航角到四元数

def quaternion_yaw(q: Quaternion) -> float:  # 从四元数到偏航角
    """
    Calculate the yaw angle from a quaternion.
    Note that this only works for a quaternion that represents a box in lidar or global coordinate frame.
    It does not work for a box in the camera frame.
    :param q: Quaternion of interest.
    :return: Yaw angle in radians.
    """

    # Project into xy plane.
    v = np.dot(q.rotation_matrix, np.array([1, 0, 0]))

    # Measure yaw using arctan.
    yaw = np.arctan2(v[1], v[0])

    return yaw

这里再推荐一个视频, 比我写的要生动一些…Fantastic Quaternions - Numberphile