实用四元数

最新推荐文章于 2024-06-04 16:16:17 发布

止于至玄

最新推荐文章于 2024-06-04 16:16:17 发布

阅读量802

点赞数 2

分类专栏： Robotics 文章标签：机器人

本文链接：https://blog.csdn.net/philthinker/article/details/103358349

版权

Robotics 专栏收录该内容

10 篇文章 8 订阅

订阅专栏

四元数（Quaternion）相关的介绍材料非常多，这里针对其中几个实用问题进行阐述。

文章目录

四元数初步
单位四元数插值
- SLERP
- - 旋转距离
其它参考

四元数初步

相关介绍可参考此文或此文（英文）。

相对于复数的二维空间表示，四元数可理解为一种四维空间表示。四元数是由一个实数和三个元素 $i, j, k$ 组成： $x\mathbf{i} + y\mathbf{j} + z\mathbf{k}$ 三个元素之间有如下关系： $\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{i}\mathbf{j}\mathbf{k} = -1$

四元数还可以定义为： $\mathbf{u}$ 即标量部分+矢量部分。后续内容中将采用上述两种表示。

四元数运算

四元数虚部的乘法不具有交换律，例如： $-k;\quad jk = i, kj = -i;\quad ki = j, ik = -j$ 四元数的共轭定义为： $q^{*}=w-ai-bj-ck$ 四元数的模定义为： $\sqrt{q\cdot q^{*}} = \sqrt{w^2+a^2+b^2+c^2}$ 注意 $qp)^{*} = p^{*}q^{*}$ ，四元数的乘逆为： $q^{-1}=\frac{q^{*}}{|q|^{2}}$

与复数一样，四元数求和将不同元素加起来即可，满足交换律和结合律。

令： $w+\vec u,\quad p = t + \vec v$ 两个四元数之间的非可交换乘积通常被称为格拉斯曼积，其完整形式为： $wt-\vec v\cdot\vec u + w\vec v + t\vec u + \vec v \times \vec u$ 或 $\begin{aligned}pq = (wt-ax-by-cz) \\ +(at+wx+bz-cy)i \\ +(bt+wy+cx-az)j \\ +(ct+zw+ay-xb)k\end{aligned}$ 由于四元数乘法的不可交换性， $p q$ 并不等于 $q p$ ： $wt-\vec u\cdot \vec v+w\vec v + t\vec u-\vec v\times \vec u$

四元数点积： $p\cdot q$
点积也叫欧几里得内积，四元数点积等同于一个四维向量的点积。点积是可交换积，返回一个标量： $p\cdot q = wt + \vec u \cdot \vec v = wt+ax+by+cz$

四元数叉积： $p\times q$
四元数叉积和向量叉积等价，返回一个向量： $\begin{aligned}p\times q &= \frac{pq-qp}{2} \\ &= \vec u \times \vec v \\ &= (bz-cy)i+(cx-az)j+(ay-xb)k \end{aligned}$ 四元数叉积满足结合律、不满足交换律。叉积的模等于模的乘积，这样保证了单位四元数的叉积依然是单位四元数。

四元数逆： $p^{-1}$
四元数的转置通过 $p^{-1}p = 1$ 被定义。其构建方式与复倒数相同： $p^{-1} = \frac{p^{*}}{p\cdot p}$ 一个四元数与自身的点积是一个标量。单位四元数的转置于共轭相同。

四元数的符号： $\mathrm{sgn}(p)$
复数的符号定义为单位圆上一个方向与原复数相同的复数。四元数的符号产生于单位四元数： $\mathrm{sgn}(p) = \frac{p}{|p|}$

四元数的辐角： $\arg(p)$
辐角函数可找出一个四元数偏离单位标量的角度。此函数输出一个标量角度： $\arg(p) = \arccos\left( \frac{w}{|p|} \right)$

单位四元数的平均值：
此文

F.L. Markley, Y. Cheng, J.L. Crassidis, Y. Oshman, Averaging quaternions, J. Guid. Control Dyn. 30 (4) (2007) 1193–1197.

中介绍，单位四元数可以被看作是单位长度的方向向量，对 $4\times 4$ 矩阵：
$\sum_{n=1}^{N}q_{n}\cdot q_{n}^{T}$ 采用主成分分析，找到其最大特征值对应的正则化的特征向量作为其平均值。

四元数与刚体旋转

单位四元数，即绝对值为1的四元数（ $\|q\|^{2} = v^{2}+\mathbf{u}^{2} = 1$ ），若实部为 $\cos(\theta)$ ，它的共轭作用是一个角度为 $2\theta$ 的转动，旋转轴为虚部的方向。相比于欧拉角，四元数表达式无奇点；相比于SO3矩阵，四元数也更为简洁。两个单位四元数的乘积也是单位四元数,所有四元数的集合组成一个三维球 $\mathcal{S}^3$ 和在乘法下的一个群（李群）。注意四元数表示的旋转是一个2-1映射，因为 $q$ 与 $- q$ 表达的是同一个旋转。

四元数与轴角对

绕旋转轴 $n$ 旋转 $\theta$ 角（右手法则），表示为四元数：
$\cos\left(\frac{\theta}{2}\right) + \sin\left(\frac{\theta}{2}\right)\left( n_{x}i+n_{y}j+n_{z}k \right)$ 换一种说法：给定轴角对 $(\omega, \theta)$ 来表示一个绕 $\omega$ 转 $\theta$ 的旋转，那么对应的单位四元数被定义为：
$\left( \cos\frac{\theta}{2}, \sin\frac{\theta}{2}\omega \right)$
这个网站可以很直观的展示四元数轴角对。注意， $q$ 与 $- q$ 代表的是相同的旋转（Quaternions have a double cover of the rotation manifold）。

四元数旋转

扩展一个欧氏空间中的点到四元数空间，只需要将其定义为 $p = (0, x, y, z)$ 即可。执行下述乘法可以使点 $p$ 绕 $n$ 旋转： $p' = qpq^{-1}$ 这个运算被称为哈密尔顿积（Hamilton product）。注意这和使用复数进行2D旋转是不同的。
多次旋转的情况： $p' = q_{2}(q_{1}pq_{1}^{-1})q_{2}^{-1} = (q_{2}q_{1})p(q_{1}^{-1}q_{2}^{-1}) = (q_{2}q_{1})p(q_{2}q_{1})^{-1}$ 注意旋转是从右向左发生的。

四元数与角速度

定义了距离之后，下一步可以定义导数。采用轴角表示，定义角速度 $\omega(t)$ ，那么四元数的导数 $\dot{q}$ 表示为： $\dot{q} = \frac{1}{2}\omega q$ 此处角速度 $\omega$ 可以被看成是一个拥有0标量的四元数。上一节中指数映射可用于求解上述微分方程，或进行积分：
给定当前时刻方向 $q (t)$ 和当前时刻角速度 $\omega(t)$ ，那么，在时间段 $\Delta t$ 内（假设角速度不变）： $q(t+\Delta t) = \exp\left( \frac{\Delta t}{2}\omega \right)q(t)$

单位四元数插值

如前所述，单位四元数定义在一个单位球面 $\mathcal{S}^{3}$ 上，两个单位四元数 $q_{1}$ 和 $q_{2}$ 之间的插值是通过球面上的一个路径最短的圆弧运动实现的。两者这件的变动（displacement）为 $q_{1}q_{2}^{*}$ 。

SLERP

球面线性插值（Spherical Linear Interpolation）运算非常有用，因为它可以实现两个四元数间的平滑插值，避免欧拉角插值的角度限制问题、万向锁问题（导致抖动、路径错误，根本问题是插值过程中角速度不恒定）。

理论上，计算四元数插值的步骤如下：

计算两个四元数的差，利用逆矩阵推导。
计算差的一部分，即四元数求幂。
在起始的值上加上差的一部分，使用四元数乘法来组合角位移。

其主要思想是施密特正交。然而实践中，有一种更有效的方法。所有单位四元数都存在于一个4D球面上，结果如下：(假设旋转的角度是 $\phi$ )
$\mathrm{SLERP}(q_{0},q_{1},t) = \frac{\sin( (1-t)\frac{\phi}{2})}{\sin \frac{\phi}{2}}q_{0} + \frac{\sin( t\frac{\phi}{2})}{\sin \frac{\phi}{2}}q_{1}$
两个四元数之间的角度通过内积计算： $\phi = 2\arccos(n_{1}\cdot n_{2})$
SLERP的推导过程如下图所示：
slerp
有几点需要注意：

四元数 $q$ 和 $- q$ 代表的是相同的方向，但是插值时可能导致不同的结果。解决方法是选择两个数的符号使得它们内积结果非负。
如果两个四元数非常接近，那么正弦值非常小，这时除法计算可能会出现问题，解决方法是当正弦非常小时使用线性插值。

由于四元数旋转是乘法形式，上述公式亦可写成： $q(t) = q_{0}(q_{0}^{-1}q_{1})^{t}$
这个表达式可以看作是从起始点逐步”加“与终点的差值的 $t$ 倍， $0 < t < 1$ 。四元数的差与矩阵类似，可以理解为选旋转 $A^{-1}$ 再旋转 $B$ 便得到 $A$ 和 $B$ 的差值。四元数的差表示为 $q_{0}^{-1}q_{1}$ 。根据上一节的公式：
$\begin{aligned} q^{1} &= \exp(\ln q) = q \\ q^{0} &= \exp(0\ln q) = \exp(0\cdot \tilde{n}) = [\cos(0),\sin(0)\cdot\hat{n}] = [1,0] \end{aligned}$ 最后我们令： $q_{0} = [1,0,0,0]$ 以及 $q_{1} = [\cos\theta,\sin\theta\cdot n]$ ，重新代入SLERP插值公式：

可见两种表现形式可以相互转化。

旋转距离

通过距离度量： $d(q_{1},q_{2}) = |q_{1}-q_{2}|$ 四元数可同胚于 $\mathbb{R}^{4}$ 的度量空间。衡量两个单位四元数的距离有其它方式，如上一节在探讨SLERP时的视角。

此文中使用内积定义两个四元数的夹角作为距离： $\begin{aligned} \theta &= \arccos\left( 2(q_{1}\cdot q_{2})^{2}-1 \right) \\ d(q_{1},q_{2}) &= 1-(q_{1}\cdot q_{2})^{2} = \frac{1-\cos\theta}{2} \end{aligned}$ 该度量对于相同方向始终输出 $0$ ，对于相差 $\pi$ 输出 $1$ 。该定义也被称为测地线（geodesic）距离。注意，这个角度 $\theta$ 衡量的是这两个四元数 $q_{1}$ 、 $q_{2}$ 差别有多大，并非这两个旋转的角度差。

此文中也提到一种类似方法：令 $r=q_{1}^{-1}q_{2}$ ，此时 $q_{1}$ 与 $q_{2}$ 的距离等同于 $r$ 与 $1+[0,0,0]^{T}$ 的距离，如果 $r$ 实部 $r_{w}$ 小于零，那么取 $r = - r$ 。那么对于单位四元数，此时 $d(q_{1},q_{2})=\phi = 2\arccos(r_{w})$ 。与之类似的，在此书中：

Morais, João Pedro, Svetlin Georgiev, and Wolfgang Sprößig. Real quaternionic calculus handbook. Springer Basel, 2014.

作者提到一种利用四元数的对数表示两个四元数之间差异的方法。如前所示，四元数的对数可以写成： $\log(q) = \log(v+u)=\left\{ \begin{aligned} &\arccos(v)\frac{u}{\|u\|}, u \neq 0 \\ &[0,0,0]^{T}, \text{ otherwise} \end{aligned} \right.$ 注意四元数的对数定义在3维欧式空间内（旋转的轴角表示）。那么， $d(q_{1},q_{2}) = \left\{ \begin{aligned} &2\pi,\quad q_{2}q_{1}^{*} = -1+[0,0,0]^{T} \\ &2\| \log(q_{2}q_{1}^{*}) \|,\quad \text{ otherwise} \end{aligned} \right.$ 注意上述所有积运算均为乘积运算。如果我们把定义域限制到 $S^{3}/\{-1+[0,0,0]^{T}\}$ 内，那么对数运算是内射（injective）映射。其逆映射，即指数（幂）映射（ $\mathbb{R}^{3}\to S^{3}$ ）如前所述可定义为： $\exp(r) = \left\{ \begin{aligned} &\cos(\|r\|)+\sin(\|r\|)\frac{r}{\|r\|},\quad r \neq 0 \\ &0,\quad \text{ otherwise} \end{aligned} \right.$
同样的，如果我们限制两个域为 $\|r\| < \pi$ 且 $S^{3}/\{-1+[0,0,0]^{T}\}$ ，那么指数与对数映射变成 one-to-one，连续可微映射，且互为逆映射。