四元数2

好吧，我必须承认到目前为止我还没有完全理解四元数，我一度把四元数理解为轴、角表示的4维向量，也就在下午我才从和同事的争辩中理解了四元数不完全是角、轴这么简单，为此写点心得给那些同我一样搞了2年3D游戏的还不清楚四元数的朋友。

为什么使用四元数
为了回答这个问题，先来看看一般关于旋转（面向）的描述方法－欧拉描述法。它使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系说的，这意味着第一次的旋转不会影响第二、三次的转轴，简单的说，三角度系统无法表现任意轴的旋转，只要一开始旋转，物体本身就失去了任意轴的自主性，这也就导致了万向轴锁（Gimbal Lock）的问题。
还有一种是轴角的描述方法（即我一直以为的四元数的表示法），这种方法比欧拉描述要好，它避免了Gimbal Lock，它使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度，即(x,y,z,angle)，一般表示为(x,y,z,w)或者(v,w)。但这种描述法却不适合插值。
那到底什么是GimbalLock呢？正如前面所说，因为欧拉描述中针对x,y,z的旋转描述是世界坐标系下的值，所以当任意一轴旋转90°的时候会导致该轴同其他轴重合，此时旋转被重合的轴可能没有任何效果，这就是Gimbal Lock，这里有个例子演示了Gimbal Lock，点击这里下载。运行这个例子，使用左右箭头改变yaw为90°，此时不管是使用上下箭头还是Insert、Page Up键都无法改变Pitch，而都是改变了模型的roll。
那么轴、角的描述方法又有什么问题呢？虽然轴、角的描述解决了Gimbal Lock，但这样的描述方法会导致差值不平滑，差值结果可能跳跃，欧拉描述同样有这样的问题。


什么是四元数
四元数一般定义如下：
q=w+xi+yj+zk
其中w是实数，x,y,z是虚数，其中:
i*i=-1
j*j=-1
k*k=-1
也可以表示为：
q=[w,v]
其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的矢量，也是非常不容易想像的。
四元数也是可以归一化的，并且只有单位化的四元数才用来描述旋转（面向），四元数的单位化与Vector类似，
首先||q|| = Norm(q)=sqrt(w2 + x2 + y2 + z2)
因为w2 + x2 + y2 + z2=1
所以Normlize(q)=q/Norm(q)=q / sqrt(w2 + x2 + y2 + z2)
说了这么多，那么四元数与旋转到底有什么关系？我以前一直认为轴、角的描述就是四元数，如果是那样其与旋转的关系也不言而喻，但并不是这么简单，轴、角描述到四元数的转化：
    w   =   cos(theta/2)

    x   =   ax * sin(theta/2)

    y   =   ay * sin(theta/2)

    z   =   az * sin(theta/2)
其中(ax,ay,az)表示轴的矢量，theta表示绕此轴的旋转角度，为什么是这样？和轴、角描述到底有什么不同？这是因为轴角描述的“四元组”并不是一个空间下的东西，首先（ax,ay,az）是一个3维坐标下的矢量，而theta则是级坐标下的角度，简单的将他们组合到一起并不能保证他们插值结果的稳定性，因为他们无法归一化，所以不能保证最终插值后得到的矢量长度（经过旋转变换后两点之间的距离）相等，而四元数在是在一个统一的4维空间中，方便归一化来插值，又能方便的得到轴、角这样用于3D图像的信息数据，所以用四元数再合适不过了。


关于四元数的运算法则和推导这里有篇详细的文章介绍，重要的是一点，类似与Matrix的四元数的乘法是不可交换的，四元数的乘法的意义也类似于Matrix的乘法－可以将两个旋转合并，例如：
Q=Q1*Q2
表示Q的是先做Q2的旋转，再做Q1的旋转的结果，而多个四元数的旋转也是可以合并的，根据四元数乘法的定义，可以算出两个四元数做一次乘法需要16次乘法和加法，而3x3的矩阵则需要27运算，所以当有多次旋转操作时，使用四元数可以获得更高的计算效率。

为什么四元数可以避免Gimbal Lock
在欧拉描述中，之所以会产生Gimbal Lock是因为使用的三角度系统是依次、顺序变换的，如果在OGL中，代码可能这样：
glRotatef( angleX, 1, 0, 0)

glRotatef( angleY, 0, 1, 0)

glRotatef( angleZ, 0, 0, 1)


注意：以上代码是顺序执行，而使用的又是统一的世界坐标，这样当首先旋转了Y轴后，Z轴将不再是原来的Z轴，而可能变成X轴，这样针对Z的变化可能失效。
而四元数描述的旋转代码可能是这样：
TempQ = From Eula(x,y,z)
FinalQ =CameraQ * NewQ
theta, ax, ay, az = From (FinalQ)
glRotatef(theta, ax, ay, az);
其中(ax,ay,az)描述一条任意轴，theta描述了绕此任意轴旋转的角度，而所有的参数都来自于所有描述旋转的四元数做乘法之后得到的值，可以看出这样一次性的旋转不会带来问题。这里有个例子演示了使用四元数不会产生Gimbal Lock的问题。

关于插值
使用四元数的原因就是在于它非常适合插值，这是因为他是一个可以规格化的4维向量，最简单的插值算法就是线性插值，公式如：
q(t)=(1-t)q1+t q2
但这个结果是需要规格化的，否则q(t)的单位长度会发生变化，所以
q(t)=(1-t)q1+t q2 / || (1-t)q1+t q2 ||
如图:

尽管线性插值很有效，但不能以恒定的速率描述q1到q2之间的曲线，这也是其弊端，我们需要找到一种插值方法使得q1->q(t)之间的夹角θ是线性的，即θ(t)=(1-t)θ1+t*θ2，这样我们得到了球形线性插值函数q(t)，如下：
q(t)=q1 * sinθ(1-t)/sinθ + q2 * sinθt/sineθ
如果使用D3D，可以直接使用D3DXQuaternionSlerp函数就可以完成这个插值过程。





硬件姿态解算

四轴的姿态解算无疑是最繁琐的步骤没有之一，但是自从MPU6050出现了硬件DMP的时候，大妈都能完成姿态解算了！

CrazePony使用了MPU6050自带的硬件四元数单元，可以通过IIC直接读取四元数，省却了软件解算繁琐的算法步骤，非常方便易用。

这里还是要首先介绍下四元数，四元数要说的实在太多，因为它的优点很多，利用起来很方便，但是理解起来就有点蹩脚了。我们百度四元数，一开始看到的就是四元数来历，还有就是四元数的基本计算。对于来历，还是想说一下，四元数（Quaternions）是由威廉·卢云·哈密尔顿(William RowanHamilton,1805-1865）在1843 年爱尔兰发现的数学概念。

将实数域扩充到复数域，并用复数来表示平面向量，用复数的加、乘运算表示平面向量的合成、伸缩和旋，这就是我们熟知的复数的二维空间含义，所以人们会继续猜想，利用三维复数不就可以表达三维空间的变换了吗，历史上有很多数学家试图寻找过三维的复数，但后来证明这样的三维复数是不存在的。有关这个结论的证明，我没有查到更明确的版本，据《古今数学思想》中的一个理由，三维空间中的伸缩旋转变换需要四个变量来决定：两个变量决定轴的方向，一个变量决定旋转角度，一个变量决定伸缩比例。这样，只有三个变量的三维复数无法满足这样的要求。但是历史上得到的应该是比这个更强的结论，即使不考虑空间旋转，只从代数角度来说，三维的复数域作为普通复数域的扩张域是不存在的。并且，据《古今数学思想》叙述，即使像哈密尔顿后来引入四元数那样，牺牲乘法交换律，这样的三维复数也得不到。经过一些年的努力之后， Hamilton 发现自己被迫应作两个让步，第一个是他的新数包含四个分量，而第二个是他必须牺牲乘法交换律。（ 《古今数学思想》第三册177 页）但是四元数用作旋转的作用明显，简化了运算，而且避免了Gimbal Lock，四元数是最简单的超复数，我们不能把四元数简单的理解为3D 空间的矢量，它是4 维空间中的的矢量，也是非常不容易想像的。

我们知道在平面(x,y)中的旋转可以用复数来表示，同样的三维中的旋转可以用单位四元数来描述。我们来定义一个四元数：

我们可以把它写成，其中，。那么V是矢量，表示三维空间中的旋转轴。w是标量，表示旋转角度。那么就是绕轴V旋转w度，所以一个四元数可以表示一个完整的旋转。只有单位四元数才可以表示旋转，至于为什么，因为这就是四元数表示旋转的约束条件。

所以大家可以理解为，单位四元数能够表示旋转。我们给出一组单位四元数和欧拉角的转换关系，通过这个关系来将采集到的四元数转化成欧拉角，原理将在软件解算中给出，这里直接使用就可以了：

所以在四轴飞行器中，传感器读取到四元数，首先应先将它归一化成单位四元数：

norm = dmpinvSqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] * norm;
q[1] = q[1] * norm;
q[2] = q[2] * norm;
q[3] = q[3] * norm;

归一化后根据四元数和欧拉角转换公式把四元数转化为欧拉角，OK,硬件姿态解算完成！

DMP_DATA.dmp_roll = (atan2(2.0*(q[0]*q[1] + q[2]*q[3]), 1 - 2.0*(q[1]*q[1] + q[2]*q[2])))* 180/M_PI;
// we let safe_asin() handle the singularities near 90/-90 in pitch
DMP_DATA.dmp_pitch = dmpsafe_asin(2.0*(q[0]*q[2] - q[3]*q[1]))* 180/M_PI;

DMP_DATA.dmp_yaw = -atan2(2.0*(q[0]*q[3] + q[1]*q[2]), 1 - 2.0*(q[2]*q[2] + q[3]*q[3]))* 180/M_PI;

 
 




posted @ 2018-08-21 17:17  Jerry_Jin  阅读(306)  评论(0)  编辑  收藏  举报