《四元数解算姿态完全解析及资料汇总》由会员分享，可在线阅读，更多相关《四元数解算姿态完全解析及资料汇总（20页珍藏版）》请在金锄头文库上搜索。
1、四元数完全解析及资料汇总本文原帖出自匿名四轴论坛，附件里的资源请到匿名论坛下载：感谢匿名的开源分享，感谢群友的热心帮助。说什么四元数完全解析其实都是前辈们的解析，小弟真心是一个搬砖的，搬得不好希望大 神们给以批评和指正，在此谢过了。因为本人是小菜鸟一枚，对，最菜的那种菜鸟所以对四元数求解姿态角这么一个在大神眼里简单的算法，小弟我还是费了很大劲才稍微理解了那么一点点，小弟搬砖整理时也是基于小弟的理解和智商的，有些太基础， 有些可能错了，大牛们发现了再骂过我后希望能够给与指正哈。好，废话到此为止，开始说主体。四元数和姿态角怎么说呢先得给和我一样的小菜鸟们理 一理思路，小鸟我在此画了一个“思维导图”
2、（我承认我画的丑），四元数解算姿态首先分为 两部分理解：第一部分先理解什么是四元数，四元数与姿态角间的关系；第二部分要理解怎么 由惯性单元测出的加速度和角速度求出四元数，再由四元数求出欧拉角。在讲解什么是四元数时，小弟的思维是顺着说的，先由四元数的定义说起，说到四元数与 姿态角间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉角一 步步反推回到惯性器件的测量数据，这样逆向说是因为便于理解，因为实际在工程应用时和理 论推导有很大差别。实际应用时正确的求解顺序应该为图1中序号顺序，即1-2-3-但在笔者讲解姿态求解时思路是如图2的。图2 逆向讲解思路大家在看四元数时最好结合着
3、代码一块看，小弟看的是匿名四轴的代码，感觉写的非常好 也非常清晰，粘出来大家一块观摩。红色部分是核心代码，总共分为八个步骤，和图1中的八 个步骤是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下载， 都是开源的，不是小弟的，所以小弟不方便加在附件中。力加速度归一化();取四元数的等效余弦矩阵中的重力分量(V_grav ity)； 量叉积得出姿态误差 V_error = acc % V_gravity； 误差进行积分 V_error_I += V_error * Ki；(补滤波，姿态误差补偿到角速度上，修正角速度积分漂移Gyro += V_error * Kp + V_er
4、ror_I； 阶龙格库塔法更新四元数 (Gyro, deltaT)； 元数归一化()；元数转欧拉角(&, &, &；好的，下面搬砖开始！。嘿咻嘿咻关于四元数的定义摘自秦永元的惯性导航，里面有非常好的讲解，大家可以直接看绪 论和第九章就可以。下面我粘贴了部分原文，粘贴的比较多比较详细，应为本人比较笨还爱较 真，所以按本人的风格就要详尽一点，大牛们都可以自动忽略。四元数定义、表达方式及运算方法摘自惯性导航-秦永元P289-2929-2姿态更新计算的四元数算法设由运载棒的机体轴确定的坐标系为乩惯导系统所采用的导航坐标系为机 则由阮系到牯系的坐标变换矩阵口称为运载休的姿态矩阵B姿态更新是帝棍摇惯 性器
5、件的输出实时计算出G矩阵*由于起系和占系均为直弟坐拯系.各軸之阖始 錨保持直用，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时可对一个坐标系作平移使其庶点与另一个坐标系的原点重合。同此，两坐标系 间的空间角位星关系可理解成刚体的宦点转动。从这一基本思想出发，可菽得姿态 更新的四元数算法及旋转矢莹算祛。本节详细介绍四元数更新算法*4 2-109元数1. 四尢數定义顾名思义四元数是由四个元构成的数：，住诃=如 + Qi* 十 Qzj + g永(S* 2, I)其中心是实数既是互相正交的单位向量，又是虚单位具体 规定休现在如下四元数乘法关系中：1i=1fjj =- u it* =一 n
6、/=j k i,ki = j ?9* 2. 2)* J = h t k = j J式中，凶表示四元数乘法*上述关系可叙述为*相同草位向量作四元数乘时呈虛单位持性相异单位向量 作四元数乘时呈单位向量叉乘特性所以四元数既可看作四维空闻中的一个向量, 又可看作一牛超震数*2. 四元數的表达方式(1) 矢量式Q =取十孕氏次3)苴中炖。称四元数Q的标量部分诃称四元数盘的矢蟻部分对照式(9. 2. 1),可看 出g是三维空间中的一个向量&(2) 复数式Q =山 + g+ q*j + g9-畧 4)可视为一个超复数,Q的共辄复数记为?* =驭?( q?j 一 g5*(9. 2* 5)(T祢为0的共觀四元数
7、。(3) 三角式0 0 c = cos y + wsin y(9* 2* 6)式中祷为实数潭为单位向量。(4) 指数式Q =(讯 2、7)B和心同上 289-(5)矩阵式牯(9. 2* 8)7l?2J?3-3.四兀數的大J范數四元数的大小用四元数的范数来表示IZ2若 I&II=1*则称为规范化四元数。4. 四元魏的运界畑滅乘除1)加法和减法设Q =如 + 十 卫讲g 十+ p2?0 + ?3/1 员知J +（肌牝PsQo 十 P电PgM=恂+门1上式写成距阵形式：* 290 *其中M（/O的构成形式为：第一列是四元数户本身.第一行是P的共純四元数P* 的转置划去第一廿和第一列余下的部分。Pv
8、Pi尸=p3Pr）-一 /*? 扒Pi-Pi 2- 15)称为财（巧的核，是由四元数P的元构成的反对称矩阵口同理M（Q）的核为L知 一幻 伽 可见MPQ）与樹成相似但核不同。由式（9.狄13）和式（9. 2-得四元数乘法的矩阵表示形式;PQ = M（P）QPQ= Mf （Q）P由于和A/r（P）的核不同所以P 2 = Af（P）6 M7P）0 = QP上式说明四元数乘法不满足交换律 四无数乘法满足分配律和结合律；尸 也+ K）=PQ + F0RPQR=CPQ）R = PQ0R）(9. 2.16)2*17a)0 2.17b)(9-2-18)(9 2- 19)3）除法一一求逆如果= 则称&为P的逆
9、，记为& =厂X或称戸为斤的逆，记为P =R棍据范数定文和式（92.12）F 3 L =如 + 打+ pd 十 （ - pi pd 一 pk）+ 禹 + 捏 + 肉=II P II.所以驱品 =1，根据上述关于逆的定買，仔卩即为P的逆，即P = T好，关于四元数定义就搬这么多，其他的大家去附件下载惯性导航的 pdf 自己看吧。下面开始搬四元数与姿态解算关系的。嘿咻嘿咻从上面我们知道了四元数的定义，可这四个数和我们要求的三个姿态角有什么关系呢下面 是详细的推导，同样摘自惯性导航-秦永元 P292-297。即X X気Z2四元数与姿态阵间罚关系图9. 2.1利体的等效璇转设刚体以3=+ 相对R系旋转
10、，初始时却位世向量处于OA = r,经过时间f 后位置向量处于OA?=r根据欧拉定理，仅考 虑刚休在0时刻和I-时刻的角位育时，刚体从 川位置转到“位置的转动可等效成绕瞬轴U (单位向量转过R弟一次完成B这样，位置向；ft 做圆锥运动M和/T位于同一圆上汕和X也于 同一圆锥面上，下面分析F与r的关系$在圆上取一点瓦 使三且。中=9叭由图得OQ =(r - u)u(7A =r OQf = r 一 (r * uuO B =w X O A=u X r * u)u x u = X rOA! =OfAc& + asin= rco3 (r uucos& + tr X rsinB四元数与姿态阵间的关系摘自惯
11、性导航-秦永元P292-297设有参考坐标系尺，坐标轴为 広口1的S轨1- 坐标轴方向的单位向衆为7、乩刚 休相对氏系作疑点转动，定点为取坐标系再有刚体固联/系的坐标轴为応、 s坐标轴方向的草位向嶺为Zk假设初始时刻b系与R系重合为了便于分析 刚体的空闾角位蛙，在刚体上取一点占*转动点。至该点引位置向量0山如图 乂 2.1所示则该位置向量的空间位置实际上描述了刚体的空阖角位置。所以rf =OQ 4- Of A! = rcos + (1 cos)X (r * 十 m X rsintf 由三逐矢积计算公式：u X (u X r) =m(u * 尸) (u - u)r=(r u)u rrf =rco
12、sQ 十(1 cosfl) r + u X ( X r) + u X =r + w X rsin? + ( cosm X (k X r)将上式向R系内投影：rsinB所以=泸 + (u X r)sin4(1 eos0 X C X 町盘- 厂;, | =1 m记-升ji -又根据更乘关系表达式:0 tim 1n0一 I-一 mI0 -他 X r)K = UrRX (u X r)R = U * X一严 + t/rsint? + (1 cos)C/ Ura=I f + 2t/sin oo冬 # 十 2sin 弓U * V 卢D = f 十 2t7$in cos + 2sinz 吕/ * UEEa则式
13、（乩Z 24）可写成;r，R = Dfr（EZ 26） 记初始时刻的刚悴固联坐标系为民由于初始时刻刚体固联坐标系与参考坐 标系觅合，所以rR （9+ 2 27而在转动过稈中位習向曲和b系都同刚体fS联+所収位置向量和鸟累的相对角位 置始继不变，即有A = r,A 将式細.2.茨）代入式（讯Z 2，得rK匸卅 将式9* 2- 29）代入式92. 26，得r,R = M(9.2. 28)谏式说明D即为小系至R系的塑标变换距阵，根据式灼.2, 25)和式乩2. 23) D = I + 2t7sjn -|icob 号 + 2sin3 -yl7 * (7(9. 2*L0 0+ ?cos 彳rtSin7一 main -一 (rE + rtsin5 -?Zwrsin2 -yZmsinz
