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

1、四元数完全解析及资料汇总本文原帖出自匡名四轴论坛，附件里的资源请到匿名论坛下载:http:感谢匿名的开源分享，感谢群友的热心帮助。说什么四元数完全解析其实都是前辈们的解析T,小小真心是一个搬砖的，搬得不好希望大神们洽以批评和指正，在此谢it了。因为本人是小莱乌一枚，对，装某的那种菜鸟所以对四元教求解姿态角这么一个在大神眼里询单的算法，小弟我还是费了很大劲才旃微理解了那么一点点,小第我转整理时也是基于小弟的理解和帮商的，有些太息耳，有些可能错了，大牛们发现了再骂过我后希望能够洽与指正哈。好，废话到此为止，开始说主体。四元数和姿方南怎么说呢？先得给和我一样的小菜鸟们理一理思路，小鸟我在此叵了一个“

2、忠雄导困”（我承认我画的丑），四元数解算姿态首先分为两部分理解：第一部分先理解什么是四无数，四元数与姿志雨闰的关系；第二部分会理解怎么由惯性单元测出的加送度和角it度求出四元教，再由四元救求出欧拉角©图1渣渣总维导团在讲解什么是四元教时，小第的忠雄是顺看说的，息由四元敢的定义说起，说到四元敲与委冬雨间的关系。但在讲解姿态解算时，小弟的思维是逆向的，就是反推回来的，从欧拉用一步步反推回到惯性器件的测量数据，这样逆向说是因为便于龙解，因为实际在工程应用时和理论推导有很大差别O实际应用时正确的求解顿序应该为图1中序号顺序，即1->2->3->.但在笔者讲解姿态求解时恐略是

3、如图2的。四元k/求出k/求出数定义7=A姿态角尸入四元数图2逆向讲解芯路大家在看四元数时最好结合看代码一块看，小第看的是匿名四轴的代码，感觉写的丰常好也本常清晰，粘出来大家一块观摩。缸。部分是核心代码，总共分为八个步碟，和图1中的八个步.喋是一一对应的。讲解介绍时也是和代码对比起来讲解的。代码可以去匿名官网上下我，都是开源的，不是小第的，所以小弟不方便加在附件中。四元教更新姿态#defineKp2.of/加速度权重，越大则向加it度测至值收敛越快#defineKio.ooif误是积分增益voidANO_IMU:Quaternion_CF(Vector3fgyro#Vector3facqflo

4、atdelta!)VectorfVgravity.Verror.VerrorI:/!重力加入度归一日accnormalize();2.提取四元数的等效余弦矩阵中的支力分至Q.vector_gravity(V_gravity);3.向宝叉枳得出第态误是V_crror=acc%V_gravity;/1对误差进行积3V_error_l+=V_error*Ki;&互补逐波，姿态误差补偿到角速度上，修正南it度积分漂移Gyro+=V_error*Kp:V_error_l;6.一阶龙格库熔法更新四元数Q.Runge_Kutta_ist(GyrofdeltaT);7四元数归一化Q.normalize

5、();8.四元教转欧拉角好的，下面救砖开始！。00嚅咻嘿咻！Q.to_euler(&angle.x#&angle.yfRangle.z);一.什么是四元教？关于四元数的定义摘自落永元的惯性导抗，里面有非帝好的讲解，大家可以直接看结论和第九幸就可以。下面我粘贴了部分原文，粘贴的比较多比较详细，应为本人比较笨还爱较真，所以按本人的风格就妥详尽一点，大牛们都可以自动忽咯。四元数定义、表达方式及运算方法摘自惯性导航-秦永元P289-2929.2姿态更新计算的四元数算法设由运载体的机体轴确定的坐标系为6,惯导系统所采用的导航坐标系为小则由儿系到系的坐标变换矩阵C称为运载体的姿态矩阵，姿态

6、更新是指根据惯性器件的输出实时计算出C；矩阵。由于系和系均为直角坐标系，各轴之间始终保持直角，所以可将坐标系理解成刚体，当只研究两个坐标系间的角位置关系时，可对一个坐标系作平移，使其原点与另一个坐你系的原点重合。因此，两坐标系间的空间角位置关系可理解成刚体的定点转动。从这一基本思想出发，可获得姿态更新的四元数算法及旋转矢根算法。本节详细介绍四元数更新算法。9. 2.1四元数1 .四元数定义顾名思义，四元数是由四个元构成的数：（23或1&小）=q。+qj+q3k（9.2.1）其中,农、41、92、会是实数一、人人既是互相正交的单位向量，又是虚单位i,具体规定体现在如下四元数乘法关系中：=

7、-1,j®j=-1,fc0a=-n1 /=无，j®k=i,k®i=j卜（922）j0i=-k90J=-1»iA=-jJ式中,表示四元数乘法。上述关系可叙述为：相同单位向量作四元数乘时呈虚单位特性;相异单位向量作四元数乘时呈单位向乐叉乘特性。所以四元数既可看作四维空间中的一个向所，又可看作一个超复数C2.四元数的表达方式(1)矢量式Q=Q0+q(9.2.3)其中,如称四元数。的标量部分,g称四元数Q的矢量部分。对照式(9.2.D,可看出,是三维空间中的一个向植。(2)复数式Q=9o+qj+q?J+Qik(9.2.4)可视为一个超复数,Q的共腕复数记为C*=

8、q。-Qiiqzjq3A(9.2.5)。称为Q的共匏四元数。(3)三角式8 8Q=cos爹+tfsin5(926)式中/为实数，“为单位向量6(4)指数式9Q=er2(9.2.7)8和同上。289(5)矩阵式(92.8)3 .西元数的大小范敷四元数的大小用四元数的范数来表示，IIQII=或+d+5+ql(9.29)若IIQII=1,则Q称为规他化四元数。4 .日元数的运算加减来除1)加法和减法设Q=qo+。"+QiJ+gA尸=%+力】i+Pzj+力3A。土产=(qo±%)+(71±Pi)i+(72土p2)j+(qa±p3)k2)乘法(9.2.11)aQ-

9、+aqIi+aq2j+aq3k其中,a为标量.尸Q=(po+P】i+P2J+%卜)®(7o+q】i+qij+%比)=(%<7o-P0一夕2<72一户内3)+(9皿1+力闯0+生93-03g2”+(20伙+p泗0+%q3)j+(2国3+。30+P3-Pzqlk(9.2.12)=+尸1+r2j+r3k上式写成矩阵形式:一力1-/>2-P'%-PiPl户3PqP-PzP0。-Qi-Qz-(13q。93-qzq、7o矶<72-qi?o-po9】=M(P)Q以-见-Dop=M'(Q)PPz4(9.2-13)(9.2.14)290其中,M(P)的构成形式为

10、：第一列是四元数P本身，第一行是P的共规四元数P的转置，划去第一行和第一列余下的部分。PQ-P3PtVp=pi%(92-15)LPiPlPo-称为M(P)的核，是由四元数P的元构成的反对称矩阵。同理(Q)的核为9。如一会V'q=-</3qoQ-(12-qi-可见”(。)与M(Q)构成相似，但核不同.由式(9.2.13)和式(9.2.14).得四元数乘法的矩阵表示形式:P®Q=M(P)Q=MYQP由于M(P)和M"P)的核不同，所以PQ=M(P)Q工M'(P)Q=fi®P上式说明四元数乘法不满足交换律。四元数乘法满足分配律和结合律，p0(e+/

11、?)=p0e-f-p0/?/QK=(P®。)区。=P<S(Q®R)(9.2.16)(9.2.17a)(9.2.17b)(9.2.18).3)除法求逆如果齿*=1,则称R为P的逆，记为/?=PT,或称P为R的逆，记为P=根据范数定义和式(9.2.12)P0P'=(/>()+p2j+p3k)®(/>opxipzj-/>3k)=加+/+用+H=IIPII所以，Pg疗丁=1，根据上述关于逆的定义，帝即为P的逆即I=皆(92,22)好，关于四元教定义就撤这么多，其他的大家去附件下载惯性导航的pdf自己看吧下面开始搬四元数与姿冬解算关系的。oo

12、ooO嘿、咻嘿咻一图9.21刚体的等效旋转二、四元数与姿态阵间的关系从上面我们知道了四元教的定义，用这四个数和我们安求的三个姿态角有什么关系呢？下面是详细的推导，同样插自惯性导抗-泰永元P292-297。四元数与姿看阵间的关系摘自惯性导航-泰永元P292-2979. 2.2四元数与姿态阵间的关系设有参考坐标系R，坐标轴为西、的、N0,坐标轴方向的单位向量为记、丸、灯.刚体相对K系作定点转动，定点为。取坐标系6与刚体固联北系的坐标轴为/、外Z,坐标轴方向的单位向嫌为i、j、k.假设初始时刻6系与R系重合.为了便于分析刚体的空间角位置.在刚体上取一点4转动点。至该点引位置向量OA,如图9.2.1所

13、示。则该位置向量的空间位置实际上描述了刚体的空间角位置。设刚体以3=3/+叱/+叫A相对R系旋转，初始时刻位置向过处于OA=i，经过时间t后位置向量处于。A'=，.根据欧拉定理，仅考虑刚体在0时刻和t时刻的角位置时刚体从A位置转到4位置的转动可等效成绕瞬轴u(单位向量)转过,角一次完成。这样，位置向量做圆锥运动，A和4位于同一圆上"和，位于同一圆锥面上.下面分析，与，的关系。在圆上取一点以使/472=90。，由图得OO'="U)MO'A=r-OO'=，一(ruyuO'B=uXO'A=uXr(ru)uXu=uXrO1A1=O，A

14、cosd+OB%xd=rcos6(ru)ucos0+uX/sin，所以rf=OOf+O'”=rcosO+(1cosO)X(ru)u+Xrsin由三重矢积计算公式：«X(«Xr)="(ur)<u«)r=(ru)ur(r=r-FwX<wXr)所以/=rcostf+(1-cos夕)r+“X(uXr)J+uXrsintf=r+uX，sin£+(1cos。)”X(wXr)将上式向R系内投影，r,R=/+(“X，尸sin夕+(1c()sG)"X(uXr)*记又根据叉乘关系表达式:(9.2.23)(9.2.24)(9.2.25)

15、'R=DrR记0-nmU=n0ILmI0.则(uX，)"=VrCuX(«Xr)代=UU*所以，火=尸+VrinO+(1-3阴UUrR=(74-2Usin-cos-+2sin2guU1rx令O=，+2Usin5cos方+2sin2与UU则式(92.24)可写成：(9.2.26)记初始时刻的刚体固联坐标系为伉，由于初始时刻刚体固联坐标系与参考坐标系重合,所以rR=(9.2.27)而在转动过程中，位置向量和力系都同刚体固联，所以位置向量和6系的相对角位置始终不变，即有4=小(9.2.28)将式(92.28)代入式(9.2.27)，得d=d(9.2.29)将式(9.2,29

16、)代入式(9,2.26),得1=6"该式说明。即为%系至A系的坐标变换矩阵，根据式<9.2.25）和式（9.2.23）Cf=D=7+2l7sinycosy+Zsin24UV乙乙乙10.0+2cos一(mj+n2)sin2%bnsin-/wsin2-yJ8nsin5.0向n万.0msin5Zwsin2-(Z2+n2)sirfwnsin24Zsin9msin,.0Zsin-乙Zwsin2与.2emnsin(mz+/2)sin2,y乙(9.2.30b)q。(9.2.31)91=，sin.0Qz=加s】n5.e如=wsin-并以电卬:、92、央构造四元数：Q=<7o+q/o+q3

17、k0=cos-+(li0+mj0+*sin“sincor则可得如下结论:<1）四元数。=cos5+dsin3描述r刚体的定点转动，即当只关心6系相对R系的角位置时，可认为6系是由R系经过无中间过程的一次性等效旋转形成的.Q包含了这种等效旋转的全部信息:，为旋转瞬轴和旋转方向。为转过的角度.（2）四元数可确定出6系至R系的坐标变换矩阵。将式（9.2.31）代入式（9：230）,得1-2(或+冠)2(<7i<72+9o<73).2(qQoQ?)2ag-<7o93>1-2(才十底)+QoQ：)2(q】qs+9o<?2>2s沟3-9o<7i)1-2

18、+由(9.2.33)由于IIQII=qW+q：+q；+d=cos2£+（/2+“/+MXil?£=1,所以可进一步推得如下结论：（1）描述刚体旋转的四元数是规范化四元数。（2）式（9,233）可写成：+<71-92-Qi2（g】q2-1。如）2"闻3+qg"Ci=2Sq+q。的）<7oQi4-qi2（q阂§gql24闯3倒佻）2（9293+9）加一垃+居（9.2.34）如果参考坐标系K是导航坐标系,刚体固联坐标系6为机体坐标系，则坐标变换阵c£就是姿态矩阵a,而由姿态矩阵可计算出航向角和姿态角。设运载体的航向角为w（习惯上

19、以北偏东为正），俯仰角为。，横滚角为人取地理坐标系g为导航坐标系，并规定H、刈、匕的指向依次为东、北、天,则机体坐标系b与导航坐标系（即地理坐标系g）的关系如图1.2.3所示。由该图可得三次基本旋转对应的坐标变换阵为sinV/0cosV001.C9.2.37)cosVC：nC；=sinW0cos/0q=*=01.sin/0"cos/sinsin/cossin/"(9.2.38)0cos。sinG.sin/sincos?coscos/-C-c：cos/cos+sin7sinsin一cos/sin+sin/cossinsin/cos"=sinWcos。cosWcqs。

20、sindLsin/coscos7sin中sin。sinYsinWcos/cossincos7cos6J(9.2.39)T22733，由于，系至占系的旋转过程中坐标系始终保持直角坐Ti27'"标系,所以C7为正交矩阵比较式式.2.39）和式（9.2.40）,得0=arcsin(T32>'主=arctan(史)3*主=arctanj-2j(9.2.41)航向角和横滚角的真值按表9.2.1和表9.2.2确定表9.21航向角W真值表O表9.2.2演浪角y真值表TuTnY-0+90.-0一90°+h-+中芸.+“780。+空主+180。h一18(r/1+18CT

21、上述分析说明：如果表征舞系至6系的旋转四元数Q已画定，则按式（9.2.33）或式（9.2.34）可计算出姿态阵仁,再按式（9.241）和表921及表9.2,2可确定出运载体的航向角、俯仰角和横滚角，因此，四元数。包含了所有的姿态信息，捷联惯导中的姿态更新实质上是如何计算四元数Q9o+q；g；2(602+如。3)2(9闯3-gm)呃，粘了这么多其实就是为了想知道推导过程小伙伴好理解，真正有用的就是（9"34）这个公式。这个公式把四元数转换成了方向余弦矩阵中的几个元素，再用这几个元素转换为欧拉南。就求解除了姿态！2(qq4)2(qg3+QMz)加一qf十或一熄2(q四3qgi)24曲3+

22、一闯】)就一诏一城+gl(9.2.34)先从四元数qOf3转成方向余弦矩阵:Spo+Qi-Q2-<73=2(4通2+q0q3)I29遇3-QoQ2)2(qig2-QoQ3)Qoq1+q；一后23%+QoQi)2(<7iq3+QoQ2)2(q2Q3-QoQi)Qo-Qi-q；+q；.*nyn4再从方向余弦矩阵椅换为欧拉角:0=sin-1(T32)俯仰角pitc/i0=tan-1）横滚角厂。6=tail_1航向角yaw好的，公式原理都讲清楚了，下面来看一下匿名的代码:四元救转欧拉角，这里四元数是qlf4和公式里q0f3相对应。voidQuatemion:to_euler(float*r

23、ollffloat*pitch,float*yaw)Iif(roll)*roll=degrees(atan2f(2.of*(qi*q2+q3*q4)fi-2.of*(q2*q2+q3*qj)；/*roll=degrees(atan2f(-2.of*(q2*q4-qi*q3)fi-2.of*(qz*q2+q3*q3)；1if(pitch)(使用safe_asin()来处理pitch接近臾/9。时的奇点*pitch=degrees(safe_asin(2.of*(qi*q3-qz*q4);/*pitch=degrees(safe_asin(2.of*(q3*q4.qi*qz);)if(yaw)*y

24、aw=degrees(atan2f(2.of*(q2*q3-qi*q4)f2.of*(qi*qi*q2*qz)-1);/*yaw=degrees(atan2f(2.of*(q2*q3-qi*q4)f2.of*(qi*qi+q3*q3)-1);)对比余弦矩阵转换为欧拉角的公式很容易理解了吧，注意一下，红已是区名原版的代码，和公式还是有出入的，自己细心现察一下吧。被注择了的黑己代码是我根据上面的公式写的。但足己的求斛出来的欧拉角反映出来的姿态是不对的，具体表现为俯仰(pitch)和横滚(roll)是相反的，为啥根据公式写的艮不对的？科里的小伙伴给与了我热心的解答O这个错误主妥是由于方向余弦矩阵的旋

25、转顺序不一样，也就是公式(9.2.39)不一样，这是由于旋转的先后顺序不同引起的，具体大家参考惯性导抗绪论来看就能明白，因为这一点小弟还有点混乱，就点到这为止。c干ycos/cos+sin/sinsincos/sin+sinYcosWsin。sin7cos夕=sinWcosdcosWcos。sin。Lsin/coscos/sinVrsinsinsinWcos/cossincosYcqsOJ(9.2.39)以上就是四元数求解欧拉角的方法©逝过公式可以看外，妥求欧拉角需妥求得四元数的方向余弦矩阵；妥求得四元数方向余弦矩阵，需妥求得四元教qOF3,那么如何求得qOq3?接下耒详如介绍

26、69;三、四元数微分及龙格库塔求Q0Q3首先我们先来看一下在程序里如何求解的q0-q3：一阶龙格库塔法更新四元致voidQuaternionRunge_Kutta_lst(VectorSf&g,floatdelta?)一一ql-05*(-q2*gx-q3*gy-q4*g.z)*deltaT.q2+=05*(ql*gx+q3*gz-q4*gy)*deltaT;q3-05*(ql*gy-q2*gz+q4*gx)*deltaT;q44=05*(ql*gz+q2*gy-q3*gx)*deltaT;)这就是一阶龙格库塔法求解q的微分方程，传入参数只需妥这个周期的角速度gx、gy、gz和冏期时间d

27、eltaTo下面一张是从某位大神的贴吧上盗的图，描绘的是一阶龙格库熔的计算式。其一阶此格-岸塔法计算式为«(，7),q(i)式中的n(t)式式(7.2.19)所示。将式(7.3.25)展开成元素的表达式，有r)=一B,r(，)/(，)一wy(t)Pj(t)-Wg(I)Pj(I)Pl(t+r)=P|(l)u»zU)P2(l)-a,r(t)P3(l)+D=Pj(/)y(a/r(<)A(t)-片(，)=P3(oy<oz(r)A(r)卬£，)片(力-/SK3Lmaj相信很多人和我一样，羊看上图很难理解其中的意思和其由来，于是我又找了很多帖子，感谢前人做出的贡献

28、，小弟在这里再次整理大神的四元数微分方程推导公式，便于大家理解。摘自附件中推沿四元救pdf虽然在下也不是很懂，不过粘出来还是能起到理解的作用，这样大家就不会觉得这是凭空变出来的，本人教学功底薄弱，没有对推导进行过验证，如果有不对的地方欢迎指正。接着使用一阶龙格库塔(Runge-Kutta)发求出q0q3,这一点很多人不知道一阶龙格库熔怎么推导的，下面也是这位网友的推导，大家参考看理解吧。3 .四元数微分四元畋微分，已知一四元敷Q=Cosl+n-Si响，那耀微分dQ1QdOdn。八1Gded7="/叫卜充+市.SE另+n-2COS2,dFA已知；:=-L黎=0,黑=31b.E朗t理座模

29、系,b舄刑亍器坐棒系等=脸(Cos|自+1Smg)=；温Q因悬陀螂倭在欢行器上测到的向速度内C3=3j+3“+3/A,故棒换成曾较悬方便JW察=：Q温展阴整理焉其中4 .更新四元舅C使用一隋Rungc-Kutta更新四元敬，假有一微分方程期其解检Xt+4t=Xt+zk.fxttu)t其中At23取煤调期，4寻套用至四元数Qt+At=(?t+dt-nt)(?t展滞上式(qQ/qQ/fq1一如q2一包q?QiQi|At/+3q。-3yq3+3/q?Qaj-IQz/+2*1+3q3+3yq()gq1<?3/e>Ac73/七一5Qz+%Qi+%q。只需利用角速度即T史新四7t鼓这里的角速度

30、以、4,、%是由捷联陀螺的揄出（射机械转子陀螺必须经过误是补偿，将在下面介绍）0对比看匿名四轴的代码看一看（gx、gy、gz是捷朕陀场的输出），代码的意思就比较清楚了。在往上一步步推，我们就会求陀螺揄出了，并且还委对数据进行互补遮波处理。四、惯性单元测量值破合这部分看似很同单，但是也有让笔者难以理解的地方，行望后人能补充修正进行更好的讲解。有了上一步的龙格库熔方程，我们现在需妥的就是南速度的测至值。在四轴上安装陀螺仪，可以测至四轴简料的角速度，由于陀埸仪揄出的是四轴的南速度，不会受到四轴振动影响。因此该信号中噪声很小。四轴的角度又是通过对角速度积分而得，这T进一步平滑信号，从而使得角度信号更加

31、稳定。因此四轴控制所需妥的角度和角速度可以使用陀螺仪所得到的信号。由于从陀螺仪的角it度获得南虎信息.需会经过积分运算。如果角速度信号存在微小的偏差，经过积分运髡之后，变化形成枳系误差。这个误差会瓯看时间延长逐步增加，最终导致电路饱和，无法形成正确的南虎信号。如何消除这个累积误差呢？于以通过上面的加速度传感器获得的角度信息对此进行校正。利用加速度计所获得的角度信息为与陀螺仪积分后的角/Ih进行比较，将比较的误差信号经过比例Tg放大之后与陀螺仪输出的角速度信号要加之后再进行积分。对于加速度计洽定的南度2,经迂比例、积分环节之后产生的角度h必，然最终等于“。由于加速度计获得的南度信息不会存在积累误

32、差，所以戊终将输出角度H中的积累误差消除了。加速度计所产生的角度信息Hg中会要加很强的有四轴运动加速度噪声信号。为了遛免该信号对于角度H的影响，因此比例系数Tg应该等韦小。这样，加it度的喙声信弓经迂比例、枳分后，在输出角度信息中就会*有小了。由-丁存在积分环节，所以无论比例Tg多么小，最终输出雨度h必然与加速度计测至的角度均相等，只是这个调节迂积会随看Tg的减小而延长。先把这个迂程的代码粘出来，看看代码一步步理解：defineKp2.Of加速度权更，越大则向加速度测费值收敛越快#defineKi0OOlf误差积分增益1.重力加连度归一化accnormalize。，2.提取四元数的等效余弦矩阵

33、中的重力分至Q.ve:tor_gravity（V_gravity）,3向萤叉枳得出姿态误差V_error=acc%V_gravity,时误是进行枳分V_error_IV_error*Ki,彳互补区波，逐东误差补偿到南运皮上，修正角速度和分漂移Gyro4Vcrror*Kp+VcrrorI,1 .重力加送度归一化：加速度计数揖归一化，把加迫度计的三维向至转换为单位向童，因为是单位矢金到参考性的投影，所以妥把加速度计数据单位化，其实归一化改变的只是这三个向主的长度，也就是只改变了相同的侍数，方向并没有改变，也是为了与单位四元数对应。2提取四元数的等效余弦矩阵中的重力分贡:/返回该四元数的等效余弦矩阵

34、中的重力分至voidQuaternionvector_gravity(Vector3f&v)(vx=2*(q2*q4-ql*q3);vy=2*(ql*q2+q3*q4),v.z=l-2*(q2*q2+q3*q3).将当前姿态的重力在三个轴上的分至分离出来，把四元数换算成方向余弦中的第三行的三个元素，根据余弦矩阵和欧拉角的定义，就是地理坐标系（参考坐标系）的Z轴的变力向至。当我读究这句话脑子挺怪的，闹不明白啊，于是又找到了下面的资料，可以进行解择了。利用四元数耨地理的重力加速度旋樽至来行器上面，再典加速度舒ifi出的值（已端一化的）做外稹，得出塞用此差封角速度做校正融合重力加速度g=gz

35、做端一化grg=z旋斡至来行器上的重力加速度悬gb兼行器上的加速度升测得的加速度Qb做一化Mq=Qo+qi-qf-Qi2(qiq2+Q0Q3).2(%-QoQ2)2(4142-Q0Q3)Qo-Qi+q；-q；2(%布+q。%)2(Qiq3+Q0Q2)2(<72Q3-QoQi)Q0-Q1-92+q；.别忘了这是个正交矩阵哦！这样就知道代码怎么来的了吧？好继续。3.向至叉枳得出姿态误差：哎呀，又来棘手问题了，这个我也不太明白怎么讲啊，还是把大神的讲解粘过来吧，大家看看是不是这么回事：acc是机体坐标参呼.系上，加速度计测出来的重力向主，也就是实际测出来的重力向童。acc是测妄得到的重力向量，

36、V-gravity是陀螺积分后的姿态来推算出的重力向至，它们都是机体坐标参照系上的重力向主。那它们之间的误是向至，就是陀埸积分后的姿态和加计测出来的姿态之间的误差。向童间的误差，可以用向主叉积（也叫向至外积、叉乘）来表示，V_error就是两个重力向至的叉枳。这个叉枳向至仍旧是位于机体坐标系上的，而陀螺积分误差也是在机体坐标系，而且叉枳的大小与陀埸枳分误是成正比，正好拿来到正陀埸。（你可以自己拿东西想象一下）由于陀螺是对机体扎接积分，所以对陀场的纠正至会直按体现在对机体坐标系的到正。看了上面的话，小弟一直对这个误差向柔感到莫名其妙，后来又找到大种的一F一段话：这里误是没说清楚，不是指向主屋。这

37、个叉枳误差是指将带有误甚的加计向至转动外与重力向受重合，需妥烧什么轴，转多少南度。逆向推理一下，这个叉积在机体三轴上的投影，就是加计和重力之间的角度误差。也就是说，如果陀埸按这个叉枳误差的轴，转动叉积误差的角度（也就是转动三轴投影的角度）那就能把加计和篁力向主的误是消除掉。（具体可看向主叉枳的定义）如果完全按叉枳误是转过去，那就是完全信任加计。如果一点也不转，邠就是完全信任陀场。那么把这个叉枳的三轴乘以x%,加用陀螺的枳分角度上去，就是这个X%互补系数的互补算法了。这个看了好像终于理解点了，再看下代码：3向云叉枳得出姿态误差Verror=acc%Vgravty;这里“”已经支定义为叉来的算法了

38、。4对误差进行积分：积分求误差,关于当前姿态分离出的重力分玄，与当前加达度计测得的重力分主的差值进行积分消除误是VerrorI+=Verror*Ki,5互补滤波，姿态误差补偿到角速度上，修正角速度枳分漂移系数不停地被沱场积分更折，也不件地被误差修正，它和公式所代表的姿态也在不断更新。将积分误差反情幻陀塔仪上，修正陀场仪的值。将该误差V_ero揄入PI控制器后与本次姿态更新周期中陀螺仪测得的南运度相加，最终得到一个修正的南速度值,将其揄入四元数微分方程,更新四元教。Gyro+=Verror*Kp+VerrorI,Gyr。就走得到的修正角述度值，丁以用于求解四元数q0q3了。到这里回顾一下八个步腺还漏了一个第七步:7.四元数归一化：规范化四元数作用：1 .表征旋转的四元数应该是规范化的四元教，但是由于计算误差等因素，计算迂程中四元教会逐渐失去规范化特性，因此必须对四元数做规范化处理。2 .意义在于单位化四元数在空间旋转时是不会拄伸的,仅有旋转角度.这类似与线性代数里面的正交变换。3 .由于误差的引入，使得计算的变换四元数的模不再等于T,变换四元数失去规