Have a Question?

四元数 | Quaternion

You are here:

1 定义

1.1 四元数

四元数是复数的不可交换延伸。如把四元数的集合考虑成多维实数空间的话,四元数就代表着一个四维空间,相对于复数为二维空间。
PS:四元数有 Hamiton (Left Hand) 和 JPL (Right Hand) 两种。在这里我们后面公式表示都对应更常用的 Hamiton 四元数。
其表示为:
\begin{equation} \mathbf{q}=[w, x, y, z]^{T} \end{equation}
也可以记为:
\begin{equation} \mathbf{q} = w+xi+yj+zk \end{equation}
其中实部 w 为标量,虚部 x, y, z 为矢量。
三个虚部满足如下性质:
\begin{equation} \left\{\begin{matrix} i^2 + j^2 + k^2 = 1\\ ij=k, ji=-k \\ jk=i, kj=-i \\ ki=j, ik=-j \end{matrix}\right. \end{equation}
也可以记为:
\begin{equation} \mathbf{q} = [s,\mathbf{v}], \mathbf{v} = [x,y,z]^\mathrm{T} \in \mathbb{R}^3 \end{equation}
其中 s 为标量,v 为虚部的矢量。

1.2 单位四元数

三维空间中的任意旋转可以用单位四元数表示(注意是单位不是任意四元数),其满足条件 w^2+x^2+y^2+z^2=1

1.3 单位四元数与角轴的关系

单位四元数和角轴的转换关系:设角轴为 \omega\theta,那么它对应的四元数为:
\begin{equation} \mathbf{q}=\left[\cos \frac{\theta}{2}, \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}} \end{equation}

2 四元数运算

给定两个四元数 \mathbf{q}_{a} = w_{a} + x_{a} \cdot i + y_{a} \cdot j + z_{a} \cdot k\mathbf{q}_{b} = w_{b} + x_{b} \cdot i + y_{b} \cdot j + z_{b} \cdot k,则可以定义下面运算:

2.1 加减法

四元数的加法定义如下:
\begin{equation} \mathbf{q}_{a} \oplus \mathbf{q}_{b} =\left(w_{a} + w_{b}\right) + \left(x_{a} + x_{b}\right) i + \left(y_{a} + y_{b}\right) j + \left(z_{a} + z_{b}\right) k \end{equation}
四元数的减法定义如下:
\begin{equation} \mathbf{q}_{a} \ominus \mathbf{q}_{b} =\left(w_{a} - w_{b}\right) + \left(x_{a} - x_{b}\right) i + \left(y_{a} - y_{b}\right) j + \left(z_{a} - z_{b}\right) k \end{equation}

2.2 乘法

四元数的乘法定义如下:
\begin{equation} \begin{aligned} \mathbf{q}_{a} \otimes \mathbf{q}_{b} &=w_{a} w_{b}-x_{a} x_{b}-y_{a} y_{b}-z_{a} z_{b} \\ &+\left(w_{a} x_{b}+x_{a} w_{b}+y_{a} z_{b}-z_{a} y_{b}\right) i \\ &+\left(w_{a} y_{b}-x_{a} z_{b}+y_{a} w_{b}+z_{a} x_{b}\right) j \\ &+\left(w_{a} z_{b}+x_{a} y_{b}-y_{a} x_{b}+z_{a} w_{b}\right) k \end{aligned} \end{equation}

或者:
\begin{equation} \mathbf{q}_{a} \otimes \mathbf{q}_{b}=\left[s_{a} s_{b}-\mathbf{v}_{a}^{\mathrm{T}} \mathbf{v}_{b}, s_{a} \mathbf{v}_{b}+s_{b} \mathbf{v}_{a}+\mathbf{v}_{a} \times \mathbf{v}_{b}\right]^{\mathrm{T}} \end{equation}

2.3 共轭与逆

共轭:
\begin{equation} \boldsymbol{q}^{*}=s-x i-y j-z k=\left[s,-\boldsymbol{v}\right] \end{equation}

逆:
\begin{equation} \boldsymbol{q}^{-1}=\boldsymbol{q}^{*} /\|\boldsymbol{q}\|^{2} \end{equation}

四元数还有很多其他运算与复数一致,参见维基百科

3 四元数与空间旋转

3.1 四元数矩阵

两个旋转叠加可以表示为四元数相乘:
\begin{equation} \mathbf{q} \otimes \mathbf{p}=\left[\begin{array}{c} q_{w} p_{x}-q_{z} p_{y}+q_{y} p_{z}+q_{x} p_{w} \\ q_{z} p_{x}+q_{w} p_{y}-q_{x} p_{z}+q_{y} p_{w} \\ -q_{y} p_{x}+q_{x} p_{y}+q_{w} p_{z}+q_{z} p_{w} \\ -q_{x} p_{x}-q_{y} p_{y}-q_{z} p_{z}+q_{w} p_{w} \end{array}\right]=[\mathbf{q}]_{L} \mathbf{p}=[\mathbf{p}]_{R} \mathbf{q} \end{equation}
其中,左乘四元数矩阵定义为:
\begin{equation} [\mathbf{q}]_{L}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & -q_{z} & q_{y} \\ q_{y} & q_{z} & q_{w} & -q_{x} \\ q_{z} & -q_{y} & q_{x} & q_{w} \end{array}\right]=q_{w} \mathbf{I}+\left[\begin{array}{cc} {\left[\mathbf{q}_{v}\right]_{\times}} & \mathbf{q}_{v}^{T} \\ -\mathbf{q}_{v} & 0 \end{array}\right] \end{equation}

右乘四元数矩阵定义为:
\begin{equation} [\mathbf{q}]_{R}=\left[\begin{array}{cccc} q_{w} & -q_{x} & -q_{y} & -q_{z} \\ q_{x} & q_{w} & q_{z} & -q_{y} \\ q_{y} & -q_{z} & q_{w} & q_{x} \\ q_{z} & q_{y} & -q_{x} & q_{w} \end{array}\right]=q_{w} \mathbf{I}+\left[\begin{array}{cc} -\left[\mathbf{q}_{v}\right] \times & \mathbf{q}_{v}^{T} \\ -\mathbf{q}_{v} & 0 \end{array}\right] \end{equation}

3.2 四元数旋转三维点

已知一个三维点 \mathbf{p},进行旋转 \mathbf{q} 后的三维点 \mathbf{p}^{\prime}
\begin{equation} \mathbf{p}^{\prime}=\mathbf{R} \mathbf{p} \Leftrightarrow \mathbf{p}^{\prime}=\mathbf{q} \otimes \mathbf{p} \otimes \mathbf{q}^{-1}=\mathbf{q} \otimes \mathbf{p} \otimes \mathbf{q}^{*}=[\mathbf{q}]_{L}\left[\mathbf{q}^{*}\right]_{R} \mathbf{p}=\left[\mathbf{q}^{*}\right]_{R}[\mathbf{q}]_{L} \mathbf{p} \end{equation}

显然我们也可以由此求出四元数与旋转矩阵的关系:
\begin{equation} \mathbf{R}=\left[\begin{array}{ccc} q_{w}^{2}+q_{w}^{2}-q_{w}^{2}-q_{w}^{2} & 2\left(q_{x} q_{y}-q_{w} q_{z}\right) & 2\left(q_{x} q_{z}+q_{w} q_{y}\right) \\ 2\left(q_{x} q_{y}+q_{w} q_{z}\right) & q_{w}^{2}-q_{w}^{2}+q_{w}^{2}-q_{w}^{2} & 2\left(q_{y} q_{z}-q_{w} q_{x}\right) \\ 2\left(q_{x} q_{z}-q_{w} q_{y}\right) & 2\left(q_{y} q_{z}+q_{w} q_{x}\right) & q_{w}^{2}-q_{w}^{2}-q_{w}^{2}+q_{w}^{2} \end{array}\right] \end{equation}

4 四元数求导

设初始旋转为 \mathbf{q} = [s,\mathbf{v}],发生了角轴为 \omega\theta 的旋转,右乘对应四元数记为 \Delta \mathbf{q} = [\theta,\mathbf{\omega}],那么 \mathbf{q} 相对该旋转的导数推导如下:
\begin{equation} \begin{aligned} \lim _{\theta \rightarrow 0} \frac{\mathbf{q} \otimes \Delta \mathbf{q}-\mathbf{q}}{\theta} &=\lim _{\theta \rightarrow 0} \frac{\left[s \cos \frac{\theta}{2}-\mathbf{v}^{T} \boldsymbol{\omega} \sin \frac{\theta}{2}, s \omega \sin \frac{\theta}{2}+\cos \frac{\theta}{2} \mathbf{v}+\mathbf{v} \times \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}-\mathbf{q}}{\theta} \\ &=\lim _{\theta \rightarrow 0} \frac{\left[s\left(\cos \frac{\theta}{2}-1\right)-\mathbf{v}^{T} \boldsymbol{\omega} \sin \frac{\theta}{2}, s \omega \sin \frac{\theta}{2}+\left(\cos \frac{\theta}{2}-1\right) \mathbf{v}+\mathbf{v} \times \boldsymbol{\omega} \sin \frac{\theta}{2}\right]^{\mathrm{T}}}{\theta} \\ &=\left[-\frac{1}{2} \mathbf{v}^{T} \boldsymbol{\omega}, \frac{1}{2} s \boldsymbol{\omega}+\frac{1}{2} \mathbf{v} \times \boldsymbol{\omega}\right]^{\mathrm{T}} \\ &=\mathbf{q} \otimes\left[0, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}} \end{aligned} \end{equation}

则若角速度为 \omega 则四元数对时间的导数为:
\begin{equation} \dot{\mathbf{q}}=\mathbf{q} \otimes\left[0, \frac{1}{2} \boldsymbol{\omega}\right]^{\mathrm{T}} \end{equation}

参考文献

[1] 《视觉SLAM进阶:从零开始手写VIO
[2] https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B8
[3] https://www.cnblogs.com/MerakXuan/p/12148013.html
[4] https://fzheng.me/2018/05/22/quaternion-matrix-so3-jacobians/
[5] https://zh.wikipedia.org/wiki/%E5%9B%9B%E5%85%83%E6%95%B0%E4%B8%8E%E7%A9%BA%E9%97%B4%E6%97%8B%E8%BD%AC

Add a Comment

您的电子邮箱地址不会被公开。 必填项已用*标注

Table of Contents