第四章四元数在3D物体姿态变化中的应用_2
第四章 四元数在3D 物体姿态变化中的应用
4.1引言
对于三维仿真系统中鱼雷运动的可视化研究，一般的仿真系统采用了按照鱼雷的六自由度参数来直接驱动模型的视景仿真。为了保证计算速度，往往需要在计算运动方程时选取较大的计算步长，这样有时会无法实现仿真运动的光滑和连续，甚至出现鱼雷运动跳跃的现象，如果选取较小的计算步长，那么计算速度的降低同样会影响到鱼雷的可视化效果。
目前，国内的仿真系统里用来解决此问题的专用软件模块十分少见。本课题着重研究了四元数在三维仿真系统中的应用，包括利用四元数对物体进行自由旋转和利用四元数来解决鱼雷运动可视化中的问题，本文利用四元数对模型姿态角变化进行插值，很好地解决了上述的鱼雷运动可视化仿真中存在的矛盾。
4.2四元数简介
四元数是1843年由英国数学家William Rowan Hamilton 发明的数学概念。它的出现很好地把二维空间中的复数扩展到了三维空间。由于四元数的乘法不满足交换律，对它的研究较实数和复数都要困难，以至在它出现后一个多世纪都得不到很好的应用。直到1985年Ken Shoemake [20]将它引入计算机图形学之后，四元数在计算机动画和三维图形绘制方面才得到实际的应用。随着计算机技术和图形学的发展，四元数所表现出来的优势也日渐得到人们的重视。
4.2.1四元数记法
一个四元数包含了四个分量，一个标量分量和一个3D 向量分量。也可以说是一个实部，三个虚部，表示如式4.1所示：
i = + +j +k Q w b c d (4.1) 式4.1中w , b , c , d 为实数，i , j , k 为虚数，满足i 2= j 2 = k 2 =－1。四元数也可以表示为：[w ,v ] 或 [w , (x ,y ,z )]，其中w 为标量，v ＝(x ,y ,z )为矢量。
4.2.2四元数的运算
四元数的加减法和一般复数的加减法相同，也满足交换律和结合律。但是四元数的乘法满足结合律但不满足交换律，这是与实数和复数最显著的不同[5]。
四元数加法：
121212[,]Q Q w w v v +=++ (4.2) 四元数点乘：1212121212
Q Q w w x x y y z z ?=+++ (4.3) 四元数的共轭：*[,(,,)]Q w x y z =??? (4.4)
四元数的模：Q (4.5)
四元数的逆：1*Q Q Q ?= (4.6)
4.2.3四元数与轴—角对
欧拉证明了一个旋转序列等价于单个旋转。因此，3D 中的任意角位移都能表示为绕单一轴的单一旋转。当然一个方位用这种形式来描述时称作为轴角对描述法。设n 为旋转轴，θ为绕轴旋转的量。因此，轴角对(n , θ)定义了一个角位移：绕n 指定了轴旋转θ角。
四元数能被解释为角位移的轴角对方式。然而，n 和θ并不是直接存储在四元数的四个数中的。下面的式4.7列出了四元数的数和n , θ的关系。
Q = [cos(θ/2) sin(θ/2) n ]
= [cos(θ/2) (sin(θ/2)n x sin(θ/2)n y sin(θ/2)n z )] (4.7)
可见，四元数只用四个数表示方位，比矩阵用9个数的效率要高。
4.2.4欧拉角转换为四元数
为了将角位移从欧拉角的形式转换为四元数的形式，可以将欧拉角的三个角度分别转换成四元数的形式，然后根据四元数乘法的几何意义将这三个四元数连接成一个四元数。设欧拉角的三个变量为p 、q 、r , 设p 1、q 1、r 1分别表示绕Y 、X 、Z 轴旋转的四元数，Q 为最终需要的旋转四元数[5]，如式4.8所示。
1cos(/2)0sin(/2)0p p p ?????????=?????????????????
1cos(/2)sin(/2)00q q q ??????????=???????????????? 1cos(/2)00sin(/2)r r r ?????????=????????????????? 111
cos(/2)cos(/2)cos(/2)0sin(/2)0sin(/2)0000sin(/2)cos(/2)cos(/2)cos(/2)cos(/2)sin(/2)cos(/2)cos(Q p q r p q r q p r p q r p q r p =????????????????????????????=????????????????????????????????????????????????????
+??=sin(/2)sin(/2)sin(/2)sin(/2)cos(/2)sin(/2)/2)sin(/2)sin(/2)sin(/2)cos(/2)cos(/2)sin(/2)sin(/2)cos(/2)cos(/2)cos(/2)sin(/2)p q r p q r q r p q r p q r p q r ??????????????????????????
?? (4.8) 4.3四元数法实现鼠标任意旋转3D 图形
在开发三维图形应用程序时经常会需要从各个角度来观察物体模型，交互式地旋转模型的朝向。关于用鼠标对物体进行三维旋转，Michael Chen 在文献[21]中提出通过构造虚拟球(Virtual Sphere)将鼠标在视口中的二维位置投影为虚拟球表面的三维坐标的方法。鉴于鼠标是一种2D 输入设备，文献[21]中还提出一种附加Z 轴的方法，用鼠标选中物体沿屏幕坐标系的X 、Y 轴的移动分别实现物体绕世界坐标系的Y 、X 轴旋转，在物体之外移动鼠标使物体绕Z 轴旋转，这样虽然能得到最终需要的方位，但由于是基于单个轴的旋转，不能使鼠标的任意移动方向与物体的旋转准确地关联起来，并且该方法
会导致万向节死锁问题(Gimbal Lock)[5]。文献[20]中通过改进计算虚拟球中旋转轴和旋转角度的方法使得旋转可以连续。本文依据欧拉角的基本原理利用四元数的方法表示三维旋转，在OpenGL中实现利用鼠标拖拽来有效地对三维模型进行任意旋转。
4.3.1与三维旋转相关的概念说明
1. 相关的坐标系
在计算机三维应用程序中，为了便于描述物体的方位，不同的情况下需要使用不同的笛卡尔坐标系。在三维图形旋转中所涉及到的坐标系包括：
(1)世界坐标系
世界坐标系又称为全局坐标系，它是一个特殊的坐标系，建立了描述其它坐标系所需要的参考框架。从另一面说，可以用世界坐标系来描述其它坐标系的位置，而不能用更大的、外部的坐标系来描述世界坐标系。它用来描述三维场景中物体的位置，设定摄像机的位置和方向等。世界坐标系的坐标原点和坐标轴的方向始终是不变的。
(2)物体坐标系
物体坐标系又称为局部坐标系，它是和特定的物体相关联的，每个物体都有自己独立的物体坐标系，该坐标系是随物体的位置和方向的变化而变化的。
在图4-1中，OXYZ表示世界坐标系，oxyz表示物体坐标系，为了符合OpenGL的规定，它们都采用右手坐标系。
图4-1 两种坐标系的关系示意图
Fig.4-1 The relation of the two reference frames
(3)惯性坐标系
在某些时候，一个好的术语能够引领人们更好的理解主题。为了简化世界坐标系到物体坐标系的转换，人们引入了一种新的坐标系，叫做惯性坐标系，意思是在世界坐标系到物体坐标系的“半途”。惯性坐标系的原点和物体坐标系的原点重合，但是惯性坐标系的轴平行于世界坐标系的轴。物体从物体坐标系转换到世界坐标系只需要旋转，从惯性坐标系转换到世界坐标系只需要平移。
(4)设备坐标系
所谓设备坐标系是指与图形设备相关的坐标系。比如显示器的屏幕就是一个以分辨率为坐标单位的设备坐标系，一般将坐标原点定义在左下角。
2. 三维空间中的方位与角位移
物体的方位主要描述物体的朝向，但是“方向”与“方位”并不是完全一样的。向量有“方向”但是没有“方位”，区别就在于当一个向量指向特定方向时，可以让它自传，此时向量是不会有任何变化的，因为向量的属性只有“大小”，没有厚度和宽度。但是，当一个物体朝向特定的方向时，让它和向量一样自传的话，它的方位就发生改变了。所以从技术的角度来讲，这就说明了在3D中，只要用两个数字(例如，极坐标)，就能用参数表示一个方向(direction)。但是，要确定一个方位(orientation)，至少需要三个数字。
因为我们知道不能用绝对坐标来描述物体的位置，要描述物体的位置，必须把物体放置于特定的参考系中。描述位置实际上就是描述相对于给定参考点的位移。同样，在描述物体方位时也不能使用绝对量。本文讨论的旋转即物体方位的变化，类似于我们在平面上用平移来改变物体的位置，用位移来表示物体位置的变化量，旋转的量称为角位移。
4.3.2三维空间中描述方位的方法
本文涉及到的描述3D空间中的方位的方法包括：
(1)矩阵形式
在三维图形中，描述坐标系中方位的一种方法就是列出这个坐标系的基向量，这里的基向量是用其它的坐标系来描述的。这些基向量构成了一个3×3的矩阵，我们就可以用这个矩阵来描述方位。换句话说，可以用一个旋转矩阵来描述这两个坐标系之间的相对方位，这个旋转矩阵用于把一个坐标系中的向量转换到另一个坐标系中，如图4-2所示。其中坐标系OXYZ表示世界坐标系，Oxyz表示物体坐标系。
图4-2用矩阵描述方位示意图
Fig.4-2 The sketch map of the orientation described by matrix
矩阵形式是一种最底层也最直接的描述方位的形式，它最重要的性质就是利用矩阵可以在物体坐标系和惯性坐标系之间旋转向量，这点是其它描述方法所做不到的。要旋转向量，必须将方位转换为矩阵形式。而且，计算机的图形API是使用矩阵来描述方位的，最终必须将用矩阵来描述所需的转换。程序中怎么样保存、变换方位都可以，但是如果选择了其它的形式，则在渲染管道的某处将其转换为矩阵形式。用矩阵来保存方位的缺点在于矩阵占用了大量的内存，如某一动画序列中的关键帧，9个数字能导致相当数目的额外空间消耗。并且矩阵形式对人来说不太直观，人们考虑方位的直观想象是通过角度，而矩阵是向量。
(2)欧拉角方法
欧拉角的基本原理是将一次旋转分解为绕三个互相垂直轴的三次旋转组成的序列。欧拉角将方位分解为绕三个互相垂直的轴的旋转，并且是任意的三个轴，任意顺序都是可以的，但是最有意义的还是使用笛卡尔坐标系并按照一定的顺序来进行旋转。最常用的约定就是所谓的“heading-pitch-roll”约定，如图4-3所示。使用欧拉角来描述方位很直观，而且节约内存。但是，用欧拉角来表示方位的缺点也很多，如在将一个角度加上360°的倍数时并不改变方位，这样，表示同一方位的时候就会出现别名问题。使用欧拉角最著名的问题是：先heading50°再pitch90°，应该与先pitch90°再heading50°是等价的。但事实上，一旦选择了pitch90°，物体的选择就被限制在这能绕竖直轴旋转。这种现象，角度为90°的第二次旋转使得第一次和第三次旋转的旋转轴相同，也即当物体绕某个轴旋转90°时，会导致物体旋转自由度的减少，这就是万向节死锁问题[3]，显然，当在两个方位欧拉角间插值遇到90°时，同样道理，会导致旋转路径的错误。使用欧拉角表达方位遇到的第二个问题就是两个角度间求插值非常困难。比如如果没有使用限制欧拉角，将会得到很大的角度差，如方位1的heading为360°，方位2的heading 为30°，其实两个方位的heading只相差30°，但是简单的插值则会在错误的方向上绕近一周。这个问题虽然可以通过限制欧拉角和将角度限制在一定范围内来解决，但是对于万向节死锁的问题，是一个底层的问题，是一个用三个数表达3D方位的方法与生俱来的问题。并且这个问题在本章的两个实验中都必须避免。
Roll
图4-3欧拉角表示方位的示意图
Fig.4-3 The sketch map of the orientation described by Euler angles
(3)四元数方法
前面已经介绍了四元数的概念，它是用四个元素来表示方位，从而可以解决用欧拉角方法描述方位所带来的万向节死锁问题。使用四元数来代替欧拉角表示方位的优点还包括它可以产生平滑的方位间插值，四元数叉乘可以将角位移转换为某个单位角位移，虽然用矩阵作同样的操作明显会慢一些，另外四元数的共轭运算有效计算反角位移的方法，通过转置旋转矩阵同样也能达到这样的目的，但是没有四元数方法简便。四元数还有个优点就是它只用4个数来表示方位，比矩阵用9个数节约资源。
4.3.3四元数法实现鼠标任意旋转3D图形
在可视化工具中经常需要用鼠标拖拽来实现对三维模型的交互式旋转操作，由于鼠标只能执行二维的操作，即它只能提供两个方向的自由度，而三维空间的旋转需要三个自由度。因此，改进一下文献[18]中的方法，当用沿屏幕坐标系X轴的移动来实现模型绕世界坐标系Y、Z轴的平分轴来旋转时，沿屏幕坐标系Y轴的移动来实现模型绕X轴的旋转时，旋转效果变得好起来。但是，这样做产生的万向节死锁问题又使得旋转变得不能随心所欲。因此，本实验将欧拉角转换为四元数来表示物体的方位，并用四元数乘法连接多次旋转，这样可以把用欧拉角表示的三次旋转合并为用四元数表示的一次旋转，从而解决了上述问题，最终实现用鼠标交互式地任意旋转三维物体。
本实验采用面向对象的方法结合OpenGL图形库来实现，为了便于说明主要的问题，实验中只考虑物体的旋转变换，而不考虑物体的平移，即该场景中世界坐标系与物体坐标系的原点始终是重合的。
旋转实验编程实现：首先编写Quaternion类和Vector3类，前者主要用来定义、初始化四元数、以及定义四元数乘法、实现重载运算符、实现从欧拉角到四元数的转换等，后者是向量类，用来定义旋转轴以及实现一些关于向量的操作。
Quaternion mouse_quat , before_quat, final_quat; //四元数对象
Vector3 axis;//旋转轴
float angle;//旋转角度
SetRot(1, (point.x - mousePrevpoint.x) , TRUE);//鼠标捕获点传给欧拉角
mouse_quat.EulerToQuat(Rot[0], Rot[1], Rot[2]);//鼠标产生的欧拉角转化为四元数before_quat = final_quat;
final_quat = before_quat * mouse_quat;//四元数乘法产生最终的旋转四元数
final_quat.GetAxisAngle(axis, angle);//得到旋转轴和旋转角度
glRotatef(angle, ax, ay, az);//调用OpenGL函数实现旋转
三维旋转的效果如图4-4所示，这种用四元数表示方位旋转的方法操作简单而且实用，旋转时画面流畅，不存在万向节死锁的现象，达到了预期的目的。但是，这种方法还有值得改进和扩展的地方，比如当模型发生平移时，在旋转模型之前需要将模型的物体坐标系原点移到世界坐标系原点。另外，本实验旋转的模型只有一个，当场景中的模型多于一个时，需要先利用OpenGL的交互技术实现鼠标捕获模型的功能，选择需要旋
转的模型来进行旋转。
图4-4自由旋转小汽车模型示例
Fig.4-4 The sketch map of rotating a car model
4.4四元数在鱼雷运动可视化中的应用
4.4.1四元数样条插值
在3D 数学中，四元数能进行球面线性插值，这个运算十分有用，因为它可以提供两个四元数之间的平滑插值。可以避免由于欧拉角插值引起的所有问题。这种插值是一种三元运算，这意味着它有着三个操作数。
先来看两个标量之间的插值，若要在两个标量a 1和a 2之间插值，可以用普通线性插值公式：
121lerp(,,)a a t a t a =+Δ (4.9) 其中21a a a Δ=?，t 为插值变量，满足01t ≤≤。但普通线性插值无法实现均匀的方位插值，因为等量的方位欧拉角不一定对应等量的弧长。单位化后的四元数描述了一个四维超球面，因此Glenn Davis 提出球面线性插值公式[5]，见式4.10，式中q 1和q 2分别表示方位1和方位2，ω表示方位1与方位2间的夹角，t 为插值变量，同式4.9。slerp 运算非常有用，因为它可以在四元数之间平滑插值，slerp 运算可以避免利用欧拉角插值方位时产生的所有问题。
1212sin(1)sin slerp(,,)sin sin t t q q t q q ????
?=+ (4.10) 该公式的推导是通过在平面上对两个2D 向量进行插值，利用三角几何学公式得到的。这里还需要考虑两点。第一，四元数Q 和-Q 代表了相同的方位，但是它们作为slerp 的参数时可能导致了不一样的结果，这是因为4D 球面不是欧氏空间的直接扩展。而这种现象在2D 和3D 中不会发生。解决方法是选择q 1和q 2的符号，使得q 1*q 2的符号为非负。第二个考虑就是如果q 1和q 2非常接近，sin ω会非常小，所以为了避免这样的情况，当sin ω非常小的时候使用简单的线性插值。
如图4-6所示，使用式4.10得到的插值结果始终位于同一球面上，而不会像普通线性插值那样沿着图4-5中的虚线穿入球面内部而导致运动速度变快，从而实现了两个方位间的平滑插值。
1
2
插值3
图4-5 四元数球面线性插值与普通线性插值的示意
Fig.4-5 Spherical linear interpolation and ordinarily linear interpolation
4.4.2鱼雷姿态序列四元数球面线性插值的实现
在本系统中，需要模拟鱼雷姿态随时间而不断地变化。本文采用3D 动画中常用的定时器和关键帧的方法来实现。因此，要得到平滑、逼真的仿真效果，最重要的是解决关键帧插值的问题，包括位置插值和姿态插值两个方面。位置插值可以直接根据速度曲线进行插值，但如果直接对姿态欧拉角进行插值来平滑姿态数据，很难得到准确的结果。因为同样的姿态可以有很多种欧拉角表达方式，如某一角度加上360，则会产生错误的插值。而且，等量的欧拉角的变化不一定产生等量的旋转，这是由于旋转群和欧氏空间之间的度量单位的差异引起的。因此，采用四元数的方法对鱼雷模型的姿态序列进行球面线性插值，解决了用欧拉角插值引起的上述问题。
四元数插值用于鱼雷运动可视化的大致过程如下：
Quaternion f_Orientation_quat, l_Orientation_quat;//起始方位四元数
Quaternion slerpOut_quat;//插值输出的四元数
f_Orientation_quat.EulerToQuat( roll_f, pitch_f, yaw_f );//转换方位的存储方式
l_Orientation_quat.EulerToQuat( roll_l, pitch_l, yaw_l );
float t; //插值变量
cosOmega=f_Orientation_quat.w*l_Orientation_quat.w+f_Orientation_quat.x*l_Orienta
tion_quat.x+f_Orientation_quat.y*l_Orientation_quat.y+f_Orientation_quat.
z*l_Orientation_quat.z; //用cos 值表示夹角
float k0,k1;
for(t=0.0f;t<1.f;t=t+0.001)
{
if(cosOmega>0.9999f)//角度趋近0时线性插值
{
k0= 1.0f-t;
k1=t;
} else { float sinOmega =sqrt(1.0f-cosOmega*cosOmega); float daoshusinOmega = 1.0f/sinOmega; float omega =atan2(sinOmega,cosOmega); k0=sin((1.0f-t)*omega)*daoshusinOmega; k1=sin(t*omega)*daoshusinOmega; } }
slerpOut_quat.w =f_Orientation_quat.w*k0+l_Orientation_quat.w*k1;
slerpOut_quat.x =f_Orientation_quat.x*k0+l_Orientation_quat.x*k1;
slerpOut_quat.y =f_Orientation_quat.y*k0+l_Orientation_quat.y*k1;
slerpOut_quat.z =f_Orientation_quat.z*k0+l_Orientation_quat.z*k1;
这样就得到了需要的四元数形式的插值结果。按照4.3.3中的方法编程就能最终实现四元数插值。
但是式4.10只能提供两个方位之间的插值，如参考文献[21]中的中间帧插值实验只
对模型的两个关键帧之间进行插值。而该系统中的鱼雷模型的方位是不断变化的，这就需要对一个方位序列进行插值。如果在每两个方位间使用式4.10，显然在采样方位处不能连续，这类似于三次参数样条曲线在型值点处必须满足二阶导数相等，才能保证整条
曲线是光滑的。Dam 和Erik B. 等人在式4.10的基础上提出squad(Spherical and Quadrangle)公式，即式4.11，构造出与原始曲线具有类似微分性质的单位四元数曲线。 1111squad(,,,,)slerp(slerp(,,),slerp(,,),2(1))i i i i i i i i q q s s h q q h s s h h h ++++=? (4.11) 其中：
1111log()log()exp()4i i i i i i q q q q s q ??+?+=?
本文利用数值法求解鱼雷运动方程得到鱼雷在航迹上若干个采样时间点处的姿态角参数，数据采用欧拉角的形式，然后按上个实验中的方法将它们转换为四元数的形式，得到最初的四元数序列：q 1,q 2,q 3,…,q n -1,q n 。运用式4.11对该序列进行整体的优化插值，提高方位序列的精度，得到需要的四元数姿态序列，然后从中提取旋转数据，最终实现模型的平滑运动。
用四元数表示鱼雷姿态角序列，在插值之前，模型的运动有时会出现明显的颤抖现象。当对姿态序列进行四元数球面线性插值之后，鱼雷模型的运动变得平滑、合理，达到了比较好的可视化效果，如图4-6所示。
图4-6 不同时刻的鱼雷模型姿态效果图
Fig.4-6 The chart of the torpedo’s state at different time
4.5本章小结
本章利用四元数的方法对鱼雷模型的姿态角序列进行球面线性插值，使得鱼雷模型的运动平滑、逼真。解决了在科研工作中长期存在的鱼雷仿真速度和仿真效果之间的矛盾。还改进了在可视化工具里需要用鼠标对物体模型进行拖拽三维旋转的方法，用四元数替代物体模型的方位欧拉角来表示旋转，这样把直观的欧拉角和高效的四元数相结合，在OpenGL中实现了用鼠标有效地对物体模型进行三维旋转。相信随着计算机技术的发展，四元数在3D图形系统中的应用将会更加广泛和深入。
第五章 视景仿真关键技术研究
5.1海底场景坐标系的相关技术
5.1.1几何变换
所有的图形可视化系统的运动模型都是建立在坐标系基础之上的，因此，首先需要建立起合适的鱼雷运动可视化系统的坐标系，本系统为了和OpenGL 的规定保持一致，采用右手坐标系，世界坐标系又称全局坐标系，是鱼雷在运动过程中的参考坐标系。系统中取海底任意一点为坐标原点，向东的方向为X 轴方向，垂直向上为Y 轴的正方向，南方为Z 轴的正方向。
鱼雷的物体坐标系原点在鱼雷上，鱼雷绕世界坐标系的X 轴的旋转角为俯仰角(α)，逆时针为正，绕世界坐标系的Y 轴旋转角为偏航角(φ)，逆时针为正方向，绕世界坐标系的Z 轴旋转角为滚转角(ψ)，同样也是以逆时针为正方向。
系统中最基本的几种几何变换包括平移变换，旋转变换，缩放变换。
1. 变换矩阵[9]
三维图形的几何变换矩阵可用T 3D 来表示，其形式如下：
11121314212223243D 3132333441424344T a a a a a a a a a a a a a a a a ??????=??????
(5.1) T 3D 可分为4个矩阵，其中：1112
132122
2331
3233a a a a a a a a a ??????????产生缩放、旋转、错切等几何变换，[a 41 a 42 a 43]产生平移变换。
2.平移变换
[x * y * z * 1] = [x y z 1] 1000010000101x y z T T T ??????????????
= [x +x T y +y T z +z T 1] (5.2) 如图5-1所示。
飞控姿态解算理解
姿态解算理解 1、姿态的描述方法 前几天在论坛里偶尔看到一个帖子，帖子的内容是问的为什么不用倾斜角表示姿态，我认为他说的倾斜角是指的斜面与斜面的夹角，或者说是物体与垂线的夹角吧，这种想法可能来源于我们日常生活的思维。 图1立方体 比如有一个立方体，我们放在水平面上的时候它的底面是和水平面平行的，但是当我们把立方体的一个脚垫起一个角度时，这样一来，立方体的一条棱与水平面的垂线就有了一定的夹角了。我们所说倾斜了多少多少度就是指的这个夹角，这是我们直观的反应。我认为这样直观的反应甚至比欧拉角还要来的直观，因为欧拉角是基于旋转的，肯定不会说这个立方体X、Y轴各旋转了多少度（假设Z旋转无效），我们可能也没那个概念，我们直观的反应就是它倾斜了一定度数。 但是我们在姿态解算的时候为啥不用的这种描述方法呢，个人认为是虽然方便我们直观的表达但不适合数学上的计算，还有就是我们仅仅知道这个倾斜角我们怎么施加控制量呢？高中物理学习物体运动的时候我们知道，物体的运动是合运动，我们可以把它的运动矢量正交分解为几个运动的合成（不正交也是可以的，但那不是在自找麻烦吗），同样道理，我们可以把刚体的旋转分解为三个轴上的旋转，这个旋转的角度就是欧拉角，如图2。 图2zxz序规欧拉角 欧拉角 欧拉角的定义不仅仅和旋转角度有关系，还和旋转轴的旋转顺序有关系，任何一种旋转顺序都是合法的。根据定义，欧拉角有12种旋转顺序（维基），一个物体通过任意一个旋转顺序都可以达到同样的姿态，在各个学科里所以为了统一，航空航天领域规定XYZ为欧拉角的旋转顺序。 上面已经说了欧拉角的定义。欧拉角的定义也是很直观而且容易理解的，也利于我们的计算，因为我们用的惯性器件也是按照单个轴向运动来测量的。定义上的欧拉角还和我们所说的Yaw、Pitch、Roll不是一回事。因为定义上的欧拉角就是刚体绕三个轴的旋转角度，
四元数转欧拉角代码解析
四元数转欧拉角代码解析 本文的内容就是解析正点原子MPU6050的mpu_dmp_get_data()函数中，三个欧拉角的由来，即如何将MPU6050输出的四元数转化为姿态解算所需要的欧拉角。 *pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch *roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll *yaw = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3; //yaw 其实上述三个公式的核心就是将一次的姿态变换分别用四元数矩阵和欧拉角矩阵表示出来，由于这两个矩阵是等价的即对应元素都相等，通过简单的对比运算就可以得到上述的三个公式。 因此，我将从1.四元数矩阵的得到；2.欧拉角矩阵的得到；3.两个矩阵的等价运算三个部分进行说明。 1.四元数矩阵的得到 三重矢量计算公式： AX(BXC)=B(A·C)-C(A·B) 这个公式很好记，右边部分就是BACK-CAB(后面的出租车)
2.欧拉角矩阵的得到 q02+q12+q22+q32=1 从9.2.33到9.2.34的化简，其实就是利用 进行化简，把1去掉即可。 将右侧的矩阵乘开，可得到一个3x1矩阵， 与左边3x1矩阵对应元素相等，这个相等的 关系，就是上个框框中求出的三个等式。 各轴上的单位1，就是图1.2.2矩阵任意 行与列各个元素的平方和为1。
到这里，用欧拉角表示描述一次旋转变换已经结束了。然而，上述的姿态矩阵C n b仅仅是《惯性导航》这本书先Z,再X,最后Y旋转变换而形成的姿态矩阵，这样的旋转顺序其实是和很多大家实际使用的飞控代码不一样的（同样的，关于θφγ的实际意义其实也没有明确的规定）。此文目的就是解析“正点原子”飞控代码中四元数转欧拉角部分，因此，接下来，
四元数姿态的梯度下降法推导和解读
四元数姿态的梯度下降法推导和解读 笔者前面几篇文章讨论的是基于四元数的互补滤波算法，并单独对地磁计融合部分做了详细的讨论和解释。而本文讨论的姿态融合算法叫做梯度下降法，这部分代码可以参见Sebastian O.H. Madgwick在2010年4月发表的一篇论文（An efficient orientation filter for inertial andinertial/magneticsensor arrays），这篇论文利用四元数微分方程求解当前姿态，然后分别利用加速度计和地磁计进行补偿，推导出两种姿态融合算法。两种算法均为梯度下降法，而其中地磁计的处理方式笔者已经在《四元数姿态解算中的地磁计融合解读》一文中详细讨论了，这里笔者将对Madgwick对于加速度计和地磁计的梯度下降法做出详细的解释，期间一定有个人不足的地方，仅供参考，希望和各位网友一起学习！ 首先来谈谈什么是梯度。维基百科中解释的是“标量场中某一点上的梯度指向标量场增长最快的方向，梯度的长度是这个最大的变化率。”很显然，梯度和变化率有关。现在我们引入标量函数f(x)，对标量函数f(x)求导，不难得到f’(x)就是梯度，就是曲线在某一点的斜率。梯度下降法就是我们顺着这个在某一点下降速度最快的反方向一直走，走到一个极值点，这个点就是最优解（稳定解）。 那么这个梯度的概念和我们的姿态解算有什么关系？ 我在前面的文章中已经说明：我们求解姿态就是求解的转换矩阵（矩阵元素就是四元数）。这个转换矩阵是有误差的，我们所要做的工作就是采用某种算法，消除误差，最后得到的解就是我们的近似精确解，也就是姿态四元数了。消除误差四个字，在实际的实现过程中，是通过误差函数来实现的。定义误差函数ef(x)，那么我们的工作就是令ef(x)=0，求解上述方程得到x的值。我们在求解高阶方程的时候，一般的方法就是求导，求极值点，根据这些值来判断精确值个数和位置。这是我们高中所学习到的知识，在这里是一样的。只不过，这里的误差函数ef(x) 不再是之前讨论的简单的标量函数了，他的自变量x变成了向量[q0 q1 q2 q3]。这也就是说，原先的标量函数ef(q) 变成了如今的标量函数ef([q0 q1 q2 q3]) ，他仍然是标量函数，但是自变量是向量[q0 q1 q2 q3]。 对上述自变量是向量的标量函数，我们要用梯度法求解，就必须求导。标量函数对向量求导很简单，只需要分别对向量中的各个变量求偏导即可： 但是，我们的姿态解算是三维姿态，不是一维姿态，所以，这里的ef(q)并不是一个标量函数，实质上是一个向量函数ef ( q )，这个向量函数里面有三个元素，分别对应xyz轴的三个分量，每个分量又由一个四元数向量q构成。那么现在就引入了一个较为复杂的误差函数ef ( q )，该误差函数不光自变量是一个向量，并且因变量也是一个向量，这种函数叫做多元向量函数。那么我们现在的问题就转化为求多元向量函数的极值问题。 针对上述极值问题，在计算机中，多采用数值解法，如最速下降法、牛顿法、共轭梯度法。我们这里讨论就是第一种算法，又叫做梯度下降法。PS：梯
捷联式姿态解算过程
（1）加速度记测得载体相对惯性空间比力b ib a ，经过误差补偿之后的b ib a 经过姿态矩阵n b C 的变换得到n ib a ：n n b ib b ib =C a a 将n ib a 进行误差补偿之后通过积分运算得到速度分量n en V 。n en V 一方面作 为系统的输出，一方面作为输入用来求解位置角速率n en W 。 （2）陀螺仪测得载体相对惯性空间角速率b i b W ， 首先通过速度分量n en V 求得位移角速率n en W ，因为n eny W =0， n enx n eny W W ????????=22yt xt 22y yt xt yt t x x t t sin cos ()R R sin cos R 11sin cos R R 11sin cos R R R αααααααα?? -???????? -???-++? -（）（）n x n y V V ?? ?????? 这样就根据陀螺仪测得的角速率b i b W ，上面所求得的位移角速率n en W ， 加上已知的地球角速率e i e W 来求的姿态角速率b n b W 。 b b n b n e i e n i e n e i e W =C W =C C W 且 b b n e n n e n W =C W b b b b n b i b i e e n W =W W W --=b i b W b n e n n e i e e n C C W +W -（） 。 然后根据姿态角速率利用四元数微分方程求出四元数中的元素a,b,c,d. 四元数描述了一个坐标系或一个矢量相对某一个坐标系的旋转。a 是标量部分表示了转角的一半余弦值，b ，c ，d 是矢量部分表示瞬时转轴的方向，瞬时转动轴与参考坐标系轴间方向的余弦值。 表达式A=a+bi+cj+dk 。
四轴姿态解算
又花了将近一个星期，终于把姿态解算的框架完成了。仅仅是把陀螺仪、加速度计、罗盘融合在一起，得出旋转姿态，没有对加速度积分，没有用到气压计，几乎没有滤波。算是阶段性的工作吧，把框架设计得合理一点，以后添加/修改就很简单了。 从传感器的读取，到四元数的学习，到空间旋转的处理方法，循序渐进，逐个击破。主要参考了以下资料（按阅读的时间循序）： 《计算机图形学几何工具算法详解》（四元数转矩阵的公式是错的！） 《交互式计算机图形学——基于OpenGL的自顶向下方法》 维基百科——四元数 框框的日记——四元数 青衫湮痕——四元数 Heath's blog——四元数与欧拉角之间的转换 阿莫电子论坛——【原创】姿态估计 下面总结一下“姿态解算”的过程，分为“传感器”、“四元数与旋转”、“姿态解算框架”、“长期融合”、“快速融合”四部分。 1.传感器 我用的是10轴姿态传感器模块，其中陀螺仪是L3G4200D，加速度计是ADXL345，罗盘是HMC5883L，气压计是BMP085。全部都通过一条共用的I2C总线访问，速度都支持400kHz。先讲讲I2C库。要配置、读取传感器，首先把通信做好，这里就是I2C库了。现在大部分单片机都有支持中断的硬件I2C了，可以写个高效的I2C库。我只实现了主机发送和主机接收模式，这里简单介绍一下接口。接口函数主要有2个： uint8_t I2C_transmit (uint8_t which,I2C_transmitCallback cb); void I2C_setNextCallback (uint8_t which,I2C_transmitCallback cb); I2C_transmit()用于触发一次传输（发送或接收），异步执行，调用后马上返回。其中有一个I2C_transmitCallback类型的参数，就是决定发送或接收、如何处理数据的回调函数了，其定义如下： /* 数据传输回调函数。 * 每（准备）传输一个字节都调用一次。 * 参数： * seq =>序号，第一次调用时为0，以后每次调用递增。 * data => seq==0时写(从机地址+W/R)到data。 * seq!=0时data是数据。发送就写data，接收就读data。 * 返回值表示下一步的行为： * I2C_RT_START =>发送开始信号。 * I2C_RT_STOP =>发送停止信号。 * I2C_RT_REPEAT_START_OR_STOP =>如果有下一次传输，就发送RepeatStart，否则发送Stop。 * I2C_RT_ACK =>继续传送，回应ACK。 * I2C_RT_NACK =>继续传送，回应NACK。 */ typedef uint8_t (* I2C_transmitCallback)(uint8_t seq,uint8_t * data); 当用I2C_transmit()成功触发一次传输后，I2C库会根据需要调用回调函数，所以使用这个I2C 库就是写回调函数了。 而I2C_setNextCallback()则是用来设置紧接着的一次传输的。当本次传输结束时，不发送“Stop”
基于四元数方法的姿态解算
基于四元数方法的姿态解算方法分析 摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。 1引言 捷联惯导是一种自主式的导航方法。该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。 姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。（就是这部分内容需要程序解算，不会搞） 2姿态矩阵的计算方法 由于载体的姿态方位角速率较大，所以针对姿态矩阵的实时计算提出了更高的要求。通常假定捷联系统“数学平台”模拟地理坐标系，即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(6)和导航坐标系(E)的空间转动关系，一般用载体坐标系相对导航坐标系的三次转动角确定，习惯上俯仰角和偏航角用B和必表示，滚转角用Y表示。目前主要的研究方法为:欧拉法、方向余弦法与四元数法。图1为捷联式惯性导航原理图。
四元数解算姿态完全解析及资料汇总
四元数完全解析及资料汇总 本文原帖出自匿名四轴论坛，附件里的资源请到匿名论坛下载： 感谢匿名的开源分享，感谢群友的热心帮助。 说什么四元数完全解析其实都是前辈们的解析，小弟真心是一个搬砖的，搬得不好希望大神们给以批评和指正，在此谢过了。因为本人是小菜鸟一枚，对，最菜的那种菜鸟······所以对四元数求解姿态角这么一个在大神眼里简单的算法，小弟我还是费了很大劲才稍微理解了那么一点点，小弟搬砖整理时也是基于小弟的理解和智商的，有些太基础，有些可能错了，大牛们发现了再骂过我后希望能够给与指正哈。 好，废话到此为止，开始说主体。四元数和姿态角怎么说呢先得给和我一样的小菜鸟们理一理思路，小鸟我在此画了一个“思维导图”（我承认我画的丑），四元数解算姿态首先分为两部分理解：第一部分先理解什么是四元数，四元数与姿态角间的关系；第二部分要理解怎么由惯性单元测出的加速度和角速度求出四元数，再由四元数求出欧拉角。 图1 渣渣思维导图 在讲解什么是四元数时，小弟的思维是顺着说的，先由四元数的定义说起，说到四元数与姿态角间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉角一步步反推回到惯性器件的测量数据，这样逆向说是因为便于理解，因为实际在工程应用时和理论推导有很大差别。
实际应用时正确的求解顺序应该为图1中序号顺序，即1->2->3->……. 但在笔者讲解姿态求解时思路是如图2的。 ` 图2 逆向讲解思路 大家在看四元数时最好结合着代码一块看，小弟看的是匿名四轴的代码，感觉写的非常好也非常清晰，粘出来大家一块观摩。红色部分是核心代码，总共分为八个步骤，和图1中的八个步骤是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下载，都是开源的，不是小弟的，所以小弟不方便加在附件中。 好的，下面搬砖开始！。。。。。。。。嘿咻嘿咻！！！！ · 一．
四元数姿态解析x
这个程序得到了四元数，怎么进一步计算姿态YAW，ROLL，PITCH？ //---------------------------------------------------------------------------------------------------- // Header files #include "AHRS.h" #include
捷联系统的四元数法姿态算法
捷联系统的四元数法姿态算法 算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；算法输出：物体的当前姿态。 具体算法： 1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A 2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b 3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B 4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A 5.重复2～4部，即可连续更新姿态 6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态 核心算法1，欧拉角转四元数 void Quaternion::FromEulerAngle(const EulerAngle &ea) { float fCosHRoll = cos(ea.fRoll * .5f); float fSinHRoll = sin(ea.fRoll * .5f); float fCosHPitch = cos(ea.fPitch * .5f); float fSinHPitch = sin(ea.fPitch * .5f); float fCosHYaw = cos(ea.fYaw * .5f); float fSinHYaw = sin(ea.fYaw * .5f); w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw; x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw; y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw; z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw; } 核心算法2，四元数转欧拉角 EulerAngle Quaternion::ToEulerAngle() const { EulerAngle ea; ea.fRoll = atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x)); ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f)); ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y)); return ea; }
第四章四元数在3D物体姿态变化中的应用_2
第四章 四元数在3D 物体姿态变化中的应用 4.1引言 对于三维仿真系统中鱼雷运动的可视化研究，一般的仿真系统采用了按照鱼雷的六自由度参数来直接驱动模型的视景仿真。为了保证计算速度，往往需要在计算运动方程时选取较大的计算步长，这样有时会无法实现仿真运动的光滑和连续，甚至出现鱼雷运动跳跃的现象，如果选取较小的计算步长，那么计算速度的降低同样会影响到鱼雷的可视化效果。 目前，国内的仿真系统里用来解决此问题的专用软件模块十分少见。本课题着重研究了四元数在三维仿真系统中的应用，包括利用四元数对物体进行自由旋转和利用四元数来解决鱼雷运动可视化中的问题，本文利用四元数对模型姿态角变化进行插值，很好地解决了上述的鱼雷运动可视化仿真中存在的矛盾。 4.2四元数简介 四元数是1843年由英国数学家William Rowan Hamilton 发明的数学概念。它的出现很好地把二维空间中的复数扩展到了三维空间。由于四元数的乘法不满足交换律，对它的研究较实数和复数都要困难，以至在它出现后一个多世纪都得不到很好的应用。直到1985年Ken Shoemake [20]将它引入计算机图形学之后，四元数在计算机动画和三维图形绘制方面才得到实际的应用。随着计算机技术和图形学的发展，四元数所表现出来的优势也日渐得到人们的重视。 4.2.1四元数记法 一个四元数包含了四个分量，一个标量分量和一个3D 向量分量。也可以说是一个实部，三个虚部，表示如式4.1所示： i = + +j +k Q w b c d (4.1) 式4.1中w , b , c , d 为实数，i , j , k 为虚数，满足i 2= j 2 = k 2 =－1。四元数也可以表示为：[w ,v ] 或 [w , (x ,y ,z )]，其中w 为标量，v ＝(x ,y ,z )为矢量。 4.2.2四元数的运算 四元数的加减法和一般复数的加减法相同，也满足交换律和结合律。但是四元数的乘法满足结合律但不满足交换律，这是与实数和复数最显著的不同[5]。 四元数加法： 121212[,]Q Q w w v v +=++ (4.2) 四元数点乘：1212121212 Q Q w w x x y y z z ?=+++ (4.3) 四元数的共轭：*[,(,,)]Q w x y z =??? (4.4) 四元数的模：Q (4.5)