基于四元数方法的姿态解算
基于四元数方法的姿态解算方法分析
摘要:载体的姿态解算算法是实现捷联式惯性导航系统精确导航的核心技术之一。分析了欧拉法、方向余弦法、四元数法求解姿态矩阵的优缺点，采用四元数法与方向余弦法两种解算方法分别计算载体姿态，两种方法的计算结果之差与理论真值比较以得到解算的相对误差，从而验证了四元数法的正确性和有效性。最后，指出提高采样频率和采用高阶计算算法能进一步减小姿态解算误差。数字化仿真与转台试验结果表明，本文提出的载体姿态解算法具有良好的实时性。
1引言
捷联惯导是一种自主式的导航方法。该方法将陀螺仪和加速度计直接安装在载体上，省掉机电式导航平台，利用计算机软件建立一个“数学平台”来代替机电平台实体[1]。由于其结构简单且抗干扰能力强，目前已成为航空航天、航海、机器人、智能交通等领域的研究热点之一。
姿态解算是捷联式惯性导航系统的关键技术，通过姿态矩阵可以得到载体的姿态和导航参数计算需要的数据，是捷联式惯导算法中的重要工作。载体的姿态和航向体现了载体坐标系与导航坐标系之间的方位关系，确定两个坐标系之间的方位关系需要借助矩阵法和力学中的刚体定点运动的位移定理。通过矩阵法推导方向余弦表，而刚体定点运动的位移定理表明，定点运动刚体的任何有限位移都可以绕过定点的某一轴经过一次转动来实现。目前描述动坐标相对参考坐标系方位关系的方法有多种，可简单地将其分为3类，即三参数法、四参数法和九参数法「1-2]。三参数法也叫欧拉角法，四参数法通常指四元数法，九参数法称作方向余弦法。欧拉角法由于不能用于全姿态飞行运载体上而难以广泛用于工程实践，且实时计算困难。方向余弦法避免了欧拉法的“奇点”现象，但方程的计算量大，工作效率低。随着飞行运载体导航控制系统的迅速发展和数字计算机在运动控制中的应用，控制系统要求导航计算环节能更加合理地描述载体的刚体空间运动，四元数法的研究得到了广泛重视。本文全面分析了3种解算方法的特点，通过对比四参法与九参法的计算结果以验证四元数法的正确性和有效性，基于数值仿真和转台实验相结合的分析方法得到进一步减少姿态解算误差的有效途径，为捷联式惯性导航技术的工程实践提供参考。（就是这部分内容需要程序解算，不会搞）
2姿态矩阵的计算方法
由于载体的姿态方位角速率较大，所以针对姿态矩阵的实时计算提出了更高的要求。通常假定捷联系统“数学平台”模拟地理坐标系，即导航坐标系;而确定载体的姿态矩阵即为研究载体坐标系(6)和导航坐标系(E)的空间转动关系，一般用载体坐标系相对导航坐标系的三次转动角确定，习惯上俯仰角和偏航角用B和必表示，滚转角用Y表示。目前主要的研究方法为:欧拉法、方向余弦法与四元数法。图1为捷联式惯性导航原理图。
加速度计导航计算
姿态矩阵陀螺仪姿态矩阵计算
姿态角计算
载体速度与位置
载体姿态角
图1 捷联惯导导航原理图
2. 1欧拉角微分方程式
一个动坐标系相对参考坐标系的方位可以完全由动坐标系依次绕3个不同的轴转动的3个角度来确定。如把载体坐标系作为动坐标系，把导航坐标系作为参考坐标系，则姿态角即为一组欧拉角，按一定的转动顺序得到导航坐标系到载体坐标系的关系。
??
?
?
????????????????-=??????????b EbZ b EbY b
EbX
ωωωγγ
θγγθθγθγθψθγcos sin 0
cos sin cos cos 0sin cos sin sin cos (1)
根据欧拉角微分方程，由角速度可以求解3个姿态角。欧拉角微分方程式只有3个，但
每个方程()ωx x f x sin ,cos = 都含有三角函数的运算，计算速度慢，且方程会出现“奇点”，
方程式退化，故不能全姿态工作。
2. 2方向余弦矩阵微分方程式
当一个坐标系相对另一个坐标系做一次或多次旋转后可得到另外一个新的坐标系，前者往往被称为参考坐标系或固定坐标系，后者被称为动坐标系，他们之间的相互关系可用方向余弦表来表示。方向余弦矩阵微分方程式可写为载体坐标系相对导航坐标系旋转角速度的斜对称矩阵表达式，方向余弦表是对这两种坐标系相对转动的一种数学描述。
b Eb
E b E b C C Ω= (2) 式中，b Eb Ω为载体坐标系相对导航坐标系旋转角速度的斜对称矩阵表达式。用方向余弦法计算姿态矩阵，没有方程退化问题，可以全姿态工作，但需要求解9个微分方程
()()ωx C x C ij
ij = ，计算量较大，实时性较差，无法满足工程实践要求。 2. 3四元数微分方程式
四元数的数学概念是1843年由哈密顿首先提出的，它是代数学中的内容之一。随着捷联式惯性导航技术的发展，为了更简便地描述刚体的角运动，采用了四元数这个数学工具，用它来弥补通常描述刚体角运动的3个欧拉角参数在设计控制系统时的不足。四元数可以描述一个坐标系或一个矢量相对某一个坐标系的旋转，四元数的标量部分表示了转角的一
半余弦值，而其矢量部分则表示瞬时转轴的方向、瞬时转动轴与参考坐标系轴间的方向余弦值。因此，一个四元数既表示了转轴的方向，又表示了转角的大小，往往称其为转动四元数。
工程上一般运用范数为1的特征四元数，特征四元数的标量部分表示转角的一般余弦值，其矢量部分表示瞬时转轴n 的方向。比如式(3)表示矢量R 相对参考坐标系旋转一个转角θ，旋转轴二的方向由四元数的虚部确定，γβαcos cos cos 、、表示旋转轴n 与参考坐标系轴间的方向余弦值。
''qRq R = (3)
式中:R 为某矢量;
γ
θβθαθθλλcos 2
sin
cos 2
sin cos 2
sin 2
cos
321321====+++=p p p k
p j p i p q
四元数姿态矩阵微分方程式只要解4个一阶微分方程式组即可，比方向余弦姿态矩阵微分方程式计算量有明显的减少，能满足工程实践中对实时性的要求。
3基于四元数法的姿态解算
验证四元数法的正确性和有效性是将算法应用于工程实践的首要前提，在算法正确性的前提下应保证解算误差符合工程实践的需要。
3. 1四元数法正确性和有效性的验证
本文根据四元数法与方向余弦法两种解算方法进行计算，通过对比两种方法的计算结果，验证四元数法的正确性和有效性。
四元数法姿态矩阵计算的步骤如下:
(1)初始四元数的确定，如式(4)其输人为初始的姿态角。
()()()()??????
?????????
????
?---+=????????????2sin 2sin 2cos 2cos 2cos 2sin 2sin 2cos 2sin 2cos 2sin 2cos 2cos 2sin 2sin 2sin 2cos 2cos 2sin 2sin 2sin 2cos 2cos 2cos 0000000000
000000
000000000000
321γθψγθψγθψγθψγθψγθψγθψγθψλp p p (4) (2)四元数标量部分与矢量部分321p p p 、、、λ的实时计算，输人信号为陀螺仪的数字输出信号dt ib t
t t ωθ?
?+=
?，其中i 为z y x 、、。计算方法采用二阶龙格库塔法，如式(5)
()()()()()()()()()()
??
??
??
?
+?+=++Ω=Ω+=Ω=21212/K K T t q T t q Y T t K t q t T t q Y t q t K b b b (5) (3)姿态矩阵的实时计算，确定姿态矩阵b
E C ，输入为()()()()n p n p n p n 321、、、λ。计
算公式如式(6)。
()
()()
()()()
???
?
?
????
?--+-++--+--+--+=22
212321322311322
3212223212313212
322212222222p p p p p p p p p p p p p p p p p p p p p p p p p p p C b
E λλλλλλλλλ (6)
(4)载体姿态角计算，以确定姿态角γψθ、、,输人为
()()()()()n T n T n T n T n T 3323131211、、、、计算公式如式(7)
()()()()()()????
?
?
???
??????
??=???? ??=-=n T n T n T n T n T 3323111213arctan arctan arcsin θθθ (7) 方向余弦法姿态矩阵的计算与四元数法的区别主要是姿态矩阵的描述不同，其描述如式
(8)所示。
????
??????
-++--=γθγ
ψγθψγ
ψγθψγθγ
ψγθψγ
ψγθψθθ
ψθψcos cos sin cos cos sin sin sin sin cos sin cos sin cos cos cos sin sin sin cos sin sin sin cos sin cos sin cos cos b E C (8) 其解算方向余弦矩阵微分方程为Ω=b
E
b
E C C ，得到方向余弦矩阵b
E C 后可提取姿态角。 验证工作均以二阶龙格库塔法展开计算。
(1)针对单轴输入，两种解算方法的计算结果与数值理论值对比，比较其相对误差。
计算条件为:陀螺仪输出角速率。=s /50
,采样时间取为s T 01.0=、，该采样频率工程
实践可行;各通道独立解算，初始角为 90-，终止角为
90。
以滚转通道为例，图2为四元数法解算结果，图3为方向余弦法解算结果。单轴数值计算结果说明:根据上述的计算条件，单轴输人下，四元数法与方向余弦法的计算结果都是正确的，即姿态解算算法在单轴输入情况下是正确的;姿态解算的相对误差数量级为%102
-左右，且四元数法与理论真值的相对误差更小。
(2)针对三轴输人，两种解算方法之差与数值理论值对比，以比较两种方法的相对误差;
计算条件为:载体三通道陀螺仪输出为角速率s /60
=ω；载体三通道陀螺仪输出为正
弦角速率，幅值
60=A ，频率Hz f 4.0=;采样时间s T 01.0=，工程实践可行；各通道独
立解算，初始角均为
同样以滚转通道为例，图4为匀速输人下两种方法的相对误差，图5为正弦输人下两种方法的相对误差。三轴数值计算结果说明:三轴输入下，根据上述的计算条件，匀速与正弦输入下四元数法与方向余弦法的计算结果都是正确的，两种解算方法之差与理论真值比较的相对误差很小，相对误差的数量级为10-%左右。因为正弦输人时每步计算的角增量小，所以相对误差要稍小些。上述理论分析和数值仿真结果表明，四元数姿态解算算法在三轴输人情况下是正确的，且其计算精度高、理论完善，并具有良好的工程实践价值。
飞控姿态解算理解
姿态解算理解 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)