四元数指数映射旋转参数化的实际应用

四元数指数映射旋转参数化的实际应用

哪吒三太子 2016/3/26 于上海卢湾

下面为本文使用术语表,表中所有词条大多直接采用英文术语,请各位读者自行伸缩去取,笔者在此不做所谓"直译".

• DOF(degree-of-freedom) 旋转自由度
• ODE(ordinary differential equation) 常微分方程
• transformation matrix 变换矩阵(变换一词包含:平移,旋转,放缩,甚至仿射)
• S3 单位四元数组成的球形表面空间
• SO3 四维空间中的三维子空间集合
• normalize 标准化,通常变模为单位长度1
• end-effector 机械臂末端-与环境交互部分
• singularity 奇点
• hierarchies 层级结构

摘要:

三维旋转自由的参数化一直不竟如人意,虽如此现有的图形应用方法除了要求我们拥有计算连续位移和物体链接旋转的能力之外,大多数还需要我们能够自行处理微积分方程,DOF的优化,和旋转插值.
(注意:在这里的旋转为 orientation 统称,但不具体指定参数化方法,e.g.欧拉角表示法,四元数表示法,亦或者本文介绍的指数映射法)但广泛使用的欧拉角或四元数参数法因为其自身数学限制,都不能很好的适应上述这些需求.
  指数映射法使用旋转轴和旋转量来描述R³中的向量. 一些图形研究员已经把这种方法应用在了旋转插值问题中,但其他却未涉及. 本文就是为了补缺这一片应用方法的空白,详细展现了DOF旋转问题中的计算,微分,积分方法供读者参考使用. 并且这种方法在计算机精度方面呈现出数值稳定,在映射中存在的奇点(singularity)也可以简单的动态重复参数化来解决. 本文将会演示如何使用指数映射法来解决类似万向自由体和DOF空间铰链问题. e.g. 精确地数学建模人体的肩膀,胯等关节的连接运动.
  在实验结果中指数映射法相比较于欧拉角和四元数法,有以下优势:更强的鲁棒性,小向量可以被更好的表达,无需明确过多的限制条件,更好的建模能力,ODE简化和更好的插值性能.

1 介绍

为什么我们要使用四元数指数映射？

  在现有的旋转参数化方法中主要有欧拉角表示法和四元数表示法两种. 在多刚体连接系统中动力学模拟必须使用 hierarchies (层次结构)来描述刚体链接. 而欧拉角(欧几里德空间参数)会遇到万向节死锁问题(由于遇到数学上的奇点子空间,导致降维), 四元数方法则必须限制其范数(通常称为"模")为单位长度1.
  在R³中每个非零向量都由方向和长度组成. 而旋转是由物体沿着一个旋转轴的旋转量组成的. 如果我们扩展零向量为0模长,单位旋转的话,整个指数映射就成为了连续空间的映射. 与四元数参数化不同的是,由于指数映射参照欧几里德空间,所以也存在万向节死锁的奇点空间,但这个奇点的坐标区间非常远离我们工作区间,并且参数化结果包含了大多数四元数参数结果的特性,并且无需担心会掉进非欧几里德流形之中. 最后本文也会毫不避讳的讨论指数映射方法的缺点,以供读者在解决特定问题中参考选择. 在第二节中我们讨论现有参数法的优缺利弊.第三节讨论指数映射为什么把旋转映射到四元数而非欧拉角组成的旋转矩阵,然后介绍了如何在这基础上进行微分. 第四节中展示了指数映射法非常适用于微分控制和前向动力学模拟,并且也讨论在插值和时空优化中的限制. 第五节中我们进一步拓展该方法使其适用于带限制条件(铰链)的DOF旋转问题.  最后在第六节中总结方法的健壮度并给出C实现的伪代码供读者参考.

2 常用参数法的评估

在图形应用中主要有5种基本方法来描述并控制物体的旋转:
   1. forward kinematics
   2. inverse kinematics
   3. forward dynamics
   4. inverse dynamics
   5. spacetime optimization
   其他方法则基于这五种基本方法之上.
  基于这些方法的应用除了在软件大小和复杂度上有差异之外,都必须具有以下能力:
  1. 能计算物体(物体的组合)上每个点的旋转和坐标, 这是forward kinematics方法所需要的基础能力.
  2. 能计算每个点旋转和坐标的导数,这是inverse kinematics, dynamics,spacetime所需要的基础能力.
  3. 能对ODE进行积分运算,这是inverse kinematics, dynamics,spacetime所需能力.
  4. 能对两个关键帧进行平滑插值.
  5. 能组合多个旋转成为一个操作.
  6. 能计算对应旋转的参数逆,找出对应参数. i.e. 上述能力都是以参数计算旋转,而此能力是以旋转计算参数. 故称为逆操作.

上述中3.4.5是参数空间与生俱来的特性. 1,2则需要把旋转表示为4x4的 transformation 矩阵. 并且因为 transformation 具有诸多线性空间变换能力,所以我们可以在方法重穿插使用多种参数方法 e.g.使用旋转矩阵计算偏微分得到点旋转和坐标导数.

\[R(v) 和 \frac{\partial R}{\partial v} \]

其中R为4x4矩阵, \(\partial R / \partial v\) 为4x4xn张量, n维向量中每个元素为4x4矩阵. 至此我们就得到了点和旋转的 Jacobi矩阵.

2.1 3x3旋转矩阵

既然旋转矩阵能很好的表示一个旋转,并且它也有乘法进行旋转组合的功能,更进一步可以使用优化代码来对矩阵进行快速运算,那我们为什么不用它来对旋转进行参数化表达?
原因是我们必须强加6个非线性的限制条件在旋转矩阵的9个参数上,3个条件限制每个矩阵列向量的模为1,3个条件使他们互相保持正交).所以每一步的运算之前都首先保证矩阵为正交矩阵,这个条件很苛刻.

2.2 欧拉角

欧拉角由一个指定坐标系的 x,y,z 轴,决定一个向量在对应坐标轴上的对应旋转. 通常我们可以看到它被表示成矩阵的形式并用 sin cos 正余弦函数来表征对应坐标旋转,虽然对旋转角的参数化是非线性的(sin,cos),但他们的导数非常容易被计算出来.
欧拉角的坏处和好处同样明显,它会遇到一个致命的问题 --- 万向节死锁. 讨论死锁问题超出了本文范畴,有兴趣的读者可参考https://en.wikipedia.org/wiki/Gimbal_lock.
虽然欧拉角有致命缺陷,但我们不能忽视它在 2DOFs 旋转问题中表现良好,并在不遇到死锁问题下非常容易使用.

2.3 四元数

四元数的历史非常离奇,它是在爱尔兰数学家 William Rowan Hamilton和妻子散步时偶然发现的. 它用一个四维向量表示三维旋转,对四元数的详细讨论同样也超出了本文的范畴,请移步参考http://baike.baidu.com/view/319754.htm.
在这里我们只说以下四元数的优缺点. 四元数由于定义及其精妙(其实四元数在群论中表达为一个S3球表面的数域,对乘法封闭(笔者其实是群论的疯狂脑残粉 😛)),用它来表示旋转问题比矩阵运算更快速,比欧拉角而言没有死锁问题,而且非常易于插值计算 e.g.动画的计算.
它的缺点是由于四元数表示四维空间中的向量,而现实世界只有三个维的自由度,所以在进行积分运算的时候,每一步运算都需要把它进行 normalize 以压缩掉一个维度,对积分运算造成了很大的麻烦. 在实际应用中积分步长很难控制,太短则会导致性能下降问题,太长则离开S3球表面,误差就会很大. 还有一种方法由Michael Gleicher 提出,它解决了 normalize 问题,但会导致 Jacobi 矩阵秩缺失,这意味着在四维空间中有一个"子空间"中所有四元数都对应同一个三维旋转 i.e.四元数在这个"奇特的"空间中任意改变并不会导致三维空间中旋转出现变化.(通常来说就是掉进了一个高维流形陷阱之中了). 可能遇到的一个实际问题就是:我们再也无法求出物体的转动惯量了.
总结来说使用四元数确实可以解决很多问题,但当软件需要计算导数来控制和优化系统的时候,事情就会变得很麻烦. 最后毋庸置疑的它是计算3DOF旋转插值的最佳方法.

3 四元数指数映射

引入指数映射的原因是四元数只用到了所有四元数集中一个一个子空间,也就是S³球表面. 所以我们希望有一种方法能让R³空间中的点一一对应S³球表面,而不会超出这个球表面空间,通常我们用强加给四元数单位长度的限制来解决这个问题(上面也说明了每步需要 normalize 的开销和积分时候的麻烦).所以我们的需求总结如下:

1. 方法必须没有万向节死锁
2. 旋转插值要很容易实现
3. 组合旋转的能力
4. 容易进行导数与积分运算

Anyway,满足上述所有条件这是不可能的:( 已经有数学家进行过严密的证明R³至SO³(四元数的所有三维子空间的统称,当然也包含球空间)是不可能消除奇点的,也就是说一定会遇到死锁问题,但是我们可以轻而易举地躲开这个不可避免的问题.

下面我们就用数学语言来描述它:
四元数: $$q = [{q}{w},{q},{q}{v2},{q}] = [{q}_{w},v] \equiv w+ai+bj+ck$$
我们定义当v=0时 $${e}^{{[0,0,0]}{T}} = {[0,0,0,1]}^{T}$$
当\(v \neq 0\) $$ {e}^{v} = {[sin(\frac{1}{2}\theta) \hat{v}, cos(\frac{1}{2}\theta)]}^{T}$$
四元数的指数形式方程推导参见https://en.wikipedia.org/wiki/Quaternion#Exponential.2C_logarithm.2C_and_power

其中\(\hat{v}=v/|v|\) \(\theta = |v|\),从方程上看出我们把四维向量包装成三维的了,其中 v = ai+bj+ck 而常量 w 则用 v 的模来表示. 上述方程式在数学上需要推导在零处函数的极限,但是在计算机中当|v|接近于 0 时计算结果溢出(因为计算机 float 最多表达1e-38 小数,小于这个数则会栈溢出),会导致计算溢出,那么我们稍许改变一下方程式\(\theta = |v|\). 这样式子就变成 \(sin(1/2\theta)\frac{v}{\theta}\) 而 \(\frac{sin(1/2\theta)}{1/2\theta} = sinc(1/2\theta)\) 为辛格函数 在零处有定义,所以方程式在计算机计算中变得稳定. 而令人沮丧的是标准C库没有辛格函数的定义,所以我们用泰勒展开表达(题外话:其实计算机计算三角函数都是用泰勒展开计算的,因为泰勒展开在三角函数的\([-\pi ,\pi]\)收敛且拉格朗日余项误差可以任意小)

\[\frac{sin(\frac{1}{2}\theta))}{\theta} = \frac{1}{\theta}TaylerExpansion(sin(\frac{1}{2}\theta)) \]

\[= \frac{1}{\theta}(\frac{\theta}{2}-\frac{{(\theta/2)}^{3}}{3!}+\frac{{(\theta/2)}^{5}}{5!}+...) \]

\[= \frac{1}{2}-\frac{{\theta}^{2}}{48}+\frac{{\theta}^{4}}{{2}^{5}\cdot 5!} \]

学过《高等数学》的读者一定知道一旦函数的泰勒展开收敛,则当估计项为 n 时,余项误差不会超过第 n+1 项的值,我们只要让 n+1 项为计算机最小精度即可,这样我们就消除了计算机计算误差了. 事实上我们只需前两项即可消误差:

\[\frac{sin(\frac{1}{2}\theta)}{\theta} = \frac{1}{2} - \frac{{\theta}^{2}}{48} \]

至此我们有了四元数的指数映射来完整表达一个旋转,且消除了四元数的四维特征使其完全落在S3球上而不用强加 normalize 的限制. Great,下面我们就来讨论它的实际用途:导数微分,积分,插值 etc.

3.1 导数

因为四元数的指数是连续函数,所以可以使用链式法则进行求导:

\[\frac{\partial R}{\partial v} = \frac{\partial R}{\partial q}\frac{\partial q}{\partial v} \]

计算的具体过程推导过程详见http://www.euclideanspace.com/physics/kinematics/angularvelocity/QuaternionDifferentiation2.pdf

3.2 缺陷

3.2.1 奇点问题

之前我们一直说该方法是不能避免遇到奇点问题的,而这个问题是可以被躲避的,原因在于该函数的奇点在 \(2n\pi\) 上. 众所周知当取到 \(2\pi\) 时,旋转其实是绕到了原处,我们只要把取值范围变成 \([0,2\pi)\)即可,当 \(\theta\) 接近\(2\pi\)时我们用 \(2\pi - \theta\) 也就是-v来代替即可.
因为模拟时间步长内旋转不可能超过\(\pi\),所以每一步模拟我们都检测一下 |v| 如果它接近\(\pi\),那么我们就用 \((1-2\pi/|v|)v\)来代替 v,在试验中上面等价的方程更利于倒数的计算.这个动态的重复计算参数理论上来说能完全避免函数掉入奇点之中.当然这也使我们不能在超出规定范围内进行动画插值(e.g. 有两个rotation状态假设沿着x轴正方向旋转,r1=30度,r2=430度,我们不能简单地在这两个状态中插值,因为后者超过了我们规定的范围,所以必须被分割成30-180, 180-360, 360-430 三个部分进行插值计算,这无疑增加了系统的复杂度)

3.2.2 旋转合并

因为四元数的乘法 \(\equiv\) 旋转矩阵的乘法.但是我们把四元数常量 w 整合进了 v 之中导致这个性质被破坏,所以在合并旋转的时候我们必须先把四元数的指数还原成标准四元数(保证模为单位长度1),然后进行乘法合并,最后再映射到四元数的指数之中.
幸运的是我们通常不需要合并指数形式的旋转,因为物体的旋转通常由它的导数来驱动,或者直接修改四元数部分的参数. 当他们转换成 transformation matrix(变换矩阵)并施加在物体上时，它们就以矩阵的形式被合并了. 故除非特殊需要,否则没有额外的运算开销.

4 应用

磨刀霍霍,我们讨论了那么久就是为了在这里说明它是如何被用来简化实际应用的.请你相信,指数映射在控制,模拟方面都具有非常好的性能.但在插值和优化方面比其他方法复杂.

微分控制和动力学模拟

研究这项工作的主要动机是因为我们想要一个较好的微分控制器,它可以直接控制物体的旋转,进行 inverse kinematics研究和实时的机械臂控制. 为了控制物体或者机械臂的末端(end-effector)(这里使用机械臂泛指用关节链接的物体运动-其实也就是多刚体系统运动),微分控制器只要求\(\partial R/\partial v\)是连续的,并且不存在万向节死锁这两个条件即可. 因为计算机控制发生在离散时间内,而我们之前介绍的动态重复参数化方法可以有效躲过万向节死锁,所以这两项条件总是可以被满足的.
在动力学模拟中,我们不仅要追踪物体某一时刻的位置和姿态,同样需要追踪它的线速度和角速度.
不要忘了电脑模拟是离散时间的,为了正确的计算物体的位移和旋转,我们需要计算导数来进行函数的数值逼近,对一个四元数表示的旋转而言,它对时间的导数为如下公式 (数学推导在这里）:

\[\dot{q} = \frac{1}{2}\tilde{\omega}\circ q \]

其中\(\tilde{\omega}\)是末尾加一个 0 的四元数对应的角速度. 那么对四元数指数映射的导数同样可以求出：

\[v=log(q)=\frac{2cos^{-1}(q_w)}{|q_v|}q_v$$ 因此 $$\dot{v} = \frac{\partial log(q)}{\partial q}\dot{q} = \frac{\partial log(q)}{\partial q}\frac{1}{2}\tilde{\omega}\circ q\]

其中 \(q_v\)是四元数的虚部. 在上述方程中如果我们用 v 完全替代 q 的话那将使公式更简洁,并且无需四元数的参与.我们来看:
当\(\theta >> 0\)时:
我们定义: $$p=\omega\times v \gamma=\theta cos(\frac{1}{2}\theta) \eta=\frac{\omega\cdot v}{\theta}(cot(\frac{1}{2}\theta)-\frac{2}{\theta})$$ 那么将有

\[\dot{v}=\frac{1}{2}(\gamma\omega+p-\eta v) \]

当\(\theta -> 0\)时,注意 \(cot(\frac{1}{2}\theta)=cos(\frac{1}{2}\theta)/sin(\frac{1}{2}\theta)\)趋向无穷,需要计算在零点处函数的极限,同样的我们用其泰勒展开代替 cot 三角函数便得到最终公式如下:

\[p=\omega\times v \gamma_0=\frac{12-\theta^2}{6} \eta_0=\omega\cdot v\frac{60+\theta^2}{360}$$然后公式便成了$$\dot{v}=\frac{1}{2}(\gamma_0\omega+p-\eta_0 v) \]

总结下来指数映射非常适用于微分控制和模拟,除了我们要采取一些小手段避免其陷入奇点之外,它比四元数的优势有以下三点

1. 没有 2.3节所说的 Jacobi 秩缺失
2. 不用在每步积分前先 normalize ,使其不脱离 S3球面
3. 系统使用三维向量而非四维,简化了计算的复杂度

5 铰链和关节

上面我们已经介绍了全自由度下四元数指数映射的使用,现在我们用一个例子来演示带限制条件的铰链关节.
假设现在我们要对手腕进行数学建模,手腕在两个自由度上有角度限制并在"直线"方向上不能扭曲. 我们设直线方向上为 z坐标轴,任意两个互相垂直且垂直于 z轴的坐标轴为 x,y 那么我们就有以下公式: $$v = \alpha\hat{x}+\beta\hat{y}$$
其中\(\hat{x}\) 表明 x 轴向量长度为单位长度. \(\alpha \beta\) 各自为 x,y 轴上的旋转角度并且有最大最小值.
更进一步我们可以用一个不等式方程表示：$$(\frac{\alpha}{a})^{2+\frac{\beta}{b})}2 \leq 1$$
上述方程在平面几何中表示一个轴长为 a , b 的实心椭圆. 可以看出使用指数映射大大方便了各种带限制空间铰链的数学表达,读者可以尝试一下使用欧拉角来建模手腕关节,你将发现欧拉角在三维上建模不但比四元数指数映射更复杂,且在超过 90度时必将发生万向节死锁问题. 需要指出的是欧拉角在一维,二维自由度情况下表现还是很良好的,如果你的需求只是在一维自由度 e.g. 膝关节 ,那么你可以考虑使用欧拉角来简化问题.
最后这个手腕关节的求导数就作为读者的课后作业,提示同样使用链式法则. 读者甚至可以更进一步自己组合一个机械臂来计算 end-effector 的位置和姿态,我就不再这里过多累述了. 本文在这里收官, 愿读者在读完本文后有所收获.