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

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

哪吒三太子 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球表面的数域,对乘法封闭(笔者实际上是群论的疯狂脑残粉 :P)),用它来表示旋转问题比矩阵运算更快速,比欧拉角而言没有死锁问题,并且很是易于插值计算 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}_{v1},{q}_{v2},{q}_{v3}] = [{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 的位置和姿态,我就再也不这里过多累述了. 本文在这里收官, 愿读者在读完本文后有所收获.