四旋翼无人机设计与制作
????0?nsinmsin??22?100????????2cos?nsin?CbR??0100?lsin??2?22?????001??????msinlsin0???22?? (4-12)
??222?2?2??(m?n)sinlmsinlnsin??222???????2?lmsin2?(l2?n2)sin2mnsin2??222??2?2?222??lnsinmnsin?(m?l)sin??222???令
??q?cos?02??q?lsin??12 (4-13) ??q?msin??22???q3?nsin?2并以q0、q1、q2、q3构造四元数：
Q?q0?q1i0?qj2?0qk3?c(?m0os?li0j?nk02?cos??2?uRsin?2)s0in2 (4-14)
?可得如下结论：
(1)四元数 Q?cos?2?uRsin?2描述了刚体的定点运动[31]， Q包含了等效旋转的
全部信息，uR为旋转瞬轴的旋转方向，?为转过的角度。
(2)四元数可以确定出b系至R系的坐标变换矩阵。将(4-8)代入(4-7)得：
22?1?2(q2?q3)2(q1q2?q0q3)2(q1q3?q0q2)???2)2(q2q3?q0q1)? (4-15) CbR??2(q1q2?q0q3)1?2(q12?q32??2(q1q3?q0q2)2(q2q3?q0q1)1?2(q12?q2)??–22–
四旋翼无人机设计与制作
222?q12?q2?q3?(l2?m2?n2)sin2由于Q?q0?2?1，所以可进一步得出如下结论：
222?q0?q12?q2?q3? CbR??2(q1q2?q0q3)?2(q1q3?q0q2)?2(q1q2?q0q3)222q0?q12?q2?q32(q2q3?q0q1)2(q1q3?q0q2)??2(q2q3?q0q1)? (4-16) 222?q0?q12?q2?q3?如果将向量rR和rb看作零标量的四元数，则rR和rb间的变换关系可采用四元数乘法表示：
rR?Q?rb?Q*
该式称为坐标变换的四元数乘表示方法，其中Q为R系至b系的旋转四元数。证明如下：
?0?Q?rb?Q*?QQ*?b??r?q1q2q3??0??q0?q1?q2?q3??q0?q???q??rb?q?qqq?qq10321032????x? ?? (4-17) b?q2q3?????q0?q1?q2q3q0?q1ry??????q0???q3?q2q1q0??rzb??q3?q2q1000????0???q2?q2?q2?q2??rb?2(qq?qq)2(qq?qq)012312031302??x???2222??2(q1q2?q0q3)q0?q1?q2?q32(q2q3?q0q1)??ryb??2222??b??2(qq?qq)2(qq?qq)q?q?q?q130223010123??rz??对比式(4-11)知上式矩阵中右下角的3?3方块即为CbR，所以式(4-12)可写成：
??000???0?0?????? ??rR???b?CbR?????r??????即
rR?CbRrb
该式称为坐标变换的矩阵表示法。所以四元数乘法表示法和矩阵表示法是等价的。
如果参考坐标系R是导航坐标系n,刚体的固联坐标系b为机体坐标系，则坐标变换矩阵CbR就是姿态矩阵Cbn，而由姿态矩阵可计算出姿态角[32]。
–23–
四旋翼无人机设计与制作
?T11T12T13??，由于在坐标系旋转的过程中坐标系始终保持直角坐标系，所TTT记Cbn??212223????T31T32T33??以Cbn为正交矩阵
?cos?cos??sin?sin?sin?bCn=??cos?sin????sin?cos??cos?sin?sin?cos?sin??sin?sin?cos?cos?cos?sin?sin??cos?cos?sin??sin?cos??? sin??cos?cos????T11T21T31?? bCn?(Cbn)T??TTT122232????T13T23T33??对比上式可得姿态角
?????arcsin(T32)?T ???arctan(?31) (4-18)
T33??T???arctan(?12)T22?因此，四元数Q包含了所有的姿态信息，姿态解算实际上是如何计算四元数Q。下面来介绍如何计算四元数： (1)初始化四元数
假设当前的坐标系为地理坐标系，则四元数列向量
q?[q0,q1,q2,q3]T=[1,0,0,0]T
(2)从传感器获取载体加速度和角速度
从MPU6050读取三轴加速度计的测量值即加速度accx、accy、accz，陀螺仪的测量值即角速度?x，?y，?z。
(3)将加速度计得出来三个轴的加速度值accx、accy、accz转化为三维的单位向量得到：
–24–
四旋翼无人机设计与制作
?accx?ax??accx2?accy2?accz2?accy? ?ay? (4-19)
222accy?accy?accz??accz?a?222?zacc?acc?accyyz?(4)将地理坐标系的重力向量转换到机体坐标系可得三轴的重力量vx，vy，vz：
22222(q1q2?q0q3)?vx??q0?q1?q2?q3?v???2(qq?qq)q2?q2?q2?q212030123?y???2(q2q3?q0q1)?vz????2(q1q3?q0q2)
?2(q1q3?q0q2)????2(qq?qq)2301??2222??q0?q1?q2?q3??2(q1q3?q0q2)??0???2(q2q3?q0q1)???0??2222?q0?q1?q2?q3???1?? (4-20)
(5)将地理坐标系转换到载体坐标系下的重力向量和载体坐标系测量的加速度向量外积，得到两坐标系的误差[33]：
?ex??ax??vx??ayvz?azvy??e???a???v???av?av? (4-21)
xz??y??y??y??zx???ez????az????vz????axvy?ayvx?(6)陀螺仪误差是导致机体坐标系误差的根本原因，因此用两坐标系误差的PI来补偿陀螺仪使得机体坐标系更加准确。
??xin???x??et??????kp?e? ?yint???y?????t??zin????z???e??e?x??ki?ey?????e?z???x??y (4-22) ?z?其中：kp和ki是调整参数，在实际调试中确定。其中ki可以等于0，kp可以以0为初始值，0.01步进调节。 (7)四元数姿态更新方程[34]。
四元数微分方程为：
?=1Qω (4-23) Q2其中Q?q0?q1ib?q2jb?q3kb，ω?0??xib??yjb??zkb 将上式写成矩阵形式
–25–
四旋翼无人机设计与制作
?0??0?q?q?x?1?1????? ?q?2?2??y????q??3???z??x0??z??y?z0??x?y??z??q0??q???y???1? (4-24) ?x??q2????0???q3?对四元数一阶微分方程进行一阶毕卡算法可得：
?t?q?q?(?q??q??q?)01xint2yint3zint?02??q?q?(q??q??q?)?t10xint2zint3xint?12 (4-25) ??q?q?(q??q??q?)?t20yint1zint3xint?22??t?q3?q3?(q0?zint?q1?yint?q2?xint)?2(8)对四元数进行规范化处理，于是得下式4-21：
??q0????q1??? ??q??2???q3???q0q02?q12?q22?q32q1q02?q12?q22?q32 (4-26)
q2q02?q12?q22?q32q3q02?q12?q22?q32(9)以上得到的新的四元数代表完成了一次四元数的运算，将此四元数回到开头，将旧的四元数更新为新四元数，作为下一次四元数运算的初始数，再从(1)开始下一次的四元数运算[35]。与此同时将新的四元数更新规范化后转化成三个欧拉角得下式(4-22)，完成了姿态的初步运算[36]：
?????arcsin(2(q2q3?q0q1))?2(qq?qq) ???arctan(?21232022) (4-27)
q0?q1?q2?q3??2(qq?qq)???arctan(?21222032)q0?q1?q2?q3?–26–