四元数解算姿态完全解析及资料汇总.docx

四元数完全解析及资料汇总本文原帖出自匿名四轴论坛，附件里的资源请到匿名论坛下载：感谢匿名的开源分享，感谢群友的热心帮助。说什么四元数完全解析其实都是前辈们的解析，小弟真心是一个搬砖的，搬得不好希望大 神们给以批评和指正，在此谢过了。因为本人是小菜鸟一枚，对，最菜的那种菜鸟所以对四元数求解姿态角这么一个在大神眼里简单的算法，小弟我还是费了很大劲才稍微理解了那么一点点，小弟搬砖整理时也是基于小弟的理解和智商的，有些太基础， 有些可能错了，大牛们发现了再骂过我后希望能够给与指正哈。好，废话到此为止，开始说主体。四元数和姿态角怎么说呢先得给和我一样的小菜鸟们理 一理思路，小鸟我在此画了一个“思维导图”（我承认我画的丑），四元数解算姿态首先分为 两部分理解：第一部分先理解什么是四元数，四元数与姿态角间的关系；第二部分要理解怎么 由惯性单元测出的加速度和角速度求出四元数，再由四元数求出欧拉角。四元数定义*图1渣渣思维导图在讲解什么是四元数时，小弟的思维是顺着说的，先由四元数的定义说起，说到四元数与 姿态角间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉角一 步步反推回到惯性器件的测量数据，这样逆向说是因为便于理解，因为实际在工程应用时和理 论推导有很大差别。实际应用时正确的求解顺序应该为图1中序号顺序，即1->2->3->但在笔者讲解姿态求解时思路是如图2的图2逆向讲解思路大家在看四元数时最好结合着代码一块看，小弟看的是匿名四轴的代码，感觉写的非常好 也非常清晰，粘出来大家一块观摩。红色部分是核心代码，总共分为八个步骤，和图1中的八 个步骤是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下载， 都是开源的，不是小弟的，所以小弟不方便加在附件中。力加速度归一化();取四元数的等效余弦矩阵中的重力分量(V_gravity);量叉积得出姿态误差V_error = acc % V_gravity；误差进行积分V_error_I += V_error * Ki；(补滤波，姿态误差补偿到角速度上，修正角速度积分漂移Gyro += V_error * Kp + V_error_I；阶龙格库塔法更新四元数(Gyro, deltaT)；元数归一化();元数转欧拉角(&, &, &；好的，下面搬砖开始！。嘿咻嘿咻关于四元数的定义摘自秦永元的惯性导航，里面有非常好的讲解，大家可以直接看绪 论和第九章就可以。下面我粘贴了部分原文，粘贴的比较多比较详细，应为本人比较笨还爱较 真，所以按本人的风格就要详尽一点，大牛们都可以自动忽略。四元数定义、表达方式及运算方法摘自惯性导航-秦永元P289-2929.2姿态更新计算的四元数算法设由运载体的机体轴确定的坐标系为知惯导系统所采用的导航坐标系为”, 则由氏系到找系的坐标变换矩阵侃称为运载体的姿态矩阵B姿态更新是指根据惯 性器件的输出实时计算出G矩阵，由于神系和8系均为直角坐标系，各触之间始 鲤保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时,可对一个坐标系作平移，使其原点与另一个坐标系的京点重合。因此，两坐标系 间的空间角位置关系可理解成刚体的定点转动从这一基本思想出发，可获得姿态 更新的四元数算法及旋转矢髭算法本节详细介绍四元数更新算法.% 2-1四元数L四元数定叉顾名思义，四元数是由四个元构成的数：Q(0gi 应£,齐)=如 + Qii 十 qij + qk【9. A I)其中如是实数，7土既是互相正交的单位向量，又是虚单位 VL具体 规定体现在如下四元数乘法关系中】i ® 1 = I,> ® J = 1 *fc ® jfc = 11- t,j®k = i,k®i = j I(933)j k ® j = i,i®k = j J式中表示四元数乘法"上述关系可叙述为十相同单位向量作四元数乘时呈虚单位特性；相异隼位向量 作四元数乘时呈单位向量叉乘特性。所以四元数既可看作四维空间中的一个向量， 又可看作一个超夏数，(1) 矢量式Q=如十q(策2,3)其中，如称四元数Q的标量部分,q称四元数。的矢量部分.对照式(9. 2. 1),可看 出g是三维空间中的一个向量，(2) 复数式G =如 + 如，+ qj + 73*<9- 2, 4)可视为一个超复数,Q的共辄复数记为£?* =即?< q?j 一 g5*(9. 2.5)Q称为。的共掘四元数.(3) 三角式Q = cos 芝 + wsin %(9. 2. 6)式中，日为实数仲为单位向量。(4) 指数式Q =已43. 2 7)B和&同上-跚9(5) 矩阵式Q= ".3.2* 8)J?3-3.四元敷的大1范敷四元数的大小用四元数的范数来表示I乏3 g 十 4 +(9, 2.9)II Q II =就 + 术若II 011 =1,则Q称为规范化四元数。4. 四元数的运算加德乘除1)加法和减法设Q = % + gJ + 如，+ %kp =加 + Pli + pzi + MII度上 P = 3()1.+ 31 土 pi)f 十(宴.M)$ <?s 土 p.A(9-2- 10)2)乘法aQ =+ aj + aqk(9- 2 11)* 290 *其中2为标量.P®Q= 3d + p"十 pd + 户就)®(Vo + qti 7zJ 十 #3左)(PoQq pll PzQz p3 中+ g 艘' + PQ> 十 *3如)，十(内如+如如十?3</1 少L%)j十璀u9,尚如+ P面P而)执 =&| + 口1上式写成矩阵形式：其中，的指成形式为：第一列是四元数F本身，第一行是P的共鲍四元数P* 的转置，划去第一行和第一列余下的部分，-何PiM'VP = 任 prj p(9+ 2- 15)-一户nP?fl"称为MCP)的核，是由四元数P的元构成的反对称矩阵，同理M(Q)的校为'?&啊一如rVj = <?、q04)(9.2*16)-%一 ,i-可见AT (Q)与AHQ)构成相似，但核不同。由式(9. 2. 13)和式(9. 2.14),得四元数乘法的拒阵表示形式；P.Q = M(P)Q(9.2.17a)P® G = Mf (Q)P(% 2. 17b>由于M3)和AT(P)的核不同，所以P®? = Af(P)QU AT(P)Q = Q®P上式说明四元数乘法不满足交换律。四元数乘法满足分配律和结合律|尸(Q + X) = Pg)Q + F&(9* 2-18)P®Q®Jt=(9. 2 19)3）除法一一求逆如果尹虐）丑=1,则称X为P的逆,记为J? = PT,或祢尹为R的逆，记为P =I矿，根据范数定义和式（9.2.12）P ® P' =3。+ />/" + W + 京M） ® </>0 - pi pd 一 P.）pi +祈+舟+树=IIP II.所以，咆j鬲f = 1，根据上述关于逆的定义，借叩为P的逆，即P' = T局教）好，关于四元数定义就搬这么多，其他的大家去附件下载惯性导航的pdf自己看吧。下面开始搬四元数与姿态解算关系的。嘿咻嘿咻从上面我们知道了四元数的定义，可这四个数和我们要求的三个姿态角有什么关系呢下面 是详细的推导，同样摘自惯性导航-秦永元P292-297。四元数与姿态阵间的关系摘自惯性导航-秦永元P292-2979- 2-2四元数与姿态阵间的关系设有参考坐标系正，坐标轴为里标轴方向的单位间重为心刚体相对X系作定点转动，定点为。，取坐标系&与刚体固联系的坐标轴为工、队 给坐标轴方向的单位向量为7奴假设初始时刻B系与&系重合。为了便于分析 刚体的空间超位置，在刚体上取一点转动点D至该点引位置向量。4,如图 9, 2,1所示.则该位置向量的空间位置实际上描述了刚体的空间角位置。设刚体以3 =心设g盘+叫A相对R系旋 转，初始时刻位置向量处于OA = r,经过时间f 后位置向量处于OA =r根据欧拉定理，仅考 虑刖体在。时刻和£时刻的南位首时，刖体从 /位置转到4位置的转动可等效成绕瞬勘U (单位向最)转过P角一次完成B这样，位置向最 做圆锥运动M和/T位于同一圆上/和/位于 同一圆锥面上下面分析/与的关系.在圆上取一点B, 使/AOF=9。由图得OQ' =(，, u)u(7A =r OQf = r 一 (r *图9. 2.1刖体的等效旋转O B =w X O A=u X r (r * 心n x u = « X r OA! =OfAc& += rco3 (r * ii)ucos + X rsinff所以rf =OQ + OX = rcos + (I coM)X (r * 十 M X rsintf 由二重矢积计算公式：u X (u X r) =u(u * r) (u - u)f(r * u)u r即(r u)u = r -H w x (u X r>所以rf =rcosQ + (1 cosfl) Qr + xi X (Xr)+“X rsinE=+ it Xsin' + ( cosm X (w X r) 将上式向&系内投影irtx =2 + (w X r)电血04(1 cos)fH X (w X)"记又根据又乘关系表达式:则(« X顼=瞄X (u X r)R = £/ * W所以r,R -rK + UrRin& + (1 一 cost?)C/ - UrRh nn !,l + 2Usin -roos 5 + 2sinz rU * U(9. 2. 25)I) = f - 2(7$in -cos g + Zsirp V » VE£ar，R = Dfr则式(9-2. 24)可写成;(9. 2- 26)记初始时刻的刚体固联坐标系为为1由于初始时刻刚体固联坐标系与参考坐 标系更合，所以rR 河(9. 2 27>而在转动过程中,位置向量和b系都同湖体固联,所以位置向量和&系的相对角位 置始姓不变，即有A = r(i(9.N28)将式(9.2.28)代氏式(9.2. 27),得rK = Tfi19.2.29)桐式9. A 纫代入式(9.2-跚,得r,R =该式说明。即为力系至R系的坐标变换距阵,根据式（9, 2. 25）和式（9.2, 23>C? = /) 7 + geos % 4- 2sinz * U (9. 2. 30a)£ £ £Zwrsin2 girtsin2一 W + g-Zmsinz (F +mwslnu2nsinE gmwsin(mJ + Z：i)sjn'£(93. 30b)并以我w】w八火构造四元数;(9. 2- 31)Q =s + %标+齐人+勺相=cos y + (/ia + mjq + 7?lfl)sin %= cosb . R ey + « SLD (9. 2- 32).则可得如下结论：四元数0 = CG5 g+M电诸f描述刖体的定点转动，即当只关心6系相对 R系的角位置时，可认为6系是由R系经过无中间过程的一株性等效旋转形成的， Q包含了这种等效旋转的全部信息:为旋转瞬轴和旋转方向t为转过的角度，（2）四.元数可确定出&系至R系的坐标变换貌阵。将式O- 2-31）代人 式（9； 2, 30）,442(。沔1 + Mi2 Si 如Ms)(9. 2. 33)-1 - 2必 + 扇） 2【们匝+ q瞄） 2（<?3 4奶）由于DQ H FF+疏+加心 号+澎十麻十/）辨 %=,所以可进 步推得如下结论：<n描述刚体施转的四元数是规范化四元数，（2）式（* 233）可写成：谯 + q；信一vi如+如）-2（彳1引一如如）Z<I?1V3 十 如）2（如央9时1）垢一水一说+必一（9.234）如果参考坐标系正是导航坐标系州，刚体固联坐标系b为机体坐标系.则坐标 变换阵伏就是姿态矩阵口,而由姿态矩阵可计算出航向角和姿态角，设运载体的航向角为w（习惯上以北偏东为正），俯仰角为8,慌滚角为y,取地 理坐标系邸为导航坐标系，并规定有、礼的指向依次为东、北.天，则机体坐标系 b与导航坐标系H即地理坐标系g）的关系如图1-2. 3所示"由核图可得三次基本旋转对应的坐标变换阵为sin更cos/ 0 si nW”10o G =8 =0100g韶s跖H-sinX 0 cosX =J& -siii co&&.CQS/sinsin/ cosinT"0costfsim?_sin/sinHcosF co$19bosZ -sinW8S世 0on(A 2. 38)'由于n系至人系的旋转过程中坐标系始终保持直角坐标系，所以C：为正交矩阵cdsFcwW + sinYsinW前泌 cosZsinl 十 sin/gw世siM sincosf?(9- A 40)比较式（93. 39）和式（9. 2- 40），得8 = arcsin(T)(9. 2. 41>' 丁，主arctanl 矿坦主=arctan航向角和横禳角的真值按表9. 2,1和表乩幻2确定,9.2 1航向角W真值表TiaTn斐f Q+T-90*+曾主啾主4-180*重主一 18b表9.横淳篇，真值表丁4:Th+十£-160°上述分折说明：如果表征h系至&系的旋转四元数Q已珊定，则按式（9.2.33） 或式93*34）可计算出姿态阵G,再按式（93.41）和表93.1及表93*2可确定 出运载体的航向角、俯仰角和横液帛，因此，四元数Q包含了所有的姿态信息，旋 联惯导中的姿态更新实质匕是如何计算四元数呃，粘了这么多其实就是为了想知道推导过程小伙伴好理解，真正有用的就是（）这个公 式。这个公式把四元数转换成了方向余弦矩阵中的几个元素，再用这几个元素转换为欧拉 角。就求解除了姿态！确+砧一磋一gl>1-2IX 应 yoCf =2（47QS + q奶）-2（/Q3 一 q奶）(93. 34)先从四元数q0q3转成方向余弦矩阵:0+口2 - ; - y2（口1口2+ 口0口3）2（口1口3 - 口0口2）2(口1口2 - 口0口3)2-邛 + 口2 - :012*3 + "1)SR + "2) 2(口2口3 - 口门) 2-邛-口2 + 口2 0123再从方向余弦矩阵转换为欧拉角: = sin-1 （斗?） 俯仰角口口口口口 = tan-1 （-乒）横滚角口口口口33 = tan-1（乒）一一航向角口口口22好的，公式原理都讲清楚了，下面来看一下匿名的代码:cosFcosW + sinZsinsin m&Fsin 函 + sin/cossinfl sin/cos-sin 酣esWewH&n&Lsin/ctislP1 cnsXsin好sin0 sin Zsin1 cosZcosTRsintf cos ZeesZ? 箕一阶龙格-库塔法计算式为9(i + F = 4”)+ 8&)q 式中的。时如式(7.2.】9)所示毋将式(7.3,25)展开成元素的表达式，有A(O + <uj(OFi(<)-始认/)凸3) - oir(t)Pi(f)P】(t * T) - P|(j) + y«r(t)A(j) +-、(。尸黄，)W)=均h) +- Wr(i)Pi(O +尸心 U 凸，)+宣+ y尸心一df虽然在下也不是很懂，不过粘出来还是能起到理解的作用，这样大家就不会觉得这是凭空 变出来的，本人数学功底薄弱，没有对推导进行过验证，如果有不对的地方欢迎指正。接着使用一阶龙格库塔(Runge-Kutta)发求出q0q3，这一点很多人不知道一阶龙格库塔 怎么推导的，下面也是这位网友的推导，大家参考着理解吧。3.四元敷微分四丑敝微火已知一四元鬟Q = Co5y + N5油勺封时冏微竹AdQ 16 d8 dn 8- 10 dff矿-万皿印不+无脚早f 砂喔豪已知n-n = -1. g = 0.=峪,E融t理座槽乱b每料行器坐棋寮.因舄陀螺傻在推行器上测到的角母度照3岸三处1+山M +由法，故将小金聊摸成3%曾蛟舄方便3爵=Q&& 。n Q，皿£序=Q ' Q' r 3知，Q =财务 Q将芸Wq三备展斜dQ 1”*"：叫方=5 (% + W + 5 + *) 3盘 + 妨打十必)= filu. L Z.£整哩繇其中PCl).r-Cdy一山八3富3y0 一3丁他 03y也yP*0 )+-更新四元敏使用一陪Run跋-Kutla更新四元敷，偶畿有一微分海d x无=抑引吐"回刖其解焉xt + m =xt + dt-/xf |,(dtj其中心第取搓遇期，将套用至四元殿Qt + At = Qt + At - ntt Ct展上式孔需利用用速建即可更新四元教这里的角速度口口、口口、是由捷联陀螺的输出(对机械转子陀螺必须经过误差补偿， 将在下面介绍)。对比着匿名四轴的代码看一看(、是捷联陀螺的输出)，代码的意思就比较清楚了。在 往上一步步推，我们就要求陀螺输出了，并且还要对数据进行互补滤波处理。/这部分看似很简单，但是也有让笔者难以理解的地方，希望后人能补充修正进行更好的讲 解。有了上一步的龙格库塔方程，我们现在需要的就是角速度的测量值。在四轴上安装陀螺仪，可以测量四轴倾斜的角速度，由于陀螺仪输出的是四轴的角速度， 不会受到四轴振动影响。因此该信号中噪声很小。四轴的角度又是通过对角速度积分而得，这 可进一步平滑信号，从而使得角度信号更加稳定。因此四轴控制所需要的角度和角速度可以使 用陀螺仪所得到的信号。由于从陀螺仪的角速度获得角度信息，需要经过积分运算。如果角速 度信号存在微小的偏差，经过积分运算之后，变化形成积累误差。这个误差会随着时间延长逐 步增加，最终导致电路饱和，无法形成正确的角度信号。如何消除这个累积误差呢可以通过上面的加速度传感器获得的角度信息对此进行校正。利 用加速度计所获得的角度信息Og与陀螺仪积分后的角度。进行比较，将比较的误差信号经 过比例Tg放大之后与陀螺仪输出的角速度信号叠加之后再进行积分。对于加速度计给定的角 度。g，经过比例、积分环节之后产生的角度。必然最终等于。g。由于加速度计获得的角度 信息不会存在积累误差，所以最终将输出角度。中的积累误差消除了。加速度计所产生的角度 信息Og中会叠加很强的有四轴运动加速度噪声信号。为了避免该信号对于角度。的影响，因 此比例系数Tg应该非常小。这样，加速度的噪声信号经过比例、积分后，在输出角度信息中 就会非常小了。由于存在积分环节，所以无论比例Tg多么小，最终输出角度O必然与加速度计 测量的角度Og相等，只是这个调节过程会随着Tg的减小而延长。先把这个过程的代码粘出来，看着代码一步步理解：#define Kp力加速度归一化();取四元数的等效余弦矩阵中的重力分量(V_gravity)；量叉积得出姿态误差V_error = acc % V_gravity；误差进行积分 .V_error_I += V_error * Ki；补滤波，姿态误差补偿到角速度上，修正角速度积分漂移Gyro += V error * Kp + V error I；1. 重力加速度归一化：加速度计数据归一化，把加速度计的三维向量转换为单位向量，因 为是单位矢量到参考性的投影，所以要把加速度计数据单位化，其实归一化改变的只是这三个 向量的长度，也就是只改变了相同的倍数，方向并没有改变，也是为了与单位四元数对应。2, 提取四元数的等效余弦矩阵中的重力分量：利用四元教爵地理的重力加速度旋I#至雅行器上面， 再舆加速度言潘出的值（已SS一化的）做外圈 得出差.用此森差封角速度做校正融合重力加速度I 召 T » = £波旋融至行器上的重力加速度舄折n,严 n 12 13 ,0、。占一场，0 - （ 2122 M 拓 JOvAfai M电 MgJ '1/、,如 x、,M”、n 9b _ 。博-（Mh'Zfbz/ XAfgj/弛行器上的如速度畜十部I得的加速度 福 做蜀一化t ab0+成-勇-勇 2（口日2 + Zoo/ "（口门-口。口2）2（口1口2 -"）口2-口2 + 口2 - 口201232 + 口0口1）口。口2） 口。口1） ：2 + 口2量叉积得出姿态误差：哎呀，又来棘手问题了，这个我也不太明白怎么讲啊，还是把大神的讲解粘过来吧，大家 看看是不是这么回事:acc是机体坐标参照系上，加速度计测出来的重力向量，也就是实际测出来的重力向 量。acc是测量得到的重力向量，V_gravity是陀螺积分后的姿态来推算出的重力向量，它 们都是机体坐标参照系上的重力向量。那它们之间的误差向量，就是陀螺积分后的姿态和加计测出来的姿态之间的误差。向量间的误差，可以用向量叉积（也叫向量外积、叉乘）来表示，V_error就是两个重 力向量的叉积。这个叉积向量仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉 积的大小与陀螺积分误差成正比，正好拿来纠正陀螺。（你可以自己拿东西想象一下）由于 陀螺是对机体直接积分，所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。看了上面的话，小弟一直对这个误差向量感到莫名其妙，后来又找到大神的一下一段话：这里误差没说清楚，不是指向量差。这个叉积误差是指将带有误差的加计向量转动到与 重力向量重合，需要绕什么轴，转多少角度。逆向推理一下，这个叉积在机体三轴上的投 影，就是加计和重力之间的角度误差。也就是说，如果陀螺按这个叉积误差的轴，转动叉积 误差的角度（也就是转动三轴投影的角度）那就能把加计和重力向量的误差消除掉。（具体 可看向量叉积的定义）如果完全按叉积误差转过去，那就是完全信任加计。如果一点也不 转，那就是完全信任陀螺。那么把这个叉积的三轴乘以x%，加到陀螺的积分角度上去，就是 这个x%互补系数的互补算法了。这个看了好像终于理解点了，再看下代码：量叉积得出姿态误差V error = acc % V gravity；这里“ % ”已经重定义为叉乘的算法了。）4.对误差进行积分：积分求误差，关于当前姿态分离出的重力分量，与当前加速度计测得的重力分量的差值进行 积分消除误差V error I += V error * Ki；5. 互补滤波，姿态误差补偿到角速度上，修正角速度积分漂移系数不停地被陀螺积分更新，也不停地被误差修正，它和公式所代表的姿态也在不断更 新。将积分误差反馈到陀螺仪上，修正陀螺仪的值。将该误差V_error输入PI控制器后与本 次姿态更新周期中陀螺仪测得的角速度相加，最终得到一个修正的角速度值，将其输入四元数微 分方程，更新四元数。Gyro += V error * Kp + V error I；Gyro就是得到的修正角速度值，可以用于求解四元数q0q3 了。到这里回顾一下八个步骤还漏了一个第七步：7.四元数归一化：规范化四元数作用:1. 表征旋转的四元数应该是规范化的四元数，但是由于计算误差等因素，计算过程中四元 数会逐渐失去规范化特性，因此必须对四元数做规范化处理。2. 意义在于单位化四元数在空间旋转时是不会拉伸的，仅有旋转角度.这类似与线性代数里 面的正交变换。3. 由于误差的引入，使得计算的变换四元数的模不再等于1，变换四元数失去规范性，因 此再次更新四元数。计算欧拉角时候必须要对四元数归一化处理。();呃，关于四元数求解姿态的砖好像搬完了。为什么要用四元数法求解姿态呢再搬一点关于 欧拉角法和旋转矢量法的介绍的。欧拉厢算法通过求解欧拉角微分方程直接计算航向角，俯仰角和横液常*欧拉 弟微分方程关系简单明了，概念直观，容易理解，解算过程中无需作正交化处理，但 方程中包含有三角运算这给实时计算带来一定困难b而且当俯仰隽接近9"时方 程出现退化现象，这相当于平台惯导中惯性平台的锁定，所以这神方法只适用于水 平姿态变化不大的情况，而不适用于全姿态运载体的姿态确定，方向余弦法对姿态矩阵微分方程作求解，避免了欧拉角法中方程的退化问题， 可全姿态工作但姿态矩阵微分方程实质上是包含九个未知黄的线性微分方程组， 与四元数法相比，计算信大，实时计算困难，所以工程上并不实用，四元数法只需求解四个未知最的线性微分方程组，计算量比方向余弦法小，且 算法简单.易于操作，是较实用的工程方法口但四元数法实质上是旋转矢域法中的 单子样算法，对有限转动引起的不可交换误差的朴俊程度不够，所以只适用于低动 态运载体(如运输机等的姿态解算，而对高动态运载体，姿态解算中的算法漂移会 十分严重.旋转矢量我可采用多子样算法实现对不可交换误差做有效补陪，算法关系简 单，易于操作，并且通过对系数的优化处理使算法漂移在相同子样算法中达到最 小，因此特别适用于角机动频繁激烈或存在严重角振动的运载体的姿态更新勺四元数法和旋转矢量法都通过计算姿态四元数实现姿态更新,但前者直接求 解姿态四元数微分方程，而后者通过求解姿态变化四元数再求解姿态四元数.两者 的算法思路并不相同.搬砖搬得好累啊，不过搬得差不多了，感觉挺乱的呃，主要是由于比较多吧，那我再串一 遍拉倒吧，你看得都累，我写着不累没闹明白再自己串一遍吧，相信第二遍就能明白了。哎对于我这样的渣渣而言也就能理解到这一步了，这也是我好几天的心血整理了一下，也 许有和我一样的菜鸟呢，对他们也许能有点帮助，做得不好希望大神们能耐心给与指正，而不 是嗤之以鼻，或者喷我一顿就走。毕竟整理了两天呢（我还以为一中午就能搞定呢）。渣 渣的学习之路也是挺不容易的，因为基础渣渣，学校渣渣，所以难以得到有效地帮助和指导， 有时在群里寻求帮助，无聊的群友会告诉你看书去，呵呵。我也知道看书啊。哪怕你 能告诉我我的问题在那本书的那部分能有相似吧一句看书去，上网查啊，等于没回答，如果一 直这样自己看下去可能半年也解决不了，因为渣渣的学习环境是有局限性的。不过好在有更多很热心的群友能提供给我帮助，把他们收集的好贴发给我，或者干脆手写 一个公式推导，一个电路图，然后拍照发给我，还有的帮我下载照片，分类命名给我，艾玛！ 热泪盈眶啊有木有再次感谢这些热心帮助我的小伙伴奇点，杜掌柜，廉价物品，忘记名字的小伙伴下面附上被我搬砖的几个好贴，谢谢大神们的乐于分享：对四元数解算姿态的理解（基于匿名六轴），感谢社区：软件姿态解算：捷联惯导算法心得: &tid=5492189&highlight=附件：匿名姿态解算代码惯性导航秦永元 推辱四元敷.pdf